Newton、Gauss-Newton 与 LM

让你的手臂末端到达一个位置需要解逆运动学；让相机和激光雷达对齐需要外参标定；让几万个路标和几千个位姿一致需要光束法平差 (bundle adjustment) 与位姿图优化。这些问题的代价函数都长一个样：很多项"预测减测量"的平方和，即非线性最小二乘。

用 6.3 节的梯度下降解它们，往往要几千步，而且步长要小心地调。用本节的 Gauss-Newton / LM，常常 5 到 20 步就到了小数点后十位。这就是为什么 Ceres、g2o、GTSAM 这些机器人领域的标准优化库，默认的求解器都是 LM：用一阶导数的代价，换来近似二阶的收敛速度。

Newton：拟合抛物面。在当前点 \(\x_k\)，用函数值、梯度和 Hessian 拟合一个二次函数（泰勒展开到二阶），然后直接跳到这个二次函数的最低点。如果函数本来就是二次的，一步到位。

Gauss-Newton：拉直残差。最小二乘的代价是 \(\tfrac12\|\mathbf r(\x)\|^2\)，里面的 \(\mathbf r\) 是弯的。不去拟合整个代价，而是把残差在 \(\x_k\) 处线性化 \(\mathbf r(\x_k+\mathbf p)\approx\mathbf r+J\mathbf p\)，这时代价变成一个关于 \(\mathbf p\) 的普通线性最小二乘（1.13 节），一步解出。它省掉了二阶导数。

Levenberg–Marquardt：给 Gauss-Newton 拴一根绳。线性化只在附近可信（6.5 节），所以对步长加一个惩罚 \(\lambda\|\mathbf p\|^2\)：\(\lambda\) 小，就是 Gauss-Newton；\(\lambda\) 大，就退化成一小步梯度下降。算法根据"实际下降/预测下降"自动调节 \(\lambda\)，就是 6.5 节信赖域的另一种写法。

在二维里，二阶模型 \(m(\mathbf p)=f+\mathbf g^\top\mathbf p+\tfrac12\mathbf p^\top H\mathbf p\) 的等高线是以 Newton 点 \(\x_k-H^{-1}\mathbf g\) 为中心的椭圆（\(H\) 正定时）。Newton 步指向椭圆中心；梯度下降的方向 \(-\mathbf g\) 垂直于等高线，对扁椭圆严重偏离中心（6.3 节的"之"字）。\(H^{-1}\) 恰好把椭圆"压圆"，这就是 Newton 法对条件数不敏感的原因。

拖动起点，看紫色模型等高线与真实等高线（青色）；"二次函数"上两者完全重合；"Rosenbrock"里重合只在一小块区域；"双谷"里 \(H\) 会变成不定，模型的"中心"是鞍点，Newton 步反而爬坡。

最小二乘 \(f=\tfrac12\|\mathbf r(\x)\|^2\) 的梯度和 Hessian 有特殊结构（推导见"再看数学"）：

Levenberg–Marquardt 的一步是解 \((J^\top J+\lambda I)\mathbf p=-J^\top\mathbf r\)。\(\lambda\to0\) 时就是 Gauss-Newton；\(\lambda\to\infty\) 时 \(\mathbf p\approx-J^\top\mathbf r/\lambda\)，是沿负梯度的一小步。扫过所有 \(\lambda\)，\(\mathbf p(\lambda)\) 画出一条从 Gauss-Newton 点到"原地不动"的曲线，路上每一点都是某个信赖域半径下的最优步（6.5 节）。下面在 Rosenbrock（作为最小二乘，\(\mathbf r=[1-x,\ 10(y-x^2)]\)）上拖动起点和 \(\lambda\)。

性质。(1) 在二次函数上一步到位。(2) 二次收敛：若 \(H(\x^\star)\succ0\)，\(\nabla^2f\) 是 Lipschitz 的，且 \(\x_0\) 足够近，则 \(\|\x_{k+1}-\x^\star\|\le C\|\x_k-\x^\star\|^2\)，正确的位数每步大约翻倍（FIG 1）。(3) 仿射不变：把变量做可逆线性变换，Newton 迭代不变，所以条件数无关。(4) 代价：每步要算（\(n\times n\) 的）Hessian 并解线性方程组，\(O(n^3)\)；\(H\) 不正定时方向不一定是下降方向；离解远时可能发散。常见补救：线搜索或信赖域（6.5 节）、给 \(H\) 加 \(\mu I\) 使其正定、拟牛顿法（BFGS）用梯度差近似 \(H\)。

设残差 \(\mathbf r:\R^n\to\R^m\)，雅可比 \(J=\partial\mathbf r/\partial\x\in\R^{m\times n}\)（3.3 节），\(f=\tfrac12\mathbf r^\top\mathbf r=\tfrac12\sum_ir_i^2\)。逐项求导：\(\partial_jf=\sum_ir_i\,\partial_jr_i\)，即 \(\nabla f=J^\top\mathbf r\)；再求一次：\(\partial_{jk}f=\sum_i\big(\partial_jr_i\,\partial_kr_i+r_i\,\partial_{jk}r_i\big)\)，即

右边的等价说明：Gauss-Newton 一步就是对线性化的残差做线性最小二乘（1.13 节），数值上可以用 QR 或 SVD 解，避免把条件数平方。它的性质：(1) 只要 \(J\) 列满秩，\(J^\top J\succ0\)，\(\mathbf p\) 一定是下降方向（\(\mathbf g^\top\mathbf p=-\|J\mathbf p\|^2<0\)）。(2) 零残差问题（\(\mathbf r(\x^\star)=0\)）上，被丢掉的项在解处恰为零，GN 与 Newton 同样二次收敛；残差小时接近二次，残差大或曲率大时，只有线性收敛，甚至不收敛（FIG 3 滑块）。(3) \(J\) 接近秩亏（例如机械臂的奇异位形，FIG 5）时，\(J^\top J\) 近乎奇异，\(\mathbf p\) 巨大而不可靠。

分母是模型预测的下降量（由 \((J^\top J+\lambda I)\mathbf p=-\mathbf g\) 可得 \(m(0)-m(\mathbf p)=\tfrac12\mathbf p^\top(\lambda\mathbf p-\mathbf g)\)），所以 \(\rho\) 正是 6.5 节的"实际/预测"。上面的更新规则来自 Madsen–Nielsen：\(\rho\) 接近 1 时 \(\lambda\) 缩小到原来的 \(1/3\)，\(\rho=\tfrac12\) 时不变，\(\rho\) 接近 0（勉强接受）时几乎翻倍；失败时乘以 \(\nu\)（初值 2），连续失败时 \(\nu\) 也翻倍。这个 \(\lambda\) 与信赖域半径一一对应：\(\lambda\) 越大，\(\|\mathbf p\|\) 越小。Marquardt 的原版把 \(\lambda I\) 换成 \(\lambda\,\mathrm{diag}(J^\top J)\)，使阻尼随各参数的尺度缩放，参数单位悬殊时更稳健（6.3 节误区 2）。

三者关系。Newton：\(H=J^\top J+\sum r_i\nabla^2r_i\)，无阻尼；GN：丢掉第二项；LM：GN + \(\lambda I\)。再往后，6.9 节会把同样的 LM 用在由几千个残差块组成的因子图上，只是 \(J\) 变成稀疏矩阵。

两连杆臂（连杆长 1.0、0.8）的 IK 就是最小二乘：残差 \(\mathbf r(\mathbf q)=\mathrm{FK}(\mathbf q)-\mathbf t\)，雅可比 \(J\) 就是机器人雅可比（3.3 节）。Gauss-Newton 的一步 \(\mathbf p=-J^{-1}\mathbf r\)（这里 \(J\) 是方阵）正是经典的"雅可比逆"IK。但当手臂几乎伸直（\(q_2\approx0\)）时，\(J\) 近乎奇异：手端没法沿手臂方向瞬时移动，要求它移动，\(J^{-1}\) 就给出巨大的关节转角。

下面让手臂从几乎伸直的姿态出发去够一个更近的目标。GN（无阻尼）的第一步就是十几弧度的疯狂转动，之后还要扑腾很多步；LM 因为 \(\lambda\) 限制了步长，平稳地收敛；GD + 回溯也安全，但更慢。拖动目标和关节空间里的起点自己试试。