CN112129285A - Magnetic/inertial combination-based frogman navigation attitude estimation method under emergency condition - Google Patents
Magnetic/inertial combination-based frogman navigation attitude estimation method under emergency condition Download PDFInfo
- Publication number
- CN112129285A CN112129285A CN202010958739.9A CN202010958739A CN112129285A CN 112129285 A CN112129285 A CN 112129285A CN 202010958739 A CN202010958739 A CN 202010958739A CN 112129285 A CN112129285 A CN 112129285A
- Authority
- CN
- China
- Prior art keywords
- error
- frogman
- attitude
- navigation
- magnetic
- 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.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/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
Abstract
The invention provides a frogman navigation attitude estimation method based on lower magnetic/inertia combination in emergency, which is used for estimating the underwater course attitude of a frogman in emergency. The method effectively calibrates the initial course error to enable the initial course error to be within the magnetic compass attitude measurement precision range, can effectively compensate the position error and the speed error caused by the gyroscope drift, effectively compensates the positioning error caused by the initial course attitude information error during emergency diving, and improves the navigation positioning precision.
Description
Technical Field
The invention relates to an underwater frogman course attitude estimation method, in particular to a frogman course attitude estimation method based on magnetic/inertia combination under emergency.
Background
With the continuous development of science and technology, frogmans have good application prospects in daily life and the military field. Due to the limitation of navigation means and equipment cost, the navigation positioning precision, the working efficiency and the reliability of the frogman system are in urgent need of improvement. In daily life, frogman can carry out a series of underwater engineering work, such as underwater leak stoppage, underwater photography, underwater overhaul, underwater salvage, underwater desilting, underwater installation, underwater rescue and the like. Underwater environments are more complex than those on water, such as poor communication caused by more interference, poor direction feeling caused by low visibility, and inconvenient movement caused by large underwater resistance. When frogmans need to search, rescue, salvage and other tasks in a large range for a long time, the frogmans cannot search or accurately locate a preset target, and are easy to cause danger underwater.
Frogman is also important in military field, with the continuous development of special combat equipment, frogman troops have become the main force of tasks such as detection, blasting, target guidance and the like in water areas such as harbors, inland and the like, frogman troops are established in the united states, germany, britain, russia and the like, and frogman troops are expected to become one of the decisive factors in future maritime and amphibious combat.
Therefore, a method for guaranteeing the safety of frogmans and improving the positioning accuracy and the attitude accuracy of frogmans is needed, when frogmans normally work underwater, based on underwater alignment results and a series of state transition matrixes from underwater alignment completion time to underwater alignment completion time, an initial course attitude error is corrected and gyro drift is estimated by adopting a state backtracking method, and navigation calculation is carried out again from the initial time by using corrected parameters and stored information so as to improve the attitude accuracy of frogmans and reduce the position error and the speed error of the frogmans.
Disclosure of Invention
Technical problem to be solved
The invention provides a frogman navigation attitude estimation method based on magnetic/inertial combination under emergency conditions, and solves the technical problem of how to improve the accuracy of underwater navigation attitude of frogmans.
(II) technical scheme
In order to solve the problems, the invention provides a frogman navigation attitude estimation method based on magnetic/inertial combination under emergency conditions, which comprehensively considers the advantages of an inertial navigation system and a magnetic compass and combines a series of state transfer matrixes from the moment of entering water to the moment of finishing underwater alignment, so that the initial navigation attitude information of an underwater frogman can be effectively corrected. The method can effectively reduce the complexity of hardware and the complexity of installation, can calibrate the initial course error only by using the magnetic compass/inertia combination, enables the initial course error to be within the range of the attitude measurement precision of the magnetic compass, can effectively compensate the position error and the speed error caused by the drift of the gyroscope, effectively compensates the positioning error caused by the initial course attitude information error during emergency diving, and improves the navigation positioning precision.
A frogman navigation attitude estimation method based on magnetic/inertia combination under emergency condition is characterized by comprising the following steps:
(1) the frogman enters water under the emergency condition or the strong magnetic interference condition of the aircraft, cannot align the initial course angle, and normally aligns the course angle after the frogman is far away from the aircraft or the magnetic interference disappears.
(2) And correcting the initial course attitude error and estimating the gyro drift by adopting a state backtracking method based on the underwater alignment result, a series of state transition matrixes from the time of entering water to the time of completing the underwater alignment and the inertia/magnetic compass combined navigation result after normal alignment.
(3) And then, the corrected parameters are utilized to carry out navigation calculation again from the initial moment, so that the influence of the initial course error is reduced.
The inertial/magnetic compass integrated navigation method in the step (2) comprises the following steps:
the combined navigation uses an INS error equation as a state equation, and the state quantity contains 15 dimensions of INS errors and IMU errors, as shown in the following formula.
X＝[φE φN φU VE VN VU L λ h E N U ▽E ▽N ▽U]T
Wherein phi isE、φN、φUMisalignment angles for east, north and sky, respectively; vE、VN、VUVelocity errors in the east, north and sky directions, respectively; l, lambda and h are respectively latitude, longitude and depth error;E、N、Urespectively zero drift of the gyroscope along east, north and sky axes; vE、▽N、▽URespectively, the zero offset of the accelerometer along the east, north and sky axes.
Let the equation of state be:
wherein the state transition matrix is:
the noise transfer matrix is:
wherein, FNDetermined by inertial navigation error equation
The system noise is selected as:
the INS/magnetic compass integrated navigation uses the attitude difference of the INS and the magnetic compass as a measurement to carry out integrated navigation, and the measurement equation is set as follows:
Z3＝H3X+η3
wherein the observed quantity is Z3＝[φx φy φz]T；
The measurement matrix is H3＝[H31 H32]
The attitude angle error can be converted into an INS digital platform misalignment angle H32＝zeros(3,12)，η3Random noise is output from the magnetic compass.
The frogman navigation attitude estimation method based on the magnetic/inertia combination under the emergency condition can effectively solve the problem of navigation errors caused by inaccurate initial information of a navigation system in an emergency diving task, and particularly solves the problem of inaccurate accuracy of initial course attitude influenced by a ferromagnetic environment.
(III) advantageous effects
The invention provides a frogman navigation attitude estimation method based on magnetic/inertial combination under emergency conditions, which comprises an inertial navigation system and the advantages of a magnetic compass, and combines a series of state transfer matrixes from the moment of entering water to the moment of finishing underwater alignment to effectively correct initial navigation attitude information of an underwater frogman. The method can effectively reduce the complexity of hardware and the complexity of installation, can calibrate the initial course error only by using the magnetic compass/inertia combination, enables the initial course error to be within the range of the attitude measurement precision of the magnetic compass, can effectively compensate the position error and the speed error caused by the drift of the gyroscope, effectively compensates the course attitude error caused by the initial course attitude information error during emergency diving, improves the navigation attitude precision, and has certain effect on the divergence inhibition of the position error and the speed error.
Drawings
Fig. 1 is a flow chart of a frogman navigation pose estimation method based on magnetic/inertial combination in emergency.
Detailed Description
In order to make the objects, contents and advantages of the present invention clearer, the following detailed description of the embodiments of the present invention will be made in conjunction with the accompanying drawings and examples.
The embodiment provides a frogman navigation pose estimation method based on magnetic/inertial combination under emergency, which comprises the following steps: the initial navigation attitude information of the underwater frogman is effectively corrected by utilizing the advantages of an inertial navigation system and a magnetic compass and combining a series state transfer matrix from the moment of entering water to the moment of finishing underwater alignment. The method can effectively reduce the complexity of hardware and the complexity of installation, can calibrate the initial course error only by using the magnetic compass/inertia combination, enables the initial course error to be within the range of the attitude measurement precision of the magnetic compass, can effectively compensate the position error and the speed error caused by the drift of the gyroscope, effectively compensates the positioning error caused by the initial course attitude information error during emergency diving, and improves the navigation positioning precision.
The specific implementation steps of the technical solution of the invention are shown in fig. 1, and the specific implementation steps are as follows:
(1) the frogman enters water under the emergency condition or the strong magnetic interference condition of the aircraft, cannot align the initial course angle, and normally aligns the course angle after the frogman is far away from the aircraft or the magnetic interference disappears.
(2) And correcting the initial course attitude error and estimating the gyro drift by adopting a state backtracking method based on the underwater alignment result, a series of state transition matrixes from the time of entering water to the time of completing the underwater alignment and the inertia/magnetic compass combined navigation result after normal alignment.
(3) And then, the corrected parameters are utilized to carry out navigation calculation again from the initial moment, so that the influence of the initial course error is reduced.
And (3) realizing the inertial/magnetic compass integrated navigation method in the step (2) by using Kalman filtering, wherein the state quantity comprises 15 dimensions of INS errors and IMU errors, and the INS errors and the IMU errors are shown in the following formula.
X＝[φE φN φU VE VN VU L λ h E N U ▽E ▽N ▽U]T
Wherein phi isE、φN、φUMisalignment angles for east, north and sky, respectively; vE、VN、VUVelocity errors in the east, north and sky directions, respectively; l, lambda and h are respectively latitude, longitude and depth error;E、N、Urespectively zero drift of the gyroscope along east, north and sky axes; vE、▽N、▽URespectively, the zero offset of the accelerometer along the east, north and sky axes.
Let the equation of state be:
the INS/magnetic compass integrated navigation uses the attitude difference of the INS and the magnetic compass as a measurement to carry out integrated navigation, and the measurement equation is set as follows:
Z3＝H3X+η3
in the above equation, the state transition matrix is:
the noise transfer matrix is:
FNdetermined by inertial navigation error equation
The measurement matrix is H3＝[H31 H32]
The attitude angle error can be converted into an INS digital platform misalignment angle H32＝zeros(3,12)。η3Random noise is output from the magnetic compass. When the scheme is designed and implemented, the whole process of the frogman underwater navigation is fully considered, pure inertial navigation is adopted within a period of time after the frogman leaves a mother ship, and after the magnetic compass can normally work and outputs higher-precision attitude information, INS/magnetic compass combined navigation and gyro drift online calibration are carried out. After the gyro drift estimation is finished, state backtracking is adoptedThe method comprises the steps of carrying out backtracking derivation and compensation on navigation initial attitude errors, position errors and speed errors caused by a gyroscope to obtain a pure INS navigation result after compensation and correction, and then continuously carrying out INS/magnetic compass combined navigation by taking the pure INS navigation result as a reference.
The above description is only one of the preferred embodiments of the present invention, and it will be apparent to those skilled in the art that various modifications and variations can be made without departing from the technical principles of the present invention, and these modifications and variations should also be considered as the protection scope of the present invention.
Claims (3)
1. A frogman navigation attitude estimation method based on magnetic/inertia combination under emergency condition is characterized in that:
(1) the frogman enters water under the emergency condition or the strong magnetic interference condition of the aircraft, cannot align the initial course angle, and normally aligns the course angle after the frogman is far away from the aircraft or the magnetic interference disappears.
(2) And correcting the initial course attitude error and estimating the gyro drift by adopting a state backtracking method based on the underwater alignment result, a series of state transition matrixes from the time of entering water to the time of completing the underwater alignment and the inertia/magnetic compass combined navigation result after normal alignment.
(3) And then, the corrected parameters are utilized to carry out navigation calculation again from the initial moment, so that the influence of the initial course error is reduced.
2. The frogman gesture estimation method based on the combination of magnetism and inertia in the emergency situation as claimed in claim 1, wherein: the initial navigation attitude information of the underwater frogman is effectively corrected by utilizing the advantages of an inertial navigation system and a magnetic compass and combining a series state transfer matrix from the moment of entering water to the moment of finishing underwater alignment. The method can effectively reduce the complexity of hardware and the complexity of installation, can calibrate the initial course error only by using the magnetic compass/inertia combination, enables the initial course error to be within the range of the attitude measurement precision of the magnetic compass, can effectively compensate the position error and the speed error caused by the drift of the gyroscope, effectively compensates the positioning error caused by the initial course attitude information error during emergency diving, and improves the navigation positioning precision.
3. The frogman gesture estimation method based on the combination of magnetism and inertia in the emergency situation as claimed in claim 1, wherein: the method effectively compensates the attitude of the inertial navigation system by using the high-precision attitude data of the magnetic compass, solves the problem that the inertial navigation system diverges along with time, improves the reliability and the accuracy of the data, can carry out on-line estimation on the constant drift of the gyroscope of the inertial navigation system by designing a Kalman filtering equation, effectively inhibits the attitude error accumulation of the inertial navigation system, and has certain effect on the divergence inhibition of position errors and speed errors.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010958739.9A CN112129285A (en) | 2020-09-14 | 2020-09-14 | Magnetic/inertial combination-based frogman navigation attitude estimation method under emergency condition |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010958739.9A CN112129285A (en) | 2020-09-14 | 2020-09-14 | Magnetic/inertial combination-based frogman navigation attitude estimation method under emergency condition |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112129285A true CN112129285A (en) | 2020-12-25 |
Family
ID=73846729
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010958739.9A Pending CN112129285A (en) | 2020-09-14 | 2020-09-14 | Magnetic/inertial combination-based frogman navigation attitude estimation method under emergency condition |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112129285A (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
| CN104374405A (en) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering |
| CN105180968A (en) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | IMU/magnetometer installation misalignment angle online filter calibration method |
| CN108170154A (en) * | 2017-12-19 | 2018-06-15 | 广东省航空航天装备技术研究所 | A kind of unmanned plane multisensor forward direction photography, which tilts, flies control adjustment method |
-
2020
- 2020-09-14 CN CN202010958739.9A patent/CN112129285A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
| CN104374405A (en) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering |
| CN105180968A (en) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | IMU/magnetometer installation misalignment angle online filter calibration method |
| CN108170154A (en) * | 2017-12-19 | 2018-06-15 | 广东省航空航天装备技术研究所 | A kind of unmanned plane multisensor forward direction photography, which tilts, flies control adjustment method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2020062791A1 (en) | Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle | |
| Nijmeijer et al. | New directions in nonlinear observer design | |
| WO2020062807A1 (en) | Method for application of improved unscented kalman filter algorithm in underwater integrated navigation | |
| CN108594283B (en) | Free installation method of GNSS/MEMS inertial integrated navigation system | |
| CN106979781B (en) | High-precision transfer alignment method based on distributed inertial network | |
| Allotta et al. | Single axis FOG aided attitude estimation algorithm for mobile robots | |
| CN108303079B (en) | Data smoothing method for underwater USBL reverse application | |
| JP2007232443A (en) | Inertia navigation system and its error correction method | |
| CN104776847B (en) | A kind of method that gyroscopic drift is estimated suitable for underwater navigation system single-point | |
| CN105300404A (en) | Calibration method for ship-referenced inertial navigation system | |
| CN104697520A (en) | Combined navigation method based on integrated gyroscope free strapdown inertial navigation system and GPS | |
| CN108151765B (en) | Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error | |
| CN112015086B (en) | Feedback control method for limited-time path tracking output of under-actuated surface ship | |
| CN110514200B (en) | Inertial navigation system and high-rotation-speed rotating body attitude measurement method | |
| CN111722295A (en) | Underwater strapdown gravity measurement data processing method | |
| CN103616026A (en) | AUV (Autonomous Underwater Vehicle) manipulating model auxiliary strapdown inertial navigation combined navigation method based on H infinity filtering | |
| CN112129285A (en) | Magnetic/inertial combination-based frogman navigation attitude estimation method under emergency condition | |
| CN106123923B (en) | A kind of inertial navigation system gyroscope drift correction method based on velocity aid | |
| CN105841717B (en) | A kind of Strapdown Inertial Navigation System course error rapid correction method | |
| Kaniewski et al. | Integrated system for heading determination | |
| CN111649744B (en) | Combined navigation positioning method based on dynamic model assistance | |
| TN et al. | Navigation of autonomous underwater vehicle using extended kalman filter | |
| Cho et al. | IMM-based INS/EM-log integrated underwater navigation with sea current estimation function | |
| CN110779551A (en) | Two-stage linear alignment on-line switching method based on additive quaternion | |
| Braga et al. | Navigation scheme for the LSTS SEACON vehicles: Theory and application |
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 | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20201225 |
|
| WD01 | Invention patent application deemed withdrawn after publication |