粒子群算法路径规划二维栅格

### 使用粒子群优化算法在二维栅格中进行路径规划

#### 1. 粒子群优化算法简介
粒子群优化(Particle Swarm Optimization, PSO)是一种模拟鸟群觅食行为的随机搜索技术，通过个体之间的协作完成全局最优解的寻找。PSO算法因其简单易实现、参数少等特点，在众多领域得到了广泛应用。

#### 2. 二维栅格模型构建
对于二维栅格环境下的路径规划问题，可采用如下方式定义地图结构：

- 将整个工作空间离散化成多个相同尺寸的小方格；
- 对于每一个小方格赋予二元属性值：`0` 表示无障碍区（即可行域），而 `1` 则标记为不可穿越区域如墙壁或其他固定障碍物[^5]；

% 初始化栅格地图
mapSize = [10, 10]; % 地图大小
obstacleMap = zeros(mapSize); % 创建全零矩阵作为初始地图
% 设置一些随机障碍物位置
for i=1:randi([5,8])
    obstacleMap(randi(size(obstacleMap))) = 1;
end
disp('Obstacle Map:');
disp(obstacleMap);

#### 3. 定义适应度函数
为了使PSO能够适用于此场景，需设计合理的适应度评价标准来衡量每条候选路线的好坏程度。通常情况下会综合考虑以下几个方面因素：

- **距离最短原则**：计算起点到终点之间直线距离与实际行走轨迹长度之差；
- **安全性考量**：避免靠近边界或已知危险地带行驶；
- **平滑性要求**：减少急转弯次数以提高飞行稳定性[^4]；

function fitnessValue = calculateFitness(path)
global start endPos mapSize
    
% 计算欧氏距离
distanceCost = norm(start-endPos);

% 遍历路径上的所有节点并累加成本
totalPathLength = 0;
smoothnessPenalty = 0; % 平滑惩罚项初始化
previousDirection = [];
for idx = 2:length(path)-1
    currentPoint = path(idx,:);
    
    % 更新总路程
    totalPathLength = totalPathLength + ...
        sqrt(sum((path(idx,:)-path(idx-1,:)).^2));
        
    % 如果不是第一个点，则检查方向变化情况
    if ~isempty(previousDirection)
        newDir = atan2(currentPoint(2),currentPoint(1))...
            -atan2(previousDirection(2),previousDirection(1));
            
        smoothnessPenalty = smoothnessPenalty + abs(newDir)*1e-3;
    end
    
    previousDirection = currentPoint-start;
end

fitnessValue = distanceCost-totalPathLength-smoothnessPenalty;
if any(isnan(fitnessValue))
    fitnessValue=-Inf;% 若存在非法操作则返回负无穷大
end
end

#### 4. 参数设置及迭代过程
设定种群规模、最大迭代次数等超参，并启动循环更新机制直至满足终止条件为止。每次迭代过程中都要重新评估各个体的位置优劣关系，并据此调整速度向量从而指导下一步动作方向[^1]。

% PSO参数配置
numParticles = 50; % 种群数量
maxIterations = 100; % 迭代上限
cognitiveComponent = 1.5; % 自我认知系数
socialComponent = 2.0; % 社会影响因子
inertiaWeight = 0.9; % 惯性权重衰减率

% ... (省略部分代码)

while iterationCount<=maxIterations && bestGlobalFitness>-inf
    for pIdx=1:numParticles
        
        % 更新当前粒子的速度和位置
        velocity(pIdx,:) = inertiaWeight*velocity(pIdx,:) + ...
                           cognitiveComponent.*rand().*(personalBestPosition{pIdx}-position{pIdx})+...
                           socialComponent.*rand().*(bestGlobalPosition-position{pIdx});
                           
        position{pIdx} = round(position{pIdx}+velocity(pIdx,:));

        % 边界处理逻辑...

        % 评估新位置对应的适应度得分
        personalCurrentFitness(pIdx)=calculateFitness(position{pIdx});

        % 更新个人历史最佳记录和个人局部极值
        if personalCurrentFitness(pIdx)>personalBestFitness(pIdx)
           personalBestFitness(pIdx)=personalCurrentFitness(pIdx);
           personalBestPosition{pIdx}=position{pIdx};
           
           % 同步刷新群体整体最优方案
           if personalCurrentFitness(pIdx)>bestGlobalFitness
               bestGlobalFitness=personalCurrentFitness(pIdx);
               bestGlobalPosition=position{pIdx};
           end
        end
     end
     
     % 输出中间结果供观察者查看进度
     disp(['Iteration ', num2str(iterationCount)]);
     
     iterationCount=iterationCount+1;
end