平时写路径规划对比实验，最怕的不是算法本身难写，而是五六个算法各跑各的，地图不一样、迭代次数不一样、适应度函数也不一样，最后给一个“对比图”根本没有说服力。这次我重新整理了一整套基于Matlab的二维栅格地图路径规划对比代码，把PSO、MPSO、TACPSO、SOA和GA五种智能算法放在同一个任务定义下做横向对比，算是把这几年踩过的坑集中做一次清算。这篇文章会从栅格地图建模、路径编码、适应度函数设计、各算法核心循环怎么写，一路聊到结果图分析和常见报错排查，适合正在做移动机器人路径规划、智能算法课程作业，或者准备拿对比实验当毕业设计支撑的读者。代码思路可以直接平移到你自己的项目里，不用照抄，把逻辑理清楚才最重要。
1. 为什么用二维栅格地图做五种智能算法的路径规划对比
1.1 栅格地图天生适合做算法验证
二维栅格地图本质上是把连续的机器人工作空间离散成一个个小格子，每个格子要么空闲要么被障碍物占据。路径规划的问题就变成：从起点格子出发，找一条连续经过若干空闲格子、最后到达终点格子的通路，并让路径长度尽量短。这种离散化表达的好处是，障碍物信息、路径长度、碰撞检测都可以用矩阵运算快速计算，不用处理复杂的几何求交，非常适合做智能算法的基准测试。
对比实验选二维栅格地图，还有一层原因：地图复杂度可以精确控制。比如可以生成规则障碍地图、随机障碍地图、迷宫式地图，甚至同一张地图里故意设置几个U型陷阱来考验算法的跳出局部最优能力。相比之下，在连续地图里做对比，每次生成的障碍位置稍有不同，结果就不公平了。固定同一张地图、同一起终点、同一套适应度函数，才能把算法差异真正暴露出来。
1.2 五种算法在路径规划里的定位差异
先看这五个算法分别是什么角色：
| 算法 | 全称与机制 | 在路径规划中的优势 | 常见问题 |
|---|---|---|---|
| PSO | 粒子群优化，模拟鸟群觅食，用速度更新和个体/全局最优引导搜索 | 参数少、收敛快、代码简单 | 容易早熟，障碍复杂时容易卡在局部路径 |
| MPSO | 改进型/多子群粒子群，通常在原版PSO基础上加变异、停滞重启或多子群协同 | 弥补PSO多样性不足，成功率更高 | 实现方式不统一，需要额外调参 |
| TACPSO | 时间自适应混沌粒子群，按迭代进度动态调整参数，并用混沌映射初始化或局部扰动 | 前期探索强，后期收敛较充分，稳定性较好 | 理解门槛高，代码结构比PSO复杂 |
| SOA | 海鸥优化算法，模拟海鸥迁徙和攻击捕食的双阶段搜索 | 全局勘探能力强，路径往往能找到意外较短的路径 | 收敛速度较慢，螺旋参数敏感 |
| GA | 遗传算法，用选择、交叉、变异模拟进化 | 全局搜索机制成熟，种群多样性有保障 | 收敛慢，局部细化能力弱，路径可能不够平滑 |
注意一点，MPSO和TACPSO在很多代码里没有统一全称。我见过的MPSO至少有两种解释，一种是Modified PSO，另一种是Multi-swarm PSO；TACPSO大多数情况下会被实现为“按时间变化的参数自适应+混沌扰动”，也就是Time Adaptive Chaos PSO。核心思路永远是围绕粒子群加多样性和自适应机制展开。你拿到不同版本的代码，名字可以不一样，但评价算法优劣的维度是一致的：路径长度更短、成功率更高、收敛更稳定。
1.3 公平对比实验的三个硬性约束
很多初学者做对比，喜欢让每个算法自己呼啦呼啦跑一遍，最后截图放在一起，看着像对比，其实变量完全失控。真正能做对比，至少满足三条：
- 同一张二维栅格地图，障碍物分布完全一致。
- 同一起点和终点，路径编码维度一致。
- 适应度函数完全一致，种群大小和最大迭代次数也一致。
算法各自的控制参数可以按经验调整，比如PSO的惯性权重、GA的交叉变异概率、SOA的螺旋参数，这些属于算法本身的特性，允许不同。但绝对不能出现“PSO用20个粒子、GA用100个个体，然后比谁路径短”这种荒唐事。代码里最好用一个统一的配置脚本管理所有公共参数，我习惯在main文件开头一次性定义好，这样后边改地图或改参数都方便。
2. 路径编码和适应度函数设计，是所有算法对比前必须统一的事
2.1 二维栅格地图的Matlab表示
在Matlab里最自然的表达方式是二维0-1矩阵，0表示可通行栅格，1表示障碍物栅格。假设地图范围是20×20，可以直接写：
map = zeros(20, 20); map(5:8, 3:6) = 1; % 一块矩形障碍物 map(10, 10:15) = 1; % 一面墙要注意的是，矩阵的行索引对应的是地图的y坐标，列索引对应x坐标。绘图如果用imagesc(map)，Matlab默认会把矩阵第一行显示在图像顶部，也就是y轴反向。如果你后续把路径点的坐标按常规(x,y)直接plot上去，很可能看到路径在障碍物里穿来穿去，其实是坐标映射错位。我的处理方法是统一定义坐标变量，比如：
x = 0.5 + (col - 1); % 格子中心的x坐标 y = 0.5 + (row - 1); % 格子中心的y坐标绘图时加上axis xy;恢复常规坐标系。
2.2 路径编码方式直接决定算法维度
智能算法不能直接操作一条路径，它需要把路径“编码”成一个向量，也就是个体。路径规划里最常用的编码方式是：从起点到终点之间取固定数量的中间点，每个中间点用(x,y)两个坐标表示，因此一个个体就是dim = 2 * num_points维的实数向量。
为什么不直接把路径经过的栅格序号作为编码？那也是一个方案，但维度和路径长度动态变化，交叉和变异都不好处理。为什么不在整张地图上二进制编码？维度过大，搜索效率惨不忍睹。所以固定中间点坐标是目前兼容性最好的做法。例如取10个中间点，粒子维度就是20，粒子第一个值表示第1个中间点的x坐标，第二个值表示第1个中间点的y坐标，以此类推。
起点和终点不参与编码，它们是固定的。每次把粒子位置解码成路径时，把起点、中间点、终点按顺序拼接成一条折线。这样做的好处是，五种算法共用同一个fitness_func.m文件，只要传入的向量是相同结构，算法怎么更新粒子都可以。
2.3 适应度函数的三项设计：长度、碰撞、平滑
适应度函数要同时反映三条要求：路径尽量短、绝对不碰障碍、拐弯尽量平缓。我通常写成：
fitness = w1 * path_length + w2 * collision_penalty + w3 * smoothness_penalty;路径长度好算，把相邻路径点之间的欧氏距离求和就行。碰撞检测是关键，不能只判断路径节点是否落在障碍格里，因为两个节点之间的连线很可能穿越障碍格。常见做法是在线段上均匀采样，把采样点坐标映射到栅格索引，看是否越界或者落入障碍格。采样步长一般取0.1到0.2个栅格宽度，太大会漏检，太小会让计算量上升。
平滑惩罚项的核心是减少相邻线段的夹角和或转弯次数。路径规划里最难受的不是路径长了点，而是机器人沿路径走的时候不断急转弯。可以用相邻线段方向角的差分绝对值求和作为平滑项，也可以用最大转角惩罚。三种权重w1、w2、w3需要调优，我建议先不写平滑项，只用长度加碰撞惩罚，选出有效路径后再逐步加大平滑权重，否则算法很容易为了转弯少而绕远路。
还有一个细节：如果初始粒子生成的路径全部碰撞，适应度会非常大。这时要让碰撞惩罚足够大，但不能大到让所有个体都是同一个无效值，否则算法失去了比较信息。可以使用“有效路径优先、无效路径按碰撞程度排序”的策略。
2.4 适配多种群迭代算法的关键：把适应度函数写成黑盒
代码设计上要强制让所有算法都只通过函数句柄调用适应度函数，不要在某个算法脚本内部再重新实现一遍碰撞检测。比如：
fitFunc = @(x) evaluatePath(x, map, startPt, goalPt, numPoint, params);PSO、MPSO、TACPSO、SOA、GA全部调用同一个fitFunc。这样后面你改了地图，或者想换成三维栅格地图，只需要改评估函数内部，不需要重写五种算法。这个设计是整套代码可维护性的核心。
3. PSO、MPSO、TACPSO、SOA、GA的Matlab实现要点拆解
3.1 主程序框架怎么组织
对比代码最好有一个统一的入口，而不是每个算法一个main文件。简化后的结构是：
clear; clc; close all; % 公共参数 maxIter = 100; popSize = 30; numPoint = 10; map = createMap(20); startPt = [1, 1]; goalPt = [20, 20]; dim = numPoint * 2; % 运行次数 runTimes = 20; for r = 1:runTimes % 依次调用五种算法 [bestPathPSO, bestFitPSO, convPSO, timePSO] = runPSO(fitFunc, dim, lb, ub, popSize, maxIter); [bestPathMPSO, bestFitMPSO, convMPSO, timeMPSO] = runMPSO(...); % 记录到结构体数组 end每个算法函数返回最优路径、最优适应度、收敛曲线和运行时间。运行20次，把结果保存在cell或者结构体里，最后统一统计均值和方差。这个模式比每个算法跑一次然后截图要严谨得多。
3.2 PSO核心循环的直觉与代码
PSO的核心思想就一句话：每个粒子跟着自己最好的位置和群体最好的位置飞。速度更新公式是：
v = w * v + c1 * rand(size(v)) .* (pbest - x) + c2 * rand(size(v)) .* (gbest - x); x = x + v;其中w是惯性权重，c1和c2分别是个体学习因子和社会学习因子。路径规划里边界处理特别重要，因为粒子坐标一旦超出地图范围，优先截断到边界：
x = max(lb, min(ub, x));速度也要限制最大速度Vmax，不然粒子很可能一步从地图左边飞到右边，路径完全变成乱飞。惯性权重我习惯用线性递减，从0.9降到0.4，迭代早期多飞，迭代后期细搜索。
PSO在对比项目中通常是最快跑通的，但也最容易陷入局部最优。如果地图上障碍物分布形成几个窄通道，PSO的个体很快全部挤到某个局部通道附近，之后再也跳不出来。
3.3 MPSO的改进点主要加在哪三个地方
MPSO不是标准算法，所以实现上我会明确标注自己用的是哪种“改进”。最常见也最容易落地的改进有三种：
第一，增加变异操作。每迭代一轮，对种群中部分粒子按小概率随机重置若干维度，相当于遗传算法里的变异，能维持多样性：
if rand < pm selectDim = rand(dim, 1) < 0.2; x(i, selectDim) = lb(selectDim) + rand(1, sum(selectDim)) .* (ub(selectDim) - lb(selectDim)); end第二，停滞检测。如果全局最优连续10代没有下降，就把全局最差的一部分粒子重新初始化，或者向全局最优附近和随机位置各生成一半，形成局部搜索和全局搜索的混合。
第三，多子群并行。把种群分成几个子群，子群内部独立进化，每隔一定代数让子群之间交换最优信息。这种方式在路径规划里能明显提升成功率，但代码复杂度高一些，运行时间也会增加。
MPSO并不是一定比PSO好，它的价值是当你在多个随机地图上测试时，成功率更高、路径长度方差更小。单独拿某一张地图看，MPSO有时反而比PSO慢，这就是随机性带来的结果。
3.4 TACPSO的时间自适应与混沌策略实现
TACPSO是另外一个基于PSO的变体，重点在“自适应”和“混沌”。在我的代码中，实现了三个策略。
第一，时间自适应惯性权重。普通的线性递减可以改成随迭代次数变化的余弦递减：
w = wMin + 0.5 * (wMax - wMin) * (1 + cos(pi * t / maxIter));这样前期较大、后期较小，但变化过程更平滑。学习因子也可以随时间反向调整，前期加强个体探索，后期加强社会收敛：
c1 = 2.5 - 2 * (t / maxIter); c2 = 0.5 + 2 * (t / maxIter);第二，混沌映射初始化种群。普通随机初始化的粒子分布往往会偏向地图某些区域，导致初始路径质量差异很大。用Tent混沌映射生成初始位置，可以让粒子在搜索空间内分布更均匀。初始化代码可以这样理解：
xInit = zeros(popSize, dim); chaosVal = rand(1, 1); for i = 1:popSize chaosVal = tentMap(chaosVal); % 比如小于0.5则乘以2，否则乘以2*(1-chaosVal) xInit(i, :) = lb + chaosVal * (ub - lb); end第三，混沌局部搜索。在每次迭代求完全局最优后，用混沌序列在gbest附近生成若干候选点，如果候选点更优就替换。这一招对逃离局部最优特别有帮助，但要注意混沌扰动幅度不能太大，否则后期会把已经找到的好路径破坏掉。
3.5 SOA的迁徙与捕食阶段怎么建模
海鸥优化算法模仿的是海鸥迁徙时的群体移动和攻击捕食时的螺旋运动。迁徙阶段核心是避碰、向最优个体靠近、保持更新，攻击阶段则是在最优个体附近做螺旋搜索。
简化后的位置更新逻辑是：
% 计算海鸥个体向最优解移动 D = abs(globalBest - x); A = C * x; % 避碰项，C为随机衰减系数 M = B * (globalBest - x); % 向最优移动 Dm = A + M; % 攻击阶段，螺旋更新 Dx = Dm .* cos(theta); Dy = Dm .* sin(theta); Dz = Dm .* theta; xNew = globalBest + Dx + Dy + Dz;这里的theta通常是一个随迭代次数增加的螺旋角度，B和C是控制搜索步长的参数。SOA最大的问题是参数没有PSO那么成熟，很多人直接照搬网上代码，结果路径总是越界。我的经验是先限制搜索半径，让每一步移动量不超过地图对角线长度的20%，同时把越界粒子重新放到地图内随机位置而不是截断到边界，否则螺旋运动会积累大量边界粒子。
3.6 GA实现里容易被忽略的细节
GA在路径规划中用的是实数编码，不是二进制编码。选择可以采用锦标赛选择，每次随机挑两个个体，保留适应度好的。交叉不建议直接按维度单点截断，因为路径中间点坐标和坐标之间有关联性，最好用算术交叉：
child1 = alpha * parent1 + (1 - alpha) * parent2; child2 = alpha * parent2 + (1 - alpha) * parent1;alpha可以取0到1之间的随机数。这种交叉方式在连续变量上更自然，后代不会突然生成一个偏离父母很远的路径点。变异用高斯扰动，以一定概率给某些维度加上零均值小方差的高斯噪声。
GA还有一个关键点是精英保留，直接把每代最优个体复制到下一代，否则很容易出现反复震荡不收敛。路径规划里GA经常出现的问题是前期收敛慢，如果最大迭代次数只有50，GA可能还没找到完整无碰撞路径就结束了。所以GA在对比时更适合看它“最终能否找到更短的路径”，而不是看收敛快慢。
3.7 结果记录与绘图输出
每个算法跑完，记录三样东西：最优路径坐标、每一代的全局最优适应度、运行时间。运行结束后，绘制三张图：
- 第一张：在栅格地图上画出每个算法最终规划出的路径，可以分subplot展示。
- 第二张：所有算法的收敛曲线叠加在同一坐标轴，横轴迭代次数，纵轴适应度。
- 第三张：多次运行后的路径长度箱线图，这比单纯一条曲线更能说明稳定性。
绘图代码没什么难度，难的是地图坐标和路径坐标保持一致。我通常先画地图和障碍物，再hold on，然后按路径点顺序plot连线，这样视觉上最直观。
4. 对比结果怎么看：路径图、收敛曲线和稳定性指标
4.1 评价指标不能只看一条路径
单个地图单次运行的结果偶然性太大，甚至可以说没有太大参考价值。规范的对比实验要把每种算法独立运行20次或30次，然后统计：
- 平均路径长度：路径质量的核心指标。
- 最优路径长度和最差路径长度：反映波动范围。
- 平均收敛代数：反应该算法的收敛效率。
- 平均运行时间：在路径质量接近时才有对比意义。
- 成功率：找到从起点到终点的无碰撞路径的次数占比，这个指标最容易被忽略。
如果某算法跑了很多代都找不到一条有效路径，说明它在当前地图上收敛能力不足。对比时我建议以成功率优先：成功率差不多的算法，再比较路径长度和消耗时间。否则一个算法虽然路径最短，但20次只有3次成功，这种优势没有实际意义。
4.2 收敛曲线图里能读出的三件事
把收敛曲线放在同一张图里后，重点看三件事：
第一，初始值的高度。初始适应度代表种群初始路径的质量。如果某个算法初始值特别高，说明初始化策略不好，或者种群随机粒子大多落在障碍区附近。第二，下降速度。PSO和TACPSO通常下降很快，GA则相对缓慢，SOA前期可能波动很大。第三，是否存在平台期和突变。如果PSO的收敛曲线是一条水平线，说明所有粒子已经聚集到同一个局部最优附近，之后没有有效更新。MPSO或TACPSO的曲线如果在中后期出现明显下降台阶，说明变异或混沌策略帮助算法跳出了局部陷阱，这是改进算法的价值体现。
4.3 路径图怎么看才不踩坑
很多人看路径规划结果图只关心“路径是否绕过了障碍物”，这是基本要求。更值得比较的是：
- 路径是否紧贴障碍物边缘。如果路径离障碍物太近，机器人实际执行时会因为定位误差撞上障碍，需要提前对障碍物栅格做膨胀处理。
- 转折点数量。路径长度接近时，转折点越少越好，因为移动机器人转弯代价通常很高。
- 是否出现反折、回绕。个别算法为了绕一个小障碍物可能走出一个回头弯，这种路径虽然能到终点，实际不可用。
对比时尽量把不同算法路径用不同颜色画在同一张地图上，否则单独看每个子图很难建立直观优劣判断。
4.4 我遇到过三种反直觉现象
第一种，GA路径反而比PSO更短。原因是PSO快速收敛到“看起来还不错”的局部路径，而GA因为收敛慢，反倒有更多机会探索到另一侧的大通道。
第二种，SOA一开始基本找不到有效路径，但最后突然跳出一跳大幅下降的收敛曲线，最终路径质量相当不错。SOA的螺旋搜索方式让它在前期看起来像“乱走”，但全局勘探能力确实强。
第三种，MPSO加变异之后，在障碍密集地图上的成功率显著提高，但在简单地图上路径长度反而不如PSO稳定。这说明改进算法不是万金油，地图越复杂，收益越明显。
5. 对比过程中最常见的五个坑和排查技巧
5.1 粒子维度不统一导致“Index exceeds matrix dimensions”
这是最让人头大的报错。通常原因是写适应度函数时把中间点数量搞混了。比如粒子维度是20，对应10个中间点，但解码时却按11个点来解，索引自然超界。排查思路很简单：在适应度函数第一行打印size(x, 2)和numPoint，确认为2倍关系，再往后执行。另外要注意，粒子更新后的坐标可能带小数，小数坐标落到哪个栅格要统一用round还是floor，不同处理方式会影响碰撞检测结果。
5.2 碰撞检测漏判路径穿障碍
路径如果从障碍物的“角”附近斜着切过去，只在整点采样可能检测不到碰撞。解决办法是提高采样密度，沿线步长取0.1格。但步长太小计算量会暴涨，一个折中方案是先判断线段与每个障碍格的包围盒是否相交，相交再做精细采样。如果地图障碍物不是特别多，也可以直接把障碍物膨胀一圈，让路径离障碍物边缘更远，反而让碰撞检测更省心。
5.3 所有算法都找不到有效路径
不要急着怀疑算法，先看地图本身是否连通。最简单的方法是在规划前用A*或BFS快速判断起点终点是否连通，如果不连通，那智能算法跑到天荒地老也没用。另一个原因是适应度函数里碰撞惩罚太大，导致无效个体和有效个体之间适应度差距过大，算法把所有计算资源都花在给无效路径“消障”上，效率反而低。把碰撞惩罚设置成“路径穿过障碍物网格数乘以一个固定系数”，比单纯设成一个大常数更合理。
5.4 Matlab运行时间越来越长，怎么定位瓶颈
建议在代码里分段计时：
disp(['初始化耗时：', num2str(toc(t0))]); t1 = tic; % 适应度计算大多数慢都在适应度函数里的循环判断。向量化的关键是提前生成障碍物索引矩阵，将采样点坐标直接映射为下标，避免在循环里反复访问map。另外，如果每种算法都单独调用一次总绘图代码，绘制200代收敛图也会卡，建议对比时先只保存数据，最后统一绘图。
5.5 地图坐标轴上下颠倒导致路径“穿墙”
前面提过imagesc默认y轴反向。如果不统一，路径明明没有碰障碍，视觉上却像穿过障碍物。建议绘图加上axis xy;，同时把格子中心坐标偏移算好。还有一个常见小坑：画障碍物网格时，如果直接用pcolor(map)，格子填充边界和坐标中心容易对不齐，我习惯用rectangle画每个障碍格，虽然代码多几行，但视觉上最可靠。
5.6 问题排查速查表
| 现象 | 可能原因 | 排查方法 |
|---|---|---|
| 程序报维度错误 | 中间点数与粒子维度不匹配 | 检查dim = 2 * numPoint和解码函数 |
| 路径明显穿过障碍物 | 坐标轴映射错或采样步长过大 | 加axis xy，缩短采样步长 |
| 收敛曲线完全是一条直线 | 全局最优长期未更新，种群过早停滞 | 增加变异/停滞重启机制 |
| 各算法路径长度差距不稳定 | 运行次数太少，随机性未消除 | 每种算法至少跑20次 |
| 运行时间过长 | 适应度函数循环过多 | 向量化碰撞检测，提前生成障碍索引 |
6. 从对比代码到实际项目，还能怎么继续扩展
这套基于二维栅格地图的对比代码，本质上是验证五种智能算法在一个固定地图上的全局搜索能力。它离真实机器人导航还有一段距离，但扩展空间很大。
可以把固定地图改成随机地图批量测试。写一个脚本，每次随机生成障碍率不同的地图，让五种算法依次跑一遍，统计成功率随障碍率变化的曲线。这个数据比单独的路径截图更能说明算法在复杂环境下的鲁棒性。
可以加路径平滑后处理。智能算法输出的是折线段，如果直接发给机器人，会出现明显速度波动。对最优路径的中间点用B样条曲线或贝塞尔曲线做平滑，同时检查平滑后的曲线是否碰撞。这一步在实际项目中几乎必须做。
也可以引入混合思想。先用A*或Dijkstra生成参考路径骨架，再用PSO或者GA在这个骨架附近做优化，相当于把精确搜索和全局搜索结合起来。很多实际部署的导航栈就是这么干的，单靠智能算法面对大规模栅格地图时计算成本太高。
我在实际使用中最深的体会是：对比实验最忌讳“只保留最好的结果”，一定要把每次运行的数据都存下来。路径规划是随机优化过程，一次成功不能说明任何问题。从PSO到GA，从基础版本到改进版本，算法各自都有适合的地图和参数区间。你要真想判断谁更好，别盯着一条路径截图，先看20次运行的成功率和方差。代码写得再漂亮，数据不扎实，最终也只能骗自己。
