CN116255988A - Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation - Google Patents
Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation Download PDFInfo
- Publication number
- CN116255988A CN116255988A CN202310529546.5A CN202310529546A CN116255988A CN 116255988 A CN116255988 A CN 116255988A CN 202310529546 A CN202310529546 A CN 202310529546A CN 116255988 A CN116255988 A CN 116255988A
- Authority
- CN
- China
- Prior art keywords
- ship
- interference
- navigation
- model
- representing
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000001914 filtration Methods 0.000 title claims abstract description 40
- 239000002131 composite material Substances 0.000 title claims abstract description 21
- 238000005259 measurement Methods 0.000 claims abstract description 41
- 239000002245 particle Substances 0.000 claims abstract description 40
- 230000010287 polarization Effects 0.000 claims abstract description 34
- 239000011159 matrix material Substances 0.000 claims description 28
- 230000026676 system process Effects 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000012952 Resampling Methods 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 2
- 238000011160 research Methods 0.000 description 2
- 230000003416 augmentation Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000000007 visual effect 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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
Abstract
The invention relates to a composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation. Aiming at the situation that a ship-based inertial/polarization/geomagnetic integrated navigation system faces multi-source complex unknown interference when a task is executed in unfamiliar open sea, firstly, a ship dynamics model is established by considering multi-source interference such as stormy waves, weather uncertainty and the like; secondly, based on the selected ship state quantity, establishing a combined navigation state model and a three-dimensional attitude measurement model; thirdly, aiming at the wind wave interference with harmonic characteristics, a wind wave interference model is built, and a combined navigation state model is combined to build a wind wave interference observer; finally, aiming at the obvious nonlinear non-Gaussian characteristic of the system model, a particle filtering algorithm is adopted to estimate the state quantity of the integrated navigation system from the statistical perspective, and the self-adaptive composite anti-interference navigation strategy is realized. The method has the advantages of strong anti-interference capability, high control precision and the like.
Description
Technical Field
The invention belongs to the field of anti-interference navigation of moving bodies, and particularly relates to a composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation.
Background
At present, the major economic body of the world disputes the attention point to the ocean, and the exploration of the deep sea in the strange open sea becomes a new engine for technological development innovation. However, the sea condition of the unfamiliar sea area is complex and unknown, the existing navigation method and the navigation model have insufficient consideration on the external environment interference, and the navigation reliability and the self-adaptability requirements of the ship under the complex sea condition cannot be met. In the existing researches, for example, chinese patent application (CN 113834483 a) "an inertial/polarization/geomagnetic fault-tolerant navigation method based on an observable measure" processes the interference of an integrated navigation system by adopting a federal filtering method ", chinese patent application (CN 114459474 a)" an inertial/polarization/radar/optofluidic integrated navigation method based on a factor graph "processes the filtering by adopting a measurement augmentation method", and the above methods all consider the external interference as gaussian noise in measurement for processing, and the integrated navigation system modeling cannot realize direct expansion modeling on the external interference, and is insufficient in noise consideration. The Chinese patent application (CN 113739795A) discloses an inertial/polarized navigation method based on polarization and solar double-vector switching, which adopts a Kalman filtering method when combined navigation filtering is carried out, and the Chinese patent application (CN 113819907B) discloses an underwater synchronous positioning and mapping method based on polarized light/inertial/visual combined navigation, which adopts unscented Kalman filtering. In summary, the existing research does not combine the dynamics of the moving body in the modeling aspect, the model has insufficient expansibility to external interference, and meanwhile, the filtering method still needs to be further improved aiming at the nonlinear characteristic of the integrated navigation system.
Disclosure of Invention
In order to solve the technical problems, the invention provides a composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation. The invention combines the ship dynamics, builds a more widely applicable integrated navigation model, does not need to increase the hardware cost additionally, and effectively improves the reliability and redundancy of the integrated navigation system. Meanwhile, an interference observer is designed to estimate the wind wave interference independently, the estimation precision of process noise in filtering is improved, and the real-time estimation of non-Gaussian characteristic noise is solved by adopting particle filtering pertinently, so that the navigation precision and the robustness are improved greatly. According to the invention, the wind wave interference is independently estimated through the interference observer, and the particle filtering is adopted to carry out real-time self-adaptive estimation on the non-Gaussian characteristic interference, so that the full-autonomous reliable navigation of the ship under the complex sea condition is realized.
In order to achieve the above purpose, the invention adopts the following technical scheme:
a composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation comprises the following steps:
step (1), aiming at the condition that the ship-based inertial/polarization/geomagnetic combined navigation system has various uncertainty interference in offshore navigation, based on ship dynamics, a ship dynamics model is established by considering multi-source composite interference including wind wave interference, sea condition and weather uncertainty interference and system model error
； wherein ,/>
Respectively representing the roll angle, the pitch angle and the course angle of the ship, namely the three-dimensional attitude angle of the ship,
respectively representing the three-axis angular velocity of the ship; />
Step (2), selecting a ship three-dimensional attitude angle based on the ship dynamics model established in the step (1)
Triaxial angular velocity with ship>
Is a state quantity, i.e
Establishing a combined navigation state model
, wherein ,/>
For system state quantity->
Is wind wave disturbance moment->
For sea state and weather uncertainty related moment, < +.>
For other moment sums to which the ship is subjected,
process noise of the system; in the measuring part, the inertial navigation system is used for measuring the triaxial angular velocity of the ship
Measuring and establishing an angular velocity measuring model +.>
，/>
Three-dimensional angular velocity of ship measured for gyroscopes, < >>
The method is characterized in that the method is used for measuring noise of an inertial navigation system, and simultaneously, polarization navigation system and geomagnetic navigation system are utilized to realize three-dimensional attitude angle of the ship>
Is used for establishing a three-dimensional posture measurement model>
, wherein ,/>
For the measured polarization vector, +.>
For the measured geomagnetic vector, +.>
For sun vector under navigation system, +.>
Is geomagnetic vector under navigation system, +.>
Measuring noise for the three-dimensional gesture; />
Representing a nonlinear function of a three-dimensional attitude measurement model calculated by a polarization vector, a geomagnetic vector and a solar vector;
step (3), constructing a wind wave interference model aiming at the wind wave interference with harmonic characteristics, and combining the combined navigation state model in the step (2) to construct a wind wave interference observer
, wherein ,/>
Estimating a system state quantity for interference, < >>
Measuring system quantity for interference estimation;
step (4), constructing a particle filter by considering that sea conditions and weather uncertainty have obvious nonlinear non-Gaussian characteristics, and combining the storm interference observer in the step (3) when the measurement information of the combined navigation system is updated
And estimating the wind wave interference, and estimating navigation parameters of the combined navigation system model by using a particle filter, so as to realize self-adaptive composite anti-interference navigation.
Further, the step (1) includes:
the ship dynamics model is established as follows:
wherein ,
，/>
，/>
respectively representing three-axis moments of inertia of the ship under the body coordinate system; />
Inverting the matrix; />
Disturbance moment acting on the ship for wind wave disturbance, +.>
，
，/>
Respectively representing the components of the storm disturbance on three coordinate axes, < ->
A transpose operation representing a vector or matrix; />
Disturbance moment introduced for sea conditions and weather uncertainty,
，/>
，/>
the components of sea conditions and weather uncertainty interference on three coordinate axes are respectively represented;
for other moments to which the ship is subjected, +.>
，/>
，/>
Representing its components on three coordinate axes; />
For the process noise of the system->
，/>
，/>
Representing the components of the system process noise on three axes, satisfying +.>
， />
Mean value of +.>
Variance is->
Is a normal distribution of (c).
Further, the step (2) includes:
sorting and discretizing the ship dynamics model established in the step (1), and selecting the ship state quantity as
The combined navigation state equation is obtained, specifically:
wherein ,
is a nonlinear conversion relation in a ship dynamics system,
is a ship moment of inertia correlation matrix, wherein +.>
Is a three-dimensional diagonal matrix>
，/>
Representation->
Summation operation from 1 to N; />
，/>
，
The method comprises the steps of carrying out a first treatment on the surface of the Subscript x, y, z represents the coordinate system triaxial;
the measurement model of the inertial/polarization/geomagnetic integrated navigation system is organized into:
wherein ,
，/>
representing the measurement value of the ship-based inertial/polarization/geomagnetic integrated navigation system; />
Indicating the angular velocity measurement of the ship->
Representing three-dimensional attitude measurement of the ship; />
，
Three-dimensional angular velocity of ship measured for gyroscopes, < >>
Representing a nonlinear function of a three-dimensional attitude measurement model calculated by a polarization vector, a geomagnetic vector and a solar vector; />
，/>
Measurement noise for inertial navigation system, +.>
Noise is measured for the three-dimensional pose.
Further, the step (3) includes:
disturbance moment of wind wave
The time evolution characteristic of (a) is modeled as:
wherein ,
for interfering with the systemStatus quantity (I)>
，/>
，
Representing the three-axis component of the state quantity of the interference system in the machine body coordinate system, wherein the initial value of the three-axis component satisfies +.>
，
Representing a normal distribution>
A variance matrix for representing the initial value of the state quantity of the interference system; />
To interfere with system process noise, satisfy->
，/>
A variance matrix representing interference system process noise; />
and />
Is a parameter matrix related to ship attitude;
combining the wind wave interference model and the inertial/polarization/geomagnetic integrated navigation system state model in the step (2), designing a wind wave interference observer as follows:
wherein ,
，/>
。
the wind wave interference observer is a linear system, and is processed by adopting a classical Kalman filtering method in the combined navigation calculation.
Further, the step (4) includes: according to the step (1), the step (2) and the step (3), the ship integrated navigation system model is obtained as follows:
the method for carrying out filtering estimation on the ship-borne inertial/polarization/geomagnetic integrated navigation system by adopting a particle filtering method comprises the following steps:
I. initializing: generating initial particles for initial state quantity
The posterior probability density function is set as:
，/>
the method comprises the steps of carrying out a first treatment on the surface of the Based on->
Generating a set of particles
For the initial wind wave interference, the +.>
Error covariance corresponding to wind wave interference>
Wherein the upper right "+" indicates the corresponding posterior;
II, predicting the system state: for particle sets
Is combined with interference amount particles->
And carrying out prior prediction estimation according to a system state equation, namely:
wherein the right superscript "-" indicates the corresponding a priori pre-measurement;
III, estimating wind wave interference: the wind wave interference observer is utilized, the Kalman filtering is adopted to estimate the wind wave interference, and the specific calculation steps are as follows:
wherein ,
representing a covariance matrix of a wind wave interference system state error; />
Representing a filtering gain matrix of the wind wave interference system;
calculating particle weights: obtaining measurement value of ship-based inertial/polarization/geomagnetic integrated navigation system
For each particle->
By measuring value->
Calculating a likelihood probability density function for each particle for the condition:
；
and V, weight normalization:
representing a likelihood probability density function of the particles after weight normalization;
and VI, resampling: using the obtained normalized likelihood probability density function to make the prior particle set
Resampling to obtain posterior particle set (I) fused with measurement information>
The method comprises the steps of carrying out a first treatment on the surface of the Further repeating II to VI until the navigation task ends.
Compared with the prior art, the invention has the following advantages:
in the ship-based navigation calculation process, a framework which is widely applicable is built by combining with ship dynamics, the external interference factors can be modeled in a more direct and simple mode while the measurement data of the integrated navigation system are effectively utilized, the additional hardware cost is not required to be increased, and the redundancy and the reliability of the integrated navigation system are improved. The wind wave interference with harmonic characteristics is estimated by designing the interference observer, so that the estimation precision of process noise in filtering is improved, meanwhile, the adopted particle filtering algorithm well meets a nonlinear system, has strong self-adaptability, and further greatly improves the navigation precision of ships.
Drawings
FIG. 1 is a flow chart of a composite anti-interference adaptive filtering method based on ship dynamics combined navigation.
Detailed Description
The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the present invention, but not all embodiments, and all other embodiments obtained by those skilled in the art without the inventive effort based on the embodiments of the present invention are within the scope of protection of the present invention.
The method is suitable for the full-autonomous navigation task of the ship under the GNSS refusing or failure condition in unknown sea conditions in unfamiliar sea areas.
According to one embodiment of the invention, all subsystems in the integrated navigation system need to finish calibration alignment in advance, and a coordinate system calibration matrix of each sensor is calculated, so that all subsystems are unified under the integrated navigation system body coordinate system for resolving.
As shown in FIG. 1, the composite anti-interference self-adaptive filtering method based on the ship dynamics combined navigation comprises the following specific implementation steps:
aiming at the condition that the ship-based inertial/polarization/geomagnetic integrated navigation system has various uncertainty interference in offshore navigation, the method establishes a ship dynamics model based on ship dynamics by considering multi-source composite interference including wind wave interference, sea condition and weather uncertainty interference and system model error
； wherein ,/>
Respectively representing the roll angle, the pitch angle and the course angle of the ship, namely the three-dimensional attitude angle of the ship,
the three-axis angular velocity of the ship is respectively represented, and the method is concretely realized as follows:
aiming at the multi-source composite interference of wind and wave interference, sea condition and weather uncertainty interference, system model error and the like of a ship navigation in an unfamiliar sea area, a ship-based inertia/polarization/geomagnetic combined navigation system is established, and a ship dynamics model is established:
wherein ,
，/>
，/>
respectively representing three-axis moments of inertia of the ship under the body coordinate system; />
Inverting the matrix; />
The disturbance moment acting on the ship is disturbed by wind waves,
，/>
，/>
respectively representing the components of the storm disturbance on three coordinate axes, < ->
A transpose operation representing a vector or matrix; />
Disturbance moment introduced for sea conditions and weather uncertainty, +.>
，/>
，/>
The components of sea conditions and weather uncertainty interference on three coordinate axes are respectively represented;
for other moments to which the ship is subjected, +.>
，/>
，/>
Representing its components on three coordinate axes; />
For the process noise of the system->
，/>
，
Representing the components of the system process noise on three axes, satisfying +.>
，/>
Mean value of +.>
Variance is->
Is a normal distribution of (c).
Step (2) selecting a three-dimensional attitude angle of the ship based on the ship dynamics model established in the step (1)
Triaxial angular velocity with ship>
Is a state quantity, i.e
Building a combined navigation state model->
, wherein ,/>
For system state quantity->
Is wind wave disturbance moment->
For sea state and weather uncertainty related moment, < +.>
For other moment sums, which are experienced by the ship, +.>
Process noise of the system; in the measuring part, the angular velocity of the ship is measured by using an inertial navigation system, and an angular velocity measuring model is established>
，/>
Three-dimensional angular velocity of ship measured for gyroscopes, < >>
For measuring noise of an inertial navigation system, measuring three-dimensional attitude angles of ships and warships is realized by utilizing a polarization navigation system and a geomagnetic navigation system, and a three-dimensional attitude measurement model is built>
, wherein ,/>
For the measured polarization vector, +.>
For the measured geomagnetic vector, +.>
For sun vector under navigation system, +.>
Is geomagnetic vector under navigation system, +.>
Measuring noise for the three-dimensional gesture; />
The nonlinear function representing the three-dimensional attitude measurement model calculated by the polarization vector, the geomagnetic vector and the solar vector is specifically realized as follows:
sorting and discretizing the ship dynamics model established in the step (1), and selecting the ship state quantity
The combined navigation state equation is obtained, specifically:
wherein ,
is a nonlinear conversion relation in a ship dynamics system,
is a ship moment of inertia correlation matrix, wherein +.>
Is a three-dimensional diagonal matrix>
，/>
Representation->
Summation operation from 1 to N; />
， />
，
The subscripts x, y, z denote the three axes of the coordinate system.
The measurement model of the inertial/polarization/geomagnetic integrated navigation system is organized into:
wherein ,
，/>
representing the measurement value of the ship-based inertial/polarization/geomagnetic integrated navigation system; />
Indicating the angular velocity measurement of the ship->
Representing three-dimensional attitude measurement of the ship; />
，/>
Three-dimensional angular velocity of ship measured for gyroscopes, < >>
Representing a nonlinear function of a three-dimensional attitude measurement model calculated by a polarization vector, a geomagnetic vector and a solar vector; />
，/>
Measurement noise for inertial navigation system, +.>
Noise is measured for the three-dimensional pose.
Step (3) constructing a wind wave interference model aiming at the wind wave interference with harmonic characteristics, and combining the combined navigation state model in the step (2) to construct a wind wave interference observer
, wherein ,/>
Estimating a system state quantity for interference, < >>
For interference estimation system quantity measurement, the following is specifically implemented:
disturbance moment of wind wave
The time evolution characteristic of (a) is modeled as:
wherein ,
for disturbing the state quantity of the system +.>
，/>
，
Representing the three-axis component of the state quantity of the interference system in the machine body coordinate system, wherein the initial value of the three-axis component satisfies +.>
，
Representing a normal distribution>
A variance matrix for representing the initial value of the state quantity of the interference system; />
To interfere with system process noise, satisfy->
，/>
A variance matrix representing interference system process noise; />
and />
Is a parameter matrix related to ship attitude;
combining the wind wave interference model and the inertial/polarization/geomagnetic integrated navigation system state model in the step (2), designing a wind wave interference observer as follows:
wherein ,
，/>
。
the wind wave interference observer is a linear system, and is processed by adopting a classical Kalman filtering method in the combined navigation calculation.
In the step (4), a particle filter is constructed in consideration of the obvious nonlinear non-Gaussian characteristic of sea conditions and weather uncertainty, when the measurement information of the integrated navigation system is updated, the wind wave interference is estimated by combining the wind wave interference observer in the step (3), and then the navigation parameters of the nonlinear model of the integrated navigation system are estimated by using the particle filter, so that the self-adaptive composite anti-interference navigation is realized, and the specific method is as follows:
according to the step (1), the step (2) and the step (3), the ship integrated navigation system model is obtained as follows:
and (3) carrying out filtering estimation on the ship-borne inertia/polarization/geomagnetic integrated navigation system by adopting a particle filtering method:
I. initializing: generating initial particles for initial state quantity
The posterior probability density function is set as:
，/>
the method comprises the steps of carrying out a first treatment on the surface of the Based on->
Generating a set of particles
For the initial wind wave interference, the +.>
Error covariance corresponding to wind wave interference>
Wherein the upper right "+" indicates the corresponding posterior;
II, predicting the system state: for particle sets
Each particle of (a) is combined with an interference amount particle
And carrying out prior prediction estimation according to a system state equation, namely:
wherein the right superscript "-" indicates the corresponding a priori pre-measurement;
III, estimating wind wave interference: the wind wave interference observer is utilized, the Kalman filtering is adopted to estimate the wind wave interference, and the specific calculation steps are as follows:
wherein ,
representing a covariance matrix of a wind wave interference system state error; />
Representing the filtering gain matrix of the wind wave interference system.
Calculating particle weights: obtaining measurement value of ship-based inertial/polarization/geomagnetic integrated navigation system
For each particle->
By measuring value->
Calculating a likelihood probability density function for each particle for the condition:
；/>
and V, weight normalization:
representing a likelihood probability density function of the particles after weight normalization;
and VI, resampling: using the obtained normalized likelihood probability density function to make the prior particle set
Resampling to obtain a meltPosterior particle set combined with measurement information +.>
The method comprises the steps of carrying out a first treatment on the surface of the Further repeating II to VI until the navigation task ends.
While the foregoing has been described in relation to illustrative embodiments thereof, so as to facilitate the understanding of the present invention by those skilled in the art, it should be understood that the present invention is not limited to the scope of the embodiments, but is to be construed as limited to the spirit and scope of the invention as defined and defined by the appended claims, as long as various changes are apparent to those skilled in the art, all within the scope of which the invention is defined by the appended claims.
Claims (5)
1. The composite anti-interference self-adaptive filtering method based on the ship dynamics combined navigation is characterized by comprising the following steps of:
step (1), aiming at the condition that the ship-based inertial/polarization/geomagnetic combined navigation system has various uncertainty interference in offshore navigation, based on ship dynamics, a ship dynamics model is established by considering multi-source composite interference including wind wave interference, sea condition and weather uncertainty interference and system model error
； wherein ,/>
Respectively representing the roll angle, the pitch angle and the course angle of the ship, namely the three-dimensional attitude angle of the ship,
respectively representing the three-axis angular velocity of the ship;
step (2), selecting a ship three-dimensional attitude angle based on the ship dynamics model established in the step (1)
Triaxial angular velocity with ship>
Is a state quantity, i.e
Establishing a combined navigation state model
, wherein ,/>
For system state quantity->
Is wind wave disturbance moment->
For sea state and weather uncertainty related moment, < +.>
For other moment sums, which are experienced by the ship, +.>
Process noise of the system; in the measuring part, the inertial navigation system is used for carrying out +.>
Measuring and establishing an angular velocity measuring model +.>
，/>
Three-dimensional angular velocity of ship measured for gyroscopes, < >>
Measuring noise for inertial navigation system and simultaneously utilizing polarized navigation systemRealizing three-dimensional attitude angle of ship with geomagnetic navigation system>
Is used for establishing a three-dimensional attitude measurement model
, wherein ,/>
For the measured polarization vector, +.>
For the measured geomagnetic vector, +.>
For sun vector under navigation system, +.>
Is geomagnetic vector under navigation system, +.>
Measuring noise for the three-dimensional gesture; />
Representing a nonlinear function of a three-dimensional attitude measurement model calculated by a polarization vector, a geomagnetic vector and a solar vector;
step (3), constructing a wind wave interference model aiming at the wind wave interference with harmonic characteristics, and combining the combined navigation state model in the step (2) to construct a wind wave interference observer
, wherein ,/>
Estimating a system state quantity for interference, < >>
Measuring system quantity for interference estimation;
step (4), constructing a particle filter by considering that sea conditions and weather uncertainty have obvious nonlinear non-Gaussian characteristics, and combining the storm interference observer in the step (3) when the measurement information of the combined navigation system is updated
And estimating the wind wave interference, and estimating navigation parameters of the combined navigation system model by using a particle filter, so as to realize self-adaptive composite anti-interference navigation.
2. The ship dynamics combination navigation based composite anti-interference adaptive filtering method according to claim 1, which is characterized by comprising the following steps:
the step (1) comprises:
the ship dynamics model is established as follows:
wherein ,
，/>
，/>
respectively representing three-axis moments of inertia of the ship under the body coordinate system; />
Inverting the matrix;
the disturbance moment acting on the ship is disturbed by wind waves,
，/>
，/>
respectively representing the components of the storm disturbance on three coordinate axes, < ->
A transpose operation representing a vector or matrix; />
Disturbance moment introduced for sea conditions and weather uncertainty, +.>
，/>
，/>
The components of sea conditions and weather uncertainty interference on three coordinate axes are respectively represented; />
For other moments to which the ship is subjected, +.>
，/>
，
Representing its components on three coordinate axes; />
Is a systemProcess noise of->
，/>
，/>
Representing the components of system process noise on three coordinate axes, meeting
，/>
Mean value of +.>
Variance is->
Is a normal distribution of (c).
3. The ship dynamics combination navigation based composite anti-interference adaptive filtering method according to claim 2, which is characterized in that:
the step (2) comprises:
sorting and discretizing the ship dynamics model established in the step (1), and selecting the ship state quantity as
The combined navigation state equation is obtained, specifically:
wherein ,
is a ship power systemThe nonlinear conversion relationship in the system,
is a ship moment of inertia correlation matrix, wherein +.>
Is a three-dimensional diagonal matrix>
，/>
Representation->
Summation operation from 1 to N; />
，/>
，
The method comprises the steps of carrying out a first treatment on the surface of the Subscript x, y, z represents the coordinate system triaxial;
the measurement model of the inertial/polarization/geomagnetic integrated navigation system is organized into:
wherein ,
，/>
representing the measurement value of the ship-based inertial/polarization/geomagnetic integrated navigation system; />
Indicating the angular velocity measurement of the ship->
Representing three-dimensional attitude measurement of the ship; />
，
Three-dimensional angular velocity of ship measured for gyroscopes, < >>
Representing a nonlinear function of a three-dimensional attitude measurement model calculated by a polarization vector, a geomagnetic vector and a solar vector; />
，/>
Measurement noise for inertial navigation system, +.>
Noise is measured for the three-dimensional pose.
4. The ship dynamics combination navigation based composite anti-interference adaptive filtering method according to claim 3, wherein the method is characterized by comprising the following steps of:
the step (3) comprises:
disturbance moment of wind wave
The time evolution characteristic of (a) is modeled as:
wherein ,
for disturbing the state quantity of the system +.>
，/>
，/>
Representing the three-axis component of the state quantity of the interference system in the machine body coordinate system, wherein the initial value of the three-axis component satisfies +.>
，/>
Representing a normal distribution>
A variance matrix for representing the initial value of the state quantity of the interference system; />
To interfere with system process noise, satisfy
，/>
A variance matrix representing interference system process noise; />
and />
Is a parameter matrix related to ship attitude;
combining the wind wave interference model and the inertial/polarization/geomagnetic integrated navigation system state model in the step (2), designing a wind wave interference observer as follows:
wherein ,
，/>
；
the wind wave interference observer is a linear system, and is processed by adopting a classical Kalman filtering method in the combined navigation calculation.
5. The ship dynamics combination navigation based composite anti-interference adaptive filtering method according to claim 4, which is characterized in that:
the step (4) comprises: according to the step (1), the step (2) and the step (3), the ship integrated navigation system model is obtained as follows:
the method for carrying out filtering estimation on the ship-borne inertial/polarization/geomagnetic integrated navigation system by adopting a particle filtering method comprises the following steps:
I. initializing: generating initial particles for initial state quantity
The posterior probability density function is set as:
，/>
the method comprises the steps of carrying out a first treatment on the surface of the Based on->
Generating a set of particles
For the initial wind wave interference, the +.>
Error covariance corresponding to wind wave interference>
Wherein the upper right "+" indicates the corresponding posterior;
II, predicting the system state: for particle sets
Is combined with interference amount particles->
And carrying out prior prediction estimation according to a system state equation, namely:
wherein the right superscript "-" indicates the corresponding a priori pre-measurement;
III, estimating wind wave interference: the wind wave interference observer is utilized, the Kalman filtering is adopted to estimate the wind wave interference, and the specific calculation steps are as follows:
wherein ,
representing a covariance matrix of a wind wave interference system state error; />
Representing a filtering gain matrix of the wind wave interference system;
calculating particle weights: obtaining measurement value of ship-based inertial/polarization/geomagnetic integrated navigation system
For each particle
By measuring value->
Calculating a likelihood probability density function for each particle for the condition:
；
and V, weight normalization:
representing a likelihood probability density function of the particles after weight normalization;
and VI, resampling: using the obtained normalized likelihood probability density function to make the prior particle set
Resampling to obtain posterior particle set (I) fused with measurement information>
The method comprises the steps of carrying out a first treatment on the surface of the Further repeating II to VI until the navigation task ends. />
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310529546.5A CN116255988B (en) | 2023-05-11 | 2023-05-11 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310529546.5A CN116255988B (en) | 2023-05-11 | 2023-05-11 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116255988A true CN116255988A (en) | 2023-06-13 |
| CN116255988B CN116255988B (en) | 2023-07-21 |
Family
ID=86688297
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310529546.5A Active CN116255988B (en) | 2023-05-11 | 2023-05-11 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116255988B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113739795A (en) * | 2021-06-03 | 2021-12-03 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101852615A (en) * | 2010-05-18 | 2010-10-06 | 南京航空航天大学 | Improved mixed Gaussian particle filtering method used in inertial integrated navigation system |
| US20120022784A1 (en) * | 2010-06-25 | 2012-01-26 | Thales | Navigation filter for a navigation system using terrain correlation |
| CN103528587A (en) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | Autonomous integrated navigation system |
| CN109556632A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering |
| CN110579740A (en) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | unmanned ship integrated navigation method based on adaptive federal Kalman filtering |
| CN110672131A (en) * | 2019-11-19 | 2020-01-10 | 北方工业大学 | UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle |
| CN112212862A (en) * | 2020-09-24 | 2021-01-12 | 天津理工大学 | GPS/INS integrated navigation method for improving particle filtering |
| CN113834483A (en) * | 2021-11-23 | 2021-12-24 | 北京航空航天大学 | Inertial/polarization/geomagnetic fault-tolerant navigation method based on observability degree |
| CN115790580A (en) * | 2022-11-17 | 2023-03-14 | 中国直升机设计研究所 | Helicopter carrier landing relative navigation method based on variational Bayesian adaptive filtering |
| CN115855049A (en) * | 2023-02-07 | 2023-03-28 | 河海大学 | SINS/DVL navigation method based on particle swarm optimization robust filtering |
| WO2023045357A1 (en) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | Integrated navigation robust filtering method based on statistical similarity measurement |
-
2023
- 2023-05-11 CN CN202310529546.5A patent/CN116255988B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101852615A (en) * | 2010-05-18 | 2010-10-06 | 南京航空航天大学 | Improved mixed Gaussian particle filtering method used in inertial integrated navigation system |
| US20120022784A1 (en) * | 2010-06-25 | 2012-01-26 | Thales | Navigation filter for a navigation system using terrain correlation |
| CN103528587A (en) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | Autonomous integrated navigation system |
| CN109556632A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering |
| CN110579740A (en) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | unmanned ship integrated navigation method based on adaptive federal Kalman filtering |
| CN110672131A (en) * | 2019-11-19 | 2020-01-10 | 北方工业大学 | UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle |
| CN112212862A (en) * | 2020-09-24 | 2021-01-12 | 天津理工大学 | GPS/INS integrated navigation method for improving particle filtering |
| WO2023045357A1 (en) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | Integrated navigation robust filtering method based on statistical similarity measurement |
| CN113834483A (en) * | 2021-11-23 | 2021-12-24 | 北京航空航天大学 | Inertial/polarization/geomagnetic fault-tolerant navigation method based on observability degree |
| CN115790580A (en) * | 2022-11-17 | 2023-03-14 | 中国直升机设计研究所 | Helicopter carrier landing relative navigation method based on variational Bayesian adaptive filtering |
| CN115855049A (en) * | 2023-02-07 | 2023-03-28 | 河海大学 | SINS/DVL navigation method based on particle swarm optimization robust filtering |
Non-Patent Citations (3)
| Title |
|---|
| 张洁等: "A novel anti-swing initial alignment method based on polarization/geomagnetic fusion for ships working on unfamiliar waters", 《2022 5TH INTERNATIONAL SYMPOSIUM ON AUTONOMOUS SYSTEMS》 * |
| 林爱军等: "惯性/星光/天体矢量粒子滤波组合导航方法", 《航空计算技术》, no. 06 * |
| 薛丽等: "自适应高斯粒子滤波在导航中的应用", 《计算机仿真》, no. 01 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113739795A (en) * | 2021-06-03 | 2021-12-03 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation |
| CN113739795B (en) * | 2021-06-03 | 2023-10-20 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116255988B (en) | 2023-07-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107314768B (en) | Underwater terrain matching auxiliary inertial navigation positioning method and positioning system thereof | |
| CN108197350B (en) | Unmanned ship speed and uncertainty estimation system and design method | |
| CN116255988B (en) | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation | |
| Li et al. | Underwater terrain-aided navigation system based on combination matching algorithm | |
| CN109579850A (en) | Deep water intelligent navigation method based on speed through water auxiliary inertial navigation | |
| CN112710304B (en) | Underwater autonomous vehicle navigation method based on adaptive filtering | |
| CN104316025A (en) | System for estimating height of sea wave based on attitude information of ship | |
| CN112665557B (en) | Wave data processing method and device, electronic equipment and readable storage medium | |
| CN112556697A (en) | Shallow coupling data fusion navigation method based on federated structure | |
| CN110703205A (en) | Ultrashort baseline positioning method based on adaptive unscented Kalman filtering | |
| Chen et al. | Improved maximum likelihood method for ship parameter identification | |
| Wang et al. | AUV navigation based on inertial navigation and acoustic positioning systems | |
| Fernandes et al. | GNSS/MEMS-INS integration for drone navigation using EKF on lie groups | |
| CN115014321B (en) | Bionic polarization multisource fusion orientation method based on adaptive robust filtering | |
| Zhang et al. | An underwater SINS/DVL integrated system outlier interference suppression method based on LSTM-EEWKF | |
| CN117113798A (en) | Drift velocity prediction method, device, equipment and storage medium for floating object | |
| CN116222582B (en) | Multi-physical-field self-adaptive combined navigation method based on variable decibel leaf-based inference | |
| CN113483769A (en) | Particle filter based vehicle self-positioning method, system, device and medium | |
| Wu et al. | Autonomous underwater vehicles navigation method based on ultra short base line and dead reckoning | |
| Li et al. | Novel terrain integrated navigation system using neural network aided Kalman filter | |
| Liu et al. | A long short term memory network-based, global navigation satellite system/inertial navigation system for unmanned surface vessels | |
| Lin et al. | An adaptive square-root unscented Kalman filter for underwater vehicle navigation | |
| CN114370879B (en) | AUV robust VBHIAKF-SLAM navigation method based on underwater environment characteristics | |
| Pan et al. | AUV Tightly Coupled Terrain Aided Navigation Strategy Based on Isogonal MBES Modeling Method | |
| Li et al. | DI-EME: Deep Inertial Ego-Motion Estimation for Autonomous Underwater Vehicle |
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 |