一种单目视觉/UWB组合的室内定位方法

乔 智，徐爱功，隋 心，郝雨时

(辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000)

0 引言

基于单目视觉的同时定位和地图构建(simultaneous localization and mapping，SLAM)技术近年来发展迅速，从其发展历程可以大致分成3个阶段，分别为基于扩展卡尔曼滤波(extended Kalman filter，EKF)的SLAM、基于粒子滤波的SLAM和基于图优化的SLAM。最早由文献[1]提出的基于EKF的SLAM，通过构建SLAM系统的状态方程和量测方程来估计相机及地图点的位置。之后又有研究人员在此基础上做了许多改进，文献[2]提出了基于无迹卡尔曼滤波的SLAM方法；文献[3]提出了基于迭代EKF的SLAM方法。由于EKF-SLAM算法只能处理噪声为高斯噪声的情况，文献[4]提出用粒子滤波器来解决这一问题，其主要思想是用粒子滤波算法对相机和地图点的位置进行估计，并且通过大量的粒子对位置进行多个假设，因此适用于不同的噪声情况。近年来基于图优化的SLAM[5]成为研究热点，其主要思想是把位姿估计作为1个全局优化问题，通过图建模，把所有的观测数据和约束条件一起放到图中，最终利用这些信息进行全局优化处理，获得相机的位姿和三维地图点位置。在图优化SLAM中具有里程碑意义的是文献[6]提出的平行跟踪和地图构建算法(parallel tracking and mapping，PTAM)，他们首次把SLAM算法分成2个并行线程，即相机特征跟踪线程和地图构建线程，实现在线的实时SLAM。继PTAM之后，文献[7-8]在PTAM算法的基础上，添加了闭环修正方法进而提出了ORB-SLAM算法。最近有学者将深度学习理论与SLAM相结合，文献[9]通过卷积神经网络学习图像数据特征，进行前端视觉里程计的最优估计。文献[10]提出一种利用贝叶斯卷积神经网络进行SLAM重定位的方法。与其他的SLAM方法相比，ORB-SLAM具有较高的定位精度和实时性。因此本文使用该算法进行单目视觉的定位。ORB-SLAM算法使用ORB(oriented FAST and rotated BRIEF)[11]作为特征提取和描述的方法，由于ORB算法的特征检测算子缺乏对尺度变化的适应性，当场景相对单一或运动速度过快的时候，检测效果会下降，进而导致整个SLAM系统跟踪失败。

在室内定位中，超宽带(ultra wide band，UWB)技术利用功率谱密度极低、脉冲宽度极窄的脉冲信号传输数据，具有时间分辨率极高、障碍穿透能力强等特点，在视距(line of sight，LOS)环境下能获得厘米甚至毫米级的测距和定位精度[12-13]，适用于室内环境下的高精度定位。UWB定位通常有3种方式：信号到达角(angle of arrival，AOA)，接收信号强度(recevied signal strength，RSS)、信号到达时间(time of arrival，TOA)/信号到达时间差(time difference of arrival，TDOA)。相比较于以上3种方法，基于往返时间(round trip time，RTT)的测距方法可以通过对时间的测量，间接的计算出UWB基准站和流动站之间距离。不需要增加其他硬件，避免了基准站和流动站之间的时间同步。通过该方法进行室内定位，能够获得较高的定位精度[14-16]。基于以上特点本文使用RTT算法进行UWB室内定位。

UWB属于有源定位方法，虽然定位精度比较高，但当信号受到干扰时，会产生较大的误差。单目视觉ORB-SLAM属于无源定位方法，当环境比较单一或运动速度比较快时容易出现跟踪失败导致系统崩溃，并且该方法属于相对定位方式，定位会随着时间增加而发散。根据上述分析可以认为在复杂的室内环境下仅仅依靠某1种定位方法，很难取得鲁棒性好、精度高的定位结果，因此，本文将2种定位方法进行组合，单目视觉ORB-SLAM可降低UWB非视距(non line of sight，NLOS)误差的影响，UWB的加入可以提升定位的可靠性，保证定位的精度，利用组合定位方法最终实现高精度室内定位。

1 单目视觉和UWB定位算法原理

1.1 单目视觉定位算法原理

ORB-SLAM是1种采用图优化理论的单目视觉同步定位与建图算法，在小型和大型的室内室外环境下实时的得到精确的相机位置和姿态。文献[8]完整地介绍了该算法，ORB-SLAM算法流程概述如图1所示。本文对它的3个并行线程做了简单的介绍。

1)特征跟踪线程：工业相机获得视频帧，对影像进行预处理，将影像转化为灰度图。对灰度图进行特征提取，采用的是FAST算子检测特征点，再给检测到的特征点加个方向信息，构成OFAST，然后用BRIEF作为特征描述符。使用词袋模型[17]对影像的特征点进行分类。经过分类后的特征点在前后帧影像中进行快速匹配，进而求出前后帧影像间的相对位置关系，最终得到当前相机的位姿信息。然后用光流法[18]来跟踪其位置。如果跟踪成功，则进行本地地图的构建；如果跟踪失败，则等待着重定位或闭环检测。

2)构建局部地图线程：存储跟踪线程中获得的每1帧影像的三维地图点和关键帧到数据库，对关键帧进行特征匹配，如果匹配成功了，就采用光束平差法(bundle adjustment，BA)来优化估计三维地图点位置和关键帧位姿。从而构建环境的初始化地图。当相机运动到新的环境时，通过特征跟踪线程产生新的关键帧并添加到地图中，在关键帧数据库中新旧关键帧会建立关联并通过重投影得到新环境下的三维地图点，从而构建新环境的局部地图。

3)闭环检测与修正线程：把局部构图线程增加的三维地图点和关键帧作为闭环检测候选帧，在建立好的地图中对候选帧进行搜索并匹配，对相机进行重定位或闭环检测。为了减少ORB-SLAM算法中的前端误差积累，对整个算法进行有效的闭环修正。首先在本征图上利用通用图优化框架(general graph optimization，G2o)优化关键帧的位姿，将累积误差沿着图分散开；其次根据优化后的关键帧位姿矫正三维地图点云；最后更新地图点。

1.2 UWB定位算法原理

RTT测距是通过流动站到基准站之间信号传播的往返时间来确定二者间的距离。则RTT测距的模型可表示为

(1)

式中：ts和Xr为UWB流动站发出信号时的时间和位置；tr和Xb为UWB基准站接收信号时的时间和位置；tD为UWB流动站与基准站之间的标准时间偏差；‖·‖2为2范数；c为光速。那么，通过RTT测距得到的流动站和基准站之间的距离可表示为

d=‖Xr-Xb‖2=rRTT-rD

(2)

式中：rRTT为真实的流动站和基准站之间的距离；rD=ctD/2为标准时间偏差造成的测距误差。为了保证组合导航的精度，本文对实验过程中使用的UWB进行标准时间偏差标定，具体方法见文献[19]。

在实际UWB平面定位中，由于流动站和基准站之间的真实距离无法准确获得，所以本文用下式表示UWB基准站和流动站之间的真实距离为

(3)

(4)

Y=AX+V

(5)

式中：

2 单目视觉/UWB组合定位算法

单目视觉ORB-SLAM/UWB组合定位方法属于非线性系统，所以本文使用处理非线性问题的EKF算法。基于EKF的单目视觉ORB-SLAM/UWB组合定位模型的状态方程和量测方程构建如下。

在单目视觉ORB-SLAM/UWB组合方法中，以UWB定位过程中所使用的独立坐标系作为全局坐标系，将单目视觉ORB-SLAM解算出的位置信息经过空间变换，转换到UWB坐标系下。将UWB和单目视觉ORB-SLAM的数据进行融合，获得组合系统的位置和速度更新，其模型为

(6)

Xk=FXk-1+Wk-1

(7)

(8)

利用经过空间变换的单目视觉ORB-SLAM的位置信息与UWB解算出的位置信息的差值作为组合系统的量测信息。则组合系统的量测方程为

Yk=HXk+Vk

(9)

(10)

式中：I为单位矩阵；Rk为观测噪声协方差矩阵。根据状态方程和量测方程，通过EKF进行状态更新和量测更新得到组合系统的定位信息。

3 实验与分析

3.1 单目视觉/UWB平台搭建

移动定位平台使用的相机是Basler公司的Aca2500-20gc型号单目相机，镜头为FL-BC1218A-VG定焦镜头，焦距为12.5 mm，分辨率为1 296个像素乘1 024个像素。设置采样频率为1 Hz。使用的UWB是Time Domain的PulsON410通信与测距模块，其带宽为3.1～5.3 GHz，数据采样频率为1 Hz。在移动定位平台上，固定2种传感器，并在实验前对2个传感器的定位中心进行统一，即保证2个中心在同一条铅垂线上。对UWB和相机采集的数据都加上时间标签，保证这个时间标签和电脑的系统时间保持统一，在笔记本电脑上同时采集2个传感器的数据。具体平台如图2所示。

3.2 室内定位实验与分析

本文在教学楼中分别进行正常光照下小场地定位实验和弱光照下大场地定位实验。小场地定位实验，在实验室中进行，实验室的长宽分别为12和6 m，实验时间选择在中午，光照充足。利用4个UWB模块、移动定位平台、莱卡TS09全站仪和联想笔记本电脑进行组合定位实验。4个UWB模块分别作为基站布置在实验室的4角，已预先通过全站仪获取固定轨迹各拐点坐标和4个UWB基准站坐标，本文实验场景的详细布设如图3所示，矩形ABCD为固定轨迹的平面图。移动定位平台以接近匀速度沿着已知的固定轨迹运动，实验时间为3 min。分别比较单一传感器和组合系统的定位精度，实验结果如图4～图6所示。

对实验过程中采集到的UWB的数据进行解算，解算结果与固定轨迹进行对比，对比结果如图4所示。可以看出在正常情况下UWB能达到厘米级的定位精度，但当场景中出现墙或人遮挡等非视距情况时，定位精度出现较大的误差，最大误差能达到20 cm。

对实验过程中单目视觉ORB-SLAM算法解算的位置信息与固定轨迹进行对比，对比结果如图5所示。由于在转弯处相机场景变化比较大，导致单目视觉ORB-SLAM跟踪失败。所以，定位轨迹缺失左边部分。因为单目视觉ORB-SLAM算法本身在定位上存在累积误差，所以在轨迹的后半段发生漂移。其中最大误差为10 cm。

采用本文提出的单目视觉ORB-SLAM/UWB组合定位方法对实验数据进行解算，解算结果与固定轨迹对比，结果如图6所示。可以看出单目视觉ORB-SLAM/UWB组合定位方法解算的结果良好，未出现显著的跳变与误差累积，解决了单目视觉ORB-SLAM由于跟踪失败导致的无法定位的问题。从轨迹上看组合方法的最大定位误差是7 cm。但相比较于单一的UWB定位结果，误差有了明显的缩小。

表1给出了小场地下单目视觉ORB-SLAM、UWB以及单目视觉ORB-SLAM/UWB组合这3种定位方法在x和y方向上的误差统计。其中STD表示标准差，MAX表示最大值。可以看出UWB的定位精度相对较高，但是稳定性不好，易受到非视距的影响；单目视觉ORB-SLAM系统的稳定性不好，鲁棒性不强，易受到环境因素影响。单目视觉ORB-SLAM/UWB组合定位方法的精度能稳定维持在6 cm左右，相比较于单一传感器鲁棒性更好，定位精度更高，定位结果比较接近固定轨迹，可靠性强。

表1 3种定位方法结果误差统计 m

大场地定位实验，在报告厅中进行，报告厅的长宽分别为18和14 m，实验时间选择在傍晚，光照相对较弱。具体实验环境如图7所示，实验布置和小场地实验一样。通过全站仪获取固定轨迹坐标，移动定位平台以接近匀速度沿着固定轨迹运动，实验时间为8 min。

分别比较单一传感器和组合系统的定位精度，实验结果如图8所示。图中实线表示组合定位轨迹，点虚线表示单目视觉ORB-SLAM定位轨迹，虚线表示固定轨迹，点表示UWB定位轨迹。椭圆形区域是由于室内柱子遮挡而产生的NLOS误差，可以看出当场景中出现遮挡等非视距情况时，UWB的定位精度出现较大跳变和误差，最大误差能达到2 m。因为ORB特征提取算法具有一定的光照不变性，所以在弱光照下单目视觉ORB-SLAM仍可以进行定位。但由于算法在定位过程中的误差累计，在轨迹的后半段发生漂移，其中最大误差为11 cm。相比较于单一传感器，可以看出单目视觉ORB-SLAM/UWB组合定位方法解算的结果良好，有效克服了显著的跳变与误差累积，组合方法的定位轨迹更加接近固定轨迹，可靠性强。其最大定位误差是8 cm。

表2给出了大场地下单目视觉ORB-SLAM、UWB以及单目视觉ORB-SLAM/UWB组合这3种定位方法在x和y方向上的误差统计。可以看出由于NLOS的影响UWB的定位精度相对较差。单目视觉ORB-SLAM系统可以在弱光照下进行定位，但稳定性不好，易发生漂移。单目视觉ORB-SLAM/UWB组合定位方法的精度能稳定维持在7 cm左右，有效减弱了UWB非视距的影响，相比较于单一传感器鲁棒性更好，定位精度更高。

表2 3种定位方法结果误差统计 m

4 结束语

针对在室内环境下单目视觉ORB-SLAM，易发生跟踪失败导致定位失效的情况，本文提出了1种单目视觉ORB-SLAM/UWB组合的室内定位方法，并使用搭建的移动定位平台在室内环境下采集影像和UWB测距数据，对本文提出的方法进行实验验证。实验结果表明所设计的组合定位方法是切实可行的，利用该方法可以在ORB-SLAM跟踪失败的时候继续定位，提高了SLAM算法的稳定性。并且减少了NLOS造成的UWB定位误差，定位精度可达亚分米级。