一种基于前视相机与毫米波雷达融合的目标跟踪方法与流程

本发明涉及智能汽车技术领域，尤其是涉及一种基于前视相机与毫米波雷达融合的目标跟踪方法。

背景技术：

智能汽车是一个综合的系统，整体可分为环境感知、决策规划和运动控制。环境感知作为智能汽车与周围环境进行信息交换的关键环节，可以让智能汽车更了解其所处的环境，这对之后的决策、规划和控制都至关重要。

传统的跟踪方法大都基于单个传感器。作为智能汽车的主要传感器，前视相机和毫米波雷达各有优缺点。相机如人眼一样可以得到丰富的环境信息，通过算法可以得到物体的形状和大小，但单一特征算法受天气、光照等因素影响较大，并且所得像素信息不能体现距离。毫米波雷达可以精确测量物体的距离和相对速度，并且抗干扰能力强，但无法测量物体的形状和大小。这些问题都有可能会造成传感器在物体跟踪过程中的跟踪丢失或者跟踪错误，从而造成交通事故，引发人员伤亡和财产损失。

因此，如何融合多传感器，利用互补的信息进行物体跟踪，使得跟踪更安全准确，这个问题亟待解决。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于前视相机与毫米波雷达融合的目标跟踪方法。

本发明的目的可以通过以下技术方案来实现：

一种基于前视相机与毫米波雷达融合的目标跟踪方法，包括以下步骤：

1)对设置在前挡风玻璃后视镜位置处的前视相机以及设置在前进气栅位置处的毫米波雷达进行联合标定，使前视相机和毫米波雷达在空间和时间上对准；

2)根据毫米波雷达测量得到的相对速度，并通过阈值区分动态目标和静态障碍物后，对动态目标进行跟踪，通过独立空间中的可行事件的概率滤除极小概率事件后，采用卡尔曼滤波对检测到的目标状态进行状态更新，获取毫米波雷达对目标的跟踪航迹；

3)通过前视相机获取目标的位置和速度信息，区分动态目标和静态障碍物后对动态目标进行跟踪，通过独立空间中的可行事件的概率滤除极小概率事件后，采用卡尔曼滤波对检测到的目标状态进行状态更新，获取前视相机对目标的跟踪航迹；

4)将毫米波雷达和前视相机的跟踪目标状态进行融合，最终获取融合后的目标状态。

所述的步骤2)具体包括以下步骤：

21)对毫米波雷达探测到的动态目标进行跟踪，对于测量值落入不同的波门或同一波门包含多个目标的测量值，根据联合概率数据关联生成确认矩阵ω，用以表示观测与目标之间的关系：

其中，t为目标，j为观测，m为目标总数，t为观测总数，ωjt为1时表示目标t和观测j之间存在关联，为0时表示目标t和观测j之间不存在关联。

22)对确认矩阵ω进行空间划分，获取每个独立空间中对应的确认矩阵ωi，i＝1,2,...,n，n为空间划分后独立空间的个数；

23)对每个独立空间中对应的确认矩阵ωi计算关联概率，并设置阈值剔除极小概率事件；

24)根据毫米波雷达关联上的目标建立航迹；

25)对关联上的目标采用扩展卡尔曼滤波进行状态更新，毫米波探测到的目标状态为相对距离、水平角度和相对速度，将毫米波探测到的目标状态转换为横向和纵向的相对距离及相对速度。

所述的步骤22)具体包括以下步骤：

对确认矩阵ω中分别对同一目标为1的行向量进行或运算，并将或运算结果中相等的行向量只保留一个，将剩余不能消除的向量重新组成一个新矩阵，新矩阵的行数n即为独立空间的个数，新矩阵每行中的值为1代表所在列数的目标相交区域。

所述的步骤23)具体包括以下步骤：

对确认矩阵ωi进行逐行扫描，每行中仅选出第一个为1的值作为可行矩阵的元素，保证除第一列之外，可行矩阵中每一列只有一个值为1，并且计算关联概率βjt，并根据关联概率判断是否关联：

其中，z^k为全部有效回波的集合，θ为事件，即观测与目标的匹配，为1时表示事件θ为可行事件，为0时表示事件θ不为可行事件，p{θ/z^k}为关联事件的后验概率。

所述的步骤24)具体包括以下步骤：

若连续三次跟踪关联上同一目标物体，或者在已经连续两次关联上后，并且在之后的三次跟踪中至少关联上一次的目标物体，对其建立初始航迹，未关联上的目标物体采用上一时刻的速度按照匀速推算当前时刻的位置，若之后的三次关联都未关联上，则放弃建立航迹。

所述的步骤25)中，对于已经确认的航迹，当连续两次没有出现点迹，则在第三次探测中扩大波门捕获丢失的目标，若未捕获到，则判断跟踪目标消失，撤销航迹。

所述的步骤4)具体包括以下步骤：

41)分别获取毫米波雷达和前视相机跟踪目标状态的协方差，并计算每个时刻各个状态的马氏距离，将各个状态的马氏距离小于设定阈值且加权和最小的两个目标认定为同一目标；

42)对毫米波雷达和前视相机跟踪目标状态根据协方差加权，获取跟踪融合后的目标状态，并存入数据库。

所述的步骤41)中，认定为同一目标的具体表达式为：

其中，mx,my,mvx,mvy分别为x和y方向上距离和速度的马氏距离，mx0,my0,mvx0,mvy0,m0分别为阈值，a，b，c，d分别为权重。

所述的步骤42)中，加权融合的具体表达式为：

其中，x,y,vx,vy分别为融合后的x,y方向上的距离和速度，xr,yr,vxr,vyr分别为毫米波雷达得到的目标在x,y方向上的距离和速度，xc,yc,vxc,vyc分别为前视相机得到的目标在x,y方向上的距离和速度，e，f分别为权重。

所述的步骤4)中，若毫米波雷达和前视相机中有一个没有检测到目标，则直接将该被单个传感器检测到的目标状态存入数据库。

与现有技术相比，本发明具有以下优点：

一、多传感器跟踪：相比单个传感器跟踪，多传感器融合弥补了单个传感器漏检、误检、跟踪失败、状态探测不精确等缺陷，利用冗余的信息增加了智能汽车的安全性；

二、减少计算量：相比传统的联合概率数据关联方法，增加了空间划分的方法，在每个独立空间中拆分确认矩阵，并删除小概率事件，可以减少计算量。

附图说明

图1为本发明的方法流程图。

图2为跟踪过程流程图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例：

本例主要采用的传感器是前视相机和毫米波雷达，分别安装在车辆挡风玻璃后视镜位置和前进气栅位置。采用一种分布式融合的方式，对两种传感器分别跟踪的物体进行融合。本发明的关键在于如何利用单个传感器对多目标进行跟踪，以及如何将相机的跟踪目标和雷达的跟踪目标进行融合。

本发明提供了一种基于空间划分的联合概率数据关联的跟踪方法，利用卡尔曼滤波进行状态估计，生成单个传感器对目标的局部跟踪航迹，再对两个局部航迹计算马氏距离，进行融合，如图1和图2所示，具体包括以下步骤：

步骤1：对安装前挡风玻璃上后视镜位置的前视相机和前进气栅位置的毫米波雷达进行联合标定，使两个传感器在空间和时间上对准；

步骤2：对毫米波输入当前车速和横摆角速度信息，过滤掉空信号、虚假信号，再根据测量得到的相对速度，设置阈值，区分动态和静态障碍物；

步骤3：对毫米波雷达探测到的动态目标进行跟踪，对测量值落入不同的波门或者某个波门含有多个其他目标的测量值，根据联合概率数据关联生成确认矩阵ω，来表达测量与目标之间的关系：

其中

其中，ωjt代表目标和观测的对应关系；

步骤4：对多目标跟踪系统中的有效量测与目标之间进行空间划分，针对每一个目标的确认矩阵中为1的行向量进行或运算，再将结果中相等的行向量消除，只保留一个，不能消除的重新组成一个新的矩阵，行数n表示独立空间的个数，每行中的1代表所在列数的目标相交区域；

步骤5：在每个独立空间中生成各自的确认矩阵ωi(i＝1,2,...,n)，对ωi进行拆分，得到可行矩阵；

具体拆分的方法为：

对确认矩阵ωi进行逐行扫描，每行仅选出一个1作为可行矩阵的元素，除第一列之外，可行矩阵中每一列只能有一个1；并计算关联概率：

其中，βjt代表第j个测量和第t个目标的关联概率，z^k代表全部有效回波的集合；为了避免确认矩阵的拆分导致计算量过大，设置概率阈值，舍弃概率小于阈值的事件；

步骤6：若连续三次关联上同一物体，或者已经连续两次关联上，在之后的三次跟踪中至少关联上一次的，建立初始航迹，未关联上的用上一时刻的速度按照匀速推算这一时刻的位置，若之后三次都未关联上则不建立航迹；

步骤7：毫米波探测到目标的状态是相对距离、水平角度和相对速度，对关联上的目标用扩展卡尔曼滤波进行状态更新，转换为横向和纵向的相对距离和相对速度；

zt＝htxt+vt

步骤8：对于一条已经确认的航迹，若已经连续两次没有出现点迹，则在第三次探测中扩大波门去捕获丢失的目标，若还未捕获到，则判断跟踪目标消失，撤销航迹，把目标从数据库中删除，其中未探测到的用上一时刻的速度按照匀速推算这一时刻的位置；

步骤9：假设前视相机已经能够检测目标，并获得目标的位置和速度信息，对前视相机探测到的目标进行滤波，过滤掉虚假信号和跳动过大的目标，再区分动态和静态目标；

步骤10：对相机过滤之后的目标做如步骤3～步骤8，对关联上的目标用卡尔曼滤波进行状态更新，得到横向和纵向的距离和速度；

步骤11：通过步骤7和步骤10分别得到两种传感器下跟目标状态的协方差，分别计算每个时刻各个状态的马氏距离，选取各个状态的马氏距离小于一定阈值且加权和最小的两个目标，认为是两个传感器探测到同一物体，即两个局部航迹的匹配；

步骤12：对步骤11中关联上的两个传感器探测到的状态，按照协方差加权，得到两个传感器跟踪融合的目标状态，存入数据库；

步骤13：若步骤11中的判断条件不满足，即两个传感器中有一个未检测到该目标，直接把该被单个传感器检测到的目标信息存入数据库。