一种基于交互式多模型的自动驾驶跟踪方法与流程

技术特征：
1.一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：包括如下步骤：1)建立参考坐标系，在参考坐标系中建立三个运动模型构成交互式多模型，所述三个运动模型为匀加速直线模型和分别对应左转向和右转向的两个匀速转弯模型；2)基于参考坐标系，确定量测方程；3)将三个运动模型分别作为目标运动的状态方程与量测方程结合，并通过离散型卡尔曼滤波算法计算得到各运动模型的运动状态预测值和对应的误差协方差矩阵；4)计算各运动模型的似然函数，动态调节匀速转弯模型的角速度和各运动模型的概率，然后进行运动状态估计和误差协方差矩阵融合。2.根据权利要求1所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：步骤1)中所述匀速转弯模型的离散化动态方程如下：x
k
＝f
ct
x
k-1
gw
k-1
其中，x
k
为k时刻目标运动的状态向量，x
k-1
为k-1时刻的目标运动的状态向量，f
ct
为匀速转弯模型的状态向量转移矩阵，a
ct
为匀速转弯模型的状态转移矩阵，g为状态噪声向量的转移矩阵，w
k-1
为k-1时刻的状态噪声向量，δt为采样周期，ω为角速度，ω>0表示向左转弯，ω<0表示向右转弯。3.根据权利要求2所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：步骤1)中所述匀加速直线模型的离散化动态方程如下：x
k
＝f
ca
x
k-1
gw
k-1
其中，a
ca
为匀加速直线模型的状态转移矩阵，f
ca
为匀加速直线模型的状态向量转移矩阵。4.根据权利要求3所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：步骤2)中所述量测方程如下：z
k
＝h
k
x
k
υ
k
其中，z
k
为量测向量，x
k
为状态向量，h
k
为观测矩阵，υ
k
为观测噪声向量。5.根据权利要求4所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：步骤3)包括如下子步骤：31)计算各运动模型的状态向量x
k
和量测向量z
k
；
32)定义匀加速直线模型为第1个运动模型，对应左转向的匀速转弯模型为第2个运动模型，对应右转向的匀速转弯模型为第3个运动模型；定义k时刻匹配的运动模型为第j个运动模型并用表示，k-1时刻匹配的运动模型为第i个运动模型并用表示，j和i的可取1、2或3，计算交互式多模型的混合概率计算公式如下：其中，并表示k-1时刻第j个运动模型m
j
的归一化常数，r为运动模型数量且r的取值为3，ω
ij
表示第i个运动模型m
i
转移到第j个运动模型m
j
的转移概率，表示k-1时刻各运动模型的概率；33)计算交互式多模型的混合运动状态估计值和对应的协方差矩阵计算公式如下：如下：其中，为k-1时刻第i个模型的协方差矩阵，为k-1时刻第i个模型的运动状态向量；34)计算各运动模型的运动状态预测值和对应的协方差矩阵计算公式如下：计算公式如下：其中，为k-1时刻第i个模型的状态向量转移矩阵，q
i
为第i个模型的状态噪声矩阵，t为矩阵转置操作运算；35)计算各运动模型的量测预测误差和对应的协方差矩阵计算公式如下：计算公式如下：其中，r
k
为量测噪声矩阵。6.根据权利要求5所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：步骤4)包括如下子步骤：41)计算各运动模型在高斯假设下所对应的似然函数计算公式如下：其中，n为的秩；
42)计算个运动模型的滤波增益矩阵计算公式如下：43)计算各运动模型的运动状态估计值和对应的误差协方差矩阵计算公式如下：计算公式如下：其中，i表示单位矩阵；44)计算各运动模型的概率计算公式如下：其中，表示交互式多模型的归一化常数；45)计算交互式多模型的运动状态估计值x
k
和对应的协方差矩阵p
k
，计算公式如下：，计算公式如下：其中，为运动模型m
j
的概率。7.根据权利要求6所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：步骤4)中动态调节匀速转弯模型的角速度ω包括如下内容：若两个匀速转弯模型的出现概率都小于预设阈值0.1，则只选用匀加速直线模型作为系统的运动模型，此时系统变为单运动模型系统；若两个匀速转弯模型的概率都不小于预设阈值0.1，则匀速转弯模型的角速度ω随概率的增大而增大或减小而减小；角速度ω大于预设的最小角速度ω
min
且小于预设的最大角速度ω
max
。8.根据权利要求7所述一种基于交互式多模型的自动驾驶跟踪方法，其特征在于：调节匀速转弯模型的角速度ω使用变步长的最小均方误差算法，公式如下：其中，ω
k 1
为k 1时刻的角速度，ω
k
为k时刻的角速度，α为遗忘因子且取值为0.95，β的值根据ω
max
和ω
min
的大小确定，为调节步长，为归一化误差。

技术总结
本发明涉及一种基于交互式多模型的自动驾驶跟踪方法，包括如下步骤：1）建立参考坐标系，在参考坐标系中建立由三个运动模型组成的交互式多模型；2）基于参考坐标系，确定量测方程；3）将三个运动模型分别作为目标运动的状态方程与量测方程结合，并滤波得到各运动模型的运动状态预测值和对应的误差协方差矩阵；4）计算各运动模型的似然函数，动态调节匀速转弯模型的角速度和各运动模型的概率，然后进行运动状态估计和误差协方差矩阵融合。本发明在交互式多模型的基础上合理选择运动模型，降低了算力需求；通过自适应调整匀速转弯模型的的角速度，解决了无法准确估计角速度的问题，减小了目标跟踪时的误差，使计算结果更准确。使计算结果更准确。使计算结果更准确。


技术研发人员：谭伟 王宽 陈剑斌 任凡
受保护的技术使用者：重庆长安汽车股份有限公司
技术研发日：2022.03.28
技术公布日：2022/8/5