1. 为什么值得重新审视运动规划里的“老问题”做机器人或者自动驾驶方向的朋友大概都有过这样的体验感知模块跑得挺顺定位也勉强能用一到规划环节就开始掉链子——要么路径抖得像心电图要么求解时间忽长忽短要么在狭窄通道里直接卡死。环境感知与规划这个专题写到第三篇前两篇聊了感知侧的标定与栅格化处理这一篇终于要正面硬刚规划本身了。标题里提到的 JLT是我自己在项目里对一类“跳点式轻量轨迹搜索”算法的简称全称不重要重要的是它解决的核心痛点在栅格或采样空间里用尽可能少的节点扩展快速找到一条可行且相对平滑的初始路径。先说清楚它是什么。JLT 本质上是一种基于跳点扩展的启发式路径搜索变体你可以把它理解成 A* 的“精简版加速器”——传统 A* 在栅格地图上会老老实实把每一个相邻格子都塞进开放列表而 JLT 的做法是沿着当前方向一路“跳”过去只在遇到障碍物拐角、方向被迫改变或者到达目标附近时才生成新的候选节点。这样一来开放列表的规模能压下来一大截搜索效率自然就上去了。它能做什么最直接的用途是给后续的轨迹优化提供一个高质量的初始解。很多团队的做法是先用搜索算法找一条粗糙的几何路径再交给优化器去磨平滑JLT 就是干前半段活的。它适合谁参考如果你正在做移动机器人导航、AGV 调度、无人机低空路径规划或者只是单纯想搞明白“为什么我的 A* 在大地图上慢得像蜗牛”这篇内容都能给你一些可以直接抄作业的东西。我先把结论摆在这儿JLT 不是万能的它在结构化栅格地图、障碍物分布相对稀疏、对实时性要求高的场景下表现最好一旦地图变成稠密杂乱的点云投影或者需要处理动态障碍它的优势会被大幅稀释。下面我会把设计思路、核心细节、实操过程和踩坑记录一层层拆开讲。2. 整体设计与思路拆解2.1 从 A* 的“笨办法”说起要理解 JLT 为什么这么设计得先回到 A* 的老问题上。标准 A* 在栅格地图上的扩展逻辑是从当前节点出发检查八邻域或四邻域的每一个格子把可通行的、没访问过的都加入开放列表然后按 f g h 排序取最小。这个流程本身没问题问题出在扩展的冗余性上。想象一张 500×500 的栅格地图起点在左下角终点在右上角中间大片区域是空的。A* 会怎么做它会把起点周围八个格子全加进去然后每个格子又各自扩展八个像水波一样一圈圈往外推。空旷区域里绝大多数扩展都是“无效劳动”——这些节点最终都不会出现在最优路径上但它们确确实实占用了内存、消耗了排序时间。我实测过在一张 800×800 的室内地图上标准 A* 的开放列表峰值能到六万多个节点光堆排序就吃掉了将近四成的总耗时。JLT 的思路就是针对这个冗余开刀如果一个节点沿着某个方向可以一路畅通地走到更远的地方那中间那些“过路节点”根本没必要单独生成。这就好比你要从小区门口走到超市没必要把路上每一块地砖都记下来只需要记住“出门直走三百米到路口左转”这种关键转折点就够了。2.2 跳点扩展的核心逻辑具体来说JLT 在扩展当前节点时不是简单地把邻居加进列表而是沿着八个方向分别做“跳跃探测”。以水平方向为例从当前节点往右一路扫描直到遇到以下三种情况之一才停下来生成候选节点撞到障碍物说明这个方向走不通直接放弃发现某个格子的邻居状态发生了变化比如上方或下方出现了障碍物拐角说明这里是个“强制邻居”位置必须作为候选点到达地图边界或目标点附近。这个“强制邻居”的概念是 JLT 的灵魂。什么叫强制邻居打个比方你沿着一条走廊直走走廊两侧都是墙走到尽头发现右边开了个口子——这个口子所在的位置就是强制邻居因为如果你不在这里拐弯就永远进不了那个口子。算法必须在这种“方向被迫改变”的地方生成节点否则就会漏掉可行路径。提示强制邻居的判定是 JLT 正确性的关键。如果判定条件写松了会漏节点导致找不到路径写紧了又退化成普通 A*失去加速意义。这个度需要根据地图的栅格分辨率来调。2.3 为什么选这套方案而不是别的市面上做路径搜索加速的路子不少我当初选 JLT 这条线是权衡了几个因素之后的决定。RRT 系列采样算法在连续空间里很优雅但它的随机性导致每次跑出来的路径都不一样调试的时候很折磨人而且窄通道里的收敛速度实在不敢恭维。Dijkstra 就不用说了它连启发式都不用在大地图上比 A* 还慢一个量级。至于各种基于神经网络的端到端规划我承认它们在特定数据集上效果惊艳但可解释性太差出了问题根本不知道从哪儿查起工程落地风险太高。JLT 的好处在于它是确定性的——同样的地图、同样的起终点每次跑出来的路径完全一致这对调试和回归测试太友好了。而且它保留了 A* 的启发式框架h 函数可以复用代码改动量不大团队里熟悉 A* 的人半天就能上手。代价是它依赖栅格地图的结构化程度对地图质量比较敏感这个后面会细说。方案确定性窄通道表现实现复杂度可解释性标准 A*是好低高RRT*否一般中中JLT是好中高端到端网络否依赖训练高低这张表是我自己在几个项目里横向对比后的主观评价不一定普适但能看出 JLT 的定位它是在“确定性”和“效率”之间找平衡的那一档。3. 核心细节解析与实操要点3.1 栅格地图的预处理不能省很多人拿到 JLT 的伪代码就直接往原始地图上套结果跑出来路径贴着障碍物边缘蹭或者干脆穿墙。问题多半出在预处理这一步被跳过了。原始栅格地图尤其是从激光雷达或深度相机投影出来的往往有噪点、有毛刺障碍物边界参差不齐。如果直接在这种地图上做跳点扩展强制邻居的判定会变得极其不稳定。我的做法是先做一轮形态学膨胀把障碍物往外扩一圈。膨胀的半径怎么定取决于机器人的实际轮廓和定位误差。假设机器人是个半径 0.3 米的圆盘定位误差保守估计 0.1 米那膨胀半径至少取 0.4 米再换算成栅格数。比如地图分辨率是 0.05 米/格那膨胀就是 8 个格子。这个计算过程看着简单但实际项目里经常有人拍脑袋定个“3 格”就完事结果机器人在窄门口反复卡住。import numpy as np from scipy.ndimage import binary_dilation def preprocess_grid(occupancy_grid, resolution, robot_radius, localization_error): # 计算膨胀半径对应的栅格数 inflate_meters robot_radius localization_error inflate_cells int(np.ceil(inflate_meters / resolution)) # 构造圆形结构元素比方形更贴合机器人轮廓 y, x np.ogrid[-inflate_cells:inflate_cells1, -inflate_cells:inflate_cells1] struct x*x y*y inflate_cells*inflate_cells inflated binary_dilation(occupancy_grid, structurestruct) return inflated.astype(np.uint8)这段代码里有个细节值得说结构元素用圆形而不是方形。方形膨胀会让障碍物拐角变成直角外扩在斜向通道里会过度侵占可通行空间圆形膨胀更贴近机器人实际扫过的区域路径质量明显更好。我对比过两种方式圆形膨胀在狭窄走廊场景下的成功率高出将近两成。3.2 强制邻居的判定条件怎么写强制邻居判定是 JLT 里最容易写错的地方我见过不止一个实现把条件写反了导致算法在空旷区域疯狂生成节点加速效果归零。这里把逻辑捋清楚。假设当前节点在 (x, y)正在往右探测即方向为 (1, 0)。在探测过程中每走到一个新格子 (xk, y)需要检查它的上下两个邻居如果 (xk, y-1) 是障碍物而 (xk-1, y-1) 不是障碍物说明下方出现了一个“凹角”当前格子是强制邻居如果 (xk, y1) 是障碍物而 (xk-1, y1) 不是障碍物说明上方出现了一个凹角同样是强制邻居。用大白话解释你沿着走廊走突然旁边墙上开了个洞这个洞对应的走廊位置就是强制邻居。如果墙是连续的没有洞那就继续往前跳不用停。斜向探测的判定更复杂一些需要同时检查水平和垂直两个方向的邻居状态。我当初在这里踩过坑斜向的强制邻居条件漏写了一个分支导致地图里所有斜向的窄缝都搜不到路径排查了大半天才发现是判定逻辑的问题。注意强制邻居判定里的“障碍物”指的是膨胀后的障碍物不是原始地图。预处理和判定这两步是绑定的顺序不能乱。3.3 启发式函数的选择与调参JLT 保留了 A* 的 f g h 框架h 函数的选择直接影响搜索行为。常用的有曼哈顿距离、欧几里得距离和切比雪夫距离。在八方向扩展的栅格上我一般用八方向距离Octile distance它比欧几里得距离更贴合实际移动代价又比曼哈顿距离更精确。Octile 距离的公式是h (dx dy) (√2 - 2) × min(dx, dy)其中 dx、dy 分别是当前点到目标点的横纵栅格差。这个公式的推导逻辑是先沿对角线走 min(dx, dy) 步剩下的沿直线走对角线单步代价是 √2直线单步代价是 1。权重方面标准 A* 里 h 的系数是 1保证最优性。但实际工程里我经常把 h 乘上一个略大于 1 的系数比如 1.1 到 1.3牺牲一点点最优性换取明显的速度提升。这个技巧叫加权启发式在实时性要求高的场景下很实用。系数取多少要看具体地图我一般从 1.1 开始试如果路径质量还能接受就往上加加到路径开始明显绕远为止。启发式类型公式适用场景我的评价曼哈顿dx dy四方向扩展低估代价扩展多欧几里得√(dx²dy²)任意方向精度高但计算稍慢八方向(dxdy)(√2-2)min八方向栅格首选平衡好加权八方向1.2 × 八方向实时优先速度快略绕远3.4 开放列表的数据结构选型开放列表用数组还是优先队列对性能影响很大。早期我图省事用 Python 的 list 加 sort地图一大直接卡死。后来换成 heapq情况好了不少但 heapq 不支持 decrease-key 操作每次更新节点代价都得重新 push 一个副本列表里会有大量过期条目。工程上的折中方案是允许重复条目存在出队时检查该节点是否已被关闭如果是就跳过。这个技巧叫“惰性删除”实现简单代价是堆的规模会膨胀一些但实测下来比手动维护索引要快。如果对性能有极致要求可以用带索引的二叉堆或者斐波那契堆不过后者常数因子大在中小规模地图上未必划算。import heapq def jlt_search(grid, start, goal): open_heap [] heapq.heappush(open_heap, (0, start)) g_cost {start: 0} closed set() parent {start: None} while open_heap: _, current heapq.heappop(open_heap) if current in closed: continue # 惰性删除跳过过期条目 if current goal: return reconstruct_path(parent, goal) closed.add(current) for successor, step_cost in get_jump_successors(grid, current, goal): new_g g_cost[current] step_cost if successor not in g_cost or new_g g_cost[successor]: g_cost[successor] new_g parent[successor] current f new_g heuristic(successor, goal) heapq.heappush(open_heap, (f, successor)) return None这段骨架代码里get_jump_successors就是跳点扩展的核心它负责沿八个方向探测并返回所有强制邻居和可达的跳跃终点。实际实现里这个函数会比较长但逻辑就是前面说的那套判定规则。4. 实操过程与核心环节实现4.1 从零搭一个可跑的测试环境光看代码不过瘾得有个能跑起来的环境。我用 Python 搭了个最小可复现的测试框架地图用二维 numpy 数组表示0 是自由空间1 是障碍物。为了模拟真实场景我写了个简单的地图生成器随机撒一些矩形障碍物和窄通道。import numpy as np def generate_test_map(width200, height200, obstacle_ratio0.15, seed42): rng np.random.default_rng(seed) grid np.zeros((height, width), dtypenp.uint8) num_obstacles int(width * height * obstacle_ratio / 400) for _ in range(num_obstacles): w rng.integers(5, 30) h rng.integers(5, 30) x rng.integers(0, width - w) y rng.integers(0, height - h) grid[y:yh, x:xw] 1 # 留出起点和终点区域 grid[5:15, 5:15] 0 grid[height-15:height-5, width-15:width-5] 0 return grid这个生成器故意让障碍物尺寸在 5 到 30 格之间随机既有大块障碍也有细长条能覆盖大部分边界情况。种子固定住保证每次测试的地图一致方便对比不同参数下的表现。4.2 跳点扩展的完整实现核心的跳跃探测函数我拆成了水平垂直和斜向两个分支。水平垂直方向的探测相对直接斜向的需要递归地检查两个分量方向逻辑上更绕一些。def jump(grid, current, direction, goal): x, y current dx, dy direction nx, ny x dx, y dy if not is_free(grid, nx, ny): return None if (nx, ny) goal: return (nx, ny) # 斜向探测需要检查两个分量方向是否有强制邻居 if dx ! 0 and dy ! 0: if (is_free(grid, nx - dx, ny dy) and not is_free(grid, nx - dx, ny)) or \ (is_free(grid, nx dx, ny - dy) and not is_free(grid, nx, ny - dy)): return (nx, ny) if jump(grid, (nx, ny), (dx, 0), goal) or jump(grid, (nx, ny), (0, dy), goal): return (nx, ny) else: # 水平或垂直探测 if dx ! 0: if (is_free(grid, nx, ny 1) and not is_free(grid, nx - dx, ny 1)) or \ (is_free(grid, nx, ny - 1) and not is_free(grid, nx - dx, ny - 1)): return (nx, ny) else: if (is_free(grid, nx 1, ny) and not is_free(grid, nx 1, ny - dy)) or \ (is_free(grid, nx - 1, ny) and not is_free(grid, nx - 1, ny - dy)): return (nx, ny) return jump(grid, (nx, ny), direction, goal)这段代码里斜向分支的递归调用是关键斜向跳跃时如果水平或垂直分量方向上存在强制邻居当前斜向位置也要作为候选点返回。这个设计保证了斜向移动不会漏掉拐角处的路径。4.3 参数计算与实测数据膨胀半径、启发式权重、地图分辨率这三个参数是相互耦合的不能孤立地调。我拿一张 200×200 的测试地图做了一组对比实验起点在左上角终点在右下角每组参数跑 50 次取平均。膨胀格数启发权重平均耗时(ms)扩展节点数路径长度(格)成功率21.018.31240312100%21.211.7780318100%41.022.11580328100%41.214.2960335100%61.219.8132035696%从数据能看出几个规律膨胀格数增加会显著推高扩展节点数和路径长度因为可通行空间被压缩了算法不得不绕更多路启发权重从 1.0 提到 1.2耗时降了约三成路径长度只增加了不到 3%这个买卖很划算。膨胀到 6 格时成功率掉到 96%说明有些窄通道被完全堵死了这提醒我们膨胀半径不能无脑加大得结合场景里最窄通道的宽度来定。实操心得我一般会先量一下目标场景里最窄通道的宽度膨胀后的剩余通行宽度至少要留出机器人直径的 1.5 倍否则规划成功率会断崖式下跌。4.4 路径后处理从折线到可执行轨迹JLT 输出的是一条由跳点连成的折线拐角处是硬转折直接交给底层控制器会出问题——轮式机器人没法瞬间改变朝向无人机也没法在直角处保持速度连续。所以后处理这一步不能省。我的做法分两步走。第一步是路径剪枝把折线上那些“可以直线连通”的中间点删掉。具体操作是从起点开始尝试直接连到尽可能远的后续节点如果连线不穿障碍物就跳过中间所有点。这一步能把节点数压掉一大半。第二步是拐角圆滑在每个转折点附近用二次贝塞尔曲线或者圆弧过渡过渡半径根据机器人最小转弯半径来定。def smooth_corner(p_prev, p_curr, p_next, radius): # 在拐角处插入圆弧过渡点 v1 normalize(p_prev - p_curr) v2 normalize(p_next - p_curr) angle np.arccos(np.clip(np.dot(v1, v2), -1, 1)) if angle 1e-3: return [p_curr] tangent_len radius / np.tan(angle / 2) t1 p_curr v1 * tangent_len t2 p_curr v2 * tangent_len # 用二次贝塞尔近似圆弧 points [] for t in np.linspace(0, 1, 10): pt (1-t)**2 * t1 2*(1-t)*t * p_curr t**2 * t2 points.append(pt) return points这段代码里 tangent_len 的计算依赖拐角角度角度越小越接近直线切线段越长角度越大切线段越短。实际调参时要注意如果两条相邻线段太短切线段可能会超出线段范围需要做截断处理否则平滑后的路径会跑到障碍物里去。5. 常见问题与排查技巧实录5.1 路径贴边与穿墙的排查这是被问得最多的问题。现象是规划出来的路径紧贴着障碍物边缘走或者干脆从两个障碍物的缝隙里穿过去。根因通常有三个膨胀没做、膨胀半径不够、或者强制邻居判定把膨胀后的障碍物当成了自由空间。排查顺序我一般是这样的先把膨胀后的地图可视化出来人眼确认障碍物确实变胖了然后检查 jump 函数里 is_free 的判定用的是不是膨胀后的地图最后看强制邻居条件里引用的邻居坐标有没有越界越界时 is_free 的返回值是否合理。第三步最隐蔽因为越界访问在 numpy 里会抛异常但如果用了 try-except 吞掉异常就会静默返回错误结果。5.2 搜索时间忽长忽短的抖动同样的地图和起终点有时候 10 毫秒跑完有时候 200 毫秒还没出结果。这种抖动多半和开放列表里的过期条目堆积有关。惰性删除虽然实现简单但如果某次搜索中大量节点被反复更新堆里会塞进大量重复条目出队时逐个跳过时间就上去了。缓解办法有两个一是给 g_cost 更新加个阈值新代价比旧代价小得不多时就不更新减少重复入队二是定期对堆做一次“压实”把已关闭节点的条目清掉。我一般用第一种实现简单且效果够用。阈值取多少要看代价的量纲栅格步长是 1 的话阈值取 0.01 到 0.05 之间比较合适。5.3 窄通道里找不到路径地图上明明有通道算法却报无解。先别怀疑算法拿尺子量一下通道宽度。如果通道宽度小于两倍膨胀半径加一格那通道在预处理阶段就被堵死了算法再厉害也变不出路来。这种情况要么减小膨胀半径前提是机器人确实能挤过去要么换更高分辨率的地图。如果通道宽度够但还是搜不到那就检查斜向探测的强制邻居条件。我遇到过一种情况通道是斜向的宽度刚好够但斜向探测时水平分量和垂直分量的递归调用顺序写反了导致某个方向的强制邻居没被识别出来。把两个分量的检查顺序调换一下就好了这种 bug 极其隐蔽只能靠构造针对性测试用例来抓。5.4 常见问题速查表现象可能原因排查动作解决方向路径贴边膨胀不足可视化膨胀地图加大膨胀半径路径穿墙判定用了原始地图检查 is_free 入参统一用膨胀地图耗时抖动堆内过期条目多打印堆规模加更新阈值或压实窄通道无解通道被膨胀堵死量通道宽度降膨胀或提分辨率斜向漏路强制邻居条件不全构造斜向测试用例补全判定分支路径绕远启发权重过高对比不同权重降到 1.1 附近5.5 几个容易被忽略的工程细节第一个是坐标系的统一。栅格坐标和世界坐标的转换如果搞混起点终点会落到错误的位置表现就是算法在莫名其妙的地方搜索。我习惯在入口处就把所有输入统一转成栅格整数坐标内部计算全程用整数只在输出时转回世界坐标。第二个是目标点落在障碍物里的处理。真实场景里目标点经常因为定位漂移落在膨胀后的障碍物区域内这时候算法会一直搜不到。稳妥的做法是在搜索前检查目标点如果不可通行就在附近找一个最近的可通行点作为临时目标并在输出时标注这个偏移量。第三个是地图边界处理。jump 函数递归探测时很容易越界我的做法是在地图外围包一圈障碍物这样越界访问自然返回“不可通行”省去大量边界判断代码也干净不少。这个技巧成本极低但收益很大强烈建议加上。6. 这套算法在实际项目里的边界在哪里聊了这么多实现细节最后说说我自己的使用体会。JLT 在结构化程度高的栅格地图上确实好用我手上一个仓储机器人项目地图是标准的货架阵列用 JLT 之后单次规划耗时从平均 40 毫秒压到了 12 毫秒左右效果立竿见影。但换到另一个室外场景地图是从点云投影出来的障碍物边界毛糙、有大量孤立噪点JLT 的优势就不明显了因为强制邻居判定被噪点频繁触发跳点扩展退化成近似逐格扩展加速比掉到 1.2 倍都不到。所以我的建议是如果你的场景地图干净、障碍物轮廓清晰、对实时性有硬要求JLT 值得投入时间调优如果地图质量本身就不稳定先把感知和建图那一段打磨好比在规划算法上抠性能要划算得多。另外JLT 输出的是几何路径不包含时间信息如果下游需要速度规划还得再叠一层时间参数化这部分内容量不小我打算在下一篇里单独展开讲包括怎么在 JLT 路径上做速度分配、怎么处理曲率约束以及和模型预测控制对接时的一些接口设计经验。