从Minimum-jerk到Minimum-snap：无人机轨迹优化的数学内核与工程抉择

当你第一次看到无人机流畅地穿过狭窄的窗户，或者机械臂丝滑地完成一个复杂的抓取动作时，有没有想过背后是什么在驱动着这些优雅的运动？这不仅仅是简单的“点到点”移动，而是一场隐藏在多项式系数里的数学舞蹈。对于算法工程师和研究者来说，理解这场舞蹈的核心——轨迹优化——是解锁高性能运动控制的关键。今天，我们不谈空洞的概念，直接切入最经典的两个范式：Minimum-jerk（最小加加速度）和Minimum-snap（最小加加加速度），看看它们如何从数学公式演变为驱动无人机、机器人乃至自动驾驶汽车的灵魂。

这两种方法听起来像是学术论文里的术语，但它们解决的问题极其实际：如何让一个运动体在指定的时间内，平滑、高效、安全地从A点移动到B点，并经过一系列中间点。平滑性直接关系到能耗、机械磨损和乘坐体验（如果是载具的话），而效率则关乎任务完成速度。Minimum-jerk和Minimum-snap提供了两种不同的“审美”标准，来量化什么是“好”的轨迹。选择哪一种，远非一个简单的阶次差异问题，它背后是系统动力学、执行器约束与计算资源的深度博弈。

1. 轨迹优化的本质：从路径点到时间函数的艺术

路径规划告诉你“去哪里”，给出一系列空间坐标点，就像在地图上标记了几个必经的城市。但轨迹规划要回答“怎么去”和“以什么状态去”的问题。它需要为每个路径点分配时间，并生成一条关于时间的连续函数，这个函数不仅要经过这些点，还要让运动状态（如速度、加速度）的变化令人满意。

一个常见的误区是认为路径点连接得越直、越短越好。实际上，对于高速运动的无人机或机械臂，一条曲折但状态变化平滑的轨迹，远比一条笔直但需要急停急启的轨迹更优。

为什么选择多项式？因为它数学性质良好，易于求导和积分，能方便地表达位置、速度、加速度乃至更高阶的导数。对于一个在 n 维空间运动的物体，我们通常为每个维度独立规划一条多项式轨迹。一条 M 段、每段为 N 阶多项式的轨迹，其数学形式可以表示为：

对于第 i 段轨迹，在时间区间 [t_{i-1}, t_i] 内：

p_i(t) = c_{i,0} + c_{i,1}t + c_{i,2}t^2 + ... + c_{i,N}t^N

其中，c_{i,j} 就是我们需要优化的系数向量。整个轨迹优化问题，就是寻找一组最优的系数 c，使得某个“代价”最小，同时满足一系列约束条件（如起点/终点状态、中间点位置、连续性等）。

这里就引出了核心的优化目标。我们究竟要最小化什么？

• Minimum-jerk：最小化加加速度（Jerk，加速度的导数）的平方积分。Jerk 直接影响运动的平滑度和舒适度。人类对加加速度非常敏感，例如电梯启动时的“推背感”或汽车急刹时的“点头”，都是高Jerk的表现。
• Minimum-snap：最小化加加加速度（Snap，Jerk的导数）的平方积分。Snap 与执行器（如电机的力矩变化率）的能耗和应力更为相关。

为了直观对比两者关注的物理量差异，可以参考下表：

优化目标	最小化的量	物理意义	主要影响系统
Minimum-jerk	Jerk (加加速度)	加速度的变化率	运动平滑性、乘坐舒适度、机械振动
Minimum-snap	Snap (加加加速度)	Jerk的变化率	执行器力矩变化率、能量消耗、结构应力

从表格可以看出，选择哪种目标，取决于你的系统更关心“舒适”还是“节能”。对于载人飞行器或相机云台，平滑无抖动的画面至关重要，Minimum-jerk往往是首选。而对于需要长时间飞行、对能耗极其敏感的无人机，或者需要快速响应、执行器输出受限的机械臂，Minimum-snap可能带来更实际的收益。

2. 数学建模：如何将直觉转化为二次规划问题

理解了物理意义，我们来看看如何用数学语言描述它。无论是Minimum-jerk还是Minimum-snap，其优化目标都可以统一表述为最小化某个导数 k 的平方积分。

对于第 i 段轨迹，其 k 阶导数的平方积分为：

J_i = ∫_{t_{i-1}}^{t_i} [p_i^{(k)}(t)]^2 dt

其中 p_i^{(k)}(t) 表示第 k 阶导数。我们的总目标就是最小化所有段的和：J = Σ J_i。

关键在于，这个积分可以转化为优化变量（多项式系数 c_i）的二次型。以Minimum-jerk为例，k=3。对于一段五阶多项式 p(t) = c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5，其三阶导数为：

p^{(3)}(t) = 6*c3 + 24*c4*t + 60*c5*t^2

我们可以将其写成向量形式：p^{(3)}(t) = [0, 0, 0, 6, 24t, 60t^2] · c，其中 c = [c0, c1, c2, c3, c4, c5]^T。

那么，[p^{(3)}(t)]^2 = c^T * (A(t)) * c，其中 A(t) 是一个由 [0, 0, 0, 6, 24t, 60t^2]^T 与其自身外积得到的矩阵。对这个矩阵在时间段内积分，我们就得到了一个常数矩阵 Q_i：

J_i = ∫_{t_{i-1}}^{t_i} c^T * A(t) * c dt = c^T * (∫_{t_{i-1}}^{t_i} A(t) dt) * c = c^T * Q_i * c

Q_i 是一个实对称矩阵，只与时间区间和导数阶次 k 有关，可以预先解析计算出来。

将所有段的系数向量 c_i 拼接成总向量 P，并将各段的 Q_i 矩阵放在块对角线上组成大矩阵 Q，总目标函数就变成了一个标准的二次型：

J(P) = P^T * Q * P

这就是一个二次规划（Quadratic Programming, QP）问题的目标函数。 QP问题是凸优化问题，存在高效可靠的求解器（如OSQP、CVXOPT等），这也是Minimum-jerk/snap方法得以广泛应用的重要原因。

注意：这里我们省略了具体的 Q_i 矩阵积分结果，因为它是一个固定的计算过程。在实际编程中，我们可以直接调用封装好的函数来生成这个矩阵，而不必每次都手动推导。

仅有目标函数还不够，我们必须施加约束，否则“最优”解可能是一条根本不经过路径点的轨迹。主要约束有两类：

1. 导数约束（边界约束与路径点约束）：规定了轨迹在特定时间点的状态。

   • 起点/终点约束：通常指定起始和终止时刻的位置、速度、加速度（对于jerk）或再加上jerk（对于snap）。例如，无人机从静止状态起飞并悬停，则起点和终点的速度、加速度都应为零。
   • 中间点约束：轨迹必须精确经过路径规划给出的中间点，即 p_i(t_i) = waypoint_i。

2. 连续性约束：保证相邻两段轨迹在连接点处平滑过渡。

   • 对于Minimum-jerk，我们通常要求位置、速度、加速度连续。
   • 对于Minimum-snap，则要求位置、速度、加速度、jerk连续。

所有这些约束都是关于多项式系数 P 的线性等式约束，可以统一写成 A_eq * P = b_eq 的形式。于是，整个轨迹优化问题就转化为一个经典的、带线性等式约束的二次规划问题：

minimize    (1/2) * P^T * Q * P
subject to  A_eq * P = b_eq

求解这个QP问题，得到最优系数 P*，就得到了我们梦寐以求的平滑轨迹。

3. 阶次选择与约束配置：平衡自由度与平滑性

现在我们来回答一个关键问题：多项式到底应该选几阶？ 这不是随意设定的，而是由约束条件数量决定的。

一个 N 阶多项式有 N+1 个系数，即 N+1 个自由度。我们需要用这些自由度来满足所有约束。以Minimum-jerk为例：

• 每段轨迹需要满足的约束包括：
  • 起点约束：位置、速度、加速度 (3个)
  • 终点约束：位置、速度、加速度 (3个)
  • 中间点位置约束 (1个，如果该点是路径点)
  • 与前后段轨迹的连续性约束（位置、速度、加速度连续，共3个，对于中间段）

如果我们只有一段轨迹（两个路径点），那么总约束数至少为6（起点3个 + 终点3个）。因此，多项式至少需要6个自由度，即5阶多项式。

对于Minimum-snap，我们通常还会约束起点和终点的Jerk，因此每端至少需要4个约束（位置、速度、加速度、jerk），两端共8个约束。所以，多项式至少需要8个自由度，即7阶多项式。

下表总结了不同优化目标下的典型多项式阶次选择：

优化目标	最小阶数	通常选用阶数	原因
Minimum-jerk	5阶	5阶或7阶	5阶满足基本约束；7阶可提供额外自由度以优化其他指标（如总时间）。
Minimum-snap	7阶	7阶或9阶	7阶满足基本约束；9阶提供更多优化空间，常用于复杂约束场景。

在实际应用中，我们常常会选择比最小阶数更高的多项式，例如用7阶多项式做Minimum-jerk优化。多出来的自由度可以用来做什么？一个重要的应用是时间分配优化。固定阶次下，更高的自由度允许我们在满足所有硬约束的同时，进一步优化轨迹的总时间或某个性能指标，而不是简单地使用均分的时间段。

约束的配置也是一门艺术。除了必须的等式约束，我们还可以引入不等式约束来反映物理限制，例如：

• 执行器限幅：速度、加速度、jerk不得超过电机或舵机的最大输出能力。
• 动态避障：在轨迹的某些时间段内，位置必须保持在安全区域内。

加入不等式约束后，问题变成了带线性等式和不等式约束的二次规划，依然可解，但求解复杂度会增加。一个实用的技巧是，可以先在无不等式约束下求解，得到轨迹后检查是否违反限制。如果违反，则可以通过添加“走廊约束”（即允许轨迹在一个通道内运动）或调整时间分配来迭代求解。

4. 从理论到代码：一个Minimum-snap的Python实战案例

理论说得再多，不如一行代码有说服力。让我们抛开复杂的符号推导，直接看一个在二维平面上实现Minimum-snap轨迹规划的简化版Python示例。我们将使用 cvxopt 库来求解QP问题。

假设我们有4个路径点，希望生成一条平滑轨迹。我们将分别规划X轴和Y轴方向的运动。

import numpy as np
from cvxopt import matrix, solvers
import matplotlib.pyplot as plt

# 1. 定义路径点和时间分配
waypoints = np.array([[0, 0], [2, 3], [5, 4], [8, 1]])  # (x, y)坐标
num_segments = len(waypoints) - 1
total_time = 10.0  # 总时间
segment_times = np.linspace(0, total_time, num_segments + 1)  # 均分时间

# 2. 设置优化参数
k = 4  # 优化Snap，即最小化4阶导数
poly_order = 2 * k - 1  # 最小阶数为7
n_coeff = poly_order + 1  # 系数个数，8个
dim = 2  # 二维空间，x和y

# 3. 构建目标函数矩阵 Q
def compute_Q_matrix(T_start, T_end, k, n_coeff):
    """计算单段轨迹的Q矩阵（Snap积分）"""
    Q = np.zeros((n_coeff, n_coeff))
    # 计算Q矩阵的解析形式（这里简化，实际需按公式积分）
    # 对于snap (k=4)，Q矩阵中只有与c4, c5, c6, c7相关的项非零
    # 以下是一个示意性的填充，真实实现需要精确计算积分
    for i in range(k, n_coeff):
        for j in range(k, n_coeff):
            # 积分项 ∫ t^{i+j-2k} dt 从 T_start 到 T_end
            exponent = i + j - 2*k + 1
            coeff_i = np.math.factorial(i) / np.math.factorial(i - k)
            coeff_j = np.math.factorial(j) / np.math.factorial(j - k)
            Q[i, j] = coeff_i * coeff_j * (T_end**exponent - T_start**exponent) / exponent
    return Q

# 构建全局Q矩阵（块对角）
n_total = num_segments * n_coeff
Q_global = np.zeros((dim * n_total, dim * n_total))
for seg in range(num_segments):
    t_start, t_end = segment_times[seg], segment_times[seg+1]
    Q_seg = compute_Q_matrix(t_start, t_end, k, n_coeff)
    # 分别填充x和y维度的Q矩阵块
    start_idx = seg * n_coeff
    end_idx = (seg + 1) * n_coeff
    Q_global[start_idx:end_idx, start_idx:end_idx] = Q_seg  # x维度
    Q_global[n_total+start_idx:n_total+end_idx, n_total+start_idx:n_total+end_idx] = Q_seg  # y维度

# 4. 构建约束矩阵 A_eq 和 b_eq
# 约束数量: 起点(4) + 终点(4) + 中间点位置(2) + 连续性(3* (num_segments-1))
n_constraints = 4 + 4 + (num_segments - 1) + 3 * (num_segments - 1)
A_eq = np.zeros((n_constraints, dim * n_total))
b_eq = np.zeros(n_constraints)
constraint_idx = 0

def derivative_vector(t, deriv_order, n_coeff):
    """生成在时间t处，求deriv_order阶导数的系数行向量"""
    vec = np.zeros(n_coeff)
    for i in range(deriv_order, n_coeff):
        coeff = 1.0
        for j in range(deriv_order):
            coeff *= (i - j)
        vec[i] = coeff * (t ** (i - deriv_order))
    return vec

# 起点约束 (位置、速度、加速度、jerk = 0)
for d in range(4):  # 0: pos, 1: vel, 2: acc, 3: jerk
    vec = derivative_vector(segment_times[0], d, n_coeff)
    A_eq[constraint_idx, 0:n_coeff] = vec  # x轴
    b_eq[constraint_idx] = waypoints[0, 0] if d == 0 else 0.0
    constraint_idx += 1
    A_eq[constraint_idx, n_total:n_total+n_coeff] = vec  # y轴
    b_eq[constraint_idx] = waypoints[0, 1] if d == 0 else 0.0
    constraint_idx += 1

# 终点约束
for d in range(4):
    vec = derivative_vector(segment_times[-1], d, n_coeff)
    seg = num_segments - 1
    start_idx = seg * n_coeff
    A_eq[constraint_idx, start_idx:start_idx+n_coeff] = vec  # x轴
    b_eq[constraint_idx] = waypoints[-1, 0] if d == 0 else 0.0
    constraint_idx += 1
    A_eq[constraint_idx, n_total+start_idx:n_total+start_idx+n_coeff] = vec  # y轴
    b_eq[constraint_idx] = waypoints[-1, 1] if d == 0 else 0.0
    constraint_idx += 1

# 中间点位置约束
for wp_idx in range(1, num_segments):
    t = segment_times[wp_idx]
    vec = derivative_vector(t, 0, n_coeff)  # 位置约束
    seg = wp_idx - 1  # 该点位于前一段的终点
    start_idx = seg * n_coeff
    A_eq[constraint_idx, start_idx:start_idx+n_coeff] = vec  # x轴
    b_eq[constraint_idx] = waypoints[wp_idx, 0]
    constraint_idx += 1
    A_eq[constraint_idx, n_total+start_idx:n_total+start_idx+n_coeff] = vec  # y轴
    b_eq[constraint_idx] = waypoints[wp_idx, 1]
    constraint_idx += 1

# 段间连续性约束 (位置、速度、加速度连续)
for seg in range(num_segments - 1):
    t = segment_times[seg + 1]  # 连接点时间
    for d in range(3):  # 连续到加速度
        vec = derivative_vector(t, d, n_coeff)
        # 前一段的结尾
        start_idx_current = seg * n_coeff
        A_eq[constraint_idx, start_idx_current:start_idx_current+n_coeff] = vec
        # 后一段的开头 (减去)
        start_idx_next = (seg + 1) * n_coeff
        A_eq[constraint_idx, start_idx_next:start_idx_next+n_coeff] = -vec
        b_eq[constraint_idx] = 0.0
        constraint_idx += 1
        # y轴同理
        A_eq[constraint_idx, n_total+start_idx_current:n_total+start_idx_current+n_coeff] = vec
        A_eq[constraint_idx, n_total+start_idx_next:n_total+start_idx_next+n_coeff] = -vec
        b_eq[constraint_idx] = 0.0
        constraint_idx += 1

# 5. 求解QP问题
P = matrix(2 * Q_global)  # cvxopt要求标准形式为 (1/2)x^T P x，所以我们的Q要乘2
q = matrix(np.zeros(dim * n_total))
G = matrix(np.zeros((1, dim * n_total)))  # 无不等式约束，设为空
h = matrix(np.zeros(1))
A = matrix(A_eq)
b = matrix(b_eq)

sol = solvers.qp(P, q, G, h, A, b)
coeff_all = np.array(sol['x']).flatten()

# 6. 提取系数并生成轨迹
coeff_x = coeff_all[:n_total].reshape((num_segments, n_coeff))
coeff_y = coeff_all[n_total:].reshape((num_segments, n_coeff))

# 采样和绘图
plt.figure(figsize=(12, 4))
# 位置轨迹
plt.subplot(1, 3, 1)
for seg in range(num_segments):
    t_vals = np.linspace(segment_times[seg], segment_times[seg+1], 50)
    poly_vals = np.vstack([t_vals**i for i in range(n_coeff)]).T
    x_vals = poly_vals @ coeff_x[seg]
    y_vals = poly_vals @ coeff_y[seg]
    plt.plot(x_vals, y_vals, 'b-')
plt.plot(waypoints[:, 0], waypoints[:, 1], 'ro', label='Waypoints')
plt.xlabel('X')
plt.ylabel('Y')
plt.title('Optimized Trajectory (Position)')
plt.axis('equal')
plt.grid(True)
plt.legend()

# 速度曲线
plt.subplot(1, 3, 2)
for seg in range(num_segments):
    t_vals = np.linspace(segment_times[seg], segment_times[seg+1], 50)
    # 速度是位置的一阶导数
    poly_vel = np.vstack([(i)*t_vals**(i-1) if i>0 else 0*t_vals for i in range(n_coeff)]).T
    vx_vals = poly_vel @ coeff_x[seg]
    vy_vals = poly_vel @ coeff_y[seg]
    speed = np.sqrt(vx_vals**2 + vy_vals**2)
    plt.plot(t_vals, speed, 'g-')
plt.xlabel('Time (s)')
plt.ylabel('Speed')
plt.title('Speed Profile')
plt.grid(True)

# 加速度曲线
plt.subplot(1, 3, 3)
for seg in range(num_segments):
    t_vals = np.linspace(segment_times[seg], segment_times[seg+1], 50)
    # 加速度是位置的二阶导数
    poly_acc = np.vstack([(i*(i-1))*t_vals**(i-2) if i>1 else 0*t_vals for i in range(n_coeff)]).T
    ax_vals = poly_acc @ coeff_x[seg]
    ay_vals = poly_acc @ coeff_y[seg]
    acc = np.sqrt(ax_vals**2 + ay_vals**2)
    plt.plot(t_vals, acc, 'r-')
plt.xlabel('Time (s)')
plt.ylabel('Acceleration')
plt.title('Acceleration Profile')
plt.grid(True)

plt.tight_layout()
plt.show()

这段代码勾勒了一个完整的Minimum-snap轨迹生成流程。运行后，你会得到三条曲线：平滑连接所有路径点的空间轨迹、连续的速度曲线以及连续的加速度曲线。你可以尝试修改起点/终点的速度、加速度约束，或者增加中间点，观察轨迹如何变化。在实际工程中，还需要考虑数值稳定性、求解器选择（cvxopt 对于小规模问题不错，大规模问题更推荐 OSQP 或 qpOASES）以及如何高效地构建约束矩阵。

5. 超越多项式：现代轨迹优化的挑战与演进

尽管Minimum-jerk/snap方法因其数学简洁和易于实现而广受欢迎，但在面对更复杂的现实场景时，它们也显露出一些局限性。纯多项式轨迹优化并非银弹，了解其边界能帮助我们在正确的地方使用它，或在需要时寻找更强大的工具。

局限性分析：

• 时间分配敏感：轨迹质量严重依赖于预先分配的各段运行时间。如果时间分配不合理，即使是最优解也可能出现超调或违反动力学约束。动态时间分配本身就是一个复杂的优化问题。
• 障碍物回避困难：将避障作为硬约束（不等式约束）引入QP问题，会使问题规模急剧增大，求解变慢。通常采用“走廊法”，即约束轨迹必须在安全的凸多面体通道内，但这需要预先规划好安全通道。
• 高阶多项式数值病态：当多项式阶数较高（如15阶以上）或时间区间很长时，用于构建约束矩阵的 Vandermonde 类矩阵可能条件数很差，导致求解不稳定。
• 全局最优性：QP求解的是凸问题下的全局最优，但这是在给定时间分配和路径点下的“局部”最优。整个运动规划问题的全局最优，还需要结合上层的路径搜索（如A*, RRT*）。

前沿的解决方案与混合思路：

1. 微分平坦性（Differential Flatness）：对于像四旋翼无人机这样的系统，其全部状态和输入可以用几个“平坦输出”（通常是位置及其导数）的代数组合来表示。这意味着我们可以直接在平坦输出空间（如位置）进行Minimum-snap规划，然后通过微分平坦变换自动得到可行的姿态和控制输入，极大地简化了规划问题。这是目前无人机轨迹规划的主流方法。

2. B样条轨迹优化：B样条具有局部支撑性和凸包性，修改一个控制点只影响局部轨迹，且轨迹自然落在控制点构成的凸包内，这为动态避障和实时交互提供了便利。将Minimum-snap的目标函数与B样条结合，是近年来的一个研究热点。

3. 软约束与惩罚函数：与其将避障、动力学限幅作为硬约束，不如将其作为惩罚项加入目标函数。例如，在目标函数中加入一项对靠近障碍物的惩罚，或者对超过最大加速度的值进行二次惩罚。这样问题仍然是一个无约束或简单约束的优化问题，可以用梯度下降、牛顿法等更通用的优化器求解，虽然可能无法严格保证约束，但更灵活高效。

4. 与模型预测控制（MPC）结合：在更复杂的动态环境中，可以将轨迹优化器作为MPC的上层，实时生成参考轨迹，而MPC底层控制器负责跟踪轨迹并处理高频扰动和模型不确定性。这种分层结构兼顾了长时域的优化和短时域的鲁棒控制。

在我参与的多个无人机集群项目中，最终的方案往往是混合的：在全局层面使用基于采样的路径规划器（如RRT*）生成粗略的、无碰撞的路径点；然后在局部，采用考虑微分平坦性的Minimum-snap生成精细、平滑的轨迹；最后通过一个快速的MPC或几何控制器进行跟踪。这种组合拳在实践中被证明是可靠且高效的。选择Minimum-jerk还是Minimum-snap，往往取决于实际硬件的测试：给飞行器分别加载两种轨迹，用高帧率相机观察机体的振动，或者用电流计测量电机的功耗，数据会给你最直接的答案。数学的优雅最终要服务于工程的稳定，这才是轨迹优化的真正终点。