[发明专利]一种异时序贯量测的改进分层式AUV协同导航定位方法

权利要求书

1.一种异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：所述方法包括以下步骤：

步骤(1)，通过优化分层式协同导航编队的结构来进行AUV协同导航，低精度层的AUV_q通过水声信号测量计算出与高精度层AUV_p之间的相对距离和方位角；同时，低精度层AUV_q接收高精度层AUV_p反馈的自身位置信息；

步骤(2)，以移动长基线定位方法对被定位的AUV进行航位推算，得出被定位的AUV的量测模型；

步骤(3)，采用异时序贯量测方法获得被定位AUV的俯仰角、横滚角以及加速度信息，解决并行滤波在移动长基线系统中产生的定位误差问题；

步骤(4)，通过序贯扩展卡尔曼滤波方法对系统状态进行更新；

步骤(5)，通过多传感器数据融合导航系统进行多传感器数据融合解算以进行导航；

步骤(6)，采用多传感器数据融合系统对滤波进行去噪处理。

2.根据权利要求1所述的异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：步骤(2)中，所述被定位的AUV的量测模型推算过程如下：

所述被定位的AUV对其自身位姿进行推算，所述推算方程为：

x_k+1＝x_k+x_k+1＝x_k+T.V_k.cos(φ_k)

y_k+1＝y_k+y_k+1＝y_k+T.V_k.sin(φ_k)

φ_k+1＝φ_k (1)

其中，T为被定位AUV的采样周期，V_k为k时刻的本体传感器所得的速度，为k时刻的本体传感器所得的航偏角；

令而且相互独立；

由此，被定位的AUV的量测模型为：

其中，为k+1时刻两AUV之间的声波传输距离，X_k为k时刻被定位的AUV的位移姿态，Γ(u_k+w_k，T)为非线性项，w_k为过程噪声，

3.根据权利要求1所述的异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：所述步骤(4)中，所述低精度层AUV接收高精度层AUV的位置信息，通过扩展卡尔曼滤波方法融合内部和外部信息完成定位的过程如下：

设AUV_p的坐标为：(x_p(k)，y_p(k)，z_p(k))，AUV_q的坐标为：(x_q(k)，y_q(k)，z_q(k))，由AUV_p与AUV_q之间的几何关系得以下表达式：

AUV_p与AUV_q之间的相对方位角：

AUV_p与AUV_q之间的水平方向距离：

4.根据权利要求1所述的异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：步骤(4)中，采用扩展卡尔曼滤波方法对系统进行状态更新的步骤如下：

使用传感器量测其被定位的AUV的更新状态，得到被定位的AUV在时刻的状态估计量为其中，为状态估计量；被定位的AUV₄接收到应答信号的时刻为之后通过预测协方差和时刻传感器的量测更新系统状态；

时刻系统的预测协方差为：

其中，为状态转移矩阵：

过程噪声分布矩阵的公式如下：