基于 ${\bf SE_2}({\bf 3})$ 的精确 IMU 预积分

${\bf SE_2}({\bf 3})$ Based Exact IMU Pre-Integration

文献¹基于 ${\bf SE_2}({\bf 3})$ 自同构指数函数引入了一种新的 IMU 预积分公式。该方法确保了 IMU 数据的高精度整合，可以有效增强快速旋转运动和延长积分时间下 SLAM 系统的跟踪性能。

1. IMU 固有补偿 - IMU intrinsic compensation

文献¹作者首先要在 IMU 预积分阶段之前添加 IMU 固有补偿。当在视觉惯性 SLAM 系统中使用 IMU 信息时，通常会将原始加速度计和陀螺仪测量值建模为

$$ \begin{align*} \check{\bf a}_ t &= \mathbf a_t + \mathbf b_{a_t} + \mathbf n_{a_t} \tag{1.a} \label{1.a} \newline \check{\boldsymbol\omega}_ t &= \boldsymbol\omega_t + \mathbf b_{\omega_t} + \mathbf n_{\omega_t} \tag{1.b} \label{1.b} \end{align*} $$

仅考虑测量噪声密度（即 $\bf n$）和偏差 (bias) $\bf b$。受加速度偏置 $\mathbf b_{a_t}$ 和陀螺仪偏置 $\mathbf b_{\omega_t}$ 以及加性噪声 $\mathbf n_a$ 和 $\mathbf n_\omega$ 的影响，模型 $\eqref{1.a} \eqref{1.b}$ 简单且有用，可为具有工厂校准内在特性的器件提供良好的近似值。然而，对于低成本的消费级惯性传感器来说，它可能会产生有损校准的结果，因为这些传感器会表现出明显的轴对齐误差和比例因子误差。受²的启发，通过引入 IMU 内在建模来扩展 IMU 模型 $\eqref{1.a} \eqref{1.b}$ ，

$$ \begin{align*} \check{\bf a}_ t &= \mathbf S_\alpha \mathbf M_\alpha \mathbf a_t + \mathbf b_{a_t} + \mathbf n_ {a_t} \tag{2.a} \label{2.a} \newline \check{\boldsymbol\omega}_ t &= \mathbf S_\omega \mathbf M_\alpha \boldsymbol\omega’_ t + \mathbf A_\omega \mathbf a_t + \mathbf b_ {\omega_t} + \mathbf n_ {\omega_t} \tag{2.b} \label{2.b} \end{align*} $$

其中，$\boldsymbol \omega_t’ = \mathbf C_\omega \boldsymbol \omega_t$ 是加速度计和陀螺仪之间的旋转矩阵。我们将 $\mathbf S_\alpha$ 和 $\mathbf S_\omega$ 定义为缩放效应的对角矩阵，$\mathbf M_\alpha$ 和 $\mathbf M_\omega$ 是对应于失准小角度的下单位三角矩阵，并令 $\mathbf A_\omega$ 为完全填充的倾斜矩阵。上述所有特性都可以通过执行广泛采用的 Kalibr ³来获得。扩展模型的使用可以参考这里 how to use scale-misalignment result ?

2. IMU 预积分 - IMU pre-integration

按照上一节的方法，我们对 IMU 轴偏差和比例因子进行补偿，并使用符号 $\bar{\boldsymbol\omega}_t$ 和 $\bar{\bf a}_t$ 表示有偏差但无噪声、无歪斜和比例校正的 IMU 测量值。在以下章节中，我们去掉或保留下标 $t$ ，以简化符号，或强调变量的时间依赖性。

现在继续介绍核心贡献：一种新颖、精确的 IMU 预积分公式，基于矩阵李群 ${\bf SE_2}({\bf 3})$ 自同构的指数函数。我们首先对系统的无噪声运动学进行建模如下： $$ \begin{cases} \dot{\mathbf R} &=& \mathbf R (\bar{\boldsymbol\omega} - \mathbf b_\omega)^\wedge \newline \dot{\mathbf p} &=& \mathbf v \newline \dot{\mathbf v} &=& \mathbf R (\bar{\mathbf a} - \mathbf b_a) + \mathbf g \end{cases} \tag{3} \label{3} $$ 其中 $(\dot{\mathbf R}, \dot{\bf p}, \dot{\bf v})$ 是 IMU 坐标系的旋转、位置和速度的一阶导数，$\bf g$ 表示重力矢量，两者均相对于世界固定坐标系表示。我们还定义了偏差 $\dot{\boldsymbol b}_ \omega = \boldsymbol\tau_\omega$ 和 $\dot{\bf b}_ a = \boldsymbol\tau_a$ 的随机游走。我们可以使用 ${\bf SE_2}({\bf 3})$ 李群中的扩展位姿来表示 IMU 位姿，使得状态为 $\boldsymbol\xi = \begin{bmatrix} \bf R & \bf p & \bf v \newline 0 & 1 & 0 \newline 0 & 0 & 1 \end{bmatrix}$ 并且状态 $\boldsymbol\xi$ 的运动学可以使用 ${\bf SE_2}({\bf 3})$​ 的自同构来表示⁴ $$ \dot{\boldsymbol\xi} = (\mathbf G - \mathbf D) \boldsymbol\xi + \boldsymbol\xi (\mathbf U - \mathbf B + \mathbf D) \tag{4}\label{4} $$ 其中， $$ \begin{align*} \mathbf U &= \begin{bmatrix} \boldsymbol\omega^\wedge & 0 & \bf a \newline 0 & 0 & 0 \newline 0 & 0 & 0 \end{bmatrix}, & \mathbf G &= \begin{bmatrix} 0 & 0 & \mathbf g \newline 0 & 0 & 0 \newline 0 & 0 & 0 \end{bmatrix}, \newline \mathbf D &= \begin{bmatrix} 0 & 0 & 0 \newline 0 & 0 & 0 \newline 0 & 1 & 0 \end{bmatrix}, & \mathbf B &= \begin{bmatrix} (\mathbf b_\omega)^\wedge & 0 & \mathbf b_a \newline 0 & 0 & 0 \newline 0 & 0 & 0 \end{bmatrix} \end{align*} $$

假设 $t_{i-1}$ 是上一帧图像 $\mathcal F_{i-1}$ 的预积分开始时间。我们定义 $t_j$ 为帧 $\mathcal F_{i-1}$ 和帧 $\mathcal F_i$ 之间任意 IMU 测量的时间戳，并让 $t_{j’}$ 为其后续 IMU 测量的时间戳，这样小积分时间 $\delta = t_{j’} - t_j$ 。设 $\xi_{t_j}$ 为时间瞬间 $t_j$ 的扩展姿态。给定 $\eqref{4}$ 并假设 $\bf U$ 和 $\bf B$ 在积分时间 $\delta$ 内恒定，则精确积分为 $$ \xi_{t_{j’}} = \exp (\delta (\mathbf G - \mathbf D )) \xi_{t_j} \exp (\delta (\mathbf U - \mathbf B + \mathbf D )) \tag{5} \label{5} $$ 假设在时间 $t_j$ 和 $t_{i-1}$ 之间有 $N$ 组 IMU 测量值，我们可以得出 $$ \xi_{t_j} = \exp \left( (\sum_{s=0}^N \delta_s) (\mathbf G - \mathbf D) \right) \xi_{t_{i-1}} \prod_{s=0}^N \exp(\delta_s (\mathbf U_s - \mathbf B_s + \mathbf D)) \tag{6}\label{6} $$ 定义 $\mathcal T = \sum_{s=0}^N \delta_s$ ，并重新排列 $\eqref{6}$ 得到 $$ \xi_{t_{i-1}}^{-1} \exp (-\mathcal T(\mathbf G - \mathbf D)) \xi_{t_j} = \prod_{s=0}^N \exp(\delta_s (\mathbf U_s - \mathbf B_s + \mathbf D)) \tag{7}\label{7} $$ 左边的指数等于 $$ \exp(-\mathcal T (\mathbf G - \mathbf D)) = \begin{bmatrix} \bf I & -\frac12 \mathcal T^2 \mathbf g & - \mathcal T \mathbf g \newline 0 & 1 & 0 \newline 0 & \cal T & 1 \end{bmatrix} \tag{8} \label{8} $$ 将 $\eqref{8}$ 代入 $\eqref{7}$ ， $\eqref{7}$ 的左侧成分与 ⁵ 中方程 (33) 的中间部分完全相同，右侧是我们新推导出的预积分项，其中每个指数都可以展开为 $$ \exp(\delta_s (\mathbf U_s - \mathbf B_s + \mathbf D)) = \begin{bmatrix} \exp(\delta_s (\boldsymbol\omega_s - \mathbf b_{\omega_s})^\wedge) & \mathbf J_2 (\mathbf a_s - \mathbf b_{a_s}) & \mathbf J_1 (\mathbf a_s - \mathbf b_{a_s}) \newline 0 & 1 & 0 \newline 0 & \delta_s & 1 \end{bmatrix} \tag{9} \label{9} $$ 其中，参考⁶ 中 $\rm SO(3)$ 中雅可比的闭式解， $$ \begin{align*} \theta &= \lVert \tilde{\boldsymbol\omega} \rVert, \qquad \tilde{\boldsymbol\omega} = \boldsymbol\omega_s - \mathbf b_{\omega_s} \tag{10} \label{10} \newline \mathbf J_1 &= \delta_s \mathbf I + \frac1{\theta^2} (1 - \cos(\delta_s \theta)) \tilde{\boldsymbol\omega}^\wedge + \frac1{\theta^3} (\delta_s\theta - \sin(\delta_s\theta))(\tilde{\boldsymbol\omega}^\wedge)^2 \tag{11}\label{11} \newline \mathbf J_2 &= \frac12 \delta_s^2 \mathbf I + \frac1{\theta^3} (\delta_s \theta - \sin(\delta_s \theta)) \tilde{\boldsymbol\omega}^\wedge + \frac1{\theta^4}(\frac12\delta_s^2\theta^2 + \cos(\delta_s \theta) - 1)(\tilde{\boldsymbol\omega}^\wedge)^2 \tag{12} \label{12} \end{align*} $$ 迭代预积分的最终结果为 $$ \begin{cases} \Delta \mathbf R_{t_{j’}}^{t_i} &=& \Delta \mathbf R_{t_j}^{t_i} \exp(\delta_j (\boldsymbol \omega - \mathbf b_{\omega_j})^\wedge) \newline \Delta \mathbf p_{t_{j’}}^{t_i} &=& \Delta \mathbf p_{t_j}^{t_i} + \delta_j \Delta \mathbf v_{t_j}^{t_i} + \Delta \mathbf R_{t_j}^{t_i} \mathbf J_2 (\mathbf a_j - \mathbf b_{a_j}) \newline \Delta \mathbf v_{t_{j’}}^{t_i} &=& \Delta \mathbf v_{t_j}^{t_i} + \Delta \mathbf R_{t_j}^{t_i} \mathbf J_1(\mathbf a_j - \mathbf b_{a_j}) \end{cases} \tag{13} \label{13} $$ 请注意，在欧拉积分方案下，雅可比项 $\mathbf J_1$ 和 $\mathbf J_2$ 简化为 $\mathbf J_1 = \delta_s \bf I$ 和 $\mathbf J_2 = \frac12 \delta_s^2 \bf I$ 。

在实际应用中，无噪声项是不可用的，因此用相应的估计值代替，用 $\hat{(\cdot)}$ 符号表示。然后，考虑到前一个估计值的不确定性和测量噪声，需要将处理协方差传播问题。误差项定义如下

$$ \begin{align*} \mathbf e_ {ij’} &= [ (\mathbf e_ {ij’}^{\Delta \bf R})^\top, (\mathbf e_ {ij’}^{\Delta \bf p})^\top, (\mathbf e_ {ij’}^{\Delta \bf v})^\top, (\mathbf e_ {ij’}^{\mathbf b_\omega})^\top, (\mathbf e_ {ij’}^{\mathbf b_a})^\top]^\top \tag{14} \label{14} \newline \mathbf e_ {ij’}^{\Delta \bf R} &= \log(\Delta \mathbf R_ {t_ {j’}}^{t_i} {}^\top \Delta \hat {\mathbf R}_ {t_ {j’}}^{t_i} )^\vee \quad \sim \quad \mathcal N(0, \Sigma_ {ij’}^{\bf R}) \in \mathbb R^3 \tag{15.a} \label{15.a} \newline \mathbf e_ {ij’}^{\Delta \bf p} &= \Delta \hat{\mathbf p}_ {t_ {j’}}^{t_i} - \Delta \mathbf p_ {t_ {j’}}^{t_i} \quad \sim \quad \mathcal N(0, \Sigma_ {ij’}^{\bf p}) \in \mathbb R^3 \tag{15.b} \label{15.b} \newline \mathbf e_ {ij’}^{\Delta \bf v} &= \Delta \hat{\mathbf v}_ {t_ {j’}}^{t_i} - \Delta \mathbf v_ {t_ {j’}}^{t_i} \quad \sim \quad \mathcal N(0, \Sigma_ {ij’}^{\bf v}) \in \mathbb R^3 \tag{15.c} \label{15.c} \newline \mathbf e_ {ij’}^{\Delta \mathbf b_\omega} &= \hat{\mathbf b}_ {\omega_j} - \mathbf b_ {\omega_j} \quad \sim \quad \mathcal N(0, \Sigma_ {ij’}^{\mathbf b_\omega}) \in \mathbb R^3 \tag{15.d} \label{15.d} \newline \mathbf e_ {ij’}^{\Delta \mathbf b_a} &= \hat{\mathbf b}_ {a_j} - \mathbf b_ {a_j} \quad \sim \quad \mathcal N(0, \Sigma_ {ij’}^{\mathbf b_a}) \in \mathbb R^3 \tag{15.e} \label{15.e} \end{align*} $$

由此推导出预积分项演化的矩阵表示

$$ \mathbf e_{ij’} = \mathbf A_{j’} \mathbf e_{ij} + \mathbf B_{j’} \mathbf n_{j’} \tag{16} \label{16} $$

其中，

$$ \begin{gather*} \mathbf A = \begin{bmatrix} \exp(-\delta_ {j’}(\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {w_ {j’}})^\wedge) & 0 & 0 & -\delta_ {j’} \bf I & 0 \newline -\Delta \hat{\bf R}_ {t_j}^{t_i} (\hat{\bf J}_ 2 (\hat{\bf a}_ {j’} - \hat{\bf b}_ {a_ {j’}}))^\wedge & \bf I & \delta_ {j’} \bf I & \mathbf D_1 & - \Delta \hat{\bf R}_ {t_j}^{t_i} \hat{\bf J}_ 2 \newline -\Delta \hat{\bf R}_ {t_j}^{t_i} (\hat{\bf J}_ 1 (\hat{\bf a}_ {j’} - \hat{\bf b}_ {a_ {j’}}))^\wedge & 0 & \bf I & -\mathbf D_2 & - \Delta \hat{\bf R}_ {t_j}^{t_i} \hat{\bf J}_ 1 \newline 0 & 0 & 0 & \bf I & 0 \newline 0 & 0 & 0 & 0 & \bf I \end{bmatrix} \newline \mathbf B = \begin{bmatrix} \delta_ {j’} \bf I & 0 & 0 & 0 \newline -\mathbf D_1 & \Delta \hat{\bf R}_ {t_j}^{t_i} \hat{\bf J}_ 2 & 0 & 0 \newline \mathbf D_2 & \Delta \hat{\bf R}_ {t_j}^{t_i} \hat{\bf J}_ 1 & 0 & 0 \newline 0 & 0 & -\delta_ {j’} \bf I & 0 \newline 0 & 0 & 0 & -\delta_ {j’} \bf I \end{bmatrix}, \quad \mathbf n_ {j’} = \begin{bmatrix} \mathbf n_ {\omega_ {j’}} \newline \mathbf n_ {a_ {j’}} \newline \boldsymbol\tau_ {\omega_ {j’}} \newline \boldsymbol\tau_ {a_ {j’}} \end{bmatrix} \end{gather*} \tag{17} \label{17} $$

有

$$ \begin{align*} \mathbf D_1 &= \Delta \hat{\bf R}_ {t_j}^{t_i} \left( \frac{\delta_ {j’}{}^2}{\lVert \hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}} \rVert^2} \Big( ((\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}})^\wedge (\hat{\bf a}_ {j’} - \hat{\bf b}_ {a_ {j’}}))^\wedge + (\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}})^\wedge (\hat{\bf a}_ {j’} - \hat{\bf b}_ {a_ {j’}})^\wedge \Big) \right. \newline &\quad \left. + \frac{2\delta_ {j’}{}^2}{\lVert \hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}} \rVert^4} \Big( ((\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}})^\wedge)^2 (\hat{\bf a}_ {j’} - \hat{\bf b}_ {a_ {j’}})(\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}})^\top \Big)\right) \tag{18} \label{18} \newline \mathbf D_2 &= \Delta \hat{\bf R}_ {t_j{}’}^{t_i} \lVert \hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}} \rVert^4 \delta_ {j’}{}^2 (\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}})^\wedge (\hat{\bf a}_ {j’} - \hat{\bf b}_ {a_ {j’}})(\hat{\boldsymbol\omega}_ {j’} - \hat{\bf b}_ {\omega_ {j’}})^\top \tag{19} \label{19} \end{align*} $$

这里的 $\hat{\boldsymbol\omega}$ 和 $\hat{\bf a}$ 是有偏差、有噪声和内在补偿的 IMU 测量值。

3. 工程实现

根据上文中的公式与推到，易将具有 IMU 固有补偿 (scale-misalignment 模型) 和自同构指数函数的 IMU 预积分公式移植到 VINS 系列开源算法中。个人的代码实现见 LSXiang/VINS-Fusion : This is an extended version of VINS-Fusion and migrates to ROS2, adding RGB-D mode, IMU preintegration in the form of SE2(3), line features and gpu acceleration 。

Reference