CN112924936B - Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation - Google Patents
Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation Download PDFInfo
- Publication number
- CN112924936B CN112924936B CN202110279337.0A CN202110279337A CN112924936B CN 112924936 B CN112924936 B CN 112924936B CN 202110279337 A CN202110279337 A CN 202110279337A CN 112924936 B CN112924936 B CN 112924936B
- Authority
- CN
- China
- Prior art keywords
- window
- information
- boat
- sliding
- observation
- 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
Images
Classifications
-
- 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
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Feedback Control In General (AREA)
Abstract
The invention designs a multi-AUV cooperative positioning method based on sliding observation. The N nearest distance measurement information and the main boat information are converted into the observed quantity of the current moment through a designed self-adaptive sliding window with the width of N, the earliest observed information in the window is removed along with the arrival of new observed quantity, and the kernel width based on the maximum correlation entropy algorithm is determined and used by adopting a median filtering method, so that the influence of abnormal values in the observed information is effectively avoided, the estimation error caused by continuous abnormal values in the observed quantity is reduced, and the positioning performance of the cooperative positioning system is improved.
Description
Technical Field
The invention relates to an Autonomous Underwater Vehicle (AUV) co-location technology, in particular to a multi-AUV co-location method based on sliding observation. When the method designed by the invention is used for AUV (autonomous Underwater vehicle) cooperative positioning, N pieces of nearest ranging information and main ship information are converted into observed quantities of the current moment through a designed self-adaptive sliding window with the width of N, the earliest observed information in the window is removed along with the arrival of new observed quantities, and meanwhile, the median filtering method is adopted to determine the kernel width based on the maximum correlation entropy algorithm, so that the influence of abnormal values in the observed information is effectively avoided, the estimation error caused by continuous abnormal values in the observed quantities is reduced, and the positioning performance of a cooperative positioning system is improved.
Background
The AUV is used as an unmanned intelligent platform capable of autonomous planning, autonomous decision-making and autonomous navigation, has the advantages of high concealment, strong maneuverability, wide range of activity and the like, is widely applied to the front edge of a war area or a dangerous sea area to replace systems to execute special combat missions such as reconnaissance and monitoring, mine sweeping and anti-diving, accurate striking and the like, and can effectively expand the attack and defense radius of a maritime combat cluster. However, the limitations of the single AUV working platform are becoming more and more significant in the process of further developing and utilizing marine resources. In recent years, research for improving the positioning and navigation performance of a single AUV through cooperation among multiple AUVs has attracted attention and is becoming a hot spot of research in the field of AUV navigation. Multiple AUV co-location has the following advantages: 1) the multi-AUV system can fully utilize the performance of each AUV to realize the improvement of the overall performance of the system; 2) the multi-AUV system has high expandability, and the increase and decrease of individual AUV in the system can not cause great influence on the positioning performance of the system; 3) the robustness of the multi-AUV system is strong; 4) and multiple AUV systems work cooperatively, so that tasks which cannot be finished by the single AUV system can be finished.
The multi-AUV cooperative positioning system generally comprises 1-2 main boats carrying high-precision navigation equipment, and the rest are slave boats equipped with lower-precision navigation equipment. The positioning accuracy of the plurality of AUVs can be improved by using the positioning states of other AUVs as long as the AUVs can directly or indirectly observe relative to each other, such as measuring the distances or the orientations between the AUVs and other AUVs.
In an actual operating system, the observed quantity between the AUVs may have continuous abnormal values, and the abnormal values will cause the positioning error of the co-positioning system to increase. Aiming at the problems, the invention designs a multi-AUV cooperative positioning method based on sliding observation. The algorithm converts the nearest N pieces of ranging information and the main boat information into the current time as the observed quantity of the current time through a sliding window with the width of N, and removes the earliest observed information in the window along with the arrival of the new observed quantity. Meanwhile, the median filtering method is adopted to determine the kernel width based on the maximum correlation entropy algorithm, so that the influence of abnormal values is avoided.
Disclosure of Invention
The invention aims to design a multi-AUV cooperative positioning method based on sliding observation, which is characterized in that on the premise of not changing the precision of an inertia device, N nearest distance measurement information and main boat information are converted into the current time as the observed quantity of the current time through a sliding window with the width of N, and the earliest observed information in the window is removed along with the arrival of new observed quantity. Meanwhile, the median filtering method is adopted to determine the kernel width used in the algorithm based on the maximum correlation entropy, so that the influence of an abnormal value is avoided, and the positioning precision of the cooperative positioning system is improved.
The object of the present invention can be achieved by the following steps:
step 1: establishing a sliding observation window;
step 2: designing the kernel width in the maximum correlation entropy-based co-location algorithm by utilizing median filtering;
and step 3: adaptive determination of the sliding window size.
In step 1, the schematic diagram of the established sliding observation window is shown in fig. 1, wherein Z in fig. 1 comprises main boat position information and main and auxiliary boat distance information, namely
When the former observation information is taken as the observation information at the current moment, the position information of the main boat needs to be translated, the movement amount is a position change vector of the slave boat in the period, and the schematic diagram of the translation of the position of the main boat is shown in the attached figure 2.
In a specific implementation, the position information of the master boat in the sliding window can be updated by using the speed of the slave boat at each moment, and is represented as the following formula:
in step 2, in order to better solve the characteristic that the cooperative system mostly has double tail noise in the operating environment, the invention adopts a factor graph cooperative positioning algorithm based on the maximum correlation entropy, and only one free parameter, namely the kernel width, is left in the algorithm. N pieces of observation information are obtained at the time k, and the ranging information in the N groups of data is measured at different times and has errors with different sizes. The ranging information can be used for determining the kernel width in the algorithm, so that the self-adaption of the algorithm is realized, abnormal values in the ranging information are caused by the characteristics of underwater acoustic ranging, the pulse noise can be well filtered by the median filter in a nonlinear calculation mode, and therefore the influence of the abnormal values in the observation information can be effectively avoided by determining the kernel width by using the median filter.
The principle of median filtering can be described as: will be numbered in sequence x1，x2，…，xmPerforming ascending arrangement, wherein the number of the middle positions of the arranged number series is called as a middle number; when m is an even number, the median is the average of the two median numbers, and med (x) is the median1，x2，…xm) To indicate. The median filtering is a nonlinear filtering, and is inferior to the mean filtering in terms of random noise, and superior to the mean filtering in terms of interference of pulse signals. Therefore, the kernel width of the algorithm is found as follows.
When the factor graph co-location algorithm based on the maximum correlation entropy is used for co-location, firstly, the formula in the step 1 is used for updating the information Z in the sliding windowl(1 … N), and then the kernel width σ of the algorithm is found according to the above equationkAnd then, calculating each information in the sliding window by using an algorithm respectively to obtain an estimation result of the slave boat
The N results obtained are then averaged using the following equation to obtain a final position estimate for the slave boat.
In step 3, the size N of the sliding window affects the accuracy of the positioning system and the size of the calculated amount, too large selected N will increase the calculated amount and introduce too much speed and attitude errors into the information of the sliding window, and too small selected N will cause an insignificant effect of suppressing the positioning errors caused by the observed abnormal values.
According to the characteristic of median filtering, if the window size is N, if the number of abnormal values in the window is not less than half of N, the kernel width is calculated by using the abnormal values according to the formula in step 2, which results in too large selection of the kernel width, and the positioning error of the algorithm is increased, so that the proportion of the abnormal values in the window can be used as the judgment basis of the window size. The specific process is as follows:
(1) an outlier is determined. Because the real distance between the main boat and the slave boat in the actual cooperative positioning system can not be known, the judgment can be carried out only according to the observed value and the position information of the main boat and the slave boat, and when new observed information arrives, the following formula is used for discrimination:
in the formula1-a flag, ol, indicating whether the newly received ranging information is an outlier 11 indicates that the current information is an abnormal value, ol 10 means no outlier;
max (·) — represents the maximum value.
(2) The window is enlarged. If the number of outliers in the window is too high, the window is considered too small, and the outliers will increase the positioning error of the algorithm, requiring the window to be enlarged, as shown in the following equation.
(3) And (5) shrinking the window. If the number of outliers in the window is too low, the window is considered too large, which may result in an increase in the number of calculations and an introduction of too much process noise in the observed volume, and the window needs to be reduced, as shown in the following equation.
Z＝[Z1…ZN]→Z＝[Z1…ZN-1]
N＝N-1
The proportion of the abnormal value in the window can be set according to the actual situation, and the proportion of the abnormal value set in the simulation of the invention is between 12.5% and 25%.
Drawings
Fig. 1 is a schematic diagram of the established sliding observation window.
Fig. 2 is a schematic view of the translation of the main boat position in the sliding window.
Fig. 3 shows the motion trajectory of the master boat and the slave boat in the simulation experiment.
Fig. 4 shows the range error of the simulation experiment.
Fig. 5 shows the positioning error of the simulation experiment.
Detailed Description
The present invention will be described in detail with reference to specific embodiments.
The invention provides a multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation, which is characterized in that N nearest ranging information and main boat information are converted into observed quantity of the current time through a designed self-adaptive sliding window with the width of N, the earliest observed information in the window is removed along with the arrival of new observed quantity, and meanwhile, a median filtering method is adopted to determine the kernel width based on the maximum correlation entropy algorithm, so that the influence of abnormal values in the observed information is effectively avoided, the estimation error caused by continuous abnormal values in the observed quantity is reduced, and the positioning performance of a cooperative positioning system is improved. The purpose of the invention is realized by the following steps:
1. establishing a sliding observation window;
2. designing the kernel width in the maximum correlation entropy-based co-location algorithm by utilizing median filtering;
3. adaptive determination of the sliding window size.
In order to verify the effectiveness of the invention, the factor graph co-location algorithm based on the maximum correlation entropy is simulated by using software.
Fig. 3 is a motion trajectory of a master boat and a slave boat in simulation, and simulation conditions are as follows: the two main boats have a course of 30 deg., initial positions (-450m,450m) and (450m ), respectively, and a speed of 2 m/s. The initial position of the slave boat is (0m,0m), the heading is 30 DEG, and the speed is 2 m/s. In the simulation, the velocity noise σ from the boatv＝(0.5m/s)2Acceleration noise σa＝(0.01m/s2)2Spinning topInstrument for measuring noise
Both are uncorrelated additive noise, and the ranging noise model between master and slave boats is shown in fig. 4. And updating the estimation of the position information by using a filtering algorithm, wherein the period of each filtering estimation is 1 s. FIG. 5 is a positioning error diagram of a simulation experiment, in which EKF algorithm represents extended Kalman filtering algorithm, FGMC algorithm represents maximum correlation entropy based factor graph co-positioning algorithm without sliding window, and MOFGMC algorithm represents maximum correlation entropy based factor graph co-positioning algorithm with adaptive sliding window. As can be seen from fig. 4 and 5, in the operation process, when the ranging information of the system contains an abnormal value, the overall positioning error of the multi-AUV cooperative positioning method based on sliding observation provided by the present invention is small, and the influence of the continuously occurring abnormal value of the observed quantity on the cooperative positioning accuracy of the system can be effectively suppressed.
The effectiveness of the sliding observation-based multi-AUV cooperative positioning method is verified through the experiment, the nearest N pieces of ranging information and main boat information can be converted into the current time as the observed quantity of the current time through a designed self-adaptive sliding window with the width of N on the premise of not improving the measurement precision of inertia components in a cooperative system, and meanwhile, the kernel width based on the maximum correlation entropy algorithm is determined by adopting a median filtering method, so that the influence of abnormal values in the observed information is effectively avoided, the estimation error caused by continuous abnormal values in the observed quantity is reduced, and the positioning performance of the cooperative positioning system is improved.
Claims (2)
1. A multi-AUV cooperative positioning method based on sliding observation is characterized by comprising the following steps:
step 1: establishing a sliding observation window;
step 2: designing the kernel width in the maximum correlation entropy-based co-location algorithm by utilizing median filtering;
and step 3: self-adaptive determination of the size of the sliding window;
in the step 1, a window with the width of N is established in the establishment of the sliding observation window, and the earliest observation information in the window is removed along with the arrival of new observed quantity; when the previous observation information is taken as the observation information of the current moment, the position information of the main boat is translated, and the movement amount is the position change vector of the auxiliary boat in the period;
in specific implementation, the position information of the master boat in the sliding window is updated by the speed of the slave boat at each moment, and is represented as the following formula:
in the kernel width in the maximum correlation entropy based co-location algorithm designed by using median filtering in step 2, the kernel width is calculated as follows:
wherein, med (a)1,a2…,aN) Array a1,a2…,aNPerforming ascending arrangement, selecting a middle number, and when N is an even number, selecting an average value of the middle two values;
then, each piece of information in the sliding window is calculated by an algorithm to obtain an estimation result of the slave boat
Then averaging the obtained N results by using the following formula to obtain a final position estimation result of the slave boat;
2. the sliding observation-based multi-AUV co-location method according to claim 1, wherein in the sliding window size adaptive determination in step 3, an adaptive determination method for sliding window size is designed, and specifically, the method comprises the following steps:
(1) determining an abnormal value, wherein the actual distance between the main boat and the auxiliary boat in the actual cooperative positioning system cannot be known, the judgment can be only carried out according to the observed value and the position information of the main boat and the auxiliary boat, and when new observed information arrives, the discrimination is carried out by using the following formula:
in the formula1-a flag, ol, indicating whether the newly received ranging information is an outlier11 indicates that the current information is an abnormal value, ol10 means no outlier; max (·) -represents the maximum value;
(2) and expanding the window, wherein if the number of the abnormal values in the window is too high, the window is considered to be too small, the abnormal values can increase the positioning error of the algorithm, and the window needs to be expanded, as shown in the following formula:
(3) and (3) reducing the window, and if the number of abnormal values in the window is too low, considering that the window is too large, wherein the too large window can cause the increase of the calculated amount and the introduction of too much process noise in the observed amount, and the window needs to be reduced as shown in the following formula:
Z＝[Z1…ZN]→Z＝[Z1…ZN-1]
N＝N-1。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110279337.0A CN112924936B (en) | 2021-03-16 | 2021-03-16 | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110279337.0A CN112924936B (en) | 2021-03-16 | 2021-03-16 | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112924936A CN112924936A (en) | 2021-06-08 |
| CN112924936B true CN112924936B (en) | 2021-10-26 |
Family
ID=76175168
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110279337.0A Active CN112924936B (en) | 2021-03-16 | 2021-03-16 | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112924936B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106482728A (en) * | 2016-09-14 | 2017-03-08 | 西安交通大学 | Communication support spacecraft relative status method of estimation based on maximum cross-correlation entropy criterion Unscented kalman filtering |
| CN107966676A (en) * | 2017-08-04 | 2018-04-27 | 大连理工大学 | Array antenna angle and information source number combined estimation method under complicated noise |
| CN108489498A (en) * | 2018-06-15 | 2018-09-04 | 哈尔滨工程大学 | A kind of AUV collaborative navigation methods without mark particle filter based on maximum cross-correlation entropy |
| CN109084767A (en) * | 2018-06-15 | 2018-12-25 | 哈尔滨工程大学 | A kind of AUV collaborative navigation method of the adaptive volume particle filter of maximum cross-correlation entropy |
| CN111486845A (en) * | 2020-04-27 | 2020-08-04 | 中国海洋大学 | AUV multi-strategy navigation method based on submarine topography matching |
-
2021
- 2021-03-16 CN CN202110279337.0A patent/CN112924936B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106482728A (en) * | 2016-09-14 | 2017-03-08 | 西安交通大学 | Communication support spacecraft relative status method of estimation based on maximum cross-correlation entropy criterion Unscented kalman filtering |
| CN107966676A (en) * | 2017-08-04 | 2018-04-27 | 大连理工大学 | Array antenna angle and information source number combined estimation method under complicated noise |
| CN108489498A (en) * | 2018-06-15 | 2018-09-04 | 哈尔滨工程大学 | A kind of AUV collaborative navigation methods without mark particle filter based on maximum cross-correlation entropy |
| CN109084767A (en) * | 2018-06-15 | 2018-12-25 | 哈尔滨工程大学 | A kind of AUV collaborative navigation method of the adaptive volume particle filter of maximum cross-correlation entropy |
| CN111486845A (en) * | 2020-04-27 | 2020-08-04 | 中国海洋大学 | AUV multi-strategy navigation method based on submarine topography matching |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112924936A (en) | 2021-06-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sun et al. | Mapless motion planning system for an autonomous underwater vehicle using policy gradient-based deep reinforcement learning | |
| CN109241552B (en) | Underwater robot motion planning method based on multiple constraint targets | |
| CN108427414B (en) | Horizontal plane self-adaptive trajectory tracking control method for autonomous underwater vehicle | |
| CN113178098B (en) | Unmanned ship event triggering layered cooperative control system | |
| CN109975839B (en) | Joint filtering optimization method for vehicle satellite positioning data | |
| CN110956853A (en) | Multi-ship collision prediction method, system and storage medium | |
| CN110378411B (en) | Method for assisting underwater maneuvering target tracking by support vector machine based on interactive multi-model | |
| CN110377034B (en) | Global robust sliding mode control method for track tracking of surface ship based on dragonfly algorithm optimization | |
| CN112710304B (en) | Underwater autonomous vehicle navigation method based on adaptive filtering | |
| CN110726415A (en) | Self-adaptive underwater multi-beam synchronous positioning and mapping method | |
| CN112325885A (en) | Factor graph co-location algorithm based on mathematical statistical characteristics | |
| CN115201799A (en) | Time-varying Kalman filtering tracking method for sonar | |
| CN112924936B (en) | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sliding observation | |
| CN109696908B (en) | Robot and flight path setting method and system thereof | |
| CN110908404B (en) | AUV intelligent observation motion method based on data driving | |
| KR101097169B1 (en) | System and method for estimating location of mobile object | |
| CN116466701A (en) | Unmanned surface vessel energy consumption minimum track planning method | |
| CN116243717A (en) | Underwater robot time optimal path planning method considering ocean current uncertainty | |
| CN113156368B (en) | Error parameter identification co-location method based on factor graph | |
| CN115480580A (en) | NMPC-based underwater robot path tracking and obstacle avoidance control method | |
| CN114721435A (en) | Soft constraint-based defense method for unmanned aerial vehicle cluster and related equipment | |
| Bogomolov | Test Results of the Long Baseline Navigation Solutions under a Large a Priori Position Uncertainty | |
| CN115077530B (en) | Multi-AUV collaborative navigation method and system based on strong tracking dimension-expanding ECKF algorithm | |
| CN116358564B (en) | Unmanned aerial vehicle bee colony centroid motion state tracking method, system, equipment and medium | |
| CN110716570B (en) | Relative field acquisition method based on ship cluster situation for open and busy water area |
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 | ||
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Zhang Ya Inventor after: Wang Yingying Inventor after: Yu Fei Inventor after: Wang Qingxin Inventor after: Gao Wei Inventor after: Wei Jianxiong Inventor after: Huang Wenjun Inventor before: Zhang Ya Inventor before: Yu Fei Inventor before: Wang Qingxin Inventor before: Gao Wei Inventor before: Wei Jianxiong Inventor before: Huang Wenjun |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |