[发明专利]一种基于SINS与DVL组合的AUV行进间快速初始对准方法

权利要求书

1.一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于步骤如下：

步骤1：建立SINS与DVL的数学模型，包括：

(1.1)建立SINS的状态方程；

(1.2)建立SINS的误差微分方程；

(1.3)建立DVL的误差模型；

(1.4)建立SINS/DVL的量测方程；

步骤2：根据步骤1建立的SINS与DVL的数学模型，建立SINS/DVL的微分更新方程，包括：

(2.1)建立姿态微分更新方程；

(2.2)建立速度微分更新方程；

(2.3)建立位置微分更新方程；

步骤3：对姿态、速度和位置更新微分方程进行离散化处理，得到离散化更新方程；

步骤4：根据步骤3得到的离散化更新方程，建立逆向解算的离散化更新方程；

步骤5：对步骤3和步骤4得到的离散化更新方程进行卡尔曼滤波估计；

步骤6：正向与逆向导航数据交替解算，实现基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准。

2.根据权利要求1所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述建立SINS状态方程，具体为：

其中，X为SINS系统的状态向量，为X的微分形式，F为SINS的状态矩阵，系统噪声W为零均值高斯白噪声；选取地理坐标系为参考系，并选取位置误差、速度误差、姿态误差、陀螺仪漂移与加速计零偏为状态量，则状态向量X为：

其中，分别为北向、天向和东向失准角；δV_N、δV_U、δV_E分别为北向、天向和东向速度误差；δL、δλ分别为纬度和经度误差；ε_N、ε_U、ε_E分别为陀螺仪在北东天方向上的陀螺漂移；分别为加速度计北东天方向上的加速度计偏置，且状态矩阵F表示如下：

其中，状态矩阵F内的各变量表示如下：

上式中，R_M,R_N分别为载体所在位置地球子午圈和卯酉圈曲率半径，近似计算公式为：R_M≈R_e(1-2e+3esin²L)与R_N≈R_e(1+esin²L)；R_e为地球参考椭球的长半轴，e为参考椭球的扁率；V_N、V_U、V_E为AUV在北天东方向上的速度，L和h分别表示载体的经度与深度；ω_ie为地球自转速率；f_N、f_U、f_E分别为载体在北天东方向上的比力；为AUV坐标系b到地理坐标系n的状态转移矩阵

其中，φ、ψ、θ分别表示AUV的横滚角、航向角与偏航角。

3.根据权利要求2所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述步骤(1.2)建立的SINS的误差微分方程，具体为：

4.根据权利要求3所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述步骤(1.3)建立的DVL误差模型，具体为：

δV^b＝ΔC·V^b+u；

其中，δV^b和V^b分别表示AUV坐标系下的测速误差与AUV的航行速度；u为量测噪声；ΔC为刻度系数误差；

SINS/DVL组合导航方式下，DVL坐标系与AUV坐标系间的速度关系表示为

其中，为DVL坐标系d系到AUV坐标系b系之间的转换矩阵；V^d表示DVL坐标系下测得的速度。