基于IMU/视觉融合的导航定位算法研究

董伯麟,柴 旭

(合肥工业大学 机械工程学院,安徽 合肥 230009)

0 引言

近年来，随着无人驾驶、虚拟现实等应用的空前发展，同时定位与地图构建(SLAM)[1]被认为是实现这些应用的关键技术，所以SLAM也逐步从实验室研究阶段迈向市场应用。SLAM可描述为机器人搭载某种传感器，在无场景信息情况下，在运动过程中进行自主定位同时构建增量式环境地图。自1986年提出SLAM概念以来，它一直是机器人领域的热点问题。

2007年，A.J.Davison等[2]提出的MonoSLAM是第一个实时单目视觉SLAM系统，它以特征点法为前端，以扩展卡尔曼滤波(EKF)为后端，其应用范围较小，且易遇到特征丢失的情况。Klein等[3]提出的同步跟踪与建图(PTAM)实现了定位与建图的并行化处理，且首次使用非线性优化作为后端方案，引入关键帧机制，PTAM的缺陷与MonoSLAM类似。ORB-SLAM[4]作为现代较完善易用的系统，它支持多种传感器模式，使用了三线程模式保证轨迹与地图的一致性，ORB特征的计算非常耗时，地图只能满足定位需求。J.Engle等[5]提出的LSD-SLAM不需计算特征就可构建半稠密地图，但也继承了直接法的缺点，对光照敏感，在相机快速运动时易丢失。Forster等[6]提出了半直接法视觉里程计(SVO)，与其他方案相比，速度极快，实时性较好，但无建图功能，位姿估计存在累积误差，也不能进行重定位。M.Labbe等[7]提出的RTAB-MAP是深度相机(RGB_D) SLAM中的经典方案，它提供了定位和建图功能。

上述基于视觉传感器的SLAM方案由于其传感器和算法原因在实际应用中具有局限性。本文基于多传感器融合将不同传感器数据融合起来，让各种传感器的优势互补，从而提高SLAM系统的精度和鲁棒性，设计了基于RGB-D与惯性测量单元(IMU)融合的多传感器SLAM方案。

1 传感器工作原理

1.1 IMU传感器导航工作原理

IMU是一种惯性传感器，它可利用传感器特性获取移动机器人载体自身的运动信息(如位置、姿态(俯仰角、偏航角、滚动角)、速度等)。由于最近几年基于视觉惯性里程计(VIO)SLAM技术方案成为SLAM研究的热点，IMU[8]也开始在组合导航系统中发挥重要作用。

本文采用的六自由度IMU主要由三轴加速度计和三轴陀螺仪组成。加速度计可测量惯性系统的三轴线加速度，进行一次积分可得到速度，二次积分可计算出位移。陀螺仪可测量系统的三轴角速度，通过一次积分获得系统转过的角度。图1为IMU传感器导航功能示意图。其中加速度计和陀螺仪测量值都是基于IMU载体坐标系下的数据，需通过坐标系变换和积分运算得到在世界坐标系中的位姿。加速度计在惯性导航系统中主要起定位及修正姿态的作用，而陀螺仪负责姿态解算及辅助定位。

图1 IMU传感器导航功能示意图

1.2 双目结构光RGB-D工作原理

在视觉SLAM中，与基于单目或双目传感器的SLAM系统相比，RGB-D由于自身结构(通过结构光技术或时间飞行原理)可主动获取图像像素的景深信息，没有单目尺度不确定性和双目通过视差法计算深度导致计算量复杂的缺陷，RGB-D已广泛使用在SLAM系统中。目前，RGB-D根据原理可分为双目方案、结构光方案及飞行时间(TOF)方案。本文使用的RGB-D是基于“双目+结构光”原理，其继承了两种方案的优势。图2为结构光成像原理图。投射器通过对发出的结构光编码投射到物体表面，在其表面形成调制的光栅三维图像，摄像机捕获此信息获得光栅二维图像。由于投射器和摄像机在相机中位置固定，光栅显示的位移与物体表面的深度信息对应，由光栅二维图像便可构建待测物体的三维轮廓。

图2 结构光成像原理图

图3为本文采用的RGB-D。RGB摄像头可直接获得应用场景的彩色图像，双目红外摄像头发射的红外结构光遇到物体表面会反射获得与彩色图像对应的深度数据。与其他RGB-D相比，本文所用相机光学增强系统的作用是：普通双目方案在室内光线较暗或被测对象特征不明显的场景下鲁棒性较差；而结构光方案虽然适应普通弱光环境，但在强光下，由于其他红外线的干扰导致测量效果受损。光学增强系统可主动调整系统亮度和图像传感器的曝光参数，对光照实现自适应，在室内外能很好的识别物体，获得更多的景深细节信息，优化成像效果。

图3 双目结构光RGB-D

2 IMU-RGBD算法设计

2.1 算法框架图

图4为本文算法流程图。设计了基于扩展卡尔曼滤波的IMU信息与RGB-D特征融合的方式来解决视觉SLAM所存在的问题。由RGB-D得到场景的彩色图和深度图，包含特征信息和深度数据(Depth)，两种数据对应便可得三维空间信息，也即一个3D点。后续通过特征提取与匹配及RANSAC算法将错误匹配点对删除，最后由迭代最近点算法(ICP)求出相邻两帧3D点间的位姿变信息(旋转矩阵R和平移向量t)。同时，由IMU所测加速度和角速度数据，通过预积分使IMU数据频率与相机获取图像的频率一致，之后由数据处理计算出IMU的位姿信息(包括位置P、速度v及姿态四元数Q)。

图4 本文算法框架图

2.2 RGB-D位姿估计

RGB-D采集到的数据为空间3D点经过相机成像后生成的2D图像，在综合评价尺度不变特征变换(SIFT)、加速鲁棒特征(SURF)、ORB 3种特征点在相机运动、环境光照变化及计算量带来的实时性问题后，选择了ORB特征点作为检测算子。

在得到匹配的特征点对后，通过迭代最近点初步估计相机的运动，得到场景的局部结构。

设pk为空间中一点，pk+1为该点在相机视频流中下一帧所在位置，那么位姿变换可看作是对于∀k，均有pk+1=Rpk+t，构建误差项ek=pk+1-(Rpk+t)，构建最小二乘法使误差项ek最小，从而达到3D特征点匹配精度最高的目的，即

(1)

根据R、t及相机的内、外部参数对RGB图像和深度图像进行处理，即可把点云加起来得到点云地图。但由于视觉相机总会遇到特征丢失的情况(如相机快速运动时，纯旋转运动，场景纹理复杂等)导致SVO所估计的相机运动过程不完整和定位不精确，因此，本文在此基础上加入IMU传感器进行辅助定位。

2.3 IMU位姿估计

IMU的测量模型为

(2)

IMU作为惯性传感器用来测量物体的角速度和加速度，对短时间内的快速运动很敏感。因此，其工作频率很高才能进行测量。通常IMU采集数据的频率与RGB-D采集图像的频率不一致，本文所用的IMU采集数据为100 Hz，而RGB-D采集图像频率为30 Hz。为了使相机在某时刻采集的图像能与此时刻IMU数据对应，需将此时间段内的IMU数据积分，预积分示意图如图5所示。根据IMU预积分理论模型，设i与j为相机在第k、k+1帧图像时所对应的IMU数据时间段，时间间隔为Δt，在已知i时刻的IMU状态量的情况下，则有：

(3)

图5 预积分示意图

2.4 基于扩展卡尔曼滤波(EKF)的RGB-DSLAM与IMU数据融合

IMU惯性导航系统的状态误差是进行(EKF)融合的基础，在IMU的测量模型中定义系统的误差状态方程为

(4)

定义EKF中的状态向量为

x=[p,v,Q,R,t]

(5)

在解算机器人运动状态过程中可获得两种数据：控制数据uk记录自身运动的传感器数据(如IMU)；观测数据zk记录环境信息的传感器数据(如相机)。假设系统状态满足马尔可夫性，采用运动方程和观测方程来描述系统的运动过程，系统的运动方程和观测方程定义为

(6)

现将系统的状态在k-1时刻进行一阶泰勒展开，进行线性化处理，有：

(7)

式中：F为状态转移矩阵;H为测量矩阵。

在EKF的预测部分中的测量状态向量为

(8)

协方差矩阵为

(9)

式中Q为过程噪声。在进行状态更新前，定义卡尔曼增益K为

(10)

式中S为测量噪声矩阵。

在卡尔曼增益下，更新后的系统状态向量为

(11)

更新后的系统协方差矩阵为

(12)

式中E为单位矩阵。

系统的整个运动状态过程是根据运动方程上一个时刻状态和当前时刻运动输入来估计预测当前状态。在视觉SLAM中，由于没有运动输入，一般是构建重投影误差进行运动优化估计。在多传感器数据融合中加入了其他传感器数据进行联合估计，可对系统状态进行更好的预测。

3 实验结果与分析

本文是在ORB-SLAM2的基础上融合IMU数据，为了验证本文算法的效果，使用EuRoC数据集进行验证。这个数据集是用微型飞行器在两个不同的房间和工业厂房中飞行，根据光照、纹理、运动快慢和运动模糊，将序列分为esay、medium和difficult。采集到的视觉惯性数据包括相机图片及同步的IMU数据，高精度外部运动捕捉系统采集到相机真实位姿(groundtruth)。

为了评估本文算法的定位精度和运行效率，在评估定位精度时，使用算法估计的移动机器人位姿和外部运动捕捉系统采集到的相机真实位姿的均方根误差(RMSE)来衡量。均方根误差计算式为

(13)

在4个不同场景和难度的数据集上进行测试，并与ORBSLAM2对比，结果如表1所示。

表1 均方根误差与运行时间对比

均方根误差值越小，说明估计轨迹与真实轨迹误差越小，则定位精度越高。表1表明了本文算法在4种场景下定位精度比ORB SLAM2均提高，这是由于融合了IMU数据能在某些场景下弥补视觉数据的缺陷。在运行效率上，与ORB SLAM2相比，其运行时间整体增加25%，这是由于加入了松耦合，算法复杂度提升，导致运行时间提高。

图6为算法在VR1_01数据集上的SVO效果图。由图可看出，房间左边、前面及右边由特征点组成的三面墙体较直观地展示了系统建图的效果。

图6 VR1_01数据集构图效果

4 结束语

本文提出了一种基于ORBSLAM2的改进SLAM算法，融合了惯性传感器IMU测量的数据，对RGB-D的位姿估计的结果进行扩展卡尔曼滤波(EKF)融合，提升了视觉里程计的定位精度。但是由于采用的是基于松耦合数据融合方法，在运行效率上有所降低，后续可以考虑采用更好的数据融合方案来提升算法的时性，如采用直接法融合IMU无需计算特征点从而减少算法计算时间。