[0028] 令Pout(k)、Vout(k)初始值为0；
[0029] 此阶段不需要对Kalman滤波处理模块的输出结果进行修正，
[0030]
[0031]
[0032] 其中Pins(k)、Vins(k)分别为SINS解算位置、速度值；即对SINS解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值。
[0033] 进一步地，所述步骤1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号，来自DVL的速度信号VDVL和来自捷联SINS的姿态 信号；当SINS/DVL/ES信息预评判模块接收到DVL的速度信号VDVL数据时，进行以下两个判断：a.DES是否小于阈值D(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)，具体包括以下步骤：
[0034] 1.1)SINS/DVL/ES信息预评判模块接收来自测深仪发送的深度信号DES进行判断，如果DES小于阈值D(th)，其中D(th)的值为所述SINS/DVL/ES组合导航系统的DVL产品的D(th)值，说明当前DVL所提供的速度数据VDVL能真实反应载体相对于海底的运动速度，DVL将VDVL送至Kalman滤波处理模块，即允许VDVL数据参与信息融合，运行1.2)，否则说明当前DVL的速度数据VDVL无效,即由于水深度值太大，VDVL测得的数据不能真实反应载体相对于海底的运动速度，而是相对于水流的速度，此时立即停止DVL的速度数据VDVL参与信息融合，程序重新运行1.1)进行下一次阈值比较，进入下一轮评判；
[0035] 1.2)SINS/DVL/ES信息预评判模块根据捷联惯性导航系统SINS所提供的纵摇角增量Δθ进行判断，当纵摇角增量Δθ大于纵摇角增量阀值Δθ(th)时，所述Δθ(th)由具体的DVL和系统设计要求决定，AUV的瞬间纵摇上界为8度；说明AUV的瞬间纵摇变化将影响DVL的测速精度，如果AUV的纵摇角增量小于上界值时DVL将VDVL送至Kalman滤波处理模块，即允许VDVL数据参与信息融合，否则VDVL数据不参与Kalman滤波。
[0036] 有益效果
[0037] 与现有技术相比，本发明通过对DVL的数据在进行在SINS/DVL进行组合导航之前进行数据有效性判断，并对组合导航系统的输出结果引入了残差x2检测法对DVL信号进行实时评断和修正。本发明既能够有效抑制SINS/DVL组合导航的姿态和速度发散速度，提高组合系统速度、姿态精度，在一定程度上提高位置精度。又能解决了在DVL 数据信息质量下降造成的情况下带来姿态误差的振荡问题。
[0038] 传统的SINS/DVL组合导航系统在DVL信号不可用的情况下姿态误差一直处于震荡状态，纵摇最大姿态误差为3.584°，横摇最大姿态误差为3.609°，航向最大误差为 9.81°；其速度误差存在较多毛刺，东向速度VE误差最大为5.148m/s，北向速度VN误差最大为5.58m/s，天向速度VU误差最大为9.215m/s。本发明在SINS/DVL组合导航前对 DVL的数据是否可用进行了判断，并对组合导航的结果进行修正，处理后的组合导航水平姿态误差在0.1°以内，系统运行8000s时航向姿态误差为-0.163°；速度误差在1m/s；如说明书附图4、5所示，本发明经过评判和补偿修正处理后的组合导航的姿态、速度误差中大部分较大尖锐毛刺可以消除，能够较大幅度提高组合导航姿态、速度精度，并能在一定程度上能够提高组合导航位置精度。
[0039] 本发明在SINS/DVL松组合系统前对DVL的数据有效性进行判断，针对组合导航后的结果引入了残差x2检测法对DVL信号进行实时评判和修正补偿。实验表明，当AUV 工作深度距海底过高引起的DVL测速误差过大或其它干扰使DVL速度数据信息质量下降时或DVL信号重新恢复后Kalman滤波器的振荡的情况下都能够引起残差x2检测法的检测函数值发生变化。通过评判结果再结合SINS/DVL组合前后两次的输出导航结果与载体的实际机动性能，按照本发明提出的导航结果输出修正方法能够较大程度提高 SINS/DVL松组合系统的姿态、速度精度，在一定程度上提高SINS/DVL松组合系统的位置精度。本发明提出的一种基于SINS/DVL松组合的水下AUV组合导航系统及方法方案简单、易于实现，具有强较的工程实用性。
附图说明
[0040] 图1为一种SINS/DVL/ES信息预评判及导航结果补偿修正方法的原理示意图；
[0041] 图2为SINS/DVL组合评判模块原理示意图；
[0042] 图3为补偿修正处理模块(速度参数)原理示意图；
[0043] 图4为姿态误差曲线；
[0044] 图5为速度误差曲线。
具体实施方式
[0045] 下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
[0046] 实施例1：
[0047] 如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。
[0048] 如说明书附图1所示，本发明由以下7个部分组成：
[0049] 本发明由安装在水下AUV(Autonomous Underwater Vehicle，自主式水下航行器，简称AUV)上的SINS(Strapdown Inertial Navigation Systems，捷联式惯性导航系统，简称SINS)，DVL(Doppler velocity log，多普勒速度计程仪，简称DVL)， ES(Echo-Sounder，测深仪，简称ES)，SINS/DVL/ES信息预评判模块，Kalman滤波处理模块，组合导航补偿修正评判模块及补偿修正处理模块组成。
[0050] 捷联惯性导航系统SINS模块包括惯性测量单元IMU元件及IMU处理单元，其中 IMU元件用于得到惯导，IMU处理单元对由IMU得到的数据进行捷联解算，得到包括速度Vins，位置Pins，姿态 等导航信息。多普勒速度计程仪DVL模块主要利用发射的声波和接收的水底反射波之间的多普勒频移测量AUV相对于水底的航速VDVL，该 DVL输出AUV的航速VDVL。测深仪ES输出深度信息DES。SINS/DVL/ES信息预评判模块主要根据DVL所提供AUV的航速VDVL、测深仪ES输出深度信息DES和SINS 的速度Vins进行数据评判决策，根据融合评判结果判断DVL是否参与组合导航。Kalman滤波处理模块是使用Kalman滤波器进行DVL和SINS的信息融合处理。组合导航补偿修正判断模块是对用于判断DVL输出信号与SINS进行组合后的导航结果是否需要补偿修正。补偿修正处理模块是对组合导航的结果进行补偿修正处理。
[0051] 一种SINS/DVL/ES信息预评判及导航结果补偿修正方法首先读取SINS的输出报文中SINS速度Vins、姿态 信息和DVL的输出报文中速度VDVL、ES输出的深度DES信息。SINS/DVL/ES信息预评判模块是按说明书附图2所示的方法进行评判的：
[0052] 第一步，该系统读取到由这四个参数后首先SINS/DVL/ES信息预评判模块对ES所提供的深度DES进行判断，如果DES小于阈值D(th)(D(th)由具体的DVL产品决定)说明当前DVL所提供的数据报文是可用的。否则，说明当前DVL的速度信息误差很大，应立刻停止信息融合；这主要是因为DVL受工作深度限制，超过数百米时，只能利用水层中的水团质点作反射层，变成对水的测速仪，而不是AUV的航速。
[0053] 第二步，如果第一步DES小于阈值D(th)，则SINS/DVL/ES信息预评判模块根据捷联惯性导航系统SINS所提供的纵摇角增量Δθ进行判断，如果在第一步中DVL的速度信息可用的情况下，AUV的纵摇角增量Δθ大于Δθ(th)时(Δθ(th)由具体的DVL和系统设计要求决定，一般AUV的瞬间纵摇不大于8度，)，这是因为AUV瞬间的纵摇变化将极大地增大DVL的所对应的相对误差值，说明AUV的瞬间纵摇变化将影响DVL 的测速精度，此刻立即停止VDVL参与信息融合。
[0054] 如果前面二步都评判通过，则将VDVL送入Kalman滤波处理模块，否则，不将VDVL送入Kalman滤波处理模块。
[0055] 本发明中组合导航补偿修正判断模块是如下定义的：定义Kalman滤波处理模块中Kalman滤波器的残差为：
[0056]
[0057] 式中 为观测量 H为观测矩阵， 为预报值，其中φk/k-1为从k-1时刻到k时刻的一步状态转移矩阵。
[0058] 当DVL信号正常时，滤波器残差rk是零均值白噪声，其方差为：
[0059] 其中，Rk为系统观测噪声方差阵。
[0060] Pk/k-1可通过
[0061]
[0062]
[0063]
[0064] 计算得到。
[0065] 组合导航修正判断模块是按以下方法和步骤进行判断的：首先，对rk作如下二元假设：H0表示无故障，H1表示有故障。
[0066] 无故障：E{rk}＝0，
[0067] 有故障：E{rk}＝μ，E{(rk-μ)(rk-μ)T}＝Ak。
[0068] 故障检测函数为 其中，γk是服从自由度为m的χ2分布，即γk～χ2(m)。 m为观测量Zk的维数。
[0069] 在获取检测函数后，根据如下判定准则判断系统是否需要进行修正处理。
[0070] 若γk>TD，判定需要修正处理，
[0071] 若γk≤TD，判定不需要修正处理。
[0072] 在上面的步骤中预先设置的门限TD，其决定了检测的性能。由奈曼-皮尔逊准则可知，当限定误检率Pf＝α时，则由Pf＝P[γk>TD/H0]＝α解出门限TD可使漏检率 Pf＝P[γk≤TD/H1]达到最小，因而TD可由误检率Pf决定。
[0073]
[0074] 本发明中补偿修正处理模块如说明书附图3补偿修正处理模块(位置参数)原理示意图所示按以下方法和步骤进行导航结果Pout(k)、Vout(k)补偿修正处理的：
[0075] 首先将Pout(k)、Vout(k)置初始值0。
[0076] 第二步，计算得到Kalman滤波器的残差
[0077] 第三步，判断γk
[0078] 其中XP(k)＝[δL,δλ,δh]、XV(k)＝[δVE,δVN,δVU]为Kalman滤波位置、速度误差估计值。 即对SINS解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值。
[0079] 第四步，若第三步中，信息评估模块检测出检测函数γk>TD时，这可能是滤波器处在初始阶段或DVL输出信息时出现了丢失、振荡等情况。此时需要对SINS/DVL融合后的位置、速度分以下三种情况进行补偿修正。
[0080] (a)当Pout(k)-Pout(k-1)≥ΔPD时，其中ΔPD为相邻滤波修正时刻位置变化率，则 Pout(k)＝Pout(k-1)+ΔP·Tn，其取值依据为ΔP小于载体实际机动的位置变化率。Tn为滤波周期。
[0081] 同理当Vout(k)-Vout(k-1)≥ΔVD时，有Vout(k)＝Vout(k-1)+ΔV·Tn，其中ΔVD为相邻滤波周期的位置变化率，一般取值较小。Tn为滤波周期。
[0082] (b)当Pout(k)-Pout(k-1)≤ΔPD时则Pout(k)＝Pout(k-1)-ΔP·Tn；
[0083] 同理若Vout(k)-Vout(k-1)≤-ΔVD时则Vout(k)＝Vout(k-1)-ΔV·Tn。
[0084] (c)当|Pout(k)-Pout(k-1)|＜ΔPD，|Vout(k)-Vout(k-1)|＜ΔVD时,则直接使用Kalman滤波的误差估计值进行补偿修正，即
[0085]
[0086] 其中 即对S I N S解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值。
