一种基于粒子滤波算法的汽车轨迹跟踪方法与流程

1.本发明属于信号处理及目标跟踪领域，尤其是一种基于粒子滤波算法的汽车轨迹跟踪方法。


背景技术：

2.随着当今科技的快速发展，汽车行业正面临着前所未有的技术革新，传统的燃油汽车正逐渐被新型的电动汽车所取代，同时新型电动汽车的逐渐普及也为汽车的智能化提供了契机。在汽车的智能化进程中，尤其以自动驾驶技术最引人注目，在自动驾驶技术体系中，汽车轨迹跟踪技术是影响该技术性能极其重要的一环。汽车轨迹跟踪技术是由一系列的测量数据(如摄像头，全球定位系统、转速仪、陀螺仪和传感器)和一系列的状态参数(如汽车速度、转角和路径)来实现的。
3.粒子滤波算法通过对传感器测量数据进行处理，将测量数据和状态参数结合起来，建立一个观测模型，从而能够计算汽车的当前位置。通过粒子滤波算法，可以实现对汽车位置的实时更新，从而更准确地定位汽车的位置。
4.传统的粒子滤波算法的具体实现步骤为：首先，根据测量数据和状态参数建立观测模型；其次，对汽车的位置进行随机分布，并计算每个粒子的概率值；然后，根据概率值更新汽车的位置，得到汽车最新的位置，从而实现汽车定位与跟踪。
5.传统的粒子滤波算法在实现汽车定位与跟踪方面只考虑了汽车的运动模型与传感器观测模型，没有考虑到汽车所处环境的地图信息，导致为用户提供定位信息极不方便；同时进行轨迹跟踪时，需要在汽车运动模型中进行采样和重采样；因此，汽车的位置是根据粒子滤波器的初始参数进行采样迭代得到的。而传统的粒子滤波往往没有进行重采样，导致生成的粒子权值发生退化，进而影响汽车实际轨迹的估计精度。综上所述，现有粒子滤波轨迹跟踪方法存在滤波器粒子无重采样、粒子退化及忽略环境地图信息等问题。


技术实现要素：

6.为了克服现有技术的不足，本发明提出了一种基于粒子滤波算法的汽车轨迹跟踪方法，用于解决现有粒子滤波定位与跟踪方法中存在的滤波器粒子无重采样、粒子退化及忽略环境地图信息等问题，并使得汽车初始状态未知的情况下能够实现对汽车的一个准确的定位与跟踪。
7.本发明解决其技术问题所采用的技术方案是：
8.一种基于粒子滤波算法的汽车轨迹跟踪方法，包括以下步骤：
9.1)初始化，将二维环境地图载入汽车，同时载入ct运动模型与速度观测模型；
10.2)初始化粒子生成器，获取传感器中环境地图的坐标信息，融合地图信息在地图区域内生成设定数量的粒子群，使用先验概率模型生成状态矩阵x0的n个粒子，即：其中i表示为n个粒子中的第i个粒子，同时将粒子权重ωi赋值为其中：
11.初始状态x0,y0在地图道路上服从均匀分布，其中v0、ω0服从高斯分布；服从正态分布n(u0，r0)，u0为3
×
1的矩阵表示初始状态v0、ω0的均值，r0表示初始状态v0、ω0的方差；
12.3)粒子预测过程，根据粒子的先验分布，选择先验分布的建议分布，使用运动模型f(x)作为其粒子预测过程的建议分布；输入初始化粒子的参数得到新的粒子权重以及粒子的位置；其中：
13.粒子的先验分布为：
[0014][0015]
表示在第k时刻观测到传感器值yk以及前一时刻的运动粒子的条件下第k时刻运动状态xk的分布；粒子滤波选择作为建议分布，为运动模型为运动模型的概率表示形式，因此先验分布的建议分布为：
[0016][0017][0018]
4)权重更新过程，在上述的粒子预测过程中，通过运动模型已经得到了预测后的粒子位置，权重更新过程就是在之前粒子预测权重的基础上，得到新生成粒子的权值其中：
[0019][0020]
根据步骤3)所选择的建议分布，粒子权值更新的计算模型简化为：
[0021][0022]
5)权值归一化，将计算好的权值进行归一化处理，其中：
[0023][0024]
6)地图匹配，在上述的粒子预测的过程中，剔除预测位置会位于建筑物的内部的部分粒子；
[0025]
7)权值再归一化，将步骤6)重新计算好的权值按照步骤5)再次进行归一化处理；
[0026]
8)状态估计，将步骤7)中计算好的的粒子以及对应的粒子的权重输入估计器中，在状态估计这一过程中，采用最小均方误差mmse估计的作为后验概率最终预测的结果；其中mmse估计的均值与方差pk计算为：
[0027][0028][0029]
9)重采样，当汽车继续运动时，速度传感器继续输出观测值，同时滤波器开始进行迭代运算，为了减小算法迭代时粒子退化问题，以及在步骤6)地图匹过程中抛弃了权重值为0的粒子，因此需要再次进行粒子的重采样；
[0030]
10)迭代运行，当汽车继续运动时，速度传感器继续输出观测值，滤波器开始进行迭代运算，滤波器将上一步生成粒子与权重作为输入，返回步骤3)继续运行，如果当速度传感器无输出时，滤波器终止计算，滤波结束。
[0031]
进一步，所述步骤1)中，二维环境地图为由行驶街区的建筑物以及道路二维坐标组成，将建筑物区域为盲区，道路区域为可通行区域；运动模型具体使用协调转弯(coordinatedturn)模型f(x)作为汽车运动模型，汽车运动模型建模为：
[0032]
xk＝f(x
k-1
)+q
k-1
(9)
[0033]
其中：q
k-1
为运动模型的估计噪声，假定其运动噪声服从如下的高斯分布：
[0034]qk-1
～n(0，q
k-1
)(10)
[0035]
其中q
k-1
为运动噪声的方差，上述运动模型中xk、yk、vk、ωk表示汽车k时刻在地图上的坐标、速度、转角、以及转角角速度，qv、q
ω
分别表示运动模型中对vk、ωk的一个估计噪声，t表示第k-1个状态到第k个状态的时间步长；
[0036]
运动模型具体使用速度传感器作为观测模型，因此，汽车速度的观测模型建模为：
[0037]
yk＝h(xk)+rk(11)
[0038]
其中yk为一个2
×
1的矩阵，rk表示观测模型的噪声，假定噪声服从以下高斯分布：
[0039]rk
～n(0，rk)(12)
[0040]
其中r
vx
表示对x轴方向速度的测量偏差，r
vy
表示对y轴方向速度的测量偏差。
[0041]
更进一步，所述步骤6)中，使用地图匹配减小估计误差方法定义为：
[0042]
p(xk|x
k-1
,m)
∝
p(m|xk)p(xk|x
k-1
)(13)
[0043]
其中p(m|xk)为给定预测xk时，粒子位于地图道路上的似然函数，p(xk|x
k-1
)即为前面所述的运动模型，由于新的运动模型非高斯分布，无法进行采样，因此依旧采用作为建议分布，但是计算粒子的权重采用如下公式计算：
[0044][0045]
从上述的公式中可以看出地图匹配过程实际上只需要在上一步的基础上将每个粒子的权重乘上似然函数即可；
[0046]
其中定义如下：
[0047][0048]
更进一步，所述步骤9)中，为了缓解滤波器粒子退化问题采用的重采样方法定义为：在(0,1)的均匀分布上随机生成n个粒子，然后按照原来的n个粒子权重值的大小将(0,1)分成n个区间，算出每个区间的新粒子数目m，然后将对应的旧粒子复制m次，最后将所有的粒子的权值赋值为
[0049]
本发明有益效果在于：在解决非线性问题时使用经典粒子滤波器的基础上，选择合适的建议分布(proposal distribution)，使得先验概率分布函数易于采样。同时，结合传感器观测信息和汽车行驶过程中的二维环境地图信息，能够更精确地逼近实际的后验分布，从而实现对汽车初始位置未知情况下的定位与跟踪。最后，通过对全部粒子进行重采样，有效降低粒子在滤波器迭代过程中的粒子退化问题，提高滤波的精度。
附图说明
[0050]
图1是基于本发明方法的整体方法流程图，该方法主要由粒子生成器、粒子预测过程、权重更新过程、地图匹配、状态估计、重采样几部分组成。
[0051]
图2是将预测粒子结合环境地图信息之后所得到的非高斯运动模型概率分布图。
[0052]
具体实施方法
[0053]
下面结合附图内容，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本技术所附权利要求书所限定的范围。
[0054]
参照图1和图2，一种基于粒子滤波算法的汽车轨迹跟踪方法包括以下步骤：
[0055]
1)初始化，在这一过程中主要将二维环境地图载入汽车，同时载入ct(coordinatedturn)运动运动模型与速度观测模型；其中二维环境地图为由行驶街区的建筑物以及道路二维坐标组成，将建筑物区域为盲区，道路区域为可通行区域，ct(coordinatedturn)模型f(x)作为汽车运动模型，建模为：
[0056]
xk＝f(x
k-1
)+q
k-1
(1)
[0057]
其中：q
k-1
为运动模型的估计噪声，假定其运动噪声服从如下的高斯分布：
[0058]qk-1
～n(0，q
k-1
)(2)
[0059]
其中q
k-1
为运动噪声的方差，上述运动模型中xk、yk、vk、ωk表示汽车k时刻在地图上的坐标、速度、转角、以及转角角速度，qv、q
ω
分别表示运动模型中对vk、ωk的一个估计噪声，t表示第k-1个状态到第k个状态的时间步长；
[0060]
2)初始化粒子生成器，获取传感器中环境地图的坐标信息，融合地图信息在地图区域内生成设定数量的粒子群，使用先验概率模型生成状态矩阵x0的n个粒子，即：其中i表示为n个粒子中的第i个粒子。同时将粒子权重ωi赋值为其中：
[0061]
初始状态x0,y0在地图道路上服从均匀分布，其次v0、ω0服从高斯分布，服从正态分布n(u0，r0)，u0为3
×
1的矩阵表示初始状态v0、ω0的均值，r0表示初始状态v0、ω0的方差；
[0062]
3)粒子预测过程，根据粒子的先验分布，选择先验分布的建议分布，使用合适的运
动模型f(x)作为其粒子预测过程的建议分布；输入初始化粒子的参数得到新的粒子权重以及粒子的位置；其中：
[0063]
粒子的先验分布为：
[0064][0065]
表示在第k时刻观测到传感器值yk以及前一时刻的运动粒子的条件下第k时刻运动状态xk的分布；粒子滤波选择作为建议分布，为运动模型xk＝f(x
k-1
)+q
k-1
的概率表示形式，因此先验分布的建议分布为：
[0066][0067][0068]
4)权重更新过程，在上述的粒子预测过程中，通过运动模型已经得到了预测后的粒子位置，权重更新过程就是在之前粒子预测权重的基础上，得到新生成粒子的权值其中：
[0069][0070]
根据步骤3)所选择的建议分布，粒子权值更新的计算模型简化为：
[0071][0072]
5)权值归一化，将计算好的权值进行归一化处理，其中：
[0073][0074]
6)地图匹配，在上述的粒子预测的过程中，部分粒子的预测位置会位于建筑物的内部，显然这些位于建筑物内部预测粒子是不正确的，为了得到更正确的轨迹，需要对这些位于建筑物内部的粒子进行剔除，因此将地图信息与运动模型相结合得到一个非高斯的运动模型，新的运动模型示意图如附图中图二所示，具体数学表达式为：
[0075]
p(xk|x
k-1
,m)
∝
p(m|xk)p(xk|x
k-1
)(9)
[0076]
其中p(m|xk)为给定预测xk时，粒子位于地图道路上(不是建筑内部)的似然函数，p(xk|x
k-1
)即为前面所述的运动模型，由于新的运动模型非高斯分布，无法进行采样，因此依旧采用旧采用作为建议分布，故计算粒子的权重采用如下公式计算：
[0077][0078]
从上述的公式中可以看出地图匹配过程实际上只需在上一步的基础上将每个粒子的权重乘上似然函数即可。
[0079]
其中可以定义如下：
[0080][0081]
7)权值再归一化，将步骤6)重新计算好的权值按照步骤5)再次进行归一化处理；
[0082]
8)状态估计，将步骤7)中计算好的的粒子以及对应的粒子的权重输入估计器中，在状态估计这一过程中，采用最小均方误差mmse估计的作为后验概率最终预测的结果；其中mmse的均值与方差pk计算为：
[0083][0084][0085]
9)重采样，当汽车继续运动时，速度传感器继续输出观测值，同时滤波器开始进行迭代运算，为了减小算法迭代时粒子退化问题，以及在步骤6)地图匹配过程中抛弃掉权重值为0的粒子减小定位误差，因此进行粒子的重采样，具体重采样的方法是在(0,1)的均匀分布上随机生成n个粒子，然后按照原来的n个粒子权重值的大小将(0,1)分成n个区间，算出每个区间的新粒子数目m，然后将对应的旧粒子复制m次，最后将所有的粒子的权值赋值为
[0086]
10)迭代运行，当汽车继续运动时，速度传感器继续输出观测值，滤波器开始进行迭代运算，滤波器将上一步生成粒子与权重作为输入，返回步骤3)继续运行，如果当速度传感器无输出时，滤波器终止计算，滤波结束。
[0087]
本说明书的实施例所述的内容仅仅是对发明构思的实现形式的列举，仅作说明用途。本发明的保护范围不应当被视为仅限于本实施例所陈述的具体形式，本发明的保护范围也及于本领域的普通技术人员根据本发明构思所能想到的等同技术手段。

技术特征：
1.一种基于粒子滤波算法的汽车轨迹跟踪方法，其特征在于，所述方法包括以下步骤：1)初始化，将二维环境地图载入汽车，同时载入ct运动模型与速度观测模型；2)初始化粒子生成器，获取传感器中环境地图的坐标信息，融合地图信息在地图区域内生成设定数量的粒子群，使用先验概率模型生成状态矩阵x0的n个粒子，即：其中i表示为n个粒子中的第i个粒子，同时将粒子权重ω
i
赋值为其中：初始状态x0,y0在地图道路上服从均匀分布，其中v0、ω0服从高斯分布；服从正态分布n(u0，r0)，u0为3
×
1的矩阵表示初始状态v0、ω0的均值，r0表示初始状态v0、ω0的方差；3)粒子预测过程，根据粒子的先验分布，选择先验分布的建议分布，使用运动模型f(x)作为其粒子预测过程的建议分布；输入初始化粒子的参数得到新的粒子权重以及粒子的位置；其中：粒子的先验分布为：粒子的先验分布为：表示在第k时刻观测到传感器值y
k
以及前一时刻的运动粒子的条件下第k时刻运动状态x
k
的分布；粒子滤波选择作为建议分布，为运动模型运动模型的概率表示形式，因此先验分布的建议分布为：因此先验分布的建议分布为：4)权重更新过程，在上述的粒子预测过程中，通过运动模型已经得到了预测后的粒子位置，权重更新过程就是在之前粒子预测权重的基础上，得到新生成粒子的权值其中：根据步骤3)所选择的建议分布，粒子权值更新的计算模型简化为：5)权值归一化，将计算好的权值进行归一化处理，其中：
6)地图匹配，在上述的粒子预测的过程中，剔除预测位置会位于建筑物的内部的部分粒子；7)权值再归一化，将步骤6)重新计算好的权值按照步骤5)再次进行归一化处理；8)状态估计，将步骤7)中计算好的的粒子以及对应的粒子的权重输入估计器中，在状态估计这一过程中，采用最小均方误差mmse估计的作为后验概率最终预测的结果；其中mmse估计的均值与方差p
k
计算为：计算为：9)重采样，当汽车继续运动时，速度传感器继续输出观测值，同时滤波器开始进行迭代运算，为了减小算法迭代时粒子退化问题，以及在步骤6)地图匹过程中抛弃了权重值为0的粒子，因此需要再次进行粒子的重采样；10)迭代运行，当汽车继续运动时，速度传感器继续输出观测值，滤波器开始进行迭代运算，滤波器将上一步生成粒子与权重作为输入，返回步骤3)继续运行，如果当速度传感器无输出时，滤波器终止计算，滤波结束。2.如权利要求1所述的一种基于粒子滤波算法的汽车轨迹跟踪方法，其特征在于，所述步骤1)中，运动模型使用协调转弯(coordinatedturn)模型f(x)作为汽车运动模型，汽车运动模型建模为：x
k
＝f(x
k-1
)+q
k-1
(1)其中：q
k-1
为运动模型的估计噪声，假定其运动噪声服从如下的高斯分布：q
k-1
～n(0，q
k-1
)(2)其中q
k-1
为运动噪声的方差，上述运动模型中x
k
、y
k
、v
k
、ω
k
表示汽车k时刻在地图上的坐标、速度、转角、以及转角角速度，q
v
、q
ω
分别表示运动模型中对v
k
、ω
k
的一个估计噪声，t表示第k-1个状态到第k个状态的时间步长；运动模型具体使用速度传感器作为观测模型，因此，汽车速度的观测模型建模为：
y
k
＝h(x
k
)+r
k
(3)其中y
k
为一个2
×
1的矩阵，r
k
表示观测模型的噪声，假定噪声服从以下高斯分布：r
k
～n(0，r
k
)(4)其中r
vx
表示对x轴方向速度的测量偏差，r
vy
表示对y轴方向速度的测量偏差。3.如权利要求1或2所述的一种基于粒子滤波算法的汽车轨迹跟踪方法，其特征在于，所述步骤6)中，使用地图匹配减小估计误差方法定义为：p(x
k
|x
k-1
,m)
∝
p(m|x
k
)p(x
k
|x
k-1
)(5)其中p(m|x
k
)为给定预测x
k
时，粒子位于地图道路上的似然函数，p(x
k
|x
k-1
)即为前面所述的运动模型，由于新的运动模型非高斯分布，无法进行采样，因此依旧采用作为建议分布，但是计算粒子的权重采用如下公式计算：从公式中(6)可以看出地图匹配过程实际上只需要在上一步的基础上将每个粒子的权重乘上似然函数即可；其中定义如下：4.如权利要求3所述的一种基于粒子滤波算法的汽车轨迹跟踪方法，其特征在于，所述步骤9)中，采用的重采样方法定义为：在(0,1)的均匀分布上随机生成n个粒子，然后按照原来的n个粒子权重值的大小将(0,1)分成n个区间，算出每个区间的新粒子数目m，然后将对应的旧粒子复制m次，最后将所有的粒子的权值赋值为

技术总结
一种基于粒子滤波算法的汽车轨迹跟踪方法，包括以下步骤：1)初始化；2)初始化粒子生成器；3)粒子预测过程；4)权重更新过程；5)权值归一化；6)地图匹配；7)权值再归一化；8)状态估计；9)重采样；10)迭代运行，当汽车继续运动时，速度传感器继续输出观测值，滤波器开始进行迭代运算，滤波器将上一步生成粒子与权重作为输入，返回步骤3)继续运行，如果当速度传感器无输出时，滤波器终止计算，滤波结束。本发明用于解决现有粒子滤波定位与跟踪方法中存在的滤波器粒子无重采样、粒子退化及忽略环境地图信息等问题，并使得汽车初始状态未知的情况下能够实现对汽车的一个准确的定位与跟踪。够实现对汽车的一个准确的定位与跟踪。够实现对汽车的一个准确的定位与跟踪。


技术研发人员：钱丽萍 刘振翔 赵文景 王倩 丁晋 王巍
受保护的技术使用者：中国电子科技集团公司第三十六研究所
技术研发日：2023.06.20
技术公布日：2023/10/15