PLC-LiSLAM线-面-圆柱体-激光SLAM

1. 摘要

平面、线段与圆柱体广泛存在于人造环境中。在本文中，我们提出了一个使用这些landmark的激光雷达SLAM系统。我们的算法有三个部分：局部建图、全局建图与定位。局部与全局建图模块通过最小化三维点到模型的残差来联合优化这些landmark的参数与位姿，这个过程我们称之为PLCA(plane-line-cylinder adjustment)。可以证明，通过一些预处理，PLCA问题与这些landmark的三维点的数量无关，因此可以使得位姿优化更加高效。定位部分通过将局部地图里的面、线与圆柱体，同全局地图里的面、线与圆柱体进行配准(PLCR, plane-line-cylinder registration)，从而进行实时的位姿估计。检测这些landmark与数据关联的过程中可能会引入误检测与关联。我们通过检查后端的cost来纠正这些错误，而这个错误纠正机制是那些基于配准的方法，例如LOAM以及ICP很难做到的，因为他们并不维护数据关联。实验结果表明我们的算法比目前最先进的LiDAR SLAM算法性能要好，并且达到了实时的效果。

主要贡献

我们证明，通过一些预处理，PLCA问题的求解与面、线以及圆柱体里的三维点的数量无关。因为这些landmark的表示是无边界的，许多三维点都会被包含在这些landmark里面。对于求解大尺度的PLCA问题，我们的方法行而有效。

我们提供了一个高效的PLCR问题求解方法。我们使用一阶泰勒展开来近似旋转，考虑到两帧之间的旋转一般是足够小的。如果比较大的旋转发生在两帧之间，我们进行迭代近似。

我们的算法能够容忍足够的检测误检测。因为遮挡以及激光雷达点云的稀疏性，检测这些landmark的过程容易引入误检测。这个错误纠正机制是那些基于配准的方法，例如LOAM以及ICP很难做到的，因为他们并不维护数据关联。我们的算法通过检验后端的cost来纠正这些误检测。

2. 方法论

2.1. 预备知识

平面：本文中的平面通过一个四维向量表示。前三维表示该平面的法向量，模长为1，最后一维为坐标系原点到该平面的距离。点到平面的残差，通过计算该点到平面的距离获得。

线：本文中的直线通过一个六维向量表示。前三维表示该直线的方向，后三维表示从原点出发的，垂直于该直线的向量，并且其模长为原点到直线的距离。计算点到线的残差，通过计算点到直线的距离获得。

圆柱体：本文中的圆柱体通过一个七维向量表示，前六维为该圆柱体所在的直线表示（同上），最后一维为该圆柱体的半径。计算点到圆柱体的残差，通过计算点到圆柱体所在的直线距离，并将该距离与半径比较获得。

2.2. 系统总览

本文提出的系统由前端与后端组成，前端检测面、线与圆柱体，并建立局部到全局的数据关联，从而进行后续的位姿估计。前端也决定了什么时候一个新的关键帧应当被创建。后端包括局部与全局的PLCA。局部的PLCA通过滑窗优化位姿与landmark。全局的PLCA优化所有的PLCA以及除了第一帧外的所有位姿。当有Landmark被再次观测到时，全局的PLCA便会被触发。

2.3. 前端

2.3.1. Landmark检测

对激光雷达每一条线的数据进行切分：类似于LOAM，计算每个三维点的曲率，并将属于edge点的部分提取出来，组成集合E。以每条线上每两个edge点作为两端，可以把这条线分为许多段，这些段里的点为平坦点，组成集合F。

平面与圆柱体检测：对于每个线上的每个段里的平坦点，寻找与其相近的，但不属于同一条线上的平坦点，如果用RANSAC法拟合平面的平均误差比较小，则利用平面模型来拟合，否则用圆柱体来拟合这些点。

直线检测：对每个段里的edge点，找到所有与其相近的，但属于另一条线的edge点p。利用这两个点可以求得直线方程。对p点再寻找一个在其他线的最近点p2。如果p2到该直线的距离足够小，则利用这三个点来拟合一条直线。重复这个过程直到没有点能够被加入到直线中。

2.3.2. Landmark关联

当新一帧点云获得后，首先计算属于该帧的集合E与集合F。并对这两个集合分别构建KD-tree。对上一帧里的每个点p，寻找其在当前帧的n个最近点。如果p属于面或者圆柱体，则这n个最近点都被保留，如果p属于直线，则保留这n个点的曲率最大的一个点。随后，利用RANSAC算法找到符合模型的内殿。通过这些内点，我们之后会使用PLCR方法来进行位姿估计。

2.3.3. 关键帧创建

受到ORB-SLAM的启发，当以下情况发生时，新的关键帧会被创建：

当前帧20%以上的点不能被追踪与关联。

当前帧与上一个关键帧的旋转角度大于5°。

当前帧与上一帧的距离大于设定的阈值。

对于新的关键帧，我们在没被关联到的三维点上检测面、线与圆柱体。对于新检测出来的landmark，首先会寻找与已有的landmark的均方根之差，如果大于设定的阈值，则新增一个landmark。

2.4. 面-线-圆柱体 联合优化 (PLCA)

2.4.1. 全局PLCA

在全局的PLCA中，对所有点计算点到模型的损失，从而构建优化问题来同时优化位姿与landmark的参数。

2.4.2. 局部PLCA

2.5. PLCR

2.5.1. 扫描内运动

采用线性插值来补偿扫描内运动

2.5.2. 约束

相对位姿内的旋转幅度相对较小，所以采用一阶泰勒展开来近似旋转：

 

实验

参考文献

PLC-LiSLAM：线-面-圆柱体-激光SLAM（RAL 2022） - 知乎

腾讯内容开放平台