基于多层激光雷达的目标检测与跟踪算法

平面拟合算法将点云数据进行地面滤除,将过滤后的点栅格化并利用区域增长算法进行目标聚类，根据

RANSAC算法构造的BBox得到的目标信息，利用改进的匈牙利算法建立对应的跟踪器。利用卡尔曼滤波

算法实时更新目标的状态。改进的匈牙利算法能够解决非平衡目标指派到的问题,适应环境中目标突然

增多或者减少的情况，以数据集论证了该算法的可靠性。关键词：目标检测与跟踪；地平面拟合算法；RANSAC算法；匈牙利算法；卡尔曼滤波中图分类号：TP391；TP212 文献标识码：A 文章编号：1000-9787(2020)06-0123-04Algorithm of target detection and tracking based on

multi-layer LiDAR **

LI Shuaiyin, DUAN Jianmin, RAN Xuhui(Department of Information Science,Beijing University of Technology,Beijing 100124,China)Abstract: In order to detect and track lhe target on road, an algorithm of target detection and tracking based on multi-layer lidar is proposed. Collected points cloud data is filtered out using ground plane fitting algorithm, the

filtered points are rasterized and clustered by region growing algorithm. The information of targets is obtained from

bounding box ( BBox ) constructed by random sample consensus ( RANSAC ) algorithm and build corresponding

tracker by improved Hungarian algorithm. Updating the state of the target using the Kalman filtering algorithm. The

improved Hungarian algorithm can solve the problem of non-equilibrium target assignment and adapt to the sudden increase or decrease of targets in the environment. The reliability of the algorithm is demonstrated by the KITTI

data set.Keywords: target detection and tracking； ground plane fitting algorithm ； random sample consensus( RANSAC) algorithm ； Hungarian algorithm ； Kalman filtering0引言量，然后计算特征向量的代价函数进行多目标关联从而进 无人驾驶汽车是国内外的热点研究课题，其中能够实

行目标的检测与跟踪。联合概率数据关联(joint probabilis­

时精准地检测与跟踪目标的位置状态等信息是保证无人驾

tic data association,JPDA)算法须计算被跟踪的目标和当前 驶能够安全进行的前提，因此，目标检测与跟踪算法的稳定

帧中真实目标的测量值和虚假目标之间的联合概率，当目 性和准确性是衡量系统性能的重要指标。标和量测数目增多时,JPDA算法的计算量将出现组合爆炸

国内外许多无人驾驶研究团队都对环境感知的目标检 现象，从而造成计算复杂⑺。文献［8］中提出了 MHT方

测与跟踪方面有深入的研究。斯坦福大学的自主车

法,是处理数据关联的最优算法，能检测出目标的终结和新

“Junior”利用激光雷达对跟踪目标的运动几何特征建模， 目标的生成。但当杂波密度增大时，计算复杂度呈指数增 然后用贝叶斯滤波器分别更新每个目标的状态E ；卡耐基

长，在实际应用中实现目标跟踪的实时性也是不可靠的。•梅隆大学的BOSS从点云数据中提取障碍物特征，通过

本文提出了一种基于多层激光雷达的目标检测与跟踪算

关联不同时刻的点云数据对动态障碍物进行检测跟

法，在KTTTI数据库上进行实验结果验证了算法的有效性。踪［2~4】。叶刚⑸通过二分图和匈牙利算法对多目标进行检

1滤除地面点测与跟踪。周俊静等人⑹通过ICP算法得到目标的特征向

为了去除地面点云本文采用平面模型(plane model)来收稿日期：2019-04-03*基金项目：北京市教委基金资助项目(JJ002790200802);北京市属高等学校人才强教计划资助项目(038000543115025)Kj：\\2020-5-28\\［天才】\\已传一传感器与微系统\\2020-06\\G 2020=^ps 20200M8 15：00：09 Time： 20200528 15：30：30124传感器与微系统第39卷拟合当前的地面,将原始的点云数据分割为地面点云匕和 个值。如果最大值与最小值的差值大于设定的阈值。，则

非地平面点云两个集合。判定此栅格为障碍物状态，否则判断为空闲状态。图2(a) 首先要选取正确的种子点集，这些种子点集来源于点

是对滤除地面后的点云进行栅格化得到的栅格图。云中高度值较小的点的集合，种子点集选取完毕之后，计算

本文采用基于区域生长算法对占用的栅格地图进行目

点云中的点到平面模型的正交投影的距离，将距离小于 标聚类，将所得到的目标聚类之后，需要对其用BBox进行

加抽的点归为地面点。线性平面模型如下标记。本文采用RANSAC算法构建的BBox ( bounding

ax +by +cz +d =0

(1)box)。RANSAC算法的输入参数data为栅格内的点云，适 即用数据的模型Model为SACMODEL_LINE,适用于模型最少

nyx — —d (2)的数据个数为n为50,算法的迭代次数k为100,用于决定

式中n =[a,b,c]T,x=[%,y,z]T,通过计算种子点集的协

数据是否适用于模型的阈值/为0. 02。经由RANSAC算法

方差矩阵来求解\"，从而确定一个平面。将种子点集Se/?3

拟合岀一条直线向量为目标矩形对应到的一个直角边，此 作为初始点集，其协方差矩阵为直线向量的垂线为矩形的另一条直角边，矩形的高度z为 构成该目标的所有点云z的平均值。则该矩形可以表示为

C=工(s； -s)(s； -s)T

(3)i = 1： 151S = [objected,d,x,y,z,w，其中 ‘objected 为聚类号,d 为障

式中§为所有点的平均值，协方差矩阵C表示所有点的 碍物相对于激光雷达的距离为障碍物的中心坐

散布情况，通过奇异值分解(singular value decomposition,

标严为障碍物的宽度丿为障碍物的长度。根据当前的矩

SVD)求得最小奇异值对应的奇异向量，此向量即为垂直于

形模型从而能够获得目标比较精确的尺寸和目标的当前位

平面的法向量\"。在\"已知的情况下，将种子点集的平均

置,为接下来的目标跟踪提供了基础。用RANSAC算法构

值？代入式中，即可求得对应的距离常量心最后计算点云

建的BBox如图2(b)所示。中的所有点到该平面的正交投影距离，将距离小于设定的 阈值加覘的点归为地面点，否则归类为非地面点。为了直观显示地面点与非地面点的区别，用不同的颜

色来表示不同的分类，具体效果如图1所示。(a)栅格地图

(b) BBox标记目标图2栅格地图和BBox标记目标3目标跟踪3.1 对单一目标进行跟踪对于简单的环境中,如果只有一个障碍物，则直接用线 性卡尔曼滤波器对该目标进行状态估计，目标的状态向量

Xk包括%方向的位置和速度和y方向的位置和速度，测量

(C)向量Z*为目标在％方向和y方向的位置,分别为过滤地面点云图像xk = (xOk vxk yQk vyk | T ,zk = j xOk yOk J 1 (4)图1去除地面点云的结果运动模型的转换矩阵图1(a)中为原始场景图,从图中可以看出路边停着几

A为/I

Az 0 0 \\辆车,旁边还有一条小胡同，图1 (b)中对应的原始点云图，

10

0图1(c)中的点云图包含了地面点和非地面点，为了更加清

A =

0 0

0 1 At(5)晰地观察结果将地面点和非地面点的点云分别用不同的颜

<0 0

0

1 >色区分开来，白色的点对应的是路面点，红色的点对应的是

式中 M为激光雷达采集1帧数据的时间。非地面点。从图中可以看出应用地平面拟合算法较好地将

测量矩阵H、过程噪声的协方差矩阵Q、测量噪声的协 地面点与非地面点区别开来。方差矩阵R分别为2目标信息提取(1 0 0 0、 (10将激光雷达的点云数据进行栅格化的方法有很

H= ，尺=£(0 0 1 0 丿 (01多⑼I。〕，在无人驾驶领域中比较常见的有均值高度图法和

(△『/4 A?/2

00 \\最大最小值高度图法两种「⑴，本文中采用的是最大最小值

2 Az3/2 Af2 00高度图法,在该方法中将地平面建模为一系列的栅格，栅格

°F 0 0

A?/4A?/2(6)中包含所有投影到同一栅格中的高度的最大值和最小值两

、0 0

A?/2A?Kj：\\2020-5-28\\［天才】\\已传一传感器与微系统\\2020-06\\G 2020=^ps 20200M8 15：00：09 Time： 20200528 15：30：32第6期李帅印，等:基于多层激光雷达的目标检测与跟踪算法125式中 ％为所加的过程噪声dm为所加的测量噪声的大小。 在多目标跟踪中匈牙利算法的基本思想是根据上一帧

测量噪声的协方差矩阵一般是通过多次测量一些比较 建立的跟踪器和当前帧检测到的目标建立两个对应的链

稳定的目标求岀得到。过程噪声和测量噪声在初步确定后 表，依次从跟踪器链表中每个顶点寻找对应的增广路径P,

都要进行离线线下数据的调整,以使系统能够达到最优。然后通过取反操作获得更大的匹配，一直重复此操作直到

图3为无人驾驶车辆前方的一个目标,在某时刻该目

无法寻找到对应的增广路径，从而求得最大匹配。标存在了被遮挡或者漏检的情况，为了保持目标的连贯性, 传统的匈牙利算法只能够解决平衡目标的指派问题,

应用卡尔曼滤波对其进行预测跟踪。虚线为实际的测量数

平衡目标主要是指没有目标的增多或减少。但对于无人驾 据，点为经过卡尔曼滤波之后得到的数据。从图3中可以 驶，周围的环境是不断更新变化的，出现在雷达的检测范围

明显的看出在第39帧和第42帧之间出现了很大的噪声干

的目标数目也是不断变化的。跟踪算法要及时为新目标建

扰,卡尔曼滤波大大降低了噪声干扰。立对应的跟踪器，也应该及时删除无效的跟踪器。因此，如 果关联矩阵出现空行时，应及时为目标建立新的跟踪器，如

果出现空列时，则及时将无效的跟踪器删除,并及时更新跟 踪器的数量。改进的匈牙利算法流程图如图4所示。⑻滤波前后目标与本车的方向

距离对比

X(b)滤波刖后目标与本车y方向距离的对比图3卡尔曼滤波示意3.2 多目标跟踪3. 2.1关联值计算当对多目标进行跟踪时,需要对目标进行目标关联。

采用数据关联矩阵的方式来对前后帧的目标信息进行关

联。关联矩阵中的关联值主要是由两个目标之间的相似值

属性来决定的，主要属性有：目标跟踪中心点的位置PQ)

以及目标与跟踪器之间的重叠率p,在大部分情况下依靠这 两个特征就能够较好的计算目标之间的相似性。计算得到 图4改进的匈牙利算法流程图关联值4实验结果C(i,j) =a xP(t,j) +0 xp(i,j)

(7)4. 1 KITTI Tracking 数据库式中C(i,j)求得的是在第k帧时检测到的第i个目标和 KITTI数据集的传感器是Velodyne HDL-64E S2 64线

第％ -1帧时第个跟踪器的关联值,其中,a和0代表两个

激光雷达，该雷达的频率是10 Hz,每一帧返回的点云数据 特性的权重因子,a+0=l,P(；J)为第：个目标和j个跟踪

包含大约13万个三维点。每一帧中雷达的点云数据C,=

器之间的距离,pg 为第：个目标和j个跟踪器之间的重

,N\\ ,Pi =\\xi,yi,zi,ii\\ o 式中表示在 t 帧时

叠面积。根据式(7)计算得出来对应的关联值，然后经过特

刻含有N个数据点，｛捡，人,zj表示巴点对应的坐标，｛订

定阈值判断得到对应的0,1矩阵,元素为1的代表大概率 表示P,的对应的返回强度。存在关联关系，这种关联关系可能是多对多的。采集平台(如图5)配有2个高清摄像机,1台Velodyne

激光雷达和一套GPS定位系统。3. 2.2改进的匈牙利算法进行匹配匈牙利算法〔⑵应用到两个概念，分别是二分图和增广 路径。二分图是图论中的一种特殊模型。设G=(y,E)是 一个无向图，如果顶点y可分割为两个互不相交的子集⑺, B),并且图中的每条边(ij)所关联的两个顶点i和_/分别

属于这两个不同的顶点集，则称G为一个二分图。增广路

径:设M为二分图G已匹配边的集合，若P是图G中一条

图5数据采集平台连通两个未匹配顶点的路径，并且属M的边和不属M的边

在KITTI数据库「⑶中，TRACKING数据库是包含了在P上交替出现，则称P为相对于M的一条增广路径。21个训练序列和29个测试序列，是专门用来测试目标跟踪

Kj：\\2020-5-28\\[天才】\\已传一传感器与微系统\\2020-06\\G 2020=^ps 20200M8 15：00：09 Time： 20200528 15：30：34126传感器与微系统第39卷性能的数据库。此数据库只给出了原始的数据点云并没有 中目标增多和减少的情况，并在KITTI数据集上进行了验 对目标进行聚类，本文选取了一个十字路口的点云进行了 证。测试结果表明:该算法能够满足目标检测与跟踪的要 算法的验证。求。算法中为对目标进行分类识别处理，因此后续主要工

4.2 KITTI数据库验证结果作内容是结合视频数据对跟踪目标进行分类识别。应用上述数据库和官网上几种算法进行了对比，从多

参考文献：目标跟踪准确率(multiple object tracking accuracy, MOTA )、

[1] PETROVSKAYA A,THRUN S. Model based vehicle detection and

运行环境和耗时方面进行对比。准确率的表达式为M0TA =

tracking for autonomous urban driving] J]. Autonomous Robotics, 1 - 丫，(叫+fPt +mm®)/丫向，其中,g,为在/帧时刻出 2009,26(2-3) ：123 -139.[2] DARMS M S,RYBSKI P E,BAKER C,et al. Obstacle detection

现的目标数,®为在t帧时刻没有匹配上的目标数，力，为

and tracking (or the urban challenge [ J ]. IEEE Transactions on 在t帧时刻没有进行匹配的跟踪器个数，血ne,为在t时刻

Intelligent Transportation Systems,2009,10(3) ：475 —485.跟踪器和对应的目标没有正确进行匹配的个数。根据目标

[3] DARMS M,RYBSKY P,URMSON C. Classification and tracking

的几何特征，可通过一些限制条件来提高检测的准确率。 of dynamic objects with multiple sensors for autonomous driving in

对于高度大于2.5 m或者小于0. 5 m的目标，以及点云数小

urban environments [ C ]// Intelligent Vehicles Symposium, IEEE, 于15的目标，在进行目标跟踪之前，直接将此目标过滤掉,

2008:1197 -1202.得出最终的结果如表1所示，可以看出，本文提出的算法具

[4] BLACKMAN S S. Multiple hypothesis tracking for multiple target

有更好的性能。tracking [ J ]. IEEE Aerospace & Electronic Systems Magazine, 表1不同方法在KTTH目标检测与跟踪对比2009,19(1):5 -18.方法精准率/%耗时/S运行环境[5]

叶刚•城市环境基于三维激光雷达的自动驾驶车辆多目标检

本文MDP88.980.54 cores@ 1.8GHz( C + +)测及跟踪算法研究[D]・北京:北京理工大学,2016.SSP76.590.98 cores @3.5 GHz (Matlab + C/C + +)57.850.61 core @2.7 GHz (Python)[6]

周俊静，段建民，杨光祖，等.基于雷达测距的车辆识别与跟

踪方法[J] •汽车工程,2014(11) ：1415 -1420.实验场景为一个T型路口，图6为利用摄像头拍摄的

[7]

康健,李一兵，林云，等.基于证据理论的联合概率数据关联 同步场景。算法[J]・系统工程与电子技术,2013,35(8):1620 -1626.[8] PICKOVER C A. On the use of symmetrized dot patterns for the

visual characterization of speech waveforms and other sampled

(a)第35帧场景图

(b)第38帧场景图data[ J ]. Journal of the Acoustical Society of America, 1986,

图6摄像头拍摄场景80(3)：955 -960.从图6中可以看出从第35帧到第38帧时驶入了一个

[9] DORAI C, WANG G,JAIN A K,et al. Registration and integration

灰色车辆并驶出一辆红色车辆和白色车辆。从图7(a)中

of multiple object views for 3D model construction [ J J. 2015

可知该算法准确地检测到了3个车辆并为其建立对应的 IEEE Transactions on Pattern Analysis & Machine Intelligence,

BBox,图7(b)中第38帧时为新出现的目标增加了对应的

2015,20(1) ：83 —89.BBox,并将消失目标对应的BBox删除，此时间段内黑色车

[10] 于金霞，蔡自兴，邹小兵，等.基于激光雷达的移动机器人障

辆一直在检测范围内，所以黑色车辆对应的BBox 一直存在

碍测距研究[J]・传感器与微系统,2006,25(5) ：31 -33,36.且对应的编号未发生变化。可以验证:该算法能够自适应

[11] 朱建新，罗刚，汪志杰，等•基于激光雷达的挖掘机器人回转

目标增多和减少的情况，并能够准确地跟踪对应的目标。避障研究[J] •传感器与微系统,2017,36(9):41 —44.[12] 李廷鹏，钱彦岭，李岳•基于改进匈牙利算法的多技能人员调

度方法[J] •国防科技大学学报,2016,38(2):144-149.[13] URTASUN R,LENZ P,GEIGER A. Are we ready for autonomous

driving the KITTI vision benchmark suite] C]〃IEEE Conference

on Computer Vision and Pattern Recognition, IEEE Computer

(a)35Society ,2012：3354—3361.第帧检测与跟踪结果(b)第38帧检测与跟踪结果作者简介：图7点云图李帅印(1994-),男，硕士，研究方向为道路识别，环境感知，

5结束语E-mail: 1573551600 @ qq. com0本文对基于激光雷达的目标检测与跟踪进行了研究,

段建民(1959 -),男，教授，博士生导师，主要研究领域为车辆 重点介绍了匈牙利算法的改进，使其能够适用于无人驾驶

环境信息融合、道路识别、自动驾驶技术。