矿山路沿检测及建图方法及装置

权利要求

1.矿山路沿检测及建图方法，其特征在于，所述方法包含以下步骤：

步骤一：通过激光雷达采集得到目标区域的激光点云数据，对采集得到的所述激光点云数据进行栅格化处理，得到栅格化数据；

步骤二：基于所述栅格化数据，结合海拔高度信息进行筛选，得到地面点云数据；

步骤三：针对所述地面点云数据进行路沿检测，得到左侧路沿点以及右侧路沿点；

步骤四：将激光雷达坐标系下的所述左侧路沿点以及所述右侧路沿点进行坐标系转换，得到全局坐标系下的路沿点数据；

步骤五：基于所述路沿点数据，利用八叉树模型更新得到全局坐标系下的路沿地图。

2.如权利要求1所述的方法，其特征在于，所述步骤一中具体包含以下步骤：

以垂直路面向上的方向为z轴，车辆的行进方向指向x轴，按照右手系建立三维坐标系；

基于所述三维坐标系，以激光雷达中心为原点，依据所述激光雷达的环信息划分扇形区域，将所述点云数据按扇形网格区域进行划分；

计算所述三维坐标系下每个单元格内部的所述点云数据的平均半径、平均高度、平均水平坐标、高度最大值以及高度最小值。

3.如权利要求2所述的方法，其特征在于，所述步骤二中具体包含以下步骤：

基于每个单元格内部的高度差以及相邻单元格的高度差来筛选得到地面候选单元格，具体来说，若当前单元格内部的高度差小于第一阈值，并且当前单元格与临近单元格的高度差小于第二阈值，则将当前单元格记录为所述地面候选单元格。

4.如权利要求3所述的方法，其特征在于，所述步骤二还包含以下步骤：若某个单元格被所述地面候选单元格包围，则将该单元格也标记为地面候选单元格。

5.如权利要求3或4中任一项所述的方法，其特征在于，所述步骤三中具体包含以下步骤：通过环压缩、垂直高度差以及水平波动差三个特征进行路沿检测。

6.如权利要求5所述的方法，其特征在于，所述步骤三中具体包含以下步骤：

依据激光雷达光束照射在水平面上的半径获取相邻环之间的距离，若环间距的压缩比例落在预设范围内，则将该单元格标记为路沿候选单元格；

沿扇形向外寻找高度突变单元格标记为路沿候选单元格；

寻找预设邻域内水平波动在第三阈值内的变化单元标记为路沿候选单元格。

7.如权利要求6所述的方法，其特征在于，通过以下公式得到所述路沿候选单元格：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri表示第i环的半径，ri+1表示第i+1环的半径，θi表示第i环相应的照射角，θi+1表示第i+1环相应的照射角，h表示传感器高度，Δri表示第i环与第i+1环之间的距离，Ii表示环间距的压缩比例预设范围，α和β表示第一系数以及第二系数。

8.如权利要求6或7中任一项所述的方法，其特征在于，所述步骤三还包含以下步骤：

将标记的所述路沿候选单元格根据点的分布规律得到左、右侧路沿的分割角度；

遍历所有路沿候选单元格，根据所述栅格化数据中的扇形位置，对不存在路沿点的扇形进行排序，寻找非路沿扇形位置的中位数，选其为路沿分割线。

9.如权利要求1所述的方法，其特征在于，所述步骤五中具体包含以下步骤：使用相同尺寸的小立方体离散化待建图区域，通过所述八叉树模型进行路沿点更新。

10.一种矿山路沿检测及建图装置，其特征在于，所述装置包含：

第一模块，其用于通过激光雷达采集得到目标区域的激光点云数据，对采集得到的所述激光点云数据进行栅格化处理，得到栅格化数据；

第二模块，其用于基于所述栅格化数据，结合海拔高度信息进行筛选，得到地面点云数据；

第三模块，其用于针对所述地面点云数据进行路沿检测，得到左侧路沿点以及右侧路沿点；

第四模块，其用于将激光雷达坐标系下的所述左侧路沿点以及所述右侧路沿点进行坐标系转换，得到全局坐标系下的路沿点数据；

第五模块，其用于基于所述路沿点数据，利用八叉树模型更新得到全局坐标系下的路沿地图。

说明书

技术领域

本发明涉及传感器感知技术与智能交通技术领域，具体地说，涉及一种矿山路沿检测及建图方法及装置。

背景技术

随着智能车辆的高速发展，矿山环境下的车辆自动化越来越受到人们的关注，其中感知路沿是矿山高精地图构建中的重要环节。通过使用精准的路沿图，可以获取车辆的有效感知范围，减少计算资源的消耗；同时，路沿图能够提供地图的先验信息，帮助进行路径规划；此外，还可以根据车辆与路沿的相对位置的比较进行定位，完成多传感器融合的车辆状态估计。

与摄像头相比，LiDAR(激光雷达)受天气和光照的影响较小，在大部分场景下正常工作，并且满足自动驾驶的实时性。根据环境的差异，路沿感知策略也千差万别。道路一般可分为结构化与非结构化两种，高速路和城市道路就是典型的结构化道路，非结构化道路一般缺乏人工标志，本发明所涉及的矿山道路可以归为此类。

近年来，与城市道路路沿检测相关的研究不少。根据所识别的特征，LiDAR的路沿检测的方法可以分为基于障碍物检测和基于路面提取两种思路。基于障碍物检测的方法适用于具有护栏结构的结构化道路，基于车道线提取的方法不适用于矿山这种无人工绘制车道线的环境。传统基于LiDAR的路沿检测方法，是通过进行二次曲线进行拟合完成的。但是，在矿山环境下，路沿具有高度的不规则形状，强行拟合则会失去很多真正的路沿点。现有的路沿检测方法多假设环境为二维平面，用于起伏多变的矿山环境进行路沿检测时，会忽视重要的海拔高度信息。

为了解决上述问题，本发明给出一种矿山路沿检测及建图方法及装置。

发明内容

为解决上述问题，本发明提供了一种矿山路沿检测及建图方法，所述方法包含以下步骤：

步骤一：通过激光雷达采集得到目标区域的激光点云数据，对采集得到的所述激光点云数据进行栅格化处理，得到栅格化数据；

步骤二：基于所述栅格化数据，结合海拔高度信息进行筛选，得到地面点云数据；

步骤三：针对所述地面点云数据进行路沿检测，得到左侧路沿点以及右侧路沿点；

步骤四：将激光雷达坐标系下的所述左侧路沿点以及所述右侧路沿点进行坐标系转换，得到全局坐标系下的路沿点数据；

步骤五：基于所述路沿点数据，利用八叉树模型更新得到全局坐标系下的路沿地图。

根据本发明的一个实施例，所述步骤一中具体包含以下步骤：

以垂直路面向上的方向为z轴，车辆的行进方向指向x轴，按照右手系建立三维坐标系；

基于所述三维坐标系，以激光雷达中心为原点，依据所述激光雷达的环信息划分扇形区域，将所述点云数据按扇形网格区域进行划分；

计算所述三维坐标系下每个单元格内部的所述点云数据的平均半径、平均高度、平均水平坐标、高度最大值以及高度最小值。

根据本发明的一个实施例，所述步骤二中具体包含以下步骤：

基于每个单元格内部的高度差以及相邻单元格的高度差来筛选得到地面候选单元格，具体来说，若当前单元格内部的高度差小于第一阈值，并且当前单元格与临近单元格的高度差小于第二阈值，则将当前单元格记录为所述地面候选单元格。

根据本发明的一个实施例，所述步骤二还包含以下步骤：若某个单元格被所述地面候选单元格包围，则将该单元格也标记为地面候选单元格。

根据本发明的一个实施例，所述步骤三中具体包含以下步骤：通过环压缩、垂直高度差以及水平波动差三个特征进行路沿检测。

根据本发明的一个实施例，所述步骤三中具体包含以下步骤：

依据激光雷达光束照射在水平面上的半径获取相邻环之间的距离，若环间距的压缩比例落在预设范围内，则将该单元格标记为路沿候选单元格；

沿扇形向外寻找高度突变单元格标记为路沿候选单元格；

寻找预设邻域内水平波动在第三阈值内的变化单元标记为路沿候选单元格。

根据本发明的一个实施例，通过以下公式得到所述路沿候选单元格：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri表示第i环的半径，ri+1表示第i+1环的半径，θi表示第i环相应的照射角，θi+1表示第i+1环相应的照射角，h表示传感器高度，Δri表示第i环与第i+1环之间的距离，Ii表示环间距的压缩比例预设范围，α和β表示第一系数以及第二系数。

根据本发明的一个实施例，所述步骤三还包含以下步骤：

将标记的所述路沿候选单元格根据点的分布规律得到左、右侧路沿的分割角度；

遍历所有路沿候选单元格，根据所述栅格化数据中的扇形位置，对不存在路沿点的扇形进行排序，寻找非路沿扇形位置的中位数，选其为路沿分割线。

根据本发明的一个实施例，所述步骤五中具体包含以下步骤：使用相同尺寸的小立方体离散化待建图区域，通过所述八叉树模型进行路沿点更新。

根据本发明的另一个方面，还提供了一种矿山路沿检测及建图装置，所述装置包含：

第一模块，其用于通过激光雷达采集得到目标区域的激光点云数据，对采集得到的所述激光点云数据进行栅格化处理，得到栅格化数据；

第二模块，其用于基于所述栅格化数据，结合海拔高度信息进行筛选，得到地面点云数据；

第三模块，其用于针对所述地面点云数据进行路沿检测，得到左侧路沿点以及右侧路沿点；

第四模块，其用于将激光雷达坐标系下的所述左侧路沿点以及所述右侧路沿点进行坐标系转换，得到全局坐标系下的路沿点数据；

第五模块，其用于基于所述路沿点数据，利用八叉树模型更新得到全局坐标系下的路沿地图。

本发明主要针对矿山环境下路况多变的情况，利用八叉树模型提供的占据更新方案，实现路沿点的更新与平滑，进一步滤除路面凹凸引入的干扰点。另外，针对矿山路面凹凸不平的特性，提出了以环压缩为主的路沿检测方案，适应非垂直上升的矿山道路边界，兼顾候选点邻域内的距离波动差。本发明对于弯道等特殊与非特殊情况下提出了基于路沿分布的左右路沿划分方案，克服了单纯依靠水平轴正负划分路沿所带来的局限性。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。在附图中：

图1显示了根据本发明的一个实施例的矿山路沿检测及建图方法流程图；

图2显示了根据本发明的一个实施例的矿山路沿检测及建图系统框架示意图；

图3显示了根据本发明的一个实施例的栅格平面分布示意图；以及

图4显示了根据本发明的一个实施例的矿山路沿检测及建图装置结构框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，以下结合附图对本发明实施例作进一步地详细说明。

现有技术提出了一种基于RANSAC(采样一致性)算法的地面分割方案，并根据同一扫描层的坡度差、高度差以及到激光雷达的距离比来提取路沿候选点。但在矿山环境下，当LiDAR的同一扫描环遭遇非垂直挡墙时，经常会向车辆行进方向延伸而不会产生明显的坡度或高度变化特征。

另一现有技术提出，可根据当前点的梯度来选取路沿候选点，并且，根据每一帧二次拟合系数变化来判断当前检测结果是否有误。但是，在矿山环境下，仅根据梯度特征进行判断，会引入不平整路面产生的干扰。而且，不规则的矿山路沿并不是每一帧都可以实现完美的二次拟合。

综上，现有的技术大多只适用于路面平整且路宽不会频繁变化的城市道路，而对于矿山环境下的多变路况及不平整路面却无能为力，单纯使用高度、坡度或者法线等特征都会导致大量的误检测，而且基于卡尔曼滤波的路沿平滑方案在路宽多变时效果较差。因此，有必要针对自动驾驶矿车提出一种路沿检测方法。

本发明针对高精度地图构建前期的路沿自动化检测与建图问题，结合LiDAR(激光雷达)及组合导航定位传感器，采用OctoMap(基于八叉树的存储结构)融合检测结果，提出了一种新型的矿山环境下的路沿检测与建图技术方案，以至少解决了现有技术中的上述问题。本发明能够基于激光点云的矿山环境下的道路边界检测方法，其能用于矿用车辆所需的高精地图构建与自动驾驶系统。

图1显示了根据本发明的一个实施例的矿山路沿检测及建图方法流程图。

如图1，步骤S101中，通过激光雷达采集得到目标区域的激光点云数据，对采集得到的激光点云数据进行栅格化处理，得到栅格化数据。

具体来说，在步骤S101中，首先，以垂直路面向上的方向为z轴，车辆的行进方向指向x轴，按照右手系建立三维坐标系。

然后，基于三维坐标系，以激光雷达中心为原点，依据激光雷达的环信息划分扇形区域，将点云数据按扇形网格区域进行划分。

最后，计算三维坐标系下每个单元格内部的点云数据的平均半径、平均高度、平均水平坐标、高度最大值以及高度最小值。

如图1，在步骤S102中，基于栅格化数据，结合海拔高度信息进行筛选，得到地面点云数据。

具体来说，在步骤S102中，基于每个单元格内部的高度差以及相邻单元格的高度差来筛选得到地面候选单元格，具体来说，若当前单元格内部的高度差小于第一阈值，并且当前单元格与临近单元格的高度差小于第二阈值，则将当前单元格记录为地面候选单元格。

另外，在步骤S102中，若某个单元格被所述地面候选单元格包围，则将该单元格也标记为地面候选单元格。

如图1，在步骤S103中，针对地面点云数据进行路沿检测，得到左侧路沿点以及右侧路沿点。

在一个实施例中，通过环压缩、垂直高度差以及水平波动差三个特征进行路沿检测。

具体来说，依据激光雷达光束照射在水平面上的半径获取相邻环之间的距离，若环间距的压缩比例落在预设范围内，则将该单元格标记为路沿候选单元格；沿扇形向外寻找高度突变单元格标记为路沿候选单元格；寻找预设邻域内水平波动在第三阈值内的变化单元标记为路沿候选单元格。

具体来说，通过以下公式得到路沿候选单元格：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri表示第i环的半径，ri+1表示第i+1环的半径，θi表示第i环相应的照射角，θi+1表示第i+1环相应的照射角，h表示传感器高度，Δri表示第i环与第i+1环之间的距离，Ii表示环间距的压缩比例预设范围，α和β表示第一系数以及第二系数。

在一个实施例中，在步骤S103中，将标记的路沿候选单元格根据点的分布规律得到左、右侧路沿的分割角度；遍历所有路沿候选单元格，根据所述栅格化数据中的扇形位置，对不存在路沿点的扇形进行排序，寻找非路沿扇形位置的中位数，选其为路沿分割线。

如图1，在步骤S104中，将激光雷达坐标系下的左侧路沿点以及右侧路沿点进行坐标系转换，得到全局坐标系下的路沿点数据。

如图1，在步骤S105中，基于路沿点数据，利用八叉树模型更新得到全局坐标系下的路沿地图。

具体来说，使用相同尺寸的小立方体离散化待建图区域，通过八叉树模型进行路沿点更新。

图2显示了根据本发明的一个实施例的矿山路沿检测及建图系统框架示意图。如图2所示的系统为解决矿山环境下路况多变，且道路表面凹凸的情形下的路沿检测问题，提出了一种基于八叉树占据更新模型的路沿检测与建图方案，感知数据来自LiDAR，定位数据来自组合导航定位模块，利用OctoMap融合路沿检测结果与全局定位信息，得到更加准确的全局路沿地图。

首先，对激光点云数据进行栅格化处理：

具体来说，以垂直路面向上为Z轴，按照右手系建立三维坐标系，X轴指向车辆行进方向；以LiDAR中心为原点，将点云数据划分成扇形网格区域，并根据激光雷达的环信息划分每个扇形(如图3所示)；计算每个单元格ci,j内部的平均半径，平均高度，平均水平坐标，高度最大值，高度最小值。

在一个实施例中，LiDAR使用16线激光雷达。

接着，利用海拔高度等依据，获取地面点云：

具体来说，根据每个单元格ci,j内部的高度差以及与相邻单元格的高度差来筛选地面候选单元格，当ci,j内部的高度差小于阈值，并且ci,j与临近单元格的高度差小于阈值时，将对应单元格记录为地面候选单元格；如果某个单元格被地面候选格包围，则将该单元格也标记为地面候选单元格。

然后，利用地面点云数据的环压缩率等特点，获取左右侧路沿点：采用环压缩、垂直高度差、水平波动差三个特征进行路沿检测。

步骤a，寻找环压缩所在的单元格：

当LiDAR发出的光束遭遇路沿时，相邻环之间会产生剧烈的压缩，根据LiDAR光束照射在水平面上的半径ri＝hcotθi获取相邻环之间的距离，若环间距的压缩比例落在某一范围内，则将该栅格标记为路沿候选单元格，公式如下：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri代表第i环的半径，θi为相应的照射角，h为传感器高度，Δri是环i与环i+1之间的距离，α和β决定了压缩率区间的范围。

通过比较环压缩的比例，选出路沿点。

步骤b，沿扇形向外寻找高度突变单元格标记为路沿候选单元格；

步骤c，寻找预设邻域内水平波动在第三阈值内的变化单元标记为路沿候选单元格；

步骤d，左右侧路沿划分：将每一帧检测到的路沿点根据点的分布规律得到左右侧路沿的分割角度。遍历全部路沿点，并根据之前栅格化后的扇形位置对不存在路沿点的扇形进行排序(分为车前和车后两部分)，寻找非路沿扇形位置的中位数，则选为路沿分割线。

接着，将激光雷达坐标系下的路沿点转换到全局坐标系下。

最后，利用八叉树模型对路沿点进行更新，从而得到全局坐标系下的路沿地图：

具体来说，使用相同尺寸的小立方体离散化待建图区域，通过OctoMap(基于八叉树的存储结构)进行路沿点更新，避免了RANSAC(随机此采样一致性)或者基于卡尔曼滤波的路沿平滑策略带来的拟合失败或者预测失败问题。

通过统计hit(光束撞击到立方格)和miss(光束未撞击到立方格)来进行概率更新，得到每个立方格的占据概率。达到阈值时认为该单元被占用，即存在路沿，反之为空闲，即不存在路沿。使用如下公式进行占据更新：

L(n|z1:t)

＝max(min(L(n|z1:t-1)+L(n|zt),lmax),lmin)

其中，lmin和lmax代表了log-odds的下限和上限。

以上基于体素的占用栅格方案可以有效的回避卡尔曼滤波中对于路宽不鲁棒的问题，而且通过调整hit、miss和占据阈值等参数可以实现不平整路面带来的大量误检测点。

综上，本发明主要针对矿山复杂环境下，路况多变的情形，利用OctoMap(基于八叉树的存储结构)融合多帧信息并建图。针对矿山路面凹凸不平的特性，提出了以环压缩为主的路沿检测方案，兼顾候选点邻域内的距离波动差。本发明对于弯道等特殊与非特殊情况下提出了基于路沿分布的左右路沿划分方案，克服了单纯依靠水平轴正负划分路沿所带来的局限性。

图4显示了根据本发明的一个实施例的矿山路沿检测及建图装置结构框图。其中，路沿检测及建图装置400包含第一模块401、第二模块402、第三模块403、第四模块404以及第五模块405。

第一模块401用于通过激光雷达采集得到目标区域的激光点云数据，对采集得到的激光点云数据进行栅格化处理，得到栅格化数据；

第二模块402用于基于栅格化数据，结合海拔高度信息进行筛选，得到地面点云数据；

第三模块403用于针对地面点云数据进行路沿检测，得到左侧路沿点以及右侧路沿点；

第四模块404用于将激光雷达坐标系下的左侧路沿点以及右侧路沿点进行坐标系转换，得到全局坐标系下的路沿点数据；

第五模块405用于基于路沿点数据，利用八叉树模型更新得到全局坐标系下的路沿地图。

综上，本发明主要针对矿山环境下路况多变的情况，利用八叉树模型提供的占据更新方案，实现路沿点的更新与平滑，进一步滤除路面凹凸引入的干扰点。另外，针对矿山路面凹凸不平的特性，提出了以环压缩为主的路沿检测方案，适应非垂直上升的矿山道路边界，兼顾候选点邻域内的距离波动差。本发明对于弯道等特殊与非特殊情况下提出了基于路沿分布的左右路沿划分方案，克服了单纯依靠水平轴正负划分路沿所带来的局限性。

应该理解的是，本发明所公开的实施例不限于这里所公开的特定结构、处理步骤或材料，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。

虽然本发明所公开的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所公开的精神和范围的前提下，可以在实施的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。