4、标定数学基础：最小二乘法、非线性优化、李群李代数、图优化、卡尔曼滤波在标定中的应用

智能驾驶多传感器标定的本质，是求解一组最优的几何变换参数（外参、内参、时间延迟等），使得同一物理目标在不同传感器观测中的投影误差最小。这一求解过程高度依赖数学工具：从经典的最小二乘，到处理旋转矩阵约束的李群李代数，再到融合多帧信息的图优化与卡尔曼滤波。本章将系统梳理这些数学工具在标定中的角色定位、核心原理与工程落地路径，帮助学员建立从“公式”到“代码”再到“实车标定”的完整认知。

4.1 最小二乘法：标定问题的起点与线性化处理

核心原理：最小二乘法是标定中最基础的数学框架。其核心思想是：给定一组观测数据（如像素坐标、激光点云距离、IMU角速度），寻找一组参数（外参、内参），使得模型预测值与实际观测值之间的残差平方和最小。在标定中，残差通常定义为重投影误差、点到面距离或惯性积分误差。对于线性模型（如相机内参中的焦距与主点），可以直接通过正规方程求解；对于非线性模型（如畸变系数、外参旋转），则需要迭代求解。

判断方法与操作流程：工程中判断一个标定问题是否适合最小二乘，主要看三点：① 残差函数是否光滑可导；② 参数维度是否适中（通常小于50维）；③ 是否有足够的观测约束（每个参数至少需要3-5个独立观测）。操作流程为：构建残差模型 → 计算雅可比矩阵 → 求解增量方程 → 更新参数 → 检查收敛性。对于线性最小二乘，直接使用SVD分解或QR分解求解；对于非线性问题，采用Gauss-Newton或Levenberg-Marquardt（LM）算法。

方法	适用场景	优点	缺点
线性最小二乘	相机内参标定（忽略畸变）、平面外参初值	求解快、全局最优	无法处理旋转矩阵约束
非线性最小二乘（LM）	相机-激光雷达外参、多传感器联合标定	精度高、鲁棒性好	依赖初值、可能陷入局部最优
加权最小二乘	不同传感器噪声方差差异大	考虑噪声统计特性	需要准确估计噪声协方差

4.2 李群李代数：旋转矩阵的优雅表达与扰动模型

核心原理：旋转矩阵构成特殊正交群SO(3)，齐次变换矩阵构成特殊欧氏群SE(3)。这些矩阵群在乘法下封闭，但不满足加法封闭性，因此无法直接使用常规的梯度下降法。李群李代数通过指数映射和对数映射，将旋转矩阵映射到其切空间（李代数so(3)），使得优化可以在向量空间中进行。在标定中，李代数提供了两个关键优势：① 无约束优化（旋转矩阵的9个参数降为3个自由参数）；② 扰动模型（右乘或左乘微小扰动）简化了雅可比矩阵的推导。

判断方法与操作流程：当标定参数中包含旋转矩阵或变换矩阵时，必须使用李代数表示。操作流程为：将旋转矩阵R转换为李代数φ（对数映射）→ 在优化中更新φ → 通过指数映射恢复R。对于微小扰动，使用右乘扰动模型：R_new = R * exp(δφ)，其中δφ为3维向量。在代码实现中，Sophus库提供了SO3和SE3的完整实现，Ceres Solver也内置了对李代数的支持。

// Sophus 示例：SE3 的增量更新
SE3d T_ic = SE3d(quat, trans); // 相机到IMU的外参
Vector6d delta; // 前3维为平移增量，后3维为旋转增量
delta.setZero();
delta.tail<3>() = Vector3d(0.01, 0.0, 0.0); // 微小旋转扰动
T_ic = SE3d::exp(delta) * T_ic; // 左乘扰动

案例拆解：在IMU-相机外参标定中，使用Kalibr工具。输入为IMU角速度/加速度数据和相机图像序列。优化变量为外参T_ic（SE3）、陀螺仪偏置、加速度计偏置、时间延迟。残差包括视觉重投影误差和IMU预积分误差。通过李代数表示T_ic，优化维度从16维（4x4矩阵）降为6维，且保证了旋转部分的正交性。最终标定结果：旋转误差小于0.1°，平移误差小于2mm。

4.3 图优化：多帧联合标定的因子图框架

核心原理：图优化将标定问题建模为因子图（Factor Graph），其中节点表示待估计变量（外参、内参、位姿、路标点），边表示观测约束（重投影误差、IMU预积分、回环检测）。相比独立帧的最小二乘，图优化能够充分利用多帧之间的时空约束，显著提高标定精度和鲁棒性。在标定中，图优化特别适合以下场景：① 多传感器联合标定（相机+激光雷达+IMU）；② 在线标定（车辆行驶中持续优化）；③ 标定结果精化（在初值基础上进行全局优化）。

操作流程：构建因子图 → 设置信息矩阵（噪声协方差的逆）→ 选择求解器（Gauss-Newton或LM）→ 稀疏矩阵分解（利用Schur补）→ 迭代求解。在工程中，g2o和GTSAM是两种主流库。GTSAM基于贝叶斯网络，支持增量优化（iSAM2），适合在线标定；g2o基于稀疏图优化，适合离线批量标定。

对比维度	g2o	GTSAM
优化方式	批量优化	增量优化（iSAM2）
适用场景	离线标定、小规模问题	在线标定、大规模问题
李代数支持	内置SE3/SO3	内置SE3/SO3
鲁棒核函数	Huber、Cauchy	Huber、Tukey
学习曲线	中等	较陡

4.4 卡尔曼滤波：在线标定与动态估计的递归方法

核心原理：卡尔曼滤波（KF）及其扩展形式（EKF、UKF）适用于在线标定场景，即车辆行驶过程中持续估计外参。其核心思想是：通过状态预测（基于运动模型）和观测更新（基于传感器测量）两个步骤，递归地估计系统状态。在标定中，状态通常包括外参（平移和旋转）、传感器偏置、时间延迟等。与批量优化不同，卡尔曼滤波是递归的，计算量恒定，适合嵌入式平台实时运行。

判断方法与操作流程：当标定需要实时性（如在线标定）、系统模型为线性或弱非线性、噪声服从高斯分布时，优先选择卡尔曼滤波。操作流程为：初始化状态向量和协方差矩阵 → 预测步骤（状态转移和协方差传播）→ 更新步骤（计算卡尔曼增益、更新状态和协方差）。对于强非线性系统，使用EKF（一阶线性化）或UKF（无迹变换）。

// EKF 在线外参标定伪代码
// 状态: x = [t_x, t_y, t_z, q_w, q_x, q_y, q_z, b_g, b_a]
// 预测: x_pred = f(x_prev, u_imu)
// 更新: K = P_pred * H^T * (H * P_pred * H^T + R)^-1
//       x_new = x_pred + K * (z - h(x_pred))
//       P_new = (I - K * H) * P_pred

案例拆解：在车辆行驶中的IMU-相机在线标定中，使用ESKF估计外参。状态量为外参（SE3）、陀螺仪偏置、加速度计偏置。预测步骤使用IMU角速度和加速度积分；更新步骤使用视觉特征的重投影误差。实验条件：车辆在城市道路行驶10分钟，包含加速、减速、转弯等工况。结果：外参收敛时间约30秒，稳态误差为0.3°和3mm。对比离线批量优化（精度0.1°和1mm），在线滤波精度略低，但满足实时性要求。

4.5 数学工具选型与工程实践总结

选型原则：在实际标定项目中，数学工具的选择取决于应用场景、精度要求和计算资源。下表给出了选型建议：

场景	推荐工具	理由
离线静态标定（单传感器）	非线性最小二乘（LM）	精度高、实现简单
离线联合标定（多传感器）	图优化（g2o/GTSAM）	充分利用多帧约束
在线标定（实时性要求高）	ESKF/UKF	递归计算、恒定开销
初值估计	线性最小二乘/闭式解	快速获得粗略结果
高精度精化	图优化+鲁棒核函数	全局最优、抗外点

工程实践要点：① 所有优化问题必须提供良好的初值，建议使用闭式解或手眼标定结果；② 残差构建时统一坐标系和单位（毫米、弧度）；③ 使用鲁棒核函数（Huber）抑制外点；④ 验证标定结果时，使用独立于训练数据的验证集；⑤ 记录优化过程中的残差下降曲线，判断是否收敛到局部最优。

总结复盘：标定数学工具的选择不是孤立的，而是相互配合的。最小二乘法是基础框架，李群李代数解决了旋转约束问题，图优化扩展了多帧联合优化能力，卡尔曼滤波实现了在线递归估计。在实际项目中，建议按照“初值估计（线性最小二乘）→ 精细优化（图优化）→ 在线更新（卡尔曼滤波）”的流程组织标定算法。同时，要重视数据质量（时间同步、外点剔除）和参数调优（噪声协方差、核函数阈值），这些工程细节往往比数学公式本身更影响最终标定精度。