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

说明书

本发明公开了水下机器人移动式对接目标运动状态估计方法及系统，利用扩展卡尔曼滤波器对惯性导航系统测量到的归航AUV的运动状态进行滤波，得到归航AUV的运动状态变化量；以归航AUV的运动状态变化量为控制向量，利用离散卡尔曼滤波器对超短基线定位系统测量到的运动母艇与归航AUV的相对运动状态进行滤波，得到修正后的所述相对运动状态；根据所述测量到的归航AUV的运动状态与修正后的相对运动状态计算运动母艇的实际运动状态。本发明用离散卡尔曼滤波器对运动母艇与归航AUV的相对运动状态进行滤波时，将归航AUV当前的状态变化量作为控制向量传递给离散卡尔曼滤波器，能提高运动母艇与归航AUV的相对运动状态的精度，进而提高运动母艇实际运动状态的计算精度。

技术领域

本发明涉及水下机器人移动式对接技术领域，尤其涉及水下机器人移动式对接目标运动状态估计方法及系统。

背景技术

着我国深海科考事业的发展，越来越多的深海探测任务需要水下机器人(Autonomous UnderwaterVehicle，简称AUV)的参与，比如水下侦查、大范围且长航程的工作网络扩展和组建等工作。由于现有的水下通讯能力有限以及AUV自身能源的有限性，AUV在深海执行任务的过程中，在能源系统警告、作业数据容量饱和或者先期任务完成之后，需要与坞站进行对接动作，以此完成AUV与坞站之间的能量交换、数据上传、任务下载等任务。水下移动式对接技术是完成AUV与坞站之间对接的关键技术，在种种深海探测任务中起着至关重要的作用，而AUV在与运动坞站的对接过程中，归航AUV需要对运动母艇的运动状态进行估计，并利用估计出的运动母艇的状态信息(包括运动母艇的位置、速度、姿态)导引AUV完成归航以及对接。

目前现有的水下机器人运动目标状态估计技术是将AUV与母艇视为一个线性运动的相对运动系统，即认为归航AUV与移动母艇之间的相对运动模型式一个匀加速运动模型。因此，目前主流的水下机器人运动目标状态估计方法是利用超短基线定位系统测量周期内归航AUV与运动母艇的相对位置，并通过离散卡尔曼滤波器对上述的线性运动系统进行滤波，估计归航AUV与运动母艇的相对位置差。然而，上述方法由于对系统真实的运动状态做了简化处理，损失了一定的估计精度，无法应用在移动式对接任务中，因为，移动式对接任务不仅需要已知AUV与母艇的相对位置，同时需要已知母艇自身的绝对位置，通过其绝对位置估计运动母艇的姿态。

因此，如何解决设计出能应用在移动式对接任务中水下目标运动状态估计方法，已成为本领域技术人员亟待解决的技术问题。

发明内容

本发明提供了水下机器人移动式对接目标运动状态估计方法及系统，用于解决现有的水下目标运动状态估计方法无法应用在移动式对接任务中的技术问题。

为解决上述技术问题，本发明提出的技术方案为：

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

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

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

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

优选的，所述相对运动状态包括运动母艇相对于归航AUV的位置以及运动母艇相对于归航AUV的速度，归航AUV的运动变化量包括速度变化量以及位置变化量；所述离散卡尔曼滤波器的第一状态转移方程为：