利用惯导测量单元确定关键帧的实时SLAM算法

卫文乐，金国栋，谭力宁，芦利斌，陈丹琪

（火箭军工程大学核爆侦测工程系，西安710025）

（∗通信作者电子邮箱xxlwei@qq.com）

0 引言

高机动性机器人（微小型无人机等）正广泛应用于情报侦察、电子对抗、目标监视和灾害救援等任务，这些机器人对载荷的要求是重量轻、尺寸小和功耗低等［1］。实时高效的状态估计是机器人完成这些任务的关键，特别是无人机，获得正确的位姿是空中机器人在空中保持稳定的前提。视觉同时定位与 建 图（Visual Simultaneous Localization and Mapping，V-SLAM）是机器人将机载相机作为唯一传感器在未知环境中实时定位，同时构建周边环境地图的一种方法，被普遍应用于视觉导航系统［2-3］。

单纯的V-SLAM 在发生纯旋转或者快速运动时会失效，结合来自摄像机的视觉信息和惯导测量单元（Inertial Measurement Unit，IMU）的视觉惯导同时定位与建图（Visual Inertial-SLAM，VI-SLAM）方法很受欢迎，因为它在全球定位系统（Global Positioning System，GPS）拒绝的环境中表现良好，并且与基于激光雷达的方法相比，只需要一个小巧轻便的传感器封装，使其成为无人机平台的首选技术［4］。

V-SLAM 中的视觉里程计（Visual Odometry，VO）是利用两帧图像信息及其之间的关联性恢复相机的三维运动并获得局部地图，主要方法有直接法和特征点法。直接法通过最小化光度误差来获得相机位姿，如LSD-SLAM（Large-Scale Direct Monocular SLAM）［5］中的VO；特征点法在检测、提取以及匹配图像特征后，最小化重投影误差，得到相机间的位姿变换，例如ORB-SLAM2［6］中的VO；相对于直接法，特征点法对光照变化更加鲁棒，没有小基线运动假设，精度较高，但是需要提取和匹配特征，计算量较大。

VI-SLAM 从融合方式上看分为松耦合和紧耦合。松耦合是分别使用IMU 信息和视觉信息来得到各自位姿变化，再将得到的两个位姿变化进行融合。例如Lynen 等［7］将IMU 输出的位姿信息与通过扩展卡尔曼滤波器进行多传感器融合以得到一个更加鲁棒的位姿。紧耦合需要先把图像特征添加到特征向量中去，而后通过滤波或者优化的方法得到系统的姿态。例如VINS-Mono［8］通过联合优化所有的视觉、惯性信息求解滑窗内的状态来求解相机的位姿。松耦合的融合方式计算精简、易扩展，但是没有考虑传感器间的内在联系，因此精度也不高；紧耦合则由于把图像特征加入到状态向量，整个系统状态向量的维数会非常高，因此也就需要较高的计算量。

VI-SLAM 中IMU 的作用主要有两方面:为帧间的特征匹配提供一个更好的初始值［9-10］；在后端优化中提供更多的有效约束，使得结果更加精确［4，7］。

在实际应用中，从低级别传感器驱动程序到高级规划算法的所有软件组件必须在嵌入式计算机上实时运行。与其他部件共享计算资源，这就给VI-SLAM 算法效率提出了更高的要求，重要的是，不会产生过多的CPU 消耗间歇性峰值。由于基于特征点法的VI-SLAM 在前端对特征点的提取、描述和匹配耗费大量时间，这是其实时性差一个重要原因，因此在保证不丢失的情况下减少前端对该部分处理的频次，对提高系统的实时性、降低嵌入式计算机的处理压力很有意义。

综上所述，本文提出一种利用IMU 确定关键帧的实时SLAM 算法，通过IMU 预积分自适应地确定关键帧，自适应阈值来自视觉惯性紧耦合优化的结果，而后仅对关键帧进行跟踪，以避免对所有帧进行特征处理。

1 IMU和相机模型

1.1 相机针孔模型

相机将三维空间信息投影到二维成像平面的过程可以用一个针孔模型来表示，针孔模型表示了三维空间点在光线的映射下透过针孔之后，在相机成像平面成像的关系。

其中:k1、k2、p1、p2均是相机的畸变参数。则该点在像素平面的投影为:

1.2 IMU预积分模型

如图1 所示，一般情况下IMU 是采样频率要远高于相机的帧率，因此采用预积分模型。该方法对相对测量进行处理，不但能大大提高优化速度，还可通过重力对齐得到系统的绝对姿态。

图1 IMU预积分示意图Fig.1 Schematic diagram of IMU pre-integration

在进行后端的非线性优化时，为了避免姿态估计的初始帧估计导致积分重新计算的情况，本文采用增量式。ΔRijB、ΔvijB和ΔpijB为i时刻到j时刻的IMU 的旋转矩阵、速度和位置的变化量，计算公式如下:

其中:

2 惯导信息确定关键帧的SLAM算法

2.1 系统概述

本文方法是在文献［4］方法的基础上，采用紧耦合的非线性优化的融合方式，同时通过基于词袋的闭环优化来减少累计漂移，使得算法能在大场景中更好地工作。算法的主要改进在于在特征跟踪之前通过IMU 预积分自适应地确定关键帧，而后仅对关键帧进行跟踪，避免了对所有帧进行特征处理。图2 为本文算法的系统结构，同时运行跟踪线程、局部建图线程和闭环线程。

1）跟踪线程:使用IMU预积分的结果来确定关键帧，而后只对关键帧进行特征跟踪。在特征跟踪时，使用关键帧间的IMU 信息提供一个较好的初值完成特征匹配。若未IMU 初始化，则进行视觉惯导对齐。最后进行相邻关键帧间的视觉惯导紧耦合优化，同时为关键帧的确定提供自适应阈值。

2）局部建图线程:利用跟踪线程所提供的关键帧更新共视图和地图点，并剔除掉质量不好地图点，再通过共视程度较高地利用三角化恢复更多的地图点。然后在一个本地窗口中优化最近的N个关键帧及所有被观测到的地图点。最后剔除掉冗余的关键帧。

3）闭环线程:通过词袋模型检测闭环帧，利用刚体变换来检验此匹配的正确性。若闭环检测成功最后执行图优化来减小轨迹中的累积误差。最后将所有的状态作为优化变量来执行全局BA 优化，最小化重投影误差和IMU 误差，来得到一个全局一致的轨迹和地图。

2.2 跟踪线程

跟踪线程负责实时地跟踪传感器的位姿、速度和IMU零偏。

2.2.1 关键帧确定

现有的确定关键帧的方法一般是通过跟踪地图点云的数量和距离上一关键帧的帧数来判断，这种方法不但要对每个当前帧进行特征的提取、描述和匹配，而且需要固定的阈值。本文利用IMU 所提供的信息来确定关键帧，而后只对关键帧进行特征处理，避免了对所有帧进行特征处理。

将第一帧设定为关键帧，第二个关键帧的判断阈值为给定τ1。从第三个关键帧开始判断阈值来自适应阈值τm(m ≥2)，它来自于视觉惯性紧耦合优化的结果，利用前两个关键帧的位姿差来作为选取关键帧的阈值。对于时间戳为tn的n 帧图像，判断关键帧算法如下:

其中:kfm中存放着关键帧的时间戳；imun为tn时刻的IMU 的积分位姿；pn为位置。转角θn可由式（5）推导而来:

关键帧的判断发生在第8）行，位置差和转角差只要有一个达到阈值，则确定为关键帧。而后本文只对关键帧进行特征跟踪，大大减轻了平台的计算压力。

在上述方法所确定的关键帧发生丢失时，则退回到上一关键帧采用ORB-SLAM2 中单纯采用视觉的方法来确定下一关键帧，而后继续采用上述利用IMU信息确定关键帧的方法。

2.2.2 特征跟踪

利用IMU 预积分结果可以得到一个可靠的当前帧位姿，摒弃了ORB-SLAM2的匀速运动模型。一旦预测到相机姿势，就能更快地搜索到上一关键帧中的特征点在地图中对应的点云与当前关键帧图像的匹配点，最后利用匹配点对当前相机的位姿进一步优化。

如果搜索不到特征点，则计算当前关键帧的词袋向量进行全局重定位的初始化位姿。一旦获得了初始相机位姿和一组初始特征匹配点，就可以将更多的地图点投影到图像上以寻找更多匹配点。

2.2.3 视觉惯导对齐

如果整个系统没有初始化，还需要进行视觉惯导对齐，其过程如图3 所示。本文采用松耦合的传感器融合方法来获得初始值，在大多数情况下，纯视觉SLAM 系统可以从相对运动中来估计相机的位姿信息SFM（Structure From Motion）。通过将IMU 预积分与纯视觉SFM 结果进行对齐，可以粗略地恢复出偏置、速度、尺度和重力。如图3 所示，框状模型是相机，五角星为相机观测到的三维路标，最上方的实线是IMU 预积分算出的相机位姿信息。

图3 初始化的视觉惯性对齐过程Fig.3 Initialized visual inertia alignment process

2.2.4 视觉惯导紧耦合优化

ΣI为预积分的信息矩阵；ΣR为零偏的随机游走［11］。本文通过高斯牛顿法求解该优化问题，将优化的结果和海塞矩阵用于下次优化。

如果地图点没有更新，利用当前关键帧k 去优化下一关键帧k + 1，并使用先前优化结束时计算的先验:

其中Eprior( k )为先验项:

其中:

2.3 局部建图线程

局部建图线程在插入来自跟踪线程的关键帧之后执行视觉惯导局部BA。这里优化滑动窗口中N 关键帧和被这N 帧所能观测到的地图点。与ORB-SLAM2不同的是，所有其余可以观测到这些地图点的关键帧不仅通过共视图与滑动窗口连接，还通过IMU信息连接。为了保证局部后端优化的可靠性，滑动窗口的长度N 取10，同时这个长度也兼顾了后端的实时表现。

纯视觉的ORB-SLAM2 后端局部优化是通过优化残差式（11）来得到最优局部位姿和地图点:

3 实验验证与分析

为了验证本文算法的实时性和可行性，对算法的每个部分的耗时进行统计。有效性的验证分为两部分:在公开数据集进行仿真并与优秀算法进行对比；在实验室搭建的无人机平台上进行实验验证。

3.1 算法耗时

PC 平台配置为Intel Core i7-7700HQ CPU @ 2.8 GHz，内存为16 GB，系统为Ubuntu16.04。本文采用了EuRoC 数据集［12］，它采用SkybotixVI 传感器提供一系列的单目及双目相机图片，以及与其对应对齐的IMU 加速度及角速度测量值。使用其中MH-01 进行算法的跟踪实时性检测，统计每帧跟踪所需要的时间，本文算法和VIORB-SLAM 跟踪耗时如图4所示。

从图4 中可以看出，本文算法的跟踪耗时要少于VIORBSLAM 耗时，同时没有随着时间的推移而增加。这主要是本文在2.2.1 节提出的采用了利用IMU 预积分信息确定了关键帧的方法，在没有丢失的情况下对中间帧不进行处理，减少了跟踪的计算量，提高了实时性。同时本文算法在进行实验时会出现了两次跟踪丢失的情况，如图4 中矩形框所示，其发生很大程度是由于初始阈值τ1的选取是个经验值。

同时局部建图和闭环线程的耗时如图5 所示，也能保持在较低的水平。

图4 两种算法的跟踪耗时Fig.4 Tracking time cosumption of two algorithms

图5 本文算法的局部建图和闭环线程耗时Fig.5 Local mapping and loop closing thread time consumption of the proposed algorithm

3.2 公开数据集仿真

为进一步验证本文算法的性能，将本文算法与ORBSLAM、VIORB-SLAM 分别进行了姿态/定位精度的对比评估。本文分别在EuRoC 中3 个难度不同的数据集MH-01、MH-03、MH-05上进行实验。

确定关键帧时初始阈值τ1=［0.4，0.2］为经验值，在较为复杂的场景下可适当减小该阈值，不过随着阈值的减小跟踪线程的计算量会相应增加。图6显示了算法在MH-05数据集上运行的效果图，图像上的点为地图点云，线条为共视图，线上的框形为关键帧的位姿。可以看出2.2.1 节所提算法能自适应地选取关键帧，在MH-05 较复杂的场景没有发生丢失情况。

图6 MH-05数据集仿真界面图Fig.6 Simulation interface of MH-05 dataset

实验结果通过文献［13］算法进行评估，其代码已经开源（https://github. com/uzh-rpg/rpg_trajectory_evaluation）。实验的轨迹对比如图7 所示，可以看出在三种不同难度的场景下本文算法表现较好，实际轨迹与真值基本重合，而单纯基于视觉的ORB-SLAM 绝对轨迹误差较大，尤其是在MH-03 数据集上，而且缺失绝对尺度。

三种方法的均方根误差（Root Mean Square Error，RMSE）对比如表1 所示。与单纯视觉的ORB-SLAM 算法相比，本文算法引入IMU预积分预测当前帧的位姿并在优化中增加了约束，因此定位精度得到了有效提高；在定位精度方面，本文算法与融合IMU信息的VIORB-SLAM算法的表现基本持平。

图7 不同算法的估计轨迹对比Fig.7 Estimated trajectory comparison of different algorithms

图8～9 是本文算法在三个数据集上的结果。从曲线中可以看出:位置漂移不超过±300 mm，且随着场景难度的增加而增大；旋转角误差不超过±2.5°，算法的精度也有明显的改善。

图8 本文算法的旋转角误差Fig.8 Rotation angle error of the proposed algorithm

图9 本文算法的位置漂移Fig.9 Position drift of the proposed algorithm

3.3 实际飞行测试

本文采用Jetson-TX2作为嵌入式处理器，以小觅S1030视觉惯导相机作为传感器，搭载于大疆经纬M100开发平台上进行实际飞行测试。NVIDIA Jetson-TX2 配置为ARM A57 +HMP Dual Denver 双 核 心CPU，内 存 为8 GB，系 统 为Ubuntu16.04。 通 过 机 器 人 操 作 系 统（Robot Operating System，ROS）将PC 作为从机来接收TX2 传过来的位姿和地图数据，并在界面上显示。在PC 上所接收到的数据如图10所示。

表1 各算法的RMSE对比Tab.1 RMSE comparison of different algorithms

图10 从机的ROS界面Fig.10 ROS interface of slave

为了对比由单目SLAM 系统、本文融合系统和IMU 信息估计出的无人机飞行轨迹，操作无人机在室内按照6.5 m×10.5 m 矩形路线飞行一圈。图11 中点划线为单目SLAM 估计出的轨迹，虚线为本文融合系统估计的轨迹，实线为IMU 信息得到得轨迹，双划线为6.5 m×10.5 m 矩形。可以看出利用IMU 信息估计无人机运动时，其导航误差就随时间而快速增长，难以估计出无人机的准确飞行轨迹；利用单目SLAM 系统估计无人机运动时，虽然可以得到无人机在空中飞行的大致轨迹，但是由于单目相机无法得到环境的绝对尺度，所以无法准确地表达无人机在空间中的飞行轨迹；利用本文算法进行视觉惯导组合导航时，能够较准确地估计出无人机飞行的真实轨迹，且能得具有尺度信息的点云地图。

图11 单目SLAM、融合IMU的单目SLAM和IMU信息估计的无人机飞行轨迹Fig.11 UAV trajectories estimated by monocular SLAM，monocular SLAM merging IMU and IMU information

4 结语

本文提出一种利用IMU 确定关键帧的实时SLAM 算法。该算法在跟踪线程时不需要对来自视觉传感器的每帧图像进行特征的提取、描述和匹配以确定关键帧，而是通过IMU预积分自适应地确定关键帧，自适应阈值来自视觉惯性紧耦合优化的结果，而后仅对关键帧进行跟踪，减少了前端对该部分处理的频次，提高了实时性。使用关键帧间的IMU 信息提供一个较好的初值来完成特征匹配，加快了算法收敛的速度。局部建图线程中，将滑动窗口中的视觉和IMU 约束通过视觉惯导光束平差法进行优化，能得到更加准确的位姿状态和环境信息。EuRoC 数据集仿真实验实验表明，算法能在不失精度和鲁棒性的情况下显著减少跟踪线程耗时，降低了VI-SLAM对计算资源的依赖。在实际飞行测试中，算法能够较准确地估计出具有尺度信息的无人机飞行真实轨迹，对于无人机的自主化研究有一定意义。