【论文笔记】DLL: Direct LIDAR Localization. A map-based localization approach for aerial robots
【论文笔记】DLL: Direct LIDAR Localization. A map-based localization approach for aerial robots
~~~~~~~本文提出dll，一种直接基于历史地图的3D LiDAR定位方法。dll通过优化点云到历史点云地图的距离实现点云到地图的配准，而不通过特征计算和同名点对匹配。在给定初始位姿的情况下，本文方法基于里程计得到的位姿，并通过和地图的匹配更新位姿，实现机器人的位姿跟踪。通过在虚拟数据和真实数据集上的实验表明本文方法比基于蒙特卡洛定位的方法性能更优，比其他基于优化的方法速度更快，并对里程计位姿的初始位姿结果较为鲁棒。
本文借鉴NDT的思想（不对点云而是抽象化表达成其他形式），将历史点云地图表示成为3D距离场这种连续空间的表达（除去物体边缘和边界处），可以避免由于最邻近点搜索带来的计算瓶颈，同时这种高维拟合的表达对噪点较为不敏感，而且由于3D距离场的表达形式，使得在完成优化时梯度求解高效。并和基于ICP,NDT,蒙塔卡罗定位的方法进行了比较。
具体方法
~~~~~~~使用点云的基于地图的机器人定位过程可以概括为找到更好地将当前 LIDAR 点云与地图对齐的变换。但是，假设我们正在定位机器人，DLL 会利用以下先验信息来构建快速准确的定位算法：
• 机器人里程计。这为我们提供了一个很好的先验信息，了解自我们最后一次定位机器人以来机器人移动了多少。该先验可用于很好地猜测机器人在地图中的位置/方向，从而节省计算时间。
• 惯性测量单元（IMU）。空中机器人必须使用精确的 IMU。虽然由于罗盘中的磁场干扰，偏航角会发生显着失真，但横滚角和俯仰角是可观察的和准确的。此信息可用于对 LIDAR 点云进行倾斜补偿，从而简化配准过程。
A. Direct map to point cloud registration
~~~~~~~对于点云和点云地图的匹配问题，可以用该式子来表达：Tmap表示将点Pi变换到地图的变换矩阵，mc表示pi在点云地图中的对应同名点。通过优化上述非线性函数得到最优的位姿Tmap。其中涉及到两个问题，1）对应同名点如何获取；2）超定方程的求解。
由于NDT 将点云建模为正态分布的组合而不是单个点，描述在某个位置找到点的概率。这是点云的分段平滑表示，它支持标准的数值优化方法进行配准。这避免了显式邻居搜索，并相应地减少了计算量。DLL 遵循 NDT 将配准过程建模为非线性优化过程的关键思想，但将点云建模为距离场。 DLL 不是构建单个点，而是构建称为 DF (x) 的地图的距离场表示。给定一个 3D 点 x = [x; y; z]^t, DF (x) 提供到地图中最近点的距离。这样，我们将 (1) 转换为以下表达式：
即将点云地图表示为3D距离场，通过优化到点云地图中最近点的距离。历史地图的距离场可以离线事先构建好，而且只需要构建一次即可。
B. Approximated distance field
~~~~~~~距离函数是根据 3D 地图计算得出的，并存储为固定分辨率的 3D 网格。网格的大小由 3D 点的空间分布决定。每个网格单元都包含到该单元的 3D 地图的最近点的距离。
3D 网格是通过系统地计算到所有单元格中最近地图点的距离来构建的。我们使用 kd-tree 来加速计算，尽管可以应用其他近似最近点搜索的方法。这个过程在计算上是昂贵的，但它是在机器人定位之前离线执行的。构建后，将为每个单元计算三线性插值参数。每个单元周围的八个距离值用于计算这些参数。它们允许为给定单元格内的位置插入 DF 值，如下所示：
其中 x = [x; y; z]^t 表示属于网格单元的 3D 点。一旦构建好距离场，可以通过3维线性插值的方式得到该网格中任意位置点的距离场值。表达成距离场减少了对点云空间进行量化带来的梯度不稳定性，同时这种形式对于优化非常友好。
C. Outlier rejection
~~~~~~~ 3D LIDAR 可能会感应到未映射的对象（静态或动态），或者某些点可能会落在地图之外。我们需要处理这些点，以防止优化过程中出现退化的解决方案甚至发散。采取两项主要措施来防止这些异常值：
• 地图外的点。在这种情况下，距离函数返回一组等于零的插值参数。这会产生误差和梯度为零，避免影响优化过程。
• 未映射的对象。将缩放因子设置为 0.1 的稳健 Cauchy Kernel 应用于 LIDAR 的每个点。因此，当到最近点的距离很大时，内核会惩罚这个约束，减少它对整个优化过程的影响。 Huber Kernel 也用不同的比例因子进行了测试，但在大多数情况下，结果不太准确。
D. Implementation details
~~~~~~~如前所述，DLL 利用机器人里程计和 IMU 进行更快的计算。因此，DLL 不计算滚转和俯仰角，而是在配准之前对点云进行倾斜补偿。因此，在对齐期间仅估计 X、Y 和 Z 平移以及偏航角。请注意，DLL 是一种通用方法，因此估计滚转和俯仰只是将相应的状态和雅可比行列式添加到优化器中的问题。当必须注册新的点云时，DLL 会计算自上次地图注册计算以来的里程偏移，并将其用作对齐的初始猜测。
实验
A. Datasets
~~~~~~~该方法已使用公开可用的真实室内数据集和使用 Airsim [33] 的模拟室外数据集进行了测试。
B. Benchmarks
~~~~~~~• ICP：已使用 PCL 库提供的 ICP 解决方案实现了基于 ICP 的定位系统
• NDT：使用PCL 库[34] 提供的解决方案实现了基于NDT [7] 的定位系统
• AMCL3D：使用了在 AMCL3D ROS 中实施的蒙特卡洛定位方法。
C.result
~~~~~~~轨迹真值（蓝虚线），dll估计轨迹（红实线）和里程计估计值（绿虚线）在四个实验数据上的结果对比。dll只估计了UAV的xyz和yaw角，row和pitch通过倾斜补偿认为是不需要优化的参数。
不同方法比较的结果
MBZIRC2020 数据集中的估计无人机位置。 GPS 基线（蓝色虚线）和 DLL 估计（红色实线）。
结论
~~~~~~~本文提出了一种使用 3D LIDAR 的直接基于地图的姿态跟踪方法。该方法使用原始点云并避免搜索点对应关系。通过直接优化点到地图的距离，使用后者的距离场表示，可以改进来自里程计的预测姿态。结果表明，对于所考虑的场景，该方法如何实现与 NDT 和 ICP 等竞争方法相似的精度，但运行速度要快一个数量级，从而允许实时执行。此外，在噪声里程计的情况下，基于优化的方法 NDT 和 DLL 的表现优于蒙特卡罗定位方法，这也是 [31] 的发现。 DLL 能够处理比无损检测更大的噪声，在这种情况下实现更低的错误。未来的工作将考虑提高该方法的空间效率。所提出方法的主要缺点之一是距离场的 3D 网格表示所需的内存量。将考虑更有效的距离场结构，如 [27]。未来的工作还将研究估计与计算解决方案相关的不确定性的可能策略。其他算法（如规划器或控制器）可以使用此信息作为定量标准来做出与定位估计相关的决策。