CN113340301B - Submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient beacon - Google Patents
Submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient beacon Download PDFInfo
- Publication number
- CN113340301B CN113340301B CN202110604518.6A CN202110604518A CN113340301B CN 113340301 B CN113340301 B CN 113340301B CN 202110604518 A CN202110604518 A CN 202110604518A CN 113340301 B CN113340301 B CN 113340301B
- Authority
- CN
- China
- Prior art keywords
- gravity gradient
- gradient data
- gravity
- random
- data
- 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
- 230000005484 gravity Effects 0.000 title claims abstract description 225
- 239000002245 particle Substances 0.000 title claims abstract description 76
- 238000005457 optimization Methods 0.000 title claims abstract description 40
- 238000000034 method Methods 0.000 title claims abstract description 24
- 238000010606 normalization Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 11
- 238000012937 correction Methods 0.000 claims description 7
- 230000002159 abnormal effect Effects 0.000 claims description 6
- 230000006835 compression Effects 0.000 claims description 5
- 238000007906 compression Methods 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000000452 restraining effect Effects 0.000 claims 2
- 238000010586 diagram Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 208000011597 CGF1 Diseases 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 230000004039 social cognition Effects 0.000 description 1
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a submersible vehicle navigation method and system based on particle swarm optimization and a gravity gradient beacon. The method comprises the steps of acquiring the current position of the underwater vehicle by using an inertial navigation system; determining a search range according to the current position; acquiring gravity gradient data of a random position in a search range and gravity gradient data corresponding to the random position on a gravity gradient background image by using a gravity gradiometer; performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on the gravity gradient background image to obtain the optimized gravity gradient data at the random position; correcting the gravity gradiometer by using the optimized gravity gradient data at the random position; acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer; correcting the inertial navigation system by using the gravity gradient data of the current position; and navigating by using the corrected inertial navigation system. The invention improves the navigation precision of the underwater vehicle.
Description
Technical Field
The invention relates to the field of underwater vehicle gravity navigation, in particular to a method and a system for navigating a submersible vehicle based on particle swarm optimization and a gravity gradient lighthouse.
Background
The underwater vehicle needs to carry out concealing work with long voyage and long voyage, so that higher requirements are provided for whether the underwater vehicle can realize real-time high-precision positioning in the deep ocean, and at present, people research and design various auxiliary navigation methods, including active signal navigation and passive signal navigation. Along with the increase of the exploration field, people tend to combine the passive signal navigation and the gravity signal navigation, wherein the gravity matching technology in the passive signal navigation introduces an underwater gravity beacon concept, the matching effect of an ocean gravity gradient field can be effectively improved, and the condition that large errors occur after gravity gradient matching inertial navigation is carried out in an unadapted area is avoided.
In the prior art, the matching algorithm of the ocean gravity gradient field is constrained by the resolution of grid points, and the corresponding matching effect is still to be improved, so that the navigation precision of the underwater vehicle is not high enough. Therefore, a new matching method is needed to further improve the navigation accuracy of the underwater vehicle.
Disclosure of Invention
The invention aims to provide a submersible vehicle navigation method and system based on particle swarm optimization and a gravity gradient beacon, which improve the navigation precision of an underwater vehicle.
In order to achieve the purpose, the invention provides the following scheme:
a submersible vehicle navigation method based on particle swarm optimization and gravity gradient lighthouse comprises the following steps:
acquiring the current position of the underwater vehicle by using an inertial navigation system;
determining a search range according to the current position;
acquiring gravity gradient data of a random position in the search range and gravity gradient data corresponding to the random position on a gravity gradient background image by using the gravity gradiometer; the gravity gradient data is six-dimensional data;
performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on a gravity gradient background graph to obtain the optimized gravity gradient data at the random position;
correcting the gravity gradiometer by using the optimized gravity gradient data at the random position; acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer;
correcting the inertial navigation system by using the gravity gradient data of the current position;
and navigating by using the corrected inertial navigation system.
Optionally, the obtaining the current position of the underwater vehicle by using the inertial navigation system specifically includes:
acquiring the angle increment and the speed increment of the underwater vehicle by using the inertial navigation system;
determining the current range and the current course according to the angle increment and the speed increment;
and determining the current position according to the current range and the current heading.
Optionally, the obtaining, by using the gravity gradiometer, gravity gradient data at a random position within the search range and gravity gradient data corresponding to the random position on a gravity gradient background map further includes:
and carrying out normalization processing on the gravity gradient data at random positions.
Optionally, the performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on the gravity gradient background map to obtain the optimized gravity gradient data at the random position specifically includes:
using a formula
Determining the similarity degree of the gravity gradient data of the random position and the gravity gradient data corresponding to the random position on a gravity gradient background image;
wherein, N is the dimension of the data,
normalized gravity gradient data for the random position,
for the normalized gravity gradient data corresponding to the random position on the gravity gradient background map,
and corresponding the normalized gravity gradient abnormal data to the random position on the gravity gradient background image.
A submarine navigation system based on particle swarm optimization and gravity gradient lighthouse comprises:
the current position acquisition module of the underwater vehicle is used for acquiring the current position of the underwater vehicle by using the inertial navigation system;
a searching range determining module for determining the searching range according to the current position;
the gravity gradient data acquisition module is used for acquiring gravity gradient data of a random position in the search range and gravity gradient data corresponding to the random position on a gravity gradient background image by using the gravity gradiometer; the gravity gradient data is six-dimensional data;
the gravity gradient data optimization module is used for performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on a gravity gradient background map to obtain the optimized gravity gradient data at the random position;
the gravity gradiometer correction module is used for correcting the gravity gradiometer by using the optimized gravity gradient data at the random position; acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer;
the inertial navigation system correction module is used for correcting the inertial navigation system by utilizing the gravity gradient data of the current position;
and the navigation module is used for navigating by utilizing the corrected inertial navigation system.
Optionally, the current position obtaining module of the underwater vehicle specifically includes:
the angular increment and speed increment acquisition unit is used for acquiring the angular increment and the speed increment of the underwater vehicle by utilizing the inertial navigation system;
the current range and current course determining unit is used for determining the current range and the current course according to the angle increment and the speed increment;
and the current position determining unit is used for determining the current position according to the current range and the current course.
Optionally, the method further includes:
and the normalization processing module is used for performing normalization processing on the gravity gradient data at the random position.
Optionally, the gravity gradient data optimization module specifically includes:
similarity degree determination unit using formula
Determining the similarity degree of the gravity gradient data of the random position and the gravity gradient data corresponding to the random position on a gravity gradient background image;
wherein, N is the dimension of the data,
normalized gravity gradient data for the random position,
for the normalized gravity gradient data corresponding to the random position on the gravity gradient background map,
and corresponding the normalized gravity gradient abnormal data to the random position on the gravity gradient background image.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
according to the submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient lighthouse, gravity gradient data are searched by utilizing a particle swarm optimization algorithm, high-precision matched navigation in an effective range of the gravity lighthouse is realized, a selection mode of a traditional algorithm for an optimal point is skipped, and the constraint of lattice point resolution is avoided. High-precision navigation positioning can be effectively realized without multi-point matching, and the efficiency is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a schematic flow chart of a submersible vehicle navigation method based on particle swarm optimization and a gravity gradient beacon according to the present invention;
FIG. 2 is a schematic diagram illustrating a principle of a submersible vehicle navigation method based on particle swarm optimization and a gravity gradient beacon according to the present invention;
FIG. 3 is a schematic view of a 6-dimensional gravity gradient lighthouse;
FIG. 4 is a schematic diagram of gravity gradient data at random locations within the search range;
fig. 5 is a schematic structural diagram of a submersible navigation system based on particle swarm optimization and a gravity gradient beacon provided in the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention aims to provide a submarine vehicle navigation method and system based on particle swarm optimization and a gravity gradient beacon, and the underwater submarine vehicle navigation precision is improved.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
In the process of underwater auxiliary navigation by the underwater vehicle, acquiring position information in real time according to an inertial navigation system, and searching corresponding data on a gravity gradient background image near a corresponding position; meanwhile, the underwater vehicle utilizes a gravity gradiometer equipped by the underwater vehicle to measure in real time to obtain corresponding gravity gradient observation data, and the available gravity gradient data is obtained through a series of data processing; and finally, matching the gravity gradient data after real-time measurement and processing with a pre-installed ocean gravity gradient background field through a series of algorithms, and correcting inertial navigation data to obtain more accurate position information of the underwater vehicle.
The particle swarm optimization algorithm is an evolutionary computing technology, and simulates the birds in a bird swarm by designing a particle without mass according to behavior research on predation of the bird swarm, wherein the particle has only two attributes: speed, which represents how fast the movement is, and position, which represents the direction of the movement.
Suppose that in an N-dimensional target search space, M particles form a population, where the ith particle is represented as a D-dimensional vector, P i Indicating the position of the ith particle.
P i ＝(P i1 ,P i2 ,...,P iD )＇,i＝1,2,...,M (1)
The ith particle movement velocity is an N-dimensional vector.
V i ＝(v i1 ,v i2 ,...,v iD )＇,i＝1,2,...,M (2)
The optimal position currently searched by the ith particle is called an individual extremum.
E best ＝(p i1 ,p i2 ,...p iN )＇,i＝1,2,...,M (3)
The global optimal position searched by the whole particle.
G best ＝(p g1 ,p g2 ,...,p gN )＇ (4)
The individual extrema and the global optimum are used in conjunction with the following equations to update the velocity and position of the particles.
Wherein the superscript n is 1, 2, …, and G represents an iteration number; d ═ 1, 2, …, D; i is 1, 2, …, M is the size of the cluster; w is the inertial weight (or momentum coefficient); c 1 ，C 2 Are positive constants, respectively called selfPhysical and social cognition factors for regulating E best And G best The influence strength of (a); r is a random number between (0, 1), V i Is the velocity of the particle; v id ∈(-V max ,V max )，V max To set a non-negative speed limit. P id ∈(P min ,P max )，P min ,P max For the position range, K is a compression factor to constrain the flight velocity of the particle.
Fig. 1 is a schematic flow diagram of a submersible vehicle navigation method based on particle swarm optimization and a gravity gradient beacon provided by the present invention, fig. 2 is a schematic diagram of a principle of a submersible vehicle navigation method based on particle swarm optimization and a gravity gradient beacon provided by the present invention, as shown in fig. 1 and fig. 2, a submersible vehicle navigation method based on particle swarm optimization and a gravity gradient beacon provided by the present invention includes:
and S101, acquiring the current position of the underwater vehicle by using an inertial navigation system.
S101 specifically comprises the following steps:
and acquiring the angular increment and the speed increment of the underwater vehicle by using the inertial navigation system.
And determining the current range and the current heading according to the angle increment and the speed increment.
And determining the current position according to the current range and the current heading.
Obtaining t 1 Time to t 2 Course, range and t of time submersible vehicle 1 The position of the underwater vehicle at the moment can be deduced to calculate the movement of the underwater vehicle to t 2 The position coordinates of the time of day.
Wherein, t 1 Inertial navigation system of time submersible vehicle position information through last time gravity gradient matchingThe position correction is carried out, and the high position precision is achieved.
And S102, determining a search range according to the current position.
S103, acquiring gravity gradient data of a random position in the search range and gravity gradient data corresponding to the random position on a gravity gradient background image by using the gravity gradiometer; the gravity gradient data is six-dimensional data. I.e. the gravity gradient data at random positions is g xx 、g xy 、g xz 、g yy 、g yz 、g zz And as shown in fig. 4. The gravity gradient data corresponding to the random position on the gravity gradient background image is G ixx 、G ixy 、G ixz 、G iyy 、G iyz 、G izz And as shown in fig. 3. And storing the data in a vector form. As shown in formula (9), when the selected random positions are five, the generated particles are 5 particles, i.e., 6 × 5 vector matrices are generated, and when N particles are generated, 6 × N vector matrices are generated.
After S103, further comprising:
and carrying out normalization processing on the gravity gradient data at random positions.
Sequence g ═ g of gravity gradient data at random locations { g ═ g 1 ,g 2 ,...g 6 The mean μ (g) and standard deviation δ (g) are calculated so that the measurement sequence g can be normalized:
wherein,
and S104, performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on the gravity gradient background map to obtain the optimized gravity gradient data at the random position.
S104 specifically comprises the following steps:
using formulas
Determining the similarity degree of the gravity gradient data of the random position and the gravity gradient data corresponding to the random position on a gravity gradient background image;
wherein, N is the dimension of the data,
normalized gravity gradient data for the random position,
for the normalized gravity gradient data corresponding to the random position on the gravity gradient background map,
and corresponding the normalized gravity gradient abnormal data to the random position on the gravity gradient background image.
Preserving the degree of similarity of each particle
And judging the particles with the highest similarity
Storing coordinate position information of corresponding time
And then adjusting the speed and the position of the updated particle according to a particle operation formula:
wherein n is the number of iterations; d is the number of the track points; i is the size of the population; c 1 ，C 2 To control the weights of the local search and the global search. The displacement velocity of each particle is adjusted according to a formula, and then the position of each particle is adjusted. With the continuous movement of the five particles to obtain better similarity, the particle swarm tends to be concentrated and finally integrated at the actual data acquisition position point of the underwater vehicle, so as to obtain the real position information of the underwater vehicle.
S105, correcting the gravity gradiometer by using the optimized gravity gradient data at the random position; and acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer.
And S106, correcting the inertial navigation system by using the gravity gradient data of the current position.
And S107, navigating by using the corrected inertial navigation system.
The invention provides an intelligent algorithm, which gets rid of the selection mode of the traditional algorithm on the optimal point and avoids the constraint of grid point resolution. The method provides a mode for utilizing gravity gradient data, high-precision navigation and positioning can be effectively realized without multipoint matching, and the efficiency is improved. The particle swarm optimization algorithm based on the gravity beacon environment can obtain the most effective combination of the algorithm, so that the local optimal condition without special environment is avoided.
Fig. 5 is a schematic structural diagram of a submarine navigation system based on particle swarm optimization and a gravity gradient beacon provided in the present invention, and as shown in fig. 5, the submarine navigation system based on particle swarm optimization and a gravity gradient beacon provided in the present invention comprises:
and a current position acquiring module 501 of the underwater vehicle, which is used for acquiring the current position of the underwater vehicle by using the inertial navigation system.
A search range determining module 502, configured to determine a search range according to the current position.
A gravity gradient data obtaining module 503, configured to obtain, by using the gravity gradiometer, gravity gradient data at a random position within the search range and gravity gradient data corresponding to the random position on a gravity gradient background map; the gravity gradient data is six-dimensional data.
And the gravity gradient data optimization module 504 is configured to perform particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on the gravity gradient background map to obtain optimized gravity gradient data at the random position.
A gravity gradiometer correction module 505 for correcting the gravity gradiometer using the optimized gravity gradient data at the random position; and acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer.
An inertial navigation system correction module 506, configured to correct the inertial navigation system using the gravity gradient data of the current position.
And a navigation module 507, configured to perform navigation by using the corrected inertial navigation system.
The current position obtaining module 501 of the underwater vehicle specifically includes:
and the angular increment and speed increment acquisition unit is used for acquiring the angular increment and the speed increment of the underwater vehicle by utilizing the inertial navigation system.
And the current range and current course determining unit is used for determining the current range and the current course according to the angle increment and the speed increment.
And the current position determining unit is used for determining the current position according to the current range and the current course.
The invention provides a submersible vehicle navigation system based on particle swarm optimization and gravity gradient lighthouse, which further comprises:
and the normalization processing module is used for performing normalization processing on the gravity gradient data at the random position.
The gravity gradient data optimization module 504 specifically includes:
a similarity degree determination unit for determining the degree of similarityBy the formula
Determining the similarity degree of the gravity gradient data of the random position and the gravity gradient data corresponding to the random position on a gravity gradient background image;
wherein, N is the dimension of the data,
normalizing the gravity gradient data for the random location,
for the normalized gravity gradient data corresponding to the random position on the gravity gradient background map,
and corresponding the normalized gravity gradient abnormal data to the random position on the gravity gradient background image.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the foregoing, the description is not to be taken in a limiting sense.
Claims (4)
1. A submersible navigation method based on particle swarm optimization and a gravity gradient lighthouse is characterized by comprising the following steps of:
acquiring the current position of the underwater vehicle by using an inertial navigation system;
determining a search range according to the current position;
acquiring gravity gradient data of a random position in the search range and gravity gradient data corresponding to the random position on a gravity gradient background image by using a gravity gradiometer; the gravity gradient data is six-dimensional data;
performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on a gravity gradient background graph to obtain the optimized gravity gradient data at the random position;
correcting the gravity gradiometer by using the optimized gravity gradient data at the random position; acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer;
correcting the inertial navigation system by using the gravity gradient data of the current position;
navigating by using the corrected inertial navigation system;
the acquiring, by the gravity gradiometer, gravity gradient data at a random position within the search range and gravity gradient data corresponding to the random position on a gravity gradient background map, and then further including:
carrying out normalization processing on the gravity gradient data at random positions;
performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on a gravity gradient background graph to obtain the optimized gravity gradient data at the random position, specifically comprising:
using formulas
Determining the similarity degree of the gravity gradient data of the random position and the gravity gradient data corresponding to the random position on a gravity gradient background image;
wherein, N is the dimension of the data,
normalized gravity gradient data for the random position,
for the normalized gravity gradient data corresponding to the random position on the gravity gradient background map,
the gravity gradient abnormal data after corresponding normalization with the random position on the gravity gradient background image;
preserving the degree of similarity of each particle
And judging the particles with the highest similarity
Storing coordinate position information of corresponding time
And then adjusting the speed and the position of the updated particle according to a particle operation formula:
in the formula, n is iteration number, d is number of track points, i is group size, C 1 ，C 2 The weight used to control the local search and the global search, W is the inertia weight or momentum coefficient,
and
k is a compression factor and is a random number between 0 and 1, the compression factor is used for restraining the flight speed of the particles and adjusting the displacement speed of each particle according to a formula, then the position of each particle is adjusted, the particle swarm tends to be concentrated along with the continuous movement of five particles to obtain better similarity, and finally the particle swarm is integrated with the actual data acquisition position points of the underwater vehicle, so that the real position information of the underwater vehicle is obtained.
2. The particle swarm optimization and gravity gradient beacon-based submersible navigation method according to claim 1, wherein the obtaining of the current position of the underwater vehicle by the inertial navigation system specifically comprises:
acquiring the angle increment and the speed increment of the underwater vehicle by using the inertial navigation system;
determining the current range and the current course according to the angle increment and the speed increment;
and determining the current position according to the current range and the current heading.
3. The utility model provides a latent ware navigation based on particle swarm optimization and gravity gradient beacon which characterized in that includes:
the current position acquisition module of the underwater vehicle is used for acquiring the current position of the underwater vehicle by using the inertial navigation system;
a searching range determining module for determining a searching range according to the current position;
the gravity gradient data acquisition module is used for acquiring gravity gradient data of a random position in the search range and gravity gradient data corresponding to the random position on a gravity gradient background image by using a gravity gradiometer; the gravity gradient data is six-dimensional data;
the gravity gradient data optimization module is used for performing particle swarm optimization on the gravity gradient data at the random position and the gravity gradient data corresponding to the random position on the gravity gradient background map to obtain the optimized gravity gradient data at the random position;
the gravity gradiometer correction module is used for correcting the gravity gradiometer by using the optimized gravity gradient data at the random position; acquiring the gravity gradient data of the underwater submarine at the current position by using the corrected gravity gradiometer;
the inertial navigation system correction module is used for correcting the inertial navigation system by utilizing the gravity gradient data of the current position;
the navigation module is used for navigating by utilizing the corrected inertial navigation system;
the normalization processing module is used for performing normalization processing on the gravity gradient data at the random position;
the gravity gradient data optimization module specifically comprises:
a similarity degree determination unit for using a formula
Determining the similarity degree of the gravity gradient data of the random position and the gravity gradient data corresponding to the random position on a gravity gradient background image;
wherein, N is the dimension of the data,
normalized gravity gradient data for the random position,
for the normalized gravity gradient data corresponding to the random position on the gravity gradient background map,
the gravity gradient abnormal data after corresponding normalization with the random position on the gravity gradient background image;
preserving the degree of similarity of each particle
And judging the particles with the highest similarity
Storing coordinate position information of corresponding time
And then adjusting the speed and the position of the updated particle according to a particle operation formula:
in the formula, n is iteration number, d is number of track points, i is group size, C 1 ，C 2 The weight used to control the local search and the global search, W is the inertia weight or momentum coefficient,
and
k is a compression factor and is a random number between 0 and 1, the compression factor is used for restraining the flight speed of the particles and adjusting the displacement speed of each particle according to a formula, then the position of each particle is adjusted, the particle swarm tends to be concentrated along with the continuous movement of five particles to obtain better similarity, and finally the particle swarm is integrated with the actual data acquisition position points of the underwater vehicle, so that the real position information of the underwater vehicle is obtained.
4. The particle swarm optimization and gravity gradient beacon-based submersible navigation system according to claim 3, wherein the current position acquisition module of the underwater vehicle specifically comprises:
the angular increment and speed increment acquisition unit is used for acquiring the angular increment and the speed increment of the underwater vehicle by utilizing the inertial navigation system;
the current range and current course determining unit is used for determining the current range and the current course according to the angle increment and the speed increment;
and the current position determining unit is used for determining the current position according to the current range and the current course.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110604518.6A CN113340301B (en) | 2021-05-31 | 2021-05-31 | Submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient beacon |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110604518.6A CN113340301B (en) | 2021-05-31 | 2021-05-31 | Submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient beacon |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113340301A CN113340301A (en) | 2021-09-03 |
| CN113340301B true CN113340301B (en) | 2022-08-02 |
Family
ID=77473536
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110604518.6A Active CN113340301B (en) | 2021-05-31 | 2021-05-31 | Submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient beacon |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113340301B (en) |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1642089B1 (en) * | 2003-07-03 | 2010-09-22 | Northrop Grumman Corporation | Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients |
| CN111649763B (en) * | 2020-04-26 | 2021-08-27 | 中国人民解放军61540部队 | Submarine navigation method and system established based on gravity beacon |
| CN111595345B (en) * | 2020-06-02 | 2021-08-31 | 中国人民解放军61540部队 | Submarine navigation method and system based on multi-dimensional gravity gradient lighthouse |
| CN112432642B (en) * | 2020-11-06 | 2023-03-14 | 中国人民解放军61540部队 | Gravity beacon and inertial navigation fusion positioning method and system |
-
2021
- 2021-05-31 CN CN202110604518.6A patent/CN113340301B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113340301A (en) | 2021-09-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN102426019B (en) | Unmanned aerial vehicle scene matching auxiliary navigation method and system | |
| CN113916243B (en) | Vehicle positioning method, device, equipment and storage medium for target scene area | |
| CN110930495A (en) | Multi-unmanned aerial vehicle cooperation-based ICP point cloud map fusion method, system, device and storage medium | |
| CN105043388B (en) | Vector search Iterative matching method based on INS/Gravity matching integrated navigation | |
| CN103914068A (en) | Service robot autonomous navigation method based on raster maps | |
| CN107831515B (en) | Underwater Navigation method and system | |
| CN110726415B (en) | Self-adaptive underwater multi-beam synchronous positioning and mapping method | |
| CN115952691B (en) | Optimal station distribution method and device for multi-station passive time difference cross joint positioning system | |
| CN114061591B (en) | Contour line matching method based on sliding window data backtracking | |
| CN111397599A (en) | Improved ICCP (Integrated Circuit chip) underwater geomagnetic matching method based on triangular matching algorithm | |
| CN109375646A (en) | AUV docking recycling autonomous navigation method based on FMSRUPF algorithm | |
| CN111895995B (en) | PSO-based aircraft formation multidimensional geomagnetic matching navigation method and system | |
| CN113252038A (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
| WO2020247721A1 (en) | Systems and methods to improve geo-referencing using a combination of magnetic field models and in situ measurements | |
| CN116642482A (en) | Positioning method, equipment and medium based on solid-state laser radar and inertial navigation | |
| CN110132281A (en) | A kind of autonomous acoustic navigation method of underwater high-speed target with high precision based on inquiry answer-mode | |
| CN110196425A (en) | A kind of passive acoustic direction method of mobile platform to submarine target | |
| CN112762924B (en) | Navigation positioning method based on gravity gradient-topography heterologous data matching | |
| CN111275748B (en) | Point cloud registration method based on laser radar in dynamic environment | |
| CN113129377A (en) | Three-dimensional laser radar rapid robust SLAM method and device | |
| CN109855623A (en) | Geomagnetic model online approximating method based on Legendre multinomial and BP neural network | |
| CN113340301B (en) | Submersible vehicle navigation method and system based on particle swarm optimization and gravity gradient beacon | |
| CN113252039B (en) | Terrain-assisted navigation-oriented particle swarm fast matching method | |
| CN113624227A (en) | Contour line iteration gravity matching algorithm based on Mahalanobis distance | |
| Zhuangsheng et al. | Study on initial gravity map matching technique based on triangle constraint model |
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 |