[发明专利]一种基于M估计的AUV组合导航方法及系统

权利要求书

1.一种基于M估计的AUV组合导航方法，其特征在于，包括以下几个步骤：

步骤一：建立SINS/USBL组合导航模型；

步骤二：对组合导航模型的量测方程进行重构；

步骤三：根据重构后的量测方程，对组合导航模型中的状态变量X求解，获得X的最优估计值；

步骤四：根据步骤三得到的X最优估计值进行校正，从而实现AUV组合导航定位。

2.根据权利要求1所述的基于M估计的AUV组合导航方法，其特征在于：建立SINS/USBL组合导航模型，具体为：

(1.1)选取东北天E-N-U地理坐标系为导航坐标系，选取捷联惯导系统导航参数的误差量作为系统状态量，即选取位置误差[δL δλ δh]、速度误差[δV_E δV_N δV_U]、姿态误差角[φ_Eφ_N φ_U]、沿载体坐标系X-Y-Z轴上的陀螺漂移[ε_x ε_y ε_z]和加速度计零偏作为状态量，建立状态向量如下

(1.2)SINS/USBL组合导航模型的状态方程如下所示：

为X(t)的一阶导数；

系统噪声为：

η(t)＝[n_ax,n_ay,n_az,n_wx,n_wy,n_wz]^T；

其中，n_ax,n_ay,n_az,n_wx,n_wy,n_wz为三轴加速度计和三轴陀螺仪零均值高斯白噪声；

G(t)为噪声驱动矩阵：

其中，T为捷联矩阵，o为零矩阵；

F(t)为状态转移矩阵，把F(t)变成分块矩阵形式如下所示，

其中，F_INS,9×9代表SINS误差方程；

(1.3)将状态方程进行一阶离散化

X_k＝Φ_k/k-1X_k-1+w_k-1

式中：

X_k＝X(t_k)

T_s＝t_k-t_k-1

其中：I为单位阵，w_k为高斯白噪声，即w_k～(0,Q)；Q是指高斯白噪声方差；(1.4)SINS/USBL组合导模型的量测方程如下.

y_k＝H_kX_k+v_k

其中，y_k为量测量，H_k为量测矩阵，v_k为量测噪声；

量测矩阵为：

式中，

其中：R为地球半径，h为高度，L为纬度，λ为经度，e为地球曲率半径，为SINS计算的载体坐标系下应答器的位置信息；

为平台坐标系到载体坐标系转换矩阵，为地球坐标系到平台坐标系转换矩阵。