本文还有配套的精品资源，点击获取
简介：一套即装即用的Matlab路径规划仿真工具，用Dijkstra算法在二维栅格地图中自动计算起点到终点的最短通行路径，同时支持动态障碍物规避。包含核心寻路脚本Dijkstras.m、地图生成creMap.m、坐标转换Get_xy.m、栅格可视化PlotGrid.m和Plot.m、网格数据格式转换G2D.m，以及调试辅助debug.m和原理说明文档。所有函数在Matlab 2021a实测可用，允许用户自由设定地图大小、障碍物位置、起止点坐标，运行后输出带路径标记的栅格图、节点遍历顺序记录及距离标签更新过程。支持逐层观察算法如何扩展搜索范围、更新最短距离、回溯生成最终路径，适用于机器人导航教学演示、智能体运动规划原型开发、算法原理理解与课程实验验证。
1. 这不是“跑个demo”，而是一套能真正帮你讲清Dijkstra原理的教学级仿真系统
你有没有在讲机器人路径规划课时，被学生问住：“老师，Dijkstra到底怎么‘一层层往外扩’的？那个距离标签到底是怎么更新的？为什么它不直接跳到终点？”——我试过用PPT动画演示，也画过黑板树状图，但直到我把这套Matlab工具包第一次投到教室大屏上，实时拖动障碍物、点击起点终点、按下F5单步运行，看着红色搜索波前一格一格漫延、绿色路径从终点逆向生长出来，学生才真正“看见”了算法的呼吸感。
这不是一个封装好的path = find_path(map, start, goal)黑箱函数。它是一套可拆解、可暂停、可回溯、可质疑的仿真系统。核心关键词——Dijkstra算法、栅格路径规划、Matlab避障仿真——每一个都不是贴在表面的标签，而是贯穿在每一行代码、每一张可视化图、每一次debug输出里的设计逻辑。它面向的不是只想调个API的工程师，而是需要亲手“捏碎算法再组装一遍”的高校教师、课程设计指导者、刚接触运动规划的研究生，以及那些被教科书里抽象伪代码折磨得睡不着觉的本科生。
整套工具包最硬核的价值，在于它把Dijkstra从“图论定义”拉回到“二维空间中的物理行走”：起点是坐标(x_s, y_s)，终点是(x_g, y_g)，障碍物是地图矩阵中值为1的格子，自由通行区域是值为0的格子，每一步移动只允许上下左右四个方向（即4-连通），每条边权重统一设为1（等价于曼哈顿距离最小化）。这个设定看似简单，却精准锚定了机器人在网格化环境（如ROS的costmap、AGV调度场、无人机俯视建模）中最基础、最典型的运动约束。你不需要先学完图论才能上手——打开creMap.m，改两行参数就能生成10×10带随机障碍的地图；运行Dijkstras.m，它会自动调用G2D.m把坐标转成线性索引，用Get_xy.m把索引转回坐标，再通过PlotGrid.m把每一轮open_set扩展过程画成不同颜色的热力图。整个流程没有魔法，只有清晰的数据流和可视化的因果链。
更重要的是，它天然支持“教学调试闭环”：你在debug.m里设置断点，观察dist数组如何从Inf被逐次覆盖为1, 2, 3...；你对比path_result.png和控制台打印的visited_order，就能验证“节点访问顺序是否符合优先队列最小堆逻辑”；你甚至可以把Dijkstras.m里while ~isempty(open_set)循环体拆成三步手动执行——先找最小距离节点，再遍历邻居，再更新距离——就像在白板上一步步推演。这种“算法可触摸性”，是任何现成工具箱（比如MATLAB Robotics System Toolbox里的plannerRRT）都无法替代的。它不追求工业级鲁棒性，但死磕教育级透明度。如果你正为《智能机器人导论》准备实验课，或要给大三学生布置“修改Dijkstra支持对角线移动”的进阶作业，这套工具包就是你讲台上的实体教具。
2. 整体架构与设计逻辑：为什么是这7个文件？它们之间如何咬合？
一套能讲清楚原理的仿真系统，绝不是把算法代码扔进一个.m文件就完事。它必须有清晰的职责边界、低耦合的数据接口、以及可独立验证的模块单元。这套工具包的7个核心.m文件，不是随意堆砌，而是按“数据生成→空间建模→算法执行→过程可视化→结果验证”五段式流水线设计的。下面我带你一层层拆开它的齿轮咬合关系。
2.1 地图生成层：creMap.m——从抽象参数到具象栅格的翻译器
creMap.m是整个仿真的地基。它接收三个输入：rows（行数）、cols（列数）、obstacle_ratio（障碍物密度比），输出一个rows × cols的二值矩阵map。关键在于它的实现逻辑——它不依赖随机种子固定化（避免每次运行地图一样导致学生误以为算法“碰巧成功”），而是用rand(rows, cols) < obstacle_ratio生成布尔矩阵，再强制将起点(1,1)和终点(rows,cols)设为0（确保问题有解）。这个细节很重要：很多初学者写的地图生成器会把起点/终点随机置为障碍，导致算法无解却报错不明，白白消耗调试时间。creMap.m还内置了% Optional: add border walls注释块，提示你可以手动添加一圈边界墙——这是模拟真实机器人不越界的关键约束，只需取消注释并修改map(1,:) = 1; map(end,:) = 1; ...即可。
2.2 坐标与索引转换层：G2D.m与Get_xy.m——打通“数学世界”与“编程世界”的桥梁
Dijkstra算法在图论中操作的是节点编号（1~N），但在栅格地图中，我们天然用二维坐标(i,j)思考。G2D.m负责把坐标(i,j)转为线性索引idx = (j-1)*rows + i（注意：MATLAB是列优先存储，所以是j-1而非i-1）；Get_xy.m则做逆变换：[i,j] = Get_xy(idx, rows, cols)。这两个函数的存在，彻底隔离了算法核心与空间表示——Dijkstras.m内部只处理idx，完全不知道自己在操作哪一行哪一列；而所有可视化（PlotGrid.m）和用户交互（input('Enter start x:')）都基于坐标。这种分离让代码可读性飙升：当你看到dist(idx) = Inf，你知道这是在初始化第idx个节点的距离；当你看到[i,j] = Get_xy(min_idx, rows, cols)，你知道这是在把算法选出的最优节点“翻译”回地图位置。我曾见过太多学生把坐标和索引混用，导致路径画歪、邻居遍历错位，根源就在于缺少这两道“翻译关卡”。
2.3 算法执行层：Dijkstras.m——教科书伪代码的逐行落地
这是整个包的灵魂。它严格遵循CLRS《算法导论》中Dijkstra的标准流程，但做了教学友好化改造：
- 使用dist数组存储当前已知最短距离（初始化为Inf，起点为0）
- 使用prev数组记录路径回溯指针（初始化为0，起点为-1）
- 使用visited布尔数组标记已确定最短路径的节点（避免重复处理）
-关键创新：open_set不采用MATLAB内置的containers.Map或heap类（兼容性差），而是用两个同步向量open_idx和open_dist实现简易最小堆——每次min_idx = open_idx(find(open_dist == min(open_dist), 1))，虽非O(log n)但足够教学演示，且逻辑透明可见。
- 每轮循环末尾插入if mod(iter, 5) == 0, PlotGrid(map, visited, dist, prev, start_idx, goal_idx); pause(0.3); end，实现“每5次迭代刷新一次画面”，避免动画过快无法捕捉。
2.4 可视化层：PlotGrid.m与Plot.m——让算法“活”起来的显微镜
PlotGrid.m是动态过程可视化主力：它用imagesc绘制底图，用不同颜色区分障碍物（黑色）、已访问节点（灰色渐变）、当前open_set节点（黄色）、起点（蓝色星号）、终点（红色方块）、当前最优路径（绿色连线）。特别设计dist数值标注功能：当show_dist == true时，在每个已访问格子中心显示当前距离值（如'3'），让学生亲眼见证“距离标签如何从Inf变为1、2、3…”。而Plot.m是最终结果快照：它只绘制静态路径图，叠加箭头指示行走方向，并保存为path_result.png。两者分工明确——前者用于教学演示，后者用于实验报告截图。
2.5 调试支撑层：debug.m——专为“为什么没走这条路？”设计的探针
debug.m不是简单的disp()集合。它提供三个核心诊断模式：
-debug_mode = 1: 输出每轮迭代的min_idx、对应坐标、当前dist(min_idx)及邻居idx列表
-debug_mode = 2: 打印完整dist数组（reshape为rows×cols）和prev数组，便于定位距离更新异常
-debug_mode = 3: 绘制visited数组的累积热力图，直观显示搜索范围扩张轨迹
我在带学生做课程设计时，常让他们先运行debug_mode=1，观察前三轮迭代——如果第二轮就跳到了终点附近，说明邻居遍历逻辑有误（比如忘了检查边界或障碍）；如果dist数组里出现NaN，立刻知道是除零或未初始化。这种“问题-现象-定位”链条，比任何理论讲解都管用。
提示：所有函数均采用“输入校验+默认参数”设计。例如
Dijkstras.m开头有if nargin < 5, show_plot = true; end，PlotGrid.m支持PlotGrid(map, [], [], [], start, goal)空参数调用，方便学生分阶段测试。
3. 核心细节解析与实操要点：从零开始跑通第一个案例
现在，我们动手跑通一个最简案例：10×10地图，障碍物密度20%，起点(1,1)，终点(10,10)。这不是复制粘贴命令，而是带你理解每一行代码背后的“为什么”。
3.1 第一步：生成地图——creMap.m的隐藏参数与陷阱
在MATLAB命令窗口输入：
map = creMap(10, 10, 0.2);此时map是一个10×10的double矩阵，值为0或1。但请注意两个易错点：
-陷阱1：MATLAB索引从1开始，但学生常误用0-based思维。map(1,1)是左上角，map(10,10)是右下角。若你习惯Python，务必提醒自己这里没有map[0][0]。
-陷阱2：creMap.m默认不显示地图。想立刻查看？补一句figure; imagesc(map); colormap(gray); axis equal; title('Generated Map');。你会发现障碍物是散点状分布，而非连成一片——这正是20%密度的合理表现，避免学生误以为“障碍必须成片才有挑战性”。
实操心得：我建议在
creMap.m末尾加一行assignin('base', 'current_map', map);，这样生成的地图会自动存入工作区变量current_map，后续脚本可直接调用，省去手动赋值。
3.2 第二步：坐标转索引——G2D.m的列优先真相
假设起点坐标是(1,1)，终点是(10,10)。调用：
start_idx = G2D(1, 1, 10); % 返回 1 goal_idx = G2D(10, 10, 10); % 返回 100为什么G2D(10,10,10)等于100？因为MATLAB按列存储：第1列是[1;2;...;10]（索引1~10），第2列是[11;12;...;20]（索引11~20），…，第10列是[91;92;...;100]（索引91~100）。所以(10,10)即第10列第10行，索引=(10-1)*10 + 10 = 100。这个计算必须刻在脑子里，否则当你要手动验证邻居时（比如(i,j)的右邻居是(i,j+1)，索引=(j+1-1)*rows + i = j*rows + i），就会出错。
3.3 第三步：运行主算法——Dijkstras.m的参数详解与调试开关
标准调用：
[path, dist, prev, visited_order] = Dijkstras(map, start_idx, goal_idx);但强烈建议首次运行时开启调试：
[path, dist, prev, visited_order] = Dijkstras(map, start_idx, goal_idx, true, true, 1);参数依次为：地图、起点索引、终点索引、是否绘图、是否显示距离标签、debug模式（1/2/3）。此时你会看到：
- 命令窗滚动输出迭代日志，如Iter 1: min_idx=1, coord=(1,1), dist=0, neighbors=[2,11]
- 图形窗实时刷新，左上角起点亮起，第一轮扩展出右边(1,2)和下边(2,1)
- 当dist(goal_idx)首次被更新（比如变成18），算法尚未结束，因为可能还有更短路径——这正是Dijkstra“贪心但全局最优”的体现，值得暂停讲解。
注意：
Dijkstras.m内部对邻居的遍历顺序是[i-1,j; i+1,j; i,j-1; i,j+1]（上、下、左、右）。如果你想验证“方向偏好是否影响结果”，可临时改成[i,j+1; i+1,j; i,j-1; i-1,j]（右、下、左、上），你会发现最终路径长度不变，但visited_order和中间dist更新序列不同——这恰好说明：Dijkstra的最优性不依赖遍历顺序，只依赖优先队列的最小距离选择。
3.4 第四步：可视化验证——PlotGrid.m的三层信息叠加
PlotGrid.m的调用签名是：
PlotGrid(map, visited, dist, prev, start_idx, goal_idx, show_dist, show_path)其中visited是逻辑数组（已访问为true），dist是距离数组，prev是前驱索引数组。最关键的参数是show_dist（默认false）。当你设为true，会在每个已访问格子中心显示数字：
- 起点(1,1)显示0
- 其右邻居(1,2)显示1
-(1,2)的右邻居(1,3)显示2
- …
- 终点(10,10)显示18
这个数字不是“步数”，而是从起点出发到达该格子的最短路径长度（因每步权重为1，故数值上等于步数）。学生常混淆“距离”与“欧氏距离”，这里用dist数组的数值标注，一目了然地建立“图论距离=栅格移动步数”的认知。
3.5 第五步：结果分析——path数组的解读与逆向工程
Dijkstras.m返回的path是一个索引向量，如[1, 2, 3, 13, 23, 33, ..., 100]。要把它转为坐标序列供后续使用（比如发给机器人控制器），必须用Get_xy.m：
path_coords = zeros(length(path), 2); for k = 1:length(path) [i,j] = Get_xy(path(k), 10, 10); path_coords(k,:) = [i,j]; end此时path_coords是N×2矩阵，每行是(行坐标, 列坐标)。注意：MATLAB绘图用plot(path_coords(:,2), path_coords(:,1), 'g-o')（先列后行！），因为imagesc的X轴对应列，Y轴对应行。
实操心得：我常让学生做个小练习——手动从
prev数组反推路径。给定prev(100)=90,prev(90)=80,prev(80)=70…，让他们写出完整路径索引。这比直接看path输出更能强化“前驱指针构建路径”的理解。
4. 实操过程与核心环节实现：深度拆解Dijkstra的三阶段生命周期
Dijkstra算法在栅格地图上的执行，可清晰划分为三个生命阶段：初始化播种期、搜索扩张期、路径收敛期。这套工具包的精妙之处，在于每个阶段都有对应的可视化反馈和调试入口。下面我们以10×10地图为例，逐帧解析算法如何“活”过来。
4.1 阶段一：初始化播种期（Iteration 0）
当Dijkstras.m启动，它首先执行：
dist = Inf * ones(num_nodes, 1); dist(start_idx) = 0; prev = zeros(num_nodes, 1); visited = false(num_nodes, 1); open_set = [start_idx]; open_dist = [0];此时dist数组有100个元素，仅dist(1)=0，其余全为Inf；prev全为0；visited全为false；open_set=[1]。这是算法的“种子状态”。PlotGrid.m在此时绘制：起点(1,1)为蓝色星号，其余格子为白色（未访问）或黑色（障碍）。关键教学点：为什么起点距离是0？因为“站在起点不动”就是到达起点的最短路径，长度为0。这个看似简单的设定，是整个算法正确性的基石。
提示：在
debug_mode=2下，disp(dist(1:20)')会输出[0, Inf, Inf, ..., Inf]，前20个值。让学生数一数Inf有几个，确认初始化无遗漏。
4.2 阶段二：搜索扩张期（Iteration 1 to N-1）
这是算法最富戏剧性的部分。以第一轮迭代为例：
- 从open_set取最小距离节点：min_idx = 1（唯一元素）
- 标记为已访问：visited(1) = true
- 获取其邻居：neighbors = [2, 11]（右邻居(1,2)索引2，下邻居(2,1)索引11）
- 对每个邻居n：
- 若n未访问且非障碍：if ~visited(n) && map(Get_xy(n,10,10)) == 0
- 计算新距离：new_dist = dist(min_idx) + 1 = 0 + 1 = 1
- 若new_dist < dist(n)（即1 < Inf成立），则更新：dist(n) = 1; prev(n) = min_idx; open_set = [open_set, n]; open_dist = [open_dist, new_dist];
此时dist(2)=1,dist(11)=1,prev(2)=1,prev(11)=1。PlotGrid.m将(1,2)和(2,1)涂为浅灰色，并在其中心标注1。这就是“波前扩散”的第一帧。
随着迭代深入，open_set像滚雪球般扩大。第5轮时，open_set可能包含[2,11,3,12,21]，对应坐标(1,2),(2,1),(1,3),(2,2),(3,1)，dist值均为1或2。此时PlotGrid.m显示一个以起点为中心的“十字形”扩散区。教学重点：Dijkstra的“层”不是几何圆，而是曼哈顿距离等高线。所有dist=2的节点，恰好构成以起点为中心、曼哈顿距离为2的菱形顶点。
实操技巧：在
Dijkstras.m的while循环内加入if iter == 5, save('iter5_state.mat','dist','prev','visited','open_set'); break; end，可保存第5轮中间状态。后续用load('iter5_state.mat')加载，用PlotGrid单独查看，实现“慢镜头回放”。
4.3 阶段三：路径收敛期（Iteration K when goal_idx is visited）
算法终止条件是visited(goal_idx) == true。当某轮迭代中，min_idx恰好等于goal_idx（即终点被选为当前距离最小的未确定节点），算法将其标记为visited，并退出循环。此时dist(goal_idx)就是最短路径长度，prev数组已构建好从起点到终点的完整指针链。
回溯路径的代码在Dijkstras.m末尾：
path = []; curr = goal_idx; while curr ~= -1 path = [curr; path]; curr = prev(curr); end注意prev(start_idx) = -1是人为设定的终止符。因此路径path是以起点开头、终点结尾的索引序列。Plot.m最终绘制时，用line函数连接这些坐标点，形成绿色折线。
关键验证：计算
length(path)-1（路径步数）应等于dist(goal_idx)。例如dist(100)=18，则path应有19个索引（18步）。这是检验算法正确性的黄金法则。我在批改实验报告时，第一眼就看这个等式是否成立。
5. 常见问题与排查技巧实录：那些让我熬夜到三点的坑
即使这套工具包经过2021a严格测试，学生在实操中仍会踩进一些经典陷阱。以下是我收集的Top 5高频问题，附带现场排查日志和根治方案。这些问题不是“代码写错了”，而是对算法本质理解偏差导致的“逻辑病”。
5.1 问题1：路径绕远路，不走直线——“邻居遍历漏掉了对角线？”
现象：在无障碍10×10地图上，起点(1,1)到终点(10,10)，算法返回路径长度为18（正确），但路径是(1,1)->(1,2)->...->(1,10)->(2,10)->...->(10,10)，呈L形，而非对角线方向。
排查日志：
- 运行debug_mode=1，观察前几轮：Iter 1: neighbors=[2,11]（只有右、下）
-Iter 2: min_idx=2, neighbors=[3,12]（(1,2)的右、下）
- 始终未出现[12,21]等对角邻居索引
根因：Dijkstras.m中邻居生成逻辑是neighbors = [i-1,j; i+1,j; i,j-1; i,j+1]，严格4-连通。而学生期望8-连通（含对角线），认为“斜着走更近”。但Dijkstra在4-连通下，曼哈顿距离|dx|+|dy|的最短路径长度恒为18，L形路径只是其中一种合法解（另一解是先下后右）。算法没错，是学生对“最短路径不唯一”缺乏认知。
根治方案：
- 在Dijkstras.m开头添加注释：“本实现采用4-连通邻域，若需8-连通，请修改neighbors为[i-1,j-1; i-1,j; i-1,j+1; …]”
- 或在实验指导书中强调：“请证明：在无障碍栅格中，4-连通Dijkstra的所有最短路径，其曼哈顿距离总和恒为|x_g-x_s|+|y_g-y_s|”
5.2 问题2：程序卡死在while循环——“open_set为空了？”
现象：运行Dijkstras.m后，MATLAB光标一直闪烁，无输出，Ctrl+C中断后显示In Dijkstras at 45（指向while ~isempty(open_set)行）。
排查日志：
- 在循环内加fprintf('Open set size: %d\n', length(open_set));
- 发现输出Open set size: 0后仍进入循环
根因：open_set被意外清空，但while条件未及时检测。常见于学生修改代码时，误删了open_set的更新语句，或在邻居检查中用了break导致open_set未追加新节点。
根治方案：
- 在while循环首行加安全卫士：if isempty(open_set), error('Open set is empty! No path exists.'); end
- 教学提示：“open_set为空意味着从起点出发，所有可达节点均已探索完毕，但仍未到达终点——即路径不存在。此时应报错，而非死循环。”
5.3 问题3：PlotGrid.m报错Index exceeds matrix dimensions——“坐标转索引翻车了”
现象：调用PlotGrid(map, visited, dist, prev, 1, 100)时报错，提示索引超出范围。
排查日志：
-size(map) = [10, 10]
-size(visited) = [100, 1]（正确，线性向量）
-size(dist) = [100, 1]（正确）
- 但visited(100)访问时出错
根因：visited数组长度不是100。检查creMap.m发现学生改成了map = creMap(5, 5, 0.2)，但忘记同步修改Dijkstras.m中的rows、cols参数，导致Get_xy计算错误索引。
根治方案：
- 强制参数绑定：在Dijkstras.m开头添加[rows, cols] = size(map); num_nodes = rows*cols;
- 所有Get_xy调用改为Get_xy(idx, rows, cols)，不再依赖外部传入
- 教学警示：“栅格地图的尺寸信息必须唯一源头，不能在多个地方手动维护。”
5.4 问题4：路径不显示，或显示为单点——“path数组为空或无效”
现象：Plot.m只画出起点和终点，中间无连线；或path返回空数组[]。
排查日志：
-disp(goal_idx)显示100
-disp(visited(100))显示0（false）
-disp(dist(100))显示Inf
根因：终点被障碍物阻挡。creMap.m虽保证起点终点初始为0，但学生可能在生成后手动修改了map(10,10)=1，或Dijkstras.m中邻居检查漏掉了边界判断，导致终点从未被加入open_set。
根治方案：
- 在Dijkstras.m中，对每个邻居n增加双重校验：matlab if n >= 1 && n <= num_nodes && ~visited(n) && map(Get_xy(n, rows, cols)) == 0 % update logic end
- 添加终点可达性预检：if map(Get_xy(goal_idx, rows, cols)) == 1, error('Goal is blocked!'); end
5.5 问题5：debug.m输出乱码，或dist数组全是NaN——“未初始化的幽灵变量”
现象：debug_mode=2输出dist数组，大量元素为NaN，而非Inf。
排查日志：
-whos dist显示dist为double，size正确
- 但dist(1)是NaN，非0
根因：学生在Dijkstras.m中误写了dist = NaN * ones(...)，或在更新时用了dist(n) = dist(min_idx) + NaN（因dist(min_idx)本身是NaN）。NaN具有传染性，一旦引入，整个计算失效。
根治方案：
- 初始化强制用Inf：dist = Inf * ones(num_nodes, 1);
- 更新前加校验：if ~isfinite(dist(min_idx)), error('Distance is NaN or Inf at min_idx'); end
- 教学金句：“Inf是算法的‘未知’，NaN是程序的‘崩溃’。永远用Inf初始化距离。”
6. 进阶应用与教学拓展：从仿真到真实世界的桥接
这套工具包的价值，远不止于跑通一个Dijkstra demo。它的模块化设计，天然支持向更高阶能力延伸。以下是我在实际教学中验证过的三条拓展路径，每一条都配有可立即上手的代码片段和教学目标。
6.1 拓展一：支持动态障碍物——让地图“活”起来
真实机器人导航中，障碍物会移动（如行人、其他AGV）。我们可以改造Dijkstras.m，使其接受一个“障碍物更新函数句柄”：
function [path, ...] = Dijkstras(map, start_idx, goal_idx, ..., obs_update_func) % ... initialization ... for iter = 1:max_iter % ... standard Dijkstra step ... % Dynamic update: call user function to modify map if nargin >= 7 && isfunction(obs_update_func) map = obs_update_func(map, iter, visited, dist); end % ... continue ... end end学生可编写my_moving_obstacle.m：
function new_map = my_moving_obstacle(old_map, iter, visited, dist) new_map = old_map; % Move an obstacle from (3,3) to (3,4) at iter 10 if iter == 10 new_map(3,3) = 0; % clear old new_map(3,4) = 1; % set new end end调用：Dijkstras(map, 1, 100, true, true, 1, @my_moving_obstacle)。这让学生直观理解“重规划”的必要性——当障碍突现，原路径失效，算法如何快速响应。
6.2 拓展二：集成A*算法——对比启发式的价值
Dijkstra是A的特例（启发函数h=0）。我们可在同一框架下实现A，只需修改距离计算：
% In Dijkstras.m, replace: % new_dist = dist(min_idx) + 1; % With: h_n = abs(i_g - i_n) + abs(j_g - j_n); % Manhattan heuristic new_dist = dist(min_idx) + 1 + h_n;然后对比：在相同地图上，Dijkstra访问节点数 vs A访问节点数。学生会发现A只探索终点方向的“扇形区”，而Dijkstra探索整个“圆形区”。这完美诠释了“启发式如何引导搜索”。
6.3 拓展三：对接ROS仿真——从MATLAB到真实机器人
虽然MATLAB不直接运行在机器人上，但其路径可导出为ROS格式。在Plot.m末尾添加：
% Export to ROS Path message format ros_path = struct(); ros_path.header.stamp = rostime('now'); ros_path.header.frame_id = 'map'; ros_path.poses = cell(length(path), 1); for k = 1:length(path) [i,j] = Get_xy(path(k), rows, cols); % Convert grid coord to meter: assume 0.5m/cell, origin at (0,0) x = (j - 1) * 0.5; y = (rows - i) * 0.5; % flip Y for ROS convention ros_path.poses{k} = struct('position', struct('x',x,'y',y,'z',0)); end save('ros_path.mat', 'ros_path');学生可将ros_path.mat导入ROS Python节点，用rospy.Publisher('/move_base/NavfnROS/plan', Path)发布，驱动TurtleBot3沿MATLAB规划路径行走。这一刻，“仿真”与“现实”的鸿沟被真正跨越。
最后分享一个小技巧：在
creMap.m中，把obstacle_ratio参数改为obstacle_positions，接受N×2坐标矩阵，就能精确放置教学用障碍物（如“U型迷宫”、“窄通道”）。这比随机生成更能聚焦算法弱点——比如在窄通道中，Dijkstra会因盲目扩展而效率低下，自然引出D* Lite等增量式算法的必要性。教学，从来不是展示算法多强大，而是暴露它在哪种场景下会喘不过气来。
本文还有配套的精品资源，点击获取
简介：一套即装即用的Matlab路径规划仿真工具，用Dijkstra算法在二维栅格地图中自动计算起点到终点的最短通行路径，同时支持动态障碍物规避。包含核心寻路脚本Dijkstras.m、地图生成creMap.m、坐标转换Get_xy.m、栅格可视化PlotGrid.m和Plot.m、网格数据格式转换G2D.m，以及调试辅助debug.m和原理说明文档。所有函数在Matlab 2021a实测可用，允许用户自由设定地图大小、障碍物位置、起止点坐标，运行后输出带路径标记的栅格图、节点遍历顺序记录及距离标签更新过程。支持逐层观察算法如何扩展搜索范围、更新最短距离、回溯生成最终路径，适用于机器人导航教学演示、智能体运动规划原型开发、算法原理理解与课程实验验证。
本文还有配套的精品资源，点击获取
