听到“A星路径规划”这六个字很多人第一反应是算法原理好懂但真要自己在Matlab里实现一版能跑、能改地图、能自定义起点终点的程序往往卡在数据结构、坐标换算、边界处理这些细节上。如果你也需要做一套可以自由变换障碍物、随机调整起终点的A星寻路演示工具那么这篇文章正好解决这个问题。这篇博文会给你一份完整的Matlab实现思路和可运行的代码骨架包含栅格地图怎么建、8方向搜索怎么写、openlist怎么维护、路径怎么回溯以及我实际调试中踩过的一些坑。无论你是本科生做课程设计、研究生搭算法验证环境还是想把A星作为上板前的Matlab原型都能直接拿来改造使用。1. 为什么我会选Matlab来写A星先把问题边界想明白1.1 这个A星最适合做什么A星本身不复杂但它不是一个“背完伪代码立刻能跑”的算法。很多人的路径规划试验环境是在ROS或者Python里搭的但如果你还处在验证阶段、没有机器人硬件、也没有完整仿真框架Matlab是一个很舒服的中间层。用Matlab写A星最大的优势是矩阵天然就是栅格地图。地图本身就是二维数组障碍物就是数组里的1可行区域就是0。这种表达方式和教科书里的栅格地图模型几乎一一对应不需要额外转换。加上Matlab的绘图能力你可以直接在地图上画出起点、终点、最终路径每一步还能把搜索过程可视化出来这对理解算法非常有帮助。我自己在给课题搭原型时也经常先拿Matlab验证主逻辑确认没问题之后再翻译成C或者其他部署语言。这正是这篇博文适用的人群你需要一个能跑、能改、能出图的A星实现而不是一个封闭的黑盒。1.2 A星在栅格地图上到底在搜什么先花点时间把A星的搜索逻辑彻底说清楚这是后文写代码的基础。栅格地图上的每一个格子都可以看作一个节点。算法从起点开始每次选择一个“看起来最优”的格子向外扩展。所谓“看起来最优”就是用下面这个评价函数去衡量f(n) g(n) h(n)其中g(n)是已经实际走过的路径代价也就是从起点到当前格子一共走了多远。h(n)是启发函数用来估计当前格子距离终点还有多远。f(n)值越小的格子会被优先扩展。A星和Dijkstra的核心区别就在这个h(n)上。Dijkstra只考虑起点到当前点的实际距离完全不看目标在哪所以它会像水波一样以均匀的速度向四周扩散浪费很多搜索空间。而A星通过h(n)让搜索有了“方向感”性能上通常比Dijkstra高很多。在栅格地图这种离散空间里节点不是抽象的点而是具体的地图格子。每个当前格子可以向周围若干方向扩展这些扩展出来的格子就是它的“邻居”。如果邻居是障碍物或者已经关闭节点就不处理否则计算新的g值看是否比之前记录的更小。整个搜索就是不断重复“取最小f节点、扩展邻居、更新代价”这个过程直到终点被取出。2. 地图、障碍物和起终点的“自定义”都落在哪些参数上所谓“自己想要改地图和障碍物”并不需要做得多复杂只要把地图定义、障碍物定义、起点终点定义分开成独立变量程序的可玩性就会完全不一样。2.1 栅格地图矩阵0和1就够了我习惯用二维矩阵map来表示地图。一张map矩阵每个元素对应真实环境里的一个方格。0可通行区域。1障碍物区域。比如下面这张地图尺寸是10x10一堵墙从左边第3行延伸到第7行map zeros(10, 10); map(3:7, 5) 1; % 第3到第7行第5列设置为障碍物如果地图尺寸是m行n列那就是zeros(m, n)。自定义尺寸时注意这一点行数代表纵向格子数列数代表横向格子数。不是数学坐标系里的二维坐标而是矩阵下标。要想随机生成障碍可以用rand函数生成设定比例的障碍物% 随机障碍物密度20% map zeros(20, 20); obstacleMask rand(20, 20) 0.2; map(obstacleMask) 1; % 保证起点和终点不被挡住 map(1, 1) 0; map(20, 20) 0;随机障碍物做算法演示非常合适每次跑的结果都不同适合展示给其他人看。但有一点要提醒纯随机地图有可能会出现障碍物把起点和终点隔开的情况也就是根本没有可行路径。如果是在做课程演示最好把障碍物密度控制在20%左右或者提前判断一下起点和终点的连通性。如果你有真实地图文件也可以直接把栅格化结果读入map。比如将地图图片转成二值图像直接作为A星的输入这也是很常用的做法。2.2 起终点坐标的设计与合法性校验起点和终点我用[startRow, startCol]和[goalRow, goalCol]两个向量表示为什么不用很多教学程序里的(x,y)因为Matlab矩阵访问是map(row, col)第一个下标是行第二个是列。如果你在代码里把x当作列、y当作行最后很容易出现越界或者路径画错位置的问题。起点终点至少要做两个检查% 检查越界 if startRow 1 || startRow size(map, 1) || ... startCol 1 || startCol size(map, 2) error(起点超出地图范围); end % 检查是否落在障碍物上 if map(startRow, startCol) 1 || map(goalRow, goalCol) 1 error(起点或终点不能是障碍物); end如果起点落在障碍物里A星第一步就无法扩展代码会直接卡死所以这个校验很有必要。同样如果起点等于终点可以提前退出直接返回一个空路径或单点路径避免后面进入死循环。2.3 4方向还是8方向步长代价表直接影响路线形态A星搜索时可以向哪些方向走决定了路径长什么样。常见的有两种4方向上下左右。8方向上下左右加左上、右上、左下、右下。我一般用8方向。原因很简单实际机器人或小车在自由空间移动时能够斜着走如果强行限制成4方向最后路径会明显变长而且会很多不必要的直角转折。8方向对应一个8行2列的邻居偏移表% 8个方向的偏移量[行偏移, 列偏移] neighborOffset [ -1, 0; % 上 1, 0; % 下 0, -1; % 左 0, 1; % 右 -1, -1; % 左上 -1, 1; % 右上 1, -1; % 左下 1, 1 % 右下 ];方向不同移动代价也不同。上下左右每走一步距离是1个格子斜向走一步实际距离是根号2也就是1.414左右。这一步如果没设置好算出来的可能是“伪最短路径”。代价数组可以写成moveCost [1, 1, 1, 1, sqrt(2), sqrt(2), sqrt(2), sqrt(2)];搜索过程中g值的更新就是当前格子的g值加上对应方向的moveCost。这个设置保证了最终搜索结果是几何意义上的最短路径而不是曼哈顿意义下的最短栅格距离。有一点需要提前说明如果地图里的方格代表真实环境中的固定尺寸那么相邻格子间真正移动距离是“格子边长 * 上述权重”。比如每个格子对应0.5米则上下左右一步是0.5米斜向是0.5*sqrt(2)米。A星不需要特殊处理只要保持所有代价比例一致即可。3. openlist、closelist与parentA星主循环的Matlab代码骨架3.1 节点数据结构与初始化在Matlab里写A星可以有几种数据结构方案我刚开始接触时也纠结过到底用cell数组还是结构体数组。后来在实际实现中最简单的方案是用几个大小跟地图完全一样的矩阵去记录代价信息再用一个openlist数组记录待搜索节点。为什么不给每个节点建立结构体因为Matlab的结构体数组在频繁增加、删除时开销相对大而且访问起来也不如矩阵直观。节点信息通常需要这几样g值起点到该节点已走的最短代价。f值[g 启发值]用于决定搜索顺序。父节点路径回溯的关键记录当前格子是从哪个格子扩展来的。是否已经关闭已经确定最短路径的节点不再重复访问。先初始化的代码[nrow, ncol] size(map); % 把二维下标压缩成一维索引方便操作openlist startIdx sub2ind([nrow, ncol], startRow, startCol); goalIdx sub2ind([nrow, ncol], goalRow, goalCol); % g值初始化无穷大 gScore inf(nrow * ncol, 1); gScore(startIdx) 0; % 父节点每一行记录[父行, 父列] parent zeros(nrow * ncol, 2); % 是否关闭 closed false(nrow, ncol); % openlist还没有最终确定路径代价、准备扩展的节点初始放起点 openList startIdx; % 计算起点的f值h用对角距离估计 startH diagHeuristic(startRow, startCol, goalRow, goalCol); fScore inf(nrow * ncol, 1); fScore(startIdx) startH;这里用一维索引的好处是openList里只需要存数字判断某个节点是否在openlist里也很方便。要拿到行列号就用ind2sub要拼回来用sub2ind。3.2 主循环取最小f节点、扩展邻居、更新代价核心主循环逻辑分成几步从openList中找出f值最小的节点。如果这个节点是终点搜索结束跳出循环。否则把这个节点从openlist中移除并标记为关闭。遍历它的所有合法邻居。对每个未关闭的邻居计算“经过当前节点”这条路径的新g值。如果新g值比原来记录的gScore更小更新该邻居的父节点和f值并把邻居加入openlist。写成Matlab代码如下while ~isempty(openList) % 1. 从openList中找f值最小的节点 [~, pos] min(fScore(openList)); currentIdx openList(pos); openList(pos) []; % 判断是否到达终点 if currentIdx goalIdx break; end [curRow, curCol] ind2sub([nrow, ncol], currentIdx); closed(curRow, curCol) true; % 2. 扩展邻居 for k 1:8 nbRow curRow neighborOffset(k, 1); nbCol curCol neighborOffset(k, 2); % 边界检查 if nbRow 1 || nbRow nrow || nbCol 1 || nbCol ncol continue; end % 障碍物检查 if map(nbRow, nbCol) 1 continue; end % 是否已经关闭 if closed(nbRow, nbCol) continue; end nbIdx sub2ind([nrow, ncol], nbRow, nbCol); % 新g值 当前节点g值 移动代价 newG gScore(currentIdx) moveCost(k); % 只有新g值更小才更新路径记录 if newG gScore(nbIdx) gScore(nbIdx) newG; parent(nbIdx, :) [curRow, curCol]; fScore(nbIdx) newG diagHeuristic(nbRow, nbCol, goalRow, goalCol); % 如果这个邻居不在openList里就加入 if ~ismember(nbIdx, openList) openList(end 1) nbIdx; end end end end这段代码里有几个地方初学者容易写错。一是openList里存的是“待扩展”的节点而不是所有搜索过的节点。如果某个节点已经在openList里但通过新的路径发现它的g值变小了这时只需要更新parent和fScore不需要重复添加。上面的代码用ismember做了去重代价是在节点多的时候会慢一点但地图不大时完全够用。第二个容易错的地方是closed标记到底在什么时候设置。正确的做法是取出openlist中最小f节点时立刻标记closed而不是在把邻居加入openlist时就标记否则容易漏掉更优路径。3.3 路径回溯与结果矩阵生成搜索结束后如果currentIdx等于goalIdx说明已经找到了终点。回溯就是从终点通过parent一路寻找到起点的过程。path []; if currentIdx goalIdx nodeRow goalRow; nodeCol goalCol; while ~(nodeRow startRow nodeCol startCol) path [path; nodeRow, nodeCol]; % 从终点往前拼 p parent(sub2ind([nrow, ncol], nodeRow, nodeCol), :); if isequal(p, [0, 0]) path []; error(路径回溯出错可能没有找到父节点); end nodeRow p(1); nodeCol p(2); end path [path; startRow, startCol]; path flipud(path); % 翻转成从起点到终点 end这里path是一个N行2列的矩阵每一行是路径途经格子的[row, col]。这个矩阵可以直接用来画图也可以进一步转换成机器人运动轨迹。需要注意如果终点不可达循环结束后currentIdx不等于goalIdxpath就保持空数组。外部调用时一定要判断这个情况不要直接去画图否则会报索引越界。完整的启发函数我用对角距离function h diagHeuristic(row, col, goalRow, goalCol) % 对角距离适合8方向移动且对角代价是sqrt(2)的情况 dx abs(col - goalCol); dy abs(row - goalRow); h max(dx, dy) (sqrt(2) - 1) * min(dx, dy); end为什么不用曼哈顿距离因为曼哈顿距离假设只能上下左右走在实际8方向搜索中会低估代价导致扩展节点变多。为什么不用欧氏距离欧氏距离也能用但离终点不远时它会比实际路径短不少搜索范围会大一点。对角距离是和运动模型最匹配的启发式保证搜索既高效又不至于高估实际代价。4. 让界面变成可交互的小工具地图可视化、随机障碍与鼠标选点光有控制台运行结果还不够直观这个项目的乐趣在于你能亲眼看见地图、起点终点和路径。Matlab做这个非常省事。4.1 基础可视化用imagesc把地图画出来最顺手的绘图方式是imagesc。它可以把矩阵按颜色画出来障碍物和空地一目了然。figure; imagesc(map); axis equal; axis tight; colormap([1 1 1; 0.3 0.3 0.3]); % 空地白色障碍物深灰色 hold on;这里有个细节imagesc和plot的纵轴方向不一致imagesc默认第1行在图像上方而plot的y轴默认向上。如果你用plot在地图上画起点和终点需要在plot时加一次坐标转换。我的习惯是直接把坐标转换封装在一个函数里以plot的行列坐标为准绘图时反过来设置坐标方向set(gca, YDir, reverse); % 让plot的y方向和矩阵行方向一致 xlabel(列); ylabel(行);这样后面用plot画点、画线就比较自然plot(startCol, startRow, go, MarkerSize, 10, LineWidth, 2); plot(goalCol, goalRow, rx, MarkerSize, 10, LineWidth, 2);因为这句set(gca, YDir, reverse)plot里的x传列号y传行号也正好符合“第一个坐标是行”的直觉。4.2 手动编辑障碍物和随机障碍效果的两种玩法如果想手动改障碍物最简单的办法是用ginput取鼠标点击位置的网格disp(鼠标点击地图设置障碍物右键结束); x []; y []; while true [tmpX, tmpY, button] ginput(1); if button ~ 1 break; end r round(tmpY); c round(tmpX); if r 1 r size(map,1) c 1 c size(map,2) map(r, c) 1; end imagesc(map); colormap([1 1 1; 0.3 0.3 0.3]); end因为坐标方向设置了reverseginput返回的x坐标对应列号y坐标对应行号直接round就能落到格子上。注意ginput(1)会阻塞等待用户点击测试脚本时不容易自动跑所以我通常在手动交互和自动脚本之间做一个开关。随机障碍就更简单前面已经写了用rand生成。建议随机地图固定一个随机种子方便复现结果rng(42); % 固定随机种子 map zeros(20, 20); obstacleMask rand(20, 20) 0.2; map(obstacleMask) 1; map(1,1) 0; map(20,20) 0;固定随机种子这个习惯很实用。否则每次运行地图都不一样遇到一次“没有路径”的情况时你无法稳定地给其他人复现问题和代码。4.3 用ginput自定义起点和终点起终点自定义我直接做了左右键交互拆开disp(左键选择起点右键选择终点); [tmpX, tmpY, button] ginput(1); if button 1 startRow round(tmpY); startCol round(tmpX); end [tmpX, tmpY, button] ginput(1); if button ~ 1 goalRow round(tmpY); goalCol round(tmpX); end注意鼠标点的位置只是一个点round之后才会落到具体格子中心。如果你的地图放大以后能看清格子用grid on能让交互更精准。手动交互的优点是不需要提前设计坐标鼠标一点就完事非常适合给老师或者客户现场演示。自动跑专题实验时就直接用代码定义起点终点两类场景要能切换。交互方式都合理我的经验是先跑自动脚本确认算法无误再切换成手动模式去做可视化演示。5. 跑通之后我踩过并且你应该避免的坑A星代码短但真正调试起来小问题不少。我总结几个实际踩过的坑每一个都直接浪费过不少时间。5.1 openlist重复节点处理不一致导致绕路问题场景同一个节点在openList里被加入了两次第一次g值较大第二次g值较小但你在取出时不清楚哪个是旧哪个是新最终可能把一条次优路径当成结果输出。我之前的处理是每次找到更小g值时就简单openList(end1) nbIdx不判断是否已在openList里结果路径偶尔会拐一个明显多余的弯。后来加上了ismember去重只在不在openList时才加入如果已经存在则只更新gScore、fScore和parent问题就消失了。这背后的逻辑是A星在扩展过程中允许“修正”一个已经放进openList节点的路径代价。如果只加节点不更新代价等于丢掉了新发现的更优父节点如果重复加入又不去重后续可能取出陈旧信息。用if ~ismember(nbIdx, openList)配合更新逻辑是最稳妥的。5.2 起点/终点贴着障碍物时路径崩坏或者路径不合理我试过把终点设在两个障碍物的夹缝里程序没报错但搜索出来路径为了绕开障碍物走出一个很夸张的弧线看起来完全不合理。问题出在邻居扩展的方向和地图边界限制。如果终点所在格子的上下左右全被障碍物围住但斜对角方向还有入口那么8方向搜索其实能找到这个入口这是正常的。但如果你用的是4方向搜索这种终点必然无解这时你的程序必须给出友好的提示而不是抛一个空数组让后面绘图脚本崩溃。我建议对外统一封装一个函数返回路径同时返回是否成功function [path, foundFlag] astar_planner(map, startRC, goalRC) ... if isempty(path) foundFlag false; else foundFlag true; end end这样主脚本里就可以很快判断[path, found] astar_planner(map, startRC, goalRC); if ~found disp(当前起终点之间没有可行路径请调整地图或起终点); else plotPath(path); end5.3 8方向搜索穿墙角的视觉穿模与修正规则如果你使用8方向搜索并且没有做任何限制路径可能在一个墙角处斜切过去看上去就像路径穿过了障碍物的角。因为栅格地图里对角线确实通过了两个格子共有的角点如果这个角点附近都是障碍物实际机器人在墙边这样走可能撞到墙角。这个问题严格来说取决于你使用的路径代价模型。如果障碍物是实心方块那斜对角能不能走需要单独约束。常见做法是在斜向移动前判断相邻的两个正方向格子是否至少有一个是空地或者两个都是空地才允许斜穿% 以左上为例 if k 5 % 左上 if map(curRow-1, curCol) 1 map(curRow, curCol-1) 1 continue; end end或者更保守一点要求相邻的两个正交格子都不是障碍物才能斜走。具体用哪种取决于你的机器人物理尺寸和地图栅格尺寸的对应关系。如果是大型扫地机器人通常把每个格子的尺寸定成机器人直径的1.2倍以上同时加上上面的墙角禁止逻辑提高安全性。5.4 地图坐标与plot的坐标方向搞反这个问题几乎所有新手都会遇到。矩阵第1行在最上方colormap画出来以后起点在第1行最上方可你习惯性用plot(startRow, startCol, go)画出来的点却跑到地图下方去了因为plot的默认y轴向下是反的。解决办法有两个要么在画完地图后补一句set(gca, YDir, reverse)要么在调用plot时手动转换坐标。我前面的代码已经用了第一种方式。这一点虽然不涉及算法本身但如果你不做处理后面的路径可视化就会完全错乱看起来是绕路其实只是坐标反了。6. 从教学演示到一个真能用的规划模块我的扩展和改造方向跑通基础版A星只是开始。按我的习惯接下来一定会做下面几个方面的加工让它从一个“教学过程”升级成“小而美的规划器”。6.1 路径冗余点压缩A星搜出来的是栅格路径里面有很多连续成直线的中间点比如从(1,1)到(1,5)中间可能经过(1,2)、(1,3)、(1,4)。如果直接把整条路给机器人执行机器人会收到一连串密集路径点控制起来不优雅而且转弯角度不直观。我常用的简化方法是“只保留拐点”从起点开始每次往后看如果下一个点与当前点、再下一个点共线则删除中间点。更复杂一点可以使用Ramer-Douglas-Peucker算法但栅格路径用前者效率足够。6.2 对大规模地图的性能优化思路如果你的地图是500x500甚至更大上面这种简单的openlist线性找最小值就会变慢。瓶颈来自两点一是每次从openList里min(fScore)都要扫一遍二是ismember判断节点是否在openlist里也要扫一遍。优化方向有几个维护一个closed标记用逻辑索引访问矩阵不要反复用ind2sub。openlist可以用二叉堆实现。Matlab里没有内置维护简单的最小堆类但可以写一个类或者用双数组模拟偏排序。如果你的地图尺寸很大这一步价值非常明显。启发权重适当调整。把h乘以一个略大于1的系数比如1.0~1.1搜索会更快但代价是路径不一定严格最优。工程上如果时间优先可以用这种“权重A星”。6.3 根据矩阵尺寸动态调整栅格粒度同样一张环境地图栅格太细会导致节点爆炸栅格太粗会丢失可行路径。我给无人车做测试时一般以车体宽度为单位设定栅格边长。假设车宽0.5米地图40米x40米那80x80的栅格已经比较合适。再细到400x400A星跑起来就有点吃力了这个时候就会考虑把地图分成多层先用粗栅格规划走廊再用细栅格做局部规划。如果你只是做一个课程设计通常20x20到100x100的地图都足够不用过度优化但要把代码结构写成“只要传入map矩阵就自动适配尺寸”的样子而不是把尺寸写死在程序里。6.4 后续可扩展方向基础A星完成后有些方向可以继续往下做动态障碍物当机器人走一段后地图更新了A星就重新规划一次本质上就是D* Lite的思想。多起点多目标在多个车辆同时调度时可以把A星嵌套进循环分别计算距离矩阵再接上贪心或遗传算法做任务分配。结合轨迹平滑A星给出折线路径后续可以用B样条或多项式轨迹生成平滑曲线让路径真正可执行。根据我的实际经验先在Matlab里把A星的地图生成、起终点输入、搜索循环、路径回溯、结果可视化这一整套流程跑通后面换语言、换算法、接真机剩下的都是工程适配问题而不是算法理解问题了。如果你要动手做这个项目我的建议是先照上面的核心代码跑通一个20x20的随机地图然后逐步改成手动交互再考虑各种扩展。等你把障碍物换成自己业务场景的地图数据起点终点改成地图上任意点击的位置A星这个工具就算真正接入了你的工作流。