简介一份基于粒子群优化算法PSO的机器人最优路径规划Matlab代码包主要面向本科、硕士阶段的算法教学与仿真研究。内容覆盖二维障碍环境下的路径建模、粒子编码、适应度设计、迭代优化与结果可视化可帮助读者在较短时间内掌握粒子群算法在路径规划中的实际应用。压缩包共14个文件整体大小约7.65MB包含6个m功能脚本、4张仿真结果图、1份PDF研究论文、1份markdown说明文档以及txt使用说明。m脚本提供从地图创建、初始解生成、约束处理到解解析与绘图输出的完整链路png图片可直观对照不同场景下的路径规划效果PDF与md文档则从原理和代码两个维度辅助理解算法细节。已有137人学习配套文件不仅可以直接运行复现结果也适合在此基础上做参数调优、对比实验或向三维空间路径规划做扩展具有较好的教学与二次开发价值。1. 把 PSO 用在机器人路径规划先想清楚要优化什么很多人拿到「基于粒子群优化算法的最优机器人路径规划」的 Matlab 代码包第一反应是跑一遍看到那条从起点绕过障碍到达终点的红色折线就认为任务完成。但粒子群优化算法在这一场景中并不是一个「绘图工具」而是一个在黑箱空间里找极小值的优化器路径规划则是把「路径质量」翻译成数值度量的过程。两者之间的接口如果没理解透后面调参数、换地图都会踩坑。我一般会把这类项目拆成三块地图与路径编码、适应度函数、PSO 更新循环。机器人在栅格地图上从起点到终点有无数条候选路径粒子群做的事情是用一小群粒子在候选解空间里反复搜索让路径长度、安全距离、平滑度等指标越来越小。这篇博客就从这三块往下讲最后给出一段能改参数重跑的 Matlab 实现思路。适合已经会跑仿真、但想真正看懂 PSO 路径规划代码的人也适合想从 A* 换成群体智能算法做离线规划的场景。2. PSO 的核心机制与栅格路径的编码方式2.1 速度-位置模型为什么要反着看标准 PSO 里每个粒子代表一个候选解位置向量 X_i 就是解本身速度向量 V_i 控制下一轮解往哪移动。更新公式是V_i(t1) w V_i(t) c1 r1 (P_i - X_i(t)) c2 r2 (P_g - X_i(t)) X_i(t1) X_i(t) V_i(t1)第一项是上一次速度的惯性第二项把粒子拉向自己历史最优 P_i第三项把粒子拉向群体最优 P_g。在机器人路径规划里很多讲原理的书会把公式念一遍然后直接跳到代码。但真正用得到的地方在于「你希望粒子往哪个方向飞」w 负责保留多大程度的原飞行趋势c1 和 c2 负责粒子在个体经验和群体经验之间如何取舍。理解成「反着看」是指不要盯着粒子的运动轨迹而要盯着 P_i 和 P_g 的变化路径如果 P_g 长时间不更新说明搜索陷入某个局部极小接下来要判断是搜不到位还是适应度函数本身把不可行的路径也判成了低代价。在此基础上路径规划问题里粒子位置的含义不是固定的。同一个 PSO 框架换一种编码方式搜索空间的大小和难度完全不同。机器人路径规划的常见做法是把连续环境离散成栅格地图然后让粒子的每一维对应路径上的一个控制点偏移量。下面说编码。2.2 栅格地图与路径编码把粒子变成一条路径栅格地图是一个二维矩阵0 表示可通行1 表示障碍物。假设地图尺寸是 rows x cols起点 S 和终点 G 都在自由栅格上。最简单粗暴的编码是把路径点序列的 x、y 坐标全部展平成一个向量粒子维度就是 2 乘以路径点数量。这种做法的问题很明显路径点顺序一旦被速度更新打乱解码出来的折线会来回扭曲而且路径点可能飞到障碍内部适应度函数要承担大量「无效路径」的惩罚。所以更稳的编码方式是固定路径点数量并且只优化横向或纵向偏移。以起点到终点的直线为基准在直线上等距取 N 个中间点每个点的 x 坐标固定为插值位置粒子每一维只控制该点的 y 方向偏移。这样做维度从 2N 降到 N同时保证路径点沿起点到终点方向单调推进不会出现回头看、反复绕圈这类无意义路径。下面这段解码逻辑是所有后续代码的地基n length(offsets) 2; xs linspace(startPt(1), endPt(1), n); % x 方向固定插值 ys linspace(startPt(2), endPt(2), n); % 基准线的 y 坐标 ys(2:end-1) ys(2:end-1) offsets; % 加上粒子提供的偏移量 ys min(max(ys, 1), size(map, 2)); % 越界截断到地图范围这段代码的关键参数有三个offsets 的维度、limit 偏移范围和地图大小。offsets 里的值控制在 [-limit, limit] 之间limit 一般取栅格数的 10% 到 30%否则粒子要花大量迭代把偏移量从大范围拉回到合理通道里。xs 和 ys 的长度是 N2因为包含起点和终点起点终点不能动所以只对中间 N 个点加偏移。越界截断虽然简单但会让很多粒子贴在地图边缘导致这部分搜索失效后面的越界处理会给出替代方案。编码方式直接决定搜索空间大小。如果地图是 20x20取 N8粒子维度是 8搜索空间在 [-5,5]^8 这样一个超矩形内和直接编码 16 维坐标相比覆盖率天差地别。这也是为什么不少 PSO 路径规划代码看起来迭代很快实际上是把搜索空间压窄了。2.3 适应度函数路径长度优先还是安全优先适应度函数是 PSO 与路径规划问题的连接点。它必须同时回答两个问题这条路够不够短这条路能不能走。常见定义是fitness L_path lambda * CollisionPenaltyL_path 是所有相邻路径点之间的欧氏距离之和CollisionPenalty 是路径段穿过障碍的次数或程度lambda 是惩罚系数。这个公式看起来简单但 lambda 的量级经常被忽略。如果地图尺寸是 20x20一条合理路径的长度大约在 25 到 30 之间如果 CollisionPenalty 每撞一次只给 10那么穿过一个障碍换来的路径缩短可能比绕行更「划算」最终会得到一条穿墙路径。反之 lambda 取 1000碰撞罚分远大于路径长度粒子一旦碰墙基本就不会被选为最优搜索压力集中在可行通道内。我习惯把惩罚项做成碰撞采样点数。每条线段内均匀采样若干个点检查采样点所在栅格是否为障碍命中一个障碍点罚 100 或 200。这样比只判断路径点是否在障碍格内更严格因为一条线段完全可能从障碍角上切过而两端点都在自由格内。下表是三种常见适应度设计的取舍| 设计方式 | fitness 构成 | 适合场景 | 参数注意 | | 长度优先 | 仅 L_path碰撞时赋 inf | 空旷地图 | 地图一复杂就容易无解 | | 安全优先 | L_path lambda * CollisionCount | 障碍密集地图 | lambda 要远大于单段路径长度 | | 平滑度混合 | L_path alpha * 转角惩罚 lambda * CollisionCount | 轮式机器人 | alpha 按转弯半径归一化 |选哪种策略取决于地图比例。我一般在简单地图上用第三种取 alpha0.1 左右让路径既不过度绕弯也不会出现夸张的直角折线。如果只需要交作业长度优先加碰撞罚是默认答案。3. Matlab 实现 PSO 路径规划的核心代码3.1 最小骨架地图、参数和粒子初始化我通常把代码拆成三个函数职责如下表地图变了不需要碰主循环函数职责输入输出init_map生成和显示栅格地图地图尺寸、障碍列表map 矩阵evaluate_path解码路径并计算适应度offsets、起点终点、mapfitness, pathX, pathYpso_path_plannerPSO 主循环地图、PSO 参数gbest, 收敛曲线第一步是设定地图和 PSO 参数。地图这里用手工写障碍块的规则实际使用可以把图片导入并二值化。mapSize [20 20]; map zeros(mapSize); map(4:7, 5:9) 1; % 第一个矩形障碍 map(12:15, 10:12) 1; % 第二个矩形障碍 map(3:5, 15:17) 1; % 第三个矩形障碍 startPt [2 2]; endPt [18 18]; N 40; % 粒子数量 D 10; % 中间路径点数量 maxIter 150; % 最大迭代代数 wMax 0.9; wMin 0.4; c1 1.5; c2 1.5; limit 6; % 偏移量边界单位栅格 rows mapSize(1); cols mapSize(2);参数里最容易出问题的是 D 和 limit 的关系。D 过大时路径点之间的间距变小粒子维度增高但单个偏移对路径形状的贡献也被稀释收敛速度明显变慢limit 过小时路径只能在基准线附近小幅摆动如果起点终点连线被障碍物隔开粒子根本没有机会绕到障碍物另一侧。我的经验是 limit 至少取起点到终点直线距离的四分之一。接着初始化粒子群positions -limit 2 * limit * rand(N, D); velocities zeros(N, D); pbest positions; pbestFitness inf(N, 1); for i 1:N pbestFitness(i) evaluate_path(positions(i,:), startPt, endPt, map); end [gbestFitness, bestIdx] min(pbestFitness); gbest positions(bestIdx, :);初始化时没有把粒子约束在「起点终点直线附近」之外的空间里靠的是 evaluate_path 内部的越界截断。随机均匀初始化能让粒子覆盖搜索域但如果地图障碍复杂初始粒子里大概率全部是碰撞路径此时 pbest 的罚分值很高需要几代迭代才能把部分粒子拉入可行走廊。所以不必因为初始适应度全是几百就认为程序写错了先看 gbest 是否随迭代下降。3.2 evaluate_path适应度计算与碰撞检测适应度函数是本项目里最需要抠细节的地方。我的实现方式是每段路径均匀采样十个点检查这些采样点所在栅格是否为障碍命中一次累积一个碰撞计数最后用 lambda 放大。function [fitness, pathX, pathY] evaluate_path(offsets, startPt, endPt, map) n length(offsets) 2; xs linspace(startPt(1), endPt(1), n); ys linspace(startPt(2), endPt(2), n); ys(2:end-1) ys(2:end-1) offsets; ys min(max(ys, 1), size(map, 1)); % 计算路径长度 segLen sqrt(diff(xs(:)).^2 diff(ys(:)).^2); L sum(segLen); % 逐段碰撞检测 samplesPerSeg 10; collisionCount 0; for k 1:n-1 ts linspace(0, 1, samplesPerSeg); sx xs(k) (xs(k1)-xs(k)) * ts; sy ys(k) (ys(k1)-ys(k)) * ts; rx round(sx); ry round(sy); valid rx 1 rx size(map,1) ry 1 ry size(map,2); if any(valid) collisionCount collisionCount sum(map(sub2ind(size(map), rx(valid), ry(valid)))); end end lambda 200; fitness L lambda * collisionCount; pathX xs; pathY ys; end几个容易忽略的细节round 会把路径点映射到最近栅格采样点落在障碍边界时按障碍处理偏保守sub2ind 的索引要先用 valid 滤一遍否则越界会直接报错collisionCount 累加的是障碍采样点的数量不是障碍物数量这样能把「擦边而过」和「穿心而过」区分开。lambda 固定为 200 是因为在这个 20x20 地图上绕行一段大约增加 3-5 个单位的路径长度而穿墙一次要罚 200才能保证安全优先级高于长度。地图换大以后lambda 要随地图尺寸同步放大否则障碍附近会出现贴近墙角的危险轨迹。3.3 主循环速度更新、越界处理与 gbest 记录核心迭代循环如下for iter 1:maxIter w wMax - (wMax - wMin) * iter / maxIter; % 线性递减惯性权重 for i 1:N r1 rand(D, 1); r2 rand(D, 1); velocities(i,:) w * velocities(i,:) ... c1 * r1 .* (pbest(i,:) - positions(i,:)) ... c2 * r2 .* (gbest - positions(i,:)); positions(i,:) positions(i,:) velocities(i,:); % 越界反射比截断更温和 for d 1:D if positions(i,d) -limit positions(i,d) -limit (-limit - positions(i,d)); elseif positions(i,d) limit positions(i,d) limit - (positions(i,d) - limit); end end [f, ~, ~] evaluate_path(positions(i,:), startPt, endPt, map); if f pbestFitness(i) pbest(i,:) positions(i,:); pbestFitness(i) f; end if f gbestFitness gbest positions(i,:); gbestFitness f; end end if mod(iter, 50) 0 fprintf(iter %3d, gbest fitness %.2f\n, iter, gbestFitness); end end速度更新公式里的 r1 和 r2 每次迭代重新随机目的是让搜索不至于完全确定性地朝着 pbest 和 gbest 移动。这里用 r1 和 r2 把列向量转成行向量是因为 positions 是 N x D 的矩阵循环里每次处理一行。越界反射处理比简单截断好用的原因在于反射后粒子仍在有效区间内而且速度方向被反转下一轮不会继续执着地冲出边界截断会把边界变成粒子堆积点早期搜索时很多粒子挤在 limit 边缘等于白白浪费种群容量。主循环结束后gbest 就是最优的偏移向量把它传给 evaluate_path 得到 pathX 和 pathY用 plot 画出来即可。这部分代码没有做早停但在 maxIter 足够大时gbest 后期基本不再更新。4. 参数怎么调惯性权重、学习因子与种群规模4.1 先看粒子运动再设参数很多人改 PSO 参数只看最终路径长度忽略搜索过程中的粒子分布。其实从图上看粒子群有几个典型状态所有粒子快速聚到某个点说明 c2 太大群体早熟粒子满地图乱飞且 gbest 停滞说明 w 太大或者 c1 过大粒子各自为政前几十代适应度快速下降后面几乎不动说明搜索已经从全局过渡到局部此时应该检查是不是陷入局部障碍死角。参数调整思路应该是「先范围后细调」。先把种群 N 固定在 40跑三次随机种子看最优路径是否一致如果三次结果相差很大说明搜索空间覆盖不足优先加大 N 或 D而不是去调 c1、c2。如果三次结果一致但路径明显绕远说明收敛到了一个局部最优这时再把 w 的初始值调高到 1.0或者给速度更新加入变异项。4.2 一张可以直接套用的参数表下表是我在 20x20 左右栅格地图上常用的默认参数组合可以直接作为起点参数默认值调节方向影响种群数 N4030-80覆盖率和单代耗时路径点数 D108-15路径自由度与维度惯性权重 w0.9 - 0.40.4-1.0全局搜索与局部收敛的平衡学习因子 c1/c21.5 / 1.51.2-2.0个体经验与群体经验的权重偏移上限 limit62-10搜索空间宽度碰撞罚 lambda200100-1000安全与长度的优先级先用表中参数跑通再按地图复杂度调整。障碍密集时把 lambda 调大简单地图上把 lambda 调小到 50 可以让路径更贴近障碍物边缘从而缩短长度但贴近边缘属于危险驾驶只在仿真里演示可以真机建议保持 lambda 在 300 以上。4.3 三个容易让结果变「好看」但很危险的改动第一个是去掉越界处理任由粒子飞到地图外。这时适应度函数为了防止越界报错往往会把越界后的坐标截断回边界导致路径出现大量沿地图边缘的线段视觉上路径确实绕开了障碍但实际没法用。第二个是只检查路径点而忽略路径段障碍比较小或路径点稀疏时路径会从障碍格之间斜穿过去plot 出来是一条没有穿过黑色格子的直线但展开看每一段的中间点都在障碍里。第三个是过早结束迭代路径看起来还没碰墙再迭代几代可能找到更短绕行但很多人为了图快用固定最大迭代数收住。想要快速判断这三种情况可以在每次迭代后保存 gbest 路径最后用动画回放。如果看碰撞计数已经降到 0但路径形状还在剧烈变化说明还没收敛如果碰撞计数一直是 0 且路径长度稳定再继续迭代的意义就不大了。4.4 把标准 PSO 改成不易早熟的版本前面提过线性递减权重它已经能把搜索从全局逐步过渡到局部。但路径规划地图中的局部极小非常多标准 PSO 仍可能卡在某个障碍凹槽里。最简单的两种改进编码量都不大。第一种是自适应权重。记录连续若干代 gbestFitness 没有变化就把 w 拉回到 0.8让粒子群重新获得大范围搜索能力回头找到更好的区域后再线性下降。第二种是混沌初始化。将 rand(N,D) 替换为 logistic 映射生成的序列x rand(N, D); for k 1:20 x 4 * x .* (1 - x); end positions -limit 2 * limit * x;混沌让粒子分布相对均匀避免随机初始化时的聚集效应。这两种方法对最终路径长度的改善通常在 3% 到 8%但对收敛稳定性的提升更明显至少不会出现每次运行结果忽长忽短的问题。也可以直接把 PSO 的结果作为种子再用 A* 做一次局部修正不过这是另一个话题这里不展开。5. 输出实机路径前先做路径平滑与栅格回放校验5.1 用三次样条把折线变成可执行轨迹上一章得到的路径本质上是一条由 N2 个点连成的折线。差速机器人可以原地转向对这种路径容忍度很高但对阿克曼底盘或类车机器人折线转角过大会直接导致跟踪失败。常见做法是拿 gbest 路径的路径点做三次样条插值得到平滑曲线后再重新检测碰撞。[~, pathX, pathY] evaluate_path(gbest, startPt, endPt, map); t 1:length(pathX); tt linspace(1, length(pathX), length(pathX) * 10); sp spline(t, pathX, tt); py spline(t, pathY, tt); % 对 sp, py 重新采样并跑一遍碰撞检测样条插值有个副作用是会让曲线在障碍角附近「切过去」造成安全和美观的冲突。我的习惯是插值后把碰撞点附近的控制点密度提高一倍或者直接把插值 t 的间隔缩小让曲线更贴近原始折线。验证标准很简单平滑后的曲线上每隔 0.1 个栅格取一个采样点检查对应栅格值是否全部为 0。5.2 保存 waypoints 前做三项校验从仿真到实际部署我会在保存 waypoints 前过三个检查第一逐段重新跑碰撞检测特别注意样条之后的曲线第二检查相邻路径点距离如果小于机器人车身尺寸说明路径点过密实机控制器会出现抖动第三把路径起点终点和地图边界对齐一遍避免浮点精度导致起点实际落进障碍。下面是一小段简单的距离检查minDist min(sqrt(diff(pathX).^2 diff(pathY).^2)); if minDist 0.5 warning(路径点过密需要抽稀); pathX pathX(1:5:end); pathY pathY(1:5:end); end这一套「PSO 离线规划 样条平滑 碰撞再校验」是比较通用的收尾方式。最终输出给导航模块时我会把平滑后的点保存成两列 txt或者直接进 ROS 的 waypoint 话题而不是把粒子偏移量本身发出去。路径规划全程在 Matlab 里闭环用同一套 evaluate_path 函数检验搜索阶段和实机阶段对安全性的理解就不会出现偏差。本文还有配套的精品资源点击获取