基于激光雷达的3D实时车辆跟踪*

王 海，李 洋，蔡英凤，孙 恺，陈 龙

（1.江苏大学汽车与交通工程学院，镇江 212013；2.江苏大学汽车工程研究院，镇江 212013；3.上海禾赛科技股份有限公司，上海 201702）

前言

随着智能汽车的不断发展，其对周围环境的感知需求日益增加。在环境感知算法中，多目标跟踪算法是重要的组成部分之一，其为智能汽车的轨迹规划与决策提供了必要的环境信息。相较于智能车辆上的摄像头与毫米波雷达，激光雷达具有感知精度高、对光照和天气适应能力强等特点，受到学术界和工业界的广泛关注。

跟踪算法主要目的是将前后帧目标检测结果进行匹配，进而实现对目标状态的连续感知。现有3D多目标跟踪算法由数据关联算法结合滤波算法组成。数据关联算法主要解决目标间的关联问题，滤波算法主要解决目标的状态估计和轨迹更新问题。常见的数据关联算法有联合概率数据互联（joint probabilistic data association，JPDA）［1］、多假设跟踪（multiple hypothesis tracking，MHT）［2］、全局最近邻关联（global nearest neighbor，GNN）［3］等。常见的滤波算法有卡尔曼滤波（Kalman filter，KF）［4-5］系列、粒子滤波（particle filter，PF）［6］以及适应目标机动工况的交互多模型算法（interacting multiple model，IMM）［7］等。

现有的3D多目标跟踪方法，例如文献［8］～文献［12］中基于检测的跟踪，将检测算法与跟踪算法耦合，整体精度较高。但由于跟踪算法精度依赖于检测算法的精度，将检测算法与跟踪算法耦合会掩盖跟踪算法本身的精度，无法确定是由于先进的感知算法带来的跟踪性能提升还是由于跟踪算法的改进带来的性能提升。同时这样的耦合所带来的性能提升是以整个系统的复杂性和计算成本为代价的，例如文献［8］～文献［13］中方法具有较好的性能精度，但其系统的整体复杂度较高，无法满足高实时性的需求。

针对上述问题，本文中提出了一种基于激光雷达的3D实时车辆跟踪方法。使用GNN算法作为数据关联算法，并通过双波门设计，提升了算法精度与速度。同时为消除检测与跟踪算法的耦合关系，采用目标位置信息与目标体积信息作为关联向量，并使用马氏距离作为关联距离以提升关联的准确性。最后使用3D IMM⁃KF算法，保证对机动目标的跟踪，实现了一个鲁棒性强、实时性好的跟踪系统。

1 跟踪系统设计

本文中提出的基于激光雷达的3D实时车辆跟踪系统，由3个部分组成：（1）使用双波门GNN的前后帧目标关联模块；（2）使用3DKF实现的IMM跟踪滤波器；（3）结合历史轨迹信息的跟踪轨迹生成和消亡管理器。其整体系统流程如图1所示。

图1 跟踪系统流程图

1.1 检测结果获取

随着智能车辆检测算法的不断发展，不断涌现出性能优异的检测算法，例如文献［14］～文献［16］。本文中采用文献［16］中所提出的检测算法，未进行任何改动。该检测算法并不具有领先的性能，目前其精度在KITTI的基准测试榜单中为75.64%，相较于主流检测方法有5%以上的精度差距。采用该算法是因为其算法开源且传播广泛，能更加体现出所提跟踪算法的综合优势。

同时为保证跟踪算法的通用性，本文中不使用除3D检测框结果以外的信息。对于k时刻的检测结果z（k），有z（k）=（x，y，z，l，w，h，θ），其中（x，y，z）为3D检测框中心点的三维坐标，（l，w，h）为3D检测框的长宽高，θ为3D检测框朝向角。

1.2 双波门GNN关联算法

在常用的数据关联算法中，GNN算法是一种不考虑干扰的关联算法，直接将滤波算法预测结果与最相似的检测目标关联。而JPDA与MHT综合考虑了环境中的杂波干扰，与GNN算法相比，优势是关联精准更高，劣势是速度较慢。

激光雷达原始点云数据量庞大，每帧多达数万个检测点，但经过目标检测后，跟踪算法的输入仅有数十个检测框。环境干扰对跟踪算法的影响已降到较低水平，在这种情况下GNN算法结构简单、速度较快的优势便体现出来，因此本文中选择GNN关联算法作为基础的关联算法。

1.2.1 关联向量与关联距离

在GNN关联算法中，如何综合评价前后帧目标间的相似度对关联算法十分重要，文献［11］～文献［17］中使用的关联向量不包含物体的3D信息，文献［18］中使用的关联向量除物体3D信息外还包含目标点云的反射强度信息。要提升相似度计算的全面性和综合性，需尽可能增多关联向量中包含的特征信息，但为保证跟踪算法的通用性，一些特殊的关联信息应该予以排除。由上述两点考虑，本文中选择（x，y，z，l，w，h）作为关联向量。对于朝向角θ，其满足上述两点的要求，但是由于3D检测对于物体的朝向信息容易发生混乱，实验过程中发现关联向量中加入朝向角θ，反而会导致跟踪精度的下降，因此在关联向量中去除朝向角θ。

对于关联距离的计算方式，文献［11］和文献［12］中选用欧式距离进行计算。欧氏距离计算方便，直接反映了两个物体间的空间距离。然而欧式距离对关联向量中的各个参数进行等权重的计算，无法评判不同参数的不同重要性，因此本文中选用以协方差作为权重的马氏距离进行计算［19］，从而能更好区分目标间的相似度。马氏距离dm为

式中：v（k）为z(k)-z(k∣k-1)，表示新息；S（k）为v（k）的协方差。

1.2.2 双波门算法

如图1所示，检测结果在进行关联匹配前，需要进行波门的验证。波门是以预测状态为中心的判定区域，只有落入波门内的检测，才会执行后续关联算法。而GNN算法匹配逻辑简单，波门设置方式对算法精度影响较大。以往算法只设置一道波门进行滤波，导致对目标距离信息的判定不够全面，同时在单波门的情况下对所有目标进行马氏距离的计算判定会耗费大量算力，导致算法实时性下降。

针对以上问题，提出双波门滤波方法，其结构如图2所示。首先使用欧式距离γe作为阈值，进行欧式波门的筛选。此次筛选仅对空间坐标（x，y，z）进行筛选，将在空间距离上大于阈值的观测值进行去除。

图2 双波门示意图

式中v'（k）为只包含（x，y，z）信息的新息。

随后使用马氏距离作为马氏波门的阈值γm，对满足欧式波门的检测结果再进行马氏距离的计算，滤除符合欧式波门但不符合马氏波门的检测。

式中：v''（k）为v（k）去除朝向角θ的新息；S''（k）为S（k）去除朝向角θ对应维度后的协方差。

通过进行双波门设置可有效滤除杂波干扰，提升算法精度。在算法速度上，通过欧式波门设置减少了较为费时的马氏距离计算次数，同时双波门设置的两次滤波减少了最终马氏波门内需进行关联匹配的目标数量，减少了关联匹配的计算次数，从两方面综合提升了算法速度。

1.2.3 关联匹配

在进行双波门滤波后，对满足双波门阈值的检测进行关联匹配。若几个较近的轨迹波门内不存在相互干扰的检测结果，则使用波门内马氏距离最近的检测目标作为匹配目标。如存在如图3所示的复杂情况，即预测位置P2其马氏波门内的检测z2与预测位置P1的距离最近，检测z3与预测位置P3距离最近的情况，则使用匈牙利算法作为匹配算法，将其代价矩阵定义为C m（nm为已有轨迹数，n为所有检测数），其中元素为cij。

图3 关联示意图

1.3 IMM跟踪器设计

现实中，实际车辆的运动如换道、超车等为机动状态，是几个运动模型的复合，因此只采用单一运动模型的KF滤波器无法适应该种情况。针对此类问题，使用可以应对机动目标情况的IMM算法，将多个滤波器结合起来进行跟踪。

1.3.1 运动模型的选定

IMM算法是通过考虑多个模型的交互转换来实现机动目标跟踪，使用时需要选定多个模型。而常用模型如匀速模型（constant velocity，CV）、匀转速模型（constant turn rate，CTR）、匀转速与速度模型（constant turn rate and velocity，CTRV）等都是针对点目标的运动情况，其状态向量中不包含3D信息，只针对2D信息进行计算。由于没有针对3D物体的跟踪模型，本文中针对3D物体的运动特点提出使用3DCTR模型与3DCV模型的IMM⁃KF算法。

第j种运动状态方程在k时刻为

式中：x（jk）为状态向量；F j为状态转移矩阵；H j为观测矩阵；W j(k)为过程噪声，是协方差Q j的高斯噪声；V j(k)为观测噪声，是协方差R j的高斯噪声。

在3DCV模型中k时刻的状态量x（1k）为（xk，vx，yk，vy，zk，vz，θ，l，w，h）T，基于3D CV模型将x（1k-1）中的信息推算到k时刻，匀速推算只改变3维位置，不改变其余参数。

在3D CTR模型中k时刻的状态量x（2k）为（xk，vx，yk，vy，zk，vz，θk，vθ，l，w，h）T，基于3D CTR模型将x（2k-1）的信息推算到k时刻，匀转速推算只改变3D位置和朝向角θ，不改变其余参数。

1.3.2 状态交互

定义模型间转移概率矩阵为P，其中元素pij为从第i个运动模型转移到第j个模型的概率。

从k-1时刻的状态估计xj(k-1|k-1)、模型j的概率u j(k-1)计算k-1时刻混合状态估计x0j(k-1|k-1)与混合协方差P0j(k-1|k-1)。

1.3.3 KF滤波状态更新

对于模型j进行卡尔曼滤波状态更新。首先，计算预测状态与预测协方差P j(k|k-1)：

计算卡尔曼增益K j(k)：

最后，进行状态更新，计算更新后状态与协方差P j(k|k)：

1.3.4 最终估计

计算模型j的可能性Λj(k)：

计算模型j的概率u j(k)：

式中c为归一化常数。

计算最终状态估计(k|k)：

计算最终协方差估计P(k|k)：

1.4 轨迹管理

由于检测算法会产生误检和漏检，为消除其对跟踪算法的影响，建立轨迹管理模块对轨迹的生成和消亡进行管理。本文中采用基于历史轨迹信息的轨迹管理器，其依托于历史轨迹的跟踪信息对轨迹的生成和消亡进行管理。对于轨迹的生成，由于检测算法会出现误检，因此不能直接将新目标当作轨迹起始。设置阈值fs与fmin，当在连续fs帧中，新目标能够成功关联fmin帧，即判定新轨迹确认。通过合适的fs与fmin阈值设定，即可有效消除检测中误检的影响，准确生成轨迹。对于轨迹消亡采取类似的阈值fd与fmax，在连续的fd帧中，已有轨迹fmax帧没有关联到检测，即将该轨迹删除实现轨迹的消亡。

2 实验

为了验证上述所提出的跟踪方法在实际复杂环境中的跟踪性能，在公开数据集KITTI上对算法进行评估，同时在无人车实验平台上进行真实道路环境中的算法验证。

2.1 离线实验

2.1.1 数据集

本文中采用KITTI的跟踪数据集，其数据集分为包含真值数据的21个序列共8 008帧训练数据和不包含真值数据的29个序列共11 095帧测试数据。本文中对训练数据进行了训练集和验证集的分割，并依据相关文献［17］，使用1、6、8、10、12、13、14、15、16、18、19序列进行验证。

2.1.2 评价方法

在文献［11］和文献［12］中评价跟踪精度所使用的速度误差和航向角误差，更适合单目标跟踪，无法综合评价多目标跟踪的精度。因此本文中采用多目标跟踪指标（multiple object tracking，MOT）［20-21］、包含多目标跟踪准确度（multiple object tracking accuracy，MOTA）、多目标跟踪精度（multiple object tracking precision，MOTP）、跟踪ID切换数（ID switch，IDS）和跟踪轨迹的中断数（fragmentations，FRAG）等。

其中MOTA是综合评价跟踪轨迹精确性的指标，其反映了误检、漏检和轨迹切换数在整体跟踪真值中的比例。

式中：FN为漏检数；FP为误检数；IDS为跟踪ID切换数；GT为真值总数。

MOTP是评价多目标跟踪位置精准度的指标，反应了跟踪结果和跟踪真值之间的差值。

式中：ck为第k帧跟踪结果与真值的匹配结果；dk，i为第k帧真值i与匹配结果的距离，一般用IoU计算。

MOTA、MOTP值越大跟踪结果越好，IDS、FRAG值越小跟踪结果越好。KITTI数据集的官方评价是以2D的形式实现的，对于3D跟踪则将其投影到2D图像中再进行评价。这种评价方式对于3D跟踪是不合适的，在3D跟踪结果中微小的差异可能会导致2D的结果发生巨大的变化，因此本文中将MOTA与MOTP的评价计算改为3D匹配的方式。

2.1.3 实验结果

使用在训练数据中划分出来的验证集对算法性能进行验证，其结果如表1所示。由表1可知，在主要的跟踪指标MOTA上对比算法mmMOT与FANTrack高了7%，而在MOTP与FRAG指标上较最好的mmMOT算法差距并不大，仅在IDS指标上效果较差。在这样的结果下，本文中算法实时性达到266.1 FPS，比FANTrack算法快了11.5倍，比mmMOT算法快55.4倍，所提出的跟踪算法完全满足智能驾驶实时性的需求。

表1 跟踪结果

选择KITTI训练集序列9进行跟踪，可视化结果如图4所示。图4（a）为3D视角下的跟踪结果，分别显示了检测结果与跟踪结果，同时对每个跟踪轨迹分配了编号。图4（b）～图4（e）验证了所提出的跟踪算法对车辆目标跟踪效果。对照右侧的场景图片，可以看到在自车前方编号为T279和T217的两辆车辆在整个跟踪过程中都保持了跟踪编号不变，验证了所提出的跟踪算法具有稳定跟踪目标的能力。

图4 跟踪结果

如图4（b）右侧的场景图片所示，第170帧中有效的检测目标只有T279和T217两辆车，但检测算法输出了多个误检目标。此时，在图4（b）左侧的跟踪结果视图中所提出的跟踪算法有效滤除了误检目标，生成了正确的轨迹。此种情况在图4（c）～图4（e）中也有出现，综合验证了所提出跟踪算法所具有的优秀关联能力。

2.2 对比实验

为了验证论文所提方法的有效性，对关联算法、关联向量与关联距离、轨迹管理和波门设置在KITTI数据集上进行了对比实验，从而验证提出方法的有效性。

2.2.1 不同的关联算法

对比不同的关联算法，其结果如表2所示。从中可以看出，使用GNN使得MOTA有了极大的提升，同时也实现了IDS与FRAG最小，仅在MOTP指标上较差。正如在1.2节中所述，在获得3D检测结果后，干扰有了较大的衰减，因此不考虑干扰的GNN关联算法反而更加适应此种情况，可以获得更好的关联结果。

表2 关联算法对比

2.2.2 不同的关联向量与关联距离

对关联向量与关联距离的不同设置进行对比，其结果如表3所示。从表3中第1组与第4组对比可以看出，马氏距离可以实现更高的MOTA精度。马氏距离可以更好地描述关联向量不同信息间的重要程度，使得对目标间距离的评价更加准确。而后3组的实验对比显示了单纯的增大关联向量维度并不能带来MOTA精度的提高。第2组与第4组对比可以看出目标3D体积信息的添加对关联是有益的。而第3组与第4组对比，朝向角θ的加入，并不能提高MOTA精度。本文中认为3D检测算法对目标的朝向角敏感度较低，对于同一目标在不同帧中的检测结果，其朝向角大小可能会发生180°的突变。朝向角发生180°突变，其检测框位置不会发生变化，然而3D跟踪的关联距离会产生较大的变化，从而导致关联失败，因此在关联向量中去除朝向角θ信息反而能带来MOTA精度的提高。

表3 关联向量与关联距离对比

2.2.3 不同的轨迹管理阈值设置

在轨迹管理模块中对比了不同的fs与fmin、fd与fmax参数值对精度的影响，其结果如表4所示。对比表4后3组实验可知，增大对历史轨迹的判断范围，即使轨迹生成和消亡的总体判定比fmin/fs与fmax/fd从2/3下降到3/5，仍可减小IDS与FRAG。而在合适的fs与fd设置下，将fmin/fs与fmax/fd升至1，如第1组实验，即可获得最小的IDS与FRAG。

表4 轨迹管理模块设计对比

2.2.4 不同波门算法

对比所提出的双波门算法与单波门算法的性能，其结果如表5所示。从表5可知，双波门算法比单波门算法在MOTA指标上有约3%的提高，在MOTP指标上也有1.68%的提升，仅在IDS与FRAG上有所下降。除了精度上的提高，在算法运行速度上，由于双波门算法减小了跟踪算法的计算量，使跟踪速度有了较大的提升，FPS上涨了32.92%。

表5 不同波门算法对比

2.3 实车验证

使用如图5所示的无人车实验平台，在实际道路上对跟踪算法进行算法验证，其结果如图6所示。

图5 无人车实验平台

图6分别展示了两车相近、两车相离、两车发生遮挡、两车遮挡结束4个场景的实景图和跟踪结果。图6（a）和图6（b）展示了T2车辆与T4车辆两车逐渐相近、随后T2车辆加速驶离的过程。在这个过程中两车跟踪框无相交，且跟踪ID没有发生交换，验证了所提出跟踪算法在目标相近时的多目标数据关联能力。图6（c）和图6（d）展示了T4车辆被T1车辆遮挡导致检测结果消失的情况下，所提出跟踪算法依然能够保证对T4车辆的持续跟踪，确保在遮挡结束后跟踪ID不发生变化。同时，在图6（d）中，在T3车辆被T1车辆遮挡导致检测结果消失情况下，T3车辆仍存在的跟踪结果也证明了所提出跟踪算法具有的对遮挡目标的持续跟踪能力。整个跟踪过程验证了所提出的跟踪算法所具有的良好性能和鲁棒性。

图6 实车数据验证

3 结论

本文中提出了一种基于激光雷达的3D实时车辆跟踪算法。针对3D检测结果杂波较少的特点，提出了双波门GNN作为数据关联算法，有效提升了跟踪精度和跟踪速度。同时对关联向量和关联距离进行了优化，在保证了算法的通用性的前提下提升了跟踪算法的性能。对于3D机动目标的运动情况，提出了使用3D卡尔曼滤波的IMM算法，保证了对机动目标的跟踪性能。所提出算法在公开数据集KITTI与无人车实验平台上经过了验证，表明了算法的高精度和良好的实时性。