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

摘要

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