简介基于栅格地图的Dijkstra算法路径规划资源面向学习MATLAB路径规划与图搜索算法的开发者解决在栅格化环境中从起点到终点的最短路径求解问题可应用于机器人导航、游戏AI与GIS分析等场景。资源包共6个文件以5个.m脚本为主包含DijkstraPlan、DijkstraSample、Dijkstraguihua等核心程序配套1张运行效果图压缩包仅56KB便于快速下载参考。已有5026人学习浏览。读者通过源码与示例可理解Dijkstra算法的贪心扩展逻辑、优先队列实现方式以及栅格地图的障碍物建模、邻居遍历和路径回溯等关键步骤还能对照地图数据初始化、最短路径更新与终点反推等环节掌握将算法迁移到实际工程中的完整思路。脚本结构清晰、注释直接适合在MATLAB中直接运行、修改参数或进一步封装复用。 我不止一次跟做机器人的朋友说如果让我从所有路径规划算法里挑一个做“看家本领”那一定是在栅格地图上跑Dijkstra。别急着反驳A确实快RRT也确实能处理高维空间但Dijkstra在栅格地图上有一种“笨拙的可靠”只要你给它一张完整的图它就敢跟你保证找到的那条路一定是代价最小的路。这种“稳”在工程里太宝贵了。这篇文章我想把整套东西从头到尾捋一遍——怎么把环境变成栅格地图、怎么设计数据结构让Dijkstra跑得动、怎么处理膨胀层和动态障碍物、以及最后一个能跑的C实现长什么样。适合刚入坑机器人路径规划的学生也适合那些已经在跑ROS但只想用“最朴素的算法”解决全局规划问题的工程师。1. 整体思路与方案选型1.1 栅格地图与Dijkstra“门当户对”的原因很多人有疑问Dijkstra不是图搜索算法吗栅格地图不是网格吗这俩怎么结合其实栅格地图天然就是一张加权图。每一个格子是一个节点格子与格子之间的相邻关系就是边。四邻域的栅格地图每个格子最多有四条边八邻域的话就变成八条。而Dijkstra在图中做的事情是维护一个“从起点到当前节点的最短路径代价”然后不断从待处理队列里挑代价最小的节点进行松弛操作。在栅格地图上这有什么好处好处是格子之间的代价关系极其清晰。每个格子到相邻格子的移动代价是固定的——走直线是1个单位的代价走对角线如果开了八邻域可以设成1.414也就是勾股定理的根号2。这种“代价可计算”的特性让Dijkstra不用像RRT那样靠采样碰运气也不用像A*那样需要设计一个高质量的启发函数。我见过不止一个项目把A*用在栅格地图上发现因为启发函数设计得不好导致路径贴着障碍物走最后还得加平滑模块。Dijkstra没有这个问题它就是纯BFS的加权版本沿着代价等值线“一圈一圈”往外扩展虽然慢但扩展过程就是最短路树的生长过程每一个节点的代价在被确定时就已经是全局最优解。1.2 全局规划与局部避障的分工设计实际做机器人导航时我不建议用Dijkstra直接处理动态障碍物因为动态环境意味着地图在实时变化而Dijkstra每次重新规划都是一次全图重搜索计算量扛不住。更工程化的做法是分层规划上游是一个全局规划器用Dijkstra或者A*在静态的栅格地图上算出一条从起点到目标点的全局路径下游是一个局部规划器比如DWA动态窗口法或TEB负责在跟随全局路径的同时感知周围动态障碍物实时调整速度与方向。这么拆的好处在于职责单一Dijkstra只负责“在已知地图上找最优”局部规划器只负责“在实时感知中避障”。全局规划器不需要实时感知局部规划器不需要全图搜索。这种分工也是ROS里move_base的默认架构全局路径规划器用navfn或global_planner插件局部用base_local_planner各自维护各自的代价地图。如果你只是在做一个课设级的“动态避障小车”最省事的做法是先用Dijkstra出一条全局路径然后把这条路径离散成一串路标点小车用纯追踪Pure Pursuit去跟踪路标点碰到临时障碍物时用激光雷达的数据做VFH向量场直方图或者简单地让小车停下来绕行绕过之后再回到最近的全局路标点继续走。这比实时重跑Dijkstra靠谱得多。1.3 为什么不用A*绕不开的问题既然A*比Dijkstra快为什么题目是Dijkstra我的回答是A的快建立在启发函数足够好的前提下。启发函数太乐观A会退化成Dijkstra启发函数太悲观A*可能找不到最优解。Dijkstra没有这个问题它是无信息搜索准确率是“确定性”的——它找的路径是数学意义上的全局最短。在很多比赛和工程场景里“最优性”比“实时性”更重要比如喷漆路径规划、泊车路径规划这种离线计算场景Dijkstra跑出来的平滑最优路径后期省掉的平滑处理工作量远超它多花的那点计算时间。而且在栅格地图规模不算大的场景里比如100x100的栅格一万个节点Dijkstra的性能其实是完全够用的。我用C写过一版在普通笔记本上处理100x100的八邻域地图找一条路径的耗时在10~30毫秒级别对轮式机器人来说这个速度完全能接受。只有当你的地图到了千米级比如1000x1000一百万个节点Dijkstra的时间开销才会变得不可忽略这时候才需要认真考虑A*或JPS。2. 栅格地图构建细节2.1 占用栅格地图Occupancy Grid Map的基本逻辑做路径规划的第一步不是写搜索算法而是把环境变成栅格地图。ROS里最常见的表示方式是OccupancyGrid每个格子存储一个0到100的整数0该格子确定是空闲的机器人可以走100该格子确定被占据的机器人不能走-1未知区域机器人还没有探测到为什么不用0和1的布尔值因为激光雷达的数据有噪声栅格地图需要表达“不确定性”。一次扫描中如果有5个点落在这个格子里那这个格子大概率是墙如果有1个点落进去那可能是噪声。所以占用栅格地图本质上是一个概率模型用贝叶斯更新不断修正格子的占据概率。这就是为什么SLAM建图后能用激光雷达“看到”墙背后的轮廓——当然栅格地图不会真的看到墙后的东西它只是把多次扫描的概率累加起来让不确定的格子逐渐“收敛”到一个确定状态。自己写建图程序时一个省事的方案是把栅格地图存成二维数组0表示可通行1表示障碍物。但真正要对接机器人实机时还是要按OccupancyGrid的格式来因为代价地图层和导航栈都认这个格式。2.2 膨胀层把机器人当成“一个点”太危险做栅格地图时最容易忽略的一个环节是膨胀。很多初学者直接把激光雷达数据塞进栅格地图然后拿一个“点机器人”去做路径规划结果路径贴着墙走实机一跑就撞。问题出在栅格地图里没有机器人的体积概念。一个10厘米宽的机器人在栅格分辨率为5厘米的地图里至少要占掉2个格子的宽度。路径规划如果只考虑中心点所在的格子机器人转弯时旋转半径会让车体边缘扫到障碍物。标准解决方案是给障碍物做膨胀Inflation对每个障碍物格子向外扩展若干个格子扩展区间内的格子被标记为“危险区域”禁止路径通过膨胀半径至少等于机器人内切圆半径机器人在原地旋转时车体扫过的最大半径更精确的做法把机器人近似成圆形膨胀半径 机器人半径 安全余量一个从实践中总结的经验膨胀半径别设得刚刚好最好在理论值基础上多给1~2个格子的余量。因为Dijkstra规划出的路径只是“理论可通行”实际跟踪时由于PID控制误差、惯性、地面打滑等因素车体轨迹会偏离理论路径多出来的余量就是给这些误差留的缓冲。我见过很多小车撞墙不是算法不行而是膨胀半径设小了。2.3 栅格分辨率的取舍栅格地图的分辨率直接影响两条路径的精细度和计算量。分辨率越高栅格越密地图越精细但节点数量呈平方级增长Dijkstra的搜索时间也会暴涨。我的经验法则是栅格分辨率大约是机器人直径的1/4到1/2。比如30厘米宽的机器人用5~10厘米分辨率的栅格比较合适。分辨率设得比机器人直径还粗那等于让机器人在地图里“穿墙”了因为能走的通道宽度都不够车身转弯。室内场景一般用0.05米5厘米分辨率比较多室外大场景用0.1~0.2米。这是一个典型的“效果与性能的权衡”没有绝对的标准需要根据实际场景调试。3. Dijkstra核心实现与代码详解3.1 数据结构设计在栅格地图上实现Dijkstra核心数据结构有三个地图矩阵二维数组存储每个格子是否可通行代价矩阵二维数组存储从起点到每个格子的最短代价初始化成无穷大优先队列C里的priority_queue用来高效取出当前代价最小的待处理节点为什么用优先队列而不是普通队列因为Dijkstra每一轮都要从“所有未访问节点”中选出代价最小的那个。如果遍历整个地图来找最小值每次的时间复杂度是O(N)N是节点数整张图就是O(N^2)在100x100地图里就得算一亿次太慢了。优先队列的底层是二叉堆插入和弹出都是O(logN)整体复杂度降到O(NlogN)地图越大收益越明显。C里priority_queue默认是大顶堆取最大值所以我们需要自定义比较函数让它变成小顶堆。这是自己手写Dijkstra时最容易踩的坑我见过很多次有人写完了队列pop出来的节点全是代价最大的debug半天发现是堆序搞反了。3.2 完整C代码实现下面给一个可以直接跑的版本四邻域版本地图用0和1表示#include iostream #include vector #include queue #include limits #include algorithm using namespace std; struct Node { int x, y; int cost; // 优先队列需要的是小顶堆所以这里要反过来比较 bool operator(const Node other) const { return cost other.cost; } }; vectorpairint, int dijkstra(const vectorvectorint grid, pairint, int start, pairint, int goal) { int rows grid.size(); int cols grid[0].size(); // 代价矩阵初始化为无穷大 vectorvectorint cost(rows, vectorint(cols, numeric_limitsint::max())); // 父节点矩阵用于回溯路径 vectorvectorpairint, int parent(rows, vectorpairint, int(cols, {-1, -1})); // 访问标记 vectorvectorbool visited(rows, vectorbool(cols, false)); // 四邻域方向 int dx[4] {-1, 1, 0, 0}; int dy[4] {0, 0, -1, 1}; priority_queueNode, vectorNode, greaterNode pq; cost[start.first][start.second] 0; pq.push({start.first, start.second, 0}); while (!pq.empty()) { Node cur pq.top(); pq.pop(); if (visited[cur.x][cur.y]) continue; visited[cur.x][cur.y] true; // 到达终点提前终止 if (cur.x goal.first cur.y goal.second) break; for (int i 0; i 4; i) { int nx cur.x dx[i]; int ny cur.y dy[i]; // 边界检查 if (nx 0 || nx rows || ny 0 || ny cols) continue; // 障碍物检查 if (grid[nx][ny] 1) continue; // 已经访问过就不处理 if (visited[nx][ny]) continue; int new_cost cur.cost 1; // 四邻域每步代价为1 if (new_cost cost[nx][ny]) { cost[nx][ny] new_cost; parent[nx][ny] {cur.x, cur.y}; pq.push({nx, ny, new_cost}); } } } // 回溯路径 vectorpairint, int path; if (cost[goal.first][goal.second] numeric_limitsint::max()) { return path; // 无路径可走 } pairint, int cur goal; while (!(cur.first start.first cur.second start.second)) { path.push_back(cur); cur parent[cur.first][cur.second]; } path.push_back(start); reverse(path.begin(), path.end()); return path; } int main() { // 示例地图0表示可通行1表示障碍物 vectorvectorint grid { {0, 0, 0, 0, 1, 0, 0, 0}, {0, 1, 1, 0, 1, 0, 1, 0}, {0, 0, 0, 0, 0, 0, 1, 0}, {1, 1, 0, 1, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0}, {0, 1, 0, 1, 0, 0, 0, 1}, {0, 0, 0, 1, 0, 1, 0, 0}, }; auto path dijkstra(grid, {0, 0}, {6, 7}); if (path.empty()) { cout 没有找到路径 endl; } else { cout 路径长度: path.size() - 1 endl; for (auto p : path) { cout ( p.first , p.second ) ; } cout endl; } return 0; }这段代码核心逻辑就三步从堆里弹最小代价节点、扩展邻居、更新代价并记录父节点。第17行的operator重载是唯一需要记住的“魔法”——priority_queue默认按最大元素排前面重载成才能让它变成小顶堆。如果你不想重载运算符也可以使用priority_queueNode, vectorNode, functionbool(Node, Node)加一个lambda表达式指定比较规则。3.3 八邻域扩展与对角线代价上面的代码用的是四邻域也就是机器人只能上下左右走。如果允许机器人斜着走路径会更短、更自然但代价计算要改一下// 八邻域方向 int dx[8] {-1, -1, -1, 0, 0, 1, 1, 1}; int dy[8] {-1, 0, 1, -1, 1, -1, 0, 1}; // 代价计算对角线是根号2直线是1 double step_cost (dx[i] ! 0 dy[i] ! 0) ? 1.414 : 1.0;这时候因为出现了浮点数代价矩阵和Node里的cost都要从int改成double。还有一个细节开了八邻域后机器人会“切墙角”。它不会真的撞上去因为膨胀层的格子已经被标为障碍物了但如果膨胀半径不够大斜穿障碍物角点的路径在实机上可能会擦到障碍物边缘。所以我的建议是凡是开了八邻域的规划膨胀半径最少要再加一个栅格。或者干脆别开八邻域在四邻域路径上用B样条曲线做平滑效果会更好。4. 从仿真到实车的完整实操过程4.1 仿真环境里的部署方法如果你是ROS用户最省事的是用move_base框架把全局规划器换成自己写的Dijkstra插件。具体操作用不到自己重新发明一轮只要实现nav_core::BaseGlobalPlanner接口然后在yaml文件里配置一下base_global_planner: my_dijkstra_planner/MyDijkstraPlanner但如果你想彻底搞懂整个流程我建议还是先从纯仿真开始不用ROS用Python的matplotlib可视化。步骤很简单用openCV或者PIL手绘一张二值地图黑色是障碍物白色是空地把它读成numpy数组0和1的二维矩阵对障碍物做膨胀处理把障碍物周围n个格子的值也置为1把地图、起点、终点作为Dijkstra输入计算路径用matplotlib把地图和路径画出来绿色线是搜索结果这个流程做一遍你就能直观感受到Dijkstra的“波纹扩散”过程。在起点周围路径代价等值线像水波一样从起点向外一圈一圈扩散直到触及终点。这种可视化对理解算法本质帮助巨大比盯着代码看一小时都管用。4.2 实车部署的坑与对策仿真跑通了不代表实车能跑下面这些坑我基本都踩过算力不足导致路径规划卡顿。树莓派这类低算力平台跑Dijkstra100x100地图勉强能实时地图再大就得卡。对策是把大图切块处理或者只在起点附近的小范围内重规划远端路径沿用上次的结果。地图坐标系与机器人坐标系没有对齐。路径规划算出的是一串栅格坐标实车控制程序必须把它转换到以机器人底盘为原点的局部坐标系。这个转换通常是map - odom - base_link的TF链ROS里一条lookupTransform就能拿到但很多自研代码没有引入TF导致路径点在小车坐标系里位置偏了几十个格子小车冲出去直接撞墙。全局路径与局部避障的衔接不顺畅。我之前实现过一个方案Dijkstra规划出全局路径后每隔0.2米取一个路标点小车用纯追踪跟踪路标点。如果局部感知发现前方有动态障碍物小车先使用VFH算法找一个不碰撞的方向绕过去绕行结束后再回到离当前位置最近的全局路标点继续追踪。实测下来只要膨胀层设置合理这套方案在一米每秒的速度下表现还是很稳的。4.3 搜索结果可视化与调试调试Dijkstra特别依赖可视化。你光看路径结果很难判断“为什么这里绕了一大圈”但如果你把代价矩阵画出来一眼就能看出问题——可能是膨胀层把窄道封死了或者地图本身的连通性判断有误。我常用的调试流程是打印起点、终点在栅格地图中的坐标确认没有落在障碍物里画出整张代价矩阵的热力图检查从起点到终点是否存在一条“代价逐渐增大”的连续梯度带画出搜索过程中已经访问过的节点确认优先队列扩展的确实是“从外到内”的等值线最后再画最终的路径如果第2步就发现代价梯度在某处断裂说明地图的连通性有问题需要检查障碍物标记是否把本应联通的区域切断了。5. 常见问题与排查技巧5.1 问题速查表现象可能原因排查与对策程序跑完但路径为空起点或终点在障碍物里打印检查起点终点的栅格值做越界与碰撞检查路径明显绕远优先队列堆序错误检查operator或lambda比较器的返回值方向路径穿墙膨胀半径不够增大膨胀半径至少覆盖机器人内切圆半径规划速度慢地图分辨率太高或开了八邻域降低分辨率或者改用A*或者只在局部重规划实车转弯撞墙膨胀层没生效确认规划的costmap和感知用的costmap是否同一张路径抖动地图中未知区域被当作可通行区域把未知区域(-1)也当作障碍物对待5.2 性能优化的三个实用手段如果Dijkstra跑大图真的慢有三个手段按顺序来双向Dijkstra。从起点和终点同时开始搜索两边各扩展一半相交时路径就找到了。在栅格地图上实测能减少约40%的搜索节点数量。实现上稍微复杂一点但原理还是一样的。多分辨率地图。先在一个低分辨率版本的地图上用Dijkstra粗规划一条走廊再回到高分辨率地图里只在走廊范围内精规划。这其实就是“分层规划”思想的简单版在超大场景比如整个园区非常有效。随时序增量搜索。地图变化不大的时候上一轮的路径大部分仍然有效。把Dijkstra替换成D* Lite它可以利用上次的搜索结果增量更新动态环境下性能能提升一个数量级。5.3 C实现里最容易被忽视的两个细节第一个是无穷大的选择。如果你用INT_MAX作为初始代价做加法和比较时要特别小心——cur.cost 1可能会溢出。我的习惯是初始代价设成一个比地图中任意实际路径代价都大的数比如rows * cols * 2既不会溢出也不会影响比较。第二个是提前终止条件。如果只关心起点到终点的最短路径那当终点被标记为visited时就可以跳出循环了不必把整张图都搜完。这个优化在节点多的地图上能省掉一大半时间。但要记住这个优化有一个前提你是用priority_queue按代价升序处理的那“终点首次被访问时就是最优解”这个结论才成立。如果是用普通队列做的BFS式处理就不能随便提前退出。最后再分享一个实际体会我在真实项目中回过头来审视Dijkstra最大的感悟是这个算法最困难的部分不是搜索过程而是它前面的地图处理与膨胀参数调优。很多同学把大量精力花在理解堆、松弛、复杂度分析上结果代码跑出来的路径因为地图建得不对一样一塌糊涂。我做项目时的习惯是“三分算法七分地图”把地图处理到“可信任”的状态路径规划器随便写都能走得通。如果你现在正被A*和RRT的各种变体搞得焦头烂额不妨回头试试这个最基础的Dijkstra。先把一条路径通过栅格地图稳稳当当地规划出来再去考虑效率和实时性。这个过程中建立的地图表征能力、代价计算直觉、分层规划思维会让你后面学任何先进的规划算法都事半功倍。本文还有配套的精品资源点击获取
Dijkstra栅格地图路径规划：从建图到C++工程实践
简介基于栅格地图的Dijkstra算法路径规划资源面向学习MATLAB路径规划与图搜索算法的开发者解决在栅格化环境中从起点到终点的最短路径求解问题可应用于机器人导航、游戏AI与GIS分析等场景。资源包共6个文件以5个.m脚本为主包含DijkstraPlan、DijkstraSample、Dijkstraguihua等核心程序配套1张运行效果图压缩包仅56KB便于快速下载参考。已有5026人学习浏览。读者通过源码与示例可理解Dijkstra算法的贪心扩展逻辑、优先队列实现方式以及栅格地图的障碍物建模、邻居遍历和路径回溯等关键步骤还能对照地图数据初始化、最短路径更新与终点反推等环节掌握将算法迁移到实际工程中的完整思路。脚本结构清晰、注释直接适合在MATLAB中直接运行、修改参数或进一步封装复用。 我不止一次跟做机器人的朋友说如果让我从所有路径规划算法里挑一个做“看家本领”那一定是在栅格地图上跑Dijkstra。别急着反驳A确实快RRT也确实能处理高维空间但Dijkstra在栅格地图上有一种“笨拙的可靠”只要你给它一张完整的图它就敢跟你保证找到的那条路一定是代价最小的路。这种“稳”在工程里太宝贵了。这篇文章我想把整套东西从头到尾捋一遍——怎么把环境变成栅格地图、怎么设计数据结构让Dijkstra跑得动、怎么处理膨胀层和动态障碍物、以及最后一个能跑的C实现长什么样。适合刚入坑机器人路径规划的学生也适合那些已经在跑ROS但只想用“最朴素的算法”解决全局规划问题的工程师。1. 整体思路与方案选型1.1 栅格地图与Dijkstra“门当户对”的原因很多人有疑问Dijkstra不是图搜索算法吗栅格地图不是网格吗这俩怎么结合其实栅格地图天然就是一张加权图。每一个格子是一个节点格子与格子之间的相邻关系就是边。四邻域的栅格地图每个格子最多有四条边八邻域的话就变成八条。而Dijkstra在图中做的事情是维护一个“从起点到当前节点的最短路径代价”然后不断从待处理队列里挑代价最小的节点进行松弛操作。在栅格地图上这有什么好处好处是格子之间的代价关系极其清晰。每个格子到相邻格子的移动代价是固定的——走直线是1个单位的代价走对角线如果开了八邻域可以设成1.414也就是勾股定理的根号2。这种“代价可计算”的特性让Dijkstra不用像RRT那样靠采样碰运气也不用像A*那样需要设计一个高质量的启发函数。我见过不止一个项目把A*用在栅格地图上发现因为启发函数设计得不好导致路径贴着障碍物走最后还得加平滑模块。Dijkstra没有这个问题它就是纯BFS的加权版本沿着代价等值线“一圈一圈”往外扩展虽然慢但扩展过程就是最短路树的生长过程每一个节点的代价在被确定时就已经是全局最优解。1.2 全局规划与局部避障的分工设计实际做机器人导航时我不建议用Dijkstra直接处理动态障碍物因为动态环境意味着地图在实时变化而Dijkstra每次重新规划都是一次全图重搜索计算量扛不住。更工程化的做法是分层规划上游是一个全局规划器用Dijkstra或者A*在静态的栅格地图上算出一条从起点到目标点的全局路径下游是一个局部规划器比如DWA动态窗口法或TEB负责在跟随全局路径的同时感知周围动态障碍物实时调整速度与方向。这么拆的好处在于职责单一Dijkstra只负责“在已知地图上找最优”局部规划器只负责“在实时感知中避障”。全局规划器不需要实时感知局部规划器不需要全图搜索。这种分工也是ROS里move_base的默认架构全局路径规划器用navfn或global_planner插件局部用base_local_planner各自维护各自的代价地图。如果你只是在做一个课设级的“动态避障小车”最省事的做法是先用Dijkstra出一条全局路径然后把这条路径离散成一串路标点小车用纯追踪Pure Pursuit去跟踪路标点碰到临时障碍物时用激光雷达的数据做VFH向量场直方图或者简单地让小车停下来绕行绕过之后再回到最近的全局路标点继续走。这比实时重跑Dijkstra靠谱得多。1.3 为什么不用A*绕不开的问题既然A*比Dijkstra快为什么题目是Dijkstra我的回答是A的快建立在启发函数足够好的前提下。启发函数太乐观A会退化成Dijkstra启发函数太悲观A*可能找不到最优解。Dijkstra没有这个问题它是无信息搜索准确率是“确定性”的——它找的路径是数学意义上的全局最短。在很多比赛和工程场景里“最优性”比“实时性”更重要比如喷漆路径规划、泊车路径规划这种离线计算场景Dijkstra跑出来的平滑最优路径后期省掉的平滑处理工作量远超它多花的那点计算时间。而且在栅格地图规模不算大的场景里比如100x100的栅格一万个节点Dijkstra的性能其实是完全够用的。我用C写过一版在普通笔记本上处理100x100的八邻域地图找一条路径的耗时在10~30毫秒级别对轮式机器人来说这个速度完全能接受。只有当你的地图到了千米级比如1000x1000一百万个节点Dijkstra的时间开销才会变得不可忽略这时候才需要认真考虑A*或JPS。2. 栅格地图构建细节2.1 占用栅格地图Occupancy Grid Map的基本逻辑做路径规划的第一步不是写搜索算法而是把环境变成栅格地图。ROS里最常见的表示方式是OccupancyGrid每个格子存储一个0到100的整数0该格子确定是空闲的机器人可以走100该格子确定被占据的机器人不能走-1未知区域机器人还没有探测到为什么不用0和1的布尔值因为激光雷达的数据有噪声栅格地图需要表达“不确定性”。一次扫描中如果有5个点落在这个格子里那这个格子大概率是墙如果有1个点落进去那可能是噪声。所以占用栅格地图本质上是一个概率模型用贝叶斯更新不断修正格子的占据概率。这就是为什么SLAM建图后能用激光雷达“看到”墙背后的轮廓——当然栅格地图不会真的看到墙后的东西它只是把多次扫描的概率累加起来让不确定的格子逐渐“收敛”到一个确定状态。自己写建图程序时一个省事的方案是把栅格地图存成二维数组0表示可通行1表示障碍物。但真正要对接机器人实机时还是要按OccupancyGrid的格式来因为代价地图层和导航栈都认这个格式。2.2 膨胀层把机器人当成“一个点”太危险做栅格地图时最容易忽略的一个环节是膨胀。很多初学者直接把激光雷达数据塞进栅格地图然后拿一个“点机器人”去做路径规划结果路径贴着墙走实机一跑就撞。问题出在栅格地图里没有机器人的体积概念。一个10厘米宽的机器人在栅格分辨率为5厘米的地图里至少要占掉2个格子的宽度。路径规划如果只考虑中心点所在的格子机器人转弯时旋转半径会让车体边缘扫到障碍物。标准解决方案是给障碍物做膨胀Inflation对每个障碍物格子向外扩展若干个格子扩展区间内的格子被标记为“危险区域”禁止路径通过膨胀半径至少等于机器人内切圆半径机器人在原地旋转时车体扫过的最大半径更精确的做法把机器人近似成圆形膨胀半径 机器人半径 安全余量一个从实践中总结的经验膨胀半径别设得刚刚好最好在理论值基础上多给1~2个格子的余量。因为Dijkstra规划出的路径只是“理论可通行”实际跟踪时由于PID控制误差、惯性、地面打滑等因素车体轨迹会偏离理论路径多出来的余量就是给这些误差留的缓冲。我见过很多小车撞墙不是算法不行而是膨胀半径设小了。2.3 栅格分辨率的取舍栅格地图的分辨率直接影响两条路径的精细度和计算量。分辨率越高栅格越密地图越精细但节点数量呈平方级增长Dijkstra的搜索时间也会暴涨。我的经验法则是栅格分辨率大约是机器人直径的1/4到1/2。比如30厘米宽的机器人用5~10厘米分辨率的栅格比较合适。分辨率设得比机器人直径还粗那等于让机器人在地图里“穿墙”了因为能走的通道宽度都不够车身转弯。室内场景一般用0.05米5厘米分辨率比较多室外大场景用0.1~0.2米。这是一个典型的“效果与性能的权衡”没有绝对的标准需要根据实际场景调试。3. Dijkstra核心实现与代码详解3.1 数据结构设计在栅格地图上实现Dijkstra核心数据结构有三个地图矩阵二维数组存储每个格子是否可通行代价矩阵二维数组存储从起点到每个格子的最短代价初始化成无穷大优先队列C里的priority_queue用来高效取出当前代价最小的待处理节点为什么用优先队列而不是普通队列因为Dijkstra每一轮都要从“所有未访问节点”中选出代价最小的那个。如果遍历整个地图来找最小值每次的时间复杂度是O(N)N是节点数整张图就是O(N^2)在100x100地图里就得算一亿次太慢了。优先队列的底层是二叉堆插入和弹出都是O(logN)整体复杂度降到O(NlogN)地图越大收益越明显。C里priority_queue默认是大顶堆取最大值所以我们需要自定义比较函数让它变成小顶堆。这是自己手写Dijkstra时最容易踩的坑我见过很多次有人写完了队列pop出来的节点全是代价最大的debug半天发现是堆序搞反了。3.2 完整C代码实现下面给一个可以直接跑的版本四邻域版本地图用0和1表示#include iostream #include vector #include queue #include limits #include algorithm using namespace std; struct Node { int x, y; int cost; // 优先队列需要的是小顶堆所以这里要反过来比较 bool operator(const Node other) const { return cost other.cost; } }; vectorpairint, int dijkstra(const vectorvectorint grid, pairint, int start, pairint, int goal) { int rows grid.size(); int cols grid[0].size(); // 代价矩阵初始化为无穷大 vectorvectorint cost(rows, vectorint(cols, numeric_limitsint::max())); // 父节点矩阵用于回溯路径 vectorvectorpairint, int parent(rows, vectorpairint, int(cols, {-1, -1})); // 访问标记 vectorvectorbool visited(rows, vectorbool(cols, false)); // 四邻域方向 int dx[4] {-1, 1, 0, 0}; int dy[4] {0, 0, -1, 1}; priority_queueNode, vectorNode, greaterNode pq; cost[start.first][start.second] 0; pq.push({start.first, start.second, 0}); while (!pq.empty()) { Node cur pq.top(); pq.pop(); if (visited[cur.x][cur.y]) continue; visited[cur.x][cur.y] true; // 到达终点提前终止 if (cur.x goal.first cur.y goal.second) break; for (int i 0; i 4; i) { int nx cur.x dx[i]; int ny cur.y dy[i]; // 边界检查 if (nx 0 || nx rows || ny 0 || ny cols) continue; // 障碍物检查 if (grid[nx][ny] 1) continue; // 已经访问过就不处理 if (visited[nx][ny]) continue; int new_cost cur.cost 1; // 四邻域每步代价为1 if (new_cost cost[nx][ny]) { cost[nx][ny] new_cost; parent[nx][ny] {cur.x, cur.y}; pq.push({nx, ny, new_cost}); } } } // 回溯路径 vectorpairint, int path; if (cost[goal.first][goal.second] numeric_limitsint::max()) { return path; // 无路径可走 } pairint, int cur goal; while (!(cur.first start.first cur.second start.second)) { path.push_back(cur); cur parent[cur.first][cur.second]; } path.push_back(start); reverse(path.begin(), path.end()); return path; } int main() { // 示例地图0表示可通行1表示障碍物 vectorvectorint grid { {0, 0, 0, 0, 1, 0, 0, 0}, {0, 1, 1, 0, 1, 0, 1, 0}, {0, 0, 0, 0, 0, 0, 1, 0}, {1, 1, 0, 1, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0}, {0, 1, 0, 1, 0, 0, 0, 1}, {0, 0, 0, 1, 0, 1, 0, 0}, }; auto path dijkstra(grid, {0, 0}, {6, 7}); if (path.empty()) { cout 没有找到路径 endl; } else { cout 路径长度: path.size() - 1 endl; for (auto p : path) { cout ( p.first , p.second ) ; } cout endl; } return 0; }这段代码核心逻辑就三步从堆里弹最小代价节点、扩展邻居、更新代价并记录父节点。第17行的operator重载是唯一需要记住的“魔法”——priority_queue默认按最大元素排前面重载成才能让它变成小顶堆。如果你不想重载运算符也可以使用priority_queueNode, vectorNode, functionbool(Node, Node)加一个lambda表达式指定比较规则。3.3 八邻域扩展与对角线代价上面的代码用的是四邻域也就是机器人只能上下左右走。如果允许机器人斜着走路径会更短、更自然但代价计算要改一下// 八邻域方向 int dx[8] {-1, -1, -1, 0, 0, 1, 1, 1}; int dy[8] {-1, 0, 1, -1, 1, -1, 0, 1}; // 代价计算对角线是根号2直线是1 double step_cost (dx[i] ! 0 dy[i] ! 0) ? 1.414 : 1.0;这时候因为出现了浮点数代价矩阵和Node里的cost都要从int改成double。还有一个细节开了八邻域后机器人会“切墙角”。它不会真的撞上去因为膨胀层的格子已经被标为障碍物了但如果膨胀半径不够大斜穿障碍物角点的路径在实机上可能会擦到障碍物边缘。所以我的建议是凡是开了八邻域的规划膨胀半径最少要再加一个栅格。或者干脆别开八邻域在四邻域路径上用B样条曲线做平滑效果会更好。4. 从仿真到实车的完整实操过程4.1 仿真环境里的部署方法如果你是ROS用户最省事的是用move_base框架把全局规划器换成自己写的Dijkstra插件。具体操作用不到自己重新发明一轮只要实现nav_core::BaseGlobalPlanner接口然后在yaml文件里配置一下base_global_planner: my_dijkstra_planner/MyDijkstraPlanner但如果你想彻底搞懂整个流程我建议还是先从纯仿真开始不用ROS用Python的matplotlib可视化。步骤很简单用openCV或者PIL手绘一张二值地图黑色是障碍物白色是空地把它读成numpy数组0和1的二维矩阵对障碍物做膨胀处理把障碍物周围n个格子的值也置为1把地图、起点、终点作为Dijkstra输入计算路径用matplotlib把地图和路径画出来绿色线是搜索结果这个流程做一遍你就能直观感受到Dijkstra的“波纹扩散”过程。在起点周围路径代价等值线像水波一样从起点向外一圈一圈扩散直到触及终点。这种可视化对理解算法本质帮助巨大比盯着代码看一小时都管用。4.2 实车部署的坑与对策仿真跑通了不代表实车能跑下面这些坑我基本都踩过算力不足导致路径规划卡顿。树莓派这类低算力平台跑Dijkstra100x100地图勉强能实时地图再大就得卡。对策是把大图切块处理或者只在起点附近的小范围内重规划远端路径沿用上次的结果。地图坐标系与机器人坐标系没有对齐。路径规划算出的是一串栅格坐标实车控制程序必须把它转换到以机器人底盘为原点的局部坐标系。这个转换通常是map - odom - base_link的TF链ROS里一条lookupTransform就能拿到但很多自研代码没有引入TF导致路径点在小车坐标系里位置偏了几十个格子小车冲出去直接撞墙。全局路径与局部避障的衔接不顺畅。我之前实现过一个方案Dijkstra规划出全局路径后每隔0.2米取一个路标点小车用纯追踪跟踪路标点。如果局部感知发现前方有动态障碍物小车先使用VFH算法找一个不碰撞的方向绕过去绕行结束后再回到离当前位置最近的全局路标点继续追踪。实测下来只要膨胀层设置合理这套方案在一米每秒的速度下表现还是很稳的。4.3 搜索结果可视化与调试调试Dijkstra特别依赖可视化。你光看路径结果很难判断“为什么这里绕了一大圈”但如果你把代价矩阵画出来一眼就能看出问题——可能是膨胀层把窄道封死了或者地图本身的连通性判断有误。我常用的调试流程是打印起点、终点在栅格地图中的坐标确认没有落在障碍物里画出整张代价矩阵的热力图检查从起点到终点是否存在一条“代价逐渐增大”的连续梯度带画出搜索过程中已经访问过的节点确认优先队列扩展的确实是“从外到内”的等值线最后再画最终的路径如果第2步就发现代价梯度在某处断裂说明地图的连通性有问题需要检查障碍物标记是否把本应联通的区域切断了。5. 常见问题与排查技巧5.1 问题速查表现象可能原因排查与对策程序跑完但路径为空起点或终点在障碍物里打印检查起点终点的栅格值做越界与碰撞检查路径明显绕远优先队列堆序错误检查operator或lambda比较器的返回值方向路径穿墙膨胀半径不够增大膨胀半径至少覆盖机器人内切圆半径规划速度慢地图分辨率太高或开了八邻域降低分辨率或者改用A*或者只在局部重规划实车转弯撞墙膨胀层没生效确认规划的costmap和感知用的costmap是否同一张路径抖动地图中未知区域被当作可通行区域把未知区域(-1)也当作障碍物对待5.2 性能优化的三个实用手段如果Dijkstra跑大图真的慢有三个手段按顺序来双向Dijkstra。从起点和终点同时开始搜索两边各扩展一半相交时路径就找到了。在栅格地图上实测能减少约40%的搜索节点数量。实现上稍微复杂一点但原理还是一样的。多分辨率地图。先在一个低分辨率版本的地图上用Dijkstra粗规划一条走廊再回到高分辨率地图里只在走廊范围内精规划。这其实就是“分层规划”思想的简单版在超大场景比如整个园区非常有效。随时序增量搜索。地图变化不大的时候上一轮的路径大部分仍然有效。把Dijkstra替换成D* Lite它可以利用上次的搜索结果增量更新动态环境下性能能提升一个数量级。5.3 C实现里最容易被忽视的两个细节第一个是无穷大的选择。如果你用INT_MAX作为初始代价做加法和比较时要特别小心——cur.cost 1可能会溢出。我的习惯是初始代价设成一个比地图中任意实际路径代价都大的数比如rows * cols * 2既不会溢出也不会影响比较。第二个是提前终止条件。如果只关心起点到终点的最短路径那当终点被标记为visited时就可以跳出循环了不必把整张图都搜完。这个优化在节点多的地图上能省掉一大半时间。但要记住这个优化有一个前提你是用priority_queue按代价升序处理的那“终点首次被访问时就是最优解”这个结论才成立。如果是用普通队列做的BFS式处理就不能随便提前退出。最后再分享一个实际体会我在真实项目中回过头来审视Dijkstra最大的感悟是这个算法最困难的部分不是搜索过程而是它前面的地图处理与膨胀参数调优。很多同学把大量精力花在理解堆、松弛、复杂度分析上结果代码跑出来的路径因为地图建得不对一样一塌糊涂。我做项目时的习惯是“三分算法七分地图”把地图处理到“可信任”的状态路径规划器随便写都能走得通。如果你现在正被A*和RRT的各种变体搞得焦头烂额不妨回头试试这个最基础的Dijkstra。先把一条路径通过栅格地图稳稳当当地规划出来再去考虑效率和实时性。这个过程中建立的地图表征能力、代价计算直觉、分层规划思维会让你后面学任何先进的规划算法都事半功倍。本文还有配套的精品资源点击获取
