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

1.本发明属于汽车自动驾驶的技术领域，具体涉及一种基于交互式多模型的自动驾驶跟踪方法。


背景技术：

2.随着汽车智能化的快速发展，越来越多的汽车配备了自动驾驶功能，前方机动目标跟踪是实现自动驾驶的必备能力。对前方机动目标进行跟踪主要是利用运动模型计算目标的运动状态，以往大部分汽车厂商都采用匀加速模型或匀转弯模型作为计算目标运动状态的运动模型。但由于车辆的运动模式包括匀直行、匀加速直行、匀转弯和变加速转弯等，车辆的运动模式是多样且不确定的，每种运动模式都对应一种运动模型，使用单一运动模型很难准确计算出目标的真实运动状态；因此，目前车辆对于前方机动目标的跟踪一般都采用多个运动模型，根据目标的运动模式进行切换。
3.交互式多模型算法是目前使用最为广泛的目标跟踪算法，它使用多个滤波器对涵盖目标行为的模型进行估计、交互、综合得到符合当前目标的运动状态。在交互式多模型算法中，运动模型的数量和准确程度对计算结果影响较大。中国专利cn201410778057.4一种基于交互式多模型的目标跟踪方法，该方案所使用的交互式多模型算法采用五个运动模型，运动模型较多对算力的要求过大，不符合实际应用，另外，会导致计算所需时间较长，自动驾驶的实时性无法得到保障。中国专利cn201910026173.3基于交互式多模型的机动目标状态预测优化方法，该方案只采用了三个运动模型，分别为匀直线运动模型、匀加速直线运动模型和匀速圆周运动模型，虽然降低了算力要求，提高了实时性；但在真实场景中，由于匀速圆周运动的角速度无法准确计算，一般使用一个参照量去估计角速度，而角速度估计仍存在一定误差，因此计算运动状态的准确性较差。


技术实现要素：

4.针对现有技术的上述不足，本发明要解决的技术问题是提供一种基于交互式多模型的自动驾驶跟踪方法，解决目前自动驾驶跟踪方法存在计算复杂和准确性较差的问题，取得提高准确性和实时性的效果。
5.为解决上述技术问题，本发明采用如下技术方案：
6.一种基于交互式多模型的自动驾驶跟踪方法，包括如下步骤：
7.1)建立参考坐标系，在参考坐标系中建立三个运动模型构成交互式多模型，所述三个运动模型为匀加速直线模型和分别对应左转向和右转向的两个匀速转弯模型；
8.2)基于参考坐标系，确定量测方程；
9.3)将三个运动模型分别作为目标运动的状态方程与量测方程结合，并通过离散型卡尔曼滤波算法计算得到各运动模型的运动状态预测值和对应的误差协方差矩阵；
10.4)计算各运动模型的似然函数，动态调节匀速转弯模型的角速度和各运动模型的概率，然后进行运动状态估计和误差协方差矩阵融合。
11.进一步地，步骤1)中所述匀速转弯模型的离散化动态方程如下：
12.xk＝f
ct
x
k-1
gw
k-1
[0013][0014]
xk为k时刻目标运动的状态向量，x
k-1
为k-1时刻的目标运动的状态向量，f
ct
为匀速转弯模型的状态向量转移矩阵，a
ct
为匀速转弯模型的状态转移矩阵，g为状态噪声向量的转移矩阵，w
k-1
为k-1时刻的状态噪声向量，δt为采样周期，ω为角速度，ω》0表示向左转弯，ω《0表示向右转弯。
[0015]
进一步地，步骤1)中所述匀加速直线模型的离散化动态方程如下：
[0016]
xk＝f
ca
x
k-1
gw
k-1
[0017]
其中，a
ca
为匀加速直线模型的状态转移矩阵，f
ca
为匀加速直线模型的状态向量转移矩阵。
[0018]
进一步地，步骤2)中所述量测方程如下：
[0019]
zk＝hkxk υk[0020]
其中，zk为量测向量，xk为状态向量，hk为观测矩阵，υk为观测噪声向量。
[0021]
进一步地，步骤3)包括如下子步骤：
[0022]
31)计算各运动模型的状态向量xk和量测向量zk；
[0023]
32)定义匀加速直线模型为第1个运动模型，对应左转向的匀速转弯模型为第2个运动模型，对应右转向的匀速转弯模型为第3个运动模型；
[0024]
定义k时刻匹配的运动模型为第j个运动模型并用表示，k-1时刻匹配的运动模型为第i个运动模型并用表示，j和i的可取1、2或3，计算交互式多模型的混合概率计算公式如下：
[0025][0026]
其中，并表示k-1时刻第j个运动模型mj的归一化常数，r为运动模型数量且r的取值为3，ω
ij
表示第i个运动模型mi转移到第j个运动模型mj的转移概率，表示k-1时刻各运动模型的概率；
[0027]
33)计算交互式多模型的混合运动状态估计值和对应的协方差矩阵计算公式如下：
[0028][0029][0030]
其中，为k-1时刻第i个模型的协方差矩阵，为k-1时刻第i个模型的运动状态向量；
[0031]
34)计算各运动模型的运动状态预测值和对应的协方差矩阵计算公式如下：
[0032][0033][0034]
其中，为k-1时刻第i个模型的状态向量转移矩阵，qi为第i个模型的状态噪声矩阵，t为矩阵转置操作运算；
[0035]
35)计算各运动模型的量测预测误差和对应的协方差矩阵计算公式如下：
[0036][0037][0038]
其中，rk为量测噪声矩阵。
[0039]
进一步地，步骤4)包括如下子步骤：
[0040]
41)计算各运动模型在高斯假设下所对应的似然函数计算公式如下：
[0041][0042]
其中，n为的秩；
[0043]
42)计算个运动模型的滤波增益矩阵计算公式如下：
[0044][0045]
43)计算各运动模型的运动状态估计值和对应的误差协方差矩阵计算公式如下：
[0046][0047][0048]
其中，i表示单位矩阵；
[0049]
44)计算各运动模型的概率计算公式如下：
[0050]
[0051]
其中，表示交互式多模型的归一化常数；
[0052]
45)计算交互式多模型的运动状态估计值xk和对应的协方差矩阵pk，计算公式如下：
[0053][0054][0055]
其中，为运动模型mj的概率。
[0056]
进一步地，步骤4)中动态调节匀速转弯模型的角速度ω包括如下内容：
[0057]
若两个匀速转弯模型的出现概率都小于预设阈值0.1，则只选用匀加速直线模型作为系统的运动模型，此时系统变为单运动模型系统；若两个匀速转弯模型的概率都不小于预设阈值0.1，则匀速转弯模型的角速度ω随概率的增大而增大或减小而减小；角速度ω大于预设的最小角速度ω
min
且小于预设的最大角速度ω
max
。
[0058]
进一步地，调节匀速转弯模型的角速度ω使用变步长的最小均方误差算法，公式如下：
[0059][0060]
其中，ω
k 1
为k 1时刻的角速度，ωk为k时刻的角速度，α为遗忘因子且取值为0.95，β的值根据ω
max
和ω
min
的大小确定，为调节步长，为归一化误差。
[0061]
相比现有技术，本发明的有益效果如下：
[0062]
1、本发明所述自动驾驶跟踪方法在交互式多模型的基础上，实现了自适应调整匀速转弯模型的的角速度，从而解决真实场景中无法准确估计匀速转弯模型中的角速度的难题，使预测结果能够更加贴切目标运动状态的真实值。
[0063]
2、本发明所述自动驾驶跟踪方法使用三种运动模型，分别为两个匀速转弯模型和一个匀加速直线模型，不仅所需算力较低，可有效保证目标跟踪计算的实时性，且采用匀速转弯模型相较采用匀速圆周运动模型，其计算结果更接近目标真实的运动状态。
[0064]
3、本发明所述自动驾驶跟踪方法先获取每个运动模型与目标真实运动状态的匹配度，然后根据匹配度大小动态调节匀速转弯模型的角速度大小和每个模型的概率；可有效减小对目标进行跟踪时的误差，提高交互式多模型的预测准确度，降低计算复杂度，提高实时性。
附图说明
[0065]
图1为实施例的一种基于交互式多模型的自动驾驶跟踪方法的流程图；
[0066]
图2为实施例中所述步骤3)和步骤4)具体步骤的流程图；
[0067]
图3为实施例中所述动态调整角速度的逻辑框图。
具体实施方式
[0068]
下面结合附图对本发明的具体实施方式作进一步的详细说明。
[0069]
实施例：
[0070]
请参见图1，一种基于交互式多模型的自动驾驶跟踪方法，包括如下步骤：
[0071]
1)建立参考坐标系，在参考坐标系中建立三个运动模型构成交互式多模型，所述三个运动模型为匀加速直线模型和分别对应左转向和右转向的两个匀速转弯模型；
[0072]
2)基于参考坐标系，确定量测方程；
[0073]
3)将三个运动模型分别作为目标运动的状态方程与量测方程结合，并通过离散型卡尔曼滤波算法计算得到各运动模型的运动状态预测值和对应的误差协方差矩阵；
[0074]
4)计算各运动模型的似然函数，动态调节匀速转弯模型的角速度和各运动模型的概率，然后进行运动状态估计和误差协方差矩阵融合。
[0075]
其中，所述匀速转弯模型的离散化动态方程如下：
[0076]
xk＝f
ct
x
k-1
gw
k-1
[0077]
其中，xk为k时刻目标运动的状态向量，x
k-1
为k-1时刻的目标运动的状态向量，f
ct
为匀速转弯模型的状态向量转移矩阵，a
ct
为匀速转弯模型的状态转移矩阵，g为状态噪声向量的转移矩阵，w
k-1
为k-1时刻的状态噪声向量，δt为采样周期，ω为角速度，ω》0表示向左转弯，ω《0表示向右转弯。
[0078]
所述匀加速直线模型的离散化动态方程如下：
[0079]
xk＝f
ca
x
k-1
gw
k-1
[0080]
其中，a
ca
为匀加速直线模型的状态转移矩阵，f
ca
为匀加速直线模型的状态向量转移矩阵。
[0081]
所述量测方程如下：
[0082]
zk＝hkxk υk[0083]
其中，zk为量测向量，xk为状态向量，hk为观测矩阵，υk为观测噪声向量。
[0084]
请参见图2，步骤3)包括如下子步骤：
[0085]
31)计算各运动模型的状态向量xk和量测向量zk；
[0086]
32)定义匀加速直线模型为第1个运动模型，对应左转向的匀速转弯模型为第2个运动模型，对应右转向的匀速转弯模型为第3个运动模型；
[0087]
定义k时刻匹配的运动模型为第j个运动模型并用表示，k-1时刻匹配的运动模型为第i个运动模型并用表示，j和i的可取1、2或3，计算交互式多模型的混合概率计算公式如下：
[0088][0089]
其中，并表示k-1时刻第j个运动模型mj的归一化常数，r为运动模型数量，本实施例中r为3，ω
ij
表示第i个运动模型mi转移到第j个运动模型mj的转移概率，表示k-1时刻各运动模型的概率；
[0090]
33)计算交互式多模型的混合运动状态估计值和对应的协方差矩阵计算公式如下：
[0091][0092][0093]
其中，为k-1时刻第i个模型的协方差矩阵，为k-1时刻第i个模型的运动状态向量；
[0094]
34)计算各运动模型的运动状态预测值和对应的协方差矩阵计算公式如下：
[0095][0096][0097]
其中，为k-1时刻第i个模型的状态向量转移矩阵，qi为第i个模型的状态噪声矩阵，t为矩阵转置操作运算；
[0098]
35)计算各运动模型的量测预测误差和对应的协方差矩阵计算公式如下：
[0099][0100][0101]
其中，rk为量测噪声矩阵。
[0102]
步骤4)包括如下子步骤：
[0103]
41)计算各运动模型在高斯假设下所对应的似然函数计算公式如下：
[0104][0105]
其中，n为的秩；
[0106]
42)计算个运动模型的滤波增益矩阵计算公式如下：
[0107][0108]
43)计算各运动模型的运动状态估计值和对应的误差协方差矩阵计算公式如下：
[0109][0110][0111]
其中，i表示单位矩阵；
[0112]
44)计算各运动模型的概率计算公式如下：
[0113][0114]
其中，表示交互式多模型的归一化常数；
[0115]
45)计算交互式多模型的运动状态估计值xk和对应的协方差矩阵pk，计算公式如下：
[0116][0117][0118]
其中，为运动模型mj的出现概率。
[0119]
请参见图3，步骤4)中动态调节匀速转弯模型的角速度ω包括如下内容：
[0120]
若两个匀速转弯模型的出现概率都小于预设阈值0.1，则只选用匀加速直线模型作为系统的运动模型，此时系统变为单运动模型系统；若两个匀速转弯模型的概率都不小于预设阈值0.1，则匀速转弯模型的角速度ω随概率的增大而增大或随概率的减小而减小；角速度ω大于预设的最小角速度ω
min
且小于预设的最大角速度ω
max
。
[0121]
进一步地，调节匀速转弯模型的角速度ω使用变步长的最小均方误差算法，公式如下：
[0122][0123]
其中，ω
k 1
为k 1时刻的角速度，ωk为k时刻的角速度，α为遗忘因子且取值为0.95，β的值根据ω
max
和ω
min
的大小确定，为调节步长，为归一化误差。
[0124]
本发明在交互式多模型的基础上合理选择运动模型，降低了算力需求；通过自适应调整匀速转弯模型的的角速度，解决了无法准确估计角速度的问题，减小了目标跟踪时的误差，使计算结果更准确。
[0125]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技
术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。