1.一种基于多传感器融合的车辆目标跟踪算法,其特征在于,包括:S1、多传感器模块获取传感器数据;所述传感器数据包含状态数据和观测数据;
S2、目标检测与跟踪模块为多车目标分配所述传感器数据进行检测与跟踪;根据有无观测值进行状态更新,获取后验状态数据和后验协方差数据以进行空间配准;目标检测与跟踪模块为多车目标分配观测数据,时刻初始化目标车辆 的轨迹,根据两个时刻同一位置的状态信息进行关联,将 分配给 跟踪目标; 表示当前时刻 的状态向量,设计含未知参数的状态估计器;
如果该时刻有已经关联成功的观测值,使用含未知参数状态估计器进行更新,解析未知参数向量;状态估计的形式参见(4)(5)式: (4)若上一时刻后验误差 是无偏的,则通过给定测量向量 ,标称值 来获得的无偏估计值;
(5)
在 时刻, 表示预测状态, 表示预测输出, 是 时刻的估计器增益,是预测输出中的误差,近似值为(6)式;
(6)
其中 和 是相对于Y和未知参数的线性化一阶导;设计的最小方差估计器增益为,其值为(7)式:
(7)
其中 是可逆的,先验协方差 , 为状态噪声协方差;R为观测噪声协方差矩阵;
状态估计的后验协方差为:
(8)
如果该时刻没有关联成功的观测值,则采用 更新;
S3、目标检测与跟踪模块将所述后验状态数据和后验协方差数据输入多传感器融合模块,对多车目标中各分目标进行时间配准、轨迹关联和轨迹融合。
2.根据权利要求1所述的一种基于多传感器融合的车辆目标跟踪算法,其特征在于,步骤S1中,所述状态数据为状态向量,所述观测数据为观测向量;所述状态向量和所述观测向量具体形式为: , 其中,X表示状态向量;Y表示观测向量; 和 表示目标车辆在全局坐标下的位置; 表示车辆的速度; 表示车辆的方位角;表示转向角的角速度; 为毫米波雷达极坐标中目标或杂波的距离测量值; 为毫米波雷达极坐标中目标或杂波的方位角测量值; 为通过多普勒效应获得的速度标量; 为通过多普勒效应获得的方向标量。
3.根据权利要求2所述的一种基于多传感器融合的车辆目标跟踪算法,其特征在于,步骤S1中,所述多传感器模块获取传感器数据时,还包含轨迹识别处理步骤,具体为:获取同一位置两个时刻的传感器数据以合成轨迹,并进行初始化。
4.根据权利要求3所述的一种基于多传感器融合的车辆目标跟踪算法,其特征在于,步骤S2中,包含子步骤:S21、建立含未知参数的运动学方程和测量方程;根据所述运动学方程和测量方程获取未知参数向量;
S22、根据含未知参数的状态估计器解析所述未知参数向量,获取后验状态数据;
S23、设计最小方差估计器,获取后验协方差数据。
5.根据权利要求4所述的一种基于多传感器融合的车辆目标跟踪算法,其特征在于,步骤S3中,时间配准方式具体为:获取需配准的不同传感器的观测数据;根据已建立的时间运动学方程和所述观测数据计算时间偏差值;使用卡尔曼滤波法计算时间偏移量并更新传感器数据。
6.根据权利要求5所述的一种基于多传感器融合的车辆目标跟踪算法,其特征在于,步骤S3中,还包含融合权重计算步骤:构建权重衡量模型,获取测量噪声协方差并输入所述权重衡量模型,计算得到自适应因子。
7.根据权利要求6所述的一种基于多传感器融合的车辆目标跟踪算法,其特征在于,步骤S3中,对多车目标中各分目标进行轨迹关联时,使用最近邻匹配原则对所述多传感器模块中不同传感器对同一个目标值的传感器数据进行匹配。
8.一种基于多传感器融合的车辆目标跟踪系统,其特征在于,该系统包含:多传感器模块,用于获取传感器数据;所述传感器数据包含状态数据和观测数据;
目标检测与跟踪模块,用于接收所述传感器数据、为多车目标分配所述传感器数据进行检测与跟踪,根据有无观测值进行状态更新,获取后验状态数据和后验协方差数据以进行空间配准;时刻初始化目标车辆 的轨迹,根据两个时刻同一位置的状态信息进行关联,将 分配给 跟踪目标; 表示当前时刻 的状态向量,设计含未知参数的状态估计器;
如果该时刻有已经关联成功的观测值,使用含未知参数状态估计器进行更新,解析未知参数向量;状态估计的形式参见(4)(5)式: (4)若上一时刻后验误差 是无偏的,则通过给定测量向量 ,标称值 来获得的无偏估计值;
(5)
在 时刻, 表示预测状态, 表示预测输出, 是 时刻的估计器增益,是预测输出中的误差,近似值为(6)式;
(6)
其中 和 是相对于Y和未知参数的线性化一阶导;设计的最小方差估计器增益为,其值为(7)式:
(7)
其中 是可逆的,先验协方差 , 为状态噪声协方差;R为观测噪声协方差矩阵;
状态估计的后验协方差为:
(8)
如果该时刻没有关联成功的观测值,则采用 更新;
并根据有无观测值进行状态更新,获取后验状态数据和后验协方差数据以进行空间配准;
多传感器融合模块,用于接收所述后验状态数据和后验协方差数据,输入多传感器融合模块,对多车目标中各分目标进行时间配准、轨迹关联和轨迹融合。