第三章 优化理论基础：无约束优化、梯度下降法、牛顿法、高斯-牛顿法、LM算法
各位同学，欢迎来到《复杂环境下轨迹优化实战宝典》的第三章。
这一章，咱们聊聊优化理论。说实话，很多做机器人的人一听“理论”两个字就头疼。我当年也一样，觉得能跑起来就行，管它什么梯度不梯度的。直到有一次，我做一个机械臂的轨迹规划，算法怎么调都抖得厉害，最后发现是优化器选错了。嗯，从那以后，我再也不敢小看这些基础理论了。
这一章，我会用最直白的方式，把无约束优化、梯度下降、牛顿法、高斯-牛顿法、LM算法讲清楚。你不需要有深厚的数学功底，跟着我的思路走就行。
核心观点：优化问题，说白了就是“找最小值”。你给机器人一个目标函数，它要找到让这个函数值最小的那组参数。轨迹优化里，这个函数可能是“路径长度”、“能量消耗”或者“与障碍物的距离”。
3.1 无约束优化：问题定义
先搞清楚我们在干什么。无约束优化，数学上就是：
min f(x) , x ∈ Rⁿ没有等式约束，没有不等式约束。就一个目标函数，你随便折腾。这在轨迹优化里很常见——比如你只关心路径平滑度，不考虑障碍物，那就是无约束优化。
我个人习惯把优化问题分成两类：
- 凸优化：只有一个谷底，怎么走都能到。我最喜欢这种，省心。
- 非凸优化：到处都是坑，一不小心就掉进局部最优。轨迹优化里，这种才是常态。
小提示：判断一个问题是凸还是非凸，有个简单方法——看Hessian矩阵是否半正定。不过说实话，工程上我们很少去严格证明，更多是靠经验和实验来调。
3.2 梯度下降法：最朴素的思路
梯度下降法，说白了就是“沿着最陡的坡往下滑”。你站在山上，哪边最陡就往哪边迈一步，一直走到谷底。
数学表达式很简单：
x_{k+1} = x_k - α ∇f(x_k)其中α是步长，∇f是梯度。
我在项目中遇到过一个问题：步长α怎么选？选大了，直接越过谷底，震荡发散；选小了，走半天还在原地。后来我总结了一个经验——用回溯线搜索（backtracking line search），每次迭代自适应调整步长。
避坑指南：我曾经在一个轨迹平滑项目里，直接用固定步长的梯度下降，结果跑了5000步还没收敛。后来换成Adam优化器（自适应矩估计），200步就搞定了。所以，别死磕基础梯度下降，工程上要用改进版本。
梯度下降的优缺点：
| 优点 | 缺点 |
|---|---|
| 实现简单，代码量少 | 收敛慢，尤其接近最优解时 |
| 内存占用小 | 容易在鞍点附近徘徊 |
| 适合大规模问题 | 对步长敏感 |
3.3 牛顿法：二阶信息的威力
梯度下降只用了一阶信息（梯度），牛顿法则用到了二阶信息（Hessian矩阵）。你想想看，梯度下降只知道“哪边陡”，牛顿法还知道“这个坡的曲率是多少”。
牛顿法的迭代公式：
x_{k+1} = x_k - [∇²f(x_k)]⁻¹ ∇f(x_k)其中∇²f是Hessian矩阵。
嗯，这里要注意。牛顿法收敛速度确实快，二次收敛嘛。但代价是什么？每次迭代都要计算Hessian矩阵并求逆。对于高维问题（比如机器人有几十个自由度），这个计算量是灾难性的。
警告：千万别在维度超过100的问题上直接用牛顿法。我见过一个同事，在50维的轨迹优化里用牛顿法，每次迭代要算2500个二阶导数，跑了一整天还没出结果。后来换成拟牛顿法（BFGS），半小时搞定。
3.4 高斯-牛顿法：最小二乘的专属利器
高斯-牛顿法，是专门为最小二乘问题设计的。什么是最小二乘？就是目标函数是误差平方和的形式：
f(x) = ½ Σ rᵢ(x)²其中rᵢ是残差。轨迹优化里，这种形式太常见了——比如让路径点尽量靠近一条平滑曲线，误差就是点到曲线的距离。
高斯-牛顿法的核心思想：用雅可比矩阵J近似Hessian矩阵。
∇²f(x) ≈ JᵀJ迭代公式变成：
x_{k+1} = x_k - (JᵀJ)⁻¹ Jᵀ r你看，不用算二阶导数了，只算一阶的雅可比矩阵。计算量大大降低。
我个人习惯在SLAM后端优化里大量使用高斯-牛顿法。不过要注意，JᵀJ可能奇异（不可逆），这时候算法就崩了。
3.5 LM算法：工程界的瑞士军刀
LM算法（Levenberg-Marquardt），说白了就是梯度下降和高斯-牛顿的合体。它引入了一个阻尼因子λ：
x_{k+1} = x_k - (JᵀJ + λI)⁻¹ Jᵀ r当λ很大时，JᵀJ + λI ≈ λI，算法退化为梯度下降（步长1/λ）。当λ很小时，算法接近高斯-牛顿。
为什么LM算法在工程界这么受欢迎？因为它鲁棒。高斯-牛顿在远离最优解时可能发散，梯度下降在接近最优解时太慢。LM算法通过动态调整λ，兼顾了两者的优点。
实战经验：我在做移动机器人轨迹优化时，LM算法是我的首选。它不需要精确的初始值，即使初始轨迹很粗糙，也能稳定收敛。我曾经用LM算法优化一条穿过狭窄通道的路径，初始轨迹直接撞墙，但LM算法硬是把它拉回来了。
LM算法的调参技巧：
- λ的初始值：一般设为0.01到1之间。我习惯从0.1开始。
- λ的更新策略：如果这一步让目标函数下降，就减小λ（更相信高斯-牛顿）；如果上升，就增大λ（更相信梯度下降）。
- 收敛判据：梯度范数小于阈值，或者目标函数变化小于阈值。
// 伪代码：LM算法核心逻辑
while (!converged) {
// 计算雅可比矩阵J和残差r
computeJacobianAndResidual(x, J, r);
// 求解增量
delta = solve((JᵀJ + λI), -Jᵀr);
// 尝试更新
x_new = x + delta;
f_new = computeCost(x_new);
if (f_new < f_old) {
// 接受更新，减小λ
λ = λ / 10;
x = x_new;
} else {
// 拒绝更新，增大λ
λ = λ * 10;
// 重新求解
}
}小技巧：实际工程中，我一般会限制λ的最大值和最小值（比如1e-8到1e8），防止数值溢出。另外，JᵀJ + λI的求逆可以用Cholesky分解，比直接求逆快很多。
3.6 方法对比与选择
说了这么多，到底该用哪个？我整理了一个对比表：
| 方法 | 收敛速度 | 计算量 | 鲁棒性 | 适用场景 |
|---|---|---|---|---|
| 梯度下降 | 慢（线性） | 低 | 高 | 大规模问题、深度学习 |
| 牛顿法 | 快（二次） | 极高 | 低 | 小规模、精度要求高 |
| 高斯-牛顿 | 较快 | 中 | 中 | 最小二乘问题、SLAM |
| LM算法 | 快 | 中高 | 高 | 轨迹优化、非线性拟合 |
我个人建议：做轨迹优化，优先选LM算法。它虽然不是最快的，但绝对是最稳的。你想想看，机器人跑在真实环境里，算法崩了可不是闹着玩的。
好了，这一章的内容就到这里。优化理论是轨迹优化的基石，理解这些方法的本质，比背公式重要得多。下一章，我们会把这些理论用到实际的轨迹优化问题中，到时候你就知道它们有多好用了。