LiDAR和UWB在隧道类环境下的定位能力

虽然目前针对GPS-denied环境下的定位方法很多，但实现稳健的几何退化环境下的定位，如隧道，仍然是一个具有挑战性的问题。将IMU、LiDAR和UWB相结合的传感器融合方法。实验结果表明，该方法可以实现长直隧道的鲁棒定位。

将UWB和IMU-LiDAR系统结合起来进行精 确定位。

1. LIDAR定位能力

LIDAR通过长直隧道时，因为各处的测量值都相同故无法确定其位置。如图1，通过类比管道内滑块来解释退化。滑块是通过连接激光雷达扫描的端点获得的。

2. UWB定位能力

UWB测量固定的锚点和移动的目标之间的距离。

3.LIDAR和UWB的概率融合

融合基于Error State Kalman Filter (ESKF)，与EKF类似，只是在误差状态下对系统运动学建模，对系统线性化有益。其中，积分IMU以预测机器人状态，LiDAR匹配先验地图以恢复6D位姿之后进行更新预测，UWB测量3D位置之后也对位置更新。LiDAR和UWB的更新率取决于数据频率。

 隧道内定位能力

在隧道沿线20个均匀采样点对LiDAR定位能力评估。X轴定义为沿隧道方向，Z轴定义为重力方向。在15m范围内均匀采样4000个点（大致为旋转180°激光点数量）以模拟每个地方的测量。由于距离远的点有近90度的反射角，使其有效范围减小，导致测量不可靠。在每一点通过拟合附近20点估计平面。利用式2-6可以估计其定位能力。如图6可以看出，X轴方向更差一些，因为是沿隧道方向退化严重。由于隧道有一个弧顶和几乎相同的高度和宽度，雷达测量的roll角不能很好约束，但好在融合IMU后约束较好。

如图7，特征空间的X轴与体坐标系平行，因为其是退化方向。Y、Z方向没有显著差异。

关于UWB，将力矩阵投影到特征空间评估定位能力。相较于LiDAR，UWB在X轴方向定位较好，这为融合它们提供了好的约束。然而其在锚点附近X方向定位能力下降，这是奇异点。可以增加锚点数量解决。