Minimum Snap：一步一步求出航点 Jerk

一架无人机从 0 米处出发，第 4 秒经过 4 米处的航点，第 10 秒到达 10 米处并停下。通过航点时，速度给定为 1.9 米/秒，加速度为 $7/90$ 米/秒²。

运动开始前和结束后保持静止。为了让加速度的变化也能与静止阶段衔接，起终点的 Jerk 都设为零，两段运动在航点处使用同一个 Jerk。

现在需要选择的是：航点处的 Jerk 应当取多少，才能让整段运动的 Jerk 变化代价最小？

下面用两段七次多项式，逐步计算这个数，并检查得到的运动。

一、从 Jerk 再向前一步：Snap 是什么？

1. 用变化率描述运动

位置、速度、加速度和 Jerk 依次通过求导联系起来：

$$p(t)\ \longrightarrow\ v(t)\ \longrightarrow\ a(t)\ \longrightarrow\ j(t).$$

每向右一步，都在问前一个量随时间怎样变化。继续对 Jerk 求导，得到 Snap，本文记为 $\sigma(t)$：

$$\boxed{\sigma(t)=\frac{dj(t)}{dt}=\frac{d^4p(t)}{dt^4}}.$$

例如，Jerk 在 1 秒内从 0 均匀增加到 $0.6\,\mathrm{m/s^3}$，这段 Snap 为：

$$\sigma=\frac{0.6-0}{1}=0.6\,\mathrm{m/s^4}.$$

同样的 Jerk 变化，若在 0.2 秒内完成，则：

$$\sigma=\frac{0.6-0}{0.2}=3\,\mathrm{m/s^4}.$$

Snap 描述 Jerk 变化得有多快。 在 Jerk 随时间变化的图上，它对应曲线的斜率。

2. 给整段变化一个分数

将 Snap 平方，再沿时间累积，得到：

$$\boxed{J_4=\int_0^{10}\sigma(t)^2\,dt}.$$

平方让正、负 Snap 都贡献非负代价，积分把各个时刻的贡献加起来。$J_4$ 的单位为 $\mathrm{m^2/s^7}$。

Minimum Snap 使用这个积分作为目标函数，在规定的边界与衔接条件下选择轨迹。

这里有两个需要一起确定的部分：边界条件规定起终 Jerk 为零、航点 Jerk 连续；目标函数比较满足这些条件的运动，衡量整段 Jerk 的变化。

二、把待求数放进运动要求

1. 固定条件与待求量

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

航点 W 把运动分为两段：第一段 4 秒，第二段 6 秒。把航点处的 Jerk 记为 $\kappa$，读作 kappa：

运动量	起点 S	航点 W	终点 G
位置（米）	0	4	10
速度（米/秒）	0	1.9	0
加速度（米/秒²）	0	$7/90\approx0.07778$	0
Jerk（米/秒³）	0	待求：$\kappa$	0

第一段结束时与第二段开始时，都使用同一个 $\kappa$。位置、速度、加速度和 Jerk 四个量由此在航点处连续。

途中要求保持：

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

本例固定航点速度、加速度和分段时间，只优化航点 Jerk。 先求这组条件下的最低 Snap 代价，再检查速度、加速度上限。

2. 为什么使用七次多项式？

每段起点和终点各有四项要求：位置、速度、加速度、Jerk，共八个条件。

选择含八个系数的七次多项式：

$$P(s)=b_0+b_1s+b_2s^2+\cdots+b_7s^7.$$

这里 $s$ 是本段从 0 到 1 的归一化时间。一段持续 $T$ 秒时：

$$v=\frac{P'(s)}T,\qquad a=\frac{P''(s)}{T^2},\qquad j=\frac{P'''(s)}{T^3},\qquad \sigma=\frac{P''''(s)}{T^4}.$$

撇号表示对 $s$ 求导。每求一次实际时间导数，就多除以一次 $T$。

给定 $\kappa$ 后，每段的八个边界条件都已确定，能够求出八个系数。改变 $\kappa$，两段曲线也随之变化。

三、把两段曲线写成航点 Jerk 的函数

1. 第一段：从静止进入运动

第一段使用：

$$s=\frac t4,\qquad 0\le t\le4.$$

起点的位置、速度、加速度和 Jerk 都为零，所以：

$$b_0=b_1=b_2=b_3=0.$$

例如，起点 Jerk 为 $6b_3/4^3$，令它等于零，就得到 $b_3=0$。剩下：

$$P_1(s)=b_4s^4+b_5s^5+b_6s^6+b_7s^7.$$

在本段终点 $s=1$，分别代入位置、速度、加速度和 Jerk：

$$\begin{aligned} b_4+b_5+b_6+b_7&=4,\\ 4b_4+5b_5+6b_6+7b_7&=4\times1.9=\frac{38}{5},\\ 12b_4+20b_5+30b_6+42b_7&=4^2\times\frac7{90}=\frac{56}{45},\\ 24b_4+60b_5+120b_6+210b_7&=4^3\kappa=64\kappa. \end{aligned}$$

每一行都对应表中的一项要求。例如最后一行表示：归一化曲线的三阶导数除以 64，必须得到实际 Jerk $\kappa$。

可以依次消元。第二行减去第一行的 4 倍，得到：

$$b_5+2b_6+3b_7=-\frac{42}{5}.$$

第三行减去第一行的 12 倍，再用上式消去 $b_5$，得到：

$$b_6+3b_7=\frac{92}{9}.$$

第四行先消去 $b_4$，再用这两个关系消去 $b_5$ 和 $b_6$，剩下：

$$6b_7=64\kappa-\frac{584}{15}.$$

于是：

$$b_7=\frac{32}{3}\kappa-\frac{292}{45}.$$

逐个代回，得到全部系数：

系数	关于 $\kappa$ 的表达式
$b_4$	$\dfrac{262}{9}-\dfrac{32}{3}\kappa$
$b_5$	$32\kappa-\dfrac{2174}{45}$
$b_6$	$\dfrac{1336}{45}-32\kappa$
$b_7$	$\dfrac{32}{3}\kappa-\dfrac{292}{45}$

这些数字都来自同一组边界方程。每选一个 $\kappa$，就能算出第一段完整运动。

2. 第二段：从航点状态接续

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

$$s=\frac{t-4}{6},\qquad 4\le t\le10.$$

起点位置为 4，速度为 1.9，加速度为 $7/90$，Jerk 为 $\kappa$。它们直接确定前四项：

$$P_2(s)=4+\frac{57}{5}s+\frac75s^2+36\kappa s^3 +d_4s^4+d_5s^5+d_6s^6+d_7s^7.$$

例如：

$$\frac{(57/5)}6=1.9, \qquad \frac{2(7/5)}{6^2}=\frac7{90}, \qquad \frac{6(36\kappa)}{6^3}=\kappa.$$

它们分别给出本段起始速度、加速度和 Jerk。

再把终点位置 10、速度 0、加速度 0、Jerk 0 代入，解出剩余系数：

系数	关于 $\kappa$ 的表达式
$d_4$	$-144\kappa-32$
$d_5$	$216\kappa+37$
$d_6$	$-144\kappa-\dfrac{57}{5}$
$d_7$	$36\kappa-\dfrac25$

以下数值公式统一采用米、秒为单位，$\kappa$ 按米/秒³计；多项式中的各项系数带有对应的单位。

现在，整条轨迹只剩一个待求数 $\kappa$。它同时改变航点前后的两段运动。

四、把 Snap 积分变成一个二次函数

1. 先求一段曲线的 Snap

对七次多项式求四次导数，低于四次的项都消失：

$$P''''(s)=24b_4+120b_5s+360b_6s^2+840b_7s^3.$$

例如，$b_4s^4$ 连续求导四次：

$$b_4s^4\to4b_4s^3\to12b_4s^2\to24b_4s\to24b_4.$$

对于第一段，代入上一节的系数，再除以 $4^4=256$，得到实际 Snap：

$$\begin{aligned} \sigma_1(s;\kappa) &=\left(\frac{131}{48}-\kappa\right) +\left(15\kappa-\frac{1087}{48}\right)s\\ &\quad+\left(\frac{167}{4}-45\kappa\right)s^2 +\left(35\kappa-\frac{511}{24}\right)s^3. \end{aligned}$$

每个 $\kappa$ 都对应一条具体的 Snap 曲线，后面要累计它的平方。

2. 归一化时间怎样影响代价？

一段持续 $T$ 秒，满足 $dt=T\,ds$。因此：

$$\int\sigma(t)^2dt =\frac1{T^7}\int_0^1[P''''(s)]^2ds.$$

这里，Snap 平方带来 $1/T^8$，时间换算乘上 $T$，合起来为 $1/T^7$。

本例的总代价为：

$$J_4(\kappa)= \frac1{4^7}\int_0^1[P_1''''(s)]^2ds + \frac1{6^7}\int_0^1[P_2''''(s)]^2ds.$$

3. 展开平方，逐项积分

先把一个三次式写成：

$$\sigma(s)=A+Bs+Cs^2+Ds^3.$$

平方展开后，每项都是 $s$ 的幂，可以使用：

$$\int_0^1s^m ds=\frac1{m+1}.$$

例如，平方中的常数项 $A^2$ 积分后为 $A^2$；一次项 $2ABs$ 积分后为 $AB$。把全部项相加：

$$\begin{aligned} \int_0^1\sigma(s)^2ds &=A^2+AB+\frac{B^2+2AC}{3} +\frac{AD+BC}{2}\\ &\quad+\frac{C^2+2BD}{5} +\frac{CD}{3}+\frac{D^2}{7}. \end{aligned}$$

以 $\kappa=0$ 为例，第一段使用：

$$A=\frac{131}{48},\quad B=-\frac{1087}{48},\quad C=\frac{167}{4},\quad D=-\frac{511}{24}.$$

代入后，归一化区间上的积分为 $6023/8640$。第一段实际持续 4 秒，因此：

$$J_{4,1}(0)=4\times\frac{6023}{8640} =\frac{6023}{2160}\approx2.7884.$$

这里积分的是已经换算成实际单位的 $\sigma$，所以只需乘本段时间。

保留 $\kappa$，按同样的方法计算两段，得到：

$$J_{4,1}(\kappa)=4\kappa^2+\frac{13}{12}\kappa+\frac{6023}{2160},$$

$$J_{4,2}(\kappa)=\frac83\kappa^2+\frac{32}{27}\kappa+\frac{6151}{14580}.$$

相加：

$$\boxed{ J_4(\kappa)=\frac{20}{3}\kappa^2 +\frac{245}{108}\kappa +\frac{37445}{11664}. }$$

一个航点 Jerk，决定两段轨迹；两段 Snap 的平方积分，给这个选择打出一个分数。

比较几组取值：

航点 Jerk $\kappa$（米/秒³）	第一段代价	第二段代价	总代价 $J_4$
-0.500	3.2468	0.4960	3.7427
-0.350	2.8993	0.3337	3.2330
0	2.7884	0.4219	3.2103
0.200	3.1651	0.7656	3.9307

表中所有方案使用相同的时间和边界要求，代价单位均为米²/秒⁷。最低点可以继续通过这个二次函数求出。

五、求出最优航点 Jerk

对代价函数求导：

$$\frac{dJ_4}{d\kappa}=\frac{40}{3}\kappa+\frac{245}{108}.$$

在最低点，变化率为零。令上式等于零，两边乘以 108：

$$1440\kappa+245=0.$$

得到：

$$\boxed{\kappa=-\frac{49}{288}\approx-0.17014\,\mathrm{m/s^3}}.$$

还可以把代价整理成平方形式：

$$\boxed{ J_4(\kappa)=\frac{20}{3} \left(\kappa+\frac{49}{288}\right)^2 +\frac{563105}{186624}. }$$

平方项大于或等于零，只有 $\kappa=-49/288$ 时恰好为零。因此，它是本例固定时间、航点速度和加速度条件下，两段七次曲线的唯一最低代价解。

最低代价为：

$$J_{4,\min}=\frac{563105}{186624}\approx3.0173.$$

将 $\kappa$ 从 0 调到这个值，第一段代价从约 2.7884 降到 2.7199，第二段从约 0.4219 降到 0.2974，总代价随之下降。

航点加速度为 $7/90>0$，Jerk 则为负。这两个量描述不同的变化：此时速度还在增加，加速度正在减小。

六、恢复曲线，看它怎样运动

1. 代回系数，得到两段位置公式

将 $\kappa=-49/288$ 代入，第一段为：

$$\boxed{ P_1(s)=\frac{835}{27}s^4-\frac{2419}{45}s^5 +\frac{527}{15}s^6-\frac{1121}{135}s^7. }$$

第二段为：

$$\boxed{ \begin{aligned} P_2(s)&=4+\frac{57}{5}s+\frac75s^2-\frac{49}{8}s^3 -\frac{15}{2}s^4\\ &\quad+\frac14s^5+\frac{131}{10}s^6-\frac{261}{40}s^7. \end{aligned} }$$

按总时间写成：

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

例如，第 2 秒位于第一段，$s=2/4=0.5$。位置为：

$$\begin{aligned} p(2)&=\frac{835}{27}\left(\frac12\right)^4 -\frac{2419}{45}\left(\frac12\right)^5\\ &\quad+\frac{527}{15}\left(\frac12\right)^6 -\frac{1121}{135}\left(\frac12\right)^7\\ &=\frac{12737}{17280}\approx0.7371\text{ 米}. \end{aligned}$$

在同一个 $s=0.5$ 处，计算 $P_1'/4$、$P_1''/16$ 和 $P_1'''/64$，就得到速度、加速度和 Jerk。

总时刻（秒）	位置（米）	速度（米/秒）	加速度（米/秒²）	Jerk（米/秒³）
0	0	0	0	0
2	0.7371	1.0859	0.8353	-0.2689
4	4.0000	1.9000	0.0778	-0.1701
7	8.9771	1.0462	-0.5959	-0.0562
10	10.0000	0	0	0

第 4 秒的四个量同时构成第一段末端与第二段起点的状态。

2. Jerk 怎样接到静止阶段？

下图使用一条五次轨迹作为对照。它采用相同的时间、位置、速度和加速度条件，以 Jerk 平方积分为目标求得；起终 Jerk 分别为 $23/24$ 和 $13/36\,\mathrm{m/s^3}$。

本篇七次曲线进一步指定起终 Jerk 为零。

运动前后的静止段中，速度、加速度和 Jerk 都为零。本篇曲线的这三个量在两端也都为零，位置则分别接到 0 米和 10 米处。

航点两侧同样满足：

$$\begin{aligned} p(4^-)&=p(4^+)=4,\\ v(4^-)&=v(4^+)=1.9,\\ a(4^-)&=a(4^+)=\frac7{90},\\ j(4^-)&=j(4^+)=-\frac{49}{288}. \end{aligned}$$

$4^-$ 和 $4^+$ 分别表示从第 4 秒之前、之后接近这个时刻。两侧位置及前三阶导数都接得上，称为 $C^3$ 连续。

图中两种方案的边界条件有所不同，因此这里观察的是它们的衔接与运动形态；Snap 代价的高低，则在前面那组具有相同条件的七次曲线之间比较。

3. Snap 曲线显示了什么？

Snap 为正时，Jerk 上升；Snap 为负时，Jerk 下降。曲线的正负变化共同塑造了上一张图中的 Jerk。

本例最优解在航点两侧还满足：

$$\sigma(4^-)=\sigma(4^+)=-\frac5{36} \approx-0.1389\,\mathrm{m/s^4}.$$

这个关系可以直接由两段位置公式求四次导数核对，它是本次优化产生的结果。

在运动区间两端，Snap 与静止段的零值之间仍有有限跳变。本文规定的整体连续性到 Jerk 为止。

七、检查速度与加速度上限

1. 查询整段曲线的极值

要核验上限，需要检查每一段内部可能出现的极值，再与段端点比较。

要检查的量	变化率	需要计算的候选时刻
速度 $v$	加速度 $a$	每段满足 $a=0$ 的时刻，以及段端点
加速度 $a$	Jerk $j$	每段满足 $j=0$ 的时刻，以及段端点

计算时，只保留归一化区间 $0\le s\le1$ 内的实根。求出根后，再用本段开始时刻和持续时间换回总时间。

例如，第二段速度峰值对应 $s\approx0.065813$。换回总时间：

$$t=4+6s\approx4.3949\text{ 秒}.$$

把各段候选点一起比较，得到：

检查项目	总时刻（秒，约）	数值
最高速度	4.3949	$1.9160\,\mathrm{m/s}$
最大正加速度	1.6940	$+0.8783\,\mathrm{m/s^2}$
最小负加速度	7.2151	$-0.6020\,\mathrm{m/s^2}$

整段速度保持非负。结合端点和全部极值：

$$0\le v(t)\le1.9161<2\,\mathrm{m/s},$$

$$|a(t)|\le0.8783<1\,\mathrm{m/s^2}.$$

因此，本例求得的最低代价解同时满足速度和加速度限制。位置也从 0 米单调增加到 10 米，始终沿着已检查过的直线路径运动。

2. 运动上限与平滑代价分别观察

加入起终 Jerk 条件后，曲线在整个时间区间内重新分配运动。与图中的五次曲线相比，本篇方案的速度峰值、加速度峰值略有增加，同时满足既定上限。

平方积分评价整段变化，运动上限约束某个量允许达到多大。 两者共同决定最终可用的方案。需要限制 Snap 峰值时，可以再规定 $|\sigma(t)|\le\sigma_{\max}$。

八、从这个算例理解 Minimum Snap

本例的求解过程可以整理成：

这里的航点速度 1.9 和加速度 $7/90$ 都是固定条件。得到的 $\kappa=-49/288$，是在这组固定条件下的最优选择。

当任务允许同时选择航点速度、加速度和 Jerk 时，可以把这三个量一起作为决策变量；增加航点后，再把各航点的待求状态放进同一个问题。固定分段时间时，边界方程使系数随这些变量线性变化，平方积分由此形成一个二次目标。

Minimum Jerk 与 Minimum Snap 的评价对象可以这样对应：

方法	目标函数	评价的变化
Minimum Jerk	$\displaystyle\int j(t)^2dt$	加速度的变化
Minimum Snap	$\displaystyle\int\sigma(t)^2dt$	Jerk 的变化

它们各自配合明确的边界、连续性和运动约束使用。比较方案的代价时，需要统一目标函数、时间和约束条件。

小结

在这个两段、10 米的算例中，给起终点增加 Jerk 为零的条件，再把航点 Jerk 作为变量，就得到了一组能够连续衔接的七次轨迹。

Snap 平方积分将这组曲线转化为一个关于 $\kappa$ 的二次函数。最低点为 $\kappa=-49/288\approx-0.17014$ 米/秒³，恢复后的轨迹满足指定航点状态，并通过了速度、加速度上限检查。

边界条件决定曲线怎样衔接，目标函数决定在可选曲线中怎样取舍，区间检查决定运动需求是否符合限制。

轨迹已经给出了位置、速度、加速度以及更高阶变化。接下来可以进一步研究：执行这段运动需要怎样的推力与姿态，以及这些要求怎样通过多旋翼动力学计算出来。