CN113048983B - Improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement - Google Patents
Improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement Download PDFInfo
- Publication number
- CN113048983B CN113048983B CN202110332573.4A CN202110332573A CN113048983B CN 113048983 B CN113048983 B CN 113048983B CN 202110332573 A CN202110332573 A CN 202110332573A CN 113048983 B CN113048983 B CN 113048983B
- Authority
- CN
- China
- Prior art keywords
- auv
- precision
- navigation
- information
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 95
- 230000002159 abnormal effect Effects 0.000 title claims abstract description 27
- 238000012883 sequential measurement Methods 0.000 title claims abstract description 25
- 230000004927 fusion Effects 0.000 claims abstract description 46
- 230000008569 process Effects 0.000 claims abstract description 34
- 238000001914 filtration Methods 0.000 claims abstract description 31
- 230000015572 biosynthetic process Effects 0.000 claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 230000005540 biological transmission Effects 0.000 claims abstract description 12
- 230000005236 sound signal Effects 0.000 claims abstract description 5
- 238000005259 measurement Methods 0.000 claims description 25
- 230000004044 response Effects 0.000 claims description 17
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 13
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000003384 imaging method Methods 0.000 claims description 5
- 230000003993 interaction Effects 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000026676 system process Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 claims description 3
- 238000012821 model calculation Methods 0.000 claims description 2
- 238000012545 processing Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003047 cage effect Effects 0.000 description 1
- 238000010888 cage effect Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 239000000178 monomer Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000013535 sea water Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
The invention discloses an improved hierarchical AUV (Autonomous Underwater Vehicle) collaborative navigation positioning method for abnormal time sequential measurement, which is characterized in that the abnormal time sequential measurement information is processed by an improved abnormal time sequential filtering method based on a mobile long baseline method through information sharing and transmission of a hierarchical structure, so that the problem of clock dyssynchrony and the problem of positioning error generated by parallel filtering due to underwater sound signal delay are solved; the optimized extended Kalman filtering method EKF is adopted to fuse internal and external navigation information, an improved multi-sensor data fusion navigation system is used for carrying out fusion calculation on gesture, speed and heading information obtained in the navigation process, the gesture, speed and heading information are fed back to upper computer software, online fusion of sensor data is achieved, and then related state estimation update is carried out on the AUV of a low-precision layer, and sharing and transmission of AUV information with different precision are achieved through hierarchical formation configuration.
Description
Technical Field
The invention relates to a navigation positioning method of an underwater vehicle, in particular to an improved hierarchical AUV (Autonomous Underwater Vehicle) collaborative navigation positioning method for abnormal time sequential measurement.
Background
AUV is the hot research direction in the current ocean engineering field, and through reasonable formation configuration structure, high-precision collaborative navigation among AUVs is the basis of a multi-AUV underwater navigation system. And information is shared through a plurality of monomers, so that all AUVs have positioning capability, and the robustness of the whole system is improved. When a certain AUV is influenced by external factors to lose navigation capacity, the navigation capacity of the boats is restored to a certain extent through collaborative navigation. At present, the main structures of collaborative navigation are divided into a parallel navigation structure, a master-slave collaborative navigation structure and layered navigation. The parallel navigation has a single structure, and relative position observation is performed by utilizing adjacent AUVs, but since all AUVs are required to be assembled with high-precision equipment, the cost is high, and the parallel navigation is difficult to use in actual operation at present.
In the master-slave collaborative navigation, a master AUV carries a high-precision navigation system, a slave AUV carries a low-precision navigation system, communication connection between the master AUV and the slave AUV is established by utilizing an underwater acoustic communication device, and the master AUV sends signals to the slave AUV to share information.
The hierarchical navigation adopts a hierarchical structure, the AUV is well-defined in hierarchy, the AUV is layered according to the navigation precision, the navigation precision between the same layers is equivalent, and the AUV at the high-precision layer transmits navigation information to the AUV at the low-precision layer. In this process, since the distances between the AUVs of each high-precision layer and the AUVs of the low-precision layer participating in constructing the long-baseline navigation positioning system are not equal at different positions, the time interval between the later receiving the response signal after sending the query signal by using the interrogator is different to a different extent, which leads to the problem of clock asynchronism. During the period, the navigation position of the AUV to be positioned is always changed, namely, the AUV to be positioned, which is constantly subjected to gesture change, continuously receives response signals from the AUVs distributed in the high-low precision layers after sending out the query signals, so that inapplicability of parallel filtering in the process is caused, error accumulation is caused, and long-time navigation of the AUV is not facilitated.
Therefore, how to effectively utilize the position measurement information provided by the AUV of the high-precision layer under the underwater acoustic communication condition by using the specific method, so as to improve the navigation precision of the whole navigation system is a technical problem to be solved.
Disclosure of Invention
The invention aims to: aiming at the technical problems, the invention provides an improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement, which solves the problem of asynchronous clocks by the improved abnormal time sequential measurement method, adopts an extended Kalman filtering method to fuse internal and external navigation information, adopts a multi-sensor data fusion navigation system to realize the on-line fusion of data of each sensor, and realizes the sharing and transmission of information among AUVs with different accuracies.
The technical scheme is as follows: the invention discloses an improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement, which comprises the following steps:
step (1), AUV collaborative navigation is carried out by optimizing a layered collaborative navigation formation structure, and AUV of a low-precision layer is carried out q AUV (autonomous Underwater vehicle) of high-precision layer is calculated through underwater sound signal measurement p The relative distance and azimuth between them; at the same time, low-precision layer AUV q Receiving high precision layer AUV p The feedback self-position information;
step (2), dead reckoning the positioned AUV by using a mobile long baseline positioning method to obtain a measurement model of the positioned AUV;
step (3), a different time sequential measurement method is adopted to obtain pitch angle, roll angle and acceleration information of the positioned AUV, so that the problem of positioning error generated in a moving long baseline system by parallel filtering is solved;
step (4), updating the system state through a sequential extended Kalman filtering method;
step (5), performing multi-sensor data fusion calculation through a multi-sensor data fusion navigation system so as to navigate;
and (6) denoising the filtering by adopting a multi-sensor data fusion system.
In step (2), the measurement model calculation process of the located AUV is as follows:
the AUV to be positioned calculates the self pose, and the calculation equation is as follows:
x k+1 ＝x k +x k+1 ＝x k +T.V k .cos(φ k )
y k+1 ＝y k +y k+1 ＝y k +T.V k .sin(φ k )
φ k+1 ＝φ k (1)
wherein T is the sampling period of the positioned AUV, V k For the velocity obtained by the body sensor at time k,the navigation deflection angle obtained by the body sensor at the moment k;
order theBut->And are independent of each other;
thus, the measurement model of the positioned AUV is:
wherein,is the sound wave transmission distance X between two AUVs at time k+1 k For the displacement pose of the AUV located at time k, Γ (u) k +w k T) is a nonlinear term, w k For process noise->In the step (4), the low-precision layer AUV receives the position information of the high-precision layer AUV, and the process of fusing the internal information and the external information to complete positioning by an extended Kalman filtering method is as follows:
set AUV p The coordinates of (2) are: (x) p (k)，y p (k)，z p (k))，AUV q The coordinates of (2) are: (x) q (k)，y q (k)，z q (k) By AUV) p With AUV q The geometrical relationship between them is expressed as follows:
AUV p with AUV q Relative azimuth angle between:
AUV p with AUV q Distance in horizontal direction between:
the method for updating the state of the system by adopting the extended Kalman filtering method comprises the following steps:
measuring the update state of the positioned AUV by using a sensor to obtain the position of the positioned AUVThe state estimator at time is +.>Wherein (1)>A state estimator; positioned AUV 4 The moment of receiving the response signal is +.>Then by predicting covariance sum->The measurement of the time sensor updates the system state;
the prediction covariance of the time system is: />Wherein (1)>Is a state transition matrix:
the formula of the process noise distribution matrix is as follows:
in the step (3), the step of solving the low-precision AUV positioning error by adopting the abnormal time sequential measurement method is as follows:
(31) Under water, at t according to a predetermined time period 0 At the moment, the positioned low-precision AUV sends an inquiry signal, each high-precision AUV provided with a transponder receives the inquiry signal and responds with different frequencies, after the positioned low-precision AUV receives the response signal, the relative position relation between the positioned low-precision AUV and the high-precision AUV provided with the transponder is determined according to the traditional long base line, and the process is that at t 1 Finishing before the moment;
(32)t 0 to t 2 At the moment, the high-precision AUV provided with the transponder sends underwater sound information to the positioned low-precision AUV, wherein the underwater sound information comprises the position state of each high-precision AUV at the moment of responding to the query signal;
(33) The positioned low-precision AUV updates the position information of the AUV according to the calculated relative position geometric relation with the transponder and the received high-precision AUV position information and the moving long baseline method. Wherein in step (32), the location state includes time, longitude, latitude, depth, heading, and pose.
In the step (5), the step of carrying out fusion calculation on the heading attitude information through the multi-sensor data fusion navigation system is as follows:
(51) Carrying GPS on AUV to perform GPS positioning on water, transmitting precise geographic coordinate position, and performing preliminary positioning;
(52) Collecting magnetic signals through an underwater magnetic auxiliary system and an inertial navigation unit, and collecting AUV attitude angle, heading angle, speed and position information under water;
(53) The underwater camera collects and processes visual signals, performs close-range underwater imaging, and provides environment pre-judgment for obstacle avoidance and close-range navigation actions of the AUV;
(54) The multi-sensor data fusion navigation system fuses the information of each sensor, fuses the inertial navigation system and the underwater camera on line, combines GPS information, analyzes and calculates, and a fusion plate in the multi-sensor data fusion navigation system processes image data on line;
(55) And the collected navigation information is fused and resolved through the upper computer software, and the resolved data is transmitted to the AUV of the high-precision layer for information interaction, so that the navigation information resolving work in the cooperative process is completed. Working principle: the invention processes the information of the abnormal time measurement by the improved abnormal time sequential filtering method through the information sharing and transmission of the layered structure and based on the moving long baseline method, thereby solving the problem of asynchronous clock and the problem of positioning error caused by the delay of the underwater sound signal by parallel filtering; the optimized extended Kalman filtering method EKF is adopted to fuse internal and external navigation information, an improved multi-sensor data fusion navigation system is used for carrying out fusion calculation on gesture, speed and heading information obtained in the navigation process, the gesture, speed and heading information are fed back to upper computer software, online fusion of sensor data is achieved, further relevant state estimation update is carried out on the AUV of a low-precision layer, and information sharing and transmission among AUVs of different precision are achieved through hierarchical formation configuration.
The concept of the invention adopting the abnormal time sequential measurement method is as follows: because the distance between each high-precision layer AUV and the AUV of the low-precision layer participating in constructing the long-baseline navigation positioning system is different, the time interval of receiving the response signal after the positioned low-precision layer AUV sends the query signal by using the interrogator is different to a different extent. At the same time, the navigation position of the AUV to be positioned is changed all the time, namely, the AUV with the changed pose continuously receives response signals from the AUVs at different positions after sending out the query signals. The parallel filtering method ignores the fact that the parallel filtering assumes that the AUV is located while receiving the response signal, which is contrary to the actual situation. The invention adopts a sequential filtering method to process the abnormal time measurement information, thereby overcoming the defect.
The beneficial effects are that: compared with the prior art, the invention has the following advantages:
(1) The invention improves the existing collaborative navigation method based on a long baseline, does not need high-difficulty calculation, and the high-precision layer and the low-precision layer in layered formation are distributed orderly, thereby realizing the sharing and the fusion of navigation information between layers with the same precision, sharing and the transmission of information between AUVs with different precision, and meeting the requirement of collaborative navigation.
(2) The invention uses a sequential filtering method to replace a parallel filtering method to process the abnormal time measurement information, and solves the error problem generated in the moving long baseline system due to the asynchronous clock because the clock synchronism assumption is not established between each large AUV constructing the long baseline system and the positioned AUV.
(3) According to the invention, the posture, speed and heading information obtained in the navigation process is fused and solved through the improved multi-sensor data fusion navigation system, so that the navigation precision and the robustness of the system are improved, the interaction and fusion between AUV information are realized, and accurate navigation information is provided for AUV performing collaborative navigation.
Drawings
FIG. 1 is a flow chart of an improved hierarchical AUV collaborative navigation positioning method for out-of-time sequential measurement in accordance with the present invention;
FIG. 2 is a diagram of a hierarchical collaborative navigation formation configuration in accordance with an improvement of the present invention;
FIG. 3 is a schematic diagram of an improved differential timing measurement according to the present invention;
FIG. 4 is a schematic diagram of the system update using the extended Kalman filtering method of the present invention;
fig. 5 is a schematic diagram of the differential timing measurement of the present invention to solve the positioning error of the low-precision AUV.
Detailed Description
As shown in fig. 1, the improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement of the present invention includes the following steps:
(1) Providing a layered collaborative navigation formation structure, performing AUV collaborative navigation, and providing a computing platform for the abnormal time sequential filtering method; in the navigation process, optimizing the formation of the collaborative navigation formation by optimizing the structure of the layered collaborative navigation formation, and particularly shown in fig. 2;
(2) The original Long Baseline (LBL) method is improved by a moving Long Baseline (Moving Long Baseline, LBL) method, dead reckoning is carried out by the moving Long Baseline method, and a measurement model of the positioned AUV is calculated;
(3) The problem of positioning error generated by the delay of underwater acoustic signals in a moving long baseline system by adopting a different-time sequential measurement method is solved;
(4) Updating the system state by a sequential extended Kalman filtering method;
(5) The upper computer software of the operating device performs fusion calculation on the navigation-acquired course gesture information through the improved multi-sensor data fusion navigation system so as to navigate;
(6) The filtering is subjected to denoising processing by adopting a multi-sensor data fusion system, the navigation system fuses information of each sensor, and inertial navigation and underwater imaging are subjected to online matching; and the GPS information is combined for analysis and calculation, and the fusion board processes the image data on line, so that the effect of real-time processing is realized.
The specific implementation process of the technical scheme of the invention is as follows:
as shown in fig. 1, in the improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement of the present invention, first, AUV collaborative navigation is performed, and in the navigation process, collaborative positioning operation between AUVs is performed by optimizing the structure of hierarchical collaborative navigation formation, so as to achieve information fusion between layers with the same precision and information transfer between high precision and low precision.
In the multi-AUV system, the performance of the navigation device of each AUV is different, so in this embodiment, the AUVs with the same navigation precision in the AUV group are classified into one layer, and are layered according to the navigation precision. The AUV with high navigation precision is at a high layer, the AUV with low navigation precision is at a low layer, the AUVs at the same layer share navigation information, and the AUV at the high precision layer transmits the navigation information to the AUV at the low precision layer.
As shown in FIG. 2, the invention marks the AUV with high precision layer as AUV when arranging the improved hierarchical collaborative navigation formation p (p=1, 2), the AUV of the low-precision layer is denoted as AUV q (q=3, 4,5, 6), i.e. AUV 1 And AUV (autonomous Underwater vehicle) 2 In the same layer, is a high-precision layer, AUV 3 、AUV 4 And AUV (autonomous Underwater vehicle) 5 At the same layer, a low-precision layer. Thus, the skew L (k) between adjacent AUVs is calculated by the following formula:
L(k)＝cΔt (3)
where c is the propagation velocity of the acoustic signal in the seawater and Δt is the propagation time of the pulse between the two AUVs.
After the high and low precision is divided, AUV of the low precision layer q AUV (autonomous Underwater vehicle) of high-precision layer is calculated through underwater sound signal measurement p The relative distance and azimuth angle between the two; at the same time, low-precision layer AUV q Receiving high precision layer AUV p The feedback self position information is fused with internal and external information by an extended Kalman filtering method to complete the AUV of the low-precision layer q The specific process is as follows:
setting any AUV in high-precision layer p The coordinates of (2) are: (x) p (k)，y p (k)，z p (k) And x), wherein p (k) Is the displacement of AUV in high precision layer on x-axis, y p (k) Is the displacement of AUV in high-precision layer on y-axis, z p (k) Is the displacement of the AUV in the z-axis in the high precision layer.
Setting any AUV in low-precision layer q The coordinates of (2) are: (x) q (k)，y q (k)，z q (k) And x), wherein q (k) Is the displacement of the AUV in the low precision layer in the x-axis, y q (k) Is the displacement of the AUV in the low-precision layer on the y-axis, z q (k) Is the displacement of the AUV in the low precision layer in the z-axis.
By AUV p With AUV q The geometric relationship between them yields the following expression:
AUV p with AUV q Relative azimuth angle between:
AUV p with AUV q Distance in horizontal direction between:
next, optimizing the formation of the hierarchical collaborative navigation formation, namely finding the optimal formation when performing AUV collaborative formation configuration, so that the AUV of the high-precision layer p And a low precision layer AUV q The AUV error ellipse area is minimized to perform an optimization process on the layered structure.
To quantify the uncertainty, the uncertainty of the position of any AUV in the high and low precision layers is measured by conventionally selecting the area of the error ellipse. The area of the error ellipse is:
α 2 refers to a measure of the probability that the target falls within the error ellipse, P k+1/k Representing the posterior covariance matrix. Because the posterior covariance matrix P is generally used in analyzing the positioning accuracy based on extended Kalman filtering k+1/k To measure.
Wherein the smaller the area of the error ellipse, the smaller the uncertainty of the position, the smaller the error.
In this embodiment, in the AUV of the low-precision layer, the AUV 5 The self navigation precision is higher than AUV 4 Self navigation accuracy, therefore AUV 4 Is selected to locate and locate the AUV with a mobile long baseline location method 4 Dead reckoning, i.e. reckoning the AUV being located 4 Is a measurement model of (a); then obtaining the located AUV by using the differential time sequential measurement method 4 Pitch angle, roll angle, and acceleration information; using sequential extended kalmanThe filtering method updates the system state, and finally carries out multi-sensor data fusion and calculation through a multi-sensor data fusion navigation system to navigate, and adopts the multi-sensor data fusion system to carry out denoising treatment on filtering.
Wherein the AUV is located 4 The measurement model of (2) is as follows:
AUV 4 the dead reckoning device carried by the dead reckoning device is utilized to estimate the dead reckoning device, and the reckoning equation is expressed as follows:
x k+1 ＝x k +x k+1 ＝x k +T.V k .cos(φ k )
y k+1 ＝y k +y k+1 ＝y k +T.V k .sin(φ k )
φ k+1 ＝φ k ( 1 )
wherein T is AUV 4 V is the sampling period of (1) k For the velocity obtained by the body sensor at time k,and (3) obtaining the deflection angle of the body sensor at the moment k.
Order theBut->And are independent of each other.
Thereby, the AUV being located 4 The measurement model of (2) is:
wherein,is the sound wave transmission distance X between two AUVs at time k+1 k AUV at time k 4 Is a displacement gesture of Γ (u) k +w k T) is a nonlinear term, w k For process noise->
For moving a long baseline, because the distance between each AUV of a high-precision layer and the AUV of a low-precision layer participating in constructing a long baseline navigation positioning system is not equal, the AUV needing positioning 4 The navigation position of (c) is changed all the time. Assuming that the transponder-mounted AUV responds immediately after receiving the interrogation signal, the acoustic propagation distance is expressed as:
c is the transmission speed of sound waves in water,distance of propagation of target sound, t k+1 Represent AUV 4 To AUV i Time (i=1, 2, 3) at which the inquiry information is transmitted; t' k+1 Represent AUV 4 Receiving AUV i Time of response information (i=1, 2, 3), AUV i I.e. AUVs distributed in high and low precision layers, as shown in particular in fig. 3.
But during application of the moving long baseline, both the master AUV transponder and the located AUV are in motion. Due to the slow propagation speed of the underwater acoustic signal, a time delay problem occurs. After the located AUV transmits an interrogation signal, it takes a certain time for the interrogation signal to reach the master AUV transponder, and after the master AUV receives the interrogation signal, it transmits a response signal immediately, and the response signal propagates to the located AUV, where the located AUV is not already in the original location. Therefore, an error occurs when the moving long baseline is applied, that is, the acoustic wave transmission distance when k+1 is calculated based on the time delay principle, and the error can be expressed as:
wherein: AUV are respectively provided with 1 、AUV 2 And AUV (autonomous Underwater vehicle) 3 The AUV positions at time k+1 are respectively
Set AUV 4 AUV at time k+1 with only low-precision dead reckoning system 4 Is (x) k+1 ，y k+1 )，
AUV of high-precision layer 1 And transmitting an inquiry signal by taking T as a period, and replying a response signal after the AUV of the low-precision layer carrying the transponder receives the signal. Because the low-precision layers each carry the AUV and AUV of the transponder 1 Is different from the AUV 1 The time at which the signal is received also varies.
Supposing AUV 1 The passing AUV carrying the transponder is AUV in turn 2 ，AUV 3 And AUV (autonomous Underwater vehicle) 4 AUV at this time 1 And sequentially pass through an AUV carrying a transponder 2 、AUV 3 、AUV 4 Distance between them.
The distance equation for this distance is:
wherein,is->Time AUV 1 The location at which the resulting transponder emits the response signal. And sequentially, carrying out the abnormal time measurement processing of one period on each transponder to obtain the initial value of the position of the next period. As shown in fig. 3 and 5, the acoustic transmission distance at k+1 is calculated based on the time delay principleIn step (3), the invention adopts the abnormal time sequential measurement method to solve the problem of low-precision AUV under the background of moving long base line 4 The specific procedure of the positioning error problem is as follows:
(1) Under water, at t according to a predetermined time period 0 Time of day, low precision AUV being positioned 4 Transmitting an interrogation signal, a main AUV with a transponder mounted therein 1 、AUV 2 、AUV 3 Receiving interrogation signals, responding at different frequencies, localized low-accuracy AUV 4 After receiving the response signal, determining the relative position relation between the high-precision layer AUV and the high-precision layer AUV of the installed transponder according to the principle of moving the long baseline, wherein the process is as follows 1 Finishing before the moment;
(2)t 0 to t 2 At time, the AUV with transponder, i.e. AUV 1 、AUV 2 、AUV 3 Transmitting underwater acoustic information in the form of a broadcast by means of an underwater acoustic communication device, the information comprising an AUV, i.e. AUV, of the host transponder 1 、AUV 2 、AUV 3 The location status of each instant in response to the interrogation signal, including time, longitude, latitude, depth, heading, and attitude, the low-accuracy AUV being located 4 This underwater sound information is passively received.
(3) As shown in fig. 5, the positioned low-precision AUV 4 And updating the position information of the AUV according to the calculated relative position geometric relation with the transponder and the received high-precision layer AUV position information and the moving long baseline method. Autonomous navigation is carried out on AUVs at the high-low precision layers during movement, meanwhile, relative position measurement is also carried out between the AUVs at the high-low precision layers, and positioning information of water outlet sound is fed back. The extended Kalman filtering method uses the latest state estimation as prior information, and combines the underwater sound positioning information of the previous high-low precision layer to perform abnormal sequential measurement.
As shown in fig. 4, the abnormal time measurement information of the AUV obtained in the mobile long baseline system is processed by using the abnormal time measurement method, and the system state is updated, which comprises the following specific steps: the AUV with the same precision moves in the high-precision layer and the low-precision layer respectively, and navigation is performed through the relative position measurement of the AUV between the high-precision layer and the low-precision layer; then, denoising the obtained underwater sound positioning information by using an EKF filter; and finally, taking the latest state estimation as prior information to perform abnormal time sequential measurement.
In the step (4), the step of updating the state of the system by adopting the sequential extended kalman filter method EKF is as follows:
measuring AUV where it is located using sensor 4 To obtain the located AUV 4 At the position ofThe state estimator at time is +.>Wherein (1)>For state estimation, the AUV is located 4 The moment of receiving the response signal is +.>Then by the prediction covariance and +.>The time sensor measures and updates the system state. The multi-AUV system comprises an AUV cluster consisting of a high-precision layer and a low-precision layer.
The prediction covariance of the time system is: />Wherein (1)>Is a state transition matrix:
the formula of the process noise distribution matrix is as follows:
and then, the upper computer software of the operating device performs fusion calculation on the course gesture information obtained by navigation through the improved multi-sensor data fusion navigation system. Specifically, the improved multi-sensor data fusion navigation system is used for carrying out fusion calculation on navigation and positioning information such as speed, gesture, course and the like obtained in the collaborative navigation process.
The improved multi-sensor data fusion navigation system comprises a GPS, an underwater magnetic auxiliary system, an inertial navigation unit and an underwater camera. The improved multi-sensor data fusion navigation system performs fusion and calculation on course gesture information obtained by navigation, and comprises the following steps:
(51) The AUV carries GPS to carry out GPS positioning on water, sends accurate geographic coordinate positions and carries out preliminary positioning.
(52) The GPS signal is influenced by the Faraday cage effect under the water, namely, the GPS signal receiver is subjected to electrostatic shielding under the water, so that the GPS signal cannot be received, and the GPS is not available, and only other sensor signals can be adopted, so that the underwater magnetic auxiliary system and the inertial navigation unit collect magnetic signals to collect AUV attitude angle, heading angle, speed and position information under the water.
(53) Because the underwater environment is complex and changeable, during the period, the underwater camera collects and processes visual signals, performs close-range underwater imaging, and provides accurate environment prejudgement for obstacle avoidance and close-range navigation actions of the AUV.
(54) The multi-sensor data fusion navigation system fuses the sensor information, fuses the inertial navigation system and the underwater camera on line, combines the GPS information, analyzes and calculates, and a fusion plate in the multi-sensor data fusion navigation system processes the image data on line, so that the effect of real-time processing is realized.
(55) And finally, the collected navigation information is fused and resolved through the upper computer software, and the resolved data is transmitted to the AUV of the high-precision layer to realize information interaction, so that the resolving work of the navigation information in the whole cooperative process is completed.
In the step (54), the inertial navigation system and the underwater camera are fused on line to perform real-time processing, and the method specifically comprises the following steps:
(a) The multi-sensor data fusion system carries out denoising processing on filtering;
(b) The inertial navigation system fuses the information of each sensor, and the inertial navigation system and the underwater imaging system are fused on line, so that the matching degree of the information among different sensors is improved;
(c) And by combining GPS information, analysis and calculation are carried out, and the integrated board processes the image data on line, so that the effect of real-time processing is realized.
Claims (4)
1. An improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement is characterized in that: the method comprises the following steps:
step (1), AUV collaborative navigation is carried out by optimizing a layered collaborative navigation formation structure, and AUV of a low-precision layer is carried out q AUV (autonomous Underwater vehicle) of high-precision layer is calculated through underwater sound signal measurement p The relative distance and azimuth between them; at the same time, low-precision layer AUV q Receiving high precision layer AUV p The feedback self-position information;
step (2), dead reckoning the positioned AUV by using a mobile long baseline positioning method to obtain a measurement model of the positioned AUV;
the measurement model calculation process of the positioned AUV is as follows:
the AUV to be positioned calculates the self pose, and the calculation equation is as follows:
x k+1 ＝x k +x k+1 ＝x k +T.V k .cos(φ k )
y k+1 ＝y k +y k+1 ＝y k +T.V k .sin(φ k )
φ k+1 ＝φ k (1)
wherein T is the sampling period of the positioned AUV, V k For the velocity obtained by the body sensor at time k,the navigation deflection angle obtained by the body sensor at the moment k;
order theBut->And are independent of each other;
thus, the measurement model of the positioned AUV is:
wherein,is the sound wave transmission distance X between two AUVs at time k+1 k For the displacement pose of the AUV located at time k, Γ (u) k +w k T) is a nonlinear term, w k For process noise->
Step (3), a different time sequential measurement method is adopted to obtain pitch angle, roll angle and acceleration information of the positioned AUV, so that the problem of positioning error generated in a moving long baseline system by parallel filtering is solved;
step (4), updating the system state through a sequential extended Kalman filtering method; the method comprises the following steps:
measuring the update state of the positioned AUV by using a sensor to obtain the position of the positioned AUVThe state estimator at time is +.>Wherein (1)>A state estimator; the time when the positioned AUV receives the response signal is +.>Then by predicting covariance sum->The measurement of the time sensor updates the system state;
the prediction covariance of the time system is: />
Wherein,is a state transition matrix:
the formula of the process noise distribution matrix is as follows:
step (5), performing multi-sensor data fusion calculation through a multi-sensor data fusion navigation system so as to navigate;
and (6) denoising the filtering by adopting a multi-sensor data fusion system.
2. The improved hierarchical AUV co-navigation positioning method of out-of-time sequential measurement of claim 1, wherein: in the step (3), the step of solving the low-precision AUV positioning error by adopting the abnormal time sequential measurement method is as follows:
(31) Under water, at t according to a predetermined time period 0 At the moment, the positioned low-precision AUV sends an inquiry signal, each high-precision AUV provided with a transponder receives the inquiry signal and responds with different frequencies, after the positioned low-precision AUV receives the response signal, the relative position relation between the positioned low-precision AUV and the high-precision AUV provided with the transponder is determined according to the traditional long base line, and the process is that at t 1 Finishing before the moment;
(32)t 0 to t 2 At the moment, the high-precision AUV provided with the transponder sends underwater sound information to the positioned low-precision AUV, wherein the underwater sound information comprises the position state of each high-precision AUV at the moment of responding to the query signal;
(33) The positioned low-precision AUV updates the position information of the AUV according to the calculated relative position geometric relation with the transponder and the received high-precision AUV position information and the moving long baseline method.
3. The improved hierarchical AUV co-navigation positioning method of abnormal time sequential measurement of claim 2, wherein: in step (32), the location state includes time, longitude, latitude, depth, heading, and pose.
4. The improved hierarchical AUV co-navigation positioning method of differential time sequential measurement of any one of claims 1-3, wherein: in the step (5), the step of carrying out fusion calculation on the heading attitude information through the multi-sensor data fusion navigation system is as follows:
(51) Carrying GPS on AUV to perform GPS positioning on water, transmitting precise geographic coordinate position, and performing preliminary positioning;
(52) Collecting magnetic signals through an underwater magnetic auxiliary system and an inertial navigation unit, and collecting AUV attitude angle, heading angle, speed and position information under water;
(53) The underwater camera collects and processes visual signals, performs close-range underwater imaging, and provides environment pre-judgment for obstacle avoidance and close-range navigation actions of the AUV;
(54) The multi-sensor data fusion navigation system fuses the information of each sensor, fuses the inertial navigation system and the underwater camera on line, combines GPS information, analyzes and calculates, and a fusion plate in the multi-sensor data fusion navigation system processes image data on line;
(55) And the collected navigation information is fused and resolved through the upper computer software, and the resolved data is transmitted to the AUV of the high-precision layer for information interaction, so that the navigation information resolving work in the cooperative process is completed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110332573.4A CN113048983B (en) | 2021-03-29 | 2021-03-29 | Improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110332573.4A CN113048983B (en) | 2021-03-29 | 2021-03-29 | Improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113048983A CN113048983A (en) | 2021-06-29 |
| CN113048983B true CN113048983B (en) | 2023-12-26 |
Family
ID=76516337
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110332573.4A Active CN113048983B (en) | 2021-03-29 | 2021-03-29 | Improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113048983B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113485385B (en) * | 2021-07-13 | 2023-11-07 | 中国人民解放军战略支援部队信息工程大学 | UUV cluster formation configuration design method based on error ellipse |
| CN114485613A (en) * | 2021-12-31 | 2022-05-13 | 海南浙江大学研究院 | Multi-information fusion underwater robot positioning method |
| CN116608864B (en) * | 2023-07-19 | 2023-10-27 | 青岛哈尔滨工程大学创新发展中心 | AUV cooperative positioning method based on factor graph under influence of communication time delay |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101907711A (en) * | 2010-07-16 | 2010-12-08 | 浙江大学 | Simultaneous detecting and positioning method for autonomous underwater robot |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN110243377A (en) * | 2019-07-19 | 2019-09-17 | 南京航空航天大学 | A kind of cluster aircraft collaborative navigation method based on hierachical structure |
| CN111595348A (en) * | 2020-06-23 | 2020-08-28 | 南京信息工程大学 | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015134473A2 (en) * | 2014-03-05 | 2015-09-11 | C&C Technologies, Inc. | Underwater inspection system using an autonomous underwater vehicle ("auv") in combination with a laser micro bathymetry unit (triangulation laser) and high-definition camera |
-
2021
- 2021-03-29 CN CN202110332573.4A patent/CN113048983B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101907711A (en) * | 2010-07-16 | 2010-12-08 | 浙江大学 | Simultaneous detecting and positioning method for autonomous underwater robot |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN110243377A (en) * | 2019-07-19 | 2019-09-17 | 南京航空航天大学 | A kind of cluster aircraft collaborative navigation method based on hierachical structure |
| CN111595348A (en) * | 2020-06-23 | 2020-08-28 | 南京信息工程大学 | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system |
Non-Patent Citations (2)
| Title |
|---|
| 双领航多自主水下航行器移动长基线定位最优队形研究;房新鹏，等;兵工学报;第33卷(第8期);1020-1024 * |
| 基于压缩感知的PD雷达序贯扩展卡尔曼滤波跟踪方法;王雪君，等;信号处理;第33卷(第4期);601-606 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113048983A (en) | 2021-06-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113048983B (en) | Improved hierarchical AUV collaborative navigation positioning method for abnormal time sequential measurement | |
| CA3107374C (en) | Systems and methods for autonomous machine tracking and localization of mobile objects | |
| JP5673071B2 (en) | Position estimation apparatus and program | |
| CN101900558B (en) | Combined navigation method of integrated sonar micro navigation autonomous underwater robot | |
| CN111536967B (en) | EKF-based multi-sensor fusion greenhouse inspection robot tracking method | |
| CN109782289B (en) | Underwater vehicle positioning method based on baseline geometric structure constraint | |
| CN101762273A (en) | Autonomous optical navigation method for soft landing for deep space probe | |
| US11237005B2 (en) | Method and arrangement for sourcing of location information, generating and updating maps representing the location | |
| CN111273298B (en) | Underwater acoustic target positioning and tracking method based on wave glider networking technology | |
| CN103293512A (en) | Positioning using a local wave-propagation model | |
| JP2014025925A (en) | Vehicle controller and vehicle system | |
| CN107192983B (en) | Device, method and system for observing relative position of underwater vehicle | |
| CN113311388B (en) | Ultra-short baseline positioning system of underwater robot | |
| Lu et al. | Cooperative localization for multiple AUVs based on the rough estimation of the measurements | |
| CN110389318B (en) | Underwater mobile platform positioning system and method based on three-dimensional six-element array | |
| CN116449374B (en) | Underwater sonar differential positioning method | |
| Cao et al. | WiFi RTT indoor positioning method based on Gaussian process regression for harsh environments | |
| Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
| CN111708008A (en) | Underwater robot single-beacon navigation method based on IMU and TOF | |
| CN109905846B (en) | Underwater wireless sensor network positioning method based on autonomous underwater vehicle | |
| CN113821766B (en) | Error correction method for marine acoustic navigation system | |
| CN108332749B (en) | Indoor dynamic tracking and positioning method | |
| Xin et al. | Stable positioning for mobile targets using distributed fusion correction strategy of heterogeneous data | |
| Li et al. | Position estimation and error correction of mobile robots based on UWB and multisensors | |
| Bi et al. | Positioning and navigation technology |
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 |