一种三维激光雷达SLAM算法名为地面SLAM

摘要

本文提出了一种三维激光雷达SLAM，命名为Ground-SLAM，该算法利用了多层环境下的地面的约束去减少由激光雷达测量偏差引起位姿漂移。该算法是基于位姿图优化框架开发。在前端，提出了一种基于传感器为中心的滑动地图（sensor-centric sliding map）的新型的激光点云里程计（LO）来进行运动估计。Sensor-centric sliding map可以有效的根据误差传递模型过滤掉过期的特征。在每个关键帧中，滑动的地图（sliding map）被保存为一个局部地图。在局部地图中，算法会提取临近的平面并以最近点（Closest Point）参数化形式保存。然后，在不同关键帧中被观测到的平面被联系起来，地面的约束被放入图优化框架中。最终，回环检测，利用非线性方式整体优化形成一个全局优化地图。实验结果展示了该算法拥有很高的精确度。

主要贡献

我们工作的主要贡献有：

提出一种以传感器为中心的滑动地图（sensor-centric sliding map）维护方法， 用基于观察的方法可以有效地消除冗余特征的同时保持基本的特征 。

提出了一种局部地面关联对应方法，将连续关键帧的地面平面地标关联起来。

提出一种 3D LiDAR SLAM 算法，它利用室内多层环境中的地面约束来减小由 LiDAR 测量偏差引起的漂移。

算法流程

在对如何表达无限平面的参数的问题中，文中提到Hesse Form （HF）过度参数化用一个3D向量来表示2DOF且在最小二乘优化中容易遭遇奇异矩阵的问题。球坐标在俯仰角是正负90度的时候会出现表达模糊。四元数来表达平面，他们的联系并不清晰。 本文用CP（Closest Point）的方法，通过定义在平面上到给点坐标系原点的最近点来表示无限平面。通过利用HF作为中间传递参数的方式，在利用CP可以得到：

A. 系统

激光点云的去畸变利用了IMU混合轮速计形成的运动估计（EKF）的结果。去畸变后的点云利用了point-to-plane ICP算法，进行连续扫描的点云的配准得到相对运动。本文采用传感中心化的滑动地图的方法来克服激光雷达点云的稀松性和增加LO的精度。以传感器为中心的滑动图在每个关键帧记录一个局部地图，并且用最小二乘的方式提取地平面。 当地平面的约束被放入图优化框架中。姿态漂移，尤其是由 LiDAR 测量偏差引起的竖直方向的漂移被压缩，减小。增加了LO的轨迹的精度。然后通过进行闭环检测，残差被最小化得到全局的优化地图。B. 激光雷达里程计与传感器为中心的滑动地图 我们的 LiDAR 里程计方法主要包括两个 部分：scan-to-map 点云配准框架和一个以传感器为中心的滑动地图方法。

传感器为中心的滑动地图包括下面几个步骤：

首先，配准后的的新扫描点云与滑动地图点云通过检查预定义的距离来度量，例如欧几里得距离，马氏距离， 等等

然后，选择关联的滑动地图点，并且重置这些点云的协方差矩阵为与他关联的新点云的协方差矩阵，这个通常称为观察误差。

如果滑动地图中被关联的点的协方差过大，它将被过滤。在实践中，会求取协方差矩阵的迹来跟设定值作比较。

最后，配准的点云中没有被关联到的点会被添加到滑动地图中作为新的观察。

C. 地平面的提取 为了利用几何地面约束，我们引入了一个合理的假设，假设地面靠近车辆的点可以建模为无限平面。在关键帧中，地面靠近车辆运动路径的点首先被分割。然后，初始无限平面 CP参数用RANSAC来估计。 RANSAC 估计的初始平面参数可能噪音很大。因此，我们制定了一个加权最小二乘 （7） 和 （8） ，并最小化地面点和 CP 之间的点到面距离。

上面这三个状态都与加速度计和gyro的bias有关系，所以bias也被当做待优化的项。

D. 地平面关联估计

由于在此模型中地面被建模为平面地标，在不同位置提取的地平面需要互相关联。图 4 显示了一个室内结构化多层地面估计。假设有几个顺序 关键帧 Fi， 其中i = 1， 。.. ， 。每个关键帧都有一个估计的姿态由LO，和观察到的接地平面与估计参数及其不确定性Σ提供，则剩下的问题是确定这些观察到的平面之间的关系 。 我们通过观测CP参数变化的方法来观察局部平面的相关性估计。

E.联合位姿图优化 当观察到的地平面之间的对应关系已确定之后，地面观测约束会被融合到位姿图优化框架中，如图5（b）所示。使用（3）和（4），我们可以 将帧 Fj 中的地平面观测转换为全局坐标。如（19）所示，是残差的表示。残差rij的雅可比矩阵表示为（20）、（21）和（22）。

实验分析

结论

在本文中，我们提出了一种地面约束LiDAR SLAM算法命名为Ground-SLAM。如果在室外环境中，Ground-SLAM可以正常工作，只要地面大致平坦。Ground-SLAM的主要缺点是户外场景的适应性并不好。我们计划将语义信息放入LiDAR SLAM以提高准确性和鲁棒性。

审核编辑 ：李倩