单目视觉惯性SLAM与UWB数据融合的精确定位*

李玉卿， 鲍 泓， 徐 成

(1.北京联合大学智慧城市学院,北京 100101； 2.北京联合大学 北京市信息服务工程重点实验室,北京 100101)

0 引 言

室内或封闭场景下的无人驾驶，主要通过同步定位与建图(simultaneous localization and mapping,SLAM)的方法实现定位。单纯依靠视觉传感器的SLAM[1,2]不能满足实际需求。视觉SLAM存在传统算法和深度学习两种方法。传统视觉SLAM分成特征点法和直接法。特征点法：首次实现跟踪和建图的并行化PTAM[3]，其特征跟踪在前端，地图优化在后端；ORB-SLAM2[4]在PATM的基础上添加闭环修正线程，场景相对单一或速度较快时效果变差；VINS-Mono[5]加入了惯性数据，光流跟踪的特征匹配易受光照变化影响。直接法包括DTAM[6]，特征缺失、图像模糊下仍能运行，但计算量过大；LSD-SLAM[7]直接估计关键帧之间相似变换与尺度感知，对于相机的内参和曝光要求十分严格；SVO[8]与DSO的运行速度较快，但都舍弃了回环检测，累积误差会随着时间增大。

基于深度学习的视觉SLAM也是研究的热门。PoseNet[9]在GoogleNet[10]的基础上将6自由度位姿作为回归问题进行网络改进；LIFT[11]利用深度神经网络学习图像中的特征点；CNN-SLAM[12]在LSD-SLAM基础上用卷积神经网络进行深度估计以及图像匹配；UndeepVO[13]也使用深度神经网络估计单目相机的自由度位姿及深度。但基于深度学习的视觉SLAM过于依赖训练集，当实际运动速度、帧频率与训练数据存在差异时，误差会迅速增大。

超宽带(ultra-wide band，UWB)定位技术，具有分辨率高、穿透力强、功耗低等优点。其定位原理与GPS相似，一定范围内可获得较高的测距和定位精度。UWB定位方式中信号到达时间差(time difference of arrival，TDOA)法，通过测量移动终端到不同发送端的距离或距离差进而进行定位，相较于其他方法对周围环境的抗干扰能力更强。

目前单纯视觉SLAM不能满足实际需求[14]，存在定位精度不高鲁棒性差的问题。UWB定位技术精度较高，但信号受到干扰或物体遮挡时，会产生较大的误差。因此本文将单目视觉惯性ORB(VIORB)[15]与UWB获取定位数据进行融合提升定位的精度与鲁棒性，实现高精度室内实时定位。

1 VIORB-SLAM与UWB定位

1.1 VIORB-SLAM

单目相机在拍摄过程中会丢失场景的深度信息，建图时存在尺度不确定性的问题，对于单目VIORB定位算法，在ORB-SLAM2的基础上加入惯性测量单元(inertial mea-surement unit，IMU)信息通过IMU预积分获取尺度信息，得到更精确的相机的位姿。ORB-SLAM2是由特征跟踪、局部建图、闭环检测三个线程构成。VIORB分别进行了修改，融合了IMU数据信息。VIORB算法的流程如图1所示。

图1 VIORB算法流程

1)特征跟踪：加入IMU数据信息，得到更加精确的数据，基于最小化重投影误差和最小化IMU误差，建立帧与帧之间的约束关系，从而优化当前帧。

2)局部建图：优化前N个关键帧以及这些关键帧看到的点，当前关键帧N+1固定不变，提供IMU预积分约束。连续关键帧之间的时间差异越长，IMU提供的信息越弱。

3)回环检测：IMU提供尺度信息，全局位姿优化下降到6个自由度。完成全局位姿优化后，再对速度进行矫正，执行一次全局优化，优化所有的系统状态。

1.2 UWB定位

UWB通信系统通过信号在两个异步收发机之间飞行时间之差来测量节点间的距离。TDOA定位方式利用多个基站接收到信号的时间差来确定标签的位置，不需要检测信号传输时间，对时间同步的要求大大降低。

已知标签到基站间的距离差为常数，时间差也为常数，标签的位置一定处于以这两点为焦点的双曲线上。三个基站就存在三条双曲线，曲线相交的点就是标签的位置。标签相对于基站A(x1,y1),B(x2,y2),C(x3,y3)的距离差

(1)

式中c为光速，τi为第i个基站收到标签发送信号的时间，消除式时间差Δt得到

(2)

通过求解式(2)中方程组,即可得到标签位置的坐标(x,y)。

2 视觉与UWB融合

2.1 融合算法框架

对于单目视觉惯性SLAM与UWB组合的非线性系统，通过扩展卡尔曼滤波器(extended Kalman filter,EKF)进行融合。数据融合算法流程如图2所示。

图2 数据融合算法流程

2.1 EKF

EKF主要包括预测更新和测量校正。通过前一时刻状态向量xk-1对当前的状态xk进行预测，根据预测的估计量对当前的状态修正，最后得到当前最优状态向量yk。满足如下关系

xk=f(xk-1)+wk-1,yk=h(xk)+vk

(3)

式中wk-1为过程噪声，vk为量测噪声。预测更新包括状态预测和误差协方差预测

(4)

Pk=FPk-1FT+Qk

(5)

Kk=PkHT(HPkHT+Rk)-1

(6)

(7)

(8)

式中Rk为观测噪声协方差矩阵,yk为k时刻系统观测到的状态向量。

2.2 数据融合

将VIORB与UWB的坐标信息经过空间变换转换到全局坐标系下，通过EKF将两种数据进行融合。算法的具体步骤为:

1)数据初始化：将VIORB与UWB获得的定位数据传入。输入数据分别为

(9)

2)预测更新：以k-1时刻的最优状态向量对k时刻的状态向量进行预测估计，如式(3)中，则

(10)

通过状态预测和误差协方差预测方程，获得对当前时刻状态向量的估计。

3)测量校正：预测获得k时刻的估计状态向量对当前观测到的状态向量修正。根据式(6)计算测量校正方程组，获得当前最优的状态向量。式(6)中的H为

(11)

3 实验结果与分析

实验硬件环境为i7—7700，GTX1050Ti。单目USB摄像头，六轴加速度计MPU6050模块，Link Track定位测距模块。首先固定设备，使其重心位于同一直线，调整相机与IMU的坐标轴方向一致。调节UWB频率与相机帧率一致。软件环境为Ubuntu16.04，ROS-kinetic，OpenCV3.4.5，轨迹评估工具EVO。将UWB定位基站布置在实验区域的四角上，按照预设轨迹均匀低速采集数据。实验场景如图3所示。

图3 实验场景示意

实验在室内环境下，分为正常光照与弱光光照。实验过程中采集的数据通过EVO与固定轨迹进行对比，结果如图4所示。从误差最值、误差平均值、均方根误差及标准差4个方面进行比较。如表1和表2所示。

表1 正常光照下三种方法轨迹误差比较

表2 弱光光照下三种方法轨迹误差比较

图4 正常、弱光光照下三种方法轨迹对比

UWB定位精度较高，当存在障碍物遮挡等时，定位精度会受到影响出现较大误差。VIORB旋转时，相机视野内的场景变化较大，易发生特征点丢失导致跟踪失败，且IMU

误差也随之增大。本文提出的数据融合的方法在实验过程中定位精度较高，在方向改变的场景下，没有出现明显的跳变及显著漂移。表1给出了正常光照下VIORB，UWB及本文算法的误差统计。受非视觉误差的影响，UWB的误差最值较大，但系统运行过程中的鲁棒性较好；VIORB鲁棒性较差且环境因素对定位精度的影响较大。本文算法定位精度的最大误差为0.101 316 m，RMSE为0.035 965，相较于前两种方法的最大误差分别为0.146 437 m与0.153 275 m，融合算法的轨迹更契合实际轨迹，且没有显著的漂移误差。

弱光光照条件下，虽然VIORB使用的是ORB特征提取算子，具有一定的光照不变性，但较于正常光照还是产生了部分误差，仍能完成整个定位过程，相应因累积误差而产生的漂移要大于正常光照。相较于VIORB与UWB，本文方法的定位精度仍要优于单一传感器方式的定位方法。

表2给出了弱光光照下单目VIORB、UWB及本文算法的误差统计。受弱光光照的影响，定位过程中因累积误差而产生的漂移导致了误差增大，但仍能完成定位过程，最大误差为0.136 103 m，RMSE为0.039 132，前两种方法的最大误差分别为0.195 078 m与0.153 275 m，定位精度优于前两种方法。

4 结 论

本文提出一种将单目VIORB与UWB数据融合的定位方法可有效解决视觉SLAM定位精度不高，鲁棒性不强的问题,且抑制UWB定位过程中非视距误差的影响。通过VIORB与UWB的对比,证明提出的融合算法改善了定位的效果，提高了系统定位的精度及鲁棒性。