✨ 引言

在路径规划中，轨迹的平滑性非常关键，特别是在无人机、自动驾驶、机械臂等平台中：

我们不希望轨迹只是避障成功，更希望它平滑、稳定、连续可导，这样运动控制器才易于跟踪。

平滑的数学定义中，一个常见且有效的指标是三阶导数，也就是 Jerk（加加速度）。

本篇将从物理、数学角度推导出 Minimum-Jerk 的代价项，以及其梯度的计算，适配 B 样条控制点优化。

1️⃣ 什么是 Jerk？

Jerk 是加速度的导数，即位置的三阶导数：

📌 关键：Jerk 是控制轨迹平滑程度的三阶导数项

2️⃣ B样条控制点中的 Jerk 差分形式

对于 B 样条轨迹，由控制点 ​\mathbf{P}_i \in \mathbb{R}^2 构成，我们采用三阶差分方式近似 jerk：

• 这是对连续轨迹进行三阶差分的离散近似
• 每一段 jerk 对应 4 个连续控制点

3️⃣ 平滑代价项（Smoothness Cost）

我们对每一段 jerk 进行平方并累加，构建最小化目标：

其中：

• ​N 是控制点的数量
• 每一段 jerk 贡献一项代价
• 共 ​N - 4 段 jerk（因为每段需要 4 个控制点）

✅ 目标：让轨迹在加速度变化上更加平滑，避免剧烈“抖动”。

4️⃣ 为什么是 ​N - 4 段？

因为：

• 每段 jerk 使用 4 个控制点：​P_i, P_{i+1}, P_{i+2}, P_{i+3}
• 所以共有 ​N - 3 组起点，但我们从 ​i = 0 开始，最多可用 ​i = N - 4

📌 举个例子：

假设 ​N = 8 个控制点：

• jerk 可从 ​i = 0 计算到 ​i = 4，共 ​5 段

5️⃣ 梯度推导（对控制点求导）

我们要最小化 ​J_{\text{smooth}}，必须对每个控制点 ​\mathbf{P}_k 求偏导：

设：

代价项：

每个控制点的梯度贡献：

💡 每个 jerk 段只影响这 4 个控制点，因此梯度非常稀疏，适合数值优化！

并且可以看到：

• 中间的 ​P_{i+1} 和 ​P_{i+2} 权重更大（​|\pm6|）
• 说明它们对 jerk 的变化更“敏感”
  👉 也就是说它们的变化对 smoothness cost 影响更大

6️⃣ 代码实现（C++ 示例）

以下是典型的控制点 jerk 平滑项梯度计算方式：

for (int i = 0; i < num_ctrl_points - 3; ++i) {
    Eigen::Vector2d jerk = P.col(i + 3) - 3 * P.col(i + 2)
                         + 3 * P.col(i + 1) - P.col(i);
    cost += jerk.squaredNorm();

    Eigen::Vector2d grad_term = 2.0 * jerk;

    grad[2 * i + 0]     += -grad_term.x();
    grad[2 * i + 1]     += -grad_term.y();

    grad[2 * (i + 1) + 0] += 3.0 * grad_term.x();
    grad[2 * (i + 1) + 1] += 3.0 * grad_term.y();

    grad[2 * (i + 2) + 0] += -3.0 * grad_term.x();
    grad[2 * (i + 2) + 1] += -3.0 * grad_term.y();

    grad[2 * (i + 3) + 0] += grad_term.x();
    grad[2 * (i + 3) + 1] += grad_term.y();
}

✅ 总结一句话：

我们用三阶差分构造 jerk，通过最小化 jerk² 平方和(L2范数)，获得一个连续、平滑、物理意义明确的代价项。

📌 核心公式一览

jerk 差分表达式：

平滑代价项：

每个控制点的梯度贡献：