067｜OMPL：怎样让不同运动规划算法使用同一套问题接口

总览 · 上一篇 · 下一篇

原论文： The Open Motion Planning Library，Ioan A. Şucan、Mark Moll、Lydia E. Kavraki，IEEE Robotics & Automation Magazine，2012。出版链接 · 作者论文稿。

先接住前作的问题

PRM与RRT-Connect可以各自实现，但不同碰撞检查和预算会妨碍比较。OMPL抽象共同组件并提供基准工具；与ROS组合后，规划依旧需要正确的机器人模型。

五个前置概念

理解本文需要的五个概念：

• 状态空间：包含系统状态及其距离、插值和拓扑规则的集合。
• 采样式规划：通过有限采样和有效连接近似连续空间连通性。
• 运动验证器：判断一整段运动是否可行，而不仅判断端点。
• 控制传播：在给定输入和时间后，计算动力学系统的后继状态。
• 概率完备性：在相应假设下，采样增加时有解问题成功概率趋近1。

一、问题设定：规划器比较怎样避免混入碰撞和状态实现差异

想比较RRT与PRM，通常要先写状态表示、距离计算、采样、碰撞检查和实验脚本。若两份代码连最近邻结构与碰撞检测精度都不同，测出的“算法优势”可能只是底层实现差异。将算法移到另一台机器人，又可能重写一遍基础设施。

研究背景与问题定义

运动规划寻找从起点到目标区域的连续可行路径。用教学记号写为：

$$q:[0,1]\rightarrow X_{\rm valid},\qquad q(0)=q_{\rm start},\quad q(1)\in X_{\rm goal}.$$

$X_{\rm valid}$ 是满足碰撞、关节边界等要求的状态集合。几何规划中的参数不必是真实时间；带控制的规划还需满足：

$$\dot q(t)=f(q(t),u(t)),\qquad u(t)\in U.$$

状态 $q$ 在动力学场景可能包含位置与速度，$u$是控制，$f$由用户或模拟器提供。汽车不能横移，即使两点之间几何直线没有障碍，也可能不能直接执行。OMPL明确区分这两类问题。

这不是强化学习论文：没有从奖励学习策略，也没有训练数据集或梯度更新预算。采样是搜索手段，不等于用环境回报训练。算法找到一条路径，仍需轨迹时间参数化、控制器跟踪与环境更新才能成为真实机器人动作。

原文讨论概率完备性：对满足相应假设且有解的问题，随着采样增加，找到解的概率趋近1。它不意味着在一秒期限内必然找到，更不意味着超时可以证明无解。软件接口也不能自动使所有接入算法拥有同样的理论性质。

二、按状态空间、有效性、目标和规划器划分接口

1. 规划器只依赖需要的操作

StateSpace定义距离、插值、相等判断、状态存储及分配。规划器无需知道内部坐标怎么排列，只调用这些接口。CompoundStateSpace可组合子空间，例如三维刚体由平移与旋转组成，移动双臂机器人由底盘和两条手臂组成。

拓扑非常重要。连续转动关节宜按圆周处理；有限转角关节需要边界，不能随意从上限绕回下限。把所有状态都塞进普通欧氏向量，可能让距离、采样和插值含义错误，算法即使正常运行也可能规划错误问题。

SpaceInformation汇总规划所需能力；ProblemDefinition保存起点和目标；Planner执行搜索；Path保存结果；SimpleSetup负责装配合理默认组件。默认值降低入门负担，但不代表针对每个机器人都已完成最优调参。

2. 状态有效，不代表连接有效

StateValidityChecker判断一个状态是否允许，常见实现调用碰撞检测，但也可检查其他约束。MotionValidator检查两个状态之间的运动段。

原文默认方案沿插值以一定分辨率取样检查。两个端点都不碰撞，中间仍可能穿过薄障碍。因此检查精度影响速度与正确性，必须作为实验配置报告。论文说明可替换为连续碰撞检测；不能把可扩展接口理解成默认已获得连续安全保证。

带动力学时，StatePropagator从初始状态和控制积分得到后继。可以封装微分方程积分器，也可以使用物理引擎。Box 2中ODE支持车驶离斜坡与推动物体等例子，但物理模型与允许接触仍须由应用定义。

3. 采样与目标各有层次

状态采样器了解空间拓扑；有效状态采样器结合有效性检查，把采样偏向可行区域或障碍附近；控制采样器产生输入；定向控制采样器尝试朝给定状态传播。它们不是同一个随机数接口。

目标最一般可由布尔判断定义，再增加到目标的启发式距离，或允许直接从目标区域采样。例如“末端到达杯子附近”可能对应多组逆运动学解；采样这些目标解通常比强制指定一个关节姿态更灵活。原文还设计后台生成目标样本，以减少较慢逆运动学过程对规划器的阻塞。

4. 抽象也有克制

几何RRT与控制RRT在库中可以是独立实现。作者明确选择不以更多抽象强迫二者共享同一严格代码骨架，因为那会增加实现限制和运行开销。因此可扩展架构并不等于尽可能合并所有代码。

核心库不承担全部机器人系统。OMPL.app通过外部几何加载和PQP/FCL碰撞库提供图形演示；ROS接口把URDF、关节组、运动学和碰撞场景转成OMPL接口。论文所述2012集成是当时的软件结构，不应当成今天版本的安装教程。

输入：机器人问题、起点、目标条件、计算期限
建立正确拓扑和边界的状态空间
实现状态有效性检查，并指定运动段检查精度或替代验证器
若考虑动力学：
    建立控制空间，提供传播器与控制持续时间规则
建立目标表示；需要时提供目标采样或逆运动学接口
选择兼容的规划器，或使用可审查的默认设置
循环采样、扩展或连接，直到找到解或期限耗尽
对返回结果检查实际目标误差、可行性和路径质量
若做比较，以相同问题接口重复运行多个规划器并记录配置
输出：路径或控制序列，以及求解状态与评价记录

这是根据§III—IV重构的教学流程。数学可优化路径长度等应用指标，但本文没有给全部规划器定义同一个新损失；机制改动是组件组织，实验收益是复用、集成和比较能力。不能虚构成“OMPL损失降低所以路径更优”。

相同接口不等于相同问题，比较前先锁定可行性检查

教学反例：一维路径端点0和1均合法，障碍为 $(0.49,0.51)$。只检查端点的验证器报告可行；每0.1检查一次会撞到0.5；若采样网格恰好避开障碍仍可能漏检。更换规划器并不能修复错误的运动验证器。状态合法性、整段运动合法性与动力学传播是三个接口责任。

公平基准应固定状态空间距离、碰撞几何、验证分辨率、终止预算和目标函数，再多次运行随机规划器。若十次有两次未找到解，仅平均另外八次的路径长度会产生幸存者偏差；应同时报成功率和截止时成本分布。

本篇把PRM、RRT-Connect、RRT*组织成可替换模块，并与ROS组合。原文第II节及Benchmark框强调共同组件和重复比较。库的价值是让条件一致的对比和集成更容易，不是证明每个接口默认值适合所有机器人。

接口把规划器与机器人模型分开，也把比较中的配置责任留给应用。下面按状态、检查器、目标和预算组装一次实验，再读集成案例。

三、组装规划问题，再固定预算运行基准

自构例子，未运行。 两个姿态位于一堵薄墙两侧，端点都有效。规划器尝试连接，若MotionValidator只检查端点，它会误判直线可行；检查中间点可能发现碰撞，更细分辨率代价更高。换一个规划器但保留错误验证器，不会修复问题。这说明规划算法和几何有效性是两个独立责任。

另一个例子是连续关节从179度到负179度。圆周空间应走约2度短弧；普通有界实数插值可能走358度。正确拓扑能改变最近邻与扩展，而不仅是显示角度更好看。

阅读范围：核对2012作者论文稿的概念、实现、示例及集成章节。没有将后续OMPL版本功能当成2012成果；未运行库或上述自构例子，也未重测规划性能。

四、读几何、动力学与ROS集成案例

论文怎样验证设计

读图：颜色和箭头区分用户配置的对象与库实现的抽象接口。

读图：模型、关节组和碰撞场景仍是必要输入，统一接口并不免除这些设定。

读图：最后的solve(1.0)只给本示例一秒期限，不承诺任意机器人场景在一秒内求解成功。

图 3调用一秒求解期限，这是示例参数，不是“所有机器人一秒内规划成功”的实验结论。Box 1描述重复试验功能，也不等于论文每个示例已经采用相同种子、次数和硬件完成系统性统计比较。本文未确认这样的统一实验预算。

对随机规划算法，公平性还要控制状态有效性分辨率、最近邻结构、时间与内存上限、目标容差及路径后处理。共享框架可以减少差异，不能消除所有调参影响。如果只比较成功运行的耗时，超时失败被删除，也会形成选择偏差。

读图：GUI 把机器人模型、规划参数和路径可视化并置，展示库如何接入真实几何场景。界面里能显示路径，不代表任意碰撞模型、距离函数或一秒预算都具有同样保证。

证据1：图 2与§IV.A。 抽象状态空间提供拓扑相关操作，复合空间组合已有结构。它支持“算法代码不必直接操作机器人坐标数组”的架构结论；不能证明任意用户定义的距离都满足某个规划器的理论假设。

证据2：§IV.B。 状态检查、运动检查与控制传播分别定义。论文明确允许替换默认离散检查，说明安全有效性由验证器语义决定。这个设计降低耦合，但不把数值积分误差或碰撞漏检自动消除。

证据3：图 3。 短示例说明只需配置空间、有效性和起终点便可运行基本规划。其有效性是一个简单坐标条件，并非完整真实碰撞场景，所以代码行数不能当作真机部署成本。复杂机器人仍需正确的模型与约束。

证据4：Box 1与§V.A。 基准接口能多次运行、记录配置和指标，并导出箱线图。支持系统具备统计实验工具，不支持某个规划器在全部场景最佳。中位数、失败概率和长尾延迟应共同看，而不是只取最快一次。

证据5：§V.B与VI。 ROS接口把URDF、关节组、携带物体和目标约束映射到规划问题；作者还讨论分子构象搜索使用不同状态空间。说明接口具有跨应用复用潜力。但应用演示没有量化证明新增算法或新平台的开发工作量固定降低多少。

论文的主要证据是可工作的实现、接口细节和案例。对软件论文，这种证据有价值；准确解读应承认其类型，而不是硬补不存在的3%成功率提升或十倍加速结果。

五、保留应用模型责任和实验可比性

贡献包括概念与类接口对应、规划器与应用解耦、几何和控制规划的组织、教学前端以及基准实验工具。它让研究者能把注意力集中到采样或扩展策略，而不必反复搭建所有底层组件。

代价是责任边界更明确，也要求使用者理解边界。若有效性检查漏掉夹持物体，规划器不会凭空知道手上多了一个箱子；若动力学模型忽略制动距离，控制规划也不能保证真机按预测停下。错误接口会稳定地产生错误结果。

作者把长期成功归于社区采用与维护，而不是仅凭架构即可保证。本文进一步分析：统一库可能产生共同默认参数偏差；某些专用规划器的最佳实现未必符合通用接口；概率完备算法的长尾仍可能不适合严格实时期限。工程复用和算法适用性需要分别评估。

六、追问：两个规划器使用同一OMPL接口，为什么仍需核对运动有效性检查分辨率？

七、组合具体规划器与通信框架

• 模块组合：RRT-Connect：让两棵搜索树主动相遇。通过统一接口运行双向树规划，并由应用提供碰撞模型。
• 模块组合：ROS：让机器人算法能组成系统。接入消息与部署框架后，规划问题定义仍属于应用。

八、自测与参考答案

问题1：为什么两个无碰撞状态之间，仍需要MotionValidator？

答案：路径中间可能穿过障碍，或违反其他约束。状态检查只提供点上的判断，运动验证负责整段连接。离散检查需要足够分辨率，连续检查则由相应几何算法及假设提供保证。

问题2：给汽车选择SE(2)状态空间，是否已经解决不能横移的问题？

答案：没有。SE(2)表达平面位置和朝向，但不会自动约束可实现速度。若使用普通几何插值，仍可能产生横向移动。必须采用合适局部连接或控制传播，并提供车辆运动学、控制边界和验证规则。

问题3：同在OMPL里运行，是否就能直接宣称最快的规划器最好？

答案：不能。不同规划器在成功率、首次解时间、最终路径质量和内存之间有取舍，参数与验证精度也会影响结果。应重复运行、保留失败、统一问题接口与预算，并按实际任务需求比较多个指标。