CN106530380B - 一种基于三维激光雷达的地面点云分割方法 - Google Patents

Abstract

本发明公开了一种基于三维激光雷达的地面点云分割的方法，包括步骤：1)获取车辆周围环境的三维激光雷达扫描点云极坐标数据并转换到本地直角坐标系下；2)利用车载IMU和里程计对雷达数据进行修正；3)构建极坐标网格地图，根据网格中点云分布的垂直连续性提取每个网格中的延伸顶点；4)根据延伸顶点的高度属性以及地面平滑一致性准则提取非边缘网格中的地面点，并采用3σ准则对边缘网格中的地面点进一步提取。与现有技术相比，本发明能够降低车辆自身运动造成的地面提取误差，避免出现过分割和欠分割，精度高可靠性强，并且具有较高的实时性，可以广泛用于基于雷达的环境感知技术领域中。

Description

一种基于三维激光雷达的地面点云分割方法

技术领域

本发明涉及无人驾驶技术领域，尤其涉及一种基于三维激光雷达的地面点云分割方法。

背景技术

三维激光雷达可以实时、精确的获取车辆周围的环境信息且很少受到光照、天气变化的影响，因此被广泛应用到无人驾驶车辆的环境感知系统中。在城市交通环境下，最常见的障碍物有车辆、行人、树木、建筑物等，要实现对这些障碍物精确的感知必须首先将这些障碍物从雷达数据中精确的分割出来，而这些障碍物都是分布在地面上，并通过地面点连接在一起，因此必须首先将地面提取出来，否则地面点的存在会使所有地面上的物体相互连接在一起，无法完成分割。地面点云提取的结果会直接影响到障碍物分割、识别的精度，所以研究三维激光雷地面点云提取具有重要意义。

现有的地面点云分割方法主要有基于3D点云投影的方法、基于平面模型拟合的方法、基于扫描线的方法、基于图的方法。其中，基于3D点云映射的方法，在DAPAR城市挑战赛中得到广泛的应用。基于3D点云映射的方法优点在于将三维信息降低到二维信息，大大降低了传感器数据分析的复杂度和计算量，有较好的稳定性和实时性，但是由于障碍栅格判定和滤波严格，减少了误检点，但是由于雷达点云分布不均匀，特别是在远处，雷达三维点云稀疏，容易导致远处的栅格因部分点云缺少而出现漏检。

基于平面模型拟合的方法思想简单易于实现，其主要是将地面点提取问题转化为平面模型的拟合问题。然而，基于模型拟合的方法提取精度随着迭代次数的增加而增加，由于点云的数据量大，导致该类算法实时性比较差。

针对有序点云提出了一种基于扫描线的局部地面检测方法，根据扫描线中近邻点之间的高度和坡度属性提取每一扫描线中的地面点。此类算法充分利用了近邻点之间相关性，具有较高的分割精度，但是不能处理大量的无序点云数据。

基于图的点云分割方法，借鉴了图像处理方法，和其它的方法相比，基于图的方法可以对密度不均、包含噪声等复杂场景下的点云进行分割，并且具有较高的分割精度。然而，由于建立图的拓扑结构和计算图节点之间权重过程中时间消耗比较大，此外，基于图的方法往往映射到条件随机场等概率推论模型中，这类方法需要对样本进行标记和训练，但处理大量的点常常不能达到实时性。

发明内容

为了解决现有技术的点云分割方法中部分点云数据出现漏检、无法处理大量无序点云数据和处理大量点云数据时实时性差的问题，本发明提供以下技术方案予以解决：

1、一种基于三维激光雷达的地面点云分割方法，包括以下步骤：

步骤1，以激光雷达中心位置为坐标原点建立本地直角坐标系，获取车辆周围环境的三维激光雷达扫描点云的极坐标数据并转换到本地直角坐标系下，对本地直角坐标系下的点云数据进行预处理确定目标区域；

步骤2，在本地直角坐标系下，利用车载IMU和里程计获取车辆当前位姿相对于坐标原点平移变量和旋转角度变量，利用平移变量和旋转角度变量对本地直角坐标系下的点云数据的坐标进行修正；

步骤3，构建极坐标网格地图，依次将直角坐标系下的点映射到极坐标网格地图中，提取出极坐标网格地图中障碍物在垂直方向上的延伸顶点；

步骤4,根据网格中延伸顶点的高度，将该网格划分为悬空障碍物网格、地面网格和障碍物网格，依次提取地面网格中的地面点，提取障碍物网格中的边缘障碍物网格，根据3σ准则，提取边缘障碍物网格中的地面点。

步骤1中以激光雷达中心位置为坐标原点建立本地直角坐标系，具体包括：以所述激光雷达中心为坐标原点，以激光雷达的垂直轴线方向为Z轴，以扫描第一个平面的水平射线方向为X轴，以车体向前运动的方向为Y轴。

步骤1中对本地直角坐标系下的点云数据进行预处理确定目标区域，具体包括：在本地直角坐标系下，以坐标原点为中心，保留-20m<X<20m,-30m<Y<50m区域内的3D点云数据，并将该区域作为目标区域。

步骤2具体包括：

2-1雷达的单个扫描线在一个周期内旋转扫描产生n个点，当雷达扫描到第i个点时，该点在传感器坐标系下的坐标为p_s＝(x_i,y_i,z_i)^T；

2-2利用车载里程计推算出车辆在雷达扫描到第i个点时，相对第一个扫描点的平移量

2-3利用车载IMU推算出车辆在雷达扫描到第i个点时，相对第一个扫描点的旋转欧拉角Ang_i＝(Ψ_i＝0,θ_i,Φ_i)^T，其中Ψ_i，θ_i和Φ_i分别表示车辆的航向角,翻滚角和俯仰角；

2-4雷达从第i个点扫描到第n个点期间，车辆的平移相对变化量为ΔT_i，其中，ΔT_i＝T_n-T_i＝(Δx_i,Δy_i,Δz_i)^T；车辆的旋转欧拉角变化量为ΔAng_i，其中ΔAng_i＝Ang_n-Ang_i＝(ΔΨ_i,Δθ_i,ΔΦ_i)^T。

2-5利用车辆的平移变量和旋转欧拉角变化量对本地直角坐标系下的点云数据坐标进行修正，修正后的雷达点云在本地直角坐标系下的坐标为p_i'＝R_i(p_i-ΔT_i)，其中R_i＝R_z(ΔΨ_i)R_y(Δθ_i)R_x(ΔΦ_i),R_i∈R^3×3是三个欧拉角的旋转矩阵。

步骤3具体包括：

3-1以本地直角坐标系的原点为极点，以Z轴为中心对称轴，建立极径为R的极坐标网格地图，将极坐标网格地图划分为M个等面积的扇区S₁,S₂…S_M，每个扇形对应的圆心角为Δα＝360°/M；

3-2对每个扇形S_i，将极径为r_min与r_max之间的区域沿着极径等间隔的划分为N个网格b₁,b₂…b_N,其中r_min、r_max分别表示距离极点的最小距离和最大距离，网格的径向长度为Δr＝(r_max-r_min)/N。

3-3在本地直角坐标系下，点p_i距原点的距离表示为则点p_i所属栅格为b_n，其中n＝(d_i-r_min)/Δr；点p_i与X正半轴的夹角表示为β_i＝atan2(y_i,x_i)，则点p_i所属的扇区为S_m，其中m＝β_i/Δα；

3-4重复步骤3-3，依次将所有的点映射到极坐标网格地图中；

3-5对极坐标网格地图S_m扇区b_n栅格中的k个点组成的集合按照高度进行快速排序，得到排序后的集合

3-6从p₁'开始分别计算相邻两点p'_j,p'_j+1之间的高度差Δh_j；

3-7设定阈值T，如果Δh_j大于阈值T时，则将p'_j作为该网格中的障碍物在垂直方向上延伸的顶点，停止搜索；如果Δh_j小于和等于T,且p'_j+1不是网格中最后一个点，则j＝j+1,重复步骤3-6至步骤3-7,继续向上搜索；如果Δh_j小于和等于T,且p'_j+1是网格中的最后一个点，则将p'_j+1作为该网格中的障碍物在垂直方向上延伸的顶点，停止搜索；

3-8重复步骤3-5至3-7依次找到每个网格中障碍物在垂直方向上的延伸顶点。

步骤4具体包括：

4-1对任意网格提取该网格中最低点的高度值；

4-2对任意网格如果该网格中最低点的高度值大于给定阈值min_z，则该网格标记为悬空障碍物网格；

4-3如果该网格中最低点的高度值小于或等于给定阈值min_z,并且该网格中延伸顶点高度在给定阈值M范围内，则将该网格标记为地面网格；

4-4如果该网格中最低点的高度值小于或等于给定阈值min_z并且该网格中延伸顶点的高度超过给定的阈值，则将该网格标记为障碍物网格；

4-5将地面网格中延伸顶点及其以下的点标记为地面点，在延伸顶点之上的点标记为悬空障碍点；

4-6重复执行步骤4-1至4-5依次提取每个网格中所有的地面点。

4-7对于标记为障碍物的网格，首先判断它是否为边缘障碍物网格，如果该网格的八邻域中有地面网格，则该障碍物网格是边缘障碍物网格，否则不是，继续搜索下一个障碍物网格；

4-8对任一边缘障碍物网格，提取其八邻域中所有的地面点，并计算点的高度均值μ和方差σ；根据3σ准则，对边缘障碍物网格中所有高度值在[μ-3σ,μ+3σ]范围内的点标记为地面点；

4-9更换网格，重复执行步骤4-7至4-8依次提取出所有边缘网格中的地面点。

以上技术方案和现有技术相比具有以下技术效果；

1、本发明提供的算法适应于有序点云和无序点云的的提取和处理，并且提出利用车载IMU和里程计获取车辆当前位置相对于前一时刻位置的平移变量和角度变量，对点云数据的位置进行修正，提高了算法的精确度，使得地面点的提取更加准确。

2、本发明利用极坐标网格地图中的点在垂直方向上从地面向上延伸，不同的障碍物分层分布的特点提取地面部分，该方法没有复杂的计算，实时性高，本发明通过设定合适的阈值，提取出悬空障碍物网格，很好地解决了映射方法中无法处理悬空障碍物的问题。

3、本发明在进行地面点提取时，先根据网格中延伸顶点的高度对网格进行了划分，从而提取地面网格中的地面点，对于边缘障碍物网格中的点，通过提取其八邻域中的所有的地面点，根据3σ准则，依次提取所有边缘网格中的地面点，解决了现有技术中遗漏部分地面点的情况。

附图说明

图1为本发明算法流程图；

图2是系统采集到的原始点云数据示意图；

图3是本发明点云数据修正原理图；

图4是本发明中车辆在行驶过程中的姿态变化示意图；

图5是本发明极坐标网格地图构建原理图；

图6是本发明得到的地面点云分割图。

具体实施例

如图1本发明提供的一种基于三维激光雷达的地面点云分割方法，包括以下步骤：

步骤1，以激光雷达中心位置为坐标原点建立本地直角坐标系，获取车辆周围环境的三维激光雷达扫描点云的极坐标数据并转换到本地直角坐标系下，对本地直角坐标系下的点云数据进行预处理确定目标区域；

步骤2，在本地直角坐标系下，利用车载IMU和里程计获取车辆当前位姿相对于前一时刻位置平移变量和角度变量，利用平移变量和旋转角度变量对本地直角坐标系下的点云数据的坐标进行修正；

步骤3，构建极坐标网格地图，依次将直角坐标系下的点映射到极坐标网格地图中，找出极坐标网格地图中障碍物在垂直方向上的延伸顶点；

步骤4,根据网格中延伸顶点的高度，将该网格划分为悬空障碍物网格、地面网格和障碍物网格，依次提取地面网格中的地面点，提取障碍物网格中的边缘障碍物网格，根据3σ准则，提取边缘障碍物网格中的地面点。

步骤1中以激光雷达中心位置为坐标原点建立本地直角坐标系,具体包括：以所述激光雷达中心为坐标原点，以激光雷达的垂直轴线方向为Z轴，以扫描第一个平面的水平射线方向为X轴，以车体向前运动的方向为Y轴。

步骤1中对本地直角坐标系下的点云数据进行预处理确定目标区域，具体包括：在本地直角坐标系下，保留-20m<X<20m,-30m<Y<50m区域内的3D点云数据，并将该区域作为目标区域，如图2为雷达采集的一帧雷达点云图。

如图3车辆在行驶过程中，雷达旋转一周需要0.1s，假如车辆的行驶速度是70Km/h,那么在这一个周期内，车辆向前行驶了近2m,由于雷达安装在车顶上，在扫描期间，雷达的扫描中心也向前移动了近2m，导致扫描数据出现局部扭曲，此外，车辆运动过程中，由于道路存在坡度导致车辆姿态发生变化也会导致采集的一帧雷达数据出现扭曲，如图4所示的车辆翻滚角和俯仰角变化，为了减小误差，本发明采用车载IMU和里程计获取车辆自身位姿变化信息，以对车辆运动产生的采集误差进行修正。

其中，步骤2具体包括：

2-1雷达的单个扫描线在一个周期内旋转扫描产生n个点，当雷达扫描到第i个点时，该点在传感器坐标系下的坐标为p_s＝(x_i,y_i,z_i)^T；

2-2利用车载里程计推算出车辆在雷达扫描到第i个点时，相对第一个扫描点的平移量

2-3利用车载IMU推算出车辆在雷达扫描到第i个点时，相对第一个扫描点的旋转欧拉角Ang_i＝(Ψ_i＝0,θ_i,Φ_i)^T，其中Ψ_i，θ_i和Φ_i分别表示车辆的航向角,翻滚角和俯仰角；

2-4雷达从第i个点扫描到第n个点期间，车辆的平移相对变化量为ΔT_i，其中，ΔT_i＝T_n-T_i＝(Δx_i,Δy_i,Δz_i)^T；车辆的旋转欧拉角变化量为ΔAng_i，其中ΔAng_i＝Ang_n-Ang_i＝(ΔΨ_i,Δθ_i,ΔΦ_i)^T。

2-5利用车辆的平移变量和旋转欧拉角变化量对本地直角坐标系下的点云数据坐标进行修正，修正后的雷达点云在本地直角坐标系下的坐标为p_i'＝R_i(p_i-ΔT_i)，其中R_i＝R_z(ΔΨ_i)R_y(Δθ_i)R_x(ΔΦ_i),R_i∈R^3×3是三个欧拉角的旋转矩阵。

其中，步骤3具体包括：

3-1如图5，以本地直角坐标系的原点为极点，以Z轴为中心对称轴，建立极径为R的极坐标网格地图，将极坐标网格地图划分为M个等面积的扇区S₁,S₂…S_M，每个扇形对应的圆心角为Δα＝360°/M，本实施例中Δα取0.5；

3-2对每个扇形S_i，将极径为r_min与r_max之间的区域沿着极径等间隔的划分为N个网格b₁,b₂…b_N,网格的径向长度为Δr＝(r_max-r_min)/N。本实施例中本实施例中r_min取2米，r_max50米，Δr取0.2米。

3-3在本地直角坐标系下，点p_i距原点的距离表示为则点p_i所属栅格为b_n，其中n＝(d_i-r_min)/Δr；点p_i与X正半轴的夹角表示为β_i＝atan2(y_i,x_i)，则点p_i所属的扇区为S_m，其中m＝β_i/Δα；

3-4重复步骤3-3，依次将所有的点映射到极坐标网格地图中；

3-5对极坐标网格地图S_m扇区b_n栅格中的k个点组成的集合按照高度进行快速排序，得到排序后的集合

3-6从p₁'开始分别计算相邻两点p'_j,p'_j+1之间的高度差Δh_j；

3-7设定阈值T，本实施例中T＝0.3，T取0.3为了防止障碍物的延伸顶点被漏检的情况，如果Δh_j大于阈值T时，则将p'_j作为该网格中的障碍物在垂直方向上延伸的顶点，停止搜索；如果Δh_j小于和等于T,且p'_j+1不是网格中最后一个点，则j＝j+1,重复(3-6至3-7),继续向上搜索；如果Δh_j小于和等于T,且p'_j+1是网格中的最后一个点，则将p'_j+1作为该网格中的障碍物在垂直方向上延伸的顶点，停止搜索；

3-8重复步骤3-5至3-7依次找到每个网格中障碍物在垂直方向上的延伸顶点。

其中，步骤4具体包括：

4-1对任意网格提取该网格中最低点的高度值；

4-2对任意网格如果该网格中最低点的高度值大于给定阈值min_z，其中min_z＝0.5米,则该网格标记为悬空障碍物网格；

4-3如果该网格中最低点的高度值小于或等于给定阈值min_z,并且该网格中延伸顶点高度在给定阈值M范围内，(本实施例取M＝-1.5m,此处以本地直角坐标系为参考系，雷达安装在距离地面1.73米处)则将该网格标记为地面网格；

4-4如果该网格中最低点的高度值小于或等于给定阈值min_z并且该网格中延伸顶点的高度超过给定的阈值，则将该网格标记为障碍物网格；

4-5将地面网格中延伸顶点及其以下的点标记为地面点，在延伸顶点之上的点标记为悬空障碍点；

4-6重复执行步骤4-1至4-5依次提取每个网格中所有的地面点。

4-7对于标记为障碍物的网格，首先判断它是否为边缘障碍物网格，如果该网格的八邻域中有地面网格，则该障碍物网格是边缘障碍物网格，否则不是，继续搜索下一个障碍物网格；

4-8对任一边缘障碍物网格，提取其八邻域中所有的地面点，并计算点的高度均值μ和方差σ；根据3σ准则，对边缘障碍物网格中所有高度值在[μ-3σ,μ+3σ]范围内的点标记为地面点；

4-9更换网格，重复执行步骤4-6至4-8依次提取出所有边缘网格中的地面点，如图6为本发明最终采集的分割的底面点云图。

Claims (6)

1.一种基于三维激光雷达的地面点云分割方法，其特征在于，包括以下步骤：

步骤1，以激光雷达中心位置为坐标原点建立本地直角坐标系，获取车辆周围环境的三维激光雷达扫描点云的极坐标数据并转换到本地直角坐标系下，对本地直角坐标系下的点云数据进行预处理确定目标区域；以所述激光雷达中心为坐标原点O，以激光雷达的轴线为Z轴，以轴线向上的方向为Z轴的正方向；以扫描第一个平面的水平射线方向为X轴，X轴的正方向指向汽车右侧；以车体向前运动的方向为Y轴的正方向；

步骤2，在本地直角坐标系下,利用车载IMU和里程计获取车辆相对前一时刻位置的平移变量和角度变量，利用平移变量和角度变量对本地直角坐标系下的点云数据的坐标进行修正；

步骤3，构建极坐标网格地图，依次将直角坐标系下的点映射到极坐标网格地图中，提取出极坐标网格地图中障碍物在垂直方向上的延伸顶点；

步骤4,根据网格中延伸顶点的高度，将该网格划分为悬空障碍物网格、地面网格和障碍物网格，依次提取地面网格中的地面点，提取障碍物网格中的边缘障碍物网格，根据3σ准则，提取边缘障碍物网格中的地面点。

2.如权利要求1所述的基于三维激光雷达的地面点云分割方法，其特征在于，所述的步骤1中以激光雷达中心位置为坐标原点建立本地直角坐标系，具体包括：以所述激光雷达中心为坐标原点，以激光雷达的垂直轴线方向为Z轴，以扫描第一个平面的水平射线方向为X轴，以车体向前运动的方向为Y轴。

3.如权利要求1所述的基于三维激光雷达的地面点云分割方法，其特征在于，所述的步骤1中对本地直角坐标系下的点云数据进行预处理确定目标区域，具体包括：

在本地直角坐标系下，以坐标原点为中心，保留-20m<X<20m,-30m<Y<50m区域内的3D点云数据，并将该区域作为目标区域。

4.如权利要求1所述的基于三维激光雷达的地面点云分割方法，其特征在于，所述的步骤2具体包括：

2-1雷达的单个扫描线在一个周期内旋转扫描产生n个点，当雷达扫描到第i个点时，该点在传感器坐标系下的坐标为p_s＝(x_i,y_i,z_i)^T；

2-2利用车载里程计推算出车辆在雷达扫描到第i个点时，相对第一个扫描点的平移量

2-3利用车载IMU推算出车辆在雷达扫描到第i个点时，相对第一个扫描点的旋转欧拉角Ang_i＝(Ψ_i＝0,θ_i,Φ_i)^T，其中Ψ_i，θ_i和Φ_i分别表示车辆的航向角,翻滚角和俯仰角；

2-4雷达从第i个点扫描到第n个点期间，车辆的平移相对变化量为ΔT_i，其中，ΔT_i＝T_n-T_i＝(Δx_i,Δy_i,Δz_i)^T；车辆的旋转欧拉角变化量为ΔAng_i，其中ΔAng_i＝Ang_n-Ang_i＝(ΔΨ_i,Δθ_i,ΔΦ_i)^T；

2-5利用车辆的平移变量和旋转欧拉角变化量对本地直角坐标系下的点云数据坐标进行修正，修正后的雷达点云在本地直角坐标系下的坐标为p_i'＝R_i(p_i-ΔT_i)，其中R_i＝R_z(ΔΨ_i)R_y(Δθ_i)R_x(ΔΦ_i),R_i∈R^3×3是三个欧拉角的旋转矩阵。

5.如权利要求1所述的基于三维激光雷达的地面点云分割方法，其特征在于，所述的步骤3具体包括：

3-1以本地直角坐标系的原点为极点，以Z轴为中心对称轴，建立极径为R的极坐标网格地图，将极坐标网格地图划分为M个等面积的扇区S₁,S₂…S_M，每个扇形对应的圆心角为Δα＝360°/M；

3-2对每个扇形S_i，将极径为r_min与r_max之间的区域沿着极径等间隔的划分为N个网格b₁,b₂…b_N,其中r_min、r_max分别表示距离极点的最小距离和最大距离，网格的径向长度为Δr＝(r_max-r_min)/N；

3-3在本地直角坐标系下，点p_i距原点的距离表示为则点p_i所属栅格为b_n，其中n＝(d_i-r_min)/Δr；点p_i与X正半轴的夹角表示为β_i＝atan2(y_i,x_i)，则点p_i所属的扇区为S_m，其中m＝β_i/Δα；

3-4重复步骤3-3，依次将所有的点映射到极坐标网格地图中；

3-5对极坐标网格地图S_m扇区b_n栅格中的k个点组成的集合按照高度进行快速排序，得到排序后的集合

3-6从p′₁开始分别计算相邻两点p'_j,p'_j+1之间的高度差Δh_j；

3-7设定阈值T，如果Δh_j大于阈值T时，则将p'_j作为该网格中的障碍物在垂直方向上延伸的顶点，停止搜索；如果Δh_j小于和等于T,且p'_j+1不是网格中最后一个点，则j＝j+1, 重复步骤3-6至3-7；如果Δh_j小于和等于T,且p'_j+1是网格中的最后一个点，则将p'_j+1作为该网格中的障碍物在垂直方向上延伸的顶点，停止搜索；

3-8重复步骤3-5至3-7依次找到每个网格中障碍物在垂直方向上的延伸顶点。

6.如权利要求1所述的基于三维激光雷达的地面点云分割方法，其特征在于，所述的步骤4具体包括：

4-1对任意网格提取该网格中最低点的高度值；

4-2对网格如果该网格中最低点的高度值大于给定阈值min_z，则该网格标记为悬空障碍物网格；

4-3如果该网格中最低点的高度值小于或等于给定阈值min_z,并且该网格中延伸顶点高度在给定阈值范围内，则将该网格标记为地面网格；

4-4如果该网格中最低点的高度值小于或等于给定阈值min_z并且该网格中延伸顶点的高度超过给定的阈值，则将该网格标记为障碍物网格；

4-5将地面网格中延伸顶点及其以下的点标记为地面点，在延伸顶点之上的点标记为悬空障碍点；

4-6重复执行步骤4-1至4-5依次提取每个网格中所有的地面点；

4-7对于标记为障碍物的网格，首先判断它是否为边缘障碍物网格，如果该网格的八邻域中有地面网格，则该障碍物网格是边缘障碍物网格，否则不是，继续搜索下一个障碍物网格；

4-8对任一边缘障碍物网格，提取其八邻域中所有的地面点，并计算点的高度均值μ和方差σ；根据3σ准则，对边缘障碍物网格中所有高度值在[μ-3σ,μ+3σ]范围内的点标记为地面点；

4-9更换网格，重复执行步骤4-7至4-8依次提取出所有边缘网格中的地面点。