📚 课程目录第4章标定数学基础

4. 标定数学基础

高精地图构建中的传感器标定，本质上是一个几何参数估计问题：我们需要求解传感器之间、传感器与车辆坐标系之间的精确空间关系。无论是激光雷达与相机的联合标定，还是IMU与GPS的时空对齐，其底层都依赖一套严谨的数学工具链。本章将系统梳理刚体变换、旋转矩阵、四元数、李群李代数以及最小二乘优化这五大数学支柱，并重点说明它们如何在标定流程中协同工作。理解这些数学基础，是设计鲁棒标定算法、诊断标定异常的前提。

上图展示了本章的知识框架：标定问题位于中心，四个数学工具分别解决“如何描述位姿”（刚体变换、旋转表示）、“如何求导优化”（李群李代数）、“如何求解最优参数”（最小二乘优化）。下面我们逐一深入。

4.1 刚体变换与旋转矩阵：标定的几何基石

核心原理

刚体变换描述了一个物体在三维空间中不发生形变的运动，包含旋转（R）和平移（t）两部分。在标定中，我们通常用齐次变换矩阵 T = [R | t; 0 1] 来表示两个坐标系之间的映射关系。旋转矩阵 R ∈ SO(3) 是正交矩阵（R^T R = I），行列式为 +1，保证了变换的刚性。例如，激光雷达点云从雷达坐标系转换到车辆坐标系，就是一次典型的刚体变换：P_veh = R * P_lidar + t。

判断方法与操作流程

在实际标定中，判断一个变换矩阵是否合理，需要检查以下三点：

正交性校验：计算 R^T R 是否接近单位阵，若偏差大于 1e-6，说明旋转矩阵退化或数值不稳定。
尺度不变性：刚体变换不改变两点间距离，即 ||P1 - P2|| = ||R*P1 + t - (R*P2 + t)||。
右手定则：旋转矩阵的行列式必须为 +1，若为 -1 则引入了镜像反射，这在物理上不可能。

操作流程上，通常先利用标定板或特征点对建立对应关系，然后通过 SVD 分解求解最优的 R 和 t（即 Kabsch 算法）。

案例拆解：LiDAR-Camera 外参初值求解

假设我们有一组 3D 激光雷达角点 P_lidar 和对应的相机像素坐标 p_cam，通过 PnP 或直接线性变换（DLT）可以求得初始的 R, t。但这里有一个常见陷阱：旋转矩阵的欧拉角表示存在万向锁问题。例如，当俯仰角接近 ±90° 时，滚转和偏航角无法唯一分离，导致优化不稳定。因此，在工程中我们更推荐使用四元数或旋转向量作为旋转的存储和优化形式，仅在可视化或人工检查时转换为欧拉角。

经验提醒：在标定代码中，始终以四元数或旋转矩阵作为内部状态，避免在迭代过程中直接使用欧拉角。若必须使用欧拉角，请限制俯仰角范围并添加约束。

常见误区与总结

一个常见误区是混淆了“世界坐标系到传感器坐标系”与“传感器坐标系到世界坐标系”的变换方向。在联合标定中，我们通常定义 T_world_sensor 表示传感器在世界坐标系中的位姿，而 T_sensor_world 是其逆变换。若方向搞反，所有后续的点云拼接和地图构建都会出现系统性错位。建议在代码中统一使用 T_a_b 的命名规范，并写单元测试验证 T_a_b * T_b_a = I。

4.2 四元数：紧凑且无奇异的旋转表示

核心原理

四元数 q = (w, x, y, z) 是复数在三维空间的推广，用 4 个参数表示旋转，避免了欧拉角的万向锁问题，且比旋转矩阵（9 个参数）更紧凑。单位四元数满足 w^2 + x^2 + y^2 + z^2 = 1。旋转向量 v 通过 v' = q * v * q^{-1} 进行变换（其中 q^{-1} = (w, -x, -y, -z)）。在标定中，四元数的插值（Slerp）也常用于多帧数据的平滑融合。

操作流程与转换规则

在标定工程中，四元数与旋转矩阵的转换是高频操作。以下是一个典型的转换流程：

从旋转矩阵 R 提取四元数：根据矩阵迹的大小选择不同的计算公式，避免数值不稳定。
四元数归一化：在优化迭代中，由于数值误差，四元数会逐渐偏离单位模长，需要每步重新归一化。
微分更新：在优化中使用 Δq ≈ (1, δθ/2) 的形式进行小扰动更新，其中 δθ 是旋转向量增量。

案例拆解：IMU 姿态解算中的四元数更新

在 IMU-Camera 标定中，我们需要利用陀螺仪的角速度积分来预测姿态变化。假设陀螺仪输出为 ω = (ωx, ωy, ωz)，则四元数导数满足：

dq/dt = 0.5 * q ⊗ (0, ωx, ωy, ωz)

其中 ⊗ 表示四元数乘法。离散化后，q_{k+1} = q_k + dq/dt * Δt，然后归一化。这里的关键是积分前必须去除陀螺仪零偏，否则姿态会随时间漂移。我们在实际标定中发现，若零偏未校准，30 秒内的姿态误差可达 5°以上，直接导致相机外参优化发散。

风险提示：四元数虽然无奇异性，但存在“双重覆盖”问题——q 和 -q 表示同一个旋转。在优化中，若不加约束，可能在这两个等价点之间跳跃，导致 Hessian 矩阵奇异。建议在代价函数中固定 w > 0 的符号约束。

总结复盘

四元数是标定中旋转状态的最佳载体，但需要配合归一化和符号约束。下表对比了三种旋转表示的适用场景：

表示方式	参数数量	优点	缺点	标定中的应用
旋转矩阵	9	直观、易于坐标变换	冗余、需正交化	最终结果输出、坐标转换
欧拉角	3	物理意义明确	万向锁、非线性	人工调试、可视化
四元数	4	紧凑、无奇异、易插值	双重覆盖、不直观	优化迭代、姿态传播

4.3 李群与李代数：标定优化的核心引擎

核心原理

旋转矩阵构成特殊正交群 SO(3)，刚体变换构成特殊欧氏群 SE(3)。这些群是流形，不能直接进行加法运算。李代数 so(3) 和 se(3) 是其在单位元处的切空间，对应旋转向量 φ 和变换向量 ξ。通过指数映射 exp(φ^) 可以将李代数映射回李群。在标定优化中，我们利用李代数进行无约束的加法更新，然后通过指数映射映射回流形，从而避免破坏旋转矩阵的正交性。

操作流程：扰动模型

在标定中，我们常用左乘扰动模型来求导。假设当前估计为 T，对其施加微小扰动 δξ，则更新后的变换为 T_new = exp(δξ^) * T。这样，残差对扰动量的雅可比矩阵 J 可以简洁地推导出来。具体步骤如下：

定义残差 e = z - h(T, x)，其中 z 是观测值，h 是预测模型。
计算 J = ∂e / ∂δξ，这通常涉及链式法则和叉乘运算。
构建高斯牛顿方程 (J^T J) Δξ = -J^T e，求解增量 Δξ。
更新 T ← exp(Δξ^) * T，迭代直至收敛。

案例拆解：手眼标定中的 AX=XB 问题

在机器人或自动驾驶的相机-机械臂标定中，我们需要求解方程 AX = XB，其中 A 是机械臂末端位姿变化，B 是相机位姿变化，X 是待求的手眼变换矩阵。传统方法先线性求解旋转部分，再求解平移部分。但这种方法对噪声敏感。我们采用李代数方法：将 X 参数化为 se(3) 向量，构建最小二乘问题 min ||log(A * X * B^{-1} * X^{-1})||^2，利用李代数求导进行迭代优化。实验表明，在加入 0.1° 旋转噪声和 1mm 平移噪声的情况下，李代数方法的旋转误差比线性方法降低 40%。

关键代码模式（伪代码）：

// 李代数更新
Eigen::Matrix4d T_update = exp_se3(delta_xi) * T_current;
// 残差计算
Eigen::Vector3d residual = z - project(T_update * P);
// 雅可比矩阵（左扰动）
Eigen::Matrix<double, 3, 6> J;
J.block<3,3>(0,0) = I; 
J.block<3,3>(0,3) = -skew(T_update * P);

常见误区与总结

一个常见误区是混淆了左乘扰动和右乘扰动。左乘扰动适用于全局坐标系下的误差定义，右乘扰动适用于传感器局部坐标系。在标定中，如果残差定义在相机坐标系下，应使用右乘扰动；如果定义在世界坐标系下，应使用左乘扰动。选错会导致雅可比矩阵错误，优化不收敛。另一个误区是忽略李代数的 BCH 近似，当扰动较大时，exp(Δξ) * exp(ξ) ≈ exp(ξ + J_r^{-1} Δξ) 的近似会失效，需要迭代求解。

4.4 最小二乘优化：标定参数求解的最终武器

核心原理

标定问题最终都归结为最小化残差平方和：min Σ ||r_i(x)||^2。由于残差与参数之间通常是非线性关系（如相机投影模型），我们需要使用迭代方法。最常用的是高斯牛顿法和Levenberg-Marquardt (LM) 算法。高斯牛顿法通过一阶泰勒展开近似，求解正规方程 (J^T J) Δx = -J^T r。LM 算法在高斯牛顿基础上加入阻尼项 λI，在梯度下降和高斯牛顿之间自适应切换，提高了鲁棒性。

操作流程：从残差构建到收敛判断

一个标准的标定优化流程如下：

定义残差：例如重投影误差 e = p_obs - K * (R * P + t)。
计算雅可比：解析求导或使用自动微分（如 Ceres 的 AutoDiff）。
求解增量：使用稀疏 Cholesky 分解或 QR 分解求解正规方程。
更新参数：对于旋转使用李代数更新，对于平移直接加法。
收敛判断：当增量范数小于阈值（如 1e-8）或残差变化率小于 1e-6 时停止。

案例拆解：多传感器联合标定的鲁棒优化

在车规级标定车间，我们同时标定 4 个相机、1 个激光雷达和 1 个 IMU。初始外参由 CAD 模型提供，但存在约 2° 的安装偏差。我们构建了一个包含三类残差的联合优化问题：

相机-激光雷达：3D 点到图像平面的重投影误差（约 200 个点）。
相机-相机：标定板角点的双目视差误差（约 50 个角点）。
IMU-相机：旋转增量误差（约 300 帧数据）。

使用 Ceres Solver 的 LM 算法，并采用 Huber 损失函数抑制外点。初始残差均方根为 15.2 像素，经过 20 次迭代后降至 0.8 像素。关键操作是分阶段优化：先固定 IMU 姿态，优化相机内参和外参；再固定相机，优化 IMU 与激光雷达的外参。若一次性优化所有参数，容易陷入局部极小值。

风险说明：最小二乘对初值敏感。若初始外参误差超过 10°，优化极易发散。工程上建议先用标定板或手工选点获得粗略初值，再进行精细优化。此外，病态问题（如相机与激光雷达共视区域过小）会导致 J^T J 接近奇异，此时应增加正则化项或固定部分参数。

总结复盘

下表总结了不同优化策略的适用场景：

优化方法	收敛速度	鲁棒性	适用场景
高斯牛顿	快（二次收敛）	差（对初值敏感）	初值较好、残差较小
LM	中	好（阻尼调节）	通用标定场景
Dogleg	中快	好	大规模稀疏问题
随机梯度下降	慢	较好	在线标定、数据流式到达

在实际工程中，我们推荐使用 Ceres Solver 或 G2O 等成熟库，它们内置了 LM、Dogleg 等算法，并支持自动微分和鲁棒核函数。但必须理解底层原理，才能正确设置参数（如阻尼因子初值、最大迭代次数）并诊断不收敛的原因。

综上所述，刚体变换提供了坐标映射的框架，旋转矩阵和四元数解决了旋转的表示问题，李群李代数让旋转可以在流形上高效优化，而最小二乘优化则是将这一切融合为可求解的数值问题。这四者环环相扣，构成了高精地图传感器标定的完整数学闭环。掌握这些工具，不仅能读懂标定算法的源码，更能针对实际场景设计出稳定、高精度的标定方案。