[发明专利]一种基于CKF的主从式多水下无人潜器协同定位方法

权利要求书

1.一种基于CKF的主从式多水下无人潜器协同定位方法，其特征在于，具体过程为：

(1)主、从UUV进行时钟同步；

(2)主UUV通过水声测距测量主、从UUV之间的间距信息；主UUV通过水声设备向从UUV发送自身的定位信息、间距信息；

(3)建立协同定位系统的状态方程；

(4)建立协同定位系统的量测方程；

(5)根据步骤(3)建立的系统状态方程，完成协同定位系统CKF滤波时间更新；

(6)根据步骤(4)建立的的系统量测方程，完成协同定位系统CKF滤波量测更新，实现多UUV协同定位。

2.根据权利要求1所述基于CKF的主从式多水下无人潜器协同定位方法，其特征在于，所述步骤(1)的具体过程为：主、从UUV进行时钟同步，即在航行试验开始之前将主UUV计算机时钟、水声设备时钟、从UUV计算机时钟调整到同一时间，精度准确到0.1秒。

3.根据权利要求2所述基于CKF的主从式多水下无人潜器协同定位方法，其特征在于，所述步骤(2)的具体过程为：时钟同步后，主UUV通过水声测距测量主、从UUV之间的间距信息；主UUV通过水声设备向从UUV发送自身在时刻定位信息、间距信息及信息发送时刻的时间戳，定位信息中，x轴平行于地球纬线切线方向，东向为正方向，y轴平行于地球经线切线方向，北向为正方向。

4.根据权利要求3所述基于CKF的主从式多水下无人潜器协同定位方法，其特征在于，所述步骤(3)的具体过程为：建立协同定位系统的状态方程；

系统状态量为：

X_k＝[X_ak^T X_bk^T]^T＝(x_ak，y_ak，z_ak，x_bk，y_bk，z_ak，φ_bk)^T

其中下标a、b分别代表主UUV和从UUV，下标k代表信息发送时刻T_k。X_k为系统状态量，X_ak为主UUV状态量，X_bk为从UUV状态量，x_ak，y_ak，z_ak为第二步接收的主UUV在T_k时刻的定位信息，x_bk，y_bk，z_ak为从UUV在T_k时刻的航推定位信息，φ_bk为从UUV在T_k时刻的航向角；

系统状态方程为：

X_k＝A_k-1X_k-1+G_k-1W_k-1

其中，X_k-1为T_k-1时刻系统状态量，

为系统矩阵，

其中，δt为系统滤波周期，V_b(k-1)为从UUV在T_k-1时刻的航速，φ_b(k-1)为从UUV在T_k-1时刻的航向角；

为系统噪声驱动阵，W_k-1为系统噪声阵；且有和分别为零均值且相互独立的高斯白噪声，其方差阵分别为

系统噪声方差阵

5.根据权利要求4所述基于CKF的主从式多水下无人潜器协同定位方法，其特征在于，所述步骤(4)的具体过程为：建立协同定位系统量测方程；

系统量测方程为：

其中，Z_k为系统量测量，r_k为为第二步接收的T_k时刻主从UUV间距，v_k为系统量测噪声。