使用 MoveIt 控制自己的真实机械臂【4】——了解 MoveIt 的轨迹规划实现机制

上一节中我们提到了一个planning request adapters 的概念，在这一节中我们将对其展开详细了解。

planning request adapters 是运动规划中 Motion Planning Pipline 涉及到的概念，如下图所示，Plan Request Adapters 和 Motion planners 共同形成了 Motion Planning Pipline。

Plan Request Adapters 有两个主要作用：

• 预处理运动规划请求；
• 后处理运动规划响应；

以下是 MoveIt 提供的几种默认的运动规划适配器：

研究轨迹规划，我当然更关心 AddTimeParameterization，官方文档参考：Time Parameterization Algorithms
得到 Motion Planners 规划之后的几何路径之后，MoveIt 可以支持不同的算法来对运动轨迹进行后处理，以添加时间戳和速度/加速度值。目前在 MoveIt 中默认提供三种：

Iterative Parabolic Time Parameterization（IPTP）

MoveIt 中 IPTP 源码实现
IPTP 算法是 Motion Planning Pipline 中的 默认 Planning Request Adapter。
关于该算法的相关论文资料尚未找到，欢迎了解的朋友评论区留言。
特点：

• 1、为等距插补，可以实现速度和加速度平滑，但是无法避免加速度抖动，参见讨论 Improve time parameterization。
• 2、尽管 MoveIt 使用 IPTP 算法多年来已被数百个机器人使用，但它依存在已知 Bug。

Iterative Spline Parameterization（ISP）

MoveIt 中 ISP 源码实现
没有相关论文，在 MoveIt 源码头文件中找到这样一段算法描述：

• This class sets the timestamps of a trajectory to enforce velocity, acceleration constraints. Initial/final velocities and accelerations may be specified in the trajectory. Velocity and acceleration limits are specified in the model.
• This algorithm repeatedly fits a cubic spline, adjusts the timing intervals, and repeats until all constraints are satisfied. When finished, each trajectory waypoint will have the time set, as well as the velocities and accelerations for each joint. Since we fit to a cubic spline, the position, velocity, and acceleration will be continuous and within bounds. The jerk will be discontinuous.
• To match the velocity and acceleration at the endpoints, the second and second-last point locations need to move. By default, two extra points are added to leave the original trajectory unaffected. If points are not added, the trajectory could potentially be faster, but the 2nd and 2nd-last points should be re-checked for collisions.
• Migration notes: If migrating from Iterative Parabolic Time Parameterization, be aware that the velocity and acceleration limits are more strictly enforced using this technique. This means that time-parameterizing the same trajectory with the same velocity and acceleration limits, will result in a longer trajectory. If this is a problem, try retuning (increasing) the limits.

ISP 算法为等距插补，是针对 IPTP 算法的改进，详细讨论参见 Improved IPTP by fitting a cubic spline

Time-optimal Trajectory Generation（TOTG）

MoveIt 中 TOTG 源码实现
TOTG 论文及源码开源地址
该方法在 Melodic 版本之后才整合到 MoveIt 中，生成的轨迹速度非常平滑连续，具有非常平滑和连续速度分布的轨迹。该方法基于将路径段拟合到原始轨迹，然后从优化路径中采样新的路点。这与严格的时间参数化方法不同，因为生成的航路点可能会在一定的容差内偏离原始轨迹。因此，使用此方法时可能需要额外的碰撞检查。
特点：

• 1、输出轨迹为等时间间距轨迹，这与实际工业需求相符，时间间隔可以在源码中进行设置，其默认值为 0.1s；
• 2、该规划器在小间距下规划的速度和加速度较其余两种优化算法更加合理与平滑，在较大间距下三种规划算法效果差距不大，良好的规划间距会因机械臂参数的不同而有所不同；
• 3、在过小间距下(0.001s)插补，会出现运动规划不恒定，规划速度不平滑的现象，经测试在0.005s（不同机械臂不一致，同时需要根据控制器输出频率等确定）的规划间距下可以有较为稳定和理想的规划效果。

以上三种默认 Adapters 在 ompl_planning_pipeline.launch.xml 中设置选用即可。

以上三种算法实现效果的简单对比：Add time-optimal trajectory parameterization

参考文章：
知乎：时间最优轨迹（资料）
CSDN ROS进阶——运动规划分析
机器人运动规划技术介绍