Skip to content
状态估计与 AI阅读约 10 分钟
IMU PreintegrationLie GroupSO(3)ESKFVIO

从三维旋转群 SO(3) 的几何约束出发,系统梳理 Forster 局部 IMU 预积分构造、零偏一阶雅可比修正与 ESKF 误差状态注入流程。

流形上的 IMU 预积分与李群 SO(3) 误差状态推导 ​

在视觉惯性里程计(VIO)与激光惯性里程计(LIO)中,IMU 通常以 200–1000 Hz 输出角速度与比力,而关键帧相机或激光雷达帧率仅 10–20 Hz。若在滑窗优化每次迭代重线性化位姿时都从头对数百条 IMU 测量重新积分,算力开销将迅速失控。流形上的 IMU 预积分(On-Manifold Preintegration) 通过将相对运动约束表达在局部体坐标系中,彻底消除了全局重力与初始位姿耦合,使高频惯性数据在优化迭代中无需重复积分。


1. 为什么不能直接在欧氏空间对旋转做加减? ​

三维旋转矩阵 R ∈ SO(3) 满足正交性约束 RTR = I 且 det(R) = +1,它并非欧氏向量空间,而是一个光滑李群流形。对其切空间 𝔰𝔬(3) 中的三维角速度矢量 φ ∈ ℝ3,可通过反对称算子 (·)∧ 与矩阵指数映射在流形上自由切换:

SO(3) Exponential & Logarithm Maps (Rodrigues Formula)
Exp(φ) = exp(φ∧) = I + (sin ||φ|| / ||φ||) φ∧ + ((1 − cos ||φ||) / ||φ||2) (φ∧)2(1) 指数映射
Exp(φ + δφ) ≈ Exp(φ) · Exp( Jr(φ) δφ )(2) BCH 右雅可比近似

其中 Jr(φ) 为 SO(3) 右雅可比矩阵。在离散滤波(ESKF)或因子图优化(FGO)中,我们始终在切空间更新小扰动误差 δθ,再通过右乘增量 R ← R · Exp(δθ) 将其映射回流形,从而天然规避欧拉角万向节锁与四元数归一化漂移。


2. 局部预积分量的构造与零偏一阶修正 ​

设两个相邻关键帧时刻 i 与 j 之间包含若干高频 IMU 采样。若直接对世界系速度与位置积分,积分项将显式依赖于初始姿态 Ri;一旦因子图优化调整了 Ri,所有积分必须重算。Forster 等人提出的局部预积分量 ΔRij、Δvij、Δpij 仅由体坐标系测量值决定:

Preintegrated Relative Motion & First-Order Bias Correction
ΔRij = ∏k=i..j−1 Exp( (ω̃k − bg,i − ηg,k) Δt )(3) 相对旋转预积分
Δvij = Σk=i..j−1 ΔRik (ãk − ba,i − ηa,k) Δt(4) 相对速度预积分
ΔRij(bg + δbg) ≈ ΔRij(bg) · Exp( (∂ΔRij / ∂bg) δbg )(5) 零偏一阶线性修正
高频 IMU 流ω̃_k, ã_k (400 Hz)中值法离散传播协方差矩阵递推预积分测量缓存ΔR_ij, Δv_ij, Δp_ij零偏雅可比 J_bg, J_ba信息矩阵 Σ_ij⁻¹因子图残差节点r_ΔR = Log(ΔR̂ᵀ R_iᵀ R_j)仅更新 δb 一阶展开无需重跑原始积分流形状态更新R ← R · Exp(δθ)

3. SO(3) 指数映射与预积分单步递推参考实现 ​

下面以 NumPy 给出符合数值稳定性要求的 SO3.exp(含小角度泰勒展开保护)以及中值积分预积分递推核心片段:

python
import numpy as np


def skew(v: np.ndarray) -> np.ndarray:
    """Map 3D vector to skew-symmetric matrix in so(3)."""
    return np.array([
        [0.0, -v[2], v[1]],
        [v[2], 0.0, -v[0]],
        [-v[1], v[0], 0.0],
    ], dtype=np.float64)


def so3_exp(phi: np.ndarray) -> np.ndarray:
    """Exponential map SO(3) with small-angle Taylor guard."""
    theta = float(np.linalg.norm(phi))
    K = skew(phi)
    if theta < 1e-8:
        return np.eye(3) + K + 0.5 * (K @ K)
    axis = K / theta
    return np.eye(3) + np.sin(theta) * axis + (1.0 - np.cos(theta)) * (axis @ axis)


class ImuPreintegrator:
    """Mid-point discrete preintegration on SO(3) x R^3 x R^3."""

    def __init__(self, bg: np.ndarray, ba: np.ndarray) -> None:
        self.bg = bg.copy()
        self.ba = ba.copy()
        self.dR = np.eye(3)
        self.dv = np.zeros(3)
        self.dp = np.zeros(3)
        self.dt_sum = 0.0

    def integrate(self, gyro0: np.ndarray, acc0: np.ndarray,
                  gyro1: np.ndarray, acc1: np.ndarray, dt: float) -> None:
        omega_mid = 0.5 * (gyro0 + gyro1) - self.bg
        dR_step = so3_exp(omega_mid * dt)
        dR_next = self.dR @ dR_step

        acc_world0 = self.dR @ (acc0 - self.ba)
        acc_world1 = dR_next @ (acc1 - self.ba)
        acc_mid = 0.5 * (acc_world0 + acc_world1)

        self.dp += self.dv * dt + 0.5 * acc_mid * (dt * dt)
        self.dv += acc_mid * dt
        self.dR = dR_next
        self.dt_sum += dt

4. 工程实现常见陷阱与参数配置对比 ​

工程环节典型隐患推荐工程对策
离散积分方法前向欧拉积分在剧烈角运动下产生一阶截断漂移采用中值积分(Mid-point)或 RK4,将局部截断误差降为 O(Δt3)
大零偏漂移若优化后 δbg 超过 ~0.05 rad/s,一阶线性化展开失效在因子图外层设阈值:当
旋转残差定义直接比较四元数差值会出现符号双覆盖跳变始终使用李代数残差 Log( (ΔR̃)T RiT Rj ) ∈ ℝ3
内参标定耦合陀螺仪尺度误差与非正交轴未标定导致高机动发散先做离线 Allan 方差标定噪声密度,再在线联合估计外参及时间偏移

讨论区开启了自动与人工双重审核,文明交流的评论会在短时间内展示。

Built with VitePress