CN113963025B - 水下自适应机动目标快速跟踪及追踪方法 - Google Patents

水下自适应机动目标快速跟踪及追踪方法 Download PDF

Info

Publication number
CN113963025B
CN113963025B CN202111233813.1A CN202111233813A CN113963025B CN 113963025 B CN113963025 B CN 113963025B CN 202111233813 A CN202111233813 A CN 202111233813A CN 113963025 B CN113963025 B CN 113963025B
Authority
CN
China
Prior art keywords
target
state
underwater
vector
axis
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111233813.1A
Other languages
English (en)
Other versions
CN113963025A (zh
Inventor
苏骏
李亚安
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Institute of Northwestern Polytechnical University
Original Assignee
Shenzhen Institute of Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Institute of Northwestern Polytechnical University filed Critical Shenzhen Institute of Northwestern Polytechnical University
Priority to CN202111233813.1A priority Critical patent/CN113963025B/zh
Publication of CN113963025A publication Critical patent/CN113963025A/zh
Application granted granted Critical
Publication of CN113963025B publication Critical patent/CN113963025B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/66—Sonar tracking systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/88—Sonar systems specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Abstract

本发明公开了水下自适应机动目标快速跟踪及追踪方法，通过分析目标运动态势，利用非线性卡尔曼滤波器实时的跟踪目标并求解其运动状态参数，再根据估计的运动参数使观测平台可以自适应的追踪目标。

Description

水下自适应机动目标快速跟踪及追踪方法
技术领域
本发明属于水下环境下机动目标的快速跟踪领域，具体涉及水下自适应机动目标快速跟踪及追踪方法。
背景技术
以水声探测技术为主的水声装备是我国海军水下作战的重要耳目，同时也是水下目标跟踪的最有效手段。对于水下目标跟踪系统，声纳是其最重要的组成部分。根据目标跟踪系统工作方式，水下声纳目标跟踪系统主要分为主动跟踪方式、被动跟踪方式和主被动联合跟踪方式。主动声纳目标跟踪系统是指声纳主动发出脉冲波或连续波，并接收从水下或者水面目标反射回来的回波，最后根据回波信息计算目标的状态参数。被动声纳目标跟踪系统是指声纳被动地接收目标发出的辐射信号来获得目标的参数，适用于声纳系统在隐蔽自身的环境下跟踪目标。主被动联合声纳跟踪系统采用主动和被动相结合的跟踪方式，具有跟踪精度高且不易被敌发现的优点。
水下目标跟踪的复杂性主要体现在以下几个方面：
1.水下目标跟踪系统在很多情况下是非线性的，即目标状态参数和量测数据之间是非线性关系。以纯方位被动声纳跟踪系统为例，考虑直角坐标系跟踪系统，此时目标的状态模型是线性模型，而量测模型是目标状态的反正切函数，即量测模型是严重非线性的，在这种情况下要获得精确的目标状态估计是比较困难的。
2.目标运动状态的随机性和运动模型的不确定性，使得目标跟踪问题的处理更加困难。
随着新技术在水下的应用越来越广泛，水下目标的性能不断提高，其在水下的运动速度越来越快，机动性能也大幅度提高，而传统的自适应追踪方法已经很难有效拦截到目标。世界各国在提高目标识别、目标检测的装备同时，开始着手于研究如何更快更精准的追踪目标，以达到最终可以有效拦截目标的目的。过于单一的采用直接追踪法、固定提前角法、平行接近法等常用导引方法难以满足其追踪并拦截目标的要求。
发明内容
针对现有技术中的上述不足，本发明提供的水下自适应机动目标快速跟踪及追踪方法解决了水下追踪目标不能更快跟精准的问题。
为了达到上述发明目的，本发明采用的技术方案为：水下自适应机动目标快速跟踪及追踪方法，其特征在于，包括以下步骤：
S1、建立水下目标的状态方程，进而得到水下目标的量测方程；
S2、根据水下目标的状态方程和无迹卡尔曼滤波器，得到前一时刻的水下目标的状态估计值；
S3、通过无味变换的方法将前一时刻的水下目标的状态估计值变换为sigma点构成的向量；
S4、将sigma点构成的向量通过状态方程传播，得到sigma点的预测状态和误差协方差；
S5、根据预测状态和误差协方差重新计算sigma点，并将得到的新的sigma点通过量测方程传播，得到新的sigma点预测状态向量和预测协方差，进而通过无迹卡尔曼滤波器得到当前时刻更新的状态向量和更新的误差协方差；
S6、通过IMM算法将前一时刻所有的更新状态向量和误差协方差进行交互混合，并将其混合结果通过滤波得到预测状态值和预测协方差，进而组合输出目标的估计状态和误差协方差；
S7、根据目标的估计状态和误差协方差得到目标的位置，通过圆轨迹追踪法和圆轨迹固定提前角追踪法追踪目标，完成水下自适应机动目标快速跟踪及追踪。
进一步地：所述步骤S1中，建立水下目标的状态方程的方法具体为：基于水下目标的状态转移矩阵Fk|k-1、零均值的高斯白噪声wk|k-1和系统驱动的噪声转移矩阵Γk|k-1，根据观测平台的第k-1时刻采样的水下目标状态向量
Figure BDA0003316805310000031
建立关于观测平台的第k时刻采样的水下目标状态向量
Figure BDA0003316805310000032
的水下目标的状态方程；
Figure BDA0003316805310000033
状态转移矩阵
Figure BDA0003316805310000034
系统驱动的噪声转移矩阵
Figure BDA0003316805310000035
T为观测平台的采样间隔；
水下目标状态向量为
Figure BDA0003316805310000036
观测平台运动状态向量为
Figure BDA0003316805310000037
其中，
Figure BDA0003316805310000038
为水下目标的x轴、y轴和z轴位置，
Figure BDA0003316805310000039
为水下目标的x轴、y轴和z轴的速度，
Figure BDA00033168053100000310
为观测平台的x轴、y轴和z轴位置，
Figure BDA00033168053100000311
为观测平台的x轴、y轴和z轴的速度，k为观测平台的采样时刻序数；
水下目标的量测方程的表达式具体为：
Figure BDA0003316805310000041
Figure BDA0003316805310000042
Figure BDA0003316805310000043
Figure BDA0003316805310000044
式中，zk为量测向量，h(*)为水下目标的量测方程关系式，vk为量测噪声向量，
Figure BDA0003316805310000045
为水平方位角θ的量测向量，
Figure BDA0003316805310000046
为竖直俯仰角
Figure BDA0003316805310000047
的量测向量，
Figure BDA0003316805310000048
为距离r的量测向量，
Figure BDA0003316805310000049
为水平方位角θ的量测噪声向量，
Figure BDA00033168053100000410
为竖直俯仰角
Figure BDA00033168053100000411
的量测噪声向量，
Figure BDA00033168053100000412
为距离r的量测噪声向量；其中，
Figure BDA00033168053100000413
和
Figure BDA00033168053100000414
均服从零均值的高斯分布。
上述进一步方案的有益效果为：根据水下目标的状态方程和量测方程可以确定水下目标的位置。
进一步地：所述步骤S3中，无味变换的方法具体为通过下式，根据前一时刻的水下目标的状态估计值
Figure BDA00033168053100000415
和水下目标的状态向量
Figure BDA00033168053100000416
的误差协方差Pk-1，得到2nx+1个sigma点的向量ξk-1；
Figure BDA00033168053100000417
Figure BDA00033168053100000418
式中，wn为sigma点的向量的权重，n为权重的序数，nx为状态维度，λ为影响sigma点传播速度的缩放参数，
Figure BDA00033168053100000419
为sigma点的向量构成的矩阵的第m列，m为矩阵列的序数。
上述进一步方案的有益效果为：无味变换是一种针对非线性变换去近似统计随机变量的方法，可以确定的取样点来近似后验概率。
进一步地：所述步骤S4中，得到sigma点的预测状态
Figure BDA00033168053100000420
和误差协方差Pk|k-1的表达式具体为：
Figure BDA0003316805310000051
Figure BDA0003316805310000052
Figure BDA0003316805310000053
式中，
Figure BDA0003316805310000054
为无迹卡尔曼滤波器中的sigma采样点，
Figure BDA0003316805310000055
为权重的序数n的sigma点的向量，Qk|k-1为协方差矩阵，其中Qk|k-1＝Γk|k-1qΓk|k-1，q是零均值的高斯白噪声wk|k-1对应的误差协方差矩阵。
上述进一步方案的有益效果为：通过状态方程传播可以得到sigma点的预测状态和误差协方差。
进一步地：所述步骤S5中，新的sigma点ζk-1的表达式具体为：
Figure BDA0003316805310000056
将新的sigma点通过量测方程传播，生成的新的sigma点量测方程的量测向量
Figure BDA0003316805310000057
和第n权重的新的sigma点量测方程的量测向量
Figure BDA0003316805310000058
的表达式具体为：
Figure BDA0003316805310000059
Figure BDA00033168053100000510
计算新的sigma点预测状态向量
Figure BDA00033168053100000511
和预测协方差
Figure BDA00033168053100000512
的表达式具体为：
Figure BDA00033168053100000513
Figure BDA00033168053100000514
式中，
Figure BDA00033168053100000515
为无迹卡尔曼滤波器中的新的sigma采样点；
当前时刻更新的状态向量
Figure BDA00033168053100000516
和更新的误差协方差Pk的表达式具体为：
Figure BDA00033168053100000517
Figure BDA00033168053100000518
式中，
Figure BDA0003316805310000061
为更新的第k-1时刻状态向量，
Figure BDA0003316805310000062
为测量方差矩阵，且
Figure BDA0003316805310000063
其中Rk为观测平台k时刻采样的测量噪声方差。
上述进一步方案的有益效果为：通过无迹卡尔曼滤波器滤波可以得到更新的状态向量和更新的误差协方差。
进一步地：所述步骤S6包括以下分步骤：
S61、通过IMM算法将前一时刻所有无迹卡尔曼滤波器的状态向量和误差协方差分别进行交互混合，得到IMM算法的模型交互概率，进而根据IMM算法的模型交互概率计算IMM算法匹配模型的混合初始条件；
其中，计算经过交互混合后的IMM算法的模型交互概率
Figure BDA0003316805310000064
的表达式具体为：
Figure BDA0003316805310000065
式中，
Figure BDA0003316805310000066
是IMM算法模型概率，i为k时刻IMM算法匹配的模型，j为k+1时刻IMM算法匹配模型，cj为IMM算法模型j的归一化因子，pij为从IMM算法模型i到IMM算法模型j的转移概率；
所述混合初始条件包括
Figure BDA0003316805310000067
和
Figure BDA0003316805310000068
计算IMM算法匹配模型的混合初始条件的表达式具体为：
Figure BDA0003316805310000069
式中，
Figure BDA00033168053100000610
为在k时刻IMM算法模型i的状态估计值，
Figure BDA00033168053100000611
为在k时刻IMM算法模型j的状态估计值，值
Figure BDA00033168053100000612
为IMM算法模型j的交互协方差，
Figure BDA00033168053100000613
为在k时刻IMM算法模型i的误差协方差；
S62、基于每个IMM算法匹配模型的混合初始条件，通过无迹卡尔曼滤波器滤波，得到每个无迹卡尔曼滤波器的预测状态值和预测协方差；
S63、基于每个无迹卡尔曼滤波器的预测状态值和预测协方差，通过计算似然函数和归一化系数，得到组合输出目标的估计状态；
其中，计算似然函数
Figure BDA0003316805310000071
的表达式具体为：
Figure BDA0003316805310000072
式中，
Figure BDA0003316805310000073
为IMM算法模型j的新息协方差矩阵，且
Figure BDA0003316805310000074
Figure BDA0003316805310000075
为IMM算法模型j的测量方差矩阵，
Figure BDA0003316805310000076
为IMM算法模型j采样k+1时刻的测量噪声方差；zk+1为观测平台k+1时刻获取的观测值，
Figure BDA0003316805310000077
为IMM算法模型j的sigma点量测方程的量测向量；
计算归一化系数c的表达式具体为：
Figure BDA0003316805310000078
式中，g为IMM算法模型的序数，r为IMM算法模型的总数，
Figure BDA0003316805310000079
为IMM算法第g个模型的似然函数，cg为IMM算法第g个模型的归一化因子；
得到组合输出的目标估计状态
Figure BDA00033168053100000710
和误差协方差Pk+1的表达式具体为：
Figure BDA00033168053100000711
Figure BDA00033168053100000712
式中，
Figure BDA00033168053100000713
为在k时刻IMM算法模型g的误差协方差。
上述进一步方案的有益效果为：IMM算法可以对并行滤波所得的所有滤波器估计结果进行加权综合，以获得最终的估计状态。
进一步地：所述步骤S7中，得到的水下目标的位置具体为B点，其坐标为(xT,yT,zT)；
圆轨迹追踪法具体为：根据观测平台的位置具体为A点和水下目标的位置具体为B点确定运动圆轨迹的水平方向角和竖直方向角，进而通过自适应追踪轨迹控制观测平台追踪目标；
其中，自适应追踪轨迹的表达式具体为：
Figure BDA0003316805310000081
Figure BDA0003316805310000082
Figure BDA0003316805310000083
式中，
Figure BDA0003316805310000084
为在k时刻A点的x轴位置，
Figure BDA0003316805310000085
为在k时刻A点的y轴位置，
Figure BDA0003316805310000086
为在k时刻A点的z轴位置，
Figure BDA0003316805310000087
为在k-1时刻A点的x轴位置，
Figure BDA0003316805310000088
为在k-1时刻A点的y轴位置，
Figure BDA0003316805310000089
为在k-1时刻A点的z轴位置，vS为观测平台的速度，θ6为观测平台运动的水平方向角，
Figure BDA00033168053100000810
为观测平台运动的竖直方向角，T为采样时间间隔；
其中，计算观测平台运动的水平方向角θ6的表达式具体为：
Figure BDA00033168053100000811
式中，xS为观测平台的x轴坐标，yS为观测平台的y轴坐标，a和b均为水平面圆心O1点坐标(a,b)的值，圆心O1点坐标(a,b)通过下式计算获得；
(xS-a)+(yS-b)＝r2
(xT-a)+(yT-b)＝r2
式中，r为水平面中圆轨迹的半径，且
Figure BDA00033168053100000812
其中，
Figure BDA00033168053100000813
为观测平台到目标的水平面上的投影距离，θ5为水平面中线段BO1与线段AB的夹角；
计算观测平台运动的竖直方向角
Figure BDA00033168053100000814
的表达式具体为：
Figure BDA00033168053100000815
式中，xS为观测平台的x轴坐标，zS为观测平台的z轴坐标，az和bz均为竖直面圆心O2点坐标(az,bz)的值，圆心O2点坐标(az,bz)通过下式计算获得；
Figure BDA0003316805310000091
Figure BDA0003316805310000092
式中，rz为竖直面中圆轨迹的半径，且
Figure BDA0003316805310000093
其中，
Figure BDA0003316805310000094
为观测平台到目标的竖直面上的投影距离，
Figure BDA0003316805310000095
为竖直面中线段BO2与线段AB的夹角。
上述进一步方案的有益效果为：本发明采用圆轨迹追踪法和圆轨迹固定提前角追踪法追踪目标，比直接追踪目标效率高。
进一步地：确定水平面圆心O1点的方法为：
计算水平面圆心O1点坐标表达式的根为(a1,b1)和(a2,b2)，其满足：
Figure BDA0003316805310000096
Figure BDA0003316805310000097
其中，参数
Figure BDA0003316805310000098
当水平面中目标运动方向在线段AB的上方时，取(a,b)＝(a1,b1)，当水平面中目标运动方向在线段AB的下方时，取(a,b)＝(a2,b2)；
确定竖直面圆心O2点的方法为：
计算竖直面圆心O2点坐标表达式的根为
Figure BDA0003316805310000099
和
Figure BDA00033168053100000910
Figure BDA00033168053100000911
Figure BDA00033168053100000912
其中，参数
Figure BDA00033168053100000913
当竖直面中目标运动方向在线段AB的上方时，取
Figure BDA00033168053100000914
当竖直面中目标运动方向在线段AB的下方时，取
Figure BDA00033168053100000915
上述进一步方案的有益效果为：观测平台可以根据圆轨迹追踪法自适应的追踪目标。
进一步地：所述步骤S7中，圆轨迹固定提前角追踪法具体为：当水平方向角θ2和θ3满足θ2-θ3<3°时，使观测平台运动的水平方向角θ6＝θ1，继续通过自适应追踪轨迹控制观测平台追踪水下目标；
其中，θ2为水平面中线段AB与x轴的夹角，θ3为目标的运动方向与x轴的夹角，θ1为线段AB与y轴的夹角。
上述进一步方案的有益效果为：本发明的圆轨迹固定提前角追踪法可以让观测平台更早的做出角度变化，以便快速追踪水下目标。
本发明的有益效果为：
(1)本发明的圆轨迹固定提前角追踪法可以根据水下目标的状态自适应地调整观测平台的追踪方向，最终可以靠近水下目标并满足追踪的终止条件。
(2)本发明的圆轨迹固定提前角追踪法会根据目标的运动方式实时的调整观测平台的运动方向，相比与直接追踪法会多使用一定的时间以更准确的追踪目标。
(3)观测平台的追踪运动方向是依据已估计出的水下目标的状态进行调整的，圆轨迹固定提前角追踪法采用了快速IMM算法-无迹卡尔曼滤波的方法，当水下目标进行机动时，可以准确的估计出水下目标的转角率。
附图说明
图1为水下自适应机动目标快速跟踪及追踪方法的流程图；
图2为本发明的圆轨迹追踪法示意图；
图3为单次仿真跟踪追踪效果对比图；
图4为两种追踪方法的目标跟踪位置均方根误差；
图5为两种追踪方法的目标跟踪速度均方根误差。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
如图1所示，在本发明的一个实施例中，水下自适应机动目标快速跟踪及追踪方法，包括以下步骤：
S1、建立水下目标的状态方程，进而得到水下目标的量测方程；
S2、根据水下目标的状态方程和无迹卡尔曼滤波器，得到前一时刻的水下目标的状态估计值；
S3、通过无味变换的方法将前一时刻的水下目标的状态估计值变换为sigma点构成的向量；
S4、将sigma点构成的向量通过状态方程传播，得到sigma点的预测状态和误差协方差；
S5、根据预测状态和误差协方差重新计算sigma点，并将得到的新的sigma点通过量测方程传播，得到新的sigma点预测状态向量和预测协方差，进而通过无迹卡尔曼滤波器得到当前时刻更新的状态向量和更新的误差协方差；
S6、通过IMM算法将前一时刻所有的更新状态向量和误差协方差进行交互混合，并将其混合结果通过滤波得到预测状态值和预测协方差，进而组合输出目标的估计状态和误差协方差；
S7、根据目标的估计状态和误差协方差得到目标的位置，通过圆轨迹追踪法和圆轨迹固定提前角追踪法追踪目标，完成水下自适应机动目标快速跟踪及追踪。
水下目标跟踪是依据估计和滤波理论，对声纳或者水下传感器接收到的量测信息进行处理，估计水下运动目标的运动状态(位置、速度、和加速度等)的过程。水下目标跟踪问题主要包括两个模型，即目标状态模型和量测模型。
所述步骤S1中，目标状态向量为
Figure BDA0003316805310000121
观测平台运动状态向量为
Figure BDA0003316805310000122
其中，
Figure BDA0003316805310000123
为目标的x轴、y轴和z轴位置，
Figure BDA0003316805310000124
为目标的x轴、y轴和z轴的速度，
Figure BDA0003316805310000125
为观测平台的x轴、y轴和z轴位置，
Figure BDA0003316805310000126
为观测平台的x轴、y轴和z轴的速度，k为观测平台第k时刻采样；定义水下目标的运动状态为匀速直线运动；
建立水下目标的状态方程的方法具体为：基于水下目标的状态转移矩阵Fk|k-1、零均值的高斯白噪声wk|k-1和系统驱动的噪声转移矩阵Γk|k-1，根据观测平台的第k-1时刻采样的水下目标状态向量
Figure BDA0003316805310000127
建立关于观测平台的第k时刻采样的水下目标状态向量
Figure BDA0003316805310000128
的水下目标的状态方程；
Figure BDA0003316805310000129
状态转移矩阵
Figure BDA00033168053100001210
系统驱动的噪声转移矩阵
Figure BDA00033168053100001211
T为观测平台的采样间隔；
水下目标的量测方程的表达式具体为：
Figure BDA0003316805310000131
Figure BDA0003316805310000132
Figure BDA0003316805310000133
Figure BDA0003316805310000134
式中，zk为量测向量，h(*)为水下目标的量测方程关系式，vk为量测噪声向量，
Figure BDA0003316805310000135
为水平方位角θ的量测向量，
Figure BDA0003316805310000136
为竖直俯仰角
Figure BDA0003316805310000137
的量测向量，
Figure BDA0003316805310000138
为距离r的量测向量，
Figure BDA0003316805310000139
为水平方位角θ的量测噪声向量，
Figure BDA00033168053100001310
为竖直俯仰角
Figure BDA00033168053100001311
的量测噪声向量，
Figure BDA00033168053100001312
为距离r的量测噪声向量；其中，
Figure BDA00033168053100001313
和
Figure BDA00033168053100001314
均服从零均值的高斯分布。
所述步骤S3中，无味变换的方法具体为：通过下式，根据前一时刻的水下目标的状态估计值
Figure BDA00033168053100001315
和水下目标的状态向量
Figure BDA00033168053100001316
的误差协方差Pk-1，得到2nx+1个sigma点的向量ξk-1；
Figure BDA00033168053100001317
Figure BDA00033168053100001318
式中，wn为sigma点的向量的权重，n为权重的序数，nx为状态维度，λ为影响sigma点传播速度的缩放参数，
Figure BDA00033168053100001319
为sigma点的向量构成的矩阵的第m列，m为矩阵列的序数。
所述步骤S4中，得到sigma点的预测状态
Figure BDA00033168053100001320
和误差协方差Pk|k-1的表达式具体为：
Figure BDA00033168053100001321
Figure BDA0003316805310000141
式中，
Figure BDA0003316805310000142
为无迹卡尔曼滤波器中的sigma采样点，
Figure BDA0003316805310000143
为状态维度nx的sigma点的向量，Qk|k-1为协方差矩阵，其中Qk|k-1＝Γk|k-1qΓk|k-1，q是零均值的高斯白噪声wk|k-1对应的误差协方差矩阵。
所述步骤S5中，新的sigma点ζk-1的表达式具体为：
Figure BDA0003316805310000144
将新的sigma点通过量测方程传播，生成的新的sigma点量测方程的量测向量
Figure BDA0003316805310000145
和第n权重的新的sigma点量测方程的量测向量
Figure BDA0003316805310000146
的表达式具体为：
Figure BDA0003316805310000147
Figure BDA0003316805310000148
计算新的sigma点预测状态向量
Figure BDA0003316805310000149
和预测协方差
Figure BDA00033168053100001410
的表达式具体为：
Figure BDA00033168053100001411
Figure BDA00033168053100001412
式中，
Figure BDA00033168053100001413
为无迹卡尔曼滤波器中的新的sigma采样点；
当前时刻更新的状态向量
Figure BDA00033168053100001414
和更新的误差协方差Pk的表达式具体为：
Figure BDA00033168053100001415
Figure BDA00033168053100001416
式中，
Figure BDA00033168053100001417
为更新的第k-1时刻状态向量，
Figure BDA00033168053100001418
为测量方差矩阵，且
Figure BDA00033168053100001419
其中Rk为观测平台k时刻采样的测量噪声方差。
所述步骤S6包括以下分步骤：
S61、通过IMM算法将前一时刻所有无迹卡尔曼滤波器的状态向量和误差协方差分别进行交互混合，得到IMM算法的模型交互概率，进而根据IMM算法的模型交互概率计算IMM算法匹配模型的混合初始条件；
其中，计算经过交互混合后的IMM算法的模型交互概率
Figure BDA0003316805310000151
的表达式具体为：
Figure BDA0003316805310000152
式中，
Figure BDA0003316805310000153
是IMM算法模型概率，i为k时刻IMM算法匹配的模型，j为k+1时刻IMM算法匹配模型，cj为IMM算法模型j的归一化因子，pij为从IMM算法模型i到IMM算法模型j的转移概率；
所述混合初始条件包括
Figure BDA0003316805310000154
和
Figure BDA0003316805310000155
计算IMM算法匹配模型的混合初始条件的表达式具体为：
Figure BDA0003316805310000156
式中，
Figure BDA0003316805310000157
为在k时刻IMM算法模型i的状态估计值，
Figure BDA0003316805310000158
为在k时刻IMM算法模型j的状态估计值，值
Figure BDA0003316805310000159
为IMM算法模型j的交互协方差，
Figure BDA00033168053100001510
为在k时刻IMM算法模型i的误差协方差；
S62、基于每个IMM算法匹配模型的混合初始条件，通过无迹卡尔曼滤波器滤波，得到每个无迹卡尔曼滤波器的预测状态值和预测协方差；
每个IMM算法匹配模型中的无迹卡尔曼滤波器均相同，最终获得每个无迹卡尔曼滤波器的预测状态值和预测协方差。
S63、基于每个无迹卡尔曼滤波器的预测状态值和预测协方差，通过计算似然函数和归一化系数，得到组合输出目标的估计状态；
其中，计算似然函数
Figure BDA00033168053100001511
的表达式具体为：
Figure BDA0003316805310000161
式中，
Figure BDA0003316805310000162
为IMM算法模型j的新息协方差矩阵，且
Figure BDA0003316805310000163
Figure BDA0003316805310000164
为IMM算法模型j的测量方差矩阵，
Figure BDA0003316805310000165
为IMM算法模型j采样k+1时刻的测量噪声方差；zk+1为观测平台k+1时刻获取的观测值，
Figure BDA0003316805310000166
为IMM算法模型j的sigma点量测方程的量测向量；
计算归一化系数c的表达式具体为：
Figure BDA0003316805310000167
式中，g为IMM算法模型的序数，r为IMM算法模型的总数，
Figure BDA0003316805310000168
为IMM算法第g个模型的似然函数，cg为IMM算法第g个模型的归一化因子；
得到组合输出的目标估计状态
Figure BDA0003316805310000169
和误差协方差Pk+1的表达式具体为：
Figure BDA00033168053100001610
Figure BDA00033168053100001611
式中，
Figure BDA00033168053100001612
为在k时刻IMM算法模型g的误差协方差。
所述步骤S7中，得到的水下目标的位置具体为B点，其坐标为(xT,yT,zT)；
在本实施例中，如图2所示，圆轨迹追踪法具体为：根据观测平台的位置具体为A点和水下目标的位置具体为B点确定运动圆轨迹的水平方向角和竖直方向角，进而通过自适应追踪轨迹控制观测平台追踪目标；
其中，自适应追踪轨迹的表达式具体为：
Figure BDA00033168053100001613
Figure BDA00033168053100001614
Figure BDA00033168053100001615
式中，
Figure BDA00033168053100001616
为在k时刻A点的x轴位置，
Figure BDA00033168053100001617
为在k时刻A点的y轴位置，
Figure BDA00033168053100001618
为在k时刻A点的z轴位置，
Figure BDA0003316805310000171
为在k-1时刻A点的x轴位置，
Figure BDA0003316805310000172
为在k-1时刻A点的y轴位置，
Figure BDA0003316805310000173
为在k-1时刻A点的z轴位置，vS为观测平台的速度，θ6为观测平台运动的水平方向角，
Figure BDA0003316805310000174
为观测平台运动的竖直方向角，T为采样时间间隔；
其中，计算观测平台运动的水平方向角θ6的表达式具体为：
Figure BDA0003316805310000175
式中，xS为观测平台的x轴坐标，yS为观测平台的y轴坐标，a和b均为水平面圆心O1点坐标(a,b)的值，圆心O1点坐标(a,b)通过下式计算获得；
(xS-a)+(yS-b)＝r2
(xT-a)+(yT-b)＝r2
式中，r为水平面中圆轨迹的半径，且
Figure BDA0003316805310000176
其中，
Figure BDA0003316805310000177
为观测平台到目标的水平面上的投影距离，θ5为水平面中线段BO1与线段AB的夹角；
计算观测平台运动的竖直方向角
Figure BDA0003316805310000178
的表达式具体为：
Figure BDA0003316805310000179
式中，xS为观测平台的x轴坐标，zS为观测平台的z轴坐标，az和bz均为竖直面圆心O2点坐标(az,bz)的值，圆心O2点坐标(az,bz)通过下式计算获得；
Figure BDA00033168053100001710
Figure BDA00033168053100001711
式中，rz为竖直面中圆轨迹的半径，且
Figure BDA00033168053100001712
其中，
Figure BDA00033168053100001713
为观测平台到水下目标的竖直面上的投影距离，
Figure BDA00033168053100001714
为竖直面中线段BO2与线段AB的夹角。
计算水平面圆心O1点坐标表达式的根为(a1,b1)和(a2,b2)；
Figure BDA0003316805310000181
Figure BDA0003316805310000182
其中，参数
Figure BDA0003316805310000183
当水平面中水下目标运动方向在线段AB的上方时，取(a,b)＝(a1,b1)，当水平面中水下目标运动方向在线段AB的下方时，取(a,b)＝(a2,b2)；
计算竖直面圆心O2点坐标表达式的根为
Figure BDA0003316805310000184
和
Figure BDA0003316805310000185
Figure BDA0003316805310000186
Figure BDA0003316805310000187
其中，参数
Figure BDA0003316805310000188
当竖直面中水下目标运动方向在线段AB的上方时，取
Figure BDA0003316805310000189
当竖直面中水下目标运动方向在线段AB的下方时，取
Figure BDA00033168053100001810
所述步骤S7中，圆轨迹固定提前角追踪法具体为：当水平方向角θ2和θ3满足θ2-θ3<3°时，使观测平台运动的水平方向角θ6＝θ1，继续通过自适应追踪轨迹控制观测平台追踪目标；
其中，θ2为水平面中线段AB与x轴的夹角，θ3为目标的运动方向与x轴的夹角，θ1为线段AB与y轴的夹角。
在本发明的第二个实施例中，定义目标将不再做匀速直线运动，而是满足转角率恒定的转弯机动(转弯的时刻未知，转弯速率未知，仅假设水下目标在水平面机动)，目标的仿真参数如下表所示：
Figure BDA00033168053100001811
Figure BDA0003316805310000191
其中，CT(Constant turn)模型的状态向量
Figure BDA0003316805310000192
与状态转移矩阵
Figure BDA0003316805310000193
具体为：
Figure BDA0003316805310000194
Figure BDA0003316805310000195
在水下目标跟踪过程中，由于无迹卡尔曼滤波器不知道目标何时发生转角，也不知道转角持续的时间，观测平台采用水平方向上的圆轨迹追踪法，深度方向上采取圆轨迹固定提前角追踪法，提前角设定为-10°。
观测平台可以追踪水下目标到达的平均最小距离distancemin的表达式具体为：
Figure BDA0003316805310000196
式中，
Figure BDA0003316805310000197
为观测平台与估计的目标之间距离的向量，N为蒙特卡洛试验的总数，其中，第k次蒙特卡洛试验的距离的向量
Figure BDA0003316805310000198
的表达式具体为：
Figure BDA0003316805310000199
观测平台与目标真值可以到达的平均最小距离distancemin_true的表达式具体为：
Figure BDA0003316805310000201
式中，d为观测平台与估计的目标之间距离的向量，其中，第k次蒙特卡洛试验的真值距离的向量d(k)的表达式具体为：
Figure BDA0003316805310000202
式中，
Figure BDA0003316805310000203
为目标的真值。
观测平台达到目标预测轨迹的最小距离平均所需时间tmin_true的表达式具体为：
Figure BDA0003316805310000204
式中，num(distancemin_true)为到达最小距离时的平台采样次数，T是量测的最小采样周期。
在蒙特卡洛重复试验下，观测平台成功追踪目标的概率Ptrue为
Figure BDA0003316805310000205
式中，Itrue为示性函数，观测平台追到目标的真实位置时为1，否则为0；
在本实施例中，由于水下目标和观测平台均为点，没有碰撞体积。并且观测平台获取量测数据时存在采样间隔则，而不是连续采样。因此水下目标与观测平台这两个点不可能重合，需要设置一定的终止条件，当满足该条件时认为平台追上了目标。本发明的终止条件为：
当观测平台与水下目标之间的距离向量的值小于估计距离的门限值L时，则令观测平台的预测追踪结果Ipredict为1，并且当水下目标和观测平台间的真实距离小于估计距离的门限值L+1时，令示性函数Itrue为1。
如图3所示，为蒙特卡洛实验次数为N＝100时的仿真结果，蓝色实线和紫色虚线分别表示圆轨迹固定提前角追踪法和直接追踪法中，观测平台的追踪轨迹；红色实线表示目标的真实运动轨迹，而黑色实线与青蓝色实线则分别表示两种追踪方法下的目标跟踪估计轨迹。
如图4和图5所示，圆轨迹固定提前角追踪法和直接追踪法均对于水下目标状态(包括位置及速度)的估计精度类似，误差均比较小。观测平台的追踪运动方向是依据已估计出的水下目标状态进行调整的，而两种追踪策略均采用了快速IMM算法-无迹卡尔曼滤波的方法。当目标进行机动时，可以准确的估计出目标的转角率。
本发明方法实施的过程为：旨在通过分析水下目标运动态势，利用非线性无迹卡尔曼滤波器实时的跟踪水下目标并求解其运动状态参数，再根据估计的运动参数使观测平台可以自适应的追踪水下目标。
本发明以观测平台和目标的状态向量建立状态方程，并建立量测方程，利用无迹卡尔曼滤波器得到状态估计值，并通过无味变换的方法变换为sigma点构成的向量，将sigma点构成的向量通过状态方程传播，得到新的sigma点预测状态和预测协方差，完成sigma点的量测更新，根据IMM算法计算混合概率和混合初始条件，将其通过无迹卡尔曼滤波器滤波得到预测状态值和预测协方差，并更新模型概率，得到输出组合后的目标的估计状态和协方差，根据目标的估计状态得到水下目标的位置，并根据观测平台的位置确定圆轨迹追踪法运动的水平方向角和竖直方向角自适应追踪轨迹控制观测平台追踪水下目标，当观测平台接近时，采用圆轨迹固定提前角追踪法追踪目标，完成水下自适应机动目标快速跟踪及追踪。
本发明的有益效果为：圆轨迹固定提前角追踪法可以根据目标的状态自适应地调整观测平台的追踪方向，最终可以靠近水下目标并满足追踪的终止条件；而直接追踪法从一开始观测平台就径直的朝向目标所在的位置运动。虽然可以大体方向上不断的靠近水下目标，但是受到观测平台采样频率、水下目标及观测平台自身移动速度、观测平台自身最大转角率等条件的限制，观测平台与水下目标距离较近时无法及时调整自身运动方向，不能满足追踪的终止条件，最终与水下目标错过。
本发明的圆轨迹固定提前角追踪法会根据水下目标的运动方式实时的调整观测平台的运动方向，相比与直接追踪法会多使用一定的时间以更准确的追踪水下目标。
观测平台的追踪运动方向是依据已估计出的水下目标的状态进行调整的，圆轨迹固定提前角追踪法采用了快速IMM算法-无迹卡尔曼滤波的方法，当水下目标进行机动时，可以准确的估计出水下目标的转角率。
在本发明的描述中，需要理解的是，术语“中心”、“厚度”、“上”、“下”、“水平”、“顶”、“底”、“内”、“外”、“径向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性或隐含指明的技术特征的数量。因此，限定由“第一”、“第二”、“第三”的特征可以明示或隐含地包括一个或者更多个该特征。

Claims (6)

1.水下自适应机动目标快速跟踪及追踪方法，其特征在于，包括以下步骤：
S1、建立水下目标的状态方程，进而得到水下目标的量测方程；
S2、根据水下目标的状态方程和无迹卡尔曼滤波器，得到前一时刻的水下目标的状态估计值；
S3、通过无味变换的方法将前一时刻的水下目标的状态估计值变换为sigma点构成的向量；
S4、将sigma点构成的向量通过状态方程传播，得到sigma点的预测状态和误差协方差；
S5、根据预测状态和误差协方差重新计算sigma点，并将得到的新的sigma点通过量测方程传播，得到新的sigma点预测状态向量和预测协方差，进而通过无迹卡尔曼滤波器得到当前时刻更新的状态向量和更新的误差协方差；
S6、通过IMM算法将前一时刻所有的更新状态向量和误差协方差进行交互混合，并将其混合结果通过滤波得到预测状态值和预测协方差，进而组合输出目标的估计状态和误差协方差；
S7、根据目标的估计状态和误差协方差得到目标的位置，通过圆轨迹追踪法和圆轨迹固定提前角追踪法追踪目标，完成水下自适应机动目标快速跟踪及追踪；
所述步骤S7中，得到的水下目标的位置具体为B点，其坐标为(xT,yT,zT)；
圆轨迹追踪法具体为：根据观测平台的位置具体为A点和水下目标的位置具体为B点确定运动圆轨迹的水平方向角和竖直方向角，进而通过自适应追踪轨迹控制观测平台追踪目标；
其中，自适应追踪轨迹的表达式具体为：
Figure FDA0003741538760000021
Figure FDA0003741538760000022
Figure FDA0003741538760000023
式中，
Figure FDA0003741538760000024
为在k时刻A点的x轴位置，
Figure FDA0003741538760000025
为在k时刻A点的y轴位置，
Figure FDA0003741538760000026
为在k时刻A点的z轴位置，
Figure FDA0003741538760000027
为在k-1时刻A点的x轴位置，
Figure FDA0003741538760000028
为在k-1时刻A点的y轴位置，
Figure FDA0003741538760000029
为在k-1时刻A点的z轴位置，vS为观测平台的速度，θ6为观测平台运动的水平方向角，
Figure FDA00037415387600000215
为观测平台运动的竖直方向角，T为采样时间间隔；
其中，计算观测平台运动的水平方向角θ6的表达式具体为：
Figure FDA00037415387600000210
式中，xS为观测平台的x轴坐标，yS为观测平台的y轴坐标，a和b均为水平面圆心O1点坐标(a,b)的值，圆心O1点坐标(a,b)通过下式计算获得；
(xS-a)+(yS-b)＝r2
(xT-a)+(yT-b)＝r2
式中，r为水平面中圆轨迹的半径，且
Figure FDA00037415387600000211
其中，
Figure FDA00037415387600000212
为观测平台到目标的水平面上的投影距离，θ5为水平面中线段BO1与线段AB的夹角；
计算观测平台运动的竖直方向角
Figure FDA00037415387600000213
的表达式具体为：
Figure FDA00037415387600000214
式中，xS为观测平台的x轴坐标，zS为观测平台的z轴坐标，az和bz均为竖直面圆心O2点坐标(az,bz)的值，圆心O2点坐标(az,bz)通过下式计算获得；
Figure FDA0003741538760000031
Figure FDA0003741538760000032
式中，rz为竖直面中圆轨迹的半径，且
Figure FDA0003741538760000033
其中，
Figure FDA0003741538760000034
为观测平台到目标的竖直面上的投影距离，
Figure FDA0003741538760000035
为竖直面中线段BO2与线段AB的夹角；
确定水平面圆心O1点的方法为：
计算水平面圆心O1点坐标表达式的根为(a1,b1)和(a2,b2)，其满足：
Figure FDA0003741538760000036
Figure FDA0003741538760000037
其中，参数
Figure FDA0003741538760000038
当水平面中目标运动方向在线段AB的上方时，取(a,b)＝(a1,b1)，当水平面中目标运动方向在线段AB的下方时，取(a,b)＝(a2,b2)；
确定竖直面圆心O2点的方法为：
计算竖直面圆心O2点坐标表达式的根为
Figure FDA0003741538760000039
和
Figure FDA00037415387600000310
Figure FDA00037415387600000311
Figure FDA00037415387600000312
其中，参数
Figure FDA00037415387600000313
当竖直面中目标运动方向在线段AB的上方时，取
Figure FDA00037415387600000314
当竖直面中目标运动方向在线段AB的下方时，取
Figure FDA00037415387600000315
所述步骤S7中，圆轨迹固定提前角追踪法具体为：当水平方向角θ2和θ3满足θ2-θ3<3°时，使观测平台运动的水平方向角θ6＝θ1，继续通过自适应追踪轨迹控制观测平台追踪水下目标；
其中，θ2为水平面中线段AB与x轴的夹角，θ3为目标的运动方向与x轴的夹角，θ1为线段AB与y轴的夹角。
2.根据权利要求1所述的水下自适应机动目标快速跟踪及追踪方法，其特征在于，所述步骤S1中，建立水下目标的状态方程的方法具体为：基于水下目标的状态转移矩阵Fk|k-1、零均值的高斯白噪声wk|k-1和系统驱动的噪声转移矩阵Γk|k-1，根据观测平台的第k-1时刻采样的水下目标状态向量
Figure FDA0003741538760000041
建立关于观测平台的第k时刻采样的水下目标状态向量
Figure FDA0003741538760000042
的水下目标的状态方程：
Figure FDA0003741538760000043
状态转移矩阵
Figure FDA0003741538760000044
系统驱动的噪声转移矩阵
Figure FDA0003741538760000045
T为观测平台的采样间隔；
水下目标状态向量为
Figure FDA0003741538760000046
观测平台运动状态向量为
Figure FDA0003741538760000047
其中，
Figure FDA0003741538760000048
为水下目标的x轴、y轴和z轴位置，
Figure FDA0003741538760000049
为水下目标的x轴、y轴和z轴的速度，
Figure FDA00037415387600000410
为观测平台的x轴、y轴和z轴位置，
Figure FDA0003741538760000051
为观测平台的x轴、y轴和z轴的速度，k为观测平台的采样时刻序数；
水下目标的量测方程的表达式具体为：
Figure FDA0003741538760000052
Figure FDA0003741538760000053
Figure FDA0003741538760000054
Figure FDA0003741538760000055
式中，zk为量测向量，h(*)为水下目标的量测方程关系式，vk为量测噪声向量，
Figure FDA0003741538760000056
为水平方位角θ的量测向量，
Figure FDA0003741538760000057
为竖直俯仰角
Figure FDA0003741538760000058
的量测向量，
Figure FDA0003741538760000059
为距离r的量测向量，
Figure FDA00037415387600000510
为水平方位角θ的量测噪声向量，
Figure FDA00037415387600000511
为竖直俯仰角
Figure FDA00037415387600000512
的量测噪声向量，
Figure FDA00037415387600000513
为距离r的量测噪声向量；其中，
Figure FDA00037415387600000514
和
Figure FDA00037415387600000515
均服从零均值的高斯分布。
3.根据权利要求2所述的水下自适应机动目标快速跟踪及追踪方法，其特征在于，所述步骤S3中，无味变换的方法具体为：通过下式，根据前一时刻的水下目标的状态估计值
Figure FDA00037415387600000516
和水下目标的状态向量
Figure FDA00037415387600000517
的误差协方差Pk-1，得到2nx+1个sigma点的向量ξk-1；
Figure FDA00037415387600000518
Figure FDA00037415387600000519
式中，wn为sigma点的向量的权重，n为权重的序数，nx为状态维度，λ为影响sigma点传播速度的缩放参数，
Figure FDA0003741538760000061
为sigma点的向量构成的矩阵的第m列，m为矩阵列的序数。
4.根据权利要求3所述的水下自适应机动目标快速跟踪及追踪方法，其特征在于，所述步骤S4中，得到sigma点的预测状态
Figure FDA0003741538760000062
和误差协方差Pk|k-1的表达式具体为：
Figure FDA0003741538760000063
Figure FDA0003741538760000064
Figure FDA0003741538760000065
式中，
Figure FDA0003741538760000066
为无迹卡尔曼滤波器中的sigma采样点，
Figure FDA0003741538760000067
为权重的序数n的sigma点的向量，Qk|k-1为协方差矩阵，其中Qk|k-1＝Γk|k-1qΓk|k-1，q是零均值的高斯白噪声wk|k-1对应的误差协方差矩阵。
5.根据权利要求4所述的水下自适应机动目标快速跟踪及追踪方法，其特征在于，所述步骤S5中，新的sigma点ζk-1的表达式具体为：
Figure FDA0003741538760000068
将新的sigma点通过量测方程传播，生成的新的sigma点量测方程的量测向量
Figure FDA0003741538760000069
和第n权重的新的sigma点量测方程的量测向量
Figure FDA00037415387600000610
的表达式具体为：
Figure FDA00037415387600000611
Figure FDA00037415387600000612
计算新的sigma点预测状态向量
Figure FDA00037415387600000613
和预测协方差
Figure FDA00037415387600000614
的表达式具体为：
Figure FDA00037415387600000615
Figure FDA0003741538760000071
式中，
Figure FDA0003741538760000072
为无迹卡尔曼滤波器中的新的sigma采样点；
当前时刻更新的状态向量
Figure FDA0003741538760000073
和更新的误差协方差Pk的表达式具体为：
Figure FDA0003741538760000074
Figure FDA0003741538760000075
式中，
Figure FDA0003741538760000076
为更新的第k-1时刻状态向量，
Figure FDA00037415387600000713
为测量方差矩阵，且
Figure FDA0003741538760000077
其中Rk为观测平台k时刻采样的测量噪声方差。
6.根据权利要求5所述的水下自适应机动目标快速跟踪及追踪方法，其特征在于，所述步骤S6包括以下分步骤：
S61、通过IMM算法将前一时刻所有无迹卡尔曼滤波器的状态向量和误差协方差分别进行交互混合，得到IMM算法的模型交互概率，进而根据IMM算法的模型交互概率计算IMM算法匹配模型的混合初始条件；
其中，计算经过交互混合后的IMM算法的模型交互概率
Figure FDA0003741538760000078
的表达式具体为：
Figure FDA0003741538760000079
式中，
Figure FDA00037415387600000710
是IMM算法模型概率，i为k时刻IMM算法匹配的模型，j为k+1时刻IMM算法匹配模型，cj为IMM算法模型j的归一化因子，pij为从IMM算法模型i到IMM算法模型j的转移概率；
所述混合初始条件包括
Figure FDA00037415387600000711
和
Figure FDA00037415387600000712
计算IMM算法匹配模型的混合初始条件的表达式具体为：
Figure FDA0003741538760000081
式中，
Figure FDA0003741538760000082
为在k时刻IMM算法模型i的状态估计值，
Figure FDA0003741538760000083
为在k时刻IMM算法模型j的状态估计值，值
Figure FDA0003741538760000084
为IMM算法模型j的交互协方差，
Figure FDA0003741538760000085
为在k时刻IMM算法模型i的误差协方差；
S62、基于每个IMM算法匹配模型的混合初始条件，通过无迹卡尔曼滤波器滤波，得到每个无迹卡尔曼滤波器的预测状态值和预测协方差；
S63、基于每个无迹卡尔曼滤波器的预测状态值和预测协方差，通过计算似然函数和归一化系数，得到组合输出目标的估计状态；
其中，计算似然函数
Figure FDA0003741538760000086
的表达式具体为：
Figure FDA0003741538760000087
式中，
Figure FDA0003741538760000088
为IMM算法模型j的新息协方差矩阵，且
Figure FDA0003741538760000089
Figure FDA00037415387600000810
为IMM算法模型j的测量方差矩阵，
Figure FDA00037415387600000811
为IMM算法模型j采样k+1时刻的测量噪声方差；zk+1为观测平台k+1时刻获取的观测值，
Figure FDA00037415387600000812
为IMM算法模型j的sigma点量测方程的量测向量；
计算归一化系数c的表达式具体为：
Figure FDA00037415387600000813
式中，g为IMM算法模型的序数，r为IMM算法模型的总数，
Figure FDA00037415387600000814
为IMM算法第g个模型的似然函数，cg为IMM算法第g个模型的归一化因子；
得到组合输出的目标估计状态
Figure FDA00037415387600000815
和误差协方差Pk+1的表达式具体为：
Figure FDA0003741538760000091
Figure FDA0003741538760000092
式中，
Figure FDA0003741538760000093
为在k时刻IMM算法模型g的误差协方差。
CN202111233813.1A 2021-10-22 2021-10-22 水下自适应机动目标快速跟踪及追踪方法 Active CN113963025B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111233813.1A CN113963025B (zh) 2021-10-22 2021-10-22 水下自适应机动目标快速跟踪及追踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111233813.1A CN113963025B (zh) 2021-10-22 2021-10-22 水下自适应机动目标快速跟踪及追踪方法

Publications (2)

Publication Number Publication Date
CN113963025A CN113963025A (zh) 2022-01-21
CN113963025B true CN113963025B (zh) 2022-08-16

Family

ID=79466452

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111233813.1A Active CN113963025B (zh) 2021-10-22 2021-10-22 水下自适应机动目标快速跟踪及追踪方法

Country Status (1)

Country Link
CN (1) CN113963025B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115128597B (zh) * 2022-08-25 2022-11-25 西安电子科技大学 基于imm-stekf的非高斯噪声下机动目标跟踪方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103308896A (zh) * 2013-05-07 2013-09-18 北京工商大学 一种适于非引擎机动目标的高精度跟踪方法
CN112947593A (zh) * 2020-08-25 2021-06-11 北京蓝警科技有限公司 一种利用无人机拦截目标的方法和系统
CN113467470A (zh) * 2021-07-23 2021-10-01 西安理工大学 一种无人自主小车的轨迹跟踪控制方法

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SG101444A1 (en) * 2001-04-20 2004-01-30 Singapore Tech Aerospace Ltd A maneuvering target tracking method via modifying the interacting multiple model (imm) and the interacting acceleration compensation (iac) algorithms
WO2008079301A2 (en) * 2006-12-21 2008-07-03 Massachusetts Institute Of Technology Methods and apparatus for 3d surface imaging using active wave-front sampling
CN103926579B (zh) * 2014-04-23 2016-06-22 东南大学 一种开放式阵列信号的仿真方法
JP6770393B2 (ja) * 2016-10-04 2020-10-14 株式会社豊田中央研究所 トラッキング装置及びプログラム
CN111290376B (zh) * 2018-11-22 2021-05-04 中国科学院沈阳自动化研究所 一种水面/水下无人航行器圆形轨迹跟踪方法
CN109933087B (zh) * 2019-03-18 2021-12-10 西安爱生技术集团公司 无人机与地面机动目标虚拟编队战位保持控制方法
CN110044356B (zh) * 2019-04-22 2020-10-09 北京壹氢科技有限公司 一种通信拓扑切换下分布式协同目标跟踪方法
CN110516193B (zh) * 2019-07-08 2023-03-21 杭州电子科技大学 笛卡尔坐标系下基于转换瑞利滤波器的机动目标跟踪方法
CN110618606B (zh) * 2019-09-30 2022-06-17 哈尔滨工程大学 一种复合干扰下的欠驱动auv反步自适应模糊滑模控制方法
CN110956653B (zh) * 2019-11-29 2021-05-04 中国科学院空间应用工程与技术中心 相关滤波器与运动估计融合的卫星视频动态目标追踪方法
CN112269164A (zh) * 2020-10-15 2021-01-26 西北工业大学 深海可靠声路径下基于干涉结构匹配处理弱目标定位方法
CN112486168B (zh) * 2020-11-19 2022-05-20 哈尔滨工程大学 一种基于回转圆的移动式对接轨迹规划方法
CN113064154B (zh) * 2021-03-18 2023-09-01 沈阳理工大学 一种空中目标跟踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103308896A (zh) * 2013-05-07 2013-09-18 北京工商大学 一种适于非引擎机动目标的高精度跟踪方法
CN112947593A (zh) * 2020-08-25 2021-06-11 北京蓝警科技有限公司 一种利用无人机拦截目标的方法和系统
CN113467470A (zh) * 2021-07-23 2021-10-01 西安理工大学 一种无人自主小车的轨迹跟踪控制方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A Derivative-free Distributed Optimization Algorithm with Applications in Multi-Agent Target Tracking;Said Al-Abri等;《2021 American Control Conference (ACC)》;20210528;第3834-3839页 *
无人机目标跟踪与避障研究;孟凡琨;《中国博士学位论文全文数据库 工程科技II辑》;20190115(第（2019）01期);C031-66 *

Also Published As

Publication number Publication date
CN113963025A (zh) 2022-01-21

Similar Documents

Publication Publication Date Title
CN109116311B (zh) 基于知识辅助稀疏迭代协方差估计的杂波抑制方法
CN109655822A (zh) 一种改进的航迹起始方法
CN104035083B (zh) 一种基于量测转换的雷达目标跟踪方法
CN110441788B (zh) 一种基于单线激光雷达的无人艇环境感知方法
CN104199022B (zh) 一种基于目标模态估计的临近空间高超声速目标跟踪方法
CN107255805B (zh) 基于加权最小二乘的雷达目标rcs的预测方法
CN110209180B (zh) 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法
CN113963025B (zh) 水下自适应机动目标快速跟踪及追踪方法
CN110471029B (zh) 一种基于扩展卡尔曼滤波的单站无源定位方法及装置
CN109856623B (zh) 一种针对多雷达直线航迹线的目标状态估计方法
CN113156418B (zh) 基于蒙特卡洛仿真的雷达目标跟踪精度预测方法
CN112666519B (zh) 一种基于广义二阶时延差的水下目标高精度定位方法
RU2692837C2 (ru) Способ определения параметров движения шумящего объекта
CN108761384A (zh) 一种抗差的传感器网络目标定位方法
CN112684411B (zh) 一种基于改进到达频率差的水下目标定位方法
CN115469314A (zh) 一种均匀圆环阵稳健水下目标方位跟踪方法及系统
Li et al. Tracking an underwater maneuvering target using an adaptive Kalman filter
CN113075633A (zh) 一种基于距离-俯仰联合字典的目标定位方法
CN113534164A (zh) 一种基于主被动联合声纳阵列的目标路径跟踪方法
Jianxing et al. A Particle Filtering Method for Radar Target Tracking
CN114578368B (zh) 一种基于目标方位与线谱的小平台水声被动探测信息融合方法
CN114577212B (zh) 一种基于方位和频率的单站水下机动目标运动分析方法
CN110703187B (zh) 一种基于水下机动平台的声信标测向及定位方法
CN112198504B (zh) 一种主被动观测特征交织的融合滤波方法
CN116224320B (zh) 一种极坐标系下处理多普勒量测的雷达目标跟踪方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant