CN113252041A - 一种适用于小型水下机器人的组合导航方法 - Google Patents

Abstract

一种适用于小型水下机器人的组合导航方法，属于水下机器人技术领域。该组合导航方法利用优势互补的北斗卫星定位系统和SINS惯性导航系统，有效解决水下机器人长距离航行时定位误差发散的问题，使用自适应的卡尔曼滤波算法使导航定位更加准确。在导航系统初始化后，水下机器人的上位机实时采集北斗卫星定位模块和SINS惯性导航模块数据，并对卫星定位数据和水下机器人初始位置的计算结果实时地进行有效性检验，最后通过北斗卫星定位数据对惯性导航的位置信息进行在线修正。本发明利用简单硬件设备，对北斗卫星定位信号和惯性导航原始输出结果的有效性进行检测；装置简单、稳定可靠，导航修正结果实时、准确，可广泛用于小型水下机器人导航定位领域。

Description

一种适用于小型水下机器人的组合导航方法

技术领域

本发明属于水下机器人技术领域，涉及一种适用于小型水下机器人的组合导航方法。

背景技术

水下机器人工作范围广，具有较大的航程，在科研调查中发挥着日益重要的作用，实时、精确、廉价的导航定位方法是目前水下自主航行器技术发展的重点，但是机器人内部的惯性导航模块的导航误差随时间积累，长时间航行时误差发散使导航失败，需要及时的利用其它导航方式组成组合导航对水下机器人位置进行修正，保证定位精度。我国自主研制的北斗导航定位系统具有较高的定位精度并可全天候实时地提供北斗定位信息，北斗定位信息与可连续输出位姿信息SINS惯性导航系统具有高度的优势互补性，但电磁波在水下衰减剧烈极大地限制了卫星定位系统在水下自主航行器上的运用。小型水下机器人工作深度有限，提供了使用卫星定位信息组成组合导航系统的可能，且传统的声学定位如长、短基线，设备成本高，铺设过程复杂，给小型水下自主航行器定位大材小用，同时随着电子技术的发展，高集成度、模块化的惯性导航设备和北斗卫星接收机设备，使水下机器人有了使用尽可能简单的硬件连接，达到可靠的导航定位的可能。

本发明使用模块化的捷联惯性导航和北斗卫星定位设备，提供了一种硬件设备简单，可对卫星信号和惯性导航原始输出结果进行检测的组合导航方法，可大大降低小型水下自主航行器的导航定位成本，具有较高的应用前景。

发明内容

本发明是利用优势互补的北斗卫星定位系统和SINS惯性导航系统，可有效解决水下机器人长距离航行时定位误差发散的问题，并使用一种自适应的卡尔曼滤波算法，使导航定位更加准确。针对小型水下机器人电子舱容量有效的特点，本发明尽可能地使用了少地、集成度高的硬件设备；由于水对电磁波的影响，利用北斗卫星定位信息前必须对卫星定位信息进行有效性检验，本发明利用卫星通讯协议的有效信息，提供了一种简单高效的判断方法，能同时对北斗定位信息和惯性导航原始输出结果进行有效性判断；针对水下自主航行器复杂的工作环境，提供了一种自适应的组合导航算法。

本发明为实现上述目的所采取的技术方案是：

本发明提供一种组合导航方法，设备包括：无源天线、北斗导航模块、捷联惯性导航模块、导航计算机和深度计。所述无源天线直接与北斗导航模块相连，安装在水下机器人壳体外部，开启后将接收到的北斗定位系统发出的北斗导航电文信息传输至北斗导航模块。所述北斗导航模块根据导航电文计算出水下机器人的位置、速度和航向并传输给导航计算机。所述捷联惯性导航模块包括陀螺仪、加速度计、磁力计和捷联惯性导航芯片，实时传输水下机器人的各项运动参数，包括三维位移、三维速度、姿态角和航向角，并且通过捷联惯性导航芯片的积分运算输出水下机器人的三维速度和三维位移。所述深度计实时传输水下机器人深度给导航计算机导航。所述计算机按固定周期采集北斗导航模块、捷联惯性导航模块和深度计输出的信息，实时地分析并计算出水下机器人当前的位姿。所述水下机器人组合导航方法包括如下步骤：

第一步，水下机器人下水后，以北斗卫星导航数据对捷联惯性导航模块进行初始化。

1.1)水下机器人静止或运动过程中，捷联惯性导航模块完成姿态初始化；

1.2)等待导航计算机收到有效的北斗卫星定位数据，并确保卫星定位达到一定精度(GPGGA协议中，可用卫星数不小于4颗且HDOP小于10)，以卫星定位数据完成捷联惯性导航模块的位置、速度、航向的初始化；

1.3)初始化完成后，导航计算机储存捷联惯性导航模块的姿态、位置、速度、航向的初始化信息，作为捷联惯性导航模块的状态量初始值。

若卫星定位精度达不到上述要求，导航计算机不再利用卫星定位数据对惯性导航模块进行初始化，而是读取卫星定位结果。

第二步，初始化完成后，导航计算机对北斗导航模块、捷联惯性导航模块和深度计进行实时数据采集：

2.1)北斗导航模块按固定周期接受一次北斗卫星定位数据，输出此刻的运动参数给导航计算机，其中周期在每5～30秒之间。

2.2)导航计算机按固定周期采集一次捷联惯性导航模块数据，实时计算水下机器人位姿信息，其中周期在每0.2～2秒间。

2.3)导航计算机按固定周期采集一次深度计数据，周期与北斗导航模块一致。

第三步，导航计算机根据采集的捷联惯性导航数据，实时解算水下机器人一步预测状态。

3.1)导航计算机提取上一步组合导航结果，建立水下机器人初始状态矢量：

其中，初始位置矢量P₀＝[P_e0,P_n0,P_u0]^T为上一次组合导航位置解算结果，且当导航计算机进行第一次导航解算时，P_e0,P_n0,P_u0分别为存储的捷联惯性导航模块的三维位置量初始值，速度矢量v＝[v_e，v_n，v_u]，角速度矢量a＝[a_x，a_y，a_z]和航向角

均为t时刻捷联惯性导航模块输出数据；

选取东北天坐标系作为导航坐标系，用下标e,n,u表示；舰体坐标系o-xyz定义为：ox轴沿水下机器人纵轴，oy轴平行于基平面与ox轴垂直且指向舰体右舷，oz轴与ox轴和oy轴组成右手坐标系；利用欧拉角得到舰体系b到导航坐标系h的姿态转移矩阵和由捷联惯性导航输出的导航坐标系下速度矢量P₀：

R_rpy×^BP₀＝P₀

其中，^BP₀＝[v_x，v_y，v_u]为捷联惯性导航t时刻输出的速度矢量；R_rpy为姿态转换矩阵；γ为t时刻捷联惯性导航模块输出的偏航角；ρ为t时刻捷联惯性导航模块输出的俯仰角；r为t时刻捷联惯性导航模块输出的横滚角。

3.2)建立一步预测状态模型：

X_k|k-1＝FX_K-1+GW_k

其中，F为状态系数阵，由捷联惯导系统的运动微分方程确定；G为10行6列的噪音系数矩阵；白噪音误差矩阵W_k＝[w_gx，w_gy，w_gz，w_ax，w_ay，w_az]^T，w_gx，w_gy，w_gz分别为X轴、Y轴及Z轴陀螺仪白噪声误差，w_ax，w_ay，w_az分别为X轴、Y轴及Z轴加速度计白噪声误差，且满足cov(W_k)＝Q，Q为系统状态噪音协方差矩阵；X_K-1为初始状态矢量；X_k|k-1为一步预测状态矢量。

第四步，判断北斗定位信息是否有效。

4.1)导航计算机通过串口实时读取北斗导航模块信号，按固定格式从缓存中读取水下机器人的水平方向上的位置、三维速度和航向角信息；

4.2)北斗导航模块实时检索北斗定位数据，提取当前可见卫星数(记为nembei)和水平精度因子(记为HDOP)作为判断有效位；如果nembei大于4且HDOP<20成立，则认为北斗定位信息有效，否则认为北斗定位信息无效；

4.3)如果北斗定位信息无效，则舍弃此次卫星定位数据，直接将计算的一步预测状态矢量X_k|k-1作为水下机器人当前导航计算结果，并返回第二步；如果北斗定位信息有效，则进入第五步。

第五步，判断计算的水下机器人位置是否有效。

5.1)提取本次北斗卫星定位信息中的水平面坐标(P_eBDS，P_nBDS)与推算航位求差后取平方根误差，即：

其中，P_eBDS为北斗卫星定位信息中的经度；P_nBDS P_nBDS为北斗卫星定位信息中的纬度；P_e、P_n为一步预测矩阵X_k|k-1中的水平位置信息，其中P_e为一步预测矩阵X_k|k-1的经度信息，P_n为一步预测矩阵X_k|k-1的纬度信息；

如果e<threshold，则判定当前初始位置有效，否则判定当前初始位置无效，上式中，threshold一般取2～5倍的HDOP；P_e，P_n为一步预测矩阵X_k|k-1的水平位置；

5.2)如果初始位置无效，则返回第一步重新对捷联惯性导航模块进行初始化；如果初始位置有效，则进入第六步。

第六步，导航计算机利用自适应的组合导航算法修正一步预测矢量。

考虑到在复杂的水文条件下，北斗卫星定位信息将受到外部环境干扰使得在某些时刻的观测误差过大，且外部环境的噪声干扰有时不再是简单的高斯白噪声，故引入新息序列

作为观测噪声矩阵R_k的更新标准，设计了一种自适应的组合导航算法修正一步预测矢量。规定：P_k为X_k的预测均方误差矩阵，P_k|k-1为P_k的一步预测均方误差矩阵；Z_k为导航计算机采集的北斗定位信息和深度计数据组成的量测矩阵；H_k为量测转移矩阵，K_k为卡尔曼增益，V_k为量测噪音矩阵。具体步骤为：

6.1)建立一步预测均方误差方程：

P_k|k-1＝FP_k-1F^T+Q X_k|k-1＝FX_K-1+GW_k

其中，P_k|k-1为一步预测均方误差矩阵；P_k-1为上一次组合导航计算出的预测均方误差矩阵，P₀由捷联惯性导航相关误差特性确定；Q为系统状态噪音协方差矩阵；

6.2)利用新息序列更新观测噪声矩阵：

6.2.1)建立量测误差方程：

其中，

v_eBDS、v_nBDS、v_uBDS、

为本次北斗定位信息中的三维速度与航向角；P_dep为深度计输出值；H_k＝[I_7×7 0_3×7]；

6.2.2)利用新息序列一步更新观测噪声矩阵：

R_k＝C_k-H_kP_k|k-1H_k ^T

其中，C_k为新息序列；R_k为观测噪声矩阵；

6.2.3)引入阈值因子检验新息序列质量：

定义加权矩阵D_k＝[η_k1，η_k2，η_k3，η_k4，η_k5，η_k6，η_k7]^T，其中：

6.2.3)修正观测噪声矩阵：

R_k＝D_KR_kD_K ^T

6.3)更新卡尔曼滤波增益：

6.4)更新预测均方误差矩阵：

P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T

6.5)建立状态最优预测方程：

由上述状态最优预测方程，计算得出本次组合导航的解算结果X_k，提取X_k中的三维位置信息P_e，P_n，P_u作为当前水下机器人预测的位置，提取X_k中的三维速度信息v_e，v_n，v_u作为当前水下机器人预测的速度，可有效抑制惯性导航系统在水下机器人长时间航行时误差发散的问题，提高导航的精度。

本发明具有以下优点及有益效果：

(1)使用的硬件设备少，连接简单。本发明装置仅包括无源天线、北斗导航模块，INS惯性导航模块、深度计和导航计算机，利用模块化、高集成度的小型电子设备，避免了使用大型、复杂设备带来的巨量成本和困难的铺设条件。

(2)导航修正结果可靠、准确。本发明使用一种自适应的卡尔曼滤波算法作为北斗卫星定位信息与捷联惯性导航模块输出信息的融合算法，不仅实时性强，而且发挥了北斗卫星导航和SINS惯性导航优点的高度互补性，保证水下机器人精确的导航定位，且能够同时对北斗卫星定位信号和捷联惯性导航原始输出结果进行有效性检验，保证了组合导航输出结果的可靠性。

(3)可广泛用于小型水下机器人导航定位领域。本发明运用自适应的卡尔曼滤波算法，融合北斗定位数据、深度计输出的深度和SINS惯性导航数据进行组合导航，实时对水下机器人的位置信息进行修正，可广泛用于小型水下机器人导航定位领域。

附图说明

图1是本发明组合导航系统的数据传输图；

图2是本发明组合导航系统的算法计算流程图；

图3是自适应的卡尔曼滤波算法的流程图。

具体实施方式

下面结合附图及具体实例对本发明做进一步的详细说明。

本发明装置由无源天线、北斗导航模块，INS惯性导航模块、深度计和导航计算机组成，组合导航的数据传输关系如图1所示。

本发明的水下机器人组合导航系统的算法计算流程如图2所示，具体包括如下步骤：

第一步，水下机器人下水后，以北斗卫星导航数据对捷联惯性导航模块进行初始化。

进一步的，所述第一步北斗卫星导航数据对捷联惯性导航模块进行初始化过程，包括以下具体步骤：

1.1)水下机器人静止或运动过程中，捷联惯性导航模块完成姿态初始化；

1.2)等待导航计算机收到有效的北斗卫星定位数据，并确保卫星定位达到一定精度(GPGGA协议中，可用卫星数不小于4颗且HDOP小于10)，以卫星定位数据完成捷联惯性导航模块的位置、速度、航向的初始化。若卫星定位精度不满足要求，导航计算机不再利用卫星定位数据对惯性导航模块进行初始化，而是读取卫星定位结果。

1.3)初始化完成后，导航计算机储存捷联惯性导航模块的姿态、位置、速度、航向的初始化信息，作为捷联惯性导航模块的状态量初始值。

若卫星定位精度达不到上述要求，导航计算机不再利用卫星定位数据对惯性导航模块进行初始化，而是读取卫星定位结果。

本实例中默认卫星定位精度满足要求，卫星定位初始化完成后导航计算机进入状态“A”，启动利用捷联惯性导航模块的输出数据解算水下机器人位姿信息的程序。

第二步，初始化完成后，导航计算机对北斗导航模块、捷联惯性导航模块和深度计进行实时数据采集。

2.1)所述北斗导航模块按固定周期接受一次北斗卫星定位数据，输出此刻的运动参数给导航计算机，周期在每5～30秒之间。

2.2)所述导航计算机按固定周期采集一次捷联惯性导航模块数据，实时计算水下机器人位姿信息，周期在每0.2～2秒间。

2.3)所述导航计算机按固定周期采集一次深度计数据，周期与北斗导航模块一致。

本实施选择捷联惯性导航模块的数据采集周期

为0.5秒，北斗导航模块和深度计的数据采集周期均为

为5秒。

第三步，导航计算机根据采集的捷联惯性导航数据，实时解算水下机器人一步预测状态。

进一步的，所述第三步计算水下机器人初始位置包括以下步骤：

3.1)导航计算机提取上一步组合导航结果，建立水下机器人初始状态矢量：

其中，初始位置矢量P₀＝[P_e0,P_n0,P_u0]^T为上一次组合导航位置解算结果，且当导航计算机进行第一次导航解算时，P_e0,P_n0,P_u0分别为存储的捷联惯性导航模块的三维位置量初始；速度矢量v＝[v_e，v_n，v_u]，角速度矢量a＝[a_x，a_y，a_z]和航向角

均为t时刻捷联惯性导航模块输出数据；

选取东北天坐标系作为导航坐标系，用下标e,n,u表示；舰体坐标系o-xyz定义为：ox轴沿水下机器人纵轴，oy轴平行于基平面与ox轴垂直且指向舰体右舷，oz轴与ox轴和oy轴组成右手坐标系；利用欧拉角得到舰体系b到导航坐标系h的姿态转移矩阵和由捷联惯性导航输出的导航坐标系下速度矢量P₀：

R_rpy×^BP₀＝P₀

其中，^BP₀＝[v_x，v_y，v_u]为捷联惯性导航t时刻输出的速度矢量；R_rpy为姿态转换矩阵；γ为t时刻捷联惯性导航模块输出的偏航角；ρ为t时刻捷联惯性导航模块输出的俯仰角；r为t时刻捷联惯性导航模块输出的横滚角。

本实例初始时，取P_e0＝25,P_n0＝45,P_u0＝0；v_e，v_n，v_u，a_x，a_y，a_z，

均为0，则X₁＝[25，45，0，0，0，0，0，0，0，0^T]。

3.2)建立一步预测状态模型：

X_k|k-1＝FX_K-1+GW_k

其中，F为状态系数阵，由捷联惯导系统的运动微分方程确定；G为10行6列的噪音系数矩阵；白噪音误差矩阵W_k＝[w_gx，w_gy，w_gz，w_ax，w_ay，w_az]^T，w_gx，w_gy，w_gz分别为X轴、Y轴及Z轴陀螺仪白噪声误差，w_ax，w_ay，w_az分别为X轴、Y轴及Z轴加速度计白噪声误差，且满足cov(W_k)＝Q，Q为系统状态噪音协方差矩阵；X_K-1为初始状态矢量；X_k|k-1为一步预测状态矢量。

第四步，判断北斗定位信息是否有效。

4.1)导航计算机通过串口实时读取北斗导航模块信号，按固定格式从缓存中读取水下机器人的水平方向上的位置、三维速度和航向角信息；

4.2)北斗导航模块实时检索北斗定位数据，提取当前可见卫星数(记为nembei)和水平精度因子(记为HDOP)作为判断有效位；如果nembei大于4且HDOP<20成立，则认为北斗定位信息有效，否则认为北斗定位信息无效；

4.3)如果北斗定位信息无效，则舍弃此次卫星定位数据，直接将计算的一步预测状态矢量X_k|k-1作为水下机器人当前导航计算结果，此时导航计算机进入状态“B”，表示本次导航解算结束，并返回第二步；如果北斗定位信息有效，则进入第五步。

第五步，判断计算的水下机器人位置是否有效。

5.1)提取本次北斗卫星定位信息中的水平面坐标(P_eBDS，P_nBDS)与推算航位求差后取平方根误差，即：

其中，P_eBDS为北斗卫星定位信息中的经度；P_nBDS为北斗卫星定位信息中的纬度；P_e、P_n为一步预测矩阵X_k|k-1中的水平位置信息，其中P_e为一步预测矩阵X_k|k-1的经度信息，P_n为一步预测矩阵X_k|k-1的纬度信息；

如果e<threshold，则判定当前初始位置有效，否则判定当前初始位置无效，上式中，threshold一般取2～5倍的HDOP，本优选实施方式取threshold为3倍的HDOP；P_e，P_n为一步预测矩阵X_k|k-1的水平位置；

5.2)如果初始位置无效，则返回第一步重新对捷联惯性导航模块进行初始化，；如果初始位置有效，则进入第六步，且导航计算机进入状态“C”，开启自适应的组合导航模式。

第六步，导航计算机利用自适应的组合导航算法修正一步预测矢量。

考虑到在复杂的水文条件下，北斗卫星定位信息将受到外部环境干扰使得在某些时刻的观测误差过大，且外部环境的噪声干扰有时不再是简单的高斯白噪声，故引入新息序列

作为观测噪声矩阵R_k的更新标准，设计了一种自适应的组合导航算法修正一步预测矢量。规定：P_k为X_k的预测均方误差矩阵，P_k|k-1为P_k的一步预测均方误差矩阵；Z_k为导航计算机采集的北斗定位信息和深度计数据组成的量测矩阵；H_k为量测转移矩阵，K_k为卡尔曼增益，V_k为量测噪音矩阵。具体步骤如图3所示：

6.1)建立一步预测均方误差方程：

P_k|k-1＝FP_k-1F^T+Q X_k|k-1＝FX_K-1+GW_k

其中，P_k|k-1为一步预测均方误差矩阵；P_k-1为上一步组合导航计算出的预测均方误差矩阵，P₀由捷联惯性导航模块的相关误差特性确定；Q为系统状态噪音协方差矩阵；

6.2)利用新息序列更新观测噪声矩阵：

6.2.1)建立量测误差方程：

其中，

v_eBDS、v_nBDS、v_uBDS、

为本次北斗定位信息中的三维速度与航向角；P_dep为深度计输出值；H_k＝[I_7×7 0_3×7]；

6.2.2)利用新息序列一步更新观测噪声矩阵：

R_k＝C_k-H_kP_k|k-1H_k ^T

其中，C_k为新息序列；R_k为观测噪声矩阵；

6.2.3)引入阈值因子检验新息序列质量：

定义加权矩阵D_k＝[η_k1，η_k2，η_k3，η_k4，η_k5，η_k6，η_k7]^T，其中：

6.2.3)修正观测噪声矩阵：

R_k＝D_KR_kD_K ^T

6.3)更新卡尔曼滤波增益：

6.4)更新预测均方误差矩阵：

P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T

6.5)建立状态最优预测方程：

导航计算机提取X_k中的三维位置信息P_e，P_n，P_u作为当前水下机器人预测的位置，提取X_k中的三维速度信息v_e，v_n，v_u作为当前水下机器人预测的速度，本次组合导航解算结束，返回到状态“A”，准备下一次解算导航信息。

以上所述实施例仅表达本发明的优选实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些均属于本发明专利的保护范围。

Claims (4)

1.一种适用于小型水下机器人的组合导航方法，其特征在于，所述的水下机器人组合导航方法基于无缘天线、北斗导航模块、捷联惯性导航模块、导航计算机和深度计实现；所述无缘天线直接与北斗导航模块相连，用于将北斗定位系统发出的北斗导航电文信息传输至北斗导航模块；所述北斗导航模块根据导航电文计算出水下机器人的位置、速度和航向并传输给导航计算机；所述捷联惯性导航模块用于实时传输水下机器人的各项运动参数，并通过捷联惯性导航芯片的积分运算输出水下机器人的三维速度和三维位移；所述深度计实时传输水下机器人深度给导航计算机导航；所述计算机按固定周期采集信息，计算水下机器人当前的位姿；所述导航方法包括如下步骤：

第一步，水下机器人下水后，以北斗卫星导航数据对捷联惯性导航模块进行初始化；

1.1)水下机器人静止或运动过程中，捷联惯性导航模块完成姿态初始化；

1.2)等待导航计算机收到有效的北斗卫星定位数据，并确保卫星定位达到一定精度，以卫星定位数据完成捷联惯性导航模块的位置、速度、航向的初始化；

1.3)初始化完成后，导航计算机储存捷联惯性导航模块的姿态、位置、速度、航向的初始化信息，作为捷联惯性导航模块的状态量初始值；若卫星定位精度达不到上述精度要求，导航计算机不再利用卫星定位数据对惯性导航模块进行初始化，而是读取卫星定位结果；

第二步，初始化完成后，导航计算机对北斗导航模块、捷联惯性导航模块和深度计进行实时数据采集：

2.1)北斗导航模块按固定周期接受一次北斗卫星定位数据，输出此刻的运动参数给导航计算机；

2.2)导航计算机按固定周期采集一次捷联惯性导航模块数据，实时计算水下机器人位姿信息；

2.3)导航计算机按固定周期采集一次深度计数据，周期与北斗导航模块一致；

第三步，导航计算机根据采集的捷联惯性导航数据，实时解算水下机器人一步预测状态；

3.1)导航计算机提取上一步组合导航结果，建立水下机器人初始状态矢量：

其中，初始位置矢量P₀＝[P_e0,P_n0,P_u0]^T为上一次组合导航位置解算结果，且当导航计算机进行第一次导航解算时，P_e0,P_n0,P_u0分别为存储的捷联惯性导航模块的三维位置量初始值，速度矢量v＝[v_e，v_n，v_u]，角速度矢量a＝[a_x，a_y，a_z]和航向角

均为t时刻捷联惯性导航模块输出数据；

选取东北天坐标系作为导航坐标系，用下标e,n,u表示；舰体坐标系o-xyz定义为：ox轴沿水下机器人纵轴，oy轴平行于基平面与ox轴垂直且指向舰体右舷，oz轴与ox轴和oy轴组成右手坐标系；利用欧拉角得到舰体系b到导航坐标系h的姿态转移矩阵和由捷联惯性导航输出的导航坐标系下速度矢量P₀：

R_rpy×^BP₀＝P₀

其中，^BP₀＝[v_x，v_y，v_u]为捷联惯性导航t时刻输出的速度矢量；R_rpy为姿态转换矩阵；γ为t时刻捷联惯性导航模块输出的偏航角；ρ为t时刻捷联惯性导航模块输出的俯仰角；r为t时刻捷联惯性导航模块输出的横滚角；

3.2)建立一步预测状态模型：

X_k|k-1＝FX_K-1+GW_k

其中，F为状态系数阵，由捷联惯导系统的运动微分方程确定；G为噪音系数矩阵；白噪音误差矩阵W_k＝[w_gx，w_gy，w_gz，w_ax，w_ay，w_az]^T，w_gx，w_gy，w_gz分别为X轴、Y轴及Z轴陀螺仪白噪声误差，w_ax，w_ay，w_az分别为X轴、Y轴及Z轴加速度计白噪声误差，且满足cov(W_k)＝Q，Q为系统状态噪音协方差矩阵；X_K-1为初始状态矢量；X_k|k-1为一步预测状态矢量；

第四步，判断北斗定位信息是否有效；

4.1)导航计算机通过串口实时读取北斗导航模块信号，按固定格式从缓存中读取水下机器人的水平方向上的位置、三维速度和航向角信息；

4.2)北斗导航模块实时检索北斗定位数据，提取当前可见卫星数和水平精度因子作为判断有效位，其中当前可见卫星数记为nembei，水平精度因子记为HDOP；如果nembei大于4且HDOP<20成立，则认为北斗定位信息有效，否则认为北斗定位信息无效；

4.3)如果北斗定位信息无效，舍弃此次卫星定位数据，将计算的一步预测状态矢量X_k|k-1作为水下机器人当前导航计算结果，并返回第二步；如果北斗定位信息有效，则进入第五步；

第五步，判断计算的水下机器人位置是否有效；

5.1)提取本次北斗卫星定位信息中的水平面坐标(P_eBDS，P_nBDS)与推算航位求差后取平方根误差，即：

其中，P_eBDS为北斗卫星定位信息中的经度；P_nBDS P_nBDS为北斗卫星定位信息中的纬度；P_e、P_n为一步预测矩阵X_k|k-1中的水平位置信息，其中P_e为一步预测矩阵X_k|k-1的经度信息，P_n为一步预测矩阵X_k|k-1的纬度信息；

如果e<threshold，则判定当前初始位置有效，否则判定当前初始位置无效，threshold取2～5倍的HDOP；P_e，P_n为预测矩阵X_k|k-1的水平位置；

5.2)如果初始位置无效，则返回第一步重新对捷联惯性导航模块进行初始化；如果初始位置有效，则进入第六步；

第六步，导航计算机利用自适应的组合导航算法修正一步预测矢量；

引入新息序列

作为观测噪声矩阵R_k的更新标准，设计一种自适应的组合导航算法修正一步预测矢量；规定：P_k为X_k的预测均方误差矩阵，P_k|k-1为P_k的一步预测均方误差矩阵；Z_k为导航计算机采集的北斗定位信息和深度计数据组成的量测矩阵；H_k为量测转移矩阵，K_k为卡尔曼增益，V_k为量测噪音矩阵；具体步骤为：

6.1)建立一步预测均方误差方程：

P_k|k-1＝FP_k-1F^T+Q X_k|k-1＝FX_K-1+GW_k

其中，P_k|k-1为一步预测均方误差矩阵；P_k-1为上一次组合导航计算出的预测均方误差矩阵，P₀由捷联惯性导航相关误差特性确定；Q为系统状态噪音协方差矩阵；

6.2)利用新息序列更新观测噪声矩阵：

6.2.1)建立量测误差方程：

其中，

v_eBDS、v_nBDS、v_uBDS、

为本次北斗定位信息中的三维速度与航向角；P_dep为深度计输出值；H_k＝[I_7×7 0_3×7]；

6.2.2)利用新息序列一步更新观测噪声矩阵：

R_k＝C_k-H_kP_k|k-1H_k ^T

其中，C_k为新息序列；R_k为观测噪声矩阵；

6.2.3)引入阈值因子检验新息序列质量：

定义加权矩阵D_k＝[η_k1，η_k2，η_k3，η_k4，η_k5，η_k6，η_k7]^T，其中：

6.2.3)修正观测噪声矩阵：

R_k＝D_KR_kD_K ^T

6.3)更新卡尔曼滤波增益：

6.4)更新预测均方误差矩阵：

P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T

6.5)建立状态最优预测方程：

由状态最优预测方程计算得出本次组合导航的解算结果X_k，提取X_k中的三维位置信息P_e，P_n，P_u作为当前水下机器人预测的位置，提取X_k中的三维速度信息v_e，v_n，v_u作为当前水下机器人预测的速度，可有效抑制惯性导航系统在水下机器人长时间航行时误差发散的问题，提高导航的精度。

2.根据权利要求1所述的一种适用于小型水下机器人的组合导航方法，其特征在于，所述步骤1.2)中，卫星定位达到一定精度要求为：可用卫星数不小于4颗且HDOP小于10。

3.根据权利要求1所述的一种适用于小型水下机器人的组合导航方法，其特征在于，步骤2.1)的周期在每5～30秒之间。

4.根据权利要求1所述的一种适用于小型水下机器人的组合导航方法，其特征在于，步骤2.2)的周期在每0.2～2秒间。

Images