matlab扫描线检测法,一种基于激光雷达点云统计几何模型的非结构化道路检测方法与流程...

本发明涉及一种对无人驾驶的环境感知技术，特别是一种基于激光雷达点云统计几何模型的非结构化道路检测方法。

背景技术：

无人车的安全行驶依赖于其对周围环境的智能感知。对于路面检测技术而言，无人车通常将道路划分为结构化、非结构化道路两类。纵观国内外近年来的相关进展，对于结构化道路的路面检测技术已经日益成熟，而非结构化道路(例如荒野)通常具有形状不明确、边缘不确定等复杂多变的视觉特征，对于其路面检测技术的研究造成了极大的困难与挑战，至今仍无较为成熟的解决方案。三维激光雷达作为一种集成多种高新技术的测绘传感器，其探测范围广，且分辨率高，同时对光线等环境因素较为鲁棒，能够满足在复杂环境下执行任务的需求，因此被广泛应用于无人车的环境感知任务。

目前基于三维激光雷达的路面检测技术主要是基于栅格地图的分割算法和基于扫描线的几何特征分割算法两大类。由CN.108460416A可知一种可行驶路面区域的检测方法，通过提取三维激光雷达的兴趣数据点，采用K-Means聚类算法和高度特征提取障碍物，结合DBSCAN聚类方法提取路沿，通过最小二乘拟合路沿曲线，最终确定道路可通行区域。由CN.108280840A可知另一种可行驶路面区域的检测方法，通过提取单线确定方位角序列-距离关系图，通过改进模糊线段法绘制线段地图、识别路面以及障碍物，最终得到可通行区域。

上述方法虽然在一定程度上都取得了较为不错的效果，但其只适用于结构化环境。对于非结构化环境而言，道路两侧障碍物时有时无，利用障碍物信息确定道路边缘并非严格满足，因此上述路面检测算法无法适用。

技术实现要素：

本发明要解决的技术问题在于针对现有技术的缺陷，提出一种基于激光雷达点云统计几何模型的非结构化道路检测方法。

本发明解决其技术问题所采用的技术方案是：一种基于激光雷达点云统计几何模型的非结构化道路检测方法，包含以下步骤：

(1)利用车载激光雷达获取原始三维点云数据；

(2)利用三维点云数据构造激光点云三维直方图，具体步骤如下：

(2.1)以三维激光雷达为坐标原点建立笛卡尔坐标系，X轴方向平行于地面指向无人车前进方向，Y轴方向平行于地面指向无人车左方，Z轴垂直于X-Y平面，方向为垂直地面向上；

(2.2)以X轴正方向坐标的倒数作为横轴坐标，以激光雷达线束作为纵轴坐标，并统计每一条激光雷达线束上在位置的扫描点数量，构建激光点云三维直方图；

(3)拟合三维直方图中的数据点得到道路平坦区域的基本轮廓，具体步骤如下：

(3.1)通过RANSAC算法拟合三维直方图中的数据点得到道路直线方程：

n＝km+b

式中n为激光雷达线束，m为笛卡尔坐标系中X轴正方向坐标的倒数；k和b为道路直线方程的参数；

(3.2)利用道路直线方程建立道路模型容限区间，区间上下界的直线方程为：

n＝km+αb

n＝km+βb

式中，α为确定区间容限上界的参数，α＜1，β为确定区间容限下界的参数，β＞1；

(3.3)将原始激光点云数据投射到三维直方图中，若投射到三维直方图中的数据点落在道路模型容限区间内，则为道路区域点；反之若数据点落在道路模型容限区间外，则为非道路区域点；

(3.4)利用三维直方图与原始激光点云的对应关系，统计落在道路模型容限区间内的激光点云数据，得到道路平坦区域的基本轮廓；

(4)采用最小二乘方法拟合道路平坦区域的基本轮廓上的点云数据，得到道路平坦区域的二次曲面方程；

(5)设置栅格分辨率，在笛卡尔坐标系下得到空间栅格地图，并统计各栅格单元内所有激光点云数据的最大高度差；

(6)计算激光点云数据到二次曲面的距离，并根据激光点云数据所属栅格单元点云数据的最大高度差，确定最终的可通行区域；具体为：设定激光点云数据到二次曲面的距离的最大阈值为D，计算激光点云数据到二次曲面的距离；若距离小于D且该点云数据所属的栅格单元内点云数据的最大高度差小于阈值E，则判断该点为路面点，否则为非路面点；将得到的所有路面点的集合作为道路可通行区域，得到最终的路面检测结果。

进一步地，所述步骤(3.1)中，通过RANSAC算法拟合三维直方图中的数据点得到道路直线方程的具体过程为：首先随机选取两个数据点得到直线方程，设定点到直线方程的距离阈值为T，若点到直线的距离小于T,则为内点；若点到直线的距离大于T，则为外点；计算得到包含内点最多的直线方程，即所求的道路直线方程。

进一步地，所述步骤(4)中，采用matlab最小二乘拟合工具包拟合道路平坦区域的基本轮廓上的点云数据，得到二次曲面的参数P0,P1,P2,P3,P4,P5，从而得到如下二次曲面方程：

f(x,y)＝P0x2+P1y2+P2xy+P3x+P4y+P5

式中，x为笛卡尔坐标系中X轴方向的坐标值，y为笛卡尔坐标系中Y轴方向的坐标值；采用该二次曲面方程来拟合路面满足简单平面和复杂曲面等多种场景。

进一步地，所述步骤(5)中，在栅格地图中的X-Y平面选定感兴趣范围内的栅格单元，统计每个栅格单元内点云数据的Z方向高度的最大差值。

本发明的有益效果是：根据三维激光雷达点云的统计几何模型构造激光点云的三维直方图，用RANSAC算法拟合三维直方图中道路区域对应的数据点，通过二次曲面拟合得到的道路轮廓，可以满足路面起伏较大的非结构化道路。此外结合栅格地图，统计栅格单元的高程差，通过计算激光点云到道路二次曲面的距离，得到准确可靠的道路可通行区域。

附图说明

图1为基于激光雷达统计几何模型的非结构化道路检测方法的流程图；

图2为64线激光雷达数据的三维直方图；

图3为RANSAC拟合三维直方图结果图；

图4(a)为道路区域的平面方程拟合结果图；

图4(b)为道路区域的二次曲面拟合结果图；

图5为激光点云的栅格地图；

图6为本发明检测非结构化道路的实际效果图。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

如图1所示，本发明提供了一种基于激光雷达点云统计几何模型的非结构化道路检测方法，具体实施方式如下：

步骤1.在无人车车顶安装三维激光雷达，通过三维激光雷达扫描周围环境得到原始三维点云数据。以Velodyne 64线激光雷达为例，其垂直测量角度范围为+2°到-24.33°，其中，从+2°到-8.33°包括从第1到32条扫描线，角度分辨率为0.33°，从-8.33°到-24.33°包括了第32到64条扫描线，角度分辨率为0.5°。以三维激光雷达为坐标原点建立笛卡尔坐标系，X轴方向平行于地面指向无人车前进方向，Y轴方向平行于地面指向无人车前向左手方向，Z轴垂直于X-Y平面，方向为垂直地面向上。

步骤2.构造激光点云的三维直方图，其步骤如下：建立三维直方图坐标系，其中横轴坐标是正前方距离的倒数，纵轴坐标是64条激光雷达线束，并统计每一条激光雷达线束上在位置的扫描点数量。对于第32～64条激光扫描线来说，激光雷达线束n与的函数关系为：

其中H为激光雷达距离地面的高度。如图2是本发明得到的64线激光雷达数据的三维直方图。

步骤3.对三维直方图进行数据筛选，剔除数值较小的干扰点。用RANSAC直线拟合算法拟合道路平坦区域在直方图中的数据点，首先随机选取两点得到直线方程，设定点到直线方程的阈值T＝0.1，若点到直线的距离小于T,则为内点；若点到直线的距离大于T，则为外点。计算得到包含内点最多的直线方程，即所求的道路直线方程：

n＝km+b

根据实验所得，式中k＝0.2，b＝21.79，n为激光雷达线束，m为笛卡尔坐标系中X轴正方向坐标的倒数。由于道路平坦区域并不是完美的平面，建立道路模型容限区间，区间上下界的直线方程为：

n＝km+αb

n＝km+βb

通过实验选择最佳的区间容限上下界参数为α＝0.5，β＝1.2。

将原始激光点云数据投射到三维直方图，若数据点落在道路直线的容限区间内，则为道路区域点；反之若数据点落在道路直线的容限区间外，则为非道路区域点。统计落在道路直线容限区间内的激光点云，得到道路平坦区域的基本轮廓。如图3是本发明得到的直线拟合结果图。

步骤4.使用Matlab的最小二乘拟合工具包，在道路平坦区域的基本轮廓的点云数据上拟合得到道路二次曲面方程如下：

f(x,y)＝P0x2+P1y2+P2xy+P3x+P4y+P5

式中P0＝-0.0021，P1＝0.0012，P2＝-1.5926×10-4，P3＝-0.0643，P4＝0.0096,P5＝-1.7353，为使用Matlab的最小二乘拟合工具包拟合得到的参数。采用该二次曲面方程可以拟合路面是复杂曲面的场景。如图4(a)和图4(b)是本发明得到的道路平面方程与二次曲面方程在同一场景下的拟合结果对比图。

步骤5.建立激光点云的二维栅格地图，其步骤如下：设定栅格地图感兴趣范围为X方向0～60m，Y方向-20m～20m，设定栅格单元的尺寸，即栅格分辨率为R＝0.2×0.2m2。统计每个栅格单元内点云数据的Z方向高度的最大差值。如图5是本发明建立的二维栅格地图。

步骤6.通过实验测试，设定点到二次曲面的距离阈值D＝0.2m，栅格单元的最大高度差阈值E＝0.15m。计算激光点云到二次曲面的距离，如果距离小于D且该点所属的栅格单元内的最大高度差小于E，判断该点为路面点，否则为非路面点。对原始激光点云进行上述操作后得到最终的路面检测结果。如图6是本发明最终得到的道路可通行区域。

以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的优选实施例子，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。