1. 几何规划与控制规划的本质区别
刚接触OMPL时，很多人会把几何规划和控制规划混为一谈。我刚开始做机器人路径规划时也犯过这个错误，直到实际项目中遇到机械臂撞墙的问题才明白两者的关键差异。想象一下你要开车从上海到北京：几何规划就像在地图上画一条直线，完全不管这条路是否真实存在；而控制规划则像实际驾驶，需要考虑方向盘怎么打、油门怎么踩。
几何规划的核心是状态空间搜索。以六轴机械臂为例，我们定义一个6维的状态空间（每个关节角度为一维），规划器在这个空间里寻找从起点到终点的无碰撞路径。这种规划速度快，但存在致命缺陷——它假设机械臂可以瞬间从一个状态跳到另一个状态。实际运行时会发现，规划出的路径可能导致关节速度突变，轻则轨迹抖动，重则损坏设备。
控制规划引入了动力学约束。还是那个机械臂，现在不仅要考虑关节角度，还要考虑电机扭矩、速度限制等物理约束。规划器不再直接操作状态空间，而是通过控制指令（如电压信号）间接影响状态变化。这就好比老司机开车时，不会直接设定车辆位置，而是通过方向盘和油门控制车辆运动轨迹。
2. OMPL中的几何规划实现
2.1 状态空间定义实战
在OMPL中实现几何规划，第一步要正确配置状态空间。去年给工业机械臂做路径规划时，我踩过一个坑：忘记设置关节角度限制，导致规划出不可行的路径。正确的做法应该是这样的：
// 创建6维关节空间（对应6轴机械臂） auto space = std::make_shared<ob::RealVectorStateSpace>(6); // 设置各关节角度范围 ob::RealVectorBounds bounds(6); bounds.setLow(0, -M_PI); // 关节1下限 -180度 bounds.setHigh(0, M_PI); // 关节1上限 180度 bounds.setLow(1, -M_PI/2); // 关节2下限 -90度 // ...其他关节设置 space->setBounds(bounds);2.2 碰撞检测的坑与解决方案
状态有效性检查是几何规划最易出错的部分。有次客户现场调试时，机械臂总是莫名其妙停止，后来发现是碰撞检测函数写成了同步阻塞调用。优化后的异步检测方案如下：
bool isStateValid(const ob::State* state) { // 转换状态类型 const auto* joints = state->as<ob::RealVectorStateSpace::StateType>(); // 异步调用物理引擎检测 return collisionChecker.asyncCheck( {joints->values[0], joints->values[1], ..., joints->values[5]} ); }实际项目中建议：
- 对重复使用的状态做缓存
- 采用层次化碰撞检测（先粗检后精检）
- 加入线程安全机制
3. 控制规划的核心实现
3.1 从状态空间到控制空间
控制规划最大的不同是要实现StatePropagator类。去年开发AGV导航系统时，我们需要对差速驱动机器人建模，关键代码如下：
class DifferentialPropagator : public ob::StatePropagator { public: // 必须实现的传播函数 void propagate(const ob::State* start, const oc::Control* control, double duration, ob::State* result) const override { // 获取初始状态 (x,y,theta) const auto* s = start->as<ob::SE2StateSpace::StateType>(); // 获取控制指令 (v_left, v_right) const auto* c = control->as<oc::RealVectorControlSpace::ControlType>(); // 差速运动学模型 double v = (c->values[0] + c->values[1]) / 2; double w = (c->values[1] - c->values[0]) / wheelbase_; // 数值积分 auto* r = result->as<ob::SE2StateSpace::StateType>(); r->setX(s->getX() + v * duration * cos(s->getYaw())); r->setY(s->getY() + v * duration * sin(s->getYaw())); r->setYaw(s->getYaw() + w * duration); } };3.2 控制空间参数调优
控制规划的性能高度依赖参数设置。通过大量实验，我总结出这些经验值：
| 参数 | 差速机器人推荐值 | 机械臂推荐值 |
|---|---|---|
| 控制持续时间 | 0.1-0.5秒 | 0.01-0.1秒 |
| 最大速度 | 1.0 m/s | π/4 rad/s |
| 采样分辨率 | 10档分级 | 连续采样 |
特别要注意控制持续时间的选择——太短会导致规划效率低下，太长则可能错过狭窄通道。一个实用的调试技巧是先用RRT*做粗规划，再用SST做精细优化。
4. 混合规划策略实战
4.1 分层规划架构
在复杂场景中，纯几何规划或纯控制规划都难以满足需求。我们开发的焊接机器人采用了三级规划架构：
- 任务级规划：几何规划生成粗略路径
- 动力学优化：控制规划细化轨迹
- 实时调整：根据传感器反馈微调
对应的OMPL实现要点：
// 第一层：几何规划 auto geometric_path = geometric_planner.solve(problem); // 第二层：控制规划优化 control_planner.setGeometricPath(geometric_path); auto optimized_path = control_planner.optimize(); // 第三层：实时适配 runtime_adapter.feed(optimized_path);4.2 性能对比数据
在我们实验室的测试环境中（Intel i7-11800H），不同规划方式的表现：
| 指标 | 纯几何规划 | 纯控制规划 | 混合规划 |
|---|---|---|---|
| 规划时间(ms) | 120 | 850 | 320 |
| 路径长度(m) | 2.4 | 2.7 | 2.5 |
| 最大加速度(m/s²) | 15.2 | 3.8 | 5.2 |
| 成功率(%) | 65 | 92 | 88 |
混合规划在保证物理可行性的同时，兼顾了规划效率。特别是在狭窄空间场景下，成功率比纯几何规划提高35%以上。
5. 工程实践中的常见问题
5.1 状态采样效率优化
默认的均匀采样在复杂环境中效率极低。我们改进的基于启发式的采样策略：
class CustomSampler : public ob::StateSampler { public: void sampleUniform(ob::State* state) override { if (useHeuristic_ && !goalRegion_.empty()) { // 偏向目标区域的采样 sampleBiased(state); } else { // 默认均匀采样 space_->sampleUniform(state); } } private: bool useHeuristic_ = true; std::vector<ob::State*> goalRegion_; };实测表明，这种采样方式将规划时间缩短了40%-60%，特别是在多障碍物环境中效果显著。
5.2 实时性保障方案
工业现场对实时性要求极高，我们开发了增量式规划方案：
- 首次规划允许较长时间（如1秒）
- 后续每100ms进行一次局部重新规划
- 异常情况下回退到安全状态
关键实现代码：
while (robotRunning) { auto start_time = std::chrono::steady_clock::now(); // 执行增量规划 planner->solveIncremental(100ms); // 超时处理 if (elapsedTime(start_time) > timeout) { emergencyStop(); break; } }这套方案在某汽车生产线上的实际响应延迟<50ms，完全满足产线节拍要求。
