4、运动规划器（Planner）详解：OMPL规划器家族

运动规划器，说白了就是机器人的“大脑”里负责想路的那部分。你给机器人一个起点和终点，它得自己琢磨出一条不撞墙、不卡死的路径来。在 MoveIt 里，这件事主要由 OMPL（Open Motion Planning Library）来干。

我个人习惯把 OMPL 比作一个工具箱。里面扳手、螺丝刀、锤子都有，但你不能拿锤子去拧螺丝。选对规划器，你的机器人就能“灵光一闪”；选错了，它可能在那发呆半天，或者给你一条扭成麻花的路径。

    核心观点： 没有最好的规划器，只有最合适的规划器。理解它们的脾气秉性，比死记硬背参数更重要。

4.1 OMPL 规划器家族：RRT、PRM、STOMP

OMPL 里规划器多得让人眼花缭乱。但别慌，真正常用的就那几类。我挑三个最有代表性的聊聊。

4.1.1 RRT（快速扩展随机树）

RRT 的思路很简单：从起点开始，随机乱长树枝，直到某根树枝碰到终点。你想想看，这就像在黑暗里瞎摸，但摸得足够快，总能摸到开关。

优点： 快！尤其在高维空间里，它比很多传统方法都快得多。
缺点： 路径通常很“糙”，拐来拐去的，不够平滑。
变种： RRT-Connect（双向生长，更快）、RRT*（渐进最优，慢慢优化）。

我的经验： 做抓取规划时，我习惯先用 RRT-Connect 快速找一条可行路径。如果时间充裕，再用 RRT* 去优化它。别一上来就用 RRT*，它收敛起来挺慢的。

4.1.2 PRM（概率路线图）

PRM 的做法是“先修路，再导航”。它在空间里随机撒点，把能连起来的点连成一张图。之后查路径，就是在图上找最短路线。

优点： 适合多次查询的场景。图建好了，查路径就是一瞬间的事。
缺点： 建图本身挺耗时。而且如果环境变了，图就废了，得重建。

我记得有个项目是做仓库机器人，环境基本固定。我就用 PRM 预计算好路线图，运行时直接查，响应速度非常快。

4.1.3 STOMP（随机轨迹优化运动规划）

STOMP 跟上面两位不太一样。它不是随机采样，而是拿一条初始轨迹，通过“随机扰动+代价评估”的方式，反复迭代优化。

优点： 生成的路径非常平滑，适合对运动质量要求高的场景。
缺点： 容易陷入局部最优。如果初始轨迹太离谱，它可能优化不出好结果。

注意： STOMP 对参数很敏感。噪声标准差、迭代次数这些，调不好反而会越优化越差。我曾经在一个焊接项目里，STOMP 把一条本来还行的路径优化成了“蛇形走位”……后来发现是噪声设太大了。

4.2 规划器参数配置

选好了规划器，还得给它“喂”对参数。不然再好的算法也白搭。这里说三个最关键的。

参数名	作用	推荐范围	我的建议
规划时间	允许规划器思考的最长时间	0.5s ~ 5.0s	实时场景设短点，离线规划可以放宽
尝试次数	规划失败后重试的次数	5 ~ 20 次	别设太多，失败3次就该换个思路了
路径平滑度	对路径拐弯的惩罚权重	0.1 ~ 1.0	设太高路径会“僵住”，设太低又太“浪”

举个例子，在 MoveIt 的 ompl_planning.yaml 里，你可以这样配：

planner_configs:
  RRTConnectkConfigDefault:
    type: geometric::RRTConnect
    range: 0.0  # 扩展步长，0表示自动
    goal_bias: 0.05  # 偏向目标的概率
    timeout: 1.0  # 规划超时（秒）
    attempts: 10  # 最大尝试次数

避坑指南： 我曾经把 goal_bias 设到 0.5，想着“快点到终点”。结果 RRT 疯狂往终点方向长，反而因为太“急”而频繁撞墙。后来老老实实改回 0.05，反而更快找到路径。

4.3 自定义规划器

有时候 OMPL 自带的规划器满足不了你。比如你想用自己写的某种奇奇怪怪的算法。MoveIt 支持通过插件机制集成自定义规划器。

怎么做？核心三步：

实现规划接口： 继承 ompl::base::Planner，实现 solve() 方法。
注册为插件： 用 PLUGINLIB_EXPORT_CLASS 宏导出。
配置 YAML： 在规划器配置里引用你的插件名。

代码骨架大概长这样：

#include <ompl/base/Planner.h>

namespace my_namespace {
    class MyCustomPlanner : public ompl::base::Planner {
    public:
        MyCustomPlanner(const ompl::base::SpaceInformationPtr &si, const std::string &name = "MyPlanner")
            : ompl::base::Planner(si, name) {}
        
        ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override {
            // 在这里写你的规划逻辑
            // 返回 SUCCESS 或 FAIL
        }
    };
}

// 注册插件
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(my_namespace::MyCustomPlanner, ompl::base::Planner);

警告： 自定义规划器不是闹着玩的。你得确保你的算法在 solve() 里能响应终止条件（PlannerTerminationCondition），否则 MoveIt 可能永远等不到结果。我见过有人写了个死循环，机器人直接“死机”了。

嗯，规划器这块内容不少，但核心就这些。选对规划器、调好参数、必要时自己写一个。做到这三点，你的机器人基本就能“想走就走，想停就停”了。