室内环境下立体视觉惯导融合定位

敖龙辉，郭 杭

(南昌大学，江西 南昌 330031)

居家服务机器人是未来机器人发展的重要方向，状态估计是机器人技术的基础。持续且准确的状态估计在家庭服务机器人运用上格外重要。为了提高机器人的稳健性和自主能力，越来越多的传感器安装在机器人上。IMU模组与相机模组凭借低成本、小型化，在导航定位领域也有着大量的应用。在众多研究人员的共同努力下，提出了许多基于纯视觉里程计的算法，如SVO[1],LSD-SLAM[2],DSO[3],ORB-SLAM[4]。除了相机以外，IMU是另一个流行的状态估计选项。IMU通过测量解算高频率下加速度和角速度，实现对运载体导航定位的目的，这对于实时应用中的低延迟姿态反馈是必要的[5]。在室内自主机器人的研究中，结合视觉及惯导的组合定位导航方法有着极大的研究前景[6-10]。视觉导航方法融合惯性信息能够有效地克服相机快速运动造成的问题，同时视觉信息又能够在慢速运动时解决IMU的漂移问题。考虑双目相机和IMU具有天然的互补特性，立视觉惯导组合导航成为机器人定位导航的重要选择[11]。本文研究基于紧耦合非线性优化的立体视觉惯性融合导航方法，提出一种稳健的系统初始化方法，实现了基于该方法的视觉惯性融合导航系统，并设计试验进行验证，系统能够得到很好的初始化效果，达到相当高的定位精度，非常适合运用于居家场景机器人的导航定位。

1 数据处理方法

1.1 IMU预积分技术

IMU预积分技术[10]在基于紧耦合优化的视觉惯性融合导航方法中具有举足轻重的作用。在双目视觉惯性导航系统工作时，IMU的数据采集频率远远大于图像关键帧的获取频率，如图1所示。

假设给定时刻k和k+1的两帧图像，以及k到k+1时间内的惯性测量值，则两个时刻的状态有如下关系

(1)

(2)

(3)

(4)

(5)

(6)

其中

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

式中，i为对应于k到k+1时间内的IMU测量的离散时刻；δt为i到i+1之间的时间间隔。

1.2 协方差矩阵计算

在k到k+1之间的两个离散时刻，计算不同测量的协方差矩阵，用来做加权融合。通过非线性系统的线性过程，t时刻误差项的导数为

(16)

(17)

式(17)表示下一时刻IMU测量误差与上一时刻的测量误差呈线性关系。根据当前时刻的状态，就可以预测下一时刻的IMU均值和协方差，协方差预测公式如下

(18)

1.3 IMU残差

IMU的残差表示两帧之间的状态约束关系，可以用IMU的状态变化量来表示

(19)

式中第3行的[]im表示对四元数的取虚部，是四元数误差的近似表示。IMU残差项的协方差矩阵即是预积分迭代后增量误差的协方差矩阵。

2 双目图像处理

2.1 图像处理

经过离线校准每个相机的固有参数和相机之间的非固有变换，在双目摄像头获取图像信息之后；对每一帧图像提取Harris角点[12]，对左右目图像提取的点进行匹配，采用Lucas-Kanade光流法[13]跟踪相邻帧的角点；最后将跟踪的结果发送至优化后端进行处理。图像处理的具体流程如图2所示。

2.2 视觉残差

(20)

图像观测的结果决定了视觉残差的确定性，协方差矩阵与相机的标定结果有关，在标定相机内参时，重投影误差一般在0.5～1像素之间。

2.3 IMU初始化

由于非线性系统对系统初值精度的要求，对比于VINS-Mono提出的松耦合的初始化方法，本文提出了一种紧耦合稳健的IMU初始化方法。通过求解视觉惯性系统的解析解(如图3所示)，可以算得初始的重力矢量、速度和三维点的深度。为了有效避免IMU漂移的情况，在后端优化中使用这些结果作为初值。

假设时刻k和时刻k+1同时观测到编号为j的特征点，k为初始帧。对应单个点的观测，有特征点观测量与IMU观测量之间的直接关系

(21)

(22)

3 视觉惯导融合

采用一种滑动窗口的紧耦合优化方法进行稳健且高精度的状态估计。基于紧耦合优化的视觉惯性融合方法将滑动窗口内所有的待估计变量(包括位置、姿态、速度、IMU零偏、相机-IMU外参、所有点的深度)都放在一个优化框架进行优化。

3.1 目标函数

整个滑动窗内的状态向量定义如下

(23)

状态估计的本质是一个极大似然估计问题，然后对于一个符合高斯噪声模型的SLAM问题进行最大后验概率推断，求解一个非线性最小二乘问题。有如下形式表达

(24)

3.2 边缘化信息

随着时间的增加，系统的状态数也随之增加，计算复杂度也越来越高。为了约束计算的复杂性，再保留之前有用信息，本文采用了边缘化技术。

边缘化技术是由概率论中的边缘分布所产生的，给定联合概率分布P(x,y)，求出边缘概率分布P(x)。在该非线性优化问题中，边缘化技术通过一系列操作将某些不需要的状态标量进行消元。由高斯牛顿法求解非线性最小二乘问题可以表示为

(25)

式中，xa、xb为系统的两个状态变量,通过高斯消元，将H的左下角消掉可得

(26)

通过式(26)可以直接计算Δxb

(B-CTA-1C)Δxb=bb-CTA-1ba

(27)

由此式，即可舍弃状态Δxa，直接求取Δxb，这就是边缘化先验。

4 试验及分析

4.1 试验环境说明

本文在试验室中搭建模拟居家环境(如图4所示)，在HUSKY A200平台上搭载LeaDsense双目相机，LeaDsense双目摄像头设备包含两个快门摄像机及一个九轴IMU传感，相机频率为60帧，IMU频率为200帧，操作系统为Ubuntu16.04版本。为验证本文中提到方法的可行性，首先对提出的初始化方法进行了验证，其次验证了本文方法运用于室内居家服务机器人的定位精度及稳定性。在模拟环境中分别随机驱动机器人行走半圈、一圈及两圈并得到试验数据。

4.2 试验结果

图5为系统启动时VINS-Mono初始化方法与本文初始化方法的效果及对定位精度的影响对比图。其中图5(a)为VINS-Mono初始定位结果，图5(b)为本文方法在同等场景下的初始定位结果。本文方法的初值估计较为准确，定位效果也更加出色，轨迹基本重合于RTS的真值轨迹。

图6为采用本文方法驱动机器人行驶半圈、一圈和两圈的试验结果图，其中浅色为RTS高精度光学动作捕捉设备采集的真值数据轨迹，深色为本文方法的试验结果轨迹。从试验结果可以看出，本文提出的视觉惯导融合导航方法运用在室内服务机器人的精度较高，平均误差能控制在0.1 m以内，试验结果可到达亚厘米级。

5 结 语

本文提出了一种基于紧耦合非线性优化的立体视觉惯性融合导航方法，并在自己搭建的居家环境下，分别对系统的初始化效果、定位精度及稳定性进行了验证，得到了很好的结果，在室内条件下，定位平均误差能控制在0.1 m以内。为视觉惯导在居家服务机器人的定位途径提供了参考；同时系统也存在需要改进的地方，在硬件方面，本文中的IMU数据与相机数据时间同步精确度有待提高，系统在初始化时可以将该参数一起联合进行估计，从而完成更加精确的系统状态估计。