基于三维激光雷达的动态障碍物检测和追踪方法＊

邹斌 刘康 王科未

（武汉理工大学，现代汽车零部件技术湖北省重点实验室 汽车零部件技术湖北省协同创新中心，武汉 430070）

基于三维激光雷达的动态障碍物检测和追踪方法＊

邹斌 刘康 王科未

（武汉理工大学，现代汽车零部件技术湖北省重点实验室 汽车零部件技术湖北省协同创新中心，武汉 430070）

为解决无人驾驶车辆在城市路况下对多个动态障碍物同时检测和跟踪的关键问题，提出一种基于三维激光雷达的多目标实时检测和跟踪方法。通过对单帧激光雷达点云数据进行聚类，提取障碍物外接矩形轮廓特征；采用多假设跟踪模型（MHT）算法对连续两帧的障碍物信息进行数据关联；利用卡尔曼滤波算法对动态障碍物进行连续地预测和跟踪。试验结果表明，该算法能够在自行搭建的智能车平台上以每帧100 ms的速度准确、稳定地检测和跟踪。

1 前言

利用激光雷达对动态障碍物进行检测和轨迹预测是无人驾驶课题的研究重点之一[1]。近年来，国内外学者在此课题中主要采用两种方法。一种方法是栅格地图法。周俊静等[2]建立栅格地图，利用贝叶斯推理推测由于传感器误差造成栅格状态的不确定性，通过比较当前时刻和随时间累积形成的栅格状态的不一致性检测运动目标。Trung-Dung Vu等[3]也利用该方法对运动目标进行检测跟踪，但是相比于文献[2]，没有对更新后的后验概率进行修正。文献[4]和文献[5]均采用了类似方法。以上方法可以较为准确地检测到运动的栅格，但仍存在缺陷，如果运动目标体积较大，几个周期内栅格状态一直保持“占据”,容易误判为静态障碍物。另一种方法是对原始数据进行分割、聚类，提取障碍物形态特征，对每帧的障碍物进行数据关联，从而对其进行跟踪。斯坦福大学Anna Petrovskaya等[6]提取被跟踪车辆的几何、动态特性，建立相应特征模型，利用贝叶斯滤波器对跟踪目标进行更新，但是被遮挡的车辆仍无法检测。文献[7]通过建立二维虚拟扫描模型较好地解决了这一问题。然而，两种方法都无法识别行人、自行车等运动障碍物。杨飞等[8]利用模板匹配法对障碍物的外轮廓形状特征进行匹配，但模型类别较少，障碍物匹配的适应性较差[9]。

本文以Velodyne 16线三维激光雷达作为主要传感器，运用DBSCAN（Density-Based Spatial Clustering of Applications with Noise）方法，并结合可变阈值，对原始数据进行聚类，大大提高了聚类的准确性，简化应用了模糊线段法准确提取动态障碍物的矩形轮廓，最后应用多目标假设跟踪（Multiple Hypothesis Tracking，MHT）方法以及卡尔曼滤波器对目标进行跟踪。

2 障碍物检测

2.1 有效障碍点的筛选

由于激光雷达每次采集障碍点时视角不同，采集的部分障碍点坐标变化较大，而且很多障碍点与障碍物的跟踪不相关，如路面、树叶、墙体、高楼等。障碍点过多会影响外围矩形轮廓的提取，因此有必要对原始数据进行筛选。

首先建立感兴趣空间区域，将此空间以外的点剔除。图1为三维激光雷达原始数据，坐标（0,0）为激光雷达所在位置，本文选取的高度范围为0.5～2 m，x轴方向范围为0～25 m，y轴方向范围为-10～10 m。筛选结果如图2所示，经筛选，剔除了树枝、地面等不在感兴趣区域内的障碍点，只保留了障碍物大致的轮廓，有利于提取车辆外围矩形。

图1 三维激光雷达原始数据

图2 障碍点筛选结果

2.2 特征提取

为了提取障碍物的外围矩形轮廓，有必要对障碍物点进行聚类，本文采用DBSCAN算法，该算法利用密度进行聚类，即要求聚类空间中一定区域内所包含对象（点或其他空间对象）的数目不小于给定阈值，计算速度快且能够有效处理噪声点和发现任意形状的空间聚类。为减少DBSCAN聚类算法的运算量、提高实时性，先对所有的障碍点进行预聚类。具体做法为：在x轴方向依据点与点的距离差值聚类，在此基础上再在y轴方向根据点与点的距离差值聚类。最后输出障碍点数聚类集合m={m1,m2,m3,…,mn}。然后对输出数据集中的每个障碍点集合分别应用DBSCAN聚类算法，对障碍点进行进一步分类。DBSCAN聚类算法需要事先输入过滤噪声的核心点领域距离阈值Eps以及核心点邻域范围内最少点个数的阈值MinPts。这两个值如果过大，多个障碍物易被误分为同一个障碍物；相反，则同一障碍物有可能被误分为若干障碍物。由于激光雷达采集的障碍点的密度随激光点深度的变大而变小，所以Eps、MinPts应随激光点深度而变化。本文采用自适应阈值的方法：

式中，rn-1为障碍点Pn-1的深度值；σr为激光雷达的测量误差；Δϕ为激光雷达的角度分辨率，本文取Δϕ=0.2°；γ为阈值参数，它决定了最大距离阈值的大小，本文取γ=1°；NT为预聚类中mi中障碍点的个数；∂为阈值因数，由经验获得，本文取∂=5。

图3为障碍点的预聚类结果。结果显示不同障碍物误识别为同一个障碍物。这是由于两车体倾斜，障碍点投影到x轴后，其x坐标相差较小，障碍点投影到y轴后，其y坐标相差也较小。

图3 预聚类结果

图4为应用DBSCAN算法后的聚类结果，结果显示，该算法可将预聚类误识别同一障碍物的不同障碍物分开，同时可将长度过长，面积过小，点密度太稀疏的障碍物剔除，它们可能是墙壁、树叶、树干等静态障碍物。

图4 DBSCAN聚类结果

本文中提取的障碍物主要特征有跟踪点位置Q（x,y）、运动方向与x轴的偏离角度θ、长度L、宽度W。提取包罗障碍物的最小外接矩形框上距原点最近的顶点作为Q(x,y)，因为此点随雷达视角的变化一般较小，矩形框的最长边线与x轴的夹角作为障碍物角度信息θ。本文选用最小凸包法结合模糊线段的方法提取障碍物的包罗矩形框。首先，利用Graham扫描提取凸包点，然后利用简化的模糊线段法[10]提取障碍物的矩形边框。具体步骤如下（见图5）：

a.选取距起点（如图中P0）的距离大于一定阈值的最近点（如图中P2）。

b.计算起点P0、P2两点确定的直线的斜率Kg1，且P0和P2之间的点全部属于G1（图5中只存在点P1）。

c.计算下一个点（如图中P3）与P0确定的直线的斜率Kg2，令θ=arctan(Kg1-Kg2)，如果|θ|大于一定的阈值，则G1组关闭，以G1中最后一个点为起点建立G2。

d.在|θ|小于一定的阈值的情况下，向量P0P2、P0P3相加得到PP′，过PP′两侧距离最远的点P3、P2，分别作PP′的平行线，计算两平行线的距离η，如果η大于设定的阈值ηtr，则P3∈G1，否则G1关闭，以G1中最后一个点为起点建立G2。

e.若步骤c、d中有新起点建立，则循环从步骤a开始，否则循环从步骤c开始，直到计算完成最后一个点，输出G1、G2、……、Gx。然后对每组内的点运用最小二乘求得最佳斜率，作出每组点的直线，选取距离最长、邻域超过一定数量点的直线作边线，作出障碍点的边框。

图5 模糊线段提取

在图5中，很显然P4与P0～P3不属于同一组，P4为G2的起点。P0～P4为最小凸包点，经过判定，P0～P3为同一线段上的点，P4将被剔除，不作为线段上的点。

求得的障碍物外接边框如图6所示。本文可以提取任何角度的矩形框，对于行人等目标较小且激光点较为密集的障碍物集合，矩形框与x轴的角度设为0°。单帧图像的特征提取结果如图7所示。

3 障碍物跟踪

为了跟踪障碍物目标并预测其运动轨迹，本文首先采用MHT方法对相邻两帧之间的障碍物进行数据关联，由于目标的位置存在一定的噪声（如同一个障碍物跟踪点位置发生偏移），本文采用卡尔曼滤波器对目标障碍物的位置和速度进行滤波和预测，算法流程如图8所示。

图6 单个障碍物轮廓提取结果

图7 单帧特征提取结果

图8 障碍物跟踪算法流程

3.1 数据关联

表1列举了常用的多目标数据关联算法，并列出了各算法的优缺点。

显然NN法无法适应自主驾驶车辆周围的复杂环境，JPDA法不需要任何目标和杂波的先验信息，较为适合解决障碍点密集情况下的数据关联问题。自主驾驶车辆周围障碍物间距离较远，对雷达点云进行数据处理后存在大量的特征信息，从之前帧的匹配概率可得到大量杂波的先验信息，因此MHT法是最佳选择。

表1 数据关联算法对比

建立每个障碍物的几何特征列表。具体信息包括：障碍物矩形的顶点坐标Q(x,y)、宽度W、长度L、矩形边界与x轴的夹角θ。这是利用MHT进行多障碍物数据关联的前提，进而根据障碍物之间的几何特征建立关联关系。

首先介绍本文的数据关联算法。{Tj}（j=1,…,M）表示前一帧跟踪的障碍物，{Zi}（i=1,…,N）为当前帧检测的障碍物。根据实际状况，可以分为3种情况（见图9）：已经跟踪的障碍物逐渐或突然消失，当前帧没有与之关联的任何障碍物，可以表示为{TjN}；新障碍物突然或者逐渐进入雷达搜索区域，之前帧没有障碍物与之关联，可以表示为{ZiN}；当前帧某个障碍物与之前跟踪的障碍物形成关联，表示为{Y(Tj,Zi)}。图9中圆形区域表示障碍物可能出现的位置，圆形半径为SR，本文取SR=3 m。图中障碍物T1、T2、T3表示经过卡尔曼滤波器预测后的障碍物位置，并继承之前帧的其他几何特征，Z1、Z2、Z3为当前帧出现的障碍物，那么根据上述3种情况，可以形成一系列假设：

假设在Hk中，P(Y(Tj,Zi))表示成功关联的概率，P(TjN)表示之前帧障碍物无关联的概率，P(ZiN)表示当前帧障碍物无关联的概率。P(Y(Tj,Zi))是以下4个关联特征匹配概率的联合概率：两障碍物存在重叠面积的概率Pcov(Tj,Zi)；中心点的匹配概率Pcenter(Tj,Zi)；障碍物长、宽的匹配概率PWL(Tj,Zi)；障碍物倾斜角度的匹配概率Pratio(Tj,Zi)。

图9 目标匹配过程

根据图9出现的情况，可以用矩阵表示两帧之间关联的概率：

圆形区域内障碍物由于存在一定的面积，如果两个障碍物的面积有重叠部分，那么其为相邻两帧同一个障碍物的概率较大，本文判别是否存在重叠面积的条件为：

式中，Vi1为其中一个矩形的顶点，Vi1、Vi2、Vi3、Vi4依次为逆时针排列的矩形顶点；Gjx为另一个矩形的某个顶点。

各概率的计算公式分别为：

式中，Sij为可能匹配的两障碍物跟踪点之间的距离；WiO、LiO分别为滤波器预测障碍物的宽度和长度；Wi、Li分别为当前帧中可能与滤波器预测障碍物匹配的障碍物的宽度和长度；KiO为滤波器预测障碍物矩形边界与x方向的夹角；Ki为当前帧中可能与之匹配的障碍物矩形边界与x方向的夹角。

障碍物Tj、Zi成功匹配的概率表示为：

下面对各种假设进行处理，选取最优假设，由于障碍物数量较少，可以依次计算各种假设Hk的出现概率。假设Hk出现的概率为：

式中通过函数argmax选取概率最大的假设。以上算法在实车中运行得到的连续3帧的判别结果如图10所示。判别结果显示，公路上1号和6号障碍物一直被跟踪，3号障碍物突然消失，在第35帧和第36帧，8号、10号障碍物分别突然出现，跟踪效果明显。此算法能够同时检测障碍物的突然出现和消失。

3.2 滤波器预测与更新

受测量噪声和自主驾驶车辆车体自身运动的影响，存在很大的测量误差。为了解决目标跟踪中测量噪声引起的目标运动轨迹的大幅变动对预测目标运动轨迹造成的影响，以及在障碍物关联中对于预测目标下一帧位置的需要，有必要采用卡尔曼滤波器[11]对目标的位置进行滤波和预测。为了预测障碍物下一帧的位置，给出状态向量（被估计的物理量）：X=[x,y,vx,vy]，vx、vy分别为障碍物在x、y方向上的速度分量。针对此问题，状态转换矩阵为：

预测的状态方程为：

测量转换矩阵为：

预测状态量和预测最小均方差矩阵方程分别为：

图10 判别结果

计算误差增益、最优估计状态量方程和均方差误差更新矩阵分别为：

可以通过R、Q的大小调节观测值的可信度。Q值越大，预测结果越接近预测值，R值越大，预测结果越接近观测值。

4 试验结果

首先分析单帧雷达数据，利用DBSCAN、模糊线段等算法，准确提取了障碍物的几何特征，与文献[1]、文献[3]和文献[8]中的算法相比，改进的自适应阈值DB⁃SCAN算法解决了由于雷达数据分布不均造成的聚类不准确等问题，经过大量的试验测试，正确率达到98.2%；模糊线段与最小凸包相结合的方法，解决了由于模板匹配造成的障碍物角度匹配精度不高的问题。基于两帧提取的障碍物特征，采用多假设跟踪算法进行数据关联，关联正确率达到99%。将本文提出的算法应用在武汉理工大学智能车团队搭建的实时运算平台上，试验结果表明，该算法能够以每帧100 ms的速度实现准确、稳定的检测和跟踪多个动态障碍物。该平台硬件配置为INTEL 8核3.4 GHz处理器，内存16 GB，软件采用Ubuntu下的ROS机器人实时操作系统。图11为自行搭建的无人车平台。

图11 无人自主车

图12为摄像头拍摄实景，图13为文中算法实时连续跟踪和检测动态障碍物25帧的结果。工况为校园路况，图中同时跟踪车辆和行人，目标车辆正在远离无人车，行人正在接近无人车，车辆表示为较大的矩形，行人表示为较小的矩形，图中显示障碍物在第2、12、22帧时刻的位置。“+”表示障碍物每帧跟踪点的位置坐标，折线表示经过卡尔曼滤波器滤波后的运动轨迹，可以观察出，未经滤波，跟踪点变动较大，经过卡尔曼器滤波后轨迹曲线趋于平滑。

由图13可知，雷达投射到车辆上的激光点分布不均匀，自适应阈值的DBSCAN算法可以对障碍点进行准确聚类，目标车辆在经过周围停放车辆时，虽然相距较近，本文聚类算法也可以准确分辨目标车辆。随着目标车辆的远离，激光雷达投射到车上的激光点逐渐减少，最终导致目标车辆的轮廓逐渐变小，如图中第22帧检测到的目标车辆，但并不影响对目标车辆的跟踪。

图12 摄像头拍摄实景

图13 雷达跟踪结果

从图13可知，目标车辆与x轴方向并不是平行的，本文所采用的模糊线段与最小凸包相结合的算法可以精确地提取到这一特征，并且车辆外接矩形框的方向也与目标车辆运动方向相符。图中可以观察出较长一段行人运动轨迹附近没有出现“+”，由于测量误差以及树叶的遮挡，对目标行人的检测突然“消失”，本文中MHT算法继续保持障碍物丢失前的特征信息，并利用卡尔曼滤波器继续跟踪，结果显示，恢复了对目标行人的跟踪状态。

本文通过所跟踪车辆的航向角和速度来体现跟踪效果的稳定性和准确性。将使用毫米波雷达的跟踪效果与本文算法的跟踪效果进行对比，速度跟踪对比结果如图14所示，图中同时检测了车辆和行人的速度。运动车辆行进约20 m，车辆在超车过程中速度变慢，之后速度持续加快，由曲线可以看出，本文算法对于障碍物速度检测与毫米波雷达所检测的速度最大相差0.432 m/s，验证了该算法对速度跟踪的准确性。

图15为运动车辆航向角跟踪对比结果，将从障碍点特征提取的航向角与跟踪算法计算的航向角融合，使用卡尔曼滤波器进行滤波，并与图中毫米波雷达检测到的航向角进行对比，由于传感器误差的存在，单纯从分析障碍点特征获取的航向角变化较为剧烈，但经过卡尔曼滤波器后，输出的航向角度与毫米波雷达检测的角度基本一致。两传感器测量值最大相差0.32°，验证了本文算法对航向角跟踪的准确性。

图14 速度对比结果

图15 航向角对比结果

5 结束语

本文基于单帧激光雷达点云数据提取障碍物的几何特征，采用多假设跟踪模型（MHT）、卡尔曼滤波算法对动态障碍物进行预测跟踪，通过试验验证了本算法的实用性与稳定性。但是本算法尚存在一些不足：在障碍物距离自主驾驶车辆较远时，由于障碍点数量不足，不仅会造成障碍物的分裂，而且提取的障碍物面积尺寸变化较为明显，带来了数据关联上的误差；无人车在行驶过程中，由于地面不平，激光雷达的抖动会影响聚类效果，进而影响几何特征的提取；当行人距静态障碍物较近时，容易将行人判别为静态障碍物。因此，本算法还尚需完善。

1 Martinez L,Paulik M,Krishnan M,et al.Map-based lane identification and prediction for autonomous vehicles.IEEE International Conference on Electro/information Technology，Milwaukee，2014.

2 周俊静，段建民.基于栅格地图的智能车辆运动目标检测.系统工程与电子技术，2015（2）：436～442.

3 Baig Q,Vu T D,Aycard O.Online Localization and Mapping with Moving Object Tracking in Dynamic Outdoor Environments.IEEE IntelligentVehicles Symposium，Xi’an，2009.

4 Bouzouraa M E,Hofmann U.Fusion of occupancy grid mapping and model based object tracking for driver assistance systems using laser and radar sensors.IEEE Intelligent Vehicles Symposium，La Jolla，2010.

5 Konrad M,Szczot M,Schüle F,et al.Generic grid mapping for road course estimation.IEEE Intelligent Vehicles Symposium，Baden-Baden，2011.

6 Petrovskaya A,Thrun S.Model based vehicle detection and tracking for autonomous urban driving.Kluwer Academic Publishers,2009.

7 Chen T,Dai B,Liu D,et al.Likelihood-Field-Model-Based Dynamic Vehicle Detection with Velodyne.IEEE International Conference on Intelligent Human-Machine Systems and Cybernetics，Hangzhou，2015.

8 杨飞，朱株，龚小谨，等.基于三维激光雷达的动态障碍实时检测与跟踪.浙江大学学报：工学版，2012，46（9）：1565～1571.

9 Broggi A,Cerri P,Ghidoni S,et al.A New Approach to Urban Pedestrian Detection for Automatic Braking.IEEE Transactions on Intelligent Transportation Systems，2009，10（4）：594～605.

10 Debled-Rennesson I,Feschet F,Rouyer-Degli J.Optimal blurred segments decomposition of noisy shapes in linear time.Computers&Graphics，2006，30（1）：30～36.

11 刘健.基于三维激光雷达的无人驾驶车辆环境建模关键技术研究：[学位论文].合肥：中国科学技术大学，2016.

（责任编辑 斛 畔）

修改稿收到日期为2017年6月22日。

Dynamic Obstacle Detection and Tracking Method Based on 3D LiDAR

Zou Bin,Liu Kang,Wang Kewei
（Hubei Key Laboratory of Advanced Technology for Automotive Components,Hubei Collaborative Innovation Center for Automotive Components Technology,Wuhan University of Technology,Wuhan 430070）

In order to solve the key problem of simultaneous detection and tracking of unmanned vehicles in urban traffic conditions,this paper presented a method based on 3D LiDAR for simultaneous detection and tracking of multitargets.By clustering the single-frame LiDAR point cloud data,external contour features of obstacles were extracted.Then,the Multi-Hypothetical Tracking model（MHT）algorithm was used to correlate the obstacle information of two consecutive frames.Finally,the Kalman filter algorithm was used to continuously predict and track the dynamic obstacles.Test results show that this algorithm can detect and track multi-target at 100 ms/frame speed on the self-built intelligent vehicle platform.

3D LiDAR,DBSCAN,MHT,Kalman filter

三维激光雷达 DBSCAN MHT 卡尔曼滤波

TP181；U495；TP14 文献标识码：A 文章编号：1000-3703（2017）08-0019-07

湖北省科技厅资助项目（2016BEC116）。