GCLO：具备地面约束的适用于停车场AVP任务的lidar里程计(ICRA 2022)

论文阅读：GCLO: Ground Constrained LiDAR Odometry with Low-drifts for GPS-denied Indoor Environments (ICRA 2022)

Motivation

在具体的工程实践中，lidar在地下停车场容易沿垂直方向向上漂移，导致建图结果不佳。具体原因是当入射角较大时，激光雷达测量的深度可能会出现偏差。因此，当车辆在地面上移动时，从道路上观察到的点会稍微弯曲，LO 估计的轨迹会沿垂直方向漂移。因此为了消除 LiDAR 测量偏差并压缩可预测的位姿漂移，通常做法是需要针对不同的传感器调整比例因子，并且需要根据环境温度产生差异，所以为了提升鲁棒性，加入地面平面的考量减少无关项的影响是一个实用的选择。

下图是motivation的可视化，具体表达的是见图结果的地面点与拟合的地平面之间的距离的俯视图，a图表示当车辆在地下停车场的地平面上移动时，LO 结果向上漂移，所以最终地图里表示的地面是一个扭曲向上的弓形，b图表示当采用本文提出的方法后的地面见图效果。

Contribution

1. 提出了一种利用地面观测约束来减少多层室内和平坦室外环境中的位姿漂移的 LO 方法。
2. 提出了一种通过检测连续关键帧之间的急剧变化来关联地面平面地标的地面匹配方法。
3. 基于KITTI 和 HIK 数据集的实验结果说明了准确性，并且讨论了在算法泛化方面所存在的局限性。

Content

1. 关于无限平面参数化的讨论

本文最终选定的无限平面参数化方式为closest point(CP)，未选的方式包括：Hesse form(HF), 球坐标系表示和单位四元数表示，他们分别具有如下缺点：

1）HF包含法向量n和平面到坐标系原点的距离，这往往是过参数化的，所以它会在最小二乘优化中受到奇异信息矩阵的影响，并且难以维护法向量。

2）球坐标系表示包含具有方位角和仰角的法向量，但是，当仰角等于 ±Π /2 时，会产生歧义。

3）单位四元数表示平面存在的问题在于，目前尚未能清楚描述四元数和平面之间的物理联系

虽然在CP表示的情况下，当给定坐标系的原点位于平面上时，也会遇到奇异性问题。但是由于地平面与车辆上配备的 LiDAR 有一定距离，因此本文可以避免 CP 表示的奇异性。

另外由于本文将地面建模为无限平面地标，因此在不同帧之间转换地面参数至关重要，所以采用 HF作为地面转换的中间参数使得转换方程变得容易，具体过程如下：

假设帧a存在一个地面用HF表示为：

，点是该平面的点，即

，给定a帧和b帧之间存在变换：

，带入可得：

，从而可以得到新的HF表示：

，最后将HF表示转为CP表示：

2.系统框架

系统框架如上图。在预处理模块中，使用EKF方法融合 IMU 和车轮编码器，以提供高频运动估计结果。LiDAR 点云的运动畸变通过使用运动估计结果来补偿。去畸变的 LiDAR 点云基于点到平面的 ICP 算法来估计连续扫描帧之间的相对位姿变换。维护以传感器为中心的滑窗，从而减轻 LiDAR 扫描的稀疏性并提高 LO 的准确性。在每个关键帧，滑动图被记录为局部地图。从局部地图中分割地平面，并使用加权最小二乘法估计 CP 参数。之后，关联在不同关键帧观察到的地平面，并且构建一个位姿图并联合优化残差。

3.以传感器为中心滑窗的scan2map

1）Scan2map注册

假设在时间k的时候，全局位姿为，并且滑窗地图已经基于帧进行了维护，当一个新的扫面帧在时间k+1进入的时候，通过点到面的ICP估计出相对变换和对应的协方差。

滑窗中点的坐标和不确定性协方差可以通过下述公式变换到新入帧：

其中，

是点关于旋转分量的雅可比。

2）以传感器为中心的滑窗地图更新

滑窗图在更新观测特征的同时需要过滤掉具有较大不确定性的点。主要步骤如下：

a)首先，新注册的激光点需要先检查预定义的距离阈值，之后和滑窗地图点关联。

b)然后，选择关联的滑窗点，并将它们的不确定性重置为关联的新扫描点的协方差矩阵，称之为观测误差。

c)具有大不确定性的滑窗点会被过滤掉。在实验中，只要对应的协方差矩阵的迹大于阈值，该点就会被消除。

d)最后，将没有关联成功的激光点作为新的观测值添加到滑窗图中。

3)地面提取和建模

考虑到停车场的地面大多都平坦，所以采用无限平面进行建模。对于每个关键帧，首先从以传感器为中心的滑窗地图中分割出地面点。然后基于这些分割出的地面点集应用RANSAC获得初始的CP表示。

RANSAC获得的表示肯定有噪声，因此定义一个加权最小二乘问题来最小化点到地面的距离：

其中，是第i个测量的不确定性的逆，是点pi的不确定性。

关于残差对于的雅克比可以定义为：

相应的局部增量可以表示为：

新的平面参数可以根据局部增量进行更新：

最终的不确定性可以近似为：

4)地面平面匹配

假设已知j个历史关键帧：

，并且每个关键帧都有从里程计获得的位姿和其对应的观察到的地面参数及其不确定性，接下来的问题可以表述为：如何确定哪个历史地平面与当前地平面共面。

一个直接的方案是通过在欧式空间的一个相同的坐标系中比较它们的参数来匹配地平面。但是如果漂移过大的话会错误地关联地平面。相反，基于一个先验知识 ( 地面通常是结构化比较好的，并且在论文motivation的环境（室内停车场）中，地面连接存在明显差异。)，

如上图所述，当地面变化的时候，CP变化极其明显，因此可以通过如下公式检测CP参数的剧烈变化从而关联对应的地平面

其中f()代指：

然后通过如下公式估计的不确定性 （假设短时间内的LO结果足够精确，所以不需要计算LO的不确定性）

5)联合位姿图优化

在确定地平面之间的匹配关系后，将地面约束融合到位姿图优化框架中，从而压缩 LO 的漂移，如下图所示。

假设在世界坐标系存在一个地面P，其CP表示为：。然后位姿为的帧观测到地面P得到对应的CP表示和HF表示

，运用之前提到的公式，以HF为中介，将平面在当前坐标系转换到全局坐标系，从而可以从观测到的CP与估计的CP之间构建对应的残差：

残差关于和的雅可比表示为：

最后通过LM方法联合优化LO和地平面残差：

6)其他细节

本文在每个关键帧提取一个地平面作为地面观测。但是，此策略不适用于所有目标环境。首先，CP 参数可能会因减速带而产生噪声。其次，地面不能总是被建模为平面地标，因为弯曲和螺旋坡道在地下停车场很常见。第三，当关键帧位于多层路口的交叉区域时，该关键帧对应两个地平面。

采用如下几种方法来处理上述情况。首先，使用 Huber 损失函数来减少异常值（例如减速带）对 CP 参数估计的影响。其次，通过计算残差和不确定性来评估地面的平面度。平面度低的地面不会被建模为平面地标，相应的观测约束也不会被融合。第三，当地面匹配不明确时，在某些指标（平面度、点数）下只选择一个地平面。这样，无论关键帧在停车场的哪个位置，每个关键帧都只拥有一个具有精确估计CP参数的地平面。

4.实验

本文采用的数据集是HIK和KITTI。HIK是海康内部数据集，包含了三种场景，室内，校园环境，城市公路。KITTI不作赘述，大家都知道。最终的实验数据记录如下：

注意，上表中的A/B,A指的是GCLO*, B指的是GCLO，GCLO*代表不加入地面约束。

注意，上图中，kitti分成了两个子集，子集1旨在说明自己算法有优势的场景，子集2旨在分析为什么算法在这些场景不行，作者以Seq.09举例分析，

如下图所示，虽然每个关键帧的地面是局部平坦的，但地面的坡度是逐渐变化的。最终不正确的关联结果会导致更高的位姿漂移。因此与 GCLO* 相比，GCLO 结果的旋转和平移误差分别增长了 36% 和 18%。

Conclusion

这篇文章的作者利用多层室内和平坦室外环境的地面约束来优化 LO 的位姿漂移。虽然最初是为 AVP 应用而设计的，但实验证明对于其他环境也很有价值，并且也通过实验结果对缺点进行了分析与讨论，展现出了有希望的改进方向，主要原因就是由于grounds的建模和关联方法使得GCLO对一般场景的适应性较差。所以后续可以考虑整合其他的建模方法或者额外的语义信息，以提高准确性和鲁棒性。

本文仅做学术分享，如有侵权，请联系删文。