CN114266801A - 基于三维激光雷达的移动机器人越野环境中地面分割方法 - Google Patents

Abstract

本发明提供基于三维激光雷达的移动机器人越野环境中地面分割方法，将获取到的点云数据网格化寻找最大密度区域，确定最大密度点作为聚类初始点，根据密度聚类算法寻找点云中的核心点，以初始点为密度聚类的第一点开始按每一个核心点聚类半径向周围寻找密度可达点，最终完成聚类，将无法聚类的点判断为离群点滤除。使用基于邻近重心点的体素滤波将点云降采样处理，按照半径与竖直角度将无序点云有序化，通过坡度角与竖直角度的关系得到实际路况中的期望距离，再与雷达安装高度、坡度角关系得到局部阈值高度与全局阈值高度，判断每一点的半径与实际期望距离以及每一点的高度与全局阈值和局部阈值高度得到该点所属的类别是地面或者非地面。

Description

基于三维激光雷达的移动机器人越野环境中地面分割方法

技术领域

本发明涉及移动机器人越野环境感知技术领域，具体涉及一种基于三维激光雷达的移动机器人越野环境中地面分割方法。

技术背景

环境感知技术是移动机器人在越野环境中安全行驶的关键技术之一。环境感知技术利用各种传感器对环境进行数据采集，获取行驶环境信息和机器人与周围障碍物的相对距离，相对速度等信息，进而为各种控制决策提供信息依据。它是移动机器人实现避障与路径规划等高级智能行为的前提条件与基础。

现阶段，用于移动机器人环境感知的传感器主要有三维激光雷达与相机。利用相机视觉图像处理技术来进行场景的认知是该领域的重要方法，但移动机器人在线获取的视觉图像质量受光线变化影响较大，特别是在光线较暗的场景更难以应用。相较于深度相机三维激光雷达可直接获取三维点云信息，省去了深度图像转点云的处理过程，更加方便快捷和可靠，在自动驾驶邻域被广泛应用。在越野环境中，利用相机进行环境感知易受到杂草与树木遮挡无法准确反映出当前环境中路面的变化；同时移动机器人长期在户外越野环境中行驶，长时间曝光在日照下，受到强光或逆光不同角度的照射将会影响相机稳定工作。使用多线程激光雷达进行环境感知，探测距离相对较远，对于预测较远处的环境情况提供有力保障；同时激光雷达受到光照影响相对较小，无论处理明亮环境还是较暗环境中都有着较高的可靠性。

三维激光雷达采用多线程方式进行扫描，获得的数据量十分庞大，Velodyne16线激光雷达每秒可获取30万点云数据，Velodyne32线激光雷达每秒可获取60万甚至120万点云数据。在移动机器人行进过程中，由于道路不平致使雷达抖动或者由于物体的相互遮挡造成的无效点或离群点在获取到的点云数据中大量存在，若不及时滤除将影响分割效果；同时又存在大量的地面点云信息，在目标检测识别过程中地面点云是无效点，增加了计算负担。

发明内容

本发明为解决上述在环境感知中雷达在越野环境中数据处理的问题，提出一种基于三维激光雷达的移动机器人越野环境中地面分割方法，该方法包含自适应密度聚类的点云离群点滤除以及基于射线自适应坡度阈值地面分割，极大提高了地面滤除的效果和准确度。

针对点云数据进行离群点与无效点的滤除，提出了自适应密度聚类的点云离群点滤除的方法，将获取到的点云数据网格化按照网格索引和每个网格中的点数寻找最大密度区域，在该区域中确定最大密度点作为聚类初始点，寻找初始点的同时，根据密度聚类算法寻找点云中的核心点，以初始点为密度聚类的第一点开始按每一个核心点聚类半径向周围寻找密度可达点，最终完成聚类，将无法聚类的点判断为离群点滤除。

其次对滤波完成的点云进行地面分割，对此提出了基于射线自适应坡度阈值地面分割，使用基于邻近重心点的体素滤波将点云降采样处理，按照半径与竖直角度将无序点云有序化，设置坡度角，通过坡度角与竖直角度的关系得到实际路况中的期望距离，再通过实际的期望距离与雷达安装高度、坡度角关系得到局部阈值高度与全局阈值高度，判断每一点的半径与实际期望距离以及每一点的高度与全局阈值和局部阈值高度得到该点所属的类别是地面或者非地面。

具体采取的方法如下：

1、自适应密度聚类的点云离群点滤除

定义最大密度区域、最大密度点、聚类半径相关概念；点云数据网格化按照网格索引和每个网格中的点数寻找点数最多的网格记为最大密度区域，该区域中寻找点p，该点是与邻近区域中所有点距离最近的点，记为最大密度点；寻找初始点的同时，根据初始半径使用半径搜索寻找点云中的核心点；

寻找密度可达点时使用初始半径会造成半径过大或者过小问题，对此采用自适应聚类半径方法，定义最大距离、最小距离与聚类半径概念，使某一点成为核心点的领域内距该点距离最远的点与该点的距离称为核最大距离，最近的点与该点的距离称为最小距离，最大距离与最小距离的差称为最大距离差，最远点与初始半径的距离差称为半径差；半径差与最大距离差的大小关系反映了当前区域内点云的疏密情况，疏密情况的不同得到每个核心点不同的聚类半径，以初始点为密度聚类的第一点开始向周围寻找密度可达点，遍历所有核心点最终完成聚类，将无法聚类的点判断为离群点滤除；

方法步骤如下：

S1：基于网格的初始点选择，将点云数据网格化，通过网格将接收到的点云划分为n×m块，并对每个区域进行编号，遍历点云，将每一点对应放进所在区域，记录每个区域点的数量，最后选择数量最多的区域作为初始点的选择区域成为密度最大区域；

S2：在聚类半径的选择上，将半径划分为初始半径和聚类半径，初始半径为使核心点成为聚类核心的半径，该参数在核心点选择时已经设定；聚类半径为实际在判断是否是可聚类点时使用的半径；

通过octree的半径搜索，使用初始半径eps，定义参数min_pets判断出输入点云中的核心点，min_pets为邻域内的密度阈值，每个核心点的初始半径内的点数大于min_pets则为核心点云。

S3：计算相邻两点间的欧式距离dist(i，j)，max_dist和min_dist为所有欧式距离中最大的距离和最小距离，同时max_dist是使核心点成为半径，min_dist是距离核心点最近的点；mdd是两者的差值；pdd是初始半径与最大欧式距离的差值。

dist＝sqrt(x*x+y*y+z*z) (1)

max_dist＝Max{dist(i，j)|0＜＝i＜n，0＜＝i＜n} (2)

min_dist＝Min{dist(i，j)|0＜＝i＜n，0＜＝i＜n} (3)

mdd＝max_dist-min_dist (4)

pdd＝eps-max_dist (5)

S4：通过S3得到了自适应半径所需的参数，通过判断mdd与pdd的关系判断初始半径内点云的疏密程度，进而进行聚类半径r的选择，分为以下三种情况：

(1)mdd/pdd＝l；为第一种情况，mdd＝pdd，这种情况下所反映的是初始半径内点云分布比较均匀，但不一定紧密，故此时聚类半径选择初始半径。

(2)mdd/pdd＞1；为第二种情况，mdd＞pdd，此时认为在初始半径内点云分布较为分散，并且最大距离的邻近点接近初始半径，此时认为初始半径只是略大与让其成为核心点的核心半径其内部无法包含同一簇点云的所有点，应扩大聚类半径，故聚类半径选择初始半径加最小距离；

(3)mdd/pdd＜1；为第三种情况，mdd＜pdd，此时认为初始半径内点云在核心点周围点相邻较近，核心点周围点云分布较为密集，此时若将初始半径设置为聚类半径则会造成两簇甚至多簇合并现象，故应减小聚类半径r＝eps-min_dist。

2、基于射线自适应坡度阈值地面分割

使用基于邻近重心点的体素滤波将点云降采样处理，纵向按每个点所在射线的竖直角度排列横向按每个点所在半径排列将无序点云有序化，计算同一水平角度内相邻两点的水平距离与期望距离，判断该点所处位置是否为平坦路面，设置坡度角，通过坡度角与竖直角度的关系得到实际路况中的期望距离；再通过实际的期望距离与雷达安装高度、坡度角关系得到局部阈值高度与全局阈值高度，判断每一点的半径与实际期望距离以及每一点的高度与全局阈值和局部阈值高度得到该点所属的类别是地面或者非地面。

方法步骤如下：

S1：采用基于体素重心邻近点的降采样方法对点云进行处理。在体素化栅格滤波的基础上使用Kdtree寻找与每个小立方体重心最近的原始点代替该重心。

S2：自适应坡度阈值地面分割，以雷达水平分辨率对其进行射线划分将每个角度所对应的区域看做一条射线，每条射线上分布着所有激光射线所对应的点。对同一射线上每个点按照半径的大小进行排序，计算前后两点间的距离与坡度，自适应调整水平距离、局部高度和全局高度阈值来区分地面点与非地面点。当移动机器人行驶在平坦路面且无路障时，其在路面上会扫描出一个个扫描圈，这些扫描圈的半径各不相同，随着竖直角度减小，其半径越大；多线程激光雷达的竖直角度固定，已知激光雷竖直角分辨率，激光雷达的安装高度设置为H，则相邻两个扫描圈的距离为ΔR。

其中r_i、r_i+1为前后两个扫描圈半径，i＝1，2，3，…，n，x、y为其平面上的坐标。

每个扫描圈的半径已知，雷达高度已知可计算出第i个扫描圈即第i条激光射线所对应的竖直角度β_i，r_i为第i条激光到雷达的距离。

若在平坦路面，每两条激光线间的水平距离固定不变，在上坡路段和有障碍阻挡路段时，激光会打在障碍物或上坡路面，相邻两点间的水平距离会减小甚至为零。

ΔR′＝r′_i+1-r′_i (8)

其中r′_i+1、r′_i为实测相邻两点的半径，ΔR′为实测两点间的水平距离。

上坡路段和障碍阻挡路段时，设定自适应水平距离阈值、局部高度阈值与全局高度阈值：

Δh＝|h_i+1-h_i| (11)

其中h_i为坡度角

对应的竖直高度，r″_i为第i条射线的半径，Δr为相邻两点间水平距离，Δh为两点间高度差，H_l为局部高度阈值，H_g为全局高度阈值。

当判定每个区域第一条射线时，若h_i+1＜H_g则为地面点，否则为非地面点。

若前一点为地面点，根据水平距离和局部高度阈值判断当前点是否为地面点，前一点与当前点的水平距离ΔR′＜Δr时，则当前点为障碍物点；上一点与当前点的水平距离ΔR′＞Δr且Δh＜H_l，则当前点为地面点。

若前一点为非地面点，当前点同时满足Δh＜H_l，h_i+1＜H_g，则当前点为地面点，否则为非地面点。

附图说明

图1最为步骤S1中的大密度区域与最大密度点示意图；

图2邻域内点示意图；

图3为步骤S4中的情况1示意图；

图4为步骤S4中的情况2示意图；

图5为步骤S4中的情况3示意图；

图6离群点滤波算法流程示意图；

图7雷达扫描半径示意图；

图8a射线自适应坡度阈值方法示意图之一；

图8b射线自适应坡度阈值方法示意图之二；

图9射线自适应坡度阈值流程示意图。

具体实施方式

结合附图和实施例说明本发明的具体技术方案。

1、自适应密度聚类的点云离群点滤除

如图6所示，方法步骤如下：

S1：基于网格的初始点选择，将点云数据网格化，通过网格将接收到的点云划分为n×m块，并对每个区域进行编号，遍历点云，将每一点对应放进所在区域，记录每个区域点的数量，最后选择数量最多的区域作为初始点的选择区域成为密度最大区域，图1中p点所在区域的即为密度最大区域；在密度最大区域中进行k邻近搜索确定初始点p，则p点为核心点，图1密度最大区域中p点为所选择初始点。

S2：在聚类半径的选择上，此算法将半径划分为初始半径和聚类半径，初始半径为使核心点成为聚类核心的半径，该参数在核心点选择时已经设定；聚类半径为实际在判断是否是可聚类点时使用的半径。

通过octree的半径搜索，使用初始半径eps，定义参数min_pets判断出输入点云中的核心点，min_pets为邻域内的密度阈值，每个核心点的初始半径内的点数大于min_pets则为核心点云。

S3：计算相邻两点间的欧式距离dist(i，j)，max_dist和min_dist为所有欧式距离中最大的距离和最小距离，同时max_dist是使核心点成为半径，min_dist是距离核心点最近的点；mdd是两者的差值；pdd是初始半径与最大欧式距离的差值。如图2所示。

dist＝sqrt(x*x+y*y+z*z) (1)

max_dist＝Max{dist(i，j)|0＜＝i＜n，0＜＝i＜n} (2)

min_dist＝Min{dist(i，j)|0＜＝i＜n，0＜＝i＜n} (3)

mdd＝max_dist-min_dist (4)

pdd＝eps-max_dist (5)

S4：通过以上式子得到了自适应半径所需的一些参数，通过判断mdd与pdd的关系判断初始半径内点云的疏密程度，进而进行聚类半径r的选择，分为以下三种情况：

(1)mdd/pdd＝1；图3为第一种情况，mdd＝pdd，这种情况下所反映的是初始半径内点云分布比较均匀，但不一定紧密，故此时聚类半径选择初始半径。

(2)mdd/pdd＞1；图4为第二种情况，mdd＞pdd，此时认为在初始半径内点云分布较为分散，并且最大距离的邻近点接近初始半径，此时认为初始半径只是略大与让其成为核心点的核心半径其内部无法包含同一簇点云的所有点，应扩大聚类半径，故聚类半径选择初始半径加最小距离；

(3)mdd/pdd＜1；图5为第三种情况，mdd＜pdd，此时认为初始半径内点云在核心点周围点相邻较近，核心点周围点云分布较为密集，此时若将初始半径设置为聚类半径则会造成两簇甚至多簇合并现象，故应减小聚类半径r＝eps-min_dist。

2、基于射线自适应坡度阈值地面分割

如图9所示，方法步骤如下：

S1：采用基于体素重心邻近点的降采样方法对点云进行处理。其原理是在体素化栅格滤波的基础上使用Kdtree寻找与每个小立方体重心最近的原始点代替该重心。

S2：自适应坡度阈值地面分割，以雷达水平分辨率对其进行射线划分将每个角度所对应的区域看做一条射线，每条射线上分布着所有激光射线所对应的点。对同一射线上每个点按照半径的大小进行排序，计算前后两点间的距离与坡度，自适应调整水平距离、局部高度和全局高度阈值来区分地面点与非地面点。当移动机器人行驶在平坦路面且无路障时，其在路面上会扫描出一个个扫描圈，这些扫描圈的半径各不相同，随着竖直角度减小，其半径越大；多线程激光雷达的竖直角度固定，已知激光雷竖直角分辨率，激光雷达的安装高度设置为H，则相邻两个扫描圈的距离为ΔR。

其中r_i、r_i+1为前后两个扫描圈半径，i＝1，2，3，…，n，x、y为其平面上的坐标。如图7所示。

每个扫描圈的半径已知，雷达高度已知可计算出第i个扫描圈即第i条激光射线所对应的竖直角度β_i，r_i为第i条激光到雷达的距离。

若在平坦路面，每两条激光线间的水平距离固定不变，在上坡路段和有障碍阻挡路段时，激光会打在障碍物或上坡路面，相邻两点间的水平距离会减小甚至为零。

ΔR′＝r′_i+1-r′_i (8)

其中r′_i+1、r′_i为实测相邻两点的半径，ΔR′为实测两点间的水平距离。

上坡路段和障碍阻挡路段时，如图8a和图8b所示，设定自适应水平距离阈值、局部高度阈值与全局高度阈值：

其中h_i为坡度角

对应的竖直高度，r″_i为第i条射线的半径，Δr为相邻两点间水平距离，Δh为两点间高度差，H_l为局部高度阈值，H_g为全局高度阈值。

当判定每个区域第一条射线时，若h_i+1＜H_g则为地面点，否则为非地面点。

若前一点为地面点，根据水平距离和局部高度阈值判断当前点是否为地面点，上一点与当前点的水平距离ΔR′＜Δt时，则当前点为障碍物点；上一点与当前点的水平距离ΔR′＞Δr且Δh＜H_l，则当前点为地面点。

若前一点为非地面点，当前点同时满足Δh＜H_l，h_i+1＜H_g，则当前点为地面点，否则为非地面点。

Claims (3)

1.基于三维激光雷达的移动机器人越野环境中地面分割方法，其特征在于，包括自适应密度聚类的点云离群点滤除，基于射线自适应坡度阈值地面分割；

对点云数据进行离群点与无效点的滤除，采用自适应密度聚类的点云离群点滤除，将获取到的点云数据网格化按照网格索引和每个网格中的点数寻找最大密度区域，在该区域中确定最大密度点作为聚类初始点，寻找初始点的同时，根据密度聚类算法寻找点云中的核心点，以初始点为密度聚类的第一点开始按每一个核心点聚类半径向周围寻找密度可达点，最终完成聚类，将无法聚类的点判断为离群点滤除；

对滤波完成的点云进行地面分割，采用基于射线自适应坡度阈值地面分割，使用基于邻近重心点的体素滤波将点云降采样处理，按照半径与竖直角度将无序点云有序化，设置坡度角，通过坡度角与竖直角度的关系得到实际路况中的期望距离，再通过实际的期望距离与雷达安装高度、坡度角关系得到局部阈值高度与全局阈值高度，判断每一点的半径与实际期望距离以及每一点的高度与全局阈值和局部阈值高度得到该点所属的类别是地面或者非地面。

2.根据权利要求1所述的基于三维激光雷达的移动机器人越野环境中地面分割方法，其特征在于，所述的自适应密度聚类的点云离群点滤除，包括以下步骤：

S1：基于网格的初始点选择，将点云数据网格化，通过网格将接收到的点云划分为n×m块，并对每个区域进行编号，遍历点云，将每一点对应放进所在区域，记录每个区域点的数量，最后选择数量最多的区域作为初始点的选择区域成为密度最大区域；

S2：在聚类半径的选择上，将半径划分为初始半径和聚类半径，初始半径为使核心点成为聚类核心的半径，该参数在核心点选择时已经设定；聚类半径为实际在判断是否是可聚类点时使用的半径；

通过octree的半径搜索，使用初始半径eps，定义参数min_pets判断出输入点云中的核心点，min_pets为邻域内的密度阈值，每个核心点的初始半径内的点数大于min_pets则为核心点云；

S3：计算相邻两点间的欧式距离dist(i，j)，max_dist和min_dist为所有欧式距离中最大的距离和最小距离，同时max_dist是使核心点成为半径，min_dist是距离核心点最近的点；mdd是两者的差值；pdd是初始半径与最大欧式距离的差值；

dist＝sqrt(x*x+y*y+z*z) (1)

max_dist＝Max{dist(i，j)|0＜＝i＜n，0＜＝i＜n} (2)

min_dist＝Min{dist(i，j)|0＜＝i＜n，0＜＝i＜n} (3)

mdd＝max_dist-min_dist (4)

pdd＝eps-max_dist (5)

S4：通过S3得到了自适应半径所需的参数，通过判断mdd与pdd的关系判断初始半径内点云的疏密程度，进而进行聚类半径r的选择，分为以下三种情况：

(1)mdd/pdd＝1；为第一种情况，mdd＝pdd，这种情况下所反映的是初始半径内点云分布比较均匀，但不一定紧密，故此时聚类半径选择初始半径；

(2)mdd/pdd＞1；为第二种情况，mdd＞pdd，此时认为在初始半径内点云分布较为分散，并且最大距离的邻近点接近初始半径，此时认为初始半径只是略大与让其成为核心点的核心半径其内部无法包含同一簇点云的所有点，应扩大聚类半径，故聚类半径选择初始半径加最小距离；

(3)mdd/pdd＜1；为第三种情况，mdd<pdd，此时认为初始半径内点云在核心点周围点相邻较近，核心点周围点云分布较为密集，此时若将初始半径设置为聚类半径则会造成两簇甚至多簇合并现象，故应减小聚类半径r＝eps-min_dist。

3.根据权利要求1所述的基于三维激光雷达的移动机器人越野环境中地面分割方法，其特征在于，所述的基于射线自适应坡度阈值地面分割，包括以下步骤：

S1：采用基于体素重心邻近点的降采样方法对点云进行处理；在体素化栅格滤波的基础上使用Kdtree寻找与每个小立方体重心最近的原始点代替该重心；

S2：自适应坡度阈值地面分割，以雷达水平分辨率对其进行射线划分将每个角度所对应的区域看做一条射线，每条射线上分布着所有激光射线所对应的点；对同一射线上每个点按照半径的大小进行排序，计算前后两点间的距离与坡度，自适应调整水平距离、局部高度和全局高度阈值来区分地面点与非地面点；当移动机器人行驶在平坦路面且无路障时，其在路面上会扫描出一个个扫描圈，这些扫描圈的半径各不相同，随着竖直角度减小，其半径越大；多线程激光雷达的竖直角度固定，已知激光雷竖直角分辨率，激光雷达的安装高度设置为H，则相邻两个扫描圈的距离为ΔR；

其中r_i、r_i+1为前后两个扫描圈半径，i＝1，2，3，…，n，x、y为其平面上的坐标；

每个扫描圈的半径已知，雷达高度已知可计算出第i个扫描圈即第i条激光射线所对应的竖直角度β_i，r_i为第i条激光到雷达的距离；

若在平坦路面，每两条激光线间的水平距离固定不变，在上坡路段和有障碍阻挡路段时，激光会打在障碍物或上坡路面，相邻两点间的水平距离会减小甚至为零；

其中

为实测相邻两点的半径，ΔR′为实测两点间的水平距离；

上坡路段和障碍阻挡路段时，设定自适应水平距离阈值、局部高度阈值与全局高度阈值：

其中

为坡度角

对应的竖直高度，

为第i条射线的半径，Δr为相邻两点间水平距离，

为两点间高度差，H_l为局部高度阈值，H_g为全局高度阈值；

当判定每个区域第一条射线时，若

则为地面点，否则为非地面点；

若前一点为地面点，根据水平距离和局部高度阈值判断当前点是否为地面点，前一点与当前点的水平距离ΔR′＜Δr时，则当前点为障碍物点；上一点与当前点的水平距离ΔR′＞Δr且

则当前点为地面点；

若前一点为非地面点，当前点同时满足Δh＜H_l，

则当前点为地面点，否则为非地面点。

Images