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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

本发明的具体步骤如下：

步骤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_d、y_d分别为x、y方向预测值与观测值差值，γ_g为欧式距离波门。

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

其中Z_j(k)(j＝1,2,…,m_k)为第j个测量值，S为对应信息协方差矩阵，γ_t为马氏距离波门。

设置确认矩阵Ω(k)

Ω(k)＝[ω_jt]

其中

对于确认矩阵中每个可能的关联事件θ_i其关联概率为：

而关联概率β_jt为：

其中c为归一化常数，φ(θ_i(k))为k时刻未关联测量数量，τ_j[θ_i(k)]为k时刻关联测量数量,σ_t(θ_i(k))为k时刻关联指示器，量测关联时取1，否者为0，V为波门面积和，P_D为检测概率，N_tj[Z_j(k)]表示k时刻测量Z_j(k)服从高斯分布，为k时刻测量j与目标t关联指示器，两者关联时取1，否则为0，m_k、n_k分别为k时刻测量总数和关联事件总数。

由关联概率β_jt更新状态量和协方差，其计算公式为：

其中P^c(k|k)＝[1-K(k)H]P(k|k-1)，K(k)为k时刻的卡尔曼增益，H为对应卡尔曼滤波的观测矩阵，β₀(k)＝1-∑β_jt。

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

其中Pⁱ(k|k)i＝1,2,3,分别指使用匀速直线运动模型、匀加速直线运动模型、匀速圆周运动模型的JPDA滤波器产生的状态量和协方差，u_k(i)为k时刻模型i的概率。

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

对于激光雷达与毫米波雷达经由各自的双波门IMM-JPDA滤波器所分别产生的a条与b条轨迹，使用欧氏距离对其当前时刻的状态量中的坐标点距离进行计算记为d_ij(i＝1,2,…,a,j＝1,2,…,b)。将d_ij扩展为方阵，并作为代价矩阵使用匈牙利算法进行分配计算，进行初步关联。

在二次关联中，使用欧式距离作为阈值d_t对初步关联轨迹距离d_ij进行检验，如d_ij<d_t则确认关联，若d_ij>d_t则判定轨迹关联失败，两轨迹独立。对于二次关联的阈值d_t设置时，可根据实际情况进行调整，一般设置在的区间内,其中w，l为目标的宽和长。

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

其中X_l为激光雷达产生轨迹状态量，X_r为毫米波雷达产生轨迹状态量，其中状态量不涉及物体体积部分即长宽高l,w,h部分，P_r为激光雷达产生轨迹协方差，P_l为毫米波雷达产生轨迹协方差，X_f为融合后的状态量，P_f为融合后的轨迹协方差。

对融合后状态量X_f＝[x,y]进行扩维加入对应激光雷达轨迹中的l,w,h部分。对于关联失败的轨迹则不进行融合，则假定目标为轿车，以轿车尺寸对其进行扩维。

本发明的有益效果：

(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设置相关波门门限γ_t(t＝0,1,…,T)，当传感器观测满足判定式小于波门门限时，确认其为有效观测。

这种方法在交通场景简单，杂波较少下较为有效，但在交通环境较为复杂的十字路口等场景下杂波与有效观测交叉混合，波门门限γ_t的设置十分困难，过大则杂波过滤不彻底，过小则易丢失有效观测。因此本发明创新采用双波门设置，对所有观测结果先采用欧式距离作为第一道波门的波门门限对观测进行筛选，满足下式时，将其作为初步待选测量。

其中，x_d、y_d分别为x、y方向预测值与观测值差值，γ_g为欧式距离波门。

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

其中Z_j(k)(j＝1,2,…,m_k)为第j个测量值，S为对应新息协方差矩阵，γ_t为马氏距离波门。

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

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

Ω(k)＝[ω_jt]

其中

步骤2.4：确认矩阵包含了当前所有测量与目标可能的关联事件θ_i(i＝1,2,…,n_k)，依据确认矩阵对关联概率β_jt进行计算。对于确认矩阵中每个可能的关联事件θ_i其关联概率为：

而关联概率β_jt为：

其中c为归一化常数，φ(θ_i(k))为k时刻未关联测量数量，τ_j[θ_i(k)]为k时刻关联测量数量,σ_t(θ_i(k))为k时刻关联指示器，量测关联时取1，否者为0，V为波门面积和，P_D为检测概率，N_tj[Z_j(k)]表示k时刻测量Z_j(k)服从高斯分布，为k时刻测量j与目标t关联指示器，两者关联时取1，否则为0，m_k、n_k分别为k时刻测量总数和关联事件总数。

步骤2.5：由关联概率β_jt更新状态量和协方差，其计算公式为：

其中P^c(k|k)＝[1-K(k)H]P(k|k-1)，K(k)为k时刻的卡尔曼增益，H为对应卡尔曼滤波的观测矩阵，β₀(k)＝1-∑β_jt。。

步骤2.6：对目标的跟踪过程是将当前时刻的观测和上一时刻对同一目标的观测进行匹配关联，具体在算法流程中即将当前时刻的状态量与上一时刻同一目标的状态量X(k-1|k-1)关联，从起始时刻不断迭代以生成从起始时刻到当前时刻的跟踪对象的轨迹，即每个时刻的状态量。而由于在跟踪机动目标时，目标的运动模型经常会发生变化，如从匀速变到匀加速等，此时单纯的一个模型来对目标进行跟踪，容易造成跟踪误差过大，甚至跟踪失败。通过交互多模型算法使用匀速直线运动模型、匀加速直线运动模型、匀速圆周运动模型来对目标进行跟踪。即由采用不同运动模型的JPDA滤波器生成的状态量与协方差合成，其公式为：

其中Pⁱ(k|k)i＝1,2,3,分别指使用匀速直线运动模型、匀加速直线运动模型、匀速圆周运动模型的JPDA滤波器产生的状态量和协方差，u_k(i)为k时刻模型i的概率。

对于激光雷达与毫米波雷达双波门IMM-JPDA滤波器参数来说，激光雷达状态量X_l＝[x,y,l,w,h,v_x,v_y]，其中x，y为目标横纵向距离，v_x，v_y为目标横纵向速度，l,w,h为目标长宽高信息且l,w,h在激光雷达观测初始化阶段获得定值，而毫米波雷达状态量X_r＝[x,y,l,w,h,v_x,v_y]中的l,w,h由假定目标类型为轿车，人为赋值确定，其余IMM-JPDA滤波器参数设置都相同。

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

步骤3.1：对于激光雷达与毫米波雷达经由各自的双波门IMM-JPDA滤波器所分别产生的a条与b条轨迹，使用欧氏距离对其当前时刻的状态量中的坐标点距离进行计算记为d_ij(i＝1,2,…,a,j＝1,2,…,b)。将d_ij按a行，b列排列为矩阵，记为D_ab，若a≠b，假设a>b，则使用0元素对D_ab进行补充，使其成为方阵D_aa如下所示：

将D_aa作为代价矩阵即D_aa中的每个元素代表激光雷达所产生的第i条轨迹与毫米波雷达产生的第j条轨迹的距离d_ij，用匈牙利算法进行分配计算匈牙利算法将两个滤波器所产生的各条轨迹依据D_aa中对应的值进行关联，使所匹配的轨迹总的相距距离最小。又因为代价矩阵为方阵，即一定会将所有的轨迹一一对应进行匹配，但在假设a>b的情况下，会有a-b条匹配为虚假匹配，若产生的匹配对应D_aa中的距离为0，则两轨迹的匹配为虚假匹配，判定匹配失败，其余轨迹为成功匹配轨迹，由此完成初步关联。由于匈牙利算法仅能按照欧式距离的大小将欧式距离相距较小的轨迹进行匹配，但若欧式距离相距较小的两目标轨迹并非同一个目标，而是相邻的两个目标，如在道路上并排行驶的两辆车辆，就会造成关联错误。因此对初步关联结果进行二次关联。

在二次关联中，使用欧式距离作为阈值d_t对初步关联轨迹距离d_ij进行检验，如d_ij<d_t则确认关联，若d_ij>d_t则判定轨迹关联失败，两轨迹独立。对于二次关联的阈值d_t设置时，可根据实际情况进行调整，一般设置在的区间内,其中w，l为目标的宽和长。

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

其中X_l为激光雷达产生轨迹状态量，X_r为激光雷达产生轨迹状态量，其中状态量不涉及l,w,h部分，P_r为激光雷达产生轨迹协方差，P_l为毫米波雷达产生轨迹协方差，X_f为融合后的状态量，P_f为融合后的轨迹协方差。

步骤3.3：对融合后状态量X_f＝[x,y]进行扩维加入对应激光雷达轨迹中的l,w,h部分。对于关联失败的轨迹则不进行融合，则假定目标为轿车，以轿车尺寸对其进行扩维。

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

Claims (7)

1.基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，包括：

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

步骤2：对毫米波雷达和激光雷达所采集的前方目标信息分别使用两个独立的双波门滤波器进行机动目标数据关联和跟踪；

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

所述步骤2中的双波门滤波器采用IMM-JPDA设计实现；

所述步骤2的双波门滤波器的具体设计方法包括如下：

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

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

其中，x_d、y_d分别为x、y方向预测值与观测值差值，γ_g为欧式距离波门；

步骤2.3，对于满足第一次筛选的初步待选测量，采用马氏距离作为第二道波门的波门门限，对观测进行再次筛选；

步骤2.4，设置确认矩阵Ω(k)：Ω(k)＝[ω_jt]

其中

对于确认矩阵中每个可能的关联事件θ_i其关联概率为：

步骤2.5，设计关联概率β_jt为：

其中c为归一化常数，φ(θ_i(k))为k时刻未关联测量数量，τ_j[θ_i(k)]为k时刻关联测量数量,σ_t(θ_i(k))为k时刻关联指示器，量测关联时取1，否者为0，V为波门面积和，P_D为检测概率，表示k时刻测量Z_j(k)服从高斯分布，/>为k时刻测量j与目标t关联指示器，两者关联时取1，否则为0，m_k、n_k分别为k时刻测量总数和关联事件总数；

步骤2.6，由关联概率β_jt更新状态量和协方差；

步骤2.7，使用交互多模型算法对机动目标进行跟踪。

2.根据权利要求1所述的基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，所述步骤2.3中的筛选公式如下所示：

其中Z_j(k)为第j个测量值，j＝1,2,…,m_k，S为对应新息协方差矩阵，γ_t为马氏距离波门。

3.根据权利要求1所述的基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，所述步骤2.6中更新状态量和协方差的计算公式为：

其中P^c(k|k)＝[1-K(k)H]P(k|k-1)，K(k)为k时刻的卡尔曼增益。

4.根据权利要求1所述的基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，所述步骤2.7中交互多模型的计算公式为：

其中Pⁱ(k|k)指代使用匀速直线运动模型、匀加速直线运动模型、匀速圆周运动模型的JPDA滤波器产生的状态量和协方差，u_k(i)为k时刻模型i的概率。

5.根据权利要求1所述的基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，所述步骤3中，使用匈牙利算法进行轨迹关联的方法包括：

初次关联：对于激光雷达与毫米波雷达经由各自的双波门IMM-JPDA滤波器所分别产生的a条与b条轨迹，使用欧氏距离对其当前时刻的状态量中的坐标点距离进行计算记为d_ij(i＝1,2,…,a,j＝1,2,…,b)；将d_ij按a行，b列排列为矩阵，记为D_ab，若a>b，则使用0元素对D_ab进行补充，使其成为方阵D_aa如下所示：

将D_aa作为代价矩阵即D_aa中的每个元素代表激光雷达所产生的第i条轨迹与毫米波雷达产生的第j条轨迹的距离d_ij，用匈牙利算法进行分配计算匈牙利算法将两个滤波器所产生的各条轨迹依据D_aa中对应的值进行关联，使所匹配的轨迹总的相距距离最小；在a>b的情况下，会有a-b条匹配为虚假匹配，若产生的匹配对应D_aa中的距离为0，则两轨迹的匹配为虚假匹配，判定匹配失败，其余轨迹为成功匹配轨迹，由此完成初步关联；

二次关联：使用欧式距离作为阈值d_t对初步关联轨迹距离d_ij进行检验，如d_ij<d_t则确认关联，若d_ij>d_t则判定轨迹关联失败，两轨迹独立。

6.根据权利要求5所述的基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，在二次关联的阈值d_t设置时，其设置在的区间内,其中w，l为目标的宽和长。

7.根据权利要求5所述的基于激光雷达与毫米波雷达融合的机动目标跟踪方法，其特征在于，所述步骤3中，在轨迹关联完成后使用凸组合算法对关联轨迹进行融合的方法包括：

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

其中X_l为激光雷达产生轨迹状态量，X_r为激光雷达产生轨迹状态量，其中状态量不涉及l,w,h部分，P_r为激光雷达产生轨迹协方差，P_l为毫米波雷达产生轨迹协方差；

对融合后状态量X_f＝[x,y]进行扩维加入对应激光雷达轨迹中的l,w,h部分，l,w,h为目标的长、宽、高；对于关联失败的轨迹则不进行融合，在扩维时以跟踪目标尺寸对其进行扩维。