CN111932580A - 一种基于卡尔曼滤波与匈牙利算法的道路3d车辆跟踪方法及系统 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法及系统，属于智能汽车感知技术领域，包括步骤1、建立检测模块对当前帧t进行目标检测，获取第t帧的目标检测集合；步骤2、建立3D卡尔曼滤波器模型对当前帧t进行目标状态预测；步骤3、建立数据关联模块，利用匈牙利算法将检测结果与预测的跟踪目标相匹配；步骤4、建立3D卡尔曼滤波器模型根据关联结果更新目标状态。步骤5、建立轨迹管理模块对轨迹进行更新。本发明提出的将结合卡尔曼滤波状态更新和匈牙利算法应用于智能车辆的3D目标跟踪，可以快速的发现、定位和跟踪目标车辆，并且跟踪精度非常高。

Description

一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法及 系统

技术领域

本发明属于智能汽车感知领域，具体涉及一种基于卡尔曼滤波与匈牙利算法的道路3D车辆识别及跟踪的方法和系统。

背景技术

智能车是一个包括感知、决策及控制的复杂系统，环境感知是路径规划和决策控制的重要前提，3D车辆跟踪是环境感知的关键内容之一，对于自主驾驶的路径规划具有重要影响。

目前有大量的目标跟踪算法被提出并在智能交通系统中得到广泛的应用，目前大多数的目标跟踪和检测算法，处理对象都集中在RGB图像上，比如由Girshick相继提出的Fast RCNN、Faster RCNN、YOLO等，这些算法都是比较成功的基于深度学习的目标检测算法。由xiang等提出的一种基于马尔可夫决策(MDP)框架的在线多目标跟踪方法，但只能在道路环境不复杂的情况下取得较好的结果。但随着立体视觉的兴起，人们致力于研究目标3D框跟踪算法的可行性。

常用的车辆跟踪方法包括：基于区域跟踪算法，假设车辆是由一个个像素点形成的连通块，根据连通块的特征去计算与检测出的连通块的特征的相似度确定跟踪目标；基于模型的跟踪算法，根据建立好的目标模型库与检测出来的运动目标进行匹配以达到跟踪的目的；基于特征匹配跟踪算法：利用运动目标不变或不易受外界因素干扰的特征在相邻图像帧中进行目标匹配。但如果面对城市复杂的交通环境，在解决目标遮挡，复杂运动建模，权衡计算复杂度与计算精确度等问题上，上述方法仍存在一定的不足。

发明内容

针对上述存在的问题，提供一种改进的基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法及系统，实现复杂城市道路环境下3D车辆跟踪。

本发明采用的技术方案是：一种改进的基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，该方法包括以下步骤：

步骤1、建立检测模块对当前帧t进行目标检测，获取第t帧的目标检测集合；

步骤2、建立3D卡尔曼滤波器模型对当前帧t进行目标状态预测；

步骤3、建立数据关联模块将检测结果与预测的跟踪目标相匹配；

步骤4、建立3D卡尔曼滤波器模型根据关联结果更新目标状态。

步骤5、建立轨迹管理模块对轨迹进行更新；

进一步，步骤1的具体实现包括：

基于预设的车辆检测器，进行视频帧的3D目标检测处理，获取目标检测结果，得到车辆目标的检测集合

其中

表示在t帧时刻检测的第n个目标车辆(共n个目标车辆)，

检测结果为车辆目标的3D包围盒，所述3D包围盒的信息包括：中心坐标(x,y,z)，尺度大小(h,w,l)，方向角θ，检测置信度s。

进一步，步骤2的具体实现包括：

为预测下一帧车辆目标的状态，采用恒定速度模型近似车辆目标的帧间位移，建立状态变量T＝{x,y,z,θ,h,w,h,v_x,v_y,v_z}，其中v_x,v_y,v_z表示车辆目标在三维空间中的运动速度；

假设t-1的所有目标车辆轨迹为

其中

表示t-1时刻第m个目标车辆轨迹数(共m个目标车辆轨迹数)；

基于恒定速度模型：

则t时刻预测的每个跟踪目标车辆轨迹状态为T_t-1|t＝{x_t-1|t,y_t-1|t,z_t-1|t,θ,h,w,h,v_x,v_y,v_z}。

进一步，步骤3的具体实现包括：

步骤301：确定第i个检测目标与第j个预测的跟踪目标的关联系数C_ij；

通过计算第i个检测目标车辆D_t和第j个预测跟踪目标T_t之间的IOU_3D(Intersection over Union)确定两者的关联系数C_ij，C_ij越大表示两者关联程度越高，即第i检测目标可能属于第j个预测跟踪目标。

所述关联系数C_ij的计算公式为：

其中，V_i为第i个检测目标3D框的体积，V_j为第j个预测的跟踪目标3D框的体积，V_ij为检测目标3D框与预测目标3D框间的相交体积；

步骤302：使用匈牙利算法匹配第i个检测目标车辆D_t和第j个预测跟踪目标车辆T_t；

为每一个检测/预测目标定义一个二值系数X_ij：当IOU_3D大于设定的阈值0.3时，第i个检测目标匹配给第j个预测跟踪目标，则X_ij＝1，否则X_ij＝0，建立的匹配模型表达式为：

由关联系数C_ij组成的矩阵C是匹配模型的系数矩阵，矩阵C大小为n×m，在系数矩阵C找到一些C_ij使整体效率z最大，其中每一行/列至多有一个元素被选中；

步骤303：输出一组匹配成功的检测目标

和匹配成功的预测跟踪目标

和匹配失败的检测目标

以及匹配失败的预测跟踪目标

进一步，步骤4的具体实现包括：

步骤401：建立3D卡尔曼滤波器模型根据关联结果初始目标状态更新；

在跟踪过程中，设目标状态方程为：

T_t＝FT_t-1+w_t-1

其中，F为状态转移矩阵，w_t-1为t-1时刻的过程噪声，服从正态分布，其均值为0，协方差矩阵为Q；

设目标观测方程为：

Z_t＝HT_t+v_t

其中，其中，H为观测矩阵，v_t为t时刻的观测噪声，服从正太分布，其均值为0，协方差矩阵为R。

根据T的形式可以得出：

根据运动模型，先验状态协方差矩阵为：

P_t|t-1＝FP_t-1F^T+Q_t

其中，P_t-1为t-1时刻更新预测后的状态协方差矩阵。

根据卡尔曼滤波器的计算规则，更新环节车辆目标状态表达式为：T_t＝T_t|t-1+K_t(Z_t-FT_t|t-1)；

其中，K_t为残余增益，其计算公式如下：

K_t＝P_t|t-1H^T(HP_t|t-1H^T+R)^-1

步骤402：建立方向校准模块修正初始目标状态；

当检测目标

与跟踪目标轨迹

之间的方向角大于

时，则目标轨迹

的方向角加π，此时跟踪目标轨迹

方向角可以近似于检测目标

达到一致。

进一步，步骤5的具体实现包括：

步骤501：新的轨迹生成处理

对于当前帧未匹配的车辆目标，若下一帧仍被检测到，则创建一个新的跟踪轨迹

并初始化其状态为(x,y,z,θ,h,w,h,0,0,0)；

步骤502：现存轨迹丢失、终止处理：

若轨迹丢失，继续保持追踪3帧该轨迹，若仍未被匹配，则做轨迹终止处理

本发明提出的基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪系统，包括检测模块、3D卡尔曼滤波器模型、数据关联模块、3D卡尔曼滤波器模型以及轨迹管理模块；

所述检测模块用于对当前帧t进行目标检测，获取第t帧的目标检测集合；

所述3D卡尔曼滤波器模型用于对当前帧t进行目标状态预测；

所述数据关联模块用于将检测结果与预测的跟踪目标相匹配；

所述3D卡尔曼滤波器模型还用于根据关联结果更新目标状态；

所述轨迹管理模块用于对轨迹进行更新。

进一步，所述检测模块对当前帧t进行目标检测的具体内容包括：

基于预设的车辆检测器，进行视频帧的3D目标检测处理，获取目标检测结果，得到车辆目标的检测集合

其中

表示在t帧时刻检测的第n个目标车辆(共n个目标车辆)；

检测结果是车辆目标的3D包围盒，所述3D包围盒的信息包括：中心坐标(x_3D,y_3D,z_3D)，尺度大小(h_3D,w_3D,l_3D)，方向角θ_3D，检测置信度s_3D；

所述3D卡尔曼滤波器模型对当前帧t进行目标状态预测的具体内容包括：

为了预测下一帧车辆目标的状态，采用恒定速度模型近似车辆目标的帧间位移，建立状态变量T＝{x,y,z,θ,h,w,h,v_x,v_y,v_z}，其中v_x,v_y,v_z表示车辆目标在三维空间中的运动速度；

假设t-1的所有目标车辆轨迹为

其中

表示t-1时刻第m个目标车辆轨迹数(共m个目标车辆轨迹数)；

基于恒定速度模型：

则t时刻预测的每个跟踪目标车辆轨迹状态为T_t-1|t＝{x_t-1|t,y_t-1|t,z_t-1|t,θ,h,w,h,v_x,v_y,v_z}；

所述数据关联模块将检测结果与预测的跟踪目标相匹配的具体内容包括：

确定第i个检测目标与第j个预测的跟踪目标的关联系数C_ij；

通过计算第i个检测目标车辆D_t和第j个预测跟踪目标T_t之间的3D IOU确定两者的关联系数C_ij，C_ij越大表示两者关联程度越高，即第i检测目标可能属于第j个预测跟踪目标；

所述关联系数C_ij的计算公式为：

其中，V_i为第i个检测目标3D框的体积，V_j为第j个预测的跟踪目标3D框的体积，V_ij为检测目标3D框与预测目标3D框间的相交体积；

使用匈牙利算法匹配第i个检测目标车辆D_t和第j个预测跟踪目标车辆T_t；

为每一个检测/预测目标定义一个二值系数X_ij：当IOU_3D大于设定的阈值时，第i个检测目标匹配给第j个预测跟踪目标，则X_ij＝1，否则X_ij＝0，建立匹配模型表达式为：

由关联系数C_ij组成的矩阵C是匹配问题的系数矩阵，矩阵C大小为n×m，在系数矩阵C找到一些C_ij使整体效率z最大，其中每一行/列至多有一个元素被选中；

数据关联模块输出一组匹配成功的检测目标

匹配成功的预测跟踪目标

匹配失败的检测目标

以及匹配失败的预测跟踪目标

所述3D卡尔曼滤波器模型根据关联结果更新目标状态的具体内容包括：

根据关联结果更新初始目标状态：

在跟踪过程中，设状态方程为：

T_t＝F_tT_t-1+w_t-1

其中，F为状态转移矩阵，w为过程噪声，其均值为0，协方差矩阵为Q；

设观测方程为：

Z_t＝H_tT_t+v_t

其中，H为观测矩阵，v为观测噪声,其均值为0，协方差矩阵为R；

根据运动模型，状态协方差矩阵为：

根据T得出：

根据卡尔曼滤波器的计算规则，更新环节车辆目标状态表达式为：T_t＝T_t|t-1+K_t(z_t-FT_t|t-1)

其中，K为残余增益，其计算公式如下：

当检测目标

与跟踪目标轨迹

之间的方向角大于

时，则目标轨迹

的方向角加π，此时跟踪目标轨迹

方向角可以近似于检测目标

达到一致；

所述轨迹管理模块用于对轨迹进行更新的具体内容包括：

新的轨迹生成处理：对于当前帧未匹配的车辆目标，若下一帧仍被检测到，创建一个新的跟踪轨迹

并初始化其状态为(x,y,z,θ,h,w,h,0,0,0)；

现存轨迹丢失、终止处理：若轨迹丢失，继续保持追踪3帧该轨迹，若仍未被匹配，做轨迹终止处理

本发明的有益效果：

本发明提出的将结合卡尔曼滤波状态更新和匈牙利算法应用于智能车辆的3D目标跟踪，可以快速的发现、定位和跟踪目标车辆，并且跟踪精度非常高。

附图说明

图1为本发明一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法流程图；

图2为第1副道路场景3D车辆跟踪图像；

图3为第2副道路场景3D车辆跟踪图像；

具体实施方式

为了使本发明的目的、技术方案解释的更加清楚明白，以下结合说明书附图对本发明的具体实施方式进一步详细的说明。

本发明提出的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，如图1所示，为本申请实施提供的一种道路3D车辆跟踪的流程图，主要包含以下步骤：

步骤1、建立检测模块对当前帧t进行目标检测，获取第t帧的目标检测集合；

基于预设的车辆检测器，进行视频帧的3D目标检测处理，获取目标检测结果，得到车辆目标的检测集合

其中

表示在t帧时刻检测的第n个目标车辆(共n个目标车辆)。

检测结果是车辆目标的3D包围盒，所述3D包围盒的信息包括：中心坐标(x,y,z)，尺度大小(h,w,l)，方向角θ，检测置信度s。

步骤2、建立3D卡尔曼滤波器模型对当前帧t进行目标状态预测；

为了预测下一帧车辆目标的状态，采用恒定速度模型近似车辆目标的帧间位移，建立状态变量T＝{x,y,z,θ,h,w,l,v_x,v_y,v_z}，其中v_x,v_y,v_z表示车辆目标在三维空间中的运动速度。

假设t-1时刻的所有目标车辆轨迹为

其中

表示t-1时刻第m个目标车辆轨迹数(共m个目标车辆轨迹数)。

基于恒定速度模型：

则t时刻预测的每个跟踪目标车辆轨迹状态为T_t-1|t＝{x_t-1|t,y_t-1|t,z_t-1|t,θ,h,w,h,v_x,v_y,v_z}。

步骤3、建立数据关联模块将检测结果与预测跟踪目标相匹配；

步骤301：确定第i个检测目标与第j个预测的跟踪目标的关联系数C_ij；

通过计算第i个检测目标车辆D_t和第j个预测跟踪目标T_t之间的IOU_3D(Intersection over Union)确定两者的关联系数C_ij，C_ij越大表示两者关联程度越高，即第i检测目标可能属于第j个预测跟踪目标。

所述关联系数C_ij的计算公式为：

其中，V_i为第i个检测目标3D框的体积，V_j为第j个预测的跟踪目标3D框的体积，V_ij为检测目标3D框与预测目标3D框间的相交体积。

步骤302：使用匈牙利算法匹配第i个检测目标车辆D_t和第j个预测跟踪目标车辆T_t；

为每一个检测/预测目标定义一个二值系数X_ij：当IOU_3D大于设定的阈值0.3时，第i个检测目标匹配给第j个预测跟踪目标，则X_ij＝1，否则X_ij＝0，。

建立匹配模型表达式为：

由关联系数C_ij组成的矩阵C是匹配问题的系数矩阵，矩阵C大小为n×m，在系数矩阵C找到一些C_ij使整体效率z最大，其中每一行/列至多有一个元素被选中。

步骤303：最终数据关联模块输出一组匹配成功的检测目标

和匹配成功的预测跟踪目标

和匹配失败的检测目标

以及匹配失败的预测跟踪目标

其中

表示在t帧时刻匹配成功的第c个检测目标(共c个匹配成功的检测目标)；

表示在t帧时刻匹配成功的第c个预测跟踪目标(共c个匹配成功的预测跟踪目标)；D_unmatch表示在t帧时刻匹配失败的第n-c个检测目标(共n-c个匹配失败的检测目标)；T_unmatch表示在t帧时刻匹配失败的第m-c个预测跟踪目标(共m-c个匹配失败的预测跟踪目标)。

步骤4、建立3D卡尔曼滤波器模型根据关联结果更新目标状态；

步骤401：建立3D卡尔曼滤波器模型根据关联结果更新初始目标状态；

在跟踪过程中，根据卡尔曼滤波器的原理，设目标状态方程为：

T_t＝FT_t-1+w_t-1

其中，F为状态转移矩阵，w_t-1为t-1时刻的过程噪声，服从正态分布，其均值为0，协方差矩阵为Q；

同时设目标观测方程为：

Z_t＝HT_t+v_t

其中，H为观测矩阵，v_t为t时刻的观测噪声，服从正太分布，其均值为0，协方差矩阵为R。

根据T的形式可以得出：

根据运动模型，先验状态协方差矩阵为：

P_t|t-1＝FP_t-1F^T+Q

其中，P_t-1为t-1时刻更新预测后的状态协方差矩阵。

根据卡尔曼滤波器的计算规则，更新环节车辆目标状态表达式为：T_t＝T_t|t-1+K_t(Z_t-FT_t|t-1)

其中，K_t为残余增益，其计算公式如下：

K_t＝P_t|t-1H^T(HP_t|t-1H^T+R)^-1

步骤402：建立方向校准模块修正初始目标状态；

对于由步骤401建立的卡尔曼滤波器模型不能精确得定位目标3D方向，因此检测目标

的方向可以近似与跟踪目标轨迹

的方向完全相反，当检测目标

与跟踪目标轨迹

之间的方向角大于

时，则目标轨迹

的方向角加π，此时跟踪目标轨迹

方向角可以近似于检测目标

达到一致，有效提高车辆目标3D跟踪的方向估计。

步骤5、建立轨迹管理模块对轨迹进行更新；

步骤501：新的轨迹生成处理；

对于当前帧未匹配的车辆目标，若下一帧仍被检测到，创建一个新的跟踪轨迹

并初始化其状态为(x,y,z,θ,h,w,h,0,0,0)。

步骤502：现存轨迹丢失、终止处理；

由于所有的现存轨迹都可能终止，为避免误报的跟踪，若轨迹丢失，继续保持追踪3帧该轨迹，若仍未被匹配，做轨迹终止处理

图2、图3给出了本实例所述方法对原始道路场景车辆预测跟踪得到的结果图，显示表明，本发明实现了对道路场景的有效3D车辆跟踪，并且跟踪准确度非常高。

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

Claims (8)

1.一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，其特征在于，包括如下步骤：

步骤1、当前帧t进行目标检测，获取第t帧的目标检测集合；

步骤2、对当前帧t进行目标状态预测；

步骤3、将检测结果与预测的跟踪目标相匹配；

步骤4、根据匹配结果更新目标状态；

步骤5、对目标状态轨迹进行更新。

2.根据权利要求1所述的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，其特征在于，步骤1的具体实现包括：

基于预设的车辆检测器，进行视频帧的3D目标检测处理，获取目标检测结果，得到车辆目标的检测集合

其中

表示在t帧时刻检测的第n个目标车辆(共n个目标车辆)，

检测结果为车辆目标的3D包围盒，所述3D包围盒的信息包括：中心坐标(x,y,z)，尺度大小(h,w,l)，方向角θ，检测置信度s。

3.根据权利要求1所述的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，其特征在于，步骤2的具体实现包括：

为预测下一帧车辆目标的状态，采用恒定速度模型近似车辆目标的帧间位移，建立状态变量T＝{x,y,z,θ,h,w,h,v_x,v_y,v_z}，其中v_x,v_y,v_z表示车辆目标在三维空间中的运动速度；

假设t-1的所有目标车辆轨迹为

其中

表示t-1时刻第m个目标车辆轨迹数(共m个目标车辆轨迹数)；

基于恒定速度模型：

则t时刻预测的每个跟踪目标车辆轨迹状态为T_t-1|t＝{x_t-1|t,y_t-1|t,z_t-1|t,θ,h,w,h,v_x,v_y,v_z}。

4.根据权利要求1所述的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，其特征在于，步骤3的具体实现包括：

步骤301：确定第i个检测目标与第j个预测的跟踪目标的关联系数C_ij；

通过计算第i个检测目标车辆D_t和第j个预测跟踪目标T_t之间的IOU_3D确定两者的关联系数C_ij，C_ij越大表示两者关联程度越高，即第i检测目标可能属于第j个预测跟踪目标。

所述关联系数C_ij的计算公式为：

其中，V_i为第i个检测目标3D框的体积，V_j为第j个预测的跟踪目标3D框的体积，V_ij为检测目标3D框与预测目标3D框间的相交体积；

步骤302：使用匈牙利算法匹配第i个检测目标车辆D_t和第j个预测跟踪目标车辆T_t；

为每一个检测/预测目标定义一个二值系数X_ij：当IOU_3D大于设定的阈值0.3时，第i个检测目标匹配给第j个预测跟踪目标，则X_ij＝1，否则X_ij＝0，建立的匹配模型表达式为：

由关联系数C_ij组成的矩阵C是匹配模型的系数矩阵，矩阵C大小为n×m，在系数矩阵C找到一些C_ij使整体效率z最大，其中每一行/列至多有一个元素被选中；

步骤303：输出一组匹配成功的检测目标

和匹配成功的预测跟踪目标

和匹配失败的检测目标

以及匹配失败的预测跟踪目标

5.根据权利要求1所述的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，其特征在于，步骤4的具体实现包括：

步骤401：建立3D卡尔曼滤波器模型根据关联结果初始目标状态更新；

在跟踪过程中，设目标状态方程为：

T_t＝FT_t-1+w_t-1

其中，F为状态转移矩阵，w_t-1为t-1时刻的过程噪声，服从正态分布，其均值为0，协方差矩阵为Q；

设目标观测方程为：

Z_t＝HT_t+v_t

其中，H为观测矩阵，v_t为t时刻的观测噪声，服从正太分布，其均值为0，协方差矩阵为R。

根据T可以得出：

根据运动模型，先验状态协方差矩阵为：

P_t|t-1＝FP_t-1F^T+Q

其中，P_t-1为t-1时刻更新预测后的状态协方差矩阵。

根据卡尔曼滤波器的计算规则，更新环节车辆目标状态表达式为：T_t＝T_t|t-1+K_t(Z_t-FT_t|t-1)；

其中，K_t为残余增益，其计算公式如下：

K_t＝P_t|t-1H^T(HP_t|t-1H^T+R)^-1

步骤402：建立方向校准模块修正初始目标状态；

当检测目标

与跟踪目标轨迹

之间的方向角大于

时，则目标轨迹

的方向角加π，此时跟踪目标轨迹

方向角可以近似于检测目标

达到一致。

6.根据权利要求1所述的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪方法，其特征在于，步骤5的具体实现包括：

步骤501：新的轨迹生成处理

对于当前帧未匹配的车辆目标，若下一帧仍被检测到，则创建一个新的跟踪轨迹

并初始化其状态为(x,y,z,θ,h,w,h,0,0,0)；

步骤502：现存轨迹丢失、终止处理：

若轨迹丢失，继续保持追踪3帧该轨迹，若仍未被匹配，则做轨迹终止处理

7.一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪系统，其特征在于，包括检测模块、3D卡尔曼滤波器模型、数据关联模块、3D卡尔曼滤波器模型以及轨迹管理模块；

所述检测模块用于对当前帧t进行目标检测，获取第t帧的目标检测集合；

所述3D卡尔曼滤波器模型用于对当前帧t进行目标状态预测；

所述数据关联模块用于将检测结果与预测的跟踪目标相匹配；

所述3D卡尔曼滤波器模型还用于根据关联结果更新目标状态；

所述轨迹管理模块用于对轨迹进行更新。

8.根据权利要求7所述的一种基于卡尔曼滤波与匈牙利算法的道路3D车辆跟踪系统，其特征在于，所述检测模块对当前帧t进行目标检测的具体内容包括：

基于预设的车辆检测器，进行视频帧的3D目标检测处理，获取目标检测结果，得到车辆目标的检测集合

其中

表示在t帧时刻检测的第n个目标车辆(共n个目标车辆)；

检测结果是车辆目标的3D包围盒，所述3D包围盒的信息包括：中心坐标(x_3D,y_3D,z_3D)，尺度大小(h_3D,w_3D,l_3D)，方向角θ_3D，检测置信度s_3D；

所述3D卡尔曼滤波器模型对当前帧t进行目标状态预测的具体内容包括：

为了预测下一帧车辆目标的状态，采用恒定速度模型近似车辆目标的帧间位移，建立状态变量T＝{x,y,z,θ,h,w,h,v_x,v_y,v_z}，其中v_x,v_y,v_z表示车辆目标在三维空间中的运动速度；

假设t-1的所有目标车辆轨迹为

其中

表示t-1时刻第m个目标车辆轨迹数(共m个目标车辆轨迹数)；

基于恒定速度模型：

则t时刻预测的每个跟踪目标车辆轨迹状态为T_t-1|t＝{x_t-1|t,y_t-1|t,z_t-1|t,θ,h,w,h,v_x,v_y,v_z}；

所述数据关联模块将检测结果与预测的跟踪目标相匹配的具体内容包括：

确定第i个检测目标与第j个预测的跟踪目标的关联系数C_ij；

通过计算第i个检测目标车辆D_t和第j个预测跟踪目标T_t之间的3D IOU确定两者的关联系数C_ij，C_ij越大表示两者关联程度越高，即第i检测目标可能属于第j个预测跟踪目标；

所述关联系数C_ij的计算公式为：

其中，V_i为第i个检测目标3D框的体积，V_j为第j个预测的跟踪目标3D框的体积，V_ij为检测目标3D框与预测目标3D框间的相交体积；

使用匈牙利算法匹配第i个检测目标车辆D_t和第j个预测跟踪目标车辆T_t；

为每一个检测/预测目标定义一个二值系数X_ij：当IOU_3D大于设定的阈值时，第i个检测目标匹配给第j个预测跟踪目标，则X_ij＝1，否则X_ij＝0，建立匹配模型表达式为：

由关联系数C_ij组成的矩阵C是匹配问题的系数矩阵，矩阵C大小为n×m，在系数矩阵C找到一些C_ij使整体效率z最大，其中每一行/列至多有一个元素被选中；

数据关联模块输出一组匹配成功的检测目标

匹配成功的预测跟踪目标

匹配失败的检测目标

以及匹配失败的预测跟踪目标

所述3D卡尔曼滤波器模型根据关联结果更新目标状态的具体内容包括：

根据关联结果更新初始目标状态：

在跟踪过程中，设状态方程为：

T_t＝F_tT_t-1+w_t-1

其中，F为状态转移矩阵，w为过程噪声，其均值为0，协方差矩阵为Q；

设观测方程为：

Z_t＝H_tT_t+v_t

其中，H为观测矩阵，v为观测噪声,其均值为0，协方差矩阵为R；

根据运动模型，状态协方差矩阵为：

根据T得出：

根据卡尔曼滤波器的计算规则，更新环节车辆目标状态表达式为：T_t＝T_t|t-1+K_t(z_t-FT_t|t-1)

其中，K为残余增益，其计算公式如下：

当检测目标

与跟踪目标轨迹

之间的方向角大于

时，则目标轨迹

的方向角加π，此时跟踪目标轨迹

方向角可以近似于检测目标

达到一致；

所述轨迹管理模块用于对轨迹进行更新的具体内容包括：

新的轨迹生成处理：对于当前帧未匹配的车辆目标，若下一帧仍被检测到，创建一个新的跟踪轨迹

并初始化其状态为(x,y,z,θ,h,w,h,0,0,0)；

现存轨迹丢失、终止处理：若轨迹丢失，继续保持追踪3帧该轨迹，若仍未被匹配，做轨迹终止处理

Images