超详细的激光点云地面分割（可行驶区域提取）方案

目录

1. 前言

2. 基本思路分析

2.1 平面栅格法

2.2 点云法向量法

2.3 模型拟合法

2.4 面元网格法

3. 几种优秀的开源方案

3.1 linefit_ground_segmentation

3.2 plane_fit_ground_filter

3.3 depth_clustering

3.4 Patchwork&Patchwork++

4. 参考：

---

1. 前言

    在自动驾驶和机器人的应用场景中，经常需要对地面进行检测或分割（ground segmentation），以便提取出可行驶区域用于规控，或者对地面以外的障碍物点进行检测以用于避障。本文主要介绍激光雷达在地面检测分割领域的相关方法，考虑到实时性、资源占用、数据成本等情况，主要介绍传统方案，并提供几个优秀的开源方案做参考。

2. 基本思路分析

        地面分割思路通常可以分为：平面栅格法、点云法向量法、模型拟合法、面元网格法。

2.1 平面栅格法

        栅格法通常是根据设定好的尺寸建立平面网格（也可以做多层网格或者三维体素），然后将原始点云投影到对应的网格中，对每个网格中的点集提取特征，比如：平均高度、最大高度、高度差、密度等等。

具体方法：

设置网格尺寸生成栅格图；根据点的x,y坐标投影到对应的栅格中；统计每个栅格中的点集高度特征；用统计量与设定的阈值比较进行分类。

存在问题：

当激光雷达线束比较少的时候会漏检或误检，比如用16线激光雷达采集的道路数据，车辆前方20米以外的地面上，能够打到的激光点已经比较少了，而且打到障碍物上的激光线束一般也只有一条，如果在栅格中采用高度特征进行地面过滤，遇到低矮的障碍物很容易会被当成地面点过滤掉。

2.2 点云法向量法

        点的法线估计是需要其邻域点的做支持，邻域的大小一般由邻域半径值或临近点个数来表示。过大的邻域会抹平三维结构细节使得法向量过于粗糙，而过小的邻域由于包含了太少的点受噪声干扰较大。

        用法向量进行地面分割是考虑到地面点的法向量一般为竖直方向，通过设置角度阈值，求每个点的法向量与地面法向量的夹角，与阈值对比进行分类。

        考虑到人行道，车顶等道路上很多物体都存在法向量为（0,0,1）或（0,0,-1）的区域，所以通常会把点云法向量作为特征与其他方法结合过滤地面点集。

        引入法向量能够解决什么场景呢？

        在栅格法中，我们一般会利用高度信息对点集进行分类，但是往往地面高度和路两边的人行道高度差距只在30cm左右，如果没有先验信息提取出道路ROI区域的话，使用高度阈值划分道路，路沿，障碍物可能存在误差，但是引入点云法向量，可以较好的提取出道路两旁法向量突变的点集构成路沿，从而配合栅格化将道路区域，非道路区域，障碍物进行划分。

2.3 模型拟合法

        常用的模型拟合方法包括以下几种：

平面拟合-RANSAC

高斯过程-GP-INSAC

平面参数拟合（特征值）

具体方法：

以RANSAC平面拟合的方法为例，具体流程如下：

在设置的迭代次数内，每次随机选取3个点确定一个平面方程（Ax+By+Cz+D=0）；

将所有点依次带入该平面方程中，根据设定的距离阈值做判定，若在阈值范围内，则认为属于该平面的内点，否则为外点，遍历所有点后统计内点个数；

在迭代次数内内点数量最多的平面方程即为地面方程，该地面方程下的内点就是地面点云集，外点就是障碍物点云集；

在理想的路面采用基于模型类的方法分割地面，效果尚可。

存在问题：

1. 交通道路从排水考虑通常是中间凸起，两边低洼，类似于拱桥形状，虽然曲率不大，但是通过随机采样一致性算法计算地平面，可能会得到倾斜于一侧的平面作为地面方程；

2. 在上下坡的时候同样因为地面非绝对平面，计算出的地面方程，会出现把前方地面点集作为障碍物点的情况；

3. 由于RANSAC是在点云空间随机的取三个点构建平面，如果场景中存在大块墙面时，会出现将墙面作为地面方程的情况；

2.4 面元网格法

        基于面元的分割可以分为局部类型或者表面类型，常采用区域增长的方式进行地面分割。其核心是基于点法线之间角度的比较，将满足平滑约束的相邻点合并在一起，以一簇点集的形式输出，每簇点集被认为是属于相同平面。其实跟第三章提到的GPF方法有点类似，只不过是用曲率代替距离和高度。

区域增长的具体流程：

选择种子点：在当前点集A中选择有最小曲率的点加入种子点集；

区域增长：寻找种子点的邻域点，对于位于A中且与种子点的法向量夹角小于阈值的邻域点，将其加入当前区域中，如果其曲率小于阈值则也加入种子点集；

对于每个种子点重复上一步操作，最终输出一组类，每个类的点都认为是同一平滑表面的一部分；

存在问题：

采用面元区域增长的方法在一定程序上能够较好的应对地面存在曲率的情况。对于比较平缓的曲面或者平面能够达到较好的分割效果，但是实际道路中的噪声点太多，直接使用区域增长分割地面，会出现较多零星的地面点被当成障碍物点集。

而且区域增长算法的时耗较大，时间复杂度和面积成正比，对于实时性要求较高的感知算法模块，需要进一步优化。比如将平面的区域增长降维到边缘，或者划分区域，在小范围内进行分割等等。

3. 几种优秀的开源方案

        在上一节中介绍了4种地面分割的基本思路，包括算法流程和存在的问题。它们在比较平整、简单的路面可以起到不错的效果，但是实际路面经常会遇到坑坑洼洼或者存在坡度的场景，在这些情况下上一节的4种方法就很难满足实际使用需求了。本节同样介绍4种开源的地面分割算法，实测效果比较鲁棒，分别是linefit_ground_segmentation（2010）、plane_fit_ground_filter（2017）、depth_clustering（2016、2017）、Patchwork（2021），这几种方法同样可以用来做物体分割。

3.1 linefit_ground_segmentation

论文：《Fast Segmentation of 3D Point Clouds for Ground Vehicles》

代码：https://github.com/lorenwel/linefit_ground_segmentation

原理（基于扇区的直线拟合）：

1. 对点云进行有序化处理，将点云的xyz坐标转化为弧度，xy平面的长度d，和z轴高度。根据Δα将圆分成M个segment，根据d将每个segment分为N个bins。每个区域保存的是最小z值的点，从而将点云投影在一个个扇面Si上。
2. 根据每个bin里保存的z轴方向上的最低点来做直线拟合。根据直线的斜率，点到直线的距离，直线拟合的误差等来判断是否是地面点。

3.2 plane_fit_ground_filter

论文：《Fast Segmentation of 3D Point Clouds,A Paradigm on LiDAR Data for Autonomous Vehicle Applications》

代码：GitHub - HuangCongQing/plane_fit_ground_filter: 点云分割论文2017 Fast segmentation of 3d point clouds: A paradigm on lidar data for autonomous vehicle applications

原理（基于平面拟合）：

整个gpf的算法逻辑如下伪代码：

1. 挑选种子点用来进行第一次平面拟合；对输入点云按高度进行排序，取 Nlpr（Lowest Point Representative, LPR）个最小点，求高度平均值 lpr_height，选取高度小于 lpr_height + th_seeds_ 的点作为种子点。
2. 进行平面拟合，第一次是利用种子点进行拟合，后面循环采用的是根据拟合的平面筛选之后的点。筛选原则是点到平面的距离小于一定阈值即认为属于地面。经过分类以后的所有地面点被当作下一次迭代的种子点集，迭代优化。
3. 如何拟合平面？平面模型ax+by+cz+d = 0 ，即

n为垂直于平面的法向量，通过初始点集求解协方差矩阵（R3x3）,这个矩阵描述了点的分布情况，可通过SVD求解其三个奇异向量；由于是平面模型，n表示具有最小方差的方向，可以通过计算具有最小奇异值的奇异向量来求得。求出n之后将点集的平均值代入方程即可求出d。

        4. 如何求点到平面的距离？通过向量在平面法向量上的投影，即 points * n。

3.3 depth_clustering

论文：《2016-Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation》

《2017-Efficient Online Segmentation for Sparse 3D Laser Scans》

代码：https://github.com/PRBonn/depth_clustering
原理（基于range image、点云局部斜率差）：

1. 如图，计算上下两帧激光线束与地面交点的连线斜率α，与设定的阈值进行比较，小于阈值即认定为地面点。
2. 使用Savitsky-Golay滤波器对局部区域内的斜率进行平滑

3.4 Patchwork&Patchwork++

论文：《Concentric Zone-Based Region-Wise Ground Segmentation With Ground Likelihood Estimation Using a 3D LiDAR Sensor》

《Fast and Robust Ground Segmentation Solving Partial Under-Segmentation Using 3D Point Cloud》

代码：

https://github.com/LimHyungTae/patchwork

https://github.com/url-kaist/patchwork-plusplus

原理（基于多区域分割地面拟合）：

Patchwork:

如下图所示，patchwork分为三部分，CZM、R-GPF和GLE。

1. CZM（同心区模型）：可理解为不均匀或多分辨率的区域分割模型；大多数基于多平面的方法都基于假设：可观测世界可能不是平坦的。因此，地平面估计应该被推倒，通过假设可能的非平坦世界有小块或bins，并且地面在该区域内确实可以是平坦的。
2. R-GPF（区域地面平面拟合）：每个 bin 通过 R-GPF 进行地面估计，然后地面合并。在本文中，使用主成分分析 (PCA) 而不是使用 RANSAC。
3. GLE（地面似然估计）：一种用于二元分类的区域概率测试，用于判断是否是实际地面。

Patchwork++：Patchwork的扩展，

1. 一些地面分割方法需要根据环境对参数进行微调，这非常费时费力，此外，即使参数调整得当，仍可能出现部分分段不足问题，这意味着某些区域的地面分割失败，Patchwork++利用自适应地面似然估计（A-GLE）自适应地计算适当的参数。利用时间地面恢复（TGR）缓解了部分欠分割问题。
2. 提出了两种新的异常值抑制模块，即反射噪声去除（RNR）和区域垂直平面拟合（R-VPF），以防止假设因抑制噪声或非接地点而不成立的情况。

4. 参考：

https://blog.csdn.net/Yong_Qi2015/article/details/114826920