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 PDF

Info

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
Application number
CN202310529546.5A
Other languages
Chinese (zh)
Other versions
CN116255988A (en
Inventor
乔建忠
张洁
郭雷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202310529546.5A priority Critical patent/CN116255988B/en
Publication of CN116255988A publication Critical patent/CN116255988A/en
Application granted granted Critical
Publication of CN116255988B publication Critical patent/CN116255988B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation
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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,，/>，/>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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,，/>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, the liquid crystal display device comprises a liquid crystal display device,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 liquid crystal display device comprises a liquid crystal display device,，/>。
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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,，/>，/>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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,，/>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, the liquid crystal display device comprises a liquid crystal display device,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 liquid crystal display device comprises a liquid crystal display device,，/>。
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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,，/>，/>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, the liquid crystal display device comprises a liquid crystal display device,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, the liquid crystal display device comprises a liquid crystal display device,，/>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, the liquid crystal display device comprises a liquid crystal display device,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 liquid crystal display device comprises a liquid crystal display device,，/>；
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, the liquid crystal display device comprises a liquid crystal display device,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.
CN202310529546.5A 2023-05-11 2023-05-11 Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation Active CN116255988B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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 北方工业大学 UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle

Family Cites Families (9)

* Cited by examiner, † Cited by third party
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 哈尔滨工程大学 Combined navigation robust filtering method based on statistical similarity measurement
CN113834483B (en) * 2021-11-23 2022-03-25 北京航空航天大学 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
CN115855049B (en) * 2023-02-07 2023-05-12 河海大学 SINS/DVL navigation method based on particle swarm optimization robust filtering

Patent Citations (2)

* Cited by examiner, † Cited by third party
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 北方工业大学 UKF (unscented Kalman Filter) 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
CN116255988B (en) Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation
CN113252033B (en) Positioning method, positioning system and robot based on multi-sensor fusion
Li et al. Underwater terrain-aided navigation system based on combination matching algorithm
CN112710304B (en) Underwater autonomous vehicle navigation method based on adaptive filtering
CN112665557B (en) Wave data processing method and device, electronic equipment and readable storage medium
CN109579850A (en) Deep water intelligent navigation method based on speed through water auxiliary inertial navigation
CN104316025A (en) System for estimating height of sea wave based on attitude information of ship
CN112556697A (en) Shallow coupling data fusion navigation method based on federated structure
Zhou et al. Terrain aided navigation for autonomous underwater vehicles with coarse maps
Wang et al. Lever arm compensation of autonomous underwater vehicle for fast transfer alignment
Zhang et al. A self-contained interactive iteration positioning and orientation coupled navigation method based on skylight polarization
CN110703205A (en) Ultrashort baseline positioning method based on adaptive unscented Kalman filtering
Chen et al. Improved maximum likelihood method for ship parameter identification
Qian et al. An INS/DVL integrated navigation filtering method against complex underwater environment
Wang et al. AUV navigation based on inertial navigation and acoustic positioning systems
Ccolque-Churquipa et al. Implementation of a Measurement System for the Attitude, Heading and Position of a USV Using IMUs and GPS
CN115014321B (en) Bionic polarization multisource fusion orientation method based on adaptive robust filtering
Li et al. Adaptively robust filtering algorithm for maritime celestial navigation
Li et al. Marginalized unscented quaternion estimator for integrated INS/GPS
CN116222582B (en) Multi-physical-field self-adaptive combined navigation method based on variable decibel leaf-based inference
Zhang et al. An underwater SINS/DVL integrated system outlier interference suppression method based on LSTM-EEWKF
Li et al. Novel terrain integrated navigation system using neural network aided Kalman filter
Wu et al. Autonomous underwater vehicles navigation method based on ultra short base line and dead reckoning
Hinostroza et al. Nonparametric estimation of directional wave spectra using two hyperparameters
Huang et al. A novel algorithm of improved cubature unscented kalman filter based on the model of magnetometer for underwater glider navigation system

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