CN115077530B - 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统 - Google Patents

一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统 Download PDF

Info

Publication number
CN115077530B
CN115077530B CN202210678112.7A CN202210678112A CN115077530B CN 115077530 B CN115077530 B CN 115077530B CN 202210678112 A CN202210678112 A CN 202210678112A CN 115077530 B CN115077530 B CN 115077530B
Authority
CN
China
Prior art keywords
auv
state
time
calculating
measurement
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
CN202210678112.7A
Other languages
English (en)
Other versions
CN115077530A (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.)
Harbin Institute of Technology Weihai
Original Assignee
Harbin Institute of Technology Weihai
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 Harbin Institute of Technology Weihai filed Critical Harbin Institute of Technology Weihai
Priority to CN202210678112.7A priority Critical patent/CN115077530B/zh
Publication of CN115077530A publication Critical patent/CN115077530A/zh
Application granted granted Critical
Publication of CN115077530B publication Critical patent/CN115077530B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • 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
    • 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/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Algebra (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Operations Research (AREA)
  • Automation & Control Theory (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Navigation (AREA)

Abstract

本发明提出一种基于强跟踪扩维ECKF算法的多AUV协同导航方法及系统，涉及多自主水下航行器协同导航技术领域，用以解决主从式多AUV协同导航系统先验信息不准确或系统状态发生突变时定位失效的问题。本发明的技术要点包括：根据强跟踪滤波器(StrongTracking Filter,STF)的原理，用STF对基于扩维ECKF的多AUV协同导航方法进行改进，提高多AUV协同导航系统的定位导航性能。本发明可应用于主从式多AUV协同导航之中。

Description

一种基于强跟踪扩维ECKF算法的多AUV协同导航方法及系统
技术领域
本发明涉及多自主水下航行器(Autonomous Underwater Vehicle,AUV)协同导航技术领域，具体涉及一种基于强跟踪扩维嵌入式容积卡尔曼滤波的多AUV协同导航方法及系统。
背景技术
多AUV系统中，跟随AUV结合相对观测获取的导航信息，能够通过协同导航算法对自身位置进行滤波估计，以此降低系统整体的导航误差。多AUV协同导航算法根据类型主要分为三种，分别为：基于优化理论的协同导航算法、基于图论的协同导航算法、基于贝叶斯估计的协同导航算法。
多AUV协同导航的过程中，由于系统初始状态的先验信息或是噪声统计特性不准确，在复杂恶劣的海洋环境中系统状态可能发生突变，从而引起协同导航算法的状态估计值误差增大，甚至有可能导致滤波发散。基于贝叶斯估计的扩维嵌入式容积卡尔曼滤波(Embedded Cubature Kalman Filter,ECKF)算法通过嵌入式容积准则增加了容积点的选取并改变其权值，从而抑制传统容积卡尔曼滤波随着状态空间维数增加而增大的定位估计误差；但其仍旧对系统状态突变以及初始状态的先验信息比较敏感，难以保证异常测量或是扰动情况下的稳定性。
发明内容
鉴于以上问题，本发明提出一种基于强跟踪扩维嵌入式容积卡尔曼滤波(StrongTracking Filter ECKF,STF-ECKF)算法的多AUV协同导航方法及系统，用以解决主从式多AUV协同导航系统先验信息不准确或系统状态发生突变时定位失效的问题。
根据本发明的一方面，提供一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，该方法包括以下步骤：
步骤一、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；
步骤二、采用扩维ECKF算法对多个AUV进行状态预测和量测预测；
步骤三、在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。
进一步地，步骤一中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：
其中，表示k时刻跟随AUV的n维状态变量，xk、yk、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，Vk表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的nw维过程噪声向量；vk＝[vk]T表示k时刻相对距离测量噪声vk所构成的nv维量测噪声向量；
其中，状态函数表示为：
式中，Δt表示航位推算的系统周期；wk表示k时刻AUV航行的角速度；
量测函数表示为：
式中，上标M代表领航AUV；上标S代表跟随AUV。
进一步地，步骤二中状态预测的具体过程包括：
假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差Pk-1k-1已知，对Pk-1k-1进行Cholesky分解得到过程变量Ck-1k-1：
使用nw维过程噪声对状态变量进行扩维，得到过程变量以及
计算容积点Xi,k-1k-1：
其中，i＝1,2,…,N,ξi表示容积点集；Qk-1表示k-1时刻的过程噪声协方差矩阵；
根据ECKF算法中的嵌入式容积准则，替换容积点集ξi以及权值
式中，[0]i为维数为n的零向量；[δ]＝[s1σ2,s1σ2,…,s1σ2]T，σ＝1且s1＝±1；
根据系统函数传播容积点Xi,k-1k-1的状态变量分量以及过程噪声分量得到传播后的容积点
根据系统函数传播后的容积点计算k时刻的状态预测值
计算k时刻的状态误差协方差预测值Pkk-1：
进一步地，步骤二中量测预测的具体过程包括：
通过量测函数传播的状态变量分量
根据量测函数传播后的容积点计算k时刻的观测预测值
式中，rk＝E(vk)为vk的均值；
计算k时刻的自相关协方差矩阵PZZ,kk-1：
式中，表示量测噪声协方差矩阵；
计算k时刻互相关协方差矩阵PXZ,kk-1：
进一步地，步骤三的具体步骤包括：
通过k时刻实际相对距离量测值Zk以及观测预测值计算残差γk：
计算滤波残差γk的协方差阵
式中，ρ表示遗忘因子；
计算中间过程变量Nk和Mk：
式中，β表示弱化因子；
计算自适应渐消因子λk：
在自相关协方差矩阵PZZ,kk-1中引入渐消因子λk：
计算k时刻的卡尔曼滤波增益Kk：
计算k时刻状态估计值
计算k时刻的状态误差协方差估计值Pk|k：
根据本发明的另一方面，提供一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，该系统包括：
系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；
协同定位模块，其配置成采用扩维ECKF算法对跟随AUV进行位置估计，实现多AUV协同定位导航；包括预测子模块和修正子模块，所述预测子模块配置成采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；所述修正子模块配置成在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。
进一步地，所述系统方程构建模块中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：
其中，表示k时刻跟随AUV的n维状态变量，xk、yk、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，Vk表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的nw维过程噪声向量；vk＝[vk]T表示k时刻相对距离测量噪声vk所构成的nv维量测噪声向量；
其中，状态函数表示为：
式中，Δt表示航位推算的系统周期；wk表示k时刻AUV航行的角速度；
量测函数表示为：
式中，上标M代表领航AUV；上标S代表跟随AUV。
进一步地，所述预测子模块中状态预测的具体过程包括：
假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差Pk-1|k-1已知，对Pk-1|k-1进行Cholesky分解得到过程变量Ck-1|k-1：
使用nw维过程噪声对状态变量进行扩维，得到过程变量以及
计算容积点Xi,k-1|k-1：
其中，i＝1,2,…,N,ξi表示容积点集；Qk-1表示k-1时刻的过程噪声协方差矩阵；
根据ECKF算法中的嵌入式容积准则，替换容积点集ξi以及权值
式中，[0]i为维数为n的零向量；[δ]＝[s1σ2,s1σ2,…,s1σ2]T，σ＝1且s1＝±1；
根据系统函数传播容积点Xi,k-1|k-1的状态变量分量以及过程噪声分量得到传播后的容积点
根据系统函数传播后的容积点计算k时刻的状态预测值
计算k时刻的状态误差协方差预测值Pk|k-1：
进一步地，所述预测子模块中量测预测的具体过程包括：
通过量测函数传播的状态变量分量
根据量测函数传播后的容积点计算k时刻的观测预测值
式中，rk＝E(vk)为vk的均值；
计算k时刻的自相关协方差矩阵PZZ,k|k-1：
式中，表示量测噪声协方差矩阵；
计算k时刻互相关协方差矩阵PXZ,k|k-1：
进一步地，所述修正子模块中修正多个AUV的状态估计值和状态误差协方差估计值的具体步骤包括：
通过k时刻实际相对距离量测值Zk以及观测预测值计算残差γk：
计算滤波残差γk的协方差阵
式中，ρ表示遗忘因子；
计算中间过程变量Nk和Mk：
式中，β表示弱化因子；
计算自适应渐消因子λk：
在自相关协方差矩阵PZZ,k|k-1中引入渐消因子λk：
计算k时刻的卡尔曼滤波增益Kk：
计算k时刻状态估计值
计算k时刻的状态误差协方差估计值Pk|k：
本发明的有益技术效果是：
针对系统模型不确定或系统发生突变时引起的滤波发散、数值不稳定问题，本发明根据强跟踪滤波器(Strong Tracking Filter,STF)的原理，用STF对基于扩维ECKF的多AUV协同导航算法进行改进，以解决协同导航系统模型存在一定的不确定性或是系统发生突变时协同导航精度下降的问题，从而提高协同导航系统的定位导航性能。
附图说明
本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解，所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分，而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。
图1是本发明实施例一种基于强跟踪扩维ECKF算法的多AUV协同导航方法的流程图；
图2是本发明实施例中实验仿真过程的领航艇和跟随艇真实航迹示例图；
图3是本发明实施例中实验仿真过程中本发明方法与另外两种方法的导航结果对比图；
图4是本发明实施例中实验仿真过程中本发明方法与另外两种方法的导航结果误差曲线对比图；其中，(a)对应x轴与y轴的位置误差曲线比较；(b)对应航向角的误差曲线比较；
图5是本发明实施例中实验仿真过程中本发明方法与另外两种方法的导航结果-定位误差曲线对比图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然，所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例，而不是全部的。基于本发明中的实施方式或实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例，都应当属于本发明保护的范围。
本发明实施例提供一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，如图1所示，该方法包括以下步骤：
步骤一：建立AUV协同定位系统的离散时间状态方程和量测方程，表示为：
其中，表示跟随AUV在k时刻的n维状态变量，且xk、yk及分别代表跟随AUV在k时刻的x轴位置，y轴位置及航向角；表示看k时刻跟随AUV的传感器输入量，且Vk表示跟随AUV在k时刻航行方向上的前向速度；是k时刻跟随AUV获取前向速度噪声以及航向角传感器噪声所构成的nw维过程噪声向量；vk＝[vk]T是k时刻相对距离测量噪声vk所构成的nv维量测噪声向量。
考虑过程噪声协方差矩阵与量测噪声协方差矩阵是正定对称阵且存在非线性系统互不相关的系统过程噪声wk和观测噪声vk，wk的均值为0且vk的均值为rk，假设系统具有如式(2)所示的时变统计特性：
状态函数为：
式中，Δt表示航位推算的系统周期；wk为k时刻AUV航行的角速度。
量测函数为：
式中，上标M代表领航AUV；上标S代表跟随AUV。
对于多AUV协同导航非线性系统，采用扩维ECKF滤波对状态变量和过程噪声进行容积变换，同时引入强跟踪滤波，从而提出扩维STF-ECKF协同导航算法。
步骤二：状态预测
假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差Pk-1|k-1已知，对Pk-1|k-1进行Cholesky分解可以得到过程变量Ck-1|k-1：
使用nw维过程噪声对状态变量进行扩维可得过程变量以及
计算容积点Xi,k-1|k-1
根据ECKF算法中的嵌入式容积准则，替换容积点集ξi以及权值
式中，[0]i为维数为n的零向量；[δ]＝[s1σ2,s1σ2,…,s1σ2]T为容积点集且下标i表示的是在点集中此基本点的序号。令σ＝1，且s1＝±1，对n维容积点集[s1σ2,s1σ2,…,s1σ2]T中的元素进行全排列可得：
通过系统函数传播容积点Xi,k-1|k-1的状态变量分量以及过程噪声分量可得传播后的容积点
通过系统函数传播后的容积点计算k时刻的状态预测值
计算k时刻的状态误差协方差预测值Pk|k-1：
步骤三：量测预测
通过量测函数传播的状态变量分量可得传播后的容积点Zi,k|k-1：
通过量测函数传播后的容积点计算k时刻的观测预测值
计算k时刻的自相关协方差矩阵PZZ,k|k-1：
计算k时刻互相关协方差矩阵PXZ,k|k-1：
步骤四：渐消因子的计算与引入
通过k时刻实际相对距离量测值Zk以及观测预测值计算残差γk：
计算滤波残差γk的协方差阵
式中，ρ为遗忘因子，一般取0.95≤ρ≤0.995。
计算中间过程变量Nk和Mk：
式中，β为弱化因子，可以取βk＝1-(1-ρ)/(1-ρk-1)。
计算自适应渐消因子λk：
在状态预测协方差矩阵PZZ,k|k-1中引入渐消因子λk：
步骤五：状态更新
计算k时刻的卡尔曼滤波增益Kk：
计算k时刻状态估计值
计算k时刻的状态误差协方差估计值Pk|k：
跟随AUV通过更新的状态估计值可以完成自身位置的估计与修正，同时将更新的状态估计值以及状态误差协方差估计值Pk|k作为下一次迭代中后验概率密度函数的初始值，并不断重复步骤二至步骤五实现多AUV系统的协同导航。
进一步通过实验验证本发明的技术效果。
在二维平面上，建立一个基于单领航艇的多AUV协同导航系统，系统中领航艇和跟随艇仅设置了一艘。假设领航艇通过自主导航的定位误差足够小，而跟随艇通过船位推算自主导航技术对自身进行定位，与此同时领航艇和跟随艇间只进行相对距离量测以获取相对距离信息。
领航艇沿某一方向进行匀速直线航行，初始位置坐标设置为(0,500)，初始航向角设置为45°；跟随艇进行曲线圆周运动，初始位置坐标设置为(500,500)，初始航向角同样设置为45°，这样能够尽量使领航AUV和跟随AUV相邻采样时刻相对方位变化更大，从而提高系统的可观测性；同时领航艇和跟随艇的运动速度在正常情况下保持为2m/s。系统状态更新周期Δt＝10s，运载体航行时间为T＝2500s。可得领航艇和跟随艇真实航迹，如图2所示。
跟随艇通过自身携带的速度传感器获取自身的速度信息，通过自身携带的方位传感器获取自身的航向角，显然在复杂的海洋环境下测量过程中的量测噪声是无法避免的，假设上述两种传感器的量测噪声满足高斯白噪声的条件，互不相关的同时其均值为零，设置这两种噪声的方差分别为和除此之外，领航艇通过声学测距的方法对艇间相对距离进行测量，测量时的量测噪声依旧满足高斯白噪声条件，设置其方差为
取扩维的状态估计初始值为：
P00＝diag[1 1 0.01 0 0] (29)
为了验证本发明方法对系统发生突变时依旧能够在较短时间内跟踪上从AUV，从而保证定位的精度，在系统时间为800s以及1400s时对从AUV设置了两次强机动，速度提高到3m/s并保持10s，以此来验证本发明方法对于多AUV协同导航的有效性。
图3给出了跟随AUV的真实运动轨迹以及分别采用CKF，扩维ECKF与本发明方法—扩维STF-ECKF协同导航算法得到的目标运动轨迹。根据对图3进行分析可知，跟随AUV做匀速圆周运动时基于扩维STF-ECKF与其他协同导航方法在系统发生突变时的定位精度的优劣对比。显而易见，根据本发明方法对跟随AUV进行协同导航定位的航迹几乎与跟随AUV的真实航迹相重合，在系统发生突变时也能快速恢复较高的定位精度，而根据基于其他滤波的协同导航方法所得的航迹与真实航迹存在不同程度的偏差。
图4(a)为三种算法得到的x轴与y轴位置误差曲线比较，图4(b)为三种算法的航向角的误差曲线比较。由图4(a)与图4(b)可知，在跟随AUV突然加速的情况下，三种算法的x轴与y轴位置误差以及航向角误差波形均出现不同大小的尖峰，本发明方法的尖峰最小，能有效地减小系统发生突变的情况下扩维ECKF算法协同定位的x轴与y轴位置以及航向角误差上升的幅度，而CKF算法的误差曲线收敛速度缓慢，难以保持较好的鲁棒性并存在发散的趋势。
图5为三种算法得到的定位误差曲线比较。通过分析图5可知，在系统时间800s以及1400s系统发生突变时，本发明方法相较于扩维ECKF算法的定位误差具有更快的收敛速度，且误差曲线波动幅度更小，说明其具有较好的准确性以及鲁棒性。而CKF算法的最大误差距离以及误差曲线波动程度出现了大幅度波动，具有发散的趋势，相较于本发明方法难以保持较高的准确性以及稳定性。表1是仿真场景中跟随AUV的x轴与y轴位置和航向角以及定位误差的均方根统计数据。
表1三种算法协同导航性能比较
结合图5以及表1可知，在两种场景下，通过本发明方法得到的位置、航向角和定位误差均小于采用CKF和扩维ECKF算法协同导航得到的误差。相较于CKF和扩维ECKF算法，本发明方法在仿真场景中协同导航性能提升了18.62％。由此可以看出特别是在系统出现扰动或是与状态发生突变的情况下，本发明方法能够有效抑制跟随AUV定位误差发散，同时减小协同定位误差，提高协同导航精度和稳定性。
表2列出了仿真场景下3种不同算法的总运行时间以及平均每次迭代的时间。3种算法都具有较高的运算效率，由于采用了嵌入式容积准则的扩维ECKF算法增加了容积点，使得运行时间相较于CKF有所增加。考虑到运算量，本发明方法将量测噪声从状态变量中取出并引入渐消因子，使状态变量的维度降低，减少了容积点个数，降低了计算的复杂度，虽然采用强跟踪算法会增加一定运算时间，但是总运行时间有所减少。相较于扩维ECKF算法，本发明方法在场景中协同导航效率提升了39.13％。
表2仿真场景中3种算法的运行时间
本发明另一实施例还提供一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，该系统包括：
系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；
协同定位模块，其配置成采用扩维ECKF算法对跟随AUV进行位置估计，实现多AUV协同定位导航；包括预测子模块和修正子模块，所述预测子模块配置成采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；所述修正子模块配置成在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。
本发明实施例一种基于强跟踪扩维ECKF算法的多AUV协同导航系统的功能可以由前述一种基于强跟踪扩维ECKF算法的多AUV协同导航方法说明，因此系统实施例未详述部分，可参见以上方法实施例，在此不再赘述。
尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

Claims (4)

1.一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，其特征在于，包括以下步骤：
步骤一、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；
步骤二、采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；其中，
状态预测的具体过程包括：
假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差Pk-1|k-1已知，对Pk-1|k-1进行Cholesky分解得到过程变量Ck-1|k-1：
使用nw维过程噪声对状态变量进行扩维，得到过程变量以及
计算容积点Xi,k-1|k-1：
其中，i＝1,2,…,N,ξi表示容积点集；Qk-1表示k-1时刻的过程噪声协方差矩阵；
根据ECKF算法中的嵌入式容积准则，替换容积点集ξi以及权值
式中，[0]i为维数为n的零向量；[δ]＝[s1σ2,s1σ2,…,s1σ2]T，σ＝1且s1＝±1；
根据系统函数传播容积点Xi,k-1|k-1的状态变量分量以及过程噪声分量得到传播后的容积点
根据系统函数传播后的容积点计算k时刻的状态预测值
计算k时刻的状态误差协方差预测值Pk|k-1：
量测预测的具体过程包括：
通过量测函数传播的状态变量分量
根据量测函数传播后的容积点计算k时刻的观测预测值
式中，rk＝E(vk)为vk的均值；
计算k时刻的自相关协方差矩阵PZZ,k|k-1：
式中，表示量测噪声协方差矩阵；
计算k时刻互相关协方差矩阵PXZ,k|k-1：
步骤三、在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性；具体步骤包括：
通过k时刻实际相对距离量测值Zk以及观测预测值计算残差γk：
计算滤波残差γk的协方差阵
式中，ρ表示遗忘因子；
计算中间过程变量Nk和Mk：
式中，β表示弱化因子；
计算自适应渐消因子λk：
在自相关协方差矩阵PZZ,k|k-1中引入渐消因子λk：
计算k时刻的卡尔曼滤波增益Kk：
计算k时刻状态估计值
计算k时刻的状态误差协方差估计值Pk|k：
2.根据权利要求1所述的一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，其特征在于，步骤一中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：
其中，表示k时刻跟随AUV的n维状态变量，xk、yk、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，Vk表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的nw维过程噪声向量；vk＝[vk]T表示k时刻相对距离测量噪声vk所构成的nv维量测噪声向量；
其中，状态函数表示为：
式中，Δt表示航位推算的系统周期；ωk表示k时刻AUV航行的角速度；
量测函数表示为：
式中，上标M代表领航AUV；上标S代表跟随AUV。
3.一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，其特征在于，包括：
系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；
协同定位模块，其配置成采用扩维ECKF算法对跟随AUV进行位置估计，实现多AUV协同定位导航；包括预测子模块和修正子模块，所述预测子模块配置成采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；所述修正子模块配置成在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性；其中所述预测子模块中状态预测的具体过程包括：
假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差Pk-1|k-1已知，对Pk-1|k-1进行Cholesky分解得到过程变量Ck-1|k-1：
使用nw维过程噪声对状态变量进行扩维，得到过程变量以及
计算容积点Xi,k-1|k-1：
其中，i＝1,2,…,N,ξi表示容积点集；Qk-1表示k-1时刻的过程噪声协方差矩阵；
根据ECKF算法中的嵌入式容积准则，替换容积点集ξi以及权值
式中，[0]i为维数为n的零向量；[δ]＝[s1σ2,s1σ2,…,s1σ2]T，σ＝1且s1＝±1；
根据系统函数传播容积点Xi,k-1|k-1的状态变量分量以及过程噪声分量得到传播后的容积点
根据系统函数传播后的容积点计算k时刻的状态预测值
计算k时刻的状态误差协方差预测值Pk|k-1：
量测预测的具体过程包括：
通过量测函数传播的状态变量分量
根据量测函数传播后的容积点计算k时刻的观测预测值
式中，rk＝E(vk)为vk的均值；
计算k时刻的自相关协方差矩阵PZZ,k|k-1：
式中，表示量测噪声协方差矩阵；
计算k时刻互相关协方差矩阵PXZ,k|k-1：
所述修正子模块中修正多个AUV的状态估计值和状态误差协方差估计值的具体步骤包括：
通过k时刻实际相对距离量测值Zk以及观测预测值计算残差γk：
计算滤波残差γk的协方差阵
式中，ρ表示遗忘因子；
计算中间过程变量Nk和Mk：
式中，β表示弱化因子；
计算自适应渐消因子λk：
在自相关协方差矩阵PZZ,k|k-1中引入渐消因子λk：
计算k时刻的卡尔曼滤波增益Kk：
计算k时刻状态估计值
计算k时刻的状态误差协方差估计值Pk|k：
4.根据权利要求3所述的一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，其特征在于，所述系统方程构建模块中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：
其中，表示k时刻跟随AUV的n维状态变量，xk、yk、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，Vk表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的nw维过程噪声向量；vk＝[vk]T表示k时刻相对距离测量噪声vk所构成的nv维量测噪声向量；
其中，状态函数表示为：
式中，Δt表示航位推算的系统周期；ωk表示k时刻AUV航行的角速度；
量测函数表示为：
式中，上标M代表领航AUV；上标S代表跟随AUV。
CN202210678112.7A 2022-06-16 2022-06-16 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统 Active CN115077530B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210678112.7A CN115077530B (zh) 2022-06-16 2022-06-16 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210678112.7A CN115077530B (zh) 2022-06-16 2022-06-16 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统

Publications (2)

Publication Number Publication Date
CN115077530A CN115077530A (zh) 2022-09-20
CN115077530B true CN115077530B (zh) 2024-04-23

Family

ID=83253359

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210678112.7A Active CN115077530B (zh) 2022-06-16 2022-06-16 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统

Country Status (1)

Country Link
CN (1) CN115077530B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118243106B (zh) * 2024-04-01 2024-08-27 哈尔滨工业大学（威海） 一种基于平方根分解的扩维容积eckf算法的多auv协同导航的滤波方法、系统及存储介质

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102980579A (zh) * 2012-11-15 2013-03-20 哈尔滨工程大学 一种自主水下航行器自主导航定位方法
CN103792562A (zh) * 2014-02-24 2014-05-14 哈尔滨工程大学 一种基于变换采样点的强跟踪ukf的滤波方法
CN104320108A (zh) * 2014-09-25 2015-01-28 郑州牧业工程高等专科学校 基于ahcif的集中式测量值加权融合方法
CN104655131A (zh) * 2015-02-06 2015-05-27 东南大学 基于istssrckf的惯性导航初始对准方法
CN107707220A (zh) * 2017-08-31 2018-02-16 东南大学 一种应用在gnss/ins中的改进型ckf方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN110823217A (zh) * 2019-11-21 2020-02-21 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN113587926A (zh) * 2021-07-19 2021-11-02 中国科学院微小卫星创新研究院 一种航天器空间自主交会对接相对导航方法
CN114370878A (zh) * 2022-01-04 2022-04-19 江苏科技大学 一种基于stackf的多auv协同定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5095200B2 (ja) * 2006-12-22 2012-12-12 オンセミコンダクター・トレーディング・リミテッド エレクトロルミネッセンス表示装置及び表示パネルの駆動装置

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102980579A (zh) * 2012-11-15 2013-03-20 哈尔滨工程大学 一种自主水下航行器自主导航定位方法
CN103792562A (zh) * 2014-02-24 2014-05-14 哈尔滨工程大学 一种基于变换采样点的强跟踪ukf的滤波方法
CN104320108A (zh) * 2014-09-25 2015-01-28 郑州牧业工程高等专科学校 基于ahcif的集中式测量值加权融合方法
CN104655131A (zh) * 2015-02-06 2015-05-27 东南大学 基于istssrckf的惯性导航初始对准方法
CN107707220A (zh) * 2017-08-31 2018-02-16 东南大学 一种应用在gnss/ins中的改进型ckf方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN110823217A (zh) * 2019-11-21 2020-02-21 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN113587926A (zh) * 2021-07-19 2021-11-02 中国科学院微小卫星创新研究院 一种航天器空间自主交会对接相对导航方法
CN114370878A (zh) * 2022-01-04 2022-04-19 江苏科技大学 一种基于stackf的多auv协同定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
粒子群优化平方根强跟踪CKF及应用;严浙平;邓超;李本银;赵玉飞;;北京理工大学学报;20150815(第08期);全文 *

Also Published As

Publication number Publication date
CN115077530A (zh) 2022-09-20

Similar Documents

Publication Publication Date Title
Avzayesh et al. The smooth variable structure filter: A comprehensive review
CN108427414B (zh) 一种自主水下航行器水平面自适应轨迹跟踪控制方法
CN111752280B (zh) 一种基于有限时间不确定观测器的多无人船编队固定时间控制方法
Gadsden et al. A study of variable structure and sliding mode filters for robust estimation of mechatronic systems
CN115077530B (zh) 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统
CN116185024A (zh) 一种无人艇的艏向抗扰控制方法
CN114879657B (zh) 一种基于随体坐标系的无模型全分布式无人艇协同时变队形控制方法
CN114370878A (zh) 一种基于stackf的多auv协同定位方法
CN117892223A (zh) 一种基于Sage-Husa自适应滤波的交互式多模型水下目标跟踪方法
CN111426322B (zh) 同时估计状态和输入的自适应目标跟踪滤波方法及系统
CN111562737B (zh) 一种模糊逻辑控制的改进agimm跟踪方法
US9207314B1 (en) Rapid determination of model transitions for interacting models with bounded parameters
CN109693774B (zh) 一种水下航行器轨迹的控制方法和系统
KR101097169B1 (ko) 이동체의 위치 예측 시스템 및 위치 예측 방법
CN116680500A (zh) 水下航行器在非高斯噪声干扰下的位置估计方法及系统
Yunhong et al. Maneuvering target tracking based on adaptive turning rate interactive multiple model
CN111998854B (zh) 基于Cholesky分解计算的精确扩展Stirling插值滤波方法
CN110007298A (zh) 一种目标超前预测跟踪方法
CN114445459A (zh) 基于变分贝叶斯理论的连续-离散最大相关熵目标跟踪方法
Zhang et al. An Improved GPSO-PF for Underwater Terrain-Aided Navigation
Hashirao et al. Maneuver target tracking with an acceleration estimator using target past positions
Crouse Strong tracking filters: derivation and improved heuristic
Huang et al. A Novel Bayesian Kalman Filter Under Non-Gaussian Noise for Underwater Integrated Navigation
Hasan et al. Target tracking by adaptive EKF using fast genetic algorithm
Pillai et al. Underwater Target Motion Analysis using Product Beamformer and Bearing Match in Simulated Space

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