基于SINS/DVL/GPS的AUV组合导航系统

摘要

本发明公开了一种基于SINS/DVL/GPS的AUV组合导航系统，包括SINS系统、GPS接收机、DVL计程仪和数据融合中心，所述SINS系统、GPS接收机、DVL计程仪和数据融合中心均安装在AUV上；当AUV位于水面时，所述优化滤波模块结合SINS系统导航信息，对应于SINS系统的伪距、伪距率，以及GPS接收机输出的可用星历数据进行滤波融合计算，得到校正信息；当AUV位于水下时，所述优化滤波模块结合SINS系统输出的导航信息，以及DVL计程仪输出的三维航速信息进行滤波融合计算，得到校正信息。本发明提高了系统导航定位精度及鲁棒性，实现了不间断的水下及水面载体高精度导航跟踪功能。

说明书

技术领域

本发明涉及一种基于SINS/DVL/GPS的AUV组合导航系统，可用于水下潜器及水 面航行器的导航及跟踪定位。

背景技术

AUV（Autonomous Underwater Vehicle自主水下航行器）是探索和开发利用海洋环 境资源的重要工具，也在海洋军事应用方面发挥着重要作用。因其远程性、隐蔽性和自 主性，使得AUV的导航问题仍是目前面临的主要技术挑战之一。由于水下环境复杂， 常采用组合导航方式对AUV进行导航定位，而组合导航系统滤波器设计是保证导航精 度的关键。目前常规AUV的导航定位精度可达到<0.8nmile/h左右，在一定程度上满足 了AUV导航定位的精度要求。

SINS（Strapdown Inertial Navigation Systems捷联惯性导航系统）系统具有自主导航 能力，不受环境、载体机动及无线电干扰的影响，能够连续的提供载体位置、速度和姿 态等导航定位信息，其数据更新频率快，且在短时间内具有较高的相对精度。但是，随 着系统工作时间的延长，SINS系统的导航误差会随之积累增长，此时就需要利用外部 传感器的观测信息通过滤波算法来修正补偿SINS系统，以抑制其随时间积累的误差。

GPS（Global Positioning System全球定位系统）是具有海、陆、空全方位实时三维 导航与定位能力的全球卫星导航定位系统。GPS系统通过接收分布在空中的24颗导航 卫星发送的无线信号来计算导航定位信息，具有全球性、全天候、实时性等优点。但是 电磁波信号在水下衰减迅速，达到一定深度时，GPS信号将无法捕获，不能提供导航和 定位功能，不能满足某些需要实时定位的水下航行器的功能需求。

DVL（Doppler Velocity Log多普勒计程仪）是广泛应用于水下及水面航行器组合系 统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。DVL 系统通过安装于AUV底部的四个换能器信息求得声波频移，进而计算得到载体在载体 系的三维速度信息，具有精度高、可靠性、实时性等优点。

由于水下环境复杂、航位推算系统位置误差随时间累积以及AUV的长航时、隐蔽 性要求，基于SINS/DVL的航位推算组合系统的常规滤波算法精度难以达到高精度导航 定位功能需求；在提高滤波算法精度的同时，每经过一段时间间隔后，AUV需要上浮 至水面，此时通过引入GPS信号对其进行位置校正，使其能够沿着设定的轨迹航行。

在系统模型和噪声统计特性精确已知时，Kalman滤波是最优估计，精度高，但是 在实际应用中，只能建立近似的数学模型并且很难获得精确的噪声统计特性，若系统模 型精确性差或者系统噪声为非高斯白噪声时，Kalman滤波精度下降甚至出现滤波发散， 难以实现系统高精度导航跟踪定位功能。

基于Krein空间的H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简单、 鲁棒性好的优点。但是H∞滤波算法是以牺牲一定精度为代价获得系统鲁棒性的，精度 难以保证。

综合考虑Kalman滤波算法的高精度和H∞滤波算法的鲁棒性，将Kalman滤波算法 与H∞滤波算法相结合，得到一种高性能的优化滤波算法；该滤波算法具有较强的关于 模型不确定性的鲁棒性以及对系统状态的较高精度跟踪能力；运用所提出的优化滤波技 术将SINS导航信息、GPS接收机星历数据、DVL计程仪的三维速度信息进行数据融合 计算，在保证导航精度的基础上可以显著提高系统鲁棒性能，进一步提升了捷联惯导系 统导航定位性能。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种基于SINS/DVL/GPS 的AUV组合导航系统，将DVL测速信息及GPS位置信息引入AUV组合系统，以辅助 SINS进行导航及跟踪定位；该系统整合了多个子导航系统信息，克服了AUV导航系统 易受环境影响、SINS系统的导航定位误差随时间延续不断累积达不到精度要求以及滤 波模型不准确或者系统受到有色噪声干扰而导致的Kalman滤波精度下降的缺陷。

技术方案：为实现上述目的，本发明采用的技术方案为：

基于SINS/DVL/GPS的AUV组合导航系统，包括SINS系统、GPS接收机、DVL 计程仪和数据融合中心，所述SINS系统、GPS接收机、DVL计程仪和数据融合中心均 安装在AUV上；

所述SINS系统包括IMU元器件及IMU处理单元，所述IMU传感器用于检测AUV 的惯性数据，并输出至IMU处理单元，所述IMU处理单元对惯性数据进行导航积分计 算，获得AUV在导航坐标系下的导航信息；

所述GPS接收机用于接收并输出AUV的导航测量信息、可用星历数据以及卫星健 康状态等数据；

所述DVL计程仪用于测量AUV相对于海底的纵向及横向三维航速信息；

所述数据融合中心用于信息融合并输出校正后的组合导航信息，包括传感器信息转 换模块和优化滤波模块；所述传感器信息转换模块结合GPS接收机输出的可用星历数 据和SINS系统输出的导航信息，计算得到对应于SINS系统的伪距、伪距率；所述优 化滤波模块结合SINS系统的导航信息，对应于SINS系统的伪距、伪距率，以及GPS 接收机输出的可用星历数据和DVL计程仪输出的三维航速信息进行滤波融合计算，得 到校正信息；通过校正信息对SINS系统输出的导航信息进行校正，最终得到校正后的 组合导航信息；

当AUV位于水面时，所述优化滤波模块结合SINS系统导航信息，对应于SINS系 统的伪距、伪距率，以及GPS接收机输出的可用星历数据进行滤波融合计算，得到校 正信息；

当AUV位于水下时，所述优化滤波模块结合SINS系统输出的导航信息，以及DVL 计程仪输出的三维航速信息进行滤波融合计算，得到校正信息。

优选的，所述IMU中的惯性传感器包括三轴正交安装的陀螺仪及三轴正交安装的 加速度计，所述陀螺仪用于提供三轴角速度测量值，所述加速度计用于提供三轴加速度 测量值。

优选的，所述陀螺仪为光纤陀螺仪，所述加速度计为石英挠性加速度计。

优选的，DVL计程仪为基于多普勒效应的四波束绝对速度测量装置。

优选的，DVL计程仪包括换能器、收发器和接口单元，所述收发器包括发射系统、 接收系统和计算补偿电路；所述换能器为声电换能器，安装于AUV底端距艏部约1/3 处，数目为四，依艏艉线呈对称安装，用于将发射系统输出的电信号转换为声信号后发 出，并将回波信号转换为电信号后输出至接收系统；所述发射系统用于产生具有一定频 率、一定脉冲宽度和功率的电振荡信号并发送给换能器，所述接收系统用于将换能器输 出的电信号进行放大处理后求得多普勒频移并转换为三维航速模拟信号送给接口单元， 接口单元将发射系统输出的三维航速模拟信号转换为三维航速，并以数字方式输出。

优选的，所述优化滤波模块综合了Kalman滤波器和H∞滤波器，对于输入的观测 值Z，所述优化滤波模块的滤波过程包括如下步骤：

（1）计算Kalman滤波器在t_k时刻状态估计：

（2）计算H∞滤波器在t_k时刻状态估计：

（3）计算Kalman滤波器的性能指标M_k；

（4）计算自适应调节权值：$d_k=\left(\begin{array}{cc}1,& M_k\le M_2\\ a^{-\frac{M_k}{b}},& M_2<M_k\le M_{\infty };\\ 0,& M_k>M_{\infty }\end{array}\right)$

（5）计算系统综合状态估计：

其中，M₂为Kalman滤波方法高精度运行的上临界，M_∞为Kalman滤波方法低精 度运行的下临界；在系统模型和噪声统计特性精确已知时，Kalman滤波是最优估计， 精度高，Kalmanl滤波器的性能指标M_k为接近于1的数值；若系统模型精确性差或者系 统噪声为非高斯白噪声时，Kalman滤波精度下降甚至出现滤波发散，Kalmanl滤波器的 性能指标M_k大于1，此时，应该降低对Kalman滤波器估计值的信任度；d_k∈[0,1]表示 对Kalman滤波器的信任度，a和b为设计参数，1<a≤5，1<b≤10；a和b的取值大 小决定了自适应调节权值d_k对Kalman滤波器的性能指标变化的灵敏度，显示了优化滤 波模块的精度及鲁棒性。

有益效果：本发明提供的基于SINS/DVL/GPS的AUV组合导航系统，将DVL测 速信息及GPS位置信息引入AUV组合系统，以辅助SINS进行导航及跟踪定位；该系 统整合了多个子导航信息，克服了AUV导航系统易受环境影响、SINS系统的导航定位 误差随时间延续不断累积达不到精度要求的缺陷；同时综合考虑了Kalman滤波算法的 高精度和H∞滤波算法的鲁棒性，将Kalman滤波和H∞滤波相结合，具有较强的关于模 型不确定性的鲁棒性以及对系统状态的较高精度跟踪能力，克服了滤波模型不准确或者 系统受到有色噪声干扰而导致的Kalman滤波精度下降的缺陷。

附图说明

图1为本发明系统组成的主框图；

图2为系统组合模型框图；

图3为SINS系统结构原理图；

图4为位置精度对比效果图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

设计原理说明

AUV是探索和开发利用海洋环境资源的重要工具，也在海洋军事应用方面发挥着 重要作用。因其远程性、隐蔽性和复杂的水下环境，常采用组合导航方式对AUV进行 导航定位。SINS系统具有自主导航能力，不受环境、载体机动及无线电干扰的影响， 能够连续的提供载体位置、速度和姿态等导航定位信息，其数据更新频率快，且在短时 间内具有较高的相对精度。但是随着系统工作时间的延长，SINS系统的导航误差会随 之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿SINS系 统，以抑制其随时间积累的误差。DVL是广泛应用于水下及水面航行器组合系统的速 度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。具有精度高、 可靠性、实时性等优点。由于水下环境复杂、航位推算系统位置误差随时间累积以及 AUV的长航时、隐蔽性要求，基于SINS/DVL的航位推算组合系统的常规滤波算法精 度难以达到高精度导航定位功能需求；在提高滤波算法精度的同时，每经过一段时间间 隔后，AUV上浮至水面，通过引入GPS信号对其进行位置校正，使其能够沿着设定的 轨迹航行。

在系统模型和噪声统计特性精确已知时，Kalman滤波是最优估计，精度高，若系 统模型精确性差或者系统噪声为非高斯白噪声时，Kalman滤波精度下降甚至出现滤波 发散。基于Krein空间的H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简 单、鲁棒性好的优点。但是H∞滤波算法是以牺牲一定精度为代价获得系统鲁棒性的， 精度难以保证。综合考虑Kalman滤波算法的高精度和H∞滤波算法的鲁棒性，将Kalman 滤波算法与H∞滤波算法相结合，得到一种高性能的优化滤波算法；该滤波算法具有较 强的关于模型不确定性的鲁棒性以及对系统状态的较高精度跟踪能力；运用所提出的优 化滤波技术将SINS导航信息、GPS接收机星历数据、DVL计程仪的三维速度信息进行 数据融合计算，在保证导航精度的基础上可以显著提高系统鲁棒性能，进一步提升了捷 联惯导系统导航定位性能。

系统设计说明

如图1至图4所示为一种基于SINS/DVL/GPS的AUV组合导航系统，其特征在于： 包括SINS系统、GPS接收机、DVL计程仪和数据融合中心，所述SINS系统、GPS接 收机、DVL计程仪和数据融合中心均安装在AUV上；下面就各个部分加以说明。

所述SINS系统包括IMU元器件及IMU处理单元，所述IMU传感器用于检测AUV 的惯性数据，并输出至IMU处理单元，所述IMU处理单元对惯性数据进行导航积分计 算，获得AUV在导航坐标系下的导航信息。其中，所述IMU中的惯性传感器包括三轴 正交安装的陀螺仪及三轴正交安装的加速度计，所述陀螺仪用于提供三轴角速度测量 值，所述加速度计用于提供三轴加速度测量值。具体设计，陀螺仪为光纤陀螺仪，加速 度计为石英挠性加速度计。

所述GPS接收机用于接收并输出AUV的导航测量信息、可用星历数据以及卫星健 康状态等数据。

所述DVL计程仪用于测量AUV相对于海底的纵向及横向三维航速信息；具体设计， DVL计程仪为基于多普勒效应的四波束绝对速度测量装置，包括换能器、收发器和接 口单元，所述收发器包括发射系统、接收系统和计算补偿电路；所述换能器为声电换能 器，安装于AUV底端距艏部约1/3处，数目为四，依艏艉线呈对称安装，用于将发射 系统输出的电信号转换为声信号后发出，并将回波信号转换为电信号后输出至接收系 统；所述发射系统用于产生具有一定频率、一定脉冲宽度和功率的电振荡信号并发送给 换能器，所述接收系统用于将换能器输出的电信号进行放大处理后求得多普勒频移并转 换为三维航速模拟信号送给接口单元，接口单元将发射系统输出的三维航速模拟信号转 换为三维航速，并以数字方式输出。

所述数据融合中心用于信息融合并输出校正后的组合导航信息，包括传感器信息转 换模块和优化滤波模块；所述传感器信息转换模块结合GPS接收机输出的可用星历数 据和SINS系统输出的导航信息，计算得到对应于SINS系统的伪距、伪距率；所述优 化滤波模块结合SINS系统导航信息，对应于SINS系统的伪距、伪距率，以及GPS接 收机输出的可用星历数据和DVL计程仪输出的三维航速信息进行滤波融合计算，得到 校正信息；通过校正信息对SINS系统输出的导航信息进行校正，最终得到校正后的组 合导航信息；

当AUV位于水面时，所述优化滤波模块结合SINS系统导航信息，对应于SINS系 统的伪距、伪距率，以及GPS接收机输出的可用星历数据进行滤波融合计算，得到校 正信息；

当AUV位于水下时，所述优化滤波模块结合SINS系统输出的导航信息，以及DVL 计程仪输出的三维航速信息进行滤波融合计算，得到校正信息。

优选的，所述优化滤波模块综合了Kalman滤波器和H∞滤波器，对于输入的观测 值Z，所述优化滤波模块的滤波过程包括如下步骤：

（1）计算Kalman滤波器在t_k时刻状态估计：

（2）计算H∞滤波器在t_k时刻状态估计：

（3）计算Kalman滤波器的性能指标M_k：

（4）计算自适应调节权值：$d_k=\left(\begin{array}{cc}1,& M_k\le M_2\\ a^{-\frac{M_k}{b}},& M_2<M_k\le M_{\infty };\\ 0,& M_k>M_{\infty }\end{array}\right)$

（5）计算系统综合状态估计：

其中，为t_k时刻的新息序列计算公式，为系统状 态一步预测估计值，Φ_k,k-1为t_k-1时刻至t_k时刻的状态一步转移矩阵；tr(A)表示的是对 任意矩阵A的求迹运算，Z_k为t_k时刻的量测向量，H_k为量测矩阵，R_k为量测噪声序列 的方差阵，P_k,k-1为t_k时刻的系统一步预测误差方差阵，M₂为Kalman滤波方法高精度 运行的上临界，M_∞为Kalman滤波方法低精度运行的下临界，d_k∈[0,1]表示对Kalman 滤波器的信任度，a和b为设计参数，1<a≤5，1<b≤10；a和b的取值大小决定了自 适应调节权值d_k对Kalman滤波器的性能指标变化的灵敏度，显示了优化滤波模块的精 度及鲁棒性。

设计推导说明

（1）地球几何模型与物理参数

①地球形状描述

赤道半径长半轴R_e=6378137.0m，椭球扁率e=1/298.257223563；子午面曲率半径R_N和卯酉圈曲率半径R_E可以由下列方程求得：

$\left(\begin{array}{c}{R}_N=\frac{R_e{(1-e)}^2}{{[{(1-e)}^2{\sin }^{2}L+{\cos }^{2}L]}^{\frac{3}{2}}}\\ R_E=\frac{R_e}{{[{(1-e)}^2{\sin }^{2}L+{\cos }^{2}L]}^{\frac{1}{2}}}\end{array}\right)$

②地球自转角速率ω_ie=15.0411°/h=7.29211585×10^-5rad/s

③地球重力加速度$g=9.7803267714\times \frac{1+0.00193185138639{\sin }^{2}L}{\sqrt{1-0.00669437999013{\sin }^{2}L}}{(1+\frac{h}{R_e})}^{-2}$

其中，L表示当地纬度，h表示高度。

（2）坐标系定义

①地心惯性坐标系：i系

用ox_iy_iz_i表示，原点位于地球中心，ox_i与oy_i轴在地球赤道平面内，ox_i轴指向春分 点，oz_i轴和地球自转轴重合，oy_i与ox_i、oz_i构成右手直角坐标系，三个坐标轴在惯性 空间的指向固定。IMU的输出以i系为参考基准。

②地球坐标系：e系

用ox_ey_ez_e表示，原点位于地球中心，oz_e轴和地球自转轴重合，ox_e轴沿格林尼治子 午面和地球赤道平面的交线，oy_e轴在赤道平面内，ox_e、oy_e、oz_e三轴构成右手直角坐 标系。地球坐标系与地球固连，e系相对i系转动的角速率即为地球自转角速度ω_ie。

③地理坐标系：g系

用ox_gy_gz_g表示，原点位于载体重心，一般采用东北天坐标系作为地理坐标系，即ox_g轴指向东（E），oy_g轴指向北（N），oz_g轴指向天（U）。

④导航坐标系：n系

用ox_ny_nz_n表示，采用东北天地理坐标系（ENU）作为导航坐标系。

⑤载体坐标系：b系

用ox_by_bz_b表示，原点一般取IMU几何中心，ox_b轴沿载体横轴向右，oy_b轴沿载体 纵轴向前，oz_b轴沿载体立轴向上。

（3）姿态角与姿态矩阵之间的转换关系

①姿态角定义

航向角：载体纵轴oy_b在水平面上的投影与地理子午线北向之间的夹角，称为航向 角，计为Ψ。航向角数值是以地理北向为起点顺时针方向计算的，其定义域为0～360°。

俯仰角：载体绕横轴ox_b转动时，载体纵轴和水平面的夹角，称为俯仰角，记为θ。 俯仰角从水平面算起，向上为正，向下为负，其定义域为-90°～+90°。

横摇角：载体纵向对称平面与纵向铅垂平面之间的夹角，称为横摇角，记为γ。横 摇角从铅垂平面算起，右倾为正，左倾为负，其定义域为-180°～+180°。

②姿态矩阵

实现载体坐标系b系到地理坐标系（导航系n系）的变换可按下列顺序经三次转动 得到，具体顺序为：绕-oz_g轴转Ψ角，绕ox₁轴转θ角，再绕oy₂轴转γ角；得到地理坐 标系到载体坐标系的转换矩阵为：

$C_g^b=\left(\begin{array}{ccc}\cos \gamma \cos \psi +\sin \gamma \sin \psi \sin \theta & -\cos \gamma \sin \psi +\sin \gamma \cos \psi \sin \theta & -\sin \gamma \cos \theta \\ \sin \psi \cos \theta & \cos \psi \cos \theta & \sin \theta \\ \sin \gamma \cos \psi -\cos \gamma \sin \psi \sin \theta & -\sin \gamma \sin \psi -\cos \gamma \cos \psi \sin \theta & \cos \gamma \cos \theta \end{array}\right)$

以地理坐标系作为导航坐标系，则有捷联惯性导航系统姿态矩阵为：

$C_b^n={\left(C_g^b\right)}^T={\left[T_{\mathrm{ij}}\right]}_{3\times 3}$

③姿态矩阵至主值区间姿态角的转换关系为：

（4）SINS系统基本原理

①SINS系统微分方程

姿态矩阵微分方程为：

${\stackrel{·}{C}}_b^n=C_b^n({\omega }_{\mathrm{nb}}^b\times )$

其中，表示的是矢量的叉乘形式，为载体角速度，由 陀螺仪测得，${\omega }_{\mathrm{in}}^n={\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n.$

载体在导航系内的比力方程为：

${\stackrel{·}{V}}^n=f^n-({2\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)\times V^n+g^n$

其中，f^b为加速度计测得的比力，$V^n={\left(\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right)}^T$为载体对地速 度在导航系的大小，gⁿ＝[0 0 -g]^T。

位置更新微分方程：在以地理坐标系作为导航坐标系的系统中，纬度L、经度λ和 高度h的微分方程为：

$\stackrel{·}{L}=\frac{V_N^n}{R_N+h},$$\stackrel{·}{\lambda }=\frac{V_E^n}{(R_E+h)\cos L},$$\stackrel{·}{h}=V_U^n$

②SINS系统误差模型

陀螺仪误差模型：ε_i=ε_bi+ε_wi(i=x,y,z)，ε_bi为随机常数，ε_wi为白噪声。

加速度计误差模型：▽_i＝▽_bi+▽_wi(i＝x，y，z)，▽_bi为随机常数，▽_wi为白噪声。

姿态误差方程为：${\stackrel{·}{\phi }}^n=-{\omega }_{\mathrm{in}}^n\times {\phi }^n+\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n-C_b^n{ϵ}^b,$φⁿ=[φ_x φ_y φ_z]^T为姿态误差。

速度误差方程为：$\delta {\stackrel{·}{V}}^n=-\phi \times f^n-(2\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n)\times V^n-({2\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)\times {\mathrm{\delta V}}^n+C_b^n{▿}^b.$

位置误差方程为：

$\delta \stackrel{·}{L}=\frac{{\mathrm{\delta V}}_n^n}{R_N+h}-\frac{V_N^n}{{(R_N+h)}^2}\mathrm{\delta h}$

$\delta \stackrel{·}{\lambda }=\frac{\sec L}{R_E+h}\delta V_e^n+\frac{V_E^n\sec L\tan L}{R_E+h}\mathrm{\delta L}-\frac{V_E^n\sec L}{{(R_E+h)}^2}\mathrm{\delta h}$

$\delta \stackrel{·}{h}=\delta V_u^n$

（5）滤波算法

①Kalman滤波算法

考虑如下线性时变离散系统模型：

$\left(\begin{array}{c}{X}_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right)$

Kalman滤波假设系统的过程噪声序列W_k和量测噪声序列V_k为零均值的高斯白噪声 序列。连续时间系统经过离散化处理后，若系统模型和噪声统计特性精确已知时，且卡 尔曼滤波器稳定，则根据k时刻量测向量Z_k，的（线性最小方差）最优估计递推过 程可简述为如下形式：

$\left(\begin{array}{c}{\hat{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}\end{array}\right)$

上式即为离散型卡尔曼滤波基本方程。只要给定系统初始状态和初始估计误差 方差阵P₀，根据k时刻的量测Z_k就可以递推算到k时刻的最优状态估计(k=1，2， 3，…)。

式中各项定义如下：

X_k为t_k时刻的待估计系统状态向量；

Φ_k,k-1为t_k-1时刻至t_k时刻的状态一步转移矩阵；

Γ_k-1为系统过程噪声驱动矩阵；

Q_k为系统噪声序列的方差阵，为非负定阵；

R_k为量测噪声序列的方差阵，为正定阵；

Z_k为量测状态向量；

H_k为量测矩阵；

W_k-1为系统随机过程噪声序列；

V_k为系统随机量测噪声序列；

X_k,k-1为系统状态一步预测；

P_k为估计误差方差阵；

K_k为滤波增益矩阵。

②H∞滤波算法介绍

考虑如下基于Krein空间离散状态空间系统模型：

$\left(\begin{array}{c}{X}_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k,k-1}{W}_{k-1}\\ \left(\begin{array}{c}{Z}_k\\ y_k\end{array}\right)=\left(\begin{array}{c}{H}_k\\ L_k\end{array}\right)X_k+V_k\end{array}\right)$

y_k是给定的系统状态X_k的线性组合观测量。其中，W_k和V_k为l₂能量有界的噪声， 即对其统计特性不做任何假设。设系统的初始状态为 X₀，初始估计误差方差阵为P₀，为系统初始状态X₀的一个估计。令 表示在给定量测值{Z_k}条件下对y_k的估计。

次优H∞滤波递推算法如下：

$P_k={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k,k-1}{\Gamma }_{k,k-1}^T-{\Phi }_{k,k-1}{P}_{k-1}\left(\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right)R_{e,k}^{-1}\left(\begin{array}{c}{H}_k\\ L_k\end{array}\right)P_{k-1}{\Phi }_{k,k-1}^T$

$R_{e,k}=\left(\begin{array}{cc}I& 0\\ 0& -{\gamma }^{2}I\end{array}\right)+\left(\begin{array}{c}{H}_k\\ L_k\end{array}\right)P_k\left(\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right)$

${\hat{y}}_k=L_k{\hat{X}}_k$

${\hat{X}}_k={\Phi }_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\Phi }_{k,k-1}{\hat{X}}_{k-1})$

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

H∞滤波算法在P_k的求解过程中，给定正数γ值通过R_e,k进而调整P_k。参数γ决定了 H∞滤波器的鲁棒性。

③改进算法

在系统模型和噪声统计特性精确已知时，Kalman滤波是最优估计，精度高，若系 统模型精确性差或者系统噪声为非高斯白噪声时，Kalman滤波精度下降甚至出现滤波 发散。基于Krein空间的H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简 单、鲁棒性好的优点。但是H∞滤波算法是以牺牲一定精度为代价获得系统鲁棒性的， 精度难以保证。综合考虑Kalman滤波算法的高精度和H∞滤波算法的鲁棒性，将Kalman 滤波算法与H∞滤波算法相结合，得到一种高性能的优化滤波算法。

如果系统模型是精确的，W_k和V_k为互不相关的零均值高斯白噪声序列，而且它们的 方差Q_k、R_k是精确的，则kalman滤波算法求得的新息序列{r_k}是一个零均值、白噪声 序列，新息序列计算公式为$r_k=Z_k-H_k{\hat{X}}_{k/k-1},$且其方差为：$E\left(r_k{r}_k^T\right)=H_k{P}_{k,k-1}{H}_k^T+R_k.$理想情况时，Kalman滤波过程中输出残差与的比值接近1。当由 于模型不确定性等因素的影响，造成滤波器的状态估计值偏离系统状态时，输出残差 与的比值偏离1；估计残差反映了实际估计误差的信息，与的比值反映了滤波器滤波性能。定义一个Kalman滤波器性能指标 M_k，计算方法为：

$M_k=\frac{r_k^T{r}_k}{\mathrm{tr}(H_k{P}_{k,k-1}{H}_k^T+R_k)}$

定义M₂为Kalman滤波器高精度运行的上临界，M₂接近为1，即对于每一时刻， 有M_k≤M₂，此时Kalman滤波器性能好。定义M_∞为Kalman滤波器低精度运行的下临 界，M_∞显著大于1，即对于每一时刻，有M_k≥M_∞，此时Kalman滤波器性能差甚至发 散。当M₂≤M_k≤M_∞时，Kalman滤波器性能一般。

优化滤波算法过程如下：

1)利用Kalman滤波算法求得t_k时刻状态估计：

2)利用H∞滤波算法求得t_k时刻状态估计：

3)计算Kalman滤波器性能指标M_k；

4)自适应调节权值d_k计算：$d_k=\left(\begin{array}{cc}1,& M_k\le M_2\\ a^{-\frac{M_k}{b}},& M_2<M_k\le M_{\infty }\\ 0,& M_k>M_{\infty }\end{array}\right)$

式中，d_k∈[0,1]，表示优化融合滤波器对Kalman滤波性能的信任度，a和b为设 计参数，1<a≤5，1<b≤10；a和b的取值大小决定了自适应调节权值d_k对Kalman滤 波性能指标变化的灵敏度和融合滤波器的精度及鲁棒性。

5)系统综合状态估计：${\hat{X}}_k=d_k{\hat{X}}_{2k}+(1-d_k){\hat{X}}_{\infty k}.$

（6）AUV系统滤波模型说明

①AUV位于水下潜航时，建立基于SINS/DVL的连续系统状态方程如下：

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

将SINS作为参考系统，状态变量X取速度误差姿态角误差(φ_x,φ_y,φ_z)、 加速度计随机常值偏置(▽_x，▽_y)和陀螺随机常值漂移(ε_x,ε_y,ε_z)，共10维： $X={[{\mathrm{\delta V}}_e^n,{\mathrm{\delta V}}_n^n,{\phi }_x,{\phi }_y,{\phi }_z,{▿}_x,{▿}_y,{ϵ}_x,{ϵ}_y,{ϵ}_z]}^T;$W=[w_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0]^T为系统 噪声向量；

以SINS解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之 差作为系统的量测值，则量测方程表示为：

$Z=\left(\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right)=\mathrm{HX}+V$

其中，系统量测量为量测噪声序列为V=[w_vx,w_vy]^T。将系统状态 方程和量测方程离散化可得离散系统滤波模型。

②AUV浮于水面时，建立基于SINS/GPS的连续系统状态方程如下：

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

将SINS作为参考系统，状态变量X取位置误差(δL,δλ)、速度误差姿态 角误差(φ_x,φ_y,φ_z)、加速度计随机常值偏置(▽_x，▽_y)和陀螺随机常值漂移(ε_x,ε_y,ε_z)、 GPS时钟误差及时钟频率误差(δt_u,δt_ru)，共14维： $X={[\mathrm{\delta L},\mathrm{\delta \lambda },{\mathrm{\delta V}}_e^n,{\mathrm{\delta V}}_n^n,{\phi }_x,{\phi }_y,{\phi }_z,{▿}_x,{▿}_y,{ϵ}_x,{ϵ}_y,{ϵ}_z,{\mathrm{\delta t}}_u,{\mathrm{\delta t}}_{\mathrm{ru}}]}^T;$系统噪声向量为： W=[0,0,w_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0,w_tu,w_tru]^T；

根据GPS输出的卫星星历数据，将SINS解算出的位置、速度量转化为对应于SINS 的伪距、伪距率；与GPS测得的伪距、伪距率分别作差，作为系统的量测值，则量测方 程表示为：

$Z=\left(\begin{array}{c}{Z}_{\rho }\\ Z_{\stackrel{·}{\rho }}\end{array}\right)=\left(\begin{array}{c}{H}_{\rho }\\ H_{\stackrel{·}{\rho }}\end{array}\right)X+\left(\begin{array}{c}{V}_{\rho }\\ V_{\stackrel{·}{\rho }}\end{array}\right)$

其中，伪距量测量用Z_ρ表示，伪距量测噪声序列为V_ρ，伪距量测矩阵为H_ρ；伪距 率量测量用表示，伪距率量测噪声向量为伪距率量测矩阵为将系统状态方 程和量测方程离散化可得离散系统滤波模型。

（6）仿真说明

AUV位于水下潜航阶段时，建立基于SINS/DVL的连续系统滤波模型，分别采用 Kalman滤波算法、H∞滤波算法和H2/H∞滤波算法对系统状态进行估计进而对SINS系 统进行校正；其中，M₂=1.5，M_∞=50，a=2.72，b=5.0，仿真时间为2小时；位置精度(纬 度误差△L，经度误差△λ)比较结果如图4所示。

工作过程说明

本案的基于SINS/DVL/GPS的AUV组合导航系统，工作过程包括如下步骤：

1、通过IMU组件中的陀螺仪敏感得载体的三轴角速度信息和加速度计测得三轴加 速度信息，IMU处理模块接收IMU输出的传感器信息，通过导航积分计算获得载体位 置、速度和姿态等导航信息；

2、通过安装于AUV底端的四个换能器发射具有一定频率、一定脉冲宽度的超声波 和接收具有频移特性的反射回波；

3、利用收发器中发射系统产生具有一定频率、一定脉冲宽度和功率的电振荡信号， 送给换能器；

4、利用收发器中接收系统将换能器送来的回波信号经放大处理后求得多普勒频移 并转换为航速模拟信号送给接口单元；

5、利用收发器中接口单元将收发器送来的四个航速模拟信号转换为航速，并以数 字方式向外部单元输出；

6、数据融合中心中的传感器信息转换模块根据接口单元输出的四波束速度信息周 期性地计算载体的实时三维载体系速度量；

7、数据融合中心中的传感器信息转换模块根据GPS输出的星历数据将SINS解算 得到的相关信息转化为对应于SINS的伪距、伪距率；

8、数据融合中心中的优化滤波模块对接收到的SINS导航信息、GPS接收机星历数 据、DVL计程仪的三维速度信息进行滤波融合计算，得到最终更高精度的AUV组合系 统的载体导航定位信息。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员 来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也 应视为本发明的保护范围。