IEEE Transactions on Robotics

Kinematically Constrained Marching for Optimal Reeds–Shepp Nonholonomic Path Planning on 2-D Cartesian Grids

作者：Ibrahim Ibrahim, Wilm Decré, Jan Swevers · 机构：Department of Mechanical Engineering, MECO Research Team, KU Leuven, Heverlee, Belgium; Flanders Make@ KU Leuven, Heverlee, Belgium

会议/期刊：IEEE Transactions on Robotics · 发布日期：2026 · 页码：2215-2230

路径规划运动规划优化

精读笔记

Problem Setting

《Kinematically Constrained Marching for Optimal Reeds–Shepp Nonholonomic Path Planning on 2-D Cartesian Grids》（IEEE Transactions on Robotics / 2026）处理的是一个很具体但工程价值很高的问题：给定二维占据栅格和 Reeds–Shepp 车辆模型，计算可绕障的非完整约束距离场，并从中恢复连续可执行路径。

这里真正困难的不是 free-space Reeds–Shepp 两点最短路——那个已有成熟解析结构；困难在于障碍物破坏了 free-space geodesic 的可见性，使得最短路径必须在多个障碍诱导的同伦类之间选择，同时还要满足最小转弯半径和前后向运动约束。传统做法通常把问题压成一个离散图：离散位置、离散朝向、离散 primitive，然后搜索；或者把它压成 PDE 数值求解，再从距离场做梯度回溯。两者都牺牲了连续几何结构。

这篇论文抓住的关键矛盾是：occupancy grid 是离散的，但车辆运动模型不必离散。作者试图保留二维栅格作为障碍表达和 marching 支撑，同时让朝向、转向和 primitive 拼接仍在连续域里计算。这个矛盾处理得好，就能同时拿到 grid planner 的全局覆盖能力和 analytic RS solver 的局部精度。

Motivation

作者对已有路线的不满不是“搜索太慢”这么简单，而是认为主流非完整约束 planner 把错误的东西离散化了。State lattice 和 Hybrid A* 的核心瓶颈是图的连通性：路径质量上限由 heading bins、primitive set、cell merging、analytic expansion 和 smoothing 决定。即使参数调得很细，本质上还是在离散图上近似连续 SE(2) 几何。

各向异性 FMM 在思想上更接近本文，因为它也传播距离场，但它把 Reeds–Shepp 度量写成 PDE/stencil 数值近似，最后还常需要 gradient descent backtracking。作者认为这条路线的问题是把本来有解析结构的局部几何又离散化了一遍。

因此关键缺口是：有没有一种方法可以像 FMM/Dijkstra 一样全局传播、像 any-angle planner 一样跨 cell 做直接连接、又像 Reeds–Shepp analytic solver 一样保持连续局部路径？本文的答案是 visibility-based marching：传播的不是离散动作，而是“哪些区域能用解析 RS geodesic 直接解释，哪些区域需要新增 pivot”。

Core Idea

论文真正的核心不是“提出了一个新 planner plugin”，而是改变了距离场 update 的建模方式。它把障碍环境中的 Reeds–Shepp 距离表示为一组局部解析距离函数的层级组合：在可见区域，直接查询 free-space under-specified Reeds–Shepp 解；在不可见区域，引入 pivot 作为拓扑转折点，并把 pivot 处的 heading 当作连续优化变量。这样，global marching 只负责决定信息传播顺序和 parent/pivot 结构，局部几何仍由解析 RS 模型决定。

这相当于引入了一个很强的 inductive bias：障碍绕行路径可以由少量位于栅格中心的 pivot 加连续 RS primitive 拼接来表达。这个 bias 比 lattice 的固定 primitive 更贴近真实车辆几何，也比 PDE stencil 更少数值扩散。信息流上，它把“在 SE(2) 中搜索”改成“在 2D grid 上传播一棵带连续朝向隐变量的几何树”。因此它可能更 scalable：状态数不乘以 heading resolution；也可能更准确：每次局部 update 不是 stencil 近似，而是解析距离/低维角度优化。

和 prior 的本质区别在于，它没有试图全局离散化 Reeds–Shepp configuration space，而是只离散化障碍诱导的 pivot 位置。这个选择是整篇论文的关键，也是其能力和上限的共同来源。

Method

方法可以压缩成三个必要机制。

第一，Reeds–Shepp visibility。它解决的是“当前目标 cell 是否可以由某个已有 parent/ancestor 通过 free-space 最短 RS 运动直接到达”。如果可见，就不需要新增转向拓扑点，直接用解析 RS 距离更新。这个机制的作用不是单纯 collision checking，而是决定距离场中哪些区域仍属于同一个局部 geodesic patch。

第二，pivot-based occluded update。不可见时，在最后可达节点附近创建 pivot，让路径被约束为从祖先到 pivot 再到目标。pivot 的意义是把障碍导致的遮挡转化成一个局部几何拼接问题。它带来的核心变化是：绕障不通过离散动作序列表达，而通过 parent/pivot 层级表达。

第三，pivot heading optimization。经过 pivot 的位置是离散的，但经过 pivot 的朝向不是。作者为若干基本 maneuver type 写出距离关于 pivot heading 的解析表达，然后用少量 GD 或类似一维搜索求局部最优。这个步骤避免了 heading bin 的主要误差来源。需要注意的是，论文声称不需要 GD backtracking，但 pivot 处仍用了 GD；区别在于 GD 用于局部角度优化，而不是在离散距离场上回溯整条路径。

最后，marching 框架按最小当前距离扩展 cell，并存储 parent/pivot 结构。路径查询时沿层级结构重算/恢复每三个连续 waypoint 的转向角和 primitive。这里的关键不是优先队列本身，而是优先队列中的 update operator 已经从 grid stencil 换成了连续 RS 几何。

Key Insight / Why It Works

最重要的 insight 是：在二维 occupancy grid 上，导致路径质量差的主要不是 grid 位置采样，而是 orientation/action 的离散化。本文保留 grid 位置但释放 heading，这正好打中了 lattice/Hybrid A* 的软肋。路径看起来更短、更平滑，很大概率主要来自这个 inductive bias，而不是某个复杂算法 trick。

第二个有效点是 memory reuse。one-to-all marching 先构建全局距离/parent/pivot 结构，之后任意 query 都是沿已有结构恢复路径。这不是 single-query planner 的同类比较，而是把 test-time compute 前移为 map-dependent preprocessing。在多查询、fleet、task allocation 场景下这是实质优势；在一次性 start-goal planning 中，这个优势会被摊销假设放大。

第三个有效点是局部解析 update 减少了数值误差。相比 anisotropic FMM 的 stencil 近似和 GD backtracking，解析 RS 距离在 free-space patch 内更准确，pivot 处只优化一维角度，优化问题较小且几何结构强。论文对某个 maneuver type 给出凸性论证；但所有组合、退化情况、拓扑切换处的完整证明并不充分。这里的“局部最优”更可信，“全局最短”需要谨慎理解。

哪些可能只是辅助？visibility threshold、memoization、C++ 优化、Nav2 plugin 集成、固定少量 GD 迭代主要是工程层面的加速和稳定化。它们重要，但不是根本贡献。真正贡献是把 Reeds–Shepp analytic geometry 嵌进 marching update 中，并用 pivot hierarchy 代替 SE(2) graph。

增益归因上，我会判断：相对 Smac/Hybrid A* 的距离和 smoothness 改善，大部分来自避免 heading/primitive discretization；相对 RRT*/RRTConnect 的有限时间优势，则大部分来自 occupancy-grid 特化和 one-to-all preprocessing，而不是说明采样法在更长时间或更强 variants 下不可竞争。

Relation To Prior Work

这篇最接近的谱系不是普通 Hybrid A*，而是 visibility-based marching / any-angle shortest path / fast marching 的几何距离场路线，再结合 Reeds–Shepp analytic solver。它本质上是把 VBM 从 holonomic/Minkowski 距离扩展到非完整约束 RS 距离。

相对 state lattice，真正不同点是状态表达：lattice 在 SE(2) 上离散节点和边；本文只在 R2 上 marching，heading 是 update 时产生的连续隐变量。相对 Hybrid A*，不同点是它不把每个 cell 内的若干 heading 状态作为搜索对象，也不依赖 analytic expansion 作为偶发 shortcut；解析 RS 几何是每次 update 的核心。

相对 anisotropic FMM，本文的实质创新是不用 PDE stencil 近似 RS metric，而用可见性切分区域后直接传播解析距离函数。它不是更精细的 FMM，而是用 visibility/pivot 重写了 marching 的局部算子。

哪些看似新其实是重组？priority queue marching、parent matrix、one-to-all distance map、memoized lookup、双向 wave meet 都是已有思想。实质新增的信息在于：RS visibility 的使用方式、occluded region 的 pivot 距离函数表示、以及 pivot heading 连续优化和 marching 的结合。

Dataset / Evaluation

评估覆盖了理想占据栅格、噪声扰动栅格、SLAM 生成地图、不同 map size，以及 Nav2 中 Smac/Hybrid A* 和 OMPL Reeds–Shepp sampling planners 的对比。对一个 planner 论文来说覆盖面算比较完整，尤其是把方法做成 Nav2 plugin 而不是只跑自定义脚本，这增加了工程可信度。

实验确实支持两个核心 claim：路径质量上，相比离散图 planner 更短更平滑；效率上，预处理后 query 很快，marching 近线性 scaling。噪声和 SLAM map 结果说明实现不是只在玩具 polygon map 上工作。

但 evaluation 也有明显边界。第一，和 Smac/Hybrid A* 的比较受参数、实现成熟度和 cost setting 影响很大；文中说增加 headings 影响不大，但这不等于覆盖了所有调参空间。第二，和 RRT*/RRTConnect 的比较采用固定 100 ms budget，更像说明本文在 dense 2D occupancy grid 下有限时间更合适，而不是证明 sampling-based 方法本质较弱。第三，真实世界评估停留在 SLAM map 离线规划，没有闭环执行、动态障碍、定位误差、控制跟踪误差。第四，核心理论 claim“局部最优”在实验中主要通过路径长度对比间接支持，没有系统验证 pivot hierarchy 是否错过更优同伦类。

Limitation

最核心限制是：方法把连续规划问题中的一部分离散化从 heading/primitive 转移到了 pivot position 和 parent hierarchy。它避免了朝向离散化，但没有避免拓扑选择离散化。pivot 只能在栅格中心，parent 候选来自局部 marching 传播；因此全局最优性上限受栅格分辨率、visibility 判定和 pivot 生成策略限制。

第二，局部最优的成立依赖固定 maneuver type 和固定路径拓扑。一旦路径拓扑发生切换、切线消失、圆/点退化、障碍边界复杂，凸性和唯一最小值论证未必直接适用。文中对 edge cases 的处理大量指向开源实现，理论闭合度不够。这里有一定“工程 regularization 托底”的味道。

第三，scalability 的上限不只是 O(n)。如果引入非圆 footprint、orientation-dependent collision checking、更密集 primitive validation、动态障碍或时空约束，常数会明显上升，而且连续 heading 优化与离散碰撞查询之间会出现新的不一致。当前圆形 envelope 对移动机器人常见但对细长 Ackermann 车辆偏保守。

第四，one-to-all 是双刃剑。多查询场景很强；single-query、未知地图频繁变化、局部动态重规划时，完整 marching 可能过度计算。作者提到 greedy variant，但这部分不是本文贡献。

第五，泛化不是学习意义上的泛化，而是几何模型适用性。它依赖 Reeds–Shepp shortest-path analytic structure；换成有速度/加速度/轮胎动力学/倒车代价不对称的真实车模后，局部解析 update 可能不再成立或会变成另一个复杂优化问题。

Takeaway

  • 1. 最值得记住的不是某个公式，而是建模选择：在 occupancy grid planner 中，位置可以离散，heading 未必要离散；把 heading 作为局部连续隐变量往往比提高 lattice resolution 更有效。
  • 2. Visibility-based marching 是一种很有迁移价值的结构：用可见性决定何时复用解析 geodesic，用 pivot 表达障碍诱导的拓扑变化。
  • 这套思想可迁移到 Dubins、带曲率代价路径、甚至某些有解析局部 steering function 的系统。
  • 3. 这篇推动的是“解析几何 steering function + 全局距离场传播”的融合路线，而不是又一个搜索 planner。

一句话总结

这篇论文把 Reeds–Shepp 解析几何嵌入 visibility-based marching，用连续 heading 的 pivot 距离场替代离散 SE(2) 图搜索，是非完整约束栅格规划从 primitive discretization 向 analytic-distance propagation 演化的一步。