基于紧耦合的视觉惯性定位方法

卢佳伟，许哲

(上海海洋大学工程学院，上海 201306)

0 引言

结合视觉和惯性测量进行同步定位和地图构建，在智能机器人和计算机视觉领域已经引起广泛关注.通过引入惯性测量单元（IMU）的信息可以很好地弥补单目视觉即时定位与地图构建（SLAM）算法本身存在的一些缺陷.一方面将IMU和相机估计的位姿序列对齐可以得到相机轨迹的真实尺度，而且IMU可以很好地预测出图像帧的位姿，提高特征跟踪算法的匹配速度和应对快速旋转的算法鲁棒性.另一方面IMU中加速度计提供的重力向量可以将估计的位姿转化为实际导航需要的世界坐标系中.

根据相机和IMU数据融合框架的不同，视觉惯性里程计（VIO）可分为紧耦合和松耦合[1].松耦合直接将VIO和惯性导航的输出结果进行融合.典型的方法是S.Weiss等[2-3]提出的基于扩展卡尔曼滤波的SSF、MSF算法，将视觉里程计（VO）处理完的数据与IMU数据进行融合.这两种算法虽然能实时获得航行器的位姿，但是无法纠正视觉测量引入的漂移，所以定位精度没有紧耦合框架的高.而在紧耦合中是使用两个传感器的原始数据共同估计一组变量，传感器的噪声也是相互影响的.虽然紧耦合的算法比较复杂，但充分利用了传感器数据，是目前研究的重点.Mourikis等[4]于2007年提出了一种基于滤波的VIO算法模型多状态约束卡尔曼滤波（MSCKF）.MSCKF利用一个特征点同时约束多个相机位姿，进行卡尔曼滤波更新，但是在跟踪情况下没有使用IMU的先验数据，且无法用到全局的信息进行全局优化，因此系统极易不收敛，导致定位精度较差.Stefan Leutenegge等[5]于2015年提出基于双目相机和IMU的OKVIS算法.该算法通过IMU测量值对当前状态做预测，根据预测进行特征提取与匹配，并将优化中的重投影和IMU的测量误差放在一起做优化.但是由于OKVIS不支持重定位，也没有闭环检测和校正方案，在机器人剧烈运动时容易跟踪失败.香港科技大学沈劭劼团队[6]于2017年开源了一套Visual-Inertial融合定位算法VINS-MOMO，该算法以KTL光流跟踪作为前端，而且具有比OKVIS更加完善和鲁棒的初始化以及闭环检测过程.但是该算法不容易构建全局地图，且视觉约束只靠滑动窗口里的关键帧.

针对上述问题，本文采用将IMU与视觉SLAM两种定位方案在基于非线性的紧耦合结构下相互融合，构建具有较高定位精度的定位系统[7-8].采用VINS-MONO算法，使用单目相机作为视觉传感器与IMU进行组合定位与建图.但是在前端中选用快速特征点提取和描述的算法（ORB）[9]特征，保证了特征子具有旋转，尺度不变性的同时，提取特征点的速度与匹配效率明显提升，从而能够及时的扩展场景，构建全局地图.

1 视觉里程计

在经典的视觉SLAM框架中，VO是通过相邻图像之间匹配的特征点估算出相机的运动并构建局部场景的地图.VO又称前端（Front End），它通过匹配好的特征点计算出相邻图像之间的运动，得到机器人的运动轨迹，从而解决了定位问题.另一方面，根据每个时刻的相机位置，计算出各像素对应空间点的位置，就得到了地图.VO的算法流程如图1所示.

图1 VO算法流程图

1.1 图像特征点提取与匹配

在图像特征中，尺度不变特征转换(SIFT)[10]是考虑得比较完善的一种.它能够适应机器人运动过程中出现的光照及旋转等变化，但是会增加大量的计算.另一些特征，考虑适当地降低精度和鲁棒性，提升计算速度.例如FAST关键点属于计算特别快的一种特征点.而ORB特征可以用来对图像中的关键点快速创建特征向量，这些特征向量可以用来识别图像中的对象.它在FAST检测子不具有方向性的问题上做了改进，并采用速度极快的二进制描述子二元鲁棒独立基本特征（BRIEF），使整个图像特征提取环节大大加速.提取ORB特征分为两个步骤：

1）查找图像中的关键点，并使用FAST算法对关键点进行检测；

2）使用BRIEF算法将关键点变成特征向量.

特征匹配是通过计算特征向量之间的距离来确定路标点前后时间的对应关系.当特征点数量较小时，可使用暴力匹配算法，它计算某一个特征点的特征向量与其他所有特征点的特征向量之间的距离，然后取向量距离最小的一个作为匹配点.但特征点多时，暴力匹配的运算量变大，会影响算法的实时性要求.因此，选用快速近似最近邻（FLANN）算法[11]更加适合于匹配点数量极多的情况.该算法理论已集成到OpenCV，使用时可直接从中调用.

1.2 单目相机运动估计

通过特征提取与匹配，可从两张图像中得到一对匹配好的特征点，如图2所示.相邻的图像存在很多对匹配点，可通过它们之间的对应关系得到相机的运动.

图2 对极几何约束

在第一帧的坐标系下，设P点的空间位置为：

由针孔相机模型推出两个像素点p1,p2的像素位置为：

式中：K为相机内参矩阵；R、t为两个坐标系下相机的旋转与平移运动.

两个像素点在归一化平面上的坐标x1,x2为：

对式（3）进行一系列变换并带入p1,p2得：

可将上式（4）中间部分记作两个矩阵：基础矩阵F和本质矩阵E，带入（4）式得：

相机的位置估计问题变为以下两步：

1）根据配对点的像素位置，求出E或者F；

2）根据E或者F，求出R、t.

2 IMU传感器模型构建

一般而言，IMU内部包含加速度计和陀螺仪，分别测量机器人的加速度信息和角速度信息.

忽略尺度因子的影响，只考虑白噪声和随机游走偏差，VIO中IMU模型为：

式中：上标b为IMU坐标系，w为世界坐标系world；上标a为加速度计，g为陀螺仪；ωb和ab分别为IMU输出的角速度和加速度；为IMU真实的角速度和加速度数据；mg和ma为陀螺仪和加速度计的零偏；ng和na为陀螺仪和加速度计的高斯白噪声；gw为世界坐标系下的重力加速度；qbw为世界坐标系向IMU坐标系下的旋转矩阵.

IMU的位移,速度,旋转对时间的导数为：

根据式（10）、（11）、（12）导数关系，使用中值法得到运动模型的离散积分，即两个相邻时刻k到k+1的位姿用两个时刻的测量值ω，a的平均值来计算：

其中：

3 VIO紧耦合优化器设计

融合IMU是为了克服纯视觉SLAM的一些缺点.视觉受光线条件影响和限制，且在纹理丰富的场景中可以正常工作，然而遇到玻璃、白墙等特征稀少的场景时就无法正常工作.纯视觉还难以处理动态场景，环境变化时，会误认为自己在运动，而IMU能够感受到自己的运动，避免运动误判.纯视觉也难以处理运动过快的场景，两帧图像之间的重叠区域太少甚至没有，造成无法基于两帧图像中相同点和不同点测算运动，而IMU可以补充发挥作用，提供可靠的位姿估算.

VINS系统主要包括3部分.如图3所示：前端进行数据预处理，包括特征提取匹配和IMU预积分；初始化是对系统初始状态变量（重力方向、速度、尺度等等）进行处理；后端通过滑动窗口算法得到最优状态估计.

图3 VINS系统框架

3.1 测量数据预处理

特征提取与跟踪在视觉里程计部分已进行详细讨论.接下来主要是对IMU数据进行预积分.可对IMU数据进行积分得到从i时刻到j时刻的位移、旋转和速度.但是每次优化更新后，都需要重新进行积分，导致运算量较大.此时，可用一个简单的公式转换，将积分模型转化为预积分模型：

通过式（18），位移、速度、旋转积分公式中的积分项则变成相对于第i时刻的姿态，而不是相对于世界坐标系的姿态.此时，位移、速度、旋转积分公式为：

式(19)、(20)、(21)中的积分项仅跟IMU测量值有关，它将一段时间内的IMU数据直接积分起来就得到了预积分量：

3.2 VINS系统初始化

单目紧耦合VIO是一个非线性的系统.VIO无法直接观测到尺度，如果没有良好的初始值，很难直接将相机和IMU的数据融合在一起.可以假设一个静止的初始条件来启动单目VINS估计器.然而，这种假设是不合适的，因为在实际应用中经常会遇到运动下的初始化.当IMU测量结果被大偏置破坏时，情况就变得更加复杂了.事实上，初始化通常是单目VINS最脆弱的步骤.需要一个鲁棒的初始化过程以确保系统的适用性.

初始化过程中，首先要建立视觉坐标系和IMU坐标系的联系.假设相机和IMU之间有粗略测量的外部参数：旋转qbc、位移pbc，我们可以将姿态从IMU坐标系转换到相机坐标系：

由于IMU采集数据的频率大于相机的，所以初始化过程中需将IMU预积分数据与相机的位姿数据对齐.具体过程如下：

1)旋转外参数qbc未知,则先估计旋转外参数.

2)利用旋转约束估计陀螺仪偏离.

3)利用平移约束估计重力方向、速度、以及尺度初始值.

5)求解相机坐标系与世界坐标系之间的旋转矩阵，并将轨迹对齐到世界坐标系.

3.3 滑动窗口优化

初始化完成后，VINS系统采用基于滑动窗口的紧耦合单目VIO进行高精度和鲁棒的状态估计.

VINS系统优化的状态变量为：

式中：xk表示第k帧图像时的IMU的位姿；n是关键帧的总数；m是滑动窗口中的特征总数；λi为第一次观测到第i个特征的逆深度.

通过最小化滑动窗口中的残差项来估计系统的状态变量：

式中：rB和rC分别是IMU和视觉测量的残差；B是所有IMU数据的集合；C是在当前滑动窗口中至少观测到两次的一组特征；（rP，JP）是来自边缘化的先验信息；ρ为Huber范数.

4 实验验证与分析

本文中实验所用电脑为Inter Core i5-4210记本，内存12 GB，系统为Ubuntu16.04，ROS版本为kinetic.

实验1的算法验证是基于图3所示的VINS系统框架（无回环检测）.通过数据文本构建仿真场景，如图4所示该场景是由三个长方体堆叠组成，长、宽、高分别为：10×10×3、7×7×3、4×4×4.然后由仿真场景生成视觉特征的仿真数据集，采样频率为30 Hz，其中特征点的3D坐标如图4所示为长方体的顶点和附近的点，共48个.IMU的仿真数据集是由在x、y平面内做椭圆运动，z轴做正弦运动的曲线生成，采样频率为180Hz.最后通过配置不同陀螺仪和加速度计噪声的大小，验证该算法的有效性，其中图像的像素误差给定为1.5.

图4 仿真场景

将已经准备好的仿真数据集（视觉特征，IMU数据）接入VINS代码，并运行出轨迹结果.具体实验步骤为：

1)IMU仿真数据集无噪声；

2)IMU仿真数据集有噪声,具体噪声参数的设置如表1所示.表中g_bias、a_bias分别表示陀螺仪和加速度计的随机游走偏差，g_noise、a_noise分别表示陀螺仪和加速度计的高斯白噪声.

表1 仿真数据集噪声参数

当IMU仿真数据集无噪声，如图5所示，优化轨迹imu_int和带有噪声的轨迹imu_int_noise重合，但由于存在像素误差，它们与真实轨迹ground_truth并未重合.当IMU仿真数据集有噪声时，由表1所示，第三组数据各项参数大概是第二组的10倍，但如图7所示，尽管imu_int_noise轨迹变化很大，最终优化轨迹imu_int都能像图6有有效的跟踪真实轨迹ground_truth.

图5 第1组实验结果

实验2采用的是EuRoC提供的V1_01_easy数据集，将本文方案与ORB-SLAM 2单目作比较.图8和图9分别为两种算法的V1_01_easy轨迹平面图，图中的虚线表示真实轨迹，彩色线表示算法的定位轨迹，从这两幅幅图中，可以看出VINS算法定位轨迹在V1_01_easy数据集下与真实轨迹更为稳合.图10表示两种算法在V1_01_easy数据集下的轨迹误差，图中可以明显看出VINS_result_no_loop_V1.csv的定位误差在整体上要优于ORB-SLAM 2_1.txt单目的定位误差.表2显示了定位误差的状态对比，VINS-MONO定位误差的最大值、最小值、标准差、均方根误差均小于ORB-SLAM 2单目的定位误差.图11是定位误差的小提琴图，主要统计和分析数据的离散程度和分布密度，两图中左边表示VINS_result_no_loop_V1.csv，右边表示ORB-SLAM 2_1.txt.图中可以看出VINS_result_no_loop_V1.csv误差的分布集中在0.2m附近，而ORB-SLAM 2_1.txt分布集中在1m附近且分布范围较广；图中VINS_result_no_loop_V1.csv误差的中位数和四分位数都优于ORBSLAM 2_1.txt单目；VINS_result_no_loop_V1.csv误差的上侧和下侧的离散情况也要优于ORBSLAM 2_1.txt单目.

图6 第2组实验结果

图7 第3组实验结果

图8 VINS算法轨迹

图9 单目ORB-SLAM算法轨迹

图10 两种算法轨迹误差对比

表2 两种算法在V1_01_easy数据集下误差状态对比 m

图11 两种算法定位误差对比的小提琴图

5 结束语

本文针对惯性传感器积分时位姿容易发散，且机器人剧烈运动时，VO定位容易失效等问题，提出了一种基于单目相机与IMU的机器人定位算法.该算法主要是利用VINS-MONO框架，但在前端进行特征提取与匹配的时候，采用ORB特征，提高了新增关键点的特征匹配效率，更好地构建场景地图.在处理IMU数据时，利用中值法得到运动模型的离散积分，能够更加精确地定位机器人的位置.后端仍采用滑动窗口的非线性优化得到机器人运动的最优状态估计.

实验一在IMU仿真数据中添加噪声进行试验验证，该算法能够有效地减小惯性传感器的累积误差.实验二利用V1_01_easy数据集验证了该算法优于单目ORB-SLAM算法.但本文算法没有闭环检测模块，在机器人长时间、大范围的导航和定位中，单纯依靠VINS系统的状态估计会出现严重偏差.且该算法处理的数据量很大，实时性问题还存在问题，现只适用于室内小范围场景.因此下一步会将闭环检测添加到该算法框架，考虑进一度简化该算法框架，达到实时性的要求.