[发明专利]水下机器人移动式对接目标运动状态估计方法及系统

权利要求书

1.一种水下机器人移动式对接目标运动状态估计方法，应用于自带惯性导航系统和超短基线定位系统的归航AUV上，其特征在于，包括以下步骤：

利用扩展卡尔曼滤波器对所述惯性导航系统测量到的归航AUV的运动状态进行滤波，得到归航AUV的运动状态变化量；

以归航AUV的运动状态变化量为控制向量，利用离散卡尔曼滤波器对超短基线定位系统测量到的运动母艇与归航AUV的相对运动状态进行滤波，得到修正后的运动母艇与归航AUV的相对运动状态；

根据所述惯性导航系统测量到的归航AUV的运动状态与所述修正后的运动母艇与归航AUV的相对运动状态计算运动母艇的实际运动状态。

2.根据权利要求1所述的水下机器人移动式对接目标运动状态估计方法，其特征在于，所述相对运动状态包括运动母艇相对于归航AUV的位置以及运动母艇相对于归航AUV的速度，归航AUV的运动变化量包括速度变化量以及位置变化量；所述离散卡尔曼滤波器的第一状态转移方程为：

其中，m表示超短基线系统对相对运动系统的测量时刻序号，所述相对运动系统由运动母艇与归航AUV构成，ΔL表示运动母艇相对于归航AUV的位置，ΔL_m表示第m时刻运动母艇相对于归航AUV的位置，ΔL_m-1表示第m-1时刻运动母艇相对于归航AUV的位置，ΔP_auv和ΔV_auv分别代表归航AUV自身位置和速度变化量，ΔP_auv|m,m-1代表第m-1时刻至第m时刻内归航AUV的位置变化量，ΔV_auv|m,m-1代表第m-1时刻至第m时刻内归航AUV的速度变化量，V_relative表示运动母艇相对于归航AUV的相对速度，V_relative,m-1表示第m-1时刻运动母艇相对于归航AUV的速度，V_auv，m-1表示第m-1时刻归航AUV的速度，ΔT表示超短基线系统的测量周期的时长。

3.根据权利要求2所述的水下机器人移动式对接目标运动状态估计方法，其特征在于，利用离散卡尔曼滤波器对超短基线定位系统测量到的运动母艇与归航AUV的相对运动状态进行滤波，包括以下步骤：

初始化所述第一状态转移方程的状态变量、控制向量、状态转移矩阵、控制矩阵以及协方差矩阵；

根据m-1时刻预测的状态变量、m-1时刻的控制向量、m-1至m的状态转移矩阵以及控制矩阵预测第一状态转移方程m时刻的一步状态变量；

基于m-1时刻预测的状态转移矩阵、m-1时刻的控制矩阵以及m-1时刻的协方差矩阵计算m-1至m时刻的一步协方差矩阵；

获取超短基线定位系统测量到的运动母艇与归航AUV在m时刻的相对运动状态以及对应的测量噪音矩阵，根据预测的m时刻的一步状态变量、m-1至m时刻的一步协方差矩阵、超短基线定位系统测量到的运动母艇与归航AUV在m时刻的相对运动状态以及对应的测量噪音矩阵计算m时刻的增益；基于m时刻的增益更新m时刻的协方差矩阵，并修正m时刻的一步状态变量，得到修正后的运动母艇在m时刻与归航AUV的相对运动状态。