基于A*算法的网格地图多机器人导航Matlab仿真与实现

最近整理了一个机器人导航的小Demo项目标题是“基于A_Star算法模拟网格地图多机器人导航”配套的Matlab代码也一起放出来了。简单说就是给定一张二维栅格地图用A算法给每个机器人找一条从起点到终点的无碰撞路径然后让它们在仿真环境里依次或同时移动。这个项目表面看起来不复杂但真把它跑起来、把机器人数量从一台加到三台、五台各种问题就冒出来了路径重叠、互相堵死、某台机器人绕远路绕到离谱……这篇文章主要是把整个项目的设计思路、A算法在网格地图里的实现细节、多机器人冲突处理办法以及我调参排坑的过程一次性讲清楚。它适合刚接触路径规划的学生、准备做移动机器人竞赛的小队也适合那些想快速验证A*效果又不想从零造轮子的开发者。1. 项目到底做了什么一个网格地图上的多机器人导航Demo1.1 先说清楚A_Star和网格地图之间的关系网格地图是机器人导航里最常用的环境表达方式之一。把现实空间按固定分辨率切成一个个方块每个格子记录一个状态0表示空闲1表示障碍这就构成了一张栅格地图。A_Star算法做的事情是把这张图看作一张带权图每个格子是一个节点格子之间的通行关系是边然后从起点节点不断向外扩展找出一条代价最小的格子序列。这里有个常见的误区A本身并不知道机器人是什么也不关心机器人长什么样它只负责在格子里搜索。真正让机器人“动起来”的是上层调度逻辑比如多台机器人按什么顺序规划、遇到冲突怎么处理。所以做这个项目时第一步是把地图建对第二步才是写A。很多人一上来就写算法地图没建明白后面所有调试都会乱成一团。1.2 多机器人导航为什么不是把A*跑N遍就完事把A在单台机器人上跑通之后最自然的想法是有三台机器人就调用三次A各走各的最短路径。真这么做了你会发现大概率会撞车。原因很简单单机A*规划时只考虑静态障碍物完全不考虑其他机器人在时间轴上的位置。三台机器人各自的最短路径可能都通过同一个路口如果时间上又恰好重叠那就堵死了。所以多机器人导航的本质是把“空间维度”和“时间维度”一起考虑进去或者至少用某种策略让多台机器人在时间和空间上错开。这个Demo里我采用的策略是顺序规划加简单时间窗先给优先级高的机器人规划路径然后把它的占用信息记录下来后续机器人规划时把这些占用信息作为临时约束必要时等待原地而不是重新绕一大圈。这个思路第四章会详细展开。1.3 这个Demo适合谁能验证什么我整理这份代码时主要面向三类人。第一类是刚接触路径规划的学生可以用它理解A*的完整执行过程。代码里开列表、闭列表、启发式函数、路径回溯这些概念都可以实际跑一遍再对照变量变化理解比看公式快得多。第二类是准备竞赛或者做小车项目的团队需要快速搭一个多机避碰仿真原型验证调度思路是否可行。第三类是手里有路径规划任务、又不想自己从头抠算法的工程师拿到代码改地图、改参数就能用。当然它也有很明确的边界这是一个二维静态栅格地图下的Matlab仿真不涉及真实机器人的里程计、定位、SLAM和底层电机控制。想直接跑到实体小车上还需要很多外围工作但这部分不在今天这篇文章范围内。2. 网格地图建模与A*算法的核心原理2.1 栅格地图建模的细节坐标系、障碍物和膨胀这个项目的输入是一张二维矩阵约定很直接用map zeros(rows, cols)生成全空地图然后把障碍物所在格子置1。坐标顺序是[行, 列]也就是map的第几行第几列路径点也用这个格式表示。矩阵的行列坐标和图像坐标里的xy并不一样但A*只关心节点之间的邻接关系所以用行列坐标完全够用不需要额外转换。需要注意的一点是障碍物膨胀。如果障碍物只占一个格子但机器人实际是有尺寸的那么路径规划出来虽然安全通过相邻格子真实环境中可能根本开不过去。所以建图时要在障碍物周围再扩展一圈不可通行区域比如障碍物在(5,10)就把(4,10)、(5,9)、(6,10)、(5,11)这些相邻格子也标成障碍。膨胀半径取决于机器人尺寸与栅格分辨率的比例这是一个非常容易漏掉的工程细节。2.2 启发式函数怎么选曼哈顿、欧几里得还是对角距离A的估价函数是f(n) g(n) h(n)g(n)是从起点到当前节点的实际代价h(n)是当前节点到目标点的启发式估计值。启发式函数的选择直接决定搜索效率和路径形态。如果h(n)始终不大于真实代价也就是满足“可采纳”条件A保证能搜出最短路径h越接近真实代价扩展的节点越少搜索越快。这个Demo里我用了四种常见距离实际代码默认是第四种启发式公式适用邻域特点曼哈顿距离|dx| |dy|四连通路径呈折线扩展较均匀适合只能上下左右走的场景欧几里得距离sqrt(dx^2 dy^2)任意永远满足可采纳但偏离真实代价较多扩展节点偏多切比雪夫距离max(|dx|, |dy|)八连通且斜走代价为1各方向代价相同时精确用于普通八邻域搜索很稳Octile距离max (sqrt(2)-1)*min八连通且斜走代价为sqrt(2)最贴合斜向移动的实际情况推荐默认使用如果你用四连通邻域建议用曼哈顿距离用八连通邻域则应把斜移动代价设为sqrt(2)并配合Octile距离。比如何从(1,1)到(5,5)曼哈顿距离是8但斜切过去只需要约5.66如果还拿曼哈顿距离当启发式虽然不会出错但搜索范围明显变大网格一多就能感觉到卡顿。2.3 A*主循环的数据结构开列表、代价值与路径回溯代码里我用矩阵直接存储g值和f值用单独的标志矩阵表示某个节点是否在开列表中。对于20乘20这种小地图完全够用真要跑500乘500以上再换成堆结构。主循环的逻辑是从开列表中取出f值最小的节点作为当前节点如果它就是终点就回溯路径并结束否则遍历它的所有可行邻居计算从起点经过当前节点到邻居的临时g值如果这个临时g值比邻居原来的g值小就更新邻居的g值、f值和父节点并把邻居加入开列表。路径回溯用了一个很朴素的方法维护两个矩阵prevR和prevC分别记录每个节点的父节点行坐标和列坐标。到达终点后从终点倒着找父节点直到回到起点再把顺序反过来就是完整路径。这个方法牺牲了一点空间但好处是调试非常方便你可以随时停下来看某个格子是从哪个格子走过来的特别适合学习阶段。3. Matlab实现与核心代码拆解3.1 地图初始化和参数定义我这次提供一个完整的执行流程。首先建图并设置机器人的起点和终点。% demo_main.m —— 多机器人网格地图导航入口 clear; clc; close all; % 地图尺寸20行20列 rows 20; cols 20; map zeros(rows, cols); % 手动设置一些障碍物按 [行 列] 填写 obsIdx [5 5; 5 6; 6 5; 9 12; 9 13; 10 13; 14 3; 15 3; 16 3; 17 8; 17 9; 18 8]; for i 1:size(obsIdx, 1) map(obsIdx(i,1), obsIdx(i,2)) 1; end % 三台机器人的起点终点 robotStarts {[2 2], [2 19], [19 19]}; robotGoals {[19 19], [19 2], [2 2]}; % 邻域模式4表示四连通8表示八连通 neighborMode 8; % 提前为每台机器人缓存路径 nRobots length(robotStarts); paths cell(1, nRobots);这里起点和终点最好不要直接放在障碍物格子里。我调试时遇到过有人把起点放在障碍物上的情况A*循环跑完也返回空路径容易让人误以为算法写错了。3.2 A*核心函数实现接下来是A*主体函数我尽量写得直观没做过重的性能优化方便阅读和修改。function [path, cost] astar_grid(map, startPos, goalPos, mode) % 输入map地图startPos和目标点为[row col]mode为4或8 [rows, cols] size(map); g inf(rows, cols); % g值 f inf(rows, cols); % f值 prevR zeros(rows, cols); % 父节点行 prevC zeros(rows, cols); % 父节点列 startIdx sub2ind([rows, cols], startPos(1), startPos(2)); goalIdx sub2ind([rows, cols], goalPos(1), goalPos(2)); g(startIdx) 0; f(startIdx) heuristicCost(startPos, goalPos, mode); openFlag false(rows, cols); openFlag(startIdx) true; while any(openFlag(:)) % 在开列表中找f值最小的节点 fCopy f; fCopy(~openFlag) inf; [~, currentIdx] min(fCopy(:)); [cr, cc] ind2sub([rows, cols], currentIdx); if currentIdx goalIdx path reconstructPath(prevR, prevC, startPos, goalPos); cost g(currentIdx); return; end openFlag(currentIdx) false; % 相当于加入闭列表 nbrs findNeighbors(cr, cc, rows, cols, mode); for i 1:size(nbrs, 1) nr nbrs(i,1); nc nbrs(i,2); if map(nr, nc) 1 continue; end if abs(nr-cr) abs(nc-cc) 2 stepCost sqrt(2); else stepCost 1; end nIdx sub2ind([rows, cols], nr, nc); tentativeG g(currentIdx) stepCost; if tentativeG g(nIdx) g(nIdx) tentativeG; f(nIdx) tentativeG heuristicCost([nr nc], goalPos, mode); prevR(nIdx) cr; prevC(nIdx) cc; openFlag(nIdx) true; end end end path []; cost inf; end这个版本没有单独维护开列表的堆结构而是每次用min在整张f矩阵里找最小值。好处是代码短、不容易写错坏处是地图大了以后速度会明显下降。如果你只需要在20乘20的地图上跑几台机器人这个写法完全没问题。配套的两个辅助函数也比较关键。邻居生成函数决定了搜索连通方式启发式函数前面已经讲过。function nbrs findNeighbors(r, c, rows, cols, mode) if mode 4 dirs [-1 0; 1 0; 0 -1; 0 1]; else dirs [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; end nbrs []; for i 1:size(dirs, 1) nr r dirs(i,1); nc c dirs(i,2); if nr 1 nr rows nc 1 nc cols nbrs(end1, :) [nr nc]; end end end function h heuristicCost(pos, goalPos, mode) dr abs(pos(1) - goalPos(1)); dc abs(pos(2) - goalPos(2)); if mode 4 h dr dc; else m min(dr, dc); h m * sqrt(2) (max(dr, dc) - m); end end function path reconstructPath(prevR, prevC, startPos, goalPos) path goalPos; cr goalPos(1); cc goalPos(2); while ~(cr startPos(1) cc startPos(2)) nr prevR(cr, cc); nc prevC(cr, cc); if nr 0 || nc 0 break; end path [[nr nc]; path]; cr nr; cc nc; end end这里要注意reconstructPath里如果父节点是0说明路径出了问题直接跳出避免死循环。在调试阶段这种防御性写法能帮你快速定位是搜索失败还是回溯失败。3.3 路径可视化与多机器人移动仿真路径算出来后怎么展示是一个很重要的问题。我的做法是先画地图再把路径点画成折线然后在仿真循环里让机器人沿着路径点逐格移动。% 显示地图与路径 figure; hold on; axis equal; for i 1:rows for j 1:cols if map(i,j) 1 rectangle(Position, [j-1, i-1, 1, 1], FaceColor, [0.3 0.3 0.3], EdgeColor, none); end end end % 规划所有机器人的路径 for r 1:nRobots [paths{r}, cost] astar_grid(map, robotStarts{r}, robotGoals{r}, neighborMode); plot(paths{r}(:,2)-0.5, paths{r}(:,1)-0.5, LineWidth, 2); end真正仿真移动时不能直接让机器人“瞬移”到终点。我通常把每段路径细分在每两个路径点之间线性插值几步然后用drawnow刷新画面。这样看起来就是连续移动而不是跳格子。插值步数越多动画越平滑但要注意实际机器人速度是有上限的插值步数其实对应了时间分辨率这个细节到第四章还会提到。3.4 参数调优栅格粒度、邻域方式与代价单位地图分辨率直接决定搜索空间的规模和路径的精细度。同样的真实环境下栅格越细路径越贴近可行道路但节点数变多计算时间成倍上升。栅格越粗计算快但机器人可能通过不了狭窄通道路径也容易出现明明靠墙很近却必须绕一大圈的尴尬情况。实际使用时我会先根据机器人尺寸和通行通道宽度确定栅格分辨率一般让机器人直径占1到2个栅格这样既能保证安全又不会让地图太大。邻域方式的选择同样影响结果。四连通路径只能横平竖直转折多但不钻墙角八连通路径可以斜走视觉上更像人走出来的路线但是容易出现斜穿对角缝隙的“偷懒”行为。比如两个障碍物的对角位置恰好空出一个格子八连通路径会斜着穿过去这在某些真实场景里会撞车。解决方案是在邻居扩展时加判断斜向移动前先检查相邻的两个正方向格子是否都是空地如果其中一个是障碍则不允许斜走。4. 多机器人导航的冲突处理与协同调度4.1 最简单可用的方案固定优先级顺序规划多机器人导航最直接的做法是按优先级依次规划。比如让1号机器人先规划2号机器人规划时把1号机器人规划的整条路径格子都视为临时障碍3号机器人则把1号和2号的路径都视为临时障碍。这样做三台机器人的路径在空间上完全不重叠不会撞车。但这个方案有明显的缺点后规划的机器人很容易被前面的完全堵住绕很远的路。我跑过一个案例三台机器人从三个角落互相换位按固定优先级规划后第三台机器人的路径长度是第一台的三倍看起来很不自然。所以固定优先级通常只适合机器人数量很少、空间比较宽敞的场景。它的优点是逻辑简单作为起点非常好方便你验证A*和整个仿真框架是否正常。4.2 加入时间维度时间窗与等待策略我更推荐在路径规划的后期处理中加入“时间窗”概念。思路是每台机器人的路径不只是格子序列而是带时间戳的节点序列。1号机器人规划完后把它在每个时刻占用的格子存下来2号机器人规划时每走一步都要检查当前时刻是否有其他机器人已经占用目标格子如果占用就等待一拍等格子释放了再走。实现时可以维护一个单元格的占用时间表把每台机器人路径上的(行, 列, 时间步)标记为占用。新机器人规划时如果发现下一步的目标格子在下一步时间已被占用就让当前机器人停留在原地同时把停留产生的额外占用也写进时间表。这个策略在MATLAB里实现很直接本质上是把一维路径变成二维时空路径空间冲突就被转换成了时间冲突然后用等待解决。4.3 冲突检测方法对照做多机器人导航首先要能识别冲突然后才能处理冲突。我把最常见的冲突类型和处理思路整理在下面冲突类型判断条件典型处理方式节点冲突两台机器人在同一时间步到达同一格子低优先级机器人等待一拍或重新规划相向冲突两个机器人在相邻两个格子互相靠近下一时刻会交换位置其中一台原地等待避免“对穿”交叉冲突两条路径在空间上交叉但到达时间不同一般无需处理只需要关注时间窗口重叠部分追尾冲突同一条路径上前后机器人距离过近通过调整速度或插入等待来拉开间距处理相向冲突很容易被忽略。举个例子1号机器人在格子A下一步要到格子B2号机器人在格子B下一步要到格子A如果只看“同一时刻到达同一格”它们并没有撞车因为它们会交换位置。但现实中两个机器人如果尺寸足够大交换位置这一步就会挤在一起。所以判断时要把相邻格子互换也识别为冲突最简单的办法是检查两台机器人是否同时出现在相邻两格并且移动方向相对。4.4 动态重规划遇到突发障碍怎么办有时候地图不是完全静态的或者路径预先规划好之后某台机器人因为执行偏差晚了几步原本的时间窗就废了。这种情况下可以在仿真循环里做局部检查每一步判断机器人下一位置是否安全如果不安全就调用一次A*重新规划剩余路径。重规划时要注意别把问题复杂化。我见过有人每走一步就重新规划一遍整条路径其实没必要。网格地图没那么大时每5到10个时间步检查一次就够了。重规划的起点是机器人当前位置终点不变其他机器人当前占用的格子可以作为临时障碍写入地图副本。这样做等于把全局规划变成了局部滚动规划牺牲一点最优性换来了对动态环境的适应能力。4.5 仿真参数建议多机器人导航的效果和几个参数关系很大我整理了一份常用的起始参数你可以按项目实际场景再去调参数建议值说明地图尺寸20x20到50x50再大建议换成堆优化的A*机器人数量3到6台数量更多时需要更复杂的调度策略邻域模式8路径好看但记得检查斜穿问题等待时间上限10个时间步超过上限就强制重规划避免死锁路径插值步数每格分5步仿真动画平滑也对应时间分辨率密度是一个需要观察的核心指标。地图上有30%的区域是障碍、6台机器人同时跑和只有3台机器人跑冲突概率完全不同。机器人越密集等待时间占比越高路径越长。如果你的场景里机器人数量多建议把地图扩大或把起点终点设置分散一些否则调度算法的瓶颈很快会暴露。5. 实操中遇到的常见问题与排查经验5.1 路径为什么会穿墙或者斜穿障碍物八连通模式下最容易出现斜穿障碍物的情况。比如障碍物在(3,4)和(4,3)地图上(3,3)是空地(4,4)也是空地八连通A*会让机器人从(3,3)斜着走到(4,4)看起来就是从两个障碍物之间挤过去了。如果机器人是圆形且半径很大实际肯定蹭到障碍物。解决办法是在邻居扩展时增加一个“拐角检查”。对于斜向邻居检查与当前格和邻居格都相邻的正方向格也就是水平垂直方向上的那两个格子是否都是空地只要有一个是障碍就禁止斜走。这个检查对路径安全性提升明显强烈建议加进去。5.2 有通路却提示找不到路径这个问题我踩过几次原因主要有三种。第一种是起点或终点本身被标记为障碍这个检查几行代码就能做但经常被漏掉。第二种是启发式函数返回了NaN或Inf比如某个坐标越界导致f值全部是Inf开列表一直为空的假象。第三种是地图本身被障碍分隔成了多个不连通的区域A*确实搜不到这时要检查地图的连通性。排查顺序建议是先用一个单起点单终点的A*调用做测试地图用一个2x2的无障碍小地图如果这个简单的场景都跑不通那肯定是代码问题而不是环境问题。等到小地图通了再逐步换成实际地图。5.3 多机器人跑到一半全部卡死多机器人导航里很典型的问题是“循环等待”也叫死锁。机器人A在等B让路B在等C让路C又在等A让路结果谁都不动。我的经验是给等待加一个上限等待步数超过上限就强制重新规划。重规划时可以把等待中的邻居视为障碍这样至少能打破循环。另外多机器人路径规划的初始状态很重要。如果所有机器人的起点和终点在布局上就构成了一个交叉换位问题那么无论怎么调度都会很难看。我遇到过三台机器人把起点和终点设置成了一个旋转对称的布局结果每台机器人都必须穿到地图中心绕路绕得极其痛苦。后来把终点换成分散布局问题迎刃而解。5.4 地图变大之后计算时间暴涨用矩阵找最小值的方式在20x20地图上毫无压力但一旦地图变成500x500每次循环都扫描整张f矩阵效率会低得让人怀疑人生。这时候要做两件事一是把开列表改成最小堆或者至少用Matlab的sort维护一个有序数组二是考虑用Jump Point Search这类改进算法它在栅格地图这样的均匀网格上会跳过大量中间节点搜索速度一般能提升数倍。如果不想改算法还有两个工程上的技巧。一是降采样地图把4个格子合并成1个搜索完之后再映射回原分辨率二是先做一次粗粒度全局规划再用细粒度局部分段优化。这两种方式在实际项目里都很常用。5.5 问题速查表现象可能原因解决办法路径斜穿障碍物八连通未做拐角检查检查斜向移动时相邻正方向格子规划结果为空起点终点在障碍上或地图不连通检查start和goal测试小地图多机器人互相堵死没有等待上限或路径交叉集中加等待上限超时强制重规划地图一大就很慢开列表用矩阵扫描实现换最小堆或改为JPS算法路径明显绕远启发式与移动代价不匹配四邻域配曼哈顿八邻域配Octile仿真动画跳格缺少路径点间插值每段路径做线性插值并drawnow6. 后续扩展方向与个人体会6.1 从Matlab仿真到真实机器人之间差了什么这个Demo验证的是路径规划算法但真实机器人和仿真之间还有不少距离。真实场景的地图通常不是手工填出来的而是通过激光雷达或视觉SLAM构建的栅格地图里面充满了噪声和动态障碍物。A*规划出来的路径通常是折线但真实机器人底盘有运动学约束不能瞬间转向所以还需要在局部做一个运动规划比如把路径平滑成曲线或者用动态窗口法去跟踪全局路径。我自己的做法是先把Matlab里规划的路径点导出成文本每行一个坐标点再在实体小车上用这套点做全局指引。真实运行时只保留全局路径的整体形状具体怎么走、多快走完全交给底层的局部避障模块处理。这个分工非常重要全局规划负责在宏观上找一条可行路线局部规划负责微观上的安全和运动。6.2 算法层面的升级建议如果你对这个项目有兴趣下一步可以尝试在几个方向上做改动。第一个方向是单机寻路升级把A*换成Jump Point Search看看在相同地图上搜索效率提升了多少。第二个方向是地图表达升级把栅格地图换成概率路图或快速扩展随机树用来处理连续空间里栅格化带来的精度问题。第三个方向是多机调度升级引入更系统的冲突解决算法比如基于冲突搜索的CBS算法它把多机问题拆成多棵搜索树在高密度场景下效果比简单优先级法好很多。每个方向都能独立做一个新项目。这个Demo最值得学习的地方不是代码本身而是它把所有基础模块串起来了环境建模、路径搜索、时间窗、可视化、参数分析。你在这套框架上换算法、换地图、换调度策略都不会太费劲。6.3 一点个人心得我在实际调试这个Demo的过程中最深的体会是路径规划的核心不是“找一条路”而是“让规划出来的路真正能用”。单机A*很容易让人产生错觉以为路径通就是结束。加上多机器人之后你才会明白时间、速度、等待、优先级这些东西对最终效果影响有多大。所以建议拿到代码后不要只跑一边看效果交差可以试着把机器人数量改成6台把地图障碍物调密集一些你会在调整优先级和等待策略的过程中真正理解多机器人导航的难点。如果你也打算在自己的项目里用这套代码我最后再提醒一句大胆改参数但每次只改一个变量。我见过不少同学一次改地图、改启发式、改邻居数出了问题根本不知道是哪一步引入的。保持“单一变量”的实验习惯能让你调参时省掉大量时间。