本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

MoveIt2规划器模块：RRT到CHOMP的路径规划

导读：机械臂如何找到一条从起点到目标的"无碰撞路径"？MoveIt2 集成了 OMPL、CHOMP、STOMP 三大规划器家族，本文带你搞懂各类规划器的原理、特点及选型建议。

原理简析

运动规划是在关节空间或笛卡尔空间中，寻找从起始状态到目标状态的可行路径，同时满足避障约束。MoveIt2 通过 Planning Pipeline 机制串联多个规划器与后处理插件，支持组合使用。

flowchart TB A(["起点状态"]) --> B["规划器探索"] B --> C["采样新状态"] C --> D{"碰撞检测"} D -->|"碰撞"| C D -->|"无碰撞"| E["加入搜索树"] E --> F{"到达目标?"} F -->|"否"| C F -->|"是"| G["路径返回"] G --> H["路径平滑/优化"] H --> I(["输出轨迹"])

三大规划器家族：
- OMPL：基于采样（RRT、RRT、PRM、RRTConnect），随机探索，适合高维空间
- CHOMP：基于梯度优化，对初始轨迹做协方差平滑，适合生成平滑轨迹
- STOMP*：基于随机优化，无需梯度，适合带噪声代价场场景

实操步骤

规划器对比与选型

规划器	类型	适用场景	速度	路径质量
RRTConnect	采样	默认通用，双向快速探索	快	一般
RRTstar	采样	渐近最优路径	慢	最优
PRM	采样	多查询场景，可预计算	中	一般
CHOMP	优化	平滑轨迹、避障	中	平滑
STOMP	优化	平滑+无梯度避障	快	平滑

flowchart TB A(["选择规划器"]) --> B{"场景类型"} B -->|"单次查询"| C["RRTConnect<br/>默认首选"] B -->|"多次查询"| D["PRM<br/>预计算路图"] B -->|"需要平滑"| E["CHOMP / STOMP"] C --> F["后处理优化"] E --> F D --> F

避坑点：CHOMP 在 MoveIt2 中是独立 Planning Pipeline（非 OMPL 插件），需在 ompl_planning.yaml 之外单独配置 chomp_planning.yaml；常见组合是 OMPL 规划 + CHOMP 平滑后处理。
来自 linuxros.cn · linuxROS

代码实现

配置 OMPL 规划器（ompl_planning.yaml）

# ompl_planning.yaml
planning_pipelines:
  ompl:
    default_planner: RRTConnect
    planner_configs:
      RRTConnect:
        type: geometric::RRTConnect
        range: 0.0   # 0 表示自动
      RRTstar:
        type: geometric::RRTstar
        goal_bias: 0.05
      PRM:
        type: geometric::PRM
        max_nearest_neighbors: 10

CHOMP 独立配置（chomp_planning.yaml）

# chomp_planning.yaml
planning_pipelines:
  chomp:
    planner_type: "CHOMP"
    collision_clearance: 0.04
    learning_rate: 0.05
    max_iterations: 200
    smoothness_cost_weight: 0.1
    obstacle_cost_weight: 1.0

C++ 规划调用示例

// planner_example.cpp
#include <moveit/move_group_interface/move_group_interface.h>

void PlanExample(moveit::planning_interface::MoveGroupInterface& move_group)
{
    // 设置起始状态
    move_group.setStartStateToCurrentState();

    // 设置目标（关节空间）
    std::vector<double> goal_joint_values = {0.0, -0.5, 0.3, 0.1, 0.5, 0.0};
    move_group.setJointValueTarget(goal_joint_values);

    // 设置规划时间限制与规划器
    move_group.setPlanningTime(5.0);
    move_group.setPlannerId("RRTConnect");

    // 规划并执行
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS) {
        move_group.execute(plan);
    }
}

笛卡尔路径规划

// cartesian_path.cpp
void CartesianPathExample(
    moveit::planning_interface::MoveGroupInterface& move_group)
{
    std::vector<geometry_msgs::msg::Pose> waypoints;
    geometry_msgs::msg::Pose pose = move_group.getCurrentPose().pose;

    pose.position.x += 0.2;
    waypoints.push_back(pose);

    pose.position.z += 0.1;
    waypoints.push_back(pose);

    // 笛卡尔路径规划
    moveit_msgs::msg::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.01;
    double fraction = move_group.computeCartesianPath(
        waypoints, eef_step, jump_threshold, trajectory);

    // 90% 以上视为成功
    if (fraction > 0.9) {
        move_group.execute(trajectory);
    }
}

注意：computeCartesianPath 不做避障规划，仅做线性插值 + IK；若需避障应使用 computeCartesianPath 后再做碰撞校验，或改用 Pilz 工业规划器。

常见问题解决

Q1：规划时间太长怎么优化？
A：优先使用 RRTConnect（双向搜索）；减小 range 参数细化采样或增大粗化采样；启用 parallel_planning 多规划器并发。

Q2：规划出的路径不光滑怎么办？
A：在 Planning Pipeline 后处理阶段追加 CHOMP 或 STOMP；或使用 iterative_spline_parameterization 做时间最优重参数化。

Q3：狭小空间规划失败？
A：调整 OMPL 的 projection_evaluator 与 range；改用 BiTRRT 或 KPIECE；适当放宽 contact_distance。

总结

MoveIt2 的规划器生态丰富，OMPL 的 RRT 系列适合快速探索，CHOMP/STOMP 适合生成平滑轨迹。实际项目往往需要组合使用：先用 RRTConnect 快速找到可行解，再用优化算法平滑轨迹。下期讲解碰撞检测模块，搞懂 FCL 与 Octomap 的避障机制。