基于抗差估计的VIO/UWB 自适应组合定位算法

李雪强，李建胜，王安成，罗豪龙，杨子迪，李凯林

（1.信息工程大学 地理空间信息学院，郑州 450000；2.61618 部队，北京 100080）

室外定位通常是依靠全球导航卫星系统（Global Navigation Satellite System,GNSS）来实现，然而在卫星拒止的室内环境中，GNSS 信号往往失效。近年来，同时定位与建图（Simultaneous Localization and Mapping,SLAM）技术的发展很大程度上解决了GNSS信号缺失情况下的定位问题。

视觉SLAM 凭借成本低、体积小、精度高等优势被广泛应用于各类室内移动导航平台，但是在纯旋转、尺度不确定等退化场景下存在缺陷。因此，在实际应用中需引入惯性测量单元（Inertial Measurement Unit,IMU）进行组合，使得两种传感器特性互补，组合后的视觉惯性里程计[1,2]（Visual-inertial Odometry,VIO）可以实现可靠、实时的位姿估计。VIO 本质上是一种局部定位算法，在没有其他传感器信息辅助和回环检测的情况下，系统定位误差随时间的推移而累积。对于零加速度和恒加速的特定运动，VIO 尺度变得不可观[3]，因此，目前许多学者设计全局的路标标志或利用其他全局信息[4,5]进行辅助，从而提高VIO 长时间运行的适用性。

在射频识别（Radio Frequency Identification,RFID）、ZigBee、蓝牙等能提供全局位置信息的室内无线定位技术研究中，超宽带（Ultra-wideband,UWB）技术因其定位精度高且不易受干扰而受到广泛关注，理论研究发展较为成熟，但由于多径效应、非视距（Non Line of Sight,NLOS）等因素的影响，导致部分距离测量精度较低，进而影响定位结果[6]。

鉴于VIO 和UWB 各自定位存在的问题，引入UWB 全局定位信息可以校正VIO 长时间运行的累积误差，而VIO 可以减小NLOS 对UWB 定位结果的影响，两者具有良好的互补性。对于多传感器融合，现仍有大量方法采用卡尔曼滤波（Kalman Filter,KF）及其变体[7,8]。文献[9][10]提出基于滤波松耦合的VIO/UWB 多传感器融合方法，并依赖于组合系统的状态方程。然而对于移动机器人，状态方程是高度非线性的，并且在一些复杂情况下难以建立，将系统状态方程简化为匀速或匀加速运动，不可避免带来估计误差以及对UWB 异常值较为敏感。不同于传统的滤波方法，文献[11][12]提出基于图优化的松耦合全局位姿估计框架。文献[11]没有考虑UWB 非视距情况；文献[12]则是对上一时刻估计位置与基站的距离和当前测距值进行比较，通过设定阈值来判断NLOS 异常值。

抗差估计多用于滤波方法中抵御粗差的影响[13,14]。在上述文献研究成果的基础上，不同于以往将抗差用于滤波，本文将抗差估计应用于非线性优化中，提出了一种基于抗差的单目VIO-UWB 多传感融合定位算法，提高室内复杂条件下定位的精度和鲁棒性。主要创新点为：1）构建了UWB 与VIO 图优化框架，利用UWB 全局定位结果对VIO 定位结果进行优化，并在仿真数据集和真实场景下对算法进行验证；2）将抗差估计加入后端优化过程，进行重优化以实时调整传感器之间的权值，提高算法在UWB 非视距情况下的定位精度，进而提升组合系统的稳定性和鲁棒性。

1 方法与原理

1.1 UWB 定位算法

通常，UWB 系统通过飞行时间（Τime of Flight,ΤoF）测量UWB 基站（Anchor）与标签（Τag）的距离，采用基于距离的方法进行定位。对于二维平面定位来说，需要三个及以上基站到标签之间的距离才能确定位置，而对于三维空间定位，则需要四个及以上基站到标签之间的距离[15]。

三维空间标签的位置计算公式为：

其中，(x j,y j,z j)(j=1,2…N)为第j个UWB 基站位置；(x,y,z) 为标签的真实位置；d j为第j个基站到标签的真实距离值；为第j个基站到标签距离的测量值；ηj为第j个基站到标签测量距离的噪声。

采用最小二乘估计法[16]求解标签坐标估计值。

1.2 VIO 定位算法

使用基于滑动窗口的紧耦合单目VIO 优化框架，如图1 所示，将视觉特征信息与IMU 预积分结果共同输入非线性优化的滑动窗口内，以获得最大后验估计，得出VIO 局部位姿估计结果。

滑动窗口中的完整状态向量定义为：

通过滑动窗口因子图模型构建非线性优化总体目标代价函数：

1.3 VIO/UWB 组合定位算法

为减少VIO 算法的累积漂移，长期运行后仍可得到准确可靠的定位数据，对UWB 与VIO 输出的定位数据进行非线性松耦合融合解算，如图2 所示。在VIO/UWB 组合定位算法中，以IMU 中心为原点，UWB 定位过程中所使用到的独立坐标系三个轴方向建立全局坐标系。

图2 UWB/VIO 组合算法后端优化Fig.2 Back-end optimization of VIO/UWB positioning algorithm

完整全局变量为：

其中，W代表全局坐标系标志；s表示滑动窗口内待优化的状态向量个数；待优化状态向量包括全局坐标系下第i帧平移向量和旋转四元数。

松耦合优化方程为：

1.3.1 VIO 残差

由于局部估计在较小范围内是准确的，可以利用两帧之间的相对位姿来构建局部因子，即VIO 两帧之间的变换与全局两帧之间的变换差：

根据基于相邻两帧之间相对姿态构建的局部因子残差公式，对待优化变量求偏导数，得到相应的Jacobian 矩阵。

1.3.2 UWB 残差

由于UWB 只能得到位置信息，因此可以将当前帧的UWB 解算位置转到全局坐标系下，再与待估位置参数之差来构建UWB 因子，如式(10)所示。

根据全局UWB 位置约束因子的残差公式，对待优化变量求偏导数，得到相应的Jacobian 矩阵。

1.4 基于抗差估计的VIO/UWB 自适应组合定位算法

在非视距环境下，UWB 测距值含有较大误差，难以保证定位精度。如果优化过程中仍采用权值初值，就会造成组合系统位姿精度的损失。为了减少观测值粗差对算法的影响，采用抗差估计的方法减弱粗差对位姿精度的影响。算法流程框架如图3 所示，在第一次非线性优化后进行抗差估计，自适应调节UWB 定位权重因子，然后用更新的权值进行重优化。

图3 基于抗差估计的VIO/UWB 组合算法框架Fig.3 Structure of VIO/UWB integrated algorithm framework based on robust estimation

抗差估计采用IGGⅢ权函数[18]：

2 实验验证与分析

2.1 开源数据集仿真实验

由于目前公开的开源视觉、惯性和UWB 组合数据集较少，因此本文在采用公开的视觉惯性SLAM 权威数据集EuRoc[19]基础上，根据数据集轨迹真值数据，仿真出UWB 数据，从而进行视觉/惯性/UWB 组合系统实验。

在仿真数据中，将EuRoc 数据集提供的载体位置真值坐标系作为世界坐标系，并根据运行轨迹设置四个UWB 基站，使载体轨迹包含在基站所构成的空间内。文献[20]对UWB 测距误差进行了深入研究，发现在LOS 和NLOS 条件下分别服从高斯和伽马分布，并采用高斯和伽马分布结合对LOS 和NLOS 混合环境测距误差模型进行准确建模。测距误差分布的概率密度函数f(d) 如式(13)所示。

其中，d为测距误差；u和σd为高斯分布对应的均值和标准差；λ和k为伽马分布对应的参数；c0为固定误差。

数据集实验以及后续对比实验所用的计算机处理器是Intel(R) Xeon(R) Silver 4110 CPU @ 2.10GHz×16，操作系统为Ubuntu 18.04.6 LΤS，都是在机器人操作系统（Robot Operating System,ROS）下运行。

2.1.1 组合定位性能分析

为了验证基于松耦合优化VIO/UWB 组合定位算法的可行性，使用EuRoc 数据集全序列仿真LOS 条件下UWB 测距数据，并与VINS_Mono、UWB 定位算法进行比较。

图4 显示了UWB、VINS_Mono 和VIO/UWB 组合算法在MH04 困难序列上的轨迹和绝对位姿误差（Absolute Pose Error,APE）。图4(a)中Refernce 为轨迹参考真值，图4(b)中数据记录时间为ROS 时间戳，故横坐标时间为刻度值与右下角用科学计数法表示的数值之和。从图4(a)可以看出，与UWB、VINS_Mono相比，VIO/UWB 组合算法估计出的轨迹与真实轨迹有更好的拟合度。图4(b)显示了VIO/UWB 组合算法的大部分定位估计误差在0.1 m 以内，表明UWB 传感器能够提供稳定的约束，进而使组合算法的定位结果保持较高精度。

图4 MH04 序列：算法轨迹与定位精度比较Fig.4 MH04 sequence: comparison of trajectories and positioning accuracy of the algorithms

表1 给出不同传感器组合在EuRoc 数据集上的位置均方根误差（Root-mean-squared Error,RMSE）。从表1 可以看出，UWB 定位算法的定位误差大约为0.17 m，即UWB 在良好视距环境下定位精度相对稳定。相比于VINS_Mono，VIO/UWB 组合算法在全序列下定位精度提升27.94%～85.19%不等，平均提升66.72%；相比于UWB 在全序列下定位精度提升55.84%～82.62%不等，平均提升69.27%。表明组合系统引入UWB 传感器可以提供全局的位置约束，进而有效提高系统性能。

表1 不同传感器组合定位精度对比Tab.1 Comparison of positioning accuracy of different integrated sensors

2.1.2 遮挡下组合定位分析

为了研究UWB 在短时间遮挡情况下对组合算法的影响，以及验证基于抗差估计的VIO/UWB 组合算法在NLOS 环境下的有效性，本实验使用MH 全序列和V102序列模拟UWB在NLOS情况下的测距数据。

图5(a)显示了V102 序列上两个时间段内模拟非视距场景下的UWB 定位误差，图5(b)显示了该序列重优化后的UWB 权重值变化。从图中可以看出，在视距阶段存在个别突变误差，UWB 权值进行了相应降权调整，而在非视距阶段因其误差太大，导致调整后的权值趋于0，说明抗差模型可以根据误差大小进行权值调整。

图5 V102 序列：UWB 定位输出和权重变化Fig.5 V102 sequence: UWB positioning output and weight change

为了更加具体比较VIO/UWB 组合算法与基于抗差估计的VIO/UWB 组合算法的差异性，图6 给出了不同算法在V102 序列上的轨迹和定位误差结果，图中Proposed 代表基于抗差估计的VIO/UWB 组合算法。从图6(a)可以看出，基于抗差估计的VIO/UWB 组合算法的定位轨迹与真实轨迹的贴合程度优于不加抗差估计的VIO/UWB 组合算法，尤其是在拐角转弯处和非视距环境下。从图6(b)可以看出，不加抗差估计的VIO/UWB 组合算法的大部分定位估计误差在0.25 m以内，但是在非视距环境下其定位估计误差不稳定，最高可达到将近0.5 m。改进后的基于抗差估计的VIO/UWB 组合算法的定位误差绝大部分在0.2 m 以内，可有效抑制非视距误差带来的影响。可知，即使UWB 在短暂遮挡下，基于抗差估计改进后的组合算法依旧能输出较高精度的定位结果，对非视距环境具有良好鲁棒性。

图6 V102 序列：算法轨迹与定位精度比较Fig.6 V102 sequence: comparison of trajectories and positioning accuracy of the algorithms

表2 给出遮挡条件下不同组合算法在EuRoc 数据集上MH 全序列的位置RMSE。UWB 受非视距因素影响定位偏差较大，不加抗差估计的VIO/UWB 组合算法在一定程度上对VIO 的累积误差和UWB 的非视距误差进行了抑制，但是其平均定位误差仍为0.2 m左右；而基于抗差估计的VIO/UWB 组合算法定位精度更高，其平均定位误差为0.07 m 左右。相比于VINS_Mono，基于抗差估计的VIO/UWB 组合算法在MH 全序列下定位精度提升49.15%～80.74%不等，平均提升72.07%；相比于不加抗差估计的VIO/UWB 组合算法，定位精度提升58.33%～73.85%不等，平均提升66.11%。

表2 遮挡条件下定位精度对比Tab.2 Comparison of positioning accuracy under occlusion

2.2 真实场景实验

为了验证算法的真实有效性，本文在某高校室内实验场进行测试。图7(a)为实验场景图，其中红色方框标注的为四个UWB 基站放置的位置，实验场地范围大约为8 m×8 m。图7(b)为实验数据采集设备，以UGV 无人地面小车为平台，UWB 模块采用Nooploop设计的LinkΤrack P 型号标签和基站，测距精度约为10 cm；VIO 模块采用Intel RealSense d455，包括一组双目镜头以及集成硬件同步的IMU 模块，实验中选取左目相机与IMU 数据进行算法验证。轨迹运动位置姿态真值由Vicon 光学运动捕捉系统进行记录，精度为毫米级。

图7 实验场景与采集设备图Fig.7 Experiment scene and acquisition equipment

实验中传感器性能指标如表3 所示。

表3 传感器性能参数Tab.3 Sensor performance parameters

实验场景分为UWB 信号无遮挡和有遮挡两种，分别记为场景1 和场景2。无遮挡场景是在通视良好、场地无障碍物情况下进行；有遮挡场景主要是通过在某个时间段对基站进行遮挡实现。

2.2.1 非遮挡条件下组合定位性能分析

图8 给出 VIO/UWB 组合算法与 UWB、VINS_Mono 算法在场景1 上的轨迹和绝对位姿误差对比。从图8(a)可以看出，相比于其它两种算法，VIO/UWB组合算法估计出的轨迹更贴近于真实轨迹，且基本与真实轨迹重合。图8(b)显示VIO/UWB 组合算法的定位误差大多在0.1 m 以内，表明UWB 传感器提供的约束能够提高组合算法的定位精度。

表4 给出不同传感器组合定位精度对比。从表中可以看出，VIO/UWB 组合算法的位置RMSE 相比于VINS_Mono、UWB 分别提升75.05%和56.03%，验证了组合算法具有更加准确可靠的定位性能。

表4 不同传感器组合定位精度对比Tab.4 Comparison of positioning accuracy of different integrated sensors

2.2.2 遮挡条件下组合定位分析

图9 显示了基于抗差估计的VIO/UWB 组合算法与VINS_Mono、不加抗差估计的VIO/UWB 组合算法在场景2 下的轨迹和绝对位姿误差对比。图9(a)显示不加抗差估计的VIO/UWB 组合算法在非视距时间段内与轨迹真值偏差较大，而加入抗差后的组合算法的定位轨迹更贴近于真实轨迹。从图9(b)可以看出，在UWB 非视距阶段，不加抗差估计的VIO/UWB 组合算法定位误差最高达到0.65 m 左右，而加入抗差估计后降低到大约0.2 m 左右，表明加入抗差估计的组合算法能够有效抑制UWB 非视距误差，提供稳定的高精度定位结果。

图9 场景2：算法轨迹与定位精度比较Fig.9 Scene 2: comparison of trajectories and positioning accuracy of the algorithms

从表5 可以看出，基于抗差估计的VIO/UWB 组合算法的位置RMSE 相比于VINS_Mono、不加抗差估计的 VIO/UWB 组合算法分别提升 33.53%和37.53%，验证了基于抗差估计的VIO/UWB 组合算法具有更加准确可靠的定位性能。

表5 遮挡条件下定位精度对比Tab.5 Comparison of positioning accuracy under occlusion

3 结论

本文提出了一种基于抗差估计的UWB 与VIO 图优化模型自适应定位算法，可有效解决VIO 无回环情况下产生偏移误差累积的问题，且能抑制UWB 定位过程中非视距误差的影响。真实场景下的实验结果表明：在良好视距环境下，VIO/UWB 组合算法相对于VINS_Mono 和UWB 定位精度分别提高了75.05%和56.03%；在非视距环境下，加入抗差估计的VIO/UWB组合算法相比于正常的VIO/UWB 组合算法定位精度提高了37.53%，验证了基于抗差估计的VIO/UWB 自适应组合算法具有较高的定位精度和良好的鲁棒性。

尽管本文算法在不同的场景下取得了较好的定位效果，但是仍需要四个以上UWB 基站，极大限制了场景的应用性。此外，本文算法是与UWB 定位结果进行松组合，没有充分利用其距离信息，后续需要与UWB 测距值进行紧组合以减少基站布设的复杂度和提高组合系统的定位稳定性。