经典文献阅读之--GCLO(用于无GPS室内环境的低漂移地面约束激光里程计)

0. 简介

自动代驾泊车（AVP）是自动驾驶技术的一个很有前景的应用，其旨在使车辆自行导航并自动停车到目标位置。高清地图在AVP中发挥着关键作用，因为它可以以厘米级的精度提供目标停车场的先验信息。相较于开放道路场景来说，RTK其实可以起到比较关键的作用，但是比如在底下停车场这类结构比较单一，且没有GPS的场景，lidar在地下停车场容易沿垂直方向向上漂移，导致建图结果不佳。具体原因是当入射角较大时，激光雷达测量的深度可能会出现偏差。因此，当车辆在地面上移动时，从道路上观察到的点会稍微弯曲，LO 估计的轨迹会沿垂直方向漂移。

为此本文《Ground-SLAM: Ground Constrained LiDAR SLAM for Structured Multi-Floor Environments》利用结构化多层环境中的地面来压缩主要由LiDAR测量偏差引起的位姿漂移。Ground-SLAM基于众所周知的位姿图优化框架进行开发。在前端，使用LiDAR Odometry (LO)进行运动估计，引入了一种新颖的传感器中心滑动地图，通过根据误差传播模型过滤掉过期特征来维护该地图。在每个关键帧上，滑动地图被记录为一个局部地图。附近的地面被提取并建模为无限平面的地标，采用最近点（CP）参数化的形式。然后，将不同关键帧观察到的地面平面进行关联，并将地面约束融合到位姿图优化框架中，以压缩LO的位姿漂移。最后，进行闭环检测，并联合最小化残差误差，从而得到一个全局一致的地图。

1. 主要贡献

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

2. 符号和准备工作

2.1 符号

在下面的部分中，我们将齐次变换矩阵表示为 $T^b_a ∈ SE(3)$ ，它将帧 $F_a$ 中的点 $p^a_i ∈ R^3$ 转换为帧 $F_b$ 中的点。 $R^b_a ∈ SO(3)$ 和
$t^b_a ∈ R^3$ 分别是旋转矩阵和平移向量。我们用 $L 、 B 、 W$ 表示LiDAR、IMU和全局地图的坐标系。我们用 $\Pi^a$ 表示帧 $F_a$ 中无限平面的最近点（CP）参数。

2.2 前提条件

研究人员提出了几种无限平面的参数化方法。Hesse形式（HF）由一个法向 $\vec{n}$ 和平面与给定坐标系原点之间的距离 $d$ 组成。HF是一种过参数化方法，因为它采用3D向量来表示具有2个自由度的法向量。因此，在最小二乘优化中，它会遇到奇异信息矩阵的问题，并且法向量的一致性变得难以保持。球坐标是一种用方位角和仰角表示法向量的最小参数化方法。然而，当仰角等于 $±\pi/2$ 时，它会遇到模糊性问题。单位四元数[30]也被用来表示无限平面。然而，四元数与平面之间的物理联系不清楚。
受[31]的启发，本文采用了CP来表示无限平面，CP是指平面上离给定坐标系原点最近的点。CP是一种最小表示方法，因此当给定坐标系原点位于平面上时，也会遇到奇异性问题。然而，在本文中，由于地面平面与车辆上装备的LiDAR有一定的距离，CP表示的奇异性问题可以很容易地避免。
在本文中，将地面的参数在不同坐标系之间进行转换是至关重要的，因为本文将地面建模为一个无限平面地标。HF使得表示平面方程变得容易，因此我们将使用HF作为中间参数来进行地面参数的转换。假设在坐标系 $F_a$ 中有一个具有HF $[\vec{n}^a,d^a]$ 的平面。点 $p^a_i$ 位于该平面上，那么该点满足平面方程，如式（1）所示:
在这里插入图片描述
如果我们已知从帧 $F_a$ 到帧 $F_b$ 的变换矩阵为 $T^a_b$ ，并且 $F_b$ 是已知的，那么我们有 $p^a_i = R^a_bp^b_i + t^a_b$ 。将其代入公式(1)，我们可以得到公式(2)。

然后我们可以使用（3）中的方程得 $\vec{n}^b$ 和 $d^b$ 。在得到以HF表示的变换结果之后，我们可以使用（4）将其转换为CP参数化。
在这里插入图片描述

3. 系统框架（核心是利用地面约束来限制的LO的垂直漂移）

在这里插入图片描述

图3展示了所提出框架的简要概述。在预处理模块中，使用扩展卡尔曼滤波（EKF）方法将IMU和轮式编码器测量结果融合，以提供高频运动估计结果。通过使用运动估计结果，减小了LiDAR扫描的运动畸变。去畸变的LiDAR扫描点用于使用点对平面ICP算法估计连续扫描之间的相对变换。为了克服LiDAR扫描的稀疏性并提高LO的准确性，维护了一个以传感器为中心的滑动地图。传感器为中心的滑动地图在每个关键帧处记录为一个局部地图，并使用加权最小二乘法提取地面平面。确定了在不同关键帧观察到的地面平面之间的局部对应关系。然后，将地面观测约束融合到姿态图优化框架中。压缩了姿态漂移，特别是由LiDAR测量偏差引起的垂直漂移。提高了LO结果轨迹的准确性。然后进行闭环检测，并将新关联的关键帧之间的闭环边添加到姿态图中。最后，最小化残差误差，组装出一个全局一致的地图。下面介绍关键模块的详细信息。

4 以传感器为中心滑窗的scan2map(重点内容)

我们的激光雷达里程计方法主要由两部分组成：扫描到地图的配准框架和基于观测的传感器中心滑动地图的维护方法。配准框架的主要功能是进行扫描配准和数据转换。假设在时间 $k$ 时，激光雷达的全局姿态为 $T_{L_k}^W$ ，并且滑动地图 $M_{L_k}$ 相对于 $L_k$ 帧已经维护好了 $w . r . t$ 。当一个新的扫描 $S_{k+1}$ 在时间 $k + 1$ 到达时，首先使用点对平面ICP [32]和Censi的方法[33]估计传感器变换 $T^{k+1}_k$ 以及相应的协方差 $Σ_{T^{k+1}_k}$ 。滑动地图中点的坐标和不确定性协方差可以通过公式(5)和(6)从 $L_k$ 帧转换到 $L_{k+1}$ 帧，其中 $p^k_i$ 是滑动地图 $M_{L_k}$ 中的第 $i$ 个点， $Σ_{p^k_i}$ 是相应的协方差。 $Σ_{R^{k+1}_k}$ 和 $Σ_{t^{k+1}_k}$ 分别是估计的旋转和平移分量的协方差矩阵。 $J_{R^{k+1}_k}$ 和 $J_{p^{k}_i}$ 分别是相对于旋转分量和点的雅可比矩阵。
在这里插入图片描述
基于观测的维护方法旨在用于传感器为中心的滑动地图更新，包括以下步骤：
1）首先，通过检查预定义的距离度量（如欧氏距离、马氏距离等），将注册的新扫描点与滑动地图点关联起来。
2）然后，选择关联的滑动地图点，并将它们的不确定性重置为它们关联的新扫描点的协方差矩阵，通常称为观测误差。
3）滑动地图中捕获到相当不确定性的点将被过滤掉。在实践中，只要相应的协方差矩阵的迹比阈值更显著，就会消除该点。
4）最后，未能关联的注册扫描点将作为新的观测添加到滑动地图中。

基于观测的维护方法过滤掉具有相当不确定性的点，同时保留和更新观测特征。因此，滑动地图的一致性和密度得以保持，这是后续扫描配准和运动估计的重要要求。

5. 地面约束

地面提取和建模考虑到停车场的地面大多都平坦，所以采用无限平面进行建模。对于每个关键帧，首先从以传感器为中心的滑窗地图中分割出地面点。然后基于这些分割出的地面点集应用RANSAC获得初始的CP表示。RANSAC获得的表示肯定有噪声，因此定义一个加权最小二乘问题来最小化点到地面的距离：
在这里插入图片描述
通过高斯牛顿法来最小化上式，从而得到一个合适的用CP表示法表示的平面方程：

6. 平面的连续性估计

由于本文中地面被建模为平面地标，因此需要关联提取出的不同位置的地面平面。图4展示了结构化多层室内环境中地面对应估计的经典场景。假设有几个连续的关键帧 $F_i$ ，其中 $i = 1, \dots, N$ ， $N$ 是关键帧的数量。每个关键帧都具有由LO提供的估计位姿 $T^w_i$ 和观测到的地面平面及其估计参数 $\Pi^i$ 和不确定性 $Σ_{\Pi^i}$ ，因此剩下的问题是确定这些观测到的地面平面之间的对应关系。

这个问题的一个直接解决方案是通过在一个相同的坐标系下使用距离度量（例如欧氏距离）比较地面平面的参数来关联它们。然而，LO的误差会累积，并且这个误差会传播到平面参数中。因此，可能会发生错误的数据关联。需要注意的是，LO会稍微漂移，并在短期内保持高精度。此外，室内地面通常具有良好的结构，并且在多层连接处地面参数变化剧烈，如图4所示。因此，我们通过检测地面CP参数的剧烈变化，在连续的两个关键帧之间进行局部地面对应估计，如式（14）和（15）所示，函数 $f$ 指的是式（3）和（4）中的变换， $Ω_{∆\Pi^i}$ 是相对测量 $∆\Pi^i$ 的不确定性的逆。
在这里插入图片描述
其中 $f ()$ 代指：