当前位置：首页 > news >正文

matlab版本粒子群算法(PSO)在路径规划中的应用

news 2025/9/5 10:13:58

基于粒子群优化（PSO）算法的路径规划

MATLAB代码实现

1. 初始化环境和参数

% 初始化环境参数
mapSize = [10, 10]; % 地图大小
startPoint = [1, 1]; % 起点
endPoint = [9, 9]; % 终点
obstacles = [3, 3; 5, 5; 7, 7]; % 障碍物位置% PSO参数
numParticles = 30; % 粒子数量
numIterations = 100; % 迭代次数
w = 0.5; % 惯性权重
c1 = 1.5; % 个体学习因子
c2 = 1.5; % 社会学习因子

2. 定义适应度函数

适应度函数用于评估路径的优劣，通常考虑路径长度和避障能力。

function fitness = calculateFitness(path, startPoint, endPoint, obstacles)% 计算路径长度pathLength = 0;for i = 1:length(path)-1pathLength = pathLength + norm(path(i,:) - path(i+1,:));end% 检查路径是否与障碍物相交penalty = 0;for i = 1:length(obstacles)if any(ismember(path, obstacles(i,:), 'rows'))penalty = penalty + 1000; % 高惩罚值endend% 适应度函数fitness = pathLength + penalty;
end

3. 初始化粒子群

% 初始化粒子位置和速度
particlePositions = repmat(startPoint, 1, numParticles); % 所有粒子从起点开始
particleVelocities = zeros(2, numParticles); % 初始速度为零
particleBestPositions = particlePositions; % 个体最优位置
particleBestFitness = inf(1, numParticles); % 个体最优适应度
globalBestPosition = startPoint; % 全局最优位置
globalBestFitness = inf; % 全局最优适应度

4. PSO主循环

for iter = 1:numIterationsfor i = 1:numParticles% 更新粒子位置particleVelocities(:,i) = w * particleVelocities(:,i) ...+ c1 * rand * (particleBestPositions(:,i) - particlePositions(:,i)) ...+ c2 * rand * (globalBestPosition - particlePositions(:,i));particlePositions(:,i) = particlePositions(:,i) + particleVelocities(:,i);% 评估适应度currentFitness = calculateFitness(particlePositions(:,i)', startPoint, endPoint, obstacles);% 更新个体最优if currentFitness < particleBestFitness(i)particleBestFitness(i) = currentFitness;particleBestPositions(:,i) = particlePositions(:,i);end% 更新全局最优if currentFitness < globalBestFitnessglobalBestFitness = currentFitness;globalBestPosition = particlePositions(:,i);endend% 显示当前最优适应度fprintf('Iteration %d: Best Fitness = %.2f\n', iter, globalBestFitness);
end

5. 可视化结果

% 绘制路径
figure;
hold on;
plot(startPoint(1), startPoint(2), 'go', 'MarkerSize', 10, 'LineWidth', 2); % 起点
plot(endPoint(1), endPoint(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2); % 终点
plot(obstacles(:,1), obstacles(:,2), 'ks', 'MarkerSize', 6, 'LineWidth', 2); % 障碍物
plot(globalBestPosition(1,:), globalBestPosition(2,:), 'b', 'LineWidth', 2); % 最优路径
xlabel('X');
ylabel('Y');
title('PSO Path Planning');
legend('Start', 'End', 'Obstacles', 'Path');
grid on;
hold off;