[发明专利]一种复杂环境下SINS/DVL紧组合导航方法

权利要求书

1.一种基于SINS/DVL紧组合的水下航行器导航方法，其特征在于，具体包括以下步骤：

步骤1：根据SINS/DVL系统的采集数据，建立状态方程；

步骤2：根据步骤1中的状态方程建立量测方程；

步骤3：对DVL波束信息缺失处理；

步骤4：DVL波束信息受到洋流影响处理；

步骤5：基于卡尔曼滤波的数据融合处理：根据步骤1和步骤2中的状态方程和量测方程，建立卡尔曼递推估计，将卡尔曼递推估计得到的状态向量反馈到SINS结果中便得到了最终的位置和速度信息；

步骤1中所述的SINS/DVL系统的采集数据，包括：IMU采集的光纤陀螺数据；其中：表示载体坐标系下x轴的角速度；表示载体坐标系下y轴的角速度；表示载体坐标系下z轴的角速度；IMU采集的加速度计数据其中：表示载体系下x轴的加速度值；表示载体系下y轴的加速度值；表示载体系下z轴的加速度值；DVL采集的四个波束通道的数据：其中：表示DVL波束通道1输出的速度信息；表示DVL波束通道2输出的速度信息；表示DVL波束通道3输出的速度信息；表示DVL波束通道4输出的速度信息；

输出量：载体的速度信息：其中：表示导航系下载体x轴的速度值；表示导航系下载体y轴的速度值；表示导航系下载体z轴的速度值；

载体的位置信息：λ,L,h，其中：λ表示地理经度值；L表示地理纬度值；h表示高度值；

步骤1中所述的建立状态方程，具体包括如下步骤；

定义建立的状态方程如下：

其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量，状态向量为21维，表示如下：

其中，[φ_x φ_y φ_z]^T表示俯仰、横滚、航向姿态角误差；表示东向、北向、天向速度误差；[δλ δL δh]^T表示经度误差、纬度误差、高度误差；[▽_x ▽_y ▽_z]^T表示加速度计零偏；[ε_x ε_y ε_z]^T表示陀螺零偏；[b_D1 b_D2 b_D3 b_D4]^T表示DVL四个波束零偏；K_D表示DVL刻度因子；b_PS表示深度计零偏；状态转移矩阵F表示为：

其中，

其中：表示b系到n系的姿态转移矩阵；β表示重力扁率；表示加速度计噪声；表示陀螺噪声；ω_ie表示地球自转角速率；V_E表示东向速度；V_N表示北向速度；V_U表示天向速度；表示导航系下天向加速度；表示导航系下东向加速度；表示导航系下北向加速度；L表示地理纬度；h表示高度；β₁＝(1/8)(2βf+f²)，β₂＝3.08×10^-6s^-2；f表示椭球扁率；