CN107677272B - AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering - Google Patents

AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering Download PDF

Info

Publication number
CN107677272B
CN107677272B CN201710805228.1A CN201710805228A CN107677272B CN 107677272 B CN107677272 B CN 107677272B CN 201710805228 A CN201710805228 A CN 201710805228A CN 107677272 B CN107677272 B CN 107677272B
Authority
CN
China
Prior art keywords
information
auv
time
state
vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710805228.1A
Other languages
Chinese (zh)
Other versions
CN107677272A (en
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 Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201710805228.1A priority Critical patent/CN107677272B/en
Publication of CN107677272A publication Critical patent/CN107677272A/en
Application granted granted Critical
Publication of CN107677272B publication Critical patent/CN107677272B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses an AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering. In the method, a positioning task in a collaborative navigation process is completed by adopting a distributed structure of an unscented information filter. In the process of cooperative positioning, firstly establishing a state equation and a measurement equation of an AUV navigation system; then, state information of the main AUV is obtained by adopting traceless information filtering, the state information is expanded at the time of transmitting the data packet, estimation of the state of the slave AUV is finished by the traceless information filtering, and the data packet information is processed at the time of receiving the data packet; and finally, recovering navigation information obtained by the main AUV and the auxiliary AUV through information filtering. The method solves the problem of low AUV positioning precision caused by information delay in underwater acoustic communication, fully considers the problem of information correlation caused by information transmission between AUVs, solves the problem by using an information marginalization method, avoids navigation information divergence and realizes the target of high-precision real-time positioning of collaborative navigation.

Description

AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering
Technical Field
The invention relates to the technical field of nonlinear filtering and collaborative navigation, in particular to an AUV collaborative navigation method based on nonlinear information filtering.
Background
In the technical field of AUV collaborative navigation, high-precision navigation is the primary problem to be solved urgently. The centralized collaborative navigation method transmits original measurement information of all AUVs to the fusion center for processing, a data fusion process is carried out in the fusion center, the flexibility of the structure is poor, real-time operation on navigation data cannot be carried out, the practicability is not strong, and once the fusion center fails, the whole system is paralyzed. In order to solve the problem of centralized collaborative navigation, researchers have proposed a distributed collaborative navigation structure. The distributed collaborative navigation method carries out real-time processing on the measurement information of all AUVs on respective platforms, makes full use of the distance measurement information among the AUVs, and is a better choice for real-time navigation. However, the distributed navigation approach also faces several major problems: underwater acoustic navigation is severely information delay limited compared to land-based navigation systems. The speed of sound propagation underwater is approximately 1500m/s, and propagating packets on a kilometer scale results in delays on the order of seconds. The delay is inevitable in underwater communication and can greatly influence the performance of collaborative navigation positioning; secondly, the information transfer before each AUV makes its internal information have relevance, which must be considered in the data processing process.
The existing research on the general nonlinear filtering problem is quite active, and commonly used methods include 'extended Kalman filtering EKF', insensitive Kalman filtering UKF, particle filtering PF and the like. A general non-linear optimal filtering can be attributed to the problem of conditional expectations. For the case of a limited number of observations, the conditional expectation can in principle be calculated using bayesian formulae. However, even in a simpler situation, the results obtained in this way are quite complicated and inconvenient for practical application or theoretical research. Similar to kalman filtering, one would also want some recursive algorithm that gives nonlinear filtering or the random differential equations it satisfies. They are not generally present and therefore appropriate restrictions must be placed on the processes X and Y in question. The research work of nonlinear filtering is quite active and involves many recent achievements of stochastic process theory, such as stochastic process general theory, halter strap, stochastic differential equations, point processes, etc. One of the very important issues is to investigate under what conditions there is halter strap M, so that at any time M and Y contain the same information; such M is called the innovation process of Y. For a class of so-called "conditional normal processes", strictly realizable recursions of nonlinear optimal filtering have been presented. In practical applications, various linear approximation methods are often adopted for the nonlinear filtering problem.
At present, research of people in the technical field of distributed collaborative navigation is in an exploration stage, and when information is transmitted, an AUV (autonomous Underwater vehicle) cannot receive accurate collaborative navigation information due to high delay signals, so that navigation accuracy is greatly influenced; ignoring the correlation between navigation information will result in a severe decrease in positioning accuracy over long-term navigation.
In order to solve the problems, the invention provides an AUV collaborative navigation method based on nonlinear information filtering, which considers the information correlation generated in AUV information transmission, has strong real-time operability and can ensure that the AUV keeps high positioning accuracy in an information delay environment.
Disclosure of Invention
The invention relates to an AUV collaborative navigation method based on nonlinear information filtering, which comprises the steps of firstly establishing a state model and a measurement model of an AUV collaborative navigation system; then, the state of the main AUV is estimated by applying nonlinear information filtering, the state vector is expanded at the moment when the AUV data packet is transmitted, the state information at the current moment is added, and the data packet information transmitted at the previous moment is removed from the data packet information transmitted at the next moment, so that the problem caused by narrow bandwidth of an underwater acoustic channel can be avoided to the greatest extent; then, performing state estimation based on a nonlinear information filtering method on the slave AUV, and receiving and processing data packet information at the arrival time of a data packet, so as to improve the navigation and positioning accuracy of the slave AUV; and finally, performing data recovery on the information filtering results of the master AUV and the slave AUV to obtain high-precision navigation positioning information.
The method specifically comprises the following steps:
(1) establishing a state equation and a measurement equation for describing the AUV collaborative navigation system;
(2) performing state estimation based on nonlinear information filtering on the master AUV, storing current time information into a state vector at the moment when a data packet is transmitted to the slave AUV, and performing an information marginalization process on the state vector after the data packet is transmitted;
(3) performing state estimation based on nonlinear information filtering on the slave AUV, receiving and processing data at the moment when a data packet transmitted by the master AUV arrives, and performing an information marginalization process on a state vector after the received data packet is processed;
(4) and recovering the information filtering state of the master AUV and the slave AUV to obtain the navigation information of the AUV.
The method is characterized in that the step (1) is specifically as follows:
the nonlinear system model is established as follows:
Figure GDA0002565600580000021
wherein the state equation is xk＝f(xk-1)+nk-1The observation equation is zk＝h(xk)+vk，xkIs the n-dimensional state vector at the kth time; z is a radical ofkAn m-dimensional measurement vector at the kth moment; f (-) and h (-) are known non-linear functions; n isk-1N-dimensional system noise at the k-1 time; v. ofkFor m-dimensional observation of noise at the k-th time, assume random system noise nk-1～N(0,Qk-1) Q to N (μ, Σ) represent that the random vector q follows a gaussian distribution with mean μ and variance Σ; random measurement noise vk～N(0,Rk) And n isk-1And vkIs not relevant.
The method is characterized in that the step (2) is specifically as follows:
(2.1) performing one-step prediction updating: when the main AUV does not transmit the data packet at the current time, the state expansion is not carried out by one-step prediction, namely the state of the current time is not added, and the current state is assumed as follows:
Figure GDA0002565600580000022
wherein the content of the first and second substances,
Figure GDA00025656005800000328
representing a joint state vector at time k, which consists of two parts,
Figure GDA00025656005800000329
in the state at the time point k, the state,
Figure GDA00025656005800000330
the state is a historical moment state;
information filtering redefines the states as follows:
Figure GDA0002565600580000031
wherein the content of the first and second substances,
Figure GDA0002565600580000032
representing the covariance of the estimation error at time k,
Figure GDA0002565600580000033
for a matrix of time information of k, using
Figure GDA0002565600580000034
It is shown that,
Figure GDA0002565600580000035
for time information vectors of k, using
Figure GDA0002565600580000036
In this way, the joint state matrix and state vector at time k are expressed as follows:
Figure GDA0002565600580000037
wherein
Figure GDA0002565600580000038
Represents the joint information matrix at the time k,
Figure GDA0002565600580000039
information matrices representing time k and historical time respectively,
Figure GDA00025656005800000310
each represents a correlation information matrix of the k time and the historical time,
Figure GDA00025656005800000311
represents the joint information vector at time k,
Figure GDA00025656005800000312
represents the information vector at the time instant k,
Figure GDA00025656005800000313
representing a historical time information vector;
the one-step prediction results are as follows:
Figure GDA00025656005800000314
Figure GDA00025656005800000315
Figure GDA00025656005800000316
Figure GDA00025656005800000317
Figure GDA00025656005800000318
wherein the content of the first and second substances,
Figure GDA00025656005800000319
representing random system noise
Figure GDA00025656005800000320
The covariance of (a) of (b),
Figure GDA00025656005800000321
a pseudo system matrix representing a non-linear function f (-) can be defined as follows:
Figure GDA00025656005800000322
wherein the content of the first and second substances,
Figure GDA00025656005800000323
to represent
Figure GDA00025656005800000324
And
Figure GDA00025656005800000325
the cross-covariance of (a) can be expressed as follows in the unscented kalman filter algorithm by sigma sampling points:
Figure GDA00025656005800000326
wherein the content of the first and second substances,
Figure GDA00025656005800000327
all are sampling points, and 2n is the total sampling number;
when the main AUV transmits the data packet at the current moment, the state expansion is carried out by one-step prediction, the current state is added, and the data packet transmitted at the k moment is represented as:
Figure GDA0002565600580000041
Figure GDA0002565600580000042
wherein, ΛTInformation matrix, η, representing the main AUV at the time of the last packet transferTAn information vector representing a main AUV at the time of last packet transmission;
after the information transfer is finished, the pair of lambdaT、ηTTimely updating:
Figure GDA0002565600580000043
and expanding the state information at the time k into a state vector, wherein the result is as follows:
Figure GDA0002565600580000044
the corresponding information matrix and information vector are as follows:
Figure GDA0002565600580000045
Figure GDA0002565600580000046
(2.2) measurement updating:
Figure GDA0002565600580000047
Figure GDA0002565600580000048
wherein the content of the first and second substances,
Figure GDA0002565600580000049
representing the measurement noise vkThe variance of (a) is determined,
Figure GDA00025656005800000410
represents the measurement vector at time k +1,
Figure GDA00025656005800000411
represents a pseudo-metrology matrix of the nonlinear function h (-) as follows:
Figure GDA00025656005800000412
wherein the content of the first and second substances,
Figure GDA00025656005800000413
represents the cross-covariance of the one-step prediction estimate and the metrology prediction,
Figure GDA00025656005800000414
representing a one-step prediction error covariance obtained by using an unscented Kalman filtering basic equation;
(2.3) edging treatment: and after the measurement updating is finished, performing information marginalization processing on the state vector.
The method is characterized in that the step (3) is specifically as follows:
(3.1) one-step predictive update:
Figure GDA0002565600580000051
wherein
Figure GDA0002565600580000052
Represents the joint information matrix at the time k,
Figure GDA0002565600580000053
respectively representing an information matrix at the time k and a history information matrix,
Figure GDA0002565600580000054
both represent a correlation information matrix at the time k and a historical correlation information matrix,
Figure GDA0002565600580000055
represents the joint information vector at time k,
Figure GDA0002565600580000056
represents the information vector at the time instant k,
Figure GDA0002565600580000057
representing a history information vector;
the one-step prediction results are presented below:
Figure GDA0002565600580000058
Figure GDA0002565600580000059
Figure GDA00025656005800000510
Figure GDA00025656005800000511
Figure GDA00025656005800000512
wherein the content of the first and second substances,
Figure GDA00025656005800000513
a one-step prediction information matrix is represented,
Figure GDA00025656005800000514
a one-step prediction information vector is represented,
Figure GDA00025656005800000515
representing a pseudo system matrix of a non-linear function f (-),
Figure GDA00025656005800000516
representing random system noise
Figure GDA00025656005800000517
The variance of (a);
(3.2) measurement updating: when the slave AUV does not receive the data packet transmitted by the main AUV at the current moment, the data packet processing is not carried out after one-step prediction, and the local update is directly carried out:
Figure GDA00025656005800000518
Figure GDA00025656005800000519
wherein the content of the first and second substances,
Figure GDA00025656005800000520
represents a pseudo-measurement matrix of a non-linear function h (-),
Figure GDA00025656005800000521
representing the measurement noise vkThe variance of (a) is determined,
Figure GDA00025656005800000522
a measurement vector representing the time k + 1; when the slave AUV receives the data packet transmitted from the master AUV at the current moment, the data packet is processed and updated after further prediction;
Figure GDA00025656005800000523
ΛΔadding after zero padding:
Figure GDA00025656005800000524
Figure GDA00025656005800000525
the distance measurements are updated as follows:
Figure GDA00025656005800000526
Figure GDA00025656005800000527
the local measurement information is updated as follows:
Figure GDA0002565600580000061
Figure GDA0002565600580000062
(3.3) edging treatment: and after the measurement updating is finished, performing information marginalization processing on the state vector, wherein the specific algorithm is the same as the main AUV information marginalization process.
The invention has the advantages that:
(1) an AUV collaborative navigation system model is established, the existence of delay problem is fully considered by using distance measurement information between AUVs, and a high-precision collaborative navigation method based on nonlinear information filtering is provided.
(2) The information correlation problem caused by information transmission between AUVs is fully considered, marginal processing is carried out on the related information, and the accuracy of navigation positioning information is ensured while the calculation complexity is simplified.
Drawings
FIG. 1 is a flow chart of a method of the present invention;
FIG. 2 is a mean square error curve of the main AUV navigation system for estimating the position in the x coordinate axis direction based on the nonlinear information filtering method provided by the present invention;
FIG. 3 is a mean square error curve of the main AUV navigation system for estimating the position in the y coordinate axis direction based on the nonlinear information filtering method provided by the present invention;
FIG. 4 is a mean square error curve of the main AUV navigation system for estimating the speed in the x coordinate axis direction based on the nonlinear information filtering method provided by the present invention;
FIG. 5 is a mean square error curve of the velocity estimation of the main AUV navigation system for the y coordinate axis direction based on the nonlinear information filtering method provided by the present invention;
FIG. 6 is a mean square error curve estimated from the AUV navigation system for x coordinate axis direction position based on the nonlinear information filtering method provided by the present invention;
FIG. 7 is a mean square error curve estimated from the AUV navigation system for the y coordinate axis direction position based on the nonlinear information filtering method provided by the present invention;
FIG. 8 is a mean square error curve estimated from the AUV navigation system for x coordinate axis direction velocity based on the nonlinear information filtering method provided by the present invention;
FIG. 9 is a mean square error curve estimated from the AUV navigation system for the velocity in the y coordinate axis direction based on the nonlinear information filtering method provided by the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
The invention relates to an AUV collaborative navigation method based on nonlinear information filtering, which comprises the following steps:
(1) and establishing a state equation and a measurement equation for describing the AUV collaborative navigation system. Specifically, a nonlinear system model is established as follows:
Figure GDA0002565600580000071
wherein the state equation is xk＝f(xk-1)+nk-1The observation equation is zk＝h(xk)+vk，xkPosition information and velocity information of the AUV are represented for the n-dimensional state vector at the kth time, zkCharacterizing the orientation observation information of the AUV for the m-dimensional measurement vector at the k-th moment, wherein f (-) and h (-) are known nonlinear functions, n (-) isk-1For n-dimensional system noise, v, at time k-1kFor m-dimensional observation of noise at the k-th time, assume random system noise nk-1～N(0,Qk-1) (q-N (μ, Σ) denotes that the random vector q follows a Gaussian distribution with mean μ and variance Σ), and the random measurement noise vk～N(0,Rk) And n isk-1And vkIs not relevant.
(2) And carrying out state estimation based on nonlinear information filtering on the main AUV system.
(2.1) one-step predictive update
When the main AUV does not transmit the data packet at the current moment, the state expansion is not carried out by one-step prediction, namely the state of the current moment is not added, and the specific algorithm is as follows:
assume the current state as follows:
Figure GDA0002565600580000072
wherein the content of the first and second substances,
Figure GDA0002565600580000073
representing a joint state vector at time k, which consists of two parts,
Figure GDA0002565600580000074
in the state at the time point k, the state,
Figure GDA0002565600580000075
is a historical time state.
Information filtering redefines the states as follows:
Figure GDA0002565600580000076
wherein the content of the first and second substances,
Figure GDA0002565600580000077
representing the covariance of the estimation error at time k,
Figure GDA0002565600580000078
for a matrix of time information of k, using
Figure GDA0002565600580000079
It is shown that,
Figure GDA00025656005800000710
for time information vectors of k, using
Figure GDA00025656005800000711
And (4) showing. Then the joint state matrix and state vector for time k are represented as follows:
Figure GDA00025656005800000712
wherein
Figure GDA00025656005800000713
Represents the joint information matrix at the time k,
Figure GDA00025656005800000714
information matrices representing time k and historical time respectively,
Figure GDA00025656005800000715
each represents a correlation information matrix of the k time and the historical time,
Figure GDA00025656005800000716
represents the joint information vector at time k,
Figure GDA00025656005800000717
represents the information vector at the time instant k,
Figure GDA00025656005800000718
representing a historical time information vector.
The one-step prediction results are presented below:
Figure GDA00025656005800000719
Figure GDA0002565600580000081
Figure GDA0002565600580000082
Figure GDA0002565600580000083
Figure GDA0002565600580000084
wherein the content of the first and second substances,
Figure GDA0002565600580000085
representing random system noise
Figure GDA0002565600580000086
The covariance of (a) of (b),
Figure GDA0002565600580000087
a pseudo system matrix representing a non-linear function f (-) can be defined as follows:
Figure GDA0002565600580000088
wherein the content of the first and second substances,
Figure GDA0002565600580000089
to represent
Figure GDA00025656005800000810
And
Figure GDA00025656005800000811
the cross-covariance of (a) can be expressed as follows in the unscented kalman filter algorithm by sigma sampling points:
Figure GDA00025656005800000812
wherein the content of the first and second substances,
Figure GDA00025656005800000813
are all sampling points, and 2n is the total number of samples.
When the main AUV transmits the data packet at the current moment, the state expansion is carried out by one-step prediction, and the current state is added, wherein the specific algorithm is as follows:
at this time, packet transfer is performed first, and packet information at time k is transferred to the slave AUV. When the data packet is transmitted, all information at the moment k does not need to be transmitted, and only increment information from the transmission moment of the last data packet to the moment k needs to be transmitted, so that the integrity of the transmitted information is ensured, and the requirement on bandwidth is low.
The packet transmitted at time k may be represented as:
Figure GDA00025656005800000814
Figure GDA00025656005800000815
wherein, ΛTInformation matrix, η, representing the main AUV at the time of the last packet transferTAn information vector representing the main AUV at the time of last packet delivery.
After the information transfer is finished, the pair of Λ is neededT、ηTTimely updating:
Figure GDA00025656005800000816
and expanding the state information at the time k into a state vector, wherein the result is as follows:
Figure GDA00025656005800000817
then the corresponding information matrix and information vector are as follows:
Figure GDA0002565600580000091
Figure GDA0002565600580000092
(2.2) measurement update
Figure GDA0002565600580000093
Figure GDA0002565600580000094
Wherein the content of the first and second substances,
Figure GDA0002565600580000095
representing the measurement noise vkThe variance of (a) is determined,
Figure GDA0002565600580000096
represents the measurement vector at time k +1,
Figure GDA0002565600580000097
a pseudo-metrology matrix representing a non-linear function h (-) can be expressed as follows:
Figure GDA0002565600580000098
wherein the content of the first and second substances,
Figure GDA0002565600580000099
represents the cross-covariance of the one-step prediction estimate and the metrology prediction,
Figure GDA00025656005800000910
and the covariance of the prediction error in one step can be obtained by using an unscented Kalman filtering basic equation.
(2.3) edging treatment
In order to ensure that the dimension of the main AUV state vector is not too high and causes difficulty in calculation, after the measurement update is completed, the state vector is subjected to information marginalization processing, and the specific algorithm is as follows:
the first condition is as follows: when the state information needing marginalization is located at the bottom position of the information vector:
Figure GDA00025656005800000911
Figure GDA00025656005800000912
the marginalization of the beta information results in:
Figure GDA00025656005800000913
Figure GDA00025656005800000914
case two: when the state information needing marginalization is located in the middle position of the information vector:
Figure GDA0002565600580000101
Figure GDA0002565600580000102
the marginalization of the beta information results in:
Figure GDA0002565600580000103
Figure GDA0002565600580000104
(3) and performing information filtering-based state estimation on the slave AUV.
(3.1) one-step predictive update
When the slave AUV performs one-step prediction, a data packet does not need to be transmitted to the master AUV, so that information expansion is not needed, and the specific algorithm is as follows:
Figure GDA0002565600580000105
wherein
Figure GDA0002565600580000106
Represents the joint information matrix at the time k,
Figure GDA0002565600580000107
respectively representing an information matrix at the time k and a history information matrix,
Figure GDA0002565600580000108
both represent a correlation information matrix at the time k and a historical correlation information matrix,
Figure GDA0002565600580000109
represents the joint information vector at time k,
Figure GDA00025656005800001010
represents the information vector at the time instant k,
Figure GDA00025656005800001011
representing a history information vector. Note that: at this time, the history information is the information transmitted by the master AUV data packet, and the master AUV information and the slave AUV information have correlation due to the addition of the distance measurement information. Here, the algorithm is an algorithm in which information correlation is considered.
The one-step prediction results are presented below:
Figure GDA00025656005800001012
Figure GDA00025656005800001013
Figure GDA00025656005800001014
Figure GDA00025656005800001015
Figure GDA0002565600580000111
wherein the content of the first and second substances,
Figure GDA0002565600580000112
a one-step prediction information matrix is represented,
Figure GDA0002565600580000113
a one-step prediction information vector is represented,
Figure GDA0002565600580000114
representing a pseudo system matrix of a non-linear function f (-),
Figure GDA0002565600580000115
representing random system noise
Figure GDA0002565600580000116
The variance of (c).
(3.2) measurement update
When the slave AUV does not receive the data packet transmitted by the main AUV at the current moment, the data packet is not processed after one-step prediction, and local update is directly performed, wherein the specific algorithm is as follows:
Figure GDA0002565600580000117
Figure GDA0002565600580000118
wherein the content of the first and second substances,
Figure GDA0002565600580000119
represents a pseudo-measurement matrix of a non-linear function h (-),
Figure GDA00025656005800001110
representing the measurement noise vkThe variance of (a) is determined,
Figure GDA00025656005800001111
representing the measurement vector at time k + 1.
When the slave AUV receives the data packet transmitted from the master AUV at the current moment, the data packet is further processed after prediction, and the specific algorithm is as follows:
Figure GDA00025656005800001112
Figure GDA00025656005800001113
note that at this time
Figure GDA00025656005800001114
ΛΔThe direct addition is not possible and the matrix needs to be zero-padded and added again because the transfer packet does not contain the information of the AUV at the latest moment, and similarly, the information of the AUV at the current moment is not related to the packet information.
And (3) distance measurement updating:
Figure GDA00025656005800001115
Figure GDA00025656005800001116
local measurement information update
Figure GDA00025656005800001117
Figure GDA00025656005800001118
(3.3) edging treatment
In order to ensure that the dimension of the slave AUV state vector is not too high and causes difficulty in calculation, after measurement updating is completed, information marginalization processing is performed on the state vector, and a specific algorithm is the same as the main AUV information marginalization process.
(4) And recovering the information filtering state of the master AUV and the slave AUV to obtain the navigation information of the AUV.
Example (b): in AUV collaborative navigation positioning, underwater acoustic communication conditions are limiting factors which must be considered. Due to the fact that the underwater environment is complex and underwater acoustic communication is limited, the distributed AUV collaborative navigation method meets practical requirements in combination with practical situations. However, the existing distributed approach also faces a number of problems. The method provided by the invention aims to solve the problems of communication delay and information correlation in a distributed structure and provide high-precision navigation information for the AUV. The advantages of the present invention will be described below with reference to specific embodiments. The method comprises the following specific steps:
in this example, we take two AUVs cooperating with the navigation system as an example, where one master AUV and one slave AUV, the master AUV may transmit its own information and distance measurement data to the slave AUV, and the slave AUV has only packet receiving capability and does not transmit information.
In an underwater navigation system, the attitude and the depth of an AUV can be measured by using corresponding sensors respectively, and navigation information with bounded errors is obtained. When modeling, only the position and speed information of the AUV are considered, and the dimension of the state vector is reduced, so that the underwater bandwidth limitation requirement is more easily met, and the state vector is selected as follows:
x＝[x y vx vy]T (43)
then the state model and the distance measurement model are established as follows:
xk+1＝Fkxk+nk (44)
wherein the content of the first and second substances,
Figure GDA0002565600580000121
Δ T is the discrete model sampling interval. n iskIs the system noise at time k, nk～N(0,Qk)，Qk＝diag([10m 10m 0.02m/s 0.02m/s])，QkThe uncertainty of the system model is characterized.
Figure GDA0002565600580000122
Wherein z iskDistance measurement information indicating the kth time;
Figure GDA0002565600580000123
and
Figure GDA0002565600580000124
position information indicating the current time from the AUV,
Figure GDA0002565600580000125
and
Figure GDA0002565600580000126
navigation information, v, representing the main AUV received from the AUV at the current timekIs the measurement noise at the k-th time, vk～N(0,Rk)，Rk＝9m，RkThe uncertainty of the distance measurement is characterized.
The initial true state values and the initial covariance matrix are set as follows:
Figure GDA0002565600580000127
Figure GDA0002565600580000128
Figure GDA0002565600580000129
Figure GDA00025656005800001210
wherein xsAnd xcRespectively representing the initial states of the master and slave AUVs,
Figure GDA00025656005800001211
and
Figure GDA00025656005800001212
respectively representing the initial error covariance values of the master and slave AUVs,
Figure GDA00025656005800001213
and
Figure GDA00025656005800001214
the uncertainty of the initial position of the target is characterized.
Then, according to the initial state and covariance setting, the initial information matrix and information vector of the master and slave AUVs can be calculated respectively, and the specific result is as follows:
Figure GDA0002565600580000131
Figure GDA0002565600580000132
the implementation process comprises the following steps: in the simulation process, the following defined mean square error performance indexes are adopted to compare the errors of the filtering method:
Figure GDA0002565600580000133
where N is the Monte Carlo count. The smaller the mean square error value estimated for the AUV navigation information, the higher the representation positioning precision, and the better the effect.
The simulation time is 1000 seconds, 500 times of Monte Carlo simulation is carried out, and the high-precision positioning information provided by the invention is verified.

Claims (2)

1. An AUV collaborative navigation method based on nonlinear information filtering is characterized by specifically comprising the following steps:
(1) establishing a state equation and a measurement equation for describing the AUV collaborative navigation system;
(2) performing state estimation based on nonlinear information filtering on the master AUV, storing current time information into a state vector at the moment when a data packet is transmitted to the slave AUV, and performing an information marginalization process on the state vector after the data packet is transmitted;
(3) performing state estimation based on nonlinear information filtering on the slave AUV, receiving and processing data at the moment when a data packet transmitted by the master AUV arrives, and performing an information marginalization process on a state vector after the received data packet is processed;
(4) recovering the information filtering state of the master AUV and the slave AUV to obtain navigation information of the AUV;
the step (1) is specifically as follows:
the nonlinear system model is established as follows:
Figure FDA0002565600570000011
wherein the state equation is xk＝f(xk-1)+nk-1The observation equation is zk＝h(xk)+vk，xkIs the n-dimensional state vector at the kth time; z is a radical ofkAn m-dimensional measurement vector at the kth moment; f (-) and h (-) are known non-linear functions; n isk-1N-dimensional system noise at the k-1 time; v. ofkFor m-dimensional observation of noise at the k-th time, assume random system noise nk-1～N(0,Qk-1) Q to N (μ, Σ) represent that the random vector q follows a gaussian distribution with mean μ and variance Σ; random measurement noise vk～N(0,Rk) And n isk-1And vkNot related;
the step (2) is specifically as follows:
(2.1) performing one-step prediction updating:
when the main AUV does not transmit the data packet at the current time, the state expansion is not carried out by one-step prediction, namely the state of the current time is not added, and the current state is assumed as follows:
Figure FDA0002565600570000012
wherein the content of the first and second substances,
Figure FDA0002565600570000013
representing the joint state vector at time k,
Figure FDA0002565600570000014
in the state at the time point k, the state,
Figure FDA0002565600570000015
the state is a historical moment state;
information filtering redefines the states as follows:
Figure FDA0002565600570000016
wherein the content of the first and second substances,
Figure FDA0002565600570000017
representing the covariance of the estimation error at time k,
Figure FDA0002565600570000018
for a matrix of time information of k, using
Figure FDA0002565600570000019
It is shown that,
Figure FDA00025656005700000110
for time information vectors of k, using
Figure FDA00025656005700000111
In this way, the joint state matrix and state vector at time k are expressed as follows:
Figure FDA00025656005700000112
wherein
Figure FDA00025656005700000113
Represents the joint information matrix at the time k,
Figure FDA00025656005700000114
information matrices representing time k and historical time respectively,
Figure FDA00025656005700000115
each represents a correlation information matrix of the k time and the historical time,
Figure FDA0002565600570000021
represents the joint information vector at time k,
Figure FDA0002565600570000022
represents the information vector at the time instant k,
Figure FDA0002565600570000023
representing a historical time information vector;
the one-step prediction results are as follows:
Figure FDA0002565600570000024
Figure FDA0002565600570000025
Figure FDA0002565600570000026
Figure FDA0002565600570000027
Figure FDA0002565600570000028
wherein the content of the first and second substances,
Figure FDA0002565600570000029
representing random system noise
Figure FDA00025656005700000210
The covariance of (a) of (b),
Figure FDA00025656005700000211
a pseudo system matrix representing a nonlinear function f (·), defined as follows:
Figure FDA00025656005700000212
wherein the content of the first and second substances,
Figure FDA00025656005700000213
to represent
Figure FDA00025656005700000214
And
Figure FDA00025656005700000215
the cross-covariance of (a) is expressed in the unscented kalman filter algorithm as follows by sigma sampling points:
Figure FDA00025656005700000216
wherein the content of the first and second substances,
Figure FDA00025656005700000217
all are sampling points, and 2n is the total sampling number;
when the main AUV transmits the data packet at the current moment, the state expansion is carried out by one-step prediction, the current state is added, and the data packet transmitted at the k moment is represented as:
Figure FDA00025656005700000218
Figure FDA00025656005700000219
wherein, ΛTInformation matrix, η, representing the main AUV at the time of the last packet transferTAn information vector representing a main AUV at the time of last packet transmission;
after the information transfer is finished, the pair of lambdaT、ηTTimely updating:
Figure FDA00025656005700000220
and expanding the state information at the time k into a state vector, wherein the result is as follows:
Figure FDA00025656005700000221
the corresponding information matrix and information vector are as follows:
Figure FDA00025656005700000222
Figure FDA0002565600570000031
(2.2) measurement updating:
Figure FDA0002565600570000032
Figure FDA0002565600570000033
wherein the content of the first and second substances,
Figure FDA0002565600570000034
representing the measurement noise vkThe variance of (a) is determined,
Figure FDA0002565600570000035
represents the measurement vector at time k +1,
Figure FDA0002565600570000036
represents a pseudo-metrology matrix of the nonlinear function h (-) as follows:
Figure FDA0002565600570000037
wherein the content of the first and second substances,
Figure FDA0002565600570000038
represents the cross-covariance of the one-step prediction estimate and the metrology prediction,
Figure FDA0002565600570000039
representing a one-step prediction error covariance obtained by using an unscented Kalman filtering basic equation;
(2.3) edging treatment: and after the measurement updating is finished, performing information marginalization processing on the state vector.
2. The AUV collaborative navigation method based on nonlinear information filtering according to claim 1, wherein the step (3) is specifically as follows:
(3.1) one-step predictive update:
Figure FDA00025656005700000310
wherein
Figure FDA00025656005700000311
Represents the joint information matrix at the time k,
Figure FDA00025656005700000312
respectively representing an information matrix at the time k and a history information matrix,
Figure FDA00025656005700000313
both represent a correlation information matrix at the time k and a historical correlation information matrix,
Figure FDA00025656005700000314
represents the joint information vector at time k,
Figure FDA00025656005700000315
represents the information vector at the time instant k,
Figure FDA00025656005700000316
representing a history information vector;
the one-step prediction results are presented below:
Figure FDA00025656005700000317
Figure FDA00025656005700000318
Figure FDA00025656005700000319
Figure FDA00025656005700000320
Figure FDA00025656005700000321
wherein the content of the first and second substances,
Figure FDA00025656005700000322
a one-step prediction information matrix is represented,
Figure FDA00025656005700000323
a one-step prediction information vector is represented,
Figure FDA00025656005700000324
representing a pseudo system matrix of a non-linear function f (-),
Figure FDA00025656005700000325
representing random system noise
Figure FDA00025656005700000326
The variance of (a);
(3.2) measurement updating:
when the slave AUV does not receive the data packet transmitted by the main AUV at the current moment, the data packet processing is not carried out after one-step prediction, and the local update is directly carried out:
Figure FDA0002565600570000041
Figure FDA0002565600570000042
wherein the content of the first and second substances,
Figure FDA0002565600570000043
represents a pseudo-measurement matrix of a non-linear function h (-),
Figure FDA0002565600570000044
representing the measurement noise vkThe variance of (a) is determined,
Figure FDA0002565600570000045
a measurement vector representing the time k + 1; when the slave AUV receives the data packet transmitted from the master AUV at the current moment, the data packet is processed and updated after further prediction;
Figure FDA0002565600570000046
ΛΔadding after zero padding:
Figure FDA0002565600570000047
Figure FDA0002565600570000048
the distance measurements are updated as follows:
Figure FDA0002565600570000049
Figure FDA00025656005700000410
the local measurement information is updated as follows:
Figure FDA00025656005700000411
Figure FDA00025656005700000412
(3.3) edging treatment: and after the measurement updating is finished, performing information marginalization processing on the state vector, wherein the specific algorithm is the same as the main AUV information marginalization process.
CN201710805228.1A 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering Active CN107677272B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710805228.1A CN107677272B (en) 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710805228.1A CN107677272B (en) 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering

Publications (2)

Publication Number Publication Date
CN107677272A CN107677272A (en) 2018-02-09
CN107677272B true CN107677272B (en) 2020-11-10

Family

ID=61134575

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710805228.1A Active CN107677272B (en) 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering

Country Status (1)

Country Link
CN (1) CN107677272B (en)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108594834B (en) * 2018-03-23 2020-12-22 哈尔滨工程大学 Multi-AUV self-adaptive target searching and obstacle avoiding method oriented to unknown environment
CN108827305A (en) * 2018-05-25 2018-11-16 哈尔滨工程大学 A kind of AUV collaborative navigation method based on robust information filtering
CN108802692A (en) * 2018-05-25 2018-11-13 哈尔滨工程大学 A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter
CN109084767A (en) * 2018-06-15 2018-12-25 哈尔滨工程大学 A kind of AUV collaborative navigation method of the adaptive volume particle filter of maximum cross-correlation entropy
CN108489498A (en) * 2018-06-15 2018-09-04 哈尔滨工程大学 A kind of AUV collaborative navigation methods without mark particle filter based on maximum cross-correlation entropy
CN109974706B (en) * 2019-03-08 2021-05-11 哈尔滨工程大学 Master-slave mode multi-AUV collaborative navigation method based on double motion model
CN109931936A (en) * 2019-03-18 2019-06-25 西北工业大学 A kind of weak connectedness AUV collaborative navigation method based on mobile-relay station
CN111257913B (en) * 2019-11-29 2024-04-30 交通运输部长江通信管理局 Beidou satellite signal capturing method and device
CN111308576A (en) * 2019-11-29 2020-06-19 哈尔滨工程大学 Multi-AUV system and deep sea coral detection method
CN112857313B (en) * 2020-12-31 2023-03-24 哈尔滨工程大学 Sounding information transmission method oriented to low-bandwidth acoustic channel
CN112945245B (en) * 2021-02-05 2022-09-27 中国航天空气动力技术研究院 Observability analysis method in multi-AUV (autonomous underwater vehicle) collaborative navigation system based on condition number theory
CN113074725B (en) * 2021-05-11 2022-07-22 哈尔滨工程大学 Small underwater multi-robot cooperative positioning method and system based on multi-source information fusion
CN113608169B (en) * 2021-05-20 2023-08-25 济南大学 Multi-AUV (autonomous Underwater vehicle) co-location method based on sequential fusion algorithm

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103336267A (en) * 2013-07-02 2013-10-02 哈尔滨工程大学 Master-slave mode multi-UUV (Unmanned Underwater Vehicle) cooperative location method based on underwater acoustic communication delay
CN103616026A (en) * 2013-12-17 2014-03-05 哈尔滨工程大学 AUV (Autonomous Underwater Vehicle) manipulating model auxiliary strapdown inertial navigation combined navigation method based on H infinity filtering
US8903641B2 (en) * 2012-04-26 2014-12-02 The United States Of America, As Represented By The Secretary Of The Navy Collaborative robot manifold tracker
CN105445722A (en) * 2015-11-09 2016-03-30 哈尔滨工程大学 Underwater acoustic two-way distance-measuring error compensation method applied in dynamic condition of multi-AUV coordinative navigation
CN106441300A (en) * 2016-09-08 2017-02-22 哈尔滨工程大学 Self-adaptive collaborative navigation and filtering method
CN106525042A (en) * 2016-09-27 2017-03-22 哈尔滨工程大学 Multi-AUV synthetic location method based on combination of ant colony and extended Kalman filtering

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8903641B2 (en) * 2012-04-26 2014-12-02 The United States Of America, As Represented By The Secretary Of The Navy Collaborative robot manifold tracker
CN103336267A (en) * 2013-07-02 2013-10-02 哈尔滨工程大学 Master-slave mode multi-UUV (Unmanned Underwater Vehicle) cooperative location method based on underwater acoustic communication delay
CN103616026A (en) * 2013-12-17 2014-03-05 哈尔滨工程大学 AUV (Autonomous Underwater Vehicle) manipulating model auxiliary strapdown inertial navigation combined navigation method based on H infinity filtering
CN105445722A (en) * 2015-11-09 2016-03-30 哈尔滨工程大学 Underwater acoustic two-way distance-measuring error compensation method applied in dynamic condition of multi-AUV coordinative navigation
CN106441300A (en) * 2016-09-08 2017-02-22 哈尔滨工程大学 Self-adaptive collaborative navigation and filtering method
CN106525042A (en) * 2016-09-27 2017-03-22 哈尔滨工程大学 Multi-AUV synthetic location method based on combination of ant colony and extended Kalman filtering

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
A collaborative control framework with multi-leaders;Yunxin Zhao;《Journal of the Franklin Institute》;20161231;第353卷(第3期);第657-669页 *
一种基于无迹卡尔曼滤波的UUV协同定位方法;刘明雍等;《鱼雷技术》;20110615(第03期);第205-208页 *
一种考虑时钟同步问题的多AUV协同定位算法;张福斌等;《鱼雷技术》;20131015(第05期);第355-359页 *
水下航行器组合导航定位***设计研究;赵辉;《南通大学学报(自然科学版)》;20090615(第02期);第17-20页 *

Also Published As

Publication number Publication date
CN107677272A (en) 2018-02-09

Similar Documents

Publication Publication Date Title
CN107677272B (en) AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering
Liu et al. Maximum correntropy square-root cubature Kalman filter with application to SINS/GPS integrated systems
CN108255791B (en) Maneuvering target tracking method based on distributed sensor consistency
Viegas et al. Discrete-time distributed Kalman filter design for formations of autonomous vehicles
Li et al. 3-D motion estimation and online temporal calibration for camera-IMU systems
Wang et al. Gaussian filter for nonlinear systems with one-step randomly delayed measurements
CN108594272B (en) Robust Kalman filtering-based anti-deception jamming integrated navigation method
JP6525325B2 (en) Method and device for determining device location
CN107270894B (en) GNSS/SINS deep integrated navigation system based on dimension reduction
Xu et al. A novel adaptive filter for cooperative localization under time-varying delay and non-Gaussian noise
Zhou et al. Critical issues on Kalman filter with colored and correlated system noises
Sun et al. A maximum correntropy divided difference filter for cooperative localization
Xu et al. A novel robust filter for outliers and time-varying delay on an SINS/USBL integrated navigation model
CN115307643A (en) Double-responder assisted SINS/USBL combined navigation method
CN113175931A (en) Cluster networking collaborative navigation method and system based on constraint Kalman filtering
Liu et al. A SINS aided correct method for USBL range based on maximum correntropy criterion adaptive filter
Shin et al. A new fusion formula and its application to continuous-time linear systems with multisensor environment
Zhao et al. Gaussian filter for nonlinear networked systems with synchronously correlated noises and one-step randomly delayed measurements and multiple packet dropouts
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN107886058B (en) Noise-related two-stage volume Kalman filtering estimation method and system
US8433544B2 (en) Nonlinear variable lag smoother
CN112762936B (en) Multi-source positioning information fusion method applied to long-endurance unmanned aerial vehicle load
CN103605143A (en) Neural network integrated navigation method
US20100289699A1 (en) Method and Apparatus for Determining a Position of a Mobile Station in a Wireless Communication System
Zhao et al. A single beacon-aided cooperative localization algorithm based on maximum correntropy criterion

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