室内移动机器人的视觉惯性组合定位研究

汪志刚，郭 杭，敖龙辉

(南昌大学信息工程学院，南昌 330031)

室内机器人的导航定位问题是机器人研究的一大核心课题。室内环境下，卫星信号弱，卫星导航定位方法并不适用。一般利用各种传感器得到机器人初始位姿和各个时刻的运动状态，来推算机器人位姿，实现室内机器人定位。

视觉传感器能获得环境图像信息或深度信息，被广泛适用于各种定位算法中，如半直接单目视觉里程计(semi-direct monocular visual odometry，SVO)[1]、多功能、高精度的单目同步定位与地图构建系统(oriented FAST and rotated BRIEF-simultaneous localization and mapping,ORB-SLAM)[2]、大尺度直接单目同步定位与地图构建系统(large-scale direct monocular-simultaneous localization and mapping,LSD-SLAM)[3]、直接稀疏里程计(direct sparse odometry,DSO)[4]。除此之外，惯性测量单元(inertial measurement unit, IMU)由于抗干扰能力强、短期定位结果精确等优点，成为惯性导航系统[5-6]的关键传感器。在室内机器人的导航定位研究中，结合视觉信息和惯性数据的视觉惯性组合导航方法有着很大的发展前景[7-9]。一方面，相机能够捕捉场景中丰富的信息，而IMU能够在短时间内高频地获得精准的估计，减轻动态物体对相机的影响；另一方面，视觉信息能有效地修正IMU的零漂。一定程度上，两种传感器互补，搭配使用能得到更好的定位结果。

针对室内机器人的定位问题，提出一种紧耦合的视觉惯性组合定位方法。本文方法将IMU的位姿等状态量与双目相机的位姿等状态量合并在一起，共同构建优化函数，然后进行状态估计。并提出了一种新的基于Vicon[10-12]的IMU内参标定方法。Vicon是高精度的光学动作捕捉系统，常用Vicon数据作为数据集的真值，并以此评估其他定位系统的定位精度。

1 惯性测量单元

1.1 参考坐标系

如图1所示，本文方法主要涉及4个坐标系，分别为地球惯性坐标系{E}、Vicon参考坐标系{W}、Vicon体坐标系{V}以及IMU体坐标系{I}，不同坐标系的变换为

图1 参考坐标系

1.2 预积分

采用预积分技术[13]，如图2所示，将Vicon和IMU的数据融合，计算两帧之间的相对位置、速度、姿态之间的变化。由于Vicon跟IMU采集频率相同，于是对Vicon数据进行降采样，降至30 Hz。

图2 Vicon和IMU的采集频率对比

对于连续两个关键帧ik和ik+1,它们的对应时刻分别为k和k+1,从k时刻的状态对IMU测量值进行积分得到k+1时刻的状态，即

(1)

(2)

(3)

式中：

可由Vicon数据直接得到，表示载体在Vicon参考坐标系下的速度、位置和姿态；w、a为IMU的陀螺仪和加速度计的测量值；gw为Vicon参考坐标系下的重力矢量；Δtk为k～k+1的时间间隔；bg为陀螺仪的零偏估计参数；ba为加速度计的零偏估计参数。

为

(4)

(5)

(6)

式中：

为预积分的值，表示载体在IMU体坐标系下的位置、速度、姿态变化量。它们只和IMU的bias有关，则当bias变化很小的时候，使用一阶线性展开公式，求得

(7)

(8)

(9)

在初始时刻

(10)

(11)

(12)

式中：i为在[k,k+1]中测量值对应的离散时刻,δt为测量值在[k,k+1]之间的时间间隔。

1.3 协方差矩阵与雅可比矩阵

离散形式下有误差传递过程，k+1时刻下的状态量误差来源主要是前一时刻状态量的误差传播以及各种噪声。采用误差随时间变化的递推方程，则t时刻误差项的导数为

(13)

式(13)中：Ft、Gt为两个时刻间的协方差矩阵；

为加速度计和陀螺仪的高斯白噪声；

为加速度计和陀螺仪随机游走bias的高斯白噪声。

根据导数的定义，由式(13)得出IMU误差运动方程为

(14)

式(14)表示了下一时刻IMU测量误差与上一时刻的测量误差成线性关系，那么根据当前时刻的状态，可以预测下一时刻的IMU均值和协方差，协方差预测公式为

(Gtδt)Q(Gtδt)ηt

(15)

通过式(14)，使用迭代的方式求得误差项的雅可比矩阵为

Jt+δt=(I+Ftδt)Jt

(16)

式(16)中：雅可比矩阵的初值Jik=I。

1.4 残差

预积分残差表示一段时间内预积分量作为测量值，对两时刻之间的状态量进行约束，如式(17)所示。其中，等式右边第3项是关于四元数的旋转误差，[·]xyz表示只取四元数的虚部(x,y,z)组成的三维向量。

(17)

1.5 IMU内参标定

首先最小化相对姿态残差，得到陀螺仪的零偏估计参数bg,即

(18)

然后固定bg,最小化相对速度残差和相对位置残差，得到加速度计的零偏估计参数ba,即

(19)

2 视觉数据处理

2.1 视觉观测值残差

视觉观测值残差是特征点在当前帧的重投影误差，这里选用切平面重投影误差。

(20)

式(20)中：π-1为利用摄像机内参将像素位置转换成单位向量的反投影函数。

2.2 视觉惯性联合初始化

视觉惯性组合定位方法是个非线性程度很高的系统，因此初始化直接影响整个紧耦合系统的鲁棒性和定位精度。由于IMU的内参是基于Vicon数据求得，比较精确，故只对速度和重力加速度初始化，即

(21)

3 视觉惯性组合方法

3.1 优化函数

视觉惯性组合定位方法是将视觉和惯性的原始观测量进行有效组合，将滑动窗口内的状态量(包括位置、姿态、速度、IMU 零偏、相机-IMU 外参等)放到一个状态向量中，即

(22)

式(22)中：第1个式子为滑动窗口内整个状态向量，n为帧数，m为滑动窗口中的特征点总数；第2个式子表示第k帧图像捕捉到的IMU状态，包含位姿、速度、旋转等；第3个式子为相机外参；λ为特征点的逆深度。

针对状态向量，引入边缘化技术，建立非线性优化函数为

(23)

式(23)中：第1部分为IMU在相邻两帧之间产生的残差；第2部分为特征点在相机下的投影产生的视觉残差；第3部分为边缘化残差，表示从滑动窗口中去掉的位姿和特征点的约束。

3.2 边缘化和舒尔补理论

边缘化[14]的目的是防止位姿和特征点的个数随着时间不断增加，使得优化函数始终在一个有限的复杂度内。边缘化在移除位姿或特征点的时候，将相关联的约束转变成一个约束项作为先验信息放到优化函数中。

边缘化来源于概率论中的边际分布，在非线性优化中，边缘化将概率问题变成最小二乘法问题，即

(24)

式(24)中：δx为滑动窗口内的变量；b表示边缘化残差。假设边缘化δxa,保留δxb,通过舒尔补理论直接得到：

(25)

通过舒尔补理论[14]完成了对δxa边缘化的操作，同时不会丢失变量间的约束信息。

4 实验

4.1 实验环境

在实验室中搭建模拟室内环境(图3)，在HUSKY A200平台上搭载领晰双目相机和Vicon，操作系统选用ubuntu18.04版本。其中，领晰双目摄像头设备包含两个快门摄像机以及一个九轴IMU传感，相机频率为60帧，IMU频率为200帧。

图3 实验环境和模拟机器人

首先通过EuRoc[15]数据集实验对IMU内参标定方法进行验证，EuRoc数据集本身提供了基于多状态约束下的卡尔曼滤波器(multi-state constraint Kalman filter，MSCKF)融合滤波方法得到的IMU内参真值，可以看作真值；然后对视觉惯性组合定位方法的准确性和稳定性进行验证，在模拟环境中分别随机驱动机器人行走5圈、1圈以及半圈。

4.2 实验结果

表1和表2分别是加速度计和陀螺仪的零偏估计参数的标定结果。结果显示，本方法对陀螺仪的内参标定相当准确，大部分结果都与EuRoc提供的真值很接近。而加速度计的标定结果则有不小的偏差，但也处于同一范围内，这是因为加速度计测量的加速度内包含重力，采用运动标定的方式无法忽略这种影响。总之，基于Vicon的IMU内参标定方法是有效的，可以应用于视觉惯性组合定位系统。

表1 加速度计的标定结果

表2 陀螺仪的标定结果

图4为机器人在模拟环境下行走5圈、1圈以及半圈的实验结果，分别为瑞立视跟踪系统(Realis tracking system，RTS)高精度光学动作捕捉设备采集的真值数据轨迹和本文方法的实验结果轨迹。可以看出，本文提出的视觉惯性组合系统与真实轨迹几乎贴合，具有极高的稳定性，平均误差在0.1 m以内，完全可以满足室内移动机器人的定位要求。

图4 真值轨迹与实验轨迹对比

5 结论

提出一种紧耦合的视觉惯性组合定位方法，在搭建的模拟环境中，分别对IMU内参标定效果、定位精度及系统稳定性进行验证，IMU内参标定能得到比较准确的真值，定位的平均误差能控制在 0.1 m以内，为室内移动机器人的定位提供了一种新的可借鉴方案。当然也存在可以改进的地方，如对加速度计的零偏估计参数时需要考虑重力的影响以及需要提高IMU数据与相机数据时间同步精确度。