3D激光SLAM:LOAM 论文--算法详细解读

LOAM是 Lidar SLAM 中非常基础的框架之一,后面在此基础上又延伸 优化 出 一些 框架,比如

• A-LOAM
• Lego-LOAM
• LIO-SAM

所以还是要好好研究下这个基础框架 本篇主要是论文和算法内容方面的解读

LOAM简介

LOAM 实现的功能:
LOAM 实现 了 一种实时激光里程计并建图的算法,使用的硬件是一个三维空间中运动的两轴单线激光雷达.

两轴单线激光雷达,就是在水平方向和垂直方向的单线雷达在两个方向不断变换形成点云,其中每个点的时间戳则不同.
现在主流用的velodyne 的16线\32线的则是 在一个垂直方向上有 16\32个点,它们有相同的时间戳,在水平方向时间戳则是递增的.这个算法同样可以用于16线的lidar

实现功能的难点:
这个问题困难的原因是由于点云不是相同时间获得的,并且运动畸变会导致点云的位置出现错误.这种错误也会造成匹配时的问题,也就造成了里程计信息估计的错误
目前一致性比较好的3d地图都是通过离线的方式获得的.通常还会通过回环检测来消除累计误差.
回环检测目前是 VSLAM和LSLAM中,消除累计误差比较好的手段.因为里程计肯定会有累计误差的

LOAM的优点:
LOAM方法实现了低的漂移,并且计算的复杂度低,实时性很好.并且不需要高精度的lidar和惯导

算法核心:
这个方法的核心思想就是把SLAM问题进行了拆分,通过两个算法来进行.一个是执行高频率的里程计但是低精度的运动估计（定位）,另一个算法在比定位低一个数量级的频率执行匹配和注册点云信息（建图和校正里程计）.
这个两个算法都需要提特征点,就是经典的角点和面点,然后进行配准.在前端的那个算法中也就是里程计算法,特征点的提取会用到快速计算的方法.在建图的后端算法中,相互关联的特征点是通过特征值和特征向量来获得的

后来也就衍生出 SLAM中大部分使用的方法:前端+后端.前端:高频低精 后端:低频高精
将这两个算法结合就获得了高精度、实时性的激光里程计

这个方法在KITTI数据集上做了大量实验,效果很好.霸榜很长时间

LOAM是希望仅使用一个激光雷达来得到高精度的地图和低偏移的里程计

目的是最小化里程计漂移所以不涉及回环检测的问题。后面在lego loam 里面加入了回环检测

LOAM本身可以不需要IMU,但是如果有IMU,可以提高对运动的估计频率.

论文里面的符号表示

一般的k的下标,就是第k帧
%%%%%%%%%%%%%%%%%%%

第k帧的 所有点
%%%%%%%%%%%%%%%%%%%

lidar坐标系下,第k帧的第i个点
%%%%%%%%%%%%%%%%%%%

世界坐标系下,第k帧的第i个点
%%%%%%%%%%%%%%%%%%%

第k帧的点,全部投影到k+1时刻的点集
%%%%%%%%%%%%%%%%%%%

在第k+1帧里面提取的角点
%%%%%%%%%%%%%%%%%%%

在第k+1帧里面提取的面点
%%%%%%%%%%%%%%%%%%%

算法部分

激光雷达的每个帧分别输入两个算法.

前端里程计计算相邻两帧的运动计算的运动用于校正畸变.运行的频率大致为10hz

前端的输出之后会被后端处理,运动补偿后的点云和地图进行配准.运行频率1hz

最终,最后的位姿是两个算法结合,10hz里程计的输出

激光雷达里程计

A 特征点提取

选择的特征点是:角点 和 面点

判断i是什么点
就是取i左右两边的点,计算曲率

把一帧里面的每个点的曲率计算出来,然后根据曲率进行排序.

然后选择最大的几个曲率点做为边缘点,最小的几个曲率点做为平面点.

为了使提取的特征点均匀,把一帧雷达分成4等分.对于velodye这种雷达相当于一根线分成4等分.

每个部分提取出2个边缘点和4个面点.

曲率值要满足阈值范围才会被判断为是否是特征点,并且选择的数量是不允许超过设定的值的.

这个降采样非常的狠,为了计算的速度

在提取特征点的时候,要避免点过于集中

有一些情况,认为点是不可靠的点,

• 平面上的点和激光的射线平行
• 被遮挡的点
  这样的点不稳定,在下一帧很可能就没了,造成匹配不上

Ｂ 找特征点的匹配对

里程计算法是估计lidar一帧里面的运动.

tk是第k帧lidar的开始时间
点云在这一帧结束的过程中,逐渐接收点 形成点云Pk

tk+1 是 第k帧雷达结束的时间 ,将k帧里面的所有点都投影到tk+1时刻的点上,形成点云 ^Pk

在下一帧点云(k+1)帧形成的时候,^Pk用来和新接受的点云(第k+1帧点云 Pk+1)一起,估计lidar的运动

假设 ^Pk 和 Pk+1 都是可用的了,然后开始找出特征点的匹配对.

对于Pk+1,找出边缘点和面点,用上一节曲率的方法. 用Ek+1和Hk+1来代表 边缘点集合和面点集合
我们将会找出^Pk 中的与之对应的边缘点和面点

在第k+1帧开始的时候,Pk+1还是空的,点在之后逐渐接收
lidar里程计在k+1帧开始接收的时候,递归的估计6自由度的运动,

在每一次迭代,用当前估计的变换,将Ek+1和Hk+1投影到tk+1时刻的坐标系中,用 ^Ek+1 和 ^Hk+1 表示投影后的点集

对于在^Ek+1 和 ^Hk+1中的每个点,需要找到在 ^Pk 中的距离最近的点 ,通过3d KD-tree的方法 . ^Pk 存在一个KD-tree中

下面是找到 在 ^Ek+1 中的边缘点 找到 ^Pk中对应边缘线的方法

i 是 ^Ek+1 中的一个点

一个边缘点由两个点表示,下面是在^Pk中找到这两个点的方法

j是在^Pk中 离i最近的点, l是在 ^Pk 中 和j不同线上的 i的 最近邻点 这样是为了确保 这3个点 不在一条直线上

然后确认 j和l 都是 边缘点 ,依然用曲率的方法确认.

找的 j和l 在不同的激光线束上,是认为不可能 一个线束 含有超过一个的边缘点, [这种就是不考虑完全与线束平行的线了,这种情况也很少]

下面是找到面点 与之对应的面的方法

i 是 ^Hk+1 中的一个面点

一个平面由三个点便是,下面是在^Pk中找到这三个点的方法

j是在^Pk中 离i最近的点,然后在j的线束上,再找一个和i最近的点,l.然后在不同线束上找和i最近的点 m .
这么做目的就是这三个点 不 是一条直线.

然后通过曲率,确认 j l m 是面点

然后就是计算 边缘点到对应直线的距离 和 面点到对应平面的距离

这两个公式 就是 向量的方法求点到直线的距离和点到平面的距离

在后面的运动估计中会通过非线性优化的方式,来让这两个距离最小,从而估计出位姿变换

C 运动估计

在每帧的过程中,lidar的运动视为匀速运动. 因为时间很短嘛

这使得可以通过线性插值的方式赋值一帧里面每个点的位姿变换

t是当前时间,tk+1是k+1帧开始的时间, Tk+1是 tk+1 和t 时刻lidar位姿的变换
那么对于 Pk+1 中的一点 i,其时间戳是 ti ,那么其对应 tk+1时刻的位姿为

通过这个,就可以把 Ek+1 投影到tk+1时刻,成 ^Ek+1 ,同理 ^Hk+1

然后求出对应的距离后通过LM方法 求出 非线性优化的结果

lidar 建图

建图算法 运行 的频率要比里程计低
每一帧 执行 一次

在k+1帧的最后,激光里程计得到了经过畸变校正的点云 ^Pk+1,并且同时得到了 一个位姿变换 Tk+1.
建图算法就是将 ^Pk+1 配准到世界坐标系中.

Qk 是 已存在地图中的点云 是前k帧的累积
Tw 是 地图中 最后一帧 k 的位姿 在tk+1时刻

用来自激光里程计的输出,建图算法 将 ^Pk+1 投影到世界坐标系中,形成 ^Qk+1

然后就是配准 ^Qk+1 和Qk, 这个过程会优化lidar的位姿 Twk+1

特征提取的方法和之前一样,但是要多10倍的个数

在进行特征点配对上面有些许的不同,主要就是因为地图的点很多,为了提高效率

地图的点会按10m的立方体划分,在立方体中有点和 ^Qk+1 相交立方体的点才会存入 3D kd-tree中.

找出Qk中特征点周围一定范围的点,对于边缘点,就把范围内的边缘点 作为一个点集,对于平面点,就把平面点作为一个点集

然后求这个点集的协方差矩阵,并计算特征值和特征向量.

如果点集是边缘点的话,那么 特征值 会一个大 ,两个小

如果点集是面点的话,那么 特征值会一个小,两个大,特征向量和面正交

边缘线和平面 的位置 就认为是 在 几何中心

然后就是求 点到直线的距离 和 点到平面的距离 方法和后面的方法和 前面里程计的类似了

然后通过LM非线性优化出 位姿
最后将整个地图进行一个体素滤波

以上就是算法部分的内容

测试结果