CN111947651A

CN111947651A - 水下组合导航信息融合方法、系统及自主式水下航行器

Info

Publication number: CN111947651A
Application number: CN202010691625.2A
Authority: CN
Inventors: 朱兵; 李鼎; 林恩凡; 许江宁; 吴苗; 何泓洋
Original assignee: Naval University of Engineering PLA
Current assignee: Naval University of Engineering PLA
Priority date: 2020-07-17
Filing date: 2020-07-17
Publication date: 2020-11-17
Anticipated expiration: 2040-07-17
Also published as: CN111947651B

Abstract

本发明属于水下定位技术领域，公开了一种水下组合导航信息融合方法、系统及自主式水下航行器，利用SINS导航技术获取载体的姿态、速度、位置相关信息；利用DVL导航技术获取外部速度信息；利用AST导航技术获取外部位置信息；利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合，并输出融合后的导航信息。本发明能够实现长航时、长航程导航定位，同时能够校正和抑制定位过程中的误差。本发明可有效抑制观测野值对滤波结果的影响，具有更好的数据平稳性。同时试验结果初步表明：相比于TFKF算法，本发明提出的FRKF算法在非高斯环境中具有更高的组合导航精度和稳定性。

Description

水下组合导航信息融合方法、系统及自主式水下航行器

技术领域

本发明属于水下定位技术领域，尤其涉及一种水下组合导航信息融合方法、系统及自主式水下航行器。

背景技术

目前，高精度、高可靠性的水下导航定位技术在自主式水下航行器(AutonomousUnderwater Vehicles，AUV)的发展战略中具有重要地位。AUV具有活动范围大、机动性好、安全灵活、智能化、隐蔽性好等优点，目前是水下航行器的主要发展方向。AUV顺利完成任务，达到预定使命要求，离不开水下导航定位技术的有力支持，水下导航定位技术是促使AUV技术发展和应用的关键技术。与水面以上导航相比，水下导航辅助信息源少、环境十分复杂，因此水下导航定位难度相对更大。随着现代水下各方面需求的复杂多样化，尤其是水下作战样式逐渐向无人化、智能化、编队化等方向发展，对AUV的自主控制、远程续航及隐蔽长航时等方面的能力提出了更高的要求。未来AUV必须具有自主、精确的水下长航时、长航程的导航定位能力。

目前常用的水下导航定位手段有：惯性导航、水声定位及海洋物理场匹配导航等。

(1)惯性导航技术：

惯性导航技术是导航技术的重要分支，而INS是惯性技术的核心。INS利用加速度计和陀螺仪等惯性传感器测量运载体相对于惯性空间的线运动和角运动，根据牛顿第二定律在给定运动运动初始信息的条件下，通过计算机实时解算载体的三维姿态、速度、位置等导航信息。按照惯性测量装置安装在载体上的方式，INS可分为平台式惯导系统(GINS)和捷联式惯导系统(SINS)。它们的主要区别在于GINS利用环架将惯性敏感元件与载体的角速度隔离开来，而INS将陀螺仪和加速度计直接与载体固联。前者使用的是机械物理平台，而后者使用的是计算机建立的数学平台。

GINS存在体积庞大、质量大及制造成本高昂等诸多缺点。相比于GINS，SINS有着结构简单、重量轻、制造成本低、方便维护等优势，因此SINS将逐步取代GINS并成为现代惯性导航技术研究和发展的主流。SINS在导航过程中不向外界发射电磁波或其他信号，不易受外界干扰，具有高隐蔽性、抗干扰能力强的独特优势。SINS已经成为各种航行器自主式导航的核心装置。

无论是GINS还是SINS，由于初始姿态误差及IMU器件误差的存在，基于牛顿第二定律进行积分推算的惯导系统在解算过程中不可避免的存在振荡型、常值型和积累型的系统误差，且INS的导航定位误差随着时间不断积累，因此系统的长期导航定位精度难以保证。

(2)水声定位技术

水下有界误差导航定位需要借助外部传感器系统实现，由于海水对电磁波拒止屏蔽作用的影响，在水下无法有效使用无线电或卫星导航定位手段。与电磁波信号不同，声波信号在海水中传播衰减很小，它是目前水下通讯及导航定位最有效的信息传播载体。水声定位系统一般由多个基元(接收器或应答器)组成，其可按基线(基元间的连线)的长度分为三种：(1)长基线(Long Baseline，LBL)系统：基线长度一般为100m～6000m；(2)短基线(Short Baseline，SBL)系统；基线长度一般为1m～50m；(3)超短基线(ultra-shortbaseline，USBL)系统：基线长度一般小于1m。

较SBL和USBL定位系统，LBL定位系统可实现大范围、深海域较高精度的定位导航，但是LBL定位系统的复杂性也决定了其布设、校准难度也较SBL、USBL定位系统高。LBL导航定位主要依赖于声应答器 (信标)，在AUV执行任务之前，这些声应答器通常以系泊的方式部署在海底以上50-600米。LBL定位系统可以被认为是由多个AST组成的距离辅助导航系统，它通过同时测量来自多个AST的距离，实现对水下目标的导航定位。

AST是水下距离辅助导航定位的最小实现单元，可通过对AST进行距离测量来为AUV提供绝对位置参考信息。AST的工作模式可分为信标模式和应答模式，信标模式是指AST仅向外界发射声脉冲信号；应答模式是指AST在接收到潜器或水面运载体发射的询问信号时，同时发射应答信号。在实际应用中，可根据需求选择AST的工作模式。AST具有制造成本低、布设灵活方便、局部海域定位精度高等优点，基于AST 的水下距离辅助导航定位逐渐受到国内外学者的关注。

(3)航位推算技术

航位推算通常指二维平面内或三维空间中的位置推算。在水下环境中，利用DVL速信息并同时利用磁罗经(magnetic compass pilot，MCP)测航向信息可递推出二维平面的位置信息。而对于三维空间的航位推算，则需要通过陀螺罗经输出的三维姿态信息及DVL的测速信息来递推获取三维位置信息。DVL本质上属于速度声纳设备，它以多普勒效应为基础，利用声脉冲发射信号与散射回波信号之间的多普勒频移信息，得到运载体相对海水层或海底的速度。DVL的测速误差随时间不会累积，短期内航位推算方式具有较高的定位精度。对于航位推算系统来说，其无法直接消除导航传感器的误差。因此与INS一样，航位推算系统也存在定位误差随时间不断积累的问题，其长期定位精度难以保证。

(4)水下组合导航技术

在水下环境中，SINS难以长时间独立工作，需要利用各种外部辅助手段为SINS提供参考信息，再利用信息融合技术，实现对SINS初始姿态误差的补偿及累积误差的校正和抑制。目前主要的方法有：

1)利用AUV上浮或者投布放浮标等方式获取位置辅助信息；

2)利用水下声学定位系统(Acoustic positioning system，APS)提供位置辅助信息；

3)利用多普勒速度计程仪(Doppler velocity log，DVL)提供速度辅助信息；

4)通过海洋物理场匹配获取位置辅助信息等。

在以上的方式中，目前存在的问题：1)AUV通过上浮或者利用浮标获取位置信息的方式存在暴露目标的风险；

2)海洋物理场匹配现阶段仍然处于理论研究阶段，精确地建立地球物理数据库及检测地球物理参数等重大问题尚未得到有效解决，海洋物理长的建设还不能满足实际应用的需求；

3)水下DVL测速容易受到外部复杂环境的干扰，不能保证连续的高精度测速；

4)水下声学定位系统的布设成本高，尤其是在远海、深海布设的难度更加大。

综上所述，现有技术存在的问题是：1)现有水下单一定位方式，持续时间短，且定位精度不高，误差大。

(2)现有水下组合定位方法存在暴露目标的风险，容易受到外部复杂环境的干扰，无法保证连续高精度测速；且成本高，难度大。

解决上述技术问题的难度：不同于陆地上或水面上，由于电磁波信号在水中衰减极快，导致水下SINS 的动态启动以及长航时、长航程导航定位无法获取来自GPS的速度和位置信息的辅助。由于初始姿态误差及IMU器件误差的存在，基于牛顿第二定律进行积分推算的惯导系统在解算过程中不可避免的存在振荡型、常值型和积累型的系统误差，且INS的导航定位误差随着时间不断积累，因此系统的长期导航定位精度难以保证。在水下无卫星的环境中，SINS难以长时间独立工作，获取的速度、位置信息将产生较大的误差，使导航工作变得困难。为解决这种问题，需要利用各种外部辅助手段为SINS提供参考信息。难度在于如何选择合适的组合导航方式及导航数据信息融合算法的实现。

解决上述技术问题的意义：随着科学技术的不断发展，占据着地球70％以上面积的海洋已经成为许多国家争相开发和利用的领域以及军事必争之地。未来战争将是由陆地向太空和海洋迅速延伸的立体战争，海洋空间成为各海洋大国以及濒海国家积极开发与利用的主要阵地，深海战场将成为未来冲突与战争的主要场所。水下导航定位技术成为海洋资源勘探、开发，海洋工程以及军事作战和精确打击等国民和国防应用领域中必不可少的关键技术。高精度、高可靠性的水下导航定位技术在AUV的发展战略中具有重要地位。随着水下导航定位技术的迅猛发展，单一的导航传感器由于其自身原因无法满足AUV对高精度和可靠性导航信息的需求，因此通过多种导航传感器输出的导航信息进行有效融合，解决上述问题，可以提高导航系统的导航定位精度和容错能力，可实现水下AUV的长航时、长航程高精度低误差导航工作，可解决现有水下组合导航定位方法存在暴露目标的风险，实现AUV远程续航及隐蔽长航时工作。

发明内容

针对现有技术存在的问题，本发明提供了一种水下组合导航信息融合方法、系统及自主式水下航行器。具体涉及一种SINS/DVL/AST水下组合导航信息融合方法及水下组合导航信息融合系统。

本发明是这样实现的，一种SINS/DVL/AST水下组合导航信息融合方法，所述SINS/DVL/AST水下组合导航信息融合方法包括：

步骤一，利用SINS导航技术获取载体的姿态、速度、位置相关信息；利用DVL导航技术获取外部速度信息；利用AST导航技术获取外部位置信息；

步骤二，利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合，并输出融合后的导航信息。

进一步，步骤一中，所述利用DVL获取外部速度信息的方法包括：

(1)单波束配置测速，

发射声波频率为f₀，其速度为c，AUV沿水平方向航行，速度记为v_x；

根据多普勒效应，在P点处接收到的频率为：

在O'点处接受到的频率为：

DVL接收频率减发射频率即多普勒频移：

f_d＝f₂-f₀；

v_x在水中的传递速度远远小于c，α≈α'，式简化为：

f₀，α，c已知数，求出DVL的频移计算出相应的速度：

(2)双波束Janus配置测速：沿艏部方向波束的频率为：

艉部方向的频率为：

f_r1和f_r2是DVL艏部和艉部方向的接受频率，计算出多普勒频移为：

得到双波束配置情况下的航行器速度为：

(3)四波束Janus配置测速，在水下航行器的左右侧分别各添加一个发射器，构成四波束多普勒配置，航速为：

f_d13，f_d24为x和y方向的多普勒频移，计算航行器速度为：

根据多普勒的f_d13和f_d24计算出偏流角

进一步，步骤一中，所述AST导航技术获取外部位置信息的方法包括：

AST位于P点，坐标为(x,y,z)，USBL到AST的矢径为

的方向余弦为：

式中θ_x为

与x正向轴的夹角；θ_y为

与y正向轴的夹角；θ_z为

与z正向轴的夹角；得USBL定位公式：

x＝rcosθ_x，y＝rcosθ_y，z＝rcosθ_z；

式中，r＝c_s·t_s/2表示USBL到测量目标的斜距；c_s表示水中声速，t_s为声信号在水中的双程传播时延；θ_x、θ_y由USBL两轴上的阵元进行方位估计得到；θ_x、θ_y和θ_z满足如下关系式

cos²θ_x+cos²θ_y+cos²θ_z＝1；

当USBL的位置准确已知时，通过USBL与AST之间进行问答，并通过USBL对AST进行方位估计求解获取AST的位置；当AST的位置准确已知时，通过USBL与AST之间进行问答求解获取USBL的位置。

进一步，步骤二所述利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括：

基于获取到的信息是否受到非高斯噪声污染从而设置子滤波器采用RKF算法或标准KF算法；

当速度观测信息和位置观测信息均受到非高斯噪声的污染时，则子滤波器1和子滤波器2均采用RFK 算法；

当DVL输出的速度观测信息受到非高斯噪声污染，而AST输出的位置观测信息正常，则可设置子滤波器1为RKF算法，子滤波器2为标准的KF算法；

当DVL输出的速度观测信息正常，AST输出的位置观测信息受到非高斯噪声污染，则设置子滤波器1为标准的KF算法，子滤波器2为RKF算法。

进一步，步骤二，所述利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括以下步骤：

(1)时间更新：

上式中：i，k分别表示第i个子滤波器和时刻k；

为滤波结果；P为滤波方差；

(2)量测更新：

2.1)根据

计算μ_i,k，根据

判断观测量z_i,k是否正常；

2.2)若判定观测量z_i,k正常，则令λ_i,k＝1；若判定观测量z_i,k异常，则根据下式计算λ_i,k；

2.3)根据

计算

2.4)用

替代R_i,k，并根据

计算卡尔曼滤波增益K_i,k；

2.5)根据

计算状态量的后验估计

2.6)根据P_k|k＝(I-K_kH_k)P_k|k-1计算状态估计误差协方差矩阵P_i,k；

上述公式中：z_k表示k时刻的观测量；

表示观测量的先验估计；

表示k时刻评判指标；

表示马氏距离；λ_k表示为膨胀因子；R_k表示膨胀量测噪声协方差阵。

本发明的另一目的在于提供一种实施所述SINS/DVL/AST水下组合导航信息融合方法的 SINS/DVL/AST水下组合导航信息融合系统，所述SINS/DVL/AST水下组合导航信息融合系统包括：

SINS导航模块，用于利用SINS导航技术获取载体的姿态、速度、位置相关信息；

DVL导航模块，用于利用DVL导航技术获取外部速度信息；

AST导航模块，用于利用AST导航技术获取外部位置信息；

融合模块，用于利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合，并输出融合后的导航信息。

本发明的另一目的在于提供一种存储在计算机可读介质上的计算机程序产品，包括计算机可读程序，供于电子装置上执行时，提供用户输入接口以实施所述SINS/DVL/AST水下组合导航信息融合方法。

本发明的另一目的在于提供一种计算机可读存储介质，储存有指令，当所述指令在计算机上运行时，使得计算机执行所述SINS/DVL/AST水下组合导航信息融合方法。

本发明的另一目的在于提供一种实施所述水下组合导航信息融合方法的自主式水下航行器。

综上所述，本发明的优点及积极效果为：本发明能够实现长航时、长航程导航定位，同时能够校正和抑制定位过程中的误差。本发明提出的FRKF算法可有效抑制观测野值对滤波结果的影响，且相比于TFKF 算法，FRKF算法具有更好的数据平稳性。同时试验结果初步表明：相比于TFKF算法，本发明提出的FRKF 算法在非高斯环境中具有更高的组合导航精度和稳定性。

本发明由SINS、DVL和AST等导航传感器组成，基于各个导航传感器提供相应的导航信息，利用滤波算法将这些导航信息进行融合，进而得到更高精度、更加稳定的导航信息。

本发明设计FKF算法为无反馈模式，即主滤波只对子滤波器输出进行融合，且融合后的结果对子滤波器无反馈重置；不仅提高了滤波的容错性能，同时保证某个子滤波器发生故障时不会影响整个滤波器的滤波性能，从而确保水下组合导航系统的容错性能。

附图说明

图1是本发明实施例提供的水下组合导航信息融合方法流程图。

图2是本发明实施例提供的捷联惯导系统原理图。

图3是本发明实施例提供的DVL测速的配置中测速原理图。

图4是本发明实施例提供的AST水声导航定位原理示意图。

图5是本发明实施例提供的AST+USBL定位原理几何示意图。

图6是本发明实施例提供的水下组合导航信息融合系统结构示意图。

图中：1、SINS导航模块；2、DVL导航模块；3、AST导航模块；4、融合模块。

图7是本发明实施例提供的SINS/DVL/AST水下组合导航信息融合系统原理图。

图8是本发明实施例提供的SINS/AST组合导航原理示意图。

图9是本发明实施例提供的SINS/DVL/AST组合导航原理示意图。

图10是本发明实施例提供的FKF算法的结构图。

图11是本发明实施例提供的不同组合导航方法得到的位置误差示意图。

图12是本发明实施例提供的不同方法的组合导航姿态误差示意图。

图13是本发明实施例提供的不同方法的组合导航速度误差示意图。

图14是本发明实施例提供的不同方法的组合导航位置误差示意图。

图15是本发明实施例提供的不同方法的组合导航姿态误差图。

图16是本发明实施例提供的不同方法的组合导航速度误差图。

图17是本发明实施例提供的不同方法的组合导航位置误差图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

现有水下单一定位方式，持续时间短，且定位精度不高，误差大。现有水下组合定位方法存在暴露目标的风险，容易受到外部复杂环境的干扰，无法保证连续高精度测速；且成本高，难度大。

针对现有技术存在的问题，本发明提供了一种水下组合导航信息融合方法、系统及自主式水下航行器，下面结合附图对本发明作详细的描述。

如图1所示，本发明实施例提供的水下组合导航信息融合方法包括：

S101，利用SINS导航技术获取载体的姿态、速度、位置相关信息；利用DVL导航技术获取外部速度信息；利用AST导航技术获取外部位置信息。

S102，利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合，并输出融合后的导航信息。

步骤S101中，利用SINS获取载体的姿态、速度、位置相关信息包括：

陀螺和加速度计直接固联在运载体上，所以这种惯性导航系统称之为捷联式惯导系统(SINS)，简称捷联惯导。陀螺仪用来测量运载体的角速度信息。加速度计用来测量运载体的线运动信息，机载计算机根据陀螺仪输出的角速度信息，通过姿态矩阵解算可得到航向和姿态角参数。加速度计输出加速度信息经过坐标转换后变换成沿导航系的加速度信息，经过计算机两次积分可得到载体位置参数。捷联惯导系统原理图如图2。

步骤S101中，利用DVL获取外部速度信息包括：

多普勒计程仪(DVL)是声学仪器，利用载体发射的超声波至水底返回产生的多普勒频移进行工作。多普勒计程仪可以提供在载体坐标系中声波发射源的发射速度。当其在可测量的海底范围内时，可以提供前进方向以及侧向的基准速度，输出的速度信息准确度很高且较为稳定。

DVL测速的配置分为3种：单波束配置、双波束Janus配置和四波束Janus配置，其配置的测速原理如图3所示。具体包括：

(1)单波束配置测速

单向单波束配置的DVL如上图所示。假设其发射声波频率为f₀，其速度为c，AUV沿水平方向航行，速度记为v_x。

根据多普勒效应原理，在P点处接收到的频率为：

在O'点处接受到的频率为：

DVL接收频率减其发射频率即多普勒频移：

f_d＝f₂-f₀

可知，v_x在水中的传递速度远远小于c，此时可以近似α≈α'，式可以简化为：

f₀，α，c都是已知数，只要求出DVL的频移就可以计算出相应的速度：

(2)双波束Janus配置测速：

双波束值的是在航行器的艏部和艉部方向各发射一个波束，相比于单波束的情况可以提高一定的测速精度。这是因为当水下航行器上下方向存在波动时，因双波束自身对称特性，两个波束的发射频率相同，其波束倾斜角也相同。如上图3所示，可以得出此时沿艏部方向波束的频率为：

由于向艉部发射的波束与水下航行器运动的方向是反方向，所以速度应该为负值，此时艉部方向的频率是：

在这其中，f_r1和f_r2是DVL艏部和艉部方向的接受频率，根据上述原理可以计算出多普勒频移为：

可得到双波束配置情况下的航行器速度为：

(3)四波束Janus配置测速：

在实际应用中，还需要测量水下航行器的横向移动速度，在水下航行器的左右侧分别各添加一个发射

器，构成四波束多普勒配置，此时刻航速为：

f_d13，f_d24为x和y方向的多普勒频移，可计算航行器速度为：

根据多普勒的f_d13和f_d24可计算出偏流角

事实上，只需要三条波束就可以提供水下航行器的三个方向上的速度分量，但目前实际工程中大多采用四波束Janus配置。其原因是平面阵列天线产生四波束更为容易，其精度相比于三波束而言更高，

还可以给系统提供适当的测量冗余度，以提高系统的可靠性。

步骤S101中，AST+USBL水下定位基本原理

AST作为LBL定位系统的延伸，已在水下组合导航系统中获得了成功应用。海底AST定位原理示意图如图4所示。

由上图4可知，在AST的位置事先得到校准的前提下，只需测得AUV与AST之间的相对位置X_UVS，即可实现水下声学单应答器(或信标)辅助下的定位导航。AUV的绝对位置表达式如下式所示。

X_UV＝X_S+X_UVS

式中，X_UV为AUV在n系下的坐标，X_S为事先校准得到的AST在n系下的坐标，X_UVS为n系下AUV相对于AST的坐标。AST的导航定位方式可分为距离方位法定位、纯距离定位方式和纯方位定位方式。各方式的原理如下：

距离方位法定位：UV上的USBL与AST进行单词问答，即可测定相对距离和方位，然后解算出UV 的位置。此方法单次问答即可实现定位，但是系统复杂性高。

纯距离定位：UV上的USBL或收发合置换能器与AST进行多点多次问答，即可测定相对坐标，然后解算出UV的位置。此方法系统复杂性低，但需要多点测量，测量精度与测量点的位置密切相关。

纯方位定位：UV上的USBL与AST进行多点多次问答，可测定AST的方位信息，即可测定测量点的相对位置坐标，然后可解算出UV的位置。此方法可工作于被动方式，隐蔽性好，但系统较复杂，需要多点测量。

AUV在水下航行的过程总，若注重定位信息的实时性，应选择距离方位法定位方法；若注重降低系统的整体复杂性，应选择纯距离定位方式；若注重AUV的隐蔽性，应选择纯方位定位方式。

AST+USBL定位原理三维几何关系示意图如图5所示。USBL是一个4元平面阵，各阵元之间的几何关系满足十字垂直。以声学阵的中心为原点o，阵元1和阵元2在x轴(x轴指向船右舷)，阵元3和阵元 4在y轴(y轴指向船艏)，根据右手螺旋定则确定z轴，oxyz即为基阵坐标系(记为s系)。

假设AST位于P点，其坐标为(x,y,z)，USBL到AST的矢径为

的方向余弦为：

式中θ_x为

与x正向轴的夹角；θ_y为

与y正向轴的夹角；θ_z为

与z正向轴的夹角。因此，可得 USBL定位公式如下式所示。

x＝rcosθ_x，y＝rcosθ_y，z＝rcosθ_z

式中，r＝c_s·t_s/2表示USBL到测量目标的斜距；c_s表示水中声速，t_s为声信号在水中的双程传播时延。θ_x、θ_y可由USBL两轴上的阵元进行方位估计得到。θ_x、θ_y和θ_z满足如下关系式

cos²θ_x+cos²θ_y+cos²θ_z＝1

当USBL(即AUV)的位置准确已知时，可通过USBL与AST之间进行问答，并通过USBL对 AST进行方位估计求解获取AST的位置。反之，当AST的位置准确已知时，亦可通过USBL与AST之间进行问答来求解获取USBL(即AUV)的位置。

步骤S102中，本发明实施例提供的利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括：

基于获取到的信息是否受到非高斯噪声污染从而设置子滤波器采用RKF算法或标准KF算法。

当速度观测信息和位置观测信息均受到非高斯噪声的污染时，则子滤波器1和子滤波器2均采用RFK 算法。

当DVL输出的速度观测信息受到非高斯噪声污染，而AST输出的位置观测信息正常，则可设置子滤波器1为RKF算法，子滤波器2为标准的KF算法。

当DVL输出的速度观测信息正常，AST输出的位置观测信息受到非高斯噪声污染，则设置子滤波器 1为标准的KF算法，子滤波器2为RKF算法。

本发明实施例提供的利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括以下步骤：

(1)时间更新：

上式中：i，k分别表示第i个子滤波器和时刻k；

为滤波结果；P为滤波方差。

(2)量测更新：

2.1)根据

计算μ_i,k，根据

判断观测量z_i,k是否正常。

2.2)若判定观测量z_i,k正常，则令λ_i,k＝1；若判定观测量z_i,k异常，则根据下式计算λ_i,k。

2.3)根据

计算

2.4)用

替代R_i,k，并根据

计算卡尔曼滤波增益K_i,k。

2.5)根据

计算状态量的后验估计

2.6)根据P_k|k＝(I-K_kH_k)P_k|k-1计算状态估计误差协方差矩阵P_i,k。

上述公式中：z_k表示k时刻的观测量；

表示观测量的先验估计；

表示k时刻评判指标；

如图6-图7所示，本发明实施例提供的水下组合导航信息融合系统包括：

SINS导航模块1，用于利用SINS导航技术获取载体的姿态、速度、位置相关信息。

DVL导航模块2，用于利用DVL导航技术获取外部速度信息。

AST导航模块3，用于利用AST导航技术获取外部位置信息。

融合模块4，用于利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合，并输出融合后的导航信息。

下面结合具体实施例对本发明的技术方案作进一步说明。

实施例1：

1、组合导航技术

1.1SINS/AST组合导航技术

SINS/AST组合导航系统只需在AUV上装备一个水声测距设备(如USBL系统或收发合置换能器)，利用水下预先投(布)放水平位置和深度已知的单应答器(信标)，通过获取单应答器(信标)与AUV 之间的位置信息，再利用信息融合算法即可实现精确的导航定位。SINS/AST组合导航模式充分发挥了SINS 和APS的优势，并且极大简化了使用条件和降低了使用成本。SINS/AST组合导航基本原理是当AUV运动到AST的作用区域内时，由AST为其提供精确的位置信息从而抑制SINS的误差发散，SINS/AST组合导航原理示意图如图8所示。

1.2SINS/DVL/AST组合导航技术

AST的作用范围是有限的，由图9可以看出，当AUV运动到AST作用区域时，SINS的位置误差发散会得到有效抑制。但是当AUV离开AST的作用区域时，SINS的位置误差将会以很快的速度持续发散，限制了AUV在水下航行的能力和范围。因此使用SINS/AST组合导航方式存在一定的局限性。DVL测得的速度误差不会随时间累积，AUV在大深度、广海域航行时，DVL可为SINS提供可靠的外速度辅助信息。当AUV在水下机动航行时，为了抑制SINS位置误差的持续发散可设计以下组合导航方案：当AUV进入 AST的作用区域时，采用SINS/DVL/AST组合导航模式，此时SINS的导航定位误差保持稳定；当AUV 离开AST的作用区域时，采用SINS/DVL组合导航模式，此时AUV在DVL速度辅助的条件下导航定位误差缓慢发散，在即将达到误差上限时，使得AUV能够再次进入AST作用范围。SINS/DVL/AST组合导航原理示意图如图9所示。

2、本发明方案

2.1多传感器组合导航系统模型

本发明采用的水下组合导航系统如图所示。组合导航系统主要由SINS、DVL和AST等导航传感器组成。各个导航传感器提供相应的导航信息，利用滤波算法将这些导航信息进行融合，进而得到更高精度、更加稳定的导航信息。

2.2组合导航系统状态方程

(1)SINS状态方程建立

SINS经过初始对准后，俯仰失准角、横滚失准角和航向失准角均满足线性条件。根据SINS误差方程的推导，选取SINS状态量为：

式中：δL；δλ分别为纬度误差与精度误差。δv_E；δv_N分别为东向速度误差、北向速度误差。α＝[α_x；α_y；α_z]为欧拉平台误差角；

为陀螺仪常值漂移；

为加速度计零偏。

上式对应的状态方程如下所示。

式中，W_SINS～N(0，Q_SINS)为系统噪声，Q_SINS为系统噪声协方差阵。

由式(2.2.2)建立的状态转移矩阵F_SINS为：

式中，F为7×7矩阵，F中的非零元素如下：

F_1，4＝1/R_e，

F_2，1＝(v_E/R_e)tanLsecL，

F_2,3＝secL/R_e，

F_3，3＝(v_N/R_e)tanL，

F_3，4＝2ω_iesinL+(v_E/R_e)tanL，

F_3，6＝-f_U，

F_3，7＝f_N，

F_4,3＝-[2ω_iesinL+(v_E/R_e)tanL]，

F_4，5＝f_U，

F_4，7＝-f_E

F_5，4＝-1/R_e,

F_5，6＝ω_iesinL+(v_E/R_e)tanL,

F_5，7＝-[ω_iecosL+(v_E/R_e)],

F_6，1＝-ω_iesinL,

F_6，3＝1/R_e,

F_6,5＝-[ω_iesinL+(v_E/R_e)tanL],

F_6，7＝-v_E/R_e,

F_7,1＝ω_iecosL+(v_E/R_e)(secL)²

F_7，3＝tanL/R_e,

F_7，5＝[ω_iecosL+(v_E/R_e)]，

F_7，6＝v_N/R_e (2.2.4)

其中，

G为6×6矩阵，其中具体形式如下：

式中，

表示姿态矩阵

的前两行。

(2)DVL状态方程建立

刻度系数误差δC、速度偏移误差δV_d以及偏流角误差δφ等是影响DVL对速度的测量主要因素，其中，δC和δφ可表示为一阶Markov过程。因此,DVL的误差模型如(2.2.6)所示。

式中，β_d、β_φ分别为δV_d和δφ的一阶Markov过程相关时间；w_d、w_φ分别为δV_d和δφ的Gauss白噪声；δC为常数。

根据式，选取DVL状态量为：

X_DVL＝[δV_d；δφ；δL] (2.2.7)

上式对应的状态方程为；

式中，F_DVL＝diag(-β_d,-β_φ,0)，W_DVL为Gauss白噪声。

(3)AST状态方程的建立

类似于LBL定位系统，AST定位系统的定位误差可表示为一阶Markov过程，如下式所示。

式中，δL、δλ、δh分别为纬度误差、经度误差和高度误差；

分别为δL、δλ和δh 对应的一阶Markov过程相关时间；w_L、w_λ和w_h分别为δL、δλ和δh的Gauss白噪声。根据式(2.2.9)，选取AST的状态量为：

X_AST＝[δL；δλ；δh] (2.2.10)

由式(2.2.9)和式(2.2.10)可得AST的状态方程为：

式中，F_AST＝diag(-τ_L,-τ_λ,-τ_h)，W_AST为Gauss白噪声。

2.2.2组合导航系统量测方程

(1)SINS/DVL量测方程

DVL与SINS组合，一般选取速度误差作为观测量，即选取SINS测得的速度

与DVL测得的b系下速度

在n系上投影之差作为观测量。SINS/DVL量测方程如式(2.2.12)所示。

式中，H_v为量测矩阵，量测噪声V_v～N(0,R_v),R_v为量测噪声阵。在线性小失准角条件下，

若选取东向速度误差δv_E和北向速度误差δv_N作为观测量，则有：

在实际应用中，用

代替

可得量测矩阵H_v为

式中，

表示矩阵

的前两行。

(2)SINS/AST量测方程

AST可在作用范围内向AUV提供距离辅助信息X_UVS，进而通过(2.1.1)转化为n系下的经度λ_AST、纬度L_AST和高度h_AST等位置辅助信息。选取纬度误差δL和精度误差δλ作为观测量，SINS/AST 量测方程如式(2.2.15)所示。

式中，H_p为量测矩阵，量测噪声V_p～N(0,R_p),R_p为量测噪声阵。在水下环境中可由AST+USBL定位系统为AUV提供n系下的位置辅助信息。在线性条件下，选取纬度误差δL和精度误差δλ作为观测量，则有：

式中，L_AST、λ_AST可式(2.1.1)获取，量测矩阵H_p＝[I_2×2,0_2×11]。

2.3联邦鲁棒卡尔曼滤波(FRKF)算法

AUV在深水环境中航行时，其获取的外部辅助信息可分为DVL提供的速度信息和AST提供的位置信息。单一的集中式滤波器无法全面客服不同导航传感器系统带来的不良影响，FKF利用其独特的结构优势可将多种导航传感器的信息进行组合，为运载体提供准确、可靠的导航信息。FKF的基本构成单元是KF，可以说FKF不仅具备KF的优点，而且还继承了KF的缺点。深海环境复杂，这就导致AUV获取的外部辅助信息易受非高斯噪声污染，此种情形下FKF也像KF一样易发散。针对此问题，本发明根据现有的鲁棒KF算法 (RobustKalman Filter,RKF),提出联邦鲁棒卡尔曼滤波(FRKF)算法。

2.3.1RKF算法

k时刻离散KF的时间更新方程和量测更新方程如下：

(a)时间更新方程

(b)量测更新方程

P_k|k＝(I-K_kH_k)P_k|k-1 (2.3.6)

为了使KF具备鲁棒性，选择k时刻的观测量z_k与观测量的先验估计

之间的马氏距离作为评判指标，则k时刻评判指标

的定义如下式所示：

式中，

为马氏距离。对于真实的观测量

若其评判指标

满足

则观测量

将被标记为正常的观测量；反之，若其评判指标

满足

则观测量

将被标记为野值，此时通过引入膨胀因子λ_k用以膨胀量测噪声协方差阵R_k，即：

将式(2.3.8)带入(2.3.7)可得：

式(2.3.9)可转化为求解λ_k的非线性问题，如式(2.3.10)所示。

式中，λ_k可通过牛顿迭代法求解。因此λ_k(i+1)与λ_k(i)的关系可表示为：

式中，

且λ_k(i)初始值为λ_k(0)＝1。当评判指标满足

时，迭代终止。在求解出λ_k后，通过式(2.3.8)对量测噪声阵R_k进行膨胀，得到新的量测噪声阵λ_kR_k。用λ_kR_k替换R_k进行标准KF滤波即可得到RKF算法。

2.3.2 FKF算法

2.3.2.1 FKF基本原理

FKF算法的基本结构如图所示。由图10可以看出，FKF采用两个局部滤波器和一个主滤波器组成的两级滤波结构。对于组合导航系统来说，因SINS能够完备地给出姿态、速度和位置信息，因此组合导航系统将SINS作为公共参考系统。记SINS/DVL组合导航系统对应的子滤波器为“子滤波器1”，其对应的标准KF算法为“KF1算法”，其对应的鲁棒KF算法为“RKF1算法”。SINS/AST组合导航系统对应的子滤波器为“子滤波器2”，其对应的标准KF算法为“KF2算法”，其对应的鲁棒KF算法为“RKF2算法”。

DVL可为SINS提供速度观测z_v，AST可为SINS提供位置观测z_p。根据信息分配系数β_i的分配策略，可将FKF分为四个模式：

(1)无反馈模式：在此模式下仅在滤波初始时刻对信息进行分配，主滤波器只发挥融合作用，融合结果不对子滤波器进行反馈重置，容错性能较高。

(2)零复位模式：在此种模式下，当子滤波器向主滤波器输出融合结果后，自动置零。

(3)融合反馈模式：在此种模式下，主滤波器融合子滤波器的输出后，将结果反馈给子滤波器以重置子滤波器，此种模式滤波精度较高，但容错性较差。

(4)变比例模式：在此种模式下，子滤波器与主滤波器对信息平均分配，此种模式容错性较差。在实际应用中可根据需求选择合适的工作模式。由于各个观测源相互独立，k时刻FKF的具体过程如下。

(a)信息分配

式中，i，k分别表示第i个子滤波器和时刻k；

为滤波结果；P为滤波方差；下标g表示全局融合滤波器。β_i为信息分配系数，满足信息守恒原则：

∑β_i+β_m＝1 (2.3.13)

式中，β_m为主滤波器的信息分配系数，下标m表示主滤波器。

(b)时间更新

(c)量测更新

(d)信息融合

将各个子滤波器的估计进行数据融合，得到融合状态

和融合协方差P_g,k|k如下式所示：

本发明为了确保滤波的容错性能，设计FKF算法为无反馈模式，即主滤波只对子滤波器输出进行融合，且融合后的结果对子滤波器无反馈重置。这样设置的好处是在某个子滤波器发生故障时不会影响整个滤波器的滤波性能，从而确保水下组合导航系统的容错性能。

2.3.2.2信息分配系数自适应选取方法

在SINS/DVL/AST组合导航系统中，各个子滤波器公共状态向量相同，因此设计主滤波器状态量

和协方差阵P_m不进行时间更新，即设计P_m,k＝0。组合导航系统选择信息分配系数是决定FKF滤波性能的重要因素，传统的信息分配系数选取方法多依赖于经验或大量实验，一般设置为各个子滤波器具有相同的分配系数。在此种分配方式下，一个子滤波器性能的下降可能会对FKF的全局滤波性能造成较大影响。因此根据各子滤波器滤波精度自适应地调整信息分配系数对FKF的整体性能具有重要意义。为了选取合理的信息分配系数，首先对分配系数β_i与滤波精度之间的关系进行分析，具体如下。

FKF状态量估计误差协方差的定义如下式所示。

上式中，P_i是对状态量X_i估计精度的描述，也就是说X_i估计精度越高，

越接近于真实值，P_i越小；反之，X_i估计精度越低，

越偏离于真实值，P_i越大。用协方差阵的全局估计重置子滤波器协方差矩阵，得：

由上式可以看出，

越大,P_i越小，表示X_i的估计精度越高；反之，

越小,P_i越大，表示X_i的估计精度越低。由式(2.3.12)、式(2.3.14)和式(2.3.16)可知，第i个子滤波器在k时刻的先验估计为：

式中，

由式(2.3.14)可知，k时刻第i个子滤波器状态估计误差协方差的先验估计为：

式中，

由式(2.3.15)可知，k时刻第i个子滤波器的状态量的后验估计为：

又卡尔曼滤波增益K_i,k可表示为：

将式(2.3.20)和(2.3.22)代入式(2.3.21)得：

K时刻第i个子滤波器状态估计误差协方差的后验估计P_i,k为：

将式(2.3.23)和式(2.3.24)分别代入式(2.3.16)得：

通过以上推导可知，子滤波器的估计精度越高，P_i,k越小，由(2.3.24)可知

越大；反之，子滤波器的估计精度越低，P_i,k越大，由(2.3.24)可知

越小。因此，选取信息分配系数β_i时应满足：估计精度越高的子滤波器应分配更大的β_i。又因状态估计误差协方差P_i,k是反应子滤波器估计精度的重要指标，因此β_i的调整策略如下所示。

式中，

表示矩阵

的对角线元素组成的一个新的对角矩阵；||·||_F表示 Frobenius范数。由式(2.3.26)可以看出，当子滤波器的估计精度越高，即

越大，从而β_i越小，因此估计精度低的子滤波器应分配更小的信息分配系数。以上信息分配系数选取方法可使FKF始终保证精度高的子滤波器具备更大的信息分配系数，从而可在保证容错性能的前提下有效提高FKF的滤波精度。

重写融合状态

的表达式，如式(2.3.27)所示。

由式(2.3.27)可知，P_i,k越大，

越小，即第i个滤波器的状态量后验估计

的权矩阵

越小；反之，P_i,k越小，

越大，即第i个滤波器的状态量后验估计

的权矩阵

越大。因此，重新定义融合状态

的表达式如下：

由上式可以看出，若第i个子滤波器估计精度较高，即P_i,k越小，

越大，

越大，β_i越大，从而第i个子滤波器分配较大的权值；反之，若第i个子滤波器估计精度较低，即P_i,k越大，

越小，

越小，β_i越小，从而第i个子滤波器分配较小的权值。式(2.3.28) 不仅能够使FKF算法具备较好的容错性能，而且可使FKF算法保持较高的滤波精度。

2.3.2.3融合参与信息时间不同步问题

式(2.3.12)-(2.3.16)对FKF算法进行了公式描述，其要求所有参与信息融合的子滤波器获取的导航传感器子系统输出的观测信息是同步的，但是这在实际应用中尤其是水下组合导航过程中是无法保证的，如SINS输出频率一般为100Hz-1000Hz，而DVL的输出频率一般为1Hz-几Hz，AST的输出频率一般只有0.1Hz-1Hz，且AST与DVL的输出并不能保证完全同步。因此，必须在信息融合时对各子滤波器输出作同步处理。本发明只考虑AST和DVL数据更新周期为SINS数据更新周期的整数倍时的简单情况，具体分析处理过程如下。假设SINS数据更新频率和周期分别为f_INS、T_INS，DVL数据更新频率和周期分别为f_DVL、 T_DVL，AST数据更新频率和周期分别为f_AST、T_AST。SINS、DVL和AST数据更新周期之间的关系如式(2.3.29) 所示。

T_DVL＝N1·T_INS，T_AST＝N2·T_INS (2.3.29)

式中，N1、N2为正整数，实际应用中N1一般不等于N2。设计FKF融合策略，具体如下。

S1:若N1≠N2，且N1＞N2，则在kk·N2·T_INS时刻进行主滤波器信息融合，融合时的子滤波器1输出的状态量X₁和状态估计误差协方差P₁均是通过时间更新过程(预测)获得的，直至子滤波器1获取观测量时，融合时的X₁和P₁均是通过量测更新过程获得；

S2:若N1≠N2，且N1＜N2，则在kk·N1·T_INS时刻进行主滤波器信息融合，融合时的子滤波器2输出的状态量X₂和状态估计误差协方差P₂均是通过时间更新过程获得的，直至子滤波器2获取观测量时，融合时的X₂和P₂均是通过量测更新过程获得；

S3:若N1≠N2，且N1·T_INS＞1s，N2·T_INS＞1s，则主滤波器在T's时刻进行融合，T'为不为0的自然数，且T'＜N1·T_INS，T'＜N2·T_INS，N1·T_INS和N2·T_INS均是T'的整数倍。在kk·N1·T_INS进行信息融合时的子滤波器1输出的状态量X₁和状态估计误差协方差P₁均是通过量测更新过程获得的，其余时刻子滤波器1输出的状态量X₁和状态估计误差协方差P₁均是通过时间更新过程获得的。同理，在kk·N2·T_INS进行信息融合时的子滤波器2输出的状态量X₂和状态估计误差协方差P₂均是通过量测更新过程获得的，其余时刻子滤波器2输出的状态量X₂和状态估计误差协方差P₂均是通过时间更新过程获得的。这种融合策略在无观测信息更新时利用子滤波器的预测进行T's一次的融合，有利于充分利用子滤波器的预测信息，但也可能使滤波整体性能低于S1和S2策略。

2.3.3 FRKF算法

2.3.1节通过引入膨胀因子λ实现对KF的鲁棒化，得到RKF算法。KF与RKF的主要区别在于量测更新过程：对于标准的KF算法来说，用λ_kR_k替换R_k的按照式(2.3.3)-(2.3.6)进行量测更新即可得到 RKF算法；对于RKF算法来说，当λ_k＝1时，RKF算法将退化为标准的KF算法。因此，FRKF算法与FKF算法具有相同的时间更新过程，区别仅在于两者的量测更新过程。以下给出k时刻FRKF算法的量测更新过程如S1-S4所示。

S1：根据式(2.3.3)计算μ_i,k，根据式(2.3.7)判断观测量z_i,k是否正常。若判定观测量z_i,k正常，则令λ_i,k＝1；若判定观测量z_i,k异常，则根据式(2.3.9)-(2.3.11)计算λ_i,k。根据式(2.3.8)计算

S2：用

替代R_i,k，并根据式(2.3.4)计算卡尔曼滤波增益K_i,k；

S3：根据式(2.3.5)计算状态量的后验估计

S4：根据式(2.3.6)计算状态估计误差协方差矩阵P_i,k。

由以上过程推导出的FRKF算法，适用于速度观测信息和位置观测信息均受到非高斯噪声的污染的情形，在此情形下其子滤波器1和子滤波器2均为RKF算法。若DVL输出的速度观测信息受到非高斯噪声污染，而AST输出的位置观测信息正常，在FRKF算法中可设置子滤波器1为RKF算法，子滤波器2为标准的KF算法；反之，若DVL输出的速度观测信息正常，AST输出的位置观测信息受到非高斯噪声污染，在 FRKF算法中可设置子滤波器1为标准的KF算法，子滤波器2为RKF算法。因此，可以认为本发明提出的 FRKF算法是广义上的联邦鲁棒滤波算法，即可根据子滤波器获取的观测信息是否受到非高斯噪声污染来设置子滤波器为RKF算法或标准KF算法。

下面结合具体实验对本发明作进一步说明。

3实验：

3.1基于船载实测数据半物理仿真试验验证

3.1.1数据生成

选取两组3600s船载实测数据进行水下组合导航半物理仿真试验，仿真试验是基于matlab 2015a软件进行的。选取的两组3600s数据包含：陀螺仪和加速度计的原始数据，对应的姿态、速度和位置基准，对应的GPS输出，对应的DVL输出。这两组3600s数据对应的DVL输出、GPS输出满足：

(1)第一组DVL输出数据不受非高斯噪声污染；第二组DVL输出数据受数据野值污染。

(2)GPS输出不受非高斯噪声污染。

利用GPS输出的位置信息模拟水下AST输出的位置信息：AST的输出频率一般为0.1Hz-1Hz，由此对 GPS输出的位置信息进行降频，使其10s输出一个位置信息(频率为0.1Hz)。AST+USBL导航定位误差最大不超过15m。因此，仿真试验在生成AST定位信息时，基于降频后的GPS位置信息认为地引入幅值为20m 的随机定位误差。

3.1.2实验验证1

为了检验2.3.2节提出分配系数自适应选取方法的有效性，利用第一组数据进行导航仿真试验。在组合导航试验中，FKF设置为无反馈模式，即各个子滤波器进行独立滤波。

记按照式(2.3.26)确定信息分配系数的FKF为IFKF(Improved FKF)；信息分配系数平均分配的FKF 算法为TFKF(tradition FKF)，TFKF算法即为经典的FKF算法。选择第一组3600s数据，结合AUV在水下组合导航的实际，即AUV会使出AST的作用范围的情况：设置从第1500s开始AUV使出AST的作用范围；从第2400s开始，设置AUV重新进入AST的作用范围。也就是说AST提供的位置信息丢失了900s，且在无观测信息期间子滤波器2只进行时间更新(预测)，即在此期间子滤波器2的融合结果是通过预测得到的。设置AST在初始时刻对AUV的定位误差为20m。在以上仿真条件下，分别利用KF2算法(SINS/AST组合导航系统)、IFKF算法、TFKF算法进行组合导航试验，组合导航采用闭环修正模式，即在主滤波器进行信息融合后，利用融合后的状态量对SINS输出的姿态、速度和位置进行修正，同时子滤波器中的姿态、速度和位置对应的状态量重新置0。组合导航得到的位置误差结果分别为图11(a)-(b)所示。图11(a)-(b) 分别为纬度估计误差和经度估计误差，其中，黑色虚线为KF2算法(SINS/AST)得到的位置估计误差，蓝色虚线为TFKF算法得到的位置估计误差，红色实线为IFKF算法得到的位置估计误差。

由图11可以明显看出，当AUV驶出AST作用范围(1500s～2400s)时，KF2算法对位置的估计误差曲线呈发散趋势，TFKF算法和IFKF算法对位置的估计误差曲线虽有缓慢发散趋势，TFKF算法和IFKF算法的组合导航精度和稳定性明显优于KF2算法。在此期间，KF2算法得到的纬度/经度误差最大值为 -242.90m/-86.37m，TFKF算法得到的经度/纬度误差最大值为-22.40m/-32.24m，IFKF算法得到的纬度/经度误差最大值为-20.29m/-31.31m。这是因为在AUV的航行过程中，IFKF算法主滤波器的输出是通过式 (2.3.28)进行加权融合得到的，且IFKF算法采用闭环修正模式实时地修正公共参考系统SINS的解算误差，这就使得AUV在驶出AST作用范围之前SINS/DVL组合导航子系统与SINS/AST组合导航子系统对位置的估计精度相当。AUV离开AST作用范围后，SINS/AST无观测信息，此期间可认为子滤波器2发生故障。式(2.3.28)使主滤波器输出中的子滤波器1的权值高于子滤波器2的权值，因此在AUV离开AST作用范围后，子滤波器1起主要作用，致使IFKF算法得到的位置估计误差曲线呈缓慢发散趋势。验证了本发明所提信息分配系数自适应选取方法的有效性。

3.1.3试验验证2

利用选取的第二组3600s数据验证非高斯条件下FRKF算法相比与TFKF算法的优势。基于上节模拟生成的AST数据，分别利用FRKF算法和TFKF算法进行组合导航试验。

DVL属于主动声纳设备，在特殊环境下频繁使用DVL发散声呐信号容易降低AUV的隐蔽性。为了进一步增强AUV的隐蔽性，模拟AUV在进入AST作用范围后降低DVL的使用频率的情形：对DVL的输出进行人为地阻隔，使AUV每隔30s获取一个速度观测信号。同时，由于水下环境的复杂性，AST输出易受非高斯噪声的污染，每隔150s人为地将幅值为500m的位置误差(野值)引入式(2.2.15)。

图12中，黑色实现表示利用TFKF算法进行组合导航的姿态误差曲线，红色虚线表示利用FRKF算法进行组合导航的姿态误差曲线。由图可以看出，当SINS/AST子滤波器观测量受到野值污染时，TFKF算法的组合导航姿态误差曲线在野值出现的时刻发生了突变，而FRKF算法的组合导航姿态误差曲线平稳。当组合导航收敛后，FRKF算法得到的俯仰角误差从0.1°下降到0.010°以内，横滚角误差从0.1°下降到 0.015°以内，航向角误差从0.5°下降到0.3°以内。

图13和图14分别为利用不同方法进行组合导航的速度误差和位置误差。图13(a)和图13(b)分别为东向速度误差和北向速度误差；图14(a)和图14(b)分别为纬度误差和精度误差。图13中，黑色实线为利用TFKF算法进行组合导航的速度误差曲线，红色虚线为利用FRKF算法进行组合导航的速度误差曲线；图14中，黑色实线为利用TFKF算法进行组合导航的位置误差曲线，红色虚线为利用FRKF算法进行组合导航的位置误差曲线。

通过试验可知，当组合导航收敛后，利用FRKF算法组合导航的东向速度误差收敛到0.2m/s以内，北向速度误差收敛到0.15m/s以内；纬度误差收敛到25m以内，经度误差收敛到15m以内。可以看出，当观测信息受到野值污染是，FRKF算法的组合导航精度和稳定性明显优于TFKF算法。通过试验及计算可知，利用TFKF算法和FRKF算法得到的速度误差标准差和位置误差标准差如下表所示。

表1不同方法得到的组合导航速度、位置误差标准差

由图12-图13及表1可以看出，本发明提出的FRKF算法可有效抑制观测野值对滤波结果的影响，且相比于TFKF算法，FRKF算法具有更好的数据平稳性。试验结果初步表明：相比于TFKF算法，本发明提出的FRKF算法在非高斯环境中具有更高的组合导航精度和稳定性。

下面结合基于车载实测数据半物理仿真试验验证对本发明作进一步描述。

为了进一步验证本发明提出FRKF算法的有效性及相比于单一RKF算法的优势，设计了基于FOSN 光纤捷联惯导系统的车载实验。

车载实验在武汉市内进行，数据采集系统的数据主要包含：UTC时间基准、陀螺仪和加速度计输出的原始数据、GPS输出的位置数据以及由SINS/GPS组合导航后生成的姿态、速度和位置基准数据。

利用SINS/GPS组合导航后的姿态和速度基准数据模拟生成深海DVL输出的速度数据，利用GPS输出的位置数据模拟生成AST输出的位置数据，模拟数据生成过程如下。

(1)深海DVL测速数据模拟

假设车载实验数据采集系统得到的SINS/GPS组合导航姿态基准、速度基准分别为

则模拟生成深海DVL的输出数据

如下式所示。

设置DVL漂移为航程的0.5％，DVL短时精度为0.05m/s，考虑到深海环境的复杂性，设置每间隔12s 载体系测速受到幅值为50m/s的速度野值污染。实验中，将上述设置条件人为地引入上式。

(2)深海AST定位数据模拟

人为地对GPS数据进行降频，由1Hz降为0.1Hz。同时人为地引入幅值为50m的随机位置误差。由于深海环境的复杂性，AST输出极易受非高斯噪声的污染，每隔30s人为地将幅值为1nm的位置误差(野值)引入。

为了进一步验证本发明提出的FRKF算法的有效性和优势，选取3600s深海模拟数据进行水下组合导航仿真试验。考虑到提高AUV的隐蔽性，假设AUV进入AST作用范围以后，使AUV每隔30s受到一个 DVL测速信号。同时，考虑到AUV会驶入AST的作用范围的情况：设置从第1600s开始AUV驶出AST 的作用范围；从第2600s开始，设置AUV重新进入AST的作用范围。分别利用RKF1算法(SINS/DVL)、 RKF2算法(SINS/AST)和FRKF算法进行组合导航试验，下图为不同方法组合导航后的姿态、速度和位置误差。图15不同方法的组合导航姿态误差。图16不同方法的组合导航速度误差。图17不同方法的组合导航位置误差。

图15、16、17为车载实测数据半物理仿真试验验证的误差曲线。具体地，图15不同方法的组合导航姿态误差。图15(a)～图15(c)分别为利用不同方法进行组合导航后的俯仰角误差、横滚角误差和航向角误差。

图16不同方法的组合导航速度误差。图16(a)、图16(b)分别为利用不同方法进行组合导航后的东向速度误差和北向速度误差。

图17不同方法的组合导航位置误差。图17(a)、图17(b)分别为利用不同方法进行组合导航后的纬度误差和经度误差。

由上图15-17可以看出，相比于RKF1和RKF2，本文提出的FRKF具有更高的容错性能，即当子滤波器2发生故障时，FRKF仍会使组合导航系统整体的导航定位精度保持稳定。这是因为FRKF根据子滤波器精度对信息分配系数进行自适应调整，使滤波器的整体性能始终与性能较好的子滤波器保持一致。通过试验可知，组合导航收敛后，FRKF算法获取的纬度误差绝对值在48m以内，经度误差绝对值在56m以内。仿真试验结果进一步验证了本文提出的FRKF算法的有效性及相比于单一的RKF1算法和单一的RKF2 算法在深海组合导航中的优势。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的硬件平台的方式来实现，当然也可以全部通过硬件来实施。基于这样的理解，本发明的技术方案对背景技术做出贡献的全部或者部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。