【泡泡图灵智库】LiDAR和UWB在隧道类环境下的定位能力估计

【泡泡图灵智库】LiDAR和UWB在隧道类环境下的定位能力估计

泡泡图灵智库，带你精读机器人顶级会议文章

标题：Estimating the Localizability in Tunnel-like Environments using LiDAR and UWB

作者：Weikun Zhen; Sebastian Scherer et. al

来源：ICRA 2019

编译：郑帅康

审核：万应才，李鑫

欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

摘要

大家好，今天为大家带来的文章是 --Estimating the Localizability in Tunnel-like Environments using LiDAR and UWB 该文章发表于2019 ICRA。

虽然目前针对GPS-denied环境下的定位方法很多，但实现稳健的几何退化环境下的定位，如隧道，仍然是一个具有挑战性的问题。本文提出了一种新的退化特征模型来估计先验地图中给定位置时的定位能力。并且分析了LiDAR和UWB的定位问题。提出了一种将IMU、LiDAR和UWB相结合的概率传感器融合方法。实验结果表明，该方法可以实现长直隧道的鲁棒定位。

主要贡献

1) 主要贡献在于提出了一种易于实现且具有物理意义定位方法，可以最大限度地降低定位失败的风险。

2)其次，我们提出了一种简单的概率传感器融合方法，该方法将UWB和IMU-LiDAR系统结合起来进行精确定位。

算法流程

1. LIDAR定位能力

LIDAR通过长直隧道时，因为各处的测量值都相同故无法确定其位置。如图1，通过类比管道内滑块来解释退化。滑块是通过连接激光雷达扫描的端点获得的。与表面法线类似的接触力禁止向管道侧面的运动。然而，由于没有摩擦力来约束物体，物体沿管道的运动就不会被限制。

机器人定位后扫描的点应该与地图对齐可描述为：

x为位置向量，R为姿态矩阵，ρ表示距离，i表示扫描点的标号，r表示机器人坐标系下的单位测距向量，(n, d)表示通过将局部平面与相邻点拟合得到的法向量和距离。

可以通过观察机器人姿态的灵敏度来评估约束的强度。即如果机器人的姿态稍微受到干扰时测量值变化不大，那么约束就很弱。否则，约束是强的。

可以计算ρ关于x, R的导数来表示其灵敏度。将所有约束条件下的导数叠加得到两个矩阵：

对信息矩阵进行特征值分解：

任何显著小于其他特征值的特征值都表示对应特征向量方向的退化。即评价退化程度的一个直接的选择是特征值。然而，我们发现这个度量很难解释，因为它的物理意义不清楚。为了解决这个问题，我们选择将F和T中的每一行投影到特征空间中。

定义其每行绝对值之和为定位能力向量：

观察式2、3、6，可以得到更直观的解释。如图2，可以将位置约束解释为n方向的力，将方向约束解释为r*n方向的力矩。F和T是约束机器人移动和旋转的扳手（力和力矩）的集合。则条件良好的F、T表示无摩擦力闭合(frictionless force-closure)（操纵力学中描述对物体坚实抓握的术语），其特征是F、T行向量能张成R3空间。这与用特征值识别退化类似。此外，我们可以将定位的物理意义解释为从测量中获得的累积虚拟力和力矩的大小，以抑制姿态估计的不确定性。

2. UWB定位能力

UWB测量固定的锚点和移动的目标之间的距离，假设目标位于机器人本体原点。有以下约束：

Xa是锚点的位置向量，ϒ是测量的距离。类似的，其力矩阵F为：

F也可以看作是单位力的集合。在F中只有一列，代表了从锚到目标方向上的一个力。力随后被投影到先前导出的特征空间中，与激光雷达的定位能力进行比较。由于传感器不提供任何关于方向的信息，扭矩矩阵T通常为零。

3.LIDAR和UWB的概率融合

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

Gaussian Particle Filter (GPF)将UWB测量转换成位置。首先根据先验信息和实验得到的噪声投放一系列粒子并设置权重：

然后计算加权平均和协方差找到后验位置：

进一步更新卡尔曼增益，恢复位置测量。最后融入ESKF，如图4.

主要结果

实验环境和设备如图5. 隧道大小为35*2.4*2.5m，先验地图通过多段扫描得到。四轴具有40Hz30m的雷达，100Hz的IMU，100Hz100m的UWB。

1. 隧道内定位能力

在隧道沿线20个均匀采样点对LiDAR定位能力评估。X轴定义为沿隧道方向，Z轴定义为重力方向。在15m范围内均匀采样4000个点（大致为旋转180°激光点数量）以模拟每个地方的测量。由于距离远的点有近90度的反射角，使其有效范围减小，导致测

量不可靠。在每一点通过拟合附近20点估计平面。利用式2-6可以估计其定位能力。如图6可以看出，X轴方向更差一些，因为是沿隧道方向退化严重。由于隧道有一个弧顶和几乎相同的高度和宽度，雷达测量的roll角不能很好约束，但好在融合IMU后约束较好。

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

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

2. 隧道定位实验

手动操作飞行器穿过隧道，平均速度0.7m/s。禁用UWB时，起飞后不久开始漂移。融合UWB和LiDAR时，整个过程都可以成功定位。图8为估计轨迹，先验地图和估计地图。由于没有真值捕捉，故利用重建的地图定性评估精度。虽然重构地图噪声较大，但边

缘结构得到了恢复，表明定位是正确的。

Abstract

The application of robots in inspection tasks has been growing quickly thanks to the advancements in autonomous navigation technology, especially the robot localization techniques in GPS-denied environments. Although many methods have been proposed to localize a robot using onboard sensors such as cameras and LiDARs, achieving robust localization in geometrically degenerated environments, e.g. tunnels, remains a challenging problem. In this work, we focus on the robust localization problem in such situations. A novel degeneration characterization model is presented to estimate the localizability at a given location in the prior map. And the localizability of a LiDAR and an Ultra-Wideband (UWB) ranging radio is analyzed. Additionally, a probabilistic sensor fusion method is developed to combine IMU, LiDAR and the UWB. Experiment results show that this method allows for robust localization inside a long straight tunnel.