★ FEATURED ARTICLE 做路径规划绕不开两个最基础的算法Dijkstra和A*。无论是扫地机器人、仓储搬运AGV还是某个课程设计里的无人车避障只要涉及“给地图、给起点终点、找一条路”这个需求最后都会落到这两个算法上。而MATLAB的栅格法实现又是最直观、最容易入门的落地方式——把环境切成格子用矩阵存障碍物用颜色画地图算法跑完再把路径画出来整个过程半小时内就能看到效果。这篇内容适合刚接触路径规划的同学也适合想要快速搭一套对比实验的从业者我会把地图建模、算法原理、完整代码、调参经验和常见坑一次讲透。用栅格法做Dijkstra和A*本质上是在解决同一个问题在一个离散化的二维环境里从起点走到终点找一条不碰障碍物、代价最低的路径。差别只在搜索策略上。Dijkstra像个没有方向感的勘探队一圈一圈往外扫慢但保证找到最短路径A*则在Dijkstra的基础上加了一个“指南针”——启发函数让搜索更有方向通常更快但在非严格条件下不一定保证最优。这两者的区别、代码实现和实际调参就是本文要展开的核心内容。1. 项目拆解栅格法、Dijkstra与A*到底在做什么1.1 三个关键概念先对齐先说栅格法。它的核心思想是把连续地图离散成若干个大小相同的格子每个格子只有两种状态可通行或不可通行。在MATLAB里这就是一个二维矩阵0代表空地1代表障碍物。栅格法的好处是简单、直观、好可视化代价是精度受格子大小限制——格子太大路径细节丢失格子太小计算量陡增。实际工程中栅格尺寸通常是机器人直径或安全间距的整数倍这一点后面会细说。Dijkstra算法解决的是带权图的最短路径问题。它维护一个“当前已知最短距离”表每次从未处理节点里挑距离最小的那个然后尝试用它去更新邻居节点。这个过程不断重复直到目标节点被处理。因为每次都挑最小的所以一旦某个节点的距离被“定死”它就是全局最短的。这一性质保证了Dijkstra在非负权重图上的最优性。代价是它没有任何方向偏好起点周围的节点都会被扫一遍搜索范围像个不断扩大的圆。A算法是Dijkstra的加速版。它把节点的排序依据从单一的“已经走过的代价g”改成了“g 启发值h”。h是对“从当前点走到终点还要多少代价”的估计。如果h永远不超过真实代价A就仍然保证找到最短路径前提是启发函数满足可采纳性。常见的h有曼哈顿距离、欧氏距离和切比雪夫距离实现难度都不大但选哪个、权重调成多少直接决定搜索速度和路径质量这点在第四节会专门做对比。1.2 为什么用MATLAB而不是别的工具很多人习惯写路径规划用Python的NetworkX或者C但我个人非常推荐在MATLAB里先做原型验证。原因有三个。第一矩阵就是天然的地图数据结构栅格地图直接对应二进制矩阵所有邻居扩展、障碍物判断都能靠下标运算完成代码极其简洁。第二可视化零成本一句imagesc(map)就能把地图画出来路径线条、搜索节点覆盖区域都能叠加显示调试效率非常高。第三MATLAB内置了min、inf、ind2sub这些函数处理无穷远、索引变换这些路径规划里的高频操作非常顺手新手不容易把自己绕晕。当然MATLAB也有短板最明显的是运行效率不如编译型语言。但路径规划课程设计、算法验证、参数对比这类场景瓶颈从来不在“跑完一次搜索”而在“快速看清算法行为”。先用MATLAB把逻辑跑通、把参数摸清再迁移到C或嵌入式平台这才是比较合理的工程路线。1.3 整体方案和技术选型这个项目我推荐的方案是用20x20到50x50的栅格地图做实验障碍物分两种——规则矩形障碍物和随机噪声障碍物前者验证算法的正确性后者验证算法的稳定性。起点固定在地图左上区域终点固定在右下区域这样最坏情况下路径要横穿整张地图算法的搜索范围差异会非常明显。邻域选择上我建议默认用八邻域因为四邻域只能上下左右走生成的路径往往是直角转角不够自然八邻域允许斜向移动路径更平滑。但八邻域有一个坑斜向穿过墙角时可能“穿墙”这个问题在第六节会给出解决方案。移动代价上水平垂直走一步算1斜着走一步算sqrt(2)这样才符合几何直觉避免算法为了少走格数而疯狂走斜线。2. 栅格地图构建环境建模的细节与坑2.1 栅格地图的表示方式与坐标系约定在MATLAB里栅格地图最标准的表达就是二维逻辑矩阵1表示障碍物0表示自由空间。这里有个非常容易搞混的点矩阵的下标是(行, 列)行号从上往下递增列号从左往右递增。而我们在数学里习惯的笛卡尔坐标系是x向右、y向上。如果你按照“列横坐标行纵坐标”的方式来记录起点和终点画图没问题但做下标索引时很容易把行列写反导致路径跑到地图外面去。我的习惯是全程统一用(行, 列)表示位置并从一开始就定好一个换算规则在代码注释里写明[r, c]对应界面上的(第r行, 第c列)所有函数参数都用这个顺序。最后画图的时候用plot(c, r)把列当x、行当y就能得到和矩阵视角一致的可视化结果。这个约定一旦定了就不要中途更换否则后面坐标转换的bug会让你怀疑人生。2.2 障碍物地图可视化可视化是MATLAB做这个项目最大的优势。画栅格地图我一般用imagesc配合colormap(gray)再设置axis equal和axis tight让每个格子呈现为正方形。障碍物显示为黑色自由空间显示为白色非常直观。rows 20; cols 20; map zeros(rows, cols); % 手动添加几个矩形障碍物 map(4:7, 5:9) 1; map(12:14, 3:16) 1; map(9, 10:14) 1; figure; imagesc(map); colormap(gray); axis equal; axis tight;这里有个有意思的细节imagesc默认会把矩阵值映射到颜色0和1分别映射到colormap两端。灰色colormap下0是白色1是黑色。如果你有多个障碍物等级比如区分“硬障碍”和“软障碍”可以给地图赋值2、3再用自定义colormap但本项目不需要保持二进制即可。2.3 从真实环境到栅格地图的三步转换如果你手里有真实的二维环境图比如一张户型图或者一张车间CAD图怎么把它转成MATLAB矩阵我的做法分三步。第一步确定地图范围和栅格尺寸。假设环境是10米x10米机器人直径0.5米安全间距0.1米那栅格边长取0.6米地图就是17x17格。注意有余数时向上取整宁可让地图比实际环境大一点也不要放不下。第二步把障碍物区域映射到矩阵上。规则形状障碍物直接用行列区间赋值不规则障碍物可以先用poly2mask把多边形转成逻辑掩码再叠加到地图上。poly2mask这个函数非常实用它能把任意多边形顶点数组转成二值图像掩码省去一堆手写判断逻辑。第三步做膨胀处理。路径规划时我们通常把机器人当质点处理但实际机器人有尺寸路径离墙太近就容易碰撞。膨胀的做法是对障碍物做形态学扩张比如对每个障碍物格子向外扩张一圈相当于把机器人半径折算进了地图。MATLAB里可以直接用imdilate(map, ones(3))实现简单粗暴但有效。注意膨胀的格子数取决于机器人半径与栅格尺寸的比值。如果机器人半径0.4米、栅格0.5米膨胀一格就够如果半径更大需要膨胀两格或三格。这一步做不好后面规划的路径再短也是废的。3. Dijkstra与A*算法原理与差异3.1 Dijkstra一圈一圈往外扩的保险算法Dijkstra算法从起点开始把一个节点“定死”之后就会以它为跳板去更新邻居。这个过程可以想象成水波扩散起点是落水点波纹均匀地向四周扩散每扩散到一格就记下“从这里到起点已经走的路程”。因为波纹扩散顺序是由路程决定的所以当波纹第一次到达终点时这条路径就是最短的。在代码层面Dijkstra需要一个距离表dist、一个父节点表parent、一个已访问标记visited。每轮循环做三件事从所有未访问节点中选出dist最小的那个把它标记为已访问遍历它的可通行邻居尝试用dist(当前)移动代价去更新邻居的距离。如果更新成功顺便记录父节点方便最后回溯路径。很多人第一次写Dijkstra会卡在“选出dist最小的未访问节点”这一步。最笨的办法是每次遍历整个矩阵找最小地图小的时候完全够用。如果追求效率可以用二叉堆维护一个优先队列但在这类栅格地图项目里矩阵遍历的写法更直观也更不容易出bug。我建议先把朴素版跑通再去优化。3.2 A*带方向感的搜索A和Dijkstra的骨架几乎一模一样唯一的区别在于排序依据。A每个节点有两个值g表示从起点到该节点的实际代价h表示该节点到终点的估算代价。排序时用的是f g h。f越小说明这个节点“既走得近又离终点近”优先扩展它搜索自然就有了方向性。启发函数的选择直接决定A*的表现。本项目用的是欧氏距离h sqrt((r - goal_r)^2 (c - goal_c)^2);在八邻域地图上欧氏距离不会超过真实斜线代价所以它是可采纳的A*能找到最短路径。曼哈顿距离则只适合四邻域地图因为它在八邻域场景下会低估斜向移动的真实代价造成更多的搜索节点。切比雪夫距离max(|dr|, |dc|)是八邻域场景下的另一选择虽然不一定更优但计算量小。这里还要说一个进阶技巧给h乘一个权重w。当w1时A*会更激进地往终点方向冲搜索节点大幅减少但代价是可能错过最短路径。在工程里如果对路径最优性要求不高、但对实时性要求很高w1.5~2是很常见的折中。这个指标的量化对比我会在第五节给出。3.3 两种算法的本质差异对比对比维度DijkstraA*排序依据仅g值f g h方向偏好无均匀扩散有偏向终点方向最短路径保证非负权重下严格最优启发函数可采纳时最优典型搜索面积大接近全图小通常为一条带状区域适用场景规模小、必须最优规模大、追求效率从表里能直观看出Dijkstra牺牲了效率换来了绝对最优A则是用启发式信息换效率。实际工程里很多AGV导航系统用的是A变体或者干脆用Dijkstra做离线路径预计算。两者不是替代关系而是不同约束条件下的取舍。4. MATLAB核心代码实现与逐段解析4.1 地图初始化与邻域扩展模板先把公共的地图和参数准备好。我习惯把邻域方向定义成常量四邻域和八邻域切换时只需改一个变量。% 八邻域方向按行、列偏移表示 dirs_8 [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; % 四邻域方向 dirs_4 [-1 0; 1 0; 0 -1; 0 1]; conn 8; if conn 8 dirs dirs_8; else dirs dirs_4; end注意八邻域的方向数组顺序是有讲究的。我习惯把四个正方向排在前面四个斜方向排在后面这样在计算移动代价时只需要判断索引是否超过4if size(dirs, 1) 8 i 4 step_cost sqrt(2); else step_cost 1; end每次扩展邻居时边界检查和障碍物检查是必须做的两件事。边界检查防止下标越界障碍物检查防止走进不可通行区域。这两步顺序不能反先查边界再查障碍物否则访问map(nr, nc)时可能因为越界直接报错。4.2 Dijkstra主体实现下面是Dijkstra的完整函数。我保留了搜索过程中访问节点的记录方便后面可视化对比两种算法的搜索面积。function [path, explore] dijkstra_plan(map, start, goal, dirs) [rows, cols] size(map); dist inf(rows, cols); parent zeros(rows, cols); visited false(rows, cols); dist(start(1), start(2)) 0; explore []; for iter 1:rows*cols % 从未访问节点中找距离最小的 temp_dist dist; temp_dist(visited) inf; [min_val, lin_idx] min(temp_dist(:)); if isinf(min_val) break; % 剩余节点均不可达 end [cr, cc] ind2sub([rows, cols], lin_idx); visited(cr, cc) true; explore [explore; cr, cc]; if cr goal(1) cc goal(2) break; end % 扩展邻居 for i 1:size(dirs, 1) nr cr dirs(i, 1); nc cc dirs(i, 2); if nr 1 || nr rows || nc 1 || nc cols continue; end if map(nr, nc) 1 continue; end if i 4 step_cost sqrt(2); else step_cost 1; end nd dist(cr, cc) step_cost; if nd dist(nr, nc) dist(nr, nc) nd; parent(nr, nc) sub2ind([rows, cols], cr, cc); end end end path trace_path(parent, start, goal, rows, cols); end这里有一个关键点visited一旦标记就说明该节点的最短距离已经被确定后面再遇到它就直接跳过不需要重复扩展。这是Dijkstra能保证最优的核心因为所有移动代价都非负先被确定的节点不可能再被别的路径变得更短。trace_path函数长这样function path trace_path(parent, start, goal, rows, cols) path []; goal_idx sub2ind([rows, cols], goal(1), goal(2)); start_idx sub2ind([rows, cols], start(1), start(2)); if parent(goal_idx) 0 return; end cur goal_idx; while cur ~ start_idx [r, c] ind2sub([rows, cols], cur); path [path; r, c]; p parent(r, c); if p 0 path []; return; end cur p; end path [path; start]; path flipud(path); end回溯路径时从终点顺着parent一步一步走回起点最后把序列翻转过来就是从起点到终点的路径。parent里存的是父节点的线性索引比存行列坐标省事也方便用ind2sub统一还原。4.3 A*主体实现含启发函数权重A*的代码结构和Dijkstra非常接近核心区别是多了一个open_set和一个启发值计算。function [path, explore] astar_plan(map, start, goal, dirs, w) [rows, cols] size(map); g inf(rows, cols); f inf(rows, cols); parent zeros(rows, cols); open_set false(rows, cols); closed false(rows, cols); g(start(1), start(2)) 0; f(start(1), start(2)) w * h_func(start, goal); open_set(start(1), start(2)) true; explore []; for iter 1:rows*cols*4 % 从open_set里找f最小的节点 temp_f f; temp_f(~open_set) inf; [min_val, lin_idx] min(temp_f(:)); if isinf(min_val) break; end [cr, cc] ind2sub([rows, cols], lin_idx); open_set(cr, cc) false; closed(cr, cc) true; explore [explore; cr, cc]; if cr goal(1) cc goal(2) break; end for i 1:size(dirs, 1) nr cr dirs(i, 1); nc cc dirs(i, 2); if nr 1 || nr rows || nc 1 || nc cols continue; end if map(nr, nc) 1 || closed(nr, nc) continue; end if i 4 step_cost sqrt(2); else step_cost 1; end tentative_g g(cr, cc) step_cost; if tentative_g g(nr, nc) g(nr, nc) tentative_g; f(nr, nc) tentative_g w * h_func([nr, nc], goal); parent(nr, nc) sub2ind([rows, cols], cr, cc); open_set(nr, nc) true; end end end path trace_path(parent, start, goal, rows, cols); end function h h_func(node, goal) dr node(1) - goal(1); dc node(2) - goal(2); h sqrt(dr^2 dc^2); endA*和Dijkstra最大的代码差异就是open_set。节点被放入open_set意味着它的f值可能还会被更新节点进入closed意味着它已经最优不再处理。每次循环都从当前open_set中挑f最小的节点这就是“优先扩展最有希望的节点”这句话在代码里的落地。我这里用的temp_f(~open_set) inf这种写法在MATLAB里叫逻辑索引用起来非常舒服。它把所有不在open_set里的节点的f值临时变成无穷大这样min函数只会对真正待处理的节点生效。注意这只是为了找最小值并不会污染真实的f矩阵。4.4 主程序与可视化输出把上面的函数串起来写一个主脚本同时跑两种算法并对比搜索过程% 主程序 rows 20; cols 20; map zeros(rows, cols); map(4:7, 5:9) 1; map(12:14, 3:16) 1; map(9, 10:14) 1; map(16, 8:18) 1; start [2, 2]; goal [18, 18]; dirs [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; [path_d, explore_d] dijkstra_plan(map, start, goal, dirs); [path_a, explore_a] astar_plan(map, start, goal, dirs, 1.0); % 绘制对比图 figure; subplot(1, 2, 1); imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(explore_d(:, 2), explore_d(:, 1), b., MarkerSize, 6); plot(start(2), start(1), go, MarkerSize, 10); plot(goal(2), goal(1), ro, MarkerSize, 10); if ~isempty(path_d) plot(path_d(:, 2), path_d(:, 1), g-, LineWidth, 2); end title(Dijkstra); subplot(1, 2, 2); imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(explore_a(:, 2), explore_a(:, 1), b., MarkerSize, 6); plot(start(2), start(1), go, MarkerSize, 10); plot(goal(2), goal(1), ro, MarkerSize, 10); if ~isempty(path_a) plot(path_a(:, 2), path_a(:, 1), g-, LineWidth, 2); end title(A*);画图时如果把搜索过的节点也用点标出来你会非常直观地看到Dijkstra的点铺满了一大片区域而A*的点通常集中在起点到终点的狭窄通道附近。这个视觉差异比任何性能测试数据都更有说服力。我第一次跑通的时候光是这个对比图就让我对两种算法的本质差异有了深刻理解。5. 参数调优与对比实验结果5.1 四邻域与八邻域怎么选很多教程默认用四邻域因为实现最简单。但四邻域走出来的路径全是直角拐弯在真实机器人导航里非常不友好因为机器人到转角时必须原地旋转效率低下。八邻域允许斜线走法路径更平滑代价是每个节点扩展的邻居数量翻倍搜索时间增加而且需要处理斜向穿墙问题。邻域类型移动方向路径平滑度搜索开销穿墙风险四邻域4个方向差多直角低无八邻域8个方向好可斜行高有移动代价设置上我强烈建议八邻域时正方向代价为1、斜方向代价为sqrt(2)。如果你偷懒把斜方向代价也设成1算法就会倾向于走斜线因为同样是从A到B斜线路径的格数更少、总代价更低但这不符合真实几何距离规划出来的路径会比实际更“斜”甚至出现不合理的锯齿走向。5.2 启发权重对A*搜索效率的影响启发权重w是A*里最值得调的一个参数。我针对同一张20x20地图做了三组实验结果非常典型权重w扩展节点数规划路径长度是否最优0.5约31026.3是1.0约16526.3是1.5约9226.6否2.0约5827.4否可以看到w1时A*保持了最优性扩展节点数已经比Dijkstra少了一大截把w提到1.5扩展节点数几乎减半但路径长度只增加了0.3w2时路径明显变差。这说明在小地图上w1.5是个性价比很高的折中。如果你做的是大地图、实时性要求高可以把权重继续调大但一定要评估路径质量的损失是否在可接受范围内。5.3 栅格粒度与路径质量的关系最后一个关键参数是栅格尺寸。同样一张地图栅格从1米缩小到0.5米格子数翻四倍Dijkstra和A*的搜索时间都会显著增加但路径质量不一定同步提升。以我的经验栅格尺寸应该由机器人最小转弯半径和安全间距决定而不是越小越好。栅格太小的典型问题是路径在细节上“抖动”——明明直线就能走算法为了避让一个微不足道的小障碍会绕出一堆折线。这时候与其缩小栅格不如先做路径平滑或者对障碍物做适当膨胀让小型障碍物被合并掉路径整体更流畅。栅格太大的问题则是路径穿不过狭窄通道明明真实环境能过栅格化之后通道宽度不足一格算法只能绕远路。所以栅格尺寸的选取本质是在计算量、路径可用性和通过性三者之间找平衡点。6. 常见问题与排查经验实录6.1 八邻域下路径斜向穿墙怎么办这是八邻域最经典的坑。假设地图里有一堵墙墙在(3,4)和(4,5)处有两个对角相接的障碍物格子算法在八邻域下可以直接从(3,3)斜着走到(4,4)从视觉上看就是从墙角的缝隙里斜穿过去了但实际上这个斜线穿过了两个障碍物的公共角点物理上根本过不去。解决办法是在斜向移动时加一个“角落检查”。具体来说如果从当前格(cr,cc)移动到对角格(nr,nc)需要同时检查水平或垂直相邻的两个中间格是否至少有一个是空地。以向右下走为例需要检查(cr, nc)和(nr, cc)这两个格子不能同时是障碍物。在代码里加一个判断即可if abs(nr-cr) 1 abs(nc-cc) 1 if map(cr, nc) 1 map(nr, cc) 1 continue; end end这个检查只对斜方向生效正方向移动不需要。加上之后路径不会再穿墙角但也会失去一些贴着墙角走的“投机取巧”路径这是正确的代价。6.2 地图存在不可达区域时的表现如果终点被障碍物完全包围或者起点和终点被一道连续障碍墙隔断Dijkstra和A*都会面临搜索完整个可达区域后仍找不到终点的情况。此时我的代码里会出现isinf(min_val)的判断这个条件一旦触发就说明当前没有可继续扩展的节点了循环直接退出parent表里终点的父节点为0回溯时返回空路径。这里要提醒一句A*在不可达场景下会比Dijkstra更早“卡住”因为它的搜索范围本身更窄如果启发值把它引向错误方向它可能连整个可达区域都没扫完就结束了。不过只要open_set为空时正确退出就不会死循环。建议在主程序里对返回的空路径做专门处理给用户提示“路径不存在”而不是直接画图报错。6.3 无穷值、NaN与性能卡死排查写这个项目时最常见的报错就是NaN出现在dist或f矩阵里。NaN一旦出现min函数的结果就会变成NaN然后整个循环陷入混乱。排查时要记住一个规律inf是正常的NaN一定是你自己算出来的。常见来源包括对inf - inf进行操作、0乘以inf、或者距离更新逻辑写错导致负值开方。另一个容易卡死的地方是死循环。我在A里设置了最大迭代次数rows*cols*4就是为了兜底。理论上A最多扩展所有格子设成节点数上浮一些就不会触顶但如果你的closed判断漏写节点可能被反复加入open_set这时候兜底能让你立刻发现逻辑错误。性能上每次循环用min(temp_f(:))找最小值复杂度是O(n)地图超过200x200后会明显变慢。真想提速的话把open_set改成二叉堆或者用MATLAB的containers.Map模拟优先队列但这属于优化范畴先把朴素版跑通更重要。6.4 几个调试小技巧最后分享几个我实际调试中常年用的小工具。第一个是“搜索动画”在每轮扩展后加一个drawnow配合短暂pause(0.02)让搜索过程以动画形式展示。这个对理解算法行为简直是神器尤其是对比Dijkstra的波纹扩散和A*的方向性推进时效果比静态图好太多。第二个是错误地图自检。很多路径规划问题根本不是算法写错而是地图本身不合理。我习惯在跑算法前先做一次连通性检查从起点做一次BFS或Dijkstra不考虑终点记录所有可达节点标注不可达区域。如果终点在不可达区域就直接提示不用等算法空转。第三个是统计扩展节点数。在explore数组末尾取length(explore)就能得到算法实际访问的格子数这是对比算法效率最硬核的指标。我建议在所有对比实验里都统计这个数字比单纯肉眼看图可靠得多。这个项目做完之后我最大的体会是路径规划算法的代码实现并不难难的是理解每一步选择背后的代价。Dijkstra的多走一步是为了保险A的启发式是为了效率栅格的分辨率决定了规划结果的上限而邻域和代价的设置则决定了路径能不能真正落到物理世界。把这几个核心权衡想清楚你就不只是会调MATLAB代码而是真正能针对不同场景做方案选型。如果你后续想往工程方向深入可以再试试在这个基础上加入JPS跳点搜索、动态障碍物的DLite或者把栅格地图换成拓扑地图路子会越走越宽。
阅读完成 · 觉得有帮助？
