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

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

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

Plan Request Adapters 有两个主要作用：

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

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

阶段	Plan Request Adapters	作用	使用场景
pre-processing	FixStartStateBounds	将机器人初始状态限制在 URDF 中定义的关节约束区间内	机器人可能处于一个或者多个关节略微超出关节限制的情况，此时， Motion Planner 将认为起始状态在关节范围之外而无法规划，通常用于超出边界并不严重的情况
pre-processing	FixWorkspaceBounds	指定一个默认大小的工作空间
pre-processing	FixStartStateCollision	通过少量扰动关节值来在指定配置（碰撞中）附近对新的无碰撞配置进行采样。它将扰动值的量由“jiggle_fraction”参数指定，该参数将扰动控制为关节总运动范围的百分比。
pre-processing	FixStartStatePathConstraints	尝试在机器人的当前配置与遵循路径约束的新位置之间规划路径。新位置将作为规划的起始状态。	机器人的初始姿态不满足路径约束
post-processing	AddTimeParameterization	motion planners 生成的是一条即和路径，该路径不遵循速度、加速度约束，也不是时间参数化的。该 adapter 可以为这条空间轨迹进行速度、加速度约束，为每个轨迹点加入速度、加速度、时间等参数。	说白了就是轨迹规划
pre-processing	ResolveConstraintFrames	Goal constraints can be set using subframes (e.g. a pose goal in the frame cup/handle, where handle is a subframe on the object cup). This adapter changes the frame of constraints to an object or robot frame (e.g. cup).	坐标约束转换

研究轨迹规划，我当然更关心 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 中设置选用即可。

规划算法	调用形式
Iterative Parabolic Time Parameterization	AddTimeParameterization
Iterative Spline Parameterization	AddIterativeSplineParameterization
Time-optimal Trajectory Generation	AddTimeOptimalParameterization

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

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