CN112945224B - 一种采用因子图和和积算法的多auv协同导航方法 - Google Patents

一种采用因子图和和积算法的多auv协同导航方法 Download PDF

Info

Publication number
CN112945224B
CN112945224B CN202110161315.4A CN202110161315A CN112945224B CN 112945224 B CN112945224 B CN 112945224B CN 202110161315 A CN202110161315 A CN 202110161315A CN 112945224 B CN112945224 B CN 112945224B
Authority
CN
China
Prior art keywords
auv
factor graph
collaborative navigation
satisfies
variance
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110161315.4A
Other languages
English (en)
Other versions
CN112945224A (zh
Inventor
奔粤阳
孙炎
李倩
赵玉新
周广涛
吴磊
蒋正华
魏晓峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Hatran Navigation Technology Co ltd
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202110161315.4A priority Critical patent/CN112945224B/zh
Publication of CN112945224A publication Critical patent/CN112945224A/zh
Application granted granted Critical
Publication of CN112945224B publication Critical patent/CN112945224B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

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

Description

一种采用因子图和和积算法的多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位置和方位角。
在CNKI中关于AUV协同导航技术的相关文献较多，其中典型文献如哈尔滨工程大学范颖在2018年无人载体导航与控制技术发展及应用学术研讨峰会上发表的《一种基于最大互相关熵容积卡尔曼滤波的AUV协同导航方法》一文，主要研究基于最大互相关熵准则，提出了一种最大互相关熵容积卡尔曼滤波(Maximum Correntropy Cubature KalmanFilter,MCCKF)，然后设计出了基于MCCKF的AUV协同导航方法，而本发明主要是基于FG和SPA提出一种新的AUV协同导航方法。
2017年哈尔滨工程大学徐博在《系统工程与电子技术》期刊发表的《基于交互式模型的多AUV协同导航鲁棒性滤波算法》一文，主要研究基于交互式模型的多AUV协同导航滤波方法，该方法仍然属于卡尔曼滤波框架。2017年哈尔滨工程大学李宁的专利《一种基于非线性信息滤波的AUV协同导航方法》(专利号：ZL 201710805228.1)，该发明主要考虑了AUV之间信息传递带来的信息相关性问题，并利用信息边缘化方法解决了该问题。但是，该方法是属于卡尔曼滤波的体系下方法，本发明所提出的基于图论的AUV协同导航算法与该方法不同。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种采用因子图和和积算法的多AUV协同导航方法，通过因子图理论建立多AUV协同导航系统因子图模型、利用和积算法实现该因子图模型上的消息的计算、融合和传递，实现多个AUV的协同导航任务。
为解决上述技术问题，本发明的一种采用因子图和和积算法的多AUV协同导航方法，包括以下步骤：
步骤1、获得待定位AUV的初始时刻的位置和方位角；
步骤2、多AUV协同导航系统中，待定位AUV接收位置已知的主AUV广播的其当前自身位置；
步骤3、待定位AUV根据配备的传感器获得其与位置已知的主AUV之间的测距信息和相对观测角信息；
步骤4、建立描述待定位AUV的状态方程；
步骤5、建立描述待定位AUV的量测方程；
步骤6、根据AUV的状态方程和量测方程建立相应的因子图模型，距离和相对观测角同时作为系统的观测量，所述因子图模型含有环结构；
步骤7、将含环结构的AUV协同导航系统因子图模型转换成含有一个聚合节点的无环因子图模型；
步骤8、根据系统状态方程进行待定位AUV位置和方位角的一步预测更新；
步骤9、根据系统获得的AUV之间的观测量和量测方程估计待定位AUV位置和方位角。
本发明还包括：
1.步骤4中描述待定位AUV的状态方程具体为：
xk＝xk-1+dxk cosθk-1-dyk sinθk-1
yk＝yk-1+dxk sinθk-1+dyk cosθk-1
θk＝θk-1+dθk
其中，已知AUV起始时刻状态向量为x0＝[x0 y0 θ0]T，xk,yk为k时刻AUV在地理坐标系的东向和北向的位置，θk为k时刻方位角，北偏西为正，(·)k代表k时刻的变量，控制输入uk＝[dxk dyk dθk]T，dxk代表东向位移增量，dyk代表北向位移增量，dθk代表方位角增量；
2.步骤5中描述待定位AUV的量测方程具体为：
待定位AUV收到的位置已知的主AUV与待定位AUV之间的距离满足：
Figure BDA0002936810710000031
第i个位置已知的主AUV与待定位AUV之间的相对观测角
Figure BDA0002936810710000032
满足：
Figure BDA0002936810710000033
其中，xk,yk为k时刻AUV在地理坐标系的东向和北向的位置，
Figure BDA0002936810710000034
分别表示第i个位置已知的主AUV当前的东向位置和北向位置，
Figure BDA0002936810710000035
为水声距离量测噪声；
Figure BDA0002936810710000036
Figure BDA0002936810710000037
Figure BDA0002936810710000038
为角度量测噪声，
Figure BDA0002936810710000039
与
Figure BDA00029368107100000310
相互独立。
3.步骤8中根据系统状态方程进行AUV位置和方位角的一步预测更新具体为：将系统状态方程用泰勒公式将其展开进行线性化，根据和积算法，沿着信息在因子图上的流动计算各节点处的均值和方差，进而计算出待估计节点的一步预测值：
待定位AUV东向位置的一步预测更新值
Figure BDA00029368107100000312
的均值及其方差满足，其中μ·表示变量·的均值，
Figure BDA00029368107100000311
表示变量·的方差：
Figure BDA0002936810710000041
待定位AUV北向位置的一步预测更新值
Figure BDA0002936810710000042
的均值及其方差满足：
Figure BDA0002936810710000043
待定位AUV方位角的一步预测更新值
Figure BDA0002936810710000044
的均值及其方差满足：
Figure BDA0002936810710000045
4.步骤9中根据系统获得的AUV之间的观测量和量测方程估计待定位AUV位置和方位角具体为：
待定位AUV东向位置的一步预测更新值xk的均值及其方差
Figure BDA0002936810710000046
满足：
Figure BDA0002936810710000047
其中，n表示此时接受到观测值的主AUV的个数，下标含有
Figure BDA0002936810710000048
表示经过距离和相对观测角量测更新后的值，
Figure BDA0002936810710000049
和
Figure BDA00029368107100000410
满足：
Figure BDA00029368107100000411
式中，
Figure BDA00029368107100000412
Figure BDA00029368107100000413
Figure BDA0002936810710000051
待定位AUV北向位置的一步预测更新值yk的均值及其方差
Figure BDA0002936810710000052
满足：
Figure BDA0002936810710000053
其中，
Figure BDA0002936810710000054
和
Figure BDA0002936810710000055
满足：
Figure BDA0002936810710000056
式中，
Figure BDA0002936810710000057
Figure BDA0002936810710000058
待定位AUV方位角的一步预测更新值θk的均值及其方差
Figure BDA0002936810710000059
满足：
Figure BDA00029368107100000510
式中，
Figure BDA00029368107100000511
Figure BDA00029368107100000512
本发明的有益效果：本发明针对装配有低精度、低成本的多AUV协同导航系统，不仅将AUV艇间测距信息作为系统量测信息，还将AUV艇间相对观测角度作为量测信息，同时实现了对AUV位置和方位角的确定。该方法采用主从AUV之间的距离和相对观测角作为协同导航系统的观测量来确定AUV当前所在位置和方位角。首先，建立描述AUV导航系统的状态方程和量测方程；其次，根据AUV的状态方程和量测方程建立相应的多AUV协同导航系统因子图模型。由于距离和相对观测角同时作为系统的观测量，该因子图模型含有环结构。为了获得系统的精确解，该含环因子图被等价的转换成含有一个聚合节点的无环因子图模型。最后，根据系统状态方程进行AUV位置和方位角的一步预测更新，然后根据系统获得的AUV之间的观测量结合量测方程估计AUV位置和方位角。
与现有基于传统的基于卡尔曼滤波和各种改进的卡尔曼滤波的AUV协同导航方法相比，该方法是一种基于因子图和和积算法的AUV协同导航方法，是一种基于图论的方法，方法比较新颖。另外，在满足实际工程定位精度需求下，计算量更小，基于图论的方法展示了更好地系统可扩展性，在实际工程应用中应用性更强。
附图说明
图1为本发明的方法流程图；
图2为基于FG的AUV协同导航系统因子图模型；图中，Ak、Bk、
Figure BDA0002936810710000061
Gk为函数节点，xk、yk、θk、
Figure BDA0002936810710000062
为变量节点；各函数节点的约束关系是，Ak点：
Figure BDA0002936810710000063
Bk点：
Figure BDA0002936810710000064
Figure BDA0002936810710000065
点：
Figure BDA0002936810710000066
Figure BDA0002936810710000067
点：
Figure BDA0002936810710000068
Figure BDA00029368107100000613
点：
Figure BDA0002936810710000069
Figure BDA00029368107100000610
点：
Figure BDA00029368107100000611
Gk点：
Figure BDA00029368107100000612
图3为基于FG的AUV协同导航系统开环因子图模型。
图4为基于FG协同导航方法与基于EKF协同导航方法在AUV东向定位上的误差比较；实线表示基于FG协同导航方法，虚线表示基于EKF协同导航方法。
图5为基于FG协同导航方法与基于EKF协同导航方法在AUV北向定位上的误差比较；实线表示基于FG协同导航方法，虚线表示基于EKF协同导航方法。
图6为基于FG协同导航方法与基于EKF协同导航方法在AUV方位角确定上的误差比较；实线表示基于FG协同导航方法，虚线表示基于EKF协同导航方法。
具体实施方式
下面结合附图和具体实施方式对本发明做进一步说明。
本发明的一种基于因子图和和积算法的多AUV协同导航方法，具体包括：
步骤1、获得待定位AUV的初始时刻的位置和方位角；
步骤2、多AUV协同导航系统中，待定位AUV接收其他AUV广播的当前自身位置；
步骤3、待定位AUV根据配备的传感器获得其与其他AUV之间的测距信息和相对观测角信息；
步骤4、建立描述待定位AUV的状态方程；
步骤5、建立描述待定位AUV的量测方程；
步骤6、根据AUV的状态方程和量测方程建立相应的因子图模型。由于距离和相对观测角同时作为系统的观测量，该因子图模型含有环结构；
步骤7、含环结构的AUV协同导航系统因子图模型转换成含有一个聚合节点的无环因子图模型；
步骤8、根据系统状态方程进行AUV位置和方位角的一步预测更新；
步骤9、根据系统获得的AUV之间的观测量和量测方程估计待定位AUV位置和方位角。
步骤6中建立的多AUV协同导航系统因子图模型是同时将AUV之间的距离信息和相对观测角作为量测量加入系统中。
步骤7中建立的无环的多AUV协同导航系统因子图模型是含环因子图模型的等价转换。转换成无环因子图模型得到的估计值可以比靠限制迭代次数得到含环因子图模型近似解更精确。
步骤8中待定位AUV位置和方位角的一步预测更新和步骤9中估计待定位AUV位置和方位角都是基于和积算法计算、更新各个节点的信息。和积算法计算、更新各节点信息，进而估计待定位AUV的位置和方位角。
结合图1，本发明一种采用因子图和和积算法的多AUV协同导航方法，包括如下步骤：
步骤1、获得待定位AUV的初始时刻的东向位置x0、北向位置y0和方位角θ0；
步骤2、多AUV协同导航系统中，待定位AUV接收其他AUV广播的当前自身位置
Figure BDA0002936810710000071
分别表示第i个主AUV当前的东向位置和北向位置；
步骤3、待定位AUV根据配备的传感器获得其与其他AUV之间的测距信息
Figure BDA0002936810710000072
和相对观测角信息
Figure BDA0002936810710000073
和控制输入信号uk＝[dxk,dyk dθk]T。这里假设这些变量都服从高斯分布。
Figure BDA0002936810710000074
其中，μ·代表变量·的均值，
Figure BDA0002936810710000075
代表变量·的方差。
步骤4、建立描述待定位AUV的状态方程；虽然水下航行器工作在三维空间，但是考虑到实际深度信息可以由压力传感器实时精确测量，对于航行器的定位误差不起积累作用，因此进一步简化为二维空间下的协同定位问题。本发明中参与协同导航的AUV以航位推算方式进行自主导航。
已知AUV起始时刻状态向量为x0＝[x0 y0 θ0]T，其中xk,yk为k时刻AUV在地理坐标系的东向和北向的位置，θk为k时刻方位角，北偏西为正。(·)k代表k时刻的变量。控制输入uk＝[dxk dyk dθk]T，dxk代表东向位移增量，dyk代表北向位移增量，dθk代表方位角增量。根据矢量叠加原理，可写出如下状态方程。
Figure BDA0002936810710000081
步骤5、建立描述待定位AUV的量测方程；根据收到的主AUV与从AUV之间的距离可以由下式表示
Figure BDA0002936810710000082
式中，
Figure BDA0002936810710000083
为水声距离量测噪声，通常假设为零均值高斯白噪声序列，满足
Figure BDA0002936810710000084
相对观测角
Figure BDA0002936810710000085
为第i个AUV与从AUV之间的相对角度观测，可由下式表示
Figure BDA0002936810710000086
式中，
Figure BDA0002936810710000087
Figure BDA0002936810710000088
为角度量测噪声，通常假设为零均值高斯白噪声序列，满足
Figure BDA0002936810710000089
式(4)和式(5)共同构成系统的量测方程，
Figure BDA00029368107100000810
与
Figure BDA00029368107100000811
相互独立。
步骤6、根据AUV的状态方程和量测方程建立相应的因子图模型。由于距离和相对观测角同时作为系统的观测量，该因子图模型含有环结构；该基于FG的AUV协同导航系统因子图模型如附图2所示。
步骤7、含环结构的AUV协同导航系统因子图模型转换成含有一个聚合节点的无环因子图模型；该基于FG的AUV协同导航系统无环因子图模型如附图3所示。
步骤8、根据系统状态方程进行AUV位置和方位角的一步预测更新；系统状态方程为非线性方程，这里将其用泰勒公式将其展开进行线性化。根据和积算法，沿着信息在因子图上的流动计算各节点处的均值和方差，进而计算出待估计节点的一步预测值。从AUV东向位置的一步预测更新值
Figure BDA00029368107100000812
北向位置的一步预测更新值
Figure BDA00029368107100000813
和方位角的一步预测更新值
Figure BDA00029368107100000814
及其相应的方差如下所示。
从AUV东向位置的一步预测更新值
Figure BDA00029368107100000816
的均值及其方差如下所示，其中μ·表示变量·的均值，
Figure BDA00029368107100000815
表示变量·的方差。
Figure BDA0002936810710000091
从AUV北向位置的一步预测更新值yk-的均值及其方差如下所示，
Figure BDA0002936810710000092
从AUV方位角的一步预测更新值
Figure BDA0002936810710000093
的均值及其方差如下所示，
Figure BDA0002936810710000094
步骤9、根据系统获得的AUV之间的观测量和量测方程估计待定位AUV位置和方位角。根据步骤8中的一步预测值、量测信息和各个函数节点的约束关系估计从AUV东向位置xk、东向位置yk和方位角θk及其方差值。
从AUV东向位置的一步预测更新值xk的均值及其方差
Figure BDA0002936810710000095
如下所示，
Figure BDA0002936810710000096
其中，n表示此时接受到观测值的主AUV的个数。下表含有
Figure BDA0002936810710000097
表示经过距离和相对观测角量测更新后的值。
Figure BDA0002936810710000098
和
Figure BDA0002936810710000099
可由下式表示。
Figure BDA00029368107100000910
式中，
Figure BDA00029368107100000911
Figure BDA0002936810710000101
从AUV北向位置的一步预测更新值yk的均值及其方差
Figure BDA0002936810710000102
如下所示，
Figure BDA0002936810710000103
其中，
Figure BDA0002936810710000104
和
Figure BDA0002936810710000105
可由下式表示。
Figure BDA0002936810710000106
式中，
Figure BDA0002936810710000107
Figure BDA0002936810710000108
从AUV方位角的一步预测更新值θk的均值及其方差
Figure BDA0002936810710000109
如下所示，
Figure BDA00029368107100001010
式中，
Figure BDA00029368107100001011
Figure BDA00029368107100001012
至此就完成了基于因子图的AUV的协同导航。
将基于EKF的协同导航方法和基于FG的协同导航方法仿真滤波效果进行比较。
仿真条件：
(1)主AUV和从AUV在同一深度航行，均以3m/s航行。从AUV航向角θk按照θk＝1°·sin(2·π·k/T)变化，T为仿真总时长，为600s；k表示仿真实验进行到第k秒。在运动过程中，主AUV每1s向从AUV广播一次其位置信息，从AUV接收到该信息后计算相应的量测距离，并利用量测值和主AUV所提供的信息得到当前时刻从AUV的位置和方位角估计值。
(2)仿真实验中，设置系统噪声Qk＝(diag[0.5(m/s)0.5(m/s)1(°)])2，量测噪声Rk＝(diag[10(m)3(°)])2。
仿真实验将本发明的协同导航方法同工程中广泛使用的基于EKF的协同导航算法相比较。这里，我们使用MATLAB的内联函数tic和toc来计算两种方法的实际运行时间。经过多次仿真实验，实验结果显示基于EKF的AUV协同导航方法平均耗时为0.533574386s，而基于FG的AUV协同导航方法平均耗时为0.415607836s，时间节省了22％的时间。另外，仿真实验比较了这两个方法在确定从AUV位置和方位角上的误差，用RMSE值作为评价指标。
Figure BDA0002936810710000111
上式中，q可以表示从AUV东向位置x、北向位置y和方位角θ。下角标标注ref,k表示是从AUV在k时刻的估计量的真实值，下角标标注的k表示在k时刻的状态估计值。
仿真结果表示，基于EKF的协同导航方法东向位置、北向位置和方位角的RMSE值分别是1.3253m、1.2018m和1.5779°，而基于FG的协同导航方法东向位置、北向位置和方位角的RMSE值分别是1.1991m、1.0920m和1.3946°。本方法与基于EKF的协同导航方向相比，东向定位精度提高了10.5％，北向定位精度提高了10.1％，方位角精度提高了13.1％。仿真结果证明，基于FG的AUV协同导航方法，比传统的基于EKF的AUV协同导航方法，不仅计算量更小、耗时更少，而且提高了位置和方位角的精度。

Claims (1)

1.一种采用因子图和和积算法的多AUV协同导航方法，其特征在于，包括以下步骤：
步骤1、获得待定位AUV的初始时刻的位置和方位角；
步骤2、多AUV协同导航系统中，待定位AUV接收位置已知的主AUV广播的其当前自身位置；
步骤3、待定位AUV根据配备的传感器获得其与位置已知的主AUV之间的测距信息和相对观测角信息；
步骤4、建立描述待定位AUV的状态方程，具体为：
xk＝xk-1+dxk cosθk-1-dyk sinθk-1
yk＝yk-1+dxk sinθk-1+dyk cosθk-1
θk＝θk-1+dθk
其中，已知AUV起始时刻状态向量为x0＝[x0 y0 θ0]T，xk,yk为k时刻AUV在地理坐标系的东向和北向的位置，θk为k时刻方位角，北偏西为正，(·)k代表k时刻的变量，控制输入uk＝[dxk dyk dθk]T，dxk代表东向位移增量，dyk代表北向位移增量，dθk代表方位角增量；
步骤5、建立描述待定位AUV的量测方程，具体为：
待定位AUV收到的位置已知的主AUV与待定位AUV之间的距离满足：
Figure FDA0003894427880000011
第i个位置已知的主AUV与待定位AUV之间的相对观测角
Figure FDA0003894427880000012
满足：
Figure FDA0003894427880000013
其中，xk,yk为k时刻AUV在地理坐标系的东向和北向的位置，
Figure FDA0003894427880000014
分别表示第i个位置已知的主AUV当前的东向位置和北向位置，
Figure FDA0003894427880000015
为水声距离量测噪声；
Figure FDA0003894427880000016
Figure FDA0003894427880000017
Figure FDA0003894427880000018
为角度量测噪声，
Figure FDA0003894427880000019
与
Figure FDA00038944278800000110
相互独立；
步骤6、根据AUV的状态方程和量测方程建立相应的因子图模型，距离和相对观测角同时作为系统的观测量，所述因子图模型含有环结构；
步骤7、将含环结构的AUV协同导航系统因子图模型转换成含有一个聚合节点的无环因子图模型；
步骤8、根据系统状态方程进行待定位AUV位置和方位角的一步预测更新，具体为：将系统状态方程用泰勒公式将其展开进行线性化，根据和积算法，沿着信息在因子图上的流动计算各节点处的均值和方差，进而计算出待估计节点的一步预测值；所述计算出待估计节点的一步预测值具体为：
待定位AUV东向位置的一步预测更新值
Figure FDA0003894427880000021
的均值及其方差满足，其中μ.表示变量·的均值，
Figure FDA0003894427880000022
表示变量·的方差：
Figure FDA0003894427880000023
Figure FDA0003894427880000024
待定位AUV北向位置的一步预测更新值
Figure FDA0003894427880000025
的均值及其方差满足：
Figure FDA0003894427880000026
Figure FDA0003894427880000027
待定位AUV方位角的一步预测更新值
Figure FDA0003894427880000028
的均值及其方差满足：
Figure FDA0003894427880000029
步骤9、根据系统获得的AUV之间的观测量和量测方程估计待定位AUV位置和方位角，具体为：
待定位AUV东向位置的一步预测更新值xk的均值及其方差
Figure FDA00038944278800000210
满足：
Figure FDA00038944278800000211
其中，n表示此时接受到观测值的主AUV的个数，下标含有
Figure FDA00038944278800000212
表示经过距离和相对观测角量测更新后的值，
Figure FDA00038944278800000213
和
Figure FDA00038944278800000214
满足：
Figure FDA00038944278800000215
式中，
Figure FDA0003894427880000031
Figure FDA0003894427880000032
Figure FDA0003894427880000033
待定位AUV北向位置的一步预测更新值yk的均值及其方差
Figure FDA0003894427880000034
满足：
Figure FDA0003894427880000035
其中，
Figure FDA0003894427880000036
和
Figure FDA0003894427880000037
满足：
Figure FDA0003894427880000038
式中，
Figure FDA0003894427880000039
Figure FDA00038944278800000310
待定位AUV方位角的一步预测更新值θk的均值及其方差
Figure FDA00038944278800000311
满足：
Figure FDA00038944278800000312
式中，
Figure FDA00038944278800000313
Figure FDA00038944278800000314
Figure FDA00038944278800000315
CN202110161315.4A 2021-02-05 2021-02-05 一种采用因子图和和积算法的多auv协同导航方法 Active CN112945224B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110161315.4A CN112945224B (zh) 2021-02-05 2021-02-05 一种采用因子图和和积算法的多auv协同导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110161315.4A CN112945224B (zh) 2021-02-05 2021-02-05 一种采用因子图和和积算法的多auv协同导航方法

Publications (2)

Publication Number Publication Date
CN112945224A CN112945224A (zh) 2021-06-11
CN112945224B true CN112945224B (zh) 2022-12-09

Family

ID=76242570

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110161315.4A Active CN112945224B (zh) 2021-02-05 2021-02-05 一种采用因子图和和积算法的多auv协同导航方法

Country Status (1)

Country Link
CN (1) CN112945224B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114040325B (zh) * 2021-11-05 2022-08-19 西北工业大学 惯导辅助下的单锚节点网络协同定位方法
CN114370878B (zh) * 2022-01-04 2023-10-27 江苏科技大学 一种基于stackf的多auv协同定位方法
CN114485646B (zh) * 2022-01-27 2024-04-26 浙江大学 一种集成超短基线系统的uuv集群协同定位方法
CN116595897B (zh) * 2023-07-17 2023-10-10 广东工业大学 一种基于消息传递的非线性动态系统状态估计方法和装置
CN117739978A (zh) * 2023-12-06 2024-03-22 无锡学院 一种基于因子图的多auv并行式协同导航定位方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102338867A (zh) * 2011-07-01 2012-02-01 上海海事大学 一种适用于移动机器人导航的激光束匹配误差估计方法
CN102611465A (zh) * 2012-04-07 2012-07-25 西安电子科技大学 结构化多元非规则重复累积码的编码器与编码方法
CN102905363A (zh) * 2012-07-20 2013-01-30 北京邮电大学 一种基于因子图的定位方法
CN108139225A (zh) * 2015-07-29 2018-06-08 大众汽车有限公司 确定机动车的布局信息
CN108375782A (zh) * 2018-01-18 2018-08-07 西北工业大学 基于卫星导航定位系统的因子图协同定位方法
CN109883426A (zh) * 2019-03-08 2019-06-14 哈尔滨工程大学 基于因子图的动态分配与校正多源信息融合方法
CN110191411A (zh) * 2019-04-28 2019-08-30 浙江大学 一种基于时空域联合处理的分布式协作定位系统和方法
CN110274588A (zh) * 2019-06-19 2019-09-24 南京航空航天大学 基于无人机集群信息的双层嵌套因子图多源融合导航方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9720069B2 (en) * 2012-10-10 2017-08-01 Lg Electronics Inc. Apparatus and method for measuring location of user equipment located indoors in wireless network
CN105898691B (zh) * 2016-03-14 2019-06-04 河海大学常州校区 基于粒子化和积算法的无线传感器网络目标跟踪方法
CN110225454B (zh) * 2019-06-26 2020-12-18 河南大学 一种置信度传递的分布式容积卡尔曼滤波协作定位方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102338867A (zh) * 2011-07-01 2012-02-01 上海海事大学 一种适用于移动机器人导航的激光束匹配误差估计方法
CN102611465A (zh) * 2012-04-07 2012-07-25 西安电子科技大学 结构化多元非规则重复累积码的编码器与编码方法
CN102905363A (zh) * 2012-07-20 2013-01-30 北京邮电大学 一种基于因子图的定位方法
CN108139225A (zh) * 2015-07-29 2018-06-08 大众汽车有限公司 确定机动车的布局信息
CN108375782A (zh) * 2018-01-18 2018-08-07 西北工业大学 基于卫星导航定位系统的因子图协同定位方法
CN109883426A (zh) * 2019-03-08 2019-06-14 哈尔滨工程大学 基于因子图的动态分配与校正多源信息融合方法
CN110191411A (zh) * 2019-04-28 2019-08-30 浙江大学 一种基于时空域联合处理的分布式协作定位系统和方法
CN110274588A (zh) * 2019-06-19 2019-09-24 南京航空航天大学 基于无人机集群信息的双层嵌套因子图多源融合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An Advanced Cooperative Positioning Algorithm Based on Improved Factor Graph and Sum-Product Theory for Multiple AUVs;Shiwei Fan 等;《IEEE Access》;20190604;第7卷;第67006-67017页 *
基于因子图的北斗导航系统协同定位方法;翟瑞鑫 等;《第九届中国卫星导航学术年会》;20180523;第1-4页 *
基于联合分布状态信息滤波的机器人协同定位;奔粤阳 等;《系统工程与电子技术》;20150228;第37卷(第2期);第385-393页 *

Also Published As

Publication number Publication date
CN112945224A (zh) 2021-06-11

Similar Documents

Publication Publication Date Title
CN112945224B (zh) 一种采用因子图和和积算法的多auv协同导航方法
EP3904908A1 (en) Method and system for map improvement using feedback from positioning based on radar and motion sensors
Li et al. Simultaneous registration and fusion of multiple dissimilar sensors for cooperative driving
CN1325932C (zh) 载人潜水器的组合导航定位方法
CN107990891B (zh) 基于长基线和信标在线标定的水下机器人组合导航方法
CN106017467B (zh) 一种基于多水下应答器的惯性/水声组合导航方法
CN109974706A (zh) 一种基于双运动模型的主从式多auv协同导航方法
CN109782289B (zh) 一种基于基线几何结构约束的水下航行器定位方法
CN105823480A (zh) 基于单信标的水下移动目标定位算法
WO2018072350A1 (zh) 车辆轨迹预测方法及装置
Ahn et al. Simple pedestrian localization algorithms based on distributed wireless sensor networks
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
Xu et al. A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles
CN103776453A (zh) 一种多模型水下航行器组合导航滤波方法
CN104656058A (zh) 分布式多移动节点协作定位系统
Medagoda et al. Autonomous underwater vehicle localization in a spatiotemporally varying water current field
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN103323815A (zh) 一种基于等效声速的水下声学定位方法
Petillot et al. Acoustic-based techniques for autonomous underwater vehicle localization
Fan et al. Cooperative positioning for multi-AUVs based on factor graph and maximum correntropy
Xu et al. Accurate two-step filtering for AUV navigation in large deep-sea environment
CN112859133A (zh) 一种基于雷达与北斗数据的船舶深度融合定位方法
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
Gustafsson et al. Navigation and tracking of road-bound vehicles
Chen et al. An optimal selection of sensors in multi-sensor fusion navigation with factor graph

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20230629

Address after: Room 308, Building 4, No. 180, Software Avenue, Yuhuatai District, Nanjing, Jiangsu Province, 210012

Patentee after: Nanjing Inertial System Technology Co.,Ltd.

Address before: 150001 Intellectual Property Office, Harbin Engineering University science and technology office, 145 Nantong Avenue, Nangang District, Harbin, Heilongjiang

Patentee before: HARBIN ENGINEERING University

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230724

Address after: 150000 floor 2, East unit 22, Tianping Road, building 2D, dui'e Park, Yingbin Road concentration area, high tech Industrial Development Zone, Harbin, Heilongjiang Province

Patentee after: HARBIN HATRAN NAVIGATION TECHNOLOGY Co.,Ltd.

Address before: Room 308, Building 4, No. 180, Software Avenue, Yuhuatai District, Nanjing, Jiangsu Province, 210012

Patentee before: Nanjing Inertial System Technology Co.,Ltd.

TR01 Transfer of patent right