4. 规划器性能调优：OMPL参数调优与自定义集成

规划器选型与参数调优，是MoveIt性能优化中最立竿见影的一环。说白了，就是让机器人“动脑子”的方式更聪明一些。我见过不少团队，花了大把时间调PID、改硬件，结果换个规划器参数，运动时间直接砍半。嗯，这章我们就来聊聊怎么榨干OMPL的性能。

4.1 OMPL规划器家族：选对工具做对事

OMPL里规划器多得让人眼花缭乱。但实际工程中，常用的就那几类。我个人习惯把它们分成三派：

RRT派：快速探索随机树。适合纯几何避障，速度快，但路径质量一般。
RRT*派：带优化的RRT。路径更平滑，但收敛慢。适合对路径质量有要求的场景。
PRM派：概率路线图。适合静态环境，预处理一次，多次查询。

你想想看，如果每次规划环境都在变，PRM就不太合适了。反过来，如果机械臂每天干同样的活，PRM的预处理优势就出来了。

核心观点：没有最好的规划器，只有最合适的。选型前先问自己三个问题：环境是否静态？路径质量要求多高？规划时间预算多少？

4.2 RRT与RRT*：参数调优实战

RRT的参数不多，但每个都很关键。我调过最狠的一次，把规划时间从8秒压到了1.2秒。怎么做到的？看下面这张表：

参数名	默认值	推荐范围	调优说明
range	0.1	0.05 - 0.5	步长。太小则探索慢，太大容易碰撞。我习惯先设0.3，看碰撞率再调。
goal_bias	0.05	0.05 - 0.3	向目标采样的概率。设太高容易陷入局部最优。我曾经设到0.4，结果路径绕了远路。
max_planning_time	1.0	0.5 - 10.0	最大规划时间。别设太死，给规划器留点余地。

RRT*多了个优化环节，所以多了个rewire_factor参数。这个参数控制重连的范围。默认1.0，但如果你发现路径抖动厉害，可以试着降到0.5。嗯，这里要注意：RRT*的收敛速度跟迭代次数强相关，别指望短时间能出完美路径。

避坑指南：我曾经在RRT*上设了5秒规划时间，结果路径质量还不如RRT跑1秒。后来发现是goal_bias设得太低，树一直往错误方向长。调高到0.15后，效果立竿见影。

4.3 PRM：预处理的艺术

PRM的核心在于“建图”和“查询”分离。建图阶段花时间，查询阶段就快。参数调优主要集中在这两个阶段：

num_samples：采样点数量。默认100，复杂环境建议500-2000。但别贪多，我见过有人设到5000，建图花了半分钟，得不偿失。
num_nearest_neighbors：近邻数。默认10，调高到20-30能提高路径质量，但建图时间会涨。
max_nearest_neighbors：最大近邻数。限制一下，防止高维空间里近邻搜索爆炸。

我个人习惯是先跑一次PRM建图，保存下来。如果环境不变，后续直接加载，省掉建图时间。这在产线场景里特别实用。

4.4 规划时间与成功率的平衡

这是个老生常谈的问题。规划时间越长，成功率越高，但用户等不了。怎么平衡？我总结了一个经验公式：

成功率 ≈ 1 - e^(-k * t)

其中t是规划时间，k是环境复杂度系数。说白了，规划时间翻倍，成功率不会翻倍，而是逐渐饱和。所以没必要死磕那最后5%的成功率。

警告：别把max_planning_time设成无穷大。我见过一个案例，规划器卡了30秒才找到路径，机器人早就该干活了。实际工程中，建议设2-3秒，如果失败，降级到更简单的规划器或走安全轨迹。

我常用的策略是“多级规划”：先给RRT 0.5秒，失败后给RRT* 2秒，再失败就切到PRM查询。这样既保证了实时性，又兼顾了成功率。

4.5 自定义规划器集成

有时候OMPL自带的规划器不够用。比如你有个特殊的约束条件，或者想用深度学习引导采样。这时候就需要集成自定义规划器。

集成步骤其实不复杂：

继承ompl::base::Planner类
实现solve()方法
注册到OMPL的规划器注册表
在MoveIt的ompl_planning.yaml里引用

下面是个简单的骨架代码：

#include <ompl/base/Planner.h>

class MyCustomPlanner : public ompl::base::Planner
{
public:
    MyCustomPlanner(const ompl::base::SpaceInformationPtr &si)
        : ompl::base::Planner(si, "MyCustomPlanner") {}

    ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override
    {
        // 你的规划逻辑
        // 记得检查ptc是否终止
        return ompl::base::PlannerStatus::EXACT_SOLUTION;
    }
};

然后在yaml里这样配：

planner_configs:
  MyCustomPlanner:
    type: geometric::MyCustomPlanner
    range: 0.2
    goal_bias: 0.1

个人经验：自定义规划器调试时，建议先写个简单的RRT变体，验证集成流程没问题，再上复杂算法。我曾经直接写了个基于神经网络的规划器，结果集成后一直崩溃，最后发现是内存泄漏。嗯，教训深刻。

4.6 知识体系总览

为了让你更直观地理解本章内容，我画了张图：

这张图把本章的核心脉络串起来了。从左到右，从上到下，分别是RRT系列、PRM、平衡策略和自定义集成。你可以在实际项目中对照这张图，一步步排查性能瓶颈。

好了，规划器调优这块就聊到这儿。记住，参数调优不是玄学，是有章可循的工程实践。多试几次，找到适合你场景的那组参数，比什么都强。