非线性优化——从 Gauss-Newton 到 SQP 的完整理论与工程实践

前置自测

📋 前置自测（答不出 ≥ 2 题 → 先回 40_凸优化算法路线图 复习）

1. 写出 \(L\)-光滑函数的下降引理，并解释为什么步长 \(1/L\) 保证单步下降。
2. Newton 法的迭代公式是什么？它为什么有局部二次收敛？
3. 什么是 KKT 条件？它是约束优化最优性的什么条件（充分/必要）？
4. 条件数 \(\kappa\) 如何影响梯度下降的收敛速度？
5. 什么是 Lipschitz 连续？梯度的 Lipschitz 连续意味着什么？

---

本章目标

学完本章后，你将能够：(1) 从 Taylor 展开推导 Gauss-Newton 法，理解其在零残差时二次收敛、大残差时退化的数学原因；(2) 解释 Levenberg-Marquardt 的"正则化 vs 信赖域"双重身份；(3) 独立实现 Armijo 回溯与 Wolfe 线搜索、Dogleg 信赖域子求解器；(4) 理解 BFGS/L-BFGS 的拟 Newton 思想与 Dennis-More 超线性条件；(5) 掌握 SQP 框架与 Ipopt 的内点法实现；(6) 为 SLAM BA、MPC 轨迹优化、流形优化等具体问题选对求解器（Ceres/GTSAM/Ipopt/acados）并知道失败时为什么失败。

---

1. 全景地图：非线性优化的三大支柱 ⭐

1.1 动机：凸优化之后的世界

上一章我们学习了凸优化——函数是"碗形"的，局部最优就是全局最优，一切都很美好。但机器人学中的真实问题几乎都是**非凸**的：

• SLAM 后端：位姿变量在 \(SE(3)\) 流形上，旋转的乘法结构天然非凸
• 轨迹优化：非线性动力学约束 \(x_{t+1} = f(x_t, u_t)\) 使可行域非凸
• 参数辨识：非线性模型的最小二乘拟合有多个局部极小

非线性优化的核心挑战：我们无法保证找到全局最优，只能保证收敛到**局部最优**（稳定点）。但好消息是——对许多机器人问题，好的初始化 + 正确的算法选择可以在实践中找到"足够好"的解。

本质洞察：非线性优化的三大支柱是 (1) Gauss-Newton/LM 专治最小二乘（SLAM 的骨骼），(2) 线搜索/信赖域 提供全局化保证（非凸世界的安全绳），(3) SQP/IPM 处理一般约束 NLP（轨迹优化与 MPC 的脊柱）。三者之上的**流形优化**把欧氏假设替换为 retraction，使 \(SE(3)\), \(SO(3)\), Stiefel 流形成为一等公民。

1.2 知识体系结构

非线性优化
├── 无约束（支柱 1 + 2）
│   ├── Newton 法（§2）⭐⭐ ——— 二阶局部最快
│   ├── 非线性最小二乘（§3）⭐⭐
│   │   ├── Gauss-Newton ——— 零残差二次收敛
│   │   └── Levenberg-Marquardt ——— GN + 信赖域
│   ├── 全局化策略（§4）⭐⭐
│   │   ├── 线搜索（Wolfe/Armijo）
│   │   └── 信赖域（Cauchy/Dogleg/Steihaug-CG）
│   └── 拟牛顿法（§5）⭐⭐⭐
│       ├── BFGS ——— 超线性收敛
│       └── L-BFGS ——— 大规模首选
├── 约束 NLP（支柱 3）
│   ├── SQP（§6）⭐⭐⭐ ——— QP 子问题
│   └── 内点法 for NLP（§7）⭐⭐⭐ ——— Ipopt
├── 流形优化（§8）⭐⭐⭐⭐
│   ├── Riemannian GD/Newton/TR
│   └── SE-Sync: 可证全局最优的 SLAM
└── 工程实践（§9）⭐⭐
    ├── Ceres Solver (SLAM/BA)
    ├── GTSAM (因子图)
    ├── Ipopt (通用 NLP)
    └── acados (实时 NMPC)

1.3 非线性优化与凸优化的桥梁

回顾凸优化算法路线图（前一章）：凸优化中的 GD/AGD/FISTA/ADMM/IPM 都利用了凸性保证全局收敛。在非线性优化中，这些方法的变体仍然是核心，但需要修正和补充来处理非凸性。

凸优化方法	非线性优化对应	关键区别
梯度下降	非凸 GD	只能收敛到稳定点（\(\nabla f = 0\)），不保证全局最优
Newton 法	Newton + 信赖域/线搜索	需要 Hessian 修正（保证正定性）
FISTA	Prox-linear method	非凸 \(g\) 的 prox 可能不唯一
IPM	Ipopt (filter line-search IPM)	非凸约束需要额外的全局化策略
ADMM	非凸 ADMM	收敛性只有部分理论保证

从凸到非凸的理论降级：凸优化中的 \(O(1/k)\) 收敛率（对函数值），在非凸情形下变为"\(O(1/k)\) 收敛到**稳定点**"（Ghadimi-Lan 2013）： $\(\min_{k \le K} \|\nabla f(x_k)\|^2 \le \frac{2(f(x_0) - f^*)}{K \alpha}\)$

注意区别：凸情形保证 \(f(x_k) - f^* \le O(1/k)\)（函数值差距），非凸情形只保证 \(\|\nabla f(x_k)\|^2 \le O(1/K)\)（梯度范数的最小值）。这个差距不是技术限制——在非凸问题上，找全局最优本身就是 NP-hard 的。

"凸化"策略——把非凸问题变成序列凸问题：

许多成功的非线性优化算法的本质是"局部凸化"： - GN：在当前点线性化残差，得到凸的线性最小二乘 - SQP：在当前点二阶近似 Lagrangian + 线性化约束，得到凸 QP - IPM：barrier 目标在固定 \(\mu\) 下通常"更凸"（barrier 项是凸的） - DDP/iLQR：在当前轨迹上线性化动力学 + 二阶近似 cost，得到 LQR（凸）

本质洞察：非线性优化的大多数方法可以统一理解为"反复构造局部凸近似，在凸近似上精确求解，然后移到新的点再构造新的凸近似"。算法的区别在于(1)凸近似的形式（二次 vs 线性 vs 锥），(2)全局化策略（线搜索 vs 信赖域 vs filter），(3)如何利用问题特殊结构（稀疏性、最小二乘、流形）。

⚠️ 常见陷阱

🧠 思维陷阱：认为"非凸 = 无法求解" - 非凸并不意味着无法求解，只意味着**无法保证全局最优** - 对许多问题（SLAM、轨迹优化），好的初始化 + 局部方法在实践中表现极好 - SE-Sync 等方法甚至可以为某些非凸问题提供**全局最优性证书**

💡 概念误区：混淆"全局最优"和"满意解" - 在机器人工程中，通常不需要全局最优——只要解足够好、足够快、足够稳定 - MPC 每 10ms 重新求解一次，上一步的"局部最优"就是下一步的"好初始化" - 工程师关心的不是 \(f(x^*_{\text{global}})\) 与 \(f(x^*_{\text{local}})\) 的差距，而是**闭环性能**是否足够

练习

1. 给出一个有多个局部极小值的非凸函数例子，并解释为什么梯度下降从不同初始点会收敛到不同的极小值。
2. 为什么 SLAM 后端优化是非凸的？具体指出哪个变量/约束导致了非凸性。
3. （概念）Rastrigin 函数 \(f(x) = 10n + \sum_{i=1}^n [x_i^2 - 10\cos(2\pi x_i)]\) 有多少个局部极小值（\(n = 2\) 时）？这说明什么？

---

2. Newton 法与二阶信息 ⭐⭐

2.1 动机与历史：利用曲率信息加速收敛

梯度下降只使用一阶信息（梯度方向），在"椭球形"目标函数上表现差（沿窄谷来回震荡）。如果我们额外知道**曲率信息**（Hessian），就能直接"跳到"二次近似的极小点。

历史：Newton 法用于方程求根可以追溯到 Isaac Newton（1669）和 Joseph Raphson（1690）。将其推广到多维优化问题是 20 世纪数值分析的核心成就之一。Kantorovich（1948）给出了第一个严格的收敛性分析，奠定了现代 Newton 法理论的基础。

Newton 法的推导：

在当前点 \(x_k\) 构造 \(f\) 的二阶 Taylor 近似：

\[m_k(x) = f(x_k) + \nabla f(x_k)^T(x-x_k) + \frac{1}{2}(x-x_k)^T \nabla^2 f(x_k)(x-x_k)\]

对 \(m_k\) 关于 \(x\) 求导并令其为零：

\[\nabla m_k(x) = \nabla f(x_k) + \nabla^2 f(x_k)(x - x_k) = 0\]

解得：

\[x_{k+1} = x_k - [\nabla^2 f(x_k)]^{-1} \nabla f(x_k)\]

实际计算中不求逆：解线性方程组 \(\nabla^2 f(x_k) \cdot p_k = -\nabla f(x_k)\) 得到 Newton 方向 \(p_k\)，然后 \(x_{k+1} = x_k + p_k\)。直接求逆 \(O(n^3)\)，解线性系统（用 Cholesky 分解）也是 \(O(n^3)\)，但常数更小且数值更稳定。

Newton 法的直觉：每步用一个"碗"（二次函数）去拟合当前点附近的目标函数，然后跳到"碗底"。如果目标函数本身就是二次的，一步到位。如果不是，误差来自三阶及更高阶项——这就是二次收敛的来源。

2.2 局部二次收敛

定理（Newton 法局部二次收敛）。 若 \(\nabla^2 f\) Lipschitz 连续（常数 \(M\)），\(\nabla^2 f(x^*)\) 非奇异，则在 \(x^*\) 的邻域内：

\[\|x_{k+1} - x^*\| \le \frac{M}{2\|\nabla^2 f(x^*)^{-1}\|^{-1}} \|x_k - x^*\|^2\]

证明要点：

Step 1：由 \(\nabla f(x^*) = 0\)，Newton 步 \(p_k = -[\nabla^2 f(x_k)]^{-1}\nabla f(x_k)\) 满足： $\(x_{k+1} - x^* = x_k - x^* - [\nabla^2 f(x_k)]^{-1}\nabla f(x_k)\)$ $\(= [\nabla^2 f(x_k)]^{-1}\left[\nabla^2 f(x_k)(x_k - x^*) - (\nabla f(x_k) - \nabla f(x^*))\right]\)$

Step 2：由 Taylor 展开：\(\nabla f(x_k) - \nabla f(x^*) = \nabla^2 f(x^*)(x_k - x^*) + O(\|x_k - x^*\|^2)\)，所以： $\(\nabla^2 f(x_k)(x_k-x^*) - (\nabla f(x_k) - \nabla f(x^*)) = [\nabla^2 f(x_k) - \nabla^2 f(x^*)](x_k-x^*) + O(\|x_k-x^*\|^2)\)$

Step 3：由 \(\nabla^2 f\) 的 Lipschitz 性：\(\|\nabla^2 f(x_k) - \nabla^2 f(x^*)\| \le M\|x_k - x^*\|\)，所以： $\(\|x_{k+1} - x^*\| \le \|\nabla^2 f(x_k)^{-1}\| \cdot M\|x_k - x^*\|^2\)$

当 \(x_k\) 足够接近 \(x^*\) 时，\(\|\nabla^2 f(x_k)^{-1}\| \approx \|\nabla^2 f(x^*)^{-1}\|\)，得到二次收敛。

直觉：Newton 法在每步精确求解二阶模型，误差来源仅是三阶及更高阶项——因此误差是平方缩小的。

2.3 Newton 法的困难与 Hessian 修正策略

Newton 法在非凸区域面临两个根本困难。理解这些困难以及对应的修正策略，是掌握所有现代非线性求解器的前提。

困难 1：Hessian 不正定。 在非凸点，\(\nabla^2 f(x_k)\) 可能有负特征值，Newton 方向可能是**上升**方向。

为什么会这样？回忆 Newton 法求解的是二阶模型 \(m_k(p) = f_k + g_k^T p + \frac{1}{2}p^T H_k p\) 的极小点。当 \(H_k\) 有负特征值时，\(m_k\) 沿对应特征方向是无下界的"马鞍面"——极小点不存在，Newton 方程 \(H_k p = -g_k\) 的解是鞍点方向而非下降方向。

跨领域类比：这就像在一个山脊上寻找最低点——如果你的模型告诉你"这里像一个碗"（正定 Hessian），你可以跳到碗底；但如果模型告诉你"这里像一个马鞍"（不定 Hessian），碗底根本不存在，你需要先把马鞍"掰弯"成碗形，才能确定一个合理的下降方向。

2.3.1 修正 Cholesky 分解（Gill-Murray-Wright 1981）⭐⭐

最经典的 Hessian 修正方法基于 Cholesky 分解。思路是：对 \(H_k\) 做 Cholesky 分解 \(H_k = LDL^T\)，如果 \(D\) 的某个对角元素为负或太小，就把它"抬高"到某个正数 \(\delta > 0\)。

算法流程：

1. 对 \(H_k\) 执行带扰动的 \(LDL^T\) 分解：\(H_k + E = LDL^T\)
2. 修正矩阵 \(E\) 选择为对角矩阵，使得 \(D\) 的每个对角元素满足 \(d_{ii} \ge \delta > 0\)
3. 求解修正后的 Newton 系统：\(LDL^T p = -g_k\)

**Gill-Murray 策略**的具体修正规则。在分解过程中，令 \(\beta^2 = \max(\gamma, \xi/\sqrt{n}, \epsilon_M)\)，其中 \(\gamma = \max_i |H_{ii}|\)（对角元素的最大绝对值），\(\xi = \max_{i \ne j} |H_{ij}|\)（非对角元素的最大绝对值），\(\epsilon_M\) 是机器精度。然后在 Cholesky 分解的第 \(j\) 步：

\[c_{jj}^{\text{mod}} = \max\left(|c_{jj}|,\; \left(\frac{\max_i |c_{ij}|}{\beta}\right)^2,\; \delta\right)\]

其中 \(c_{jj}\) 是 Cholesky 因子的对角元素，\(\delta\) 是一个小正数（如 \(\epsilon_M^{2/3}\)）。

为什么这个策略有效？ 关键在于两个设计目标之间的平衡：

• 修正量要足够大：保证 \(B_k = H_k + E\) 正定，Newton 方向是下降方向
• 修正量不能太大：否则 \(B_k\) 远离真实 Hessian，Newton 退化为梯度下降（\(E \to \infty I\) 时 \(p \to -\frac{1}{\lambda}g\)）

本质洞察：修正 Cholesky 的核心思想是"最小侵入"——只修改 Hessian 的负特征值方向，保留正特征值方向的精确二阶信息。这比简单地加 \(\lambda I\)（LM 策略）更精细，因为 \(\lambda I\) 会均匀地改变所有方向的曲率，而修正 Cholesky 只动"坏的"方向。

2.3.2 加对角阻尼（Levenberg 思想）⭐⭐

最简单的修正：\(B_k = H_k + \lambda I\)，其中 \(\lambda\) 足够大使 \(B_k \succ 0\)。

\(\lambda\) 的选择：设 \(H_k\) 的最小特征值为 \(\lambda_{\min}\)。需要 \(\lambda > \max(0, -\lambda_{\min})\)。实践中取 \(\lambda = \max(0, -\lambda_{\min}) + \delta\)，\(\delta\) 是安全余量。

与修正 Cholesky 的对比：

特性	修正 Cholesky	对角阻尼 \(\lambda I\)
修正精细度	只改负特征值方向	均匀改所有方向
计算代价	\(O(n^3)\) 一次分解	需要估计 \(\lambda_{\min}\)
步的质量	更接近真 Newton 步	可能过度保守
实现复杂度	高	低
实际应用	通用非线性优化	LM（专用于最小二乘）

2.3.3 Newton-CG（Hessian-free Newton）⭐⭐⭐

当 \(n\) 很大（\(> 10^4\)）时，存储和分解完整 Hessian 不可行。Newton-CG 的思想：用共轭梯度法（CG）**迭代**求解 \(H_k p = -g_k\)，只需要 Hessian-vector 积 \(H_k v\)（不需要完整 \(H_k\)）。

Hessian-vector 积的计算方法：

方法 A——有限差分：\(H_k v \approx \frac{\nabla f(x_k + \epsilon v) - \nabla f(x_k)}{\epsilon}\)，代价 = 一次梯度计算。

方法 B——自动微分（前向模式）：精确计算 \(H_k v\)，代价 \(\approx 2 \times\) 一次函数求值。

CG 的截断条件（Steihaug 1983）：

1. CG 残差 \(\|r_j\| \le \eta_k \|g_k\|\)：足够精确
2. 负曲率 \(d_j^T H_k d_j \le 0\)：沿 \(d_j\) 走到信赖域边界
3. 超出信赖域 \(\|p_j\| \ge \Delta_k\)：截断到边界

为什么 CG 在这里如此自然？ Newton 系统 \(Hp = -g\) 是一个线性方程组。CG 是求解对称正定线性系统最高效的迭代方法，每步只需一次矩阵-向量积。即使 \(H\) 不正定，截断 CG 也能给出有意义的方向（沿负曲率逃逸鞍点）。

反事实推理：如果不用 CG 而是直接分解 Hessian 做 SLAM BA 会怎样？对于 100 台相机 + 10000 个路标的 BA 问题，\(n = 6 \times 100 + 3 \times 10000 = 30600\)。直接分解需要 \(O(n^3) \approx 3 \times 10^{13}\) 运算——单次 Newton 步就需要几分钟。而 CG + Schur 补利用稀疏性后，每步只需毫秒级。这就是 Ceres 的 ITERATIVE_SCHUR 求解器的核心。

困难 2：Hessian 计算/存储代价。 \(n \times n\) Hessian 需要 \(O(n^2)\) 存储和 \(O(n^3)\) 求解。

对策总结：

方法	存储	每步代价	适用规模	典型实现
直接 Newton	\(O(n^2)\)	\(O(n^3)\)	\(n < 1000\)	手写/Eigen
修正 Cholesky	\(O(n^2)\)	\(O(n^3)\)	\(n < 1000\)	Nocedal-Wright 代码
拟牛顿 BFGS	\(O(n^2)\)	\(O(n^2)\)	\(n < 10000\)	scipy, Ceres
L-BFGS	\(O(mn)\)	\(O(mn)\)	\(n > 10000\)	scipy, PyTorch
Newton-CG	\(O(n)\)	\(O(kn)\)（\(k\)=CG步）	\(n\) 无上限	Ceres ITERATIVE_SCHUR

2.4 收敛阶的精确定义与判定 ⭐⭐

在比较不同优化算法时，"收敛速度"是最关键的指标。但"线性收敛""超线性收敛""二次收敛"到底意味着什么？它们之间的差距有多大？

定义 2.4.1（Q-线性收敛）。序列 \(\{x_k\}\) Q-线性收敛到 \(x^*\)，如果存在 \(\rho \in (0, 1)\) 使得：

\[\|x_{k+1} - x^*\| \le \rho \|x_k - x^*\| \quad \text{对所有 } k \text{ 充分大}\]

\(\rho\) 称为收敛率。\(\rho\) 越小，收敛越快。

定义 2.4.2（Q-超线性收敛）。若：

\[\lim_{k \to \infty} \frac{\|x_{k+1} - x^*\|}{\|x_k - x^*\|} = 0\]

即"每步的压缩比趋于零"。不如二次收敛快，但比任何线性收敛都快。

定义 2.4.3（Q-二次收敛）。若存在 \(C > 0\) 使得：

\[\|x_{k+1} - x^*\| \le C \|x_k - x^*\|^2\]

直觉理解——错误位数翻倍：

假设当前误差为 \(10^{-3}\)（3 位精度）： - 线性收敛（\(\rho = 0.5\)）：下一步 \(5 \times 10^{-4}\)，每步增加约 0.3 位精度 - 超线性收敛：下一步可能 \(10^{-4}\) 到 \(10^{-5}\)，精度加速增加 - 二次收敛（\(C = 1\)）：下一步 \(10^{-6}\)，精度翻倍！再一步 \(10^{-12}\)

迭代次数	线性（\(\rho=0.5\)）	超线性（BFGS）	二次（Newton）
0	\(10^{-1}\)	\(10^{-1}\)	\(10^{-1}\)
5	\(3 \times 10^{-3}\)	\(\sim 10^{-4}\)	\(\sim 10^{-16}\)（机器精度）
10	\(10^{-4}\)	\(\sim 10^{-10}\)	—
20	\(10^{-7}\)	—	—

这就是为什么 SLAM/BA 用 Gauss-Newton 而不是梯度下降——5 步达到机器精度 vs 20+ 步才到 \(10^{-7}\)。

Dennis-More 定理（超线性收敛的判定）⭐⭐⭐

定理（Dennis-More 1974）。 设 \(f \in C^2\)，\(\nabla^2 f(x^*)\) 正定，\(x_k \to x^*\)。则迭代 \(x_{k+1} = x_k - B_k^{-1} \nabla f(x_k)\) 是 Q-超线性收敛的，当且仅当：

\[\lim_{k \to \infty} \frac{\|(B_k - \nabla^2 f(x^*)) s_k\|}{\|s_k\|} = 0, \quad s_k = x_{k+1} - x_k\]

这个定理为什么深刻？ 它告诉我们超线性收敛**不需要** \(B_k \to \nabla^2 f(x^*)\)（即 Hessian 近似整体收敛到真 Hessian）。只需要沿搜索方向 \(s_k\) 的近似越来越准确即可。这正是 BFGS 能超线性收敛的原因——BFGS 的割线条件 \(B_{k+1} s_k = y_k\) 保证了沿最近的搜索方向精确，虽然其他方向可能仍然不准。

跨领域类比：这就像学开车——你不需要完美地了解整辆车的每一个零件（\(B_k \to \nabla^2 f(x^*)\)），只需要在你实际行驶的路线上操作越来越精准（沿 \(s_k\) 方向准确）。BFGS 就是在"你走过的路线"上不断积累经验。

2.5 与机器人学的联系

Newton 法是所有高性能求解器的内核： - Ceres Solver：Gauss-Newton/LM 中的 Newton 步求解 \(J^TJ \Delta x = -J^T r\) - Ipopt：IPM 的每步是 KKT 系统的 Newton 步 - acados：SQP 的每步是 QP 子问题的精确求解（等价于约束 Newton 步）

反事实推理：如果不用 Newton/二阶方法，而只用梯度下降做 SLAM BA 会怎样？答案是灾难性的——BA 的 Hessian 条件数通常在 \(10^6\) 量级，GD 需要 \(10^6\) 次迭代；而 Gauss-Newton 通常 10-30 步就收敛。这就是为什么 SLAM 后端永远不会用一阶方法。

---

3. 非线性最小二乘：Gauss-Newton 与 Levenberg-Marquardt ⭐⭐

3.1 动机：SLAM/BA/ICP 的核心算法

机器人学中最重要的优化问题形式是**非线性最小二乘**（Nonlinear Least Squares, NLS）：

\[\min_x f(x) = \frac{1}{2}\|r(x)\|^2 = \frac{1}{2}\sum_{i=1}^m r_i(x)^2\]

其中 \(r(x): \mathbb{R}^n \to \mathbb{R}^m\) 是残差向量（\(m\) 个残差，\(n\) 个参数，通常 \(m \gg n\)）。

为什么最小二乘如此特殊？ 一般的优化问题 \(\min f(x)\) 没有特别的结构可以利用。但最小二乘的目标函数可以分解为 \(f = \frac{1}{2}\|r\|^2\)——这个平方结构使得 Hessian 可以被 Jacobian 的乘积 \(J^TJ\) 近似，无需计算二阶导数就能获得二阶收敛。这是 NLS 方法（GN/LM）相对于通用 Newton 法的核心优势。

具体例子：

问题	残差 \(r_i(x)\)	参数 \(x\)	\(m\) vs \(n\)
Bundle Adjustment	\(z_{ij} - \pi(T_i, p_j)\)（观测 - 重投影）	相机位姿 + 路标	\(m \gg n\)（观测远多于参数）
ICP 点云配准	\(p_i - T \cdot q_{\text{nn}(i)}\)（源点 - 最近点）	刚体变换 \(T\)	\(m \gg n = 6\)
IMU 预积分	\(\Delta \hat{R}_{ij}^T (R_i^T R_j)\)（预积分 - 实际）	位姿序列	\(m \sim n\)
曲线拟合	\(y_i - f(t_i; \theta)\)（观测 - 模型）	模型参数 \(\theta\)	\(m \gg n\)
机器人标定	理论关节位置 - 测量关节位置	DH 参数偏差	\(m \gg n\)

3.2 Gauss-Newton 法的完整推导

Step 1：目标函数的梯度与 Hessian。

令 \(J(x) = \frac{\partial r}{\partial x} \in \mathbb{R}^{m \times n}\) 为 Jacobian 矩阵。

\[\nabla f(x) = J(x)^T r(x)\]

\[\nabla^2 f(x) = J(x)^T J(x) + \sum_{i=1}^m r_i(x) \nabla^2 r_i(x) = J^T J + S(x)\]

其中 \(S(x) = \sum r_i \nabla^2 r_i\) 是**二阶残差项**。

Step 2：Gauss-Newton 近似。

当残差 \(r(x)\) 较小时（接近最优解），\(S(x) \approx 0\)（因为 \(r_i \approx 0\)）。Gauss-Newton 的核心思想：忽略 \(S(x)\)，用 \(J^TJ\) 近似 Hessian。

Newton 步：\(\nabla^2 f \cdot \Delta x = -\nabla f\) 变为：

\[J^T J \cdot \Delta x = -J^T r \quad \text{（正规方程）}\]

Step 3：等价于线性化残差后的最小二乘。

在 \(x_k\) 处一阶 Taylor 展开残差：\(r(x_k + \Delta x) \approx r(x_k) + J(x_k)\Delta x\)。

最小化线性化后的残差范数： $\(\min_{\Delta x} \frac{1}{2}\|r(x_k) + J(x_k)\Delta x\|^2\)$

对 \(\Delta x\) 求导令其为零：\(J^T(r + J\Delta x) = 0\)，即 \(J^TJ\Delta x = -J^Tr\)——正是 Gauss-Newton 方程。

本质洞察：Gauss-Newton 不是"近似 Newton"，而是"在每步**精确**求解残差的线性化最小二乘"。这个视角解释了为什么它在小残差时如此高效——线性近似越准确，一步 GN 就越接近最优。

3.3 收敛性分析

定理（GN 局部收敛，Nocedal-Wright Thm 10.1）。 若 \(J(x^*)\) 列满秩，\(r \in C^2\)，则 GN 在 \(x^*\) 邻域内线性收敛，率为： $\(\rho = \|(J^*TJ^*)^{-1} S^*\| = \|(J^*TJ^*)^{-1} \sum r_i^* \nabla^2 r_i^*\|\)$

三种情况：

情况	残差大小	收敛率	解释
零残差	\(r(x^*) = 0\)	二次（\(\rho = 0\)）	\(S^* = 0\)，GN = Newton
小残差	\(\|r(x^*)\|\) 小	快线性	\(\rho\) 小但非零
大残差	\(\|r(x^*)\|\) 大	慢线性或不收敛	\(\rho\) 可能 \(\ge 1\)，GN 失败

工程含义： - BA/ICP 在好的初始化下通常是"小残差"问题 → GN 足够 - 存在离群点时变成"大残差"问题 → 必须用 LM + 鲁棒核函数

与 Newton 法的精确关系：

属性	Newton 法	Gauss-Newton
适用范围	任意 \(f\)	最小二乘 \(f = \frac{1}{2}\|r\|^2\)
Hessian	精确 \(\nabla^2 f = J^TJ + S\)	近似 \(J^TJ\)（忽略 \(S\)）
每步代价	需要计算 \(\nabla^2 r_i\)（二阶导数）	只需 \(J\)（一阶导数）
零残差收敛	二次	二次（\(S = 0\)，退化为 Newton）
大残差收敛	二次（如果 Hessian 正定）	可能发散（\(J^TJ\) 无法反映负曲率）

本质洞察：Gauss-Newton 之所以能"只用一阶导数就获得二次收敛"，关键在于最小二乘的特殊结构——目标函数的 Hessian 天然可以分解为 \(J^TJ\)（一阶信息构成的正半定矩阵）+ \(S\)（二阶残差项）。当 \(S \approx 0\)（小残差），一阶信息就足够了。这就是为什么 SLAM/BA 如此偏爱 Gauss-Newton——它用"便宜"的一阶导数获得了"昂贵"的二阶收敛。

3.4 Levenberg-Marquardt：GN + 信赖域

动机：GN 在大残差或远离最优点时可能发散。LM 通过加阻尼使算法全局收敛。

LM 子问题： $\((J^T J + \lambda D^T D) \Delta x = -J^T r\)$

其中 \(D = I\)（Levenberg 1944）或 \(D = \text{diag}(\sqrt{J^TJ})\)（Marquardt 1963）。

双重解释：

解释 1（正则化）：\(\lambda > 0\) 使 \(J^TJ + \lambda I \succ 0\)，即使 \(J\) 不满秩也有唯一解。\(\lambda\) 大时 \(\Delta x \approx -\frac{1}{\lambda}J^Tr\)（梯度下降方向）；\(\lambda\) 小时 \(\Delta x \approx -(J^TJ)^{-1}J^Tr\)（GN 方向）。

解释 2（信赖域）：LM 步等价于信赖域子问题： $\(\min_{\Delta x} \frac{1}{2}\|J\Delta x + r\|^2 \quad \text{s.t. } \|D\Delta x\| \le \Delta\)$

的 Lagrange 解，\(\lambda\) 是对应 \(\|\cdot\| \le \Delta\) 约束的乘子。\(\lambda\) 大 ↔ \(\Delta\) 小（保守），\(\lambda\) 小 ↔ \(\Delta\) 大（激进）。

LM 的 \(\lambda\) 更新策略（Nielsen 1999）：

计算实际下降与预测下降的比率： $\(\rho = \frac{f(x_k) - f(x_k + \Delta x)}{\frac{1}{2}\|r\|^2 - \frac{1}{2}\|r + J\Delta x\|^2}\)$

\[\lambda_{k+1} = \begin{cases} \lambda_k / 3 & \text{if } \rho > 0.75 \text{（模型好，增大信赖域）} \\ \lambda_k \cdot 2 & \text{if } \rho < 0.25 \text{（模型差，缩小信赖域）} \\ \lambda_k & \text{otherwise} \end{cases}\]

3.5 LM 全局收敛性

定理（More 1978）。 若 \(J\) 列满秩（在迭代序列上一致），\(\lambda\) 按 Nielsen/More 策略更新，则 \(\|\nabla f(x_k)\| = \|J^Tr_k\| \to 0\)，且聚点为一阶临界点。

证明思路：LM 步的下降量 \(\ge\) Cauchy 步的下降量（沿梯度方向走到信赖域边界），而 Cauchy 步保证至少 \(O(\|\nabla f\|^2 / L)\) 的下降。累加后得 \(\sum \|\nabla f\|^2 < \infty\)。

3.6 从 GN/LM 到鲁棒核函数 ⭐⭐

问题：SLAM/BA 的残差中不可避免地混入离群点（outliers）——错误的特征匹配、误回环等。标准最小二乘 \(\frac{1}{2}r_i^2\) 对离群点极度敏感：一个大残差 \(r_i = 100\) 对目标函数的贡献是 \(5000\)，主导了整个优化。

解决方案——鲁棒核函数：用 \(\rho(r_i)\) 替代 \(\frac{1}{2}r_i^2\)，其中 \(\rho\) 的增长比平方慢：

\[\min_x \sum_{i=1}^m \rho(r_i(x))\]

常用核函数对比：

核函数	\(\rho(s)\), \(s = \frac{1}{2}r^2\)	大残差行为	工程特性
平方（默认）	\(s\)	二次增长	对离群点敏感
Huber (\(\delta\))	$\delta(	r	- \delta/2)$ 当 $
Cauchy (\(c\))	\(\frac{c^2}{2}\log(1 + s/c^2)\)	对数增长	更激进的软截断
Tukey (\(c\))	\(\frac{c^2}{6}[1-(1-s/c^2)^3]\) 当 $	r	\le c$

使用鲁棒核后的 GN 修改：

目标函数的梯度变为 \(\nabla F = J^T W r\)，其中 \(W = \text{diag}(\rho'(r_i^2))\) 是由核函数导数决定的权重矩阵。Hessian 近似为 \(J^T W J\)。因此**鲁棒核的效果等价于对残差做加权最小二乘**——大残差的权重被自动降低。

Ceres 中的使用（详见 §3.8）：

// Huber 核：小残差正常平方，大残差线性——最稳妥的默认选择
ceres::LossFunction* huber = new ceres::HuberLoss(1.0);

// Cauchy 核：更激进的软截断
ceres::LossFunction* cauchy = new ceres::CauchyLoss(0.5);

// 添加到问题中
problem.AddResidualBlock(cost_function, huber, &params);

3.8 Ceres Solver 中的 LM 实现

// Ceres Solver 的 LM 关键选项
ceres::Solver::Options options;

// 线性求解器选择（决定每步 Newton 系统的求解方式）
options.linear_solver_type = ceres::SPARSE_SCHUR;
// DENSE_QR: 小规模（<100 参数）
// SPARSE_NORMAL_CHOLESKY: 中等规模
// SPARSE_SCHUR: BA（利用路标-位姿结构）
// ITERATIVE_SCHUR: 大规模 BA（CG 近似求解）

// 信赖域策略
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
// 或 DOGLEG（更适合不确定步长的场景）

// 初始信赖域半径
options.initial_trust_region_radius = 1e4;  // 过小 → 保守；过大 → 首步可能失败

// 鲁棒核函数（处理离群点/大残差）
ceres::LossFunction* loss = new ceres::HuberLoss(1.0);
// Huber: 小残差用平方，大残差用线性 → 降低离群点影响
// Cauchy: 更激进的软截断
// Tukey: 超过阈值的残差完全忽略

3.9 Schur 补——BA 的灵魂

Bundle Adjustment 的 Hessian \(H = J^TJ\) 有特殊的**块稀疏结构**：

\[H = \begin{bmatrix} H_{cc} & H_{cp} \\ H_{pc} & H_{pp} \end{bmatrix}\]

其中 \(H_{cc}\) 是相机-相机块（稠密），\(H_{pp}\) 是路标-路标块（块对角，因为路标之间不直接耦合），\(H_{cp}\) 是相机-路标耦合。

**Schur 补**消去路标变量： $\(\underbrace{(H_{cc} - H_{cp}H_{pp}^{-1}H_{pc})}_{\text{reduced camera system}} \Delta x_c = -(b_c - H_{cp}H_{pp}^{-1}b_p)\)$

由于 \(H_{pp}\) 块对角，\(H_{pp}^{-1}\) 是 \(O(|\text{landmarks}|)\)；消元后的相机系统维度只有 \(6|\text{cameras}|\)。

复杂度对比： - 直接求解 \(H\Delta x = b\)：\(O((6C + 3L)^3)\)（\(C\) 相机，\(L\) 路标） - Schur 补：\(O(C^3 + CL)\)——对 \(L \gg C\) 的典型 BA 快数个数量级

⚠️ 常见陷阱

⚠️ 编程陷阱：Jacobian 列不满秩导致 GN 发散 - 原因：\(J^TJ\) 奇异 → GN 步无穷大或方向错误 - 典型场景：BA 中未固定第一帧（gauge freedom）、ICP 退化方向 - 对策： - 固定首帧：problem.SetParameterBlockConstant(poses[0]) - 使用 LM（\(\lambda I\) 正则化保证非奇异） - Ceres 的 SubsetManifold 固定部分参数

💡 概念误区：认为 LM 是"GN 的改进版" - 更准确的说法：LM = GN + 信赖域的自适应混合 - \(\lambda \to 0\) 时 LM 退化为 GN（利用二阶信息，快速收敛） - \(\lambda \to \infty\) 时 LM 退化为梯度下降（全局安全，但慢） - LM 的智慧在于**自动在两个极端之间切换**

🧠 思维陷阱：认为"大残差问题用不了 NLS 方法" - 大残差确实让 GN 退化，但加入**鲁棒核函数**（Huber/Cauchy/Tukey）后，有效残差被压低 - 鲁棒核的本质：把 \(\frac{1}{2}r^2\) 替换为 \(\rho(r)\)（增长更慢的函数），使得大残差的"影响力"被限制 - SLAM 中的鲁棒核是处理错误数据关联（误匹配）的标准工具

练习

1. （推导） 从 \(\nabla^2 f = J^TJ + S\) 出发，证明当 \(r(x^*) = 0\) 时 GN 具有局部二次收敛。
2. （双重解释） 证明 LM 步 \((J^TJ + \lambda I)\Delta x = -J^Tr\) 是信赖域子问题 \(\min \frac{1}{2}\|J\Delta x + r\|^2\) s.t. \(\|\Delta x\| \le \Delta\) 的 Lagrange 最优性条件。
3. （编程） 用 Ceres Solver 实现 2D 点集的圆拟合 \((x_i - a)^2 + (y_i - b)^2 = R^2\)，加入 3 个离群点，对比无鲁棒核 vs Huber 核的拟合结果。

---

4. 全局化策略：线搜索与信赖域 ⭐⭐

4.1 动机：Newton/GN 只有局部收敛

Newton 法和 GN 法都只在最优解的**邻域**内有收敛保证。远离最优解时： - Newton 方向可能不是下降方向（Hessian 不正定） - GN 步可能太大（线性化精度不够）

全局化策略**的作用：在任意初始点保证目标函数**单调下降，最终收敛到稳定点。

两种主流全局化策略： - 线搜索：固定方向 \(p_k\)，沿该方向找合适步长 \(\alpha_k\) - 信赖域：固定步长范围 \(\|\Delta x\| \le \Delta_k\)，在此范围内找最优方向

4.2 线搜索框架与 Armijo-Wolfe 条件的完整推导

4.2.1 一般框架

线搜索将"选方向"和"选步长"解耦为两个子问题： 1. 确定下降方向 \(p_k\)（如 Newton 方向、BFGS 方向、梯度方向） 2. 找步长 \(\alpha_k\) 满足某个"充分下降"条件 3. 更新 \(x_{k+1} = x_k + \alpha_k p_k\)

为什么需要步长控制？ 如果不控制步长，即使方向是下降的，过大的步长也可能跳过极小点甚至导致发散。以 \(f(x) = x^2\) 为例：梯度方向 \(p = -2x\)，如果步长 \(\alpha > 1\)，则 \(|x_{k+1}| = |x_k - 2\alpha x_k| = |1-2\alpha||x_k| > |x_k|\)——越走越远。

4.2.2 Armijo 条件（充分下降）⭐⭐

动机：我们需要一个标准来判断"步长足够小、目标函数确实下降了"。最简单的想法是要求 \(f(x_k + \alpha p_k) < f(x_k)\)，但这太弱了——步长可以无穷小地满足这个条件。

Armijo 条件（也称"充分下降条件"或"第一 Wolfe 条件"）：

\[f(x_k + \alpha p_k) \le f(x_k) + c_1 \alpha \nabla f(x_k)^T p_k \quad (c_1 \in (0,1))\]

推导：在 \(\alpha = 0\) 处做一阶 Taylor 展开： $\(\phi(\alpha) := f(x_k + \alpha p_k) \approx f(x_k) + \alpha \nabla f(x_k)^T p_k\)$

由于 \(p_k\) 是下降方向（\(\nabla f^T p_k < 0\)），右端是一条向下的直线。Armijo 条件要求 \(\phi(\alpha)\) 在这条直线的"\(c_1\)-斜率版本"之下： $\(\phi(\alpha) \le \phi(0) + c_1 \alpha \phi'(0)\)$

\(c_1\) 越小，条件越宽松（几乎任何下降都接受）；\(c_1\) 越大，条件越严格。实践中 \(c_1 = 10^{-4}\) 是标准选择——非常宽松，几乎只排除"完全没有下降"的步长。

Armijo 回溯线搜索（最简单的实现）：

def armijo_backtrack(f, grad_f, x, p, c1=1e-4, rho=0.5, alpha_init=1.0):
    """
    Armijo 回溯线搜索
    参数:
        f: 目标函数
        grad_f: 梯度（在 x 处）
        x: 当前点
        p: 搜索方向（必须是下降方向）
        c1: Armijo 参数（充分下降系数）
        rho: 回溯因子（每次将步长乘以 rho）
        alpha_init: 初始步长
    """
    alpha = alpha_init
    f_x = f(x)
    slope = grad_f @ p  # 方向导数，应为负数

    # 持续缩小步长直到 Armijo 条件满足
    while f(x + alpha * p) > f_x + c1 * alpha * slope:
        alpha *= rho  # 步长减半（rho=0.5）

    return alpha

反事实推理：如果不用 Armijo 而只要求 \(f(x_{k+1}) < f(x_k)\) 会怎样？考虑 \(f(x) = x^4\)，梯度下降方向 \(p = -4x^3\)。取极小的步长 \(\alpha_k = 1/k^2\)，每步确实下降了，但 \(x_k \to x^*\) 的速度可以任意慢。Armijo 条件排除了这种"名义下降但实际无进展"的情况。

4.2.3 Wolfe 条件（充分下降 + 曲率条件）⭐⭐

Armijo 条件只保证"步长不太大"，但步长可以太小。太小的步长浪费了计算资源（明明可以走更远），也可能导致拟牛顿法的 Hessian 近似质量下降。曲率条件（第二 Wolfe 条件）排除了过小的步长：

\[\nabla f(x_k + \alpha p_k)^T p_k \ge c_2 \nabla f(x_k)^T p_k \quad (c_2 \in (c_1, 1))\]

推导：令 \(\phi(\alpha) = f(x_k + \alpha p_k)\)，则 \(\phi'(\alpha) = \nabla f(x_k + \alpha p_k)^T p_k\)。曲率条件要求 \(\phi'(\alpha) \ge c_2 \phi'(0)\)。

由于 \(\phi'(0) < 0\)（下降方向），\(c_2 \phi'(0)\) 是一个负数。如果 \(\alpha\) 太小，\(\phi'(\alpha) \approx \phi'(0)\) 仍然是很大的负数（说明还可以继续走）。曲率条件要求斜率变得"不那么负"（函数变平或开始上升），即步长足够大。

强 Wolfe 条件：把曲率条件改为 \(|\nabla f(x_k + \alpha p_k)^T p_k| \le c_2 |\nabla f(x_k)^T p_k|\)，额外要求步长不要落在极小点的"另一侧"（排除步长过大的情况）。

参数选择：\(0 < c_1 < c_2 < 1\)。Newton/拟牛顿推荐 \(c_1 = 10^{-4}\), \(c_2 = 0.9\)；CG 推荐 \(c_2 = 0.1\)。\(c_2\) 对 CG 更小是因为 CG 的共轭性对步长精度敏感。

Wolfe 步长存在性定理（Nocedal-Wright Lemma 3.1）：若 \(f \in C^1\)，\(p_k\) 是下降方向，\(f\) 下有界，则**总存在**满足 Wolfe 条件的步长区间。

证明：由 \(f\) 下有界和 \(\phi'(0) < 0\)，函数 \(\phi(\alpha)\) 必须在某处"回升"。由中值定理，存在 \(\bar{\alpha}\) 使 \(\phi'(\bar{\alpha}) = \frac{\phi(\bar{\alpha}) - \phi(0)}{\bar{\alpha}}\)。选取 \(\bar{\alpha}\) 为满足 Armijo 条件的上界（即 \(\phi(\bar{\alpha}) = \phi(0) + c_1 \bar{\alpha} \phi'(0)\)），则 \(\phi'(\bar{\alpha}) = c_1 \phi'(0)\)。由 \(c_1 < c_2\)，有 \(\phi'(\bar{\alpha}) = c_1 \phi'(0) > c_2 \phi'(0)\)（注意 \(\phi'(0) < 0\)），满足曲率条件。中间的 \(\alpha\) 值（连续性保证存在）同时满足两个 Wolfe 条件。

4.2.4 Zoutendijk 定理（全局收敛的基石）⭐⭐⭐

定理（Zoutendijk 1970）。 设 \(f \in C^1\)，\(\nabla f\) Lipschitz 连续（常数 \(L\)），\(f\) 下有界。若每步使用满足 Wolfe 条件的步长，且搜索方向满足 \(\cos\theta_k = \frac{-\nabla f_k^T p_k}{\|\nabla f_k\| \|p_k\|} > 0\)，则：

\[\sum_{k=0}^\infty \cos^2\theta_k \|\nabla f(x_k)\|^2 < \infty\]

完整证明：

Step 1：由 Armijo 条件：\(f_{k+1} \le f_k + c_1 \alpha_k g_k^T p_k\)

Step 2：由曲率条件：\(g_{k+1}^T p_k \ge c_2 g_k^T p_k\)，因此 \((g_{k+1} - g_k)^T p_k \ge (c_2 - 1) g_k^T p_k\)

Step 3：由 \(\nabla f\) 的 Lipschitz 连续性：\(\|g_{k+1} - g_k\| \le L \|x_{k+1} - x_k\| = L\alpha_k\|p_k\|\)

结合 Step 2 和 3（Cauchy-Schwarz）： $\(L\alpha_k\|p_k\|^2 \ge (g_{k+1} - g_k)^T p_k \ge (c_2 - 1)g_k^T p_k\)$

因此 \(\alpha_k \ge \frac{(c_2 - 1)g_k^T p_k}{L\|p_k\|^2}\)

Step 4：代入 Armijo 条件： $\(f_k - f_{k+1} \ge -c_1 \alpha_k g_k^T p_k \ge \frac{c_1(1-c_2)}{L} \frac{(g_k^T p_k)^2}{\|p_k\|^2} = \frac{c_1(1-c_2)}{L} \cos^2\theta_k \|g_k\|^2\)$

Step 5：对所有 \(k\) 求和（telescoping）： $\(f_0 - f^* \ge f_0 - \lim_{K \to \infty} f_K \ge \frac{c_1(1-c_2)}{L} \sum_{k=0}^\infty \cos^2\theta_k \|g_k\|^2\)$

由 \(f_0 - f^*\) 有限，级数收敛。\(\blacksquare\)

推论：若 \(\cos\theta_k \ge \delta > 0\)（方向与梯度夹角有下界），则 \(\sum \|g_k\|^2 < \infty\)，因此 \(\|g_k\| \to 0\)——全局收敛到稳定点。

为什么这个定理是基石？ 它为所有基于线搜索的方法提供了统一的收敛保证框架。具体算法只需验证两件事：(1) 搜索方向是下降方向，(2) 方向与梯度的夹角不趋于 \(90°\)。梯度下降（\(\theta_k = 0\)）、Newton 法（局部 \(\theta_k \to 0\)）、BFGS（可证 \(\cos\theta_k\) 有下界）都满足这些条件。

4.3 信赖域框架

一般框架： 1. 在当前点 \(x_k\) 构造二次模型 \(m_k(\Delta x) = f(x_k) + g_k^T\Delta x + \frac{1}{2}\Delta x^T B_k \Delta x\) 2. 在 \(\|\Delta x\| \le \Delta_k\) 内求解 \(\min m_k(\Delta x)\)（信赖域子问题） 3. 计算实际/预测下降比 \(\rho_k = \frac{f(x_k) - f(x_k + \Delta x)}{m_k(0) - m_k(\Delta x)}\) 4. 根据 \(\rho_k\) 决定是否接受步并调整 \(\Delta_k\)

信赖域更新规则： $\(\begin{cases} \Delta_{k+1} = 2\Delta_k, & \text{accept step} & \text{if } \rho_k > 0.75 \\ \Delta_{k+1} = \Delta_k, & \text{accept step} & \text{if } 0.25 \le \rho_k \le 0.75 \\ \Delta_{k+1} = \Delta_k / 4, & \text{reject step} & \text{if } \rho_k < 0.25 \end{cases}\)$

信赖域全局收敛定理（Powell 1975, Conn-Gould-Toint Thm 6.4.6）：若 \(B_k\) 一致有界、每步至少获得 Cauchy 下降，则 \(\liminf \|\nabla f(x_k)\| = 0\)。

4.4 信赖域子问题的三种求解方式——Cauchy → Dogleg → Steihaug-CG 完整链

信赖域框架的核心子问题是：\(\min_{p} m_k(p)\) s.t. \(\|p\| \le \Delta_k\)。这是一个带球约束的二次规划。三种求解方式形成递进关系——从最简单的解析解到能处理大规模问题的迭代解。

方法 1：Cauchy 点（最简单，全局收敛的下界保证）⭐⭐

思想：沿负梯度方向（最速下降方向），在信赖域内走到模型最小点。

精确推导：沿 \(p = -\alpha g_k\) 方向，模型函数为： $\(m_k(-\alpha g_k) = f_k - \alpha \|g_k\|^2 + \frac{1}{2}\alpha^2 g_k^T B_k g_k\)$

对 \(\alpha\) 求导令其为零： $\(\alpha^* = \frac{\|g_k\|^2}{g_k^T B_k g_k}\)$

两种情况： - 若 \(\alpha^* \|g_k\| \le \Delta_k\)：Cauchy 点在信赖域内部，\(p^C = -\alpha^* g_k\) - 若 \(\alpha^* \|g_k\| > \Delta_k\)：截断到信赖域边界，\(p^C = -\frac{\Delta_k}{\|g_k\|} g_k\)

Cauchy 点下降量的下界（Conn-Gould-Toint Lemma 6.3.1）：

\[m_k(0) - m_k(p^C) \ge \frac{1}{2}\|g_k\| \min\left(\Delta_k, \frac{\|g_k\|}{\|B_k\|}\right)\]

这个下界是信赖域全局收敛定理的关键——任何比 Cauchy 点更好的方法都自动保证全局收敛。

Cauchy 点的局限：只用了梯度信息，没有利用 \(B_k\) 的曲率结构。收敛率退化为梯度下降——最坏情况 \(O(\kappa \log(1/\epsilon))\) 步。

方法 2：Dogleg（Powell 1970）——Cauchy 与 Newton 的最佳折中 ⭐⭐

动机：Cauchy 点太保守（只用梯度），Newton 点可能在信赖域外。能否找到一个在两者之间的"好"路径？

构造：定义两个关键点： - \(p^U = -\frac{\|g_k\|^2}{g_k^T B_k g_k} g_k\)（无约束 Cauchy 点，沿梯度最优） - \(p^B = -B_k^{-1} g_k\)（Newton/BFGS 点，二次模型最优）

Dogleg 路径是连接 \(0 \to p^U \to p^B\) 的折线：

\[p(\tau) = \begin{cases} \tau p^U & \text{if } \tau \in [0, 1] \\ p^U + (\tau-1)(p^B - p^U) & \text{if } \tau \in [1, 2] \end{cases}\]

关键性质（需要 \(B_k \succ 0\)）：

1. \(\|p(\tau)\|\) 沿路径**单调递增**
2. \(m_k(p(\tau))\) 沿路径**单调递减**

因此可以唯一地找到 \(\tau^*\) 使 \(\|p(\tau^*)\| = \Delta_k\)。

\(\tau^*\) 的求解：在第二段 \((\tau \in [1,2])\)，解方程 \(\|p^U + (\tau-1)(p^B - p^U)\|^2 = \Delta_k^2\)，这是关于 \((\tau - 1)\) 的一元二次方程，有解析解。

为什么 Dogleg 比 Cauchy 好？ Dogleg 在信赖域足够大时退化为 Newton 步（获得二次收敛），在信赖域很小时退化为 Cauchy 步（保证全局收敛）。中间状态则是两者的平滑插值。

跨领域类比：Dogleg 就像自动驾驶中的路径规划——当"道路宽敞"（信赖域大）时走最短路径（Newton 步），当"道路狭窄"（信赖域小）时沿安全方向缓慢前行（Cauchy 步），在中间情况下自动折中。

局限：Dogleg 要求 \(B_k \succ 0\)（否则 \(p^B\) 可能不是极小点方向），且需要显式求解 \(B_k^{-1} g_k\)——\(O(n^3)\) 或 \(O(n^2)\)（若 \(B_k\) 已分解）。

方法 3：Steihaug-CG（大规模首选）⭐⭐⭐

动机：Dogleg 需要 \(B_k^{-1}\)（\(O(n^3)\)）且要求 \(B_k \succ 0\)。对大规模非凸问题，我们需要一个只用 \(B_k v\)（Hessian-vector 积）、能处理不定 \(B_k\) 的方法。

算法（Steihaug 1983, Toint 1981）：

从 \(p_0 = 0, r_0 = g_k, d_0 = -g_k\) 出发执行 CG 迭代：

for j = 0, 1, 2, ...:
    1. 计算 Hessian-vector 积：w = B_k * d_j
    2. 曲率检查：如果 d_j^T w <= 0（负曲率方向）
       → 沿 d_j 走到信赖域边界：找 tau>0 使 ||p_j + tau*d_j|| = Delta
       → 返回 p_j + tau*d_j（沿负曲率方向逃逸鞍点）
    3. CG 步长：alpha_j = (r_j^T r_j) / (d_j^T w)
    4. 更新解：p_{j+1} = p_j + alpha_j * d_j
    5. 信赖域检查：如果 ||p_{j+1}|| >= Delta
       → 截断到信赖域边界：找 tau>0 使 ||p_j + tau*d_j|| = Delta
       → 返回 p_j + tau*d_j
    6. 更新残差：r_{j+1} = r_j + alpha_j * w
    7. 收敛检查：如果 ||r_{j+1}|| <= eta * ||g_k||
       → 返回 p_{j+1}（足够精确）
    8. 更新共轭方向：beta_{j+1} = (r_{j+1}^T r_{j+1}) / (r_j^T r_j)
       d_{j+1} = -r_{j+1} + beta_{j+1} * d_j

Steihaug-CG 的三种优雅退出：

退出原因	含义	效果
负曲率 \(d^T B d \le 0\)	当前方向是"鞍点逃逸"方向	沿此方向走到边界，帮助越过鞍点
超出信赖域	CG 步太大，超出安全范围	截断到边界，保持信赖域约束
CG 收敛	子问题已充分精确求解	返回近似 Newton 步

优势：只需 Hessian-vector 积 \(B_k \cdot v\)（不需要完整 \(B_k\)），适合大规模问题。Ceres 的 DOGLEG 和 Ipopt 的内部求解器都源于此。

与 Dogleg 的对比：

特性	Dogleg	Steihaug-CG
要求 \(B_k \succ 0\)	是	否（能处理不定 Hessian）
需要 \(B_k^{-1}\)	是	否（只需 \(B_k v\)）
每步代价	\(O(n^3)\) 或 \(O(n^2)\)	\(O(kn)\)（\(k\)=CG 步数）
步的质量	在 \(B_k \succ 0\) 时接近最优	渐近最优（\(k \to n\) 时精确求解）
适用规模	\(n < 10^4\)	无上限
负曲率处理	不适用	自然逃逸鞍点

4.5 线搜索 vs 信赖域：对偶关系与统一视角 ⭐⭐

线搜索和信赖域看似是两种完全不同的策略，但从数学上看它们有深刻的联系：

特性	线搜索	信赖域
步控制	固定方向，调步长	固定范围，调方向+步长
负曲率处理	需要 Hessian 修正	自然处理（截断到边界）
每步代价	可能多次函数求值（找 \(\alpha\)）	一次子问题求解
拒绝步	不拒绝（减小步长直到接受）	可能拒绝（缩小 \(\Delta\)）
理论收敛	Zoutendijk + 方向条件	全局收敛定理（更干净）
实际表现	Newton + Wolfe 通常很好	对不定 Hessian 更鲁棒

对偶关系：信赖域子问题 \(\min m(p)\) s.t. \(\|p\| \le \Delta\) 的 Lagrange 对偶是：

\[\min_p m(p) + \frac{\lambda}{2}\|p\|^2, \quad \lambda \ge 0\]

当 \(\lambda = 0\) 时，这就是无约束 Newton 步（信赖域足够大）。当 \(\lambda > 0\) 时，这等价于 \(H_k + \lambda I\) 的正则化 Newton 步——恰好是 LM 方法。

因此 LM 既是信赖域方法（通过 \(\Delta\)），也等价于正则化线搜索方法（通过 \(\lambda\)）。\(\lambda\) 和 \(\Delta\) 是同一枚硬币的两面。

工程选择准则：

需要选择全局化策略？
├── Hessian 正定（凸子问题）？
│   ├── 是 → 线搜索（简单高效，$\alpha=1$ 通常直接满足 Wolfe）
│   └── 否 → 信赖域（自动处理负曲率）
├── 问题规模大（$n > 10^4$）？
│   ├── 是 → 信赖域 + Steihaug-CG（只需 Hessian-vector 积）
│   └── 否 → 线搜索或 Dogleg 都可以
└── 是最小二乘问题？
    └── 是 → LM（天然的信赖域/正则化双重身份）

4.6 全局收敛定理的统一框架 ⭐⭐⭐

信赖域全局收敛定理（Powell 1975, Conn-Gould-Toint Thm 6.4.6）可以精确陈述如下：

定理。 设 \(f \in C^2\)，\(\nabla^2 f\) 一致有界，\(f\) 下有界。若信赖域算法满足： 1. 每步至少获得 Cauchy 点下降（fraction of Cauchy decrease） 2. 信赖域半径 \(\Delta_k\) 按标准规则更新（§4.3）

则 \(\liminf_{k \to \infty} \|\nabla f(x_k)\| = 0\)。

与 Zoutendijk 的比较：

定理	框架	条件	结论
Zoutendijk	线搜索	Wolfe 步长 + \(\cos\theta_k \ge \delta\)	\(\sum \cos^2\theta_k \|g_k\|^2 < \infty\)
Powell-CGT	信赖域	Cauchy 下降 + 标准 TR 更新	\(\liminf \|g_k\| = 0\)

两者的区别：Zoutendijk 给出 \(\|g_k\| \to 0\)（强于 liminf），但需要更强的方向条件。Powell-CGT 只需 Cauchy 下降（几乎任何合理算法都满足），但结论稍弱（liminf vs lim）。

为什么这些定理是"基石"？ 它们告诉我们：只要满足一些很温和的条件（步长合理、方向不太偏），算法就**一定**能找到稳定点。这是工程师信任优化求解器的理论基础——不是"碰运气"，而是"有数学保证"。

⚠️ 常见陷阱

⚠️ 编程陷阱：Wolfe 条件中 \(c_1, c_2\) 搞反 - \(c_1 < c_2\) 是必要条件。若 \(c_2 \le c_1\)，可能不存在满足条件的步长 - Newton/BFGS：\(c_1 = 10^{-4}\), \(c_2 = 0.9\)（步长 \(\alpha = 1\) 通常直接满足） - CG：\(c_2 = 0.1\)（需要更严的曲率条件保证方向质量）

💡 概念误区：认为"信赖域每步比线搜索贵" - 对 Dogleg/Steihaug，信赖域子问题的代价与一次 Newton 步相当 - 信赖域的优势在于它**不需要试探步长**——一步决定接受或拒绝 - 线搜索的代价在于**可能需要多次函数求值**来找满足 Wolfe 的 \(\alpha\)

🧠 思维陷阱：认为"全局收敛 = 找到全局最优" - "全局收敛"（global convergence）的数学含义是"从任意初始点都能收敛到某个稳定点" - 它**不**保证收敛到的是全局最优——只是某个 \(\nabla f = 0\) 的点（可能是局部极小或鞍点） - 不要与"全局优化"（global optimization）混淆——那是完全不同的问题类，需要分支定界等指数级代价的算法

练习

1. （编程） 实现带 Armijo 回溯 + 强 Wolfe 条件的线搜索，在 Rosenbrock 函数上测试。记录每步的函数求值次数，验证 \(\alpha = 1\) 对 Newton 方向通常直接被接受。
2. （推导） 证明 Dogleg 路径上模型函数 \(m_k(p(\tau))\) 单调下降。提示：分两段分别证明——第一段沿梯度方向下降是直接的，第二段需要利用 \(B_k \succ 0\)。
3. （概念） 为什么 CG 遇到负曲率时要"走到信赖域边界"而不是"回退"？提示：负曲率方向是目标函数的"鞍点逃逸"方向——沿此方向前进能降低函数值。
4. （推导） 从 LM 子问题 \((J^TJ + \lambda I)\Delta x = -J^Tr\) 出发，证明它等价于信赖域子问题 \(\min \frac{1}{2}\|J\Delta x + r\|^2\) s.t. \(\|\Delta x\| \le \Delta\) 的 KKT 条件。\(\lambda\) 和 \(\Delta\) 是什么关系？

---

5. 拟牛顿法：BFGS 与 L-BFGS ⭐⭐⭐

5.1 动机：二阶信息太贵，但一阶太慢

Newton 法需要完整 Hessian（\(O(n^2)\) 存储 + \(O(n^3)\) 求解），梯度下降只需 \(O(n)\) 但收敛极慢。拟牛顿法在两者之间取得平衡：用一阶信息（梯度差）逐步构建 Hessian 近似。

核心思想：在迭代过程中维护一个 \(B_k \approx \nabla^2 f(x_k)\) 的近似矩阵（或其逆 \(H_k \approx [\nabla^2 f]^{-1}\)），每步用新的梯度信息做秩-1 或秩-2 更新。

为什么不直接用有限差分估计 Hessian？ 可以，但代价是 \(O(n)\) 次梯度计算（对 Hessian 的每一列做一次差分）。拟牛顿法只用 1 次 梯度计算就更新 Hessian 近似——因为它利用了**累积信息**（所有历史步的梯度差都被编码在 \(B_k\) 中），而不是从头计算。

跨领域类比：拟牛顿法就像"经验丰富的登山者"——他不需要看完整的地形图（Hessian）就知道往哪走，因为他从之前的每一步中积累了对地形的认知（通过 \(s_k, y_k\) 对更新 \(B_k\)）。走的步数越多，对地形的认知越准确，步伐也越精确。

一阶/二阶/拟牛顿的代价-收益对比：

方法	每步信息代价	每步计算代价	收敛率	总代价（到 \(\epsilon\) 精度）
GD	1 次梯度	\(O(n)\)	\(O(\kappa \log 1/\epsilon)\) 步	\(O(n\kappa \log 1/\epsilon)\)
BFGS	1 次梯度	\(O(n^2)\)	\(O(\log\log 1/\epsilon)\) 步（超线性）	\(O(n^2 \cdot \text{少量步})\)
Newton	1 次梯度 + 1 次 Hessian	\(O(n^3)\)	\(O(\log\log 1/\epsilon)\) 步（二次）	\(O(n^3 \cdot \text{很少步})\)

结论：BFGS 在 \(n \in [100, 10000]\) 的中等规模问题上通常是最佳选择——每步比 Newton 便宜一个数量级，但收敛率几乎一样。

5.2 割线条件（Secant Condition）与拟 Newton 法的历史 ⭐⭐

历史背景：拟 Newton 法的历史可以追溯到 1959 年 Davidon 的工作（DFP 方法）。Davidon 在美国阿贡国家实验室工作时，为了加速参数拟合的计算，提出了用一阶信息构造 Hessian 近似的想法。但他的论文一直到 1991 年才正式发表——期间 Fletcher 和 Powell (1963) 独立重新发现并推广了这个方法（DFP），而 BFGS (1970) 则是四位数学家——Broyden, Fletcher, Goldfarb, Shanno——在同一年独立发现的改进版本。

割线条件的推导：

对 \(\nabla f\) 在 \(x_k\) 和 \(x_{k+1}\) 之间做 Taylor 展开：

\[\nabla f(x_{k+1}) = \nabla f(x_k) + \nabla^2 f(\xi) (x_{k+1} - x_k)\]

其中 \(\xi\) 在 \(x_k\) 和 \(x_{k+1}\) 之间。定义 \(s_k = x_{k+1} - x_k\)，\(y_k = \nabla f(x_{k+1}) - \nabla f(x_k)\)，则：

\[\nabla^2 f(\xi) \cdot s_k = y_k\]

我们要求 \(B_{k+1} \approx \nabla^2 f(\xi)\)，自然地：

\[B_{k+1} s_k = y_k \quad \text{（割线条件）}\]

割线条件的几何意义：\(B_{k+1}\) 在方向 \(s_k\) 上"模仿"真实 Hessian 的行为。\(s_k\) 是最近走过的方向——在这个方向上的曲率信息最可靠（因为我们有两个梯度的差值 \(y_k\) 作为证据）。

割线条件不唯一确定 \(B_{k+1}\)：\(n \times n\) 对称矩阵有 \(n(n+1)/2\) 个自由度，但割线条件只给 \(n\) 个约束。需要额外的准则来确定剩余自由度——这就是"最小变化"原则的角色（§5.3.1）。

DFP vs BFGS 的区别：DFP 对 \(H_k = B_k^{-1}\) 做最小变化更新（\(\min \|H_{k+1} - H_k\|_W\) s.t. \(H_{k+1}y_k = s_k\)），BFGS 对 \(B_k\) 做最小变化更新（\(\min \|B_{k+1} - B_k\|_W\) s.t. \(B_{k+1}s_k = y_k\)）。虽然数学上两者是对偶的，但实践中 BFGS 几乎总是更好——因为 \(B_k\) 的最小变化比 \(H_k\) 的最小变化给出更好的自修正性质（Byrd-Nocedal 1989）。

5.3 BFGS 更新公式的完整推导 ⭐⭐

BFGS（Broyden-Fletcher-Goldfarb-Shanno, 1970 年由四位数学家独立发现）是拟牛顿法的"黄金标准"。理解它的推导过程，需要从一个优雅的最优化问题出发。

5.3.1 推导：最小变化原则

问题：已有 \(B_k\)（当前 Hessian 近似），观测到新的曲率信息 \((s_k, y_k)\)，如何构造 \(B_{k+1}\)？

自然的要求： 1. 割线条件：\(B_{k+1} s_k = y_k\) 2. 对称性：\(B_{k+1} = B_{k+1}^T\)（Hessian 是对称矩阵） 3. 最小改变：\(B_{k+1}\) 尽可能接近 \(B_k\)

将第 3 条形式化为优化问题：

\[\min_{B} \|B - B_k\|_W \quad \text{s.t. } B = B^T, \; B s_k = y_k\]

其中 \(\|\cdot\|_W\) 是加权 Frobenius 范数 \(\|A\|_W = \|W^{1/2} A W^{1/2}\|_F\)。

解的显式形式（DFP 或 BFGS，取决于 \(W\) 的选择）：

取 \(W = \bar{G}_k^{-1}\)（\(\bar{G}_k\) 是某个平均 Hessian），解得 BFGS 对 \(B_k\) 的更新：

\[B_{k+1} = B_k - \frac{B_k s_k s_k^T B_k}{s_k^T B_k s_k} + \frac{y_k y_k^T}{y_k^T s_k}\]

第一项是秩-1 减：扣除 \(B_k\) 沿 \(s_k\) 方向的"旧信息"。第二项是秩-1 加：补入沿 \(y_k\) 方向的"新信息"。两项合起来是**秩-2 更新**。

5.3.2 逆 Hessian 的直接更新

实际编程中，我们需要的是搜索方向 \(p_k = -B_k^{-1} g_k = -H_k g_k\)。直接维护 \(H_k = B_k^{-1}\) 更方便。用 Sherman-Morrison-Woodbury 公式，从 \(B_{k+1}\) 的更新推得 \(H_{k+1}\)：

\[H_{k+1} = \left(I - \rho_k s_k y_k^T\right) H_k \left(I - \rho_k y_k s_k^T\right) + \rho_k s_k s_k^T\]

其中 \(\rho_k = \frac{1}{y_k^T s_k}\)。

验证割线条件：\(H_{k+1} y_k = (I - \rho_k s_k y_k^T) H_k (y_k - \rho_k y_k s_k^T y_k) + \rho_k s_k s_k^T y_k\)。由 \(\rho_k = 1/(y_k^T s_k)\)，\(\rho_k (s_k^T y_k) = 1\)，化简得 \(H_{k+1} y_k = s_k\)。正确。

5.3.3 正定性保持的严格证明 ⭐⭐⭐

命题：若 \(H_k \succ 0\) 且 \(y_k^T s_k > 0\)（曲率条件），则 \(H_{k+1} \succ 0\)。

证明：对任意非零 \(v \in \mathbb{R}^n\)，需证 \(v^T H_{k+1} v > 0\)。

令 \(\alpha = \rho_k (y_k^T v)\)，\(w = v - \alpha s_k\)。则：

\[v^T H_{k+1} v = (v - \rho_k s_k y_k^T v)^T H_k (v - \rho_k y_k s_k^T v) + \rho_k (s_k^T v)^2\]

\[= (v - \alpha s_k \cdot \frac{y_k^T v}{s_k^T y_k} \cdot \frac{s_k^T y_k}{y_k^T v})^T H_k (\cdots) + \rho_k (s_k^T v)^2\]

简化后：

\[v^T H_{k+1} v = w^T H_k w + \rho_k (s_k^T v)^2\]

其中 \(w = v - \rho_k (y_k^T v) s_k\)。

情况 1：\(w \ne 0\)。由 \(H_k \succ 0\)，\(w^T H_k w > 0\)，第二项 \(\ge 0\)，所以 \(v^T H_{k+1} v > 0\)。

情况 2：\(w = 0\)，即 \(v = \rho_k (y_k^T v) s_k\)。此时 \(v\) 与 \(s_k\) 共线，\(s_k^T v = \rho_k(y_k^T v)(s_k^T s_k) \ne 0\)（因为 \(v \ne 0\) 且 \(s_k \ne 0\)），所以第二项 \(> 0\)。

综合两种情况，\(v^T H_{k+1} v > 0\) 对所有 \(v \ne 0\) 成立。\(\blacksquare\)

这个证明为什么重要？ 它保证了 BFGS 方向 \(p_k = -H_k g_k\) 永远是下降方向（\(g_k^T p_k = -g_k^T H_k g_k < 0\)），从而 Zoutendijk 定理的前提自动满足。这是 BFGS 全局收敛的基石之一。

本质洞察：BFGS 保正定性的关键条件是 \(y_k^T s_k > 0\)（曲率条件）。在凸问题上，Wolfe 线搜索保证这个条件自动成立（因为凸性 + 曲率条件 \(\Rightarrow\) \(y_k^T s_k > 0\)）。在非凸问题上，这个条件可能失败——这就是非凸 BFGS 的理论困难的根源。

5.4 SR1 更新与不定 Hessian 近似 ⭐⭐⭐

动机：BFGS 始终维护正定的 \(B_k\)。但真实 Hessian 在非凸区域是不定的。如果我们的近似始终"假装"Hessian 正定，就永远无法捕捉到鞍点结构或负曲率方向。

SR1（Symmetric Rank-1）更新：

\[B_{k+1} = B_k + \frac{(y_k - B_k s_k)(y_k - B_k s_k)^T}{(y_k - B_k s_k)^T s_k}\]

性质： - 满足割线条件 \(B_{k+1} s_k = y_k\) - 不保证正定性——这是特征，不是缺陷 - 秩-1 更新（比 BFGS 的秩-2 更"经济"） - 在 \(n\) 步后可以精确重构真 Hessian（对二次函数）

为什么 SR1 允许不定？ 在配合信赖域使用时，不定的 \(B_k\) 是有用的——信赖域子问题可以利用负曲率方向逃逸鞍点。Steihaug-CG 天然处理不定 \(B_k\)（遇到负曲率就沿该方向走到信赖域边界）。

SR1 的危险与对策：

分母 \((y_k - B_k s_k)^T s_k\) 可能为零或很小，导致更新不稳定。标准对策是**跳过更新**：当 \(|(y_k - B_k s_k)^T s_k| < \epsilon \|s_k\| \|y_k - B_k s_k\|\) 时不执行更新。

SR1 vs BFGS 的选型：

场景	推荐方法	理由
凸问题 + 线搜索	BFGS	保正定 → 方向自动下降
非凸 + 信赖域	SR1	能捕捉负曲率，配合 Steihaug-CG
大规模凸	L-BFGS	内存 \(O(mn)\)
大规模非凸	L-SR1	L-BFGS 的 SR1 版本

5.5 L-BFGS：大规模的救星

BFGS 存储 \(H_k\) 需要 \(O(n^2)\)。对 \(n > 10^4\)（深度学习、大规模 SLAM），不可接受。

L-BFGS 的思想：只存最近 \(m\) 对 \((s_k, y_k)\)（\(m = 5 \sim 20\)），需要 \(H_k v\) 时用"两循环递推"（two-loop recursion）在 \(O(mn)\) 内计算。

def lbfgs_two_loop(g, s_history, y_history, H0=1.0):
    """
    L-BFGS 两循环递推：给定梯度 g，返回搜索方向 r = -H_k * g

    参数：
        g: 当前梯度
        s_history: 最近 m 个 s_k = x_{k+1} - x_k
        y_history: 最近 m 个 y_k = grad_{k+1} - grad_k
        H0: 初始 Hessian 逆的标量近似
    """
    m = len(s_history)
    q = g.copy()
    alpha = np.zeros(m)
    rho = np.zeros(m)

    # 第一循环（从新到旧）
    for i in range(m-1, -1, -1):
        rho[i] = 1.0 / (y_history[i] @ s_history[i])
        alpha[i] = rho[i] * (s_history[i] @ q)
        q = q - alpha[i] * y_history[i]

    # 初始 Hessian 逆近似（标量缩放）
    r = H0 * q

    # 第二循环（从旧到新）
    for i in range(m):
        beta = rho[i] * (y_history[i] @ r)
        r = r + s_history[i] * (alpha[i] - beta)

    return -r  # 搜索方向

L-BFGS 在机器人学中的应用： - GTSAM 的 DoglegOptimizer 内部可选 L-BFGS 近似 - PyTorch 的 torch.optim.LBFGS 用于小 batch 的精确行搜索训练 - scipy 的 L-BFGS-B 是带盒约束的标准选择

5.6 L-BFGS 两循环算法的详细分析 ⭐⭐

L-BFGS 是大规模优化中使用最广泛的拟牛顿方法。深入理解两循环递推的每一步，对于调试和改进优化器至关重要。

为什么需要 L-BFGS？ BFGS 存储 \(n \times n\) 的 \(H_k\)（\(O(n^2)\) 内存）。对 \(n = 10^6\)（深度学习参数量级），\(H_k\) 需要 \(8 \times 10^{12}\) 字节 = 8 TB。完全不可行。

L-BFGS 的核心观察：从 \(H_0\)（初始近似，通常为标量矩阵 \(\gamma_k I\)）出发，\(k\) 步 BFGS 更新后的 \(H_k\) 可以用 \(k\) 对向量 \((s_0, y_0), ..., (s_{k-1}, y_{k-1})\) 隐式表示。只存最近 \(m\) 对，截断更早的历史，得到 L-BFGS。

初始标量 \(\gamma_k\) 的选择：

\[\gamma_k = \frac{s_{k-1}^T y_{k-1}}{y_{k-1}^T y_{k-1}} = \frac{y_{k-1}^T s_{k-1}}{\|y_{k-1}\|^2}\]

这个选择使 \(\gamma_k I\) 的特征值近似于 \(\nabla^2 f(x_k)^{-1}\) 的特征值的平均——确保两循环递推的起点不至于太偏。

两循环递推的正确性：算法产生的结果 \(r = H_k g_k\) 与显式维护 \(H_k\) 然后乘以 \(g_k\) 的结果完全一致。证明方法：对 BFGS 更新公式做递归展开，利用向量外积的结合律。

L-BFGS 在 SLAM/BA 中的角色：

Ceres Solver 的 DENSE_SCHUR 和 SPARSE_SCHUR 求解器在 Schur 补消元后的 reduced camera system 上，可以选择 L-BFGS 作为 preconditioner 来加速 CG。具体而言： - 外层：LM/GN 迭代 - 内层（线性系统求解）：CG + L-BFGS preconditioner - 效果：CG 迭代次数从 \(O(n)\) 降至 \(O(\sqrt{\kappa})\)

5.7 BFGS 的凸与非凸行为

凸情形（Powell 1976）：一致凸 + Wolfe 线搜索 → BFGS 全局收敛 + Q-超线性。

证明思路（凸情形全局收敛）：

Step 1：由 Wolfe 线搜索，每步保证 \(y_k^T s_k > 0\)（凸性 + 曲率条件的推论）。

Step 2：由正定性保持定理（§5.3.3），\(H_k \succ 0\) 对所有 \(k\) 成立。

Step 3：方向 \(p_k = -H_k g_k\) 满足 \(\cos\theta_k = \frac{g_k^T H_k g_k}{\|g_k\|\|H_k g_k\|} > 0\)。进一步，可以证明 \(\cos\theta_k\) 有下界 \(\delta > 0\)（利用 \(H_k\) 的特征值有界性）。

Step 4：由 Zoutendijk 定理，\(\sum \cos^2\theta_k \|g_k\|^2 < \infty\)，结合 \(\cos\theta_k \ge \delta\)，得 \(\|g_k\| \to 0\)。

超线性收敛的证明更精细，需要用到 BFGS 更新的"自修正"性质：即使初始 \(H_0\) 与真 Hessian 相差很远，经过足够多步后 \(H_k\) 沿步方向的近似精度会自动提高（Byrd-Nocedal 1989 的 trace-determinant 分析）。

非凸情形的危险： - 曲率条件 \(y_k^T s_k > 0\) 可能失败（非凸点的 Hessian 有负特征值） - \(H_k\) 可能失去正定性 → 方向不是下降方向 - Dai 2002 给出反例：BFGS 在非凸上可能发散（即使用精确线搜索）

对策： - Damped BFGS（Powell 修正）：当 \(y_k^T s_k\) 不够大时，用修正后的 \(\tilde{y}_k\) 替代

具体而言，令 \(\theta = \begin{cases} 1 & \text{if } s_k^T y_k \ge 0.2 s_k^T B_k s_k \\ \frac{0.8 s_k^T B_k s_k}{s_k^T B_k s_k - s_k^T y_k} & \text{otherwise} \end{cases}\)

然后 \(\tilde{y}_k = \theta y_k + (1-\theta) B_k s_k\)。这保证 \(s_k^T \tilde{y}_k \ge 0.2 s_k^T B_k s_k > 0\)。

• Cautious update（Li-Fukushima 2001）：\(y_k^T s_k < \epsilon \|g_k\| \|s_k\|^2\) 时跳过更新
• SR1 + 信赖域：允许不定 Hessian 近似，用信赖域保全局收敛

⚠️ 常见陷阱

⚠️ 编程陷阱：L-BFGS 的 \(m\) 选择 - \(m\) 太小（如 3）：近似精度差，收敛慢 - \(m\) 太大（如 100）：每步代价 \(O(mn)\) 不再便宜，且内存增加 - 经验值：\(m = 5 \sim 20\) 对大多数问题都够用

💡 概念误区：认为"BFGS 比 Newton 慢所以不如 Newton" - BFGS 的**每步代价**是 \(O(n^2)\)，Newton 是 \(O(n^3)\) - 对 \(n = 1000\)：BFGS 每步 \(\sim 10^6\) 运算；Newton 每步 \(\sim 10^9\) 运算 - 虽然 BFGS 需要更多步（超线性 vs 二次），但总时间可能更短

练习

1. （推导） 证明 BFGS 更新保正定性：若 \(H_k \succ 0\) 且 \(y_k^T s_k > 0\)，则 \(H_{k+1} \succ 0\)。
2. （编程） 实现 L-BFGS 两循环递推，在 Rosenbrock 函数上对比 \(m = 3, 10, 50\) 的收敛速度。
3. （概念） 解释 Dennis-More 条件为什么不要求 \(B_k \to \nabla^2 f(x^*)\)（即 Hessian 近似整体收敛），只要求"沿步方向正确"。

---

6. SQP（序列二次规划）⭐⭐⭐

6.1 动机：有约束的非线性优化

轨迹优化、NMPC 的典型形式是**非线性规划**（Nonlinear Programming, NLP）：

\[\min_x f(x) \quad \text{s.t. } c_E(x) = 0,\; c_I(x) \le 0\]

其中 \(f: \mathbb{R}^n \to \mathbb{R}\) 是目标函数，\(c_E: \mathbb{R}^n \to \mathbb{R}^{m_E}\) 是等式约束，\(c_I: \mathbb{R}^n \to \mathbb{R}^{m_I}\) 是不等式约束。

机器人学中的具体例子：

• 轨迹优化：\(\min \int_0^T \|u(t)\|^2 dt\)，s.t. \(\dot{x} = f(x, u)\)（动力学），\(h(x(t)) \le 0\)（避障）
• NMPC：同上但在有限时域 \([0, T]\) 上离散化，实时求解
• 逆运动学：\(\min \|q - q_{\text{ref}}\|^2\)，s.t. \(\text{FK}(q) = x_d\)（末端位姿），\(q_L \le q \le q_U\)（关节限位）

反事实推理：如果没有约束会怎样？无约束 NLP 可以用 Newton/BFGS/L-BFGS 直接求解（§2-5 的内容）。约束的存在使问题本质上更复杂——最优点不再是 \(\nabla f = 0\)，而是 KKT 条件（一组包含 Lagrange 乘子的方程组）。SQP 的思想是：把约束 NLP 分解为一系列 QP 子问题，每个 QP 都是"约束 Newton 步"。

6.2 等式约束 SQP 的完整推导 ⭐⭐

先从最简单的情况开始：只有等式约束 \(\min_x f(x)\) s.t. \(c(x) = 0\)，\(c: \mathbb{R}^n \to \mathbb{R}^m\)。

6.2.1 Lagrangian 与 KKT 系统

Lagrangian：\(\mathcal{L}(x, \lambda) = f(x) + \lambda^T c(x)\)

KKT 条件（一阶最优性必要条件）： $\(\nabla_x \mathcal{L} = \nabla f(x) + A(x)^T \lambda = 0\)$ $\(c(x) = 0\)$

其中 \(A(x) = \nabla c(x)^T \in \mathbb{R}^{m \times n}\) 是约束的 Jacobian。

关键观察：KKT 条件是一个 \((n + m)\) 维的非线性方程组 \(F(x, \lambda) = 0\)。对这个方程组做 Newton 法，就得到 SQP。

6.2.2 从 Newton 法到 SQP

对 KKT 系统做 Newton 步：

\[\begin{bmatrix} \nabla^2_{xx}\mathcal{L}_k & A_k^T \\ A_k & 0 \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta \lambda \end{bmatrix} = -\begin{bmatrix} \nabla f_k + A_k^T \lambda_k \\ c_k \end{bmatrix}\]

其中 \(\nabla^2_{xx}\mathcal{L}_k = \nabla^2 f(x_k) + \sum_i \lambda_{k,i} \nabla^2 c_i(x_k)\) 是 Lagrangian 的 Hessian。

关键等价性：这个 Newton 步恰好等价于求解以下 QP 子问题：

\[\min_{d} \frac{1}{2} d^T W_k d + \nabla f_k^T d$$ $$\text{s.t. } A_k d + c_k = 0\]

其中 \(W_k = \nabla^2_{xx}\mathcal{L}_k\)。QP 的 KKT 条件正是上面的 Newton 系统。

这就是 SQP 名字的由来：Sequential Quadratic Programming——每步求解一个 QP（二次规划）子问题，序列地逼近 NLP 的解。

本质洞察：SQP 不是"对约束 NLP 的某种启发式近似"，而是"对 KKT 系统做 Newton 法"的等价形式。它继承了 Newton 法的所有优良性质（局部二次收敛），同时以 QP 的形式呈现（可以用高效的 QP 求解器）。

6.2.3 SQP 的几何直觉

在当前点 \(x_k\)： 1. 目标函数 \(f\) 被二阶 Taylor 近似为二次函数 2. 约束 \(c(x)\) 被一阶 Taylor 线性化为 \(c_k + A_k(x - x_k) = 0\) 3. QP 子问题就是在这个线性化的约束集上，最小化目标的二次模型

跨领域类比：SQP 就像牛顿法和线性方程组求解器的结合体——Newton 法给出"朝极值点跳"的策略，QP 求解器确保每一跳都"落在约束面上"（至少是约束的线性化近似面上）。

6.3 含不等式约束的 SQP ⭐⭐⭐

一般 NLP \(\min f(x)\) s.t. \(c_E(x) = 0\), \(c_I(x) \le 0\) 的 SQP 子问题：

\[\min_d \frac{1}{2} d^T W_k d + \nabla f_k^T d$$ $$\text{s.t. } \nabla c_E(x_k)^T d + c_E(x_k) = 0$$ $$\nabla c_I(x_k)^T d + c_I(x_k) \le 0\]

这是一个**带等式和不等式约束的凸 QP**（当 \(W_k \succ 0\) 时）。可以用 active-set 法、interior-point 法或 HPIPM 求解。

**QP 子问题的 KKT 条件**给出 \((d_k, \lambda_{k+1}, \mu_{k+1})\)：

\[W_k d + \nabla f_k + A_E^T \lambda_{k+1} + A_I^T \mu_{k+1} = 0$$ $$A_E d + c_E(x_k) = 0$$ $$A_I d + c_I(x_k) \le 0, \quad \mu_{k+1} \ge 0, \quad \mu_{k+1}^T(A_I d + c_I) = 0\]

注意 QP 子问题**同时输出**下一步的 Lagrange 乘子 \(\lambda_{k+1}, \mu_{k+1}\)。

Hessian 近似的选择： - 精确 Hessian \(W_k = \nabla^2_{xx}\mathcal{L}_k\)：局部二次收敛，但计算代价高 - BFGS 近似：维护 \(B_k \approx \nabla^2_{xx}\mathcal{L}_k\)，超线性收敛 - Gauss-Newton 近似（最小二乘 NLP）：\(W_k = J^T J\)，忽略二阶残差项

6.4 SQP 的局部超线性收敛 ⭐⭐⭐

定理（Boggs-Tolle 1995）。 设 \((x^*, \lambda^*)\) 是 NLP 的 KKT 点，满足以下正则性条件：

1. LICQ（线性独立约束规范）：活跃约束的梯度 \(\{\nabla c_i(x^*)\}_{i \in \mathcal{A}}\) 线性独立
2. 严格互补性：活跃不等式约束的乘子 \(\mu_i^* > 0\)
3. 二阶充分条件：\(d^T \nabla^2_{xx}\mathcal{L}(x^*, \lambda^*) d > 0\) 对所有约束切空间中的 \(d \ne 0\)

则在 \((x^*, \lambda^*)\) 的邻域内： - 用精确 Hessian：SQP Q-二次收敛 - 用满足 Dennis-More 条件的 BFGS 近似：SQP Q-超线性收敛

证明思路：SQP = 对 KKT 系统的 Newton 法。KKT 系统的 Jacobian 在正则条件下非奇异，因此 Newton 法的标准收敛理论直接适用。

6.5 SQP 的全局化：Merit Function 与 Filter

SQP 的 QP 子问题可能给出一个**不下降的**步（因为在非凸约束集上局部线性化可能不准确）。需要全局化策略来保证算法从任意初始点收敛。

6.5.1 \(\ell_1\) Merit Function ⭐⭐

思想：将约束 NLP 转化为无约束优化问题，通过惩罚约束违反：

\[\phi_1(x; \mu) = f(x) + \mu \|c_E(x)\|_1 + \mu \|[c_I(x)]_+\|_1\]

其中 \(\mu > 0\) 是惩罚参数，\([z]_+ = \max(0, z)\)。

\(\mu\) 的选择：\(\mu\) 必须大于 KKT 乘子的 \(\ell_\infty\) 范数——\(\mu > \|\lambda^*\|_\infty\)——才能保证 NLP 的解也是 \(\phi_1\) 的极小点。实践中从小的 \(\mu\) 开始，当 SQP 步被拒绝时增大 \(\mu\)。

接受步长当且仅当 \(\phi_1(x_k + \alpha d_k) < \phi_1(x_k) - c \cdot \alpha \cdot \text{pred}\)，其中 pred 是模型预测的下降量。

为什么用 \(\ell_1\) 范数而不是 \(\ell_2\)？ \(\ell_1\) merit 是**精确罚函数**——对足够大的 \(\mu\)，\(\phi_1\) 的极小点恰好是 NLP 的解。而 \(\ell_2\) 罚函数 \(f + \frac{\mu}{2}\|c\|_2^2\) 只有在 \(\mu \to \infty\) 时才趋近 NLP 的解，且大 \(\mu\) 导致 Hessian 病态。\(\ell_1\) 的代价是不光滑（在 \(c_i = 0\) 处不可微），但这在实践中不是问题（用方向导数代替梯度）。

6.5.2 Maratos 效应与二阶修正 ⭐⭐⭐

现象：即使单位步长能达到二次收敛，\(\ell_1\) merit 可能拒绝它（因为约束违反临时增大）。

经典例子：考虑 \(\min x_1\) s.t. \(x_1^2 + x_2^2 = 1\)。从 \((0, 1)\) 出发，SQP 的 Newton 步指向 \((-1, 0)\)。但 \(x_k + d_k = (-1, 1)\) 不在约束面上（\(\|(-1,1)\|^2 = 2 \ne 1\)），约束违反增大。若 \(\mu\) 不够大，\(\phi_1\) 会拒绝这个"其实很好"的步。

解决方案——二阶修正（SOC）：

在 SQP 步 \(d_k\) 之后，附加一步修正： $\(d^{\text{soc}} = \text{solution of: } A_k d^{\text{soc}} = -c(x_k + d_k)\)$

即用一个最小二乘步修正掉 \(x_k + d_k\) 处的约束违反。修正后的步 \(d_k + d^{\text{soc}}\) 通常能被 merit function 接受，且不破坏二次收敛。

6.5.3 Filter 方法（Fletcher-Leyffer 2002）⭐⭐⭐

思想：完全放弃 merit function，用"多目标优化"的视角处理全局化。

Filter 的定义：维护一个集合 \(\mathcal{F} \subset \mathbb{R}^2\)，元素为 \((f_i, h_i)\) 对，其中 \(h_i = \|c(x_i)\|_1\) 是约束违反量。

支配关系：\((f_1, h_1)\) 支配 \((f_2, h_2)\) 当且仅当 \(f_1 \le f_2\) 且 \(h_1 \le h_2\)。

接受准则：新点 \((f_{\text{new}}, h_{\text{new}})\) 被接受当且仅当它**不被 filter 中的任何点支配**——即在 \(f\) 或 \(h\) 的至少一个维度上有改善。

Filter 的优势： - 避免了 \(\mu\) 选择的困难 - 自然允许"增大约束违反但减小目标"或"增大目标但减小约束违反"的步 - 不受 Maratos 效应影响

Ipopt 使用 filter line-search——这是 Ipopt 在实际工程中鲁棒性极好的原因之一。

6.5.4 SNOPT vs Ipopt 的深度对比 ⭐⭐

维度	SNOPT (SQP)	Ipopt (IPM)
算法核心	每步解一个 QP 子问题	每步解一个 barrier KKT Newton 系统
约束处理	活跃集法——显式追踪活跃约束	barrier 法——所有不等式隐式处理
迭代路径	可以在约束边界上/外跳跃	始终在可行域严格内部
Warm-start	极好——上次活跃集是天然热启动	困难——需要重新计算 barrier 参数路径
实时性	适合（活跃集变化通常很小）	不适合（迭代次数不可预测）
大规模稀疏	受限于 QP 求解器能力	天然适合（稀疏 KKT 系统）
病态问题	可能数值不稳定	内置正则化，鲁棒性好
开源/商业	商业（Stanford）	开源（EPL 2.0）
典型用户	航天（NASA）、轨迹优化	过程控制、通用 NLP

工程选型建议： - 研究原型 / 离线规划 → Ipopt（开源、鲁棒、不需要调参） - 实时 MPC → acados SQP-RTI（代码生成、确定性时间） - 大规模稀疏 NLP → Ipopt + MA57/MUMPS - 需要 warm-start（连续优化问题） → SNOPT 或 SQP 类方法

6.6 acados 中的 SQP-RTI

acados 实现了 RTI（Real-Time Iteration） 策略：每个控制周期只做 一步 SQP（不迭代到收敛），利用 warm-start 和系统的连续性保证闭环稳定性。

# acados Python 接口示例：四旋翼 NMPC
from acados_template import AcadosOcp, AcadosOcpSolver

ocp = AcadosOcp()
ocp.model = quadrotor_model  # CasADi 符号模型

# 求解器选项
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'  # 实时迭代
ocp.solver_options.integrator_type = 'ERK'       # 4 阶 Runge-Kutta
ocp.solver_options.tf = 1.0                      # 预测时域
ocp.solver_options.N_horizon = 20                # 时域步数

solver = AcadosOcpSolver(ocp)

# 实时控制循环
while running:
    solver.set(0, "lbx", x_current)  # 设置初始状态约束
    solver.set(0, "ubx", x_current)
    status = solver.solve()          # 一步 SQP-RTI（< 1ms）
    u_opt = solver.get(0, "u")       # 取第一个控制输入
    apply_control(u_opt)

⚠️ 常见陷阱

⚠️ 编程陷阱：Ipopt 被误称为 SQP - Ipopt 是 内点法（IPM）+ filter line-search，不是 SQP - IPM 通过 barrier 把不等式约束吸收进目标，在增大的 KKT 系统上做 Newton - SQP 在每步解一个 QP 子问题（保持约束的组合结构） - 本质区别：IPM 走**中心路径**（约束内部），SQP 可以**跨越约束边界**

💡 概念误区：认为 SQP 每步需要精确求解 QP - 对实时应用（MPC），一步不完整的 QP 求解（inexact SQP）+ warm-start 就够了 - RTI 策略更激进：每个控制周期只做一步 SQP，依赖闭环稳定性

练习

1. （推导） 从等式约束 NLP 的 KKT 系统出发，推导 SQP 子问题的完整 QP 形式，并验证 QP 的 KKT 条件与 Lagrangian Newton 步等价。
2. （工程） 在 CasADi 中定义 cart-pole swing-up 问题（直接 collocation），分别用 Ipopt 和 acados (SQP) 求解，对比求解时间与迭代次数。
3. （概念） 解释 Maratos 效应：构造一个简单的 2D 等式约束 NLP（如 \(\min x_1\) s.t. \(x_1^2 + x_2^2 = 1\)），使得 \(\ell_1\) merit 拒绝单位 SQP 步。计算二阶修正步并验证修正后 merit 接受。
4. （跨章综合） 回顾凸优化中的 KKT 条件和对偶理论：对比凸 QP 和非凸 NLP 的 KKT 条件的区别。在凸情况下 KKT 是充分必要的，在非凸情况下为什么只是必要条件？给出一个 KKT 点不是局部最优的反例。

---

7. 内点法用于 NLP（Ipopt 视角）⭐⭐⭐

7.1 Ipopt 的算法结构 ⭐⭐⭐

Ipopt（Interior Point OPTimizer, Wachter-Biegler 2006）是最广泛使用的通用 NLP 求解器之一。理解它的内部结构，对于调试 MPC/轨迹优化问题至关重要。

标准形式：

\[\min_x f(x) \quad \text{s.t. } c(x) = 0,\; x_L \le x \le x_U\]

核心步骤（四阶段）：

阶段 1：Barrier 变换。 将盒约束 \(x_L \le x \le x_U\) 通过 log-barrier 吸收进目标：

\[\min_x f(x) - \mu \sum_{i=1}^n [\log(x_i - x_{L,i}) + \log(x_{U,i} - x_i)]$$ $$\text{s.t. } c(x) = 0\]

\(\mu > 0\) 是 barrier 参数。\(\mu \to 0\) 时解趋近原问题的解。

为什么用 log-barrier？ \(-\log(x - x_L)\) 在 \(x \to x_L^+\) 时趋于 \(+\infty\)，自然地阻止迭代点越过边界。与显式处理不等式约束（如 SQP 的 active-set）相比，barrier 方法将不等式"连续化"了，使得 Newton 步的 KKT 系统结构固定（不随活跃集变化）。

阶段 2：对扰动 KKT 系统做 Newton。 Barrier 问题的 KKT 条件：

\[\nabla f(x) - \mu X^{-1} e + A^T \lambda = 0$$ $$c(x) = 0\]

其中 \(X = \text{diag}(x - x_L)\)，\(e = (1, ..., 1)^T\)。Newton 步求解：

\[\begin{bmatrix} W_k + \Sigma_k & A_k^T \\ A_k & 0 \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta \lambda \end{bmatrix} = -\begin{bmatrix} \nabla f_k + A_k^T\lambda_k - \mu X_k^{-1} e \\ c_k \end{bmatrix}\]

其中 \(\Sigma_k = \mu X_k^{-2}\) 是 barrier 贡献的对角正则项。

阶段 3：Filter line-search。 使用 §6.5.3 的 filter 方法保证全局收敛，同时避免 Maratos 效应。

阶段 4：\(\mu\) 的更新。 当当前 \(\mu\) 下的 Newton 迭代"足够收敛"（barrier KKT 残差小于某阈值），降低 \(\mu\)（如 \(\mu \leftarrow \mu / 10\)），重新开始 Newton 迭代。最终 \(\mu \to 0\)，得到原问题的解。

跨领域类比：Ipopt 的 barrier 方法就像"慢慢收紧的弹簧"——一开始弹簧（barrier）很强，把变量拉在可行域中央；随着优化进行，弹簧逐渐松弛（\(\mu \to 0\)），允许变量靠近真正的约束边界。最终弹簧完全消失，变量到达最优点。

7.2 IPM 的收敛性与中心路径 ⭐⭐⭐

中心路径（central path）：对每个固定的 \(\mu > 0\)，barrier 问题有唯一解 \(x(\mu)\)。当 \(\mu\) 从大到小变化时，\(x(\mu)\) 构成一条从可行域内部通向最优解的光滑曲线——这就是中心路径。

IPM 的迭代点不精确地追踪中心路径，而是在路径的"邻域管道"中行进。\(\mu\) 降低的速率决定了算法的效率： - 降太快：迭代点远离中心路径，Newton 步需要多次才能回到邻域 - 降太慢：\(\mu \to 0\) 需要太多外迭代

收敛率：对凸 NLP，IPM 可以在 \(O(\sqrt{n} \log(1/\epsilon))\) 次 Newton 步内找到 \(\epsilon\)-最优解（\(n\) 是变量数）。这个"多项式复杂度"保证是 IPM 的理论优势——SQP 没有类似的全局复杂度保证。

但对非凸 NLP，这个多项式保证不再成立——Ipopt 只能保证收敛到 KKT 点。实际迭代次数取决于问题的非凸程度和初始化质量。

Ipopt 的正则化策略：当 KKT 系统不定（非凸区域）或近奇异（约束退化）时，Ipopt 自动添加正则化项 \(\delta_x I\)（对 \(x\) 部分）和 \(-\delta_c I\)（对 \(\lambda\) 部分），使 KKT 矩阵可因式分解。这是 Ipopt 在实际问题上"极少发散"的关键原因。

跨领域类比：IPM 的中心路径就像"河流入海"——河流（中心路径）从内陆（可行域深处）流向大海（最优解），沿途地势逐渐降低。算法就像一艘船顺流而下，但不精确地走在河流正中间，而是在河道宽度内自由航行。\(\mu\) 控制"我们离河口还有多远"——\(\mu\) 越小，越接近入海口（最优解），但河道也越窄（数值越困难）。

7.3 Ipopt 关键选项解读

import cyipopt

# Ipopt 关键选项
nlp = cyipopt.Problem(...)
nlp.add_option('mu_strategy', 'adaptive')     # barrier 参数自适应降低
nlp.add_option('hessian_approximation', 'limited-memory')  # L-BFGS 近似 Hessian
# 或 'exact' 需要提供 Hessian callback
nlp.add_option('linear_solver', 'ma57')       # 稀疏线性求解器（关键性能瓶颈）
nlp.add_option('tol', 1e-6)                   # 收敛容差
nlp.add_option('max_iter', 3000)

7.4 Ipopt vs SQP 的选型依据

维度	Ipopt (IPM)	SNOPT/acados (SQP)
约束处理	barrier（严格可行性）	QP 子问题（可越界）
稀疏性利用	对称不定系统	QP 求解器内部
Warm-start	困难（\(\mu\) 路径）	容易（上次 active set）
实时性	差（迭代数不可预测）	好（RTI 固定步数）
病态处理	好（正则化内置）	需要额外处理
适用场景	研究原型、离线规划	实时 MPC、嵌入式

Warm-start 为什么对 IPM 困难？ IPM 的迭代点沿中心路径前进。改变初始条件后，新的中心路径可能完全不同——从旧路径上的点出发，可能离新路径很远，需要大量迭代才能回到新路径的邻域。而 SQP 的 active-set 在相邻问题间通常变化很小（如 MPC 中相邻时刻的活跃约束集几乎相同），因此天然适合 warm-start。

⚠️ 常见陷阱

💡 概念误区：认为"Ipopt 用了内点法所以是凸优化方法" - Ipopt 的内点法是**非凸** NLP 的内点法，与凸优化中的 IPM 有重要区别 - 凸 IPM 保证全局最优 + 多项式复杂度；非凸 IPM 只保证 KKT 点 - 非凸 IPM 需要额外的 filter/merit function 和正则化来保证全局收敛

⚠️ 编程陷阱：Ipopt 的 hessian_approximation = 'limited-memory' 导致收敛慢 - L-BFGS 近似 Hessian 在约束 NLP 上通常比精确 Hessian 需要 2-5 倍的迭代 - 如果问题允许（CasADi 自动微分），优先提供精确 Hessian - 检查方法：对比 exact vs limited-memory 的迭代次数和求解时间

练习

1. （编程） 用 CasADi + Ipopt 求解一个简单的 NLP：\(\min (x_1 - 2)^2 + (x_2 - 1)^2\) s.t. \(x_1 + x_2 \le 3\), \(x_1^2 + x_2^2 \le 4\)。打印每步的 barrier 参数 \(\mu\) 和 KKT 残差，观察中心路径的行为。
2. （概念） 解释为什么 Ipopt 对"约束不一致"的问题报 "Restoration failed"，而不是简单地报"无可行解"。Ipopt 的 restoration phase 在做什么？

---

8. 流形优化 ⭐⭐⭐⭐

8.1 动机：当变量不在欧氏空间时

前面所有的方法——Newton, GN, LM, BFGS, SQP——都假设变量在欧氏空间 \(\mathbb{R}^n\) 中。但机器人学中许多变量天然在**流形**上：

• 旋转矩阵：\(R \in SO(3)\)（3x3 正交矩阵，\(\det = 1\)），维度 3，但嵌入在 \(\mathbb{R}^{9}\) 中
• 刚体位姿：\(T \in SE(3) = SO(3) \ltimes \mathbb{R}^3\)，维度 6
• 单位四元数：\(q \in S^3\)（\(\|q\| = 1\)），维度 3，但嵌入在 \(\mathbb{R}^4\) 中
• 低秩矩阵：Grassmann 流形 \(\text{Gr}(k, n)\)
• Stiefel 流形：\(\text{St}(k, n) = \{X \in \mathbb{R}^{n \times k} \mid X^TX = I_k\}\)，出现在 SE-Sync 中

流形优化的核心动机**可以用一句话概括：**在正确的空间中做优化，比在错误的空间中加约束更好。

如果在 \(\mathbb{R}^{12}\) 中直接对旋转矩阵做优化（12 个自由变量），会有两个问题： 1. 过参数化：\(SO(3)\) 只有 3 个自由度，12 维空间中的 9 个约束 \(R^TR = I\) 使问题退化（Hessian 奇异） 2. 约束丢失：欧氏梯度步 \(R_{k+1} = R_k + \Delta R\) 不保证 \(R_{k+1} \in SO(3)\)

如果不这样做会怎样？ 假设对旋转矩阵做"朴素"的梯度下降 \(R_{k+1} = R_k - \alpha \nabla_R f\)。一步之后 \(R_{k+1}\) 就不再是正交矩阵了。强行用 SVD 投影回 \(SO(3)\)（\(R \leftarrow UV^T\)）可以修补，但投影步可能干扰优化的下降方向，破坏收敛保证。更糟糕的是，对于 \(SE(3)\)（旋转 + 平移的耦合），投影变得更复杂且不唯一。

流形优化的解决方案：在流形的**切空间**中计算步方向，然后通过 retraction 映射回流形：

\[x_{k+1} = \text{Retr}_{x_k}(-\alpha_k \text{grad} f(x_k))\]

这个框架优雅地解决了上述两个问题：(1) 切空间的维度等于流形的自由度（\(SO(3)\) 的切空间是 3 维的），没有过参数化；(2) retraction 保证结果始终在流形上，不需要投影。

8.2 核心概念与 \(SO(3)\) 实例

流形优化的核心是将欧氏空间中的优化概念"搬到"流形上。下表总结了这种对应关系：

概念	欧氏空间对应	流形上的定义
梯度	\(\nabla f(x)\)	\(\text{grad} f(x)\) = Riemannian 梯度（投影到切空间）
步更新	\(x + \Delta x\)	\(\text{Retr}_x(\Delta x)\)（retraction）
Hessian	\(\nabla^2 f(x)\)	\(\text{Hess} f(x)\) = Riemannian Hessian
线搜索	沿直线 \(x + \alpha d\)	沿 geodesic 或 retraction 曲线
距离	\(\|x - y\|\)	Riemannian 距离 \(d(x, y)\)
平行移动	向量的平移	沿曲线的协变导数积分

\(SO(3)\) 上的完整例子：

\(SO(3)\) 是所有 \(3 \times 3\) 正交矩阵（\(R^TR = I\), \(\det R = 1\)）构成的群。作为流形，它的维度是 3（虽然嵌入在 \(\mathbb{R}^{3 \times 3}\) 的 9 维空间中）。

切空间：\(T_R SO(3) = \{R\hat{\omega} \mid \omega \in \mathbb{R}^3\} \cong \mathfrak{so}(3)\)，其中 \(\hat{\omega}\) 是 \(\omega\) 的反对称矩阵（skew-symmetric matrix）。直觉：切空间中的向量表示"微小旋转"的角速度。

Retraction：\(\text{Retr}_R(\omega) = R \cdot \exp(\hat{\omega})\)。这里 \(\exp(\hat{\omega})\) 由 Rodrigues 公式给出： $\(\exp(\hat{\omega}) = I + \frac{\sin\theta}{\theta}\hat{\omega} + \frac{1-\cos\theta}{\theta^2}\hat{\omega}^2, \quad \theta = \|\omega\|\)$

Riemannian 梯度：若 \(f\) 定义在嵌入空间 \(\mathbb{R}^{3 \times 3}\) 上，其欧氏梯度为 \(\nabla_R f\)，则 Riemannian 梯度是其到切空间的投影： $\(\text{grad} f(R) = R \cdot \text{skew}(R^T \nabla_R f)\)$

其中 \(\text{skew}(A) = \frac{1}{2}(A - A^T)\) 是反对称化。这保证了梯度方向始终在切空间中——沿此方向做 retraction 后得到的仍然是旋转矩阵。

与欧氏情况的对比：

在欧氏空间中，梯度下降是 \(x_{k+1} = x_k - \alpha \nabla f(x_k)\)。在 \(SO(3)\) 上： $\(R_{k+1} = R_k \cdot \exp(-\alpha \widehat{\text{grad} f(R_k)})\)$

注意 \(\exp\) 保证了 \(R_{k+1} \in SO(3)\)——不需要任何投影或归一化。这是流形优化的核心优势：约束自动满足，无需显式处理。

反事实推理：如果不用流形优化而是在 \(\mathbb{R}^9\) 中加约束 \(R^TR = I\) 做约束优化会怎样？6 个约束（\(R^TR = I\) 是 \(3 \times 3\) 对称矩阵方程，有 6 个独立分量），6 个 Lagrange 乘子。KKT 系统是 \(15 \times 15\)（\(9 + 6\)）而非流形方法的 \(3 \times 3\)。更重要的是，约束优化需要追踪约束的活跃状态（SQP/IPM），而流形方法根本没有约束——简洁得多。

8.3 Riemannian 梯度下降与 Newton ⭐⭐⭐

Riemannian 梯度下降：

\[x_{k+1} = \text{Retr}_{x_k}(-\alpha_k \text{grad} f(x_k))\]

其中 \(\alpha_k\) 通过 Armijo 条件确定（在 retraction 曲线上做线搜索）。

收敛性：对 geodesically convex 函数（流形上的凸概念），Riemannian GD 以 \(O(1/k)\) 速率收敛（类比欧氏空间中的凸 GD）。

Riemannian Newton 法：

\[\text{Hess } f(x_k) [\eta_k] = -\text{grad} f(x_k), \quad x_{k+1} = \text{Retr}_{x_k}(\eta_k)\]

其中 \(\text{Hess } f\) 是 Riemannian Hessian（Levi-Civita 连接诱导的二阶导数），\(\eta_k\) 是切空间中的 Newton 步。

局部二次收敛：与欧氏 Newton 一样，若 retraction 是二阶的，且 \(\text{Hess } f(x^*)\) 非退化，则 Riemannian Newton 局部二次收敛。

Manopt/Pymanopt 实现：

import pymanopt
from pymanopt.manifolds import Rotations
from pymanopt.optimizers import TrustRegions

# 定义流形
manifold = Rotations(3)  # SO(3)

# 定义目标函数（旋转平均：找到与所有 R_i 最接近的旋转）
@pymanopt.function.numpy(manifold)
def cost(R):
    return sum(np.linalg.norm(R - R_i, 'fro')**2 for R_i in measurements)

# 定义梯度（Pymanopt 也支持自动微分）
@pymanopt.function.numpy(manifold)
def euclidean_gradient(R):
    return sum(2 * (R - R_i) for R_i in measurements)

# 创建问题并求解
problem = pymanopt.Problem(manifold, cost, euclidean_gradient=euclidean_gradient)
optimizer = TrustRegions()  # Riemannian 信赖域
result = optimizer.run(problem)
print(f"最优旋转:\n{result.point}")

8.5 Riemannian 信赖域（RTR）

定理（Absil-Baker-Gallivan 2007）。 若 retraction 是二阶的，\(f \in C^2\)，使用 truncated CG 子求解器，则 RTR 的聚点满足一阶和二阶最优性条件：\(\text{grad} f = 0\) 且 \(\text{Hess} f \succeq 0\)。

这比欧氏信赖域的结果更强——它保证收敛到**二阶临界点**（排除鞍点）。这个性质对 SLAM 特别有价值——鞍点通常对应"翻转"的位姿估计（旋转 \(180°\) 的镜像解），RTR 能自动避免这类解。

RTR 的信赖域子问题：

在切空间 \(T_{x_k}\mathcal{M}\) 中求解： $\(\min_{\eta \in T_{x_k}\mathcal{M}} \hat{f}(\eta) := f(x_k) + \langle \text{grad} f(x_k), \eta \rangle + \frac{1}{2}\langle \text{Hess} f(x_k)[\eta], \eta \rangle\)$ $\(\text{s.t. } \|\eta\|_{x_k} \le \Delta_k\)$

这个子问题与欧氏信赖域子问题形式完全一致——只是"向量空间"变成了"切空间"，"内积"变成了"Riemannian 度量"。因此 Cauchy/Dogleg/Steihaug-CG（§4.4）的所有方法都直接适用。

8.6 SE-Sync：可证全局最优的 SLAM

SE-Sync（Rosen et al. 2019）是流形优化在机器人学中最成功的应用之一。它解决位姿图优化（Pose Graph Optimization）：

\[\min_{R_i \in SO(d)} \sum_{(i,j) \in \mathcal{E}} \kappa_{ij} \|R_i - \tilde{R}_{ij} R_j\|_F^2\]

三步策略： 1. SDP 松弛：把 \(R_i \in SO(d)\) 松弛为 \(R_i \in \text{St}(d, p)\)（Stiefel 流形，\(p \ge d\)） 2. Riemannian staircase：在 \(\text{St}(d, p)\) 上用 RTR 求解 3. 全局最优性证书：检查 \(\lambda_{\min}(S - \Lambda) \ge 0\)（SDP 的互补松弛条件）

如果证书通过：松弛是紧的，找到的解就是**全局最优**（即使原问题非凸！）。实验表明，在标准 SLAM 数据集上（如 CSAIL、Manhattan、Intel），SE-Sync 几乎总是能证明全局最优性。

SE-Sync 的实际影响：它第一次为 SLAM 后端提供了全局最优性的**数学证明**，而非仅仅"跑了很多初始化都得到同样的解所以大概是全局最优"的经验判断。这对安全关键应用（自动驾驶、手术机器人）的可信度保证具有重要意义。

SE-Sync 的局限：(1) 只处理旋转一致性问题（不直接适用于含路标的 BA）；(2) 松弛可能不紧（噪声太大时），此时证书检查失败但仍可返回一个"好的"局部解；(3) SDP 的规模随位姿数增长，超大地图（\(> 10000\) 节点）可能需要分布式求解或分层方法。

后续工作：Rosen 等人的后续工作（2023）将认证方法扩展到了 SE-Sync++（处理平移）和 STRIDE（更快的分布式求解器），进一步拓宽了可证全局最优 SLAM 的适用范围。

⚠️ 常见陷阱

⚠️ 编程陷阱：\(SE(3)\) 上直接欧氏 GN 导致发散 - 12 维参数化（\([R|t]\) 展平为 12 维向量）会丢失正交性约束 - GN 步后 \(R_{k+1}\) 不再是旋转矩阵 → 数值发散 - 正确做法：使用 Ceres Manifold（或旧 API LocalParameterization）、GTSAM Pose3

💡 概念误区：认为"流形优化只是加了约束的欧氏优化" - 流形优化不是在嵌入空间中加约束（那是约束优化） - 而是**直接在流形上**定义梯度、Hessian、迭代——完全不需要约束 - 这种"内禀"视角让算法更简洁、数值更稳定、没有 Lagrange 乘子

练习

1. （推导） 证明 \(SO(3)\) 上的 retraction \(\text{Retr}_R(\omega) = R \exp(\hat{\omega})\) 满足二阶条件。
2. （编程） 用 Pymanopt 实现旋转平均问题 \(\min_{R \in SO(3)} \sum_i \|R - R_i\|_F^2\)，比较 Riemannian GD 和 Riemannian TR 的收敛速度。
3. （跨章综合） 结合凸优化（SDP 松弛）和流形优化（RTR），解释 SE-Sync 为什么能给非凸 SLAM 提供全局最优性保证。

---

9. 工程实践：求解器选型与调参 ⭐⭐

9.1 SLAM 后端选型

场景	推荐求解器	算法	理由
纯 BA（COLMAP, OpenMVG）	Ceres SPARSE_SCHUR	LM + Schur 补	Schur 补利用路标-位姿结构
VIO/VISLAM（ORB-SLAM3, Kimera）	GTSAM ISAM2	增量 LM	iSAM2 只重算受影响节点
Pose Graph（回环修正）	GTSAM 或 g2o	LM/GN	成熟生态
可证全局最优 PGO	SE-Sync	Riemannian TR + SDP	全局最优性证书

9.2 MPC/轨迹优化选型

场景	推荐方案	频率	备注
研究原型（离线）	CasADi + Ipopt	< 1 Hz	5 行代码写 OCP
实时嵌入式	acados (RTI + HPIPM)	kHz 级	代码生成到 C
线性 MPC	OSQP / HPIPM	kHz 级	直接 QP
DDP/iLQR	Crocoddyl	100 Hz	Pinocchio 解析微分
混合整数 MPC	Gurobi / CPLEX	< 10 Hz	步态切换等离散决策

选型决策流程：

有约束的轨迹优化/MPC？
├── 约束是否线性？
│   ├── 是 → 目标是否二次？
│   │   ├── 是 → QP 求解器（OSQP/HPIPM/qpOASES）
│   │   └── 否 → 线性规划器（HiGHS/Gurobi）
│   └── 否 → 需要 NLP 求解器
│       ├── 实时要求？
│       │   ├── < 1ms → acados RTI（代码生成）
│       │   ├── 1-100ms → CasADi + Ipopt 或 Crocoddyl DDP
│       │   └── 无限制 → CasADi + Ipopt（最通用）
│       └── 有离散变量？
│           └── 是 → MINLP（Bonmin）或松弛+取整

9.3 关键调参指南 ⭐⭐

Ceres Solver 调参检查清单：

参数	默认值	调参建议	错误选择的后果
trust_region_strategy	LM	BA/ICP 用 LM，不确定步长用 DOGLEG	DOGLEG 在大残差时可能不稳定
linear_solver_type	DENSE_QR	小规模: DENSE_QR; BA: SPARSE_SCHUR; 大BA: ITERATIVE_SCHUR	选错求解器：慢 10-1000 倍
initial_trust_region_radius	\(10^4\)	初始化差时调小（\(10^2\)），好时调大（\(10^6\)）	太大：首步发散；太小：保守
max_num_iterations	50	BA: 30-100; 标定: 200-500	太少：未收敛；太多：浪费时间
function_tolerance	\(10^{-6}\)	实时应用可放宽到 \(10^{-4}\)	太严：不必要的额外迭代
num_threads	1	大规模问题设为 CPU 核心数	未并行：速度只有理论值的 1/N

Ipopt 调参检查清单：

参数	默认值	调参建议	说明
mu_strategy	monotone	用 adaptive（自适应降低 barrier）	adaptive 通常更快收敛
linear_solver	ma27	推荐 ma57 或 mumps（开源）	ma57 更适合稀疏问题
hessian_approximation	exact	无精确 Hessian 时用 limited-memory（L-BFGS）	L-BFGS 近似可能降低收敛率
warm_start_init_point	no	连续优化问题设 yes	但对 IPM 效果有限（barrier 路径问题）
print_level	5	调试时设 5-8，生产环境设 0	高 print_level 影响性能

9.4 故障诊断决策树

求解失败
├── 迭代数达到上限但未收敛
│   ├── cost 停滞 + gradient 大 → Jacobian 可能有误（数值验证）
│   ├── cost 停滞 + gradient 小 → 已到局部最优（换初始化）
│   └── cost 振荡 → 步长/信赖域问题（调 trust_region_radius）
├── NaN / Inf 出现
│   ├── 首步就 NaN → 初始点不可行 / 参数化错误
│   └── 中途 NaN → 数值溢出（检查 Jacobian 条件数）
└── 收敛但结果错误
    ├── 局部最优 → 换初始化（多次随机启动）
    ├── 约束违反 → 检查约束实现 / 放大惩罚
    └── 数值精度 → 检查 float32 vs float64

---

10. 稀疏性利用与机器人优化 ⭐⭐

10.1 动机：大规模优化的核心瓶颈

非线性优化的每一步都需要求解一个线性系统（Newton 系统、QP 的 KKT 系统、或正规方程）。对于机器人学中的大规模问题——BA 有百万级变量、SLAM 有千级位姿节点、MPC 有数百个时间步——线性系统求解是瓶颈。

关键观察：这些问题的 Hessian/KKT 矩阵几乎总是**稀疏**的。稀疏性来自物理世界的局部耦合： - BA 中，每个路标只被少数相机观测 → \(J^TJ\) 中路标-路标块为对角 - SLAM 中，每个位姿只与相邻位姿和局部路标耦合 → 稀疏图结构 - MPC 中，动力学约束只连接相邻时间步 → 带状结构

本质洞察：利用稀疏性不是一个"实现优化"——它是大规模优化从**不可行**变为**可行**的关键。对 \(n = 30000\) 的 BA 问题：稠密求解 \(O(n^3) \approx 2.7 \times 10^{13}\)（小时级）；稀疏求解 \(O(n \cdot \text{bandwidth}^2) \approx 10^7\)（毫秒级）。五个数量级的差距！

10.2 BA 的稀疏结构 ⭐⭐

回顾 §3.9 的 Schur 补。BA 的 Hessian \(H = J^TJ\) 的结构可以更详细地分析。

因素图视角：BA 可以表示为因子图（factor graph），其中： - 变量节点：相机位姿 \(T_i \in SE(3)\)（6 个参数）+ 路标 \(p_j \in \mathbb{R}^3\)（3 个参数） - 因子节点：每个观测 \(z_{ij}\)（相机 \(i\) 观测路标 \(j\)）

Hessian 的稀疏模式：\(H_{ab} \ne 0\) 当且仅当变量 \(a\) 和 \(b\) 被同一个因子连接。路标之间不共享因子 → 路标-路标块对角。这个"箭头"结构使 Schur 补极其高效。

Schur 补消元的物理意义：

消去路标变量等价于"在已知路标的条件下，边缘化相机之间的信息"。消元后的 reduced camera system 包含了所有路标传递给相机的间接耦合信息。

自动稀疏性检测：

现代优化库（Ceres, CasADi）提供自动稀疏性检测： - Ceres：根据残差块（ResidualBlock）的连接关系自动推断 \(J\) 的稀疏模式 - CasADi：通过符号 AD（自动微分）的图传播自动检测 Jacobian/Hessian 的稀疏模式

# CasADi 自动稀疏性检测示例
import casadi as ca

x = ca.SX.sym('x', 100)
f = ca.sum1(x[:-1]**2 + x[1:]**2)  # 只有相邻变量耦合
H = ca.hessian(f, x)[0]

print(f"Hessian 非零元素: {H.nnz()}")  # 约 200（稀疏）
print(f"Hessian 总元素: {100*100}")     # 10000（稠密的话）
print(f"稀疏度: {H.nnz() / 10000:.1%}") # 约 2%

10.3 MPC 的带状结构 ⭐⭐

直接多重打靶（direct multiple shooting）形式的 NMPC：

\[\min \sum_{k=0}^{N-1} \ell(x_k, u_k) + \ell_N(x_N) \quad \text{s.t. } x_{k+1} = F(x_k, u_k)\]

KKT 矩阵的结构：决策变量为 \((x_0, u_0, x_1, u_1, ..., x_N)\)。由于动力学约束只连接 \((x_k, u_k, x_{k+1})\)，KKT 矩阵是**块三对角**的：

\[\begin{bmatrix} Q_0 & S_0^T & A_0^T & & & \\ S_0 & R_0 & B_0^T & & & \\ A_0 & B_0 & & & & \\ & & Q_1 & S_1^T & A_1^T & \\ & & S_1 & R_1 & B_1^T & \\ & & A_1 & B_1 & & \ddots \end{bmatrix}\]

求解方法——Riccati 递推：

这个块三对角系统可以用 Riccati 递推在 \(O(N \cdot n_x^3)\) 时间内求解（\(N\) 是时域步数，\(n_x\) 是状态维度），而不是稠密求解的 \(O((N \cdot n_x)^3)\)。这就是 HPIPM（acados 的 QP 求解器后端）的核心算法。

跨领域类比：MPC 的 Riccati 递推与 Kalman 滤波的 Riccati 方程是**对偶**关系。Kalman 从前向后传播不确定性，MPC 的 Riccati 从后向前传播最优性。两者利用的都是相同的带状稀疏结构。

10.4 SLAM 的消元顺序与图结构 ⭐⭐⭐

SLAM 后端（Pose Graph Optimization 或 Landmark SLAM）的 Hessian 是一个**稀疏对称正定矩阵**。对稀疏矩阵做 Cholesky 分解的效率高度依赖于**消元顺序**（elimination ordering）。

填入（fill-in）的概念：直接对稀疏矩阵做 Cholesky，分解因子 \(L\) 可能有比原矩阵更多的非零元素——这些额外的非零元素叫"fill-in"。好的消元顺序最小化 fill-in。

常用消元顺序策略：

策略	原理	适用场景
AMD（近似最小度）	每步消去剩余非零元素最少的行/列	通用稀疏矩阵
COLAMD	AMD 的列版本，适合非对称 \(J\)	最小二乘问题的 \(J\)
Nested Dissection	递归将图分割为两半，先消内部再消分隔节点	大规模网格/位姿图
SLAM 专用顺序	先消路标后消位姿（Schur 补）	BA/Landmark SLAM

GTSAM 的 Bayes tree：GTSAM 使用 Bayes tree 数据结构进行增量优化（iSAM2）。核心思想：将消元顺序编码为树结构，新因子加入时只需局部更新树的受影响分支——\(O(\log n)\) 到 \(O(\sqrt{n})\) 的增量更新代价，远优于完整重新分解的 \(O(n^{1.5})\)。

⚠️ 常见陷阱

⚠️ 编程陷阱：忽略消元顺序导致 fill-in 爆炸 - 对 \(n = 10000\) 的稀疏 SLAM 问题，错误的消元顺序可能导致 fill-in 使因子 \(L\) 几乎变稠密 - Ceres 默认使用 AMD 排序（options.sparse_linear_algebra_library_type = SUITE_SPARSE），通常足够好 - 但对于特殊结构（如长走廊环境的位姿图），手动指定消元顺序（先消路标）可能快 10 倍以上

💡 概念误区：认为"稀疏 = 小" - 稀疏矩阵可以非常大（\(n = 10^6\)），但非零元素只有 \(O(n)\) 个 - 利用稀疏性的关键不是矩阵小，而是**非零元素的模式有结构** - 随机稀疏矩阵（非零位置随机）很难高效求解；结构稀疏矩阵（来自物理问题）几乎总能高效求解

练习

1. （编程） 用 scipy.sparse 构造一个 1000x1000 的随机稀疏矩阵和一个带状稀疏矩阵（带宽 10），对比 Cholesky 分解时间。
2. （概念） 解释为什么 BA 的 Schur 补先消路标后消相机（而不是反过来）。如果反过来会怎样？
3. （跨章综合） 回顾凸优化中的内点法。IPM 的 Newton 步也涉及稀疏 KKT 系统求解——结合本节的稀疏分析，解释为什么 Ipopt + MUMPS 能高效求解大规模 NLP。

---

11. 典型例题：Rosenbrock 函数与 SLAM 后端 ⭐⭐

11.1 Rosenbrock 函数——优化算法的"试金石"

Rosenbrock 函数：

\[f(x_1, x_2) = (1 - x_1)^2 + 100(x_2 - x_1^2)^2\]

全局最优 \(x^* = (1, 1)\)，\(f^* = 0\)。

为什么 Rosenbrock 是经典测试函数？ 它有一个弯曲的窄谷（"香蕉谷"），从初始点 \((-1, 1)\) 出发： - 快速下降到谷底（\(x_1 \approx -1, x_2 \approx 1\)） - 然后沿着窄谷缓慢爬向 \((1, 1)\)

窄谷使 Hessian 条件数极大（\(\kappa \approx 2500\) 在谷底附近），一阶方法在此表现极差。

各算法的表现对比：

算法	到 \(\|g\| < 10^{-6}\) 的迭代次数	每步代价	总时间特性
梯度下降	\(> 10000\)	\(O(n)\)	极慢（锯齿路径）
共轭梯度	\(\sim 200\)	\(O(n)\)	中等
BFGS	\(\sim 30\)	\(O(n^2)\)	快
L-BFGS(\(m=10\))	\(\sim 35\)	\(O(mn)\)	快
Newton（精确 Hessian）	\(\sim 6\)	\(O(n^3)\)	最快（\(n=2\)）

Rosenbrock 函数的梯度和 Hessian：

\[\nabla f = \begin{bmatrix} -2(1-x_1) - 400x_1(x_2 - x_1^2) \\ 200(x_2 - x_1^2) \end{bmatrix}\]

\[\nabla^2 f = \begin{bmatrix} 2 + 1200x_1^2 - 400x_2 & -400x_1 \\ -400x_1 & 200 \end{bmatrix}\]

在最优点 \((1,1)\)：\(\nabla^2 f^* = \begin{bmatrix} 802 & -400 \\ -400 & 200 \end{bmatrix}\)，条件数 \(\kappa \approx 2508\)。

为什么梯度下降表现差？ 在窄谷中，梯度方向近似垂直于谷的走向。每步沿梯度走，跨过谷到对面山壁，然后被弹回——形成"锯齿"路径。条件数 \(\kappa \approx 2500\) 意味着 GD 需要 \(O(\kappa) \approx 2500\) 步才能将误差减半。

为什么 Newton 法表现好？ Newton 方向 \(p = -H^{-1}g\) 利用了曲率信息，自动"拉直"窄谷方向。在谷中，Newton 步沿谷的方向前进（而不是垂直于谷），因此只需 \(O(1)\) 步穿越窄谷。

反事实推理：如果 Rosenbrock 函数是二次函数（没有弯曲的谷），所有方法的表现会怎样？二次函数上：GD \(O(\kappa)\) 步，CG 精确 \(n\) 步，Newton 1 步。Rosenbrock 的额外困难在于非二次性——这使得 Newton 从 1 步增加到 6 步（仍然非常快），但 GD 从 \(O(\kappa)\) 增加到 \(10000+\) 步。

11.2 SLAM 后端优化的完整建模 ⭐⭐

问题设定：2D Pose Graph Optimization

给定： - 里程计约束 \((i, i+1, \Delta z_{i,i+1})\)：相邻位姿的相对测量 - 回环约束 \((i, j, \Delta z_{ij})\)：非相邻位姿的识别 - 每个约束带有信息矩阵 \(\Omega_{ij} \in \mathbb{R}^{3 \times 3}\)（测量不确定性的逆）

变量：\(x = (x_1, y_1, \theta_1, ..., x_N, y_N, \theta_N) \in \mathbb{R}^{3N}\)

目标函数（非线性最小二乘）：

\[\min_x \sum_{(i,j) \in \mathcal{E}} \|e_{ij}(x_i, x_j)\|_{\Omega_{ij}}^2\]

其中残差 \(e_{ij} = h(x_i, x_j) - \Delta z_{ij}\)，\(h\) 是相对位姿计算函数：

\[h(x_i, x_j) = \begin{bmatrix} \cos\theta_i & \sin\theta_i & 0 \\ -\sin\theta_i & \cos\theta_i & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_j - x_i \\ y_j - y_i \\ \theta_j - \theta_i \end{bmatrix}\]

为什么是非凸的？ 旋转角 \(\theta\) 的三角函数 \(\cos\theta, \sin\theta\) 使残差对 \(\theta\) 非线性。当回环约束跨越大角度差时，\(\cos(\theta_j - \theta_i)\) 可能有多个局部极小。

Jacobian 的稀疏结构：

每个残差 \(e_{ij}\) 只涉及两个位姿 \(x_i, x_j\)。因此 Jacobian \(J\) 的每行最多有 6 个非零元素（3 来自 \(x_i\)，3 来自 \(x_j\)）。对于 \(N\) 个位姿和 \(M\) 条边，\(J\) 的大小为 \(3M \times 3N\)，但只有 \(6M\) 个非零元素。

Hessian \(H = J^T\Omega J\) 的稀疏模式：\(H_{ii'} \ne 0\) 当且仅当位姿 \(i\) 和 \(i'\) 被某条边直接连接。这就是位姿图的邻接矩阵对应的稀疏模式。

Gauss-Newton 求解步骤：

1. 线性化：在当前 \(x_k\) 计算 \(r = [e_{ij}^T]^T\)，\(J = [\frac{\partial e_{ij}}{\partial x}]\)
2. 组装正规方程：\(H = J^T\Omega J\)，\(b = -J^T\Omega r\)
3. 求解：\(H \Delta x = b\)（用稀疏 Cholesky，AMD 排序）
4. 更新：\(x_{k+1} = x_k \oplus \Delta x\)（\(\oplus\) 是流形加法：平移直接加，角度取模 \(2\pi\)）
5. 检查收敛：\(\|\Delta x\| < \epsilon\) 或 \(\|b\| < \epsilon\)

GTSAM 实现要点：

import gtsam
import numpy as np

# 构建因子图
graph = gtsam.NonlinearFactorGraph()

# 添加先验因子（固定首帧，消除 gauge freedom）
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.005]))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(0, 0, 0), prior_noise))

# 添加里程计因子
odom_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
for i in range(N-1):
    graph.add(gtsam.BetweenFactorPose2(i, i+1, odom_measurements[i], odom_noise))

# 添加回环因子
loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.02]))
for (i, j, measurement) in loop_closures:
    graph.add(gtsam.BetweenFactorPose2(i, j, measurement, loop_noise))

# 初始值（里程计累积，可能有大漂移）
initial = gtsam.Values()
for i in range(N):
    initial.insert(i, initial_poses[i])

# 优化（Levenberg-Marquardt）
params = gtsam.LevenbergMarquardtParams()
params.setMaxIterations(100)
params.setRelativeErrorTol(1e-8)

optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()

# 检查收敛
print(f"初始误差: {graph.error(initial):.4f}")
print(f"最终误差: {graph.error(result):.4f}")
print(f"迭代次数: {optimizer.iterations()}")

典型结果：对 1000 个位姿节点 + 50 个回环约束，GN 通常 5-15 步收敛，LM 通常 10-25 步收敛（更鲁棒但略慢）。

11.3 从 Rosenbrock 到 SLAM：一般性教训

比较两个例子揭示了非线性优化在工程中的一般规律：

维度	Rosenbrock	SLAM PGO
变量规模	\(n = 2\)	\(n = 3N\)（千到万级）
结构利用	无（稠密 Hessian）	稀疏 Hessian（图结构）
条件数	\(\kappa \approx 2500\)（固定）	\(\kappa \approx 10^3 \sim 10^6\)（依赖数据）
最佳算法	Newton/BFGS	GN/LM + 稀疏 Cholesky
初始化敏感性	中等	高（回环跨越大角度时）
全局最优保证	有（唯一极小值）	无（但 SE-Sync 可证）

本质洞察：非线性优化在工程中的成功不仅依赖于算法的理论性质（收敛率、全局性），更依赖于**算法与问题结构的匹配**。Rosenbrock 虽然只有 2 个变量却很难（高条件数 + 非二次性），SLAM 虽然有几万个变量却相对容易（好的稀疏结构 + 好的初始化 + 残差通常较小）。选对求解器比改进算法更重要。

⚠️ 常见陷阱

🧠 思维陷阱：用 Rosenbrock 的经验判断 SLAM 算法性能 - Rosenbrock 是稠密、高条件数、无约束的——SLAM 是稀疏、中等条件数、流形约束的 - 在 Rosenbrock 上 L-BFGS 略优于 BFGS，但在 SLAM 上 GN/LM 远优于两者 - 原因：GN/LM 利用了最小二乘的特殊结构（\(J^TJ\) 近似 Hessian），BFGS 没有利用

⚠️ 编程陷阱：SLAM 未固定 gauge freedom 导致不收敛 - Pose Graph 有 3 个自由度的 gauge（平移 2 + 旋转 1 in 2D） - 不固定首帧 → \(H\) 奇异 → GN 步无穷大 → 发散 - 正确做法：graph.add(PriorFactorPose2(0, initial_pose, tight_noise))

练习

1. （编程） 实现梯度下降、BFGS、Newton 三种方法在 Rosenbrock 函数上的对比。绘制迭代路径、收敛曲线（\(\|g_k\|\) vs \(k\)），验证收敛阶（线性/超线性/二次）。
2. （推导） 计算 Rosenbrock 在 \((1,1)\) 的 Hessian 条件数，解释为什么 GD 需要 \(O(2500)\) 步。
3. （跨章综合） 构造一个 2D PGO 问题（10 个位姿，方形轨迹，1 个回环约束），手动写出 Jacobian \(J\) 的稀疏模式，并用 Python 的 scipy.sparse 验证 Schur 补消元的加速效果。

---

本章小结

方法	核心思想	收敛率	每步代价	典型用途
Newton	精确二次模型	Q-二次	\(O(n^3)\)	小规模通用
Gauss-Newton	线性化残差的 LS	零残差二次	\(O(n^2m)\)（QR）或 \(O(n^3)\)（正规方程）	BA, ICP, 标定
LM	GN + 信赖域	趋 GN	同 GN + \(\lambda\) 更新	SLAM 工业标准
BFGS	秩-2 Hessian 近似	Q-超线性	\(O(n^2)\)	中等规模无约束
L-BFGS	有限内存 BFGS	线性~超线性	\(O(mn)\)	大规模无约束
SR1 + TR	秩-1 近似 + 信赖域	Q-超线性	\(O(n^2)\)	非凸无约束
SQP	QP 子问题	超线性/二次	QP 求解代价	轨迹优化, MPC
Ipopt (IPM-NLP)	barrier + Newton	多项式	稀疏 KKT	通用 NLP
RTR (Riemannian)	流形上信赖域	二阶临界点	retraction + CG	PGO, 旋转平均

收敛阶速查（以误差从 \(10^{-3}\) 收敛到 \(10^{-12}\) 为例）：

收敛阶	含义	达到 \(10^{-12}\) 所需步数	典型方法
Q-线性（\(\rho=0.5\)）	每步误差减半	\(\sim 30\) 步	GD（好条件数）
Q-线性（\(\rho=0.99\)）	每步误差减 1%	\(\sim 2000\) 步	GD（坏条件数）
Q-超线性	压缩比趋于零	\(\sim 15\) 步	BFGS
Q-二次	精度位数翻倍	\(\sim 5\) 步	Newton/GN

---

累积项目：本章新增模块

项目：Mini-SLAM 后端优化器

本章在前章的基础上新增以下模块：

模块	文件名	功能	对应章节
GN 求解器	gauss_newton.py	基础 Gauss-Newton 迭代	§3.2
LM 求解器	levenberg_marquardt.py	LM + Nielsen \(\lambda\) 更新策略	§3.4
位姿图优化	pose_graph_2d.py	2D PGO 完整实现	§11.2
Schur 补	schur_complement.py	Schur 补消元 + BA 结构利用	§3.9
线搜索	line_search.py	Armijo 回溯 + 强 Wolfe	§4.2
L-BFGS	lbfgs.py	两循环递推 + 初始 \(\gamma_k\) 缩放	§5.6

依赖关系： - 依赖前章的 lie_group.py（\(SE(2)\) 的 exp/log） - 依赖前章的 sparse_utils.py（稀疏矩阵操作）

本章新增的测试： - test_rosenbrock.py：在 Rosenbrock 函数上对比 GD/BFGS/Newton/L-BFGS - test_pgo_square.py：方形轨迹 PGO 的 GN/LM 收敛测试 - test_convergence_order.py：验证各算法的收敛阶（线性/超线性/二次）

下一章（约束优化工程实践）的模块预告： - 将新增 CasADi 接口包装器、Ipopt 调参模板、acados RTI 示例

---

延伸阅读

资源	难度	主题	推荐章节
Nocedal & Wright《Numerical Optimization》2nd ed. 2006	⭐⭐	绝对主力教材，本章的理论基础	Ch3(线搜索) Ch4(信赖域) Ch6(拟Newton) Ch10(NLS) Ch18(SQP)
Conn-Gould-Toint《Trust-Region Methods》2000	⭐⭐⭐	信赖域方法的权威参考	Ch6(全局收敛) Ch7(局部收敛)
Boumal《An Introduction to Optimization on Smooth Manifolds》2023	⭐⭐⭐	流形优化的现代入门教材	Ch3(Riemannian GD) Ch7(信赖域)
Triggs et al. "Bundle Adjustment—A Modern Synthesis" 2000	⭐⭐	BA 的经典综述（必读）	全文
Rosen et al. "SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group" 2019	⭐⭐⭐⭐	可证全局最优的 SLAM	§3-4(松弛与求解)
Wachter & Biegler "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" 2006	⭐⭐⭐	Ipopt 的原始论文	§2(算法) §3(filter)
Dennis & More "Quasi-Newton Methods, Motivation and Theory" 1977	⭐⭐⭐	拟 Newton 方法的理论基石	§8(超线性条件)
Ceres Solver 文档 (ceres-solver.org)	⭐⭐	工程实现参考	Modeling, Solving
GTSAM Tutorial (gtsam.org)	⭐⭐	因子图优化框架	Factor Graphs
Rawlings, Mayne, Diehl《Model Predictive Control》2nd ed. 2017	⭐⭐⭐	MPC 理论与 SQP/RTI	Ch8(非线性MPC)
Absil, Mahony, Sepulchre《Optimization Algorithms on Matrix Manifolds》2008	⭐⭐⭐⭐	流形优化的经典参考	Ch4(线搜索) Ch7(信赖域)
Dellaert & Kaess "Factor Graphs for Robot Perception" 2017	⭐⭐	因子图与 SLAM 优化综述	全文

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关章节
GN/LM 不收敛，cost 停滞	Jacobian 错误	1. 用有限差分验证 \(J\) 2. 检查 Manifold/参数化 3. 打印残差分量	§3
LM \(\lambda\) 持续增大（退化为 SD）	初值太差/模型错误	1. 改善初始化 2. 检查 cost function 定义 3. 尝试 Huber 核	§3.4
BFGS 方向非下降	曲率条件失败（非凸）	1. 使用 damped BFGS 2. 切换到 L-BFGS（跳过坏更新）3. 改用信赖域	§5
Ipopt 报 "Restoration failed"	约束不一致/初始点太差	1. 检查约束可行性 2. 放松 bound_relax_factor 3. 提供更好初始点	§7
acados RTI 闭环不稳定	QP 子问题精度不够	1. 增加 RTI 步数 2. 检查 warm-start 3. 验证模型线性化精度	§6.6
BA/SLAM 结果"扭曲"	错误回环未被鲁棒核抑制	1. 可视化残差分布 2. 增大 Huber/Cauchy 核的阈值参数 3. 检查特征匹配质量	§3.6
稀疏 Cholesky 极慢	消元顺序导致 fill-in 爆炸	1. 打印 fill-in 数量 2. 尝试 AMD/COLAMD 重排序 3. 检查 Schur 补是否正确利用	§10
Newton 步后旋转矩阵不合法	未使用流形参数化	1. 检查是否用了 Ceres Manifold 2. 确认 retraction（exp map）正确 3. 不要直接加法更新 \(R + \Delta R\)	§8

深入排查指南——"收敛到错误解"：

优化器"收敛了"但结果明显不对
├── 检查：是否陷入了局部最优？
│   ├── 方法 1：从多个随机初始点运行，比较最终 cost
│   ├── 方法 2：检查 KKT 残差——如果很小但 cost 不是期望值，确实是局部最优
│   └── 方法 3：对 SLAM/BA，用 SE-Sync 检查全局最优性
├── 检查：约束是否被正确施加？
│   ├── 打印最终点的约束违反量
│   ├── 特别注意角度归一化（-π, π] vs [0, 2π)）
│   └── 检查 Ipopt 的 "Number of equality/inequality constraints"
└── 检查：目标函数是否正确？
    ├── 手动计算几个残差值，与优化器报告对比
    ├── 检查信息矩阵 Ω 是否正确（协方差的逆，不是协方差本身！）
    └── 检查 Jacobian 与有限差分的一致性

---