CN109596128B - Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone - Google Patents

Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone Download PDF

Info

Publication number
CN109596128B
CN109596128B CN201910033292.1A CN201910033292A CN109596128B CN 109596128 B CN109596128 B CN 109596128B CN 201910033292 A CN201910033292 A CN 201910033292A CN 109596128 B CN109596128 B CN 109596128B
Authority
CN
China
Prior art keywords
auv
following
navigation
time
hydrophones
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
CN201910033292.1A
Other languages
Chinese (zh)
Other versions
CN109596128A (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 CN201910033292.1A priority Critical patent/CN109596128B/en
Publication of CN109596128A publication Critical patent/CN109596128A/en
Application granted granted Critical
Publication of CN109596128B publication Critical patent/CN109596128B/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
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

The invention belongs to the field of autonomous underwater navigation, and particularly relates to a method for improving the multi-AUV (autonomous underwater vehicle) co-location performance based on multiple hydrophones. The method comprises the following steps: configuring a plurality of hydrophones on the following AUV, setting the positions of the hydrophones and fixing the hydrophones; the navigation AUV and the following AUV perform underwater acoustic ranging through hydrophones, and simultaneously broadcast and send the self position to the following AUV; establishing a single-pilot AUV mode co-location system state space model; and positioning the follow-up AUV by using an EKF filtering method. By designing the positions of a plurality of hydrophones on the following AUV, the measurement matrix dimension of the state space model is increased, the observability of the system is greatly improved, and the requirement on high maneuverability of each AUV in a single navigation scheme is effectively reduced; the navigation AUVs of the multi-AUV collaborative navigation system are reduced, the high-precision inertial measurement unit required to be equipped by arranging a plurality of navigation AUVs is avoided, and the cost is reduced; the method does not relate to the problem of multi-pilot AUV time synchronization, is easy to implement and has higher positioning precision.

Description

Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone
Technical Field
The invention belongs to the field of autonomous underwater navigation, and particularly relates to a method for improving multi-AUV (autonomous underwater vehicle) cooperative positioning performance based on multiple hydrophones.
Background
The collaborative navigation is one of the most effective navigation methods of the multi-autonomous underwater vehicle in the middle layer area at present, and has wide application prospect. The AUV collaborative navigation technology of the autonomous underwater vehicle based on the underwater acoustic communication network is a new AUV underwater navigation scheme developed along with a multi-AUV collaborative system, compared with the traditional inertial navigation and acoustic baseline navigation, the collaborative navigation is high in precision, low in cost and good in robustness, the collaborative operation capability of the multi-AUV system can be greatly enhanced, and the AUV collaborative navigation technology has important research value. According to the different number of the piloting AUVs, the single piloting mode and the multi-piloting mode can be divided. The single-pilot mode has the advantages of simple structure, easiness in deployment, easiness in operation and low cost requirement, but the integral navigation performance of the system depends on a single pilot AUV, so that the reliability requirement on the pilot AUV is high; secondly, the observability of a single-piloted AUV system is low, and the piloted AUV is required to continuously carry out navigation maneuver so as to improve the observability of the system. Compared with a single-pilot AUV scheme, the multi-pilot scheme has the advantage that the reliability requirement of the system on the single-pilot AUV is greatly reduced due to the increase of the number of pilot AUVs. Meanwhile, due to the fact that observability of the system is greatly improved, the observability of the system is guaranteed without the aid of navigation maneuvers of the piloted AUV in the cooperation process, the cooperation positioning performance of the system is improved, and the cooperation execution of tasks according to a preset task formation is facilitated. But the navigation cost of the system is increased due to the increased number of piloted AUVs compared to the single piloting mode. From the development trend of underwater collaborative navigation technology, collaborative navigation based on a single-pilot AUV scheme is undoubtedly more attractive and is receiving more and more attention and development. However, system observability is a prerequisite for achieving coordinated navigation of underwater vehicles, and is especially crucial for single-pilot solutions. If the described system is observable, the carrier self-positioning error can be corrected cooperatively by fusing the relatively accurate position information of the piloted AUV and the distance measurement information between the piloted AUV and the carrier self-positioning error; on the contrary, no matter what filtering algorithm is adopted, accurate estimation of the position state cannot be realized, so that how to enhance observability of the system is very important for improving cooperative positioning performance of the multi-AUV cooperative navigation system.
Disclosure of Invention
The invention aims to provide a multi-AUV collaborative navigation method which is provided with a plurality of hydrophones on a following AUV and is based on nonlinear filtering estimation, so that the observability of a master-slave multi-AUV collaborative navigation system in a single pilot mode can be improved, and the collaborative positioning performance of the system is finally improved.
The purpose of the invention is realized as follows:
a method for improving multi-AUV cooperative positioning performance based on multiple hydrophones comprises the following steps:
step 1: configuring a plurality of hydrophones on the following AUV, setting the positions of the hydrophones and fixing the hydrophones;
step 2: the navigation AUV and the following AUV perform underwater acoustic ranging through the hydrophones to obtain the distance between a plurality of hydrophones configured on the following AUV and the navigation AUV, and the navigation AUV broadcasts the position of the navigation AUV to the following AUV;
and step 3: establishing a single-pilot AUV mode co-location system state space model according to the position information of the hydrophone, the distance information between the hydrophone on the following AUV and the pilot AUV, the pilot AUV position information and the position information of the combined position-push of the following AUV;
and 4, step 4: and obtaining an estimated value of the system state by using an EKF filtering method, and positioning the following AUV.
As described in step 1Configuring a plurality of hydrophones on a following AUV includes: establishing a carrier coordinate system Ox by taking the mass center following AUV as the origin of coordinatesbybzbX of the carrier coordinate systembAxis follows AUV horizontal axis to right, ybAxis pointing forward following AUV longitudinal axis, zbThe axis follows the AUV vertical axis and is parallel to xb、ybThe axes form a right-hand rectangular coordinate system; sailing along the longitudinal axis of the carrier coordinate system along with the AUV, wherein the positions of a plurality of hydrophones configured on the following AUV are not all in the same longitudinal axis; respectively fixing a hydrophone A and a hydrophone B at the head and tail sides of the following AUV, and respectively setting the position coordinates of the hydrophone A and the hydrophone B in the carrier coordinate system as (a, B,0) and (-c, -d,0), wherein a is x of the hydrophonebAxial distance, c is x following AUV centroidbAxial distance, b is hydrophone ybAxial distance, d is y following AUV centroidbAxial distance in meters.
And 3, establishing a single-pilot AUV mode cooperative positioning system state space model expressed as:
Figure BDA0001944998790000021
wherein k +1 represents tk+1Time of day; xk＝(xk,yk,θk)TTo follow AUV at tkState quantity of time, xkTo follow AUV at t in the navigation coordinate systemkEast position coordinate of time, ykTo follow AUV at tkNorth position coordinate of time thetakTo follow AUV at tkA course angle at a moment; xk+1To follow AUV at tk+1A state quantity at a time; u. ofk＝(Vkθk)TIs tkSensor input, V, of a time systemkTo follow AUV at tkForward speed of the moment; zk+1Is tk+1A measurement vector of a moment;
Figure BDA0001944998790000022
AUV at t for pilotingk+1The amount of state at the moment of time,
Figure BDA0001944998790000023
to follow and pilot AUV at tk+1The east-oriented position coordinates of the time of day,
Figure BDA0001944998790000024
AUV at t for pilotingk+1A north position coordinate of time; w is akIs a process noise vector, vk+1To measure the noise vector, and wk、vk+1Are both Gauss white noise; function of state
Figure BDA0001944998790000025
δ t is the sampling period; ω is the angular rate; the measurement function is expressed as:
Figure BDA0001944998790000031
wherein x isk+1To follow the AUV at t in the navigation coordinate systemk+1East position coordinates of time; y isk+1To follow AUV at tk+1A north position coordinate of time;
Figure BDA0001944998790000032
is tk+1The east distance of the hydrophone a to the centroid of the following AUV in the navigational coordinate system at time,
Figure BDA0001944998790000033
is tk+1The north distance of the hydrophone a to the centroid of the following AUV in the navigation coordinate system at the moment,
Figure BDA0001944998790000034
is tk+1The east distance of the hydrophone B to the centroid of the following AUV in the navigational coordinate system at time,
Figure BDA0001944998790000035
is tk+1The north distance of the hydrophone B to the following AUV centroid in the navigation coordinate system at time.
Hypothesis State vector Xk＝[xk,yk]TAt tkPosterior probability state of time system
Figure BDA0001944998790000036
And a posterior probability density function
Figure BDA0001944998790000037
Known as PkkIs tkState estimation variance matrix at time, ZkIs tkMeasuring vectors formed by distances from two hydrophones on the following AUV to the pilot AUV, which are obtained by underwater acoustic ranging at the moment, and the EKF filtering method used in the step 4 comprises the following steps:
step 4.1: updating the time;
step 4.2: updating the measurement;
step 4.3: the process is gradually recurred until the covariance of the state estimation error reaches a stable value.
Step 4.1 the time update includes a one-step prediction equation of state expressed as:
Figure BDA0001944998790000038
the state prediction variance matrix is represented as:
Figure BDA0001944998790000039
wherein the system state transition matrix
Figure BDA00019449987900000310
Figure BDA00019449987900000316
Is FkTransposing; system noise excitation array
Figure BDA00019449987900000311
Figure BDA00019449987900000312
Is GkTransposing; system noise covariance matrix
Figure BDA00019449987900000313
wk TIs wkThe process of transposition is carried out,
Figure BDA00019449987900000314
for the sensor speed noise variance to be the sensor speed noise variance,
Figure BDA00019449987900000315
is the heading noise variance.
The measurement update of step 4.2 includes the filter gain expressed as:
Figure BDA0001944998790000041
wherein the measurement matrix is represented by the following formula:
Figure BDA0001944998790000042
wherein the content of the first and second substances,
Figure BDA0001944998790000043
h is Hk+1Transposing; measuring noise covariance matrix Rk+1＝E[vk+1vk+1 T]，vk+1 TIs v isk+1Transposition is carried out;
the state estimate is expressed as:
Figure BDA0001944998790000044
the state estimation variance matrix is expressed as:
Pk+1|k+1＝[I-Kk+1Hk+1]Pk+1|k
in the formula, I is a unit array,
Figure BDA0001944998790000045
following AUV at t, estimated by filteringk+1A state quantity at a time;
and gradually recursing the process until the covariance of the state estimation error reaches a stable value, so as to obtain the estimation of the system state and position the following AUV.
The invention has the beneficial effects that:
1. the following AUV is provided with a plurality of hydrophones, and the position of the plurality of hydrophones on the following AUV is designed to increase the measurement matrix dimension of the state space model, so that the observability of the system is greatly improved, and the requirement on the high maneuverability of each AUV in a single navigation scheme is effectively reduced;
2. the navigation AUV number of the multi-AUV collaborative navigation system is reduced, and the cost is reduced because a plurality of high-precision inertia measurement units which are required to be equipped by the navigation AUV are arranged;
3. the collaborative navigation method based on the multiple hydrophones does not relate to the problem of multi-pilot AUV time synchronization, is easy to implement and has higher positioning precision.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a schematic view of the installation of a hydrophone on a follow-up AUV;
FIG. 3 is a scheme of ranging between a hydrophone pilot AUV and a following AUV;
FIG. 4 is an EKF filtering flow diagram;
FIG. 5 is a diagram of the true sailing trajectory of the piloting AUV and the following AUV;
FIG. 6 is a comparative diagram of collaborative navigation following AUV trajectory;
FIG. 7 is a positioning error contrast diagram.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
The multi-AUV collaborative navigation system of the single navigation scheme has low cost and easy operation, but has higher requirements on navigation AUV and following AUV motion states. In the single-pilot mode, if pilot AUV and following AUV maneuver along mutually parallel linear paths at the same navigation speed, the system global is not observable; if the piloting AUV and the following AUV navigate at the same speed and in the same direction or the tangential directions of the navigation speeds are consistent, the multi-AUV collaborative navigation system is locally unobservable. The observability of the collaborative navigation system can be met by designing a complex route planning scheme. However, for an AUV system with slow speed and poor maneuverability, it is often impractical to perform complex route maneuvers during the actual task execution process in order to improve the observability of the system. Aiming at the problems, the patent designs a method for improving observability of a multi-AUV (autonomous underwater vehicle) collaborative navigation system based on multiple hydrophones, which is characterized in that multiple hydrophones are arranged on a following AUV (autonomous underwater vehicle), the distance from each hydrophone to a pilot AUV and the coordinate relation between the hydrophone and the mass center of the following AUV are obtained through underwater acoustic ranging, a state space model of the multi-AUV collaborative navigation system is established, observability of the system is enhanced, and collaborative positioning performance of the multi-AUV system is improved.
The method comprises the following steps: a hydrophone is disposed on the trailing AUV.
Establishing a carrier coordinate system Ox by taking the mass center following AUV as the origin of coordinatesbybzbX of the carrier coordinate systembAxis follows AUV horizontal axis to right, ybAxis pointing forward following AUV longitudinal axis, zbThe axis follows the AUV vertical axis and is parallel to xb、ybThe axes form a right-handed rectangular coordinate system. The following AUV sails along the longitudinal axis of the carrier coordinate system, the positions of a plurality of hydrophones configured on the following AUV are not all on the same longitudinal axis, and the hydrophones are far away from each other as far as possible. The observability of the collaborative navigation system can be improved by installing a plurality of hydrophones on the AUV, but the positioning performance of the collaborative navigation system is not obviously improved by installing too many hydrophones, and the cost requirement is high. Taking the example of configuring two hydrophones on the following AUV, fixing the hydrophone A and the hydrophone B on the head and the tail of the following AUV respectively, and setting the position coordinates of the hydrophone A and the hydrophone B in the carrier coordinate system as (a, B,0) and (-c, -d,0) respectively, wherein a and c are x of the mass center of the hydrophone and the mass center of the following AUVbThe axial distance b and d are y of the hydrophone and the mass center following the AUVbAxial distance in meters.
Step two: and performing underwater acoustic ranging on the piloted AUV and the following AUV, and broadcasting the self position of the piloted AUV.
And the navigation AUV and the following AUV perform underwater acoustic ranging through the hydrophones to respectively obtain the distance between the two hydrophones configured on the following AUV and the navigation AUV, and simultaneously, the navigation AUV broadcasts the self position information to the following AUV.
Step three: and establishing a single-pilot AUV cooperative positioning system state space model based on the hydrophone.
In actual underwater co-location, the depth and horizontal position of the AUV are independent of each other, and accurate depth information can be obtained through the pressure sensor, so that the three-dimensional co-location problem can be simplified into two dimensions, and the model is projected to a two-dimensional horizontal plane for analysis in discussion. In a navigation coordinate system OxtytIn, defining following AUV at tkTime t andk+1the position vectors at the time are respectively (x)k,yk) And (x)k+1,yk+1) The position vectors of the piloting AUV at the time tk and the time tk +1 are respectively
Figure BDA0001944998790000061
And
Figure BDA0001944998790000062
Figure BDA0001944998790000063
following AUV and piloting AUV at tkEast position coordinates of time;
Figure BDA0001944998790000064
following AUV and piloting AUV at tkA north position coordinate of time;
Figure BDA0001944998790000065
following AUV and piloting AUV at tk+1East position coordinates of time;
Figure BDA0001944998790000066
following AUV and piloting AUV at tk+1Time of dayNorth position coordinates of. The collaborative navigation system state equation based on the relative position measurement specifically comprises the following steps:
Figure BDA0001944998790000067
in the formula, xkAnd xk+1Respectively following AUV at t in the navigation coordinate systemkTime t andk+1east position coordinates of time; y iskAnd yk+1Respectively following AUV at tkTime t andk+1a north position coordinate of time; δ t is the sampling period; vkTo follow AUV at tkForward speed of time, thetakTo follow AUV at tkA course angle at a moment; ω is the angular rate.
Will follow the hydrophone A on the AUV to x following the AUV centroidbAxial distance and ybThe axial distance is obtained by projecting the carrier coordinate system to the navigation coordinate system:
Figure BDA0001944998790000068
in the formula (I), the compound is shown in the specification,
Figure BDA0001944998790000069
the east distance and the north distance from the hydrophone A to the following AUV centroid distance projected to the navigation coordinate system are shown, and the position vector of the hydrophone A in the navigation coordinate system is shown as
Figure BDA00019449987900000610
The same principle is that:
Figure BDA00019449987900000611
in the formula (I), the compound is shown in the specification,
Figure BDA00019449987900000612
projecting the distance from the hydrophone B to the following AUV centroidThe east distance and the north distance in the navigation coordinate system are obtained to obtain the position vector of the hydrophone B in the navigation coordinate system as
Figure BDA00019449987900000613
Following AUV at tkRelative observation information obtained by underwater acoustic communication at moment is piloting AUV at tkPosition information of time of day
Figure BDA0001944998790000071
And the measurement information of the relative distance between the piloting AUV and the following AUV are respectively
Figure BDA0001944998790000072
And
Figure BDA0001944998790000073
measurement vector
Figure BDA0001944998790000074
The coordinate position relation between the pilot AUV and the following AUV obtained by the information is as follows:
Figure BDA0001944998790000075
in the formula (I), the compound is shown in the specification,
Figure BDA0001944998790000076
respectively piloting AUV at t in a navigation coordinate systemkTime t andk+1east and north position coordinates of the time;
based on the formula (1) and the formula (2), a discrete time state space model (a state equation and a measurement equation) of the multi-AUV cooperative positioning system is established as follows:
Figure BDA0001944998790000077
wherein k +1 represents tk+1Time of day; xk＝(xk,yk,θk)TTo follow AUV at tkA state quantity at a time; xk+1To follow AUV at tk+1A state quantity at a time; u. ofk＝(Vkθk)TIs tkSensor input of the time system; zk+1Is tk+1A measurement vector of a moment;
Figure BDA0001944998790000078
AUV at t for pilotingk+1A state quantity at a time; w is akIs a process noise vector, vk+1To measure the noise vector, and wk、vk+1Are both Gauss white noise; function of state
Figure BDA0001944998790000079
Measurement function
Figure BDA00019449987900000710
Figure BDA00019449987900000711
Is tk+1The east and north distances from the hydrophone a to the centroid of the following AUV in the navigational coordinate system at time,
Figure BDA00019449987900000712
is tk+1And the east distance and the north distance from the hydrophone B to the centroid of the following AUV in the navigation coordinate system at the moment.
Step four: and obtaining an estimated value of the system state by utilizing an EKF filtering method, thereby realizing the cooperative positioning of the following AUV.
Hypothesis state vector Xk＝[xk,yk]TAt tkPosterior probability state of time system
Figure BDA00019449987900000713
And a posterior probability density function
Figure BDA00019449987900000714
Known as PkkIs tkState estimation variance at time of dayArray, ZkIs tkAnd the time is a measurement vector formed by the distances from the two hydrophones on the following AUV to the pilot AUV, which is obtained by underwater acoustic ranging.
(1) Time updating
One-step prediction equation of state:
Figure BDA0001944998790000081
state prediction variance matrix:
Figure BDA0001944998790000082
wherein the system state transition matrix
Figure BDA0001944998790000083
Figure BDA0001944998790000084
Is FkTransposing; system noise excitation array
Figure BDA0001944998790000085
Figure BDA0001944998790000086
Is GkTransposing; system noise covariance matrix
Figure BDA0001944998790000087
wk TIs wkThe process of transposition is carried out,
Figure BDA0001944998790000088
and
Figure BDA0001944998790000089
sensor speed noise variance and heading noise variance, respectively.
(2) Measurement update
Filtering gain:
Figure BDA00019449987900000810
wherein the measuring matrix
Figure BDA00019449987900000811
Figure BDA00019449987900000812
H is Hk+1Transposing; measuring noise covariance matrix Rk+1＝E[vk+1vk+1 T]，vk+1 TIs v isk+1And (4) transposition.
And (3) state estimation:
Figure BDA00019449987900000813
state estimation variance matrix:
Pk+1|k+1＝[I-Kk+1Hk+1]Pk+1|k (10)
in the formula, I is a unit array,
Figure BDA00019449987900000814
following AUV at t, estimated by filteringk+1The state quantity of the moment.
The process is carried out by recursion successively until the covariance of the state estimation error reaches a stable value, and the estimation of the system state is obtained, so that the following AUV is positioned.
In order to further explain the beneficial effects of the invention, the simulation is carried out on the multi-AUV co-location scheme based on the double hydrophones under the experimental environment that the navigation AUV is provided with the inertial navigation equipment and the Doppler log and the MEMS inertial system and the Doppler log are arranged along with the AUV:
the simulation time is 3600 s; the sampling period is 1 s; the piloting AUV makes curvilinear motion at the angular rate of 0.1 degree/s and follows the AUV to directly navigate at a constant speed along the direction with the course angle of 60 degrees; the speeds of the piloting AUV and the following AUV are both 4 kn; following AUV speed sensor volumeMeasure noise as
Figure BDA0001944998790000091
Zero mean gaussian white noise; the noise is taken as following AUV course angle measurement
Figure BDA0001944998790000092
The zero mean Gaussian white noise exists, and a course drift error of 5 degrees/h exists along with the AUV course angle; the simulation also introduces the measurement noise of underwater acoustic distance measurement between the pilot AUV and the following AUV, and the variance is (0.5m)2(ii) a The hydrophones A and the hydrophones B are respectively installed at the upper right corner and the lower left corner of the following AUV, the transverse distance between the two hydrophones and the mass center of the following AUV is 2 meters, the longitudinal distance is 5 meters, and a simulation experiment is carried out.

Claims (4)

1. A method for improving multi-AUV co-location performance based on multiple hydrophones is characterized by comprising the following steps:
step 1: configuring a plurality of hydrophones on the following AUV, setting the positions of the hydrophones and fixing the hydrophones;
establishing a carrier coordinate system Ox by taking the mass center following AUV as the origin of coordinatesbybzbX of the carrier coordinate systembAxis follows AUV horizontal axis to right, ybAxis pointing forward following AUV longitudinal axis, zbThe axis follows the AUV vertical axis and is parallel to xb、ybThe axes form a right-hand rectangular coordinate system; navigating along the longitudinal axis of the carrier coordinate system along the AUV, wherein the positions of a plurality of hydrophones configured on the following AUV are not in the same longitudinal axis; respectively fixing a hydrophone A and a hydrophone B at the head and tail sides of the following AUV, and respectively setting the position coordinates of the hydrophone A and the hydrophone B in the carrier coordinate system as (a, B,0) and (-c, -d,0), wherein a is x of the hydrophonebAxial distance, c is x following AUV centroidbAxial distance, b is hydrophone ybAxial distance, d is y following AUV centroidbThe distance between the shafts;
step 2: the navigation AUV and the following AUV perform underwater acoustic ranging through the hydrophones to obtain the distance between a plurality of hydrophones configured on the following AUV and the navigation AUV, and the navigation AUV broadcasts the position of the navigation AUV to the following AUV;
and step 3: establishing a single-pilot AUV mode co-location system state space model according to the position information of the hydrophone, the distance information between the hydrophone on the following AUV and the pilot AUV, the pilot AUV position information and the position information of the combined position-push of the following AUV;
the state space model of the co-location system in the single-pilot AUV mode is expressed as follows:
Figure FDA0003590557370000011
wherein k +1 represents tk+1Time of day; xk＝(xk,yk,θk)TTo follow AUV at tkState quantity of time, xkTo follow the AUV at t in the navigation coordinate systemkEast position coordinate of time, ykTo follow AUV at tkNorth position coordinate of time thetakTo follow AUV at tkA course angle at a moment; xk+1To follow AUV at tk+1A state quantity at a time; u. uk＝(Vkθk)TIs tkSensor input of the time system; vkTo follow AUV at tkForward speed of the moment; zk+1Is tk+1A measurement vector of a moment;
Figure FDA0003590557370000012
AUV at t for pilotingk+1The amount of state at the moment of time,
Figure FDA0003590557370000013
to follow and pilot AUV at tk+1The east-oriented position coordinates of the time of day,
Figure FDA0003590557370000014
AUV at t for pilotingk+1A north position coordinate of time; w is akIs a process noise vector; v. ofk+1To measure the noise vector, and wk、vk+1Are both Gauss white noise; function of state
Figure FDA0003590557370000015
δ t is the sampling period; ω is the angular rate;
the measurement function is expressed as:
Figure FDA0003590557370000021
wherein x isk+1To follow AUV at t in the navigation coordinate systemk+1East position coordinates of time; y isk+1To follow AUV at tk+1A north position coordinate of time;
Figure FDA0003590557370000022
is tk+1The east distance of the hydrophone a to the centroid of the following AUV in the navigational coordinate system at time,
Figure FDA0003590557370000023
is tk+1The north distance from the hydrophone A to the mass center of the AUV in the navigation coordinate system at the moment;
Figure FDA0003590557370000024
is tk+1The east distance from the hydrophone B to the mass center of the AUV in the navigation coordinate system at the moment;
Figure FDA0003590557370000025
is tk+1The north distance from the hydrophone B to the mass center of the AUV in the navigation coordinate system at the moment;
and 4, step 4: and obtaining an estimated value of the system state by using an EKF filtering method, and positioning the following AUV.
2. The method of claim 1, wherein the method for improving the co-location performance of multiple AUVs is based on multiple hydrophones, and comprises the following steps: hypothesis state vector Xk＝[xk,yk]TAt tkPosterior probability state of time system
Figure FDA0003590557370000026
And a posterior probability density function
Figure FDA0003590557370000027
Known as Pk|kIs tkState estimation variance matrix at time, ZkIs tkMeasuring vectors formed by distances from two hydrophones on the following AUV to the pilot AUV, which are obtained by underwater acoustic ranging at the moment, and the EKF filtering method used in the step 4 comprises the following steps:
step 4.1: updating the time;
step 4.2: updating the measurement;
step 4.3: the process is gradually recurred until the covariance of the state estimation error reaches a stable value.
3. The method of claim 2, wherein the time update of step 4.1 comprises a state one-step prediction equation expressed as:
Figure FDA0003590557370000028
the state prediction variance matrix is represented as:
Figure FDA0003590557370000029
wherein the system state transition matrix
Figure FDA00035905573700000210
Fk TIs FkTransposing; system noise excitation array
Figure FDA00035905573700000211
Figure FDA00035905573700000212
Is GkTransposing; system noise covariance matrix
Figure FDA0003590557370000031
wk TIs wkThe process of transposition is carried out,
Figure FDA0003590557370000032
for the sensor speed noise variance to be the sensor speed noise variance,
Figure FDA0003590557370000033
is the heading noise variance.
4. The method of claim 2, wherein the measurement update of step 4.2 comprises a filter gain expressed as:
Figure FDA0003590557370000034
wherein the measurement matrix is represented by the following formula:
Figure FDA0003590557370000035
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003590557370000036
h is Hk+1Transposing; measuring noise covariance matrix Rk+1＝E[vk+1vk+1 T]，vk+1 TIs v isk+1Transposition is carried out;
the state estimate is expressed as:
Figure FDA0003590557370000037
the state estimation variance matrix is expressed as:
Pk+1|k+1＝[I-Kk+1Hk+1]Pk+1|k
in the formula, I is a unit array,
Figure FDA0003590557370000038
following AUV at t, estimated by filteringk+1A state quantity at a time;
and gradually recursing the process until the covariance of the state estimation error reaches a stable value, so as to obtain the estimation of the system state and position the following AUV.
CN201910033292.1A 2019-01-14 2019-01-14 Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone Active CN109596128B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910033292.1A CN109596128B (en) 2019-01-14 2019-01-14 Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910033292.1A CN109596128B (en) 2019-01-14 2019-01-14 Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone

Publications (2)

Publication Number Publication Date
CN109596128A CN109596128A (en) 2019-04-09
CN109596128B true CN109596128B (en) 2022-06-21

Family

ID=65966253

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910033292.1A Active CN109596128B (en) 2019-01-14 2019-01-14 Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone

Country Status (1)

Country Link
CN (1) CN109596128B (en)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110186461B (en) * 2019-06-27 2021-02-19 武汉科技大学 Collaborative navigation method based on gravity gradient information ranging
CN110908393A (en) * 2019-10-31 2020-03-24 中国矿业大学 Underwater unmanned vehicle formation cooperation method based on detection and communication integration
CN110954107B (en) * 2019-12-04 2023-09-19 上海迈波科技有限公司 Unmanned aircraft cluster collaborative navigation method and device
CN111256727B (en) * 2020-02-19 2022-03-08 广州蓝胖子机器人有限公司 Method for improving accuracy of odometer based on Augmented EKF
CN111595348B (en) * 2020-06-23 2022-12-23 南京信息工程大学 Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system
CN113176533B (en) * 2021-04-28 2023-11-24 中国电子科技集团公司第三十六研究所 Direction finding method and device for underwater acoustic communication signals and electronic equipment
CN113324547A (en) * 2021-05-25 2021-08-31 江苏科技大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on iterative extended RTS (smooth Transmission) smoothing filter algorithm
CN113945892B (en) * 2021-10-11 2022-05-03 哈尔滨工程大学 Method for measuring three-dimensional motion trail of body target

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB1368590A (en) * 1972-09-05 1974-10-02 Oki Electric Ind Co Ltd Method and apparatus for computing and displaying sound waves of a sonar system
US5659520A (en) * 1995-04-24 1997-08-19 Sonatech, Inc. Super short baseline navigation using phase-delay processing of spread-spectrum-coded reply signals
CN102901514A (en) * 2012-09-25 2013-01-30 北京航空航天大学 Collaborative initial alignment method based on multiple-inertia-unit informational constraint
CN103292813A (en) * 2013-05-24 2013-09-11 哈尔滨工程大学 Information filtering method for improving formation and navigation accuracy of water surface boat
CN103398712A (en) * 2013-08-02 2013-11-20 中国人民解放军63983部队 Alternatively piloted collaborative navigation method
WO2014204820A1 (en) * 2013-06-17 2014-12-24 Westerngeco Llc Seismic data processing
CN104457754A (en) * 2014-12-19 2015-03-25 东南大学 SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method
CN104569917A (en) * 2014-12-26 2015-04-29 中国船舶重工集团公司第七一五研究所 Self-adaption positioning and navigation system and method for AUV (automatic underwater vehicle) platform
CN105319534A (en) * 2015-11-09 2016-02-10 哈尔滨工程大学 Multiple AUV cooperative positioning method based on underwater sound double pass range finding
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
CN106569179A (en) * 2016-11-08 2017-04-19 东南大学 Genetic particle filtering algorithm-based underwater target tracking and positioning method
CN106767793A (en) * 2017-01-19 2017-05-31 东南大学 A kind of AUV underwater navigation localization methods based on SINS/USBL tight integrations

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB1368590A (en) * 1972-09-05 1974-10-02 Oki Electric Ind Co Ltd Method and apparatus for computing and displaying sound waves of a sonar system
US5659520A (en) * 1995-04-24 1997-08-19 Sonatech, Inc. Super short baseline navigation using phase-delay processing of spread-spectrum-coded reply signals
CN102901514A (en) * 2012-09-25 2013-01-30 北京航空航天大学 Collaborative initial alignment method based on multiple-inertia-unit informational constraint
CN103292813A (en) * 2013-05-24 2013-09-11 哈尔滨工程大学 Information filtering method for improving formation and navigation accuracy of water surface boat
WO2014204820A1 (en) * 2013-06-17 2014-12-24 Westerngeco Llc Seismic data processing
CN103398712A (en) * 2013-08-02 2013-11-20 中国人民解放军63983部队 Alternatively piloted collaborative navigation method
CN104457754A (en) * 2014-12-19 2015-03-25 东南大学 SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method
CN104569917A (en) * 2014-12-26 2015-04-29 中国船舶重工集团公司第七一五研究所 Self-adaption positioning and navigation system and method for AUV (automatic underwater vehicle) platform
CN105319534A (en) * 2015-11-09 2016-02-10 哈尔滨工程大学 Multiple AUV cooperative positioning method based on underwater sound double pass range finding
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
CN106569179A (en) * 2016-11-08 2017-04-19 东南大学 Genetic particle filtering algorithm-based underwater target tracking and positioning method
CN106767793A (en) * 2017-01-19 2017-05-31 东南大学 A kind of AUV underwater navigation localization methods based on SINS/USBL tight integrations

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
"An Improved Cooperative Localization Method for Multiple Autonomous Underwater Vehicles Based on Acoustic Round-Trip Ranging";Gao, Wei等;《2014 IEEE/ION Position, Location and Navigation Symposium, PLANS 2014》;20141231;1420-1423 *
"Bayesian linearized two-hydrophone localization of a pulsed acoustic source";Emmanuel Skarsoulis等;《The Journal of the Acoustical Society of America》;20151231;第137卷(第4期);2241 *
"Moving Horizon Estimation for Cooperation Localization of Multiple Unmanned Underwater Vehicles with Communication Delay";Gao, Wei等;《2014 IEEE/ION POSITION, LOCATION AND NAVIGATION SYMPOSIUM - PLANS 2014》;20141231;全文 *
"多 AUV 协同导航问题的研究现状与进展";徐博，等;《自动化学报》;20150331;第41卷(第3期);445-461 *
"近海水下目标多水听器阵列协同定位";聂星阳，等;《指挥信息系统与技术》;20160831;第7卷(第4期);82-86 *

Also Published As

Publication number Publication date
CN109596128A (en) 2019-04-09

Similar Documents

Publication Publication Date Title
CN109596128B (en) Method for improving multi-AUV (autonomous Underwater vehicle) cooperative positioning performance based on multi-hydrophone
CN111595348B (en) Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system
CN103697910B (en) The correction method of autonomous underwater aircraft Doppler log installation error
CN105823480A (en) Underwater moving target positioning algorithm based on single beacon
CN109884730B (en) Virtual array passive detection method based on deep-sea underwater glider platform
CN107966145B (en) AUV underwater navigation method based on sparse long baseline tight combination
CN111829512A (en) AUV navigation positioning method and system based on multi-sensor data fusion
CN103398712B (en) The collaborative navigation method alternately navigated
CN108444476B (en) Polar region collaborative navigation method of multi-underwater unmanned vehicle considering underwater acoustic communication delay
Kimball et al. Sonar-based iceberg-relative navigation for autonomous underwater vehicles
Matsuda et al. Accurate and efficient seafloor observations with multiple autonomous underwater vehicles: Theory and experiments in a hydrothermal vent field
CN103292813B (en) A kind of information filter method improving water surface ship formation navigation accuracy
CN112015086B (en) Feedback control method for limited-time path tracking output of under-actuated surface ship
CN112747748A (en) Pilot AUV navigation data post-processing method based on reverse solution
Xu et al. Accurate two-step filtering for AUV navigation in large deep-sea environment
CN111735455A (en) Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method
CN103090861A (en) Multi-thread terrain matching and navigating method for underwater robot
CN109387198A (en) A kind of inertia based on sequential detection/visual odometry Combinated navigation method
Matsuda Low-cost high-performance seafloor surveying by multiple autonomous underwater vehicles
CN108759864A (en) Underwater Strapdown Inertial Navigation System Initial Alignment Method based on single transponder
Song et al. FACON: A flow-aided cooperative navigation scheme
Zhang et al. Ocean current-aided localization and navigation for underwater gliders with information matching algorithm
CN113984061B (en) UUV multi-sea-area integrated navigation system design method based on factor graph optimization
Harris et al. Cooperative acoustic navigation of underwater vehicles without a DVL utilizing a dynamic process model: Theory and field evaluation
CN105526931A (en) Combined navigation method for underwater vehicle based on individual seabed transponder

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