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

说明书

本发明提供一种基于CKF的主从式多水下无人潜器协同定位方法，具体过程为：(1)主、从UUV进行时钟同步；(2)主UUV通过水声测距测量主、从UUV之间的间距信息；主UUV通过水声设备向从UUV发送自身的定位信息、间距信息；(3)建立协同定位系统的状态方程；(4)建立协同定位系统的量测方程；(5)根据步骤(3)建立的系统状态方程，完成协同定位系统CKF滤波时间更新；(6)根据步骤(4)建立的的系统量测方程，完成协同定位系统CKF滤波量测更新，实现多UUV协同定位。本发明实现利用主UUV的精确相对定位信息和从UUV航推定位信息，提高从UUV的协同定位精度。

技术领域

本发明涉及的是一种协同定位技术，特别是涉及主从式多水下无人潜器(UUV)的协同定位技术，尤其是涉及一种基于CKF(容积卡尔曼滤波)的主从式多水下无人潜器协同定位方法。

背景技术

随着海洋开发技术的发展，单水下无人潜器(UUV)越来越难完成复杂的军事及民事任务，从而使得多UUV协同系统在海洋探索及开发、军事作战等方面拥有越来越重要的作用。定位技术是协同作业的前提条件和关键技术。由于多UUV协同定位具有各UUV独自定位所不具有的多种优势，因此多UUV协同定位正逐步成为一个热门研究课题，设计能够提高多UUV定位精度的协同定位算法具有重要的理论价值和现实意义。

如果多UUV群体在定位时存在着相对观测，那么通过一定的信息交换，就可以实现UUV间定位信息的共享，达到提高多UUV系统整体定位能力的目的，这种定位方法称为“协同定位”。多UUV协同定位具有下列优势：能够充分利用系统中某些UUV的高精度导航信息，从而使得装备低精度导航设备的UUV可以提高自身的导航精度；在多UUV系统中，部分UUV具有有界定位误差的导航能力，通过协同定位实现定位信息共享，可以使得系统中每个UUV都具有误差有界的定位能力；当某些UUV由于传感器或环境因素丧失独立导航能力时，协同定位可以在一定程度上恢复这些平台的定位能力。

发明内容

有鉴于此，本发明提出了一种基于容积卡尔曼滤波的多UUV协同定位方法，实现利用主UUV的精确相对定位信息和从UUV航推定位信息，提高从UUV的协同定位精度。

本发明通过以下步骤实现：

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

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

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

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

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

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

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

具体过程是：

第一步：主、从UUV进行时钟同步，即在航行试验开始之前将主UUV计算机时钟、水声设备时钟、从UUV计算机时钟调整到同一时间，精度准确到0.1秒。

第二步：时钟同步后，主UUV通过水声测距测量主、从UUV之间的间距信息；主UUV通过水声设备向从UUV发送自身在时刻定位信息、间距信息及信息发送时刻的时间戳，定位信息中，x轴平行于地球纬线切线方向，东向为正方向，y轴平行于地球经线切线方向，北向为正方向。

第三步：建立协同定位系统的状态方程；

系统状态量为：