平滑轨迹插值方法之多项式插值（附代码）

å¹³æ»è½¨è¿¹æå¼æ¹æ³ä¹å¤é¡¹å¼æå¼ï¼éä»£ç ï¼

前言

今天我们来聊聊轨迹插值，在机器人的运动规划和控制领域，参考轨迹的生成是一个历史悠久的问题，已经发展出了一系列的方法。今天我们就来聊一聊轨迹插值领域中最常见的轨迹插值方法：多项式插值。

说明：本文约5000字，全部看懂需要一定的时间，建议先收藏。本文所涉及的代码全部开源，提供Matlab和Python两个版本，仓库地址详见文末。

首先，我们定义一下问题：

如下图所示，给定一些离散的数据点，我们需要通过插值的方式生成一条能够通过所有给定数据点的参考曲线。

并且，根据实际应用对于“平滑”的要求，通常会有以下不同的约束：

要求生成的参考曲线是连续的；
在1的基础上，要求参考曲线的速度是连续的；
在1和2的基础上，要参考曲线的加速度是连续的；

注意：速度是位置对时间的导数，加速度是速度对时间的导数，加速度对时间的导数我们称之为jerk。

通常情况下，在机器人高速运动的时候，想要得到非常连续、平滑、噪音低的运动控制，第3个约束条件是必不可少的，有的甚至还要求加速度的导数jerk都是连续的。

在多项式插值里面，给定多项式的阶次越高，能拟合的函数曲线就越复杂，但越高阶次的多项式对于计算资源的要求越多。因此对于这3个要求，我们可以分别用不同阶次的多项式函数来拟合，实际应用时根据需求选择合适的方法。

1. 线性插值（一阶，恒定速度）

线性插值，顾名思义，就是使用线性的方法来进行插值。即将给定的数据点依次用线段连起来，点与点之间运动的速度是恒定值。假设我们用

$q(t)$ 来表示插值以后的曲线，则用数学的方式来表示线性插值就是：

$q(t) = a_0 + a_1(t-t_0)$

其中，

$a_0, a_1$ 是待确定的常量参数。

$t_0$ 表示初始时刻，

$a_0$ 表示初始时刻的位置，

$a_1$ 表示斜率，也就是速度，这里为常量。因此，给定下一个时刻

$t_1$ 处的位置

$q(t_1)$ ，我们就有：

$\left\{\begin{array}{l} q\left(t_{0}\right)=q_0=a_{0} \\ q\left(t_{1}\right)=q_1=a_{0}+a_{1}\left(t_{1}-t_{0}\right) \end{array}\right.$

可以计算得到两个常量参数：

$\left\{\begin{array}{l} a_{0}=q_0 \\ a_{1}=(q_1-q_0)/(t_{1}-t_{0}) \end{array}\right.$

曲线的速度为：

$\dot{q}(t) = a_1$

线性插值的实验结果为：

红圈内为给定的中间点值，黑色实线为插值的结果。

从图中可以明显地看到，线性插值带来的最大问题就是在各个数据点交接处会出现一个急剧的“拐弯”，在这个拐弯处其速度不连续，因此对于运动控制来说，在这里会有一个速度的阶跃。例如，对于电机来说，这里会导致控制电流急剧变化，使得电机的输出不稳定，从而引发“抖动”问题，严重者还会损坏机构本身。因此，线性插值本身的问题导致其在控制领域应用范围受限。

2. 抛物线插值（二阶，恒定加速度）

抛物线差值（Parabolic Spline）是二阶多项式插值方法。与线性插值法将各个数据点用线段连起来不同，抛物线插值方法是用二次曲线将各个数据点连接起来，在连接处使用平滑的曲线来过渡，而避免速度不连续导致的“急剧拐弯”。抛物线差值的特征是具有恒定的加速度/减速度，一般是由两个多项式的组合来得到。为什么是两个多项式呢？因为一个用于“加速阶段”，一个用于“减速阶段”。“加速阶段”和“减速阶段”的分割点叫flex point。

考虑2个数据点之间插值的情况。假设初始时刻是

$t_0$ , 在flex point处对应的时刻是

$t_f$ ，最终时刻为

$t_1$ 。

先来看一个特殊情况，flex point的位置是起点和终点的中间位置（也可以任意设定，后面我们会讨论，这里先讨论在中间位置的情况），即

$t_f=\frac{t_0+t_1}{2}$

$q_f=\frac{q_0+q_1}{2}$ 我们作几个符号定义： $h=q_q-q_0, T= t_1 - t_0, T_a = (t_f - t_0)/2, q_f - q_0=h/2$ 。

2.1 加速阶段

对于加速阶段，其数学表达式为：

$q_a(t) = a_0 + a_1(t-t_0) + a_2(t-t_0)^2, \ \ t \in [t_0, t_f]$

其中，

$a_0, a_1, a_2$ 是待确定的常量参数，当我们给定

$q_0=q_a(t_0)$ 和

$q_f=q_a(t_f)$ 以及初始时刻的速度

$v_0$ 以后，有如下关系：

$\left\{\begin{array}{l} q_{a}\left(t_{0}\right)=q_0=a_{0} \\ q_{a}\left(t_{f}\right)=q_f=a_{0}+a_{1}\left(t_{f}-t_{0}\right)+a_{2}\left(t_{f}-t_{0}\right)^{2} \\ \dot{q}_{a}\left(t_{0}\right)=\mathrm{v}_{0}=a_{1} \end{array}\right.$

因此，常量参数可以计算为：

$a_0 = q_0, \quad a_1 = v_0, \quad a_2 = \frac{2}{T^2}(h - v_0 T)\$

这样，我们可以计算得到当

$t \in [t_0, t_f]$ 时，插值曲线为：

$\left\{\begin{array}{l} q_{a}(t)=q_0+\mathrm{v}_{0}\left(t-t_{0}\right)+\frac{2}{T^{2}}\left(h-\mathrm{v}_{0} T\right)\left(t-t_{0}\right)^{2} \\ \dot{q}_{a}(t)=\mathrm{v}_{0}+\frac{4}{T^{2}}\left(h-\mathrm{v}_{0} T\right)\left(t-t_{0}\right) \\ \ddot{q}_{a}(t)=\frac{4}{T^{2}}\left(h-\mathrm{v}_{0} T\right) \end{array} \right.$

在flex point的速度可以这样计算：

$\mathrm{v}_{\max }=\dot{q}_{a}\left(t_{f}\right)=2 \frac{h}{T}-\mathrm{v}_{0}$

大家可能注意到了，与线性插值不同的是，除了指定初始时刻的位置，我们还需要给定初始时刻的速度

$v_0$ 。

在flex point处，由加速阶段进入减速阶段，因此此时加速度的符号是会反转的，会导致加速度不连续。

2.2 减速阶段

对于减速阶段，其数学表达式为：

$q_{b}(t)=a_{3}+a_{4}\left(t-t_{f}\right)+a_{5}\left(t-t_{f}\right)^{2} \quad t \in\left[t_{f}, t_{1}\right]$

其中，$a_3, a_4, a_5$为待定的常量参数。如果给定最终时刻的速度$v_1$，则有如下关系：

$\left\{\begin{array}{l} q_{b}\left(t_{f}\right)=q_f=a_{3} \\ q_{b}\left(t_{1}\right)=q_1=a_{3}+a_{4}\left(t_{1}-t_{f}\right)+a_{5}\left(t_{1}-t_{f}\right)^{2} \\ \dot{q}_{b}\left(t_{1}\right)=\mathrm{v}_{1}=a_{4}+2 a_{5}\left(t_{1}-t_{f}\right) \end{array}\right.$

因此，我们可以计算得到：

$a_{3}=q_f=\frac{q_0+q_1}{2}, \quad a_{4}=2 \frac{h}{T}-\mathrm{v}{1}, \quad a{5}=\frac{2}{T^{2}}\left(\mathrm{v}_{1} T-h\right)$

这样，当

$t \in [t_0, t_f]$ 时，插值曲线为：

$\left\{\begin{array}{l} q_{b}(t)=q_f+\left(2 \frac{h}{T}-\mathrm{v}_{1}\right)\left(t-t_{f}\right)+\frac{2}{T^{2}}\left(\mathrm{v}_{1} T-h\right)\left(t-t_{f}\right)^{2} \\ \dot{q}_{b}(t)=2 \frac{h}{T}-\mathrm{v}_{1}+\frac{4}{T^{2}}\left(\mathrm{v}_{1} T-h\right)\left(t-t_{f}\right) \\ \ddot{q}_{b}(t)=\frac{4}{T^{2}}\left(\mathrm{v}_{1} T-h\right) \end{array}\right.$

这里值得注意的是，如果

$v_0 \neq v_1$ ，那么在

$t=t_f$ 处（flex point），速度曲线是不连续的。

如果在

$t=t_f$ 处，

$q_f$ 不处于起点和终点的中间位置，即不满足

$q_f=\frac{q0+q1}{2}$ 那么，为了保证速度曲线的连续，即

$\dot{q}_a(t_f)=\dot{q}_b(t_f)$ ，我们有以下关系：

$\left\{\begin{array}{ll} q_{a}\left(t_{0}\right)=a_{0} & =q_{0} \\ \dot{q}_{a}\left(t_{0}\right)=a_{1} & =\mathrm{v}_{0} \\ q_{b}\left(t_{1}\right)=a_{3}+a_{4} \frac{T}{2}+a_{5}\left(\frac{T}{2}\right)^{2} & =q_{1} \\ \dot{q}_{b}\left(t_{1}\right)=a_{4}+2 a_{5} \frac{T}{2} & =\mathrm{v}_{1} \\ q_{a}\left(t_{f}\right)=a_{0}+a_{1} \frac{T}{2}+a_{2}\left(\frac{T}{2}\right)^{2} & =a_{3}=q_{b}\left(t_{f}\right) \\ \dot{q}_{a}\left(t_{f}\right)=a_{1}+2 a_{2} \frac{T}{2} & =a_{4}=\dot{q}_{b}\left(t_{f}\right) \end{array}\right.$

其中，

$\frac{T}{2}=t_f-t_0=t_1-t_f$ ，则联立多项式我们可以得到：

$\left\{\begin{aligned} a_{0} &=q_{0} \\ a_{1} &=\mathrm{v}_{0} \\ a_{2} &=\frac{4 h-T\left(3 \mathrm{v}_{0}+\mathrm{v}_{1}\right)}{2 T^{2}} \\ a_{3} &=\frac{4\left(q_{0}+q_{1}\right)+T\left(\mathrm{v}_{0}-\mathrm{v}_{1}\right)}{8} \\ a_{4} &=\frac{4 h-T\left(\mathrm{v}_{0}+\mathrm{v}_{1}\right)}{2 T} \\ a_{5} &=\frac{-4 h+T\left(\mathrm{v}_{0}+3 \mathrm{v}_{1}\right)}{2 T^{2}} \end{aligned}\right.$

从图中我们可以看到，插值的结果中，加速度并不恒定，在

$t=t_f$ 时刻，加速度存在一个阶跃，但加速度和减速度的绝对值是一样的，因此，这种情况属于加速度对称的情况，加速时间和减速时间也一致，都为

$\frac{t_f- t_0}{2}$

本文附带的代码里面实现了加速度对称的情况，但是留了设置不对称加速度的接口，非常容易实现。

下面我们来看一下加速度不对称的情况。

2.3 加速度不对称的情况

直观上，加速度不对称也就是表示加速时间和减速时间不一致，也就是说

$t_0 < t_f < t_1$ ，但是不局限于

$t_f = \frac{t_1 - t_0}{2}$

$t_f$ 可以是区间内的任意值。此时，我们可以结合前面的两个多项式来构造插值的曲线：

$\begin{array}{ll} q_{a}(t)=a_{0}+a_{1}\left(t-t_{0}\right)+a_{2}\left(t-t_{0}\right)^{2}, & t_{0} \leq t<t_{f} \\ q_{b}(t)=a_{3}+a_{4}\left(t-t_{f}\right)+a_{5}\left(t-t_{f}\right)^{2}, & t_{f} \leq t<t_{1} \end{array}$

给定初始时刻

$t_0$ 和最终时刻

$t_1$ 处的位置和速度信息，且给定在

$t_f$ 处要保证位置和速度曲线的连续性，则我们可以计算得到插值曲线：

$\left\{\begin{array}{ll} q_{a}\left(t_{0}\right)=a_{0} & =q_{0} \\ q_{b}\left(t_{1}\right)=a_{3}+a_{4}\left(t_{1}-t_{f}\right)+a_{5}\left(t_{1}-t_{f}\right)^{2} & =q_{1} \\ \dot{q}_{a}\left(t_{0}\right)=a_{1} & =\mathrm{v}_{0} \\ \dot{q}_{b}\left(t_{1}\right)=a_{4}+2 a_{5}\left(t_{1}-t_{f}\right) & =\mathrm{v}_{1} \\ q_{a}\left(t_{f}\right)=a_{0}+a_{1}\left(t_{f}-t_{0}\right)+a_{2}\left(t_{f}-t_{0}\right)^{2} & =a_{3}\left(=q_{b}\left(t_{f}\right)\right) \\ \dot{q}_{a}\left(t_{f}\right) & =a_{1}+2 a_{2}\left(t_{f}-t_{0}\right) \end{array}\right.$

假定加速时间为

$T_a=t_f-t_0$ ，减速时间为

$T_d=t_1-t_f$ ，则由上述多项式可以计算得到：

$\left\{\begin{aligned} a_{0} &=q_{0} \\ a_{1} &=\mathrm{v}_{0} \\ a_{2} &=\frac{2 h-\mathrm{v}_{0}\left(T+T_{a}\right)-\mathrm{v}_{1} T_{d}}{2 T T_{a}} \\ a_{3} &=\frac{2 q_{1} T_{a}+T_{d}\left(2 q_{0}+T_{a}\left(\mathrm{v}_{0}-\mathrm{v}_{1}\right)\right)}{2 T} \\ a_{4} &=\frac{2 h-\mathrm{v}_{0} T_{a}-\mathrm{v}_{1} T_{d}}{T} \\ a_{5} &=-\frac{2 h-\mathrm{v}_{0} T_{a}-\mathrm{v}_{1}\left(T+T_{d}\right)}{2 T T_{d}} \end{aligned}\right.$

因此，在

$t_0 \le t < t_f$ 阶段，曲线的速度和加速度可以计算为：

$\begin{aligned} \dot{q}_{a}(t) &=a_{1}+2 a_{2}\left(t-t_{0}\right)=\mathrm{v}_{0}+\frac{2 h-\mathrm{v}_{0}\left(T+T_{a}\right)-\mathrm{v}_{1} T_{d}}{T T_{a}}\left(t-t_{0}\right) \\ \ddot{q}_{a}(t) &=2 a_{2}=\frac{2 h-\mathrm{v}_{0}\left(T+T_{a}\right)-\mathrm{v}_{1} T_{d}}{T T_{a}} \end{aligned}$

在

$t_f \le t < t_1$ 阶段，曲线的速度和加速度可以计算为：

$\begin{aligned} \dot{q}_{b}(t) &=a_{4}+2 a_{5}\left(t-t_{f}\right)=\frac{2 h-\mathrm{v}_{0} T_{a}-\mathrm{v}_{1} T_{d}}{T}-\frac{2 h-\mathrm{v}_{0} T_{a}-\mathrm{v}_{1}\left(T+T_{d}\right)}{T T_{d}}\left(t-t_{f}\right) \\ \ddot{q}_{b}(t) &=2 a_{5}=-\frac{2 h-\mathrm{v}_{0} T_{a}-\mathrm{v}_{1}\left(T+T_{d}\right)}{T T_{d}} \end{aligned}$

值得注意的是，如果

$v_0=v_1=0$ ，那么就与前面我们讨论的加速度对称的情况结果一致了。实验结果如下：

从图中可以看到，位置曲线是“平滑”的，速度曲线是连续的，加速度也是恒定的（在加速和减速阶段内保持恒定），但是加速度曲线在

$t=t_f$ 时刻存在一个阶跃。

3. 三次多项式插值（三阶，加速度可变）

三次多项式插值方法（Cubic Spline）是一种常用的插值方法，其位置和速度曲线是连续的，加速度是可变的，但加速度不一定连续。考虑2个数据点之间插值的情况，其数学表达式为：

$q(t)=a_{0}+a_{1}\left(t-t_{0}\right)+a_{2}\left(t-t_{0}\right)^{2}+a_{3}\left(t-t_{0}\right)^{3}, \quad t_{0} \leq t \leq t_{1}$

其中，

$a_0, a_1, a_2, a_3$ 为待确定的参数。

3.1 给定每一个点的位置和速度信息

考察给定2个数据点进行插值的情况，如果给定了在初始时刻

$t_0$ 和最终时刻

$t_1$ 处的位置与速度信息

$q_0, q_1, v_0, v_1$ ，设

$h=q_1-q_0, T=t_1-t_0$ ，则这些参数可以使用以下公式计算：

$\left\{\begin{array}{l} a_{0}=q_{0} \\ a_{1}=\mathrm{v}_{0} \\ a_{2}=\frac{3 h-\left(2 \mathrm{v}_{0}+\mathrm{v}_{1}\right) T}{T^{2}} \\ a_{3}=\frac{-2 h+\left(\mathrm{v}_{0}+\mathrm{v}_{1}\right) T}{T^{3}} . \end{array}\right.$

对于给定 n 个一系列数据点进行插值的情况，只需要对所有相邻的两个数据点使用上述公式即可依次计算得到整条插值曲线。

3.2 给定每一个点的位置信息，但中间点的速度未给定

如果我们只是通过给定一系列的位置信息

$q_0, q_1, \dots, q_n$ ，而中间点的速度信息并未给定，整条曲线最开始的起点和最终的终点速度需要直接给定，一般为零，

$v_0=v_1=0$ 。中间各个数据点的速度我们可以通过启发式方法得到，即通过求解位置对时间的导数得到，那么对于第$k$个中间点，我们有：

$v_k = \left\{ \begin{array}{l} 0, \quad sign(d_k) \neq sign(d_{k+1})\\ \frac{1}{2}(d_k + d_{k+1}), \quad sign(d_k) = sign(d_{k+1}) \end{array} \right.$

其中，

$d_k = (q_k-q_{k-1})/(t_k-t_{k-1})$ ，表示曲线的导数或者“斜率”，

$sign(\cdot)$ 为符号函数，返回值为 1 或者 -1 。直观上的理解也就是说，考察第 k 个数据点，如果其导数在该点进行了符号反转，则该点速度为 0 ，否则，该点速度为其导数。

三次多项式插值能够保证位置曲线和速度曲线是连续的，但加速度曲线不一定连续。虽然已经可以满足许多应用上对于“平滑”的要求了，但是在高速控制领域，一般要求加速度也要是连续的。因此，我们需要引入更高阶次的多项式插值方法。

实验结果如下：

从图中可以看到，位置曲线是“平滑”的，速度曲线是连续的，加速度曲线是可变的，但是不连续。这样，对于高速控制的场合来说，控制器的输入仍然会存在阶跃，导致不连续的情况。

4. 五次多项式插值（五阶，加速度连续）

考虑2个数据点之间插值的情况，与其他阶次的多项式形式类似，五次多项式插值方法的数学表达式为：

$q(t)=a_{0}+a_{1}\left(t-t_{0}\right)+a_{2}\left(t-t_{0}\right)^{2}+a_{3}\left(t-t_{0}\right)^{3}+a_{4}\left(t-t_{0}\right)^{4}+a_{5}\left(t-t_{0}\right)^{5}$

其中，

$a_0, a_1, \dots, a_5$ 为待确定的参数。这里我们一共需要6个约束条件，即起点

$t_0$ 和终点

$t_1$ 的位置、速度和加速度信息。即给定如下条件：

$\begin{array}{ll} q\left(t_{0}\right)=q_{0}, & q\left(t_{1}\right)=q_{1} \\ \dot{q}\left(t_{0}\right)=\mathrm{v}_{0}, & \dot{q}\left(t_{1}\right)=\mathrm{v}_{1} \\ \ddot{q}\left(t_{0}\right)=\mathrm{a}_{0}, & \ddot{q}\left(t_{1}\right)=\mathrm{a}_{1} . \end{array}$

设

$T=t_1 - t_0, h=q_1 - q_0$ ，则我们可以计算得到：

$\left\{\begin{array}{l} a_{0}=q_{0} \\ a_{1}=\mathrm{v}_{0} \\ a_{2}=\frac{1}{2} \mathrm{a}_{0} \\ a_{3}=\frac{1}{2 T^{3}}\left[20 h-\left(8 \mathrm{v}_{1}+12 \mathrm{v}_{0}\right) T-\left(3 \mathrm{a}_{0}-\mathrm{a}_{1}\right) T^{2}\right] \\ a_{4}=\frac{1}{2 T^{4}}\left[-30 h+\left(14 \mathrm{v}_{1}+16 \mathrm{v}_{0}\right) T+\left(3 \mathrm{a}_{0}-2 \mathrm{a}_{1}\right) T^{2}\right] \\ a_{5}=\frac{1}{2 T^{5}}\left[12 h-6\left(\mathrm{v}_{1}+\mathrm{v}_{0}\right) T+\left(\mathrm{a}_{1}-\mathrm{a}_{0}\right) T^{2}\right] \end{array}\right.$

对于具有 n 个数据点的情况，可以对所有相邻的2个点应用上述公式，最终得到最终的插值曲线。实验结果如下：

从图中可以看出，位置、速度、加速度三条曲线都是连续的，并且位置和速度还是“平滑”的。到这里，我们已经满足了本文最开始所提到的三个要求。因此，五阶的多项式插值已经能够覆盖大多数应用场景。如果我们对加速度曲线也要求是平滑的，那么就需要更高阶次的多项式插值方法了，例如七阶多项式插值。

5. 七次及更高阶次的多项式插值

理论上，多项式的阶次越高，我们可以获得越“平滑”的曲线，但是同时带来的是对运算资源要求的急剧上升，所以一般情况下，七次及更高阶次的多项式方法只用于某些特殊的场合，因此这里我们不再做深入的分析，有兴趣的读者请参考经典书籍《Trajectory Planning for Automatic Machines and Robots》，可在微信公众号【博士的沙漏】底部点击菜单栏或后台回复【TPA】获取下载链接。网上类似的书籍有：

冗余空间机器人操作臂:运动学、轨迹规划及控制

数值分析（第5版）

6. 实验结果对比

在实际的实验中，我们除了实现给定位置点，还给定了速度点和加速度点。这里我们放一张所有方法插值结果的对比图，从中可以直观地看到使用各个阶次多项式进行插值的结果差异。

本文所涉及的代码全部开源，提供Matlab和Python两个版本：

Github仓库github.com/chauby/PolynomialInterpolation

参考

《Trajectory Planning for Automatic Machines and Robots》

平滑轨迹插值方法之多项式插值（附代码）

沙漏

前言

1. 线性插值（一阶，恒定速度）

2. 抛物线插值（二阶，恒定加速度）

2.1 加速阶段

2.2 减速阶段

2.3 加速度不对称的情况

3. 三次多项式插值（三阶，加速度可变）

3.1 给定每一个点的位置和速度信息

3.2 给定每一个点的位置信息，但中间点的速度未给定

4. 五次多项式插值（五阶，加速度连续）

5. 七次及更高阶次的多项式插值

6. 实验结果对比

参考

为你推荐

STM32按键控制L298N驱动两直流电机正反转以及停止(有代码)

Matlab 仿真——单自由度倒立摆（2）系统分析

机器人运动估计——IMU运动方程与ESKF原理介绍（下）

精选IMU/捷联惯导常见的术语，以及性能评价标准（附Python解析代码）

6. 能观性与能构性 (1) Observability and Constructibility

项目使我憔悴——遥控船实现

评论（0）

关于作者

沙漏

15

0

14

3

【双足机器人（1）】线性倒立摆及其运动控制（附代码）

Dynamic Movement Primitives介绍及Python实现与UR5机械臂仿真

【V-REP自学笔记（六）】基于V-REP逆运动学模块的机械臂轨迹规划

相关推荐

精选11 学习PID--步进电机速度环控制实现

从零手写VIO——（二）IMU是什么？

（一）为什么要控制

Robomaster电控入门（3）RM系列电机控制

飞行机器人（一）DJI平台OSDK简介

控制算法手记-无模型自适应控制

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板