基于SLAM/UWB 的室内融合定位算法研究

周韦，孙宪坤，吴飞

(上海工程技术大学 电子电气工程学院,上海 201620)

0 引言

随着传感器技术、计算机视觉、人工智能等技术的快速发展，许多智能移动机器人开始进入我们的生活[1].在这种情况下，移动机器人的自主定位显得十分重要，因为它是实现移动机器人路径规划、自主避障等功能的前提[2].由于移动机器人在室内导航时缺乏可靠的全球卫星导航系统(GNSS)[3-4]信号，因此相对于室外导航有着更多的挑战.

目前，室内移动机器人的定位主要通过惯性导航系统(INS)、激光雷达(LR)、视觉定位等来实现.但是这些定位方式由于其传感器感知能力的局限性并不能解决所有的定位问题.INS 在运行一段时间后会出现漂移，导致定位发散.LR 定位不适用于变化的环境，否则会造成激光扫描信息与地图不匹配导致定位失败，并且其价格较为昂贵.视觉定位[5]是通过即时定位与地图构建(SLAM)算法利用连续图像帧来估计相机的旋转与平移，因每次估计均存在误差，长时间或长距离运行后，会造成误差累计.综上所述，单一的室内定位技术无法实现移动机器人室内自主导航功能.为此许多专家学者将目光转向于多传感器融合[6-7]定位方向.

视觉SLAM 具有低成本、获取环境信息丰富等特点，不仅能够提供定位信息，而且能够构建相应的点云地图.由于误差累计的问题，需要一种能够提供绝对定位信息的定位方式对此进行修正，而超宽带(UWB)定位系统具有高带宽、穿透能力强等特点，可以提供绝对的位置服务，适合室内定位应用.但是在复杂的室内环境下，UWB 容易受到反射、多径效应、非视距(NLOS)等因素的影响，导致定位精度下降.针对这些问题，文献[8]提出一种基于单目视觉与UWB相结合室内定位方法，能够有效克制单目视觉ORBSLAM 跟踪失败和抑制UWB 非视距误差的影响，其定位精度可达到亚分米级，然而该算法并没有对视觉SLAM 失效的情况做处理，因此缺乏一定的稳定性；文献[9]提出一种基于UWB、里程计和RGB-D融合的室内定位方法，可以将定位误差抑制在10 cm以内，偏转角误差抑制在1°以内，彻底解决RGB-D摄像头SLAM 时跟踪失败问题，但是该方法没有考虑到UWB 的非视距误差的影响；文献[10]提出通过SLAM 提高UWB 定位增强无人机室内导航，能将90%分位数误差为20.5 cm 的UWB 定位精度提高到13.9 cm，实现无人机在环境中的有效导航，然而该方法使用权重的方式进行融合，并不能应对复杂的室内环境，以及长时间运行视觉SLAM 造成的累积误差的问题.

为了更好地解决移动机器人室内定位问题，本文采用扩展卡尔曼滤波(EKF)算法[11]将UWB 的全局绝对坐标和视觉SLAM 的相对位移增量进行融合，利用UWB 提供的全局定位信息补偿视觉SLAM 的累计误差.接着考虑到EKF 虽然能够处理大多数复杂且非线性的系统，但它要求测量噪声符合零均值的高斯分布.现实情况下，UWB 易受多径效应、非视距等因素影响，视觉定位易受图像纹理和清晰度的影响，其测量噪声并不完全符合高斯分布.因此在传统EKF 算法的基础上添加Sage-Husa 测量噪声估计器来减弱非高斯噪声对滤波器性能的影响，并添加阈值判断来识别和剔除测量中的异常值.通过实验表明：本文算法在有障碍物、小范围场景和大范围场景下都能良好运行，并且比传统EKF 算法拥有更高的定位精度和更好的鲁棒性.

1 UWB 与SLAM 定位算法

1.1 UWB 定位算法

UWB 定位系统采用基于信号达到时间差(TDOA)的算法，此算法通过标签向多个基站发射信号，根据标签信号到达每个基站的时间差，得到标签到各个基站的距离差，最后通过双曲线模型求出标签坐标[12].与到达时间(TOA)算法相比，此算法无需基站和标签之间实现时钟同步，只需基准进行时钟同步，因此具有通信次数少，定位精度高的优点.

其定位原理如图1 所示，UWB基站安装在固定位置且坐标已知，坐标分别为BSi(x1,y1)(i=1,2,3)，待求取的标签MS坐标为(x,y)，标签到基站的距离为di(i=1,2,3).

图1 TDOA 算法原理图

当所有基站实现时间同步后，标签发出定位信号.定义标签信号到达基站的时间为ti(i=1,2,3)，光速为c，可利用式(1)求出标签坐标.

1.2 视觉SLAM 定位算法

视觉定位采用ORB-SLAM2 算法，这是一套基于单目、双目和RGB-D 相机的开源SLAM 算法.它将经典的视觉SLAM 算法划分为三个线程：位置跟踪(Tracking)、建图(Local Mapping)和回环检测(Loop closing)，并通过System 线程完成定位与建图功能.文献[13]完整详细地介绍了该算法.算法的流程框图如图2 所示.

图2 ORB-SLAM2 流程框图

本文选用RGB-D 相机作为图像输入源，相比单目和双目相机，RGB-D 相机能够同时采集彩色图片和对应的深度图.既能解决单目视觉中的尺度不确定性的问题，也能省去双目视觉中计算左右相机视差的繁琐步骤，减少计算机运算量，保证实时性要求.

2 SLAM/UWB 融合定位算法

通过对UWB 和视觉SLAM 定位技术的介绍，可以得知单一的室内定位方式无法有效的保证其定位的精度和稳定性.因此本文对上述两类定位技术进行数据融合，形成优势互补，实现移动机器人在室内环境下高精度、高稳定性的自主定位.

2.1 时间同步与坐标系统一

如图3 所示，在进行数据融合之前，必须考虑到两种定位方式的时间同步与坐标系统一，因此在实验之前设计一个预处理阶段，此阶段分为两个部分：第一部分将两种定位方式的解算设备进行时间同步，并对SLAM 与UWB 的输出数据添加时间标签；第二部分通过坐标系转化的方式将ORB-SLAM2 的相机坐标系统一到UWB 坐标下.

图3 预处理结构框图

为求取ORB-SLAM2 的相机坐标系到UWB 坐标系的旋转和偏移矩阵，在时间同步之后需采集同一轨迹下一定样本数量的UWB 和SLAM 数据.记此轨迹下UWB 的坐标数据集为U，SLAM 的坐标数据集为S，使用迭代法求取两个数据集时间临近的点集，得到新的数据集U′={u1,u2,u3···,un}和S′={s1,s2,s3···,sn}，设相机坐标系到UWB 坐标系的旋转矩阵和平移矩阵为R和T.

则有公式

将新的数据集去质心化，得：

2.2 基于EKF 的数据融合算法

经过时间同步与坐标系统一后，运用EKF 滤波算法对SLAM 数据和UWB 数据进行融合，得到更加准确的定位信息.基于EKF 的状态方程和量测方程见式(7)～(8).

状态方程为

量测方程为

式中：Xk指状态向量；Yk指观测向量；wk-1表示过程噪声；δk表示观测噪声；f(·) 表示状态函数；h(·)表示观测函数.

则融合定位系统的状态方程为

F为状态转移矩阵，Δt为数据采用间隔，则有

由于视觉SLAM 定位输出频率远低于UWB 定位，无法实现同步输入观测值.于是本文将两种定位方式的观测值分别代入EKF 滤波器中，即EKF 中存在两种量测方程.

则SLAM 的量测方程可表示为

UWB 的量测方程可表示为

EKF 算法包括状态更新和量测更新，具体过程如下：

状态更新：

式中：Pk/k为第k时刻误差协方差预测；Pk为误差协方差矩阵；Ak为状态转移矩阵，是状态函数f(·)雅克比矩阵；Qk为过程噪声的协方差矩阵.

量测更新：

式中：Kk为卡尔曼增益；Rk为过程噪声协方差矩阵；Hk为量测转移矩阵，是量测函数h(·)的雅克比矩阵,Hk根据输入数据的定位类型选择或者.

2.3 测量噪声估计与阈值判断

传统的 EKF 在迭代过程中要求噪声服从零均值的高斯分布，即Qk～N(0,Q)，Rk～N(0,R).其中Qk主要与系统预测模型有关，Rk主要与传感器测量数据有关.考虑到SLAM 和UWB 定位时均易受环境影响，其测量噪声Rk并不完全符合高斯分布，这将导致EKF 的估计误差增大甚至发散.为此本文在EKF基础上添加Sage-Husa 噪声估计器和阈值判断.一方面，噪声估计器可以不断调整测量噪声强度大小；另一方面，通过添加阈值判断机制，删除严重偏移实际位置的测量数据，实现对滤波器的调优.

当固定Qk的情况下，使用Sage-Husa 算法对系统测量噪声的协方差[15]估计为：

式中，b表示遗忘因子，取值范围为 0.95～0.99，具体数值可通过实验获得.

为剔除异常的测量数据，在融合之前首先比较测量值与状态向量的估计值，将二者的差值与提前设置的阈值进行相减，超过阈值将使用上一次的状态估计代替.

结合阈值判断后，EKF 中的式(15)被修改为

式中，thresh为提前选定的阈值.最终的室内融合定位算法主要步骤如图4 所示.

图4 融合定位算法框架图

3 实验与结果分析

3.1 实验说明

为了验证SLAM/UWB 的融合定位算法的效果，搭建室内测试场景，选用智能移动小车作为室内移动机器人.该车搭载了因特尔NUC 迷你电脑，并运行ROS 操作系统.UWB 定位传感器使用上海物联网公司的硬件设备，其输出频率可达50 Hz；视觉传感器选用乐视LeTMC-520 型号的RGB-D 相机，支持每秒30 帧的RGB 和深度图像输出，其深度测距每米误差小于3 mm.将UWB 标签和RGB-D 相机固定在智能小车上，保持标签与相机相对静止.

3.2 实验与结果分析

在实验楼中分别进行小场地定位实验和大场地定位实验.按照图5 所示搭建室内测试场景，小场地实验在实验室内进行，实验室的长宽分别为14 m 和8 m.根据实验室场地大小，首先将4 个UWB基站架设在场地的4 个角落，然后在内部选择矩形ABCD作为智能小车移动的轨迹，最后在BC和CD段放置3 个障碍物，模拟UWB 的NLOS 路段.其中小车行进的固定轨迹作为参考值与融合算法的输出轨迹进行对比.

图5 实验场景

对实验过程中采集到的UWB 数据进行解算，并与参考轨迹进行对比，对比结果如图6 所示.可以得出在BC和CD段，由于小车行进途中存在障碍物的遮挡，引起NLOS 误差，导致UWB 定位精度下降且出现明显的抖动，定位误差最大为0.79 m，平均定位精度为0.21 m.而AB段和DA段受NLOS 误差影响较小，其定位轨迹接近参考轨迹，最大定位误差为0.51 m，平均定位精度为0.06 m.

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

图7 为了RGB-D 相机通过ORB-SLAM2 算法解算的轨迹.如图7 所示，在AB段和BC段前半部分视觉SLAM 解算的轨迹接近真值.但是由于累计误差的存在，BC后半段解算的轨迹开始偏离真实轨迹.在经过点C后，解算轨迹与真实轨迹形成一定的夹角，定位误差也越来越大，最大定位误差为1.65 m.

图7 SLAM 轨迹与参考轨迹对比

使用本文提出的SLAM/UWB 融合定位算法进行解算，其结果如图8 所示.可以得出其解算轨迹无论是在有障碍的BC和CD段，还是无障碍的AB和DA段都接近真值.其中最大定位误差为0.34 m，平均定位精度为0.10 m.

图8 SLAM/UWB 融合轨迹与参考轨迹对比

表1 给出了UWB、SLAM 以及SLAM/UWB 融合定位在每段路径中的误差统计.经过分析可知，在无遮挡的室内环境下，UWB 定位精度可达厘米级，在有遮挡的情况下，也能达到分米级定位.视觉SLAM 在初始阶段具有厘米级的定位精度，但随着误差累计，定位结果逐渐偏离实际轨迹.SLAM/UWB融合定位算法在全路径中定位精度最高，即使在有遮挡的复杂环境下，也能达到0.14 m 的定位精度.

表1 单一定位与本文融合定位误差对比 m

使用传统EKF 算法将视觉SLAM 与UWB 定位数据进行融合，其融合结果如图9 所示.结果表明，在观测良好的环境下，EKF 算法解算的结果能够很好地接近真实轨迹.但在UWB 存在NLOS 误差的情况下，定位误差变大.传统EKF 算法与本文算法解算的轨迹和真实轨迹误差曲线如图10 所示，在前期AB段和BC段两种融合算法效果相差不大，传统EKF 算法部分优于本文算法，但是在后期CD段，传统EKF 算法误差峰值远远大于本文算法，同时本文算法轨迹也更加平滑.两种融合算法的定位精度如表2 所示，本文算法平均定位精度相比传统EKF 算法提高的0.03 m，最大定位误差减少0.25 m.

图9 EKF、本文算法与真实轨迹对比

图10 EKF、本文算法与真实轨迹误差曲线

表2 小场地下两种融合定位算法误差对比 m

大场地定位试验在实验室外的走廊进行，走廊的长和宽分别为32 m、8 m，其形状呈现“冂”字形.实验布置与小场地试验类似，在四个角架设UWB基站，并在智能小车行进的轨迹旁放置障碍物.图11 为大场地下UWB 定位、SLAM 定位与参考轨迹对比，可以看出UWB 定位在BC段的两端，由于NLOS 误差，定位结果出现较大跳变和抖动，最大误差可达1.08 m，其平均定位精度为0.21 m.SLAM 定位在B点转弯处由于图像变化较快，导致误差累计，其结果逐渐偏离参考轨迹，最大定位误差为1.96 m，平均定位精度为0.82 m.

图11 大场地单一定位方式实验结果

图12 为大场地下比较本文融合算法和传统EKF融合算法，从图中可以看出本文融合算法比传统EKF融合的轨迹更加平滑，尤其是在BC段两端UWB 出现NLOS 误差的情况下，定位结果也更接近参考轨迹.表3 给出了两种融合算法的误差统计，本文算法的定位精度能够达到0.15 m，相比于传统EKF 算法拥有更高的定位精度和更好的鲁棒性.

图12 大场地两种融合定位实验结果

表3 大场地下两种融合定位算法误差对比 m

4 结束语

本文利用视觉SLAM技术和UWB技术，针对目前室内移动机器人单一定位方式精度低、稳定性差的问题，提出一种SLAM/UWB 融合定位算法.该算法在传统EKF 滤波基础上添加测量噪声估计器和阈值检测，提升了融合定位系统在复杂环境下的稳定性.实验结果表明：其定位精度优于单一视觉SLAM或UWB 定位；当UWB 出现NLOS 影响时，该算法比传统EKF 算法拥有更稳定的定位效果.综上，本文融合定位算法能够同时满足室内移动机器人对定位精度和稳定的要求.