3. 数学基础回顾（下）：优化理论基础
好，咱们接着聊。上一节我们把微积分和线性代数过了一遍，这一节要啃的，是轨迹优化的核心——优化理论。说白了，你给机器人规划一条路，本质上就是在解一个优化问题：在满足各种约束的前提下，找到一条代价最小的路径。
我个人习惯把优化问题拆成三块来看：目标函数（你要优化什么）、决策变量（你能调什么）、约束条件（你不能违反什么）。这三块搞清楚了，剩下的就是选算法的事了。
3.1 凸优化 vs 非凸优化
先问一个问题：为什么大家都喜欢凸优化？
答案很简单——凸优化有全局最优解。你想想看，一个凸函数就像一口锅，锅底就是全局最小值。不管你从哪个方向往下滚，最后都会滚到同一个锅底。这在工程上意味着什么？意味着你不需要担心算法陷入局部最优，可以放心大胆地用梯度下降这类简单方法。
那非凸优化呢？就像连绵起伏的山脉。你从不同的起点出发，可能掉进不同的山谷里。这些山谷都是局部最优解，但不一定是全局最优。我在做无人机避障轨迹规划时，就经常遇到这个问题——明明感觉路径已经挺好了，但换个初始值一算，发现还能更好。
- 函数形式：f(θx + (1-θ)y) ≤ θf(x) + (1-θ)f(y)，对所有θ∈[0,1]成立
- 二阶条件：Hessian矩阵半正定（∇²f(x) ⪰ 0）
实际项目中，我遇到的轨迹优化问题大部分是非凸的。为什么？因为约束条件太复杂了——障碍物避碰、动力学限制、执行器饱和……这些约束加进来，可行域往往不是凸集。但别慌，我们有办法处理。
3.2 代价函数与约束条件
代价函数，也叫目标函数，就是你想要最小化的东西。在轨迹优化里，常见的代价函数有这些：
| 代价类型 | 数学形式 | 物理含义 |
|---|---|---|
| 最小时间 | ∫ dt | 让机器人尽快到达终点 |
| 最小能量 | ∫ ||u(t)||² dt | 节省电池/燃料 |
| 最小加加速度 | ∫ ||jerk(t)||² dt | 让轨迹更平滑 |
| 最小跟踪误差 | ∫ ||x(t) - x_ref(t)||² dt | 精确跟踪参考轨迹 |
约束条件呢，分两类：等式约束和不等式约束。
- 等式约束：h(x) = 0。比如机器人必须从起点出发，必须到达终点。
- 不等式约束：g(x) ≤ 0。比如速度不能超过上限，不能进入障碍物区域。
嗯，这里要注意一点：约束条件不是越多越好。我曾经接手过一个项目，同事把能想到的约束全加进去了，结果求解器跑了一个小时还没收敛。后来我帮他删掉了几个冗余约束，求解时间降到了两分钟。约束越多，可行域越小，求解难度指数级上升。
3.3 拉格朗日乘子法
拉格朗日乘子法，说白了就是把带约束的优化问题，变成无约束的优化问题。这个方法我几乎每天都在用，尤其是在处理动力学约束的时候。
先看最简单的形式——等式约束：
min f(x)
s.t. h(x) = 0构造拉格朗日函数：
L(x, λ) = f(x) + λ · h(x)然后对x和λ分别求导，令导数为0：
∂L/∂x = ∇f(x) + λ · ∇h(x) = 0
∂L/∂λ = h(x) = 0解这个方程组，就得到了最优解。λ就是拉格朗日乘子，它的物理意义很直观——λ的大小反映了约束的"紧度"。如果λ=0，说明这个约束实际上没起作用；如果λ很大，说明这个约束在"用力"把解往可行域里拉。
对于不等式约束，情况稍微复杂一点，要用到KKT条件。我直接给结论：
min f(x)
s.t. g(x) ≤ 0
KKT条件：
1. ∇f(x) + μ · ∇g(x) = 0 （梯度条件）
2. g(x) ≤ 0 （原始可行）
3. μ ≥ 0 （对偶可行）
4. μ · g(x) = 0 （互补松弛）第四条互补松弛条件很有意思。它说：要么约束起作用（g(x)=0），要么乘子为0（μ=0）。不会出现两者都非零的情况。我在做轨迹优化时，经常用这个条件来判断哪些约束是"活跃的"——活跃的约束才是真正限制轨迹的因素。
3.4 知识体系总览
为了让你对本章内容有个整体把握，我画了一张图：
这张图把优化理论的核心脉络理清楚了。你从中心出发，往左看是问题分类（凸 vs 非凸），往中间看是优化三要素（代价、变量、约束），往右看是求解方法（拉格朗日乘子法、KKT条件）。这三块是相互关联的——你选什么求解方法，取决于你的问题类型和三要素的具体形式。
好了，数学基础部分就到这里。这些工具在后面的章节中会反复用到，尤其是当我们开始讲具体的轨迹优化算法时——比如用拉格朗日乘子法处理动力学约束，用KKT条件判断约束的活跃性。到时候你会发现，今天花时间理解这些概念，绝对值。
公众号:蓝海资料掘金营，微信deep3321