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

徐爱功，闫可新，高 嵩，宋 帅，隋 心

（1. 辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000；2. 广州南方测绘科技股份有限公司 南方高铁研发中心，广州 510000）

0 引言

随着人们对定位服务需求的提高，室内定位技术成为研究热点[1]。在室内环境下，由于信号的遮挡，全球卫星导航系统（global navigation satellite system，GNSS）及与其相关的组合定位系统无法进行有效定位[2]。在此环境下，视觉传感器作为无源传感器，是定位常用的一类传感器，具有成本低廉、轻巧便携、安装简单、无需先验信息且数据信息丰富等优点，因此可以在弱GNSS 信号环境下进行定位[3]。文献[4]首先提出了视觉里程计（visual odometry，VO）一词，指通过连续的图像作为输入估计自身的运动，是同时定位与地图构建（simultaneous localization and mapping，SLAM）技术的重要组成部分。按照相机的种类，VO 可分为单目VO 和立体VO。相比于立体VO，单目VO计算量小且标定简单，应用更加广泛[5]。按照实现方法可分为基于特征点法的VO 和直接法的VO[6]。相比于直接法VO，基于特征点法的VO 对光照变化和动态场景鲁棒性强，不易受到光照和噪声的影响。因此本文选用基于特征点法的VO 进行单目视觉的定位。然而单目VO 算法属于相对定位方式，本身无法估计绝对尺度，在特征单一的场景下，其定位精度较低，容易跟踪失败且误差随时间累积。超宽带（ultra-wide band，UWB）[7-8]没有场景纹理特征限制且没有误差累积问题，在视距（line of sight，LOS）环境下，能达到厘米级甚至毫米级的测距定位精度。UWB 有多种定位方式，其中基于往返时间（round trip time，RTT）方法，通过对时间的测量间接计算出UWB 基准站与流动站之间的距离，不需要增加其他硬件，没有时间同步误差[9]。因此本文选择RTT 方法进行UWB 的定位。但是UWB 属于有源定位方法，在非视距（non line of sight，NLOS）环境下，由于障碍物的遮挡，信号无法直线传输，会产生折射、反射或透射现象，使UWB 定位精度和可靠性大幅降低[10-12]。

综合上述分析，认为在复杂室内环境下单独采用单目视觉传感器或UWB 进行定位很难得到可靠、高精度的定位结果，因此本文采用二者组合方式进行定位[13]。文献[14]提出将单目视觉ORB-SLAM算法输出的位置信息与UWB 解算的定位信息作为量测信息，通过扩展卡尔曼滤波（extended Kalman filter，EKF）进行数据融合来实现室内定位，但该文没有对UWB 的NLOS 误差进行鉴别与剔除。文献[15]提出了一种基于EKF 的单目视觉与UWB 融合定位的算法，利用UWB 流动站的平面坐标实现了单目视觉尺度因子的实时计算，适用于纹理稀疏、光亮度变化频繁的室内环境中的行人定位，两类传感器数据组合方式为松组合。文献[16]提出了一种通过单目视觉SLAM 增强UWB进行无人机室内导航的方法，实现无线定位未覆盖区域的自主飞行，在这种方法中，VO 被用来估计高度，然而飞行区域图是多次飞行建立的。文献[17]提出了一种利用惯性传感器、单目视觉和UWB，通过EKF 进行定位的方法，该方法用UWB 来辅助视觉惯性同步定位和制图，以获得改进的无漂移全局6 个自由度位姿估计，虽然结果精度较高，但使用3 个传感器增加了成本和融合的复杂度。基于以上分析，本文建立一种单目VO/UWB 组合的定位方法，将单目VO 的位移增量和UWB 的测距信息作为量测值，利用EKF 算法进行参数解算。该组合定位方法利用单目VO 推算的距离信息剔除UWB 的NLOS 误差，利用UWB 恢复单目VO尺度，在其中一种传感器定位精度降低或无法定位时仍能保证定位的精度。

1 单目VO/UWB 组合定位方法

在室内定位中，由于平面定位的需求多于高程定位[18]，且高程定位所需基站多，成本高，不适用于实际应用，所以本文采用平面定位。

在单目VO/UWB 组合定位方法中，以UWB坐标系作为组合系统的坐标系，将单目VO 量测的位移增量经过空间变换转换到UWB 坐标系下。

1.1 单目VO 尺度因子估计

由于单目VO 无法直接测量尺度，在利用开始的前几帧图像进行初始化后，只能得到一个相对的尺度。因此，本文利用UWB 在这几帧运动过程中获得的绝对位置信息，推算得到的帧间绝对距离值，通过最小二乘算法对单目VO 进行绝对尺度因子的估计。

在组合移动平台初始运动阶段，平台从静止状态开始以较低的速度匀速向前行进，单目相机和UWB 按照设置的频率采集数据，单目相机得到一组等时间间隔的图像并进行初始化，在VO 中处理后得到相机在两帧之间的相对距离估计值dc，同时UWB 通过得到两帧之间的绝对距离值dr，则单目VO 的尺度因子s可表示为

上述过程存在UWB 的观测误差和单目相机的图像特征匹配误差，整体误差近似服从高斯分布，通过增加观测数据量的方式可以减小误差的影响。因此，本文用n组观测数据对单目VO 的尺度因子进行估计，分别计算每组数据的尺度因子，取平均值作为最终的尺度因子，具体表示为

利用上述方法可以消除一定的随机误差，特别在前后两组估计出的尺度因子相差较大时，UWB的观测误差还是会对结果造成影响。因此，本文利用最小二乘方法进行单目VO 尺度因子的估计，当误差最小时，认为估计出的尺度因子为正确值。

根据n组观测数据及式（1）得

通过该方法可以估计出单目VO 的尺度因子，这样既解决了误差的影响，又为其恢复了绝对尺度。在实际应用中，如果初始化时间较短，可以采用等权的方式进行最小二乘解算；如果初始化时间较长，同时考虑单目VO 本身的尺度漂移影响，应对不同的观测数据采用不同的权值，然后利用最小二乘算法进行尺度估计。

1.2 UWB 非视距误差鉴别

当UWB 定位出现NLOS 误差时，RTT 测距值会发生较大变化，如果不对NLOS 误差进行处理，会大幅降低单目VO/UWB 组合定位系统的精度及稳定性。单目VO 通过对图像序列进行预处理、初始化、位姿估计、跟踪局部地图等操作，得到相机每个关键帧的位姿。当在运动过程中，相机速度过快或在特征单一、光照不均匀、景深变化明显的环境下定位时，可能会发生跟踪失败的现象，此时单目VO 的尺度因子会发生大幅变化，根据前后历元的位移增量可以很容易对其鉴别。在跟踪正常情况下，单目VO 的位移增量是相对准确的，因此本文借助单目VO 的位移增量，对UWB 的RTT 测距值进行NLOS 误差鉴别。

根据单目VO 的坐标增量以及组合系统上一历元的坐标，计算当前历元组合系统的坐标，进而推算出组合系统与UWB 基准站间的距离，即

1.3 单目VO/UWB 组合定位算法

由于单目VO/UWB 组合是非线性系统，本文采用处理非线性系统的EKF 算法进行数据融合。

1.3.1 状态方程

单目VO/UWB 组合的状态方程为

图1 单目VO/UWB 组合定位流程

2 实验与分析

2.1 实验说明

在本次室内实验中，组合移动平台使用的相机是德国巴斯勒（Basler）公司生产的Aca2500-20gc型号工业单目相机，曝光时间设置为20 ms，光圈大小设置为F2.8，图片分辨率设置为648×512。组合移动平台中使用的 UWB 是美国泰姆·多曼（Time Domain）公司生产的PulsON440 测距模块，其带宽为3.1～4.8 GHz。实验场景为教学楼内某会议室，如图2 所示。

图2 室内实验场景

实验前首先将相机和UWB 固定在移动平台上，通过标定外参数完成空间坐标系统统一，然后利用计算机对二者添加时间标签，进行时间同步。实验过程中设置相机和 UWB 的采样频率同为2 Hz，相机通过吉格·埃（GigE）接口、UWB 通过USB 接口同时向计算机传输数据。将徕卡（Leica）棱镜通过基座固定在移动平台上，组合定位实验平台如图3 所示。

图3 单目相机/UWB 组合定位实验平台

实验前将5 个UWB 基站安装在三脚架上，分布于室内各个角落，利用全站仪自动跟踪棱镜确定其真实坐标，利用7 个移动电源为1 个单目相机和6 个UWB 设备供电。设计好行进轨迹，实验时移动平台按照线路顺时针由静止开始匀速前进，利用全站仪自动跟踪观测移动平台的位置作为参考轨迹，本次实验轨迹总长度为31 m，实验时长为5 min，实验设计示意图如图4 所示。

图4 实验设计示意

2.2 实验结果与分析

对实验过程中单目VO 解算的结果与参考轨迹进行对比，结果如图5 所示。

图5 单目VO 解算轨迹与参考轨迹对比

从图5 可以看出，整体轨迹与真实轨迹较为接近，然而由于在转弯处相机景深和光照强度变化较大，导致单目VO 定位出现较大误差；由于单目VO 定位存在累积误差，导致定位误差逐渐增大，且最终无法回到起点，最大定位误差为35.6 cm。

对实验过程UWB 解算的结果与参考轨迹进行对比，结果如图6 所示。

图6 UWB 解算轨迹与参考轨迹对比

从图6 可以看出，在视距良好的情况下，UWB的定位精度能够达到厘米级，解算轨迹与参考轨迹无较大偏差。在有2 个柱体或人为遮挡的时候，非视距情况的出现导致定位结果存在较大误差，最大定位误差为29.5 cm。

将本文提出的单目VO/UWB 组合算法解算的轨迹与参考轨迹进行对比，结果如图7 所示。

图7 单目VO/UWB 组合解算轨迹与参考轨迹对比

从图7 可以看出，在有障碍物遮挡时，由于利用单目 VO 的位移增量鉴别并剔除了 UWB 的NLOS 误差，轨迹贴近参考轨迹；由于UWB 测距信息的加入，减小了单目VO 的误差累积。最大定位误差为11.4 cm。相比较于单一传感器，可以看出单目VO/UWB 组合定位方法解算的结果良好，有效地减弱了UWB 的非视距误差对定位结果的影响，减小了单目VO 的轨迹漂移。组合方法的定位轨迹更加接近参考轨迹，可靠性强。

单目VO 定位、UWB 定位以及单目VO/UWB组合定位方法在x和y方向上的误差如图8 所示。

图8 单目VO，UWB 和单目VO/UWB 组合三者误差

分析图8 可知，误差在各个方向上的趋势具有一致性。当出现非视距情况时，UWB 定位出现较大误差，误差的幅度大于单目VO 定位方法和单目VO/UWB 组合定位方法；在视距良好时，单目VO定位误差的幅度大于 UWB 定位方法和单目VO/UWB 组合方法。在整个实验过程中，单目VO/UWB 组合定位误差的幅度均小于单目VO 定位方法和UWB 定位方法。

表1 给出了在室内场景下，单目VO、UWB 以及单目VO/UWB 组合这3 种定位方法的结果在x和y方向上的误差统计。表1 中：RMSE 为均方根误差；MAX 为误差最大值。

表1 三种定位方法结果误差统计 单位：m

从表1 可以看出：1）单目VO 的定位精度相对较差，x方向的平均误差为10.3 cm，y方向的平均误差为6.6 cm，x方向最大误差为33.9 cm，y方向最大误差为13.8 cm；2）UWB 的定位精度相对较高，x方向的平均误差为4.1 cm，y方向的平均误差为4.7 cm，但易受到非视距的影响，x方向最大误差为29.3 cm，y方向最大误差为9.9 cm；3）单目VO/UWB 组合定位方法x方向的平均误差为3.6 cm，y方向的平均误差为3.5 cm，x方向最大误差为9.5 cm，y方向最大误差为8.4 cm，定位精度能稳定维持在8 cm 左右，相较于单一传感器鲁棒性更好，定位精度更高，可靠性更强。

本次实验结果表明，本文所设计的组合定位方法是切实可行的，该方法可以有效解决单目VO 定位中的误差累积问题，提高了VO 算法的稳定性，并解决了UWB 的NLOS 误差问题，提高了UWB定位结果的鲁棒性，组合定位精度可达亚分米级。

3 结束语

针对室内环境下单目VO 定位缺少绝对尺度且存在误差累积、UWB 定位受NLOS 误差影响的等问题，本文提出了一种基于 EKF 的单目VO/UWB 组合的室内定位方法，将单目VO 的位移增量和UWB 的测距信息作为量测值，通过EKF算法进行数据融合并进行了室内实测实验。实验结果表明，单目VO/UWB 组合方法减小了单目VO的误差累积，消除了UWB 的NLOS 影响，相较于单一传感器提高了整体定位的精度及鲁棒性。