最近在折腾路径规划项目越做越觉得A这个算法是真的又简单又给力。不管你是做机器人导航、游戏寻路、自动驾驶局部规划还是仓库搬运小车A基本是绕不开的入门首选。我这次就直接用Matlab从零撸了一套带自定义地图的A路径规划代码完全手写不调第三方库整个逻辑摊开给你看保证比课本上的伪代码清楚得多。这篇文章就按我实际开发的顺序来写从算法原理到完整代码从地图定义到可视化跑通再说几个我调试时踩过的坑属于那种你照着敲就能跑通的东西适合刚入门路径规划、或者想彻底搞懂A内部逻辑的同学拿去直接抄作业。1. 项目整体设计与思路拆解1.1 路径规划那么多算法为什么先撸A*路径规划算法一大把Dijkstra、BFS、DFS、A*、RRT、PRM、人工势场、蚁群、遗传……真要对比起来各有各的主场。但A有个特别好的特性它是有信息搜索里最聪明的一个在栅格地图这种离散环境下A能在保证最优路径在启发函数满足一致性条件时的前提下比Dijkstra少搜一大片区域。原因就在于它把搜索方向指向了终点而不是像Dijkstra那样四面八方平均用力。这个特性放到实际工程里就是两个字快。你在游戏里看到的小兵寻路、扫地机器人避开茶几回充、AGV小车在地图里跑底层大概率就是A或者A的变种。所以我一直觉得你要入门路径规划A就是你工具箱里的第一把扳手先把这把扳手攥明白了后面看JPSJump Point Search、DLite那些进阶货才会觉得顺畅。1.2 Matlab做算法验证是真的省事有人说生产环境用C/PythonMatlab就是个玩具。这话我不完全同意。做算法验证和项目原型的时候Matlab的矩阵操作、绘图内置函数、脚本式调式能把从想法到验证的链路压缩到极短。你花10分钟写个可视化函数立刻就能看到路径有没有绕、节点有没有搜多、地图建模对不对。尤其是A*这种需要频繁操作地图矩阵和节点集合的算法Matlab代码可以直接把地图数组画成图像连导出论文插图都好用。所以我的建议是方案阶段用它快速验证验证完了再翻译成C或Python进工程这个节奏非常舒服。1.3 自定义地图用矩阵说话要让A*跑起来第一步不是写算法是把地图定义清楚。我采用的方式是一个二维矩阵矩阵的每个元素代表一个栅格。0表示空地1表示障碍物。比如下面这张8x8地图map [ 0 0 0 0 1 0 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 1 0 1 1 0 1 0 0 0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ];这里map(1,1)是地图左上角map(end,end)是右下角。起点和终点都用[row, col]格式的坐标表示注意Matlab矩阵下标先列后行我全程统一用[row行, col列]免得把自己绕晕。自定义地图的好处是你想测什么场景就搭什么场景比如凹型障碍、狭长通道、岛屿状障碍都可以手工造出来这比拿现成地图去跑更能帮你理解算法特性。2. A*核心原理解析f(n)g(n)h(n)到底在算什么2.1 三个值的关系和意义A*的灵魂就是那行公式f(n) g(n) h(n)g(n)从起点出发已经走到当前节点n累计花掉的实际代价。栅格地图里四方向移动每步代价可以统一设为1所以g值本质上就是从起点到这里走了几步。h(n)从当前节点n到终点用启发函数估算出来的还差多远。这个值是猜的但猜得要合理。f(n)综合上面两者。A*每次从待处理列表里挑f值最小的节点去扩展就是在已经花掉的代价和预计还要花的代价之间做权衡。你可以这么理解g值让算法不会因为单纯喜欢朝着终点猛冲就无视绕路障碍碰得头破血流而h值又拉着算法不至于把所有方向都当爹供着傻乎乎全搜一遍。两者合在一起才让A*既保证最终路径最优又保持搜索高效。2.2 启发函数为什么这题用曼哈顿距离栅格地图上如果只允许上下左右四个方向移动那么从点(row1,col1)到点(row2,col2)的最短距离就不能走斜线只能横着加竖着走。这种情况下可采纳的启发函数最经典的就是曼哈顿距离h abs(row1 - row2) abs(col1 - col2)为什么强调可采纳因为在A里h值永远不能高估实际代价。低估可以顶多多搜几个格子但高估会让算法直接丢弃真正最优的路径。曼哈顿距离在四方向地图上永远不会超过真实代价因为真实代价至少要等于曼哈顿距离你得横着走那么多格、竖着走那么多格所以它一定可采纳A在四方向栅格地图上用它就能保证最终搜出来的路径是最优的。如果你用的是八方向可以斜着走那曼哈顿距离就偏懒了通常会换成切比雪夫距离或者欧几里得距离同时斜向移动的代价要用根号2而不是1。这个待会在问题章节再展开。2.3 开放列表和关闭列表A*的左右手A*运行过程里始终维护两个关键结构开放列表open list已经发现但还没扩展的节点集合。每次循环都从这里挑f值最小的节点。关闭列表closed list已经扩展过的节点集合。这些节点不会再被拿出来扩展。这个机制很像你日常找东西关闭列表是已经翻过的抽屉开放列表是还没翻、但怀疑有可能在其中的抽屉。如果你翻完某个抽屉就没必要再翻第二遍除非后来你找到一条更便宜的路到达这个抽屉——但A*在大多数情况下不会发生这种情况这也是它优化Dijkstra的关键。在Matlab的简单实现里我直接用struct数组来存这两个列表节点信息包括位置pos、g值、h值、f值和父节点parent。父节点是用来最后回溯路径的这个设计是整条路径的组织链条。3. 代码实现与逐段拆解3.1 主函数整体框架astar_path.m下面这是算法的核心主函数。为了让你看的时候不迷路我把整体结构先列出来初始化起点 - 循环搜索 - 取出f值最小节点 - 判断是否到达终点 - 扩展四个邻居 - 更新开放列表 - 循环结束。看代码function path astar_path(map, start, goal) % ASTAR_PATH A*路径规划主函数 % 输入 % map - 二维地图矩阵0可通行1障碍物 % start - 起点坐标 [row, col] % goal - 终点坐标 [row, col] % 输出 % path - 路径坐标矩阵每行为[row, col]若无路径返回空矩阵 [row_num, col_num] size(map); % 初始化开放列表结构体数组 open_list struct(pos, {}, g, {}, h, {}, f, {}, parent, {}); closed_list struct(pos, {}, g, {}, h, {}, f, {}, parent, {}); % 起点节点g0h为到终点的曼哈顿距离 start_node struct(pos, start, g, 0, ... h, heuristic(start, goal), ... f, heuristic(start, goal), ... parent, []); open_list [open_list, start_node]; while ~isempty(open_list) % 从开放列表中找到f值最小的节点 f_values [open_list.f]; [~, idx] min(f_values); current open_list(idx); % 如果当前节点就是终点说明找到了路径 if isequal(current.pos, goal) path reconstruct_path(current); return; end % 把当前节点从开放列表移除加入关闭列表 open_list(idx) []; closed_list [closed_list, current]; % 获取当前节点的可行邻居 neighbors get_neighbors(current.pos, row_num, col_num); for i 1:size(neighbors, 1) n_pos neighbors(i, :); % 跳过障碍物 if map(n_pos(1), n_pos(2)) 1 continue; end % 如果邻居已经在关闭列表里跳过 if is_in_list(n_pos, closed_list) continue; end % 从起点到该邻居的新代价 new_g current.g 1; % 检查邻居是否已经在开放列表里 open_idx find_in_list(n_pos, open_list); if open_idx 0 % 已经在开放列表如果新路径g值更小则更新 if new_g open_list(open_idx).g open_list(open_idx).g new_g; open_list(open_idx).f new_g open_list(open_idx).h; open_list(open_idx).parent current; end else % 没在开放列表创建新节点加入 new_node struct(pos, n_pos, ... g, new_g, ... h, heuristic(n_pos, goal), ... f, new_g heuristic(n_pos, goal), ... parent, current); open_list [open_list, new_node]; end end end % 开放列表耗尽还没到终点说明没有路径 path []; end代码逻辑其实就这么点。注意的是我把open_list和closed_list都声明成带五个字段的空struct这在Matlab里是标准的初始化方式不然后续append会报字段不匹配的错。这个坑我当年第一次写Matlab struct数组时就遇到过先初始化好字段结构再加数据就稳了。3.2 邻居节点扩展get_neighbors.mA*在栅格地图上怎么走取决于你允许哪些方向。我这版先用最经典的四方向上、下、左、右。每步代价都是1这样整个地图就成了一个无向加权图权值处处相等逻辑最简单。function neighbors get_neighbors(pos, row_num, col_num) % GET_NEIGHBORS 获取当前节点在地图范围内的四方向邻居 % 输入 % pos - 当前坐标 [row, col] % row_num - 地图行数 % col_num - 地图列数 % 输出 % neighbors - 邻居坐标矩阵每行为[row, col] dirs [-1 0; % 上 1 0; % 下 0 -1; % 左 0 1]; % 右 count 0; neighbors zeros(4, 2); % 四方向最多4个 for i 1:4 nrow pos(1) dirs(i, 1); ncol pos(2) dirs(i, 2); % 必须在边界内 if nrow 1 nrow row_num ncol 1 ncol col_num count count 1; neighbors(count, :) [nrow, ncol]; end end neighbors neighbors(1:count, :); end这里最关键的是边界检查。我见过很多初学者直接在pos基础上加减1结果负下标或者超出数组维度直接报错或者更隐蔽的越界行为让地图边缘的路径莫名其妙绕圈。边界判断不能省这是算法和地图坐标系统一性的保证。3.3 三个辅助函数启发、查找、回溯启发函数前面推过了直接写成函数function h heuristic(pos, goal) % HEURISTIC 曼哈顿距离启发函数四方向移动适用 h abs(pos(1) - goal(1)) abs(pos(2) - goal(2)); end查找节点是否在某个列表里这里我封装了一个函数同时返回是否找到以及索引。注意这个逻辑对开放列表和关闭列表都适用入参直接传列表就行function idx find_in_list(pos, list) % FIND_IN_LIST 在节点列表中查找指定坐标 % 找到返回索引找不到返回0 idx 0; for i 1:length(list) if isequal(list(i).pos, pos) idx i; return; end end end注意我这里没有单独写是否在关闭列表的函数直接用find_in_list判断返回值是否为0就行。如果你喜欢语义化一点可以再包一层is_in_list但那样会多一重函数调用。Matlab脚本化运行没关系怎么清楚怎么来。路径回溯函数从终点节点沿着parent一路怼回起点function path reconstruct_path(node) % RECONSTRUCT_PATH 从终点节点沿parent回溯到起点生成路径 path []; current_node node; while ~isempty(current_node) path [current_node.pos; path]; current_node current_node.parent; end end这里我用的是头插法每次把当前节点的坐标插到path矩阵的最前面一行这样循环走完之后path第一行就是起点最后一行就是终点。如果你从头往尾拼最后还得多加一个flipud或者reverse没必要。这三个函数加起来就是A*的全部零件了。你把它们和主函数放在同一个目录下再写个主脚本调用就能跑出路径。3.4 主控脚本与可视化demo_astar.m为了让你直接开整我把主控脚本也写好里面包含自定义地图、起点终点、调用算法、可视化四件套%% 主控脚本自定义地图 A*路径规划 clear; clc; close all; %% 1. 自定义地图 map [ 0 0 0 0 1 0 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 1 0 1 1 0 1 0 0 0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]; %% 2. 设置起点和终点 start [8, 1]; goal [1, 8]; %% 3. 调用A*算法求路径 path astar_path(map, start, goal); %% 4. 输出结果与可视化 if isempty(path) disp(没有找到可行路径); else disp(找到路径); disp(path); visualize_path(map, path, start, goal); end顺便把可视化函数也贴出来画地图、障碍、起点终点、路径一次搞定function visualize_path(map, path, start, goal) % VISUALIZE_PATH 把地图和路径一起画出来 figure(Name, A* Path Planning, NumberTitle, off); imagesc(map); colormap(flipud(gray)); axis equal; axis xy; % 关键让坐标轴方向与矩阵的行列方向一致 grid on; hold on; % 画障碍物 [r, c] find(map 1); plot(c, r, ks, MarkerFaceColor, k, MarkerSize, 12); % 画路径 if ~isempty(path) plot(path(:, 2), path(:, 1), r-o, LineWidth, 2.5, MarkerSize, 8); end % 画起点和终点 plot(start(2), start(1), go, MarkerFaceColor, g, MarkerSize, 14); plot(goal(2), goal(1), r^, MarkerFaceColor, r, MarkerSize, 14); legend({障碍物, 路径, 起点, 终点}, Location, best); title(A* 路径规划结果); end注意axis xy这行我刚开始写的时候漏了它结果路径画出来上下颠倒怎么看怎么别扭。这个属于Matlab绘图的经典坑后面问题章节我会再重点谈。4. 实操过程把算法从脚本跑到图4.1 搭建地图和跑通第一次运行我在上面那张8x8地图上搭了一条有点曲折的走廊起点在左下角[8,1]终点在右上角[1,8]中间故意安排了好几堵墙逼迫路径必须绕弯。第一次运行特别顺利脚本输出和期望一致找到路径 8 1 8 2 8 3 7 3 6 3 6 4 6 5 5 5 4 5 3 5 3 6 3 7 2 7 1 7 1 8对照地图看路径确实没有穿墙也没有大幅绕路从起点一步步贴着可通行区域走到了终点。这一步跑通之后后面所有调试都变得有底气了。4.2 反复改地图观察算法行为变化有了这个基础脚本我开始疯狂改地图来观察A的行为。比如把中间某个障碍物挪走让它形成一条直路路径立刻变成从起点直直横穿过去几乎就是一条直线加一段竖线。再把终点塞进一个U型凹槽里你会看到A先尝试靠近终点的方向被墙挡回来后老老实实绕进去整个过程在路径图里都能看得一清二楚。这种自己搭地图自己观察路径的方式比任何理论描述都深刻。我强烈建议你把这张8x8地图换成自己想的场景比如故意造一个死胡同观察A进去之后怎么退出来——其实A没有退的概念它只是开放列表里那个f值最小的节点换到了另一个分支上但视觉上看起来就像它知道死胡同走不通似的。4.3 验证无路径场景另一个必测场景是无解路径把地图中间用一整排障碍物拦住让起点和终点彻底隔开。这种情况A*会把整个连通区域都搜索完开放列表最终变空然后主函数返回空路径。脚本会提示没有找到可行路径可视化里红色路径也就不会画出来。这里有一个值得观察的点你会发现关闭列表覆盖了整个起点所在的连通区域而另一边的区域节点完全没被展开。这说明A*在有解空间里会尽量朝着终点使劲但无解时它会退化成对整个可达区域的遍历直到穷尽。了解这个特性以后你在做工程时就能预判算法在极端情况下要花多长时间。5. 常见问题与排查技巧实录5.1 死循环节点反复进出开放列表我自己早期写A*遇到最恶心的问题就是死循环。症状是程序卡住不退出或者某个节点被反复加入开放列表。排查后发现两个常见元凶第一忘记把扩展过的节点加入关闭列表导致同一个节点被反复拿出来扩展。这个本质上是对A*流程理解不足。注意我代码里经典的流程顺序拿出f最小节点 - 如果是终点就退出扩展它 - 把它从open挪到closed - 再扩展邻居。closed的加入必须和open的移除同时发生中间不能漏。第二比较坐标时用了而不是isequal。当pos是1x2的向量时pos goal返回的是1x2的逻辑向量if判断会出错。这种错误在Matlab里比较隐蔽因为你直接用if pos goal并不总是报错而是可能永远为假或永远为真。统一用isequal是最稳的。5.2 找到的路径不是最优如果你把四方向改成八方向或者把启发函数换成欧几里得距离但步长没改就可能出现路径不是最优的情况。原因在于A*保证最优的前提是h函数可采纳、且每次移动代价一致且为正。八方向移动时你还按每步1来算g值实际上斜着走一步便宜了搜索就会倾向于多走斜线而最终路径长度可能被高估。我建议调试时把每个节点的f、g、h打出来用disp语句或者Matlab的断点工作区查看沿着路径走一遍看看是否有某个节点的f值比起点到它的实际g值还小如果有那基本就是代价设置不一致或h高估了。这种问题理论好写实操debug全靠打印数据。5.3 八方向扩展时斜穿墙角把四方向改成八方向加左上、右上、左下、右下四个斜向邻居之后会遇到一个我在项目中确实踩过的坑路径会斜着擦过障碍物的角。比如障碍在[2,5]路径可能从[2,4]直接斜到[3,5]严格来说这个斜向穿过了障碍角的边缘在栅格地图里要看你的机器人尺寸允不允许。标准解法是在扩展斜向邻居时额外检查两个相邻的正方向是否被堵住。比如从[2,4]斜到[3,5]需要同时检查[2,5]和[3,4]是否有一个是障碍如果是就不允许这个斜向扩展。这本质上是给算法加了一道墙角防切割的判断逻辑非常简单% 伪代码示意斜向扩展前的防切割检查 if dir为斜向 if map(当前行 dirRow, 当前列) 1 || ... map(当前行, 当前列 dirCol) 1 跳过; % 防止斜穿墙角 end end这个细节特别重要尤其你后面做移动机器人时要考虑实际车身尺寸不可让它擦肩而过。5.4 绘图坐标翻转问题前面我特意在可视化函数加了axis xy这里要解释清楚。Matlab的imagesc显示矩阵时默认y轴朝下和矩阵的行号一致第一行在最上面但plot画图时y轴默认朝上。两个函数混用就会出现路径和地图上下颠倒的诡异效果。加上axis xy之后坐标轴方向被强制调整为行号向下递增这样plot的y值和矩阵的行号就对应上了路径和障碍才能正确叠在一个图里。我的建议是地图类可视化一律把axis xy作为固定套路否则图越画越乱。如果你做的是自动驾驶那种全局坐标地图x东y北那可以不要axis xy但要把地图矩阵转换成世界坐标再画这个属于另一个话题了。5.5 大数据地图性能优化方向我想多说一句性能。现阶段用struct数组只是跑通级别地图到100x100、500x500时open列表用线性扫描选f最小节点会明显变慢。优化方向无非三条用二叉堆或优先队列维护开放列表取f最小变成O(log N)这是最关键的优化。节点字段改用Matlab struct数组时注意预分配或者改用containers.Map、表格型(timetable)存储避免频繁append。引入闭节点记录g值的哈希表减少关闭列表线性查找。实际工程里如果是在真实机器人上跑我一般会用C重写这个逻辑并加一些工程优化比如线程池建图、分层寻路。但作为项目原型验证Matlab这套已经完全够用了还能随时改参数看效果这是最省时间的阶段。结尾一点个人经验总结撸完这版A*之后我最大的感受是纸上得来终觉浅这句话放在路径规划里太对了。光看伪代码你可能永远理解不了开放列表和关闭列表为什么会这么设计只有自己调两个列表、画几十次图、看几种极端地形才能真正明白fgh这个公式背后的优雅之处。最后再分享一个小技巧调试A时别急着看结果先在地图上画出搜索过程——每扩展一个节点就把这个节点标记成灰色把当前f值最小的节点标成红色。这种算法在眼前跑的感觉非常直观能让你一眼看出A是怎么被启发函数拉向终点的也能让你发现h设置不合理时搜索范围会有多离谱。我代码里没塞这个动画逻辑你有兴趣可以自己加加完你会觉得A*看起来更聪明了。多试几种地图多跑几个极端场景路径规划的基本功就是这么练出来的。这个项目后续想扩展也顺手改成八方向、加上机器人半径的膨胀层、换启发函数、甚至换成D* Lite做动态环境重规划都是从这个框架里长出去的。希望这篇拆解能帮你把A*从听过名字变成能用顺手。