最近在折腾路径规划项目，越做越觉得A这个算法是真的又简单又给力。不管你是做机器人导航、游戏寻路、自动驾驶局部规划还是仓库搬运小车，A基本是绕不开的入门首选。我这次就直接用Matlab从零撸了一套带自定义地图的A路径规划，代码完全手写，不调第三方库，整个逻辑摊开给你看，保证比课本上的伪代码清楚得多。这篇文章就按我实际开发的顺序来写，从算法原理到完整代码，从地图定义到可视化跑通，再说几个我调试时踩过的坑，属于那种你照着敲就能跑通的东西，适合刚入门路径规划、或者想彻底搞懂A内部逻辑的同学拿去直接抄作业。
1. 项目整体设计与思路拆解
1.1 路径规划那么多算法，为什么先撸A*
路径规划算法一大把，Dijkstra、BFS、DFS、A*、RRT、PRM、人工势场、蚁群、遗传……真要对比起来各有各的主场。但A有个特别好的特性：它是有信息搜索里最"聪明"的一个，在栅格地图这种离散环境下，A能在保证最优路径（在启发函数满足一致性条件时）的前提下，比Dijkstra少搜一大片区域。原因就在于它把搜索方向"指"向了终点，而不是像Dijkstra那样四面八方平均用力。
这个特性放到实际工程里就是两个字：快。你在游戏里看到的小兵寻路、扫地机器人避开茶几回充、AGV小车在地图里跑，底层大概率就是A或者A的变种。所以我一直觉得，你要入门路径规划，A就是你工具箱里的第一把扳手，先把这把扳手攥明白了，后面看JPS（Jump Point Search）、DLite那些进阶货才会觉得顺畅。
1.2 Matlab做算法验证是真的省事
有人说生产环境用C++/Python，Matlab就是个玩具。这话我不完全同意。做算法验证和项目原型的时候，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', {}); % 起点节点：g=0，h为到终点的曼哈顿距离 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.m
A*在栅格地图上怎么走，取决于你允许哪些方向。我这版先用最经典的四方向：上、下、左、右。每步代价都是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*之后，我最大的感受是："纸上得来终觉浅"这句话放在路径规划里太对了。光看伪代码，你可能永远理解不了开放列表和关闭列表为什么会这么设计，只有自己调两个列表、画几十次图、看几种极端地形，才能真正明白f=g+h这个公式背后的优雅之处。
最后再分享一个小技巧：调试A时别急着看结果，先在地图上画出"搜索过程"——每扩展一个节点，就把这个节点标记成灰色，把当前f值最小的节点标成红色。这种"算法在眼前跑"的感觉非常直观，能让你一眼看出A是怎么被启发函数"拉"向终点的，也能让你发现h设置不合理时搜索范围会有多离谱。我代码里没塞这个动画逻辑，你有兴趣可以自己加，加完你会觉得A*看起来更"聪明"了。
多试几种地图，多跑几个极端场景，路径规划的基本功就是这么练出来的。这个项目后续想扩展也顺手：改成八方向、加上机器人半径的膨胀层、换启发函数、甚至换成D* Lite做动态环境重规划，都是从这个框架里长出去的。希望这篇拆解能帮你把A*从"听过名字"变成"能用顺手"。
