企业官网建设流程全解析

简介：基于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列置为障碍物，对应地图上坐标(row=8, col=3)到(row=8, col=12)的一排格子。注意MATLAB索引从1开始，第1行和第1列就是地图最上边和最左边，这和常见的图像坐标是一致的。障碍物不止可以手动画，SLAM建图扫出的激光数据也可以用occupancyMap对象包装后转成逻辑矩阵，数据结构上完全兼容。还有一个容易被忽视的参数是地图分辨率：如果把分辨率从1m改成0.5m，同一个环境的地图尺寸会变成40×40，Dijkstra的搜索空间扩大4倍，路径规划的耗时也会显著增加。实际中应该根据机器人最小转弯半径和定位精度来选择分辨率，过细的地图会让算法陷入对栅格级细节的过度优化。

2.2 Dijkstra算法的优先队列本质：为什么它能保证最优

Dijkstra算法解决的问题是带非负权图上的单源最短路径。栅格地图里每个格子是图的节点，格子与相邻格子之间连边，边的权值由移动代价定义。两个关键设计直接决定算法行为：一是邻域如何定义，二是非障碍物格子的代价如何设置。

邻域定义：4邻域允许上下左右移动，8邻域额外允许对角移动。对角移动的边长按欧氏距离取√2，否则会低估斜向路径的真实代价，导致搜索结果看起来“绕路”。下表给出典型设置。

一个常被忽略的细节是8邻域下的穿墙问题：如果只检查对角格子是否为障碍物，那么机器人可能贴着墙角斜穿过去，实际通不过。标准做法是额外检查对角经过的两个相邻格子——从(r, c)移动到(r+1, c+1)，必须同时检查(r+1, c)和(r, c+1)是否可通行。这一条在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的地图，N=250000，数组扫描的次数是625亿次量级，MATLAB即使有向量化也明显吃力，更别说动态避障小车路径规划场景还需要实时重规划。

提示：MATLAB没有内置泛型优先队列，很多教学代码直接循环找最小值。地图小于200×200时这种做法没问题；地图更大时用min函数配合ismember做批量松弛，或者写一个基于java.util.PriorityQueue的包装类，都是实际工程里常见的做法。

内存方面，dist、visited、prev三个矩阵占用空间随地图尺寸线性增长。一个1e4格的矩阵在MATLAB中大约占80KB，500×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)到(r+1, c+1)时，(r, c+1)和(r+1, 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:r0+h-1, c0:c0+w-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('无可达路径'); end

rng(17)在MATLAB里固定随机数种子，保证每次运行生成同样的障碍物布局。imagesc(map)把矩阵显示为图像，深色格子就是障碍物。这里用plot(path(:, 2), path(:, 1))而不是反着的坐标，是因为图像的显示是x轴对应列、y轴对应行，imagesc默认纵轴从下往上，如果是手动绘制的栅格图，需要结合具体坐标系调整显示方向。随机地图验证的重点不是看路径“像不像样”，而是确认total_cost与手动计算的代价一致，以及路径上每个相邻点都满足1或√2的距离关系。

4.2 4邻域与8邻域的结果对比与参数调试

直接用同一张地图分别跑4邻域和8邻域，能够直观看出差距。下表给出两者在同一张40×40地图上的典型结果。从路径长度看8邻域更优，从转向频次看4邻域更平滑。

一个工程上常用的做法是：路径质量要求高时用8邻域，执行平滑性要求高时用4邻域或对8邻域路径做平滑后处理。还可以在neighbor_type为8时把对角代价设为1.5或1.6而不是1.414，轻微惩罚斜走，让路径更贴近“先直行再斜行”的L型风格，这在带阿克曼转向的泊车路径规划算法调试中尤其常见。修改方式只需要改动3.2节里step_cost分支的对角值，改成全局变量或参数传入即可，不需要动主循环。

另一个常被忽略的参数是起点和目标的选取。在一个复杂栅格地图中，即使起点和目标只隔一个障碍物格子，路径也可能会绕整个障碍物一圈。这不是Dijkstra的问题，而是地图离散化后可行路径本身就变了。验证算法时不建议一开始就用复杂大图，先跑一个如5×5的玩具地图，手动算一遍最短路径，再和程序输出对比，能更快暴露索引或回溯的问题。

4.3 不可达与死区场景：空路径的排查思路

随机地图中障碍物可能把起点或目标包围，这时dist中目标对应的值始终是Inf，min函数会返回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(i+1, :); p2 = path(i+2, :); 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(end+1, :) = path(end, :); end

贝塞尔插值不会改变路径经过的关键节点，只是把转角处圆滑化。更严谨的做法是使用梯度下降的路径平滑，即定义代价函数包含“离障碍物距离”和“路径曲率”两项，迭代调整路径点。对MATLAB原型验证来说，上面的二次贝塞尔已经足够说明平滑效果，要部署到真机时再换用基于时间最优轨迹生成（TOPP）的算法。

5.2 从Dijkstra切换到A*与双向Dijkstra

Dijkstra的劣势在于扩展方向是各向同性的，从起点向四周均匀扩散。若已知目标位置，把搜索方向偏置到目标一侧能显著减少扩展节点数，这就是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表被覆盖的问题，是调试栅格路径规划时成本最低、收益最高的手段之一。

本文还有配套的精品资源，点击获取