第四章 标定原理与数学模型：标定问题的数学建模、最小二乘法、非线性优化
各位同学，欢迎来到标定原理的核心章节。说实话，很多做激光雷达的朋友，一听到「数学建模」、「非线性优化」这几个词就开始头疼。我当年刚入行时也一样，觉得这东西太抽象了。但后来在项目里被坑过几次，才明白——不懂数学原理，你连标定结果对不对都判断不了。
这一章，我们就来把标定问题「翻译」成数学语言，然后看看怎么用最小二乘法和优化算法把它解出来。
4.1 标定问题的数学建模
先问一个问题：标定到底在干什么？
说白了，就是找一组参数，让传感器观测到的数据和真实世界尽可能一致。
以激光雷达-相机联合标定为例。我们有一个3D点 P_w（世界坐标系下），它经过激光雷达坐标系、相机坐标系，最后投影到图像平面上。整个过程可以写成：
P_cam = R * P_lidar + t
p_img = K * P_cam其中 R 和 t 就是我们要标定的外参（旋转和平移），K 是相机内参（通常已知）。
那么问题来了：我们怎么知道这组 R 和 t 好不好？
答案很简单——看误差。
假设我们有一个标定板，板上有个角点。激光雷达扫到这个角点，得到一个3D坐标；相机也拍到同一个角点，得到一个2D像素坐标。理论上，用 R 和 t 把激光点投影到图像上，应该和相机检测到的像素位置重合。
如果不重合，就有了误差。这个误差就是我们的优化目标。
核心思想：标定问题 = 寻找一组参数，使得所有观测点的投影误差之和最小。
用数学公式表达就是：
minimize ∑ || p_img_i - project(P_lidar_i, R, t) ||²这就是一个典型的最小二乘问题。
4.2 最小二乘法：最朴素的求解思路
最小二乘法，名字听着高大上，其实核心就一句话：让误差的平方和最小。
为什么是平方？不是绝对值？不是四次方？
嗯，这里有个小故事。我刚开始做标定时，也想过用绝对值误差。结果发现优化出来的参数总是「飘」的，尤其是遇到几个噪声点的时候。后来才明白——平方误差对大的偏差惩罚更重，这会让优化结果更稳定。
对于线性最小二乘问题，比如我们只标定平移（旋转已知），可以直接用公式求解：
t = (A^T * A)^{-1} * A^T * b但现实中的标定问题，几乎都是非线性的。旋转矩阵 R 本身就有约束（必须是正交矩阵），再加上投影过程涉及三角函数，整个问题就变成了非线性最小二乘。
我的经验：线性最小二乘只适合做初值估计。真正要拿到高精度标定结果，必须上非线性优化。我曾经在一个项目中直接用线性方法算外参，结果重投影误差有5个像素，后来用非线性优化压到了0.3个像素。
4.3 非线性优化：高斯-牛顿法
非线性优化，说白了就是迭代地寻找最优解。我们从一个初始猜测开始，每一步都往「下降」的方向走一小步，直到走不动了（误差不再下降）。
高斯-牛顿法是最常用的方法之一。它的核心思想是：把非线性函数在当前点做一阶泰勒展开，然后求解线性最小二乘问题。
具体来说，假设我们的误差函数是 e(x)，其中 x 是待优化参数（比如 R 和 t）。在当前点 x_k 处，我们把 e(x) 近似为：
e(x_k + Δx) ≈ e(x_k) + J * Δx其中 J 是雅可比矩阵（说白了就是误差对参数的导数）。然后我们求解：
minimize || e(x_k) + J * Δx ||²这个问题的解是：
Δx = -(J^T * J)^{-1} * J^T * e(x_k)然后更新参数：x_{k+1} = x_k + Δx。重复直到收敛。
注意：高斯-牛顿法要求 J^T * J 可逆。如果这个矩阵接近奇异（比如参数之间有强相关性），算法就会发散。我曾经在标定一个6自由度的外参时遇到过这个问题，后来加了正则化才解决。
4.4 LM算法：高斯-牛顿的「安全版」
LM算法（Levenberg-Marquardt）可以理解为高斯-牛顿法的「增强版」。它引入了一个阻尼因子 λ，让算法在高斯-牛顿和梯度下降之间做平衡。
LM的更新公式是：
Δx = -(J^T * J + λ * I)^{-1} * J^T * e(x_k)当 λ 很大时，算法接近梯度下降（慢但稳定）；当 λ 很小时，算法接近高斯-牛顿（快但可能发散）。
LM算法会自动调整 λ：如果这一步让误差下降了，就减小 λ（更激进）；如果误差反而上升了，就增大 λ（更保守）。
我的建议：在实际工程中，优先使用LM算法。它比高斯-牛顿更鲁棒，收敛速度也足够快。我在做自动驾驶量产项目的标定工具时，底层优化器用的就是LM。
4.5 三种方法的对比
| 方法 | 适用场景 | 优点 | 缺点 |
|---|---|---|---|
| 线性最小二乘 | 线性问题、初值估计 | 速度快、有闭式解 | 无法处理非线性 |
| 高斯-牛顿 | 非线性、残差较小 | 收敛快（二阶收敛） | 可能发散、需要好初值 |
| LM算法 | 通用非线性优化 | 鲁棒性好、自动调节步长 | 每步计算量稍大 |
4.6 知识体系总览
下面这张图总结了本章的核心逻辑，从标定问题的数学建模到优化求解的完整流程：
4.7 避坑指南
最后，分享几个我在实际项目中踩过的坑：
- 初值很重要。我曾经直接用随机数做初值，结果LM算法迭代了200次还没收敛。后来改用线性方法算初值，10次迭代就搞定了。
- 注意参数化方式。旋转矩阵用欧拉角还是四元数？欧拉角有万向锁问题，四元数需要归一化约束。我个人习惯用李代数（so3）做优化，既无约束又光滑。
- 数据质量决定上限。再好的优化算法，也救不了噪声大的数据。我见过有人拿运动中的标定数据去优化，结果外参一直在「抖动」。后来加了静止检测，问题就解决了。
一句话总结：标定 = 建模 + 优化。建模要准，优化要稳。两者缺一不可。
好了，这一章的内容就到这里。数学原理是标定的「内功」，虽然枯燥，但值得花时间吃透。下一章我们会进入实战，看看怎么用代码实现这些算法。