WO2022205526A1 - 一种用于水下无人机器人集群的动态定位信息融合方法 - Google Patents
一种用于水下无人机器人集群的动态定位信息融合方法 Download PDFInfo
- Publication number
- WO2022205526A1 WO2022205526A1 PCT/CN2021/088258 CN2021088258W WO2022205526A1 WO 2022205526 A1 WO2022205526 A1 WO 2022205526A1 CN 2021088258 W CN2021088258 W CN 2021088258W WO 2022205526 A1 WO2022205526 A1 WO 2022205526A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- value
- local
- posterior
- auv
- filter
- Prior art date
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims abstract description 43
- 238000000034 method Methods 0.000 claims abstract description 43
- 238000004891 communication Methods 0.000 claims abstract description 36
- 238000001914 filtration Methods 0.000 claims abstract description 14
- 230000004927 fusion Effects 0.000 claims abstract description 5
- 238000012360 testing method Methods 0.000 claims description 15
- 230000006870 function Effects 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 9
- 238000005259 measurement Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000012544 monitoring process Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 239000000178 monomer Substances 0.000 claims description 3
- 230000002194 synthesizing effect Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 239000003795 chemical substances by application Substances 0.000 claims description 2
- 238000005457 optimization Methods 0.000 claims description 2
- 230000015572 biosynthetic process Effects 0.000 description 3
- 238000005755 formation reaction Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005265 energy consumption Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000032683 aging Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000009792 diffusion process Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
Definitions
- the invention relates to the technical field of unmanned robot swarms, in particular to a dynamic positioning information fusion method for underwater unmanned robot swarms.
- AUV advanced ultrasonic vision
- multi-AUV has some advantages that a single AUV does not have, such as higher fault tolerance, robustness, efficient operation, reconfiguration, distributed perception and coordination, and wider application fields.
- multi-AUV cluster co-location based on acoustic communication networks has completely different technical characteristics and difficulties.
- Single AUV only has local perception and communication capabilities, and underwater acoustic communication has problems such as narrow channel bandwidth, low data transmission rate, long measurement time, and poor real-time performance.
- the methods of centralized decision-making and global decision-making are not suitable for the submarine environment. the following needs.
- the deep-sea environment is complex and changeable, which leads to the sparse and dynamic characteristics of the communication of the AUV cluster formation.
- the message passing method in the distributed co-location method relies on the tree or ring communication topology, so for AUVs with dynamic topology Cluster formations are hardly applicable.
- the present invention provides a dynamic positioning information fusion method for underwater unmanned robot swarms.
- Each node of the communication network is embedded with a local unscented filter and a weighted average consistency filter.
- the local unscented filter uses the local multi-source observation information of each AUV node to obtain a local unbiased estimate of the location status of the tested node.
- the weighted average consistency filter uniformly fuses all local posterior estimates, so that the global posterior estimate result is output in the form of information matrix weighting the posterior estimate mean.
- the present invention provides the following technical solutions:
- a dynamic positioning information fusion method for an underwater unmanned robot swarm comprising the following steps:
- Step 1 Build the state space model of the multi-AUV cooperative positioning system and the communication network model based on the average network undirected graph, comprehensively consider the dynamic characteristics of the platform, determine the input parameters, add Gaussian noise, and establish the equation of motion;
- Step 2 Threshold weighting to eliminate gross errors in local information: A threshold weighting module is added to the back end of local observations to optimize special node information;
- Step 3 Local information filter: Use unscented transformation to approximate the probability density of the function to obtain the expectation and variance of the target event, transform the nonlinear problem into a Kalman filter problem, and output the local posterior value;
- Step 4 Weighted average consistency filter: The weighted average consistency filter uniformly fuses all local posterior estimates, so that the global posterior estimate result is output in the form of weighting the posterior estimate mean by the information matrix;
- Step 5 Optimize the global posterior result: when the consistency filter outputs the global posterior estimated value and covariance, determine whether it corresponds to the latest time and optimize it to generate a stable estimated value as the input value of the local filter , to solve the problem of filter asynchrony.
- step 1 the deep-sea sparse dynamic wireless communication network is described based on the average network undirected graph model, and the sparsity is described by the effective communication connection edge set between any nodes.
- the state space model of any node is as follows:
- x k is the real state variable of the AUV under test
- observation variable is the real observation value of the AUV under test
- state equation and the observation equation are both nonlinear functions
- Q k-1 is the prediction noise
- R k is the observation noise, assuming that both are Gaussian white noise, satisfying the normal distributed
- the change value of the elements in this matrix can clearly reflect the random dynamic of the sparse network. change, and the average Laplace matrix is a positive semi-definite matrix, and its minimum eigenvalue greater than zero is a necessary and sufficient condition for the average network undirected graph to be connected;
- the convergence of the consistency method is described from the perspective of mean square convergence.
- the communication topology changes randomly and dynamically with a certain probability, so it is more suitable to examine the convergence of the consistency method from the perspective of mean square convergence. If the average network undirected graph is connected, the average consensus method can make the mean square of all node states converge to the average consensus value.
- the present invention is further improved, and the concrete steps of the local information filter in step 3 are as follows:
- L (i) represents the i-th column of matrix L, and the weight is
- L 1(i) represents the i-th column of matrix L 1 , and the weight is
- step 4 the consistency method of multi-agent clusters is used, and all local a posteriori estimates are uniformly fused through a weighted average consistency filter, so that the global posterior estimate result is weighted by the information matrix to the mean value of the posterior estimate. output in the form:
- the estimation error is a stable linear system whose Lyapunov function is
- step 5 when the consistency filter outputs the global posterior estimated value and covariance, determine its timeliness, that is, whether it corresponds to the current moment, if so, it is used as the input of the local filter;
- the estimation module generates a reliable estimated value as the posterior value of the local filter, and generates a reliable estimated value as the posterior value of the local filter, specifically:
- the estimated value of the AUV state is set to be the weighted average of the hypothetical value and the local posterior value
- This estimate enters the local method as a global posterior estimate and covariance.
- the present invention utilizes the knowledge of graph theory and topological network, combines unscented Kalman filtering and consistency method, and is fully suitable for sparse communication of multi-AUV co-location in deep water environment, random dynamic change of communication topology, and clutter interference. scenario.
- the present invention proposes an AUV observation value threshold weighting method, which reduces the measurement deviation caused by severe underwater signal conditions and dynamic and static obstacles, eliminates observation gross errors according to the AUV's own state and environmental factors, and improves the reliability of positioning information.
- the present invention introduces a global a posteriori value prediction mechanism, by judging the aging of the consistency filtering, selecting whether it enters the local filtering at the next moment, and using the estimated value to replace the lagging global posterior result, ensuring consistency
- the timeliness of the method effectively avoids the filtering asynchronous problem of local filtering and consistent filtering.
- Fig. 1 is the local information filtering method of the present invention
- Fig. 2 is the threshold weighting module of the present invention in local filter
- Fig. 3 is the parallel fusion consistent distributed filtering method of the present invention.
- FIG. 4 is an estimation module for processing filter asynchrony according to the present invention.
- a dynamic positioning information fusion method for underwater unmanned robot swarm includes the following steps:
- Step 1 Build the state space model of the multi-AUV co-location system, comprehensively consider the dynamic characteristics of the platform, determine the input parameters, add Gaussian noise, and establish the equation of motion.
- Build a communication network model based on the average network undirected graph describe the sparsity through the set of effective communication connection edges between any nodes, and describe the sparse network by monitoring the changes in the correlation probability values in the average adjacency matrix and the average Laplacian matrix. Random dynamic changes, and the convergence of the consistency method is described through the perspective of mean square convergence.
- Step 2 Eliminate gross errors of local information by threshold weighting: A threshold weighting module is added to the back end of local observations to eliminate gross errors and optimize special node information.
- Step 3 Improve the unscented Kalman filter to process local information: use the unscented transform (UT) to approximate the probability density of the function to obtain the expectation and variance of the target event, and transform the nonlinear problem into a Kalman filter problem.
- UT unscented transform
- Step 4 Weighted average consistency filter: The weighted average consistency filter uniformly fuses all local posterior estimates, so that the global posterior estimate result is output in the form of weighting the posterior estimate mean by the information matrix.
- Step 5 Optimize the global posterior result: When the consistency filter outputs the global posterior estimated value and covariance, judge whether it corresponds to the latest time. If the value corresponds to the correct time, it is used as the input of the local filter. At the current moment, the estimation module is entered to generate a stable estimated value as the input value of the local filter to solve the problem of asynchronous filtering.
- the above-mentioned step 1 is specifically: the platform state of a single AUV is composed of elements such as the position, speed, and attitude of the platform. Comprehensively consider the dynamic characteristics of the platform, determine the input parameters, add Gaussian noise, and establish the equation of motion.
- the state equation of the whole co-location system can be obtained from the state, input and noise of the whole platform.
- the observation equation of the single platform and the inter-platform is properly extended, the observation equation of the whole system can be obtained.
- the state space model is the starting point for the design of the co-location method. In the sparse dynamic wireless sensor network, the state space model of any node i is as follows:
- x k is the real state variable of the tested AUV, including position, speed, attitude and other information
- the observation variable is the real observation value of the AUV under test.
- the state equation and the observation equation are both nonlinear functions.
- Q k-1 is the prediction noise
- R k is the observation noise. It is assumed that both are Gaussian white noise and satisfy the normal distribution.
- a set of valid communication connection edges whose cardinality is the number M k of valid communication connection edges.
- M k in a sparse dynamic network satisfies the sparsity condition:
- the change value of the elements in this matrix can clearly reflect the random dynamic of the sparse network. Changes.
- the average Laplacian matrix is a positive semi-definite matrix, and its minimum eigenvalue greater than zero is a necessary and sufficient condition for the undirected graph connectivity of the average network.
- the communication topology changes randomly and dynamically with a certain probability, so it is more suitable to examine the convergence of the consistency method from the perspective of mean square convergence. If the average network undirected graph is connected, the average consensus method can make the mean square of all node states converge to the average consensus value.
- the above-mentioned step 3 is specifically: knowing the estimated expectation of the detected AUV node state x k-1 at the current moment (for the previous moment, the posterior estimated mean) and the covariance Decompose the covariance matrix which requires must be righteous;
- L (i) represents the i-th column of matrix L, and the weight is
- L 1(i) represents the i-th column of matrix L 1 , and the weight is
- the above-mentioned step 4 is specifically: considering a network composed of n AUVs, the AUV monomer estimates a signal disturbed by Gaussian noise according to the measured value, and the signal model is slightly modified in form as follows:
- the estimation error is a stable linear system whose Lyapunov function is
- step 5 is specifically as follows: under the condition that the hardware level is significantly improved, the calculation cycle will also be greatly shortened, and the latest global posterior result should be fully utilized by the system. Output a global posterior estimate at the consistency filter and covariance When , judge whether it corresponds to time k. If the value corresponds to the correct time, it will be used as the input of the local filter; if it does not correspond to the current time, it will enter the estimation module, and generate a reliable estimated value as the aftermath of the local filter. test value, specifically:
- the estimated value of the AUV state is set to be the weighted average of the hypothetical value and the local posterior value
- This estimate enters the local method as a global posterior estimate and covariance.
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种用于水下无人机器人集群的动态定位信息融合方法。通过引入动态拓扑下的一致性方法的概念，结合分布式无迹卡尔曼滤波方法，在通信网络的每个节点中都嵌入局部无迹滤波器和加权平均一致性滤波器，局部无迹滤波器利用各个AUV节点的局部多源观测信息，得到关于被测节点定位状态的局部无偏估计量；同时，加权平均一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出。
Description
本发明涉及无人机器人集群技术领域，具体地说，是一种用于水下无人机器人集群的动态定位信息融合方法。
随着AUV技术日益成熟，其面临的任务难度和复杂程度也随之提高，单个AUV由于其自身的限制已不能满足任务日益精确化、多样化、复杂化的新需求，AUV正朝着小型化、结构简单化、智能化、混合式和群体化方向发展。特别是多AUV具备一些单个AUV所不具备的优势，如更高的容错性、鲁棒性、高效率作业、可重构、分布式感知与协调、更广泛的应用领域等。
然而，与传统的陆地及空中基于无线电通信网络的协同定位方法相比，基于声学通信网络的多AUV集群协同定位具有完全不同的技术特点与难点。单体AUV仅具有局部感知和通信能力，并且水声通信存在信道带宽窄、数据传输率低、量测时间长、实时性不够好等问题，集中式决策和全局决策的方法并不适合海底环境下的需要。深海环境复杂多变，这导致了AUV集群编队的通信呈现出稀疏、动态变化的特征，而分布式协同定位方法中的消息传递方法，依赖树形或环形的通信拓扑，所以对于动态拓扑的AUV集群编队几乎无法适用。以信道滤波器为代表的消息扩散形式，只需要邻近节点之间进行单跳通信，在理论上对于通信拓扑具有较强的普适性，从而逐渐成为分布式方法的主要研究方向。专利“一种水下集群行为实验平台”中将LED灯光和低频无线电波作为双通信介质，将探测设备从高功耗的声呐改变为低功耗的LED灯带和水下相机，降低功耗，实现岸上对实验平台的实时监控，便于观察实验现象，但是面对深海环境恶劣，系统的能耗、计算量、通信量等在系统控制和协同定位方案设计中所占比重也随之大幅上升，所以在这样的严苛约束条件下，完成系统能耗、计算量、通信量的优化，对于多AUV集群系统的快速集结，甚至调整编队结构，有着十分重要的意义。
发明内容
为了解决上述技术问题，本发明提供了一种用于水下无人机器人集群的动态定位信息融合方法，通过引入动态拓扑下的一致性方法的概念，结合分布式无迹卡尔曼滤波方法，在通信网络的每个节点中都嵌入局部无迹滤波器和加权平均一 致性滤波器，局部无迹滤波器利用各个AUV节点的局部多源观测信息，得到关于被测节点定位状态的局部无偏估计量；同时，加权平均一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出。
为实现上述目的，本发明提供如下技术方案：
一种用于水下无人机器人集群的动态定位信息融合方法，该方法包括以下步骤：
步骤一：搭建多AUV协同定位系统状态空间模型和基于平均网络无向图的通信网络模型，综合考虑平台的动力学特性、确定输入参变量，加入高斯噪声，建立运动方程；
步骤二：阈值加权消除局部信息的粗差：局部观测值的后端加入了阈值加权模块对特殊的节点信息进行优化；
步骤三：局部信息滤波器：利用无迹变换，对函数的概率密度作近似，求出目标事件的期望和方差，将非线性问题转变成卡尔曼滤波问题，输出局部后验值；
步骤四：加权平均一致性滤波器：加权平均一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出；
步骤五：优化全局后验结果：在一致性滤波器输出全局后验估计值及协方差时，判断其是否对应最新时刻并对其优化，产生一个稳定的预估值作为局部滤波器的输入值，以解决滤波异步问题。
本发明进一步改进，步骤一中基于平均网络无向图模型对深海稀疏动态无线通信网络进行描述，通过任意节点之间有效通信连接边集合来描述稀疏度，任意节点的状态空间模型如下：
状态方程：x
k＝f(x
k-1)+Q
k-1
观测方程：
其中x
k为被测AUV的真实状态变量，
为观测变量，为被测AUV的真实观测值，状态方程和观测方程均为非线性函数，Q
k-1为预测噪声，R
k为观测噪声，假设二者都为高斯白噪声，满足正态分布；
通过监控平均邻接矩阵和平均拉普拉斯矩阵中相关概率数值的改变来描述 稀疏网络的随机动态变化，用无向图G
k＝(V,ε
k)描述任意时刻k的稀疏动态网络通信拓扑，其中V为传感器节点集合，其基数为传感器节点数量N，ε
k为当前时刻节点之间的有效通信连接边集合，其基数为有效通信连接边的数量M
k，稀疏动态网络中M
k，满足稀疏条件：
综合前k时刻的邻接矩阵可以获得其平均邻接矩阵，与平均度矩阵作差即可获得平均拉普拉斯矩阵(L＝D-A)，该矩阵中元素的变化值可以清晰反映出稀疏网络随机动态变化情况，而且平均拉普拉斯矩阵是半正定矩阵，其最小特征值大于零是平均网络无向图连通的充要条件；
通过均方收敛的角度描述一致性方法的收敛性，在平均网络无向图中，通信拓扑以一定概率发生着随机动态变化，所以更适合从均方收敛的角度考察一致性方法的收敛性，若平均网络无向图是连通的，则平均一致性方法能使所有节点状态均方收敛于平均一致值。
本发明进一步改进，步骤二中局部观测值的后端加入了阈值加权模块对特殊的节点信息进行优化，假设待测信号对应时刻k-2，依据k-2时刻被跟踪节点的位置、速度、艏向角等状态量和采样周期以及洋流等环境干扰下的最大误差
计算出k-1时刻跟踪节点的预估期望值存在范围，设定阈值
计算出当前时刻跟踪节点的预估期望值存在范围，根据前一时刻的速度与信号采样周期设定阈值，依据前后两时刻下量测目标跟本体的距离l
k-1以及l
k-2，设定权值
其中Δl＝
|l
k-1-l
k-2|，若观测信号超过阈值，则产生一个虚拟的位置期望值
代替观测值，参与局部滤波计算，规避遥远信号过于失真的风险。
本发明进一步改进，步骤三中局部信息滤波器的具体步骤如下：
已知当前时刻被探测AUV节点状态x
k-1的预估期望(对于前一时刻，为后验估计均值)和协方差
将协方差矩阵分解
其中要求
必须要正定；
1)为了将该AUV的状态传递函数f(x
k-1)近似为正态分布，进行UT变换，取sigma点
L
(i)代表矩阵L的第i列，权值为
2)
则
得到被探测节点状态先验值
的近似概率密度，其满足正态分布
5)同理第一步，将
分解，令
6)
L
1(i)代表矩阵L
1的第i列，权值为
5)
则
k时刻被测节点的观测值的先验期望
其先验方差
6)观测到被测AUV节点的观测值y
m
7)
记k＝P
xy(P
y)
-1
8)k时刻被测AUV节点的状态后验期望
其后验方差
至此，k时刻的状态后验估计值和协方差已经获得，可进入下一个循环，继续递推。
本发明进一步改进，步骤四中运用多智能体集群的一致性方法，通过加权平均一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出：
考虑由n个AUV组成的网络，AUV单体根据测量值对一被高斯噪声干扰的信号进行估计，该信号模型稍作形式上的更改如下：
x(k+1)＝x(k)+w(k)
每个AUV对信号的测量值为
z
i(k)＝H
i(k)x(k)+v
i(k)
其中，过程噪声和测量值分别为
E[w(k)w(l)
T]＝Q(k)δ
kl，
E[v
i(k)v
j(l)
T]＝R
i(k)δ
klδ
ij
其中当k＝l时，δ
kl＝1；否则，δ
kl＝0。
考虑由n个AUV组成的网络，假设n(A,H)可观测；假设每个AUV应用以下分布式估计方法
且初始条件P
i＝P
0，
则，估计误差
是一个稳定的线性系统，其Lyapunov函数为
本发明进一步改进，步骤五在一致性滤波器输出全局后验估计值及协方差时，判断其时效性，即是否对应当前时刻，若是，则作为局部滤波器的输入；若不是，则进入预估模块，产生一个可靠的预估值作为局部滤波器的后验值，根据产生一个可靠的预估值作为局部滤波器的后验值，具体为：
1)计算k-1时刻被测AUV状态的全局估计值的变化量Δ；
2)保持在变化量Δ下的k时刻得到被测AUV的状态假设值；
3)设定AUV状态的预估值为假设值与局部后验值的加权平均值；
4)该预估值作为全局后验估计值和协方差进入局部方法。
本发明的有益效果：
(1)本发明运用了图论和拓扑网络的知识，结合了无迹卡尔曼滤波和一致性方法，充分适用于深水环境下多AUV协同定位的稀疏通信、通信拓扑随机动态变化、杂波干扰的情景。
(2)本发明提出AUV观测值阈值加权方法，降低了由水下信号条件严苛以及动静态障碍造成的测量偏差，依据AUV自身状态和环境因素消除观测粗差，提高定位信息的可靠性。
(3)本发明引入全局后验值预估机制，通过判断一致性滤波的时效，选择其是否进入下一时刻的局部滤波，并利用预估值替代滞后的全局后验结果，保证了一致性方法的时效性，有效避免了局部滤波和一致性滤波的滤波异步问题。
图1是本发明的局部信息滤波方法；
图2是本发明在局部滤波器中的阈值加权模块；
图3是本发明的并行融合一致性分布式滤波方法；
图4是本发明用于处理滤波异步的预估模块。
为了加深对本发明的理解，下面将结合实施例对本发明做进一步详细描述，该实施例仅用于解释本发明，并不对本发明的保护范围构成限定。
实施例：如图1至图4所示，一种用于水下无人机器人集群的动态定位信息融合方法，该方法包括以下步骤：
步骤一：搭建多AUV协同定位系统状态空间模型，综合考虑平台的动力学特性、确定输入参变量，加入高斯噪声，建立运动方程。搭建基于平均网络无向图的通信网络模型，通过任意节点之间有效通信连接边集合来描述稀疏度，通过监控平均邻接矩阵和平均拉普拉斯矩阵中相关概率数值的改变来描述稀疏网络的随机动态变化，通过均方收敛的角度描述一致性方法的收敛性。
步骤二：利用阈值加权消除局部信息的粗差：局部观测值的后端加入了阈值加权模块进行粗差消除，对特殊的节点信息进行优化。
步骤三：改进无迹卡尔曼滤波处理局部信息：利用无迹变换(UT)，对函数的概率密度作近似，求出目标事件的期望和方差，将非线性问题转变成卡尔曼滤 波问题。
步骤四：加权平均一致性滤波器：加权平均一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出。
步骤五：优化全局后验结果：在一致性滤波器输出全局后验估计值及协方差时，判断其是否对应最新时刻，如果该值对应时刻正确，即作为局部滤波器的输入，如果不对应当前时刻，则进入预估模块，产生一个稳定的预估值作为局部滤波器的输入值，以解决滤波异步问题。
本实施例中，上述步骤一具体为：单AUV的平台状态由平台的位置、速度、姿态等元素组成。综合考虑平台的动力学特性、确定输入参变量，加入高斯噪声，建立运动方程。由全体平台的状态、输入和噪声可以得到整个协同定位系统的状态方程。同理，当把单平台和平台间观测方程进行适当扩展，便可得到整个系统的观测方程。状态空间模型是协同定位方法设计的出发点，稀疏动态无线传感器网络中，任意节点i的状态空间模型如下：
状态方程：x
k＝f(x
k-1)+Q
k-1
观测方程：
其中，x
k为被测AUV的真实状态变量，包含位置、速度、姿态等信息，
为观测变量，为被测AUV的真实观测值，考虑到航迹推位和相对距离量测模型的非线性，状态方程和观测方程均为非线性函数。Q
k-1为预测噪声，R
k为观测噪声，假设二者都为高斯白噪声，满足正态分布。
首先，用无向图G
k＝(V,ε
k)描述任意时刻k的稀疏动态网络通信拓扑，其中V为传感器节点集合，其基数为传感器节点数量N，ε
k为当前时刻节点之间的有效通信连接边集合，其基数为有效通信连接边的数量M
k。稀疏动态网络中M
k，满足稀疏条件：
综合前k时刻的邻接矩阵可以获得其平均邻接矩阵，与平均度矩阵作差即可获得平均拉普拉斯矩阵(L＝D-A)，该矩阵中元素的变化值可以清晰反映出稀疏网络随机动态变化情况。而且平均拉普拉斯矩阵是半正定矩阵，其最小特征值大于零是平均网络无向图连通的充要条件。
在平均网络无向图中，通信拓扑以一定概率发生着随机动态变化，所以更适合从均方收敛的角度考察一致性方法的收敛性。若平均网络无向图是连通的，则平均一致性方法能使所有节点状态均方收敛于平均一致值。
本实施例中，上述步骤二具体为：假设待测信号对应时刻k-2，依据k-2时刻被跟踪节点的位置、速度、艏向角等状态量和采样周期以及洋流等环境干扰下的最大误差
计算出k-1时刻跟踪节点的预估期望值存在范围，设定阈值
依据前后两时刻下量测目标跟本体的距离l
k-1以及l
k-2，设定权值
其中Δl＝|l
k-1-l
k-2|。如果观测信号超过阈值，则产生一个虚拟的位置期望值
代替观测值，参与局部滤波计算，规避遥远信号过于失真的风险。
本实施例中，上述步骤三具体为：已知当前时刻被探测AUV节点状态x
k-1的预估期望(对于前一时刻，为后验估计均值)和协方差
将协方差矩阵分解
其中要求
必须要正定；
1)为了将该AUV的状态传递函数f(x
k-1)近似为正态分布，进行UT变换，取sigma点
L
(i)代表矩阵L的第i列，权值为
2)
则
得到被探测节点状态先验值
的近似概率密度，其满足正态分布
3)同理第一步，将
分解，令
4)
L
1(i)代表矩阵L
1的第i列，权值为
5)
则
k时刻被测节点的观测值的先验期望
其先验方差
6)观测到被测AUV节点的观测值y
m
7)
记k＝P
xy(P
y)
-1
8)k时刻被测AUV节点的状态后验期望
其后验方差
至此，k时刻的状态后验估计值和协方差已经获得，可进入下一个循环，继续递推。
本实施例中，上述步骤四具体为：考虑由n个AUV组成的网络，AUV单体根据测量值对一被高斯噪声干扰的信号进行估计，该信号模型稍作形式上的更改如下：
x(k+1)＝x(k)+w(k)
每个AUV对信号的测量值为
z
i(k)＝H
i(k)x(k)+v
i(k)
其中，过程噪声和测量值分别为
E[w(k)w(l)
T]＝Q(k)δ
kl，
E[v
i(k)v
j(l)
T]＝R
i(k)δ
klδ
ij
其中当k＝l时，δ
kl＝1；否则，δ
kl＝0。
考虑由n个AUV组成的网络，假设n(A,H)可观测。假设每个AUV应用以下分布式估计方法
且初始条件P
i＝P
0，
则，估计误差
是一个稳定的线性系统，其Lyapunov函数为
本实施例中，上述步骤五具体为：在硬件水平显著改善的情况下，计算周期也会大大缩短，最新的全局后验结果应当被系统充分利用。在一致性滤波器输出全局后验估计值
及协方差
时，判断其是否对应k时刻，如果该值对应时刻正确，即作为局部滤波器的输入；如果不对应当前时刻，则进入预估模块，根据产生一个可靠的预估值作为局部滤波器的后验值，具体为：
1)计算k-1时刻被测AUV状态的全局估计值的变化量Δ；
2)保持在变化量Δ下的k时刻得到被测AUV的状态假设值；
3)设定AUV状态的预估值为假设值与局部后验值的加权平均值；
4)该预估值作为全局后验估计值和协方差进入局部方法。
以上所述的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述。对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何标记视为限制所涉及的权利要求。
Claims (6)
- 一种用于水下无人机器人集群的动态定位信息融合方法，其特征在于，该方法包括以下步骤：步骤一：搭建多AUV协同定位系统状态空间模型和基于平均网络无向图的通信网络模型，综合考虑平台的动力学特性、确定输入参变量，加入高斯噪声，建立运动方程；步骤二：阈值加权消除局部信息的粗差：局部观测值的后端加入了阈值加权模块对特殊的节点信息进行优化；步骤三：局部信息滤波器：利用无迹变换，对函数的概率密度作近似，求出目标事件的期望和方差，将非线性问题转变成卡尔曼滤波问题，输出局部后验值；步骤四：加权平均一致性滤波器：加权平均一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出；步骤五：优化全局后验结果：在一致性滤波器输出全局后验估计值及协方差时，判断其是否对应最新时刻并对其优化，产生一个稳定的预估值作为局部滤波器的输入值，以解决滤波异步问题。
- 根据权利要求1所述的一种用于水下无人机器人集群的动态定位信息融合方法，其特征在于，所述步骤一中基于平均网络无向图的通信网络模型对深海稀疏动态无线通信网络进行描述，通过任意节点之间有效通信连接边集合来描述稀疏度，任意节点的状态空间模型如下：状态方程：x k＝f(x k-1)+Q k-1观测方程：
其中x k为被测AUV的真实状态变量，为观测变量，为被测AUV的真实观测值，状态方程和观测方程均为非线性函数，Q k-1为预测噪声，R k为观测噪声，假设二者都为高斯白噪声，满足正态分布；
通过监控平均邻接矩阵和平均拉普拉斯矩阵中相关概率数值的改变来描述稀疏网络的随机动态变化，用无向图G k＝(V,ε k)描述任意时刻k的稀疏动态网络通信拓扑，其中V为传感器节点集合，其基数为传感器节点数量N，ε k为当前时刻节点之间的有效通信连接边集合，其基数为有效通信连接边的数量M k，稀疏动态网络中M k，满足稀疏条件：
综合前k时刻的邻接矩阵可以获得其平均邻接矩阵，与平均度矩阵作差即可获得平均拉普拉斯矩阵(L＝D-A)，该矩阵中元素的变化值可以清晰反映出稀疏网络随机动态变化情况，而且平均拉普拉斯矩阵是半正定矩阵，其最小特征值大于零是平均网络无向图连通的充要条件；通过均方收敛的角度描述一致性方法的收敛性，在平均网络无向图中，通信拓扑以一定概率发生着随机动态变化，所以更适合从均方收敛的角度考察一致性方法的收敛性，若平均网络无向图是连通的，则平均一致性方法能使所有节点状态均方收敛于平均一致值。 - 根据权利要求1所述的一种用于水下无人机器人集群的动态定位信息融合方法，其特征在于，所述步骤二中局部观测值的后端加入了阈值加权模块对特殊的节点信息进行优化，假设待测信号对应时刻k-2，依据k-2时刻被跟踪节点的位置、速度、艏向角等状态量和采样周期以及洋流等环境干扰下的最大误差计算出k-1时刻跟踪节点的预估期望值存在范围，设定阈值
计算出当前时刻跟踪节点的预估期望值存在范围，根据前一时刻的速度与信号采样周期设定阈值，依据前后两时刻下量测目标跟本体的距离l k-1以及l k-2，设定权值
其中Δl＝|l k-1-l k-2|，若观测信号超过阈值，则产生一个虚拟的位置期望值
代替观测值，参与局部滤波计算，规避遥远信号过于失真的风险。
- 根据权利要求1所述的一种用于水下无人机器人集群的动态定位信息融合方法，其特征在于，所述步骤三中局部信息滤波器的具体步骤如下：已知当前时刻被探测AUV节点状态x k-1的预估期望(对于前一时刻，为后验估计均值)和协方差将协方差矩阵分解
其中要求
必须要正定；
1)为了将该AUV的状态传递函数f(x k-1)近似为正态分布，进行UT变换，取sigma点
L (i)代表矩阵L的第i列，权值为
2)
则
得到被探测节点状态先验值的近似概率密度，其满足正态分布
3)同理第一步，将分解，令
4)
i＝1,2,…,nL 1(i)代表矩阵L 1的第i列，权值为
5)y k (i)＝f(x k-1 (i))，则k时刻被测节点的观测值的先验期望
其先验方差
6)观测到被测AUV节点的观测值y m7)
记k＝P xy(P y) -18)k时刻被测AUV节点的状态后验期望
其后验方差
至此，k时刻的状态后验估计值和协方差已经获得，可进入下一个循环，继续递推。 - 根据权利要求1所述的一种用于水下无人机器人集群的动态定位信息融合方法，其特征在于，所述步骤四中运用多智能体集群的一致性方法，通过加权平均 一致性滤波器对所有局部后验估计进行一致融合，使全局后验估计结果以信息矩阵对后验估计均值加权的形式输出：考虑由n个AUV组成的网络，AUV单体根据测量值对一被高斯噪声干扰的信号进行估计，该信号模型稍作形式上的更改如下：x(k+1)＝x(k)+w(k)每个AUV对信号的测量值为z i(k)＝H i(k)x(k)+v i(k)其中，过程噪声和测量值分别为E[w(k)w(l) T]＝Q(k)δ kl，E[v i(k)v j(l) T]＝R i(k)δ klδ ij其中当k＝l时，δ kl＝1；否则，δ kl＝0。考虑由n个AUV组成的网络，假设n(A,H)可观测；假设每个AUV应用以下分布式估计方法
且初始条件P i＝P 0，则，估计误差
是一个稳定的线性系统，其Lyapunov函数为
- 根据权利要求1所述的一种用于水下无人机器人集群的动态定位信息融合方法，其特征在于，所述步骤五在一致性滤波器输出全局后验估计值及协方差时，判断其时效性，即是否对应当前时刻，若是，则作为局部滤波器的输入；若不是，则进入预估模块，产生一个可靠的预估值作为局部滤波器的后验值，根据产生一个可靠的预估值作为局部滤波器的后验值，具体为：1)计算k-1时刻被测AUV状态的全局估计值的变化量Δ；2)保持在变化量Δ下的k时刻得到被测AUV的状态假设值；3)设定AUV状态的预估值为假设值与局部后验值的加权平均值；4)该预估值作为全局后验估计值和协方差进入局部方法。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110359019.5 | 2021-04-01 | ||
| CN202110359019.5A CN113048984B (zh) | 2021-04-01 | 2021-04-01 | 一种用于水下无人机器人集群的动态定位信息融合方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2022205526A1 true WO2022205526A1 (zh) | 2022-10-06 |
Family
ID=76517514
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2021/088258 WO2022205526A1 (zh) | 2021-04-01 | 2021-04-20 | 一种用于水下无人机器人集群的动态定位信息融合方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN113048984B (zh) |
| WO (1) | WO2022205526A1 (zh) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115797749A (zh) * | 2023-02-02 | 2023-03-14 | 吉林大学 | 一种水下自主航行器状态检测方法与系统 |
| CN116359642A (zh) * | 2023-03-10 | 2023-06-30 | 湖南金烽信息科技有限公司 | 一种变压器运行状态5g智能监测系统与方法 |
| CN116740922A (zh) * | 2023-05-08 | 2023-09-12 | 海南大学 | 一种基于模糊观测协议的智慧交通系统的控制方法 |
| CN117278941A (zh) * | 2023-09-15 | 2023-12-22 | 广东省机场管理集团有限公司工程建设指挥部 | 基于5g网络和数据融合的车辆驾驶辅助定位方法及装置 |
| CN117269885A (zh) * | 2023-11-23 | 2023-12-22 | 中国飞行试验研究院 | 基于机会信号融合的飞行器定位方法及装置 |
| CN117540342A (zh) * | 2024-01-05 | 2024-02-09 | 山东省科学院海洋仪器仪表研究所 | 一种蒸发波导融合短期预测方法及系统 |
| CN117686920A (zh) * | 2024-02-02 | 2024-03-12 | 河南科技学院 | 一种用于电池系统内单体电池的端电压预测方法 |
| CN117824625A (zh) * | 2024-03-05 | 2024-04-05 | 河海大学 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113673076B (zh) * | 2021-07-09 | 2022-09-02 | 华南理工大学 | 一种适用于海洋浮式结构的动力响应求解方法 |
| CN114111796B (zh) * | 2021-11-26 | 2023-10-24 | 江苏科技大学 | 一种基于信息增益的水下无人机器人的并行融合定位方法和系统 |
| CN115792796B (zh) * | 2023-02-13 | 2023-06-06 | 鹏城实验室 | 基于相对观测等效模型的协同定位方法、装置及终端 |
| CN116358564B (zh) * | 2023-06-01 | 2023-07-28 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、系统、设备及介质 |
| CN117290115A (zh) * | 2023-11-27 | 2023-12-26 | 北京航空航天大学 | 一种无人机集群分布式目标分配模型的计算方法及系统 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170225760A1 (en) * | 2014-12-15 | 2017-08-10 | Leidos, Inc. | System and Method For Fusion of Sensor Data to Support Autonomous Maritime Vessels |
| CN110702095A (zh) * | 2019-09-30 | 2020-01-17 | 江苏大学 | 一种数据驱动的高精度组合导航数据融合方法 |
| CN111397607A (zh) * | 2020-03-19 | 2020-07-10 | 哈尔滨工程大学 | 一种采用并行融合机制的信息滤波方法 |
| CN112378396A (zh) * | 2020-10-29 | 2021-02-19 | 江苏集萃未来城市应用技术研究所有限公司 | 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法 |
| CN112583380A (zh) * | 2020-12-15 | 2021-03-30 | 哈尔滨工程大学 | 一种基于收敛优化的分布式多速率粒子滤波算法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103335646A (zh) * | 2013-06-20 | 2013-10-02 | 哈尔滨工程大学 | 一种基于分散式增广信息滤波的多艇协同导航方法 |
| CN108871324A (zh) * | 2018-04-28 | 2018-11-23 | 南京信息工程大学 | 一种水下无源组合导航系统衰减自适应信息融合方法 |
| CN112527017B (zh) * | 2020-12-11 | 2022-02-11 | 中国科学院沈阳自动化研究所 | 一种基于多auv的海洋观测方法 |
-
2021
- 2021-04-01 CN CN202110359019.5A patent/CN113048984B/zh active Active
- 2021-04-20 WO PCT/CN2021/088258 patent/WO2022205526A1/zh active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170225760A1 (en) * | 2014-12-15 | 2017-08-10 | Leidos, Inc. | System and Method For Fusion of Sensor Data to Support Autonomous Maritime Vessels |
| CN110702095A (zh) * | 2019-09-30 | 2020-01-17 | 江苏大学 | 一种数据驱动的高精度组合导航数据融合方法 |
| CN111397607A (zh) * | 2020-03-19 | 2020-07-10 | 哈尔滨工程大学 | 一种采用并行融合机制的信息滤波方法 |
| CN112378396A (zh) * | 2020-10-29 | 2021-02-19 | 江苏集萃未来城市应用技术研究所有限公司 | 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法 |
| CN112583380A (zh) * | 2020-12-15 | 2021-03-30 | 哈尔滨工程大学 | 一种基于收敛优化的分布式多速率粒子滤波算法 |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115797749A (zh) * | 2023-02-02 | 2023-03-14 | 吉林大学 | 一种水下自主航行器状态检测方法与系统 |
| CN116359642A (zh) * | 2023-03-10 | 2023-06-30 | 湖南金烽信息科技有限公司 | 一种变压器运行状态5g智能监测系统与方法 |
| CN116359642B (zh) * | 2023-03-10 | 2024-01-23 | 湖南金烽信息科技有限公司 | 一种变压器运行状态5g智能监测系统与方法 |
| CN116740922B (zh) * | 2023-05-08 | 2024-04-05 | 海南大学 | 一种基于模糊观测协议的智慧交通系统的控制方法 |
| CN116740922A (zh) * | 2023-05-08 | 2023-09-12 | 海南大学 | 一种基于模糊观测协议的智慧交通系统的控制方法 |
| CN117278941A (zh) * | 2023-09-15 | 2023-12-22 | 广东省机场管理集团有限公司工程建设指挥部 | 基于5g网络和数据融合的车辆驾驶辅助定位方法及装置 |
| CN117269885A (zh) * | 2023-11-23 | 2023-12-22 | 中国飞行试验研究院 | 基于机会信号融合的飞行器定位方法及装置 |
| CN117269885B (zh) * | 2023-11-23 | 2024-02-20 | 中国飞行试验研究院 | 基于机会信号融合的飞行器定位方法及装置 |
| CN117540342A (zh) * | 2024-01-05 | 2024-02-09 | 山东省科学院海洋仪器仪表研究所 | 一种蒸发波导融合短期预测方法及系统 |
| CN117540342B (zh) * | 2024-01-05 | 2024-03-26 | 山东省科学院海洋仪器仪表研究所 | 一种蒸发波导融合短期预测方法及系统 |
| CN117686920A (zh) * | 2024-02-02 | 2024-03-12 | 河南科技学院 | 一种用于电池系统内单体电池的端电压预测方法 |
| CN117686920B (zh) * | 2024-02-02 | 2024-04-12 | 河南科技学院 | 一种用于电池系统内单体电池的端电压预测方法 |
| CN117824625A (zh) * | 2024-03-05 | 2024-04-05 | 河海大学 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
| CN117824625B (zh) * | 2024-03-05 | 2024-05-14 | 河海大学 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113048984A (zh) | 2021-06-29 |
| CN113048984B (zh) | 2023-10-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022205526A1 (zh) | 一种用于水下无人机器人集群的动态定位信息融合方法 | |
| Xiao et al. | A sampling-based Bayesian approach for cooperative multiagent online search with resource constraints | |
| Zou et al. | Moving horizon estimation meets multi-sensor information fusion: Development, opportunities and challenges | |
| Li et al. | Event-based fault-tolerant control for networked control systems applied to aircraft engine system | |
| Wan et al. | Autonomous vehicle source enumeration exploiting non-cooperative UAV in software defined internet of vehicles | |
| Xu et al. | Cooperative localization in harsh underwater environment based on the MC-ANFIS | |
| Li et al. | Distributed iterative FIR consensus filter for multiagent systems over sensor networks | |
| CN109710978B (zh) | 一种分布式异构自适应粒子滤波直接跟踪方法 | |
| CN112097774B (zh) | 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法 | |
| Fan et al. | Cooperative positioning for multi-AUVs based on factor graph and maximum correntropy | |
| CN116599602B (zh) | 一种海洋监测用低能耗长距离声纳通讯系统 | |
| Zhu et al. | Event-triggered sensor fault estimation of unreliable networked unmanned surface vehicle system with correlated noises | |
| CN111928851B (zh) | 基于tma技术的多自主水下机器人集群协同导航方法 | |
| CN117031519A (zh) | 一种多目标三维协同定位方法及系统 | |
| CN111666533A (zh) | 一种低能耗分布式图信号采样重建方法 | |
| CN108834043B (zh) | 基于先验知识的压缩感知多目标无源定位方法 | |
| Zhang et al. | Cooperative positioning of underwater unmanned vehicle clusters based on factor graphs | |
| Li et al. | Cooperative positioning algorithm of swarm UAVs based on posterior linearization belief propagation | |
| Tang et al. | Factor graph weight particles aided distributed underwater cooperative positioning algorithm | |
| CN113156368B (zh) | 一种基于因子图的误差参数辨识协同定位方法 | |
| CN115906923A (zh) | 一种基于神经网络与卡尔曼滤波融合的波束角度预测方法 | |
| Liu | Research on wsn node localization algorithm based on rssi iterative centroid estimation | |
| Zhang et al. | An improved cubature information filtering for target tracking in distributed wireless sensor networks with measurement outliers | |
| CN114415157A (zh) | 一种基于水声传感器网络的水下目标多模型跟踪方法 | |
| Shi et al. | Event-triggered fusion estimation for nonlinear stochastic system with unknown parameters and transmission fading over sensor networks |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 21934180 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 21934180 Country of ref document: EP Kind code of ref document: A1 |