基于激光雷达与毫米波雷达融合的机动目标跟踪方法

摘要

本发明公开了基于激光雷达与毫米波雷达融合的机动目标跟踪方法，步骤1：对毫米波雷达和激光雷达进行联合标定，使二者采集到的数据在时间和空间上对齐。步骤2：对毫米波雷达和激光雷达所采集的前方目标信息分别使用所设计的两个独立的双波门IMM‑JPDA(交互多模型‑联合概率数据关联)滤波器进行机动目标数据关联和跟踪。步骤3：将两个滤波器所产生的目标状态信息使用匈牙利算法进行轨迹关联，在轨迹关联完成后使用凸组合算法对关联轨迹进行融合，产生最终轨迹。本发明所提出的双波门IMM‑JPDA跟踪算法、以及凸组合融合方法融合两传感器跟踪结果，能够分别从激光雷达和毫米波雷达获得针对机动目标的跟踪结果，同时能够实现更好的跟踪精度。

说明书

技术领域

本发明属于智能汽车技术领域，特别涉及了一种基于激光雷达与毫米波雷达融合的多机动目标跟踪方法。

背景技术

智能车辆作为智能化领域中一项主要的研究内容，其将多种现代电子信息技术集成于一体，整体可分为环境感知、路径决策、车辆控制三个方面。而环境感知技术是整个智能车辆的基石，必须让智能车辆像人类一样实时感知到周围的环境的情况，才能为后续的路径决策和车辆控制提供可能。而多目标跟踪技术是智能车辆环境感知方面的重要组成部分。

现有的跟踪方法大都为基于单一激光雷达或者毫米波雷达的单模型算法。而基于激光雷达的跟踪方法在雨、雪天气下感知精度受到极大的干扰，同时激光雷达无法直接获得目标的速度信息，因此跟踪算法对目标速度变化不够敏感。基于毫米波雷达跟踪方法，由于毫米波雷达航向角偏差较大，横向位置偏差大且无法获得目标体积信息。而单模型算法无法针对目标的机动情况，在复杂交通环境下容易丢失跟踪目标。

发明内容

针对现有技术中存在不足，本发明提供了一种激光雷达与毫米波雷达融合的机动目标跟踪方法，能够分别从激光雷达和毫米波雷达获得针对机动目标的跟踪结果，同时使用凸组合融合方法融合两传感器跟踪结果，能够实现更好的跟踪精度。

本发明的具体步骤如下：

步骤1：对毫米波雷达和激光雷达进行联合标定，使二者采集到的数据在时间和空间上对齐。

步骤2：对毫米波雷达和激光雷达所采集的前方目标信息分别使用所设计的两个独立的双波门IMM-JPDA(交互多模型-联合概率数据关联)滤波器进行机动目标数据关联和跟踪。

步骤3：将两个滤波器所产生的目标状态信息使用匈牙利算法进行轨迹关联，在轨迹关联完成后使用凸组合算法对关联轨迹进行融合，产生最终轨迹。

上述步骤1所述的联合标定方法简述如下：

通过测量获得试验车上毫米波雷达与激光雷达安装位置水平方向x、y上的距离，然后将毫米波雷达观测坐标系原点移动到激光雷达坐标系原点，具体实现方法为在毫米波获得的观测数据对应减去测量获得的x、y上的距离，由此完成空间标定。

对于时间标定，将毫米波雷达与激光雷达设置为相同的工作频率即可完成。

上述步骤2所述的滤波器的具体设计方法如下：

从k-1时刻获得协方差P(k-1|k-1)及目标状态X(k-1|k-1)由系统状态方程对当前时刻状态和协方差进行预测获得当前k时刻的预测协方差

对所有观测结果先采用欧式距离作为第一道波门的波门门限对观测进行筛选，满足下式时，将其作为初步待选测量。

其中，x

对于满足第一次筛选的初步待选测量，采用马氏距离作为第二道波门的波门门限，对观测进行再次筛选，其筛选公式如下所示。

其中Z

设置确认矩阵Ω(k)

Ω(k)＝[ω

其中

对于确认矩阵中每个可能的关联事件θ

而关联概率β

其中c为归一化常数，φ(θ

由关联概率β

其中

使用交互多模型算法对机动目标进行跟踪。其计算公式为：

其中

步骤3的轨迹关联融合具体流程如下：

对于激光雷达与毫米波雷达经由各自的双波门IMM-JPDA滤波器所分别产生的a条与b条轨迹，使用欧氏距离对其当前时刻的状态量中的坐标点距离进行计算记为 d

在二次关联中，使用欧式距离作为阈值d

对于关联成功的轨迹使用凸组合算法对其位置部分进行轨迹融合，其公式如下所示：

其中X

对融合后状态量X

本发明的有益效果：

(1)本发明针对使用单一激光雷达受天气影响大、无法直接获得目标速度或单一毫米波雷达无法获得目标体积信息的缺点。通过采用激光雷达与毫米波雷达融合跟踪的方法，扩展了跟踪系统的使用场景。

(2)本发明通过采用所提出的双波门IMM-JPDA(交互多模型-联合概率数据关联)跟踪算法，对于杂波较多，跟踪目标频繁机动的复杂交通场景，该算法可以解决单波门算法波门门限设置困难，无法准确保留有效观测、滤除杂波的问题。在保证在保留目标观测信息情况下滤除多余杂波，加快算法速度的同时保证对机动目标的有效跟踪。

(3)本发明通过采用提出的二次匈牙利匹配算法与凸组合有效信息融合方法，结合了激光雷达的跟踪信息与毫米波雷达的跟踪信息，确保了目标跟踪的位置精度和形状精度。

附图说明

图1为基于激光雷达与毫米波雷达融合的多机动目标跟踪方法流程图；

图2为双波门IMM-JPDA滤波算法流程图；

具体实施方式

下面结合附图对本发明作进一步说明。

图1为基于激光雷达与毫米波雷达融合的多机动目标跟踪方法流程图，本发明方法具体步骤如下：

步骤1：对毫米波雷达和激光雷达进行联合标定，使二者采集到的数据在时间和空间上对齐。

步骤2：对毫米波雷达和激光雷达所采集的前方目标信息分别使用所设计的两个独立的双波门IMM-JPDA(交互多模型-联合概率数据关联)滤波器进行机动目标数据关联和跟踪。

步骤3：将两个滤波器所产生的目标状态信息使用匈牙利算法进行轨迹关联，在轨迹关联完成后使用凸组合算法对关联轨迹进行融合，产生最终轨迹。

上述步骤1所述的联合标定方法如下：

通过测量获得试验车上毫米波雷达与激光雷达安装位置水平方向x、y上的距离，然后将毫米波雷达观测坐标系原点移动到激光雷达坐标系原点，具体实现方法为在毫米波获得的观测数据对应减去测量获得的x、y上的距离，由此完成空间标定。

对于时间标定，将毫米波雷达与激光雷达设置为相同的工作频率即可完成。

上述步骤2所述的双波门IMM-JPDA滤波器结合双波门IMM-JPDA滤波算法流程图图2具体描述如下：

步骤2.1：从k-1时刻获得协方差P(k-1|k-1)及目标状态X(k-1|k-1)由系统状态方程对当前时刻状态和协方差进行预测获得当前k时刻的预测协方差

步骤2.2：由于传感器获得的信息包含较多杂波，无法直接进行使用，需对结果进行过滤。现有其他方法为对观测目标t设置相关波门门限γ

这种方法在交通场景简单，杂波较少下较为有效，但在交通环境较为复杂的十字路口等场景下杂波与有效观测交叉混合，波门门限γ

其中，x

对于满足第一次筛选的初步待选测量，采用马氏距离作为第二道波门的波门门限，对观测进行再次筛选，其筛选公式如下所示。

其中Z

本次筛选后的测量值即为最终有效测量值，采用双波门设置，可将第一道波门门限设置较大，第二道波门门限设置较小，相较于单波门的设置难度下降，同时因为欧式距离判定较马氏距离判定速度快，采用双波门设置可有效减少马氏距离判定次数，提高算法速度。

步骤2.3：由于落入波门内的测量值可能不只一个，同时一个测量值也可能落入多个目标的波门内，因此设置确认矩阵Ω(k)

Ω(k)＝[ω

其中

步骤2.4：确认矩阵包含了当前所有测量与目标可能的关联事件θ

而关联概率β

其中c为归一化常数，φ(θ

步骤2.5：由关联概率β

其中

步骤2.6：对目标的跟踪过程是将当前时刻的观测和上一时刻对同一目标的观测进行匹配关联，具体在算法流程中即将当前时刻的状态量

其中

对于激光雷达与毫米波雷达双波门IMM-JPDA滤波器参数来说，激光雷达状态量X

对于激光雷达与毫米波雷达产生的检测，在进行过双波门IMM-JPDA滤波器后得到目标的轨迹信息。需要进行轨迹关联将两个传感器产生轨迹相互匹配，使用凸组合融合算法提升轨迹精度，同时使得轨迹能够获得更多的信息。具体步骤3的二次匈牙利匹配轨迹关联融合流程如下：

步骤3.1：对于激光雷达与毫米波雷达经由各自的双波门IMM-JPDA滤波器所分别产生的a条与b条轨迹，使用欧氏距离对其当前时刻的状态量中的坐标点距离进行计算记为d

将D

在二次关联中，使用欧式距离作为阈值d

步骤3.2：对于关联成功的轨迹使用凸组合算法对其有效信息进行轨迹融合，其公式如下所示：

其中X

步骤3.3：对融合后状态量X

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技术所创的等效方式或变更均应包含在本发明的保护范围之内。