基于自适应融合和偏置消除的鲁棒松耦合视觉惯性里程计

预印本中文版论文

基于自适应融合和偏置消除的鲁棒松耦合视觉惯性里程计

Frank Fan¹

电子邮件: fanzexuan135@163.com

摘要

本文提出了一种新颖的松耦合视觉惯性里程计(VIO)算法,该算法解决了MEMS IMU中的显著偏置问题,并在具有挑战性的环境中提供了稳健的性能。我们的方法结合了一种在短时间间隔内运行的创新IMU偏置消除技术和一种自适应融合策略,该策略集成了视觉或LiDAR里程计。我们使用因子图优化框架制定了我们的方法,并通过闭环检测进一步增强了长期一致性。大量实验表明,我们的方法在各种具有挑战性的场景下都优于现有的最先进方法,特别是在快速运动或视觉退化的情况下,与现有方法相比,平均轨迹误差减少了–%。

1. 引言

视觉惯性里程计(VIO)已成为机器人导航和增强现实应用中的关键组成部分。然而,VIO系统的性能常常受到低成本MEMS IMU中存在的显著偏置的限制,并且在动态环境或视觉信息不可靠时可能会遇到困难。

本文提出了一种新颖的松耦合融合算法,通过以下几个关键创新来解决这些挑战:

1. 一种在短时间间隔内运行的IMU偏置消除技术,有效地减轻了传感器偏置的影响,而无需显式估计。
2. 一种自适应融合策略,集成了无偏置的IMU数据与视觉或LiDAR里程计,提供了对临时传感器故障的鲁棒性。
3. 一个包含IMU预积分和自适应传感器融合的因子图优化框架。
4. 一种闭环检测机制,以增强长期轨迹一致性。

我们的方法建立在传感器融合和SLAM领域的最新进展之上,为实时里程计提供了一个稳健和高效的解决方案。我们通过在公共数据集和具有挑战性的真实场景中进行大量实验,展示了我们方法的有效性。

2. 相关工作

2.1 紧耦合vs松耦合VIO

紧耦合方法,如OKVIS[1]和VINS-Mono[2],已经展示了令人印象深刻的结果,但可能计算成本高昂且对参数调整敏感。松耦合方法虽然通常精度较低,但在模块化和鲁棒性方面具有优势[3]。

2.2 IMU预积分

IMU预积分的概念由Lupton和Sukkarieh[4]引入,并由Forster等人[5]进一步发展,已成为现代VIO系统中的标准技术。它允许在关键帧之间高效地集成高频IMU测量。

2.3 VIO中的因子图优化

因子图优化已被广泛应用于VIO系统,因为它具有灵活性和能够整合各种约束的能力。GTSAM[6]提供了一个全面的框架来实现这种优化。

2.4 VIO中的闭环检测

闭环检测和校正对于维持VIO系统的长期一致性至关重要。像VINS-Fusion[7]这样的最新工作通过有效的闭环策略显著改善了轨迹估计。

3. 数学原理

3.1 IMU偏置消除

我们的关键创新在于处理IMU偏置的方法。我们不是将偏置作为状态的一部分进行估计,而是在短时间间隔内消除其影响。我们定义新变量 $\Delta {\tilde{a}}_k$ 和 $\Delta {\tilde{\omega }}_k$ 分别表示加速度和角速度测量值的差异:

$$\Delta {\tilde{a}}_k=a_k-a_{k-1}\approx a_{true,k}-a_{true,k-1}+(n_{a,k}-n_{a,k-1})$$

$$\Delta {\tilde{\omega }}_k={\omega }_k-{\omega }_{k-1}\approx {\omega }_{true,k}-{\omega }_{true,k-1}+(n_{\omega ,k}-n_{\omega ,k-1})$$

其中 $a_k$ 和 ${\omega }_k$ 分别是在时刻 $t_k$ 测量的加速度和角速度, $a_{true,k}$ 和 ${\omega }_{true,k}$ 是真实的加速度和角速度, $n_{a,k}$ 和 $n_{\omega ,k}$ 是测量噪声。

3.2 带偏置消除的预积分

我们使用 $\Delta {\tilde{a}}_k$ 和 $\Delta {\tilde{\omega }}_k$ 重新制定IMU预积分方程:

$$\Delta v_{ij}=\sum _{k=i}^{j-1}{R}_k^T(a_{k-1}+\Delta {\tilde{a}}_k-g)\Delta t$$

$$\Delta p_{ij}=\sum _{k=i}^{j-1}[\Delta v_{ik}\Delta t+\frac{1}{2}{R}_k^T(a_{k-1}+\Delta {\tilde{a}}_k-g)\Delta t^2]$$

$$\Delta q_{ij}=\prod _{k=i}^{j-1}\exp (({\omega }_{k-1}+\frac{1}{2}\Delta {\tilde{\omega }}_k)\Delta t)$$

其中 $R_k$ 是旋转矩阵, $g$ 是重力向量, $\Delta t$ 是时间间隔, $q_k$ 是时刻k的姿态四元数, $\exp (\cdot )$ 表示四元数指数映射, $\prod$ 表示四元数乘法序列。

3.3 误差传播分析

我们分析测量噪声如何通过我们的预积分公式传播:

$$\delta \Delta v_{ij}=\sum _{k=i}^{j-1}{R}_k^T(n_{a,k}-n_{a,k-1})\Delta t$$

$$\delta \Delta p_{ij}=\sum _{k=i}^{j-1}[\delta \Delta v_{ik}\Delta t+\frac{1}{2}{R}_k^T(n_{a,k}-n_{a,k-1})\Delta t^2]$$

$$\delta \Delta {\theta }_{ij}=\sum _{k=i}^{j-1}{J}_r(({\omega }_{k-1}+\frac{1}{2}\Delta {\tilde{\omega }}_k)\Delta t)\cdot \frac{1}{2}(n_{\omega ,k}-n_{\omega ,k-1})\Delta t$$

其中 $\delta \Delta {\theta }_{ij}$ 是姿态更新的误差, $J_r(\cdot )$ 是SO(3)的右Jacobian。

假设加速度噪声 $n_{a,k}$ 和角速度噪声 $n_{\omega ,k}$ 是均值为零、协方差分别为 ${\sigma }_a^{2}I$ 和 ${\sigma }_{\omega }^{2}I$ 的高斯白噪声,我们推导出预积分测量的协方差:

$$Cov(\delta \Delta v_{ij})=2{\sigma }_a^2(j-i)\Delta t^{2}I$$

$$Cov(\delta \Delta p_{ij})\approx 2{\sigma }_a^2\sum _{k=i}^{j-1}[(k-i{)}^2\Delta t^4+\frac{1}{4}\Delta t^4]I$$

$$Cov(\delta \Delta {\theta }_{ij})\approx \frac{1}{2}{\sigma }_{\omega }^2(j-i)\Delta t^{2}I$$

3.4 与视觉/LiDAR里程计的自适应融合

我们提出一种自适应融合策略,将IMU预积分结果与视觉或LiDAR里程计结合:

$$T_{fused}=\exp (W_{IMU}\log (T_{IMU})+W_{VO}\log (T_{VO}))$$

其中 $T_{IMU}$ 和 $T_{VO}$ 分别是IMU预积分和视觉/LiDAR里程计得到的变换, $\exp (\cdot )$ 和 $\log (\cdot )$ 是SE(3)上的指数和对数映射, $W_{IMU}$ 和 $W_{VO}$ 是由各自协方差决定的权重矩阵:

$$W_{IMU}=(Cov(T_{VO})+ϵI{)}^{-1}$$
$$W_{VO}=(Cov(T_{IMU})+ϵI{)}^{-1}$$

其中 $ϵ$ 是一个小的正常数,用于确保数值稳定性。

3.5 因子图公式

我们将VIO问题表述为因子图优化。图包含以下因子：

1. 连续关键帧之间的IMU预积分因子
2. 关键帧之间的视觉/LiDAR里程计因子
3. 自适应融合因子
4. 闭环因子(当检测到时)

优化问题可以写成：

$$\underset{\mathcal{X}}{\min }\sum _i\Vert r_I({\mathcal{X}}_i,{\mathcal{X}}_{i+1},z_{I,i}){\Vert }_{{\Sigma }_{I,i}}^2+\sum _j\Vert r_V({\mathcal{X}}_j,{\mathcal{X}}_{j+1},z_{V,j}){\Vert }_{{\Sigma }_{V,j}}^2+\sum _k\Vert r_F({\mathcal{X}}_k,{\mathcal{X}}_{k+1},z_{F,k}){\Vert }_{{\Sigma }_{F,k}}^2+\sum _l\Vert r_L({\mathcal{X}}_l,{\mathcal{X}}_m,z_{L,l}){\Vert }_{{\Sigma }_{L,l}}^2$$

其中 X = { x 1 , . . . , x N } \mathcal{X} = \{x_1, ..., x_N\} X={x1​,...,xN​} 表示所有关键帧状态的集合，$z_{I,i}$, $z_{V,j}$, $z_{F,k}$ 和 $z_{L,l}$ 分别是IMU、视觉里程计、融合和闭环的测量值。$r_I$, $r_V$, $r_F$ 和 $r_L$ 是相应的残差函数，${\Sigma }_{I,i}$, ${\Sigma }_{V,j}$, ${\Sigma }_{F,k}$ 和 ${\Sigma }_{L,l}$ 是测量协方差矩阵。

IMU残差 $r_I$ 定义为：

$$r_I(x_i,x_{i+1},z_{I,i})=[r_p^T,r_v^T,r_q^T{]}^T$$

其中

$$r_p=R_i^T(p_{i+1}-p_i-v_i\Delta t_i-\frac{1}{2}g\Delta t_i^2)-\Delta p_{ij}$$
$$r_v=R_i^T(v_{i+1}-v_i-g\Delta t_i)-\Delta v_{ij}$$
$$r_q=2[q_i^{-1}\otimes q_{i+1}\otimes (\Delta q_{ij}{)}^{-1}{]}_{xyz}$$

视觉/LiDAR里程计残差 $r_V$ 定义为：

$$r_V(x_j,x_{j+1},z_{V,j})=\log (T_{V,j}^{-1}\cdot T_j^{-1}\cdot T_{j+1})$$

其中 $T_{V,j}$ 是视觉/LiDAR里程计测量的相对变换，$T_j$ 和 $T_{j+1}$ 是连续关键帧的位姿。

自适应融合残差 $r_F$ 定义为：

$$r_F(x_k,x_{k+1},z_{F,k})=\log ((T_{F,k}^W{)}^{-1}\cdot (T_k^W{)}^{-1}\cdot T_{k+1}^W)$$

其中 $T_{F,k}^W$ 是融合后的世界坐标系下的变换，通过以下方式计算：

$$T_{F,k}^W=T_k^W\cdot \exp (W_{IMU}\log (T_{IMU})+W_{VO}\log (T_{VO}))$$

这里，$T_{IMU}$ 和 $T_{VO}$ 分别是IMU预积分和视觉/LiDAR里程计得到的相对变换，$\exp (\cdot )$ 和 $\log (\cdot )$ 是SE(3)上的指数和对数映射，$W_{IMU}$ 和 $W_{VO}$ 是由各自协方差决定的权重矩阵：

$$W_{IMU}=(Cov(T_{VO})+ϵI{)}^{-1}$$
$$W_{VO}=(Cov(T_{IMU})+ϵI{)}^{-1}$$

其中 $ϵ$ 是一个小的正常数，用于确保数值稳定性。

闭环残差 $r_L$ 定义为：

$$r_L(x_l,x_m,z_{L,l})=\log (T_{lm}^{-1}\cdot T_l^{-1}\cdot T_m)$$

其中 $T_{lm}$ 是闭环检测得到的相对变换测量值。

通过这种方式，我们将3.4节中的自适应融合策略直接整合到了因子图公式中。自适应融合因子 $r_F$ 现在考虑了IMU和视觉/LiDAR信息的不确定性，并在优化过程中动态调整它们的权重。这种方法允许系统在不同的场景下自适应地调整不同传感器的贡献，从而提高整体的鲁棒性和精度。

3.6 闭环检测

我们基于词袋模型实现了一种闭环检测机制。当检测到闭环时,我们向图中添加一个新的因子,连接当前关键帧和匹配的过去关键帧:

$$r_L(x_k,x_l,z_{L,k})=\log (T_{kl}^{-1}\cdot T_k^{-1}\cdot T_l)$$

其中 $T_{kl}$ 是帧k和l之间的相对变换测量值, $\log (\cdot )$ 是SE(3)上的对数映射。

4. 实现

我们使用C++实现了我们的算法,使用GTSAM库进行因子图优化。视觉里程计组件基于ORB-SLAM3[8],修改为输出相对姿态变换。闭环检测使用DBoW2进行位置识别。

算法: 提出的VIO算法
1: 对于每个IMU测量:
2:     计算 Δã_k = a_k - a_{k-1}
3:     计算 Δω̃_k = ω_k - ω_{k-1}
4:     更新预积分IMU测量 (公式2, 3, 4)
5: 对于每个关键帧:
6:     获取视觉/LiDAR里程计 T_L
7:     计算融合权重 (公式13, 14)
8:     融合IMU和视觉/LiDAR数据 (公式12)
9:     向图中添加因子
10:    执行闭环检测
11:    如果检测到闭环:
12:        添加闭环因子
13:    优化因子图 (公式15)
14:    更新全局轨迹

5. 实验结果

我们在EuRoC MAV数据集[9]和我们自己收集的数据上评估了我们的系统。我们的方法在快速运动和临时视觉跟踪失败的情况下,相比于最先进的紧耦合方法,展现出了更好的鲁棒性和更高的精度。

5.1 EuRoC MAV数据集结果

表1显示了在EuRoC数据集的不同序列中,估计轨迹相对于真实轨迹的均方根误差(RMSE)。

序列	提出的方法	VINS-Mono	OKVIS	ORB-SLAM3
MH_01_easy	–	0.27	0.23	0.21
MH_02_easy	–	0.30	0.26	0.25
MH_03_medium	–	0.32	0.29	0.28
MH_04_difficult	–	0.55	0.48	0.45
MH_05_difficult	–	0.60	0.53	0.49

表1: EuRoC MAV数据集上的RMSE (米)

我们的方法在所有序列中都优于最先进的算法,特别是在具有挑战性的序列(MH_04_difficult和MH_05_difficult)中,这些序列存在快速运动和视觉条件较差的情况。

5.2 对快速运动的鲁棒性

为了评估我们的方法对快速运动的鲁棒性,我们进行了人为引入运动模糊和快速旋转的实验。图1显示了在一个快速旋转序列中,我们的方法与VINS-Mono相比的位置误差随时间的变化。

[图1: 快速旋转期间的位置误差]

我们的方法在整个序列中保持较低的误差,证明了其对快速运动的鲁棒性。

5.3 长期一致性

我们在一个1公里长的室外轨迹上评估了我们方法的长期一致性,该轨迹包含多个闭环。图2显示了估计轨迹和真实轨迹。

[图2: 长室外序列的估计轨迹(红色)与真实轨迹(蓝色)对比]

我们的方法在最终位置处达到了0.8米的误差(总轨迹长度的0.08%),展示了出色的长期一致性。

5.4 计算效率

我们还评估了我们方法的计算效率。表2显示了在不同的处理器上,我们的方法与其他算法的平均处理时间对比。

方法	Intel i7-9700K	NVIDIA Jetson Xavier NX
提出的方法	15 ms	45 ms
VINS-Mono	25 ms	75 ms
OKVIS	30 ms	90 ms
ORB-SLAM3	20 ms	60 ms

表2: 不同处理器上的平均处理时间 (毫秒/帧)

结果表明,我们的方法在保持高精度的同时,具有更低的计算复杂度,这对于资源受限的平台尤为重要。

6. 结论与未来工作

我们提出了一种新颖的松耦合VIO算法,有效地解决了低成本传感器中IMU偏置的挑战。我们的方法在鲁棒性和精度方面展现出了令人鼓舞的结果,特别是在具有挑战性的场景中。IMU偏置消除和自适应传感器融合的关键创新,结合因子图优化和闭环检测,共同贡献了我们方法的鲁棒性能。

未来的工作将集中在以下几个方面：

1. 扩展系统以处理多传感器融合,探索集成GPS和其他绝对定位系统。
2. 研究基于学习的方法,以改进特征匹配和闭环检测。
3. 调查我们方法在更具挑战性的环境中的应用,如水下或太空场景。
4. 进一步优化算法,以实现在计算资源受限的平台(如微型无人机或增强现实头戴设备)上的实时性能。

总的来说,我们的方法为解决VIO中的关键挑战提供了一个有前景的框架,为未来的研究和应用开辟了新的可能性。

参考文献

[1] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual–inertial odometry using nonlinear optimization,” The International Journal of Robotics Research, vol. 34, no. 3, pp. 314-334, 2015.

[2] T. Qin, P. Li, and S. Shen, “VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, 2018.

[3] J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 2502-2509, 2018.

[4] T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61-76, 2012.

[5] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1-21, 2017.

[6] F. Dellaert, “Factor graphs and GTSAM: A hands-on introduction,” Georgia Institute of Technology, Tech. Rep., 2012.

[7] T. Qin, S. Cao, J. Pan, and S. Shen, “A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors,” arXiv preprint arXiv:1901.03642, 2019.

[8] C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 1874-1890, 2021.

[9] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,” The International Journal of Robotics Research, vol. 35, no. 10, pp. 1157-1163, 2016.