机器人轨迹规划——三次样条轨迹2（手把手推导基于加速度的求解方法）

• 点到点轨迹（Point To Point Trajectory）
  • 三次多项式插值轨迹
  • 五次多项式插值轨迹
  • 梯形速度曲线轨迹
  • 双S形速度曲线轨迹

---

三次样条轨迹

建议先阅读 机器人轨迹规划——三次样条轨迹1（基于速度的求解方法），这两篇文章中有一些思想是共通的，这一篇写得更加详细一点。

定义

当有 n + 1 个点时，可以使用 n 个次数为 p 的多项式（通常次数较低），每个多项式定义轨迹的一段。以这种方式定义的整体函数 $s(t)$ 称为次数为 p 的样条曲线。p 的值根据所需的样条曲线的连续性程度来选择。例如，为了在时间瞬间 $t_k$ 处获得速度和加速度的连续性，其中发生连续两段之间的过渡，可以假设次数为 $p=3$ 的多项式（三次多项式）。

三次样条曲线的表达式如下所示：

s ( t ) = { q k ( t ) , t ∈ [ t k , t k + 1 ] , k = 0 , … , n ? 1 } q k ( t ) = a k 0 + a k 1 ( t ? t k ) + a k 2 ( t ? t k ) 2 + a k 3 ( t ? t k ) 3 q ˙ k ( t ) = a k 1 + 2 a k 2 ( t ? t k ) + 3 a k 3 ( t ? t k ) 2 q ¨ k ( t ) = 2 a k 2 + 6 a k 3 ( t ? t k ) (1) \begin{aligned} s(t)&=\{q_k(t),t \in [t_k,t_{k+1}], k=0,\ldots,n-1\} \\ q_k(t) &=a_{k0}+a_{k1}(t-t_k)+a_{k2}(t-t_k)^2+a_{k3}(t-t_k)^3\\ \dot q_k(t) &= a_{k1} + 2a_{k2}(t-t_k)+3a_{k3}(t-t_k)^2 \\ \ddot q_k(t) &= 2a_{k2} + 6a_{k3}(t-t_k)\\ \end{aligned} \tag{1} s(t)qk?(t)q˙?k?(t)q¨?k?(t)?={qk?(t),t∈[tk?,tk+1?],k=0,…,n?1}=ak0?+ak1?(t?tk?)+ak2?(t?tk?)2+ak3?(t?tk?)3=ak1?+2ak2?(t?tk?)+3ak3?(t?tk?)2=2ak2?+6ak3?(t?tk?)?(1)

另外三次多项式可以通过逐次积分的方式，若已知每段多项式起始时刻的$q_k,v_k,w_k,j_k$，则

$$\begin{array}{rl}{\ddot{q}}_k(t)& =w_k+j_k(t?t_k)\\ {\dot{q}}_k(t)& =v_k+w_k(t?t_k)+\frac{1}{2}{j}_k(t?t_k{)}^2\\ q_k(t)& =q_k+v_k(t?t_k)+\frac{1}{2}{w}_k(t?t_k{)}^2+\frac{1}{6}{j}_{t_k}(t?t_k{)}^3\end{array}\text{\hspace{1em}(1-1)}$$
其中：
$$\begin{array}{rl}{q}_k& :在t_k点的位置\\ v_k& :在t_k点的速度\\ w_k& :在t_k点的加速度\\ j_k& :在[t_k,t_{k+1})区间内的加加速度，在该区间内是常数\end{array}$$

可以看出来，三次样条曲线的速度曲线是二次多项式，加速度曲线是一次多项式，加加速度在每个阶段是恒定的。同时我们可以得到多项式的系数和端点位置、速度、加速度的关系，可以参考 机器人轨迹规划—三次多项式插值轨迹。知道每段多项式端点的位置、速度可以得到：
$$\begin{array}{rl}& q_k(t_k)=q_k=a_{k0}\\ & q_k(t_{k+1})=q_{k+1}=a_{k0}+a_{k1}{T}_k+a_{k2}{T}_k^2+a_{k3}{T}_k^3\\ & {\dot{q}}_k(t_k)=v_k=a_{k1}\\ & {\dot{q}}_k(t_{k+1})=v_{k+1}=a_{k1}+2a_{k2}{T}_k+3a_{k3}{T}_k^2\\ & \\ ?& ?\begin{array}{rl}{a}_{k0}& =q_k\\ a_{k1}& =v_k\\ a_{k2}& =\frac{1}{T_k}\left[\frac{3(q_{k+1}?q_k)}{T_k}?2v_k?v_{k+1}\right]\\ a_{k3}& =\frac{1}{T_k^2}\left[\frac{2(q_k?q_{k+1})}{T_k}+v_k+v_{k+1}\right]\end{array}k=0,\dots ,n?1\end{array}\text{\hspace{1em}(1-2)}$$
从（1-2）四个约束求解出四个多项式系数，可以得到多项式系数和端点位置、速度的关系，其中 $T_k=t_{k+1}?t_k$。
同理：

$$\begin{array}{rl}& q_k(t_k)=q_k=a_{k0}\\ & q_k(t_{k+1})=q_{k+1}=a_{k0}+a_{k1}{T}_k+a_{k2}{T}_k^2+a_{k3}{T}_k^3\\ & {\ddot{q}}_k(t_k)=w_k=2a_{k2}\\ & {\ddot{q}}_k(t_{k+1})=w_{k+1}=2a_{k2}+6a_{k3}{T}_k\\ & \\ ?& ?\begin{array}{rl}{a}_{k0}& =q_k\\ a_{k1}& =\frac{q_{k+1}?q_k}{T_k}?\frac{T_k}{6}(w_{k+1}+2w_k)\\ a_{k2}& =\frac{w_k}{2}\\ a_{k3}& =\frac{w_{k+1}?w_k}{6T_k}\end{array}k=0,\dots ,n?1\end{array}\text{\hspace{1em}(1-3)}$$
从（1-3）可以得出多项式系数和端点位置、加速度的关系。

求解方法

为了计算样条曲线，需要求出每段多项式的4 个系数。由于定义通过 n + 1 个点的轨迹需要 n 个多项式，因此需要确定的总系数数量为 4n。为了解决这个问题，必须考虑以下条件：

• 2n 个条件用于插值给定的点： 因为每个三次函数必须在其端点处穿过这些点。
• n-1 个条件用于过渡点处速度（一阶导数）的连续性： 保证曲线在连接点处的速度平滑过渡。
• n-1 个条件用于过渡点处加速度（二阶导数）的连续性： 保证曲线在连接点处的加速度平滑过渡。

这样，一共就有 2n + 2(n-1) 个条件，因此剩余的自由度为 4n - 2n - 2(n-1) = 2。这里可以理解为一个条件就是一个线性等式，而有4n个未知数，要求出唯一解，就需要有4n个线性等式约束。为了计算样条曲线，必须再施加两个额外的约束条件。额外的条件通常有：

• 给定初始和结束的速度，$\dot{s}(t_0)=v_0$，$\dot{s}(t_n)=v_n$，这是我们重点关注的
• 给定初始和结束的加速度 $\ddot{s}(t_0),\ddot{s}(t_n)$，这种条件一般称为自然曲线（natural）
• 约束$\dot{s}(t_0)=\dot{s}(t_n),\ddot{s}(t_0)=\ddot{s}(t_n)$；这种条件称为循环曲线（cyclic），因为它会定义一个周期为 $T=t_n?t_0$的循环样条曲线
• 可以约束在$t_1,t_{n?1}$时刻的jerk的连续性：
  ${\frac{d^{3}s(t)}{d{t}^3}|}_{t=t_1^{?}}={\frac{d^{3}s(t)}{d{t}^3}|}_{t=t_1^{+}},{\frac{d^{3}s(t)}{d{t}^3}|}_{t=t_{n?1}^{?}}={\frac{d^{3}s(t)}{d{t}^3}|}_{t=t_{n?1}^{+}}$

给定边界点速度求解样条曲线

假设每一段三次多项式的端点加速度是：$\begin{array}{r}\ddot{q}(t_k)=w_k,k=0,\dots ,n\end{array}$，那么结合公式（1）和（1-3）可以得到

$$\begin{array}{c}{q}_k(t)=\frac{(t_{k+1}?t{)}^3}{6T_k}{w}_k+\frac{(t?t_k{)}^3}{6T_k}{w}_{k+1}+\left(\frac{q_{k+1}}{T_k}?\frac{T_k{w}_{k+1}}{6}\right)(t?t_k)+\\ +\left(\frac{q_k}{T_k}?\frac{T_k{w}_k}{6}\right)(t_{k+1}?t),t\in [t_k,t_{k+1}]\end{array}\text{\hspace{1em}(2)}$$

$$\begin{array}{rl}{\dot{q}}_k(t)& =\frac{(t?t_k{)}^2}{2T_k}{w}_{k+1}+\frac{(t_{k+1}?t{)}^2}{2T_k}{w}_k+\frac{q_{k+1}?q_k}{T_k}?\frac{T_k(w_{k+1}?w_k)}{6}\end{array}\text{\hspace{1em}(3)}$$

$${\ddot{q}}_k(t)=\frac{w_{k+1}(t?t_k)+w_k(t_{k+1}?t)}{T_k}\text{\hspace{1em}(4)}$$

公式（4）其实就是在$w_k,w_{k+1}$之间线性插值。

考虑中间点的速度和加速度连续性：

$$\begin{array}{rl}{\dot{q}}_{k?1}(t_k)& ={\dot{q}}_k(t_k)\end{array}\text{\hspace{1em}(5-1)}$$

$$\begin{array}{rl}{\ddot{q}}_{k?1}(t_k)& ={\ddot{q}}_k(t_k)=w_k\end{array}\text{\hspace{1em}(5-2)}$$

把（3）代入（5-1），合并（5-2），可以得到：

$$\frac{T_{k?1}}{T_k}{w}_{k?1}+\frac{2(T_k+T_{k?1})}{T_k}{w}_k+w_{k+1}=\frac{6}{T_k}\left(\frac{q_{k+1}?q_k}{T_k}?\frac{q_k?q_{k?1}}{T_{k?1}}\right),k=0,\dots ,n?1\text{\hspace{1em}(6)}$$

根据边界条件$\dot{s}(t_0)={\mathrm{v}}_0,\dot{s}(t_n)={\mathrm{v}}_n$，可以得到：

$$\begin{array}{c}\begin{array}{r}\frac{T_0^2}{3}{w}_0+\frac{T_0^2}{6}{w}_1\end{array}=q_1?q_0?T_0{\text{v}}_0\\ \begin{array}{r}\frac{T_{n?1}^2}{3}{w}_n+\frac{T_{n?1}^2}{6}{w}_{n?1}\end{array}=q_{n?1}?q_n+T_{n?1}{\text{v}}_n\end{array}\text{\hspace{1em}(7)}$$

把（6）、（7）写成矩阵格式：

$$\mathbf{Aw}=\mathbf{c}\text{\hspace{1em}(8)}$$

其中 $\mathbf{A}$是 $(n+1)\times (n+1)$ 三角对称方阵（tridiagonal and symmetric）：

$$\mathbf{A}=[\begin{array}{ccccccc}2T_0& T_0& 0& ?& & & 0\\ T_0& 2(T_0+T_1)& T_1& & & & ?\\ 0& & & ?& & & 0\\ ?& & & & T_{n?2}& 2(T_{n?2}+T_{n?1})& T_{n?1}\\ 0& & ?& & 0& T_{n?1}& 2T_{n?1}\end{array}]\text{\hspace{1em}(9)}$$

c是：

$$\mathbf{c}=\left[\begin{array}{c}6\left(\frac{q_1?q_0}{T_0}?{\mathrm{v}}_0\right)\\ 6\left(\frac{q_2?q_1}{T_1}?\frac{q_1?q_0}{T_0}\right)\\ ?\\ 6\left(\frac{q_n?q_{n?1}}{T_{n?1}}?\frac{q_{n?1}?q_{n?2}}{T_{n?2}}\right)\\ 6\left({\mathrm{v}}_n?\frac{q_n?q_{n?1}}{T_{n?1}}\right)\end{array}\right]\text{\hspace{1em}(10)}$$

根据（8）求解出所有多项式端点的加速度，就可以得到多项式的表达式，同时也可以求出多项式的系数：

求解等式（8）最直接的方法是$\mathbf{w}={\mathbf{A}}^{?1}\mathbf{c}$，但是这里由于矩阵A是三角对称矩阵，可以有一些方法避免求解逆矩阵，参考Thomas algorithm。

给定边界点速度和加速度求解样条曲线

以上我们可以给定样条轨迹边界点的速度来作为补充条件求解三次样条曲线，曲线两端点的速度可以设置为0，并且内部连续；曲线内部的加速度也是连续的，但是不能保证边界点的加速度是0，这就意味着会有一个加速度阶跃，影响轨迹的光滑性。那么如何能够同时定义边界点的速度和加速度呢？

• 在第一段和最后一段使用5次多项式（每段6个参数），可以多出来4个约束，用来定义起始和结束的加速度；一般这样会导致最大加速度、最大速度变得更大（over shoot）
• 在第一段和最后一段分别插入自由点，这两个自由点根据给定的初始和结束加速度计算

因为第一种方法的缺点，选择第二种方法，详细如下。
考虑需要被插值的 n-1 个点，可以表示为：

$$\mathbf{q}=[q_0,q_2,q_3,\dots q_{n?3},q_{n?2},q_n{]}^T\text{\hspace{1em}(11-1)}$$

对应的时间戳是：

$$\mathbf{t}={\left[\begin{array}{ccccccc}{t}_0,& t_2,& t_3,& \dots & t_{n?3},& t_{n?2},& t_n\end{array}\right]}^T\text{\hspace{1em}(11-2)}$$

注意上述公式中$q、t$的下标。

同时给定边界速度条件 $v_0,v_n$和边界加速度约束$a_0,a_n$。为了施加加速度边界约束，增加两个额外的自由点 ${\bar{q}}_1$和${\bar{q}}_{n?1}$，对应的时间戳分别是${\bar{t}}_1$和${\bar{t}}_{n?1}$，如下所示：

$$\bar{\mathbf{q}}={\left[\begin{array}{ccccccccc}{q}_0,& {\bar{q}}_1,& q_2,& q_3,& \dots & q_{n?3},& q_{n?2},& {\bar{q}}_{n?1},& q_n\end{array}\right]}^T\text{\hspace{1em}(12-1)}$$
$$\bar{\mathbf{t}}={\left[\begin{array}{ccccccccc}{t}_0,& {\bar{t}}_1,& t_2,& t_3,& \dots & t_{n?3},& t_{n?2},& {\bar{t}}_{n?1},& t_n\end{array}\right]}^T\text{\hspace{1em}(12-2)}$$

这个问题可以利用类似$(8)、(9)、(10)$的方法进行解决，但是这里面${\bar{q}}_1$和${\bar{q}}_{n?1}$是未知的，我们需要用已知的$q_0,q_n,v_0,v_n,a_0,a_n$以及$w_1,w_{n?1}$来表示它，根据公式（2），分别代入 $t={\bar{t}}_1$和$t={\bar{t}}_{n?1}$，得到：

$$q_1=q_0+T_0{\text{v}}_0+\frac{T_0^2}{3}{\text{a}}_0+\frac{T_0^2}{6}{w}_1\text{\hspace{1em}(12)}$$

$$q_{n?1}=q_n?T_{n?1}{\text{v}}_n+\frac{T_{n?1}^2}{3}{\text{a}}_n+\frac{T_{n?1}^2}{6}{w}_{n?1}\text{\hspace{1em}(13)}$$

因此我们可以构建出新的
$$\mathbf{Aw}=\mathbf{c}\text{\hspace{1em}(14-1)}$$

注意这里的$\mathbf{A}$的形状是$(n?1)\times (n?1)$，这是因为$w_0,w_n$是给定的边界条件（$w_0=a_0,w_n=a_n$），所以在替换公式（9）时，实际上第一行和最后一行是不需要的，同理$\mathbf{c}$也是，$\mathbf{w}$是$w_1,w_2,\dots ,w_{n?2},w_{n?1}$。
把（12）、（13）代入$(9)、(10)$之中，并进行化简得到：

$$\mathbf{A}=[\begin{array}{cccccccc}& 2T_1+T_0\left(3+\frac{T_0}{T_1}\right)& T_1& 0& ?& & & 0\\ & T_1?\frac{T_0^2}{T_1}& 2(T_1+T_2)& T_2& & & & ?\\ & 0& T_2& 2(T_2+T_3)& T_3& & & \\ & ?& & & ?& & & 0\\ & & & & & T_{n?3}& 2(T_{n?3}+T_{n?2})& T_{n?2}?\frac{T_{n?1}}{T_{n?2}}\\ & 0& ?& & & 0& T_{n?2}& 2T_{n?2}+T_{n?1}\left(3+\frac{T_{n?1}}{T_{n?2}}\right)\end{array}]\text{\hspace{1em}(14-2)}$$

$$\mathbf{c}=\left[\begin{array}{c}6\left(\frac{q_2?q_0}{T_1}?{\mathrm{v}}_0\left(1+\frac{T_0}{T_1}\right)?{\mathrm{a}}_0\left(\frac{1}{2}+\frac{T_0}{3T_1}\right)T_0\right)\\ 6\left(\frac{q_3?q_2}{T_2}?\frac{q_2?q_0}{T_1}+{\mathrm{v}}_0\frac{T_0}{T_1}+{\mathrm{a}}_0\frac{T_0}{3T_1}\right)\\ 6\left(\frac{q_4?q_3}{T_3}?\frac{q_3?q_2}{T_2}\right)\\ ?\\ 6\left(\frac{q_{n?2}?q_{n?3}}{T_{n?3}}?\frac{q_{n?3}?q_{n?4}}{T_{n?4}}\right)\\ 6\left(\frac{q_n?q_{n?2}}{T_{n?2}}?\frac{q_{n?2}?q_{n?3}}{T_{n?3}}?{\mathrm{v}}_n\frac{T_{n?1}}{T_{n?2}}+{\mathrm{a}}_n\frac{T_{n?1}}{3T_{n?2}}\right)\\ 6\left(\frac{q_{n?2}?q_n}{T_{n?2}}+{\mathrm{v}}_n\left(1+\frac{T_{n?1}}{T_{n?2}}\right)?{\mathrm{a}}_n\left(\frac{1}{2}+\frac{T_{n?1}}{3T_{n?2}}\right)T_{n?1}\right)\end{array}\right]\text{\hspace{1em}(14-3)}$$

这里的$T_0,T_1$和$T_{n?2},T_{n?1}$分别是${\bar{t}}_1$和${\bar{t}}_{n?1}$的函数，而${\bar{t}}_1$和${\bar{t}}_{n?1}$分别在区间$(t_0,t_2)$和$(t_{n?2},t_n)$之间，可以随便给定一个值，比如：${\bar{t}}_1=\frac{t_0+t_2}{2}$，${\bar{t}}_{n?1}=\frac{t_{n?2}+t_n}{2}$。

同理根据 Tridiagonal_matrix_algorithm可以快速求解出$\mathbf{w}$，得到所有点的加速度后，就可以根据（2）写出轨迹的表达式了。

示例

在python中使用三次样条曲线，可以使用现成的scipy.interpolate.CubicSpline，当然了它不能同时定义起始和结束点的速度与加速度，参考 机器人轨迹规划——三次样条轨迹1（基于速度的求解方法）。

代码实现

这一部分的代码大家可以根据公式去实现一下，然后和python的scipy.interpolate.CubicSpline 以及 机器人轨迹规划——三次样条轨迹1（基于速度的求解方法）做一个对比，看看结果是否一致。

总结

我们在这篇文章中详细地推导了三次样条曲线地求解方法，另外我们给出了如何利用插入自由点的方法来同时定义起始结束的速度、加速度值，这在机器人轨迹规划时是十分有用的。
另外需要注意的是计算三次样条轨迹需要给定的已知条件是每个点的位置、时间戳以及轨迹起始、结束点的速度、加速度，这里时间戳是已知的，那么如果我们不知道要在什么时刻抵达对应的位置点，如何规划三次样条轨迹呢？这就需要计算最优的时间。问题可以描述为：
已知：
$$\begin{array}{r}\mathbf{q}=[q_0,q_1,\dots ,q_{n?1},q_n]\\ {\dot{q}}_0=v_0,{\dot{q}}_n=v_n\\ {\ddot{q}}_0=a_0,{\dot{q}}_n=a_n\end{array}$$

求解：
$$\begin{array}{rlr}& & \min \sum _{i=0}^{n?1}{T}_i\\ & s.t.& \\ & & {\dot{q}}_{min}\le \dot{q}\le {\dot{q}}_{max}\\ & & {\dot{q}}_{min}\le \ddot{q}\le {\dot{q}}_{max}\end{array}$$

上式中$T_i$是每一段多项式的时间段，如果已知所有的$T_i$，那么就可以累加计算出所有的时间戳$t_i$，从而根据前面的公式，计算出三次样条曲线。同时，计算出来的三次样条曲线需要满足速度和加速度的约束。