[发明专利]一种采用因子图和和积算法的多AUV协同导航方法

说明书

本发明公开了一种采用因子图和和积算法的多AUV协同导航方法，首先，建立描述AUV导航系统的状态方程和量测方程；其次，根据AUV的状态方程和量测方程建立相应的多AUV协同导航系统因子图模型，该因子图模型含有环结构。为了获得系统的精确解，该含环因子图被等价的转换成含有一个聚合节点的无环因子图模型。最后，根据系统状态方程进行AUV位置和方位角的一步预测更新，然后根据系统获得的AUV之间的观测量结合量测方程估计AUV位置和方位角。本发明采用基于图论的估计方法，方法新颖。另外，能满足实际工程定位精度的同时，计算量更小，该方法展示了更好地系统可扩展性，在实际工程应用中应用性更强。

技术领域

本发明属于自主式水下航行领域，具体涉及一种采用因子图(Factor Graph，FG)和和积算法(Sum Product Algorithm，SPA)的多AUV协同导航方法，用于实现多个AUV之间协同导航的方法。

背景技术

AUV作为人类探索水下世界的途径，在世界范围内越来越受到人们的关注，在实现水下航行方面取得了巨大的进展。由于电磁波在水中传播的快速衰减,GPS等基于无线电的常规陆上导航方法无法在水下进行导航和定位。因此，AUV必须依靠罗盘、多普勒测速仪或惯性导航系统获得的信息，以及压力深度传感器通过对速度、姿态和压力深度的积分来计算航迹推算的导航参数。

然而，航迹推算的误差积累将随着时间的推移将导航精度降低到不可接受的程度。为了提高水下导航定位的精度，常常采用的方法是利用飞行时间水声定位系统,例如，长基线(Long Base Line，LBL)、短基线(Short Base Line，SBL)和超短基线(Ultra ShortBase Line，USBL)定位系统。然而，这些系统存在一些缺点，例如LBL和SBL系统的一个缺点是建立信标网络时需要投入大量的人力和物力，应用时相对复杂。

随着多个AUV部署的发展，多个AUV之间的协同导航是一种更明智的选择。特别地，随着先进水下通信技术的不断涌现，水下多AUV协同导航技术也得到了更多的关注和探索。从协同导航核心算法的角度分类,大致可以分成基于优化理论的协同导航方法、基于贝叶斯估计的协同导航方法和基于图论的协同导航方法三大类。基于优化理论的协同导航方法通常是将导航问题转化为求解有约束系统方程的最优解问题。比如将系统状态估计问题等效为二次约束二次规划问题，并通过拉格朗日松弛原理变为凸优化问题求解，但这种方法忽略了各平台状态之间的相关性以及模型的非线性问题。对于基于贝叶斯估计的协同导航方法，常见的实现方法包括卡尔曼滤波(Kalman Filtering，KF)和其各种改进的滤波算法。扩展卡尔曼滤波(Extended Kalman Filtering，EKF)因其操作简单，执行速度快而广泛应用于协同导航系统。然而，EKF的主要缺点来自线性化和高斯假设。此外，当协同导航系统的结构发生变化时，算法的雅克比矩阵需要重新计算，增加了计算量。同时，这也使得基于EKF的协同导航算法在实际应用时系统可延展性和灵活性较差。虽然理论上粒子滤波(Particle Filter，PF)可以在非线性系统中精确地执行，但在实际工程中，计算量大、实时性差成为难以解决的问题。

基于图论的导航方法是一种新型的导航方法。目前，这种基于FG和SPA的协同导航方法只用于解决AUV的定位问题，还没有实现AUV方位的确定。本发明提出一种既可以确定AUV位置又可以确定其方位的协同导航方法。首先，建立描述AUV导航系统的状态方程和量测方程；其次，根据AUV的状态方程和量测方程建立相应的多AUV协同导航系统因子图模型。由于距离和相对观测角同时作为系统的观测量，该因子图模型含有环结构。为了获得系统的精确解，该含环因子图被等价的转换成含有一个聚合节点的无环因子图模型。最后，根据系统状态方程进行AUV位置和方位角的一步预测更新，然后根据系统获得的AUV之间的观测量和量测方程估计AUV位置和方位角。