一边是入口一边是出口中间是隔断纵横的墙——当你站在一座巨型迷宫面前你会怎么找到那条最短的出路如果让你把这种“找路”的直觉翻译成计算机能执行的指令又要怎么设计才能既保证找到最短路径又别让程序傻乎乎地把整张地图都搜一遍这正是A*路径规划算法处理的典型问题而用Matlab做仿真是理解和验证这个算法最省力的一条路。这篇东西不是给你抄一段代码就跑而是把我自己从“只会调函数”到“手写A也心里有数”的完整过程整理出来迷宫怎么建模、A为什么能比广度优先少搜那么多格子、Matlab里怎么写主循环才不容易卡死、怎么把搜索过程变成动态图发给别人看。无论你是刚接触路径规划的学生还是想用Matlab做机器人导航仿真的工程师照着这篇的思路走一遍至少能在自己的项目里少踩一半的坑。1. 迷宫地图的建模从现实路径问题到栅格空间1.1 为什么要用栅格法建模路径规划的第一步不是写A*而是想清楚“地图在程序里长什么样”。真实世界的迷宫是用实体墙壁围出来的但计算机不认识墙它只认识数字和逻辑关系。最常用的办法就是栅格法把迷宫切成一个个大小相同的正方形小格每个格子只有“可通过”和“不可通过”两种状态通常用0表示空地、1表示障碍物。这样处理的好处非常直观地图天然变成一个二维矩阵Matlab里直接用矩阵索引就能定位任意格子机器人的位置被离散成格子坐标移动规则清晰每次只能从一个格子跳到相邻格子距离计算简单既可以用曼哈顿距离也可以用欧几里得距离。如果你面对的是一个真实环境比如房间、走廊、家具也可以用激光雷达或者视觉SLAM构建占用栅格地图再把障碍物格子标成1。先想清楚这一点后续所有算法逻辑都建立在“格子”之上才不会写着写着发现数据结构撑不起需求。1.2 地图数据结构与邻居定义在Matlab里面一个简单的迷宫地图用二维矩阵表示即可% 定义一个 10x10 的迷宫 % 0 表示可通行1 表示障碍物 map [0 0 0 1 0 0 0 0 0 0; 0 1 0 1 0 1 1 1 0 0; 0 1 0 0 0 0 0 1 0 0; 0 0 0 1 1 0 0 0 0 0; 0 1 0 0 0 0 1 1 0 0; 0 1 1 1 0 0 0 1 0 0; 0 0 0 0 0 1 0 0 0 0; 0 1 0 1 0 1 0 1 0 0; 0 0 0 1 0 0 0 1 0 0; 0 0 0 0 0 1 0 0 0 0];有了矩阵之后必须定义“从一个格子能走到哪些格子”。最基础的是四邻域上下左右四个方向进阶一点是八邻域再加上四个斜对角方向。四邻域移动规则简单路径只能走水平和垂直线转角固定是90度八邻域走起来更灵活但代价函数里要考虑斜向移动距离是根号2否则计算出来的最短路径会有问题。你的移动规则定义成函数便于后面扩展function neighbors get_neighbors(map, node) % 返回当前节点所有可通行的邻居节点坐标 [rows, cols] size(map); % 四邻域方向 dirs [-1 0; 1 0; 0 -1; 0 1]; % 八邻域则改成 % dirs [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; neighbors []; for i 1:size(dirs, 1) nr node(1) dirs(i, 1); nc node(2) dirs(i, 2); % 检查边界和障碍物 if nr 1 nr rows nc 1 nc cols map(nr, nc) 0 neighbors [neighbors; nr, nc]; end end end这里有个很容易忽略的点邻居函数必须做越界检查。很多刚开始写的同学直接把邻居下标放进矩阵索引里结果跑到地图边缘就报Index exceeds matrix dimensions找半天才发现是没查边界。1.3 起点、终点与格子的“坐标体系”A*最终返回的路径是格子坐标序列比如从(1,1)到(10,10)。这里要注意Matlab的坐标轴方向矩阵行号是从上往下的plot的时候如果直接用image或imagesc绘图会看到地图第一行显示在图片最上方。这符合我们平常看迷宫图的方向但如果你习惯把坐标当成x向右、y向上画图时可能需要axis xy调整。建议在写代码前明确自己用的坐标系避免后面调试路径时感觉方向颠倒了。2. A*算法原理细读启发函数、开放表与闭合表的工作机制2.1 从贪心到A*f(n)g(n)h(n)为什么有效A*不是魔术它是在“搜索代价”和“启发信息”之间做平衡。一个朴素的广度优先搜索BFS会从起点一圈一圈往外扩直到碰到终点。这种方式保证能找到最短路径但效率太低如果地图足够大盲目扩展的点会爆炸式增长。另一种极端是贪心搜索每次只往看起来离终点最近的方向走比如始终选择与终点曼哈顿距离最小的邻居。这样搜索很快但很容易被局部墙壁骗进死胡同走不到终点或者找到一条绕远的路。A*把两个信息合起来用评估函数f(n) g(n) h(n)g(n)从起点到当前节点n已经花费的实际代价h(n)从当前节点n到终点的预估代价也就是启发函数f(n)经过节点n的完整预估路径代价。每次从开放列表中取出 f 值最小的节点去扩展既照顾了“已经走过的路”又用启发函数把搜索方向往终点那边“拽”。这就是A*的核心逻辑说白了就是有方向感的Dijkstra。2.2 启发函数的一致性与可采纳性以及常见选择启发函数能不能保证最终路径最优关键看两个性质可采纳性和一致性。可采纳性是指h(n)永远不大于从节点n到终点的真实代价。如果不满足这一点A*会过早放弃一些实际最优的路径结果得到次优解。一致性或者说单调性要求h(n) cost(n, n) h(n)它更强但好处是每个节点的 f 值一旦确定就不会反复变实现时不用处理重新入队的情况。迷宫地图里最常用的两个启发函数是曼哈顿距离h abs(nr - gr) abs(nc - gc)适合四邻域移动欧几里得距离h sqrt((nr-gr)^2 (nc-gc)^2)适合八邻域移动。但如果用八邻域曼哈顿距离不是严格可采纳的因为它高估了斜向移动的代价斜向移动真实代价是根号2曼哈顿距离却算成2。此时应该用切比雪夫距离或者带系数修正的欧氏距离。忽略这个细节会让算法在八邻域地图上找出的路径不是真正最短的视觉上也能看出来路径有“多余的折线”或绕远。2.3 算法主流程伪代码与终止条件A*的经典流程可以用伪代码写得很短1. 把起点加入 open list 2. 循环直到 open list 为空 2.1 从 open list 中取出 f 值最小的节点 current 2.2 如果 current 是终点则返回路径 2.3 把 current 移入 closed list 2.4 遍历每个邻居 neighbor - 如果 neighbor 不可通行或已在 closed list跳过 - 计算 tentative_g g(current) move_cost - 如果 neighbor 不在 open list加入 open list - 如果 tentative_g 之前记录的 g(neighbor)更新 g 和 parent 3. 如果 open list 为空还没到终点说明地图不可达这里需要注意终止条件不是把open list弹出空而是当current就是终点时立刻停止。如果等到open list全部遍历完搜索范围会扩大很多结果虽然没错但效率大打折扣。还有一点closed list是防止走回头路的但如果你用一致性启发函数可以确保每个节点只被最终确认一次这样closed list才是安全的。3. Matlab代码设计与实现从零编写一个可运行的A*脚本3.1 地图生成与障碍物设定为了演示效果我通常先手动设置一个迷宫或者用代码随机生成障碍物。手动设置好处是可复现适合讲解随机地图可以看到算法在不同迷宫上的行为差异。一个简单的随机地图生成方式map zeros(20, 20); % 随机生成一些障碍物保证起点终点附近是空地 rng(42); % 固定随机种子 for i 1:20 for j 1:20 if rand 0.25 map(i, j) 1; end end end % 强制起点和终点可通行 map(1,1) 0; map(20,20) 0;但完全随机生成的地图可能把终点围死或者中间形成大面积不可达区域这反而适合用来测试算法对不可达情况的处理。正式项目中建议用更平滑的障碍物生成方式比如随机多边形、圆盘等更接近真实环境。3.2 核心函数Astar_maze 的完整代码下面给出一个可以直接复制运行的Matlab函数。为了可读性我没有做极致优化但结构足够清晰方便你照着改。function path Astar_maze(map, start, goal) % A*路径规划核心函数 % map: 栅格地图0可通行1障碍 % start: 起点坐标 [row, col] % goal: 终点坐标 [row, col] % path: 从起点到终点的路径坐标序列Nx2矩阵若不可达返回空数组 [rows, cols] size(map); % 记录每个节点的信息 % gScore: 从起点到该节点的实际代价 % fScore: gScore hScore % parent: 每个节点的父节点索引用于回溯路径 gScore inf(rows, cols); fScore inf(rows, cols); parent zeros(rows, cols); % 存储父节点线性索引 % open list使用结构体数组存储待扩展节点 openList struct(row, {}, col, {}, fVal, {}); % 初始化起点 gScore(start(1), start(2)) 0; fScore(start(1), start(2)) heuristic(start, goal); parent(start(1), start(2)) -1; % 起点没有父节点 openList(1).row start(1); openList(1).col start(2); openList(1).fVal fScore(start(1), start(2)); % closed list直接用一个逻辑矩阵记录 closedList false(rows, cols); % 八邻域方向及移动代价 dirs [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; moveCosts [1; 1; 1; 1; sqrt(2); sqrt(2); sqrt(2); sqrt(2)]; while ~isempty(openList) % 从open list中取出f值最小的节点 [~, minIdx] min([openList.fVal]); current.row openList(minIdx).row; current.col openList(minIdx).col; openList(minIdx) []; % 从open list中删除 % 到达目标则回溯路径 if current.row goal(1) current.col goal(2) path reconstruct_path(parent, start, goal); return; end % 移入closed list closedList(current.row, current.col) true; % 遍历邻居 for k 1:size(dirs, 1) nr current.row dirs(k, 1); nc current.col dirs(k, 2); % 越界或障碍物检查 if nr 1 || nr rows || nc 1 || nc cols continue; end if map(nr, nc) 1 || closedList(nr, nc) continue; end % 计算新的g值 tentative_g gScore(current.row, current.col) moveCosts(k); % 如果新g值更小更新节点信息 if tentative_g gScore(nr, nc) parent(nr, nc) sub2ind([rows, cols], current.row, current.col); gScore(nr, nc) tentative_g; fScore(nr, nc) gScore(nr, nc) heuristic([nr, nc], goal); % 检查邻居是否已在open list中 inOpen false; for m 1:numel(openList) if openList(m).row nr openList(m).col nc openList(m).fVal fScore(nr, nc); inOpen true; break; end end if ~inOpen newIdx numel(openList) 1; openList(newIdx).row nr; openList(newIdx).col nc; openList(newIdx).fVal fScore(nr, nc); end end end end % open list为空表示无法到达目标 path []; end function h heuristic(node, goal) % 启发函数这里使用欧几里得距离适合八邻域 h sqrt((node(1) - goal(1))^2 (node(2) - goal(2))^2); end function path reconstruct_path(parent, start, goal) % 从终点回溯到起点 [rows, cols] size(parent); path []; idx sub2ind([rows, cols], goal(1), goal(2)); while idx ~ -1 [r, c] ind2sub([rows, cols], idx); path [path; r, c]; idx parent(r, c); end % 此时path是终点到起点反转为起点到终点 path flipud(path); end这段代码有几个设计点需要解释用inf初始化gScore和fScore方便后续判断“新路径是否更优”。第一次访问某个节点时tentative_g inf成立所以会自动更新。parent矩阵用线性索引存储父节点sub2ind和ind2sub配合使用比存二维坐标节省内存。open list用结构体数组加入新节点方便删除最小f节点时要整体移动数组效率不高。如果用于大型地图建议改造成二叉堆后面我会单独讲。用closedList逻辑矩阵查询速度O(1)非常经济。3.3 路径回溯从目标节点反向找到起点路径回溯是A*最容易写错的环节之一。常见错误是知道用parent数组但回溯时用了错误的终止条件比如写成while parent(r,c) ~ 0而起点父节点设置的是-1导致死循环。正确做法是起点父节点设置为-1或一个特殊标记回溯时不断寻找父节点直到遇到特殊标记。上面代码里我用while idx ~ -1这里的前提是起点父节点确实设置成了-1。如果某次修改地图后忘记设置就会报错。更稳妥的做法是先判断parent(r,c) 0但起点坐标恰好可能是(1,1)线性索引就是1很容易混淆。建议把起点父节点设置成一个绝不会出现的负数或者干脆用另一个二维逻辑矩阵记录“是否是起点”。3.4 复杂度优化优先级队列与内存管理的可选思路上面代码为了教学直观在open list里用min函数找最小f值节点每次O(N)N是open list元素个数。当地图尺寸在几十乘几十时没感觉到几百乘几百就很吃力了。优化的标准做法是二叉最小堆或Matlab自带的containers.Map配合排序。如果你不想自己实现堆可以先试试这样一个技巧把open list拆成两列f值作为下标映射到特定数据结构但这种方式容易浪费内存。更好的办法是使用Matlab的java.util.PriorityQueue因为Matlab天生可以调用Java类库。虽然效率比不上C但比自己维护数组快很多。不过注意Java对象的索引和Matlab矩阵索引不同需要小心封装。如果只是学习验证完全没必要用Java堆如果做更大的仿真可以考虑把核心搜索逻辑用C/MEX重写或者改用Python调库。4. 可视化搜索过程用Matlab把每一步扩展都展示出来4.1 静态地图绘制与标记约定Matlab里绘制栅格地图的最简单方法是imagesc(map)也可以叠加网格线figure(Color, white); imagesc(map); colormap(gray); axis equal; grid on; set(gca, GridAlpha, 0.4); hold on;这里map里的1障碍物会显示为黑色0空地显示为白色。可以再自定义颜色让障碍物更醒目cmap [1 1 1; 0 0 0]; % 白色空地黑色障碍物 colormap(cmap);标记起点和终点最直观的方式是绘制不同颜色的圆点plot(start(2), start(1), go, MarkerFaceColor, g, MarkerSize, 12); plot(goal(2), goal(1), ro, MarkerFaceColor, r, MarkerSize, 12);注意plot的横纵轴坐标顺序plot(x, y)对应plot(col, row)很多人习惯写plot(row, col)导致起点终点画反了。建议检查一次后续就统一。4.2 动态更新open/closed区域的效果要让搜索过程“肉眼可见”可以在主循环的每一步暂停刷新并把当前扩展节点、open list节点和closed list节点用不同颜色叠加显示。具体做法是在算法的while循环里插入绘图代码% 在循环内部扩展当前节点前 currentPixel current.row (current.col - 1) * rows; % 线性索引 % 用全彩色显示 imageHandle imagesc(map); hold on; % 绘制closed list节点浅蓝色 [closedRows, closedCols] find(closedList); plot(closedCols, closedRows, s, MarkerSize, 6, MarkerFaceColor, [0.7 0.8 1], MarkerEdgeColor, none); % 绘制open list节点浅黄色 for m 1:numel(openList) plot(openList(m).col, openList(m).row, s, MarkerSize, 6, MarkerFaceColor, [1 1 0.6], MarkerEdgeColor, none); end % 绘制当前扩展节点红色 plot(current.col, current.row, s, MarkerSize, 8, MarkerFaceColor, r, MarkerEdgeColor, k); drawnow; pause(0.05); % 控制动画速度这套绘制逻辑虽然直观但有一个性能问题每次循环都重新画所有open和closed节点地图一大就非常卡。优化方法是提前创建好点对象在循环里只更新对象的XData和YDataclosedHandle plot(nan, nan, s, MarkerSize, 6, MarkerFaceColor, [0.7 0.8 1], MarkerEdgeColor, none); openHandle plot(nan, nan, s, MarkerSize, 6, MarkerFaceColor, [1 1 0.6], MarkerEdgeColor, none); % 循环内获取当前closed/open坐标后 set(closedHandle, XData, closedCols, YData, closedRows); set(openHandle, XData, openColsArray, YData, openRowsArray); drawnow;这样刷新的性能提升非常明显尤其当搜索节点达到几千个时。4.3 导出动画/图片序列的实用技巧如果只需要在Matlab窗口里看过程drawnow配合pause就够了。但想发给别人看或者直接插到论文里最好把它导出成GIF图或视频。导出GIF的思路很直接在绘图的每一帧把当前figure捕获为图像再追加写入GIF文件。filename astar_search.gif; frame getframe(gcf); im frame2im(frame); [imind, cm] rgb2ind(im, 256); if i 1 imwrite(imind, cm, filename, gif, Loopcount, inf, DelayTime, 0.1); else imwrite(imind, cm, filename, gif, WriteMode, append, DelayTime, 0.1); end这里有个细节i是主循环的帧计数不能直接在while循环里用循环变量最好单独定义一个变量递增。而且rgb2ind的调色板每次可能不同会导致GIF颜色闪烁。稳妥的做法是固定调色板或者在第一帧生成后就锁定。导出视频更简单用VideoWriterv VideoWriter(astar_path.avi); v.FrameRate 15; open(v); % 在循环里写入当前帧 writeVideo(v, getframe(gcf)); end close(v);这里的getframe会捕获整个figure如果你只画了地图窗口记得把坐标轴之外的多余空白去掉否则视频四周有大片白边。5. 实验、参数调节与踩坑记录让A*在实际运行中更稳5.1 启发函数权重对搜索过程的影响在实际仿真中可以给启发函数加一个权重变成f(n) g(n) w * h(n)。当w0时A*退化成Dijkstra会均匀向外搜索保证最优当w增大时算法更“贪心”搜索速度变快但可能失去最优性。这在很多工程场景里是可以接受的——如果你是在实时机器人上规划路径一个折中的次优解比迟迟得不到最优解更有意义。用迷宫地图做实验时你会发现w1时算法扩展出的格子数可能还挺多尤其是终点在斜对角、地图又空旷的时候。w1.5或w2后扩展的格子明显减少路径往往贴着边界走但依然能到达终点。调试时如果想验证“启发函数权重如何影响搜索过程”可以画三条不同颜色路径叠在同一张地图上一眼就能看出差异。5.2 四邻域还是八邻域路径长度与计算开销的取舍这是仿真实战里绕不开的决策。四邻域的搜索空间更小路径只能横平竖直生成路径不够自然但计算邻居的开销低A*扩展节点时每个节点最多看4个方向。八邻域路径更短、转角更平滑但因为每个节点要看8个方向搜索空间也更大。一个重要细节是如果从四邻域改成八邻域必须同步修改启发函数。四邻域用曼哈顿距离八邻域用欧氏距离或切比雪夫距离。我见过好几个同学把四邻域代码改成八邻域时忘了改启发函数结果路径明显不对劲半天查不出原因。如果地图上障碍物是斜向分布的八邻域路径会“穿过”两个障碍物斜对角之间的空隙四邻域则不会。这个行为既可能是优点也可能是缺点取决于你的机器人是否允许沿对角线移动。真实轮式机器人的运动模型通常更接近连续空间所以八邻域是更常见的简化。5.3 死循环、越界、不可达目标三个容易翻车的场景实际跑起来最容易翻车的不是A*本身而是外围细节。死循环最常见原因是parent指针没有正确更新或者open list中同一个节点被重复加入却没有检查closed list。另一个隐蔽原因是启发函数没有保证一致性导致某个节点的g值被反复更新节点被反复扩展算法迟迟不结束。解决办法是加上强制检查如果某个节点已经被确认过在closed list里就算后面发现更短的路径也不要再更新它。这在大地图上是一种性能与最优性之间的权衡。越界前面说过邻居扩展一定要检查行号和列号是否在1到地图尺寸之间。不过更隐蔽的情况是使用线性索引时sub2ind可能得到超出矩阵范围的索引因为你没检查行列先越界。所以索引前一定要先做边界判断顺序不能反。不可达目标当地图把终点完全围住时A*会把所有可达节点全部扩展一遍最后open list为空并返回空路径。如果map很大这个“必然失败的搜索”也会跑很久。工程上可以在扩展节点过程中加入实时判断比如已经扩展了一定数量的节点还是没看到终点就提前终止并输出告警避免程序长时间卡住。5.4 路径平滑与后续优化方向A*返回的路径是一系列格子中心点连线即使使用八邻域也可能出现不自然的锯齿。移动机器人如果想要平滑轨迹通常需要后处理一种简单方法是对路径做分段线性插值然后用梯度下降拉平另一种是记录关键转折点用贝塞尔曲线或样条曲线拟合。在迷宫仿真里我会先给路径画出来观察是否有明显反向折线。如果起点和终点之间有一段空地但A的路径却走了Z字形这通常说明地图分辨率和邻居定义不匹配或者启发函数有问题。正确情况下在完全空旷地图上八邻域A应该找出一条从起点到终点的直线。除此之外还可以把A扩展成DLite、Theta等算法前者适合动态环境下的重规划后者能生成更平滑的路径。但这些都是后话——先把A在Matlab里跑通比急于上复杂算法更重要。6. 从迷宫走向真实世界A*在移动机器人导航中的落地扩展6.1 栅格地图到占用地图的转换迷宫测试用的是纯几何栅格但真实机器人拿到的是传感器数据生成的占用地图。占用地图通常分为free、occupied、unknown三种状态而A*只处理障碍物和非障碍物所以需要把未知区域也投影成“可通行”或“不可通行”。简单做法是把unknown当成free算法可能规划出穿过未知区域的路径把unknown当成occupied则过度保守路径会绕远。一种常用策略是先对占用地图做膨胀处理把机器人半径考虑进去。比如机器人宽度占两个格子就把所有障碍物周围一格都标记为障碍。膨胀后的地图能让A*规划出的路径中心线离墙有足够距离真实机器人沿路径走才不容易撞墙。6.2 动态障碍与重规划思路迷宫里的障碍物是静态的但真实环境可能有行人或其他移动物体。A本身不擅长处理动态变化。常见的应对思路有两类一是每移动一小段距离就重新用A规划一次这种方法简单但对计算时间敏感地图大了容易卡顿二是使用支持增量式重规划的D* Lite它能在原搜索树上局部调整路径效率高很多。如果你暂时只想在Matlab里模拟动态障碍可以用一个简单技巧在A主循环每扩展N个节点后检查是否有障碍物状态更新如果有就重置当前搜索但保留已经计算过的g值。改进后的算法性能通常还不错能模拟出动态规避的视觉效果——虽然不是真正的DLite但对于演示已经足够。6.3 Matlab仿真在算法验证中的定位Matlab仿真在整个路径规划项目里承担的职责不是“生产部署”而是快速验证。你可以用Matlab脚本试不同邻居定义、不同启发函数、不同地图分辨率几秒钟就能看到结果变化。相比C实现Matlab的矩阵遍历能力强大而且内置可视化这对前期调参和写论文非常友好。但也要知道Matlab的局限性它默认基于解释执行循环密集的A*主循环性能远不如编译型语言。如果仿真地图规模达到1000x1000以上或者要做蒙特卡洛大批量测试我建议先用Matlab写好逻辑并攒足够的可视化素材再把核心函数翻译成C或Python/Cython版本。Matlab仿真做的是一块“试金石”不是终点。我自己在项目里经常这样组合先用Matlab脚本快速验证“算法能不能在给定地图上找到路径”然后统计扩展节点数、路径长度、运行时间等指标确认无误后再用Python配合NumPy重写一遍放到更大的地图或其他机器人框架里做集成测试。这样既享受了Matlab可视化的便利又不至于被性能拖死。最后再分享一个我自己的小习惯在A脚本里加一行tic和toc记录每次规划耗时。看似无关紧要但在对比不同参数、不同地图规模时特别有用能让你直观理解哪个环节是性能瓶颈。我一开始在100x100的随机地图上跑A可视化刷帧很流畅换成400x400后开始卡顿一测发现90%时间花在把open list元素删除并重新拼接上。改成堆优化后速度提升非常明显也让我真正理解了“数据结构影响算法性能”这句话的分量。如果你只是刚接触A*建议先不要追求太多进阶特性老老实实把四邻域版本跑通把路径画出来再逐步加入八邻域、动态可视化和参数实验。每一步都亲手改一遍遇到问题再对照这里写的排查思路你很快就能把A*变成自己工具箱里的常备工具。