CN114111796B - Parallel fusion positioning method and system of underwater unmanned robot based on information gain - Google Patents
Parallel fusion positioning method and system of underwater unmanned robot based on information gain Download PDFInfo
- Publication number
- CN114111796B CN114111796B CN202111419031.7A CN202111419031A CN114111796B CN 114111796 B CN114111796 B CN 114111796B CN 202111419031 A CN202111419031 A CN 202111419031A CN 114111796 B CN114111796 B CN 114111796B
- Authority
- CN
- China
- Prior art keywords
- auv
- state
- information
- local
- filtering
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 78
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000001914 filtration Methods 0.000 claims abstract description 99
- 239000011159 matrix material Substances 0.000 claims abstract description 15
- 238000005259 measurement Methods 0.000 claims description 16
- 238000005094 computer simulation Methods 0.000 claims description 9
- 238000012360 testing method Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 7
- 238000013461 design Methods 0.000 claims description 5
- 238000004891 communication Methods 0.000 abstract description 6
- 238000007500 overflow downdraw method Methods 0.000 abstract description 6
- 230000005236 sound signal Effects 0.000 abstract description 2
- 238000013441 quality evaluation Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 230000007246 mechanism Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009792 diffusion process Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004088 simulation Methods 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
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
The invention discloses an information gain-based parallel fusion positioning method of an underwater unmanned robot, which is characterized in that a local unscented filter is embedded in each main AUV node by combining an information entropy and an information gain concept in an information theory and a distributed local information filtering method, and the local information filter obtains local unscented estimation quantity about the positioning state of a tested node by utilizing local multisource observation information of each AUV node; and meanwhile, the weighted fusion method fuses all local posterior estimation by taking the local filtering information gain as a filtering quality evaluation standard, so that the global posterior estimation result is output in a form of weighting the posterior estimation mean value by an information matrix. The invention solves the problem that the current common communication cooperative method is difficult to match with the underwater sound signal system.
Description
Technical Field
The invention relates to the technical field of underwater robot clusters, in particular to a parallel fusion positioning method and system of an underwater unmanned robot based on information gain.
Background
The method has more important significance for the exploration and utilization of the ocean under the current increasingly sophisticated exploration of natural resources. The underwater autonomous vehicle (autonomous underwater vehicle, AUV) is a cable-free unmanned underwater robot with high autonomous capability and high concealment and extremely high application value. With the increasing maturity of AUV technology, the task difficulty and complexity of the AUV technology are also increased, the underwater task is more accurate, complex and diversified, the single AUV is limited by the AUV and cannot meet all working requirements, and in recent years, miniaturization, swarming, intellectualization and structural mixing are the main development of the AUV. The interaction cooperation among the multiple AUVs enables the AUV to surpass a single function of the single AUV, and simultaneously, the AUV system under the multi-agent structure can obtain higher fault tolerance and robustness, improve the operation complexity and the working efficiency, expand the application scene and obtain wider application fields.
The main stream AUV cluster system generally adopts a main-slave structure, the slave AUV does not need to be equipped with a sensor and a processor element at the same level as the main AUV, the system cost is greatly reduced, the requirement of individual floating correction positioning satellite signals is reduced, and the concealment of the system is improved. In the multi-AUV system with the master-slave formation structure, a master AUV is provided with a high-level sensor, can autonomously acquire self-positioning information with relatively high precision, and completes global positioning information acquisition of a slave AUV without self-positioning function through relative measurement between the master AUV and the slave AUV. The collaborative method based on the optimization theory and the graph theory has high requirements on communication quality and top centralized processing capacity, and is difficult to match with the severe conditions of the underwater sound signal system.
Disclosure of Invention
The invention aims to: in order to solve the problem that the conventional common communication cooperative method is difficult to match with an underwater acoustic signal system, the invention provides the underwater unmanned robot parallel fusion positioning method based on the information gain, and the method only needs to carry out single-hop communication in a message diffusion mode, so that the method has strong applicability to an AUV cluster system relying on underwater acoustic communication.
The technical scheme is as follows: an underwater unmanned robot parallel fusion positioning method based on information gain comprises the following steps:
(1) Modeling a system, constructing an AUV motion model and a co-location system state space model, comprehensively considering the dynamics characteristics of a platform, determining input parameters, adding Gaussian noise, and establishing a motion equation;
(2) Local information filtering, namely approximating probability density of a function by using unscented transformation, solving expected and variance of a target event, converting a nonlinear problem into a Kalman filtering problem, and outputting a local posterior value;
(3) The information gain weighting fusion, which takes each group of local filtering information gain as a weight index, carries out weighting fusion on all local posterior estimation, and enables the global posterior estimation result to be output in a weighted form;
(4) And updating the parallel structure, wherein local information filtering is independently completed by the main AUV, the parallel structure runs in parallel with the information gain weighting method of the whole system, the multi-source local filtering information participating in fusion is the current latest updating result, and each group of filtering results are fused and output from the AUV.
The state of the AUV platform in the step (1) is composed of elements such as the position, the speed and the gesture of the platform, the dynamics characteristics of the platform are comprehensively considered, input parameter values are determined, gaussian noise is added, a motion equation is established, the state equation of the whole co-location system can be obtained through the state, the input and the noise of the whole platform, when the observation equation between a single platform and the platform is expanded, the observation equation of the whole system can be obtained, a state space model is a starting point of the design of a co-location method, and the state space model of any node is as follows:
the equation of state: x is x k ＝f(x k-1 )+Q k-1
Observation equation:
wherein x is k As a real state variable of the AUV under test,for observing the variable, the state equation and the observation equation are nonlinear functions, Q k-1 To predict noise, R k For observing noise, it is assumed that both are gaussian white noise, satisfying normal distribution;
the master AUV and the slave AUV co-locate once every time T, and their relative distances from the slave AUV, as solved by the master AUV, can be expressed as:
wherein the method comprises the steps ofIs the measurement noise modeled as gaussian white noise.
The step of filtering the local information in the step (2) is as follows:
AUV node state x to be measured at current moment k-1 Is estimated expected and covariance of (2)Decomposing covariance matrixWherein the requirement->Must be positive;
(a) Acquiring a priori value of the AUV state to be measured:
L (i) represents the ith column of matrix L, and the weight is
Then
Obtaining the prior value of the detected node stateIs satisfying the normal distribution +.>
(b) Obtaining an AUV observation prior value to be measured:
will beDecomposing, let->
L 1(i) Representative matrix L 1 Is the (i) th column of the weight value
With y k (i) ＝f(x k-1 (i) ) A priori expectation of the observed value of the measured node at time k
Its a priori variance
(c) Observe the observed value y of the AUV node to be tested m ；
(d) Updating the posterior value of the AUV state to be measured:
note k =P xy (P y ) -1
State posterior expectation of the measured AUV node at time k:
posterior variance ofSo far, the state posterior estimation value and covariance of the k moment are already obtained, the next cycle is entered, and the recursion is continued.
The step (3) of information gain weighted fusion is to calculate each group of local filtering information gain, and the information gain is taken as weight for weighted fusion of all local posterior estimation values, so that the global posterior estimation result is output in a weighted form:
main AUV i The estimated conditional entropy of the state prior value and the posterior value is written as:
state posterior estimateFor a priori value->Information gain writing of (2):
the position state information obtained by fusing i main AUV measurement results by the tested AUV is as follows:
the weights are given by:
and (4) independently completing local information filtering on the premise that all the main AUV filtering has the same reliability, and running in parallel with the integral information gain weighting method of the system, so that weighting fusion can be completed in any length time interval.
The parallel fusion positioning system of the underwater unmanned robot based on the information gain comprises a system modeling module, a local information filtering module, an information gain weighting fusion module and a parallel structure updating module;
and a system modeling module: building an AUV motion model and a co-location system state space model, comprehensively considering the dynamics characteristic of a platform, determining input parameter values, adding Gaussian noise, and building a motion equation;
and the local information filtering module is used for: approximating probability density of the function by using unscented transformation, solving expected and variance of a target event, converting a nonlinear problem into a Kalman filtering problem, and outputting a local posterior value;
and the information gain weighting fusion module is used for: taking the gain of each group of local filtering information as a weight index, and carrying out weighted fusion on all local posterior estimation so as to output a global posterior estimation result in a weighted form;
and a parallel structure updating module: the local information filtering is independently completed by the main AUV, the multi-source local filtering information participating in fusion is the current latest updating result and the filtering results of all groups are fused and output from the AUV.
The state of the AUV platform in the system modeling module is composed of elements such as the position, the speed and the gesture of the platform, the dynamics characteristic of the platform is comprehensively considered, the input parameter is determined, gaussian noise is added, a motion equation is established, the state equation of the whole co-location system can be obtained through the state, the input and the noise of the whole platform, when the observation equation between a single platform and the platform is expanded, the observation equation of the whole system can be obtained, a state space model is a starting point of the design of a co-location method, and the state space model of any node is as follows:
the equation of state: x is x k ＝f(x k-1 )+Q k-1
Observation equation:
wherein x is k As a real state variable of the AUV under test,for observing the variable, the state equation and the observation equation are nonlinear functions, Q k-1 To predict noise, R k For observing noise, it is assumed that both are gaussian white noise, satisfying normal distribution;
the master AUV and the slave AUV co-locate once every time T, and their relative distances from the slave AUV, as solved by the master AUV, can be expressed as:
wherein the method comprises the steps ofIs the measurement noise modeled as gaussian white noise.
The local information filtering in the local information filtering module comprises the following steps:
AUV node state x to be measured at current moment k-1 Is estimated expected and covariance of (2)Decomposing covariance matrixWherein the requirement->Must be positive;
(a) Acquiring a priori value of the AUV state to be measured:
L (i) represents the ith column of matrix L, and the weight is
Then
Obtaining the prior value of the detected node stateIs satisfying the normal distribution +.>
(b) Obtaining an AUV observation prior value to be measured:
will beDecomposing, let->
L 1(i) Representative matrix L 1 Is the (i) th column of the weight value
With y k (i) ＝f(x k-1 (i) ) A priori expectation of the observed value of the measured node at time k
Its a priori variance
(c) Observe the observed value y of the AUV node to be tested m ；
(d) Updating the posterior value of the AUV state to be measured:
state posterior expectation of AUV node under test at k moment
Posterior variance ofSo far, the state posterior estimation value and covariance of the k moment are already obtained, the next cycle is entered, and the recursion is continued.
The information gain weighting fusion module is used for calculating local filtering information gains of each group, taking the information gain as weight weighting fusion for all local posterior estimation values, and outputting the global posterior estimation result in a weighted mode:
main AUV i The estimated conditional entropy of the state prior value and the posterior value is written as:
state posterior estimateFor a priori value->Information gain writing of (2):
the position state information obtained by fusing i main AUV measurement results by the tested AUV is as follows:
the weights are given by:
the parallel structure updating module independently completes local information filtering on the premise that all the main AUV filtering has the same reliability, and runs in parallel with the integral information gain weighting method of the system, so that weighting fusion can be completed in any length time interval.
The beneficial effects are that: compared with the prior art, the invention has the following remarkable advantages:
1. the invention utilizes the motion geodetic coordinate transformation and combines the unscented Kalman filtering to solve the problem of nonlinear filtering of the underwater robot, thereby being fully applicable to the scene of strong nonlinearity and clutter interference of the multi-AUV cooperative motion.
2. The invention provides a local filtering information gain weighted fusion method, which takes local filtering information gain as a filtering effect index, and improves the weight of a result with better filtering effect in the whole fusion method so as to improve the reliability of integral fusion positioning information.
3. The invention introduces a timely updating mechanism of parallel fusion, and the local information filtering and the whole weighted fusion method run in parallel, so that the fusion method is free from the influence of inconsistent multi-source local filtering periods and the delay characteristic of underwater acoustic communication, the timeliness of participating in fusion information is ensured, and the information ordering problem of the local filtering and the fusion method is effectively avoided.
Drawings
FIG. 1 is a flow chart of a parallel fusion positioning method of an underwater unmanned robot based on information gain;
FIG. 2 is a schematic diagram of the geodetic and kinematic coordinate systems of the AUV;
FIG. 3 is a schematic diagram of a local information filtering method according to the present invention;
FIG. 4 is a schematic diagram of a parallel fusion update mechanism according to the present invention;
fig. 5 is a schematic diagram of a parallel fusion distributed filtering method according to the present invention.
Detailed Description
The technical scheme of the invention is further described below with reference to the accompanying drawings.
Example 1:
an information gain-based parallel fusion positioning method of an underwater unmanned robot, as shown in fig. 1, comprises the following steps:
(1) Constructing a state space model of a multi-AUV co-location system, comprehensively considering the dynamics characteristics of a platform, determining input parameters, adding Gaussian noise, and establishing a motion equation; constructing an AUV coordinate system model, as shown in FIG. 2, comprising a geodetic coordinate system and a motion coordinate system; and constructing an AUV measurement model, a state and observation model.
(2) As shown in fig. 3, the local information filtering method is to improve unscented kalman filtering to process local information: the probability density of the function is approximated by a Unscented Transform (UT), the expected and variance of the target event are calculated, and the nonlinear problem is converted into a kalman filter problem.
(3) Information gain weighted fusion: taking the gain of each group of local filtering information as a weight index, and carrying out weighted fusion on all local posterior estimation so as to output a global posterior estimation result in a weighted form;
(4) As shown in fig. 4, a timely update mechanism of the parallel structure: the local information filtering is independently completed by the main AUV, the multi-source local filtering information participating in fusion is the current latest updating result and the filtering results of all groups are fused and output from the AUV;
as shown in fig. 5, the parallel fusion distributed filtering method: based on the high-quality filtering effect of each main AUV, the output of the local filter is used as the input of weighted fusion, and the filtering information gain is used as the weight to weighted fusion of a plurality of filtering results. Meanwhile, the local information filtering adopting unscented Kalman filtering is independently completed by the main AUV, and the local information filtering is parallel to the integral information gain weighting module of the system, and the output of the system weighting fusion module is used as the input of the local filter to participate in the next iteration.
In this embodiment, the step (1) specifically includes:
AUV coordinate system modeling includes a geodetic coordinate system and a kinematic coordinate system. When the movement state of the AUV in the environment is studied, the AUV position information is easy to visually represent by using a geodetic coordinate system, and the geodetic coordinate system is widely applied to AUV cluster scenes. When the same slave AUV position information obtained by the plurality of master AUVs is fused, the conversion from the motion coordinate system to the geodetic coordinate system is required to be completed, so that the position information of the plurality of master AUVs can use a unified coordinate scale.
(X, Y, Z) are the displacement of the AUV in the motion coordinate system, respectively, and (u, v, w) are the velocity components of the AUV in the (X, Y, Z) axis direction, respectively. And (phi, theta, phi) are the roll, pitch and heading angles of the AUV. (p, q, r) are the angular velocity components in the (X, Y, Z) axis direction, respectively.
The AUVs acquire relative positions through measurement, so that position coordinates under a motion coordinate system are acquired, and the position coordinates are required to be converted into a geodetic coordinate system for the convenience of calculation and system filtering fusion. Assuming that the original positions of the two coordinate systems coincide, the transformation matrix S of the coordinate system is obtained through coordinate rotation, and is as follows:
the motion coordinate system can be converted into the geodetic coordinate system by the conversion matrix, namely:
the AUV individual kinematics equation is as follows:
the scenario in which the master AUVi locates the slave AUVj once every time T locates the same slave AUV for a plurality of master AUVs, and the relative distance between the master AUVi and the slave AUVj calculated by the master AUVi can be expressed as:
is the measurement noise modeled as gaussian white noise.
The platform state of the single AUV consists of elements such as the position, the speed, the gesture and the like of the platform. Comprehensively considering the dynamics characteristics of the platform, determining input parameters, adding Gaussian noise simulation position information to measure noise, and establishing a motion equation. The state equation of the whole co-location system can be obtained from the state, input and noise of the whole platform. Similarly, when the observation equation between the single platform and the platform is properly expanded, the observation equation of the whole system can be obtained. The state space model is a starting point of the design of the co-location method, and the state space model of any node i is as follows:
the equation of state: x is x k ＝f(x k-1 )+Q k-1
Observation equation:
wherein x is k The real state variable of the AUV to be measured contains information such as position, speed, gesture and the like,for observing variables, the state equation and the observation equation are nonlinear functions in consideration of nonlinearity of the track push and the relative distance measurement model, and the actual observation value of the AUV to be measured. Q (Q) k-1 To predict noise, R k To observe noise, it is assumed that both are gaussian white noise, satisfying a normal distribution.
In this embodiment, the step (2) specifically includes:
knowing the AUV node state x measured at the current time k-1 Is estimated expected and covariance of (2)Decomposing covariance matrixWherein the requirement->Must be positive;
(a) Acquiring a priori value of the AUV state to be measured:
L (i) represents the ith column of matrix L, and the weight is
Then->
Obtaining the prior value of the detected node stateIs satisfying the normal distribution +.>
(b) Obtaining an AUV observation prior value to be measured:
will beDecomposing, let->
A priori expectations of the observed value of the measured node at time k:
its a priori variance
(c) Observe the observed value y of the AUV node to be tested m ；
(d) Updating the posterior value of the AUV state to be measured:
state posterior expectation of AUV node under test at k moment
Posterior variance ofSo far, the state posterior estimation value and covariance of the k moment are already obtained, and the next cycle can be entered to continue recursion.
In this embodiment, the step (3) specifically includes:
after the AUV numbered i performs a complete local information filtering, the observed value y m As a feature obtained by external observation, a priori estimates of stateCan be written as:
therefore, the conditional entropy of the state prior value and the posterior value of the main AUVi pre-estimation can be written respectively
Then, state posterior estimateFor a priori value->Can be written as:
the measured slave AUV fuses the position state information obtained by the i master AUV measurement results into
The weight value is given by the following equation,
in this embodiment, the step (4) specifically includes: the local information filtering is completed by the main AUV respectively and independently, the multi-source local filtering information participating in fusion is the current latest updating result and is operated in parallel with the information gain weighting method of the whole system, and the filtering results of all groups are fused and output by the AUV, as shown in figure 3, the main AUV1 has a shorter one-time iterative filtering period due to the self state characteristics, and the state information of the tested AUV is updated more frequently compared with t 2 Filtering result of time instant, AUV1 at t 3 Filtering result of time and AUV2 and AUV3 at t 2 The measurement results at the moment are closer in time and can more truly reflect the current measured resultFrom AUV state information. The method for updating the AUV1 filtering result in time not only can ensure the real-time performance of the whole fusion target of the system, but also can fully utilize the fusion result of the system to participate in the local filtering of each main AUV. Likewise, T w The time for all the master AUV filtering signals participating in observation to reach the slave AUV is determined, obviously, the instant updating mechanism enables the fusion information to be the latest state value measured by each current master AUV, the slave AUV finishes once complete information acquisition, and the result fusion can be carried out, and the fusion period can be slightly dynamically changed along with the information acquisition time. The dynamically-changing weighted fusion period plays a role in dynamically adjusting the main AUV measuring system with different observation periods to a certain extent. So far, each fusion is calculated aiming at the latest state information, and the fusion result can be fully utilized by local information filtering.
In this embodiment, the parallel fusion distributed filtering method specifically includes: on the premise that all the main AUVs have the same reliability, namely the same-level coordinate conversion errors and measurement errors, on the basis of the high-quality filtering effect of all the main AUVs, the filtering information gain is used as a weight to weight and fuse a plurality of filtering results, so that the filtering errors are further improved, and the fusion result is more approximate to the true value of the state of the AUV to be measured. Meanwhile, the local information filtering adopting unscented Kalman filtering is independently completed by the main AUV, and the local information filtering is operated in parallel with the information gain weighting algorithm of the whole system, so that the weighting fusion algorithm does not need to complete fusion in all filtering iteration periods participating in fusion, namely, the weighting fusion can be completed in any length of time interval, and the algorithm is prevented from being trouble of inconsistent time when a plurality of filters are completed.
Example 2:
the parallel fusion positioning system of the underwater unmanned robot based on the information gain comprises a system modeling module, a local information filtering module, an information gain weighting fusion module and a parallel structure updating module;
and a system modeling module: building an AUV motion model and a co-location system state space model, comprehensively considering the dynamics characteristic of a platform, determining input parameter values, adding Gaussian noise, and building a motion equation;
and the local information filtering module is used for: approximating probability density of the function by using unscented transformation, solving expected and variance of a target event, converting a nonlinear problem into a Kalman filtering problem, and outputting a local posterior value;
and the information gain weighting fusion module is used for: taking the gain of each group of local filtering information as a weight index, and carrying out weighted fusion on all local posterior estimation so as to output a global posterior estimation result in a weighted form;
and a parallel structure updating module: the local information filtering is independently completed by the main AUV, the multi-source local filtering information participating in fusion is the current latest updating result and the filtering results of all groups are fused and output from the AUV.
Claims (10)
1. The parallel fusion positioning method of the underwater unmanned robot based on the information gain is characterized by comprising the following steps of:
(1) Modeling a system, constructing an AUV motion model and a co-location system state space model, comprehensively considering the dynamics characteristics of a platform, determining input parameters, adding Gaussian noise, and establishing a motion equation;
(2) Local information filtering, namely approximating probability density of a function by using unscented transformation, solving expected and variance of a target event, converting a nonlinear problem into a Kalman filtering problem, and outputting a local posterior value;
(3) The information gain weighting fusion, which takes each group of local filtering information gain as a weight index, carries out weighting fusion on all local posterior estimation, and enables the global posterior estimation result to be output in a weighted form;
(4) And updating the parallel structure, wherein local information filtering is independently completed by the main AUV, the parallel structure runs in parallel with the information gain weighting method of the whole system, the multi-source local filtering information participating in fusion is the current latest updating result, and each group of filtering results are fused and output from the AUV.
2. The parallel fusion positioning method of the underwater unmanned robot based on the information gain is characterized in that the platform state of the AUV in the step (1) is composed of elements such as the position, the speed and the gesture of the platform, the dynamics characteristic of the platform is comprehensively considered, the input parameter is determined, gaussian noise is added, a motion equation is established, the state equation of the whole co-positioning system can be obtained by the state, the input and the noise of the whole platform, when the single platform and the observation equation between the platforms are expanded, the observation equation of the whole system can be obtained, a state space model is a starting point of the design of the co-positioning method, and the state space model of any node is as follows:
the equation of state: x is x k ＝f(x k-1 )+Q k-1
Observation equation:
wherein x is k As a real state variable of the AUV under test,for observing the variable, the state equation and the observation equation are nonlinear functions, Q k-1 To predict noise, R k For observing noise, it is assumed that both are gaussian white noise, satisfying normal distribution;
the master AUV and the slave AUV co-locate once every time T, and their relative distances from the slave AUV, as solved by the master AUV, can be expressed as:
wherein the method comprises the steps ofIs the measurement noise modeled as gaussian white noise.
3. The parallel fusion positioning method of the underwater unmanned robot based on the information gain according to claim 1, wherein the step of local information filtering in the step (2) is as follows:
AUV node state x to be measured at current moment k-1 Is estimated expected and covariance of (2)Decomposing covariance matrix->Wherein the requirement->Must be positive;
(a) Acquiring a priori value of the AUV state to be measured:
L (i) represents the ith column of matrix L, and the weight is
Then
Obtaining the prior value of the detected node stateIs satisfying the normal distribution +.>
(b) Obtaining an AUV observation prior value to be measured:
will beDecomposing, let->
L 1(i) Representative matrix L 1 Is the (i) th column of the weight value
With y k (i) ＝f(x k-1 (i) ) A priori expectation of the observed value of the measured node at time k
Its a priori variance
(c) Observe the observed value y of the AUV node to be tested m ；
(d) Updating the posterior value of the AUV state to be measured:
note k=p xy (P y ) -1
State posterior expectation of the measured AUV node at time k:
posterior variance ofSo far, the state posterior estimation value and covariance of the k moment are already obtained, the next cycle is entered, and the recursion is continued.
4. The parallel fusion positioning method of the underwater unmanned robot based on the information gain according to claim 1, wherein the information gain weighting fusion in the step (3) is to calculate each group of local filtering information gain, and the information gain is taken as the weighting fusion for all the local posterior estimation values, so that the global posterior estimation result is output in a weighted form:
main AUV i The estimated conditional entropy of the state prior value and the posterior value is written as:
state posterior estimateFor a priori value->Information gain writing of (2):
the position state information obtained by fusing i main AUV measurement results by the tested AUV is as follows:
the weights are given by:
5. the parallel fusion positioning method of the underwater unmanned robot based on the information gain according to claim 1, wherein the step (4) independently completes local information filtering on the premise that all main AUV filtering has the same reliability, and runs in parallel with the information gain weighting method of the whole system, so that weighted fusion can be completed within a time interval of any length.
6. The parallel fusion positioning system of the underwater unmanned robot based on the information gain is characterized by comprising a system modeling module, a local information filtering module, an information gain weighting fusion module and a parallel structure updating module;
and a system modeling module: building an AUV motion model and a co-location system state space model, comprehensively considering the dynamics characteristic of a platform, determining input parameter values, adding Gaussian noise, and building a motion equation;
and the local information filtering module is used for: approximating probability density of the function by using unscented transformation, solving expected and variance of a target event, converting a nonlinear problem into a Kalman filtering problem, and outputting a local posterior value;
and the information gain weighting fusion module is used for: taking the gain of each group of local filtering information as a weight index, and carrying out weighted fusion on all local posterior estimation so as to output a global posterior estimation result in a weighted form;
and a parallel structure updating module: the local information filtering is independently completed by the main AUV, the multi-source local filtering information participating in fusion is the current latest updating result and the filtering results of all groups are fused and output from the AUV.
7. The parallel fusion positioning system of the underwater unmanned robot based on the information gain according to claim 6, wherein the platform state of the AUV in the system modeling module is composed of the position, the speed, the gesture and other elements of the platform, the dynamics characteristics of the platform are comprehensively considered, the input parameter is determined, gaussian noise is added, a motion equation is established, the state equation of the whole co-positioning system can be obtained by the state, the input and the noise of the whole platform, when the single platform and the observation equation between the platforms are expanded, the observation equation of the whole system can be obtained, the state space model is a starting point of the design of a co-positioning method, and the state space model of any node is as follows:
the equation of state: x is x k ＝f(x k-1 )+Q k-1
Observation equation:
wherein x is k As a real state variable of the AUV under test,for observing the variable, the state equation and the observation equation are nonlinear functions, Q k-1 To predict noise, R k For observing noise, it is assumed that both are gaussian white noise, satisfying normal distribution;
the master AUV and the slave AUV co-locate once every time T, and their relative distances from the slave AUV, as solved by the master AUV, can be expressed as:
wherein the method comprises the steps ofIs the measurement noise modeled as gaussian white noise.
8. The parallel fusion positioning system of an underwater unmanned robot based on information gain of claim 6, wherein the step of local information filtering in the local information filtering module is as follows:
AUV node state x to be measured at current moment k-1 Is estimated expected and covariance of (2)Decomposing covariance matrix->Wherein the requirement->Must be positive;
(a) Acquiring a priori value of the AUV state to be measured:
L (i) represents the ith column of matrix L, and the weight is
Then
Obtaining the prior value of the detected node stateIs satisfying the normal distribution +.>
(b) Obtaining an AUV observation prior value to be measured:
will beDecomposing, let->
L 1(i) Representative matrix L 1 Is the (i) th column of the weight value
With y k (i) ＝f(x k-1 (i) ) A priori expectation of the observed value of the measured node at time k
Its a priori variance
(c) Observe the observed value y of the AUV node to be tested m ；
(d) Updating the posterior value of the AUV state to be measured:
note k=p xy (P y ) -1
State posterior expectation of AUV node under test at k moment
Posterior variance ofSo far, the state posterior estimation value and covariance of the k moment are already obtained, the next cycle is entered, and the recursion is continued.
9. The parallel fusion positioning system of the underwater unmanned robot based on the information gain according to claim 6, wherein the information gain weighting fusion module is used for calculating each group of local filtering information gain, taking the information gain as weight for weighting fusion of all local posterior estimation values, and outputting the global posterior estimation result in a weighted form:
main AUV i The estimated conditional entropy of the state prior value and the posterior value is written as:
state posterior estimateFor a priori value->Information gain writing of (2):
the position state information obtained by fusing i main AUV measurement results by the tested AUV is as follows:
the weights are given by:
10. the parallel fusion positioning system of the underwater unmanned robot based on the information gain, which is disclosed in claim 6, is characterized in that the parallel structure updating module independently completes local information filtering on the premise that all main AUV filtering has the same reliability, and runs in parallel with the whole information gain weighting method of the system, so that the weighted fusion can be completed within any length of time interval.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111419031.7A CN114111796B (en) | 2021-11-26 | 2021-11-26 | Parallel fusion positioning method and system of underwater unmanned robot based on information gain |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111419031.7A CN114111796B (en) | 2021-11-26 | 2021-11-26 | Parallel fusion positioning method and system of underwater unmanned robot based on information gain |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114111796A CN114111796A (en) | 2022-03-01 |
| CN114111796B true CN114111796B (en) | 2023-10-24 |
Family
ID=80369722
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111419031.7A Active CN114111796B (en) | 2021-11-26 | 2021-11-26 | Parallel fusion positioning method and system of underwater unmanned robot based on information gain |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114111796B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115792796B (en) * | 2023-02-13 | 2023-06-06 | 鹏城实验室 | Co-location method, device and terminal based on relative observation equivalent model |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113048984A (en) * | 2021-04-01 | 2021-06-29 | 江苏科技大学 | Dynamic positioning information fusion method for underwater unmanned robot cluster |
| CN113654565A (en) * | 2021-07-13 | 2021-11-16 | 哈尔滨工程大学 | Multi-pilot AUV (autonomous Underwater vehicle) collaborative navigation method based on information entropy |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11958579B2 (en) * | 2019-07-30 | 2024-04-16 | Schlumberger Technology Corporation | System and method for autonomous exploration for mapping underwater environments |
-
2021
- 2021-11-26 CN CN202111419031.7A patent/CN114111796B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113048984A (en) * | 2021-04-01 | 2021-06-29 | 江苏科技大学 | Dynamic positioning information fusion method for underwater unmanned robot cluster |
| CN113654565A (en) * | 2021-07-13 | 2021-11-16 | 哈尔滨工程大学 | Multi-pilot AUV (autonomous Underwater vehicle) collaborative navigation method based on information entropy |
Non-Patent Citations (3)
| Title |
|---|
| A probabilistic and highly efficient topology control algorithm for underwater cooperating AUV networks;LI N et al.;Sensors;全文 * |
| 基于几何解释的集群AUV协同定位误差及编队构型分析;张立川;屈俊琪;潘光;王永召;;西北工业大学学报(04);全文 * |
| 多无人水下航行器协同导航定位研究进展;张立川;许少峰;刘明雍;徐德民;;高技术通讯(05);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114111796A (en) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113048984B (en) | Dynamic positioning information fusion method for underwater unmanned robot cluster | |
| CN107315171B (en) | Radar networking target state and system error joint estimation algorithm | |
| CN113252033B (en) | Positioning method, positioning system and robot based on multi-sensor fusion | |
| CN112284384A (en) | Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity | |
| CN114111796B (en) | Parallel fusion positioning method and system of underwater unmanned robot based on information gain | |
| CN109141412B (en) | UFIR filtering algorithm and system for data-missing INS/UWB combined pedestrian navigation | |
| CN113175931B (en) | Cluster networking collaborative navigation method and system based on constraint Kalman filtering | |
| CN109507706B (en) | GPS signal loss prediction positioning method | |
| CN111008364A (en) | Method and system for cooperative passive positioning of double observers | |
| CN116086445A (en) | Multi-source information time delay fusion navigation method based on factor graph optimization | |
| CN116295386A (en) | Multi-sensor fusion-based underwater vehicle navigation control system and method | |
| CN111928851B (en) | TMA technology-based multi-autonomous underwater robot cluster collaborative navigation method | |
| CN114459474B (en) | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph | |
| Li et al. | Cooperative positioning algorithm of swarm UAVs based on posterior linearization belief propagation | |
| CN109655057A (en) | A kind of six push away the filtering optimization method and its system of unmanned plane accelerator measured value | |
| CN115113646A (en) | Satellite formation flat root state continuous estimation method and system based on Kalman filtering | |
| CN114993341A (en) | Carrier rocket trajectory estimation method and device based on space-based measurement | |
| CN115031729A (en) | SINS/DVL/USBL underwater tight combination navigation method and device and underwater carrier control equipment | |
| CN113654565A (en) | Multi-pilot AUV (autonomous Underwater vehicle) collaborative navigation method based on information entropy | |
| CN113503891A (en) | SINSDVL alignment correction method, system, medium and equipment | |
| Jie et al. | A fusion method for AUV collaborative location based on information gain weighting | |
| CN112445244B (en) | Target searching method for multiple autonomous underwater robots | |
| CN112836354B (en) | Target tracking and positioning method, system and device and readable storage medium | |
| CN117031473B (en) | Underwater target collaborative track tracking method | |
| CN114577211B (en) | Master-slave AUV cooperative positioning method based on factor graph considering ocean current influence |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |