一架无人机沿通道前进 10 米，需要在第 4 秒经过 4 米处的航点，再在第 10 秒到达终点并停下。经过中间航点时，保持 1 米/秒的向前速度。

可以把运动分为两段：第一段从 0 米到 4 米，第二段从 4 米到 10 米。每段用一个五次多项式描述，两段在航点处共享同一组位置、速度和加速度。

下面逐步算出这两条曲线，再检查它们怎样接成一份完整的运动安排。

一、给每个航点安排运动状态

通道中的直线路径已经通过碰撞检查。用沿通道的位置坐标 pp 描述无人机中心，起点为 0 米，朝终点为正方向。本文计算沿这条直线的参考运动。

W 是要求经过的中间位置，称为航点。第一段用时 4 秒，第二段用时 6 秒。本例给三个位置指定以下运动状态：

位置 到达时刻 位置 pp 速度 vv 加速度 aa
起点 S 0 秒 0 米 0 米/秒 0 米/秒²
中间航点 W 4 秒 4 米 1 米/秒 0 米/秒²
终点 G 10 秒 10 米 0 米/秒 0 米/秒²

运动前后保持静止。途中要求 0≤v≤2 m/s0\le v\le2\,\mathrm{m/s}，加速度大小满足 ∣a∣≤1 m/s2|a|\le1\,\mathrm{m/s^2}。

W 处的速度为 1，表示无人机经过这里时继续向前运动；加速度为 0，表示此刻速度的变化率为 0。

把表格拆成两段各自的边界条件（位置、速度、加速度）：

分段 起点状态 终点状态
第一段 (0, 0, 0)(0,\ 0,\ 0) (4, 1, 0)(4,\ 1,\ 0)
第二段 (4, 1, 0)(4,\ 1,\ 0) (10, 0, 0)(10,\ 0,\ 0)

第一段的终点状态就是第二段的起点状态，这组共享的数值是两段运动衔接的依据。

本例先选定 4 秒、6 秒的持续时间，以及 W 处的速度和加速度。求出曲线后，再检查这些选择对应的整段运动。

二、每段使用自己的时间

1. 总时间与本段已经过的时间

用 tt 表示从 S 出发后的总时间。

第一段从第 0 秒开始，本段已经过的时间为 tt；第二段从第 4 秒开始，本段已经过的时间为 t−4t-4。

例如，第 7 秒位于第二段，此时第二段已经运行：

7−4=3 秒.7-4=3\text{ 秒}.

为了让两段都使用从 0 到 1 的时间刻度，再定义各自的归一化时间：

s1=t4,0≤t≤4,s_1=\frac{t}{4},\qquad 0\le t\le4,

s2=t−46,4≤t≤10.s_2=\frac{t-4}{6},\qquad 4\le t\le10.

总时刻 所在位置或阶段 本段已经过的时间 归一化时间
0 秒 第一段起点 0 秒 s1=0s_1=0
2 秒 第一段中途 2 秒 s1=0.5s_1=0.5
4 秒 第一段终点、第二段起点 第一段 4 秒；第二段 0 秒 s1=1s_1=1；s2=0s_2=0
7 秒 第二段中途 3 秒 s2=0.5s_2=0.5
10 秒 第二段终点 6 秒 s2=1s_2=1

同一个第 4 秒，在第一段的时间尺上是结束，在第二段的时间尺上是开始。

2. 求导时换回实际时间

用 P1(s1)P_1(s_1)、P2(s2)P_2(s_2) 表示两段位置曲线。对归一化时间求导后，需要按各段持续时间换算成实际速度与加速度：

运动量 第一段，持续 4 秒 第二段，持续 6 秒
位置 P1(s1)P_1(s_1) P2(s2)P_2(s_2)
速度 P1′(s1)/4P_1'(s_1)/4 P2′(s2)/6P_2'(s_2)/6
加速度 P1′′(s1)/16P_1''(s_1)/16 P2′′(s2)/36P_2''(s_2)/36

撇号表示对该段的归一化时间求导。求一次实际时间导数，除以一次本段持续时间；求两次，就除以时间的平方。

下面推导某一段时，将它自己的归一化时间简写为 ss，取值均为 0≤s≤10\le s\le1。

三、先求第一段：从静止出发，运动到 4 米处

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

设第一段位置为：

P1(s)=b0+b1s+b2s2+b3s3+b4s4+b5s5.P_1(s)=b_0+b_1s+b_2s^2+b_3s^3+b_4s^4+b_5s^5.

起点对应 s=0s=0，位置、速度、加速度都为 0：

b0=0,b14=0,2b216=0.b_0=0,\qquad \frac{b_1}{4}=0,\qquad \frac{2b_2}{16}=0.

因此 b0=b1=b2=0b_0=b_1=b_2=0，只剩：

P1(s)=b3s3+b4s4+b5s5.P_1(s)=b_3s^3+b_4s^4+b_5s^5.

两次导数为：

P1′(s)=3b3s2+4b4s3+5b5s4,P_1'(s)=3b_3s^2+4b_4s^3+5b_5s^4,

P1′′(s)=6b3s+12b4s2+20b5s3.P_1''(s)=6b_3s+12b_4s^2+20b_5s^3.

2. 航点条件给出三个方程

第一段结束时 s=1s=1，应当位于 4 米处：

b3+b4+b5=4.(1)b_3+b_4+b_5=4.\tag{1}

实际速度为 1 米/秒，所以：

P1′(1)4=1.\frac{P_1'(1)}4=1.

两边乘以 4：

3b3+4b4+5b5=4.(2)3b_3+4b_4+5b_5=4.\tag{2}

加速度为 0，得到：

6b3+12b4+20b5=0.(3)6b_3+12b_4+20b_5=0.\tag{3}

式（2）右侧的 4，来自“本段持续 4 秒 × 到达速度 1 米/秒”。它把航点处的实际速度换成了归一化时间下的导数要求。

3. 消元，求出三个系数

用式（2）减去式（1）的 3 倍：

b4+2b5=4−12=−8.(4)b_4+2b_5=4-12=-8.\tag{4}

用式（3）减去式（1）的 6 倍：

6b4+14b5=−24.6b_4+14b_5=-24.

两边除以 2：

3b4+7b5=−12.(5)3b_4+7b_5=-12.\tag{5}

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

b5=−12−(−24)=12.b_5=-12-(-24)=12.

逐个代回：

b4=−8−2×12=−32,b_4=-8-2\times12=-32,

b3=4−b4−b5=4+32−12=24.b_3=4-b_4-b_5=4+32-12=24.

得到第一段：

P1(s)=24s3−32s4+12s5.\boxed{P_1(s)=24s^3-32s^4+12s^5}.

本段的实际速度、加速度为：

v1(s)=P1′(s)4=18s2−32s3+15s4,v_1(s)=\frac{P_1'(s)}4 =18s^2-32s^3+15s^4,

a1(s)=P1′′(s)16=9s−24s2+15s3.a_1(s)=\frac{P_1''(s)}{16} =9s-24s^2+15s^3.

以下数值表达中，位置按米计，时间按秒计，速度和加速度分别按米/秒、米/秒²计。

4. 代入中途与终点，看看实际运动

第 2 秒，第一段进行了一半，s=2/4=0.5s=2/4=0.5。

位置为：

P1(0.5)=24×18−32×116+12×132=3−2+0.375=1.375 米.P_1(0.5)=24\times\frac18-32\times\frac1{16}+12\times\frac1{32} =3-2+0.375=1.375\text{ 米}.

速度为：

v1(0.5)=18×14−32×18+15×116=1.4375 m/s.v_1(0.5)=18\times\frac14-32\times\frac18+15\times\frac1{16} =1.4375\,\mathrm{m/s}.

第 4 秒，s=1s=1：

P1(1)=24−32+12=4,P_1(1)=24-32+12=4,

v1(1)=18−32+15=1,v_1(1)=18-32+15=1,

a1(1)=9−24+15=0.a_1(1)=9-24+15=0.

第一段正好把无人机送到 4 米处，并带着 1 米/秒的速度交给第二段。

四、再求第二段：带着已有速度，运动到 10 米处

1. 起点条件带入已有位置与速度

第二段同样使用五次多项式：

P2(s)=d0+d1s+d2s2+d3s3+d4s4+d5s5.P_2(s)=d_0+d_1s+d_2s^2+d_3s^3+d_4s^4+d_5s^5.

它的起点对应总时间第 4 秒、本段 s=0s=0，状态为：位置 4，速度 1，加速度 0。

代入得到：

d0=4,d16=1,2d236=0.d_0=4,\qquad \frac{d_1}{6}=1,\qquad \frac{2d_2}{36}=0.

所以：

P2(s)=4+6s+d3s3+d4s4+d5s5.P_2(s)=4+6s+d_3s^3+d_4s^4+d_5s^5.

常数项 4 保留了本段起始位置；6s6s 对应本段起始速度。因为 s=(t−4)/6s=(t-4)/6，这一项换成实际时间就是 t−4t-4，其时间导数为 1。

2. 终点条件确定剩余系数

第二段结束时 s=1s=1，位置为 10：

4+6+d3+d4+d5=10,4+6+d_3+d_4+d_5=10,

整理得到：

d3+d4+d5=0.(6)d_3+d_4+d_5=0.\tag{6}

终点速度为 0。由 P2′(1)/6=0P_2'(1)/6=0 得到：

6+3d3+4d4+5d5=0,6+3d_3+4d_4+5d_5=0,

即：

3d3+4d4+5d5=−6.(7)3d_3+4d_4+5d_5=-6.\tag{7}

终点加速度为 0：

6d3+12d4+20d5=0.(8)6d_3+12d_4+20d_5=0.\tag{8}

3. 逐步算出系数

式（7）减去式（6）的 3 倍：

d4+2d5=−6.(9)d_4+2d_5=-6.\tag{9}

式（8）减去式（6）的 6 倍，再除以 2：

3d4+7d5=0.(10)3d_4+7d_5=0.\tag{10}

式（10）减去式（9）的 3 倍：

d5=18.d_5=18.

代回：

d4=−6−2×18=−42,d_4=-6-2\times18=-42,

d3=−d4−d5=42−18=24.d_3=-d_4-d_5=42-18=24.

于是第二段为：

P2(s)=4+6s+24s3−42s4+18s5.\boxed{P_2(s)=4+6s+24s^3-42s^4+18s^5}.

实际速度和加速度为：

v2(s)=P2′(s)6=1+12s2−28s3+15s4,v_2(s)=\frac{P_2'(s)}6 =1+12s^2-28s^3+15s^4,

a2(s)=P2′′(s)36=4s−14s2+10s3.a_2(s)=\frac{P_2''(s)}{36} =4s-14s^2+10s^3.

起点 s=0s=0 给出位置 4、速度 1、加速度 0；终点 s=1s=1 给出：

P2(1)=4+6+24−42+18=10,P_2(1)=4+6+24-42+18=10,

v2(1)=1+12−28+15=0,v_2(1)=1+12-28+15=0,

a2(1)=4−14+10=0.a_2(1)=4-14+10=0.

第二段从第一段留下的运动状态出发，最后在 10 米处停下。

五、在第 4 秒，把两段接起来

1. 整条轨迹怎样写？

根据总时间选择相应的曲线，再换算成本段的时间进度：

p(t)={P1(t/4),0≤t≤4,P2((t−4)/6),4<t≤10.\boxed{ p(t)= \begin{cases} P_1(t/4), & 0\le t\le4,\\[4pt] P_2((t-4)/6), & 4<t\le10. \end{cases} }

例如，第 7 秒位于第二段：

s2=7−46=0.5.s_2=\frac{7-4}{6}=0.5.

代入第二段位置公式：

p(7)=4+6×12+24×18−42×116+18×132=4+3+3−2.625+0.5625=7.9375 米.\begin{aligned} p(7) &=4+6\times\frac12+24\times\frac18 -42\times\frac1{16}+18\times\frac1{32}\\ &=4+3+3-2.625+0.5625\\ &=7.9375\text{ 米}. \end{aligned}

此时：

v(7)=1+12×14−28×18+15×116=1.4375 m/s,v(7)=1+12\times\frac14-28\times\frac18+15\times\frac1{16} =1.4375\,\mathrm{m/s},

a(7)=4×12−14×14+10×18=−0.25 m/s2.a(7)=4\times\frac12-14\times\frac14+10\times\frac18 =-0.25\,\mathrm{m/s^2}.

这组数表示：已经到达 7.9375 米处，继续向前运动，同时正在减速。

几个时刻的状态如下：

总时刻（秒） 使用的曲线与进度 位置（米） 速度（米/秒） 加速度（米/秒²）
0 第一段，s1=0s_1=0 0 0 0
2 第一段，s1=0.5s_1=0.5 1.3750 1.4375 0.3750
4 第一段末端、第二段起点 4.0000 1.0000 0
5 第二段，s2=1/6s_2=1/6 5.0810 1.2153 0.3241
7 第二段，s2=0.5s_2=0.5 7.9375 1.4375 -0.2500
10 第二段，s2=1s_2=1 10.0000 0 0

2. 接起来，具体要检查什么？

分别从两侧计算航点处的运动量：

检查项目 第一段结束，s1=1s_1=1 第二段开始，s2=0s_2=0
位置 P1(1)=4P_1(1)=4 P2(0)=4P_2(0)=4
速度 P1′(1)/4=4/4=1P_1'(1)/4=4/4=1 P2′(0)/6=6/6=1P_2'(0)/6=6/6=1
加速度 P1′′(1)/16=0P_1''(1)/16=0 P2′′(0)/36=0P_2''(0)/36=0

位置相等，使两段经过同一处；速度相等，使运动变化率接得上；加速度相等，使速度的变化率也接得上。

注意表中的归一化导数：第一段为 4，第二段为 6。分别除以各自持续时间后，实际速度都为 1。连接条件比较的是实际时间下的位置、速度和加速度。

位置及其前两阶时间导数连续，称为 C2C^2 连续。两段内部都是多项式，航点处又匹配了这三个量，因此整条位置轨迹达到 C2C^2 连续；起终加速度为零，也能与前后静止段按同样要求衔接。

还可以继续检查 Jerk，即加速度对实际时间的变化率：

j1(1)=P1′′′(1)43=1.5 m/s3,j2(0)=P2′′′(0)63=23 m/s3.j_1(1)=\frac{P_1'''(1)}{4^3}=1.5\,\mathrm{m/s^3}, \qquad j_2(0)=\frac{P_2'''(0)}{6^3}=\frac23\,\mathrm{m/s^3}.

这两个单侧值存在跳变。本例的连续性要求匹配到加速度，Jerk 的衔接属于更高阶的设计要求。

六、用曲线观察：经过航点时继续前进

1. 位置曲线怎样连接？

分段轨迹的位置曲线：第 4 秒经过 4 米处，两段以相同的非零斜率衔接

位置从 0 米增加到 10 米，在第 4 秒经过 W。连接处曲线仍然向上倾斜，斜率为 1，对应 1 米/秒的向前速度。

2. 把航点速度改为零，会发生什么？

为了观察航点速度的作用，再构造一组对照曲线：仍然在第 4 秒到达 W、第 10 秒到达 G，但在 W 处把速度降到 0。三个位置的加速度仍为 0。

起终速度、加速度都为零的一段五次曲线，可以使用形状函数：

F(s)=10s3−15s4+6s5.F(s)=10s^3-15s^4+6s^5.

因此，对照方案的两段位置分别为：

P~1(s)=4F(s),P~2(s)=4+6F(s).\widetilde P_1(s)=4F(s),\qquad \widetilde P_2(s)=4+6F(s).

第一段从静止运动到 W 并降至零速，第二段再从 W 加速出发。这里两段立即接续，没有额外安排停留时间。

两种航点速度安排：本例在第 4 秒以 1 米每秒通过，对照曲线在该时刻减速到零

实线是本篇求出的连续通过方案：速度先上升，接近 W 时回到 1；通过 W 后继续加速，最后减速到 0。

虚线是零速通过 W 的对照方案：速度在第 4 秒降到 0，随后重新上升。

航点处给定的速度，会改变它前后两段的运动安排。 到达同一个位置、使用相同时间，可以得到不同的速度曲线。

3. 加速度在航点处怎样变化？

两种安排的加速度曲线：航点处都回到零，本例的连续通过方案全程保持在正负 1 米每二次方秒以内

两种方案在 W 处的加速度都为 0，位置、速度和加速度也都分别连续。它们在航点处的速度取值不同，因而对应不同的运动需求。

相同的 4 秒时间分配下，零速对照方案第一段的加速度大小峰值约为 1.443 m/s21.443\,\mathrm{m/s^2}，超过本例的上限，需要调整时间或其他条件。本篇连续通过方案的峰值检查如下。

七、检查整段运动的速度与加速度

1. 最高速度在哪里？

速度的变化率是加速度。找速度极值时，先检查各段内加速度为零的位置，再比较端点。

将两段加速度分解成乘积：

a1(s)=3s(s−1)(5s−3),a_1(s)=3s(s-1)(5s-3),

a2(s)=2s(s−1)(5s−2).a_2(s)=2s(s-1)(5s-2).

在 0<s<10<s<1 内，第一段的加速度在 s=3/5=0.6s=3/5=0.6 时为零；第二段在 s=2/5=0.4s=2/5=0.4 时为零。

第一段的实际时刻为：

t=4×0.6=2.4 秒.t=4\times0.6=2.4\text{ 秒}.

代入速度：

v1(0.6)=18×0.36−32×0.216+15×0.1296=1.512 m/s.v_1(0.6)=18\times0.36-32\times0.216+15\times0.1296 =1.512\,\mathrm{m/s}.

第二段的实际时刻为：

t=4+6×0.4=6.4 秒.t=4+6\times0.4=6.4\text{ 秒}.

同样得到 v2(0.4)=1.512 m/sv_2(0.4)=1.512\,\mathrm{m/s}。

轨迹段 段初速度 段内最高速度 段末速度
第一段 0 1.512 米/秒 1 米/秒
第二段 1 米/秒 1.512 米/秒 0

由加速度的正负可知，两段速度均先上升再下降。结合端点值，整条轨迹满足：

0≤v(t)≤1.512<2 m/s.0\le v(t)\le1.512<2\,\mathrm{m/s}.

速度始终非负，位置便从 0 米持续增加到 10 米，保持在已经检查过的直线路径内。

2. 最大加速度怎样求？

对加速度公式再求导。为了寻找归一化时间中的极值，令下面两个导数为零：

da1ds=9−48s+45s2=0,\frac{da_1}{ds}=9-48s+45s^2=0,

da2ds=4−28s+30s2=0.\frac{da_2}{ds}=4-28s+30s^2=0.

实际 Jerk 还分别需要除以 4 和 6；这两个正数因子不改变导数为零的位置。

解方程，得到第一段的两个候选位置：

s=8±1915≈0.2427, 0.8239.s=\frac{8\pm\sqrt{19}}{15} \approx0.2427,\ 0.8239.

第二段为：

s=7±1915≈0.1761, 0.7573.s=\frac{7\pm\sqrt{19}}{15} \approx0.1761,\ 0.7573.

将它们代回各段加速度公式，并换成总时间：

轨迹段 总时刻（秒，约） 加速度（米/秒²，约）
第一段 0.971 +0.9851
第一段 3.296 -0.4873
第二段 5.056 +0.3249
第二段 8.544 -0.6567

各段端点的加速度均为 0，所以整个区间的加速度大小峰值约为：

∣a∣max⁡≈0.9851 m/s2<1 m/s2.|a|_{\max}\approx0.9851\,\mathrm{m/s^2}<1\,\mathrm{m/s^2}.

至此，这组分段时间和航点状态满足了本例的位置、通过时刻、速度与加速度要求。

八、从两段曲线推广到多个航点

1. 每个航点保存一组共享状态

增加航点后，可以给每个航点记录到达时刻、位置、速度和加速度。相邻两行就组成一段运动的边界条件。

第一段

第二段

第三段

航点 0

航点 1
两段共用状态

航点 2
两段共用状态

航点 3

第一段

第二段

第三段

航点 0

航点 1
两段共用状态

航点 2
两段共用状态

航点 3

设某一段从总时刻 tit_i 开始，到 ti+1t_{i+1} 结束，它的持续时间和归一化时间为：

Ti=ti+1−ti>0,s=t−tiTi.T_i=t_{i+1}-t_i>0, \qquad s=\frac{t-t_i}{T_i}.

这段五次多项式的六个条件为：

Pi(0)=pi,Pi(1)=pi+1,Pi′(0)=Tivi,Pi′(1)=Tivi+1,Pi′′(0)=Ti2ai,Pi′′(1)=Ti2ai+1.\begin{aligned} P_i(0)&=p_i, & P_i(1)&=p_{i+1},\\ P_i'(0)&=T_i v_i, & P_i'(1)&=T_i v_{i+1},\\ P_i''(0)&=T_i^2 a_i, & P_i''(1)&=T_i^2 a_{i+1}. \end{aligned}

它们就是前面两次手算的统一写法。时间和边界状态确定后，每一段都有六个独立条件，可以分别解出六个系数。相邻段使用同一组航点状态，便能按要求衔接。

在二维或三维中，可以分别为各坐标建立这些方程，并使用相同的航点时刻。合成运动的速度、加速度要按向量检查，整条空间曲线也要接受碰撞检测。

2. 改变分段时间时，更新对应方程

本例第一段持续 4 秒，到达 W 时速度为 1，因此使用：

P1′(1)=4×1=4.P_1'(1)=4\times1=4.

如果把第一段改为 5 秒，同时保留 W 处的实际速度 1，新的条件就是：

P1′(1)=5×1=5.P_1'(1)=5\times1=5.

这会改变系数方程，需要重新求解曲线。若第二段仍持续 6 秒，它的起始总时刻变为第 5 秒，归一化时间随之变为 (t−5)/6(t-5)/6。

分段时间、实际边界速度与多项式系数之间有明确的对应关系。 调整其中一项后，沿这组关系重新求解并检查运动。

3. 航点速度与加速度怎样选择？

本例给 W 指定了速度 1、加速度 0。改变这两个数，就会同时改变 W 前后两段的系数与运动曲线。

当任务只指定航点位置和到达时间时，中间速度与加速度可以成为待求变量。每选择一组，就得到一条对应的分段轨迹。

接下来可以引入一个衡量整条运动的标准，例如 Jerk 平方在时间上的积分，比较不同航点状态带来的加速度变化，再求出代价较小的方案。这会进一步引出 Minimum Jerk 轨迹优化。

小结

两段五次曲线可以各自使用局部时间计算，通过共享航点处的位置、速度和加速度，组成一条 C2C^2 连续轨迹。

本例第一段用 4 秒从静止运动到 4 米处，以 1 米/秒交接给第二段；第二段再用 6 秒到达 10 米处并停下。整段最高速度为 1.512 米/秒，加速度大小峰值约为 0.9851 米/秒²。

航点状态连接相邻运动，分段时间决定时间刻度，系数方程确定曲线，整段检查验证运动需求。 在这个基础上，轨迹优化可以继续选择中间状态和时间，使整条运动满足进一步的性能要求。