2、OMPL核心概念：从状态空间到规划时间
好，咱们正式开始啃OMPL。这一章是后续所有规划器调优的基础，说白了就是搞清楚OMPL到底在解决什么问题、它怎么描述机器人的运动、以及我们怎么控制规划过程。我个人觉得，理解这些概念比背API参数重要得多——参数可以查文档，但概念错了，调参就是瞎调。
2.1 OMPL库简介：一个规划器工厂
OMPL（Open Motion Planning Library）是一个开源的运动规划算法库。它不关心你用什么机器人、什么碰撞检测库，它只负责一件事：在给定的空间里找一条从起点到终点的路径，避开障碍物。
你可以把OMPL想象成一个「规划器工厂」。你告诉它：
- 我的机器人长什么样（状态空间）
- 它能怎么动（控制空间）
- 起点和终点在哪（规划问题）
- 什么时候该停（终止条件）
然后它从几十种规划算法里挑一个（或者你指定一个），开始搜索。我在项目中遇到过不少新手，上来就调RRT的参数，结果发现规划出来的路径根本不能用——其实问题出在状态空间定义错了，机器人关节限位都没设对。
2.2 状态空间（State Space）与控制空间（Control Space）
这两个概念是OMPL的基石。我刚开始学的时候也绕晕过，咱们用个例子说清楚。
2.2.1 状态空间：机器人的「快照」
状态空间描述的是机器人某一时刻的完整状态。对于机械臂，状态通常是关节角度向量 (q1, q2, ..., qn)。对于移动机器人，可能是 (x, y, theta)。
OMPL内置了多种状态空间：
| 状态空间类型 | 适用场景 | 维度示例 |
|---|---|---|
RealVectorStateSpace |
机械臂关节空间 | 6轴臂 → 6维 |
SE2StateSpace |
平面移动机器人 | (x, y, yaw) |
SE3StateSpace |
无人机、水下机器人 | (x, y, z, roll, pitch, yaw) |
CompoundStateSpace |
复合机器人（移动+机械臂） | SE3 + 关节空间 |
嗯，这里要注意：状态空间必须定义有效范围。比如关节1只能转-180°到180°，你必须在状态空间里设好边界。我曾经见过一个案例，有人忘了设关节限位，规划器在关节空间里乱跑，生成的路径在物理上根本实现不了。
setBounds()。哪怕你用的是MoveIt自动生成的规划场景，也建议手动确认一遍关节限位——MoveIt从URDF读到的限位不一定是你想要的。
2.2.2 控制空间：机器人「怎么动」
控制空间描述的是机器人能执行的动作。对于差分驱动小车，控制可能是 (v, omega)（线速度+角速度）。对于机械臂，控制可能是关节速度 (dq1, dq2, ...)。
OMPL里控制空间通常和状态传播器（StatePropagator）配合使用。传播器负责根据当前状态和控制输入，计算下一时刻的状态。说白了就是：你给我一个控制指令，我告诉你机器人会变成什么样。
为什么需要控制空间？因为有些规划器（如KPIECE、EST）需要知道机器人的运动能力，才能更高效地搜索。如果你只关心几何路径（比如RRT），可以不用控制空间——但用了能提高规划质量。
2.3 规划问题定义：SimpleSetup vs. ProblemDefinition
OMPL提供了两种方式来定义规划问题。我个人的建议是：小项目用SimpleSetup，大项目用ProblemDefinition。为什么？往下看。
2.3.1 SimpleSetup：快速上手
SimpleSetup是一个封装好的类，把状态空间、规划器、目标、终止条件都包在一起。你只需要几行代码就能跑通一个规划：
// 创建状态空间
auto space = std::make_shared<ob::RealVectorStateSpace>(6);
space->setBounds(...);
// 创建SimpleSetup
ob::SimpleSetup ss(space);
// 设置起点和终点
ss.setStartAndGoalStates(start, goal);
// 设置规划器
ss.setPlanner(std::make_shared<og::RRT>(ss.getSpaceInformation()));
// 开始规划
ob::PlannerStatus solved = ss.solve(1.0);SimpleSetup适合原型验证、教学演示、快速测试。但它的缺点是灵活性不够——如果你想自定义状态验证器、多目标规划、或者动态修改规划参数，SimpleSetup就有点力不从心了。
2.3.2 ProblemDefinition：完全控制
ProblemDefinition是更底层的接口。它把规划问题的各个部分拆开，让你可以精细控制：
// 创建空间信息
auto si = std::make_shared<ob::SpaceInformation>(space);
// 创建规划问题
auto pdef = std::make_shared<ob::ProblemDefinition>(si);
pdef->setStartAndGoalStates(start, goal);
// 创建规划器
auto planner = std::make_shared<og::RRT>(si);
planner->setProblemDefinition(pdef);
// 开始规划
planner->solve(termination_condition);你看，这里每一步都是显式创建的。你可以替换状态验证器、添加路径约束、设置多个目标状态——自由度大得多。
| 对比项 | SimpleSetup | ProblemDefinition |
|---|---|---|
| 代码量 | 少（5-10行） | 多（15-30行） |
| 灵活性 | 低 | 高 |
| 适用场景 | 快速原型、教学 | 生产环境、复杂约束 |
| 调试难度 | 低（封装好） | 中（需要理解内部机制） |
2.4 PlannerTerminationCondition与规划时间
规划器不是无限跑的——你得告诉它什么时候停。OMPL提供了PlannerTerminationCondition（PTC）来控制终止条件。
2.4.1 基本用法
最简单的终止条件是时间：
// 规划最多1秒
ob::PlannerTerminationCondition ptc =
ob::timedPlannerTerminationCondition(1.0);
planner->solve(ptc);但实际项目中，只用时间终止往往不够。比如：
- 规划器可能在0.1秒就找到了路径，但你还得等满1秒？
- 或者1秒到了，路径质量很差，能不能再给点时间？
2.4.2 组合终止条件
OMPL支持组合多个终止条件：
// 创建两个条件
auto time_ptc = ob::timedPlannerTerminationCondition(2.0);
auto iter_ptc = ob::iteratedPlannerTerminationCondition(10000);
// 组合：时间到或迭代次数到，任一满足就停
auto combined_ptc = time_ptc || iter_ptc;你还可以自定义终止条件。比如：当找到的路径长度小于某个阈值时提前停止：
class MyTerminationCondition : public ob::PlannerTerminationCondition {
bool operator()() const override {
// 检查当前最优路径长度
return best_path_cost_ < threshold_;
}
};2.4.3 规划时间的艺术
规划时间不是越长越好。你想想看：
- 时间太短：可能找不到路径，或者路径质量差
- 时间太长：实时性要求高的场景（如在线重规划）会卡死
我做过一个焊接机器人项目，要求每200ms重规划一次。一开始设了500ms的规划时间，结果机器人经常卡在规划阶段，焊接轨迹断断续续。后来改成100ms + 迭代次数限制，配合路径缓存，才解决问题。
| 场景 | 推荐规划时间 | 说明 |
|---|---|---|
| 离线预规划 | 5-30秒 | 追求路径质量，可以多跑几次 |
| 在线重规划 | 50-500ms | 实时性优先，接受次优路径 |
| 教学演示 | 1-3秒 | 平衡等待时间和成功率 |
| 高维空间（>10维） | 10秒以上 | 维度灾难，需要更多时间探索 |
2.5 本章知识体系总览
下面这张图把本章的核心概念串起来了。你可以看到，从状态空间到规划问题定义，再到终止条件，是一条完整的链路。
这张图把本章的核心概念串起来了。你可以看到，从状态空间到规划问题定义，再到终止条件，是一条完整的链路。我个人建议你把这图存下来，后面学规划器算法时，随时回来对照——很多调优问题，追根溯源都能回到这些基础概念上。