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

为解决无人驾驶车辆在城市路况下对多个动态障碍物同时检测和跟踪的关键问题，提出一种基于三维激光雷达的多目标实时检测和跟踪方法。

通过对单帧激光雷达点云数据进行聚类，提取障碍物外接矩形轮廓特征；采用多假设跟踪模型（MHT）算法对连续两帧的障碍物信息进行数据关联；利用卡尔曼滤波算法对动态障碍物进行连续地预测和跟踪。

试验结果表明，该算法能够在自行搭建的智能车平台上以每帧 100 ms 的速度准确、稳定地检测和跟踪。

本文来自 2017 年 8 月 24 日出版的《 汽车技术 》，作者是武汉理工大学现代汽车零部件技术湖北省重点实验室的邹斌、刘康和王科未。


1. 前言

利用激光雷达对动态障碍物进行检测和轨迹预测是无人驾驶课题的研究重点之一 [1]。近年来，国内外学者在此课题中主要采用两种方法。

一种方法是栅格地图法。周俊静等 [2] 建立栅格地图，利用贝叶斯推理推测由于传感器误差造成栅格状态的不确定性，通过比较当前时刻和随时间累积形成的栅格状态的不一致性检测运动目标。Trung-Dung Vu等 [3] 也利用该方法对运动目标进行检测跟踪，但是相比于文献 [2]，没有对更新后的后验概率进行修正。文献 [4] 和文献 [5] 均采用了类似方法。以上方法可以较为准确地检测到运动的栅格，但仍存在缺陷，如果运动目标体积较大，几个周期内栅格状态一直保持「占据」,容易误判为静态障碍物。

另一种方法是对原始数据进行分割、聚类，提取障碍物形态特征，对每帧的障碍物进行数据关联，从而对其进行跟踪。斯坦福大学 Anna Petrovskaya 等 [6] 提取被跟踪车辆的几何、动态特性，建立相应特征模型，利用贝叶斯滤波器对跟踪目标进行更新，但是被遮挡的车辆仍无法检测。文献 [7] 通过建立二维虚拟扫描模型较好地解决了这一问题。

然而，两种方法都无法识别行人、自行车等运动障碍物。杨飞等 [8] 利用模板匹配法对障碍物的外轮廓形状特征进行匹配，但模型类别较少，障碍物匹配的适应性较差[9]。

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

2. 障碍物检测

2.1 有效障碍点的筛选

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



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



图 2 障碍点筛选结果

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

2.2 特征提取

为了提取障碍物的外围矩形轮廓，有必要对障碍物点进行聚类，本文采用 DBSCAN 算法，该算法利用密度进行聚类，即要求聚类空间中一定区域内所包含对象（点或其他空间对象）的数目不小于给定阈值，计算速度快且能够有效处理噪声点和发现任意形状的空间聚类。为减少 DBSCAN 聚类算法的运算量、提高实时性，先对所有的障碍点进行预聚类。

具体做法为：在 x 轴方向依据点与点的距离差值聚类，在此基础上再在 y 轴方向根据点与点的距离差值聚类。最后输出障碍点数聚类集合 m = {m_1, m_2, m_3,…, m_n}。然后对输出数据集中的每个障碍点集合分别应用 DBSCAN 聚类算法，对障碍点进行进一步分类。

DBSCAN 聚类算法需要事先输入过滤噪声的核心点领域距离阈值 Eps 以及核心点邻域范围内最少点个数的阈值 MinPts。这两个值如果过大，多个障碍物易被误分为同一个障碍物；相反，则同一障碍物有可能被误分为若干障碍物。由于激光雷达采集的障碍点的密度随激光点深度的变大而变小，所以 Eps、MinPts 应随激光点深度而变化。本文采用自适应阈值的方法：



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



图 3 预聚类结果

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



图 4 DBSCAN聚类结果

本文中提取的障碍物主要特征有跟踪点位置 Q(x, y)、运动方向与 x 轴的偏离角度 θ、长度 L、宽度 W。提取包罗障碍物的最小外接矩形框上距原点最近的顶点作为 Q(x,y)，因为此点随雷达视角的变化一般较小，矩形框的最长边线与 x 轴的夹角作为障碍物角度信息 θ。本文选用最小凸包法结合模糊线段的方法提取障碍物的包罗矩形框。



图 5 模糊线段提取

首先，利用 Graham 扫描提取凸包点，然后利用简化的模糊线段法 [10] 提取障碍物的矩形边框。具体步骤如下（见图 5）：

选取距起点（如图中 P0）的距离大于一定阈值的最近点（如图中 P2）；
计算起点 P0、P2 两点确定的直线的斜率 K_g1，且 P0 和 P2 之间的点全部属于 G_1（图 5 中只存在点 P1）；
计算下一个点（如图中 P3）与 P0 确定的直线的斜率 K_g2，令 θ=arctan(K_g1-K_g2)，如果 |θ| 大于一定的阈值，则 G1 组关闭，以 G1 中最后一个点为起点建立 G2；
在 |θ| 小于一定的阈值的情况下，向量 P0P2、P0P3 相加得到 PP′，过 PP′ 两侧距离最远的点 P3、P2，分别作 PP′ 的平行线，计算两平行线的距离 η，如果 η 大于设定的阈值 ηtr，则 P3∈G1，否则 G1 关闭，以 G1 中最后一个点为起点建立 G2；
若步骤 c、d 中有新起点建立，则循环从步骤 a 开始，否则循环从步骤 c 开始，直到计算完成最后一个点，输出 G1、G2、……、Gx。然后对每组内的点运用最小二乘求得最佳斜率，作出每组点的直线，选取距离最长、邻域超过一定数量点的直线作边线，作出障碍点的边框。

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



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

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



图 7 单帧特征提取结果


3. 障碍物跟踪

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



图 8 障碍物跟踪算法流程

3.1 数据关联

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



表 1 数据关联算法对比

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

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

首先介绍本文的数据关联算法。{T_j}（j=1,…,M）表示前一帧跟踪的障碍物，{Z_i}（i=1,…,N）为当前帧检测的障碍物。根据实际状况，可以分为 3 种情况（见图 9）：
已经跟踪的障碍物逐渐或突然消失，当前帧没有与之关联的任何障碍物，可以表示为{T_jN}；
新障碍物突然或者逐渐进入雷达搜索区域，之前帧没有障碍物与之关联，可以表示为{Z_iN}；
当前帧某个障碍物与之前跟踪的障碍物形成关联，表示为{Y(T_j, Z_i)}。



图 9 目标匹配过程

图 9 中圆形区域表示障碍物可能出现的位置，圆形半径为 SR，本文取 SR = 3 m。图中障碍物 T_1、T_2、T_3 表示经过卡尔曼滤波器预测后的障碍物位置，并继承之前帧的其他几何特征，Z_1、Z_2、Z_3 为当前帧出现的障碍物，那么根据上述 3 种情况，可以形成一系列假设：



假设在 H_k 中，P(Y(T_j, Z_i)) 表示成功关联的概率，P(T_jN) 表示之前帧障碍物无关联的概率，P(Z_iN) 表示当前帧障碍物无关联的概率。P(Y(T_j, Z_i)) 是以下 4 个关联特征匹配概率的联合概率：两障碍物存在重叠面积的概率 P_cov(Tj,Zi)；中心点的匹配概率 P_center(T_j, Z_i)；障碍物长、宽的匹配概率 P_WL(Tj, Zi)；障碍物倾斜角度的匹配概率 P_ratio(Tj,Zi)。

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



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



式中，V_i1 为其中一个矩形的顶点，V_i1、V_i2、V_i3、V_i4 依次为逆时针排列的矩形顶点；G_jx 为另一个矩形的某个顶点。

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



式中，S_ij 为可能匹配的两障碍物跟踪点之间的距离；W_iO、L_iO 分别为滤波器预测障碍物的宽度和长度；W_i、L_i 分别为当前帧中可能与滤波器预测障碍物匹配的障碍物的宽度和长度；K_iO 为滤波器预测障碍物矩形边界与 x 方向的夹角；K_i 为当前帧中可能与之匹配的障碍物矩形边界与 x 方向的夹角。

障碍物 T_j、Z_i 成功匹配的概率表示为：



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



选取最优假设 Ĥ ：



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



图 10 判别结果

3.2 滤波器预测与更新

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



预测的状态方程为：



测量转换矩阵为：



式中，vt 为观测噪声，满足 vt~N(0，R) 的高斯分布，本文取 R=0.01E2， E2 为 2 阶单位矩阵；wt 为状态噪声，满足 wt~N(0，Q) 的高斯分布，本文取 Q=0.001E4；E4 为 4 阶单位矩阵；T 为系统运行周期，本文取 T = 100 ms。

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



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



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

4. 试验结果

首先分析单帧雷达数据，利用 DBSCAN、模糊线段等算法，准确提取了障碍物的几何特征，与文献 [1]、文献 [3] 和文献 [8] 中的算法相比，改进的自适应阈值 DBSCAN 算法解决了由于雷达数据分布不均造成的聚类不准确等问题，经过大量的试验测试，正确率达到 98.2%；模糊线段与最小凸包相结合的方法，解决了由于模板匹配造成的障碍物角度匹配精度不高的问题。基于两帧提取的障碍物特征，采用多假设跟踪算法进行数据关联，关联正确率达到 99%。



图 11 无人自主车

将本文提出的算法应用在武汉理工大学智能车团队搭建的实时运算平台上，试验结果表明，该算法能够以每帧 100 ms 的速度实现准确、稳定的检测和跟踪多个动态障碍物。该平台硬件配置为 INTEL 8 核 3.4 GHz 处理器，内存 16 GB，软件采用 Ubuntu下的 ROS 机器人实时操作系统。图 11 为自行搭建的无人车平台。



图 12 摄像头拍摄实景



图 13 雷达跟踪结果

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

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

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



图 14 速度对比结果

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



图 15 航向角对比结果

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


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 Intelligent Vehicles 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