基于多项式的轨迹插补算法控制机械臂

REF：工业机械臂的轨迹规划插补系统设计

1. 五次多项式插补算法

五次多项式及其一、二阶微分方程表达式如下：

• S( t) = At5 + Bt4 + Ct3 + Dt2 + Et + F
• S'( t) = 5At4 + 4Bt3 + 3Ct2 + 2Dt + E
• S″( t) = 20At3 + 12Bt2 + 6Ct + 2D

在已知插补总时间 T 时，得出多项式常量如下：

2. 轨迹插补架构

对用户命令进行解析以及周期的输出插补点，用户通过WebSocket添加的轨迹进行周期的插补，同时周期地输出各关节的变量，最后由底层的伺服驱动器对各关节进行驱动。

3. 插补工作流程

通过轨迹队列实现各段轨迹的存取，轨迹队列包含两个位置指针，分别表示最后一段轨迹存放的位置以及当前轨迹的存放位置，当这两个指针位置重合时则运动结束。轨迹队列中的任一成员包含所有该段轨迹的运动参数以及描述参数。使用运动参数进行速度插补，而后通过轨迹映射，实时的将当前位移占总路程百分比通过描述参数转化为对应轨迹描述的变量，如样条曲线的节点矢量 u、圆弧的转角等。通过得到的实时变量，就可以求出当前点的笛卡尔位姿，最后进行机械臂逆运动学求解即可得到一个周期的关节变量。

4. 单个插补子模块

基于起点、终点以及该段运动的运动参数，子程序通过始末点计算出总路程以及运动方向，其中总路程和运动参数作为速度插补参数，起点和运动方向则最为轨迹映射的参数。

5. 实际插补验证

机械臂运动时，多项式插补可以确保执行器加速度的连续可导，保证了插补作业的质量，在多段轨迹连续的情况下，通过设置首末速度，提高了插补效率。

在笛卡尔空间设置多个路点，分别采用梯形速度插补算法以及本文的五次多项式插补算法进行插补，得到的位移、速度、加速度和加加速度，采用五次多项式进行速度插补可以有效控制冲击度，插补质量高，如下图