3. 标定数学基础
在 Camera-LiDAR-IMU 联合标定工程中，数学基础决定了我们能否把“传感器之间的空间关系”写成可求解、可优化、可验证的问题。标定本质上是在估计若干坐标系之间的刚体变换，并利用观测残差不断修正这些变换。若旋转表示、误差定义或优化模型选错，后续即使采集了大量数据，也可能收敛到错误解或陷入局部极小。因此，本节从旋转表示出发，逐步过渡到刚体变换、李群李代数、手眼标定方程以及非线性优化基础，形成一条完整的数学链路。
一、旋转表示：从欧拉角到四元数
三维旋转有多种等价表示，常见包括旋转矩阵、欧拉角、轴角、四元数以及李代数。旋转矩阵 R ∈ SO(3) 直观且便于坐标变换，但存在 9 个参数、6 个约束，直接优化容易破坏正交性。欧拉角参数少、易读，但存在万向节死锁和周期跳变，不适合作为优化变量。四元数 q = [w, x, y, z] 紧凑且无奇异，但单位球约束需要额外处理。轴角用旋转轴和角度描述，适合表达小旋转误差。李代数 so(3) 则是无约束的三维向量，最适合非线性优化中的增量更新。
在工程中，通常采用“存储用四元数、优化用李代数”的策略。例如，外参中的旋转部分初始化为四元数，优化时将其映射到 so(3) 上求增量，再通过指数映射更新回 SO(3)。这样既避免了冗余参数，又保证了旋转矩阵的正交性。
二、刚体变换与齐次坐标
刚体变换由旋转和平移组成，记为 T = [R | t]，其中 R ∈ SO(3)，t ∈ R³。在齐次坐标下，点 P 的变换可写为 P' = T · P。Camera-LiDAR-IMU 联合标定中，我们通常需要估计多个外参：LiDAR 到 Camera、IMU 到 Camera、IMU 到 LiDAR。它们之间并非独立，而是满足闭环约束：T_LC = T_LI · T_IC。若分别独立标定，闭环误差会暴露不一致性；若联合标定，则需在优化中显式加入闭环约束或使用手眼标定方程统一求解。
核心关系：若已知 T_IC 和 T_LI，则 T_LC = T_LI · T_IC；反之，若已知 T_LC 和 T_IC，则 T_LI = T_LC · T_IC⁻¹。标定问题往往就是在这类约束下寻找最优变换。
三、李群李代数：为什么优化离不开它
旋转矩阵构成李群 SO(3)，刚体变换构成李群 SE(3)。李群是光滑流形，不能直接像欧氏空间那样做加减。李代数 so(3) 和 se(3) 是李群在单位元处的切空间，是向量空间，可以自由加减。指数映射 exp: so(3) → SO(3) 把李代数向量映射为旋转矩阵；对数映射 log: SO(3) → so(3) 则相反。在优化中，我们通常在当前估计 T 的切空间上求增量 δξ，然后更新 T ← exp(δξ) · T 或 T ← T · exp(δξ)。左乘与右乘对应不同的扰动模型，选择取决于残差定义和坐标系约定。
对于 Camera-LiDAR-IMU 标定，李代数带来的最大好处是：旋转更新不再需要归一化，雅可比矩阵可以解析推导，优化变量维度从 12 降到 6，数值稳定性显著提升。
四、手眼标定方程 AX = XB
手眼标定是联合标定的经典模型。设 A 为传感器 A 在两次运动之间的相对位姿，B 为传感器 B 的相对位姿，X 为待求外参，则方程 AX = XB 成立。在 Camera-LiDAR-IMU 场景中，A 可以是 IMU 的积分位姿，B 可以是 Camera 或 LiDAR 的里程计位姿，X 即待标定的外参。该方程可拆分为旋转部分 R_A R_X = R_X R_B 和平移部分 R_A t_X + t_A = R_X t_B + t_X。旋转部分可先独立求解，再代入平移部分求 t_X。
实际工程中，AX = XB 通常不直接解析求解，而是转化为最小二乘问题，因为观测存在噪声且运动激励可能不足。若运动退化，例如纯平移或纯旋转，旋转部分可能不可观，导致标定失败。因此，采集数据时必须保证充分的旋转和平移激励。
五、非线性优化基础：残差、雅可比与迭代更新
联合标定最终转化为一个非线性最小二乘问题：min Σ ||r_i(T)||²。其中 r_i 是第 i 个观测残差，T 是待标定外参。常用求解方法包括高斯-牛顿法（GN）和列文伯格-马夸尔特法（LM）。GN 收敛快，但在初值较差或雅可比病态时容易发散；LM 通过阻尼因子在 GN 和梯度下降之间切换，鲁棒性更好，是标定工程中的首选。
优化流程通常为：1）构造残差函数；2）计算残差对李代数增量的雅可比；3）求解增量方程 (JᵀJ + λI)δξ = -Jᵀr；4）更新位姿；5）判断收敛。对于 Camera-LiDAR-IMU 联合标定，残差通常包括 Camera 重投影误差、LiDAR 点面距离误差以及 IMU 预积分误差。三类残差量纲不同，需要合理设置信息矩阵或权重。
// 伪代码：LM 优化外参 T
T = initial_guess;
lambda = 1e-3;
for iter in range(max_iter):
J, r = build_residuals(T); // 重投影 + 点面 + IMU
H = J.T * J + lambda * I;
g = -J.T * r;
delta = solve(H, g); // delta ∈ se(3)
T_new = exp(delta) * T;
if cost(T_new) < cost(T):
T = T_new;
lambda *= 0.7;
else:
lambda *= 1.5;
if norm(delta) < eps:
break;
六、常见误区与复盘
| 误区 | 后果 | 正确做法 |
|---|---|---|
| 直接用欧拉角作为优化变量 | 万向节死锁、周期跳变、收敛不稳定 | 使用四元数存储，李代数增量更新 |
| 忽略手眼方程闭环约束 | 外参不一致，重投影与点云对齐冲突 | 显式加入 T_LC = T_LI · T_IC 约束 |
| 运动激励不足 | 旋转或平移不可观，标定解漂移 | 采集充分旋转和平移，检查可观性 |
| 残差权重随意设置 | 某一传感器主导优化，其他残差被淹没 | 根据噪声模型设置信息矩阵，做敏感性分析 |
| 初值过差直接优化 | 陷入局部极小，标定失败 | 先粗标定或手动给初值，再精优化 |
工程建议：在 Camera-LiDAR-IMU 联合标定中，建议先分别完成 Camera-IMU 和 LiDAR-IMU 的粗标定，得到 T_IC 和 T_LI 的初值，再以 T_LC = T_LI · T_IC 作为联合优化初值。优化时优先固定一个传感器坐标系作为参考系，避免整体漂移。每次迭代后检查残差下降曲线和闭环误差，若闭环误差不降反升，应检查雅可比符号和扰动模型是否一致。
综上，旋转表示决定了参数化方式，刚体变换描述了坐标系关系，李群李代数提供了无约束优化途径，手眼标定方程给出了约束形式，非线性优化则负责在噪声数据中求解最优外参。这五部分环环相扣，是 Camera-LiDAR-IMU 联合标定工程中必须扎实掌握的数学基础。