基于MATLAB的A*算法机器人栅格地图路径规划实现与优化

简介基于A算法的机器人路径规划MATLAB实现包面向机器人、自动化及计算机等相关方向的学习者与开发者针对栅格地图环境下从起点到终点的最短路径搜索问题提供一套可直接运行与学习的完整实现。A算法融合Dijkstra的全局最优与贪婪最佳优先搜索的高效性通过评估函数f(n)g(n)h(n)动态调整搜索方向兼顾最优性和实时性。压缩包共17个文件包含5个m源码文件如A星主程序、启发式函数、节点检测、历史记录等、11个bmp测试地图以及1个md说明文档整体大小仅35KB轻量易用并配有多种障碍地图方便对比验证。目前已有3163人学习下载适用于课程设计、科研入门或竞赛备赛。源码清晰注释完整涵盖地图读取、启发式代价计算、开放/关闭列表维护、路径回溯与可视化等关键环节读者既可从零理解算法运行机制也可修改地图和起终点扩展实验快速验证算法在不同场景下的表现。1. 为什么做机器人路径规划总要先把A*跑通1.1 从一次机器人卡墙角的调试说起我第一次在真实机器人上做路径规划时想法特别简单——给机器人一张地图、一组起点和目标点让它自己走过去就行。结果第一步就翻车了机器人只会闷头往目标方向冲被墙角、茶几、充电座围住以后彻底卡死电机原地嗡嗡响怎么都不肯绕路。后来我把那一整段激光雷达扫出来的地图导到MATLAB里一格一格打印在纸上画路径才彻底想明白问题我缺的不是运动控制而是能在二维栅格地图上找到无碰撞通道的算法层。A算法对机器人路径规划的意义就像排序算法对数据结构教学一样——它不一定是你最终上线用的方案但一定是你理解搜索类路径规划最好的起点。你不需要昂贵的传感器、不需要复杂的规划库只要一个MATLAB矩阵、几十行核心代码就能直观看到启发式搜索如何在栅格地图上一步步逼近可行路径。这也是我写这篇文章的动机把一个做过很多遍的MATLAB A实现从原理到代码再到工程坑系统整理一遍。这篇文章适合两类人一是正在做机器人方向课程设计、竞赛或者毕设被用MATLAB实现A*路径规划这个题目卡住的同学二是已经在用各种路径规划库但总感觉原理没吃透、想改不会改的开发者。如果你属于第一类我建议别急着抄代码先把第2节读透后面所有的代码你都会看得非常顺。1.2 四种经典搜索算法一轮实测Dijkstra、BFS、贪心优先与A*为了说清楚A为什么是栅格地图路径规划的默认选择我在同一张30×40栅格地图、同一组起终点上分别用BFS、Dijkstra、贪心最佳优先和A各跑了一遍。地图障碍物占比约30%8邻域移动。以下是一次典型测试的实测数据算法使用的信息是否保证最优路径扩展节点数耗时BFS无权图层数是无权网格4420.21sDijkstrag(n) 实际代价是4010.19s贪心最佳优先h(n) 估计代价否860.05sA*g(n) h(n)是启发式可采纳1570.09sBFS和Dijkstra把地图里大片无关区域都扩展了一遍虽然路径一定最优但浪费了大量计算。贪心最佳优先特别有意思它只凭启发式一股脑往目标方向冲所以扩展节点最少但路径经常绕过障碍物时绕出很多无用的U形弯。A*的数据是两者之间最均衡的路径最优扩展节点又明显少于Dijkstra。这里我想强调一个很容易被当成废话的结论A并没有比Dijkstra更复杂的原理它只是在Dijkstra的代价函数里多加了启发式项就让搜索方向从四面八方收窄成朝目标方向收拢。这也是为什么工程上只要地图能用栅格表达大家第一反应都会是A。2. 深入理解A*的代价公式三个决定搜索走向的关键点2.1 f(n)g(n)h(n)搜索不再瞎转A*的核心逻辑就一个公式f(n) g(n) h(n)。g(n)从起点到当前节点n的实际累计代价这是Dijkstra已经在做的工作h(n)从当前节点n到目标的估计代价也叫启发式函数f(n)从起点经过节点n再到目标的估计总代价。搜索时算法每次从开列表中取出f值最小的节点进行扩展相当于同时看两个维度既然出发点到这儿的代价已经那么高了与其走这条路不如换一个看起来离目标更近的方向试试。没有g算法就退化成贪心容易被局部地形欺骗没有h算法就退化成Dijkstra把整个地图扫一遍才找到目标。A*的价值就在于把两者粘在一起用h做方向盘用g做安全带。用生活类比解释一下你在一个陌生商场找某个店铺。完全不知道方向的人BFS只能从入口一圈一圈往外找知道大致方向的人A*会先朝那个方向走每到一个岔路口又回头看看自己走了多远g再结合离目的地还有多远h做判断。这个边走边校正的过程就是A*在栅格地图上发生的事。2.2 启发式函数怎么选曼哈顿、欧几里得、切比雪夫与八分位距离启发式函数的选型直接决定A*的搜索效率甚至决定路径是不是最优。在栅格地图上启发式的选择必须和移动方式强相关移动方式推荐启发式表达式特点4邻域曼哈顿距离|dx| |dy|精确可采纳效率高8邻域八分位距离(dxdy) (√2-2)×min(dx,dy)更精确推荐8邻域欧几里得距离sqrt(dx²dy²)可采纳但偏小扩展节点多8邻域切比雪夫距离max(|dx|, |dy|)会高估对角线代价一般不推荐这里给第一次接触A的同学提个醒可采纳性是A保证最优路径的数学条件意思是h(n)不能大于节点n到目标的真实最短距离。如果h高估了算法虽然跑得快但可能丢掉最优解。欧几里得距离永远是直线距离所以在栅格上它总是小于等于实际走的折线距离因此可采纳但正因为过于保守搜索时扩展的节点会更多。我实测下来同一张地图上用欧几里得距离比八分位距离大约多扩展20%的节点这在动辄上百米地图的机器人场景里是不能忽视的浪费。代码里我建议直接写八分位距离function h diagonalHeuristic(node, goal) dx abs(node(2) - goal(2)); dy abs(node(1) - goal(1)); if dx dy h (dx - dy) sqrt(2) * dy; else h (dy - dx) sqrt(2) * dx; end end这个表达式的思路是先把能对角线走的部分用√2的代价走掉剩下的直线部分再用1的代价走算出来的h就是8邻域移动下最精确的估计值。2.3 4邻域与8邻域路径质量和计算量的直接权衡在栅格地图上移动方式通常分两种4邻域只允许上下左右移动8邻域允许再叠加对角移动。8邻域路径往往更短更自然但它有两个副作用。第一个副作用是穿角问题如果允许任意对角移动机器人可能从一个格子的角上擦着障碍物边穿过去这在栅格地图上就是贴着墙角的斜线。真实机器人是有体积的这种路径执行起来十有八九会撞。解决办法是在生成邻居时加一个角落检查如果对角移动经过的两个正交邻居中任意一个是障碍物就禁止这次对角移动。比如从(r,c)移动到(r1,c1)必须先确认map(r,c1)和map(r1,c)都不是障碍物。第二个副作用是代价设置错误导致路径劣化很多初学者把对角移动代价也设成1这在8邻域下等于告诉算法斜着走和横着走一样省力路径会变得特别直愣愣而且破坏启发式的一致性。正确做法是横向纵向代价为1对角代价为√2。3. MATLAB中的栅格地图建模与数据结构设计3.1 栅格地图矩阵表达与行列坐标陷阱MATLAB里最常见的栅格地图表达方式是用矩阵0表示可通行区域1表示障碍物。比如下面这段代码就构造了一个20×30的地图四周是墙中间放了几个矩形障碍物map zeros(20, 30); map(1:20, 1) 1; map(1:20, 30) 1; map(1, 1:30) 1; map(20, 1:30) 1; map(5:8, 10:14) 1; map(12:16, 5:8) 1; map(10:14, 20:24) 1; map(3:6, 18:21) 1;这里有一个非常容易踩的坑MATLAB的矩阵索引从1开始而且第一个维度是行号第二个维度是列号。很多从C或者Python转过来的同学习惯把坐标写成(x, y)然后在MATLAB里用map(x, y)去访问最后路径全错位一个格子找半天都找不出原因。我的做法是在代码里统一用(r, c)表示节点位置r是行号、对应y方向c是列号、对应x方向到可视化阶段再用plot(c, r)把矩阵坐标映射到笛卡尔坐标的图上。这样虽然多一道转换但思路混乱的概率小很多。3.2 开列表、闭列表在MATLAB中的实现选型A*需要维护两个核心数据结构闭列表closed记录已经完成扩展、不用再看的节点。因为节点是二维栅格上的位置直接用一个和地图等大的布尔矩阵最合适closed(r, c) true表示这个格子已经扩展过了。开列表open记录候选节点需要支持三个操作取出f值最小的节点、更新某个节点的f值、判断某节点是否已经在列表里。开列表的实现是最能看出代码水平的地方。初学者最常见的写法是用结构体数组或cell数组存节点然后在循环里反复用find去查这个节点在不在列表里。地图一旦到100×100整个脚本肉眼可见地变慢。我实测过一个150×150的地图朴素写法跑了将近6秒预分配数组并增加标记矩阵之后压到0.4秒以内。对于教学演示和小尺寸地图我推荐下面这套方案openList zeros(rowN * colN, 3); % 预分配避免动态扩容 openLen 0; inOpen false(rowN, colN); % 标记节点是否在开列表中openList里每行存[r, c, f]openLen记录当前实际长度。取最小f的节点用[~, idx] min(openList(1:openLen, 3))节点是否在开列表里直接用inOpen(r, c)判断O(1)完成。唯一麻烦的是更新节点f值时要遍历开列表找到对应位置但这一步在地图不算特别大时可以接受。3.3 性能杀手动态扩充数组与无谓的find搜索为什么预分配数组对A*这么关键因为MATLAB的循环里每次执行openList [openList; newRow]都会触发动态数组扩容而搜索过程可能几万个节点扩容的内存拷贝开销非常可观。我建议直接用zeros(rowN*colN, 3)一次性把开列表长度上限设为地图节点总数再用openLen维护长度。这样省掉的不仅是分配时间还避免了MATLAB在循环里自动判断数组大小的额外开销。第二个性能杀手是find。有人习惯用find(openList(:,1) nr openList(:,2) nc)来判断节点是否已在开列表这个操作每次都把整个开列表扫描一遍。换成inOpen(nr, nc)这样的布尔标记后查找变成常数时间。整个算法的时间复杂度就从O(n²)附近降到一个非常可接受的水平。第5节我会给出具体的地图尺寸和耗时对比。4. 一步一步在MATLAB中实现A*核心代码与逐步拆解4.1 主函数框架与参数定义下面给出一个完整可跑的A*实现8邻域移动使用对角线启发式。我把它封装成函数方便不同地图、起终点反复调用function [path, closed, expandCount] astar_grid(map, start, goal) [rowN, colN] size(map); gCost inf(rowN, colN); fCost inf(rowN, colN); parentR zeros(rowN, colN); parentC zeros(rowN, colN); inOpen false(rowN, colN); closed false(rowN, colN); openList zeros(rowN * colN, 3); openLen 0; expandCount 0; gCost(start(1), start(2)) 0; fCost(start(1), start(2)) diagonalHeuristic(start, goal); openLen openLen 1; openList(openLen, :) [start(1), start(2), fCost(start(1), start(2))]; inOpen(start(1), start(2)) true; while openLen 0 [~, idx] min(openList(1:openLen, 3)); cur openList(idx, :); r cur(1); c cur(2); if r goal(1) c goal(2) path traceBack(parentR, parentC, start, goal); return; end openList(idx, :) openList(openLen, :); openLen openLen - 1; inOpen(r, c) false; closed(r, c) true; expandCount expandCount 1; for dr -1:1 for dc -1:1 if dr 0 dc 0 continue; end nr r dr; nc c dc; if nr 1 || nr rowN || nc 1 || nc colN continue; end if map(nr, nc) 1 || closed(nr, nc) continue; end % 禁止对角穿角 if dr ~ 0 dc ~ 0 if map(r, nc) 1 || map(nr, c) 1 continue; end end moveCost sqrt(dr^2 dc^2); tentativeG gCost(r, c) moveCost; if tentativeG gCost(nr, nc) gCost(nr, nc) tentativeG; fCost(nr, nc) tentativeG diagonalHeuristic([nr, nc], goal); parentR(nr, nc) r; parentC(nr, nc) c; if inOpen(nr, nc) for k 1:openLen if openList(k, 1) nr openList(k, 2) nc openList(k, 3) fCost(nr, nc); break; end end else openLen openLen 1; openList(openLen, :) [nr, nc, fCost(nr, nc)]; inOpen(nr, nc) true; end end end end end path []; end这段代码的几个细节我解释一下。首先用openList(idx, :) openList(openLen, :)这行实现删除节点思路是把最后一个元素搬到被删除的位置上再把长度减1这样就不用在大数组里移动大量元素代价是开列表的顺序会乱掉——但A*并不要求开列表有序只要有办法取出最小f即可所以这是划算的。4.2 核心搜索循环的逐步拆解整个搜索循环理解起来其实不难可以拆成四步取节点从开列表中取出f值最小的节点这就是当前要扩展的节点。判断终点如果当前节点就是目标节点直接做路径回溯搜索结束。扩展邻居遍历当前节点周围的8个邻居代码里是双层for循环遍历dr和dc凡是不在地图内、是障碍物、已经在闭列表里的邻居都跳过。更新代价计算通过当前节点到达邻居的新g值tentativeG如果比邻居原来的gCost更小就更新gCost、fCost和父节点。如果邻居已经在开列表里只更新f值如果不在加入开列表。这里有一个很重要的细节为什么要在取出最小f节点之后才判断是否到达终点而不是在生成邻居时判断因为某节点第一次被发现时它的gCost未必是最优的必须等到它从开列表中被取出来才说明所有可能更优的路径都已经被考虑过了。这个顺序如果搞反路径可能不是最短。关于角落检查我再说详细一点。假设当前节点是(r,c)要走到(r1,c1)如果map(r,c1)或map(r1,c)有障碍物那这次对角移动就等同于贴墙角斜切应该禁止。这个判断放在邻居生成阶段而不是事后修正路径是成本最低、效果最好的做法。4.3 路径回溯与可视化输出路径回溯函数非常简单从终点沿着父节点指针一路走回起点再反转顺序即可function path traceBack(parentR, parentC, start, goal) path []; r goal(1); c goal(2); while r ~ start(1) || c ~ start(2) path [path; r, c]; pr parentR(r, c); pc parentC(r, c); r pr; c pc; end path [path; start(1), start(2)]; path flipud(path); end返回的path是N×2的矩阵每一行是一个节点坐标。可视化时要注意坐标方向问题我推荐这样画figure; imagesc(map); axis equal; grid on; colormap(gray); set(gca, YDir, reverse); hold on; plot(start(2), start(1), go, MarkerSize, 12, LineWidth, 2); plot(goal(2), goal(1), ro, MarkerSize, 12, LineWidth, 2); plot(path(:,2), path(:,1), b-, LineWidth, 2); plot(path(:,2), path(:,1), b., MarkerSize, 4);这里的set(gca, YDir, reverse)很关键。imagesc默认把矩阵第1行画在图像顶部第20行画在底部而普通plot的y轴是向上增长的。不设置YDir的话地图和路径会上下颠倒看起来完全不成形。设置成reverse之后矩阵行号向下对应y轴整个图像看起来才是一张俯视地图的样子。5. 仿真结果对比与调优启发式权重、邻域和性能实测5.1 不同启发式函数在同样地图上的实测对比我拿第4节的地图做了一组对比实验把曼哈顿距离、欧几里得距离、八分位距离分别作为启发式跑A*记录扩展节点数和耗时。地图是50×60栅格障碍物密度约35%起点在左下角终点在右上角附近。启发式函数扩展节点数耗时路径长度曼哈顿距离4120.35s69.4欧几里得距离3550.31s68.3八分位距离2930.26s68.2从数据可以看到欧几里得距离虽然路径和八分位几乎一样长但扩展节点明显更多原因就是它把代价估计得偏小导致很多其实不值得走的节点也进了扩展队列。曼哈顿距离在8邻域下偏差最大所以效果最差。这个结果印证了第2节的理论在8邻域栅格地图上八分位距离是最优的启发式。如果你的项目用的是4邻域移动那就换成曼哈顿距离。5.2 加权A*用一点点次优换明显提速加权A*的思路极其简单把启发式函数乘以一个大于1的系数w即f g w * h。这样算法会更有冒险精神地往目标方向冲扩展节点显著减少。我用w1.5在同样地图上测试扩展节点从293缩减到142耗时几乎减半路径长度从68.2变成69.8只损失了大约2%的最优性。工程上这是一个非常划算的取舍尤其是机器人地图动辄上千个栅格、还要实时重规划时。但需要注意w越大路径越容易偏离最优而且容易产生锯齿感。我自己的经验是w不要超过1.5超过之后路径质量肉眼可见地变差后期平滑的工作量也会增大。5.3 跑仿真时被我踩过的三个典型坑第一个坑路径贴障碍物摩擦走。一开始运行时路径总是沿着障碍物边缘切过去看起来就像机器人贴着墙走实际执行时非常危险。排查发现是对角移动缺少角落检查导致的。加上第4节那段角落检查代码后路径明显远离墙角安全多了。第二个坑大点地图跑得巨慢。用150×150栅格跑的时候程序卡了将近10秒。逐步打点定位后发现瓶颈出两处一处是开列表动态扩容一处是用find查节点是否在开列表。改成预分配inOpen标记矩阵后同一张地图耗时降到0.5秒以内。这个优化对后续上真机特别重要因为真机地图通常不会小。第三个坑地图和路径显示上下颠倒。这个坑听起来很蠢但我真的栽过。问题就出在YDir设置上加上坐标习惯不统一画出来的路径和障碍物位置完全对不上。后来我把所有的节点坐标统一成(r,c)绘制时用plot(c, r)并设置YDir再没出过问题。6. 从MATLAB走向实际机器人动态重规划与路径平滑6.1 动态障碍物场景下的重规划策略如果环境里出现新障碍物比如有人走来走去、有箱子被移动A每次全量重规划的成本会随着地图尺寸快速上升。一个粗放但有效的做法是局部重规划全局规划一条A路径后机器人沿着路径走当传感器发现前方路径被新障碍物挡住时只把起点设为当前位置、终点设为路径上更远处的某个可达点在局部窗口内重新跑A*拼接到剩余路径上。这个方法实现简单在中等尺寸地图和小型机器人上足够用。更完整的动态方案是D* Lite这类增量重规划算法。D* Lite能把新旧栅格代价差异造成的重新扩展范围压缩到很小的局部但实现复杂度高不少。MATLAB本身适合做算法验证并不适合做实时控制的主流平台。我的建议是先用MATLAB把静态A*和局部重规划逻辑调通再迁移到C或Python的实际机器人框架里。6.2 路径平滑与机器人运动学约束的衔接A*输出的路径是一条条折线机器人如果机械地沿着折线转向会出现急停、抖动甚至超出运动学约束。平滑路径有两种常用思路剪枝把路径上三点共线的中间点去掉直接让机器人走长直线。曲线拟合用B样条曲线或三阶贝塞尔曲线对路径点做平滑拟合后曲线必须再做一次碰撞检测确保不会穿障碍物。我习惯先做一步剪枝再做曲线拟合。因为A*的路径点本身不一定都在障碍物边缘剪枝通常能直接去掉一半以上的转折点剩下的交给拟合。要注意的是拟合后的曲线一定要回测碰撞不然曲线可能从两个障碍物之间溜过去实际执行时撞上。6.3 实际部署中容易被忽略的三个工程细节第一栅格地图必须先做障碍物膨胀。真实机器人有体积A*规划的是质点路径。如果不把障碍物按机器人半径向外扩张一圈规划出来的路径在实际执行时可能离墙太近。膨胀操作在MATLAB里可以用imerode或者bwdist距离变换实现几行代码就能做掉。第二栅格分辨率与规划效率的冲突。地图分辨率越高A*地图越精细计算开销越大。很多项目会分层处理全局规划用较粗分辨率的地图到达目标附近再用较精细分辨率做局部规划。这相当于从看地图找路变成了看路牌找路口效率会高很多。第三多机器人场景下单机A规划出的路径可能会交叉冲突。这时需要考虑多机器人路径规划算法比如基于冲突搜索CBS的改进方法。不过这属于更高阶的话题先把手头的单机A做扎实再往这个方向扩展不迟。我在做机器人项目时有一个固定工作流先在MATLAB里用A把地图、路径、可视化全部调通确认算法逻辑和参数没问题再移植到实际机器人平台。这个流程能避开至少八成算法逻辑没错、一到真机就出问题的坑。如果你现在正被一个路径规划问题卡住我的建议是先把地图缩到20×30把算法跑通看路径能走通再逐步加大地图、加障碍物、加速度约束——每一步都直观可见这样你才能真正把A吃透。本文还有配套的精品资源点击获取