基于多线激光的扫描式障碍物检测方法与流程

本发明属于智能驾驶技术领域，尤其涉及一种基于多线激光的扫描式障碍物检测方法。

背景技术：

随着自动化技术与无人驾驶技术发展，对环境感知技术要求越来越高。目前，由于摄像头图像检测技术在晚上、曝光过强、雨雪雾等恶劣情况下无法工作，而毫米波雷达由于其信息量少且其只对金属等物体反射敏感因而也不适用于智能驾驶环境感知中。多线激光雷达具有测距准且对环境要求低的优点，越来越多地被应用在智能驾驶环境感知中。如图6所示，一般基于激光雷达障碍物检测技术其检测步骤如下，首先，将激光雷达扫描到的点云无序的放置在一起；其次，对这些点云进行平面拟合，从而滤出路面，最后，将残存的点云进行离散化和栅格化，形成最终的障碍物检测结果。当前传统的多线激光雷达基于障碍物检测技术存在以下问题：一、单纯的平面拟合无法解决有坡度路面问题；二、车辆在行驶过程中，由于车架和车身的非刚性连接以及地面颠簸等问题，多线激光每根线束的外参都是动态变化的，从而使得默认标定统一的外参无法真实的描述每个点云点的真实位置；三、由于多线激光的点云是由内到外非均匀分布，因此单纯进行简单的栅格和离散化并不能很好的利用点云信息。

技术实现要素：

本发明的目的在于克服上述技术的不足，而提供一种基于多线激光的扫描式障碍物检测方法，可以在障碍物检测过程中，克服由于车辆行驶在非水平路面过程中颠簸以及车身与车架之间非刚性连接等因素而影响检测结果，提高了障碍物检测的鲁棒性和精度。

本发明为实现上述目的，采用以下技术方案：一种基于多线激光的扫描式障碍物检测方法，其特征是：采用激光点云外参自动标定方式，对激光点云进行排序和收集，通过扫描式检测，对点云进行离散化和栅格化，满足在非水平路面行驶环境下的多线激光雷达扫描式障碍物检测，具体步骤如下：

步骤一、外参姿态动态自动标定

1)外参粗估计：采集一段车辆匀速行驶的点云簇，通过手工测量大致估算航向角、俯仰角、横摆角；

2)利用高度约束筛选点云：选取符合高度约束的点云数据，高度z选择在[zmin，zmax]范围内的所有点云；

3)路面拟合：利用ransac算法拟合路面，确定路面基准方程及满足基准方程要求的内点点云；将内点点云以及基准方程投影到xy平面上，精确估计航向角；将内点点云以及基准方程投影到xz平面上，精确估计俯仰角；将内点点云以及基准方程投影到yz平面上，精确估计横摆角；

步骤二、点云排序和收集

对步骤一采集到的点云簇进行极坐标转换；对点云进行排序和收集，构建极坐标下的点云矩阵进行存储，对全部激光点云按照航向角和俯仰角的角度进行存储；

步骤三、扫描式检测

对点云矩阵中每个元素进行搜索，检测出其中的障碍物；

步骤四、离散化和栅格化

根据指定的横向分辨率和纵向分辨率创建栅格地图，将障碍物对应的点云点离散化到每个栅格结果中；

步骤五，利用四元数方法输出多线点云的姿态结果完成外参的自动标定。

所述步骤一中所述路面拟合的具体方法为：选取的路面点云利用ransac算法拟合路面，确定路面的基准方程：

a)随机选取三个点云估计路面的基准方程ax+by+cz+d＝0；

b)内点定义为待选路面点，即在步骤a)中估计的基准方程上的点，将待选的路面点云进行内点统计，保留内点数目和基准方程；

c)重复步骤a)、b)直到超过迭代次数时退出，期间保留内点数据数目最多的基准方程即为路面的基准方程；

d)输出路面的基准方程。

所述步骤一中内点点云以及基准方程分别投影到xy、xz、yz平面上，精确估计航向角、俯仰角和横摆角的方法为：

a)分别忽略z，y和x坐标，将内点点云投影到三个平面上；

b)通过自动识别护栏、路沿、墙面或者建筑物，进行直线拟合；

c)将直线转化为斜率角度，作为航向角、俯仰角、横摆角的精确化估计结果。

有益效果：本发明能够克服由于车辆行驶在非水平路面产生的颠簸以及车身与车架之间非刚性连接等因素导致检测结果不精确，提高了障碍物检测鲁棒性和精度。

附图说明

图1是本发明障碍物检测方法的框架图；

图2是激光线束外参确定流程图；

图3是激光雷达安装结构示意图；

图4是栅格地图离散化前的结果示意图；

图5是栅格地图离散化后的结果示意图；

图6是传统基于多线激光雷达障碍物检测方法的框架图。

图中：a、路面点，b、检测到的障碍物。

具体实施方式

下面结合较佳实施例详细说明本发明的具体实施方式。

详见附图1、2，为解决提到的车辆行驶过程中由于车架和车身的非刚性连接以及地面颠簸等问题导致多线激光线束外参动态变化，使用默认外参估计值而不准确问题，本实施例提供一种基于多线激光的扫描式障碍物检测方法，采用激光点云外参自动标定方式，对激光点云进行排序和收集，通过扫描式检测，对点云进行离散化和栅格化，满足在非水平路面行驶环境下的多线激光雷达扫描式障碍物检测，具体步骤如下：

步骤一、外参姿态动态自动标定

1、利用安装在车顶的多线激光雷达采集一段车辆匀速行驶的点云簇，值得提的是本实施例中使用的多线激光雷达为：velodyne16线激光雷达，其垂直扫描角度为30°，垂直分辨率为2°，水平扫描角度为360°，水平分辨率率为0.2°，转速为10hz。具体安装方法如图3所示，x轴平行于车辆前进方向，y轴垂直于车辆前进方向向左，z轴垂直于车辆所在地平面向上，坐标原点为车辆后轴中心点落在地平面的交点。将velodyne16线激光雷达安装于车顶，底部通过螺丝固定，一端连接12v电源，另一端连接网线进行数据输出。通过该多线激光雷达采集到的点云簇为由x，y，z三个坐标组成的点集；另外值得提的是，驾驶路段必须保证道路两侧有护栏，路沿，墙面或者建筑物作为障碍物，对采集到的点云簇通过手动测量，大致估算出多线激光雷达相对于路面的姿态，包括航向、俯仰、横摆三个角度，采集到的三个参数作为标定的默认参数，参考此默认参数可以将激光点云扫描结果投影到真实世界坐标系中；

2、采集到的点云簇，通过简单的高度约束选出一些待选的路面点云，比如选择高度z在[zmin，zmax]范围内的所有点云；对于高度z的选取方法如下，由于激光雷达的安装位置是固定的，所以通常认为z＝0的地方为路面，zmin与zmax取值不会离0很远，本实施例中选取zmin＝-0.3m，zmax＝0.3m；

3、选取的路面点云利用ransac算法拟合路面，确定路面的基准方程。具体实施方法如下：

a)随机选取三个点云估计路面的基准方程ax+by+cz+d＝0；

b)对待选的路面点云进行内点统计，内点定义为待选路面点在步骤a)中估计的基准方程上的点，保留内点数目和基准方程；

c)重复步骤a)、b)；

d)直到超过迭代次数时退出，期间保留内点数据数目最多的基准方程即为路面的基准方程；

e)输出路面的基准方程；

步骤四，将步骤三中路面的基准方程和路面内点点云分别投影到xy、xz、yz平面，精确化前述步骤中手动测量的航向、俯仰、横摆三个角度。具体实施方法如下：

a)分别忽略z，y和x坐标，将点云投影到三个平面上；

b)通过自动识别护栏、路沿、墙面或者建筑物，进行直线拟合；

c)将直线转化为斜率角度，作为航向、俯仰、横摆三个角度的精确化估计结果；

步骤五，利用四元数方法输出多线点云的姿态结果完成外参的自动标定。具体的将航向角、俯仰角、横摆角(绕固定坐标系x-y-z依次旋转的角度)转换为四元数公式如下，为了便于描述，将航向角定义为α，俯仰角定义为β，横摆角定义为γ：

根据上面的公式可以求出逆解，即由四元数q＝(q0，q1，q2，q3)或q＝(w，x，y，z)到欧拉角转换公式为：

对于tan(θ)＝y/x：

θ＝arctan(y/x)求出的θ取值范围是[-pi/2，pi/2]；

θ＝arctan2(y,x)求出的θ取值范围是[-pi，pi]；

当(x，y)在第一象限，0<θ<pi/2；

当(x，y)在第二象限，pi/2<θ<pi；

当(x，y)在第三象限，-pi<θ<-pi/2；

当(x，y)在第四象限，-pi/2<θ<0；

具体的，依据粗估计得到的航向角，利用上述正转换公式(1)和逆转换公式(2)，得到精确的航向角、俯仰角、横摆角；

二、点云排序和收集

具体实施方法如下，因为多线激光点云是由独立激光头扫描得到，考虑到它的分布是以激光雷达中心点为中心，扫描出来的多组圆锥其本身具有几何意义的特征。首先，需要将所有点云进行极坐标转换，如下式所示：

heading＝arctan(y,x)(4)

pitch＝arctan(z,r)(5)

式中，r指圆锥体底部半径，x、y、z表示x轴、y轴和z轴坐标，heading表示航向角，pitch表示俯仰角；

其次，对点云进行排序和收集，构建极坐标下的点云矩阵h进行存储，其横纵坐标分别为：

xnew＝heading×360×a/π(6)

ynew＝(pitch-pitchmin×b)(7)

其中，a表示横向的分辨率，如a＝2时，横向分辨率为0.5°，b表示纵向分辨率，pitchmin表示pitch的最小角度，b和pitchmin跟多线激光雷达的自身参数相关。以velodyne16线激光雷达为例，pitchmin＝-15，b＝0.5。通过这样的构建，所有激光点云就可以按照航向角和俯仰角进行存储；

特别注意的是，点云矩阵h中，每个元素会包含多个点云点，其记录的信息包含：x，y，z，heading，pitch，radium，xnew，ynew。其中，radium表示投影到xy平面上的半径。对于每个元素，都按照radium进行排序，保证点的顺序是从近到远；

三、扫描式检测

由于多线激光是先航向角、后俯仰角的扫描方式进行二维扫描，对于点云矩阵h的每个元素，对其中所有点云数据进行搜索，检测出其中的障碍物，评判标准为排除前述步骤中路面基准方程内点以外的点云即为障碍物；

尽管上述所述步骤中动态自动标定能够解决一部分外参动态变化的问题，然而实际运行中，每帧点云的外参都是不同的。因此需要得到一个比z更加精确的距离参数distance，用于表示点到地面的距离。具体实施步骤如下：

a)根据前述步骤中得到的ynew数据，只保留和地面可能会有交线的点，本实施例中认为ynew小于阈值0.3时即和地面有交线。同时，为了避免墙面占用大量点影响结果，按照x、y进行采样，每0.1m×0.1m的格子里仅保留一个点；

b)根据这些点云数据，利用ransac算法估计路面基准方程ax+by+cz+d＝0；

c)如果|c-1.0|＜δ1且|d|＜δ2，其中，δ1、δ2为经验参数，不同车型取值不同。更新否则distance＝z。

利用距离参数distance进行障碍物的检测，即满足以下约束条件为路面点，否则都为障碍物点：

其中，

distancediff＝(distancei-distancei-1)/(radiumi-radiumi-1)，thr1＝0.15m表示高度的绝对约束，thr2＝0.15m表示高度的相对约束。

四、离散化和栅格化

具体实施方法如下：根据指定的横向分辨率a和纵向分辨率b，创建栅格地图，如4所示，图中a表示检测到的路面部分；进一步将障碍物对应的点离散化到每个栅格中，离散化后的结果如图5所示，图中b表示检测到的障碍物部分。

从图5可以很明显看出，利用本发明方法，能够实现多线激光雷达的障碍物检测并输出正确结果。

所述的内点是指待选路面点；内点点云为激光雷达扫描到的路面点点云数据；点云指激光雷达扫描的数据；点云簇指由多个点云组成。

上述参照实施例对该一种基于多线激光的扫描式障碍物检测方法进行的详细描述，是说明性的而不是限定性的，可按照所限定范围列举出若干个实施例，因此在不脱离本发明总体构思下的变化和修改，应属本发明的保护范围之内。