C++栅格地图全局路径规划：A*、Dijkstra与JPS算法实战解析

聊到全局路径规划，很多人的第一反应是“不就是寻路嘛”。但真要在栅格地图上，用C++写出能从A点扭到B点、还保证不撞墙的代码，里头的坑一点都不少。这篇我就直接上代码，把栅格地图里的几种经典“蛇皮走位”姿势拆开揉碎讲清楚。适合刚接触移动机器人导航、或者想自己动手实现路径搜索算法的人参考，看完你至少能自己写一个能跑的A*，并且知道怎么改成Dijkstra、怎么理解JPS的剪枝思路。

1. 全局路径规划到底在规划啥

1.1 先说清楚问题本身

全局路径规划，本质上是给机器人找一条从起点到目标点的、可通行的最优路径。所谓“最优”，在不同场景下定义不一样：可能是距离最短，可能是时间最少，也可能是能量消耗最低。但在栅格地图里，最常见的优化目标就是“路径长度最短”，顺带要求路径上的每个栅格都是无碰撞的。

这听起来像是图论里的最短路径问题。确实如此。栅格地图把连续空间离散化成一个个格子，每个格子要么是空闲（0），要么是障碍（1），于是机器人寻路就变成了在一个网格图上从起点节点走到终点节点。这里的关键点有三个：地图怎么存、节点怎么展开、代价怎么算。这三个点搞明白了，后面写代码就是水到渠成的事。

那为什么非要选C++来搞？原因很简单：性能。全局路径规划往往要处理几百乘几百的栅格地图，如果每个节点都搞成new出来的对象，光内存分配就能拖慢好几倍。C++给的底层控制力很强，可以用数组、vector、priority_queue这些容器精确管理内存，把单次规划控制在毫秒级甚至微秒级。移动机器人上跑算法，实时性就是命，C++在这方面的优势其他语言很难替代。

1.2 选算法的几个候选方案

路径搜索算法里，最常用的几类是：

• Dijkstra：经典的最短路径算法，扩展节点时没有任何“方向感”，四面八方均匀扩散，能找到最短路径但搜索范围大。
• A*：在Dijkstra的基础上加了启发函数，引导搜索方向朝着目标走，效率高很多，是目前栅格地图路径规划的事实标准。
• JPS（跳点搜索）：在A*的基础上做“跳点剪枝”，大量压缩被展开的节点数，在障碍物稀疏的栅格地图里速度堪称恐怖。
• RRT（快速扩展随机树）：基于采样的方法，适合高维空间或连续空间，但路径不是最优且可能很绕，通常需要后处理平滑。

从实现难度和效果比来看，A*是性价比最高的切入点。先把它吃透，再往上改Dijkstra就是注释一个启发函数的事，往下升级JPS也有基础。所以这篇文章的主线就用A*，同时我也会给出Dijkstra的变体代码和JPS的核心思路。

1.3 整体代码框架预览

我习惯把路径规划拆成三个独立的模块：地图表示、搜索器、可视化调试。这样地图换一张图、算法换一种，代码结构都不用大动。

地图表示用二维vector，0表示可通行，1表示障碍，255表示未知区域（如果做动态地图可以考虑）。搜索器是一个类，输入地图、起点、终点，输出一条路径。可视化调试单独拎出来，往终端或文件里打印路径结果，这一步千万别省，因为它能让你直观看到搜索过程哪里不对劲。

下文所有代码我都基于C++11/14编写，不依赖额外第三方库，标准库就够用。

2. 核心数据结构与栅格地图准备

2.1 地图表示与邻居数组

先说地图。一个300乘300的栅格地图，用vector<vector<int>>存储很直接。为了访问方便，我一般把地图设计成行优先的一维数组，这样缓存友好，遍历速度快得多：

#include <vector> class GridMap { public: int width, height; std::vector<int> data; // 行优先存储，1为障碍，0为空闲 GridMap(int w, int h) : width(w), height(h), data(w * h, 0) {} int& at(int x, int y) { return data[y * width + x]; } const int& at(int x, int y) const { return data[y * width + x]; } bool inBounds(int x, int y) const { return x >= 0 && x < width && y >= 0 && y < height; } bool isTraversable(int x, int y) const { return inBounds(x, y) && at(x, y) == 0; } };

一个很容易忽略的点：地图的坐标系和机器人坐标系的映射。栅格坐标通常是从左上角开始的，x向右、y向下；而机器人坐标大多是x向前、y向左。工程里我吃过这个亏，路径规划明明没问题，但机器人走起来却是斜着冲。建议所有算法内部统一用栅格坐标系，只在和上游地图模块交互的地方做转换，算法内部绝不混用。

然后是邻居数组。4方向还是8方向？这个选择直接影响路径的形态和长度。4方向（上下左右）的路径适合差速底盘，转弯角度固定90度；8方向（加上对角线）路径更短、更平滑，但二次规划时需要对对角线穿角做碰撞检查。原因是对角线移动时，如果相邻的两个正方向格子都是障碍，机器人从中间挤过去可能卡住。

// 8方向邻居偏移，顺序不影响正确性，影响搜索形状 const int dx[8] = {1, -1, 0, 0, 1, 1, -1, -1}; const int dy[8] = {0, 0, 1, -1, 1, -1, 1, -1};

注意，这里的cost代价分两类：正交移动代价是1.0，对角线移动代价是1.414。很多人图省事都写成1，结果就是路径会对角线“抄近路”，和直线距离明显不符，而且容易穿障碍。建议直接定义成浮点数：

const double MOVE_COST = 1.0; const double DIAG_COST = 1.41421356237;

2.2 节点定义与代价设计

A*里每个节点需要记录三个核心信息：g（从起点到当前节点的实际代价）、h（从当前节点到目标点的启发代价）、parent（父节点指针）。这里有一个实现上的取舍：是用struct Node在堆上new，还是在数组里用索引引用？

我先说结论：优先用数组索引，不要用new。路径搜索一跑就是几万次节点操作，每次new一个Node再delete，性能折损非常大。标准做法是直接用栅格坐标在预分配数组里查状态，g值和父节点分别存在数组里：

struct AStarState { std::vector<double> gScore; // 记录每个栅格的g值，初始为inf std::vector<int> cameFrom; // 记录每个栅格是从哪个栅格来的，存索引 std::vector<bool> closed; // 是否已经完成扩展 std::vector<bool> opened; // 是否已经加入open表 };

代价函数h是A*的灵魂。为什么这么说？因为h的取舍决定了A*是退化成Dijkstra还是变成贪婪搜索：h全为0时，A*退化成Dijkstra，搜索范围是全向扩散，慢但保证最优；h比实际代价大太多时，搜索范围会大幅缩小，快但可能找不到最优路径甚至找不到路。

栅格地图上常用的h有两种：

• 曼哈顿距离：abs(x1 - x2) + abs(y1 - y2)，适合4方向搜索，因为4方向下最短路径长度就是曼哈顿距离，h严格不大于实际代价，保证最优性。
• 欧氏距离：sqrt(dx*dx + dy*dy)，适合8方向搜索。8方向下实际最短路径接近欧氏距离，但略大于它（因为对角线代价是1.414而不是1.0的平方根），h依然不会高估实际代价，所以能保证最优。

我实测下来，8方向搜索用欧氏距离作为h，搜索效率最高。也有人用切比雪夫距离max(abs(dx), abs(dy))，在8方向地图上是严格精确的启发函数，收敛最快。但切比雪夫距离需要配合对角代价为1的设定才精确，如果你用的是1.414，它就会略微低估，问题不大，效率略受影响。所以这里我还是推荐欧氏距离，代码直观，不易出错。

2.3 为什么用优先队列而不是普通队列

A*的open表如果用普通队列，每次取出的节点就是先进先出，完全没有按代价排序的能力，那算法就废了。标准做法是使用优先队列，即小根堆，每次取出总代价f = g + h最小的节点进行扩展。C++标准库的std::priority_queue很好用，但它默认是大根堆，需要自定义比较器：

#include <queue> #include <tuple> using PQNode = std::pair<double, int>; // first是f值，second是栅格索引 std::priority_queue<PQNode, std::vector<PQNode>, std::greater<PQNode>> openList;

其实这里有个慎用的细节：std::priority_queue以pair作为元素时，先比较first再比较second，刚好满足我们的需求。其中int存的是栅格索引（y * width + x），这样省去存两个int，内存布局更紧凑。

还有一个性能优化技巧：优先队列里允许同一个节点被插入多次。因为在搜索过程中，一个节点可能先被一个次优路径加入open表，后来又被更优的路径再次发现。如果你严格做到“每个节点只入队一次”，需要额外的状态管理，反而增加复杂度。我一般允许重复插入，在弹出时判断它的g值是否比记录值大，如果大就直接跳过，这个“延迟删除”的技巧在工程里非常常见。

while (!openList.empty()) { double curF = openList.top().first; int curIdx = openList.top().second; openList.pop(); if (closed[curIdx]) continue; // 已经处理过 if (curF > gScore[curIdx] + heuristic(curIdx, goalIdx) + 1e-9) continue; // 延迟删除 closed[curIdx] = true; // ... 后续扩展 }

双保险判断，实测下来既省内存又不会出错。

3. 核心算法实现与代码逐段解析

3.1 A*搜索主流程

现在进入重头戏。我把A*包装成一个类，接口简单，输入起点和终点，输出路径点集合。类定义大概长这样：

class AStarPlanner { public: AStarPlanner(const GridMap& map) : map_(map) {} std::vector<std::pair<int, int>> plan(int startX, int startY, int goalX, int goalY); private: double heuristic(int idx, int goalIdx) const; void reconstructPath(int currentIdx, std::vector<std::pair<int, int>>& path, const std::vector<int>& cameFrom); const GridMap& map_; };

plan函数是主体：

std::vector<std::pair<int, int>> AStarPlanner::plan(int startX, int startY, int goalX, int goalY) { int width = map_.width, height = map_.height; int startIdx = startY * width + startX; int goalIdx = goalY * width + goalX; std::vector<double> gScore(width * height, std::numeric_limits<double>::infinity()); std::vector<int> cameFrom(width * height, -1); std::vector<bool> closed(width * height, false); std::priority_queue<PQNode, std::vector<PQNode>, std::greater<PQNode>> openList; gScore[startIdx] = 0.0; openList.push({heuristic(startIdx, goalIdx), startIdx}); while (!openList.empty()) { double curF = openList.top().first; int curIdx = openList.top().second; openList.pop(); if (closed[curIdx]) continue; if (curF > gScore[curIdx] + heuristic(curIdx, goalIdx) + 1e-9) continue; if (curIdx == goalIdx) { std::vector<std::pair<int, int>> path; reconstructPath(curIdx, path, cameFrom); return path; } closed[curIdx] = true; int curX = curIdx % width; int curY = curIdx / width; for (int dir = 0; dir < 8; ++dir) { int nx = curX + dx[dir]; int ny = curY + dy[dir]; if (!map_.inBounds(nx, ny) || !map_.isTraversable(nx, ny)) { continue; } // 对角线穿角检查 if (dx[dir] != 0 && dy[dir] != 0) { if (!map_.isTraversable(curX + dx[dir], curY) || !map_.isTraversable(curX, curY + dy[dir])) { continue; } } int nIdx = ny * width + nx; double stepCost = (dx[dir] * dy[dir] != 0) ? DIAG_COST : MOVE_COST; double tentativeG = gScore[curIdx] + stepCost; if (tentativeG >= gScore[nIdx] - 1e-9) continue; cameFrom[nIdx] = curIdx; gScore[nIdx] = tentativeG; openList.push({tentativeG + heuristic(nIdx, goalIdx), nIdx}); } } return {}; // 找不到路径 }

主循环的逻辑并不复杂：弹出f值最小的节点，判断是否有效，是终点就回溯，否则展开邻居。有几个细节值得认真看：

• std::numeric_limits<double>::infinity()初始化g值，后续比较时用无穷大判断是否第一次访问。
• 浮点比较全部加了1e-9的容忍度。这是因为代价累加可能导致微小误差，不加容差会出现“明明g值更新了但被干掉”的问题，表现为路径绕远路或者干脆搜不出来。
• 对角线穿角检查那段，是防止机器人“擦着墙角”斜穿过去。地图上看起来只有两个格子是障碍，但机器人底盘是有尺寸的，直接斜穿过去就容易卡在交界处。这个检查必须加，我在实际测试中就因为这个没加，机器人好几次卡在障碍物的角落门口。

3.2 路径回溯函数

reconstructPath从终点一路找到起点：

void AStarPlanner::reconstructPath(int currentIdx, std::vector<std::pair<int, int>>& path, const std::vector<int>& cameFrom) { while (currentIdx != -1) { int x = currentIdx % map_.width; int y = currentIdx / map_.width; path.emplace_back(x, y); currentIdx = cameFrom[currentIdx]; } std::reverse(path.begin(), path.end()); // 从起点到终点 }

注意，这里的cameFrom初始化为-1，起点没有父节点，当回溯到起点时索引就是-1，循环终止。std::reverse是为了把路径翻成从起点到终点的顺序，方便后续送给运动控制模块。

到这一步，你已经有一个能跑的A*了。但先别急着庆祝，我建议把搜索过程可视化出来再调试，否则你根本看不出来算法到底在“想”什么。

3.3 一行代码改成Dijkstra

把A*改成Dijkstra非常简单，只需要把启发函数设为0。核心区别在于：A*利用目标点信息引导搜索方向，Dijkstra则纯按g值扩散。代码上只需要把openList初始化时和push时的启发值全部替换成0即可：

// Dijkstra openList.push({0.0, startIdx}); // ... openList.push({tentativeG, nIdx});

没有启发函数，搜索范围就会像涟漪一样一波一波往外扩散，最终会找到最短路径，但扩展的节点数量通常比A*多好几倍。Dijkstra的优点在于它是“无偏”的，在带权重的复杂地图上能保证全局最优；缺点就是慢。所以工程上通常只在A*启发函数不可用时兜底，或者作为离线预计算的替代方案。

3.4 附加优化：拐点提取与路径平滑

A*返回的路径是栅格序列，看起来是锯齿状的一串点。直接拿这串点给机器人走，机器人会在栅格中心点之间走折线，原地拐弯多次，体验很差。实际项目中我会做两件事：拐点提取和线性平滑。

拐点提取的思路是：遍历路径点，如果当前点到上一个拐点之间的连线不穿过障碍，就删掉中间点。这个操作只需要在规划阶段重复调用“两点之间是否可直线通行”的检查函数：

static bool isLineTraversable(const GridMap& map, int x0, int y0, int x1, int y1) { int steps = std::max(std::abs(x1 - x0), std::abs(y1 - y0)); for (int i = 0; i <= steps; ++i) { double t = (double)i / steps; int x = (int)std::round(x0 + t * (x1 - x0)); int y = (int)std::round(y0 + t * (y1 - y0)); if (!map.isTraversable(x, y)) return false; } return true; }

这个采样步长建议设1个栅格，也就是t间隔不超过1/距离，防止把细障碍跳过。实测这方法在大多数地图上能把路径点从几百个压缩到几个，机器人走起来利落得多。

至于平滑，可以用三次B样条、贝塞尔曲线，或者更简单的“顺滑算法”：对路径点做若干次均值滤波，每次把每个点往相邻两点连线的中点拉一点。但这个操作要小心，拉过头会导致路径撞墙，所以平滑前要缩小地图障碍尺寸（做膨胀），给平滑留出安全余量。

4. 影响路径效果的关键参数与调试技巧

4.1 启发函数权重与权重陷阱

A*有一个经常被调的参数：启发函数权重w，把f表达式变成f = g + w * h。这个权重的作用是权衡最优性和速度：

• w = 1：严格A*，保证最短路径，但搜索范围大，慢。
• w = 1.5：路径长度比最优多几个百分点，但搜索节点数可能少一半以上，典型工程选择。
• w = 2及以上：搜索更快，但路径往往不直，容易绕路，可能错过明显更短的走廊。

我曾经在一个模拟仓储项目里试过把权重调到2.5，速度确实快了很多，但路径从“穿过货架区”变成“沿货架外围绕大圈”，运输时间反而增加，得不偿失。最后的结论是：如果追求的是机器人实际走完路径的总时间，最优路径 + 平滑后的效果往往比快速找出来的绕路路径好得多。所以除非路径搜索本身成为瓶颈，否则别瞎调高权重。

如果非要调高权重，我推荐配合“权重递减”技巧：先用大权重快速找到一个可行路径，再从0到1逐步降低权重做多轮搜索，每一轮以上一轮的路径作为上界裁剪搜索空间。这在大地图上能兼顾速度和最优性，代价是代码复杂度上升，新手不建议一上来就这么搞。

4.2 栅格地图膨胀半径的选择

全局路径规划的前提是地图足够安全。但标准的栅格地图上，障碍物只是一个格子，没有任何尺寸概念。机器人的底盘半径通常比半个格子大得多，如果路径直接贴着障碍格子走，机器人实体到了那里必撞。解决办法是预处理阶段对障碍物做膨胀（inflate）。

膨胀操作很简单：以每个障碍格子为中心，把所有距离它小于膨胀半径的格子也标记为障碍。半径的上限就是机器人底盘最大外接圆半径（单位栅格）。实际工程里还会加上安全余量，比如半径0.8格子的机器人，膨胀1.2格比较常见。

void inflateObstacles(GridMap& map, int radius) { int w = map.width, h = map.height; std::vector<int> temp(map.data); for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { if (temp[y * w + x] != 0) continue; for (int dy = -radius; dy <= radius; ++dy) { for (int dx = -radius; dx <= radius; ++dx) { if (dx*dx + dy*dy > radius*radius) continue; int nx = x + dx, ny = y + dy; if (map.inBounds(nx, ny)) { map.at(nx, ny) = 1; } } } } } }

这里用圆而不是正方形做膨胀，更接近机器人底盘的几何外形。另外需要注意，膨胀后地图上的“可行走区域”变窄了，路径规划会主动绕开墙边，路径会更安全，但也可能因此出现无法通过的门洞——这是正常现象，说明门洞宽度确实不够安全通过。

4.3 如何判断路径是否安全可执行

判断路径好坏，不能光看“没有撞障碍格子”。我在项目里，会在路径输出前做一个额外的安全校验：把路径点的每个坐标向外偏移机器人半径，检查附近区域是否全在可通行范围。这相当于沿着路径扫一遍“胶囊体碰撞检测”。

一个务实的做法是直接对路径做重采样，每隔0.2个栅格取一个路径点，检查该点周围的膨胀地图是否安全。这比“只检查拐点”更稳，因为A*路径即使没有撞格子，也可能擦着障碍边缘走，稍微有点定位误差就撞上了。

bool isPathSafe(const GridMap& inflatedMap, const std::vector<std::pair<int, int>>& path, double robotRadius) { for (size_t i = 0; i + 1 < path.size(); ++i) { double x0 = path[i].first, y0 = path[i].second; double x1 = path[i+1].first, y1 = path[i+1].second; int steps = (int)std::ceil(std::hypot(x1 - x0, y1 - y0) / 0.2); for (int s = 0; s <= steps; ++s) { double t = steps > 0 ? (double)s / steps : 0.0; int cx = (int)std::round(x0 + t * (x1 - x0)); int cy = (int)std::round(y0 + t * (y1 - y0)); for (int dy = -(int)robotRadius; dy <= (int)robotRadius; ++dy) { for (int dx = -(int)robotRadius; dx <= (int)robotRadius; ++dx) { if (dx*dx + dy*dy > robotRadius*robotRadius) continue; if (!inflatedMap.isTraversable(cx + dx, cy + dy)) return false; } } } } return true; }

如果校验失败，别硬走，要么重新规划，要么调大膨胀半径，要么做局部绕障。总之，路径规划的结果不只是“一串坐标”，而是要能真正被执行出来。

5. 常见问题速查与避坑心得

5.1 路径规划失败的典型原因

我整理了一张排查表，这几种问题是出现频率最高的，基本覆盖了九成以上的“为什么我搜不到路”：

其中“起点或终点在障碍区域”是最容易踩的坑。我遇到过不止一次：地图明明就一张，视觉SLAM给的目标点在大门口，结果因为门框被膨胀半径覆盖了，终点直接变成障碍，规划器返回空路径。后续处理很简单，规划前先从终点周围一圈找最近的可行走格子作为替代终点，而不是直接把终点塞进算法。

5.2 性能瓶颈在哪里

栅格地图路径规划的性能瓶颈基本都集中在两个地方：open表的排序和邻居节点对数组的随机访问。两者相比，open表的排序往往更耗时间，尤其是在大地图上，每次push/pop都是O(logN)的堆操作，再加上节点数动辄几万，总耗时就到了几十毫秒级别。

优化手段有这么几个，按推荐程度排序：

• 切换到JPS。如果地图障碍稀疏，JPS的节点数可能只有A*的十分之一，堆操作次数大幅减少，效果立竿见影。
• 用更大栅格尺寸。比如把原图压缩2倍再规划，路径在大尺度上不会差太多，但节点数降到四分之一。
• 分层规划。先在大地图上用低分辨率快速找一条粗路径，再沿着粗路径在高分辨率地图上分段精细规划。这个方法工程上很成熟，常见于大型仓储场景。
• 预计算静态地图的最短路径簇。如果机器人经常从固定起点出发，可以离线用Dijkstra生成一张“距离场”表，在线查表只要做贪心下降就可以，速度是毫秒级的。这个技巧在某个竞赛小车项目里帮我把规划时间从40ms压到了0.2ms。

5.3 我踩过最深的两个坑

第一个坑是无符号整数溢出。有一版代码我把地图索引全定义成unsigned int，然后cameFrom用-1初始化。结果回溯时，起点索引的前一个节点索引是-1，被隐式转换成4294967295，直接越界访问数组，程序崩溃到完全找不着原因。排查了两个小时才意识到是这个低级错误。现在我的习惯是：索引全部用有符号int，越界判断只用inBounds函数，绝不在索引上玩负数技巧。

第二个坑是动态障碍物下的路径失效。项目刚开始做的时候，我天真地以为规划完一次路径就够了，结果机器人一开动，地图里多了一个叉车，路径直接撞上去。后来才意识到，全局路径规划解决的是“静态已知地图里的寻路”，一旦环境变化，路径就失效了。工程上的解法有两种：一是周期性重新规划，代价是可能把好路径改差；二是叠加一个局部避障层，用视觉或激光雷达的实时信息修正全局路径的偏差。大多数移动机器人都走“全局规划 + 局部避障”的架构，全局路径给方向，局部避障保安全。

如果对实时性有进一步要求，可以考虑D* Lite或D* Lite的变体，它能在环境变化时增量式更新路径，不需要从零开始重新规划。但D* Lite的代码复杂度比A*高一个级别，我也是先彻底理解了A*之后才啃下来的，新手不必一步到位。

5.4 调试路径规划的几个实用建议

环境搭好以后，强烈建议做三件事：

1. 把地图打印成ASCII图形。障碍用#，空闲用.，起点用S，终点用G，路径用*。这样一眼就能看出路径有没有穿墙、绕路。

void printDebugMap(const GridMap& map, const std::vector<std::pair<int, int>>& path) { std::vector<std::string> canvas(map.height, std::string(map.width, '.')); for (int y = 0; y < map.height; ++y) for (int x = 0; x < map.width; ++x) if (map.at(x, y)) canvas[y][x] = '#'; for (auto& p : path) canvas[p.second][p.first] = '*'; for (auto& row : canvas) std::cout << row << "\n"; }

2. 把Open表大小和扩展节点数打出来。这两个数字是衡量算法效率最直观的指标。A*在同一张地图上，节点数可能是Dijkstra的三分之一，这个对比会给你很强的直觉。

3. 做一个简单的计时器。每跑一次规划就打印耗时，观察不同地图尺寸、不同障碍密度下的表现。某一次我发现单次规划从2ms飙到80ms，排查后才知道是地图里有一整块未知区域被当成障碍，搜索被“推开”绕了一个巨大的U形弯，改掉那块数据的值后性能马上恢复。

6. 升级方向：从A*到JPS的剪枝思路

6.1 为什么JPS比A*快

A*在栅格地图上的主要开销在于它逐个扫描邻居，哪怕这个方向明显是死路也会扩展过去。JPS的核心洞察在于：对于处在空旷地带的格子，在某个方向上的邻居要么不必扩展，要么可以直接沿着该方向“跳”到更远的格子再扩展。这相当于把优质路径上的连续栅格折叠成一个“跳点”，搜索空间的规模被大幅压缩。

举个例子：在一个完全空旷的60乘60地图上，A*可能要扩展几千个节点；JPS可能只扩展几百个。当然，JPS对地图形状有要求：它只适用于均匀代价的栅格地图，带权重的地图会破坏它剪枝的正确性。如果你的地图不是均匀代价的栅格，老老实实用A*。

6.2 JPS的最基本实现要点

JPS的代码核心是把A*的“逐个邻居扩展”改成一个“优先方向跳转搜索”的函数。主要分两步：

1. 强制邻居检测。在水平方向移动时，如果对角方向上有障碍物且前方通畅，则对角格子必须被加入open表。这保证了路径不会错过“贴着障碍斜着走”的情况。

2. 直线跳跃。沿着某个方向一直往前走，遇到跳点或障碍就停。跳点的定义是“当前格子有强制邻居，或者当前格子是终点”。

伪代码大致如下：

bool jump(int x, int y, int dx, int dy, int goalX, int goalY) { int nx = x + dx, ny = y + dy; if (!map_.inBounds(nx, ny) || !map_.isTraversable(nx, ny)) return false; if (nx == goalX && ny == goalY) return true; // 水平或垂直移动 if (dx != 0 && dy == 0) { if (hasForcedNeighbor(x, y, dx)) return true; } else if (dx == 0 && dy != 0) { if (hasForcedNeighbor(x, y, dy)) return true; } else { // 对角移动：先检查水平和垂直方向是否也有跳点 if (jump(nx, ny, dx, 0, goalX, goalY)) return true; if (jump(nx, ny, 0, dy, goalX, goalY)) return true; } return jump(nx, ny, dx, dy, goalX, goalY); }

hasForcedNeighbor的判断逻辑是：在当前格子沿dx方向向前看，如果(x + dx, y + 1)是障碍但(x, y + 1)可通行，那么(x + dx, y + 1)就是强制邻居。同理处理y - 1方向。

把A*替换成JPS后，最直观的变化就是open表操作次数剧减，尤其是地图空旷时，搜索速度快到让人怀疑是不是算错了。我实测了一个600乘600的空旷地图，A*要遍历上万个格子，JPS只碰了几百个跳点，耗时从140ms降到5ms，这种对比是很有冲击力的。

6.3 什么时候不要用JPS

JPS很好，但不是万能的。几种场景下我宁可继续用A*：

• 地图格子的代价不一致，比如有的区域限速、有的是泥地，这需要带权重的路径规划，JPS的剪枝前提就不存在了。
• 地图障碍物太密集，跳点分布密集之后，剪枝效果大幅削弱，最后性能和A*差不多，代码却复杂得多。
• 你还在学习阶段，没完全理解A*之前别碰JPS，否则出了问题完全无法定位。

总的来说，JPS是一个“锦上添花”的优化，是在理解了A*原理基础上的进阶玩法。先把A*写稳、调好、可视化搞起来，再考虑剪枝，这条路最顺。

7. 工程接入与代码组织经验

7.1 把规划器设计成可替换接口

代码写得再漂亮，如果只在一个函数里待着也白搭。实际项目里我会把规划器抽象出一个通用接口，这样可以随时切换实现：

class IPlanner { public: virtual ~IPlanner() = default; virtual std::vector<std::pair<int, int>> plan(const GridMap& map, int startX, int startY, int goalX, int goalY) = 0; }; class AStarPlanner : public IPlanner { public: std::vector<std::pair<int, int>> plan(const GridMap& map, int startX, int startY, int goalX, int goalY) override; }; class JPSPlanner : public IPlanner { // 同样实现 };

这样上层只需要知道IPlanner，配置文件里写planner_type: "AStar"或"JPS"就能切换算法。我在某个模拟项目中就靠这个设计，白天用A*调业务，晚上用JPS跑压测，互相不影响。

7.2 路径规划线程与主控制线程的隔离

全局路径规划不该和运动控制跑在同一个线程里，否则规划一卡，机器人的实时控制也跟着卡。推荐的做法是规划器跑在独立线程，结果通过消息队列传给主控制线程。规划结果带上时间戳，主控制收到后如果路径已经过期（比如超过500ms），可以选择忽略或者重新规划，这个策略在动态场景里能明显提升稳定性。

我用过最简单可靠的方式就是std::mutex + std::condition_variable，或者用一个无锁的单消费者队列。在这个架构下路径规划的耗时波动不再直接影响运动控制，整体可靠性高很多。

7.3 日志与回放，孤身调试不迷路

最后一个建议：把每次规划的地图快照、起点终点、路径结果和耗时全部落盘，做成日志。调试的时候回放这些日志，比起现场盯着控制台输出强太多。哪怕只是保存一个地图文件名、一行起点终点和一行路径点集合，都能让排查效率翻倍。尤其是当问题只在特定地图、特定起点出现时，可复现的日志简直是救命稻草。

我自己习惯用JSON格式存日志，一次规划一条记录，解析方便，还能直接写脚本批量跑回归测试。这个习惯起初只是顺手为之，后来成了排查疑难杂症的利器。