CN116255988B - 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
- CN116255988B CN116255988B CN202310529546.5A CN202310529546A CN116255988B CN 116255988 B CN116255988 B CN 116255988B CN 202310529546 A CN202310529546 A CN 202310529546A CN 116255988 B CN116255988 B CN 116255988B
- Authority
- CN
- China
- Prior art keywords
- ship
- interference
- model
- navigation
- wind wave
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 238000001914 filtration Methods 0.000 title claims abstract description 40
- 239000002131 composite material Substances 0.000 title claims abstract description 21
- 239000002245 particle Substances 0.000 claims abstract description 44
- 238000005259 measurement Methods 0.000 claims abstract description 42
- 230000010287 polarization Effects 0.000 claims abstract description 35
- 239000013598 vector Substances 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 28
- 230000026676 system process Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 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
- 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
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 errorThe method comprises the steps of carrying out a first treatment on the surface of the Wherein,,respectively represent 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.eEstablishing a combined navigation state modelWherein->For system state quantity->For the disturbance moment of the wind wave,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 +.>，/>The three-dimensional angular velocity of the ship measured by the gyroscope,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 observerWherein->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 updatedAnd 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 asThe 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 calculation of a three-dimensional attitude measurement model from polarization vectors, geomagnetic vectors, and solar vectorsA nonlinear function; />，/>Measurement noise for inertial navigation system, +.>Noise is measured for the three-dimensional pose.
Further, the step (3) includes:
disturbance moment of wind waveThe 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.
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 quantityThe 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 particlesFor 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 setsEach particle of (a) is combined with an interference amount particleAnd 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 systemFor 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 setResampling 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 errorThe method comprises the steps of carrying out a first treatment on the surface of the Which is a kind ofIn the process, ,respectively represent 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; />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).
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.eEstablishing a combined navigation state modelWherein->For system state quantity->For the disturbance moment of the wind wave,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 of the light,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 quantityThe 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 observerWherein->For the purpose of estimating the state quantity of the system for the interference,for interference estimation system quantity measurement, the following is specifically implemented:
disturbance moment of wind waveThe 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 quantityThe 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 particlesFor 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 setsEach particle of (a) is combined with an interference amount particleAnd 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 systemFor 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 setResampling 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.
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 errorThe method comprises the steps of carrying out a first treatment on the surface of the Wherein,,respectively represent 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.)>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 to which the ship is subjected,is a 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 +.>，/>For angular velocity measurement, +.>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 observerWherein->For the purpose of estimating the state quantity of the system for the 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 updatedAnd 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;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).
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 asThe 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 the polarization vectorCalculating a nonlinear function of the three-dimensional attitude measurement model by using the quantity, the geomagnetic vector and the solar vector; />，/>For the measurement of noise in an 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 waveThe 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 quantityThe 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 particlesFor 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 setsEach particle of (a) is combined with an interference amount particleAnd 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 systemFor each particleBy 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 setResampling 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 Repeating II to VI until the navigation task is finished.
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 CN116255988A (en) | 2023-06-13 |
| CN116255988B true 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) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113739795B (en) * | 2021-06-03 | 2023-10-20 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109556632A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering |
| CN110672131A (en) * | 2019-11-19 | 2020-01-10 | 北方工业大学 | A UKF alignment method for inertial/polarized light integrated navigation system under large misalignment angle |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101852615B (en) * | 2010-05-18 | 2013-01-23 | 南京航空航天大学 | Improved mixed Gaussian particle filtering method used in inertial integrated navigation system |
| FR2961897B1 (en) * | 2010-06-25 | 2012-07-13 | Thales Sa | NAVIGATION FILTER FOR A FIELD CORRELATION NAVIGATION SYSTEM |
| CN103528587B (en) * | 2013-10-15 | 2016-09-28 | 西北工业大学 | Independent combined navigation system |
| CN110579740B (en) * | 2019-09-17 | 2023-03-31 | 大连海事大学 | Unmanned ship integrated navigation method based on adaptive federal Kalman filtering |
| CN112212862A (en) * | 2020-09-24 | 2021-01-12 | 天津理工大学 | GPS/INS integrated navigation method for improving particle filtering |
| CN113819906B (en) * | 2021-09-23 | 2023-07-21 | 哈尔滨工程大学 | A Robust Filtering Method for Integrated Navigation Based on Statistical Similarity Measure |
| CN113834483B (en) * | 2021-11-23 | 2022-03-25 | 北京航空航天大学 | An Inertial/Polarization/Geomagnetic Fault Tolerant Navigation Method Based on Observability |
| CN115790580A (en) * | 2022-11-17 | 2023-03-14 | 中国直升机设计研究所 | Helicopter carrier landing relative navigation method based on variational Bayesian adaptive filtering |
| CN115855049B (en) * | 2023-02-07 | 2023-05-12 | 河海大学 | SINS/DVL navigation method based on particle swarm optimization robust filtering |
-
2023
- 2023-05-11 CN CN202310529546.5A patent/CN116255988B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109556632A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering |
| CN110672131A (en) * | 2019-11-19 | 2020-01-10 | 北方工业大学 | A UKF alignment method for inertial/polarized light integrated navigation system under large misalignment angle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116255988A (en) | 2023-06-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113252033B (en) | Positioning method, positioning system and robot based on multi-sensor fusion | |
| CN101572533B (en) | Design method of composite layered anti-interference filter | |
| CN107179693B (en) | Robust adaptive filtering and state estimation method based on Huber estimation | |
| CN102818567A (en) | AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering | |
| CN103776453A (en) | Combination navigation filtering method of multi-model underwater vehicle | |
| CN109375646A (en) | Autonomous Navigation Method for AUV Docking and Recovery Based on FMSRUPF Algorithm | |
| CN116255988B (en) | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation | |
| CN109579850A (en) | Deep water intelligent navigation method based on speed through water auxiliary inertial navigation | |
| CN103605886A (en) | Multi-model self-adaptive fusion filtering method of ship dynamic positioning system | |
| CN112556697A (en) | Shallow coupling data fusion navigation method based on federated structure | |
| CN104316025A (en) | System for estimating height of sea wave based on attitude information of ship | |
| CN108444479A (en) | Gravity Matching method based on ADAPTIVE ROBUST Unscented kalman filtering | |
| CN116772903A (en) | SINS/USBL installation angle estimation method based on iterative EKF | |
| Fernandes et al. | GNSS/MEMS-INS integration for drone navigation using EKF on lie groups | |
| CN106767928A (en) | A kind of self adaptation fast transfer alignment method | |
| Wang et al. | AUV navigation based on inertial navigation and acoustic positioning systems | |
| CN112710304A (en) | Underwater autonomous vehicle navigation method based on adaptive filtering | |
| Li et al. | Di-eme: Deep inertial ego-motion estimation for autonomous underwater vehicle | |
| CN115900708A (en) | Multi-sensor Fusion Localization Method for Robot Based on GPS-Guided Particle Filter | |
| Chen et al. | Improved maximum likelihood method for ship parameter identification | |
| CN110703205A (en) | Ultrashort baseline positioning method based on adaptive unscented Kalman filtering | |
| Zhang et al. | An underwater SINS/DVL integrated system outlier interference suppression method based on LSTM-EEWKF | |
| CN114543799B (en) | A robust federated Kalman filtering method, equipment and system | |
| CN112797977B (en) | Inertia/gravity matching combination method based on robust point group filtering | |
| Ma et al. | A Long-Term Underwater Robust Terrain Contour-Aided Navigation Method With a Single Beam Echo Sounder |
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 |