一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法与流程

1.本发明属于轨迹预测技术领域，具体涉及一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法。


背景技术：

2.卡尔曼滤波器作为传感器目标追踪的基本算法，能根据传感器的输入量测计算目标状态的最优估计，在卡尔曼滤波的预测中常用的运动模型包括cv、ca、ctrv、ctra，其中前两个是线性模型，后两个是非线性模型，卡尔曼滤波只适用于线性模型，对于非线性模型，一般使用扩展卡尔曼，对于机动目标跟踪中,由于其运动状态的不确定性，使用单一的运动模型跟踪效果不理想。


技术实现要素：

3.本发明的目的在于，提供一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法，使用多运动模型对目标进行跟踪，提高了滤波器的估计精度。
4.为解决上述技术问题，本发明的技术方案为：一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法，包括以下步骤：
5.建立卡尔曼滤波多运动模型；
6.采集目标运动期间一段时间内的运动信息，运动信息至少包括目标初始坐标，目标实时速度和目标实时加速度；
7.根据目标的运动信息获取目标的运动状态，根据运动状态的改变切换卡尔曼滤波运动模型，运动状态至少包括减速直行、匀速直行、加速直行、减速变道、匀速变道和加速变道；
8.将目标的运动信息代入根据运动状态改变切换的不同的卡尔曼滤波运动模型，计算得到目标的预测轨迹。
9.卡尔曼滤波多运动模型至少包括恒定速度模型cv，恒定加速度模型ca，基于恒定转率和速度模型ctrv转换形成的匀速圆周运动模型。
10.建立卡尔曼滤波多运动模型的具体步骤为：
11.将ctrv转换为笛卡尔坐标系下的匀速圆周运动模型，在标定过程中，令标定物从a点以固定角速度ω做匀速圆周运动，经过时间t
s
到达b点，圆心为点o，转弯半径为l，加速度a恒定指向圆心，则有：
[0012][0013][0014]
[0015][0016][0017][0018][0019][0020][0021][0022]
其中，
[0023]
v为标定物的恒定速度值，v
ax
和v
ay
分别为标定物在a点的横向速度和纵向速度，为标定物在a点的运动方向与x轴的夹角，x
a
和y
a
为a点的横坐标与纵坐标，a
ax
和a
ay
分别为标定物在a点的横向加速度和纵向加速度，v
bx
和v
by
分别为标定物在b点的速度，横向速度和纵向速度，为标定物在b点的运动方向与x轴的夹角，x
b
和y
b
为b点的横坐标与纵坐标，a
bx
和a
by
分别为标定物在b点的横向加速度和纵向加速度；
[0024]
根据上述匀速圆周运动模型特性，计算得到运动转移方程为：
[0025][0026]
其中，
[0027][0028][0029][0030][0031]
其中，x
k
，y
k
，vx
k
，vy
k
，ax
k
，ay
k
分别为k时刻目标状态，包括此时横向位移、纵向位移、横向速度、纵向速度、横向加速度、纵向加速度，为目标在k时刻的运动方向与x轴的夹角，x
k 1
，y
k 1
，vx
k 1
，vy
k 1
，ax
k 1
，ay
k 1
分别为k 1时刻状态转移后的目标状态，包括此时横向位移、纵向位移、横向速度、纵向速度、横向加速度、纵向加速度，为目标在k 1时刻的运动方向与x轴的夹角，vx
k
‑1，vy
k
‑1分别为k
‑
1时刻目标的横向速度、纵向速度，v为目标的恒定速度值，a为目标的恒定加速度值，ω为目标的恒定转角速度，t
s
为运动状态转移时间。
[0032]
该运动转移方程为非线性方程，对其进行一阶泰勒展开，其中匀速圆周运动的加速度指向圆心，大小不变，得到ctrv模型转移矩阵为：
[0033][0034]
其中：
[0035][0036][0037][0038]
其中，v
y
′
，v
x
′
分别为v
x
和v
y
上一帧的状态量；
[0039]
ca模型转移矩阵为
[0040][0041]
cv模型转移矩阵为
[0042][0043]
建立基于卡尔曼多运动模型的目标跟踪模型，假设无参数噪声：
[0044][0045]
p(k|k
‑
1)＝a
i
p(k
‑
1|k
‑
1)a
it
q(k)
[0046]
k＝p(k|k
‑
1)h
t
(hp(k|k
‑
1)h
t
r)
‑1[0047][0048]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0049]
其中，其中为目标的状态方程，a为目标的状态转移矩阵，w为状态方程每个参数的噪声，p为状态的协方差，q为噪声协方差，其中z为传感器量测，h为转换矩阵，r为观测噪声协方差，k为卡尔曼增益系数。
[0050]
还提供一种使用如上述的一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法的系统，包括：
[0051]
模型建立模块，用于建立卡尔曼滤波多运动模型；
[0052]
采集模块，用于采集目标运动期间一段时间内的运动信息，运动信息至少包括目标初始坐标，目标实时速度和目标实时加速度；
[0053]
判断模块，用于根据目标的运动信息获取目标的运动状态，根据运动状态的改变切换卡尔曼滤波运动模型，运动状态至少包括减速直行、匀速直行、加速直行、减速变道、匀速变道和加速变道；
[0054]
计算模块，用于将目标的运动信息代入根据运动状态改变切换的不同的卡尔曼滤波运动模型，计算得到目标的预测轨迹。
[0055]
卡尔曼滤波多运动模型至少包括恒定速度模型cv，恒定加速度模型ca，恒定转率和速度模型ctrv。
[0056]
建立卡尔曼滤波多运动模型的具体步骤为：
[0057]
将ctrv转换为笛卡尔坐标系下的匀速圆周运动模型，在标定过程中，令标定物从a点以固定角速度ω做匀速圆周运动，经过时间t
s
到达b点，圆心为点o，转弯半径为l，加速度a恒定指向圆心，则有：
[0058][0059][0060][0061][0062][0063][0064][0065][0066][0067][0068]
其中，v为标定物的恒定速度值，v
ax
和v
ay
分别为标定物在a点的横向速度和纵向速度，为标定物在a点的运动方向与x轴的夹角，x
a
和y
a
为a点的横坐标与纵坐标，a
ax
和a
ay
分别为标定物在a点的横向加速度和纵向加速度，v
bx
和v
by
分别为标定物在b点的速度，横向速
度和纵向速度，为标定物在b点的运动方向与x轴的夹角，x
b
和y
b
为b点的横坐标与纵坐标，a
bx
和a
by
分别为标定物在b点的横向加速度和纵向加速度；
[0069]
根据上述匀速圆周运动模型特性，计算得到运动转移方程为：
[0070][0071]
其中，
[0072][0073][0074][0075][0076]
其中，其中，x
k
，y
k
，vx
k
，vy
k
，ax
k
，ay
k
分别为k时刻目标状态，包括此时横向位移、纵向位移、横向速度、纵向速度、横向加速度、纵向加速度，为目标在k时刻的运动方向与x轴的夹角，x
k 1
，y
k 1
，vx
k 1
，vy
k 1
，ax
k 1
，ay
k 1
分别为k 1时刻状态转移后的目标状态，包括此时横向位移、纵向位移、横向速度、纵向速度、横向加速度、纵向加速度，为目标在k 1时刻的运动方向与x轴的夹角，vx
k
‑1，vy
k
‑1分别为k
‑
1时刻目标的横向速度、纵向速度，v为目标的恒定速度值，a为目标的恒定加速度值，ω为目标的恒定转角速度，t
s
为运动状态转移时间。
[0077]
该运动转移方程为非线性方程，对其进行一阶泰勒展开，其中匀速圆周运动的加速度指向圆心，大小不变，得到ctrv模型转移矩阵为：
[0078][0079]
其中：
[0080]
[0081][0082][0083]
其中，v
y
′
，v
x
′
分别为v
x
和v
y
上一帧的状态量；
[0084]
同理得到ca模型转移矩阵为
[0085][0086]
cv模型转移矩阵为
[0087][0088]
建立基于卡尔曼多运动模型的目标跟踪模型，假设无参数噪声：
[0089][0090]
p(k|k
‑
1)＝a
i
p(k
‑
1|k
‑
1)a
it
q(k)
[0091]
k＝p(k|k
‑
1)h
t
(hp(k|k
‑
1)h
t
r)
‑1[0092][0093]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0094]
其中，其中为目标的状态方程，a为目标的状态转移矩阵，w为状态方程每个参数的噪声，p为状态的协方差，q为噪声协方差，其中z为传感器量测，h为转换矩阵，r为观测噪声协方差，k为卡尔曼增益系数。
[0095]
还提供一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述任一项所述方法的步骤。
[0096]
还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上述任一项所述方法的步骤。
[0097]
与现有技术相比，本发明的有益效果为：
[0098]
1.使用多运动模型对目标进行跟踪，可以提高滤波器的估计精度；
[0099]
2.使用多运动模型切换的方法，每次只计算一个模型，不增加过多计算量；
[0100]
3.直接基于卡尔曼使用线性模型与非线性模型(cv，ca与ctrv)，避免卡尔曼滤波器与扩展卡尔曼滤波器的切换；
[0101]
4.将ctrv模型变形为与其他模型相同的状态量，避免了其它状态量的存储与同步计算迭代。
附图说明
[0102]
图1为本发明实施例中目标轨迹预测的示意图；
[0103]
图2为本发明实施例中匀速圆周运动模型的原理示意图；
[0104]
图3为本发明实施例中切换运动模型的流程示意图；
[0105]
图4为本发明实施例中毫米波雷达的观测目标轨迹示意图。
具体实施方式
[0106]
为了便于本领域普通技术人员理解和应用本发明，下面结合附图及实施例对本发明作进一步的详细叙述，应当理解，此处所描述的实施例仅用于说明和解释本发明，并不用于限定本发明。
[0107]
卡尔曼滤波器作为传感器目标追踪的基本算法，能根据传感器的输入量测计算目标状态的最优估计，卡尔曼滤波的基本步骤分为两步：
[0108]
首先选择目标的运动模型，估计目标的噪声，对于目标下一刻的状态进行预测；
[0109][0110][0111]
其中为目标的状态方程，a为目标的状态转移矩阵，w为状态方程每个参数的噪声，p为状态的协方差，q为噪声协方差。
[0112]
接下来根据传感器量测信息对目标进行更新
[0113]
k＝p(k|k
‑
1)h
t
(hp(k|k
‑
1)h
t
r)
‑1[0114][0115]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0116]
其中z为传感器量测，h为转换矩阵，r为观测噪声协方差。
[0117]
在卡尔曼滤波的预测中常用的运动模型包括cv、ca、ctrv、ctra，其中前两个是线性模型，后两个是非线性模型，卡尔曼滤波只适用于线性模型，对于非线性模型，一般使用扩展卡尔曼。
[0118]
对于机动目标跟踪中，由于其运动状态的不确定性，使用单一的运动模型跟踪效果不理想。
[0119]
1.匀速圆周模型设计
[0120]
ctrv模型的状态量为基于极坐标系，ca模型的状态量为基于笛卡尔坐标系，cv模型的状态量为
基于笛卡尔坐标系。
[0121]
将ctrv模型转换成笛卡尔坐标系下的匀速圆周运动模型，便于直接切换场景的同时避免的协方差信息存储迭代。
[0122]
匀速圆周运动过程：a以固定角速度ω在做匀速圆周运动，经过时间t
s
到达b点，圆心是点o，转弯半径为l，加速度a恒定指向圆心，如图2所示，
[0123]
则有：
[0124]
其中
[0125][0126][0127][0128][0129][0130][0131]
其中：
[0132]
根据上述匀速圆周运动特性，计算得到运动转移方程为：
[0133][0134]
其中：
[0135][0136][0137][0138][0139][0140]
运动转移方程为非线性方程，对其进行一阶泰勒展开，其中匀速圆周运动的加速度指向圆心，大小不变，所以不对其进行更新以减少计算量，得到转移矩阵为：
[0159][0160]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0161]
其中：
[0162][0163]
i＝{1，2，3}
[0164][0165][0166]
其中：
[0167][0168][0169]
(v
y
′
，v
x
′
为上一帧状态值)
[0170]
2，模型切换，
[0171]
根据场景将这段时间分为三个阶段，前期减速阶段，中期变道阶段，后期加速阶段，分别对应a1，a3，a2[0172]
即，第一个阶段：
[0173][0174]
p(k|k
‑
1)＝a1p(k
‑
1|k
‑
1)a
1t
q(k)
[0175]
k＝p(k|k
‑
1)h
t
(hp(k|k
‑
1)h
t
r)
‑1[0176][0177]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0178]
第二个阶段：
[0179]
[0180]
p(k|k
‑
1)＝a3p(k
‑
1|k
‑
1)a
3t
q(k)
[0181]
k＝p(k|k
‑
1)h
t
(hp(k|k
‑
1)h
t
r)
‑1[0182][0183]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0184]
第三个阶段：
[0185][0186]
p(k|k
‑
1)＝a2p(k
‑
1|k
‑
1)a
2t
q(k)
[0187]
k＝p(k|k
‑
1)h
t
(hp(k|k
‑
1)h
t
r)
‑1[0188][0189]
p(k|k)＝p(k|k
‑
1)
‑
khp(k|k
‑
1)
[0190]
3，根据上述模型，计算最优估计值如图1所示。
[0191]
以上所述仅是对本发明的较佳实例，并非对本发明的范围进行限定，故在不脱离本发明设计精神的前提下，本领域普通工程技术人员对本发明所述的构造、特征及原理所做的等效变化或装饰，均应落入本发明申请专利的保护范围内。