版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
一、运动规划算法简述
实现流程
通过给定的轨迹点，根据设定的最大速度和加速度计算每个点的速度，加速度和时间帧。
- 轨迹点：可通过插补获得，数据类型为
moveit_msgs::RobotTrajectory - 设定的最大速度和加速度：为
URDF文件中设定参数
规划器
当前ROS提供三种规划器：
- Time-optimal Trajectory Parameterization
- Iterative Spline Parameterization
- Iterative Parabolic Time Parameterization
分析
在规划固定轨迹时(直线，圆弧)，规划获得速度和加速度一般会有抖动，特别是在精确的规划下，因此在对轨迹要求不高的情况下，使用样条曲线(Spline Curves)拟合轨迹可以获得较好的运动规划结果，若需要精确的固定轨迹规划，建议降低速度。
应用
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group->getCurrentState()->getRobotModel(), "hand");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group->getCurrentState(), trajectory_msg);
// Thrid create a iterative time
trajectory_processing::IterativeParabolicTimeParameterization iptp;// 五次样条曲线插补
trajectory_processing::IterativeSplineParameterization isp;// 三次样条曲线
trajectory_processing::TimeOptimalTrajectoryGeneration totg(0.005,0.005);//时间优化
// Fourth compute computeTimeStamps
// bool ItSuccess = iptp.computeTimeStamps(rt);
// bool ItSuccess = isp.computeTimeStamps(rt);
bool ItSuccess = totg.computeTimeStamps(rt);
OS_INFO("Computed time stamp %s", ItSuccess ? "SUCCEDED" : "FAILED");
// Get RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory_msg);
二、Time-optimal Trajectory Parameterization
算法原理：Time-Optimal Path Following (July 2012)
注意：该规划算法在melodic版本以上才整合到moveit中
分析
- 该规划器在小间距(<0.01m)下规划的速度和加速度较其余两种优化算法更加合理与平滑，在较大间距(>0.01m)下三种规划算法效果差距不大，规划间距会因机械臂的不同而有所不同。
- 在过小间距下(0.001m)插补，会出现运动规划不恒定，规划速度不平滑的现象，经测试在0.005m的规划间距下可以有较为稳定和理想的规划效果。
- 该算法会对轨迹插补点进行优化，基本规律为两头密，中间疏，因此会导致插补的间隔会变得无法精确控制，若需要精确控制轨迹，建议采用其余两种算法。
参数设置
TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1,
const double resample_dt = 0.1);
path_tolerance，轨迹宽容度，允许轨迹相对实际轨迹的误差量，单位m。resample_dt，轨迹重新插点步距，但因两头密，中间疏，实际间距并不衡定，单位m。
减少轨迹宽容度和步距间隔会导致速度和加速度出现抖动，应根据实际情况调整。
低版本(kinetic)使用
直接将time_optimal_trajectory_generation.cpp和time_optimal_trajectory_generation.h放到workspcae中调用。
修改
std::make_unique<LinearPathSegment>(start_config, end_config)//c++14
为
std::unique_ptr<LinearPathSegment>(new LinearPathSegment(start_config, end_config))//c++11
若编译器支持c++14无需修改
实现效果
- 直线插补-步距1mm，精度1mm
注意：该间距下规划所得结果不恒定
- 操作空间
- 关节空间
- 直线插补-步距10mm，精度10mm
- 操作空间
- 关节空间
- 直线插补-步距100mm，精度100mm
- 操作空间
- 关节空间
三、Iterative Spline Parameterization
实现效果
- 直线插补-1mm
- 操作空间
- 关节空间
- 直线插补-10mm
- 操作空间
- 关节空间
- 直线插补-100mm
- 操作空间
- 关节空间
四、Iterative Parabolic Time Parameterization
分析
- 该运动规划器为moveit默认使用规划器，可以实现速度和加速度平滑，但无法避免加速度的抖动。
- 该规划器可插补间距，平滑速度和加速度，适合用在低速精确轨迹控制下。
实现效果
- 直线插补-1mm
- 操作空间
- 关节空间
- 直线插补-10mm
- 操作空间
- 关节空间
- 直线插补-100mm
- 操作空间
- 关节空间
参考
https://github.com/ros-planning/moveit/pull/1365
https://github.com/ros-planning/moveit/tree/melodic-devel/moveit_core/trajectory_processing/include/moveit/trajectory_processing