3. 自标定原理

自标定（Self-Calibration）的本质，是让系统在无需外部高精度基准设备的条件下，利用自身传感器之间的冗余观测与运动激励，反推出内参、外参或零偏等误差参数。它之所以可行，依赖三个前提：误差可参数化、参数可被观测、观测方程可求解。本节从数学基础出发，串联可观测性、参数辨识、最小二乘与卡尔曼滤波，建立自标定的完整认知链条。

一、数学基础：从误差模型到观测方程

自标定的第一步是把物理误差写成可计算的参数向量。以相机-IMU 系统为例，待标定参数通常包括相机内参、相机-IMU 外参、IMU 零偏与尺度因子，记作状态向量 θ。系统在运动过程中产生的观测量 z（如特征点重投影、IMU 预积分残差）与 θ 之间构成非线性映射：

z = h(θ, x) + n
其中：θ 为待标定参数，x 为位姿/速度等状态量，n 为观测噪声

对 h 在当前估计值附近做一阶泰勒展开，得到线性化观测方程 δz = J·δθ + n，其中 J = ∂h/∂θ 称为雅可比矩阵。自标定的全部数学工作，都围绕“如何构造 J、判断 J 是否满秩、以及如何稳定求解 δθ”展开。若 J 的某些列线性相关，说明对应参数无法被区分，标定结果必然漂移。

二、可观测性分析：判断参数能否被标定

可观测性回答的是“这组运动轨迹能否唯一确定这些参数”。工程上常用两种判据：一是雅可比矩阵秩分析，若 rank(J) < dim(θ)，则存在不可观测方向；二是Fisher 信息矩阵 F = JᵀΣ⁻¹J，其特征值大小反映各参数方向的可观测程度，特征值越小越接近不可观测。

典型结论是：纯平移运动无法激励 IMU 零偏与相机-IMU 外参的耦合项；纯旋转运动无法观测加速度计零偏；只有包含充分旋转与平移的“六自由度激励”轨迹，才能让参数可观测。因此自标定对数据采集动作有明确要求，而不是随便跑一段就能标定。

三、参数辨识方法：从最小二乘到滤波

参数辨识是在观测方程基础上求解 θ 的过程。按处理方式可分为批处理与递推两类：批处理以最小二乘及其变体为代表，适合离线高精度标定；递推以卡尔曼滤波为代表，适合在线自标定与实时补偿。

四、最小二乘与卡尔曼滤波入门

最小二乘的目标是求 θ 使 Σ‖zᵢ − h(θ)‖² 最小。线性情形下有闭式解 θ = (JᵀJ)⁻¹Jᵀz；非线性情形用高斯-牛顿或 LM 迭代，每次更新 δθ = (JᵀJ + λI)⁻¹Jᵀr，其中 λ 为阻尼因子，用于在 JᵀJ 病态时稳定求解。

卡尔曼滤波把待标定参数增广进状态向量，通过“预测—更新”递推估计。预测步用运动模型传播状态与协方差，更新步用观测残差修正参数：

预测：x̂ₖ⁻ = F·x̂ₖ₋₁,  Pₖ⁻ = F·Pₖ₋₁·Fᵀ + Q
更新：K = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹
      x̂ₖ = x̂ₖ⁻ + K·(z − H·x̂ₖ⁻)
      Pₖ = (I − K·H)·Pₖ⁻

其中 Q 为过程噪声，R 为观测噪声，K 为卡尔曼增益。对自标定而言，参数通常建模为随机游走（Q 很小），使滤波器在长时间运行中缓慢修正零偏与外参。EKF 则用于非线性观测，需在每个时刻重新计算雅可比 H。

五、案例与复盘：相机-IMU 在线自标定

某量产项目在车辆出厂后需持续修正相机-IMU 外参。数据采集要求车辆完成包含左右转向、加减速与颠簸路面的激励轨迹。离线阶段用 LM 求解外参与零偏，得到 JᵀJ 最小特征值对应的方向即为弱可观测方向，据此判断是否需要补充激励。在线阶段将外参增广进 EKF 状态，Q 设为极小值，使参数缓慢收敛。

复盘发现两个典型问题：其一，初期轨迹只有直线行驶，外参平移分量几乎不可观测，滤波器给出的参数虽收敛但偏差大；其二，R 设置过小导致观测异常值被过度信任，参数出现跳变。修正方式是增加转向激励、对观测做卡方检验剔除异常值，并适当增大 R。这说明自标定的成败不仅取决于算法，更取决于激励设计与噪声建模。

综上，自标定的数学主线是“参数化—线性化—可观测性判定—求解—在线修正”。最小二乘解决“如何一次性求准”，卡尔曼滤波解决“如何持续跟踪”。理解 J 的秩与 Fisher 信息矩阵的特征值，比记住公式更重要，因为它们直接决定标定是否可信、轨迹是否合格、量产是否稳定。