[发明专利]一种基于旋转矩阵的因子图协同定位方法

说明书

本发提供了了一种基于旋转矩阵的因子图协同定位方法：首先构建基于旋转矩阵的协同定位方法因子图模型，在因子图方法流程中加入旋转矩阵因子节点，然后建立基于旋转矩阵的协同定位方法，构造旋转矩阵以及设计旋转角度，并对系统的位置状态信息进行滤波更新，最后通过传递的概率密度函数对系统的位置状态进行滤波融合估计；从而实现在不改变系统中惯性器件的测量精度的情况下，通过实时在因子图内部变换主从艇的坐标值，使得参与因子图计算的主从艇坐标不一致，解决常规的因子图协同定位方法定位误差突然增大的问题，增强协同定位系统的鲁棒性。

技术领域

本发明涉及自主水下航行器协同定位技术，具体地，涉及一种基于旋转矩阵的因子图协同定位方法。

背景技术

作为人类探索海洋的助力器，水下航行器经历了从诞生、发展到应用的过程。其中自主式水下航行器(Autonomous Underwater Vehicle，AUV)作为集当今世界各种的尖端科技于一身的佼佼者，代表了未来水下航行器发展的方向。但是，单AUV工作平台在进一步对海洋资源进行开发利用的过程中，其局限性日渐显著。因此通过共享其他AUV的数据和信息，由N台AUV组成的多AUV协同系统的能力将远大于一台AUV的N倍。因此，近年来，通过多个AUV之间的协作来提高单个AUV导航性能的研究引起了人们的注意，并逐渐成为AUV导航领域研究的一个热点。多个AUV之间只要可以直接或者间接的相对观测，比如测量自身与其他AUV之间的距离或方位，就可以利用其他AUV的导航状态来提高自身导航精度。这样多个AUV之间就构成了一个协同导航系统。多AUV协同导航系统通过信息的共享，可以获得单AUV导航系统无法具备的优势:1)部分具有高性能导航设备的AUV通过通信共享其导航信息可以实现其高性能导航信息在整个AUV群体中的共享，从而使AUV群的整体导航精度得到提高。2)在多AUV协同导航系统中，只要部分AUV具有有界误差的定位能力，通过协同导航，就可以使系统中的每一个AUV都具有误差有界的定位能力。3)多AUV协同导航系统具有更强的容错能力和稳定性。

协同定位方法的本质是滤波问题，由于水下协同定位系统的距离观测方程是非线性的，因此对于协同定位系统的状态估计需要采用非线性滤波方法来实现。在利用因子图设计协同定位方法时，可有效避免传统协同定位方法在高斯噪声条件下定位精度差且计算量大的缺点。在协同定位系统中的噪声为高斯噪声的情况下，利用和积方法实现因子图中信息的更新，因子图和积方法将复杂的运算转变为几个简单的运算，方法中不涉及Jacobian矩阵运算，降低了计算的复杂度。因子图中不需要对距离观测方程线性化，避免了由线性化引起的误差，降低了协同定位误差，提高协同定位系统的定位能力。

然而在使用系统状态变量的期望、方差作为系统的信度信息进行协同定位时，容易出现由于主艇与从艇的某一方向坐标相近或相同协同定位误差突然增大的问题。

发明内容

本发明为了解决上述问题，有效降低协同定位误差，提高协同定位系统的鲁棒性，在因子图与和积方法的基础上，通过引入旋转矩阵，实时在因子图内部变换主从艇的坐标值，使得参与因子图计算的主从艇坐标不一致，从而解决在主从艇运行过程中由于主从艇某一方向上的坐标相近或相同而造成的协同定位误差突然增大的现象，提出了一种基于旋转矩阵的协同定位方法。

一种基于旋转矩阵的因子图协同定位方法，所述方法包括以下步骤：

步骤1：构建基于旋转矩阵的协同定位方法因子图模型，在因子图模型中加入旋转矩阵因子节点；

步骤2：建立基于旋转矩阵的协同定位方法，构造旋转矩阵以及设计旋转角度，并对协同定位系统的位置状态信息进行滤波更新；

步骤3：通过传递的概率密度函数对协同定位系统的位置状态进行滤波融合估计。