[发明专利]一种适用于小型水下机器人的组合导航方法

权利要求书

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的姿态转移矩阵和由捷联惯性导航模块t时刻输出的舰体系下速度矢量^BV₀得到导航坐标系下速度矢量V₀：

R_rpy×^BV₀＝V₀

其中，^BV₀＝[v_x，v_y，v_z]^T为捷联惯性导航模块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且HDOP20成立，则认为北斗定位信息有效，否则认为北斗定位信息无效；

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

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

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的纬度信息；

如果ethreshold，则判定当前初始位置有效，否则判定当前初始位置无效，threshold取2～5倍的HDOP；

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+QX_k|k-1＝FX_k-1+GW_k

其中，P_k|k-1为一步预测均方误差矩阵；P_k-1为上一次组合导航计算出的预测均方误差矩阵；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.4)修正观测噪声矩阵：

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作为当前水下机器人预测的速度，可有效抑制惯性导航系统在水下机器人长时间航行时误差发散的问题，提高导航的精度；

其中，卫星定位达到一定精度要求为：可用卫星数不小于4颗且HDOP小于10。