一种基于强跟踪扩维ECKF算法的多AUV协同导航方法及系统技术方案

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

【技术实现步骤摘要】
一种基于强跟踪扩维ECKF算法的多AUV协同导航方法及系统


[0001]本专利技术涉及多自主水下航行器(Autonomous Underwater Vehicle,AUV)协同导航
，具体涉及一种基于强跟踪扩维嵌入式容积卡尔曼滤波的多AUV协同导航方法及系统。

技术介绍

[0002]多AUV系统中，跟随AUV结合相对观测获取的导航信息，能够通过协同导航算法对自身位置进行滤波估计，以此降低系统整体的导航误差。多AUV协同导航算法根据类型主要分为三种，分别为：基于优化理论的协同导航算法、基于图论的协同导航算法、基于贝叶斯估计的协同导航算法。
[0003]多AUV协同导航的过程中，由于系统初始状态的先验信息或是噪声统计特性不准确，在复杂恶劣的海洋环境中系统状态可能发生突变，从而引起协同导航算法的状态估计值误差增大，甚至有可能导致滤波发散。基于贝叶斯估计的扩维嵌入式容积卡尔曼滤波(Embedded Cubature Kalman Filter,ECKF)算法通过嵌入式容积准则增加了容积点的选取并改变其权值，从而抑制传统容积卡尔曼滤波随着状态空间维数增加而增大的定位估计误差；但其仍旧对系统状态突变以及初始状态的先验信息比较敏感，难以保证异常测量或是扰动情况下的稳定性。

技术实现思路

[0004]鉴于以上问题，本专利技术提出一种基于强跟踪扩维嵌入式容积卡尔曼滤波(Strong Tracking Filter ECKF,STF
‑
ECKF)算法的多AUV协同导航方法及系统，用以解决主从式多AUV协同导航系统先验信息不准确或系统状态发生突变时定位失效的问题。
[0005]根据本专利技术的一方面，提供一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，该方法包括以下步骤：
[0006]步骤一、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；
[0007]步骤二、采用扩维ECKF算法对多个AUV进行状态预测和量测预测；
[0008]步骤三、在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。
[0009]进一步地，步骤一中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：
[0010][0011]其中，表示k时刻跟随AUV的n维状态变量，x
k
、y
k
、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，V
k
表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向
速度噪声和航向角传感器噪声组成的n
w
维过程噪声向量；v
k
＝[v
k
]T
表示k时刻相对距离测量噪声v
k
所构成的n
v
维量测噪声向量；
[0012]其中，状态函数表示为：
[0013][0014]式中，Δt表示航位推算的系统周期；w
k
表示k时刻AUV航行的角速度；
[0015]量测函数表示为：
[0016][0017]式中，上标M代表领航AUV；上标S代表跟随AUV。
[0018]进一步地，步骤二中状态预测的具体过程包括：
[0019]假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差P
k
‑
1|k
‑1已知，对P
k
‑
1|k
‑1进行Cholesky分解得到过程变量C
k
‑
1|k
‑1：
[0020][0021]使用n
w
维过程噪声对状态变量进行扩维，得到过程变量以及
[0022][0023][0024]计算容积点X
i,k
‑
1|k
‑1：
[0025][0026]其中，i＝1,2,
…
,N,ξ
i
表示容积点集；Q
k
‑1表示k
‑
1时刻的过程噪声协方差矩阵；
[0027]根据ECKF算法中的嵌入式容积准则，替换容积点集ξ
i
以及权值
[0028][0029][0030]式中，[0]i
为维数为n的零向量；[δ]＝[s1σ2,s1σ2,
…
,s1σ2]T
，σ＝1且s1＝
±
1；
[0031]根据系统函数传播容积点X
i,k
‑
1|k
‑1的状态变量分量以及过程噪声分量
得到传播后的容积点
[0032][0033]根据系统函数传播后的容积点计算k时刻的状态预测值
[0034][0035]计算k时刻的状态误差协方差预测值P
k|k
‑1：
[0036][0037]进一步地，步骤二中量测预测的具体过程包括：
[0038]通过量测函数传播的状态变量分量
[0039][0040]根据量测函数传播后的容积点计算k时刻的观测预测值
[0041][0042]式中，r
k
＝E(v
k
)为v
k
的均值；
[0043]计算k时刻的自相关协方差矩阵P
ZZ,k|k
‑1：
[0044][0045]式中，表示量测噪声协方差矩阵；
[0046]计算k时刻互相关协方差矩阵P
XZ,k|k
‑1：
[0047][0048]进一步地，步骤三的具体步骤包括：
[0049]通过k时刻实际相对距离量测值Z
k
以及观测预测值计算残差γ
k
：
[0050][0051]计算滤波残差γ
k
的协方差阵
[0052][0053]式中，ρ表示遗忘因子；
[0054]计算中间过程变量N
k
和M
k
：
[0055][0056][0057]式中，β表示弱化因子；
[0058]计算自适应渐消因子λ
k
：
[0059][0060]在自相关协方差矩阵P
ZZ,k|k
‑1中引入渐消因子λ
k
：
[0061][0062]计算k时刻的卡尔曼滤波增益K
k
：
[0063][0064]计算k时刻状态估计值
[0065][0066]计算k时刻的状态误差协方差估计值P
k|k
：
[0067][0068]根据本专利技术的另一方面，提供一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，该系统包括：
[0069]系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测本文档来自技高网...

【技术保护点】

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

【专利技术属性】
技术研发人员：罗清华，邵阳，焉晓贞，王晨旭，周志权，
申请(专利权)人：哈尔滨工业大学威海，
类型：发明
国别省市：