CN116659513A - Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph - Google Patents

Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph Download PDF

Info

Publication number
CN116659513A
CN116659513A CN202310850563.9A CN202310850563A CN116659513A CN 116659513 A CN116659513 A CN 116659513A CN 202310850563 A CN202310850563 A CN 202310850563A CN 116659513 A CN116659513 A CN 116659513A
Authority
CN
China
Prior art keywords
auv
node
information
boat
master
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
Application number
CN202310850563.9A
Other languages
Chinese (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 Institute of Technology Weihai
Original Assignee
Harbin Institute of Technology Weihai
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 Institute of Technology Weihai filed Critical Harbin Institute of Technology Weihai
Priority to CN202310850563.9A priority Critical patent/CN116659513A/en
Publication of CN116659513A publication Critical patent/CN116659513A/en
Pending legal-status Critical Current

Links

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph, relates to the technical field of AUV co-location, and aims to solve the problems that the prior art cannot cope with dynamic changes of a system topology structure, and the system scale is large or the system communication bandwidth is limited. The method comprises the following steps: collecting dynamic topological structure information of the current moment of the system; updating the information of the slave boat and the neighbor master boat thereof; constructing a factor graph model, defining a slave boat state variable, master boat position information and master boat measurement information as variable nodes, and adopting a state equation function node to pair a slave boat state variable X k Carrying out transfer updating; for the estimated position information node of the main boat of the system and the measurement information node of the main boat, CRLB of the lower boundary of the Cramerro is respectively carried out k And calculating a ranging evaluation factor, optimizing the main boat node at the current moment, and adopting a measuring equation function node to measure the main boat measurement information node Z k From boat state variable node X k And performing fusion updating. The invention gives consideration to the positioning precision, the positioning efficiency andreal-time performance.

Description

Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph
Technical Field
The invention relates to the technical field of AUV co-location, in particular to a multi-AUV co-location method and system based on node optimization selection and factor graph.
Background
Due to the complexity of the underwater environment, the number of AUVs of the multi-AUV co-location system may change during the execution of the underwater task, resulting in dynamic changes in the system topology. Under the conditions of large number and poor quality of the main AUV nodes of the system under the dynamic topological structure, the data interaction quantity of the co-location system can be increased by communicating with all the nodes, the real-time performance of the system is reduced, and under the conditions that the node scale in the dynamic topological system with reduced co-location precision is large or the underwater bandwidth is limited and the like due to relatively low-precision location information, the data interaction quantity of the co-location system can be necessarily increased by communicating with each main AUV node in the system, and the real-time performance of the system is influenced. In the conventional collaborative algorithm, the position information of the main AUV is received from the AUV to correct the position information of the main AUV, and the position information of the main AUV received by the receiving end is often considered to be accurate. In an underwater actual environment, the position information of the main AUV node may also contain noise, and the noise contained in different main AUV node information is usually independent of each other, so that the positioning accuracy is reduced due to the fact that the main AUV node information is not selected and adopted by the traditional cooperative positioning algorithm. The optimized selection of the AUV node is an effective method for solving the problems, but the solution proposed for the optimized selection of the main AUV node is lacking, and the AUV node selection mechanism is still to be perfected. Therefore, a multi-AUV co-location method with a large number of master AUV nodes and a large system scale in a dynamic topology is needed.
Disclosure of Invention
The invention aims to solve the technical problems that:
the prior art cannot cope with the dynamic change of the system topology structure, when the system scale is large or the system communication bandwidth is limited, the communication with all the main AUV nodes in the system can bring large bandwidth pressure, the real-time performance of the system is affected, and the co-location precision is easily reduced.
The invention adopts the technical scheme for solving the technical problems:
the invention provides a multi-AUV (autonomous Underwater vehicle) co-location method based on node optimization selection and factor graph, which comprises the following steps:
s1, acquiring dynamic topology structure information of a multi-AUV co-location system at the current moment;
s2, updating information of the slave boat and neighbor master boats;
s3, initializing information of a master boat and a slave boat;
s4, constructing a factor graph model of the multi-AUV co-location system;
the state variable, the position information and the measurement information of the main boat are defined as variable nodes, the state equation and the measurement equation are defined as function nodes, and the state equation function nodes are adopted to pair the state variable nodes X of the main boat k Transfer updating is carried out, and a measuring equation function node is adopted to measure a main boat measurement information node Z k From boat state variable node X k Fusion updating is carried out; meanwhile, a node optimization function node is constructed, and a position information node phi estimated aiming at a system main boat is constructed k Main boat measurement information node Z k Respectively carrying out CRLB of the lower boundary of the Keramelteon k And calculating a ranging evaluation factor, and optimizing the main boat node at the current moment;
s5, transmitting and updating in the factor graph model of the multi-AUV co-location system based on a sum-product algorithm, and transmitting information once in two directions in the factor graph respectively to realize the transmission and updating of the node information of the global factor graph;
s6, optimizing the main boat node through the node optimization function node;
and S7, based on the optimized main boat node, fusing and updating the state variable node and the measured variable node information to obtain the estimated value of the slave boat position information at the current moment.
Further, in S1, collecting current dynamic topology information of the system in each collection period T includes: the number of the master AUV and the slave AUV, the position information, the speed information v, the angular speed information and the course angle information theta of the master AUV and the slave AUV, and calculate and detect the variance of each acquisition quantity and detect the distance measurement information d between the target slave boat and each master boat.
Further, initializing the master boat and slave boat information in S3 includes: and initializing position information, speed information v, course angle information theta and distance measurement information d between the master boat and the slave boat.
Further, S5 includes the following processes:
at the kth time the system conditional probability density function is decomposed into:
wherein N represents the number of the main AUV nodes;measurement information representing the nth (n=1, 2, …, N) master AUV; x is X m,n Position information indicating an N (n=1, 2, …, N) th master AUV; f (f) i Representing probability factors corresponding to the AUV nodes, namely:
in the formula, h i () represents a metrology function; z i Representing the measured real value; sigma (sigma) i A covariance matrix representing a corresponding measurement error;
defining ranging information d, heading angle theta and speed v of AUV of the system, and obeying Gaussian distribution:
wherein d is i Representing ranging information between the slave AUV and the ith master AUV;
node f (X) is a function of the function through the state equation k |X k-1 ) To variable node X k The information transferred is:
variable node X k To the state equation function node f (X k |X k-1 ) The information transferred is:
in the method, in the process of the invention,and->Respectively represent state variables X k Is a priori estimated and variance of (1);
according to the co-located state equation:
in (x) k ,y k ) Representing the coordinates of the AUV at the moment k in a reference coordinate system; v k Indicating AUV forward speed at time k; θ k The heading angle of AUV at the moment k is represented; Δt represents a sampling interval;
the resulting state transition formula is:
in which Q k Is the system process noise covariance matrix,for measuring noise matrix->The expression of (2) is:
in θ k The heading angle information corresponding to the moment k;
substituting formula (6) and formula (7) into formula (5), and combining to obtain:
wherein S is k The expression of (2) is:
thereby, the function node f (X k |X k-1 ) And information transfer with the state variable nodes, and finally, the global factor graph node information transfer and updating are realized.
Further, S6 includes the following processes:
clamerlo lower bound CRLB for each master AUV node is calculated separately k And ranging evaluation factor alpha i ：
Wherein X is k-1 ＝[x k-1 ,y k-1 ] T A state variable representing the slave AUV at time k-1;representing the position information of the ith main AUV at the k moment; />R X X represents k-1 Error covariance matrix of (a); r is R Φ Representation ofError covariance matrix of (a); d, d i Ranging information representing the slave AUV and the ith master AUV; />Representation d i Standard deviation of (2);
establishing a node preference parameter matrix at the moment k:
where tr (·) represents the trace of the matrix; n represents the number of main boats contained in the system at the moment k;
then weighting parameters in the node optimal parameter matrix NSPM by using an information entropy method, and firstly calculating the specific gravity p of each evaluation index i ：
Wherein r is 1 Representing CRLB evaluation parameters; r is (r) 2 Representing a ranging evaluation factor;
calculating entropy values of the parameters:
e i ＝-p i ln(p i ) (14)
calculating a difference coefficient:
g i ＝1-e i (15)
the weights of two indexes are calculated:
constructing a weight vector omega:
Ω＝[ω 1 ω 2 ] (17)
weighting the node preference parameter matrix NSPM:
Η k ＝Ω·NSPM k (18)
a 1 XN matrix H obtained by the formula (18) k In the method, the numerical value of each column corresponds to the final evaluation result of the corresponding main AUV in the system, and M minimum results are screened out, wherein the corresponding main AUV is taken as a preferable result.
Further, S7 includes the following processes:
based on the preferred main boat AUV, for k timesThe coordinate difference between the slave AUV and the ith master AUV in the engraving system isAndcalculation variable node->The confidence information is obtained as follows:
in the middle ofAnd->Respectively represent->And->Standard deviation of>Representing the distance between the slave AUV and the ith master AUV at the k moment;
computing variable nodesThe transferred credibility information is as follows:
in the middle ofRepresents->Standard deviation of (2);
computing variable nodesAnd->The confidence information of (a) is respectively:
computing variable nodesAnd->The confidence information of (a) is respectively:
estimating the position of each master boat to each slave boatTransfer to x k The method comprises the following steps:
in the method, in the process of the invention,and->Is x k Variance and expectation of (a);
similarly, y k The information of (2) is:
in the method, in the process of the invention,and->Is y k Variance and expectation of (a);
weighted averaging of the position estimate from the boat and the dead reckoning estimate from the boat:
further, the distance between the slave AUV and the ith master AUV at the k-time in S7And->And->The relation of (2) is:
a multi-AUV co-location system based on node optimization selection and factor graph has program modules corresponding to the steps of any one of the above technical solutions, and executes the steps in the multi-AUV co-location method based on node optimization selection and factor graph described above when running.
A computer readable storage medium storing a computer program configured to implement the steps of the multi-AUV co-location method based on node optimization selection and factor graph of any of the above technical solutions when invoked by a processor.
Compared with the prior art, the invention has the beneficial effects that:
the invention discloses a multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and a factor graph. The invention adopts a method based on the lower boundary of the Keramelteon and the ranging evaluation factor to screen out the high-quality main AUV node with more accurate positioning information in the system, and increases and decreases the factor graph node with a target to reduce the data interaction quantity of the system, realize the plug and play function of the sensor, ensure the positioning precision, reduce the data interaction quantity of the system and efficiently utilize the underwater communication bandwidth resource.
The factor graph can be used for efficiently and accurately modeling the complex co-location problem, and the complex global problem is solved into the local problem by using a sum-product algorithm, so that the calculation complexity is simplified, the co-location efficiency can be greatly improved, and the real-time property of AUV location is ensured.
The invention provides a solution to the problems of a dynamic topology structure of a system, a large number of main AUVs and difficult quality assurance based on a factor graph-based multi-AUV cooperative positioning method under the traditional dynamic topology, and combines the positioning precision, the positioning efficiency and the instantaneity of an algorithm.
Drawings
FIG. 1 is a flow chart of a multi-AUV co-location method based on node optimization selection and factor graph in an embodiment of the invention;
FIG. 2 is a diagram of a global factor graph model in an embodiment of the present invention;
FIG. 3 shows f (Z) k |X k ) Is a schematic diagram of a partial factor graph model;
FIG. 4 is a schematic diagram of a node preference function partial factor graph model in an embodiment of the present invention;
FIG. 5 is a schematic diagram of the system architecture and AUV actual trajectory in an embodiment of the present invention;
FIG. 6 is a schematic diagram of the overall change of the dynamic system structure according to an embodiment of the present invention;
FIG. 7 is a graph of preferred quantity versus error RMSE in an embodiment of the invention;
FIG. 8 is a comparison chart of positioning errors in an embodiment of the present invention;
fig. 9 is a graph of error contrast in the X-direction and Y-direction in an embodiment of the invention.
Detailed Description
In the description of the present invention, it should be noted that the terms "first," "second," and "third" mentioned in the embodiments of the present invention are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first", "a second", or a third "may explicitly or implicitly include one or more such feature.
In order that the above objects, features and advantages of the invention will be readily understood, a more particular description of the invention will be rendered by reference to specific embodiments thereof which are illustrated in the appended drawings.
The specific embodiment I is as follows: as shown in fig. 1, the present invention provides a multi-AUV co-location method based on node optimization selection and factor graph, comprising the following steps:
s1, acquiring dynamic topology structure information of a multi-AUV co-location system at the current moment;
s2, updating information of the slave boat and neighbor master boats;
s3, initializing information of a master boat and a slave boat;
s4, constructing a factor graph model of the multi-AUV co-location system;
the state variable, the position information and the measurement information of the main boat are defined as variable nodes, the state equation and the measurement equation are defined as function nodes, and the state equation function nodes are adopted to pair the state variable nodes X of the main boat k Transfer updating is carried out, and a measuring equation function node is adopted to measure a main boat measurement information node Z k From boat state variable node X k Fusion updating is carried out; meanwhile, a node optimization function node h (Z, phi, X) is constructed, and a position information node phi estimated aiming at a system main boat is constructed k Main boat measurement information node Z k Respectively carrying out CRLB of the lower boundary of the Keramelteon k And calculating a ranging evaluation factor, and optimizing the main boat node at the current moment;
s5, transmitting and updating in the factor graph model of the multi-AUV co-location system based on a sum-product algorithm, and transmitting information once in two directions in the factor graph respectively to realize the transmission and updating of the node information of the global factor graph;
s6, optimizing the main boat node through the node optimization function node;
and S7, based on the optimized main boat node, fusing and updating the state variable node and the measured variable node information to obtain the estimated value of the slave boat position information at the current moment.
As shown in FIG. 2, the slave boat state variable nodes X at the time k-1 and the time k in the factor graph model in the embodiment k-1 And X is k Node f (xk|x) is a function of the state equation k -1) connected; main boat measurement information node Z k And slave boat state variable node X k Between the two through the measurement equation function node f (Z k |X k ) Is connected with each other; main boat measurement information node Z k Main boat position information node phi k And a state variable node X from the boat k And the nodes are connected through node optimization function nodes h (Z, phi and X) so as to perform optimal selection on the main boat nodes.
As shown in FIG. 3, structure I is a function node f (Z k |X k ) The decomposed specific structure completes the data fusion of the master AUV and the slave AUV; structure II is measurement information Z k The specific structure of (3) includes ranging information corresponding to each master AUV at time k.
As shown in FIG. 4, structure L in the figure 1 ,L 2 ,…,L N Information representing N master AUVs of the k-time system, respectively, each structure comprising a master boat position information node phi k And ranging related informationThe function nodes F and G respectively complete the calculation of CRLB and ranging evaluation factors by using the information of the main boat to obtain alpha k And beta k Function node H utilizes variable node alpha k And beta k Is subjected to weighting operation to obtain a final evaluation result h k Screening M results with the smallest value +.>The corresponding master AUV is the preferred result. The type III architecture in the figure corresponds to the process of optimal selection of a single master AUV node.
And a specific embodiment II: in S1, collecting current dynamic topological structure information of the system in each collection period T, wherein the current dynamic topological structure information comprises the following steps: the number of the master AUV and the slave AUV, the position information, the speed information v, the angular speed information and the course angle information theta of the master AUV and the slave AUV, and calculate and detect the variance of each acquisition quantity and detect the distance measurement information d between the target slave boat and each master boat. The other embodiments are the same as those of the first embodiment.
In this embodiment, the method for determining the number of neighbor master boats of the target slave AUV includes: and setting a circular area with a target slave AUV as a center and a selected diameter, and determining the number of neighbor master boats. And establishing neighbor node information by taking the AUV as a center according to the acquired information, wherein the variance represents the uncertainty of the corresponding information.
And a third specific embodiment: and S3, initializing information of the master boat and the slave boat, wherein the information comprises the following steps: and initializing position information, speed information v, course angle information theta and distance measurement information d between the master boat and the slave boat. This embodiment is otherwise identical to the second embodiment.
And a specific embodiment IV: s5 comprises the following steps:
at the kth time the system conditional probability density function is decomposed into:
wherein N represents the number of the main AUV nodes;measurement information representing the nth (n=1, 2, …, N) master AUV; x is X m,n Position information indicating an N (n=1, 2, …, N) th master AUV; f (f) i Representing probability factors corresponding to the AUV nodes, namely:
in the formula, h i () represents a metrology function; z i Representing the measured real value; sigma (sigma) i A covariance matrix representing a corresponding measurement error;
defining ranging information d, heading angle theta and speed v of AUV of the system, and obeying Gaussian distribution:
wherein d is i Representing ranging information between the slave AUV and the ith master AUV;
node f (X) is a function of the function through the state equation k |X k-1 ) To variable node X k The information transferred is:
variable node X k To the state equation function node f (X k |X k-1 ) The information transferred is:
in the method, in the process of the invention,and->Respectively represent state variables X k Is a priori estimated and variance of (1);
according to the co-located state equation:
in (x) k ,y k ) Representing the coordinates of the AUV at the moment k in a reference coordinate system; v k Indicating AUV forward speed at time k; θ k The heading angle of AUV at the moment k is represented; Δt represents a sampling interval;
the resulting state transition formula is:
in which Q k Is the system process noise covariance matrix,for measuring noise matrix->The expression of (2) is:
in θ k The heading angle information corresponding to the moment k;
substituting formula (6) and formula (7) into formula (5), and combining to obtain:
wherein S is k The expression of (2) is:
thereby, the function node f (X k |X k-1 ) And information transfer with the state variable nodes, and finally, the global factor graph node information transfer and updating are realized. The other embodiments are the same as those of the first embodiment.
Function node f (X in this embodiment k |X k-1 ) Based on the state function of the slave boat, the slave boat position information at the moment is calculated by using the slave boat position information at the last moment and the slave boat speed and course angle information at the moment.
Fifth embodiment: as shown in fig. 3, S6 includes the following process:
clamerlo lower bound CRLB for each master AUV node is calculated separately k And ranging evaluation factor alpha i ：
Wherein X is k-1 ＝[x k-1 ,y k-1 ] T A state variable representing the slave AUV at time k-1;representing the position information of the ith main AUV at the k moment; />R X X represents k-1 Error covariance matrix of (a); r is R Φ Representation ofError covariance matrix of (a); d, d i Ranging information representing the slave AUV and the ith master AUV; />Representation d i Standard deviation of (2);
establishing a node preference parameter matrix at the moment k:
where tr (·) represents the trace of the matrix; n represents the number of main boats contained in the system at the moment k;
then weighting parameters in the node optimal parameter matrix NSPM by using an information entropy method, and firstly calculating the specific gravity p of each evaluation index i ：
Wherein r is 1 Representing CRLB evaluation parameters; r is (r) 2 Representing a ranging evaluation factor;
calculating entropy values of the parameters:
e i ＝-p i ln(p i ) (14)
calculating a difference coefficient:
g i ＝1-e i (15)
the weights of two indexes are calculated:
constructing a weight vector omega:
Ω＝[ω 1 ω 2 ] (17)
weighting the node preference parameter matrix NSPM:
Η k ＝Ω·NSPM k (18)
a 1 XN matrix H obtained by the formula (18) k In the method, the numerical value of each column corresponds to the final evaluation result of the corresponding main AUV in the system, and M minimum results are screened out, wherein the corresponding main AUV is taken as a preferable result. This embodiment is otherwise identical to the fourth embodiment.
In the present embodiment, in the local factor graph, variable nodesTo function node F k Delivering confidence information, at F k The process completes the calculation and then transmits the result information to +.>And (3) node:
variable node phi k To G k After transferring the position information of the master AUV node, at G k Calculating CRLB at node k And then the information is transferred toAnd (3) node:
variable nodeAnd->Transfer information to function node H, lead toThe function node H calculates the evaluation results H of all the main AUV nodes in the current dynamic topology system k And (3) completing the optimal selection of the nodes:
h in 1,j ∈H k ,j＝1,…,N。
Specific embodiment six: as shown in fig. 3, S7 includes the following process:
based on the optimized master AUV, the coordinate difference between the slave AUV and the ith master AUV in the k-time system is as followsAndvariable node->And->Respectively through function nodes C i Finishing information updating, calculating variable node->The confidence information is obtained as follows:
in the middle ofAnd->Respectively represent->And->Standard deviation of>Representing the distance between the slave AUV and the ith master AUV at the k moment;
function node C i Directional variable nodeInformation transferred, computation variable node->The transferred credibility information is as follows:
in the middle ofRepresents->Standard deviation of (2);
through function node A i And B i Conversion of position information, i.e. function node A i Delivery to variable nodesAndcalculation variable node->And->Is of (1)The degree information is respectively:
computing variable nodesAnd->The confidence information of (a) is respectively:
combining the position estimate of each master boat pair slave boat with the prior estimate of the slave boat position through function nodes D and E to obtain a final position estimate: estimating the position of each master boat to each slave boatTransfer to x k The method comprises the following steps:
in the method, in the process of the invention,and->Is x k Variance and expectation of (a);
similarly, y k The information of (2) is:
in the method, in the process of the invention,and->Is y k Variance and expectation of (a);
weighted averaging of the position estimate from the boat and the dead reckoning estimate from the boat:
/>
this embodiment is otherwise identical to embodiment five.
In this embodiment the function node f (Z k |X k ) Based on the measurement equation of the master boat, the master-slave boat coordinate difference and master boat measurement information are fused and updated, the master-slave boat coordinate difference and slave boat position estimation information are fused and updated, and finally the slave boat position estimation is obtained by weighted average of the position estimation of each master boat to the slave boat and the slave boat dead reckoning estimation.
Specific embodiment seven: distance between slave AUV and i-th master AUV at k-time in S7And->And->Relation of (2)The method comprises the following steps:
this embodiment is otherwise identical to the sixth embodiment.
A multi-AUV co-location system based on a node optimization selection and factor graph, the system having program modules corresponding to the steps of any one of the above embodiments one to seven, the steps in the multi-AUV co-location method based on a node optimization selection and factor graph described above being performed at run-time.
A computer readable storage medium storing a computer program configured to implement the steps of the multi-AUV co-location method based on node-optimized selection and factor graph of any of the above embodiments one to seven when invoked by a processor.
Example 1
In order to embody the performance of the multi-AUV co-location method (DS-OSFG) based on the node optimization selection and the factor graph, the method is compared with a multi-AUV co-location algorithm (DSFG) based on the factor graph dynamic topology of randomly selecting the main boat node, a multi-AUV co-location algorithm based on the distance selection main boat node based on the factor graph dynamic topology, and a multi-AUV co-location algorithm based on the distance variance selection main boat node based on the factor graph dynamic topology.
The basic parameters are set as follows: co-location system simulation experiments of 10 master boats and 20 slave boats were designed. The total simulation time length is set to 1000s, and the state update period Δt=1s. The dynamic topology information collection period t=5s. In order to meet the observability of the system, a track diagram shown in fig. 5 is designed, the main AUV moves at a constant speed, and the constant speed is v m S-curve motion from AUV at speed v=2m/S s =6m/s; in order to realize the dynamic topology of the system structure, a circular area with the diameter of 3000m taking the AUV as the center is set as the information processing range of the AUV, and the dynamic change condition of the system is shown in figure 6; based on underwater acoustic ranging scene, setting ranging party of speed and course angle of master AUV and slave AUVThe differences are respectivelyAnd->
According to the change condition of the dynamic topological structure system, the master AUV information and the ranging information between the master AUV and the slave AUV are set as follows:
1,3,5,7,8,9 six pieces of master AUV position information overlap mean value is 0, and variance isGaussian white noise, ranging information superposition mean value of 0, variance of +.>Is a gaussian white noise of (c).
The superposition mean value of the position information of the four main AUVs 2,4,6 and 10 is 0, and the variance isGaussian white noise, ranging information superposition mean value of 0, variance of +.>Is a gaussian white noise of (c).
Simulation results and analysis
The number of preferred nodes of the main AUV is controlled to be m=3, 4, …,10 for the four selection methods respectively, and an error result shown in fig. 7 is obtained. When the number of preferred nodes is greater than the number of actual nodes, the number of preferred nodes is adjusted to the number of actual nodes.
According to the result, the DS-OSFG method of the invention has the best positioning effect, because the DS-OSFG method also considers the information quality of the master AUV node compared with other algorithms which only consider the information quality of the positioning ranging. Besides the random selection method, the positioning errors of the other three algorithms all show a tendency of descending and then increasing along with the increase of the node preferred quantity, because the overall positioning errors can be reduced through weighting when the node preferred quantity is increased in the early stage, when the node preferred quantity is larger, the main AUV node information with poor quality can interfere with the result, and M=6 is verified to be the optimal preferred node quantity.
Comparative experiment of co-location method
The preferred node number of the four node selection methods is fixed to be m=6, and an error result as shown in fig. 8 and 9 is obtained, because of the randomness of selection of the position information and the ranging information, the positioning accuracy of the random selection method is the lowest and the least stable. The DS-OSFG algorithm has the best positioning effect, and can perform quality evaluation from node ranging information and node position information respectively, so that high-quality main AUV nodes can be better optimized for communication when the system structure is dynamically changed, and the relative stability of positioning accuracy is ensured.
Quantitative analysis the positioning errors RMSE of the four algorithms are shown in table 1, and the root mean square error of the DS-OSFG algorithm is reduced by 19.75%,36.97% and 58.05% compared to the distance variance based, distance based and randomly selected selection algorithm.
TABLE 1
Table 2 lists the run lengths of the four algorithms, because no specific mechanism is needed for node optimization, and the time consumption of the random selection method is the shortest; the DS-OSFG algorithm takes the longest time because the node position information quality and the ranging information quality need to be comprehensively considered, and the calculated amount is the largest.
TABLE 2
The simulation experiment proves that the DS-OSFG algorithm comprehensively considers the information quality of the main node and the information quality of the ranging, and the time consumption is slightly larger than that of other algorithms, but the overall positioning effect is better than that of the other algorithms.
Although the present disclosure is disclosed above, the scope of the present disclosure is not limited thereto. Various changes and modifications may be made by one skilled in the art without departing from the spirit and scope of the disclosure, and such changes and modifications would be within the scope of the disclosure.

Claims (9)

1. A multi-AUV co-location method based on node optimization selection and factor graph is characterized by comprising the following steps:
s1, acquiring dynamic topology structure information of a multi-AUV co-location system at the current moment;
s2, updating information of the slave boat and neighbor master boats;
s3, initializing information of a master boat and a slave boat;
s4, constructing a factor graph model of the multi-AUV co-location system;
the state variable, the position information and the measurement information of the main boat are defined as variable nodes, the state equation and the measurement equation are defined as function nodes, and the state equation function nodes are adopted to pair the state variable nodes X of the main boat k Transfer updating is carried out, and a measuring equation function node is adopted to measure a main boat measurement information node Z k From boat state variable node X k Fusion updating is carried out; meanwhile, a node optimization function node is constructed, and a position information node phi estimated aiming at a system main boat is constructed k Main boat measurement information node Z k Respectively carrying out CRLB of the lower boundary of the Keramelteon k And calculating a ranging evaluation factor, and optimizing the main boat node at the current moment;
s5, transmitting and updating in the factor graph model of the multi-AUV co-location system based on a sum-product algorithm, and transmitting information once in two directions in the factor graph respectively to realize the transmission and updating of the node information of the global factor graph;
s6, optimizing the main boat node through the node optimization function node;
and S7, based on the optimized main boat node, fusing and updating the state variable node and the measured variable node information to obtain the estimated value of the slave boat position information at the current moment.
2. The multi-AUV co-location method based on node optimization selection and factor graph according to claim 1, wherein the step of collecting current dynamic topology information of the system in each collection period T in S1 includes: the number of the master AUV and the slave AUV, the position information, the speed information v, the angular speed information and the course angle information theta of the master AUV and the slave AUV, and calculate and detect the variance of each acquisition quantity and detect the distance measurement information d between the target slave boat and each master boat.
3. The multi-AUV co-location method based on node optimization selection and factor graph of claim 2, wherein initializing master and slave boat information in S3 includes: and initializing position information, speed information v, course angle information theta and distance measurement information d between the master boat and the slave boat.
4. The multi-AUV co-location method based on node optimization selection and factor graph of claim 1, wherein S5 includes the following process:
at the kth time the system conditional probability density function is decomposed into:
wherein N represents the number of the main AUV nodes;measurement information representing the nth (n=1, 2, …, N) master AUV; x is X m,n Position information indicating an N (n=1, 2, …, N) th master AUV; f (f) i Representing probability factors corresponding to the AUV nodes, namely:
in the formula, h i () represents a metrology function; z i Representation quantityMeasuring a true value; sigma (sigma) i A covariance matrix representing a corresponding measurement error;
defining ranging information d, heading angle theta and speed v of AUV of the system, and obeying Gaussian distribution:
wherein d is i Representing ranging information between the slave AUV and the ith master AUV;
node f (X) is a function of the function through the state equation k |X k-1 ) To variable node X k The information transferred is:
variable node X k To the state equation function node f (X k |X k-1 ) The information transferred is:
in the method, in the process of the invention,and->Respectively represent state variables X k Is a priori estimated and variance of (1);
according to the co-located state equation:
in (x) k ,y k ) Representing the coordinates of the AUV at the moment k in a reference coordinate system; v k Indicating AUV forward speed at time k; θ k The heading angle of AUV at the moment k is represented; Δt represents a sampling interval;
the resulting state transition formula is:
in which Q k Is the system process noise covariance matrix,for measuring noise matrix->The expression of (2) is:
in θ k The heading angle information corresponding to the moment k;
substituting formula (6) and formula (7) into formula (5), and combining to obtain:
wherein S is k The expression of (2) is:
and finally, realizing the transmission and updating of the node information of the global factor graph.
5. The multi-AUV co-location method based on node optimization selection and factor graph of claim 4, wherein S6 includes the following process:
respectively calculating lower Clamerlo bounds of all main AUV nodesCRLB k And ranging evaluation factor alpha i ：
Wherein X is k-1 ＝[x k-1 ,y k-1 ] T A state variable representing the slave AUV at time k-1;representing the position information of the ith main AUV at the k moment; />R X X represents k-1 Error covariance matrix of (a); r is R Φ Representation->Error covariance matrix of (a); d, d i Ranging information representing the slave AUV and the ith master AUV; />Representation d i Standard deviation of (2);
establishing a node preference parameter matrix at the moment k:
where tr (·) represents the trace of the matrix; n represents the number of main boats contained in the system at the moment k;
then weighting parameters in the node optimal parameter matrix NSPM by using an information entropy method, and firstly calculating the specific gravity p of each evaluation index i ：
Wherein r is 1 Representing CRLB evaluation parameters; r is (r) 2 Representing a ranging evaluation factor;
calculating entropy values of the parameters:
e i ＝-p i ln(p i ) (14)
calculating a difference coefficient:
g i ＝1-e i (15)
the weights of two indexes are calculated:
constructing a weight vector omega:
Ω＝[ω 1 ω 2 ] (17)
weighting the node preference parameter matrix NSPM:
Η k ＝Ω·NSPM k (18)
a 1 XN matrix H obtained by the formula (18) k In the method, the numerical value of each column corresponds to the final evaluation result of the corresponding main AUV in the system, and M minimum results are screened out, wherein the corresponding main AUV is taken as a preferable result.
6. The multi-AUV co-location method based on node optimization selection and factor graph of claim 5, wherein S7 includes the following process:
based on the optimized master AUV, the coordinate difference between the slave AUV and the ith master AUV in the k-time system is as followsAnd->Calculation variable node->The confidence information is obtained as follows:
in the middle ofAnd->Respectively represent->And->Standard deviation of>Representing the distance between the slave AUV and the ith master AUV at the k moment;
computing variable nodesThe transferred credibility information is as follows:
in the middle ofRepresents->Standard deviation of (2);
computing variable nodesAnd->The confidence information of (a) is respectively:
computing variable nodesAnd->The confidence information of (a) is respectively:
estimating the position of each master boat to each slave boatTransfer to x k The method comprises the following steps:
in the method, in the process of the invention,and->Is x k Variance and expectation of (a);
similarly, y k The information of (2) is:
in the method, in the process of the invention,and->Is y k Variance and expectation of (a);
weighted averaging of the position estimate from the boat and the dead reckoning estimate from the boat:
7. the multi-AUV co-location method based on node optimization selection and factor graph of claim 6, wherein the distance between the slave AUV and the ith master AUV at k-time in S7And->And->The relation of (2) is:
8. a multi-AUV co-location system based on node optimization selection and factor graph, characterized in that the system has program modules corresponding to the steps of any of the preceding claims 1-7, the steps of the multi-AUV co-location method based on node optimization selection and factor graph being executed at run-time.
9. A computer readable storage medium, characterized in that it stores a computer program configured to implement the steps of the multi-AUV co-location method based on node optimization selection and factor graph of any of claims 1-7 when called by a processor.
CN202310850563.9A 2023-07-12 2023-07-12 Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph Pending CN116659513A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310850563.9A CN116659513A (en) 2023-07-12 2023-07-12 Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310850563.9A CN116659513A (en) 2023-07-12 2023-07-12 Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph

Publications (1)

Publication Number Publication Date
CN116659513A true CN116659513A (en) 2023-08-29

Family

ID=87720818

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310850563.9A Pending CN116659513A (en) 2023-07-12 2023-07-12 Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph

Country Status (1)

Country Link
CN (1) CN116659513A (en)

Similar Documents

Publication Publication Date Title
CN112285676B (en) Laser radar and IMU external parameter calibration method and device
CN112444246B (en) Laser fusion positioning method in high-precision digital twin scene
CN111008364B (en) Double observer cooperative passive positioning method and system
CN110865343A (en) LMB-based particle filter tracking-before-detection method and system
CN116772867A (en) Multi-AUV self-adaptive co-location method and system based on node optimization of factor graph
CN116734860A (en) Multi-AUV self-adaptive cooperative positioning method and system based on factor graph
CN113189541B (en) Positioning method, device and equipment
CN111928851B (en) TMA technology-based multi-autonomous underwater robot cluster collaborative navigation method
CN111735443B (en) Dense target track correlation method based on assignment matrix
CN116659513A (en) Multi-AUV (autonomous Underwater vehicle) co-location method and system based on node optimization selection and factor graph
CN109164419B (en) Multi-platform disorder measurement processing method based on interactive multi-model
CN109343013A (en) A kind of spatial registration method and system based on Restart mechanisms
CN113503891B (en) SINSDVL alignment correction method, system, medium and equipment
CN115098609A (en) Multi-sensor combined space-time deviation calibration and multi-target association fusion method and device
CN114111796B (en) Parallel fusion positioning method and system of underwater unmanned robot based on information gain
CN114415157A (en) Underwater target multi-model tracking method based on underwater acoustic sensor network
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN111736144B (en) Maneuvering turning target state estimation method only by distance observation
CN108924734A (en) A kind of three-dimension sensor node positioning method and system
CN114705223A (en) Inertial navigation error compensation method and system for multiple mobile intelligent bodies in target tracking
CN109856624B (en) Target state estimation method for single-radar linear flight path line
CN109684771B (en) Maneuvering target state prediction optimization method based on interactive multi-model
CN113296515A (en) Explicit model prediction path tracking method for double-independent electrically-driven vehicle
CN116793366A (en) Multi-AUV (autonomous Underwater vehicle) co-location method and system based on factor graph under dynamic topology
CN112285697A (en) Multi-sensor multi-target space-time deviation calibration and fusion method

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