在地下车库、隧道、矿井等GNSS拒止环境，UWB可以提供精确的位置信息。然而，UWB定位会受到非视距(NLOS)信号的干扰，导致复杂环境精度和连续性显著恶化。我们设计了一种基于多状态约束卡尔曼滤波器(MSCKF)的UWB/LiDAR/IMU紧耦合状态估计器(MR-ULINS)，并提出了LiDAR/IMU相对轨迹辅助的UWB多历元抗差算法。实测表明：在UWB基站分布相同的情况下，NLOS与LOS场景定位精度相当(约0.1米)；在LiDAR退化场景和UWB基站稀疏场景均具有良好的精度和鲁棒性。

基于帧间数据关联的LIO本质上是一种递推导航系统，定位会随时间产生漂移。这种漂移可以通过与高精地图匹配来消除，但高精地图成本高昂，且在货仓等经常发生变化的场景中可能会失效。消除累积误差的另一种方法是引入绝对定位传感器，如GNSS和UWB。GNSS可以在开阔场景中实现厘米级的绝对定位，但在室内环境中会失效。通过布设基站，UWB可以提供精确的室内绝对定位。然而，UWB测距精度受到硬件误差(包括时钟频偏和天线相位中心偏移)的影响。现有研究通常采用事先标定的方法，忽略了硬件误差随时间和环境的变化，同时增加了成本。与此同时，在复杂环境中，非视距(NLOS)信号会显著影响UWB测距精度，而产生大量的粗差测量。现有的UWB抗差方法很大程度上依赖于绝对位姿，如果估计的位姿不准确，其性能就会剧烈恶化。特别是在UWB基站稀疏的情况下，由于初始化时无法获得准确的绝对位姿，可能导致传统抗差方法失效。

针对上述问题，我们提出了一种多历元抗差的UWB/LiDAR/IMU紧耦合状态估计器MR-ULINS [1]，实现了室内复杂环境下实时、稳健、精确的绝对定位。LiDAR使用帧间数据关联[2]，构建相对约束，保证状态估计的一致性。多状态约束卡尔曼滤波器(MSCKF)具有高效和高精度的特点，还有助于UWB多历元抗差，因此被用于状态估计。MR-ULINS的系统框图如图1所示，主要特点如下:

(1)提出了一种基于MSCKF的UWB/LiDAR/IMU状态估计器，实现了IMU、LiDAR点到平面距离和UWB测距值的紧耦合。同时，将UWB测距误差建模为零偏和比例因子，进行在线估计，从而提高定位精度。

(2)设计了一种基于RANSAC的UWB多历元抗差算法，利用LiDAR/IMU的短期高精度相对轨迹检验多历元UWB测距值的一致性，从而抑制UWB NLOS信号的干扰。所提出的多历元抗差算法不受绝对位姿误差的影响，在UWB基站稀疏的情况下仍然有效。

图1 MR-ULINS的系统框图

我们在NLOS干扰严重的地下车库验证了MR-ULINS的定位精度，测试场景、基站分布和测试轨迹如图2、3所示。NLOS环境下MR-ULINS的定位误差(RMSE)如表1所示。对比算法中，FAST-LIO2和TC-LIO分别是基于帧到地图和帧间数据关联的LIO算法，ULINS没有在线估计UWB测距误差，也没有使用多历元抗差。与FAST-LIO2和TC-LIO相比，ULINS和MR-ULINS利用UWB的绝对距离测量减少了累积误差。MR-ULINS在线估计了UWB测距误差，并利用多历元抗差算法有效抑制了NLOS干扰，因此在NLOS环境中表现出最佳性能。与ULINS相比，MR-ULINS的RMSE平均降低了38.3%。

图2 室内NLOS测试场景

图3 室内UWB基站分布及测试轨迹

表1 NLOS环境定位误差(m）

同时，在LOS和稀疏UWB基站环境中进行了充足的实验。结果表明，在UWB基站分布相同的情况下，MR-ULINS在LOS和NLOS环境中表现出相似的定位精度，约为0.1米。此外，MR-ULINS在LiDAR退化场景、UWB基站稀疏场景（同时可见基站仅2个）均表现出良好的精度和鲁棒性。

相关成果发表在SCI 2区TOP期刊 IEEE RAL上。

参考文献：

[1] Tisheng Zhang, Man Yuan, Linfu Wei, Yan Wang, Hailiang Tang*, and Xiaoji Niu, “MR-ULINS: A Tightly-Coupled UWB-LiDAR-Inertial Estimator With Multi-Epoch Outlier Rejection,” IEEE Robotics and Automation Letters, vol. 9, no. 12, pp. 11786–11793, Dec. 2024, doi: 10.1109/LRA.2024.3498780.

[2] Hailiang Tang, Tisheng Zhang*, Xiaoji Niu, Liqiang Wang, Linfu Wei, and Jingnan Liu, “FF-LINS: A Consistent Frame-to-Frame Solid-State-LiDAR-Inertial State Estimator,” IEEE Robotics and Automation Letters, vol. 8, no. 12, pp. 8525–8532, Dec. 2023, doi: 10.1109/LRA.2023.3329625.