一架无人机从 0 米处出发，第 4 秒经过 4 米处的航点，第 10 秒到达 10 米处并停下。每段运动使用五次多项式，相邻两段共享航点处的位置、速度和加速度。

给航点指定速度 1 米/秒、加速度 0，就能求出一条轨迹。改变这两个数，会得到其他同样经过航点的运动安排。

轨迹优化进一步回答：按照一个明确的评价标准，航点速度和加速度应该取多少？

本文使用 Jerk 平方积分作为评价标准，沿同一个算例，先求通过速度，再同时求速度与加速度。

一、哪些条件固定，哪些数值可以调整？

通道中的直线路径已经通过碰撞检查，用沿通道的坐标 pp 表示无人机中心的位置。下面计算沿这条直线的参考运动。

航点 W 把运动分为两段：第一段 4 秒，第二段 6 秒。用 ww 表示经过 W 时的速度，用 α\alpha 表示当时的加速度。

条件 起点 S 航点 W 终点 G
到达时刻 0 秒 4 秒 10 秒
位置 0 米 4 米 10 米
速度 0 待求：ww 0
加速度 0 待求：α\alpha 0

途中仍要求：

0≤v(t)≤2 m/s,∣a(t)∣≤1 m/s2.0\le v(t)\le2\,\mathrm{m/s},\qquad |a(t)|\le1\,\mathrm{m/s^2}.

先把 α\alpha 固定为 0，只调整 ww，例如分别以 1、1.5、1.875 米/秒经过 W（方案 A、B、C）。每选择一个速度，两段曲线就分别获得六个边界条件，可以求出全部系数。第一段终点与第二段起点使用相同的 ww，速度在航点处连续。

这里将 w=1w=1、α=0\alpha=0 作为原方案，用来对照后续结果。分段时间始终保持 4 秒和 6 秒。

二、用 Jerk 给整条运动评分

1. Jerk 描述加速度怎样变化

加速度对时间的变化率称为 Jerk（加加速度），记为 j(t)j(t)：

j(t)=da(t)dt=d3p(t)dt3.j(t)=\frac{da(t)}{dt}=\frac{d^3p(t)}{dt^3}.

例如，加速度在 1 秒内从 0 均匀增加到 0.6 m/s20.6\,\mathrm{m/s^2}，这段 Jerk 为：

j=0.6−01=0.6 m/s3.j=\frac{0.6-0}{1}=0.6\,\mathrm{m/s^3}.

相同变化若在 0.2 秒内完成，Jerk 就变成：

j=0.6−00.2=3 m/s3.j=\frac{0.6-0}{0.2}=3\,\mathrm{m/s^3}.

完成同样的加速度变化，时间越短，对应的变化率越大。

2. 为什么使用平方积分？

先把 Jerk 平方，再沿运动时间累积：

J=∫010j(t)2 dt.\boxed{J=\int_0^{10}j(t)^2\,dt}.

平方使正、负 Jerk 都产生非负代价，并让较大的变化率获得更大的权重。积分则把整个运动过程的贡献加起来。

以两段各持续 1 秒的简化记录为例：

Jerk 记录 平方后累计的代价
前 1 秒为 +1+1，后 1 秒为 −1-1 12×1+(−1)2×1=21^2\times1+(-1)^2\times1=2
前 1 秒为 +2+2，后 1 秒为 −2-2 22×1+(−2)2×1=82^2\times1+(-2)^2\times1=8

对连续变化的 Jerk，可以把时间分成很多小段，近似计算：

J≈j12Δt+j22Δt+⋯ .J\approx j_1^2\Delta t+j_2^2\Delta t+\cdots.

时间段不断细分，就得到上面的积分。JJ 的单位为 m2/s5\mathrm{m^2/s^5}，后文的代价数值均使用这个单位。

Minimum Jerk 在满足指定条件的轨迹中，寻找这个积分最小的方案。 本文在两段五次多项式组成的轨迹中进行求解，速度和加速度上限在得到候选解后逐段核验。

三、把航点速度写进两段曲线

1. 第一段：速度 ww 怎样影响系数？

第一段使用归一化时间：

s=t4,0≤t≤4.s=\frac{t}{4},\qquad 0\le t\le4.

起点位置、速度、加速度都为 0，因此五次式只剩：

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

第一段结束时，位置为 4，实际速度为 ww，加速度为 0。对应的方程为：

b3+b4+b5=4,3b3+4b4+5b5=4w,6b3+12b4+20b5=0.\begin{aligned} b_3+b_4+b_5&=4,\\ 3b_3+4b_4+5b_5&=4w,\\ 6b_3+12b_4+20b_5&=0. \end{aligned}

第二行右侧的 4w4w，来自 P1′(1)/4=wP_1'(1)/4=w。归一化时间下的导数需要除以 4，才是实际速度。

与求固定边界的曲线一样，通过消元得到：

b4+2b5=4w−12,b_4+2b_5=4w-12,

3b4+7b5=−12.3b_4+7b_5=-12.

第二式减去第一式的 3 倍：

b5=−12−3(4w−12)=24−12w.b_5=-12-3(4w-12)=24-12w.

代回，依次得到：

b4=28w−60,b3=40−16w.b_4=28w-60,\qquad b_3=40-16w.

于是：

P1(s)=(40−16w)s3+(28w−60)s4+(24−12w)s5.\boxed{ P_1(s)=(40-16w)s^3+(28w-60)s^4+(24-12w)s^5. }

这次求出的系数中保留了 ww。将不同的速度代进去，就得到不同的第一段曲线。

2. 第二段：从同一个速度 ww 接续

第二段使用自己的时间刻度：

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

起点位置为 4，速度为 ww，加速度为 0，因此：

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

6ws6ws 对实际时间求导后，在本段起点给出速度 ww。

把终点位置 10、速度 0、加速度 0 代入，得到：

d3+d4+d5=6−6w,3d3+4d4+5d5=−6w,6d3+12d4+20d5=0.\begin{aligned} d_3+d_4+d_5&=6-6w,\\ 3d_3+4d_4+5d_5&=-6w,\\ 6d_3+12d_4+20d_5&=0. \end{aligned}

使用相同的消元方法：

P2(s)=4+6ws+(60−36w)s3+(48w−90)s4+(36−18w)s5.\boxed{ P_2(s)=4+6ws+(60-36w)s^3+(48w-90)s^4+(36-18w)s^5. }

两式中的 ss 分别是各自的归一化时间。数值公式中，时间按秒计，位置按米计，ww 按米/秒计；后面的 α\alpha 按米/秒²计。相应系数带有匹配的单位。

3. 代入一次，核对曲线的来源

取原方案 w=1w=1，两式变为：

P1(s)=24s3−32s4+12s5,P_1(s)=24s^3-32s^4+12s^5,

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

它们分别满足（位置、速度、加速度）：

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

现在，两段运动都由同一个待求速度 ww 控制。 后面要做的，就是比较不同 ww 对应的总代价。

四、从曲线算出一个关于速度的代价函数

1. 归一化时间怎样进入积分？

设某一段持续 TT 秒，位置表示为 P(s)P(s)，其中 ss 从 0 变化到 1。

连续对实际时间求三次导数：

j=P′′′(s)T3.j=\frac{P'''(s)}{T^3}.

这一段中，归一化时间增加 dsds，实际时间就增加 dt=T dsdt=T\,ds。因此，本段代价为：

∫j2 dt=1T5∫01[P′′′(s)]2 ds.\int j^2\,dt =\frac1{T^5}\int_0^1[P'''(s)]^2\,ds.

其中，Jerk 的平方带来 1/T61/T^6，时间换算再乘上 TT，合起来是 1/T51/T^5。

本例的两段代价相加：

J(w,0)=145∫01[P1′′′(s)]2 ds+165∫01[P2′′′(s)]2 ds.J(w,0)= \frac1{4^5}\int_0^1[P_1'''(s)]^2\,ds + \frac1{6^5}\int_0^1[P_2'''(s)]^2\,ds.

J(w,0)J(w,0) 中的第二个参数表示：这一阶段将航点加速度固定为 0。

2. 先给原方案算一次分数

代入 w=1w=1，第一段的实际 Jerk 为：

j1(s)=144−768s+720s264=94−12s+454s2.j_1(s)=\frac{144-768s+720s^2}{64} =\frac94-12s+\frac{45}{4}s^2.

第二段为：

j2(s)=144−1008s+1080s2216=23−143s+5s2.j_2(s)=\frac{144-1008s+1080s^2}{216} =\frac23-\frac{14}{3}s+5s^2.

两段分别持续 4 秒和 6 秒，所以：

J1=4∫01(94−12s+454s2)2ds=3,J_1=4\int_0^1\left(\frac94-12s+\frac{45}{4}s^2\right)^2ds=3,

J2=6∫01(23−143s+5s2)2ds=89.J_2=6\int_0^1\left(\frac23-\frac{14}{3}s+5s^2\right)^2ds=\frac89.

这些积分可以直接把平方展开，再逐项计算。例如：

∫01sk ds=1k+1.\int_0^1s^k\,ds=\frac1{k+1}.

对于 j(s)=A+Bs+Cs2j(s)=A+Bs+Cs^2，展开后的统一结果为：

∫01j(s)2ds=A2+AB+B2+2AC3+BC2+C25.\int_0^1j(s)^2ds =A^2+AB+\frac{B^2+2AC}{3}+\frac{BC}{2}+\frac{C^2}{5}.

第一段使用 A=9/4A=9/4、B=−12B=-12、C=45/4C=45/4，上式得到 3/43/4，再乘本段的 4 秒，得到 J1=3J_1=3。

原方案的总代价为：

J(1,0)=3+89=359≈3.8889.\boxed{J(1,0)=3+\frac89=\frac{35}{9}\approx3.8889}.

这个数就成为比较其他方案的基准。

3. 保留 ww，得到整条代价曲线

在两段位置公式中保留 ww，执行同样的“求三次导数—平方—积分”。例如，第一段的实际 Jerk 为：

j1(s;w)=(154−32w)+(212w−452)s+(452−454w)s2.\begin{aligned} j_1(s;w) &=\left(\frac{15}{4}-\frac32w\right) +\left(\frac{21}{2}w-\frac{45}{2}\right)s\\ &\quad+\left(\frac{45}{2}-\frac{45}{4}w\right)s^2. \end{aligned}

将三个括号分别作为上一节公式中的 AA、BB、CC，积分后再乘本段的 4 秒，就得到 J1(w)J_1(w)。第二段采用相同计算，乘上对应的 6 秒。结果为：

J1(w)=3w2−454w+454,J_1(w)=3w^2-\frac{45}{4}w+\frac{45}{4},

J2(w)=89w2−103w+103.J_2(w)=\frac89w^2-\frac{10}{3}w+\frac{10}{3}.

相加得到：

J(w,0)=359w2−17512w+17512.\boxed{ J(w,0)=\frac{35}{9}w^2-\frac{175}{12}w+\frac{175}{12}. }

每一个 ww 对应两段完整曲线，这个函数则给出它们的总代价：

航点速度 ww（米/秒） Jerk 平方积分 J(w,0)J(w,0)
1.000 3.8889
1.500 1.4583
1.875 0.9115
2.000 0.9722

这里先比较平滑代价，整段速度与加速度的上限将在后面核验。

五、找出航点速度：1.875 米/秒

代价函数是一个二次函数，可以整理成：

J(w,0)=359(w−158)2+175192.\boxed{ J(w,0)=\frac{35}{9}\left(w-\frac{15}{8}\right)^2+\frac{175}{192}. }

平方项总是大于或等于 0。当：

w=158=1.875 m/sw=\frac{15}{8}=1.875\,\mathrm{m/s}

时，平方项恰好为 0，代价取得最低值：

Jmin⁡, α=0=175192≈0.9115.J_{\min,\,\alpha=0}=\frac{175}{192}\approx0.9115.

航点加速度固定为零时，总代价随航点速度呈抛物线变化，最低点位于每秒 1.875 米

从图上看，原方案 w=1w=1 位于最低点左侧。提高通过速度，能够减少这组运动中的加速度变化；超过最低点后，代价又开始增加。

将 w=1.875w=1.875 代回位置公式：

P1(s)=10s3−7.5s4+1.5s5,P_1(s)=10s^3-7.5s^4+1.5s^5,

P2(s)=4+11.25s−7.5s3+2.25s5.P_2(s)=4+11.25s-7.5s^3+2.25s^5.

比较几个时刻：

总时刻（秒） 原方案速度（米/秒） 只优化 ww 后的速度（米/秒）
0 0 0
2 1.4375 1.0547
4 1.0000 1.8750
7 1.4375 1.0547
10 0 0

原方案在航点前减速到 1，通过后再次加速。新方案在第一段持续加速，在第二段持续减速，航点处以更高速度通过。两种方案都在第 4 秒到达 4 米处，第 10 秒到达终点。

这一步的最优结果，针对“航点加速度固定为零、只调整航点速度”的轨迹集合。

六、再把航点加速度交给优化

1. 增加一个待求数 α\alpha

接下来，让航点加速度也参与选择。第一段末端与第二段起点仍使用相同的 α\alpha，保证加速度连续。

令上一节含 ww 的两段公式分别记为 P1(s;w,0)P_1(s;w,0) 和 P2(s;w,0)P_2(s;w,0)。加入加速度条件后，得到：

P1(s;w,α)=P1(s;w,0)+8α(s3−2s4+s5),P_1(s;w,\alpha) =P_1(s;w,0)+8\alpha(s^3-2s^4+s^5),

P2(s;w,α)=P2(s;w,0)+18α(s2−3s3+3s4−s5).P_2(s;w,\alpha) =P_2(s;w,0)+18\alpha(s^2-3s^3+3s^4-s^5).

这些新增项来自两段的加速度边界方程。它们在相应端点保留原有位置与速度，并调整加速度。

例如，第一段新增项在 s=1s=1 处的二阶导数为 16α16\alpha，除以 424^2 后，正好增加实际加速度 α\alpha。第二段新增项在 s=0s=0 处的二阶导数为 36α36\alpha，除以 626^2 后，同样得到 α\alpha。

2. 重新计算总代价

继续对两段求三次导数、平方并积分，得到：

J(w,α)=359w2−52wα+154α2−17512w+256α+17512.\boxed{ J(w,\alpha)= \frac{35}{9}w^2 -\frac52w\alpha +\frac{15}{4}\alpha^2 -\frac{175}{12}w +\frac{25}{6}\alpha +\frac{175}{12}. }

令 α=0\alpha=0，就回到上一节的代价函数。现在每一组 (w,α)(w,\alpha) 都对应一条完整轨迹和一个分数。

要找这个二次函数的最低点，分别计算两个方向的变化率。

先暂时把 α\alpha 当作常数，对 ww 求导：

∂J∂w=709w−52α−17512.\frac{\partial J}{\partial w} =\frac{70}{9}w-\frac52\alpha-\frac{175}{12}.

再把 ww 当作常数，对 α\alpha 求导：

∂J∂α=−52w+152α+256.\frac{\partial J}{\partial\alpha} =-\frac52w+\frac{15}{2}\alpha+\frac{25}{6}.

符号 ∂\partial 表示只改变其中一个变量时的变化率。令两者为 0，并去掉分母，得到两个线性方程：

56w−18α=105,−3w+9α=−5.\begin{aligned} 56w-18\alpha&=105,\\ -3w+9\alpha&=-5. \end{aligned}

3. 解出两个具体数值

由第二式：

α=w3−59.\alpha=\frac{w}{3}-\frac59.

代入第一式：

56w−18(w3−59)=105,56w-18\left(\frac{w}{3}-\frac59\right)=105,

整理：

56w−6w+10=105,50w=95.56w-6w+10=105, \qquad 50w=95.

因此：

w=1.9 m/s,\boxed{w=1.9\,\mathrm{m/s}},

α=790≈0.07778 m/s2.\boxed{\alpha=\frac7{90}\approx0.07778\,\mathrm{m/s^2}}.

总代价为：

Jmin⁡=385432≈0.8912.\boxed{J_{\min}=\frac{385}{432}\approx0.8912}.

还可以用平方项核对它确实是最低值。记相对这一结果的偏移为：

Δw=w−1910,Δα=α−790.\Delta w=w-\frac{19}{10},\qquad \Delta\alpha=\alpha-\frac7{90}.

同一个代价函数可以整理成：

J=385432+359(Δw−928Δα)2+375112(Δα)2.J=\frac{385}{432} +\frac{35}{9}\left(\Delta w-\frac9{28}\Delta\alpha\right)^2 +\frac{375}{112}(\Delta\alpha)^2.

两个平方项的系数都为正。只有 Δw=0\Delta w=0、Δα=0\Delta\alpha=0 时，两项同时消失，因此得到这组分段轨迹中的唯一最低点。

七、把优化结果还原成运动，再检查约束

1. 求解结果对应哪两条曲线？

将 w=19/10w=19/10、α=7/90\alpha=7/90 代回系数，第一段为：

P1(s)=460s3−362s4+82s545.\boxed{P_1(s)=\frac{460s^3-362s^4+82s^5}{45}}.

第二段为：

P2(s)=4+11.4s+1.4s2−12.6s3+5.4s4+0.4s5.\boxed{ P_2(s)=4+11.4s+1.4s^2-12.6s^3+5.4s^4+0.4s^5. }

整条位置轨迹仍按总时间分段：

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

例如，第 7 秒位于第二段，s=(7−4)/6=0.5s=(7-4)/6=0.5。代入位置公式：

p(7)=4+11.4×0.5+1.4×0.25−12.6×0.125+5.4×0.0625+0.4×0.03125=8.825 米.\begin{aligned} p(7) &=4+11.4\times0.5+1.4\times0.25\\ &\quad-12.6\times0.125+5.4\times0.0625+0.4\times0.03125\\ &=8.825\text{ 米}. \end{aligned}

同样计算速度和加速度：

总时刻（秒） 位置（米） 速度（米/秒） 加速度（米/秒²）
0 0 0 0
2 0.8319 1.0535 0.6931
4 4.0000 1.9000 0.0778
5 5.8848 1.8197 -0.2212
7 8.8250 1.0292 -0.4944
10 10.0000 0 0

航点处加速度略大于零，所以经过 W 后，速度还会短暂增加，随后进入减速阶段。

2. 检查速度和加速度峰值

对每段，仍然采用同一套极值检查方法：求位置的一、二、三阶时间导数，在加速度为零处检查速度极值，在 Jerk 为零处检查加速度极值，并把各段端点一起比较。

计算得到：

检查项目 出现时刻（秒，约） 数值
整段最高速度 4.2298 1.9088 m/s1.9088\,\mathrm{m/s}
最大正加速度 1.6616 +0.7145 m/s2+0.7145\,\mathrm{m/s^2}
最小负加速度 7.1866 −0.4965 m/s2-0.4965\,\mathrm{m/s^2}

端点与各段极值比较后，得到：

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

∣a(t)∣≤0.7146<1 m/s2.|a(t)|\le0.7146<1\,\mathrm{m/s^2}.

速度始终非负，位置从 0 米单调增加到 10 米，沿着原来已经检查过的直线前进。起终状态与航点时刻也都满足要求。

本例先求只受位置、时间和衔接条件限制的最低代价解，再验证运动上限。它通过了全部检查，因此也满足加上这些速度、加速度限制后的问题。

3. 对照三种方案

方案 航点速度 航点加速度 最高速度 加速度大小峰值 Jerk 代价
原方案 1.000 0 1.512 0.9851 3.8889
只优化速度 1.875 0 1.875 0.7217 0.9115
同时优化速度、加速度 1.900 0.0778 1.9088 0.7145 0.8912

表中速度按米/秒计，加速度按米/秒²计，Jerk 代价按米²/秒⁵计。三种方案都使用相同的到达时间和航点位置。

原方案与优化方案的速度对比：优化后以更高速度通过航点，最高速度仍小于每秒 2 米

优化后的速度在前半段逐渐升高，通过航点附近后逐渐降低。原方案中“航点前减速、航点后再加速”的过程被重新安排。

原方案与优化方案的加速度对比：优化结果在航点处保持约 0.0778 的加速度，整段满足正负 1 的限制

加速度也随之改变：更长时间地保持适度加速，再转入减速，降低了本例中的加速度变化代价。

4. Jerk 曲线发生了什么变化？

原方案与优化方案的 Jerk 对照：原方案在航点处发生跳变，优化方案两侧均为负 0.35 米每三次方秒

原方案在航点两侧的 Jerk 为：

j1(1)=1.5,j2(0)=23.j_1(1)=1.5,\qquad j_2(0)=\frac23.

优化后：

j1(1)=j2(0)=−0.35 m/s3.j_1(1)=j_2(0)=-0.35\,\mathrm{m/s^3}.

本例在共同求解 ww 和 α\alpha 后，航点两侧的 Jerk 也连续起来。曲线正、负部分平方后累积，给出前面算出的总代价。

积分衡量的是整段加速度变化的累计程度。某一时刻的 Jerk 峰值、速度上限和加速度上限，各有相应的检查；需要直接限制 Jerk 大小时，可以另外加入 ∣j(t)∣≤jmax⁡|j(t)|\le j_{\max}。

运动区间两端的 Jerk 由本次求解确定。若还要与前后静止段实现 Jerk 连续，可以继续把起终 Jerk 为零写入边界条件，并增加相应的轨迹自由度。

八、这就是一次完整的轨迹优化

整个过程可以整理为：

固定航点位置、到达时间和起终状态

把中间速度、加速度作为待求数

用边界条件把两段系数写成这些数的函数

对轨迹求 Jerk，计算平方积分

求出使总代价最小的航点状态

恢复多项式，检查整段运动

固定航点位置、到达时间和起终状态

把中间速度、加速度作为待求数

用边界条件把两段系数写成这些数的函数

对轨迹求 Jerk，计算平方积分

求出使总代价最小的航点状态

恢复多项式，检查整段运动

其中，ww 和 α\alpha 是决策变量，Jerk 平方积分是目标函数，位置、时间、连续性和运动上限构成约束。

本例的系数随 ww、α\alpha 线性变化，Jerk 也随它们线性变化。平方并积分后，目标就成为关于两个变量的二次函数，因此能通过两个线性方程求出最低点。

增加航点后，可以把各个中间速度和加速度一起作为变量。每个航点同时影响前后两段，所有段的代价相加，形成一个共同求解的问题。

如果某个候选解超出速度或加速度上限，就需要将相应限制纳入求解；到达时间允许调整时，还可以联合选择分段时间。空间曲线还需要按机体模型进行整段碰撞检查。

小结

给航点指定速度和加速度，可以生成一条分段轨迹；把这些数作为变量，再用 Jerk 平方积分比较，就形成了 Minimum Jerk 轨迹优化。

在这个 10 米算例中，原方案以 1 米/秒通过航点，总代价约为 3.8889。只优化通过速度，得到 1.875 米/秒；同时优化速度和加速度，得到 1.9 米/秒与约 0.0778 米/秒²，总代价降到约 0.8912，并满足既定运动上限。

每一个待求数都对应一个航点状态，每一次求导都对应一种运动量，目标函数把整条运动转化成可以比较的分数。 轨迹优化就在这组对应关系中，选出满足要求的运动安排。