这些年陆陆续续接过不少路径规划相关的需求从AGV调度到无人机巡检几乎每次都是先拿Matlab搭一个能看效果的Demo再往工程侧迁移。时间久了我发现自己经常在重复做同一件事用A星算法给一张栅格地图找最短路径然后顺手把起点、终点、障碍物改成可配置的。所以这次干脆把这个常用的A星路径规划项目完整拆一遍——怎么说呢它不算高深但确实是做移动机器人、自动驾驶、游戏AI、仓储机器人这些方向时最常被问到的入门核心项目。这个项目要解决什么问题简单说就是你有一张地图想从起点走到终点A星会帮你规划出一条避开障碍物的可行路径。而具体到本项目的亮点在于“可自定义”地图可以自己改障碍物想怎么摆就怎么摆起点和终点坐标也能随意设定。再配合Matlab的绘图能力运行完能直观看到搜索过程、最终路径和探索范围。如果你是刚接触路径规划的学生或者在做仿真验证的工程师这个Demo能帮你快速理解A星的计算逻辑也能省下不少重复造轮子的时间。下面我就从需求拆解、原理、代码实现、交互设计到调试经验把整个项目的思路和细节一次讲清楚。1. 项目需求拆解与方案选型1.1 它到底在解决什么问题只看标题“A星路径规划算法Matlab实现A星算法可自己改变地图和障碍物自定义起点坐标和终点坐标”其实藏着三层需求。第一层算法需求。要用A星而不是Dijkstra、BFS或者RRT。A星兼顾最优性和搜索效率尤其适合栅格地图上的静态路径规划。第二层实现需求。明确指定Matlab说明使用者需要的是快速验证、可视化方便的环境而不是追求极致性能的C。第三层交互需求。“可自己改变地图和障碍物、自定义起点终点”是核心这意味着不能是一份写死数据的代码而是要把地图、障碍物、起终点都做成输入参数让使用者随时改、随时跑。把需求翻译成功能模块这个项目需要四部分栅格地图生成模块、障碍物编辑模块、A星搜索核心模块、路径可视化模块。地图用二维矩阵表达障碍物是矩阵中的标记位起点终点是两个坐标变量A星核心负责搜索Matlab绘图负责把结果画出来。逻辑非常清晰。1.2 A星算法的核心原理为什么快为什么优A星在路径规划里之所以经典是因为它在搜索中加入了一个“方向感”。这个方向感来自评价函数f(n) g(n) h(n)其中g(n)是从起点到当前节点n已经付出的实际代价h(n)是当前节点n到终点的预估代价也就是启发函数。A星每一步都优先扩展f值最小的节点让搜索既能覆盖到已走过的区域又始终朝着终点方向前进而不是像Dijkstra那样一圈一圈毫无目标地往外扩。h(n)的设计直接决定了算法行为。如果h(n)始终为0A星退化成Dijkstra能找到最短路径但很慢。如果h(n)始终等于真实代价且满足一致性条件A星能最快找到最优路径。如果h(n)偶尔高估真实代价搜索会更快但不保证最优。所以在栅格地图里常用的启发函数是曼哈顿距离或者欧几里得距离它们都能保证不低估代价从而保证路径最优。用生活化的类比解释你在一个陌生的商场里找某个店铺g是你已经走过的距离h是你目测离目标还有多远f是“已经走的加上估计还要走的”。真正聪明的人不是盲目乱逛而是每次都挑“总路程估计最少”的方向走这其实就是A星的思路。1.3 为什么选Matlab做这个项目有人可能会问路径规划用Python不香吗为什么会用Matlab我的判断是这类项目选Matlab有三个不可替代的优势。第一矩阵天然是地图模型。一张栅格地图就是一个二维矩阵0代表空地1代表障碍物。在Matlab里生成随机地图、修改障碍物、做合法性判断代码量非常少。第二可视化极其方便。imagesc、pcolor、plot几行代码就能把地图、探索区域、最终路径画出来而且自带坐标轴调试时一眼就能看出问题。第三Matlab的矩阵操作和函数式编程风格很适合算法验证改写参数、测试边界条件都很顺手。当然Matlab性能一般真做机器人部署还是得换C或Python。但作为算法原理验证和教学演示工具Matlab在这个场景下是性价比最高的选择。2. 地图建模与数据设计万丈高楼的地基2.1 用二维矩阵表达栅格地图A星跑在地图上地图怎么存决定了后面所有代码的写法。我的建议是直接用二维矩阵map规则定死0表示可通行区域1表示障碍物。这种做法最直观也最符合Matlab的习惯。比如要生成一张10乘10的地图map zeros(10, 10); % 全空地 map(2, 2) 1; % 单个障碍物 map(3, 1:4) 1; % 一行障碍物 map(5:8, 6) 1; % 一列障碍物运行完成后map这张矩阵就是A星搜索的地图基础。每次搜索时只需判断当前节点坐标对应的map值是否为1就知道能不能走。如果你想要随机地图敲一行rand就能搞定。下面这段代码我经常用在Demo里生成一张障碍物密度可调的随机地图map zeros(20, 20); obstacle_prob 0.25; % 障碍物密度 map(rand(20, 20) obstacle_prob) 1; map(1, 1) 0; % 保证起点不被堵死 map(end, end) 0; % 保证终点不被堵死注意随机地图必须要手动确认起点和终点不是障碍物否则算法一开始就会报错或者返回空路径。这个坑是我之前踩过的后面会专门讲。2.2 起点与终点的坐标定义和合法性校验自定义起点和终点我习惯把它们存成两个二元向量start_pos和goal_pos。例如start_pos [1, 1]; goal_pos [20, 20];每个向量第一个元素是行坐标y轴第二个元素是列坐标x轴。这里有个非常容易搞混的地方Matlab的矩阵索引是row-major也就是map(row, col)row是行、col是列。如果你心里想的是笛卡尔坐标系的(x, y)在填map值的时候就容易把行列写反。我的建议是整个项目从头到尾统一用[行, 列]来表示坐标不要一会用(x, y)一会用[row, col]否则debug会怀疑人生。搜索之前必须有合法性校验至少检查三件事起点是否在地图范围内、终点是否在地图范围内、起点和终点是否落在障碍物上。代码可以这样写function [start_pos, goal_pos] validatePosition(map, start_pos, goal_pos) [rows, cols] size(map); assert(start_pos(1) 1 start_pos(1) rows ... start_pos(2) 1 start_pos(2) cols, 起点越界); assert(goal_pos(1) 1 goal_pos(1) rows ... goal_pos(2) 1 goal_pos(2) cols, 终点越界); assert(map(start_pos(1), start_pos(2)) ~ 1, 起点是障碍物); assert(map(goal_pos(1), goal_pos(2)) ~ 1, 终点是障碍物); end这个函数写好后主程序里调用一次能把大量低级错误挡在外面。2.3 为什么用栅格地图而不是几何地图可能有读者会想真实机器人不是用矢量地图吗为什么这个项目用栅格。原因是栅格和A星是绝配。A星本质上是在离散图结构上搜索栅格地图天然提供了“当前节点周围有哪些邻居”的邻接关系计算量可控代码也简单。而矢量地图、拓扑地图往往还需要额外构建路网之后才能跑A星。栅格地图的代价也很明显精度受分辨率限制地图越大内存和计算量越大。比如一张1000乘1000的地图节点数就是100万个纯Matlab跑会有点吃力。但对于教学和中小规模仿真栅格地图是够用的。这也是本项目“可自定义地图”的意义所在——你可以改小图做测试也可以改大图看性能。3. Matlab实现A星算法的核心代码逐段拆解3.1 open list和close list到底怎么存A星的核心数据结构是open list待探索节点集合和close list已探索节点集合。open list存的是“发现但还没正式扩展”的节点每次从里面挑f值最小的节点出来close list存的是“已经被扩展过”的节点防止重复造访。在C里你可能会用priority_queue加unordered_set。在Matlab里我推荐用结构体数组来存节点信息理由是这样逻辑清楚、不容易出bug。每个节点需要保存这些字段row行坐标col列坐标g从起点到该节点的实际代价h从该节点到终点的预估代价fg加hparent父节点的索引用来最后回溯路径在Matlab里定义一个节点结构node struct(row, 0, col, 0, g, 0, h, 0, f, 0, parent, 0); openList repmat(node, 0); closeList repmat(node, 0);repmat(node, 0)用来初始化一个空结构体数组后面追加节点时用openList(end1) newNode就能动态扩容。虽然动态扩容比预分配慢但先把功能跑通最重要性能优化后面再说。3.2 邻居扩展与障碍物判断怎么写才稳A星每轮从open list取出f值最小的节点然后扩展它的邻居。邻居的定义有两种四邻域和八邻域。四邻域只走上下左右移动代价都是1。八邻域可以斜着走横竖代价是1斜对角代价是sqrt(2)。两种各有适用场景四邻域适合模拟只能横向/纵向移动的机器人八邻域更适合移动范围更自由的场景。我写的邻居扩展函数如下同时支持四邻域和八邻域function neighbors getNeighbors(map, node, use_diagonal) [rows, cols] size(map); directions [1, 0; -1, 0; 0, 1; 0, -1]; if use_diagonal directions [directions; 1, 1; 1, -1; -1, 1; -1, -1]; end neighbors []; for i 1:size(directions, 1) nr node.row directions(i, 1); nc node.col directions(i, 2); if nr 1 nr rows nc 1 nc cols map(nr, nc) 0 neighbors(end1).row nr; %#okAGROW neighbors(end).col nc; end end end这里要注意两个很容易被忽略的细节。第一越界判断必须在访问map(nr, nc)之前做否则Matlab会直接报错。第二如果启用八邻域需要考虑“斜穿墙角”问题。比如当前节点在障碍物的左上角右上角又是障碍物此时斜着走到右下角虽然在数学上没碰到障碍物格子但机器人物理上可能过不去。更严谨的做法是在斜向移动前检查相邻两个正交格子是否也通行。对于简单Demo我一般直接允许斜穿但会把启发函数换成对角线距离来匹配斜向移动的真实代价。3.3 主循环实现从起点到终点的搜索过程A星主循环的逻辑可以概括成三步取f最小的open节点、扩展它的邻居、更新open和close列表。这段Matlab代码我写了很多遍下面这版是我认为可读性和正确性比较平衡的版本function path aStar(map, start_pos, goal_pos, use_diagonal) [rows, cols] size(map); startNode.row start_pos(1); startNode.col start_pos(2); startNode.g 0; startNode.h heuristic(startNode, goal_pos); startNode.f startNode.g startNode.h; startNode.parent 0; openList startNode; closeList struct(row, {}, col, {}, g, {}, h, {}, f, {}, parent, {}); numExpanded 0; while ~isempty(openList) % 找f值最小的节点 [~, idx] min([openList.f]); current openList(idx); openList(idx) []; numExpanded numExpanded 1; % 判断是否到达终点 if current.row goal_pos(1) current.col goal_pos(2) path reconstructPath(closeList, current); fprintf(扩展节点数: %d\n, numExpanded); return; end % 把当前节点加入closeList closeList(end1) current; % 扩展邻居 neighbors getNeighbors(map, current, use_diagonal); for i 1:length(neighbors) nb neighbors(i); if isInList(closeList, nb.row, nb.col) continue; end g_new current.g getMoveCost(current, nb); h_new heuristic(nb, goal_pos); f_new g_new h_new; idx_open isInList(openList, nb.row, nb.col); if idx_open 0 % 如果节点已经在open list中且新路径更优则更新 if g_new openList(idx_open).g openList(idx_open).g g_new; openList(idx_open).f f_new; openList(idx_open).parent findNodeIndex(closeList, current); end else % 否则作为新节点加入open list nb.g g_new; nb.h h_new; nb.f f_new; nb.parent findNodeIndex(closeList, current); openList(end1) nb; %#okAGROW end end end % open list为空说明无解 path []; fprintf(无可行路径\n); end这里有个实现细节值得展开parent要怎么存。我用closeList中的节点索引来存父节点例如current.parent 3表示父节点是closeList中的第3个元素。这样回溯路径时非常方便直接沿着父节点索引往回收。因为current即将加入closeList所以在扩展邻居时用findNodeIndex(closeList, current)来获取current在closeList中的位置。当然如果你不想用索引回溯也可以在节点结构里直接存父节点的row和col回溯时手动匹配。但用索引更快、更不容易出错。3.4 路径回溯与可视化直观看到最终结果找到终点节点后回溯路径是整个算法最简单也最容易写错的一步。我的回溯函数长这样function path reconstructPath(closeList, goalNode) path [goalNode.row, goalNode.col]; parentIdx goalNode.parent; while parentIdx 0 node closeList(parentIdx); path [node.row, node.col; path]; %#okAGROW parentIdx node.parent; end endpath是一个N行2列的矩阵第一行是起点最后一行是终点。用循环把父节点一个个往前推直到parentIdx为0说明回到起点。可视化这一步是我最喜欢写的。用imagesc把地图画出来黑色是障碍物白色是空地再用hold on画起点、终点和路径figure; imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(start_pos(2), start_pos(1), go, MarkerSize, 10, LineWidth, 2); plot(goal_pos(2), goal_pos(1), ro, MarkerSize, 10, LineWidth, 2); if ~isempty(path) plot(path(:,2), path(:,1), b-, LineWidth, 2); end hold off;注意这里plot的时候横坐标传的是path(:,2)也就是列坐标纵坐标传的是path(:,1)也就是行坐标。这正是之前反复强调的行列坐标问题如果搞反了画出来的路径就是镜像的。4. 自定义地图、障碍物与起终点的三种玩法4.1 玩法一脚本里直接改参数最简单如果你只是想快速跑通直接在脚本里改地图矩阵和起终点坐标就行。这是最朴素的方式适合算法验证。比如想测试一条经典U形障碍物路径可以这样map zeros(20, 20); map(5:10, 8) 1; map(5:10, 12) 1; map(5, 8:12) 1; map(10, 8:12) 1; start_pos [1, 1]; goal_pos [20, 20];这个U形障碍物构造出来后A星必须绕到下方或者上方才能过去能很直观地看到搜索区域的扩展过程。我建议调试阶段多用这种特殊构造的地图因为它能暴露算法中的很多逻辑漏洞。4.2 玩法二给Demo加交互界面用鼠标点出障碍物如果要做演示或者给不熟悉代码的人用脚本改参数就不够友好了。更好玩的是用Matlab的绘图回调实现鼠标交互用户在图上的空地点击就能把格子标记成障碍物右键点击空地设为起点左键双击设为终点。一个简单版本是这样做的先用imagesc画出地图然后用ginput等待用户点击根据点击位置换算成行列坐标再修改map矩阵。但这种做法交互感不强。如果要做得更顺手可以用WindowButtonDownFcn回调每次鼠标点击都触发自定义函数figure; imagesc(map); colormap(gray); axis equal; set(gcf, WindowButtonDownFcn, (src, event) mouseCallback(src, event, map));回调函数里通过鼠标位置换算行列function mouseCallback(src, ~, map) pt get(gca, CurrentPoint); col round(pt(1, 1)); row round(pt(1, 2)); if row 1 || row size(map, 1) || col 1 || col size(map, 2) return; end % 左键设置障碍物右键设置终点中键设置起点 switch get(src, SelectionType) case normal map(row, col) 1; case alt goal_pos [row, col]; case extend start_pos [row, col]; end % 重新绘图并调用A星 updateAndReplan(map, start_pos, goal_pos); end需要说明的是这只是交互框架实际项目里还需要把map、start_pos、goal_pos定义成全局变量或者用嵌套函数共享数据。Matlab的GUI写法有很多讲究我的经验是先把所有逻辑写在普通函数里再包一层回调不要一上来就搞GUI设计器那样会非常难调试。4.3 玩法三把地图加载和保存做成文件读写还有一类场景需要提前定义好地图文件仿真时直接加载。比如地图是一张png图片黑色像素表示障碍物白色表示空地。Matlab里读图和转矩阵只需几行img imread(map1.png); map rgb2gray(img) 128; % 深色区域视为障碍物 map double(map);反过来你也能把随机生成的地图存成图片或者mat文件下次直接load出来。这个玩法适合做系列实验把地图设计、算法测试、结果对比分成不同阶段地图文件来回复用。我个人最推荐的组合是脚本参数版本用来调试算法鼠标交互版本用来演示文件读写版本用来批量实验。三者都对“可自定义地图和障碍物”这个需求负责但使用场景不同。5. 实操记录从零把一个可交互A星Demo跑起来5.1 第一步搭一个最小框架先打通主流程我不建议一开始就把代码拆成十几个函数太容易把思路绕进去。我的做法是先写一个能跑的“直筒子”脚本所有逻辑放在同一个文件里流程顺序是生成地图、设置起终点、初始化open/close list、while循环搜索、回溯路径、画图。这段“直筒子”代码跑通之后再开始重构把邻居扩展、启发函数、回溯等模块拆出去。好处是每一步的改动范围都被控制住了出了问题能立刻定位。你要是一上来就照着模块化工程写调试起来反而麻烦。5.2 第二步逐步加入自定义地图和交互功能当你验证了核心算法没问题再开始给代码加交互层。第一步加一个随机地图生成函数让地图能换第二步加起点终点的校验函数让坐标能随便改第三步加鼠标交互让演示的时候能现场改障碍物和起终点。这个顺序也是我强烈推荐的。核心A星是一台发动机交互是外壳。发动机没跑稳之前外壳做得再好也没用。很多初学者一上来就想做GUI结果GUI画了一大堆算法却根本没调通最后Debug时间翻倍。5.3 第三步一个可以直接运行的精简版Demo为了让你能快速上手我整理一个精简版完整代码。它包含随机地图生成、A星搜索、路径绘制三个功能去掉了交互层但保留了核心结构。这段代码在Matlab R2020b以上版本都能直接运行% A星路径规划精简版Demo close all; clear; clc; % 生成20x20地图20%随机障碍物 map zeros(20, 20); map(rand(20, 20) 0.2) 1; map(1, 1) 0; map(20, 20) 0; % 自定义起终点 start_pos [1, 1]; goal_pos [20, 20]; % 运行A星 path aStar(map, start_pos, goal_pos, true); % 可视化 figure; imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(start_pos(2), start_pos(1), go, MarkerFaceColor, g, MarkerSize, 8); plot(goal_pos(2), goal_pos(1), ro, MarkerFaceColor, r, MarkerSize, 8); if ~isempty(path) plot(path(:,2), path(:,1), b-, LineWidth, 2); fprintf(路径长度: %.2f\n, sum(sqrt(sum(diff(path).^2, 2)))); else disp(没有找到路径); end hold off;运行时如果没找到路径多半是随机地图把起点和终点隔死了多跑几次或者调低障碍物密度就能看到效果。运行成功后你会看到一张灰底图黑点是障碍物绿点是起点红点是终点蓝线是A星找到的路径。6. 常见问题与排查技巧实录6.1 起点和终点相同或者相邻如果起点等于终点A星在第一次循环就会命中终点判断直接返回一个只包含起点坐标的path。这个逻辑本身没问题但如果你在可视化里画线只会看到一个孤零零的点。如果起点和终点相邻A星应该能一步到位。但如果起点和终点中间隔着一个障碍物而你又用的是四邻域那么可能绕远路或者无解。这不是bug而是地图设计的必然结果。排查此类问题建议打印起点、终点、以及搜索到终点时扩展的节点数和路径长度确认是否符合直觉。6.2 明明有路却提示无路径这是新手最容易遇见的诡异问题。代码逻辑没问题但就是返回空path。我的排查经验是优先检查起点和终点的合法性是不是终点被标记成了障碍物是不是地图坐标和绘图坐标不一致。还有一个隐蔽的坑随机地图里虽然你把map(start_pos(1), start_pos(2))和map(goal_pos(1), goal_pos(2))置为0但如果用鼠标交互时点击的行列超过地图边界就有可能把终点设成了障碍物。所有鼠标交互代码里第一件事一定是做边界判断否则很容易出现“地图上看起来有路但算法返回无解”的诡异现象。6.3 八邻域斜穿墙角路径看着怪八邻域搜索很自由路径却可能沿着障碍物边缘斜穿过去视觉上像“穿墙”。比如左上角是障碍物当前节点要往右下走斜穿的两个相邻格子都是障碍物但斜对角本身不是障碍物A星就会直接穿过去。解决办法有两种。第一种是禁用斜穿把use_diagonal设为false。第二种是加一个穿墙检测逻辑只有当水平方向和垂直方向的邻居都可通行时才允许斜向移动。改进后的邻居扩展函数可以这样写if use_diagonal % 只允许通过“拐角”的斜向移动 if map(nr, current_col) 1 || map(current_row, nc) 1 continue; % 禁止斜穿 end end我个人建议做移动机器人项目时默认启用穿墙检测因为真实机器人是有物理尺寸的斜穿墙角往往不可行。6.4 运行速度慢地图一大就卡死Matlab跑A星地图一大就容易卡。主要原因有几个open list用数组动态追加频繁扩容每次用min([openList.f])找最小f值复杂度O(n)回溯路径时用path [node.row, node.col; path]每次都在矩阵头部插入导致矩阵反复移动。能快速见效的优化有三招。第一open list用预分配的数组加计数器而不是动态追加。第二把f值单独存成一个数组min函数只对这个数组操作最小化查找成本。第三回溯路径时先收集后反转或者用正序追加最后用flipud翻转避免头部插入。如果你追求极致性能可以自定义一个最小二叉堆来维护open list。但说实话在Matlab里手动实现堆的代码量不小对于几千个节点的地图优化数组方案已经足够没必要一上来就上堆。6.5 坐标行列搞混路径镜像错位这是Matlab A星项目里发生率最高的问题没有之一。因为Matlab矩阵索引是map(row, col)但很多人的直觉是笛卡尔坐标(x, y)。一旦map的生成、A星内部坐标、绘图plot三者之间的行列约定不一致就会出现路径画出来完全错位、镜像翻转、甚至跑到地图外面。我的解决办法是写项目的第一行注释把坐标系约定写死。% 坐标系约定所有坐标都用[row, col]表示row为纵轴col为横轴然后map矩阵、起点、终点、路径、绘图全部遵循这个约定。在plot里横轴传col纵轴传row。这样就不会乱了。7. 这个项目还能怎么扩展如果你做完基础Demo还有余力可以从几个方向继续深入。第一个方向是换启发函数尝试欧几里得距离、对角线距离、加权曼哈顿距离观察它们对搜索效率和路径长度的影响。第二个方向是引入动态障碍物在A星基础上加一个重规划机制当检测到前方路径被堵时重新规划路线。第三个方向是升级多机器人场景多台机器人同时规划路径时需要引入冲突搜索和时间窗约束这时A星可以作为底层搜索函数被上层算法调用。从我个人经验看这个“Matlab可自定义地图障碍物和起终点”的A星Demo是路径规划里极少数的“一个项目打通多个知识模块”的练手项目。你能从中学到的不只是A星本身还有地图建模、算法可视化、交互设计、坐标系规范、性能优化这些在工程中都会反复用到的通用功。有一句话我说过很多次项目不怕小就怕没做透。把A星跑通只是第一步真正把它扩展成可交互、可换参数、可复用的模块才是这份代码最大的价值所在。