[发明专利]一种基于改进的PSO-ANFIS辅助的SINS/DVL紧组合导航方法

权利要求书

1.一种基于改进的PSO-ANFIS辅助的SINS/DVL紧组合导航方法，其特征在于，该方法包括以下步骤：

步骤1：根据系统误差方程建立SINS/DVL组合导航系统的状态方程，根据SINS解算的导航信息计算的伪量测信息和DVL测量的四波束速度信息、深度计测量的深度信息之差作为量测量建立SINS/DVL组合导航系统的量测方程；

步骤2：控制AUV的潜行深度到达水面附近，完成导航定位系统的初始对准后，借助GNSS信息和变分贝叶斯卡尔曼滤波算法辅助收集包含各种异常测量类型的样本数据，用于训练ANFIS；

步骤3：将步骤2中收集到的样本数据标准化，并通过粒子群优化算法对样本数据进行处理实现ANFIS模型参数寻优，以此完成ANFIS模型的训练过程；

步骤4：控制AUV潜入水下，采用改进的PSO-ANFIS算法对DVL的四波束绝对误差进行预测，其中需要在线获取特征信息；

步骤5：基于ANFIS预测结果，监测误差的特征变化，通过异常判别机制对DVL的实际测量进行补偿，并将补偿后的DVL量测量与SINS计算的伪观测量进行卡尔曼滤波。

2.根据权利要求1所述的基于改进的PSO-ANFIS辅助的SINS/DVL紧组合导航方法，其特征在于，步骤1中所述建立SINS/DVL紧组合导航系统的状态方程和量测方程，具体过程如下：

步骤1.1对需要用到的坐标系进行定义：

e——地球坐标系：与地球固联，原点位于地心，x_e轴穿越本初子午线与赤道交点，z_e轴指向北极，y_e轴x_e、z_e构成右手坐标系；n——与东-北-天地理坐标系重合的导航坐标系；b——载体坐标系：原点位于运载体中心，z_b轴垂直运载体向上，x_b指向运载体前方，y_b与x_b、z_b构成右手坐标系；d——与DVL的波束中心对齐的正交坐标系，在这里表示beam系；

步骤1.2建立SINS/DVL紧组合导航系统的状态方程，具体步骤如下：

取姿态误差角φ＝[φ_x φ_y φ_z]、速度误差δV＝[δV_E δV_N δV_U]位置误差δP＝[δL δλ δh]、陀螺常值漂移ε和加速度计随机常值误差作为SINS系统的状态量，记为：

其中，φ_x是东向失准角，φ_y是北向失准角，φ_z是天向失准角；δV_E是东向速度误差，δV_N是北向速度误差，δV_U是天向速度误差；δL是纬度误差，δλ是经度误差，δh是高度误差；ε_x是x向陀螺漂移，ε_y是y向陀螺漂移，ε_z是z向陀螺漂移；是x向加速度计随机常值误差，是y向加速度计随机常值误差，是z向加速度计随机常值误差；SINS系统的噪声：

W_SINS＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

其中，ω_g为陀螺的过程噪声矢量，ω_a为加速度计的过程噪声矢量；取DVL四波束速度零偏δb＝[δb₁ δb₂ δb₃ δb₄]、刻度系数误差δk作为DVL系统状态变量，记为：

X_DVL＝[δb₁ δb₂ δb₃ δb₄ δk]^T

其中，δb₁是beaml速度零偏，δb₂是beam2速度零偏，δb₃是beam3速度零偏，δb₄是beam4速度零偏；

DVL系统的噪声为ω_d；取深度计的偏置δb_ps作为深度计的状态变量：

X_PS＝δb_ps

深度计系统的噪声为ω_ps；

组合导航系统的状态量可以表示为：

X＝[X_SINS X_DVL X_PS]^T

由导航系统的误差模型可以得到状态方程：

其中，F是状态转移矩阵，W是系统噪声；具体F_SINS推导这里不详细展开，很多文献中都有推导过程，

步骤1.3建立SINS/DVL紧组合导航系统的量测方程，具体步骤如下：

在忽略传感器误差的情况下，速度定义如下：

其中，是n系下SINS的速度，是b系下SINS的速度，是beam系下的SINS速度，是DVL测量的速度；它们之间有这样的关系：

其中，表示由n系到b系的转换矩阵，表示由b系到beam系的转换矩阵，其具体表示如下式：

其中b_i是根据DVL波束与AUV的几何关系，从V^b到V^d的方向矢量，可以表示为：

其中α是DVL的波束倾角，是DVL固定的特性；可以表示为其中用于“+”配置的DVL，而用于“×”配置的DVL；

由SINS、DVL和深度计形成的量测量：

其中，是SINS解算出的深度信息，是深度计测量的深度信息；定义深度计的量测误差模型为：

其中，H_PS为深度真值；根据上述分析，SINS解算的计算公式为：

定义DVL的量测误差模型为：

根据上述分析，将SINS解算速度转换到beam系得到的计算公式为：

其中，[·×]表示叉乘运算；由此可以得到组合导航系统的量测方程：

Z＝HX+V

其中，

V＝[ω_d ω_ps]^T。