3. 优化问题建模：目标函数设计（最小化加速度、最小化 jerk）
好，咱们接着聊。上一节我们把优化问题的“骨架”搭起来了——变量、约束都有了。但光有骨架不行，你得告诉优化器：“嘿，什么样的轨迹才是好轨迹？”
这个“好”的标准，就是目标函数。
我个人习惯把目标函数比作“裁判的评分标准”。你想想看，同样的起点终点，有的轨迹开起来像坐过山车，有的像在冰面上滑行。为什么？因为裁判（目标函数）给不同的行为打了不同的分。
3.1 为什么是加速度和 jerk？
先问个问题：什么样的轨迹坐着舒服？
答案其实很直观——不颠簸、不突兀。你坐在车里，如果司机一脚油门一脚刹车，你肯定不舒服。这就是加速度在作怪。加速度大，推背感强，乘客难受。
那 jerk 呢？jerk 是加速度的变化率。说白了，就是“加速度变化得有多猛”。
举个例子：
- 如果加速度从 0 突然跳到 3 m/s²，jerk 无穷大——你会被猛地推一下。
- 如果加速度从 0 平滑地增加到 3 m/s²，jerk 很小——你几乎感觉不到。
我在项目中遇到过这样的情况：只优化加速度，结果轨迹虽然加速度不大，但 jerk 忽高忽低。乘客反馈说“这车怎么一窜一窜的”。嗯，这就是忽略了 jerk 的后果。
3.2 目标函数的数学形式
好，咱们上公式。别怕，很简单的。
假设我们有 N 个轨迹点，每个点的状态包括位置 p、速度 v、加速度 a。那么目标函数可以写成：
minimize: w₁ * Σ a_i² + w₂ * Σ j_i²其中：
- Σ a_i²：所有轨迹点加速度的平方和。平方是为了让大的加速度受到更大的惩罚。
- Σ j_i²：所有轨迹点 jerk 的平方和。jerk 可以用差分近似：j_i = (a_{i+1} - a_i) / Δt。
- w₁, w₂：权重系数。这两个系数决定了你更看重“舒适”还是“平顺”。
你可能会问：“为什么不用绝对值？用平方有什么好处？”
嗯，这里有个小技巧。平方函数是凸函数，而且处处可导。这对优化求解器来说非常友好。绝对值函数在零点不可导，求解器会头疼。我刚开始做优化时也踩过这个坑——用了绝对值，结果求解器半天不收敛。
3.3 离散化形式（实际代码用的）
上面是连续形式，但计算机只能处理离散数据。所以我们要把目标函数写成矩阵形式。
假设轨迹有 N 个点，时间间隔为 Δt。那么：
- 加速度向量：a = [a₁, a₂, ..., a_N]ᵀ
- jerk 向量：j = D * a，其中 D 是差分矩阵
差分矩阵 D 长这样（N-1 行，N 列）：
D = [ -1/Δt 1/Δt 0 ... 0 ]
[ 0 -1/Δt 1/Δt ... 0 ]
[ ... ... ... ... ... ]
[ 0 0 0 -1/Δt 1/Δt ]那么目标函数就是：
J = w₁ * aᵀ * a + w₂ * (D*a)ᵀ * (D*a)展开后：
J = aᵀ * (w₁ * I + w₂ * Dᵀ * D) * a你看，这其实是一个二次型。优化问题变成了一个二次规划（QP）问题。求解起来非常高效。
3.4 要不要加其他项？
除了加速度和 jerk，有时候还会加一些“惩罚项”。比如：
- 速度惩罚：如果希望轨迹尽量匀速，可以加 Σ (v_i - v_ref)²
- 位置惩罚：如果希望轨迹尽量靠近参考线，可以加 Σ (p_i - p_ref)²
- 时间惩罚：如果希望尽快到达终点，可以加总时间 T
但我个人建议：不要一开始就加太多项。先只优化加速度和 jerk，看看效果。如果轨迹偏离参考线太远，再加位置惩罚。我曾经犯过一个错误——一口气加了 5 个惩罚项，结果调参调了整整一周，最后发现去掉两项效果反而更好。
说白了，目标函数的设计是一个“做减法”的过程。少即是多。
3.5 一个小例子
假设我们有一段 10 米的直线轨迹，起点速度 0，终点速度 0。我们分别用两组权重来优化：
| 权重设置 | 最大加速度 | 最大 jerk | 乘坐感受 |
|---|---|---|---|
| w₁=1.0, w₂=0.0 | 2.5 m/s² | 8.0 m/s³ | 起步猛，刹车也猛 |
| w₁=1.0, w₂=0.5 | 1.8 m/s² | 3.2 m/s³ | 平顺，像老司机 |
| w₁=0.5, w₂=1.0 | 1.2 m/s² | 1.5 m/s³ | 非常平顺，但有点慢 |
你看，权重不同，结果天差地别。这就是目标函数设计的魅力所在。
3.6 知识体系图
下面这张图总结了目标函数设计的核心逻辑：
嗯，这张图把整个逻辑串起来了。从轨迹状态量出发，分别计算加速度和 jerk 的惩罚，乘以各自的权重，最后求和得到总目标函数。优化器的工作就是让这个 J 尽可能小。
3.7 避坑指南
最后，分享几个我踩过的坑：
- 权重不要相差太大：如果 w₁ 和 w₂ 差了几个数量级，优化器会忽略小的那个。我一般让两者在同一个量级。
- 注意单位：加速度的单位是 m/s²，jerk 的单位是 m/s³。数值大小不同，直接比较没意义。建议先归一化。
- 别忘了边界条件：目标函数再漂亮，如果起点终点的加速度约束没处理好，轨迹也是废的。
我曾经在一个项目中，花了两天时间调权重，结果发现是加速度的符号搞反了。嗯，这种低级错误，犯一次就记住了。
好，目标函数的设计就聊到这里。下一节我们讲约束条件——别忘了，光有目标还不够，你得告诉优化器“不能做什么”。