多项式轨迹：从起终条件一步一步算出平滑运动

一架无人机沿通道前进 10 米，从静止出发，在终点停下。途中速度不超过 2 米/秒，加速度大小不超过 1 米/秒²。

这次先为运动分配 10 秒，用一个随时间变化的函数描述整段运动：把任意时刻代入，就能算出位置，再由位置的变化率得到速度和加速度。

多项式轨迹的求解过程，就是把起终运动要求写成方程，求出曲线中的系数，再检查整段运动。

下面先计算满足起终位置、速度的三次轨迹，再加入加速度条件，得到能够与前后静止状态平顺衔接的五次轨迹。

一、把运动要求写成四个条件

通道中的直线路径已经通过碰撞检查。用沿通道的坐标 $p$ 表示位置，起点为 0 米，终点为 10 米，向终点运动为正方向。计算对象是沿这条直线的参考运动。

用 $p(t)$ 表示第 $t$ 秒的位置，$v(t)$ 表示速度，$a(t)$ 表示加速度。它们满足：

$$v(t)=\frac{dp(t)}{dt},\qquad a(t)=\frac{dv(t)}{dt}.$$

运动两端的要求为：

条件	写成公式	具体含义
起始位置	$p(0)=0$	第 0 秒位于起点
起始速度	$v(0)=0$	从静止出发
终点位置	$p(10)=10$	第 10 秒到达 10 米处
终点速度	$v(10)=0$	到达时停止

这些是边界条件。求出轨迹后，还要检查整个时间区间内的速度与加速度是否符合上限。

用四个系数描述一条曲线

先选择三次多项式：

$$p(t)=c_0+c_1t+c_2t^2+c_3t^3.$$

$c_0,c_1,c_2,c_3$ 是四个待求的数。系数确定后，整段时间都使用同一个位置公式。

例如，求第 2 秒的位置，就把 $t=2$ 代入；求第 5 秒的位置，就把 $t=5$ 代入。

对每一项求导，就能得到速度和加速度：

位置中的一项	求一次导数，进入速度公式	再求一次导数，进入加速度公式
$c_0$	0	0
$c_1t$	$c_1$	0
$c_2t^2$	$2c_2t$	$2c_2$
$c_3t^3$	$3c_3t^2$	$6c_3t$

例如，$t^3$ 求导后变成 $3t^2$，再求导得到 $6t$。每项的系数跟着保留下来。

把各项相加：

$$v(t)=c_1+2c_2t+3c_3t^2,$$

$$a(t)=2c_2+6c_3t.$$

这样，四个起终条件就都能写成关于系数的方程。

二、一步一步求出三次轨迹

1. 起点条件确定前两个系数

先代入 $t=0$。所有包含 $t$ 的项都变成 0，因此：

$$p(0)=c_0=0.$$

速度公式在 $t=0$ 时只剩 $c_1$，所以：

$$v(0)=c_1=0.$$

位置与速度公式简化为：

$$p(t)=c_2t^2+c_3t^3,$$

$$v(t)=2c_2t+3c_3t^2.$$

现在只剩 $c_2$ 和 $c_3$ 两个未知数。

2. 终点条件确定剩下两个系数

第 10 秒的位置应为 10 米：

$$100c_2+1000c_3=10.$$

两边除以 100，得到：

$$c_2+10c_3=0.1.\tag{1}$$

第 10 秒的速度应为 0：

$$20c_2+300c_3=0.$$

两边除以 20，得到：

$$c_2+15c_3=0.\tag{2}$$

用式（2）减去式（1），$c_2$ 被消去：

$$5c_3=-0.1, \qquad c_3=-0.02.$$

代回式（1）：

$$c_2+10\times(-0.02)=0.1, \qquad c_2=0.3.$$

于是，整段轨迹为：

$$\boxed{p(t)=0.3t^2-0.02t^3},\qquad 0\le t\le10.$$

以下数值公式中，时间按秒计，位置按米计；相应系数带有使各项单位一致的单位。

求导得到：

$$v(t)=0.6t-0.06t^2,$$

$$a(t)=0.6-0.12t.$$

3. 把几个时刻代进去

例如，第 5 秒的位置为：

$$p(5)=0.3\times25-0.02\times125 =7.5-2.5=5\text{ 米}.$$

当时的速度为：

$$v(5)=0.6\times5-0.06\times25 =3-1.5=1.5\,\mathrm{m/s}.$$

几个时刻的结果如下：

时刻（秒）	位置（米）	速度（米/秒）	加速度（米/秒²）
0	0	0	0.60
2	1.04	0.96	0.36
5	5.00	1.50	0
8	8.96	0.96	-0.36
10	10.00	0	-0.60

四个位置、速度条件都得到满足。

问题出在加速度上：刚进入运动段时需要 $+0.6\,\mathrm{m/s^2}$，离开前需要 $-0.6\,\mathrm{m/s^2}$。与前后的静止状态接起来，出发处加速度从 0 突变到 $+0.6$，停止处从 $-0.6$ 突变回 0。要让加速度在这两个连接处也接得上，可以继续加入起终加速度为零的条件。

三、加入加速度条件，求出五次轨迹

1. 现在需要满足六个条件

完整的起终要求变为：

运动量	第 0 秒	第 10 秒
位置	0 米	10 米
速度	0 米/秒	0 米/秒
加速度	0 米/秒²	0 米/秒²

三次多项式中的四个系数已经由前面的四个条件确定。增加两个加速度条件时，可以使用有六个系数的五次多项式。

为了方便求系数，先把时间换成一把从 0 到 1 的刻度尺。

2. 用“总时间过了多少”表示时间

定义归一化时间：

$$s=\frac{t}{10}.$$

实际时间 $t$	归一化时间 $s$	含义
0 秒	0	刚开始
2 秒	0.2	用掉总时间的 20%
5 秒	0.5	用掉总时间的一半
10 秒	1	到达结束时刻

$s$ 记录时间进度，对应的位置由曲线计算。

用 $P(s)$ 表示这把时间尺上的位置函数：

$$P(s)=b_0+b_1s+b_2s^2+b_3s^3+b_4s^4+b_5s^5.$$

实际位置为 $p(t)=P(t/10)$。例如，第 2 秒的位置就是 $P(0.2)$。

每过 1 秒，$s$ 增加 0.1。对 $s$ 求导得到的是“每单位时间进度的位置变化”，换成每秒的变化要再除以 10：

$$v(t)=\frac1{10}P'(s),\qquad a(t)=\frac1{100}P''(s).$$

这里的撇号表示对 $s$ 求导。第二次求导又带来一次除以 10，因此加速度中的分母是 100。

可以用一个简单函数核对：若 $P(s)=10s$，实际位置就是 $p(t)=t$，每秒前进 1 米；上式也给出 $v=10/10=1$ 米/秒。

3. 起点条件消去前三个系数

在起点 $s=0$：

$$P(0)=b_0=0,$$

$$v(0)=\frac{b_1}{10}=0,$$

$$a(0)=\frac{2b_2}{100}=0.$$

所以 $b_0=b_1=b_2=0$，位置函数只剩：

$$P(s)=b_3s^3+b_4s^4+b_5s^5.$$

它的两次导数为：

$$P'(s)=3b_3s^2+4b_4s^3+5b_5s^4,$$

$$P''(s)=6b_3s+12b_4s^2+20b_5s^3.$$

4. 终点条件变成三个方程

到达第 10 秒时，$s=1$，各次幂都等于 1。

位置等于 10 米，得到：

$$b_3+b_4+b_5=10.\tag{3}$$

速度等于 0，意味着 $P'(1)=0$，得到：

$$3b_3+4b_4+5b_5=0.\tag{4}$$

加速度等于 0，意味着 $P''(1)=0$，得到：

$$6b_3+12b_4+20b_5=0.\tag{5}$$

每个方程都有明确来源：到达哪里、以多快的速度到达、到达时怎样加速。

5. 把三个未知数算出来

先用式（4）减去式（3）的 3 倍，消去 $b_3$：

$$b_4+2b_5=-30.\tag{6}$$

再用式（5）减去式（3）的 6 倍：

$$6b_4+14b_5=-60.$$

两边除以 2：

$$3b_4+7b_5=-30.\tag{7}$$

用式（7）减去式（6）的 3 倍，消去 $b_4$：

$$b_5=-30-(-90)=60.$$

逐个代回：

$$b_4=-30-2\times60=-150,$$

$$b_3=10-b_4-b_5=10+150-60=100.$$

最终得到：

$$\boxed{P(s)=100s^3-150s^4+60s^5}.$$

换回 $s=t/10$：

$$\boxed{p(t)=0.1t^3-0.015t^4+0.0006t^5},\qquad 0\le t\le10.$$

对实际时间 $t$ 求导：

$$v(t)=0.3t^2-0.06t^3+0.003t^4,$$

$$a(t)=0.6t-0.18t^2+0.012t^3.$$

到这里，六个起终要求已经转化成了三个可以直接计算的函数：位置、速度和加速度。

四、把五次轨迹还原成具体运动

1. 第 2 秒在哪里，以多快的速度运动？

代入 $t=2$，位置为：

$$p(2)=0.1\times8-0.015\times16+0.0006\times32 =0.5792\text{ 米}.$$

速度为：

$$v(2)=0.3\times4-0.06\times8+0.003\times16 =0.768\,\mathrm{m/s}.$$

加速度为：

$$a(2)=0.6\times2-0.18\times4+0.012\times8 =0.576\,\mathrm{m/s^2}.$$

这三个数字描述同一个时刻：已经前进 0.5792 米，正以 0.768 米/秒向前运动，速度仍在增加。

时刻（秒）	位置（米）	速度（米/秒）	加速度（米/秒²）
0	0	0	0
2	0.5792	0.768	0.576
4	3.1744	1.728	0.288
5	5.0000	1.875	0
6	6.8256	1.728	-0.288
8	9.4208	0.768	-0.576
10	10.0000	0	0

第 5 秒时加速度为 0，速度为 1.875 米/秒。此刻速度正在从上升转向下降，无人机继续向前运动。

2. 三条曲线怎样对应？

下面的图同时画出本篇求得的三次与五次轨迹。两者都用 10 秒前进 10 米，运动前后保持静止。

位置曲线的斜率对应速度。 五次轨迹开始时增长较缓，中间变陡，接近终点时重新变平。第 5 秒两条曲线都经过 5 米处。

五次轨迹的速度从 0 逐渐上升，再回到 0。起终段采用更平缓的速度变化，中间就需要更高的速度来完成同样的路程，因此其速度峰值高于这里的三次轨迹。

五次轨迹的加速度先从 0 升高，再下降到 0，随后进入减速阶段，最后回到 0。前后静止段的加速度也为 0，因此连接处的位置、速度和加速度都连续，称为 $C^2$ 连续。

Jerk 是加速度的变化率。本例五次轨迹在运动段两端的单侧 Jerk 都为 $0.6\,\mathrm{m/s^3}$，静止段的 Jerk 为 0；它的连续衔接可以进一步作为更高阶的边界要求。

五、检查整段运动是否满足速度和加速度上限

起终条件已经满足，接下来计算途中可能出现的最大值。

1. 最大速度出现在什么时候？

把速度公式整理成乘积：

$$v(t)=0.003t^2(10-t)^2.$$

其中：

$$t(10-t)=25-(t-5)^2.$$

平方项最小为 0，在 $t=5$ 时取得。因此，在 $0\le t\le10$ 内，$t(10-t)$ 从 0 增加到 25，再回到 0，速度在第 5 秒达到最大值：

$$v_{\max}=0.003\times25^2=1.875\,\mathrm{m/s}.$$

整段速度均为非负，最高为 1.875 米/秒，满足 $0\le v\le2$ 的要求。位置也就从 0 米持续增加到 10 米，始终位于原来的直线路径上。

2. 最大加速度怎样找到？

加速度公式为：

$$a(t)=0.6t-0.18t^2+0.012t^3.$$

要找曲线的最高点和最低点，可以先找它的变化率等于 0 的位置，再与区间端点一起比较。加速度的变化率就是 Jerk：

$$j(t)=\frac{da}{dt}=0.6-0.36t+0.036t^2.$$

令 $j(t)=0$，两边除以 0.036：

$$t^2-10t+\frac{50}{3}=0.$$

把前两项配成平方：

$$(t-5)^2=\frac{25}{3}.$$

得到两个时刻：

$$t=5\pm\frac5{\sqrt3} \approx2.113\text{ 秒、}7.887\text{ 秒}.$$

代回加速度公式，分别得到约 $+0.57735$ 和 $-0.57735\,\mathrm{m/s^2}$。起终加速度都为 0，所以：

$$|a|_{\max}=\frac1{\sqrt3}\approx0.57735\,\mathrm{m/s^2}.$$

满足加速度大小不超过 1 米/秒²的要求。

检查项目	计算结果	要求
位置范围	从 0 米单调增加到 10 米	沿既定直线路径前进
速度范围	0～1.875 米/秒	0～2 米/秒
加速度大小峰值	约 0.57735 米/秒²	不超过 1 米/秒²
起终状态	位置符合要求，速度和加速度均为 0	与前后静止段衔接

这里的峰值由整个区间上的公式求出。前面的时刻表用于观察运动，极值计算用于检查区间内的最大运动需求。

六、把总时间改成 8 秒，会发生什么？

1. 保留同一种曲线形状，改变时间刻度

前面使用 $s=t/10$。如果总时间改为 $T$，令：

$$s=\frac{t}{T},\qquad 0\le s\le1.$$

保留已经求出的形状：

$$p_T(t)=100s^3-150s^4+60s^5.$$

开始时 $s=0$，位置为 0；结束时 $s=1$，位置为 10 米。起终速度、加速度仍为 0。

求一次时间导数，就乘一次 $1/T$：

$$v_T(t)=\frac1T\left(300s^2-600s^3+300s^4\right),$$

$$a_T(t)=\frac1{T^2}\left(600s-1800s^2+1200s^3\right).$$

同一个时间进度，例如 $s=0.2$，对应同一个位置。用 8 秒完成时，第 1.6 秒就到达这里；用 10 秒完成时，第 2 秒到达；用 12 秒完成时，第 2.4 秒到达。

2. 时间压缩怎样放大运动需求？

与 10 秒的安排相比，把时间改成 $T$ 后：

$$\text{速度放大倍数}=\frac{10}{T}, \qquad \text{加速度放大倍数}=\left(\frac{10}{T}\right)^2.$$

例如，改成 8 秒，速度放大 $10/8=1.25$ 倍，加速度放大 $1.25^2=1.5625$ 倍。

总时间	最高速度（米/秒）	加速度大小峰值（米/秒²）	检查结果
8 秒	2.344	0.902	最高速度超过 2 米/秒
10 秒	1.875	0.577	两项都满足
12 秒	1.563	0.401	两项都满足

时间分配直接影响轨迹的速度与加速度需求。 改动总时间后，需要用新时间重新计算导数并检查上限。

对于这条 10 米五次曲线，还可以反过来计算允许的时间范围。速度要求为：

$$1.875\times\frac{10}{T}\le2, \qquad T\ge9.375\text{ 秒}.$$

加速度要求为：

$$\frac1{\sqrt3}\left(\frac{10}{T}\right)^2\le1, \qquad T\ge\frac{10}{\sqrt[4]{3}}\approx7.60\text{ 秒}.$$

两项一起考虑，这种五次曲线形状要求 $T\ge9.375$ 秒。本例选择 10 秒，满足两项限制。

这个时间范围对应的是上面固定的归一化曲线。采用加速、匀速、减速等其他形状时，速度分配和对应时间需要由各自的曲线重新计算。

七、从一段曲线走向多个航点

一次单段多项式轨迹生成，可以整理成下面的过程：

上面求出的五次式已经由六个边界条件唯一确定。它展示了怎样用有限个系数表达一整段运动。

需要经过多个航点时，可以为每一段分别建立多项式。例如，沿通道经过 4 米处，再到达 10 米处，可以使用两段曲线。

假设希望经过 4 米处时保持 1 米/秒的速度，加速度为 0，那么连接处应当满足：

运动量	第一段的终点	第二段的起点
位置	4 米	4 米
速度	1 米/秒	1 米/秒
加速度	0	0

将这些数值分别代入两段的边界方程，就能求出相应系数。每段使用从该段开始计时的局部时间，再将相邻段的位置、速度和加速度接起来。

当中间航点的速度、加速度也可以选择时，轨迹就有了进一步调整的余地。分段多项式与轨迹优化会继续处理这些变量。

小结

多项式轨迹把运动要求变成系数方程。三次式可以满足本例的起终位置和速度，五次式进一步满足起终加速度要求。

求出系数后，代入时间即可得到位置，求导得到速度和加速度；检查极值能够判断途中运动是否符合限制；调整时间则会改变相应的运动需求。

从边界条件求出曲线，再检查整段曲线，是连续轨迹生成的一条基本思路。 下一步可以继续学习分段多项式，计算一条怎样平顺经过多个航点的轨迹。