一种基于激光雷达点云局部结构特征的负障碍物检测方法与流程
本发明涉及机器人避障检测方法，特别是涉及面向无人车导航的一种基于激光雷达点云局部结构特征的负障碍物检测方法。
背景技术：
传统的用于负障碍物检测的方法有基于热红外图像、基于彩色图像和基于双目视觉的检测方法。基于热红外图像的负障碍物检测方法使用热红外相机采集图像，根据负障碍物与环境温度存在温差的特点检测环境中的负障碍物。这种检测方法的缺点是对环境温度变化十分敏感。基于彩色图像的负障碍物检测方法使用彩色相机采集图像，在图像序列中提取颜色模型和几何模型检测负障碍物，这种方法受光照条件影响强烈。基于双目视觉的方法使用双目图像并利用种子生长法检测负障碍物，这种方法同样受到光照条件影响。除了受到光照条件影响，传统使用单相机图像的负障碍物检测方法无法确定负障碍物的实际距离，使用双目视觉的负障碍物检测方法虽然能计算距离，但也存在精度不高的缺点。
为解决使用传统图像检测负障碍物易受到光照条件影响以及无法准确定位的问题，在无人车导航领域一类方法是使用三维激光雷达作为导航传感器。
三维激光雷达能够获取环境的点云数据，探测范围广，精度高，且不受光照条件影响，能够精确描述环境结构信息。
目前基于激光雷达检测负障碍物有基于多帧融合的检测方法和基于SVM分类的方法。
多帧融合的负障碍物检测方法通过融合多帧点云得到高密度局部地图，进而通过遮挡关系确定负障碍物位置。这种方法对静态环境中的负障碍检测效果较好，但计算复杂度高且不适用于环境中存在大量运动物体的情况。
基于SVM的负障碍物检测方法通过计算点云的局部特征，例如高度-长度-密度特征，再通过大量样本训练得到负障碍物分类器，进而用于对候选负障碍物点云的分类与位置检测。这种方法缺点在于需要预先进行大量样本训练，并且对未训练过的负障碍物种类的检测效果较差。
技术实现要素：
在无人车越野行驶环境中，存在沟壑、坑洞等负障碍物，本发明的目的在于提供了一种基于激光雷达点云局部结构特征的负障碍物检测方法，即利用垂直扫描激光雷达点云的局部结构特征提取负障碍物候选点对，进而聚类形成负障碍物候选区域，最后进行过滤得到负障碍物结果。
本发明所述的负障碍物检测方法采取先在单线激光点云中提取候选点对，再将多线激光提取的候选点对集合进行聚类的方案。首先是计算了点云中负障碍物的三类结构特征，即局部点云距离跳变、局部点云分布密集和局部点云高度下降。根据三类结构特征提取出负障碍物候选点对，通过空间位置一致性和点对长度一致性聚类得到负障碍物候选区域，通过面积过滤、点对数量过滤得到负障碍物结果区域。
本发明采用的具体技术方案包括如下步骤：
1)已采集的激光雷达点云数据，针对单线激光点云，逐线检测局部点云距离跳变、局部点云分布密集和局部点云高度下降的三个结构特征，依据三个结构特征在单线激光点云中提取筛选获得可能属于负障碍物的候选点对；
2)由各线激光的激光点云获得的所有候选点对，依据点对长度一致性和空间位置一致性进行聚类，得到一系列负障碍物候选区域，再经过面积过滤得到负障碍物区域。
本发明的负障碍物一般指的是地面上的凹坑、壕沟等障碍物。
本发明涉及的点云数据由激光雷达采集得到，激光雷达为多线激光旋转扫描的工作方式，一根激光线对应一段连续分布的点云。
所述步骤1)具体是针对单线激光点云，采用以下方式进行处理：
1.1)逐激光点计算邻近点间距与局部平滑度，并根据局部平滑度判断获得平滑点；
1.2)逐激光点通过检测局部点云距离跳变特征选取邻近点间距局部极大值的候选点对，作为初始候选点对；
1.3)在初始候选点对中，通过检测局部点云分布密集特征选取初始候选点对中后沿点所在位置点云局部密集的点对，获得筛选后的初始候选点对；
1.4)通过局部点云高度下降特征进行进一步筛选，获得最终的候选点对。
经过步骤1.4)得到的点对即是满足负障碍物三个局部结构特征，即邻接点距离跳变、后沿点密集分布、后沿点高度下降的候选点对集合。
所述步骤1.1)具体为：
1.1.1)计算邻近点间距
针对单线激光点云，采用以下公式计算每个激光点与其邻近激光点之间的空间距离，激光点的坐标为邻近激光点的坐标为
其中，表示激光点及其邻接点之间的邻近点间距，l表示单线激光点云中的激光点序号；
1.1.2)计算局部平滑度
除邻近点间距外，还需要计算单线激光点云的局部平滑度。对于单线激光而言，其扫描方式为平面扫描，因而单线激光扫描局部地形所形成的点云可看做该扫描平面与地面的交线。在平坦的地面上，该交线呈直线分布，在地形起伏剧烈的地面上，该交线则呈现出蜿蜒曲折的折线形状。基于此，采用以下公式计算单线激光点云中每个激光点的局部平滑度sl：
式中，Nneighbor表示预设的邻近点个数，一般取n＝5，l表示单线激光点云中的激光点序号，k表示该激光点的邻近点的序号；
上述的sl大小体现了该激光点局部区域的平滑度，sl值非常小表明该激光点处于平滑的近似平面上，sl值非常大表明该激光点所处的位置地形起伏剧烈。
预设平滑阈值Thssmooth和非平滑阈值Thssharp，通过将激光点的局部平滑度sl分别与平滑阈值Thssmooth和非平滑阈值Thssharp进行比较获得激光点是否为平滑点的结果：若满足sl≤Thssmooth，该激光点为平滑点；若满足sl≥Thssharp，该激光点为非平滑点。
所述步骤1.2)具体是：对于单线激光点云上的各激光点，按以下公式处理并提取获得邻近点间距跳变点集合Pjump：
式中，Njump_nbr表示预设的邻近点个数，Thsdis为邻近点间距跳变阈值，l表示单线激光点云中的激光点序号，k表示该激光点的邻近点的序号；
邻近点间距跳变点集合Pjump中的点是作为候选点对中的后沿点，候选点对中包括前沿点和后沿点，前沿点是序号靠前的激光点，后沿点是序号靠后的激光点，判断负障碍往往需要前后沿的点对，因此再取邻近点间距跳变点集合Pjump中的每个激光点作为后沿点，其在单线激光点云中之前的一个激光点作为前沿点，形成邻近点间距跳变点对并获得邻近点间距跳变点对集合Pneg_pair，具体是采用以下公式：
式中，l表示单线激光点云中的激光点序号。
以邻近点间距跳变点对集合Pneg_pair中的邻近点间距跳变点对作为初始候选点对。
所述的步骤1.3)具体为：将初始候选点对投影到激光雷达点云空间所对应的栅格属性地图上，每个栅格统计落在该栅格内激光点的数目，针对初始候选点对的后沿点判断是否满足以下公式：
PtNum(k，l)-PtNum(m,n)≥Thsdense
m∈[k-Ngrid,k+Ngrid],m≠k
n∈[l-Ngrid,l+Ngrid],n≠l
式中，Ngrid表示预设栅格邻近距离，PtNum(k,l)表示当前栅格中包含的所有激光点总数，PtNum(m,n)表示当前栅格在预设栅格邻近距离Ngrid邻域内的一栅格内包含的所有激光点总数，Thsdense是预设的点云密集阈值，当前栅格是指初始候选点对的后沿点所在的栅格，m、n分别表示当前栅格在预设栅格邻近距离Ngrid邻域内的栅格行号与列号。
初始候选点对的后沿点满足公式，则认为满足局部点云分布密集特征，将该初始候选点对保留，继续下一步的处理；否则，将该初始候选点对舍弃，不继续下一步的处理。
所述步骤1.4)具体为：
对于步骤1.3)筛选后的每个初始候选点对，选取后沿点前方的Nline个平滑点形成平滑点集Plocal_smooth，Nline表示选取的平滑点总数，为预设值，将滑点集Plocal_smooth中的所有平滑点通过最小二乘法进行直线拟合获得直线方程；
再将后沿点空间坐标[xl,yl,zl]中位于水平平面的x,y坐标代入直线方程，并与后沿点的z坐标相比较，具体是采用以下公式进行判断筛选：
式中，dzl是筛选后的初始候选点对前方的平滑点拟合直线在后沿点处的估计高度与实际高度之差，Thsz为预设的候选点对后沿点高度下降阈值，l表示候选点对序号；
若满足上述公式，则表明该后沿点位于直线的下方，满足局部点云高度下降特征，保留该后沿点所在的初始候选点对，继续下一步的处理；否则，将该后沿点所在的初始候选点对舍弃，不继续下一步的处理。
本发明的激光点云空间的xyz坐标中，x是沿朝车辆的右方向，y是沿朝车辆的正前方向，z是沿垂直于底面朝向正上方向。
所述的直线方程采用以下方式拟合计算得到：
设空间直线经过点[x0,y0,0]，方向向量为[a,b,1]，则空间直线方程表示为：
其中，a、b分别表示空间直线的归一化的方向向量在x、y坐标轴的两个分量，x0、y0分别表示空间直线经过点的坐标的x、y坐标轴的两个分量。
上述方程写成矩阵形式为：
将这Nline个平滑点代入上述矩阵，得到：
其中，x1到y1到z1到分别表示第1个到第Nline个平滑点的坐标的x、y、z坐标轴分量；
化简得到：
上式中，l是平滑点序号；
解出上述a、b、x0、y0即求出直线方程。
所述的步骤2)中聚类具体为：
2.1)将步骤1)获得各线激光点云中的候选点对进行合并，然后采用以下方式进行聚类，聚类过程在栅格属性地图上进行，栅格属性地图中的坐标具有行u与列v，将候选点对投影到栅格属性地图上获得对应的栅格点对，栅格点对由前沿栅格点pfront和后沿栅格点pback构成；
2.2)每一对候选点对均使用向量表示；
取前沿栅格点pfront和后沿栅格点pback之间的中点表示该候选点对在栅格属性地图上的空间位置：
其中，u、v分别表示该候选点对在栅格属性地图上的行坐标和列坐标，ufront、vfront分别表示前沿栅格点pfront在栅格属性地图上的行坐标和列坐标，uback、vback分别表示后沿栅格点pback在栅格属性地图上的行坐标和列坐标；
采用以下公式取前沿栅格点pfront和后沿栅格点pback之间的距离表示候选点对的点对长度w：
2.3)由所有候选点对的向量形成候选点对向量集合Gpars：
其中，Npars表示步骤1)获得所有线激光上的候选点对总数；
2.3)使用均值偏移的方法进行聚类：
对每一个候选点对的向量均初始构造一个质心向量使得从而形成质心向量集合Gcenters：
初始的质心向量集合Gcenters满足Gcenters＝Gpars；
对质心向量集合Gcenters中的每个质心向量，采用公式遍历计算其对应的加权质心向量
其中，为高斯核函数，l、k均为质心向量的序号，h为预先设定的核函数带宽；
所述的高斯核函数采用以下公式计算：
其中，h为核函数带宽；
从而得到加权质心向量集合Gshfts：
2.4)对加权质心向量集合Gshfts中的每个加权质心向量，采用以下公式计算其与其对应的质心向量之间的欧氏距离Dshftl：
式中，l表示加权质心向量的序号。将欧氏距离Dshftl与误差阈值Epsilon采用以下公式相比较，判断获得质心向量是否达到局部概率密度极大条件：
Dshftl≤Epsilon
若满足条件，则保持不变，否则采用以下方式更新
2.5)重复步骤2.2)-2.4)，直到所有候选点对的向量对应的所有质心向量均达到局部概率密度极大条件，即对任意i∈{0,1,…,Npairs-1}，均满足Dshifti≤Epsilon；
2.6)每一个候选点对对应一个质心向量遍历这些质心向量，将相同质心向量对应的候选点对归为同一类，完成候选点对集合的聚类。
所述步骤1.3)或者步骤2.1)中的栅格属性地图具体是：栅格属性地图为基于大地平面假设，将激光雷达点云空间所在的x-y水平平面分割成大小相等的栅格，每个栅格尺寸为gridsize×gridsize，激光雷达点云空间的三维激光点坐标投影到栅格属性地图所在的栅格坐标(row,col)为：
其中，row、col分别表示该三维激光点投影到栅格属性地图所在的栅格坐标的行号与列号，Yoffset、Xoffset分别表示预设的偏移量；
并且每一个栅格包含车体区域标志位isCar、负障碍物标志位isNegObs和栅格内点云数目PtNum(row,col)的属性。本发明将负障碍物区域使用栅格属性地图来呈现，属性为负障碍的栅格表示负障碍物所在位置。
所述的步骤2)中过滤具体为：得到负障碍的若干聚类之后，还需进行过滤操作。通常认为负障碍物应当大于一定尺寸以排除点云稀疏造成的误判。负障碍物区域的面积尺寸由两个值描述：该类中的候选点对个数Numpar，以及候选点对构成多边形的面积Areaneg，多边形为该类中候选点对的所有点的最小外接多边形。由同一类的所有候选点对所形成的区域作为负障碍物候选区域，遍历所有负障碍物候选区域，采用以下公式判断是否满足：
Numpar≥Thsnum
Areaneg≥Thsarea
式中，Numpar表示负障碍物候选区域中所有候选点对的总对数，Areaneg表示负障碍物候选区域中所有候选点对构成多边形的面积，多边形为包围在负障碍物候选区域中所有候选点对外的最小外接多边形，Thsnum为区域候选点对数量阈值，Thsarea为负障碍物在栅格属性地图中形成多边形的面积阈值；
若同时满足以上两个条件，则负障碍物候选区域保留，为最终的负障碍物区域；
若不同时满足以上两个条件，则负障碍物候选区域舍弃，不为最终的负障碍物区域。
与背景技术比较，本发明具有的有益效果是：
1)本发明不依赖彩色相机、红外相机等二维传感器，因此检测结果不受光照条件的影响；
2)本发明使用激光雷达传感器检测负障碍物，能够直接测量负障碍物的距离，检测结果位置精度高；
3)本发明先将大规模点云提取出小数量的候选点对集合，从小数量的候选点对中进行负障碍物聚类，相比直接使用图像数据建模的计算复杂度更低，实时性更强；
附图说明
图1是本发明方法的流程图。
图2是垂直扫描激光雷达的安装侧视图。
图3是垂直扫描激光雷达的安装俯视图。
图4是本发明的典型使用场景，其中左上是点云俯视图，右上是场景实拍图像，右中为栅格属性地图，下方是对应点云三维渲染图。
图5是对典型场景点云实施本发明内容(1.1)计算邻近点间距(左图)与局部平滑度(右图)之结果，越亮表示值越大。其中P1、P2、P3、P4、P5是负障碍物候选点对的后沿点，可见在负障碍物候选点对中后沿点的邻近点间距为局部极大值，负障碍物候选点对前方点云的局部平滑度较小，属于平滑点。
图6是对典型场景点云实施本发明内容(1.4)拟合直线的结果，P6、P7、P8、P9、P10为负障碍物候选点对前方的5个平滑点，白色长虚线为这5个平滑点拟合的直线，可见负障碍物候选点对中后沿点相对拟合直线呈现高度下降特征。
图7是对典型场景点云实施本发明内容(1)、(2)提取负障碍物候选点对与聚类后的结果。左图为提取获得的负障碍物候选点对，用白色线段标出，右图为聚类结果，用序号标识。
图8是对典型场景点云实施本发明内容(2)过滤负障碍物候选区域的结果。左图为过滤结果，只有0号和1号两个负障碍物候选区域保留下来，右上为场景实拍图像，右下为对应栅格属性地图，可见只保留过滤后的两个负障碍物区域。
图9是对4个野外场景点云实施本发明负障碍物检测方法的结果图。对每一张子图，左边是提取的负障碍物候选点对的聚类结果，右上为场景实拍图像，右下为对应的栅格属性地图，其中标识出过滤后输出的负障碍物区域。
具体实施方式
下面结合附图和实施例对本发明做进一步说明。
如图1所示，本发明方法的实施例及其实施过程如下：
以一个典型的包含负障碍的场景例子来更清楚地表示本发明的过程。
实施例的点云数据由车辆前部安装的两个激光雷达采集得到，两个激光雷达布置方式和采集方式如图2和图3所示，激光雷达为多线激光旋转扫描的工作方式，一根激光线对应一段连续分布的点云。
场景示意见图4，其中左上为激光雷达点云俯视图，右上为场景实拍图像，右中为对应的栅格属性地图，下方位激光雷达点云三维渲染图。取gridsize为20cm，Yoffset为1500cm，Xoffset为2000cm。
步骤一：对该场景实施本发明之(1.1)，逐线计算邻近点间距、局部平滑度，其中Nneighbor取值5，Thssmooth取值50，Thssharp取值350。如图5所示，左边为邻近点间距示意图，越亮表示值越大，可见负障碍物候选点对中后沿点的邻近点间距数值较大；右边为局部平滑度示意图，越亮表示值越大，可见负障碍物候选点对前方点云的局部平滑度较小，为平滑点。
步骤二：对该场景实施本发明之(1.2)，逐激光点通过检测局部点云距离跳变特征选取点云邻近点间距局部极大值的候选点对，作为初始候选点对。其中Thsdis取值40cm，Njump_nbr取值5。
步骤三：对该场景实施本发明之(1.3)，在初始候选点对中，通过检测局部点云分布密集特征选取初始候选点对中后沿点所在位置点云局部密集的点对，获得筛选后的初始候选点对。其中Thsdense取值4，Ngrid取值2。
步骤四：对该场景实施本发明之(1.4)，在筛选后的初始候选点对中，通过局部点云高度下降特征进行进一步筛选获得最终的候选点对。选取候选点对前方Nline个平滑度数值较小的点，其中Nline取值为5，用以拟合直线，继而选取后沿点高度显著低于直线位置的负障碍点对。如图6所示，其中白色长虚线代表候选点对前方5个平滑点拟合的直线，可见负障碍物候选点对中的后沿点相对拟合直线呈现高度下降特征。
步骤五：对该场景实施本发明之(2.1)～(2.6)，将步骤四选取的负障碍物候选点对进行空间位置一致性、点对长度一致性聚类，其中核函数带宽h取值4.0，误差阈值Epsilon取值0.0001。如图7所示，左图为经过步骤四获得的最终的候选点对，用白色线段标出，右图为聚类结果，共聚成6类，用序号标识。
步骤六：对该场景实施本发明之(4)，对聚类结果根据区域面积、点对数量过滤，其中Thsnum的值取3，Thsarea值取20。如图8所示，左图为负障碍物候选区域经过滤后的结果，仅0、1号负障碍物候选区域被保留下来；右图为对应输出的栅格属性地图，其中车体前方两个白色区域代表过滤后得到的负障碍物区域。
另外测试了4组不同的场景数据来评估本发明的效果，以激光雷达点云作为输入，检测输出的栅格属性地图中负障碍物标识个数、位置。如图9所示，4张子图表示4个不同场景的检测结果。对每一张子图，左边为激光雷达点云俯视图，右上为对应场景彩色图像，右下为输出的栅格属性地图。检测的具体结果如表1所示。表1中场景1、场景2、场景3、场景4分别对应图9中的左上、右上、左下和右下四个子图。表1记录了4组实验的场景描述、光照条件、总帧数、成功检测出负障碍物的帧数、负障碍物首次检出距离、检测成功率、平均位置误差和平均每帧耗时。
表1
从上表1结果可见，在不同光照条件下本方法均能够达到90％以上的检测成功率，平均位置误差约为0.2m。对于直径为1m，深0.5m的凹坑，本方法的首次检出距离约为12.4m，而宽度为0.6m，深0.5m的壕沟，本方法的首次检出距离为12.0m。实验采用一台配备4核心主频为2.6GHz的Intel i5处理器的电脑，实验中每帧激光雷达点云的平均处理耗时约为10ms，所以本发明方法的计算代价很小。
可以看出，本发明提出的面向无人车导航的基于点云数据局部结构特征的负障碍物检测方法具有检测结果不受光照条件影响、检测结果位置精度高的特点，此外，本方法计算代价小，可以实时运行。
- 还没有人留言评论。精彩留言会获得点赞！