CN103616026A - 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 - Google Patents

Abstract

本发明属于组合导航系统技术领域，具体涉及一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法。本发明包括：根据AUV模型推导海流影响下的三自由度操纵模型；建立AUV操纵模型辅助捷联惯导组合导航系统的状态方程；建立AUV操纵模型辅助捷联惯导组合导航系统的量测方程；基于H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计，并用估计得到的位置、速度误差对捷联惯导系统进行校正。本发明在组合导航系统中，降低了AUV导航系统的成本和体积。采用H∞滤波对导航参数进行估计，解决了实际应用中系统模型不准确时引起的Kalman滤波精度降低，甚至滤波发散的问题。

Description

一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法

技术领域

本发明属于组合导航系统技术领域，具体涉及一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法。

背景技术

现阶段，大多数自主式水下航行器(autonomous underwater vehicle，AUV)采用捷联惯性导航系统（Strapdown inertial navigation system，SINS）作为主要导航方式。针对单纯的捷联惯导系统在没有有效辅助的情况下，由于误差积累引起的系统发散问题，多采用多普勒测速仪(Doppler velocity log，DVL)对漂移进行限制。然而，探测方法较为粗糙，水下地形复杂等问题使得某些情况下DVL的探测范围无法到达海底，降低了DVL/SINS组合导航的可行性。在DVL失效，无法得到准确量测结果时，需要一个能有效降低漂移的导航方式对惯导的速度、位置信息进行校正。

同时，在许多实际应用中，不仅对外部干扰信号的统计特性缺乏了解，而且系统模型本身存在一定范围的摄动，使得Kalman滤波器估计精度大大降低，严重时会出现滤波发散，而H∞滤波技术具有很好的鲁棒性，可以保证组合导航精度，提高系统可靠性，防止滤波发散。

发明内容

本发明的目的在于提供一种提高滤波精度，抑制滤波发散的一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法。

本发明的目的是这样实现的：

基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，包括：

（1）根据AUV模型推导海流影响下的三自由度操纵模型；

（2）建立AUV操纵模型辅助捷联惯导组合导航系统的状态方程；

（3）建立AUV操纵模型辅助捷联惯导组合导航系统的量测方程；

（4）基于H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计，并用估计得到的位置、速度误差对捷联惯导系统进行校正。

三自由度操纵模型为：

$\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& J_Z\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{u}}_c+{\stackrel{\·}{u}}_r\\ {\stackrel{\·}{v}}_c+{\stackrel{\·}{v}}_r\\ \stackrel{\·}{r}\end{array}\right]+\left[\begin{array}{ccc}0& 0& -m(v_c+v_r)\\ 0& 0& m(u_c+u_r)\\ m(v_c+v_r)& -m(u_c+u_r)& 0\end{array}\right]\left[\begin{array}{c}{u}_c+u_r\\ v_c+v_r\\ r\end{array}\right]={\mathrm{\τ}}_H+\mathrm{\ω}+\mathrm{\τ}$

其中，在北东坐标系下流速V_c和流向ψ_c，在船体坐标系下流速分量表示为u_c、v_c

u_c=V_ccos(ψ_c-ψ)

v_c=V_csin(ψ_c-ψ)

其中，ψ为AUV艏向，在船体坐标系中，AUV速度分量为u、v，AUV相对于海流的速度分量为u_r、v_r，

u=u_c+u_r

v=v_c+v_r

$\stackrel{\·}{u}={\stackrel{\·}{u}}_c+{\stackrel{\·}{u}}_r$

$\stackrel{\·}{v}={\stackrel{\·}{v}}_c+{\stackrel{\·}{v}}_r$

u_c、u_r、v_c、v_r分别为船体坐标系下x轴的流速与AUV速度，y轴的流速与AUV的速度，r为AUV绕z轴的角速度；m为AUV的质量，J_Z为船体坐标系下z轴的转动惯量；τ_H为AUV所受水动力和力矩；ω为风、浪、流等外部环境的作用力矢量；τ为AUV推进系统的力和力矩的矢量。

AUV操纵模型辅助捷联惯导组合导航系统的状态方程为

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中状态量

包括了三维平台误差角

速度δV_E、δV_N、δV_D和位置δL、δλ、δh，ε_bx、ε_by、ε_bz为陀螺漂移随机常数误差在三轴上的分量，ε_rx、ε_ry、ε_rz为陀螺漂移马尔可夫过程误差在三轴上的分量，▽_x、▽_y、▽_z为加速度计误差在三轴上的分量，

状态转移阵为：

$F={\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0_{18\×4}\\ 0_{4\×18}& F_{\mathrm{Aid}}\end{array}\right]}_{22\×22}$

$F_{\mathrm{SINS}}={\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right]}_{18\×18}$

其中，f_E、f_N、f_D分别为东向加速度、北向加速度、地向加速度，ω_ie为地球自转角速度；R_M为子午圈曲率半径，R_N为卯酉圈曲率半径，

$F_S=\left[\begin{array}{ccc}{R}_b^n& R_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& R_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$F_M=\mathrm{Diag}[\mathrm{0,0,0},-\frac{1}{T_{\mathrm{rx}}},-\frac{1}{T_{\mathrm{ry}}},-\frac{1}{T_{\mathrm{rz}}},-\frac{1}{T_{\mathrm{ax}}},-\frac{1}{T_{\mathrm{ay}}},-\frac{1}{T_{\mathrm{az}}}]$

$F_{\mathrm{Aid}}=\mathrm{Diag}[-\frac{1}{T_{\mathrm{px}}},-\frac{1}{T_{\mathrm{py}}},-\frac{1}{T_{\mathrm{vx}}},-\frac{1}{T_{\mathrm{vy}}}]$

系统噪声矩阵如下：

W=[w_gxw_gyw_gzw_bxw_byw_bzw_axw_ayw_azw_pxw_pyw_vxw_vy]^T

$G=\left[\begin{array}{cccc}{R}_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×4}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×4}\\ 0_{4\×3}& 0_{4\×3}& 0_{4\×3}& I_{4\×4}\end{array}\right]$

AUV操纵模型辅助捷联惯导组合导航系统的量测方程为

$Z_k\left(t\right)=\left[\begin{array}{c}{V}_{\mathrm{SE}}-V_{\mathrm{ME}}\\ V_{\mathrm{SN}}-V_{\mathrm{MN}}\\ L_S-L_M\\ {\mathrm{\λ}}_S-{\mathrm{\λ}}_M\end{array}\right]=H_k\left(t\right)X\left(t\right)+{\mathrm{\ν}}_k$

$H_k\left(t\right)=\left[\begin{array}{cccccc}{0}_{2\×3}& I_{2\×2}& 0_{2\×1}& 0_{2\×2}& 0_{2\×1}& 0_{2\×13}\\ 0_{2\×3}& 0_{2\×2}& 0_{2\×1}& I_{2\×2}& 0_{2\×1}& 0_{2\×13}\end{array}\right]$

其中，V_SE、V_SN分别为捷联惯导解算得到的东向速度和北向速度；V_ME、V_MN分别为模型解算得到的东向速度和北向速度，L_S、λ_S分别为捷联惯导解算得到的纬度和经度在直角坐标系下的投影；L_M、λ_M分别为模型解算得到的纬度和经度在直角坐标下的投影；ν_k是有色噪声。

H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计：

基于微分博弈论的H∞滤波器为：

$P_k^{-1}-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k+H_k^T{R}_k^{-1}{H}_k>0$

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k$

$K_k=P_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}$

${\hat{x}}_{k+1}=F_k{\hat{x}}_k+F_k{K}_k(y_k-H_k{\hat{x}}_k)$

$P_{k+1}=F_k{P}_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{F}_k^T$

$+G_k{Q}_k{G}_k^T$

其中，H∞滤波中系数γ>0，P₀、Q_k、R_k、S_k分别是初始估计误差协方差阵，系统噪声协方差阵，量测噪声协方差阵，状态变量线性组合估计误差协方差阵，P₀、Q_k、R_k、S_k都是对称正定矩阵，w_k和v_k是能量有限噪声，选取：

$L_k={\left[\begin{array}{cccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×13}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×13}\\ 0_{16\×3}& 0_{16\×3}& 0_{16\×3}& 0_{16\×13}\end{array}\right]}^T.$

本发明方法的有益效果在于：

在组合导航系统中，当DVL失效的紧急导航情况下AUV操纵模型辅助模块起到备份导航的作用。而且，利用数学模型的组合导航系统不需要其他外部传感器，因此大大降低了AUV导航系统的成本和体积。采用H∞滤波对导航参数进行估计，解决了实际应用中系统模型不准确时引起的Kalman滤波精度降低，甚至滤波发散的问题。

附图说明

图1是基于H∞滤波的模型辅助组合导航系统示意图；

图2是本发明中模型辅助捷联惯导组合导航方法的程序流图；

图3AUV模型与惯导组合后输出与纯惯导输出的变速运动速度误差比较；

图4AUV模型与惯导组合后输出与纯惯导输出的变速运动位置误差比较；

图5模型不准确时经由Kalman滤波与H∞滤波后的变速运动速度误差比较；

图6模型不准确时经由Kalman滤波与H∞滤波后的变速运动位置误差比较。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。

本发明采用自主式水下潜器操纵模型辅助惯性导航系统，利用海流影响下的AUV操纵模型的输出对惯性导航系统的漂移进行校正，并采用H∞滤波对组合导航系统的状态进行估计，系统框图如图1所示。模型辅助捷联惯导(Model aided SINS,MASINS)可作为DVL/SINS组合方式失效时的备份组合导航系统，同时基于H∞滤波的状态估计方法解决了实际应用中模型不准确、外部干扰信号统计特性不确定引起的Kalman滤波精度降低，甚至滤波发散的问题。因此，本发明具备良好的工程应用前景。

基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，具体包括以下步骤：

步骤一、根据AUV模型假设推导海流影响下的三自由度操纵模型。

步骤二、建立AUV操纵模型辅助捷联惯导组合导航系统的状态方程。

步骤三、建立AUV操纵模型辅助捷联惯导组合导航系统的量测方程。

步骤四、基于H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计，并利用估计得到的位置、速度误差对捷联惯导系统进行校正。

步骤一中的AUV三自由度操纵模型具体描述如下：

在AUV正常操纵时，考虑三自由度模型描述AUV运动，假设忽略来自深度、纵摇和横摇的耦合，即p=q=w=0。运用以下假设推导模型：

设AUV为质量均匀刚体；

运动坐标原点在重心；

AUV全部浸没在无界流体中，即忽略掉流体边界影响；

流体是均匀不可压缩的；

不考虑水下潜器的内部活动机制的影响；

AUV每个作用部分都能够正常工作。

应用动量定理、动量矩定理和以上假设列出并简化AUV三自由度动力学模型，该模型的矢量表示如下：

$M_{\mathrm{RB}}\stackrel{\·}{\mathrm{\ν}}+C_{\mathrm{RB}}\left(\mathrm{\ν}\right)\mathrm{\ν}={\mathrm{\τ}}_{\mathrm{RB}}$

其中，ν=[u,v,r]^T为在船体坐标系下分解的速度矢量，τ_RB=[X,Y,N]^T为合外力和力矩，M_RB为刚体系统惯性矩阵，C_RB(ν)为刚体科里奥利向心力矩阵，表示如下：

$M_{\mathrm{RB}}=\left(\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& J_Z\end{array}\right)$

$C_{\mathrm{RB}}\left(\mathrm{\ν}\right)=\left(\begin{array}{ccc}0& 0& -\mathrm{mv}\\ 0& 0& \mathrm{mu}\\ \mathrm{mv}& -\mathrm{mu}& 0\end{array}\right)$

其中，m为AUV的质量，J_Z为船体坐标系下z轴的转动惯量。

AUV所受的力和力矩的矢量τ_RB是三部分矢量的叠加，可表示为：

τ_RB=τ_H+ω+τ

式中，τ_H为AUV所受水动力和力矩；ω为风、浪、流等外部环境的作用力矢量；τ为AUV推进系统的力和力矩的矢量。

考虑海流的影响，在北东坐标系下流速V_c和流向ψ_c，在船体坐标系下流速分量表示为u_c、v_c，其关系如下：

u_c=V_ccos(ψ_c-ψ)

v_c=V_csin(ψ_c-ψ)

其中，ψ为AUV艏向。

在船体坐标系中，AUV速度分量为u、v，AUV相对于海流的速度分量为u_r、v_r，则有：

u=u_c+u_r

v=v_c+v_r

$\stackrel{\·}{u}={\stackrel{\·}{u}}_c+{\stackrel{\·}{u}}_r$

$\stackrel{\·}{v}={\stackrel{\·}{v}}_c+{\stackrel{\·}{v}}_r$

将上式带入AUV三自由度模型即可得到海流影响下的AUV三自由度模型。利用以上三自由度模型、AUV的合外力及力矩等数据解算AUV运动在船体坐标系下的速度、位置，经过坐标变换得到北东坐标系下的速度、位置信息。

所述步骤二中的AUV操纵模型辅助捷联惯导组合导航系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

状态变量的选取：

其中，状态包括SINS的9个误差参数（三维平台误差角

三维速度误差δV_E、δV_N、δV_D和三维位置误差δL、δλ、δh），三轴陀螺常值漂移ε_b，随机漂移ε_r和三轴加速度计的误差▽_x、▽_y、▽_z，AUV数学模型的二维位置误差δL_M、δλ_M、二维速度误差δV_ME、δV_MN。

利用一阶马尔柯夫过程对陀螺漂移，AUV模型位置、速度误差的有色噪声进行建模。系统噪声矩阵如下：

W=[w_gxw_gyw_gzw_bxw_byw_bzw_axw_ayw_azw_pxw_pyw_vxw_vy]^T

$F={\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0_{18\×4}\\ 0_{4\×18}& F_{\mathrm{Aid}}\end{array}\right]}_{22\×22}$

$G=\left[\begin{array}{cccc}{R}_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×4}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×4}\\ 0_{4\×3}& 0_{4\×3}& 0_{4\×3}& I_{4\×4}\end{array}\right]$

$F_{\mathrm{SINS}}={\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right]}_{18\×18}$

式中，为n系(北东坐标系)和b系(船体坐标系)之间的旋转矩阵，F_N为对应SINS的9个误差参数的系统动态矩阵

F_S在SINS中的表示为：

$F_S=\left[\begin{array}{ccc}{R}_b^n& R_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& R_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

F_M是陀螺和加速度计误差模型状态方程系数矩阵，F_Aid模型辅助导航系统误差状态方程系数矩阵，分别表示为：

$F_M=\mathrm{Diag}[\mathrm{0,0,0},-\frac{1}{T_{\mathrm{rx}}},-\frac{1}{T_{\mathrm{ry}}},-\frac{1}{T_{\mathrm{rz}}},-\frac{1}{T_{\mathrm{ax}}},-\frac{1}{T_{\mathrm{ay}}},-\frac{1}{T_{\mathrm{az}}}]$

$F_{\mathrm{Aid}}=\mathrm{Diag}[-\frac{1}{T_{\mathrm{px}}},-\frac{1}{T_{\mathrm{py}}},-\frac{1}{T_{\mathrm{vx}}},-\frac{1}{T_{\mathrm{vy}}}]$

所述步骤三中的AUV操纵模型辅助捷联惯导组合导航系统的量测方程：

将SINS与AUV操纵模型对速度和位置的量测差值作为滤波器的量测量。

$Z_k\left(t\right)=\left[\begin{array}{c}{V}_{\mathrm{SE}}-V_{\mathrm{ME}}\\ V_{\mathrm{SN}}-V_{\mathrm{MN}}\\ L_S-L_M\\ {\mathrm{\λ}}_S-{\mathrm{\λ}}_M\end{array}\right]=H_k\left(t\right)X\left(t\right)+{\mathrm{\ν}}_k$

其中，V_SE、V_SN分别为捷联惯导解算得到的东向速度和北向速度；V_ME、V_MN分别为模型解算得到的东向速度和北向速度。L_S、λ_S分别为捷联惯导解算得到的纬度和经度在直角坐标系下的投影；L_M、λ_M分别为模型解算得到的纬度和经度在直角坐标下的投影。ν_k是有色噪声。

所述步骤四中基于H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计：

考虑如下线性时变系统：

x_k+1=F_kx_k+G_kw_k

y_k=H_kx_k+v_k

z_k=L_kx_k

其中x_k∈Rⁿ为系统状态向量，y_k∈R^m为系统观测向量，w_k和v_k是能量有限噪声项，z_k是待估计向量。

对于给定的γ>0，P₀、Q_k、R_k、S_k分别是初始估计误差协方差阵，系统噪声协方差阵，量测噪声协方差阵，状态变量线性组合估计误差协方差阵，如果它们都是对称正定矩阵，并且w_k和v_k是能量有限噪声，基于微分博弈论的H∞滤波器为：

$P_k^{-1}-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k+H_k^T{R}_k^{-1}{H}_k>0$

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k$

$K_k=P_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}$

${\hat{x}}_{k+1}=F_k{\hat{x}}_k+F_k{K}_k(y_k-H_k{\hat{x}}_k)$

$P_{k+1}=F_k{P}_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{F}_k^T$

$+G_k{Q}_k{G}_k^T$

本发明利用H∞滤波方法对海流影响下的AUV操纵模型辅助惯性导航系统进行状态估计，将其最优估计反馈到捷联惯性导航系统中，校正捷联惯性导航系统的相应导航参数，实现导航系统的误差补偿，以提高整个组合导航系统的鲁棒性和可靠性。

本发明的流程如图2所示，根据模型假设推导海流影响的AUV三自由度操纵模型；建立包含AUV模型误差、捷联惯导误差的组合导航系统数学模型；采用海流影响下的AUV三自度动力学模型，AUV所受外力和力矩等信息解算速度、位置信息；读取SINS的姿态、速度、位置等信息；设计H∞滤波器，利用组合导航数学模型对导航参数进行估计；利用估计得到的速度、位置信息对SINS进行校正。具体实施步骤如下：

步骤1：根据假设推导海流影响下的AUV三自由度操纵模型：

假设：（1）设AUV为质量均匀刚体；（2）运动坐标原点在重心；（3）AUV全部浸没在无界流体中，即忽略掉流体边界影响；（4）流体是均匀不可压缩的；（5）不考虑水下潜器的内部活动机制的影响；（6）AUV每个作用部分都能够正常工作。

根据以上假设推导出的海流影响的AUV三自由度操纵模型为：

$\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& J_Z\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{u}}_c+{\stackrel{\·}{u}}_r\\ {\stackrel{\·}{v}}_c+{\stackrel{\·}{v}}_r\\ \stackrel{\·}{r}\end{array}\right]+\left[\begin{array}{ccc}0& 0& -m(v_c+v_r)\\ 0& 0& m(u_c+u_r)\\ m(v_c+v_r)& -m(u_c+u_r)& 0\end{array}\right]\left[\begin{array}{c}{u}_c+u_r\\ v_c+v_r\\ r\end{array}\right]={\mathrm{\τ}}_H+\mathrm{\ω}+\mathrm{\τ}$

其中，u_c、u_r、v_c、v_r分别为船体坐标系下x轴的流速与AUV速度，y轴的流速与AUV的速度，r为AUV绕z轴的角速度；m为AUV的质量；J_Z为船体坐标系下z轴的转动惯量；τ_H为AUV所受水动力和力矩；ω为风、浪、流等外部环境的作用力矢量；τ为AUV推进系统的力和力矩的矢量。

步骤2：系统状态方程的建立：

由SINS的误差方程和AUV操纵模型误差方程，建立MASINS组合导航系统状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

状态量包括了三维平台误差角、速度和位置及陀螺误差和加速度计误差，ε_bx、ε_by、ε_bz为陀螺漂移随机常数误差在三轴上的分量，ε_rx、ε_ry、ε_rz为陀螺漂移马尔可夫过程误差在三轴上的分量，▽_x、▽_y、▽_z为加速度计误差在三轴上的分量。

状态转移阵为：

$F={\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0_{18\×4}\\ 0_{4\×18}& F_{\mathrm{Aid}}\end{array}\right]}_{22\×22}$

$F_{\mathrm{SINS}}={\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right]}_{18\×18}$

其中，f_E、f_N、f_D分别为东向加速度、北向加速度、地向加速度，ω_ie为地球自转角速度；R_M为子午圈曲率半径，R_N为卯酉圈曲率半径。

$F_S=\left[\begin{array}{ccc}{R}_b^n& R_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& R_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$F_M=\mathrm{Diag}[\mathrm{0,0,0},-\frac{1}{T_{\mathrm{rx}}},-\frac{1}{T_{\mathrm{ry}}},-\frac{1}{T_{\mathrm{rz}}},-\frac{1}{T_{\mathrm{ax}}},-\frac{1}{T_{\mathrm{ay}}},-\frac{1}{T_{\mathrm{az}}}]$

$F_{\mathrm{Aid}}=\mathrm{Diag}[-\frac{1}{T_{\mathrm{px}}},-\frac{1}{T_{\mathrm{py}}},-\frac{1}{T_{\mathrm{vx}}},-\frac{1}{T_{\mathrm{vy}}}]$

本发明利用一阶马尔柯夫过程对陀螺漂移，加速度计误差，AUV模型位置、速度误差的有色噪声进行建模。陀螺漂移为：ε=ε_b+ε_r+ε_g；其中，ε_b表示随机常数，ε_r表示一阶马尔柯夫过程，ε_g为陀螺漂移的白噪声。三轴陀螺漂移方程为：

${\stackrel{\·}{\mathrm{\ϵ}}}_b=0$

${\stackrel{\·}{\mathrm{\ϵ}}}_r=-\frac{1}{T_r}{\mathrm{\ϵ}}_r+w_g$

其中，T_r=300s表示三轴陀螺漂移的相关时间，w_g表示陀螺漂移马尔可夫过程的白噪声，陀螺常值漂移为0.01°/h，陀螺随机漂移为0.03°/h。

加速度计的误差方程为：

${\stackrel{\·}{\▿}}_a=-\frac{1}{T_a}{\▿}_a+w_a$

其中，T_a=1000s表示加速度计误差的相关时间，w_a表示加速度计误差的白噪声，加速度计零偏为1×10^-4g；

海流影响下的AUV操纵模型的速度、位置误差一阶马尔可夫过程为：

$\mathrm{\δ}{\stackrel{\·}{V}}_M=-\frac{1}{T_v}\mathrm{\δ}{V}_M+w_v$

$\mathrm{\δ}{\stackrel{\·}{P}}_M=-\frac{1}{T_p}\mathrm{\δ}{P}_M+w_p$

其中，T_v=T_p=100s分别为模型速度、位置的误差相关时间，w_v、w_p分别为模型速度、位置误差白噪声。

系统噪声矩阵如下：

W=[w_gxw_gyw_gzw_bxw_byw_bzw_axw_ayw_azw_pxw_pyw_vxw_vy]^T

$G=\left[\begin{array}{cccc}{R}_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×4}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×4}\\ 0_{4\×3}& 0_{4\×3}& 0_{4\×3}& I_{4\×4}\end{array}\right]$

步骤3：系统量测方程的建立：

Z_k(t)是SINS的速度V_SE、V_SN，位置L_S、λ_S与AUV操纵模型解算的速度V_ME、V_MN、位置L_M、λ_M的差，MASINS组合导航系统的量测方程为：

$Z_k\left(t\right)=\left[\begin{array}{c}{V}_{\mathrm{SE}}-V_{\mathrm{ME}}\\ V_{\mathrm{SN}}-V_{\mathrm{MN}}\\ L_S-L_M\\ {\mathrm{\λ}}_S-{\mathrm{\λ}}_M\end{array}\right]=H_k\left(t\right)X\left(t\right)+{\mathrm{\ν}}_k$

$H_k\left(t\right)=\left[\begin{array}{cccccc}{0}_{2\×3}& I_{2\×2}& 0_{2\×1}& 0_{2\×2}& 0_{2\×1}& 0_{2\×13}\\ 0_{2\×3}& 0_{2\×2}& 0_{2\×1}& I_{2\×2}& 0_{2\×1}& 0_{2\×13}\end{array}\right]$

其中，ν_k是有色噪声。

步骤4：基于H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计。基于微分博弈论的H∞滤波器为：

$P_k^{-1}-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k+H_k^T{R}_k^{-1}{H}_k>0$

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k$

$K_k=P_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}$

${\hat{x}}_{k+1}=F_k{\hat{x}}_k+F_k{K}_k(y_k-H_k{\hat{x}}_k)$

$P_{k+1}=F_k{P}_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{F}_k^T$

$+G_k{Q}_k{G}_k^T$

其中，H∞滤波中系数γ=3，P_k、Q_k、R_k、S_k分别是估计误差协方差阵，系统噪声协方差阵，量测噪声协方差阵，状态变量线性组合估计误差协方差阵，它们都是对称正定矩阵，并且w_k和v_k是能量有限噪声。

本发明选取：

$L_k={\left[\begin{array}{cccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×13}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×13}\\ 0_{16\×3}& 0_{16\×3}& 0_{16\×3}& 0_{16\×13}\end{array}\right]}^T$

利用海流影响下AUV操纵辅助SINS组合导航系统输出的速度、位置误差对捷联惯性导航系统的速度、位置进行校正，以满足自主导航精度的要求。

本发明为了进一步所述方法的有效性，在以下初始条件下，变速运动状态的组合导航系统进行仿真，仿真结果如图3-图6所示，并对其进行分析比较。

AUV的初始位置

东经126.6705°，北纬45.7796°；初始速度

AUV以东向1.9m/s，北向2.9m/s的速度运动；在北东坐标系下流速初始速度V_c0为0.1m/s，初始流向ψ_c0为45°。

在变速运动仿真中，东向AUV先以

匀速运动320s，再以每秒增加0.001m/s²的加速度做变加速运动100s，最后以0.01m/s²的加速度做匀加速运动580s，北向AUV先匀速运动320s，再以每秒减少0.001m/s²的加速度做变加速运动100s，最后以-0.01m/s²的加速度做匀减速运动580s，共行驶1000s。

（1）模型准确情况的H∞滤波仿真

海流影响下的AUV模型与惯性导航经由H∞滤波后输出变速运动的速度误差与纯惯导输出变速运动的速度、位置误差的比较，如图3、图4所示。

表1给出了SINS与MASINS组合导航系统的速度、位置误差最大值的比较。其中，δV_E、δV_N分别为东向、北向速度误差，δL、δλ分别为纬度误差、经度误差在直角坐标下的投影。由此可见，AUV做变速运动时，基于H∞滤波的模型辅助捷联惯导组合导航能够大大提高捷联惯导的精度。

图3比较了纯惯性导航系统和AUV模型辅助捷联惯性导航系统东向速度和北向速度的估计误差。图4比较了纯惯性导航系统和AUV模型辅助捷联惯性导航系统纬度和经度的估计误差。可以看出纯惯导在没有其他辅助时，其系统是发散的，通过组合前后误差对比，验证了基于H∞滤波的模型辅助捷联惯导组合方式是可以提高导航系统精度的。

（2）在模型不准确的情况下Kalman滤波和H∞滤波仿真比较

在模型中设置系统噪声协方差阵：Q_k=0.1Q，量测噪声协方差阵：R_k=10R，其中Q,R为真实的噪声协方差阵，即此时系统模型不准确。分别对Kalman滤波和H∞滤波进行仿真。图5和图6分别给出了变速运动在模型不准确的情况下，经由Kalman滤波和H∞滤波后的速度误差和位置误差。

从图5、图6可以看出，在此模型噪声下，经由Kalman滤波后的组合导航系统的速度误差较H∞滤波后的速度误差大，其精度无法保证，而且采用Kalman滤波的组合导航系统的速度、位置甚至呈现了发散的趋势。而H∞滤波在此情况下保持了较高的滤波精度和鲁棒性。

本发明利用AUV操纵模型辅助捷联惯导进行组合导航，基于H∞滤波对组合导航系统进行状态估计。仿真结果表明，在海流影响下的AUV模型辅助捷联惯导组合导航后的导航精度较高。同时，针对系统模型不准确的情况，利用H∞滤波能够避免传统Kalman滤波的精度下降问题和滤波发散的问题，提高了系统鲁棒性，保证了滤波精度。

Claims (5)

1.一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，其特征在于：

（1）根据AUV模型推导海流影响下的三自由度操纵模型；

（2）建立AUV操纵模型辅助捷联惯导组合导航系统的状态方程；

（3）建立AUV操纵模型辅助捷联惯导组合导航系统的量测方程；

（4）基于H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计，并用估计得到的位置、速度误差对捷联惯导系统进行校正。

2.根据权利要求1所述的一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，其特征在于，步骤（1）所述的三自由度操纵模型为：

$\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& J_Z\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{u}}_c+{\stackrel{\·}{u}}_r\\ {\stackrel{\·}{v}}_c+{\stackrel{\·}{v}}_r\\ \stackrel{\·}{r}\end{array}\right]+\left[\begin{array}{ccc}0& 0& -m(v_c+v_r)\\ 0& 0& m(u_c+u_r)\\ m(v_c+v_r)& -m(u_c+u_r)& 0\end{array}\right]\left[\begin{array}{c}{u}_c+u_r\\ v_c+v_r\\ r\end{array}\right]={\mathrm{\τ}}_H+\mathrm{\ω}+\mathrm{\τ}$

其中，在北东坐标系下流速V_c和流向ψ_c，在船体坐标系下流速分量表示为u_c、v_c

u_c=V_ccos(ψ_c-ψ)

v_c=V_csin(ψ_c-ψ)

其中，ψ为AUV艏向，在船体坐标系中，AUV速度分量为u、v，AUV相对于海流的速度分量为u_r、v_r，

u=u_c+u_r

v=v_c+v_r

$\stackrel{\·}{u}={\stackrel{\·}{u}}_c+{\stackrel{\·}{u}}_r$

$\stackrel{\·}{v}={\stackrel{\·}{v}}_c+{\stackrel{\·}{v}}_r$

u_c、u_r、v_c、v_r分别为船体坐标系下x轴的流速与AUV速度，y轴的流速与AUV的速度，r为AUV绕z轴的角速度；m为AUV的质量，J_Z为船体坐标系下z轴的转动惯量；τ_H为AUV所受水动力和力矩；ω为风、浪、流等外部环境的作用力矢量；τ为AUV推进系统的力和力矩的矢量。

3.根据权利要求1所述的一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，其特征在于，步骤（2）所述的AUV操纵模型辅助捷联惯导组合导航系统的状态方程为

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中状态量

包括了三维平台误差角

速度δV_E、δV_N、δV_D和位置δL、δλ、δh，ε_bx、ε_by、ε_bz为陀螺漂移随机常数误差在三轴上的分量，ε_rx、ε_ry、ε_rz为陀螺漂移马尔可夫过程误差在三轴上的分量，▽_x、▽_y、▽_z为加速度计误差在三轴上的分量，

状态转移阵为：

$F={\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0_{18\×4}\\ 0_{4\×18}& F_{\mathrm{Aid}}\end{array}\right]}_{22\×22}$

$F_{\mathrm{SINS}}={\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right]}_{18\×18}$

其中，f_E、f_N、f_D分别为东向加速度、北向加速度、地向加速度，ω_ie为地球自转角速度；R_M为子午圈曲率半径，R_N为卯酉圈曲率半径，

$F_S=\left[\begin{array}{ccc}{R}_b^n& R_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& R_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$F_M=\mathrm{Diag}[\mathrm{0,0,0},-\frac{1}{T_{\mathrm{rx}}},-\frac{1}{T_{\mathrm{ry}}},-\frac{1}{T_{\mathrm{rz}}},-\frac{1}{T_{\mathrm{ax}}},-\frac{1}{T_{\mathrm{ay}}},-\frac{1}{T_{\mathrm{az}}}]$

$F_{\mathrm{Aid}}=\mathrm{Diag}[-\frac{1}{T_{\mathrm{px}}},-\frac{1}{T_{\mathrm{py}}},-\frac{1}{T_{\mathrm{vx}}},-\frac{1}{T_{\mathrm{vy}}}]$

系统噪声矩阵如下：

W=[w_gxw_gyw_gzw_bxw_byw_bzw_axw_ayw_azw_pxw_pyw_vxw_vy]^T

$G=\left[\begin{array}{cccc}{R}_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}& 0_{9\×4}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×4}\\ 0_{4\×3}& 0_{4\×3}& 0_{4\×3}& I_{4\×4}\end{array}\right]$

4.根据权利要求1所述的一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，其特征在于，步骤（3）所述的AUV操纵模型辅助捷联惯导组合导航系统的量测方程为

$Z_k\left(t\right)=\left[\begin{array}{c}{V}_{\mathrm{SE}}-V_{\mathrm{ME}}\\ V_{\mathrm{SN}}-V_{\mathrm{MN}}\\ L_S-L_M\\ {\mathrm{\λ}}_S-{\mathrm{\λ}}_M\end{array}\right]=H_k\left(t\right)X\left(t\right)+{\mathrm{\ν}}_k$

$H_k\left(t\right)=\left[\begin{array}{cccccc}{0}_{2\×3}& I_{2\×2}& 0_{2\×1}& 0_{2\×2}& 0_{2\×1}& 0_{2\×13}\\ 0_{2\×3}& 0_{2\×2}& 0_{2\×1}& I_{2\×2}& 0_{2\×1}& 0_{2\×13}\end{array}\right]$

其中，V_SE、V_SN分别为捷联惯导解算得到的东向速度和北向速度；V_ME、V_MN分别为模型解算得到的东向速度和北向速度，L_S、λ_S分别为捷联惯导解算得到的纬度和经度在直角坐标系下的投影；L_M、λ_M分别为模型解算得到的纬度和经度在直角坐标下的投影；ν_k是有色噪声。

5.根据权利要求1所述的一种基于H∞滤波的AUV操纵模型辅助捷联惯导组合导航方法，其特征在于，步骤（4）中采用如下方法H∞滤波对AUV操纵模型辅助捷联惯导组合导航系统进行状态估计：

基于微分博弈论的H∞滤波器为：

$P_k^{-1}-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k+H_k^T{R}_k^{-1}{H}_k>0$

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k$

$K_k=P_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}$

${\hat{x}}_{k+1}=F_k{\hat{x}}_k+F_k{K}_k(y_k-H_k{\hat{x}}_k)$

$P_{k+1}=F_k{P}_k{[I-{\mathrm{\γ}}^{-2}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{F}_k^T$

$+G_k{Q}_k{G}_k^T$

其中，H∞滤波中系数γ>0，P₀、Q_k、R_k、S_k分别是初始估计误差协方差阵，系统噪声协方差阵，量测噪声协方差阵，状态变量线性组合估计误差协方差阵，P₀、Q_k、R_k、S_k都是对称正定矩阵，w_k和v_k是能量有限噪声，选取：

$L_k={\left[\begin{array}{cccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×13}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×13}\\ 0_{16\×3}& 0_{16\×3}& 0_{16\×3}& 0_{16\×13}\end{array}\right]}^T.$

Images