简介面向MATLAB路径规划学习与仿真验证场景的资源包基于A-star算法实现多机器人协同路径规划可帮助智能优化算法、移动机器人避障方向的初学者快速搭建可运行的实验程序也适合作为课程设计或科研对比的参考实现。压缩包共8个文件以6个MATLAB脚本为主包含主程序与核心函数模块覆盖开放列表维护、节点拓展、距离计算等关键流程另附说明文档与1张结果图无需额外配置即可理解完整的算法调用关系整体仅41KB便于下载与传播。目前已有881人学习浏览代码经博主亲测可用在MATLAB 2019b环境下直接运行主程序即可获得规划结果。读者可以从中获得完整的A-star多机器人路径规划源码、函数调用框架以及可视化结果在此基础上替换地图或目标点即可快速验证不同场景下的避障效果尤其适合入门级用户学习与实践也可作为路径规划课程设计、毕业设计或竞赛复现的实用素材。1. 车间里两台 AGV 在同一格路口相遇的那一刻A* 才真正开始工作单机 A* 给出的是一条从起点到终点的最短路径它只对一个机器人负责。当车间里同时跑着五六台 AGV它们各自独立调用 A*把路径合到同一张栅格地图上时冲突会立刻暴露两台车同一时刻要占据同一个栅格或者在一个窄通道里迎面顶死。单机寻路越“最优”合在一起反而越容易死锁因为每台车都选了同一条最短走廊。多机器人路径规划真正要解决的问题不是“怎么走最短”而是“多个最短相互冲突时怎么协调”。这篇文章讨论的是一个能在 Matlab 里完整跑通的验证方案A* 负责底层寻路上层用时间冲突检测和优先级调度解决机器人之间的干涉。阅读对象是正在做 AGV 调度仿真、仓储物流课题或需要在自己项目里植入多机路径规划判断逻辑的工程师。2. A-star 的栅格实现从估价函数到 Matlab 最小函数2.1 为什么多机器人路径规划仍然选 A*而不是 Dijkstra路径规划发展到今天可选算法很多快速随机搜索树、跳点搜索、D* Lite 各有场景但多机器人系统里 A* 依旧是默认起点。原因有三个其一栅格地图离散化之后A* 在中等规模网格上能得到全局最优这对多机系统很重要因为后续的冲突协调依赖“每台车都尽量走在可信路径上”其二A* 的启发式是可调的你可以在完整性和实时性之间滑动这在多机动态调度里非常实用其三也是最容易被忽视的A* 天然返回一条有序的栅格序列这条序列稍加扩展就能变成带时间戳的轨迹而多机冲突检测需要的恰恰是时空数据不是几何数据。核心公式是那个极简的表达式f(n) g(n) h(n)。g(n) 是从起点到当前节点 n 的真实代价h(n) 是 n 到目标的估计代价。当 h(n) 始终不大于真实剩余代价时A* 保证找到最优路径这个性质叫可采纳性。启发函数的选择直接影响扩展节点数量和路径形状。四邻域移动的地图用曼哈顿距离即可但如果你允许八方向移动曼哈顿距离会因为忽略对角步长而高估代价可能让 A* 退出最优解。八邻域栅格的对角距离才是匹配的启发式function h diagonalDistance(node, goal, D1, D2) dx abs(node(1) - goal(1)); dy abs(node(2) - goal(2)); h D1 * max(dx, dy) (D2 - D1) * min(dx, dy); endD1 是直线步代价值D2 是对角步代价值。在 8 连通栅格里通常设 D11、D2sqrt(2)这样 h 与实际移动代价在同一尺度上A* 扩展的节点数最少。很多 Matlab 示例图省事直接给 h 乘一个权重如果地图本身允许对角线穿越这种做法会在多机场景里带来隐患——两个机器人的路径会沿着障碍边缘出现微小的锯齿时间冲突的判定窗口不稳定。2.2 搭建栅格地图稀疏矩阵存障碍结构体数组存节点Matlab 里做 A*地图用逻辑矩阵最能减少内存浪费true 表示障碍false 表示可通行。地图文件推荐用 CSV 导入而不是硬编码在脚本里因为后续做多机仿真时你需要快速测试不同障碍密度对冲突率的影响。map ones(20, 30); % 1 表示可通行 map(5:8, 12:16) 0; % 0 表示障碍区域 map(14:17, 4:9) 0;这段代码建立了一个 20x30 的栅格两处矩形障碍。用 ones 初始化再局部置零比直接列出所有障碍点要直观。如果你的实际地图来自 CAD 或 SLAM 栅格通常需要做一个降采样因为 A* 的时间和内存都随地图规模非线性增长多机场景还要留出余量给冲突检测。Open list 和 closed list 的实现方式决定 A* 的瓶颈。演示级代码可以直接用结构体数组加循环遍历寻找最小 f 值但 200x200 以上网格就会明显卡顿。我习惯用一个变通做法用 containers.Map 存节点状态用排序后的数组列维护 open list排序只发生在压入新节点时配合物理内存预分配足够应付 200x300 以内的地图。再大规模就应考虑 C 的 priority_queue 或者利用 Matlab 的 mex 接口但那是另一个话题。2.3 一个可运行的 A* 核心函数下面是去掉注释噪音后能直接放进脚本调用的核心函数。它返回路径坐标序列和访问过的节点数方便你评估启发式质量function [path, visitCount] myAstar(map, start, goal) [rows, cols] size(map); openList []; cameFrom containers.Map(KeyType,char,ValueType,char); gScore containers.Map(KeyType,char,ValueType,double); fScore containers.Map(KeyType,char,ValueType,double); sk sprintf(%d,%d, start(1), start(2)); gk sprintf(%d,%d, goal(1), goal(2)); gScore(sk) 0; fScore(sk) diagonalDistance(start, goal, 1, sqrt(2)); openList [openList; start, fScore(sk)]; visitCount 0; dirs [1 0; -1 0; 0 1; 0 -1; 1 1; -1 -1; 1 -1; -1 1]; dirCost [1 1 1 1 sqrt(2) sqrt(2) sqrt(2) sqrt(2)]; while ~isempty(openList) [~, idx] min(openList(:,3)); current openList(idx, 1:2); openList(idx,:) []; if current(1) goal(1) current(2) goal(2) path reconstructPath(cameFrom, current); return; end ck sprintf(%d,%d, current(1), current(2)); visitCount visitCount 1; for d 1:8 nr current(1) dirs(d,1); nc current(2) dirs(d,2); if nr 1 || nc 1 || nr rows || nc cols continue; end if map(nr, nc) 0 continue; end nk sprintf(%d,%d, nr, nc); tentativeG gScore(ck) dirCost(d); if isKey(gScore, nk) tentativeG gScore(nk) continue; end cameFrom(nk) ck; gScore(nk) tentativeG; fScore(nk) tentativeG diagonalDistance([nr nc], goal, 1, sqrt(2)); openList [openList; nr, nc, fScore(nk)]; end end path []; end这段代码里的 containers.Map 是关键点它让每个栅格只保留一个 g 值记录而 openList 第三列存 f 值用于取最小节点。每次扩展时检查八个方向tr 表示有效邻域剔除了地图边界和障碍内的节点。值得注意的一个细节如果 nk 已经在 closed list 里但新路径代价更小这段代码会重新把该节点塞进 open list。有人会用 visited 标记跳过但在某些地图布局下会漏解。单机验证时建议在 30x30 随机障碍地图上对比 D11D21 和 D11D2sqrt(2) 两种配置下的路径长度前者会额外产生 40% 左右的锯齿这在视觉上一个机器人看起来没什么问题多机时间冲突检测会因此频繁误报。3. 多机器人路径规划的冲突类型与两阶段解码策略3.1 时空冲突顶点冲突、边冲突和迎面交错当多个机器人各自计算完几何路径后叠加在同一时间轴上才会暴露冲突。最常见的两种冲突形态第一是顶点冲突两台车同一时刻占据同一个栅格第二是边冲突单位时间步内两台车越过同一条边典型场景是一个机器人从 (3,5) 移动到 (3,6)另一个机器人同时从 (3,6) 移动到 (3,5)没有同时占住一个格子但交换位置时相互顶住。还有一种被低估的冲突是迎面追赶速度快或路径弧更多的机器人在同一走廊内从后方逼近前方机器人两者实际车距低于安全栅格数。考虑障碍物密度超过 25% 的栅格地图时单纯几何路径几乎必然在通道汇合处产生顶点冲突。这就是为什么多机器人问题不能只把 A* 路径画出来就觉得万事大吉你把各机器人的路径画成不同颜色再按时间步同时回放很快能看出它们在十字路口的叠加时间和位置。3.2 先规划后者调度的两阶段策略从业界实现看多机器人路径规划大体分两类解耦式与集中式。集中式方法把全部机器人合并进一个多维状态空间搜索理论最优但状态空间随机器人数量指数膨胀Matlab 里 4 台机器人就很难实时跑完。解耦式方法则是先为每个机器人独立计算路径再用调度策略消除冲突非最优但可接受、可增量、好调试。两阶段策略的第一阶段就是用第 2 章的 A* 为每台机器人算最短路径第二阶段把这些路径放到时间轴上做冲突检测和避让。避让手段有三种等待、绕行、重规划。等待最简单代价是某台机器人停在路径原地上如果等待发生在窄通道入口外通常不会阻塞其他机器人绕行需要对 A* 的代价函数做改造把冲突区域标记为惩罚区重规划则是把当前时刻更新的地图重新喂给 A*适合动态障碍物突然出现的场景。3.3 优先级排序为什么意图明确的先走反而整体更快解耦式多机调度需要一个决定“谁先走”的规则。常见做法是按任务紧急度和路径长度混合排序而不是单纯按机器人编号。路径短的任务先执行往往让总体完工时间更短因为短路径任务能迅速让出一个完整通道给长路径任务腾出空间。在 Matlab 里优先级可以做成一个向量robotTasks struct(id, {}, start, {}, goal, {}, prio, {}); % 假设 robotTasks 已按任务紧急度填充 [~, order] sort([robotTasks.prio], ascend); for i order robotTasks(i).path myAstar(map, robotTasks(i).start, robotTasks(i).goal); end这里排序用 ascend也就是数值小的优先级高先规划先调度。如果所有任务优先级相等可以让最早到达目标的先走因为它的路径占用时间窗最短后续冲突检测的迭代次数更少。有一点要注意优先级排序是静态的适合任务在运行前全部确定的情况如果任务运行过程中动态插入新任务静态优先级会导致插入任务永远压后。3.4 时间膨胀法检测冲突的 Matlab 实现检测冲突最直接的办法是为每个机器人建立一张时间-位置表把路径序列展开成每个时刻占据的栅格集合。代码上就是遍历路径给每个路径点加上时刻索引function traj pathToTrajectory(path, startTime, speed) traj []; for i 1:size(path, 1) traj(end1, :) [path(i,1), path(i,2), startTime (i-1)/speed]; end end拿到轨迹后两两比较检查不同机器人之间是否存在时间差小于安全阈值且坐标相同的点对。比两层循环更快的是把轨迹按栅格索引哈希只比较落在同一栅格上的时间记录function conflict findConflict(traj1, traj2, safeTime) conflict 0; tbl containers.Map(KeyType,char,ValueType,any); for i 1:size(traj1, 1) key sprintf(%d,%d, traj1(i,1), traj1(i,2)); if ~isKey(tbl, key) tbl(key) traj1(i,3); else existTime tbl(key); if abs(existTime - traj1(i,3)) safeTime conflict 1; return; end end end for i 1:size(traj2, 1) key sprintf(%d,%d, traj2(i,1), traj2(i,2)); if isKey(tbl, key) abs(tbl(key) - traj2(i,3)) safeTime conflict 1; return; end end end这个函数的设计矛盾在于第二条循环里如果同一路径自身在某一格停留超过一个时间步会被自己误判所以要先单独检查 traj1 内部的时间重叠或者从一开始就规定路径不能有原地等待的节点。safeTime 取多大取决于机器人运动控制精度栅格地图上一个格子的实际边长为 1 米时安全时间建议设为速度倒数的两倍。比如速度是 0.5 m/ssafeTime 取 4 秒。4. 完整主循环与参数如何决定求解质量4.1 主程序结构迭代式冲突消解两阶段策略落地在主循环里是这样一种结构算初始路径检测冲突调整冲突机器人的轨迹再检测直到没有冲突或达到迭代上限。maxIter 50; timeNow zeros(numRobots, 1); for iter 1:maxIter allClear true; for i 1:numRobots traj{i} pathToTrajectory(robot(i).path, timeNow(i), robot(i).speed); end for i 1:numRobots for j i1:numRobots if findConflict(traj{i}, traj{j}, safeTime) lowerPrio max(robot(i).prio, robot(j).prio); timeNow(lowerPrio) timeNow(lowerPrio) waitStep; allClear false; end end end if allClear break; end end这个主循环里 waitStep 是冲突消解的最小时间增量一般设为一个机器人穿过一个栅格所需的时间。循环结束条件如果达到 maxIter 仍未消除全部冲突说明地图瓶颈区域的通过能力不足此时不应该继续加等待时间而应考虑给冲突区域绕行改道。参数之间的制约关系是这里最值得讲的地图障碍密度越高绕行空间越小等待策略占比越大而机器人数量接近地图可通行栅格数的 10% 以上时死锁概率急剧上升单纯等待就很难再解决问题需要触发重规划把停在原地的机器人标记为临时障碍重新跑 A*。4.2 参数表多机器人 A* 需要手工标定的 7 个参数参数典型值作用调整依据地图栅格尺寸50x50决定搜索空间规模越小越精确计算量越大障碍率map 中 0 的占比0.2~0.4决定可行通道宽度超过 0.5 需提高启发权重机器人数量3~8冲突检测的计算量来源每增加一台检测对数按平方增长速度栅格/秒0.5~1时间戳换算基准各机器人速度差异越大越容易冲突safeTime2~4 秒安全时间窗速度慢取大值速度快取小值waitStep1~2等待避让的最小时间片过小导致迭代次数暴增启发函数权重 w1.0~1.2搜索偏向目标的程度多机场景不宜超过 1.2否则路径质量劣化这 7 个参数里启发函数权重实际上很少被多机路径规划的人手动调。因为多机系统性能瓶颈通常不在搜索速度而在冲突消解次数权重提高以后路径虽然更长但所有机器人的路径更散开反而减少了冲突。如果地图只有一条通道连接起点区域和目标区域权重调节毫无意义因为地图结构决定每台车都必须走同一条走廊。4.3 可视化与轨迹回放多机器人路径规划仿真最直观的验证方式是把栅格地图和时间轴同时画出来不同机器人用不同颜色标记每执行一个时间步更新一次绘图。这里有一个让 Matlab 绘图不闪烁的小技巧用 set 命令更新图形对象而不是反复调用 plotfigure(Color,w); hold on; colors lines(numRobots); hnd gobjects(numRobots, 1); for i 1:numRobots hnd(i) plot(robot(i).path(1,2), robot(i).path(1,1), o, Color, colors(i,:), MarkerFaceColor, colors(i,:)); end for t 1:maxTime for i 1:numRobots pos getPosAtTime(robot(i).traj, t); set(hnd(i), XData, pos(2), YData, pos(1)); end pause(0.1); endgetPosAtTime 函数用二分查找在轨迹里找时间戳对应的空间点比线性查找快。如果机器人数量超过 8 台建议减少绘图频率每 3 个时间步更新一次画面否则可视化本身的耗时会影响你判断算法性能。我实际调通多机 A* 的经验和教训是可视化时要额外画出冲突发生的位置比如碰撞前的两帧用红色叉标记。这个标记能直接暴露你 safeTime 设置得是否合理如果大量冲突发生在两台车相距两个栅格时就报警说明 safeTime 取值过大反而会限制地图通行效率。5. 3 个必调参数与一个时间膨胀入队插件多机器人路径规划验证到一定程度你会发现真正决定交付质量的只有三个参数safeTime、waitStep、以及触发重规划的阈值。它们把 A* 从纯几何搜索变成了时序调度工具。建议用一个小脚本做参数扫描固定地图和机器人任务集统计总完工时间与冲突残余次数safeTimeList [1 2 3 4]; waitStepList [0.5 1 2]; for st safeTimeList for ws waitStepList [makespan, conflictLeft] runSchedule(map, robots, st, ws, maxIter); fprintf(safeTime%.1f waitStep%.1f makespan%.2f conflict%d\n, st, ws, makespan, conflictLeft); end end这个双层循环每轮跑全量仿真输出总完工时间。正常结果随 waitStep 增大makespan 线性上升safeTime 影响的是残余冲突数。如果你观察到某组参数下 conflictLeft 不是 0 但 makespan 明显最低说明地图通行能力到达上限不是调参能解决的需要回头改机器人数量或者障碍布局。针对 A* 框架自身一个容易遗漏的地方是路径上相邻节点之间的隐式等待行为。如果两个机器人路径在空间上不重叠但时间走廊交错可以给 f(n) 的 g(n) 部分加时间惩罚项让下一时刻节点代价值包含预期拥堵时间。做法是在第 2 章 myAstar 函数的 dirCost 数组后面加一个外部传入的 timeCost 向量但注意这会破坏启发式的可采纳性绘制所以只作为优化手段而不是默认配置。如果想把精力花在更通用的方案上可以给 3.4 节的路径转轨迹函数增加“停留”功能当机器人检测到前方信号标占用时轨迹里允许连续多个时间步停留在同一栅格与此对应的 conflict 检查函数需要跳过自身重叠判断。这个变通正好模仿实际 AGV 在交叉口的等待行为而把安全判断下放到运动控制器仿真层的负担会小很多。用 Matlab 跑通多机器人 A* 的最终成果形态是一张可以回放的地图、一条总完工时间曲线以及一组经过扫描确认的参数组合而不再是一条孤零零的最短路径。本文还有配套的精品资源点击获取