六轴机器人轨迹规划之五次多项式插值

1.轨迹规划的定义
轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补，在起始点和终止点之间插入中间点序列，实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划，路径规划是规划位置，在起终点之间经过的路径点，轨迹规划是规划时间，将路径点与时间相对应。
对于我们的六轴机器人而言轨迹规划可以分为：关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数，然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。
五次多项式插值能够解决三次多项式插值的角速度变化不平滑且加速度存在跳变的情况。
2.数学基础
五次多项式插值，角位移、角速度和角加速度的函数表达式为：

$$\begin{eqnarray*} \left\{ \begin{array}{l} \theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5\\ \dot{\theta}(t)=a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4\\ \ddot{\theta}(t)=2a_2+6a_3t+12a_4t^2+20a_5t^3 \end{array}\right. \end{eqnarray*}$$
约束条件如下，相对三次多项式插值，增加了对起止点角速度的约束（为简便计算设 $t_0$为0）

⎧⎩</