MATLAB实现Dijkstra栅格地图路径规划：从原理到工程实践

简介基于Dijkstra法的栅格地图路径规划是移动机器人导航中的经典基础算法这份资源专门面向学习路径规划、准备课程设计或毕业设计的本科生与研究生也适合机器人开发者快速上手。源码采用Matlab实现结构简洁主程序、栅格地图构建、Dijkstra搜索等模块分离便于阅读和二次开发。资源包共10个文件包含7个.m脚本、1个原理说明docx文档、1个README说明和1个txt辅助文件整体仅167KB轻量且无额外依赖。配套文档详细梳理了算法原理、栅格建模方式和搜索流程方便对照源码逐步理解通过运行示例可直观看到最短路径生成过程还能自行修改起点、终点或障碍物布局反复测试。目前已有445人学习使用尤其适合需要从零搭建仿真环境、验证算法效果并进一步扩展为A*或双向搜索等改进算法的读者。1. 基于Dijkstra的栅格地图路径规划移动机器人避障的起点对于一个做移动机器人的工程师来说路径规划绕不开两个基础问题环境怎么描述、最短路径怎么找。栅格地图把连续环境离散成等大小网格每个格子标记为可通行或障碍物做法直观、维护简单也正好是Dijkstra算法最擅长处理的输入。Dijkstra按代价递增顺序向外扩展保证第一次到达目标节点时路径代价全局最小这和贪心或深度优先有本质区别。对巡检小车、仓储机器人这类低速场景计算资源不紧张Dijkstra比A*写起来更少犯错也更容易在MATLAB里验证。下面顺着建图、算路、仿真验证到工程扩展这条线把MATLAB下实现Dijkstra栅格路径规划的细节和参数讲透。2. 栅格地图建模与Dijkstra算法的扩展规则2.1 占用栅格地图从传感器数据到0-1矩阵先想清楚“栅格地图”在MATLAB里到底是什么。最常见的形式是一个二维矩阵元素0表示空闲、1表示障碍物地图分辨率和网格大小由实际场景决定。比如一个20×20的矩阵表示20×20米的区域每个格子1平方米这就是1m分辨率如果一辆车宽1.5米车体半径至少占一个格子那么格子分辨率还要和机器人底盘尺寸匹配。移动机器人领域所说的占用栅格地图Occupancy Grid Map在ROS2路径规划栈里对应nav_msgs/OccupancyGrid消息数据本质和这个矩阵一致只是把0和1换成0-100的概率值。在MATLAB里手动构造栅格地图常见做法是先用全零矩阵初始化再把矩形或多边形区域标记为1。下面的代码创建了一个包含L形障碍物和边界的地图map_size [30, 30]; map zeros(map_size); % 边界障碍物防止路径跑出地图 map(1, :) 1; map(end, :) 1; map(:, 1) 1; map(:, end) 1; % L形障碍物第8行整行 第8到15列的第12列 map(8, 3:12) 1; map(8:15, 12) 1;这段代码里map(8, 3:12) 1把矩阵第8行、第3到第12列置为障碍物对应地图上坐标(row8, col3)到(row8, col12)的一排格子。注意MATLAB索引从1开始第1行和第1列就是地图最上边和最左边这和常见的图像坐标是一致的。障碍物不止可以手动画SLAM建图扫出的激光数据也可以用occupancyMap对象包装后转成逻辑矩阵数据结构上完全兼容。还有一个容易被忽视的参数是地图分辨率如果把分辨率从1m改成0.5m同一个环境的地图尺寸会变成40×40Dijkstra的搜索空间扩大4倍路径规划的耗时也会显著增加。实际中应该根据机器人最小转弯半径和定位精度来选择分辨率过细的地图会让算法陷入对栅格级细节的过度优化。2.2 Dijkstra算法的优先队列本质为什么它能保证最优Dijkstra算法解决的问题是带非负权图上的单源最短路径。栅格地图里每个格子是图的节点格子与相邻格子之间连边边的权值由移动代价定义。两个关键设计直接决定算法行为一是邻域如何定义二是非障碍物格子的代价如何设置。邻域定义4邻域允许上下左右移动8邻域额外允许对角移动。对角移动的边长按欧氏距离取√2否则会低估斜向路径的真实代价导致搜索结果看起来“绕路”。下表给出典型设置。邻域类型移动方向直行动代价对角代价4邻域上下左右1-8邻域含对角11.414一个常被忽略的细节是8邻域下的穿墙问题如果只检查对角格子是否为障碍物那么机器人可能贴着墙角斜穿过去实际通不过。标准做法是额外检查对角经过的两个相邻格子——从(r, c)移动到(r1, c1)必须同时检查(r1, c)和(r, c1)是否可通行。这一条在MATLAB实现里极其容易漏掉后面我会给出具体判断代码。算法主循环维护一个已知最小代价的表dist初始起点为0、其余为无穷大每次从未确定节点中取距离最小的那个“松弛”它的邻居。如果新代价dist(current) edge_cost dist(neighbor)就更新。由于每次弹出的都是当前最小代价节点且在非负权图中后续不可能找到更优的到达该节点的路径因此当目标节点被弹出时可以直接终止得到的解就是全局最短路径。这构成了Dijkstra与BFS的区别BFS把每条边看成等权Dijkstra显式处理不同代价值与贪心最佳优先的区别则在于Dijkstra不只考虑“离目标有多近”而是综合了已经走过的实际代价。2.3 在栅格上的复杂度分析与内存占用栅格地图总共N个格子每个格子最多扩展8个邻居。用数组实现优先队列每次扫描全部未访问节点找最小值复杂度是O(N²)用二叉堆则降到O(N log N)。对于500×500的地图N250000数组扫描的次数是625亿次量级MATLAB即使有向量化也明显吃力更别说动态避障小车路径规划场景还需要实时重规划。提示MATLAB没有内置泛型优先队列很多教学代码直接循环找最小值。地图小于200×200时这种做法没问题地图更大时用min函数配合ismember做批量松弛或者写一个基于java.util.PriorityQueue的包装类都是实际工程里常见的做法。内存方面dist、visited、prev三个矩阵占用空间随地图尺寸线性增长。一个1e4格的矩阵在MATLAB中大约占80KB500×500的也就是几MB量级不是瓶颈真正影响性能的是反复的数组索引和比较操作。若地图中包含大量重复代价的平坦区域Dijkstra会扩展几乎全部可达格子这时改用A*或双向Dijkstra能显著减少无效扩展这就是为什么实际导航系统很少直接裸跑Dijkstra做全局规划而常常把它作为分层规划里“基础层”的备选算法。3. MATLAB实现Dijkstra栅格路径规划核心代码逐段拆解3.1 输入输出设计从地图矩阵到路径序列在动手写代码前先定义接口。输入是栅格地图矩阵map、起点start、终点goal和邻域类型neighbor_type输出是路径下标序列path和总代价total_cost。起点和终点都用[row, col]格式传入因为栅格地图本质是矩阵直接用行、列索引免去坐标系转换。若使用真实机器人仿真从世界坐标转栅格坐标需要额外的分辨率参数这一步放到调用前处理。我一般会把代码分成两个函数主函数dijkstra_grid和邻居生成函数get_neighbors。后者单独成函数是因为8邻域和4邻域在边界条件处理上差异明显拆开测试更清晰。函数签名如下function [path, total_cost] dijkstra_grid(map, start, goal, neighbor_type) % dijkstra_grid: 栅格地图上的Dijkstra路径规划 % 输入: % map: 二维矩阵, 0可通行, 1障碍物 % start: [row, col], 起点 % goal: [row, col], 目标点 % neighbor_type: 4 或 8, 邻居扩展方式 % 输出: % path: Kx2矩阵, 从起点到目标点的路径坐标 % total_cost: 路径总代价设计上故意把neighbor_type作为显式参数而不是写死为8是因为4邻域和8邻域的结果差异会影响后续的控制策略。全向移动和无人机路径规划算法倾向8邻域差速底盘则经常先用4邻域找一条保守路径。path按起点到目标的顺序返回方便直接喂给可视化或轨迹跟踪模块。在真实机器人项目中路径规划模块的输出还要经过轨迹跟踪控制器路径的形态直接决定了跟踪难度。路径点过密会让控制器频繁转向过稀又无法精确贴合障碍物轮廓这是一个在仿真阶段就要想清楚的权衡。3.2 Dijkstra主循环与路径回溯主循环用visited矩阵记录已确定最短路径的节点用dist矩阵保存当前已知代价用prev保存每个节点是从哪个节点来的。MATLAB代码实现如下[rows, cols] size(map); % 初始化 INF Inf; dist INF * ones(rows, cols); prev zeros(rows, cols, 2); % prev(:,:,1)存上一行, prev(:,:,2)存上一列 visited false(rows, cols); sr start(1); sc start(2); gr goal(1); gc goal(2); dist(sr, sc) 0; while true % 1. 在未访问节点中找距离最小的节点 unvisited_dist dist; unvisited_dist(visited) INF; % 已访问节点不再参与选择 [min_val, min_idx] min(unvisited_dist(:)); if isinf(min_val) % 没有可达节点提前结束 path []; total_cost Inf; return; end [cur_r, cur_c] ind2sub([rows, cols], min_idx); if cur_r gr cur_c gc break; % 目标已弹出算法终止 end visited(cur_r, cur_c) true; % 2. 扩展邻居 neighbors get_neighbors(cur_r, cur_c, rows, cols, neighbor_type, map); for k 1:size(neighbors, 1) nr neighbors(k, 1); nc neighbors(k, 2); if visited(nr, nc) continue; end % 计算移动代价对角1.414, 直行1 if abs(nr - cur_r) abs(nc - cur_c) 2 step_cost 1.414; else step_cost 1; end new_cost dist(cur_r, cur_c) step_cost; if new_cost dist(nr, nc) dist(nr, nc) new_cost; prev(nr, nc, 1) cur_r; prev(nr, nc, 2) cur_c; end end end这段代码逻辑上最关键的一步是unvisited_dist(visited) INF。直接把已访问节点的距离置为无穷大min函数返回的min_idx就是所有未访问节点中距离最小的省去了维护一个独立open集合的麻烦。代价计算的判断条件abs(nr - cur_r) abs(nc - cur_c) 2检测的其实是“行差和列差的绝对值之和为2”只有对角移动满足直行都是1这个方法比分类讨论四个方向更简洁。注意这里目标被弹出后就跳出了主循环因为此时路径是否可达已经完全确定继续扩展只会浪费时间。路径回溯单独写一个循环% 3. 从目标回溯得到完整路径 path [gr, gc]; cur_r gr; cur_c gc; while ~(cur_r sr cur_c sc) pr prev(cur_r, cur_c, 1); pc prev(cur_r, cur_c, 2); if pr 0 || pc 0 path []; total_cost Inf; return; % 回溯中断说明起点不可达 end path [path; [pr, pc]]; cur_r pr; cur_c pc; end path flipud(path); total_cost dist(gr, gc);回溯从目标点开始不断读prev表跳到上一个节点直到回到起点。prev初始化为全零如果回溯过程中读到0说明起点和目标不在同一连通区域此时直接返回空路径。注意path用flipud翻转才是从起点到目标的顺序适合后面直接画图。这里用了path [path; [pr, pc]]的追加模式在路径长度较短时性能可接受如果地图上路径超过几千个点改成预分配数组会更快。3.3 邻居生成与8邻域防穿墙判断get_neighbors的一个易错点是边界检查。栅格地图四周的格子没有完整的8个邻居越界访问MATLAB会直接报错。另一个易错点就是前面说的对角穿墙。实现如下function neighbors get_neighbors(r, c, rows, cols, neighbor_type, map) % 生成当前格子的可通行邻居 neighbors []; % 定义方向偏移: 4邻域 和 8邻域 if neighbor_type 4 dirs [-1 0; 1 0; 0 -1; 0 1]; else dirs [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; end for i 1:size(dirs, 1) nr r dirs(i, 1); nc c dirs(i, 2); % 边界检查 if nr 1 || nr rows || nc 1 || nc cols continue; end % 障碍物检查 if map(nr, nc) 1 continue; end % 8邻域下对角移动要做防穿墙检查 if neighbor_type 8 abs(nr - r) 1 abs(nc - c) 1 if map(r, nc) 1 || map(nr, c) 1 continue; % 两个相邻格子有一个是障碍物禁止对角穿越 end end neighbors [neighbors; nr, nc]; end end防穿墙的本质是判断对角线两侧的格子是否都为空。从(r, c)到(r1, c1)时(r, c1)和(r1, c)必须同时可通行否则机器人会卡在墙角或直接穿过障碍物边缘。很多路径规划代码只检查目标格在实际履带式机器人和差速机器人上会走出不可执行的轨迹。neighbors初始化为空矩阵neighbors [neighbors; nr, nc]是MATLAB里比较自然的累加写法代价是多次内存重分配但在栅格地图规模下完全可接受。4. 在MATLAB中跑通Dijkstra栅格地图路径规划仿真4.1 随机地图上的算法验证把第3章的代码组装好先在一个随机地图上验证正确性。随机地图的生成有个细节完全随机生成的地图很容易出现大量零散障碍物路径会绕得很碎不利于观察算法行为。常见做法是先指定障碍物密度再随机放置矩形障碍物块。下面这段代码生成一个带五个矩形障碍物块的地图rng(17); % 固定随机种子保证实验可复现 map_size [40, 40]; map zeros(map_size); % 随机放置5个矩形障碍物 for i 1:5 h randi([4, 10]); % 障碍物高度4-10格 w randi([4, 10]); % 宽度4-10格 r0 randi([2, map_size(1)-h-1]); c0 randi([2, map_size(2)-w-1]); map(r0:r0h-1, c0:c0w-1) 1; end start [2, 2]; goal [39, 38]; [path, total_cost] dijkstra_grid(map, start, goal, 8); % 可视化 figure; imagesc(map); colormap(gray); axis equal; hold on; plot(start(2), start(1), go, MarkerSize, 10, LineWidth, 2); plot(goal(2), goal(1), r*, MarkerSize, 12, LineWidth, 2); if ~isempty(path) plot(path(:, 2), path(:, 1), b-, LineWidth, 2); title(sprintf(Dijkstra路径, 总代价%.2f, total_cost)); else title(无可达路径); endrng(17)在MATLAB里固定随机数种子保证每次运行生成同样的障碍物布局。imagesc(map)把矩阵显示为图像深色格子就是障碍物。这里用plot(path(:, 2), path(:, 1))而不是反着的坐标是因为图像的显示是x轴对应列、y轴对应行imagesc默认纵轴从下往上如果是手动绘制的栅格图需要结合具体坐标系调整显示方向。随机地图验证的重点不是看路径“像不像样”而是确认total_cost与手动计算的代价一致以及路径上每个相邻点都满足1或√2的距离关系。参数取值对结果的影响rng种子不同值生成不同地图用于批量测试算法稳定性障碍物数量越多路径越绕过多时出现不可达map_size200×200以下速度可接受更大用堆优化实现4.2 4邻域与8邻域的结果对比与参数调试直接用同一张地图分别跑4邻域和8邻域能够直观看出差距。下表给出两者在同一张40×40地图上的典型结果。从路径长度看8邻域更优从转向频次看4邻域更平滑。邻域路径长度转向次数适用场景4更长更少差速底盘、搜索覆盖8更短更多全向移动、无人机路径规划一个工程上常用的做法是路径质量要求高时用8邻域执行平滑性要求高时用4邻域或对8邻域路径做平滑后处理。还可以在neighbor_type为8时把对角代价设为1.5或1.6而不是1.414轻微惩罚斜走让路径更贴近“先直行再斜行”的L型风格这在带阿克曼转向的泊车路径规划算法调试中尤其常见。修改方式只需要改动3.2节里step_cost分支的对角值改成全局变量或参数传入即可不需要动主循环。另一个常被忽略的参数是起点和目标的选取。在一个复杂栅格地图中即使起点和目标只隔一个障碍物格子路径也可能会绕整个障碍物一圈。这不是Dijkstra的问题而是地图离散化后可行路径本身就变了。验证算法时不建议一开始就用复杂大图先跑一个如5×5的玩具地图手动算一遍最短路径再和程序输出对比能更快暴露索引或回溯的问题。4.3 不可达与死区场景空路径的排查思路随机地图中障碍物可能把起点或目标包围这时dist中目标对应的值始终是Infmin函数会返回Inf触发提前返回。多数代码在这里直接显示“无可达路径”但这还不够——工程上要区分两种不可达起点被围死还是目标被围死。区分方法很简单起点的4邻域或8邻域全部是障碍物或边界则起点不可达目标的邻域同理。下面的检查可以在主循环之前执行% 起点/终点是否被完全困住 start_free ~map(start(1), start(2)); goal_free ~map(goal(1), goal(2)); if ~start_free || ~goal_free error(起点或目标点在障碍物内部); end还有一种容易被忽略的情况地图不是完全连通的起点和目标在两个不同区域此时dist(gr, gc)仍为Inf但主循环已经无法继续扩展因为所有可达节点都被访问过。这个场景在不规则障碍物地图中经常出现排查思路是用bwlabel对地图做连通域分析先判断起点和目标是否属于同一个连通区域再做路径搜索可以省掉无谓的Dijkstra运行。提示对map取反后用bwlabel统计连通域数量若超过1就说明地图被障碍物分割成多个独立区域此时再检查起点和目标是否同一个标签值。这一步在ROS2路径规划里等价于代价地图的“膨胀层”检查只是表现形式不同。5. 从Dijkstra到工程落地路径平滑与算法扩展5.1 折线路径平滑减少机器人转向抖动Dijkstra产出的路径由多段直线组成在实际移动机器人上是走折线每个转角都要减速不平滑。常见做法是在MATLAB里对路径做滑动平均或贝塞尔曲线插值。这里给出一个简洁的平滑思路它基于一个事实栅格路径中的点大多是冗余的可以每3个点做二次贝塞尔插值。简单做法如下——把3个连续点作为控制点用二次贝塞尔生成中间插值点function smooth_path smooth_bezier(path) % path: 原始折线路径 smooth_path path(1, :); for i 1:size(path, 1) - 2 p0 path(i, :); p1 path(i1, :); p2 path(i2, :); for t 0.2:0.2:1 pt (1-t)^2 * p0 2*(1-t)*t * p1 t^2 * p2; smooth_path [smooth_path; pt]; end end smooth_path(end1, :) path(end, :); end贝塞尔插值不会改变路径经过的关键节点只是把转角处圆滑化。更严谨的做法是使用梯度下降的路径平滑即定义代价函数包含“离障碍物距离”和“路径曲率”两项迭代调整路径点。对MATLAB原型验证来说上面的二次贝塞尔已经足够说明平滑效果要部署到真机时再换用基于时间最优轨迹生成TOPP的算法。5.2 从Dijkstra切换到A*与双向DijkstraDijkstra的劣势在于扩展方向是各向同性的从起点向四周均匀扩散。若已知目标位置把搜索方向偏置到目标一侧能显著减少扩展节点数这就是A*。A*和Dijkstra的唯一区别是每次优先扩展f g h最小的节点其中g是起点到当前点的实际代价h是当前点到目标的启发式估计。在MATLAB里改起来只需要把第3章代码里的min比较处换一个变量h常用欧氏距离或曼哈顿距离。启发式的选择直接决定A*行为h小于或等于真实代价时保证最优等于时最优且最快大于时速度快但结果可能次优。如果不想引入启发式又想让搜索更快可以改用双向Dijkstra从起点和目标同时向中间搜索两个方向的搜索前沿相遇时回溯。这个技巧在动态避障小车路径规划场景中很实用因为双向搜索在不改变最优性的前提下通常比单向Dijkstra快一个量级。栅格地图的内存占用是已知的提前分配好两套dist和prev即可MATLAB实现也不复杂核心是在两个方向各维护一个dist表交替扩展距离较短的边界节点。下面是切换A*时建议的启发式函数写法h abs(nr - gr) abs(nc - gc); % 曼哈顿距离4邻域下可采纳 % h sqrt((nr - gr)^2 (nc - gc)^2); % 欧氏距离8邻域下可采纳第一行适合4邻域第二行适合8邻域。如果用8邻域却用曼哈顿距离启发式可能高估真实代价破坏最优性。这一条在混合A路径规划、无人机路径规划算法的实现里同样成立——所有基于栅格的搜索算法都要保证启发式不超估。最后说一个验证技巧Dijkstra跑完后逐段检查路径中相邻点的距离是否都等于1或√2。如果出现别的值多半是get_neighbors返回了非相邻节点或回溯出错。用diff(path)一行就能做这个断言seg_lens sqrt(sum(diff(path).^2, 2)); assert(all(abs(seg_lens - 1) 1e-6 | abs(seg_lens - 1.414) 1e-6));diff(path)计算相邻路径点的差值sum(...,2)按行求和开根号后得到每段长度。assert失败说明路径有问题趁早暴露比在真机上发现好得多。这个断言能暴露get_neighbors里的边界逻辑错误也能抓住回溯时prev表被覆盖的问题是调试栅格路径规划时成本最低、收益最高的手段之一。本文还有配套的精品资源点击获取