基于LiDAR点云的UWB NLOS识别及室内融合定位算法

引言

同时定位与建图（Simultaneous Localization and Mapping，SLAM）技术是机器人和自动驾驶等领域的核心技术之一，其目标是在未知环境中感知设备实现自身定位并构建环境地图。

UWB可提供高精度测距值，弥补了LiDAR SLAM的累积误差问题。同时，LiDAR SLAM生成的详细点云地图可辅助提升UWB系统在复杂环境中定位的稳定性和准确性。

在室内环境中进行LiDAR/UWB融合定位时不能忽视各类障碍物（如墙壁、柱子、移动的人或车辆等）对UWB信号传播的影响。障碍物的遮挡会导致UWB脉冲信号在传播过程中发生反射、折射和穿透，造成信号衰减、时间延迟和传播路径延长，进而引入非视距（Non Line of Sight，NLOS）误差。

针对上述UWB/LiDAR融合定位存在的UWB NLOS识别和LiDAR累积误差问题，本文提出一种基于LiDAR点云的UWB NLOS识别方法，以提高室内融合定位系统的精度和鲁棒性。

为了解决UWB因障碍物遮挡出现NLOS这一问题，本文提出了一种基于LiDAR点云识别UWB NLOS的融合定位算法。

图1 基于LiDAR点云识别UWB NLOS的融合定位系统

UWB和LiDAR属于2种不同类型的定位系统，所以它们之间的时间同步和空间标定是UWB NLOS识别及后续多传感器融合的必要步骤。

对于时间同步，ROS提供了一个成熟的数据封装格式来存储传感器信息和时间戳。因此，利用ROS的机制为每个传感器分配相同时间参考的时间戳，通过对齐UWB和LiDAR数据的时间戳来实现两种传感器的时间同步。

在室内场景中，由于存在障碍物的遮挡，通常导致UWB基准站和流动站之间出现NLOS状态，进而在TW-TOF测距中出现正向偏差。

图2 室内NLOS与LOS传播示意图

图3 融合定位实验平台

实验采用6个UWB基准站进行实验，基准站1、2设定为在LOS区域内，基准站1在开阔区域内，基准站2位于紧靠柱体但不存在视线遮挡处。

表1 不同算法识别NLOS基准站的数量(个)

本实验在某地下停车场中的复杂场景区域内进行，实验面积约为2000 m²。实验布局如图5所示，红色三角形表示6个UWB基准站，灰色矩形表示为场景中分布的承重柱。

图5 地下停车场实验布局

在动态识别实验中，通过全站仪提供融合定位系统的实时位置数据，并标记出各个UWB基准站和障碍物的三维空间位置，判断各个UWB基准站是否处于NLOS状态。

图6 动态识别算法NLOS基准站数目对比

本实验中，UWB基准站处于NLOS状态的平均比例为57.39%，LiDAR原始点云识别法NLOS基准站的识别准确率为82.08%，本文提出的LiDAR轮廓识别法NLOS基准站的识别准确率为93.22%。

为验证本文提出的基于LiDAR点云识别UWB NLOS的融合定位算法的有效性，评估识别NLOS误差对于融合定位系统定位精度的提升效果。

图7 UWB基准站原始测距误差和LOS测距误差

表2 不同基准站的原始测距值和LOS测距值的测距误差(m)

NLOS识别后的LOS测距均方根误差较原始测距值的测距均方根误差分别降低了97.34%、95.10%、94.28%、95.12%、93.65%、97.29%，显著提高了UWB测距数据的质量。

图10 不同算法定位轨迹对比