基于粒子群優化（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;
參考代碼 matlab版本粒子群算法(PSO)在路徑規劃中的應用 www.youwenfan.com/contentcsf/80589.html
