MoveIt2轨迹规划与控制器集成 - 第3节

第三章 规划管道（Planning Pipeline）深度解析：规划器适配器与自定义管道

大家好，欢迎来到第三章。前两章我们聊了MoveIt的架构和基础概念，今天咱们深入一个核心模块——规划管道。说白了，规划管道就是一条流水线，你的运动规划请求从一头进去，经过一系列处理，最终从另一头出来一个可行的轨迹。

我个人习惯把规划管道想象成一个“加工车间”。原始的运动请求是原材料，经过切割、打磨、抛光，最后变成成品。而规划器适配器（Planner Adapter），就是车间里的那些加工设备。

3.1 规划器适配器（Planner Adapter）的作用

先问大家一个问题：为什么MoveIt不直接调用OMPL、STOMP这些规划器，非要中间加一层适配器？

原因很简单——解耦。你想想看，不同的规划器输入输出格式可能不一样，有的要起始位姿，有的要目标约束，有的还要求路径点数量固定。如果没有适配器这一层，MoveIt的代码会变得极其臃肿。

适配器的作用，我总结为三点：

• 格式转换：把MoveIt的通用规划请求，转换成特定规划器能理解的格式
• 预处理/后处理：在规划前后做一些“脏活累活”，比如添加时间戳、平滑轨迹
• 错误处理：当规划失败时，适配器可以尝试修复或给出更友好的错误信息

我在项目中遇到过这样一个坑：直接调用OMPL的RRT算法，规划出来的轨迹虽然无碰撞，但关节速度忽快忽慢，机器人跑起来像在跳舞。后来才发现，是缺少了一个轨迹平滑的适配器。加上之后，轨迹立马变得丝滑。

3.2 规划请求适配器（Planning Request Adapter）链

规划请求适配器链，就是多个适配器串联起来，形成一个处理流水线。每个适配器只负责一件事，但组合起来就能完成复杂的处理任务。

默认的适配器链长这样：

# 默认的规划请求适配器链
planning_adapters:
  - default_planner_request_adapters/ResolveConstraintFrames
  - default_planner_request_adapters/FixWorkspaceBounds
  - default_planner_request_adapters/FixStartStateBounds
  - default_planner_request_adapters/FixStartStateCollision
  - default_planner_request_adapters/FixStartStatePathConstraints
  - default_planner_request_adapters/AddTimeParameterization
  - default_planner_request_adapters/AddTimeOptimalParameterization

我来逐一解释每个适配器的作用：

适配器名称	作用	我的经验
ResolveConstraintFrames	解析约束中的坐标系，确保所有约束都在正确的参考系下	新手容易在这里出错，比如把工具坐标系和基坐标系搞混
FixWorkspaceBounds	检查并修复工作空间边界，防止规划器在无效区域搜索	如果你的机器人工作空间很小，这个适配器能帮你省很多时间
FixStartStateBounds	确保起始状态在关节限位内	我曾经遇到过起始状态超出限位，规划器直接崩溃的情况
FixStartStateCollision	如果起始状态处于碰撞中，尝试微调使其脱离碰撞	这个适配器不是万能的，严重碰撞时它也无能为力
FixStartStatePathConstraints	检查起始状态是否满足路径约束	比如要求末端始终朝下，起始状态不满足的话，它会尝试调整
AddTimeParameterization	给轨迹添加时间戳，让机器人知道每个点该在什么时间到达	没有这个，机器人只会“瞬移”
AddTimeOptimalParameterization	在满足动力学约束的前提下，尽可能缩短运动时间	追求效率时必开，但要注意加速度限制

为什么会需要这么多适配器？因为规划器本身很“傻”，它只负责在配置空间里找一条无碰撞路径。至于路径是否平滑、时间是否合理、起始状态是否合法，它一概不管。这些“脏活累活”，全交给适配器链来处理。

3.3 自定义规划管道

默认的适配器链能满足大部分场景，但总有特殊情况。比如，你的机器人是移动机械臂，需要同时规划底盘和机械臂的运动；或者你的任务对轨迹平滑度要求极高，需要额外的后处理步骤。

这时候，就需要自定义规划管道了。

自定义规划管道，说白了就是自己组装适配器链。你可以从默认链里删掉不需要的，也可以添加自己写的适配器。

3.3.1 编写自定义适配器

写一个适配器其实不难，核心是继承planning_request_adapter::PlanningRequestAdapter类，然后实现adapt方法。

#include <moveit/planning_request_adapter/planning_request_adapter.h>

class MyCustomAdapter : public planning_request_adapter::PlanningRequestAdapter
{
public:
  MyCustomAdapter() : PlanningRequestAdapter() {}

  bool adapt(const planning_scene::PlanningSceneConstPtr& planning_scene,
             planning_interface::MotionPlanRequest& req,
             planning_interface::MotionPlanResponse& res,
             std::vector<std::size_t>& added_path_index) const override
  {
    // 在这里写你的处理逻辑
    // 比如：检查req中的目标约束，如果太严格就放宽一点
    ROS_INFO("My custom adapter is running...");
    return true;  // 返回true表示处理成功
  }
};

3.3.2 在配置文件中启用自定义适配器

写好适配器后，需要在MoveIt的配置文件中注册并启用它。假设你的适配器叫my_package/MyCustomAdapter，在planning_pipeline.yaml里这样配置：

planning_adapters:
  - default_planner_request_adapters/ResolveConstraintFrames
  - default_planner_request_adapters/FixWorkspaceBounds
  - default_planner_request_adapters/FixStartStateBounds
  - default_planner_request_adapters/FixStartStateCollision
  - default_planner_request_adapters/FixStartStatePathConstraints
  - my_package/MyCustomAdapter          # 插入自定义适配器
  - default_planner_request_adapters/AddTimeParameterization
  - default_planner_request_adapters/AddTimeOptimalParameterization

注意我把它插在了FixStartStatePathConstraints和AddTimeParameterization之间。为什么？因为我想在路径约束修复之后、时间参数化之前，做一些额外的路径优化。

3.3.3 规划管道的完整流程

说了这么多，咱们用一张图来总结规划管道的完整流程：

从这张图可以看出，规划请求先经过预处理适配器链，然后交给底层规划器，规划结果再经过后处理适配器链，最终输出可执行的轨迹。嗯，这里要注意：预处理和后处理的适配器可以共用同一个类，只是执行时机不同。

3.4 实战：如何调试规划管道

最后，分享几个调试规划管道的实用技巧：

• 开启详细日志：设置rosconsole的日志级别为DEBUG，可以看到每个适配器的执行时间和处理结果
• 逐个禁用适配器：如果规划结果不理想，可以逐个禁用适配器，找出是哪个环节出了问题
• 使用可视化工具：在RViz中打开规划场景，可以直观地看到适配器对轨迹的影响

好了，这一章的内容就到这里。规划管道是MoveIt的核心，理解了适配器链的工作原理，你就能灵活地定制自己的规划流程。下一章，我们会深入OMPL规划器的参数调优，敬请期待。