1.现状分析

机械手在运动时，要尽可能平稳、少冲击，显得丝滑。
Moveit给到的轨迹，除了描述了各个关键时刻的各个关节的位置，还描述了此时的各个关节的速度、加速度。假如我们的执行机构可以完全按照这些位置、速度、加速度来进行操作，理论上就可以实现平滑的移动。
但是由于我们目前用的是舵机，除了位置可控外，速度、加速度都不能直接控制。
所以，只能通过插值来插补两个位置之间的各个小位置，让运动大概实现【加速-匀速-减速 】的过程。
舵机的转动速度会因为负载的不同而不同，不能够简单认为是一个固定值。因此，想要间接通过设置定位的方式来设定速度，可行性也不太。
假如我们让舵机从位置a走到位置b，等待时间为T1。舵机实际从a走到b的时间为T2。
那么只有T1大于等于T2才能确保舵机运动到位。

2.委曲求全

由于测算舵机实际的运算速度太麻烦了。
我们先进行固定时间步进的方式算了。
从ros上可以收到各个点位的信息，以及时间戳。因此可以根据时间戳，把位置均分一下。然后再发送给下位机。
实现的代码如下，仅供参考

void MainWindow::processGoal(std::shared_ptr<const FollowJointTrajectory::Goal> goal)
{// 该轨迹分成若干个点位（每个点位都包括了此点位下，若干关节的角度信息）int pointSize = goal->trajectory.points.size();if(pointSize <= 0){return;}// 关于time_from_start，// 第一个点位的time_from_start为0,后面的每个时间都是与第一个位置时间做的偏移。// 直观地理解，该轨迹要求在各个时间点，机械手要在要求的位置// 我们先人为地规定，timeStep(ms)更新一次位置给下位机int timeStep = 50;         // 更新频率// 先不进行加速、匀速、减速的操作。现在直接均分。// 目前是按照时间来插值// 存放移动数据的队列static QQueue<QList<float>> angleQueue;angleQueue.clear();QList<float> angleList;for(int i = 0; i < pointSize; i++){// 当前点位auto curPoint = goal->trajectory.points.at(i);if(i == 0) // 第一个是起始位置，直接放进去进行{angleList.clear();foreach(auto pos, curPoint.positions){angleList << qRadiansToDegrees(pos);}angleQueue << angleList;continue;}auto lastPoint = goal->trajectory.points.at(i - 1);auto last_time = lastPoint.time_from_start;auto current_time = curPoint.time_from_start;rclcpp::Time time1(last_time.sec, last_time.nanosec);rclcpp::Time time2(current_time.sec, current_time.nanosec);auto duration_time = time2 - time1;// 从上一个点位跑到此处计划耗时qint64 duration_ms = duration_time.nanoseconds() / 1e6;qDebug() << "[sec:" << current_time.sec << ", nanosec:" << current_time.nanosec << "]" << duration_time.seconds() << duration_time.nanoseconds() ;qDebug() << "duration ms:" << duration_ms;if(duration_ms < timeStep) // 小于时间步进，无需插值{angleList.clear();foreach(auto pos, lastPoint.positions){angleList << qRadiansToDegrees(pos);}angleQueue << angleList;}else // 需要插值{// 需要分几步进行插值float divCount = duration_ms / timeStep;if(duration_ms % timeStep){divCount++;}// 插值步进QList<float> stepList;for(int idx = 0; idx < lastPoint.positions.size(); idx++){stepList << (curPoint.positions[idx] - lastPoint.positions[idx]) / divCount;}// 均分插值for(int divIdx = 0; divIdx < divCount; divIdx++){angleList.clear();for(int idx = 0; idx < lastPoint.positions.size(); idx++){angleList << qRadiansToDegrees(lastPoint.positions[idx] + stepList[idx] * (divIdx + 1));}angleQueue << angleList;}}}qDebug() << angleQueue;// 算出了整个移动队列，通过定时器定时发送该数据给单片机//（也可以把整个移动数据发送至单片机，然后单片机再在自己内部慢慢跑）QTimer *timer = new QTimer();timer->setInterval(timeStep);connect(timer, &QTimer::timeout, [=](){if(angleQueue.length() == 0){timer->stop();timer->deleteLater();return;}QList<float> angleList = angleQueue.dequeue();// 转动方向修正(根据实际情况)angleList[0] = -angleList[0];angleList[1] = -angleList[1];angleList[3] = -angleList[3];sendPose(angleList);});timer->start();}

3.实际效果

不好。还是晃得厉害，有时候甚至还不如原来的不插值的。下面视频的是用原来的没有插值的。
【Ros2中使用MoveIt进行虚拟避障】

参考资料
http://www.51hei.com/mcu/2548.html
https://www.elecfans.com/d/608940.html