[发明专利]一种基于格网水流速度辅助的惯性/多普勒中水层导航方法

权利要求书

1.一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，包括以下步骤：

S1：基于组合导航系统为SINS/GNSS或者SINS/DVL-BT系统初始化载体姿态、速度、位置信息；对航行区域进行交错格网划分；

S2：获取到有效DVL-WT量测时，根据当前载体解算位置与DVL-WT各水层量测深度判断各DVL-WT量测对应的格网区域；

S3：判断各DVL-WT量测对应格网区域水速是否已被估计，若已估计，利用该层对水测速结果进行量测更新；若未被估计，不进行量测更新，同时，利用组合后结果及当前各DVL-WT量测，进行相应格网水速估计；

S4：在DVL-WT量测之间，进行惯性捷联解算，更新姿态速度位置微分方程，持续获得载体姿态速度位置信息；

S5：重复步骤S2到S4直至组合过程结束。

2.根据权利要求1所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，所述步骤S1具体包括以下过程：

当载体起始于水面时，以SINS/GNSS组合结果作为载体初始状态信息；当载体起始于DVL-BT有效水域时，初以SINS/DVL-BT组合结果作为载体初始状态信息；

对即将航行的水域进行交错格网划分，根据DVL设备对水测速分层特性，将水域从深度0m处从上至下以b_h作为层高进行分层。在一层内，以东向b_E、北向b_N结合层高b_h进行长方体划分，形成连续扩展格网拼接，在不同层之间，下一层相较于上一层，格网交错点向东向移动a_E、北向移动a_N，以此类推，形成交错格网水域划分。

3.根据权利要求1所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，所述步骤S2具体包括以下过程：

获取到有效DVL-WT观测时，不同水层DVL-WT观测所对应的格网号(i，j，k)通过以下步骤确定；

以某一水层为例：假设该水层为当前时刻DVL设备对水测速的第N层，

λ_k＝λ₁+(k-1)a_E

L_k＝L₁+(k-1)a_N

其中为向下取整，λ、L和h为当前载体所在位置的经度、纬度和高度表示，λ_k和L_k为该水域第k层格网的格网号计算起始经纬度，R_M和R_N分别为子午圈与卯酉圈半径。