CN116734860A - 一种基于因子图的多auv自适应协同定位方法及系统 - Google Patents
一种基于因子图的多auv自适应协同定位方法及系统 Download PDFInfo
- Publication number
- CN116734860A CN116734860A CN202310851207.9A CN202310851207A CN116734860A CN 116734860 A CN116734860 A CN 116734860A CN 202310851207 A CN202310851207 A CN 202310851207A CN 116734860 A CN116734860 A CN 116734860A
- Authority
- CN
- China
- Prior art keywords
- auv
- information
- boat
- adaptive
- slave
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 76
- 238000005259 measurement Methods 0.000 claims abstract description 52
- 239000011159 matrix material Substances 0.000 claims abstract description 40
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 31
- 238000012546 transfer Methods 0.000 claims abstract description 9
- 230000004927 fusion Effects 0.000 claims abstract description 6
- 230000003044 adaptive effect Effects 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 18
- 238000001914 filtration Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 6
- 230000005540 biological transmission Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000012935 Averaging Methods 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000026676 system process Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims 1
- 238000004088 simulation Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 238000004458 analytical method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- WSNMPAVSZJSIMT-UHFFFAOYSA-N COc1c(C)c2COC(=O)c2c(O)c1CC(O)C1(C)CCC(=O)O1 Chemical compound COc1c(C)c2COC(=O)c2c(O)c1CC(O)C1(C)CCC(=O)O1 WSNMPAVSZJSIMT-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- MKXZASYAUGDDCJ-NJAFHUGGSA-N dextromethorphan Chemical compound C([C@@H]12)CCC[C@]11CCN(C)[C@H]2CC2=CC=C(OC)C=C21 MKXZASYAUGDDCJ-NJAFHUGGSA-N 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011897 real-time detection Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Feedback Control In General (AREA)
Abstract
本发明一种基于因子图的多AUV自适应协同定位方法及系统，涉及AUV协同定位技术领域，为解决现有技术无法应对系统拓扑结构的动态变化，及量测信息掺杂模糊时变噪声，对定位精度长生干扰的问题。包括如下过程：采集多AUV协同定位系统当前时刻动态拓扑结构信息；更新从艇及其邻居主艇信息；构建因子图模型，将从艇状态变量及主艇量测信息定义为变量节点，采用状态方程节点对从艇状态变量节点Xk进行传递更新；基于自适应EKF滤波器，并采用EM算法估计量测噪声协方差矩阵；采用量测方程节点对主艇量测信息节点Zk及从艇状态变量节点Xk进行融合更新，得到当前时刻从艇位置信息估计值。本发明实现了多AUV协同的实时、精准定位。
Description
技术领域
本发明涉及AUV协同定位技术领域，具体而言，涉及一种基于因子图的多AUV自适应协同定位方法及系统。
背景技术
由于水下环境的复杂性，水下任务执行过程中多AUV协同定位系统的AUV数量可能发生改变，导致系统拓扑结构发生动态变化。而且针对动态拓扑下基于因子图的多AUV协同定位，由于水温变化、介质密度不均衡、洋流干扰等多种影响，水下声速往往会发未知变化，量测信息中常常掺杂模糊时变噪声，这类噪声的统计特性未知且具有不确定的协方差矩阵，容易对协同定位算法的量测更新部分产生干扰。因子图在相关节点处传递信息具有不确定性，这种误差会随着传递而累计，使得系统定位精度下降，严重时甚至会导致系统发散。在数据量不足的情况下，目前大多数多AUV自适应协同定位算法对于噪声协方差矩阵的估计精度会受到影响，自适应估计效果降低。此外，部分自适应估计算法的计算复杂度很高，无法满足动态拓扑结构下协同定位系统的实时性需求。因此亟需一种针对动态拓扑结构下模糊时变噪声干扰的多AUV自适应协同定位方法。
发明内容
本发明要解决的技术问题是：
由于海洋环境复杂，量测信息中常常掺杂模糊时变噪声，这类噪声的统计特性未知且具有不确定的协方差矩阵，容易对协同定位算法的量测更新部分产生干扰，使得系统定位精度下降，严重时甚至会导致系统发散。
本发明为解决上述技术问题所采用的技术方案：
本发明提供了一种基于因子图的多AUV自适应协同定位方法，包括如下步骤：
S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；
S2、更新从艇及其邻居主艇信息；
S3、初始化主艇与从艇信息；
S4、构建多AUV自适应协同定位系统的因子图模型；
将从艇状态变量及主艇量测信息定义为变量节点，状态方程和量测方程定义为函数节点，采用状态方程函数节点对从艇状态变量节点Xk进行传递更新，采用量测方程函数节点对主艇量测信息节点Zk及从艇状态变量节点Xk进行融合更新；同时，构建自适应迭代估计函数节点Ik，基于自适应EKF滤波器，并采用EM算法估计量测噪声协方差矩阵；
S5、基于和积算法在多AUV自适应协同定位系统的因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；
S6、基于所述自适应迭代估计函数节点对量测噪声协方差矩阵进行迭代更新；
S7、将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。
进一步地，S1中采集多AUV协同定位系统当前时刻动态拓扑结构信息，包括：主AUV及从AUV的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及目标从艇与每艘主艇之间的测距信息d，主AUV和从AUV的数量，采集周期为T。
进一步地，S3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。
进一步地，S5包括如下过程：
在第k时刻系统条件概率密度函数分解为：
式中，N表示主AUV节点数量；表示第n(n＝1,2,…,N)艘主AUV的量测信息；Xm,n表示第n(n＝1,2,…,N)艘主AUV的位置信息；fi表示各个AUV节点对应的概率因子，即：
式中，hi(.)表示量测函数；zi表示量测真实值；∑i表示对应量测误差的协方差矩阵；
定义系统采集到的测距信息d、航向角θ和从AUV的速度v均服从高斯分布：
其中di表示从AUV与第i艘主AUV之间的测距信息；
通过状态方程函数节点f(Xk|Xk-1)向变量节点Xk传递的信息为：
变量节点Xk向状态方程函数节点f(Xk|Xk-1)传递的信息为：
式中，和/>分别代表状态变量Xk的先验估计和方差；
根据协同定位的状态方程，
式中，(xk,yk)表示k时刻AUV在参考坐标系的坐标；vk表示k时刻AUV前向速度；θk表示k时刻AUV的航向角度；Δt表示采样间隔。
得到状态转换公式为：
式中，Qk是系统过程噪声协方差矩阵，Rk为量测噪声协方差矩阵，的表达式为：
式中，θk是式是k时刻对应的航向角信息；
将式(6)和式(7)代入式(5)，得到：
式中，Sk的表达式为：
进一步地，S6中根据EKF滤波算法构建从艇状态变量方程，即：
式中，表示k-1时刻得到的k时刻的估计值，F表示状态转移矩阵，uk表示控制输入，/>
构建相应的估计误差协方差矩阵，即：
Pk|k-1＝FPk|k-1FT+Qk-1 (12)
更新从艇状态变量和估计误差协方差矩阵。
根据EM算法，首先确定初始值：
进行第l+1步滤波增益矩阵的迭代更新：
式中，为：
利用式(14)更新状态变量：
更新误差协方差矩阵：
估计量测噪声协方差矩阵Rk：
迭代N次收敛后得到Rk的估计结果：
进一步地，S7包括如下过程：
k时刻系统中从AUV与第i艘主AUV之间的坐标差为和/>计算变量节点/>得到信度信息为：
式中和/>分别代表/>和/>的标准差，/>表示k时刻下从AUV与第i艘主AUV之间的距离；
计算变量节点传递的信度信息为：
式中代表/>的标准差；
计算变量节点和/>的信度信息分别为：
计算变量节点和/>的信度信息分别为：
将各主艇对从艇的位置估计传递到xk，即：
式中，和/>为xk的方差和期望；
同样的，yk的信息为：
式中，和/>为yk的方差和期望；
对从艇的位置估计和从艇航位推算估计进行加权平均：
进一步地，S7中k时刻下从AUV与第i艘主AUV之间的距离与/>和/>的关系为：
一种基于因子图的多AUV自适应协同定位系统，该系统具有与上述技术方案任一项技术方案的步骤对应的程序模块，运行时执行上述的基于因子图的多AUV自适应协同定位方法中的步骤。
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述技术方案中任一项所述的基于因子图的多AUV自适应协同定位方法的步骤。
相较于现有技术，本发明的有益效果是：
本发明一种基于因子图的多AUV自适应协同定位方法及系统，通过实时检测系统的动态拓扑结构，增减因子图节点以实现传感器的即插即用功能，将从艇状态信息及主艇量测信息定义为变量节点，构建动态拓扑下协同定位因子图模型。在动态拓扑的协同定位系统基础上进一步考虑模糊时变噪声对基于因子图的协同定位干扰，通过引入最大期望算法(Expectation Maximization Algorithm,EM)的自适应EKF滤波对量测噪声协方差进行估计，去除量测噪声协方差中的不确定性。设计自适应迭代估计函数节点，与主从艇的状态信息变量节点与量测信息变量节点进行信息交互，加入状态变量信息传递函数传递节点与量测信息融合函数节点，构建自适应协同定位算法因子图模型，能够有效解决模糊时变噪声对动态拓扑系统的影响，实现多AUV实时定位。
附图说明
图1为本发明实施例中的基于因子图的多AUV自适应协同定位方法流程图；
图2为本发明实施例中的自适应算法的因子图模型示意图；
图3为本发明实施例中的f(Zk|Xk)的局部因子图模型示意图；
图4为本发明实施例中的系统结构与AUV实际轨迹示意图；
图5为本发明实施例中的动态系统结构整体变化情况示意图；
图6为本发明实施例中的定位误差对比图；
图7为本发明实施例中的X方向和Y方向上误差对比图。
具体实施方式
在本发明的描述中，应当说明的是，在本发明的实施例中所提到的术语“第一”、“第二”、“第三”仅用于描述目的，并不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括一个或者多个该特征。
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
具体实施方案一：如图1所示，本发明提供一种基于因子图的多AUV自适应协同定位方法，包括如下步骤：
S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；
S2、更新从艇及其邻居主艇信息；
S3、初始化主艇与从艇信息；
S4、构建多AUV自适应协同定位系统的因子图模型；
将从艇状态变量及主艇量测信息定义为变量节点，状态方程和量测方程定义为函数节点，采用状态方程函数节点对从艇状态变量节点Xk进行传递更新，采用量测方程函数节点对主艇量测信息节点Zk及从艇状态变量节点Xk进行融合更新；同时，构建自适应迭代估计函数节点Ik，基于自适应EKF滤波器，并采用EM算法估计量测噪声协方差矩阵；
S5、基于和积算法在多AUV自适应协同定位系统的因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；
S6、基于所述自适应迭代估计函数节点对量测噪声协方差矩阵进行迭代更新；
S7、将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。
如图2所示，本实施方案中，所述因子图模型中k-1时刻与k时刻的从艇状态变量节点Xk-1与节点Xk通过状态方程函数节点f(Xk|Xk-1)相连接；主艇量测信息节点Zk与从艇状态变量节点Xk之间通过量测方程函数节点f(Zk|Xk)相连接；从艇状态变量节点Xk-1、Xk及主艇量测信息节点Zk通过自适应迭代估计函数节点Ik相连接。
如图3所示，结构Ⅰ是函数节点f(Zk|Xk)分解后的具体结构，完成主AUV与从AUV的数据融合；结构Ⅱ是量测信息Zk的具体结构，包括k时刻的各个主AUV对应的测距信息。
具体实施方案二：S1中采集多AUV协同定位系统当前时刻动态拓扑结构信息，包括：主AUV及从AUV的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及目标从艇与每艘主艇之间的测距信息d，主AUV和从AUV的数量，采集周期为T。本实施方案其它与具体实施方案一相同。
本实施方案中目标从AUV的邻居主艇数量的确定方法为：设置以目标从AUV为中心、选定直径的圆形区域，确定其邻居主艇数量。
具体实施方案三：S3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。本实施方案其它与具体实施方案二相同。
具体实施方案四：S5包括如下过程：
在第k时刻系统条件概率密度函数分解为：
式中，N表示主AUV节点数量；表示第n(n＝1,2,…,N)艘主AUV的量测信息；Xm,n表示第n(n＝1,2,…,N)艘主AUV的位置信息；fi表示各个AUV节点对应的概率因子，即：
式中，hi(.)表示量测函数；zi表示量测真实值；∑i表示对应量测误差的协方差矩阵；
定义系统采集到的测距信息d、航向角θ和从AUV的速度v均服从高斯分布：
其中di表示从AUV与第i艘主AUV之间的测距信息；
通过状态方程函数节点f(Xk|Xk-1)向变量节点Xk传递的信息为：
变量节点Xk向状态方程函数节点f(Xk|Xk-1)传递的信息为：
式中，和/>分别代表状态变量Xk的先验估计和方差；
根据协同定位的状态方程，
式中，(xk,yk)表示k时刻AUV在参考坐标系的坐标；vk表示k时刻AUV前向速度；θk表示k时刻AUV的航向角度；Δt表示采样间隔。
得到状态转换公式为：
式中，Qk是系统过程噪声协方差矩阵，Rk为量测噪声协方差矩阵，的表达式为：
式中，θk是式是k时刻对应的航向角信息；
将式(6)和式(7)代入式(5)，得到：
式中，Sk的表达式为：
本实施方案其它与具体实施方案一相同。
具体实施方案五：S6中根据EKF滤波算法构建从艇状态变量方程，即：
式中，表示k-1时刻得到的k时刻的估计值，F表示状态转移矩阵，uk表示控制输入，/>
构建相应的估计误差协方差矩阵，即：
Pk|k-1＝FPk|k-1FT+Qk-1 (12)
更新从艇状态变量和估计误差协方差矩阵。
在未接收到量测信息时，采用航位推算进行位置信息的更新
根据EM算法，首先确定初始值：
进行第l+1步滤波增益矩阵的迭代更新：
式中，为：
利用式(14)更新状态变量：
更新误差协方差矩阵：
估计量测噪声协方差矩阵Rk：
迭代N次收敛后得到Rk的估计结果：
本实施方案其它与具体实施方案四相同。
具体实施方案六：如图3所示，S7包括如下过程：
k时刻系统中从AUV与第i艘主AUV之间的坐标差为和/>变量节点/>和/>分别通过函数节点Ci完成信息更新，计算变量节点/>得到信度信息为：
式中和/>分别代表/>和/>的标准差，/>表示k时刻下从AUV与第i艘主AUV之间的距离；
函数节点Ci向变量节点传递的信息，计算变量节点/>传递的信度信息为：
式中代表/>的标准差；
通过函数节点Ai和Bi进行位置信息的转换，即函数节点Ai传递到变量节点和计算变量节点/>和/>的信度信息分别为：
计算变量节点和/>的信度信息分别为：
通过函数节点D和E将各主艇对从艇的位置估计与从艇位置的先验估计进行结合，得到最终位置估计：将各主艇对从艇的位置估计传递到xk，即：
式中，和/>为xk的方差和期望；
同样的，yk的信息为：
式中，和/>为yk的方差和期望；
对从艇的位置估计和从艇航位推算估计进行加权平均：
本实施方案其它与具体实施方案五相同。
具体实施方案七：S7中k时刻下从AUV与第i艘主AUV之间的距离与/>和/>的关系为：
本实施方案其它与具体实施方案六相同。
具体实施方案八：一种基于因子图的多AUV自适应协同定位系统，该系统具有与上述具体实施方案一至七中任一项的步骤对应的程序模块，运行时执行上述的基于因子图的多AUV自适应协同定位方法中的步骤。
具体实施方案九：一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现具体实施方案一至七中任一项所述的基于因子图的多AUV自适应协同定位方法的步骤。
实施例1
将本发明的基于因子图的多AUV自适应协同定位方法(Dynamic Structure-basedAdaptive Factor Graph,DS-AFG)、传统的扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)协同定位算法及基于本发明的未进行量测噪声协方差矩阵处理的动态拓扑下基于因子图的多AUV协同定位方法(DynamicStructure-based Factor Graph,DSFG)分别进行仿真分析。
算法的基本参数设置为：设计了10艘主艇和20艘从艇的协同定位系统仿真实验。设置总仿真时长为1000s，状态更新周期Δt＝1s。动态拓扑结构检测周期ΔT＝5s。为了满足系统可观性，设计如图4所示的轨迹图，主AUV做匀速运动，均速度为vm＝2m/s，从AUV做S型曲线运动，速度为vs＝6m/s；为了实现系统结构的动态拓扑，设置从AUV的信息处理范围为以自身为中心的直径3000m的圆形区域，系统动态变化情况如图5所示；基于水声测距场景，设定主AUV与从AUV的速度和航向角的测距方差分别为和/>在DS-AFG方法对比实验中，为了模拟实际应用场景下的模糊时变噪声，设定量测噪声的均值为μd＝1m，方差为/>量测噪声的方差由正态分布随机数生成，符合模糊时变噪声特点。
仿真结果与分析
将DS-AFG方法与EKF、UKF和DSFG三种对比算法分别应用于模糊时变噪声下的动态拓扑多AUV协同定位，仿真得到如图6与图7所示的误差结果，并在表1中列出了各方法的均方根误差(Root Mean Square Error，RMSE)结果。可以看出，DS-AFG算法的定位误差最小，其定位RMSE比DSFG、UKF和EKF算法分别减小29.68％、34.39％和60.18％。这是由于DS-AFG算法能够通过估计量测噪声的协方差矩阵，减小模糊时变噪声对于系统的干扰，达到更高的定位精度。
表1
如表2所示为各方法的运行时间。可以看出，DS-AFG算法耗时最长，这是由于其添加了自适应滤波处理步骤，需要迭代估计量测量测噪声协方差矩阵，因此导致耗时略大于其他算法，但在可接受范围内。
表2
经过仿真实验验证可以发现，本发明的DS-AFG方法因为增加了自适应滤波处理步骤，虽然耗时略大于其他算法，但是整体定位效果显著优于其他算法，并且具有较好的抗干扰能力，自适应性更强。
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本发明领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。
Claims (9)
1.一种基于因子图的多AUV自适应协同定位方法，其特征在于，包括如下步骤：
S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；
S2、更新从艇及其邻居主艇信息；
S3、初始化主艇与从艇信息；
S4、构建多AUV自适应协同定位系统的因子图模型；
将从艇状态变量及主艇量测信息定义为变量节点，状态方程和量测方程定义为函数节点，采用状态方程函数节点对从艇状态变量节点Xk进行传递更新，采用量测方程函数节点对主艇量测信息节点Zk及从艇状态变量节点Xk进行融合更新；同时，构建自适应迭代估计函数节点，基于自适应EKF滤波器，并采用EM算法估计量测噪声协方差矩阵；
S5、基于和积算法在多AUV自适应协同定位系统的因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；
S6、基于所述自适应迭代估计函数节点对量测噪声协方差矩阵进行迭代更新；
S7、将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。
2.根据权利要求1所述的基于因子图的多AUV自适应协同定位方法，其特征在于，S1中采集多AUV协同定位系统当前时刻动态拓扑结构信息，包括：主AUV及从AUV的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及目标从艇与每艘主艇之间的测距信息d，主AUV和从AUV的数量，采集周期为T。
3.根据权利要求2所述的基于因子图的多AUV自适应协同定位方法，其特征在于，S3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。
4.根据权利要求1所述基于因子图的多AUV自适应协同定位方法，其特征在于，S5包括如下过程：
在第k时刻系统条件概率密度函数分解为：
式中，N表示主AUV节点数量；表示第n(n＝1,2,…,N)艘主AUV的量测信息；Xm,n表示第n(n＝1,2,…,N)艘主AUV的位置信息；fi表示各个AUV节点对应的概率因子，即：
式中，hi(.)表示量测函数；zi表示量测真实值；∑i表示对应量测误差的协方差矩阵；
定义系统采集到的测距信息d、航向角θ和从AUV的速度v均服从高斯分布：
其中di表示从AUV与第i艘主AUV之间的测距信息；
通过状态方程函数节点f(Xk|Xk-1)向变量节点Xk传递的信息为：
变量节点Xk向状态方程函数节点f(Xk|Xk-1)传递的信息为：
式中，和/>分别代表状态变量Xk的先验估计和方差；
根据协同定位的状态方程，
式中，(xk,yk)表示k时刻AUV在参考坐标系的坐标；vk表示k时刻AUV前向速度；θk表示k时刻AUV的航向角度；Δt表示采样间隔。
得到状态转换公式为：
式中，Qk是系统过程噪声协方差矩阵，Rk为量测噪声协方差矩阵，的表达式为：
式中，θk是式是k时刻对应的航向角信息；
将式(6)和式(7)代入式(5)，得到：
式中，Sk的表达式为：
5.根据权利要求3所述的基于因子图的多AUV自适应协同定位方法，其特征在于，S6中根据EKF滤波算法构建从艇状态变量方程，即：
式中，表示k-1时刻得到的k时刻的估计值，F表示状态转移矩阵，uk表示控制输入，/>
构建相应的估计误差协方差矩阵，即：
Pk|k-1＝FPk|k-1FT+Qk-1 (12)
更新从艇状态变量和估计误差协方差矩阵。
根据EM算法，首先确定初始值：
进行第l+1步滤波增益矩阵的迭代更新：
式中，为：
利用式(14)更新状态变量：
更新误差协方差矩阵：
估计量测噪声协方差矩阵Rk：
迭代N次收敛后得到Rk的估计结果：
6.根据权利要求5所述的基于因子图的多AUV自适应协同定位方法，其特征在于，S7包括如下过程：
k时刻系统中从AUV与第i艘主AUV之间的坐标差为和/>计算变量节点/>得到信度信息为：
式中和/>分别代表/>和/>的标准差，/>表示k时刻下从AUV与第i艘主AUV之间的距离；
计算变量节点传递的信度信息为：
式中代表/>的标准差；
计算变量节点和/>的信度信息分别为：
计算变量节点和/>的信度信息分别为：
将各主艇对从艇的位置估计传递到xk，即：
式中，和/>为xk的方差和期望；
同样的，yk的信息为：
式中，和/>为yk的方差和期望；
对从艇的位置估计和从艇航位推算估计进行加权平均：
7.根据权利要求6所述的基于因子图的多AUV自适应协同定位方法，其特征在于，S7中k时刻下从AUV与第i艘主AUV之间的距离与/>和/>的关系为：
8.一种基于因子图的多AUV自适应协同定位系统，其特征在于，该系统具有与上述权利要求1～7任一项权利要求的步骤对应的程序模块，运行时执行上述的基于因子图的多AUV自适应协同定位方法中的步骤。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1～7中任一项所述的基于因子图的多AUV自适应协同定位方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310851207.9A CN116734860A (zh) | 2023-07-12 | 2023-07-12 | 一种基于因子图的多auv自适应协同定位方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310851207.9A CN116734860A (zh) | 2023-07-12 | 2023-07-12 | 一种基于因子图的多auv自适应协同定位方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116734860A true CN116734860A (zh) | 2023-09-12 |
Family
ID=87911563
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310851207.9A Pending CN116734860A (zh) | 2023-07-12 | 2023-07-12 | 一种基于因子图的多auv自适应协同定位方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116734860A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117739978A (zh) * | 2023-12-06 | 2024-03-22 | 无锡学院 | 一种基于因子图的多auv并行式协同导航定位方法及系统 |
| CN118310526A (zh) * | 2024-04-09 | 2024-07-09 | 哈尔滨工业大学（威海） | 一种动态拓扑下基于因子图的多auv量测滤波方法及系统 |
-
2023
- 2023-07-12 CN CN202310851207.9A patent/CN116734860A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117739978A (zh) * | 2023-12-06 | 2024-03-22 | 无锡学院 | 一种基于因子图的多auv并行式协同导航定位方法及系统 |
| CN118310526A (zh) * | 2024-04-09 | 2024-07-09 | 哈尔滨工业大学（威海） | 一种动态拓扑下基于因子图的多auv量测滤波方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
| CN116734860A (zh) | 一种基于因子图的多auv自适应协同定位方法及系统 | |
| CN108896047B (zh) | 分布式传感器网络协同融合与传感器位置修正方法 | |
| CN116772867A (zh) | 一种基于因子图的节点优选的多auv自适应协同定位方法及系统 | |
| CN112328959B (zh) | 一种基于自适应扩展卡尔曼概率假设密度滤波器的多目标跟踪方法 | |
| CN116543016A (zh) | 一种基于深度强化学习的目标跟踪状态估计器 | |
| CN116520281B (zh) | 一种基于ddpg的扩展目标跟踪优化方法和装置 | |
| CN115204212A (zh) | 一种基于stm-pmbm滤波算法的多目标跟踪方法 | |
| CN115930949A (zh) | 一种多传感器分布式协同探测方法、系统及电子设备 | |
| CN117036400A (zh) | 基于混合高斯模型模糊聚类数据关联的多目标群跟踪方法 | |
| CN116659513A (zh) | 一种基于节点优化选择与因子图的多auv协同定位方法及系统 | |
| CN111736144B (zh) | 一种仅用距离观测的机动转弯目标状态估计方法 | |
| CN116793366A (zh) | 一种动态拓扑下基于因子图的多auv协同定位方法及系统 | |
| CN111722213B (zh) | 一种机动目标运动参数的纯距离提取方法 | |
| CN117892223A (zh) | 一种基于Sage-Husa自适应滤波的交互式多模型水下目标跟踪方法 | |
| CN111883265A (zh) | 一种应用于火控系统的目标状态估计方法 | |
| CN114415157B (zh) | 一种基于水声传感器网络的水下目标多模型跟踪方法 | |
| CN114061592B (zh) | 基于多模型的自适应鲁棒auv导航方法 | |
| CN115544425A (zh) | 一种基于目标信噪比特征估计的鲁棒多目标跟踪方法 | |
| CN115114985A (zh) | 一种基于集合理论的传感器系统分布式融合方法 | |
| CN111695617B (zh) | 一种基于改进的协方差交叉算法的分布式火控融合方法 | |
| Li et al. | Random time delay effect on Out-of-Sequence measurements | |
| CN115906413B (zh) | 一种Dirichlet过程混合模型节点自定位方法 | |
| Bogomolov | Test Results of the Long Baseline Navigation Solutions under a Large a Priori Position Uncertainty | |
| CN112491393B (zh) | 一种基于观测噪声协方差矩阵未知的线性滤波方法 |
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 |