激光雷达障碍物检测与追踪实战——基于同心区域的区域地面分割和地面似然估计

对于在地面移动平台上导航或邻近目标识别，地面分割是至关重要的。然而，地面并不平坦，存在陡峭的斜坡；崎岖不平的道路；或物体，例如路缘石、花坛等。为了解决该问题，下面学习一种最新的地面分割方法，它对于解决分割不足问题具有鲁棒性，并且工作频率超过 40 Hz。

以下重点介绍了 Patchwork 每个模块背后的问题定义和推理。 Patchwork 主要由三部分组成：CZM、R-GPF 和 GLE。

Patchwork原理

问题定义

首先，我们首先将此时的点云表示为P。然后，让 P = { p 1 , p 2 , . . . , p k , . . . , p N } P = \{p_1, p_2, . . . , p_k, . . . , p_N\} P={p1​,p2​,...,pk​,...,pN​} 是一组云点，包含由 3D LiDAR 传感器获取的时刻的 N 个点，其中每个点 $p_k$ 由笛卡尔坐标中的 p k = { x k , y k , z k } p_k = \{x_k, y_k, z_k\} pk​={xk​,yk​,zk​} 组成。 在本文中，P被明确分为两类：一组地面点 $G$ 及其补集 $G_c$，满足 $\hat{G}\cup {\hat{G}}^c=P$。注意 $G_c$ 表示非地面点，包括车辆、墙壁、街道树 、行人等。

接下来，估计的地面点可以定义为 $\hat{G}$ ,由于估计不可避免地包含固有误差，实际上来自非地面物体的一些点可能包含在 G^ 中，反之亦然。 综上所述，$\hat{G}$ 和 ${\hat{G}}_c$ 表示如下：

$$\hat{G}=TP\cup FPand{\hat{G}}^c=FN\cup TN$$
其中$\hat{G}$和 ${\hat{G}}^c$也满足$\hat{G}\cup {\hat{G}}^c=P$，并且 TP、FP、FN 和 TN 分别表示真阳性、假阳性、假阴性和真阴性的集合。 因此，我们的目标是从点云 P 中辨别$\hat{G}$和 ${\hat{G}}^c$，同时估计尽可能少的 FP 和 FN。

同心区模型

如前所述，大多数基于多平面的方法都基于假设：可观测世界可能不是平坦的。因此，地平面估计应该被推倒，通过假设可能的非平坦世界有小块或bins，并且地面在该区域内确实可以是平坦的

因此，以前的一些方法利用统一的极坐标网格表示，或 S，将点云划分为具有规则间隔的径向和方位角方向的多个 bin，即环和扇区 。更具体地说，让 $N_r$和 $N_{\theta }$ 分别是环和扇区的数量。然后，S 被划分为相同大小的扇环bin，其径向大小为 $L_{max}/N_r$，其中 $L_{max}$ 表示最大边界长，扇区方位角大小为 $2\pi /N_{\theta }$，如图 2(a) 所示。

图 2. (a) 均匀极坐标网格描述(b) 我们基于 CZM 的极坐标网格描述 ©范围的累积分布函数 (CDF)，其中超过 90% 的地面点位于20m以内。

不幸的是，如图 2© 所示，为了考虑泛化，在 SemanticKITTI 数据集 [1] 上的整个序列上测量的实验证据表明，大多数地面点都位于靠近传感器框架的位置。即90%以上属于地面的点位于20m以内。
因此，S 有两个限制。首先，随着距离越来越远，点云变得太稀疏而无法找到正确的地平面，我们称之为稀疏问题。一些方法自适应地调整 bin 的大小以应对对数点分布。但是，bin 大小以线性或二次方式增加，因此稀疏问题并未完全解决。另一方面，当靠近原点的 bin 的太小而无法表示 S 中的单位空间时，有时会导致地平面的右法向量估计失败，我们称之为代表性问题。

为了解决这些问题，提出了基于 CZM 的极坐标网格表示，表示为 C，以一种计算不复杂的方式在 bin 之间分配适当的密度。因此，P被分成多个区域，每个区域由不同大小的bin组成，如图2（b）所示。设 $〈N〉=1,2,...,N$，那么我们提出的模型定义如下：

$$C=\bigcup _{m\in <N_Z>}{Z}_m$$
其中 $Z_m$ 表示 C 的第 m 个区域，$N_Z$ 表示区域的数量，本文根据经验将其设置为 4。 令
Z m = { p k ∈ P ∣ L m i n , m ≤ ρ k < L m a x , m } Z_m=\{p_k\in P |L_{min,m}\leq \rho _k < L_{max,m} \} Zm​={pk​∈P∣Lmin,m​≤ρk​<Lmax,m​}
其中 $L_{min,m}$ 和 $L_{max,m}$ 分别表示 $Z_m$ 的最小和最大径向边界； 然后，$Z_m$ 也被划分为 $N_{r,m}\times N_{\theta ,m}$ 个 bin，其中每个区域具有不同的 bin 大小。 因此，每个 bin $S_{i,j,m}$ 定义如下：

其中，${\rho }_k=\sqrt{x_k^2+y_k^2},{\theta }_k=arctan2(y_k,x_k),\Delta L_m=L_{max,m}-L_{min,m},L_{max,m}=L_{min,m+1}m=1,2,3$

注意：$L_{max,4}=L_{max}$，$L_{min,1}=L_{min}$

其中全局最小边界 $L_{min}$用于考虑移动平台或车辆附近的空旷。 实际上，$Z_1$、$Z_2$、$Z_3$ 和 $Z_4$ 分别称为中心区、四分之一区、半区和外区。 因此有
$$L_{min,2}=\frac{7.L_{min}+L_{max}}{8},L_{min,3}=\frac{3.L_{min}+L_{max}}{4},L_{min,4}=\frac{L_{min}+L_{max}}{2}$$

请注意，Z1 和 Z4 中的 bin 大小设置得更大，以解决稀疏性问题和可表示性问题。 因此，与现有的统一表示相比，C 提高了可表达性，从而允许对法线向量进行稳健估计，从而防止分割不足。 此外，它还减少了实际的 bin 数量，例如，从 S 中的 3240 个到 C 中的 504 个，从而能够以超过 40 Hz 的频率运行（参见第 IV.E 节）

区域级地平面拟合

此后，每个 bin 通过 R-GPF 分配估计的部分接地； 然后合并部分地面点。 在本文中，使用主成分分析 (PCA) 而不是使用 RANSAC。 当然，与 PCA 相比，RANSAC 对异常值的敏感性往往较低。 然而，使用 PCA 的速度比使用 RANSAC 快得多，并且表现出可接受的性能； 因此，基于 PCA 的估计更适合作为预处理过程。 此外，实验表明基于 PCA 的方法至少比基于 RANSAC 的方法快两倍。

给定一个bin，令$C\in R^{3\times 3}$为单位空间内云点的协方差矩阵； 三个特征值 ${\lambda }_a$和对应的三个特征向量${\nu }_a$计算如下：
$$C{\nu }_a={\lambda }_a{\nu }_a$$
其中 α = 1、2、3，并假设${\lambda }_1\ge {\lambda }_2\ge {\lambda }_3$。 然后，对应于最小特征值的特征向量，即 $v_3$最有可能表示地平面的法线向量。 因此，设 $n={\nu }_3=[abc{]}^T$ ，平面系数可以计算为 $d=-n^T\bar{p}$，其中$\bar{p}$ 表示单位空间的平均点。

为简单起见，让第 n 个 bin 是所有 bin 上的 Sn，其值等于$N_c={\sum }_{m=1}^{N_z}{N}_{r,m}\times N_{\theta ,m}$。 如果 Sn 的基数足够大，则选择最低高度的点作为初始种子。 事实上，每个bin 子高度最低的点最有可能属于地面。 设̄${\tilde{z}}_{init}$为所选种子点的总$N_{seed}个数的z均值； 然后，初始估计地面点集 ${\hat{G}}_n^0$ 得到如下:
G ^ n 0 = { p k ∈ S n ∣ z ( p k ) < z ˉ i n i t + z s e e d } \hat G^0_n= \{ p_k \in S_n | z(p_k) < \bar z_{init}+z_{seed}\} G^n0​={pk​∈Sn​∣z(pk​)<zˉinit​+zseed​}
其中 z(·) 返回一个点的 z 值，$z_{seed}$ 表示高度边距。

因为我们的方法是迭代的，所以让在第$l$ 次迭代时设置的估计地面点为${\hat{G}}_n^l$，然后得到${\hat{G}}_n^l$的法向量$n_n^l$，平面系数$d_n^l$计算为:$d_n^l=-(n_n^l{)}^T{\bar{p}}_n^l$，其中${\bar{p}}_n^l$表示${\hat{G}}_n^l$的均值点，最后${\hat{G}}_n^{l+1}$公式计算如下：

其中 ${\hat{d}}_k=-(n_n^l{)}^T{p}_k$ 和 Md 表示平面的距离边距。 该过程重复多次。 根据 Zermas 等人的说法， ${\hat{G}}_n={\hat{G}}_n^3$在经验上是本文中每个 Sn 的最终输出。

请注意，原始 R-GPF与我们的主要区别在于，我们涉及使用自适应初始种子选择来防止 R-GPF 收敛到局部最小值。有时，由于多径问题或 LiDAR 信号的反射，会在实际地面下方获取错误的点云，如图 3(a) 所示。观察到这种现象多发生在$Z_1$，因为反射只发生在信号比较强的区域。这些异常值阻碍了 R-GPF 估计正确的地平面。

图 3. (a) 在应用自适应初始种子选择之前和之后 (b) 在帧 435 周围的 SemanticKITTI 数据集 [1] 序列 00 的 R-GPF 上应用自适应初始种子选择以防止错误点（虚线所示）的影响 . 在（a）中，虚线圆圈内的误测点有时被选为初始种子，然后导致区域地平面拟合失败，由蓝色区域表示。 在本文中，绿色、蓝色和红色点分别表示 TPs、FNs 和 FPs。 蓝点越少越好。

为了解决这个问题，我们利用仅在$Z_1$中的地面点的 z 值主要分布在 $-h_s$ 附近的事实，其中 hs 代表传感器高度。因此，当估计 ${\hat{G}}_n^0$ 时，如果 $z_k$ 低于 $M_h\cdot h_s$，则过滤掉属于$Z_1$ 的 $S_n$ 中的 $p_k$，其中 $M_h$ < -1 是高度边际。对于不属于$Z_1$的$S_n$，自适应阈值随着m变大而减小，以避免对可能来自下坡的点进行不当过滤，这些点实际上是TP。

地面似然估计

为有必要稳健地辨别${\hat{G}}_n$是否属于实际地面，提出了 GLE，这是一种用于二元分类的区域概率测试。为这样做，Patchwork 利用 GLE 来提高整体精度，不包括由非地面点组成的初始非预期平面。

令 $L(\theta |\chi )$为 GLE，其中 θ 表示 Patchwork 的所有参数，$\chi$ 表示遵循具有密度函数 f 的连续概率分布的随机变量。让我们假设每个 bin 彼此独立。然后，$L(\theta |\chi )$表示为

其中 θn 和 Xn 分别表示每个 ${\hat{G}}_n$ 的参数和随机变量。请注意，下标 n 表示参数来自 ${\hat{G}}_n$。

根据我们的先验知识，每个${\hat{G}}_n$是地面点以垂直度、高程和平面度来定义，分别表示为 $\phi (\cdot )、\psi (\cdot )和\phi (\cdot )$，如下：
$$f(X_n|{\theta }_n)\equiv \phi (v_3,n)\cdot \psi ({\bar{z}}_n,r_n)\cdot \phi (\psi ({\bar{z}}_n,r_n),{\sigma }_n)$$

其中${\bar{z}}_n$, $r_n$, 和 ${\sigma }_n$ 表示平均 z 值，原点之间的距离和 $S_n$ 的质心，以及表面变量，其中 ${\sigma }_n=\frac{{\lambda }_{3,n}}{{\lambda }_{1,n}+{\lambda }_{2,n}+{\lambda }_{3,n}}$。

• Uprightness
  如果 ${\hat{G}}_n$属于实际地面
  （即大部分点都在 TP 中），观察到 $v_{3,n}$ 可能与地面车辆接触的地面正交。换句话说，$v_{3,n}$ 倾向于垂直于传感器框架的 X-Y 平面。因此，提出了垂直度指示函数来利用几何特征作为
  

  其中 z = [0 0 1] 和${\theta }_{\tau }$是直立边距，表示为$v_{3,n}$与 X-Y 平面之间的角度。也就是说，${\theta }_{\tau }$越大，标准就越保守。如图 4(a) 和 (b) 所示，红色区域代表不满足垂直度的情况，因此$\varphi (v_{3,n})$等于 0。通过实验，我们将${\theta }_{\tau }$设置为 45°，根据经验确定它足够严格（参见第 IV.B 节）。

• Elevation
  不幸的是，仅使用 uprightness 无法过滤属于汽车引擎盖或车顶的 ${\hat{G}}_n$。此外，当汽车等大型物体靠近传感器框架时，会发生遮挡，从而产生局部观察问题。也就是说，被遮挡空间上方的部分测量浊点被预测为${\hat{G}}_n$，事实上，这不是空间中的最低部分。这种现象如图4（a）的左侧所示。

  
  图 4.

  • (a) (L-R)：在 SemanticKITTI 数据集上的第 2,810 帧周围的序列 00 应用高程滤波器的前后。 请注意，表示 FP 的红色浊点已被过滤。
  • (b) (L-R)：在第 286 帧周围为序列 10 应用平滑的前后，其中青色点表示通过平滑度恢复的 TP，之前通过高程过滤。 绿色、蓝色和红色区域分别表示满足 GLE、按高程过滤和按平滑过滤的区域。
  • © (T-B)：在 SemanticKITTI 数据集的整个序列上，在中央区 $Z_1$ 和四分之一区 $Z_2$ 和外部区 $Z_4$ 中单独使用垂直度，两个相应的部分地面估计${\hat{G}}_n$之间的平均 z 值的概率分布函数 (PDF)。 虚线表示传感器的地面高程

  为了解决这个问题，提出了一个条件逻辑函数 $\psi ({\hat{z}}_n,r_n)$。高程过滤器的关键思想是由 Asvadi 等人提出的。 [9]：一旦传感器框架附近的 ${\hat{z}}_n$ 与 $-h_s$ 相比相当高，${\hat{G}}_n$可能不是地面。

  实验证据支持我们的理论，如图 4（c）所示。仅使用直立性，$T{P}_s$ 和 $F{P}_s$ 基于 ${\hat{z}}_n$ 变得可区分，当 $r_n$ 很小时，$T{P}_s$的损失很小，即${\hat{G}}_n$在 $Z_1$ 和 $Z_2$ 中的情况。相反，当 rn 很大时，$T{P}_s$ 和 $F{P}_s$ 是无法区分的，即 ${\hat{G}}_n$在 $Z_4$ 中。

  基于这些观察， $\psi ({\hat{z}}_n,r_n)$定义如下：
  

  其中 $\kappa (\cdot )$ 表示根据 $r_n$ 呈指数增长的自适应中点函数。如图 4(a) 所示，如果 ${\bar{z}}_n$ 低于 $\kappa (r)$，则当$r_n$ 小于恒定范围参数$L_\tau $时，$ψ(\hat z_n, r_n)$ 的值高于 0.5。请注意，当 $r_n$ 超过$L_\tau $时，$ψ(\hat z_n, r_n)$ 总是变为 1，因为随着 rn 变大，不清楚 G ^n 是来自非地面物体还是来自陡峭的斜坡。

• Flatness
  最后，平坦度的目的是还原一些通过高程过滤的 $F{N}_s$，如果它们绝对是一个偶数平面。例如，如果${\hat{G}}_n$ 属于一个非常陡峭的上坡，因此如果${\hat{z}}_n$ 大于$\kappa (r)$，则 ${\hat{G}}_n$有时会通过 $\psi ({\hat{z}}_n,r_n)$过滤掉。为了解决这个问题，我们利用表面变量${\sigma }_n$来检查被认为是 $F{N}_s$ 的 ${\hat{G}}_n$ 的平坦度，即使$\psi ({\hat{z}}_n,r_n)$低于 0.5。为此， $\phi (\psi ({\hat{z}}_n,r_n),{\sigma }_n)$的可定义为
  

  其中 $\zeta >1$ 和 ${\sigma }_{\tau ,m}$ 分别表示增益的大小和取决于 $Z_m$ 的表面变量的阈值。通过这样做，陡峭上坡的 GLE 增加，尽管${\hat{z}}_n$高于 $\kappa (r_n)$，但它们可以恢复为地面估计。

  因此，最终估计的地面点可以直接表示为：
  
  其中 [·] 表示 Iverson 括号，如果条件满足则返回 true，否则返回 false。

代码解析

本文采用kitti数据集，是通过HDL-64E的激光雷达采集的数据，在该数据的基础上来验证本说所说的地面分割方法

首先在地面分割之前需要进行一些预处理工作，包括提取ROI感兴趣区域和体素滤波

提取ROI

提取ROI感兴趣的区域可分为两种：

• 因雷达安装位置的原因，近处的车身反射会对Detection造成影响，需要滤除
• 过高的目标，过远处的点，如大树、高楼，对于无人车的雷达感知意义也不大，过远激光雷达反射滤，效果不好，也需要滤出

pcl::PointCloud<pcl::PointXYZI>::Ptr RoiClip::GetROI(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr in,pcl::PointCloud<pcl::PointXYZI>::Ptr &out)
{pcl::PointCloud<pcl::PointXYZI>::Ptr clipCloud = ClipVehicle(in);if (clipCloud->points.size() > 0){for (auto &p : clipCloud->points){if (IsIn(p.x, roi_x_min_, roi_x_max_) && IsIn(p.y, roi_y_min_, roi_y_max_) && IsIn(p.z, roi_z_min_, roi_z_max_))out->push_back(p);}ROS_INFO("GetROI sucess");return out;}ROS_ERROR("GetROI fail");return nullptr;
}pcl::PointCloud<pcl::PointXYZI>::Ptr RoiClip::ClipVehicle(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr in){if(in->points.size() > 0){pcl::PointCloud<pcl::PointXYZI>::Ptr out(new pcl::PointCloud<pcl::PointXYZI>);for (auto &p : in->points){if (IsIn(p.x, vehicle_x_min_, vehicle_x_max_) && IsIn(p.y, vehicle_y_min_, vehicle_y_max_) && IsIn(p.z, vehicle_z_min_, vehicle_z_max_)){}else out->push_back(p);}return out;}return nullptr; 
}

体素滤波

由于点云聚类的实时性要求，我们需要通过减少点云的密度来加速地面过滤和后续聚类。一种简单的方法就是使用Voxel Grid Filter对点云进行降采样，代码如下：

void VoxelGridFilter::downsample(const pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud_ptr,pcl::PointCloud<pcl::PointXYZI>::Ptr &out_cloud_ptr)
{// if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)if (leafSize >= 0.1){// Downsampling the velodyne scan using VoxelGrid filterpcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;voxel_grid_filter.setInputCloud(in_cloud_ptr);voxel_grid_filter.setLeafSize(leafSize, leafSize, leafSize);voxel_grid_filter.filter(*out_cloud_ptr);}else{out_cloud_ptr = in_cloud_ptr;}
}

patchwork地面过滤

void PatchWork::estimate_ground(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_in,pcl::PointCloud<pcl::PointXYZI>::Ptr &ground_cloud,pcl::PointCloud<pcl::PointXYZI>::Ptr &non_ground_cloud)
// double &time_taken)
{// 1.Msg to pointcloudpcl::PointCloud<pcl::PointXYZI> laserCloudIn;laserCloudIn = *cloud_in;// start = ros::Time::now().toSec();// 2.Sort on Z-axis value.sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_z_cmp<pcl::PointXYZI>);// 3.Error point removal// As there are some error mirror reflection under the ground,// here regardless point under 1.8* sensor_height// Sort point according to height, here uses z-axis in defaultauto it = laserCloudIn.points.begin();for (int i = 0; i < laserCloudIn.points.size(); i++){if (laserCloudIn.points[i].z < -1.8 * sensor_height_){it++;}else{break;}}laserCloudIn.points.erase(laserCloudIn.points.begin(), it);// t1 = ros::Time::now().toSec();// 4. pointcloud -> regionwise settingfor (int k = 0; k < num_zones_; ++k){flush_patches_in_zone(ConcentricZoneModel_[k], num_sectors_each_zone_[k], num_rings_each_zone_[k]);}pc2czm(laserCloudIn, ConcentricZoneModel_);pcl::PointCloud<pcl::PointXYZI> cloud_nonground;pcl::PointCloud<pcl::PointXYZI> cloud_out;cloud_out.clear();cloud_nonground.clear();revert_pc.clear();reject_pc.clear();int concentric_idx = 0;for (int k = 0; k < num_zones_; ++k){auto zone = ConcentricZoneModel_[k];for (uint16_t ring_idx = 0; ring_idx < num_rings_each_zone_[k]; ++ring_idx){for (uint16_t sector_idx = 0; sector_idx < num_sectors_each_zone_[k]; ++sector_idx){if (zone[ring_idx][sector_idx].points.size() > num_min_pts_){extract_piecewiseground(k, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_);// Status of each patch// used in checking uprightness, elevation, and flatness, respectivelyconst double ground_z_vec = abs(normal_(2, 0));const double ground_z_elevation = pc_mean_(2, 0);const double surface_variable =singular_values_.minCoeff() /(singular_values_(0) + singular_values_(1) + singular_values_(2));if (ground_z_vec < uprightness_thr_){// All points are rejectedcloud_nonground += regionwise_ground_;cloud_nonground += regionwise_nonground_;}else{ // satisfy uprightnessif (concentric_idx < num_rings_of_interest_){if (ground_z_elevation > elevation_thr_[ring_idx + 2 * k]){if (flatness_thr_[ring_idx + 2 * k] > surface_variable){if (verbose_){// std::cout << "\033[1;36m[Flatness] Recovery operated. Check "//           << ring_idx + 2 * k//           << "th param. flatness_thr_: " << flatness_thr_[ring_idx + 2 * k]//           << " > "//           << surface_variable << "\033[0m" << std::endl;// revert_pc += regionwise_ground_;}cloud_out += regionwise_ground_;cloud_nonground += regionwise_nonground_;}else{if (verbose_){// std::cout << "\033[1;34m[Elevation] Rejection operated. Check "//           << ring_idx + 2 * k//           << "th param. of elevation_thr_: " << elevation_thr_[ring_idx + 2 * k]//           << " < "//           << ground_z_elevation << "\033[0m" << std::endl;reject_pc += regionwise_ground_;}cloud_nonground += regionwise_ground_;cloud_nonground += regionwise_nonground_;}}else{cloud_out += regionwise_ground_;cloud_nonground += regionwise_nonground_;}}else{if (using_global_thr_ && (ground_z_elevation > global_elevation_thr_)){std::cout << "\033[1;33m[Global elevation] " << ground_z_elevation << " > " << global_elevation_thr_<< "\033[0m" << std::endl;cloud_nonground += regionwise_ground_;cloud_nonground += regionwise_nonground_;}else{cloud_out += regionwise_ground_;cloud_nonground += regionwise_nonground_;}}}}}++concentric_idx;}}*ground_cloud = cloud_out;*non_ground_cloud = cloud_nonground;
}

实践

运行

编译运行ROS节点，可以修改bag包名后直接运行以下命令，我这里订阅的topic为/velodye_points，大家也可以自行修改运行

rosbag play -l kitti_2011_09_26_drive_0005_synced.bag /kitti/velo/pointcloud:=/velodye_points
roslaunch patch_work  path_work.launch

效果

效果确实比pcl方法、Ground Plane Fitting和ray_ground_filter方法好，我提供了几种方法的源码，大家可以自行修改对比

代码链接：https://download.csdn.net/download/weixin_42905141/85087169