引言

机械手臂的轨迹规划就是说当手臂执行某一个操作时，比如说，从一个位置取某一物件然后移动到另一个位置，这段运动轨迹就需要我们来规划，规划的内容就包括位置、速度以及加速度随时间的变化。通常我们理想的轨迹都是平滑的曲线，其中包括起始点{$T_{initial}$}，很多的中间点{$T_{t_i}$}以及终点{$T_{final}$}。如果轨迹变化很大的话，那么在变化的那个点就需要手臂有很大的torque来改变运动状态，有时手臂可能做不到，而且会让整个运动过程看起来不流畅。

轨迹规划

轨迹的规划方式有两种，一种是在joint-space下进行，一种是在Cartesian-space下进行。接下来将分别介绍两种方法，以及它们的优缺点。

Joint-space下的轨迹规划

在Joint-space下规划轨迹的方法如下：

定义这段轨迹在空间中的起始点{$T_{initial}$}，中间点{$T_{t_i}$}以及终点{$T_{final}$}。
计算这些点的IK，得到在每个时间点$t_i$，手臂的每个joint所对应的角度。
对每一个joint都在其自己的角度坐标系下规划一条平滑的曲线。
计算FK，检查手臂末端点在Cartesian-space下轨迹的可行性（可能会碰到障碍物）。

具体过程如下图：

Note：
优点：计算量小，只需要计算起始点、中间点和终点的IK。
缺点：在空间中的运动轨迹不直观。

Cartesian-space下的轨迹规划

在Cartesian-space下规划轨迹的方法如下：

定义这段轨迹在空间中的起始点{$T_{initial}$}，中间点{$T_{t_i}$}以及终点{$T_{final}$}。
对每一个手臂末端点的状态参数（$x,y,z,\theta_x,\theta_y,\theta_z$）规划一条平滑的曲线。
将规划好的手臂末端点状态的轨迹点计算IK，转换到joint-space下。
检查joint在joint-space下轨迹的可行性（有的joint的转动角度有限制或者角加速度太大手臂做不到）。

具体过程如下图：

Note：
优点：在空间中的运动轨迹比较直观。
缺点：计算量大，需要计算整个轨迹点的IK。

轨迹规划曲线—三次多项式

轨迹规划中平滑的曲线需要写成函数的形式，这样我们才能得到不同时刻的值。在这里我们用三次多项式（Cubic Polynomials）的形式来规划路径，也就是说，每两个中间点之间可以写成三次多项式的函数形式，不同轨迹区段[$t_i$ $t_{i+1}$]是不同的三次多项式。然后为了使整体的路径规划平滑，我们需要给相邻的两个区段增加限制条件，包括位置和速度（$\theta(t_i)$，$\theta(t_{i+1})$，$\dot\theta(t_i)$，$\dot\theta(t_{i+1})$）。

解三次多项式

三次多项式的通式：
$$
\theta(\tilde t)=a_0+a_1\tilde t+a_2\tilde t^2+a_3\tilde t^3, 4\space unknown\space para: a_j \space _{j=0…3}
$$
每一个区段[$t_i$ $t_{i+1}$]都有一个这样的三次多项式，只不过每段的参数不同。
$\tilde t$的取值：
对每一个区段：$t\in[t_i,t_{i+1}]$
$\tilde t=t-t_i$，所以$\tilde t|_{t=t_i}=0$
增加边界条件：
首先当$t=t_i$和$t=t_{i+1}$时，这个三次多项式函数需要落在$\theta_i$和$\theta_{i+1}$上，所以有：
$$
\begin{aligned}
&\theta(\tilde t|_{t=t_i})=\theta_i=\color{red}{a_0} \\
&\theta(\tilde t|_{t=t_{i+1} })=\theta_{i+1}=a_0+a_1\Delta t+a_2\Delta t^2+a_3\Delta t^3 \\
\end{aligned}
$$
然后这个三次多项式函数在$t=t_i$和$t=t_{i+1}$时的速度也需要我们来设计，所以有：
$$
\begin{aligned}
&\dot \theta(\tilde t|_{t=t_i})=\dot \theta_i=\color{red}{a_1} \\
&\dot \theta(\tilde t|_{t=t_{i+1} })=\dot \theta_{i+1}=a_1+2a_2\Delta t+3a_4\Delta t^2 \\
\end{aligned}
$$
联立上面4个方程式最后解出：
$$
\begin{aligned}
&\color{red}{a_2} = \frac{3}{\Delta t^2}(\theta_{i+1}-\theta_i)-\frac{2}{\Delta t}\dot \theta_i-\frac{1}{\Delta t}\dot \theta_{i+1} \\
&\color{red}{a_3} = -\frac{2}{\Delta t^3}(\theta_{i+1}-\theta_i)+\frac{1}{\Delta t^2}(\dot \theta_{i+1}+\dot \theta_i)
\end{aligned}
$$
用Matrix的方式计算：