一种新的非结构化环境可通行区域检测算法∗

闫 明 蔡云飞 陈 盟

（南京理工大学模式识别与人工智能实验室 南京 210094）

1 引言

地面无人平台技术作为国家“十三五”重点研究方向，目前正得到广泛的研究。技术包括环境感知与理解、定位定向、自主规划、决策控制和多机协同等关键技术，其中环境感知与理解是目前研究的重点和难点。对环境信息的获取，是无人平台技术实现的基础，目前地面无人平台主要的环境感知手段分为图像和距离两大部分，图像感知包括可见光图像和红外图像两种，主要用来对环境中的颜色、纹理、边缘、外形、符号等进行感知识别，对视野目标距离不敏感。距离感知借助于激光雷达、毫米波雷达、声呐等传感手段实现，其属于“盲人摸象”。其中激光雷达凭借其探测距离远、精度高等优点，得到广泛使用，激光雷达主要是利用光的收发时间差来获得被反射物体的相对方位角和距离，根据激光束发射扇面的数量，分为单线（SICK LMS511、LMS111、Hokuyo 10L 等）、4/8 线（IBEO 4L、8L）和16/32/40/64 线（Velodyne、禾赛等）等种类。水平方向每个扫描扇面由不同角度的扫描点组成，通常称为一根线。

目前用于可通行道路识别的传感器主要有摄像头和激光雷达。摄像头具有成本低，数据（图像）直观的优点，但极易受到光照、天气等外界环境条件的影响，稳定性差。激光雷达具有探测距离远，精度高和不受光照条件影响等优点。

早期采用的单线激光雷达采用单平面扫描模式，获得的环境轮廓信息极为有限，而且受地形和颠簸影响，容易“看天看地”，不适用于非结构化环境。近年来，随着多线激光雷达的发展，通过三维空间的密集点云，可获得较为丰富的环境信息，对环境的轮廓具有较好的描述特性。

地面无人平台通行环境通常分为结构化、半结构化和非结构化道路三种。

图1 三种典型环境示意图

结构化道路是指边缘规则，路面平坦，有明显车道线及其他人工标记的行车道路，例如高速公路、市区道路、国道、省道等。半结构化道路是指无明显道路边缘特征和交通标示特征的平坦道路环境，如乡间水泥路、柏油路等；非结构道路是泛指无明显道路边缘特征，非平坦路面的野外环境。在结构化、半结构化道路中，无人平台所处路面较为平整，道路边缘较为清晰，道路中的人、牲畜和车等障碍较容易检测，可通行区域识别难道较小。在非结构化环境（越野环境）中，没有普通道路特征，而且地形起伏较大，车辆颠簸厉害。此时采用基于地平面绝对坐标的普通道路边界检测方法不具有可行性，漏检和误检率都很高，无法正常使用。目前，基于相对参照系的密集3D点云特征可以较好地解决该问题。

目前流行的32/64线雷达通常安装于平台顶部前侧，受垂直扫描角度的限制，会在车辆正前方留下一片较大的盲区，而且盲区范围随着车体积和安装高度的增加而增加，不利于体积庞大的军用无人平台。项目组通过对4 线激光雷达的使用研究，安装车头部位，可较好地提供近场环境描述功能，可有效弥补盲区，甚至在一些特定场景中，可以完全取代车顶雷达。

基于以上的认识，开展非结构化环境越野自主车辆前方可通行区域检测问题的研究。首先通过点云分布和排列特征查找具有相对平面扫描特性的较规则的分布点云带，提取并拟合近似平行线段集，对区域边界进行道路假设和拟合，根据车辆连续运行时间轴的上下文关系对边界进行滤波平滑和修补，利用点云聚类特征检测区域内障碍物，将区域边界与障碍物边界进行融合，最终得到安全的可通行区域。使用效果验证了算法具有较的高鲁棒性，并满足实时性要求。

2 相关工作

可通行区域检测问题一直是非结构化环境无人平台研究的难点问题，也是基础问题。目前基于激光雷达的可通行区域检测研究主要分为两类。

1）提取道路边沿的障碍物（如马路路牙等）上的扫描点作为道路边界点，使用直线拟合道路边沿，得到道路范围，并检测道路内的障碍物确定可通行区域。文献［1］提出了一种基于DBSCAN（Density Based Spatial Clustering of Applications with Noise）的可通行区域检测算法，通过聚类得到道路边沿与障碍物，并使用最小二乘拟合道路边沿直线［2］。文献［3～4］使用点到直线的阈值拟合线段分割扫描点，然后在线段范围内使用RANSAC（Random Sample Consensus）拟合地平面。文献［5～7］利用设定边缘高度阈值的方法寻找道路边缘点，并使用最小二乘法拟合道路边缘直线。使用该类方法的优点在于计算简单，实时性高。但在非结构化环境下，道路两侧边沿障碍物不连续或没有障碍物，使用该方法极易造成误检。

2）基于点云平面分割的方法。文献［8］中提出了一种基于AMN（Associative Markov Network）的三维点云分割算法。算法提取点云相关特征［9］使用SVM 分类器训练后构造AMN 实现点云分割，检测可通行区域。文献［10～11］采用构建点云图，使用局部凸准则的方法实现图分割，得到道路与障碍物位置信息。文献［12～13］中使用极坐标栅格图表示点云位置，并使用高斯过程回归实现道路分割，得到道路区域与障碍物区域。文献［14］中使用检测点云在地面与障碍物之间粗糙度的差异实现道路分割。该类方法的优点在于最大限度利用了点云分布信息，检测结果相对准确。但是其中涉及复杂的迭代计算，并且为准确得到分割结果需使用大量点云数据，难以满足实时性要求。

两类方法均涉及基于地平面的绝对高度计算，由于车辆在非结构化环境中行驶过程中存在高频率、大幅度震动，对高度计算影响较大，因此上述方法并不适用于非结构化道路。

3 架构分析与设计

3.1 安装分析

根据项目组经验，四线激光雷达根据具体任务的不同，安装方式以俯仰角区分主要分为两种:俯仰角为0°的水平安装方式与俯仰角大于1.6°的俯视安装方式。

雷达水平安装方法［15］如图2（a）所示，四条扫描线其中两条垂直方向扫描角度大于0°，用来检测前方障碍物信息，另外两条扫描线垂直扫描角度小于0°，用来获取近距离障碍物及道路边界信息。此方法的优势在于可以同时兼顾雷达探测距离远和近距离高分辨检测的优点。考虑检测的实际障碍物高度并结合实际车辆高度，垂直地面高度不宜超过50cm，否则会产生较大近距离障碍物检测盲区。由于仅有两条扫描线获取道路边界信息，信息量较少，复杂地形中可靠性较差。在非结构化环境中，地面环境非常复杂，地势起伏较大，颠簸厉害，极小的幅度就会引起激光波速“上天入地”，无法扫描到地面获取道路信息。因此水平安装方式仅适合在结构化道路环境中使用［3，16］。

图2 雷达安装分析

雷达俯视安装方法，如图2（b）所示，高位安装，四条扫描线全部扫向近距离地面，获取尽可能多的地面信息。考虑到非结构化环境中车辆通行速度的制约，系统反应时间比较充裕，雷达探测距离可调小。通过增加安装高度，增加视野；增大俯仰角，提高鲁棒性。俯视安装方式时安装高度一般大于60cm。俯视安装方式优点在于相比水平安装方式，得到的道路信息更加丰富，并且受车辆颠簸的影响小，适合在非结构化环境中使用。俯视安装方式探测距离受限于雷达安装高度和俯仰角，对于高度较小的障碍物会存在一定范围的盲区。

本文采用俯视安装方式。右手车体坐标系为以车辆后轴中心为原点，Zc方向垂直地面向上，Yc方向为车辆前进方向，Xc方向为水平向右方向。四线激光雷达安装在车头，设其在车体坐标系下的位置为(xc,yc,zc)，则安装位置满足 xc=0，理想情况下激光雷达相对车体坐标系的横滚角与偏航角均为0。h 表示四线激光雷达相对地面安装高度，α 表示雷达安装俯仰角，αi表示第i 条扫描线相对水平面的俯仰角，di表示第i 条扫描线到雷达的水平距离。雷达垂直方向角分辨率为0.8°，各参数满足如下关系式:

其中激光雷达最上层扫描线为第1 层，从上到下序号依次递增。

3.2 姿态标定

定义雷达坐标系为以四条扫描线中间平面为xoy 平面，水平向前为Yl方向，水平向右为 Xl方向。Zl方向垂直xoy 平面向上，满足右手坐标系。从上到下四条扫描线相对xoy 平面的俯仰角满足:

αi=(1.2°,0.4°,-0.4°,-1.2°)i=1,2,3,4 （3）

设水平方向扫描角度为θ ，扫描点距离为 ρ，则扫描点在雷达坐标系下的位置可用下式表示:

扫描点从雷达坐标系到车体坐标系的转换关系可表示为

其中R3×3表示雷达坐标系到车体坐标系的旋转矩阵，T3×1为平移向量。

4 可通行区域检测算法

可通行区域定义:平台在周边360°视野范围内，根据自身的控制特性和外轮廓限制，在无破坏性碰擦的情况下，运动可达并可安全通过的区域。

4.1 算法概述

四线激光雷达在路面上的扫描点近似四条与车体坐标系X 轴平行的线段，通过检测点云中的直线段的方式得到当前行驶区域范围。在扫描范围内激光雷达不同扫描线在障碍物上的相邻扫描点之间距离与在地面上的距离存在明显差异，可采用基于欧式距离聚类算法检测行驶区域范围内的障碍物。

可通行区域检测算法分为以下五个步骤:

1）撕裂点检测:将三维雷达点云投影到XY 二维平面，使用霍夫变换检测激光雷达点云图中的直线，并根据撕裂点位置提取扫描在路面上的点云线段，得到线段两端点假设为区域边界点。

2）边界描述:采用卡尔曼滤波算法对区域边界点位置进行滤波估计，然后使用最小二乘法拟合可通行区域边界线，估计当前可通行区域范围。

3）障碍物检测:采用基于欧式距离聚类的算法实现行驶区域范围内的障碍物检测。

4）区域融合:融合估测范围边界与障碍物区域。

5）可达性评估:根据车辆运动模型，评估车辆的可达性，得到最终的可通行区域。

4.2 撕裂点检测

将三维点云投影到XOY平面上，投影后的二维扫描点坐标可表示为(xij,yij),i=1,2,3,4, j=1,…,Ni，Ni表示每条扫描线上的点云数量。使用霍夫变换获得每条扫描线上的直线方程。

由于非结构化环境复杂性，对于在直线上的扫描点，检测其在直线段上的端点。采用点云分割方法，设置相邻点之间距离阈值，计算相邻点的距离，大于阈值的相邻点为点云的撕裂点。如图3 所示，从线段两侧分别检测，若从某一点开始连续n 个点不存在撕裂点，则该点为需提取的线段端点，并标记为区域边界点，在理想情况下两侧分别有4 个边界点。但是在非结构化环境中，由于地面环境复杂和车辆颠簸等因素，最上层扫描线得到的扫描结果与实际的道路环境差异较大，本文取下三层扫描线检测结果进行区域边界拟合。

图3 撕裂点检测结果示意图

4.3 边界描述

在理想情况下，车辆行进过程中通过相邻两帧点云数据检测到每条扫描线的边界点位置基本不变或差异很小。而在非结构化环境中，由于车辆颠簸等因素的影响，激光雷达获取的相邻帧点云数据之间可能存在较大差异，造成前后帧边界点位置提取差异较大。假设车辆为直线行进，边界点位置变化为线性变化，使用卡尔曼滤波器对边界点位置进行估计、观测和校正，减小波动性和误差。

卡尔曼滤波是一种高效递归滤波器，可以根据目标的动态信息，减小测量中噪声的影响，得到相对精确的当前目标位置状态。卡尔曼滤波过程包含预测与更新两个阶段。卡尔曼滤波递归计算方程如下。

预测阶段:

更新阶段:

其中 xˆk-1|k-1表示 k-1 时刻的目标状态，xˆk|k-1表示由k-1时刻的状态对k时刻状态的预测值，Fk为k时刻的状态转移矩阵，Bk为k 时刻的输入转移矩阵，uk为k 时刻的输入向量，R 为k 时刻的测量噪声方差，Q 为k时刻过程激励噪声协方差。 Kk为k时刻的卡尔曼增益，Hk为k 时刻的测量转移矩阵，zk为该时刻目标测量值。

在本文中，对于边界点坐标(x,y)中任一状态x 或y，问题可化简为一维卡尔曼滤波，状态转移矩阵Fk，测量转移矩阵Hk均为单位矩阵。在本系统中，由于没有外界输入因素，输入向量uk为0。测量噪声方差R 为一维常量，可通过激光雷达对同一物体多次扫描得到的观测值计算得到。对过程激励噪声协方差Q ，本文在多种不同场景多次实验的基础上求取经验值。本文卡尔曼递归计算方程如下所示。

预测阶段:

更新阶段:

考虑到算法的实时性，方法采用最小二乘法拟合边界。设直线方程为 y=bx+k ，则最小二乘法计算如下所示:

将左右两侧滤波后得到的边界点带入可得到左右两侧的直线方程，该直线即为当前可通行区域边界。

4.4 障碍物检测

雷达在俯视安装状态下，障碍物检测如图4（a）所示:

图4 障碍物检测

在前方有障碍物的情况下，do表示扫描到障碍物上的相邻两条扫描线之间的距离，dg表示如果没有障碍物遮挡的情况下，相邻两条扫描线之间的距离。α0,α1分别为第一层与第二层扫描线相对水平面的俯仰角，d 表示障碍物与雷达的水平距离。图中距离关系可由下式表示:

在本文雷达安装方式下，距离关系满足do≪ dg。

设不同扫描线之间的相邻水平扫描角度的扫描点之间的距离阈值为dT1，遍历中间两条扫描线，如图5 所示，计算点与相邻扫描线邻近水平方向扫描角度点的距离dn，若dn＜dT1，则将该点加入障碍物点云队列Q。设置距离阈值dT2，从队列Q 中依次取出点云数据，计算点云数据到点云集合Ck几何中心的距离dc，若满足dc＜dT2，则将该点加入点云集合中，否则取当前点云集合Ck为一个聚类簇。遍历直到队列Q 为空，得到聚类簇划分C={C1,C2,…,Ck} 。具体实现如下。

图5 不同扫描线相邻点之间的距离关系

Di中的点pij的相邻扫描点

计算pij与相邻扫描点之间的距离dn= ||pij-

if dn＜dT1then

将 pij与加入队列Q

end if

end for

end for

初始化k=0

设置聚类点集合S=Ø

while Q ≠Ø

从队列Q中取出一个扫描点q

if S ≠Ø then 计算q与S几何中心sc距离dc

if dc＜dT2then

S=S ∪q

update sc

else

得到一个聚类簇Ck=S

K=k+1

end if

else

S=S ∪q

S几何中心sc=q

end if

end while

4.5 区域融合

本文将区域边界与障碍物区域融合得到当前车辆行驶的可通行区域。如图6 所示，点O 为雷达坐标系原点，对于左侧边界，A1表示左侧边界与雷达坐标系X 轴交点，A2,A3,A4表示激光雷达扫描到的障碍物边界顶点，A5为线段OA4延长线与左侧边界的交点。由于激光雷达仅能得到障碍物在雷达一侧的轮廓信息，如图6 阴影区域所示，障碍物内侧区域为当前未知区域，须将其从可通行区域中去除。在之前得到边界结果的基础上，取A1～A5五个点围成的相对障碍物轮廓的包络线作为新的边界线，同理可得到新的右侧边界。两侧边界线围成的内部区域为当前车辆的可通行区域。

图6 区域融合示意图

4.6 可达性评估

对于车辆行驶位置 p(x,y)，其到区域边界与m个障碍物中心 poj( )xoj,yoj,j=0,1,…,m 距离需满足一定的空间关系，如式（12）所示，其中dc表示车宽，doj表示第 j 个障碍物宽度，ε 为车辆通行的安全余量。

图7 车辆转弯示意图

在满足空间要求的同时，车辆在通过弯道与避障情况下须考虑车辆实际转弯角度与转弯半径。如图7 示，其中l 表示车辆轴距，d 表示车辆轮距，车辆后轴中点为车体坐标系原点，φ 表示前轮转向角度，ρr，ρf分别为车辆后轮与前轮的转弯半径，其与轴距、转角关系满足式（13）。在本文实验车辆实际行驶过程中，出于安全性考虑，车辆最大转向角度 φ=35°。

如图8 示，车辆前方检测到两个障碍物，车辆需绕过障碍物到达目标点。障碍物A 左侧满足车辆空间限制，但不满足车辆转弯半径限制，因此车辆无法到达障碍物A 左侧区域。障碍物B 右侧区域满足车辆转弯半径限制，但区域宽度小于车宽，因此无法通行。

图8 车辆可达性示意图

5 实验结果及分析

5.1 实验环境

为验证本文算法的检测性能与实时性，该算法的实验环境如下:实验平台为轮式无人自主车，采用四线激光雷达型号为IBEO LUX2010 4L.雷达安装高度为65cm，前向俯仰角为5.03°。四线激光雷达型号为德国IBEO 公司生产的IBEO LUX2010 4L。该款激光雷达有四条扫描线，探测范围可达200m，距离分辨率4cm，扫描精度10cm。垂直视场角为3.2°，垂直角分辨率为0.8°。在水平方向上，扫描中心视场角为85°，两侧可扩展，最大视场角为110°。水平方向角分辨率为0.125°/0.25°/0.5°三挡可调，扫描频率可调节12.5/25.0/50.0Hz。

图9 实验平台

本文在多种不同的场景下进行实验，实验场景为典型的非结构化环境，包括无障碍物道路与存在车辆、行人等障碍物的区域场景。实验包括对于扫描线的直线检测，边界点检测，边界线拟合与边界内障碍物检测。实验场景与结果如图10～11所示。

图10 通行区域边界检测

图11 弯道边界检测结果

图10 、图11 为无障碍的非结构化场景实验。其中图10（a）为直线道路场景，中间高度略高于两侧，两侧存在土堆作为边界。图11（a）为弯道场景，左侧为杂草，右侧为土堆，为两侧行驶区域边界。两个场景下可行驶区域中间均不存在障碍物。图10（b）与图11（b）分别表示相应场景下霍夫变换检测直线、边界点与边界线检测结果，均为XOY 平面表示。

图12～14 表示存在障碍物的非结构化场景实验。其中图12 表示存在汽车的区域场景，图（a1）表示汽车在远距离时的场景，图（b1）表示汽车在近距离时的场景。两侧存在与路面高度存在明显差异的连续障碍物边界，路面较为平坦。图13 表示存在摩托车区域场景，图（a1）和图（b1）分别表示远距离与近距离场景，场景中边沿不连续且与路面没有明显高度差异，场景中路面起伏较大。图14 表示存在行人的区域场景，其中图（a1），图（b1）分别为远距离与近距离场景示意图，该路面较为平坦，两侧为断崖，高度明显低于路面高度，不存在常规意义上的道路边沿。

图12～14 中对应的图（a2），图（b2）分别为对应场景不同距离下的霍夫变换检测直线、区域边界点与区域边界线检测结果。图（a3）和图（b3）表示对应场景不同距离下的边界内障碍物检测结果，图（a4）和图（b4）表示区域融合后的结果。所有实验结果均在XOY平面表示。

5.2 结果分析

在图10（a）的场景中，由于行驶区域中间高度略高于两侧，因此在图10（b）中的点云图中，扫描线中间存在一定程度的凹陷。在霍夫变换检测结果可以看出，该程度的凹陷并未影响直线检测结果，并成功提取出了下三层扫描线上的区域边界点，拟合出符合实际的区域边界线。对于图11（a）的弯道场景，激光雷达在路面上的扫描线同样近似于X 轴的平行线。本文方法可以成功地提取每条扫描线上的区域边界点，拟合的区域边界线向右侧倾斜，符合右转弯道路实际场景。

对于图12（a1）场景，在图12（b1）中可以看出，左侧边界检测结果符合实际情况，右侧由于路边障碍物高度相对较低，存在相当数量的边界外扫描点，因此在点云图中有两条障碍物边界，两条边界之间存在无扫描点的位置区域。本文方法检测结果为内侧区域边界，符合实际情况。在图12（b2）中，对侧来车车头位置在第二条扫描线处，此时该位置存在相当数量的不同扫描线上的扫描点，但由于车身上位置上只存在第一条扫描线上的扫描点，本文方法此时只能检测到目前该车车头的位置。场景图12（b1）中，两车之间距离相对图12（a1）更近，对侧来车车头位置在第四条扫描线处，如图12（b3）所示，此时在车身位置也存在一定数量的不同扫描线的扫描点，本文算法可以检测车体大部分位置。但由于车身的遮挡，第一、二两条扫描线左侧一部分扫描点不能扫描到路面上，对边界检测结果产生了一定影响。如图12（b2）所示，左侧边界点发生较为明显的倾斜，原因在于第一、二两条扫描线长度明显较第三、四条扫描线短，对第二条扫描线检测的最左侧点并不是左侧边界点。本文中采用的卡尔曼滤波对边界点进行平滑，在一定程度上减小了扫描点与真实边界点的偏差，图中卡尔曼滤波后得到的区域边界点相比实际直线中最左侧扫描点更接近真实的边界点，采用该方法最大程度减小了由于障碍物遮挡带来的边界检测误差。

图12 障碍物场景1可通行区域检测结果

图13 障碍物场景2可通行区域检测结果

图14 障碍物场景3可通行区域检测结果

在图13（a1）的场景中，路面起伏较大，如图13（a2）所示，路面上的扫描线产生倾斜，边界检测结果也产生了一定程度的倾斜。实际场景中右侧存在不连续道路边界，本文算法由于只处理扫描到路面上的边界点，两侧障碍物边沿点对于检测结果没有影响，因此可以较好地处理实际场景中存在不连续边沿的情况。图13（a3）中，本文算法成功检测出了停靠在路边的障碍物。图13（b1）的场景中，车辆距离障碍物的距离更近，障碍物上的扫描点更密集，检测结果如图13（b3）所示。

在图14（a1）中，两侧不存在障碍物作为边沿，点云图如图14（a2）所示，两侧存在大片没有扫描点的区域。本文方法可以成功提取出区域两侧的边沿点以拟合边界沿线，并未受到两侧断崖的影响。在右侧边界附近存在行人，本文算法可以成功检测，结果如图14（a3）所示。图14（b1）场景中，行人与车辆距离更近，检测结果如图14（b3）所示。由于行人宽度较小，激光雷达扫描在其上的扫描点较少，因此其检测需要相对车辆更近的距离以保证有足够数量的扫描点，如图14（a3）所示，首次检测到的距离为10m，已经过第三条扫描线。

本文方法在 Cortex-A8 处理器，1GHz，256MB RAM 硬件环境下，单帧处理时间为25ms.IBEO-LUX-2010 4L 激光雷达扫描频率为25Hz，即40ms 一帧数据，因此本文方法可以满足实时性要求，并已成功应用在无人自主车上，参加了陆军装备部“跨越险阻2016”地面无人装备挑战赛。

6 结语

本文提出了一种基于四线激光雷达的非结构化环境可通行区域检测算法。通过点云分布和排列特征查找具有相对平面扫描特性的较规则分布点云带，提取并拟合多条近似平行线段，对平行线段的端点点集进行区域边界假设和拟合，根据车辆连续运行时间轴的上下文关系对边界进行滤波平滑和修补，利用点云聚类特征检测区域内障碍物，将边界与障碍物区域进行融合，最终得到安全的可通行区域。实验表明，该算法具有高可靠性与实时性，已成功运用于无人自主车上，能够满足无人自主车的感知需求。