一种目标跟踪算法、系统及可读存储介质的制作方法

一种目标跟踪算法、系统及可读存储介质
【技术领域】
1.本发明涉及自动驾驶技术领域，尤其涉及一种目标跟踪算法、系统及可读存储介质。


背景技术：

2.无迹卡尔曼滤波方法广泛用于工程实践中的不同领域，在无人驾驶中追踪目标的时候，我们通常会使用无迹卡尔曼滤波(ukf)及ctrv(constanttrunrateandvelocitymagnitudemodel)运动模型预测出该目标下一时刻的状态(所处的位置，速度等)；该方法使用上一帧目标的状态和该状态的协方差，产生了一些λ点，再对这些λ点进行非线性变换，而后对这些λ点进行预测，然后再加权求平均得到预测状态点(当前时刻的状态预测)，以及预测的λ点的协方差；然后再利用变换得到观测点的预测及协方差，再结合观测点的传感器的协方差，求出卡尔曼增益，进而通过卡尔曼增益更新预测的点的状态和该状态的协方差；其中在使用上一帧的目标产生一些λ点的时候，产生的λ点的预测状态的分布由α等参数决定，其中α参数对预测结果状态分布影响较大；
3.现有技术一的缺点中在使用上一帧的目标产生一些λ点的时候，产生的λ点的分布由α等参数决定，其中α参数对预测结果状态影响较大；而在使用这个滤波器的时候，纯手动设置α参数，不够智能，耗时耗力，准确率不高。
4.因此，有必要研究一种目标跟踪算法、系统及可读存储介质来应对现有技术的不足，以解决或减轻上述一个或多个问题。


技术实现要素：

5.有鉴于此，本发明提供了一种目标跟踪算法、系统及可读存储介质，进一步为一种目标跟踪的参数自我学习无迹卡尔曼滤波算法、系统及可读存储介质，本发明能够大大减少寻找最优参数过程中的工作量，且能提高参数的准确性。
6.一方面，本发明提供一种目标跟踪算法，所述目标跟踪算法包括：
7.s1：设置关于α的参数网格；
8.s2：遍历全部α，通过α计算得到λ及权重wm和wc；
9.s3：获取λ点的分布情况；
10.s4：对s3中的λ点通过汽车动力学模型得到的第一预测的λ点；
11.s5：计算第一预测的λ点的均值和方差；
12.s6：将s5计算均值和方差过程中引入状态预测的第一预测的λ点设置为第二预测的λ点，对第二预测的λ点再次计算方差；
13.s7:输出第二预测的λ点的方差和均值以及第一预测的λ点中不包括第二预测的λ点的方差和均值，第二预测的λ点的均值与对应的s5中的第一预测的λ点的均值相同，判断第二预测的λ点以及第一预测的λ点中不包括第二预测的λ点的分布情况；
14.s8：对下一个α值重复s4-s7，比较两个α值下的s7的分布情况结果，保留分布状态
更好的λ点；
15.s9：根据s8中结果获取最优的α参数，重复s3，获取在α参数最佳的情况下的最优估计值。
16.如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述s7 中判断第二预测的λ点以及第一预测的λ点中不包括第二预测的λ点的分布情况具体为：在状态量为一维的情况下，分布接近于正态分布更好，二维的情况下接近于椭圆更好，当维度大于2时，在每个维度上接近于高斯分布。
17.如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述 s3通过以下方法获得：
18.λ＝α2(n k)-n；
19.χ
0.＝μ；
[0020][0021][0022]
其中，n:状态的维数；α和κ：确定δ点分布在均值多远的范围内的比例参数；μ：状态向量，包含位置，速度，加速度等状态信息；σ：当前时刻状态的协方差矩阵。
[0023]
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述s2 的计算方法如下：
[0024][0025][0026]
当i＝1,...,2n；
[0027]
其中n为状态的维数，k值为影响λ点分布的参数，取值范围不固定，满足求解λ点的公式中的被平方根矩阵为正定矩阵。
[0028]
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述s4 中汽车动力学模型包括但不限于ctrv，所述ctrv全称描述为：constant turn rateand velocity magnitude model；etc。
[0029]
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述s5 中计算方法如下：
[0030][0031][0032]
其中，n:状态的维数；wm:计算均值所使用的权重；预测的状态矩阵；预测的状态的加权均值；wc:计算协方差所使用的权重；r
t
:预测误差。
[0033]
如上所述的方面和任一可能的实现方式，进一步提供一种目标跟踪算法系统，所述目标跟踪算法系统包括：
[0034]
参数网格预设单元，用于设置关于α的参数网格；
[0035]
参数计算单元，用于遍历全部α，通过α计算得到λ及权重wm和wc；
[0036]
λ点获取单元，用于获取λ点的分布情况；
[0037]
预测λ点获取单元，用于对λ点通过汽车动力学模型得到的第一预测的λ点；
[0038]
均值方差计算单元，用于计算第一预测的λ点的均值和方差；
[0039]
二次计算单元，用于将计算均值和方差过程中引入状态预测的第一预测的λ点设置为第二预测的λ点，对第二预测的λ点再次计算方差；
[0040]
分布判断单元，用于输出第二预测的λ点的方差和均值以及第一预测的λ点中不包括第二预测的λ点的方差和均值，第二预测的λ点的均值与对应的s5中的第一预测的λ点的均值相同，判断第二预测的λ点以及第一预测的λ点中不包括第二预测的λ点的分布情况；
[0041]
λ点筛选单元，通过对下一个α值重新输入均值方差计算单元，比较两个α值下的分布判断单元的输出结果，保留分布状态更好的λ点；
[0042]
输出单元，用于根据λ点筛选单元结果获取最优的α参数，获取在α参数最佳的情况下的最优估计值。
[0043]
如上所述的方面和任一可能的实现方式，进一步提供一种可读存储介质，包括：存储器，所述存储器存储有程序；处理器，所述处理器执行所述程序时实现任一项所述的目标跟踪算法。
[0044]
与现有技术相比，本发明可以获得包括以下技术效果：
[0045]
1、手动设置产生的α点不够精确，而通过每次去猜测一个α值来达到最优状态，费事费力，人力和时间成本都比较大，并且手动找出来的参数并非最优参数，并且该参数不能随时间变化，本发明使用grid search的方法，可以在需求的精确度内寻找到最优参数α；
[0046]
2、本发明可以使用这种方法可以使ukf自我学习出最优的参数；
[0047]
3、本发明可以通过快速找到最佳参数并提高预测下一个状态的精确度。
[0048]
当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有技术效果。
【附图说明】
[0049]
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
[0050]
图1是本发明一个实施例提供的gaussian分布产生λ点图；
[0051]
图2是本发明一个实施例提供的ukf算法的流程图；
[0052]
图3是本发明一个实施例提供的寻找最优α值的过程图。
【具体实施方式】
[0053]
为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。
[0054]
应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基
于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0055]
在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。
[0056]
本发明提供本发明提供了一种目标跟踪算法、系统及可读存储介质，
[0057]
如图1所示，所述目标跟踪算法包括：
[0058]
s1：设置关于α的参数网格；
[0059]
s2：遍历全部α，通过α计算得到λ及权重wm和wc；
[0060]
s3：获取λ点的分布情况；
[0061]
s4：对s3中的λ点通过汽车动力学模型得到的第一预测的λ点；
[0062]
s5：计算第一预测的λ点的均值和方差；
[0063]
s6：将s5计算均值和方差过程中引入状态预测的第一预测的λ点设置为第二预测的λ点，对第二预测的λ点再次计算方差；
[0064]
s7:输出第二预测的λ点的方差和均值以及第一预测的λ点中不包括第二预测的λ点的方差和均值，第二预测的λ点的均值与对应的s5中的第一预测的λ点的均值相同，判断第二预测的λ点以及第一预测的λ点中不包括第二预测的λ点的分布情况；
[0065]
s8：对下一个α值重复s4-s7，比较两个α值下的s7的分布情况结果，保留分布状态更好的λ点；
[0066]
s9：根据s8中结果获取最优的α参数，重复s3，获取在α参数最佳的情况下的最优估计值。
[0067]
所述s7中判断第二预测的λ点以及第一预测的λ点中不包括第二预测的λ点的分布情况具体为：在状态量为一维的情况下，分布接近于正态分布更好，二维的情况下接近于椭圆更好，当维度大于2时，在每个维度上接近于高斯分布。
[0068]
所述s3通过以下方法获得：
[0069]
λ＝α2(n k)-n；
[0070]
χ
0.＝μ；
[0071][0072][0073]
其中n:状态的维数；α和κ：确定δ点分布在均值多远的范围内的比例参数；μ：状态向量，包含位置，速度，加速度等状态信息；σ：当前时刻状态的协方差矩阵。
[0074]
所述s2的计算方法如下：
[0075][0076][0077]
当i＝1,...,2n；
[0078]
其中n为状态的维数，k值为影响λ点分布的参数，取值范围不固定，满足求解λ点的公式中的被平方根矩阵为正定矩阵。
[0079]
所述s4中汽车动力学模型包括但不限于ctrv：constant turn rate and velocitymagnitude model；etc.。
[0080]
所述s5中计算方法如下：
[0081][0082][0083]
其中，n:状态的维数；wm:计算均值所使用的权重；预测的状态矩阵；预测的状态的加权均值；wc:计算协方差所使用的权重；r
t
:预测误差。
[0084]
所述s5中引入的状态预测的不确定性r
t
是的对预测的λ点进行轻微调整。
[0085]
本发明还提供一种目标跟踪算法系统，所述目标跟踪算法系统包括：
[0086]
数网格预设单元，用于设置关于α的参数网格；
[0087]
参数计算单元，用于遍历全部α，通过α计算得到λ及权重wm和wc；
[0088]
λ点获取单元，用于获取λ点的分布情况；
[0089]
预测λ点获取单元，用于对λ点通过汽车动力学模型得到的第一预测的λ点；
[0090]
均值方差计算单元，用于计算第一预测的λ点的均值和方差；
[0091]
二次计算单元，用于将计算均值和方差过程中引入状态预测的第一预测的λ点设置为第二预测的λ点，对第二预测的λ点再次计算方差；
[0092]
分布判断单元，用于输出第二预测的λ点的方差和均值以及第一预测的λ点中不包括第二预测的λ点的方差和均值，第二预测的λ点的均值与对应的s5中的第一预测的λ点的均值相同，判断第二预测的λ点以及第一预测的λ点中不包括第二预测的λ点的分布情况；
[0093]
λ点筛选单元，通过对下一个α值重新输入均值方差计算单元，比较两个α值下的分布判断单元的输出结果，保留分布状态更好的λ点；
[0094]
输出单元，用于根据λ点筛选单元结果获取最优的α参数，获取在α参数最佳的情况下的最优估计值。
[0095]
本发明还提供一种可读存储介质，包括：存储器，所述存储器存储有程序；处理器，所述处理器执行所述程序时实现任一项所述的目标跟踪算法。
[0096]
实施例1
[0097]
步骤1、设置关于α的参数网格，例如{α1,α2,α3,
…
}
[0098]
步骤2、遍历上面的α，通过α计算得到λ及权重wm和wc，计算公式如下，其中 n为状态的维数，k值也是一个影响λ点分布的参数，取值范围不固定，但是要保证求解λ点的公式中的被平方根矩阵为正定矩阵：
[0099]
λ＝α2(n k)-n；
[0100]
[0101][0102]
当i＝1,...,2n；
[0103]
步骤3、通过下面这三个公式得到λ点的分布，也就是图2ukf算法的流程中的公式2：
[0104]
χ
0.＝μ；
[0105][0106][0107]
步骤4、然后对上面的λ点通过一些汽车动力学模型(ctrv：constantturnrateandvelocitymagnitudemodel；etc.)得到的预测的λ点，公式可以表示为图2ukf算法的流程中的公式3；
[0108]
步骤5、得到上面的预测的λ点后，对得到的sigam点计算它们的均值和方差，公式为图2ukf算法的流程中的公式4和5；
[0109]
图2中：
[0110]
γ＝α2(n κ)-n；
[0111]
μ
t-1
:上一个时刻的状态；
[0112]
g():无迹卡尔曼滤波中的状态变换方程；
[0113]
h():对观测值进行变换；
[0114]
μ
t
:当前时刻的状态；
[0115]zt
:当前时刻的协方差；
[0116]
使用预测模型得到状态的协方差加权值；
[0117]
模型预测和观测的互协方差；
[0118]
n:状态的维数；
[0119]
wm:计算均值所使用的权重；
[0120]
预测的状态矩阵；
[0121]
预测的状态的加权均值；
[0122]
wc:计算协方差所使用的权重；
[0123]rt
:预测误差
[0124]qt
:观测误差
[0125]kt
:卡尔曼增益
[0126]st
:观测状态协方差矩阵
[0127]
α和κ：确定δ点分布在均值多远的范围内的比例参数；
[0128]
σ
t
：当前时刻状态的协方差矩阵。
[0129]
步骤6、公式5中引入的状态预测的不确定性r
t
是的对预测的λ点有了轻微的调整，需要再用新的方差再计算一次得到预测值的λ点；
[0130]
步骤7、判断这些λ点的分布情况，在状态量为一维的情况下，分布接近于正态分布为佳，二维的情况下则是接近于椭圆则比较优，如图1；更高维度的话，在每个维度上接近于高斯分布。
[0131]
步骤8、对下一个α值进行上面的操作，如果最终的预测值的λ点，比上一次分布状态更好那么就保留现在的λ点；
[0132]
步骤9、对于最终的α值，即为最优的α参数，再走一遍图2ukf算法的流程就得到了在参数最佳的情况下的最优估计值。
[0133]
以上对本技术实施例所提供的一种目标跟踪算法、系统及可读存储介质，进行了详细介绍。以上实施例的说明只是用于帮助理解本技术的方法及其核心思想；同时，对于本领域的一般技术人员，依据本技术的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本技术的限制。
[0134]
如在说明书及权利要求书当中使用了某些词汇来指称特定组件。本领域技术人员应可理解，硬件制造商可能会用不同名词来称呼同一个组件。本说明书及权利要求书并不以名称的差异来作为区分组件的方式，而是以组件在功能上的差异来作为区分的准则。如在通篇说明书及权利要求书当中所提及的“包含”、“包括”为一开放式用语，故应解释成“包含/包括但不限定于”。“大致”是指在可接收的误差范围内，本领域技术人员能够在一定误差范围内解决所述技术问题，基本达到所述技术效果。说明书后续描述为实施本技术的较佳实施方式，然所述描述乃以说明本技术的一般原则为目的，并非用以限定本技术的范围。本技术的保护范围当视所附权利要求书所界定者为准。
[0135]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的商品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种商品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的商品或者系统中还存在另外的相同要素。
[0136]
应当理解，本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，a和/或b，可以表示：单独存在a，同时存在a和b，单独存在b这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
[0137]
上述说明示出并描述了本技术的若干优选实施例，但如前所述，应当理解本技术并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述申请构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本技术的精神和范围，则都应在本技术所附权利要求书的保护范围内。