移动机器人超宽带与视觉惯性里程计组合的室内定位算法

申炳琦，张志明，舒少龙

（1.同济大学 电子与信息工程学院，上海 200092；2.浙江大学 控制科学与工程学院，杭州 310027）

0 引言

室外环境中的移动机器人定位主要依赖于全球导航卫星系统（Global Navigation Satellite System，GNSS）进行定位，但在室内环境中，由于GNSS 的信号被建筑阻隔，无法实现机器人的准确定位，故而需要借助其他技术如无线传感器网络进行定位。

为解决GNSS 信号缺失环境下的定位问题，同时定位与地图构建（Simultaneous Localization And Mapping，SLAM）近年来取得了巨大的发展［1-2］。目前，根据机器人所搭载的传感器不同，可将SLAM 技术分为传统的SLAM 技术与视觉SLAM（Visual SLAM，VSLAM）技术。近十几年来，随着计算机视觉理论技术的发展，视觉SLAM 凭借硬件成本低廉、轻便、高精度等优势获得了学术界及工业界的青睐［3］。在此基础上引入惯性测量元件（Inertial Measurement Unit，IMU），克服纯视觉导航系统在尺度不确定和纯旋转三角化算法退化等场景的缺陷，弥补IMU 测量误差随时间推移而积累的弊端，使得两种方式特性互补［4］，提供一种鲁棒性更强的室内移动机器人定位策略，即视觉惯性里程SLAM（Visual-Inertial-Odometry SLAM，VIO-SLAM）。近年来，学术界在VIO-SLAM 领域取得了许多突破：Mourikis 等［5］提出一种基于紧耦合滤波的方法，该方法解决传统VIO 方法维数过多的问题；Leutenegger 等［6］提出了一种视觉惯性融合方法OKVIS（Open Keyframe-based Visual-Inertial SLAM），该方法使用了关键帧与非线性优化估计位姿，其建图效果较好，但未能加上回环检测实现对全局的优化，所以定位误差将随着时间有所累积；Qin 等［7］提 出VINS-Mono（Visual-Inertial Navigation System Mono）算 法，其前端使用GFTT（Good Features To Track）特征点提取加上多层KLT（Kanade-Lucas-Tomasi）光流跟踪，后端采用关键帧和滑动窗口的优化手段，加入回环检测，具有较好的全局一致性。

与此同时，以无线信号强度作为主要参数的无线定位技术也得到快速发展，包括无线局域网（Wireless Local Area Network，WLAN）、射频识别（Radio Frequency IDentification，RFID）、蓝牙、ZigBee 等；但其易受外界干扰而较不稳定，难以满足室内移动机器人鲁棒定位的需求［8］。近年来在室内无线定位研究中，超宽带（Ultra Wide-Band，UWB）技术因其能获得相对高的定位精度而在学术界与工业界引起关注。UWB 技术依靠高频非正弦窄脉冲传输数据，占据很宽的频带，是一种具有低功耗高数据速率的无载波通信技术。Prorok 等［9］首次提 出了一 种UWB 到达时间差（Time Difference Of Arrival，TDOA）的测量模型，得到UWB 定位误差在空间上的多模态统计特征；Hanssens 等［10］首先测量了UWB 信号传播过程中的几何特征，如信号到达时延、信号离去角与到达角以及上述参数方差等。

UWB 属于有源定位方法，由于多径效应、非视距（Non-Line Of Sight，NLOS）等因素的影响，单靠UWB 系统无法为室内移动机器人提供稳定、可靠的定位信息；VIO-SLAM 属于无源定位方法，其定位误差会随着时间的推移而累积，无法为室内移动机器人提供长期可靠的定位。为克服上述两种定位方法的不足，文献［11-12］中构建了一套基于优化的紧耦合框架，将VIO 测量数据与UWB 测量数据融合，以进行机器人的相对位姿估计；同样，文献［13］中构建了一套基于优化的松耦合框架，通过实验证明UWB 测量可以克服纯视觉定位产生的漂移，一定程度上提高了系统的鲁棒性。受到上述文献的启发，不同于其构建基于优化的框架，本文在松耦合数据融合框架中，将UWB 与VIO 的定位信息与滤波相结合，为移动机器人在复杂条件的室内环境中提供长期、准确、可靠的实时定位策略。融合后的UWB 数据可以对视觉惯性里程计（Visual Inertial Odometry，VIO）产生的累积误差进行校正，保证定位的精度；而VIO 可以对UWB 测量数据进行平滑处理，补偿多径引起的缺陷，降低非视距误差的影响。

1 UWB与VIO组合定位算法原理

针对组合定位问题，首先将对所采用的UWB 定位算法以及VIO 定位算法原理进行分析介绍，然后采用滤波算法对二者定位结果进行数据融合，以得到各时刻下移动机器人位置的最优估计值。

1.1 UWB三边定位算法

对于二维情况下的定位问题，基于UWB 三边定位算法原理，即在已知3 个UWB 基站坐标的前提下，根据目标物体（移动机器人）到3 个UWB 基站的距离，对所在坐标进行解算。系统设计如图1 所示，UWB 基站与标签之间的测距信息汇总至基站A 中，并通过串口上传至上位机平台，进行实时的位置计算及轨迹绘制等任务。

图1 UWB定位系统Fig.1 UWB positioning system

UWB 定位包括测距及坐标解算两部分，其中测距采用双边双向测距（Double-Sided Two-Way Ranging，DS-TWR）算法［14］，该算法的原理如图2（a）中所示：设备A 主动发送TX数据，同时记录发送时间戳；设备B 在接收到数据的同时记录接收时间戳；延时Treply之后，设备B 发送数据，同时记录发送时间戳；设备A 收到数据后，同时记录接收时间戳，并在此基础上再增加一次传输，以此来计算设备A 与设备B 之间UWB 信号的飞行时间Tprop，之后乘以光速c 即可得到设备A、B 之间的距离d。二维情况下可通过三边定位法解决，其原理如图2（b）中所示：在已知3 个UWB 基站BS1、BS2、BS3 坐标的前提下，可根据目标物体到3 个UWB 基站的距离d1、d2、d3，列写方程组，对目标物体的坐标进行解算。

图2 UWB定位算法原理Fig.2 Principles of UWB positioning algorithms

1.2 VIO定位算法原理

目前，紧耦合框架下的两种主流VIO 算法分别为多状态约束下的Kalman 滤波器（Multi-State Constraint Kalman Filter，MSCKF）算法与视觉惯性导航系统（Visual-Inertial Navigation System，VINS）算法。MSCKF 在EKF 框架下融合IMU 和视觉信息，相较于单纯的视觉里程计（Visual Odometry，VO）算法，MSCKF 可以适应更剧烈的运动、并在一定时间的纹理缺失情况下鲁棒性更强；而根据Jeon 等［15］与Ma 等［16］的实验验证，与VINS 等基于优化的VIO 算法相比，MSCKF 精度略逊但速度更快，适合在计算资源有限的嵌入式平台运行。故而在本文设计中，采用MSCKF 算法的双目版本——立体视觉多状态约束下的Kalman 滤波器（Stereo-Multi-State Constraint Kalman Filter，S-MSCKF）算法［17］原理进行基于VIO 的室内定位算法设计。

与多数视觉SLAM 算法类似，S-MSCKF 算法可分为前端视觉与后端滤波两部分。前端视觉主要用于检测图像中的特征点，并向后端发布特征点在相机坐标系的坐标等数据，在此过程中需对提取到的特征点进行追踪并剔除双目误匹配点及前后帧误匹配点。前端视觉的算法流程如图3（a）所示。

图3 VIO定位模块S-MSCKF算法流程Fig.3 Flow of VIO positioning module S-MSCKF algorithm

算法流程主要包括以下关键环节。

1）初始化第一帧。双目相机在获取第一帧图片后，首先检测左目图片中的加速分割测试特征（Features from Accelerated Segment Test，FAST）特征点，根据左右目相机之间的内参将左目特征点投影至右目中进行双目特征点匹配；然后通过图像金字塔进行LK（Lucas-Kanade）光流追踪、对极几何约束等处理，剔除追踪到图像外的特征点；最后将特征点进行网格划分并输出FAST 特征点信息。

2）跟踪特征点。在相机移动的过程中会陆续接收到不同帧图像，需在下一帧图像中对上一帧图像提取到的特征点进行跟踪，并提取新的特征点。该阶段同样利用对极几何约束、LK 光流追踪以及Two-points RANSAC（RANdom SAmple Consensus）法不断剔除左右目误匹配特征点以及超出图像边界的特征点。

3）添加新特征点。随着时间流逝，视频图像中的某些特征会消失，某些特征可能也会存在累计误差，所以在持续追踪过程中势必要添加一些新的特征来保证程序能够一直运行。

S-MSCKF 后端的主要功能在于根据视觉前端传来的特征点坐标，与IMU 状态进行扩展卡尔曼滤波，从而得到相机当前位姿的最优估计值。首先通过订阅相关话题接收前端图像特征点信息，再初始化重力及偏差等相关量；然后处理IMU 数据以构建微分方程组中的状态矩阵以及输入矩阵，并求得状态转移矩阵；之后依次进行状态预测、状态扩增以及状态更新，移除历史相机状态数据，得到当前相机状态；并最终发布相机的位姿信息话题。

1.3 UWB-VIO组合定位算法

为得到长期运行后仍准确可靠的定位数据，需对UWB 与VIO 分别输出的定位数据进行融合解算。传感器信息融合方法主要包括加权平均法、卡尔曼滤波法、贝叶斯推理法、神经网络算法等。其中加权平均法简单直观，但很难得到最优加权平均，且计算最优加权平均值需要大量时间；贝叶斯推理信息被描述为概率分布，需要先验概率和似然函数，分析计算复杂；神经网络算法根据输入数据样本训练和调整网络的权重，但训练阶段需要大量数据，实时性较差［18］。考虑到本文搭建的UWB-VIO 组合定位系统为非线性系统，同时为避免可能的协方差矩阵奇异性及万向锁等问题［19］，选用处理非线性问题的误差状态扩展卡尔曼滤波（Error State-Extended Kalman Filter，ES-EKF）算法［20］进行传感器的信息融合。

UWB-VIO 组合定位算法中，以UWB 定位过程中所使用到的独立坐标系作为VIO 定位的世界坐标系。然后以移动机器人的位置与速度作为系统的状态向量，假设UWB 定位观测值与VIO 定位观测值在k时刻进行时间戳对齐，则k时刻的状态向量即，其中(xk，yk)代表第k时刻组合系统在二维平面上的位置坐标；和分别代表第k时刻组合系统在x方向和y方向上的速度。假设k时刻的状态向量的标称值为，误差值为δXk，Xk=+δXk。则该系统的误差状态模型可表示为式（1）：

其中：t为组合系统的采样间隔时间；分别代表第k-1 时刻组合系统在x方向和y方向上的加速度。ES-EKF 算法流程如图4 所示。

图4 ES-EKF算法流程Fig.4 Flow of ES-EKF algorithm

以接收到组合系统的VIO 定位数据为状态向量标称值，之后进行误差状态更新。对式（1）进行整理，则对k时刻误差状态及协方差的预测如式（2）所示：

其中：F为系统的状态矩阵，Wk-1为系统的噪声向量，分别定义为：为k-1时刻误差状态协方差的最优估计；Qk-1为k-1 时刻的噪声协方差。考虑到组合系统的状态向量仅用于追踪系统的位置与速度，故本文将加速度作为系统的随机噪声。

以UWB 与VIO 得出的定位信息差值作为该系统的观测向量Yk，则该组合系统的观测方程为：

之后可进行误差状态及协方差矩阵的更新，包括计算卡尔曼增益Kk，更新误差状态δXk|k以及更新协方差矩阵Pk|k。具体方程如式（4）所示：

其中：Rk为k时刻的观测协方差。而后根据Xk=+δXk得到k时刻真实状态的最优估计值结果。该算法的伪代码原理如下所示。

过程：

以VIO 定位数据作为标称值；

令k时刻状态向量误差值δXk=0；

计算时间间隔：dt=tk-tk-1；

根据式（1）（2）对k时刻状态向量的误差值及协方差矩阵进行预测，计算得到δXk|k-1及Pk|k-1；

根据式（3）得到系统在k时刻的观测向量Yk；

根据式（4）对k时刻状态向量的误差值及协方差矩阵进行更新，计算得到δXk|k及Pk|k；

令δXk=δXk|k根据Xk=+δXk，计算得到k时刻状态向量的最优估计；

输出k时刻系统的最优状态估计Xk

2 实验与结果分析

搭建实验平台，选择实验场地进行实验，获取定位结果，并根据结果分析造成误差的原因，得出结论并验证算法的有效性。

2.1 UWB-VIO组合定位实验平台搭建

以UGV 无人地面小车为平台，操作系统采用Ubuntu16.04+ROS Kinetic 进行软件设计；UWB 模块采用广州联网科技有限公司设计的D-DWG-PG2.5 UWB 标签和UWB基站模块，采用棒状3.1～4.9 Hz 全向高频天线，通信距离可达130 m，测距精度约10 cm，定位精度约30 cm；VIO 模块为Intel T265，包括一组双目鱼眼镜头（OV9282）；同时集成硬件同步的200 Hz 陀螺仪和62.5 Hz 加速度计，根据S-MSCKF 算法计算模块在3D空间中的方向与位置，从而实现六自由度追踪。该UWB-VIO组合定位系统的整体架构如图5所示。

图5 UWB-VIO组合定位系统整体架构Fig.5 Overall architecture of UWB-VIO integrated positioning system

2.2 室内定位实验场地选择

分别在无障碍小场地与有障碍大场地进行定位实验。无障碍小场地选在同济大学嘉定校区华楼一楼大厅，有障碍大场地选在华楼二楼大厅，其中大场地在定位过程中有室内支柱作为障碍物干扰，而小场地无障碍。实验时间均选择在14：00 左右，此时光照环境良好，利于实验的开展。本次实验采取4 个UWB 模块，包括3 个坐标已知的基站以及1 个待定位的标签。3 个UWB 基站摆放位置尽量两两等距，以确保UWB 定位的精确性。室内移动机器人以接近匀速运动的方式沿着已知的固定轨迹进行运动。实验现场以及机器人行驶轨迹如图6 所示。

图6 实验现场场景Fig.6 Scenes of experiment sites

2.3 实验结果分析

对两种定位方法得到的数据进行时间同步等一系列数据处理操作后，得到基于UWB 与VIO 的单一定位以及组合定位的定位结果对比结果。

2.3.1 无障碍小场地定位实验

在无障碍小场地定位实验中，机器人的移动过程并未受到室内支柱等障碍物的干扰，3 种定位方法的结果对比如图7 所示。定位误差指标对比如表1 所示。

表1 无障碍小场地下三种定位算法结果误差对比Tab.1 Error comparison for results of different positioning algorithms in obstacle-free small site

图7 无障碍小场地下的定位效果对比Fig.7 Positioning effect comparison in obstacle-free small site

在这种室内较为空旷的情况下，仅靠UWB 的室内定位方法便可达到厘米级的定位精度；而VIO 定位方法在初期阶段定位结果令人满意，但随着定位过程的推进，因累计误差的存在使得其在轨迹的后半段定位结果发生漂移，致使定位结果误差过大。此时组合定位结果精度劣于UWB 单一定位方法，但优于VIO 单一定位方法。

2.3.2 有障碍大场地定位实验

在有障碍大场地定位实验中，机器人的运行轨迹包含室内支柱这一障碍物，这将会对UWB 定位造成一定干扰。在该情况下，三种定位方法的定位结果对比如图8 所示。

图8 有障碍大场地下的定位效果对比Fig.8 Positioning effect comparison in large site with obstacles

在测量UWB 信号时，视距传播必不可少，当两点之间不存在直接传播路径时，只能通过反射和衍射效应由接收机接收到信号，如果以第一个脉冲的到达时间（Time Of Arrival，TOA）作为到达时间的真实值，将会带来很大的误差。与无障碍场地实验对比发现：正常情况下，UWB 能够达到厘米级定位效果，当实验场景中出现遮挡情况而产生非视距误差时，UWB 的定位精度出现较大的跳变和误差；而VIO 定位结果整体较为平滑，在定位初期效果令人满意，但因累计误差的存在使得其在轨迹的后半段定位结果发生漂移，且在拐角处因相机场景变化较大导致定位结果不甚理想。在这种情况下，经过ES-EKF 融合后的定位结果相比以上两种单一传感器定位方法的定位精度更高、可靠性更强，定位结果更接近实际轨迹。在有障碍物的室内环境下，定位结果与实际轨迹之间的最大误差与均方根误差均得到改善，如表2 中所示，与单一UWB 定位方法相比，所提算法的总体定位的最大误差减小约4.4%，均方误差减小约6.3%；与单一VIO 定位方法相比，所提算法的总体定位的最大误差减小约31.5%，均方误差减小约60.3%。

表2 有障碍大场地下三种定位算法结果误差对比Tab.2 Error comparison for results of different positioning algorithms in large site with obstacles

3 结语

相较而言，UWB 技术在定位精度上可取得不错的效果，但非视距误差等因素的存在致使其长期稳定性还无法满足需求；视觉惯性SLAM 技术在定位鲁棒性方面优势明显，但其在精确性方面仍显不足。本文采用ES-EKF 算法对UWB定位数据以及VIO 定位数据进行融合，并以地面移动机器人为测试对象，基于ROS 移动机器人平台构建UWB-VIO 混合定位系统。通过对比实验，表明了本文算法能够在含有障碍物的室内环境中，提升移动机器人定位的精确性和鲁棒性。

考虑到本文所提出的基于滤波的方法能够提供实时定位结果，但在精确性与基于优化的方法相比尚有所不足；故在未来的工作中可考虑如何同时兼顾定位的实时性和准确性。此外，鉴于本文的定位工作主要在二维平面上进行，随着未来空间技术的发展，无人机（Unmanned Aerial Vehicle，UAV）平台或将成为室内移动机器人的主体，用于隧道三维重建等场景；故在未来的研究中，可考虑以无人机等作为室内移动机器人平台，实现三维立体定位。同样的，可考虑将本文算法移植到室外移动机器人中，助力自动驾驶技术以及增强现 实（Augmented Reality，AR）和虚拟现实（Virtual Reality，VR）领域的发展。