Patchwork++：基于点云的快速、稳健的地面分割方法

1. 背景

论文发表在2022IROS，是Patchwork的改进版本。算法通过数学方法进行快速而鲁棒性很强的地面分割，在智能机器人上的可操作性非常强。通过微调算法，可以应用于16-beams等多种规格的激光雷达。由于激光雷达点云数据标注的难度非常大，生产中自制数据集的成本很高，我做了一些对比实验，和RandLA-Net，Cylinder3D，GndNet，PVKD等机器学习模型进行比较，在分割地面这一任务上，用自制数据集测试，Patchwork++的表现远好于其他。

本文对 Patchwork 和 Patchwork++ 两篇论文作分析和介绍，简单讲述论文作者的思路和数学原理，最后展示根据需求修改后的算法表现效果——在不同设备上表现依然迅速稳健。

2. 摘要

传统数学方法，采用求反射梯度与地面对比的思路，对于高度有差别的平面难以识别（如低矮路沿），分割出的地面不是很合适。论文提出一种鲁棒性很强的分割方法，基于 Patchwork 的思路进行改进，自适应地调整参数，效果提升明显，运行速度极快。

3. 理论

Patchwork++ 的基本理论思路和 Patchwork 类似，下面将结合两篇论文，对方法中的每个步骤进行说明和讲解。

3.1. Problem Definition 问题定义

对地面分割做数学建模：对于所有的点云表示为 $P=\left\{{\bf p}_{1},{\bf p}_{2},\ldots,{\bf p}_{k},\ldots,{\bf p}_{n}\right\}$ ，对点集进行二分类，分为G地面和N非地面，其中包括车辆，行人，植被，墙体等。其研究目标是最大化TP，最小化FN和FP，对于点集P可以有以下表示：

$P=\bigcup_i p_i=G \cup N=\hat{G} \cup \hat{N}$

3.2. RNR: Reflected Noise Removal 反射噪声去除

思路：取出距离传感器较近的、高度小于某阈值，强度小于某阈值的数据点，这些点被认为是噪声点。实际情况下，由于激光雷达穿透效应，在地面以下偶尔也会产生噪声点，将其称为虚拟噪声，用一个自动更新的高度阈值，认为该阈值以下的点都为噪声，将其去除。

实现：维护两个阈值，高度阈值 $h_{noise}$ 和强度阈值 $I_{noise}$ ，高度阈值在 A-GLE 环节自动更新。 $h_{noise}$ 需要自动更新，其原因在于如果采用固定值 $z_{min}$ ，在向下陡坡的场景中，就会去除真实的地面点，造成数据缺失。如下图所示，(a) 表示GT，红色点表示为高度较低的地面；(b) 表示用定值基于直接高度去除了真值（Patchwork方法）；(c) 表示在 Patchwork 算法下造成了比较严重的欠分割情况；(d) 表示 Patchwork++ 算法维护一个自动更新的 $h_{noise}$ ，成功识别到了高度较低/下降坡度的地面。

3.3. CZM: Concentric Zone Model 同心区域模型

思路：用设计的扇区分割路面，用扇区拟合路面几何形态。基于路面非平坦假设，不应直接估计地面，而是通过假设非平坦地面有多个小块bin，在小块之内地面是平坦的。以往的方法用统一的极坐标系表示扇区S，再将扇区S划分为规则间隔的BIN。范用性不强，有以下两个缺陷：

稀疏问题：随距离增长，点云越发稀疏，难以找到正确的平面。
代表性问题：靠近原点的bin太小，难以表示扇区的空间特征，有时会导致扇区内地面法向量估计失败。

实现：以一种不增加计算复杂度的方式在bin之间分配适当的密度，数学模型如下：

全点集P被分为多个zone，每个zone由不同面积大小的bin组成，定义为：

其中， $Z_m$ 表示为zone， $N_z$ 表示zone的个数，那么 $Z_m$ 表示为：

$L_{min, m}$ 和 $L_{max, m}$ 表示 $Z_m$ 的最小和最大向径边界；同样被划分为多个 $N_{r, m} \times N_{\theta, m}$ 个bins，对于不同的zone，bin的大小也不同。在这里， $S_{i,j,m}$ 的定义为：

$\mathcal{S}_{i, j, m}=\left\{\mathbf{p}_k \in Z_m \left\lvert\, \frac{(i-1) \cdot \Delta L_m}{N_{r, m}} \leq \rho_k-L_{\min }<\frac{i \cdot \Delta L_m}{N_{r, m}}\right., \frac{(j-1) \cdot 2 \pi}{N_{\theta, m}}-\pi \leq \theta_k<\frac{j \cdot 2 \pi}{N_{\theta, m}}-\pi\right\}$

其中， $\rho_k=\sqrt{x_k^2+y_k^2}, \theta_k=\arctan 2\left(y_k, x_k\right), \Delta L_m=L_{\max , m}-L_{\text {min }, m}$ ，且 $L_{max, m} = L_{min, m+1}$ ，对于全局最小边界 $L_{min}$ ，其用于考虑移动平台或车辆附近的空旷情况。

实际实现，对于 $Z_{1},Z_{2},Z_{3},Z_{4}$ 分别被称为中心区、四分之一区、半区和外区，统称为一个central zone，其中 $Z_1, Z_4$ 的bin大小被设置的更大，以解决稀疏性问题和表示性问题。其计算方法为：

3.4. R-VPF: Region-wise Vertical Plane Fitting 区域垂直平面拟合

思路：由于路面可能存在一些高低差，在拟合时认为高程差不多的平面为同一个路面。拟合平面前，找到每个bin中的垂直点子集，将bin内的垂直平面找到并作过滤掉，拟合出更准确的平面。由于 PCA（点云主成分析）对于异常点敏感，经过这一步骤处理后可适用范围更广。

实现：利用 CMZ 分割得到的bin，设第n个bin为 $P_n$ ，通过以下4个步骤得到估计出的垂直点 $\hat{V}_n$ ：

以如下操作对bin进行k次迭代，挑选出足够的种子点集。

预选种子点集

进行k次迭代，每次迭代选取”一些”（论文原句）高度最低的点，求这些点的平均值 $\mathbf{m}_{n}^{k}\in\mathbb{R}^{3}\;$ ，以及该子集的单位法向量 $v^k_{3, n}$ ，代表 PCA 后最小特征值对应的归一化后的单位特征向量。

潜在垂直平面点集

对第k次迭代的种子点集 $\hat{P}_{n}^{k}$ ，挑选出潜在的垂直平面点集 $\hat{W}_{n,k}$ ，公式如下:

其中，k = 1时 $\hat{P}_{n}^{k}\leftarrow P_{n}$ ，即种子点集为第n个bin的全部点。每次迭代时 $\hat{P}_{n}^{k}\leftarrow P_{n}-\bigcup_{i=1}^{k-1}\hat{V}_{n}^{i}$ ，即上一次的种子点集减去之前迭代出的所有垂直点； $d_v$ 表示估计的垂直平面的距离阈值。

二值化验证点集

对第k次迭代的潜在垂直平面点集 ${\hat{W}}_{n,k}$ ，二值验证出认为真正垂直的点集 $\hat{V}_n^k$ ，公式如下：

其中， $u_z$ 表示z轴方向的单位法向量 $[0, 0, 1]^T$ ， $\theta_v$ 是估计平面的单位法向量的阈值（点的法向量和

z轴法向量做点乘，两个单位向量的点乘结果就是夹角的cos值，判断该值与90度的相近程度。在范围内，由这次迭代的潜在点找到的垂直点集，可以被认为是垂直点集；否则，这次的垂直点集，不能认为是垂直的）。

最后，累积所有的垂直点得到总的垂直点集 $\hat{V}_n$ ， $K_v$ 表示总的迭代次数，公式如下：

3.5. R-GPF: Region-wise Ground Plane Fitting 区域地面拟合

思路：估计局部地面，将局部地面合并。对 R-VPF 处理后的各个bin进行PCA，取第三个特征代表高度特征。采用 PCA 在速度上至少比 RANSAC 快两倍，进行预处理后，效果类似。通俗来讲，设置一个阈值，每次迭代更新阈值，3次迭代后挑选出合适的地面种子点。

实现：对以上操作得到的bin进行计算，数学模型如下：

对zone内n个bin内的点集命名为 $S_n$ ，共计有 $\mathcal{N}_c=\Sigma_{m=1}^{N_z} N_{r, m} \times N_{\theta, m}$ 个这样的集合。如果一个 zone内有足够多的bin，即可以认为足够拟合平面，选取z最低的点作为种子点。令 $\bar{z}_{i n i t}$ 作为 $N_{seed}$ 个种子点的z的平均值，则初始的地面的估计如下：

其中函数 $z( \cdot )$ 返回点的高度值， $z_{seed}$ 表示种子点挑选高度阈值。第一次估计的地面点，是z高度不大于平均高度与高度阈值之和 $\bar{z}_{i n i t} + z_{seed}$ 的点集。

算法经过l次迭代计算出bin的地面拟合 $\hat { G } _ { n } ^ { l }$ ，第l次迭代的地面法向量为 $n^l_n$ 。接着计算地面系数 $d _ { n } ^ { l }$ ：

其中， $\bar{p}^l_n$ 是第l次迭代，所有被分类为地面的点的平均值，用l次迭代得到的地面估计的平均点与法向量点乘，可以反映两个向量之间的投影值。第l+1次迭代的地面估计值计算公式为：

其中， $\hat{d}_k=-(\mathbf{n}_n^l)^T\mathbf{p}_k$ ， $M_d$ 表示平面高度阈值。实验证明，当 $\hat{G}_n=\hat{G}_n^3$ 即进行3次迭代的效果最好。

3.6. A-GLE: Adaptive Ground Likelihood Estimation 自适应地面似然估计

思路：对上一步处理后得到的bin内种子点做处理，估计一个bin是否为地面。从 Uprightness 垂直度 $\phi(\cdot)$ 、Elevation 高程差 $\psi(\cdot)$ 和 Flatness 平整度 $\varphi(\cdot)$ 三个方向设计。

实现：从Patchwork 的 GLE 步骤讲起，如下设计函数用于评估bin为是否为平面：

Uprightness $\phi$ ：为了使 $\hat{G}_{n}$ 中大部分点都归属于地面（尽可能为TP），对每个zone的法向量 $\mathbf{v}_{3,n}$ 做如下操作：

其中， $z=[0,0,1]^T$ ， $\theta_{\tau}$ 为设置的角度阈值，这部分实际计算为bin内法向量与X-Y平面的夹角，当夹角趋于 $90^{\circ}$ 时，意味着bin拟合的区域平面越平整。 $\theta_{\tau}$ 在论文中被经验化地设置为 $45^{\circ}$ ，即拟合出的区域平面的最大倾斜角度为 $45^{\circ}$ 。

Elevation $\psi$ ：为了解决高度不同的平面计算得到一样的垂直度，而误把一些建筑物或车辆顶部当作地面的误检，引入高程差计算项，具体如下：

其中 $\kappa(\cdot)$ 为使 $r_n$ 呈指数增长的自适应中点函数；如果 $r_{n}<L_{r}$ ，即距离太远，则不予考虑，认为该平面仅由平整度和垂直度考虑。当bin的平均高度 $\bar{z}_{n}<\kappa(r_{n})$ 时，即认为其为地面的可能性较高，输出值大于0.5；反之，认为其为地面的可能性较低，输出值小于0.5。

Flatness $\varphi$ ：为增加上坡地面的判断，利用高度判断的结果，对一些坡度较高的上坡地面进行拟合。

当 $\psi \geqslant 0.5$ 时，地面已经足够平整，无需计算该项；当 $\psi < 0.5$ 时，需要判断其是否是可能的上坡路面。其中， $\zeta>1$ 为一个设定的系数，目的是使得函数结果更明显； $\sigma_n$ 表示bin的平整度， $\sigma_{\tau,m}$ 为设定的平整度阈值，计算方法如下：

$\lambda_{1,n},\lambda_{2,n},\lambda_{3,n}$ 为bin的 PCA 特征，按降序排列。

最后，对所有bin统一以上三个方面，计算出整个点云的拟合平面 $\hat{G}$ ，统计 $\mathcal{L}(\theta,\mathcal{X})>0.5$ 的bin的集合，即为所拟合出的路面。

改进：不难发现，GLE 中设计了许多参数，根据不同环境需要有不同的选择；在选择参数时，尤其是在计算高程差 $\psi$ 所用的 $\kappa(r_n)$ 、计算平整度 $\varphi$ 时所用的 $\sigma_{\tau,m}$ 非常耗时。Patchwork++论文基于 GLE 提出 A-GLE，为方便描述，将原先的 $\bar{z}_{n},\sigma_{n},\kappa(r_{n}),\sigma_{\tau,m}$ 更换为 $e_{n},f_{n},e_{\tau,m},f_{\tau,m}$ ，即高程相关用e表示，平整度相关用f表示。

图 3.5-1 U、H、C表示城市、高速公路和乡村，不同场景下e, f 的分布具有，明显差异

为解决该问题，Patchwork++论文基于 GLE 提出 A-GLE，一种自适应的参数调整方法，同时更新 RNR 步骤的高度阈值 $h_{noise}$ 。

Elevation e：用 Definite Ground $D_m$ 表示 CZM 步骤中第m个zone，计算 $\phi(\cdot)\cdot\psi(\cdot)$ 满足平面拟合标准。

令 $E_m$ 为 $D_m$ 中bin的 $e_n$ 的集合，基于 $E_m$ ，对高程标准阈值 $e_{\tau,m}$ 进行更新：

其中， $mean(\cdot)$ 表示集合均值， $stdev(\cdot)$ 表示集合标准差， $a_m$ 为 CZM 中第m个ring的标准差修正增益，是定值。

Flatness f：由于将空间划分为bin，经过 PCA 计算得到的特征值 $\lambda_{1, n}$ 和 $\lambda_{2, n}$ 是直接基于点云的，拟合出的平面会有坐标系未对齐的问题，导致平面拟合出现错误。这里对原先算法作纠正，直接用

$\lambda_{3,n}=f_n$ 平面法向量代替平整度估计。

类似的，令 $F_m$ 为 $D_m$ 中 $f_n$ 的集合，基于 $F_m$ ，对平整度标准阈值 $f_{\tau,m}$ 进行更新：

其中， $b_{m}>0$ 表示 CZM 中第m个ring的标准差修正增益，是定值。

Noise Removal Height h：思路类似。

其中， $E_1$ 表示距离传感器最近的一环zone中的种子点z的集合， $\delta < 0$ 为需要移除的高度边界。

3.7. TGR: Temporal Ground Revert 临时地面还原

思路：对 A-GLE 处理后估计的地面进行纠正，恢复一些被误检的bin。一个bin内可能存在部分异常点使得地面拟合失效（例如崎岖的草地），计算值很可能超过阈值而导致错误估计。这是因为 A-GLE 是随时间推移依据所有的bin更新阈值，在当前时间更新的值会在下次被利用，但当前时间下的估计就会发生偏差；且采用均值加权更新，显然对于一些估计值偏大的bin会出现误检。