CN116380067A - Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment - Google Patents

Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment Download PDF

Info

Publication number
CN116380067A
CN116380067A CN202310215059.1A CN202310215059A CN116380067A CN 116380067 A CN116380067 A CN 116380067A CN 202310215059 A CN202310215059 A CN 202310215059A CN 116380067 A CN116380067 A CN 116380067A
Authority
CN
China
Prior art keywords
error
navigation
estimation
representing
function
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310215059.1A
Other languages
Chinese (zh)
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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN202310215059.1A priority Critical patent/CN116380067A/en
Publication of CN116380067A publication Critical patent/CN116380067A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/185—Compensation of inertial measurements, e.g. for temperature effects for gravity
    • 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
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention provides an unmanned ship rotation modulation inertial navigation system and a method suitable for a challenging environment, wherein the system comprises a micro-electromechanical inertial unit MEMS-IMU, a servo motor, a servo driver, a single-shaft rotating table, a movable code ring, a navigation positioning resolving board card, an aviation plug and a shell frame; the method comprises the following steps: establishing an iterative volume Kalman filter based on a maximum posterior estimation idea; designing a filter to simplify an iterative updating structure; the introduction of a penalty weight function rapidly reduces the weight of the outlier measurement. The invention improves the positioning precision and the navigation parameter compensation effect, improves the adaptability of the single-axis rotation MEMS strapdown inertial navigation system in the GNSS/DVL challenge environment, and further meets the requirements of high-precision and high-reliability positioning and attitude determination and navigation of unmanned ship operation in the GNSS/DVL challenge environment.

Description

Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment
Technical Field
The invention belongs to the technical field of navigation, relates to a navigation positioning system and method of an unmanned ship, and in particular relates to an unmanned ship rotation modulation inertial navigation system and method suitable for a challenging environment.
Background
Along with the rapid increase of demands of ocean development, ocean defense and the like, the intelligent unmanned ship rapidly develops towards low cost, strong disturbance rejection, high navigational speed, intelligent and the like. And the navigation positioning is used as a key element for realizing the intellectualization of the unmanned ship, so that the operation efficiency and the operation precision of the unmanned ship are determined.
The single-axis rotation MEMS Strapdown Inertial Navigation System (SINS) is an ideal choice of an unmanned ship navigation system by means of the characteristics of low cost, low power consumption, high efficiency, self error compensation capability of a system-level inertial sensor and the like, the single-axis rotation modulation technology is utilized to effectively eliminate constant drift of a gyroscope and an accelerometer perpendicular to the direction of a rotation axis, and then the constant drift of the gyroscope and the accelerometer parallel to the direction of the rotation axis is estimated by using external measurement information of a Global Navigation Satellite System (GNSS) and an underwater Doppler Velocimeter (DVL) through the assistance of a navigation filter, so that error compensation and accurate acquisition of MEMS strapdown inertial navigation parameters are realized. Many state estimation methods, however, employ error squared minimization as a cost function, and under severe sea conditions, outlier measurements with large bias and time-varying noise due to frequently interrupted GNSS and DVL signals degrade the estimation accuracy and reliability of the filter. At this time, the existing traditional robust Kalman filter cannot guarantee the instantaneity and accuracy of the weight reduction processing of harmful information, so that the positioning and attitude determination and navigation information output of the single-axis rotation MEMS inertial navigation system are hindered, and the realization of unmanned ships in severe sea conditions is faced with a great challenge.
Therefore, a proper robust estimation method is designed to help the navigation filter to quickly inhibit serious influence of outlier measurement on the system, so that the adaptability of the single-axis rotation MEMS strapdown inertial navigation system in a GNSS/DVL challenge environment is improved, and the method becomes an important way for improving the navigation positioning precision and reliability of the unmanned ship.
Disclosure of Invention
In order to overcome the defects in the prior art, aiming at the problem that the measurement outliers of a global navigation satellite system and an underwater Doppler velocimeter in severe sea conditions affect the accurate estimation effect of navigation parameters of the GNSS/DVL/SINS in a GNSS/DVL challenge environment, the invention provides an unmanned ship rotation modulation inertial navigation system and a novel robust estimation method suitable for the challenge environment.
In order to achieve the above purpose, the present invention provides the following technical solutions:
the unmanned ship rotation modulation inertial navigation system suitable for the challenging environment comprises a micro-electromechanical inertial unit MEMS-IMU, a servo motor, a servo driver, a single-shaft rotating table, a movable code ring, a navigation positioning resolving board card, an aviation plug and a shell frame; the micro-electromechanical inertial unit integrates a triaxial MEMS gyroscope and an MEMS accelerometer, and the acceleration and angular rate information output by the micro-electromechanical inertial unit is subjected to zero offset compensation through a single-axis rotation modulation technology and a multi-source information fusion technology and then is calculated to obtain real-time attitude, speed and position information of the unmanned ship; the servo motor and the servo driver are used for driving the micro-electromechanical inertial unit to reciprocally rotate around the zenith axis, so that constant drift of the gyroscope and the accelerometer which are perpendicular to the direction of the rotating shaft is eliminated, and scale factor errors are eliminated; the navigation positioning resolving board card is used for receiving the output acceleration and angular velocity information, the speed and position information of an external global navigation satellite system and the speed information of an underwater Doppler velocimeter, and outputting navigation parameters after completing strapdown navigation resolving, combined navigation multisource information fusion and robust estimation; the aviation plug and the shell frame are respectively used for electric information access and hardware equipment assembly.
Further, the built-in software algorithm comprises the following steps:
s1: comprehensively considering MEMS strapdown inertial navigation, combined navigation characteristics and multi-error source coupling relation, and selecting attitude angle error [ delta phi ]]Speed error [ Deltav ]]Position error [ Δp ]]Zero offset [ epsilon ] of gyroscope]Error of scale factor of gyroscope [ delta K ] G ]Add zero offset [ -V)]Add scale factor error [ delta K ] A ]As the 24-dimensional correction state quantity x of the combined filter, the speed and position information [ v ] of the global navigation satellite system is selected GNSS ,p GNSS ]And velocity information of an underwater Doppler velocimeter [ v ] DVL ]Measuring y as the outside of the combined filter; designing a strapdown inertial error differential equation f (x) under the nonlinear large misalignment angle condition:
Figure BDA0004114617710000021
wherein,,
Figure BDA0004114617710000022
f b representing gyroscope and add output, +.>
Figure BDA0004114617710000023
The rotation vector representing the rotation angular velocity vector of the earth, the navigation system, and the rotation vector of the earth system and the inertial system, respectively,/->
Figure BDA0004114617710000024
Representing the rotation calculation error of the navigation system, δg representing the gravity error,/->
Figure BDA0004114617710000025
Representing a transformation matrix of the carrier system into the navigation system, < >>
Figure BDA0004114617710000026
Representing a misalignment angle error matrix between a calculated navigation system and an ideal navigation system, R M 、R N H represents the principal radius of curvature of the meridian, the principal radius of curvature of the mortise and tenon circle, and the altitude, LShowing local latitude;
s2: introducing a maximum posterior estimation and nonlinear least square regression concept, and establishing an iterative volume Kalman filter ICKF equation;
in the framework of a nonlinear sigma point filter, a posterior probability density function containing gaussian measurement noise is calculated:
Figure BDA0004114617710000031
wherein H represents a measurement matrix, R, P represents measurement noise covariance and optimal estimation covariance, p (·) represents a probability density function, exp [ · ] represents an exponential function with a natural constant e as a base, and subscript k represents an estimation time;
considering the maximum a posteriori estimate MAP concept, the a posteriori probability density function is equivalent to solving nonlinear regression parameters, equivalent to the filter update process as follows:
Figure BDA0004114617710000032
according to the newton-lavson algorithm, an iterative update procedure of the estimated state quantity x is constructed as an iterative form shaped as a nonlinear least square:
x i+1 ＝x i -(▽ 2 V(x i )) -1 ▽V(x i )＝(J T (x i )J(x i )) -1 J T (x i )r i (x i )
where i represents the number of iterations at the current estimated time, jacobian matrix V (x i )＝J T (x i )r(x i ) Heisen matrix ] 2 V(x i )≈J T (x i )J(x i )，
Figure BDA0004114617710000033
And obtaining an optimal estimation equation of the ICKF equation of the iterative volume Kalman filter through inverse matrix quotients:
Figure BDA0004114617710000034
wherein k=p xy,k|k-1 (P yy,k|k-1 ) -1 Representing a filter gain matrix; in the framework of a volumetric kalman filter, the covariance expression in the filter gain matrix K is replaced by uncertainty in the likelihood approximation, with:
Figure BDA0004114617710000035
Figure BDA0004114617710000036
re-writing to obtain an optimal estimation equation of the i-th iteration volume Kalman filter ICKF equation at the current estimation moment k:
Figure BDA0004114617710000037
s3: designing an iterative volume Kalman filter to simplify an iterative updating structure;
the simplified iterative update structure is as follows:
(7) generating a volume point x j,k-1|k-1
Figure BDA0004114617710000038
Wherein, the m table represents the number of the volume points,
Figure BDA0004114617710000039
represents a basic volume point [1 ]]Representing n-dimensional unit directions
A complete full symmetry point set generated by full arrangement and changing of element symbols is performed;
(8) priori prediction update
Calculating a one-step prediction state
Figure BDA0004114617710000041
And a priori estimated covariance->
Figure BDA0004114617710000042
Figure BDA0004114617710000043
Figure BDA0004114617710000044
Wherein Q is k Representing a system noise covariance;
(9) posterior metrology update
Calculating a metrology estimation
Figure BDA0004114617710000045
Figure BDA0004114617710000046
Calculating cross covariance P xy,k|k-1 And measure covariance P yy,k|k-1
Figure BDA0004114617710000047
Figure BDA0004114617710000048
Loop computing ith iteration posterior state estimate x i+1
Figure BDA0004114617710000049
After the iteration times reach a set value N, the method comprises the following steps of
Figure BDA00041146177100000410
Calculating 1 posterior estimated covariance
Figure BDA00041146177100000411
S4: introducing penalty weight function to quickly reduce weight of outlier measurement
Considering constructing a robust kernel function, and replacing a square term of an original error in the traditional Kalman filtering by a nonlinear damping term of a robust estimation method; robust estimation object X established for measuring outliers * The following are provided:
Figure BDA00041146177100000412
wherein W is k E (0, 1) represents the extrinsic information penalty weight matrix at k time, θ (W) k ) Representing a penalty function of the penalty function,
Figure BDA00041146177100000413
the mahalanobis distance of the representation vector;
the GM loss function is chosen as a robust kernel function as follows:
Figure BDA00041146177100000414
wherein s is GM A shape control factor representing GM loss function;
due to robust estimation of the objective function X * For convex function, for calculating optimal punishment weight matrix, robust estimation objective function X * Calculating the bias guide, and taking minimum value points:
Figure BDA0004114617710000051
Figure BDA0004114617710000052
the invention also provides a robust estimation method suitable for the GNSS/DVL challenge environment, which is used for outputting accurate navigation parameters of the unmanned ship rotation modulation inertial navigation system, and comprises the following steps:
s1: comprehensively considering MEMS strapdown inertial navigation, combined navigation characteristics and multi-error source coupling relation, and selecting attitude angle error [ delta phi ]]Speed error [ Deltav ]]Position error [ Δp ]]Zero offset [ epsilon ] of gyroscope]Error of scale factor of gyroscope [ delta K ] G ]Add zero offset [ -V)]Add scale factor error [ delta K ] A ]As the 24-dimensional correction state quantity x of the combined filter, the speed and position information [ v ] of the global navigation satellite system is selected GNSS ,p GNSS ]And velocity information of an underwater Doppler velocimeter [ v ] DVL ]Measuring y as the outside of the combined filter; designing a strapdown inertial error differential equation f (x) under the nonlinear large misalignment angle condition:
Figure BDA0004114617710000053
wherein,,
Figure BDA0004114617710000054
f b representing gyroscope and add output, +.>
Figure BDA0004114617710000055
The rotation vector representing the rotation angular velocity vector of the earth, the navigation system, and the rotation vector of the earth system and the inertial system, respectively,/->
Figure BDA0004114617710000056
Representing the rotation calculation error of the navigation system, δg representing the gravity error,/->
Figure BDA0004114617710000057
Representing the transition moment of a carrier system to a navigation systemArray (S)>
Figure BDA0004114617710000058
Representing a misalignment angle error matrix between a calculated navigation system and an ideal navigation system, R M 、R N H represents the principal radius of curvature of the meridian, the principal radius of curvature of the mortise and tenon circle, and the altitude, L represents the local latitude;
s2: introducing a maximum posterior estimation and nonlinear least square regression concept, and establishing an iterative volume Kalman filter ICKF equation;
in the framework of a nonlinear sigma point filter, a posterior probability density function containing gaussian measurement noise is calculated:
Figure BDA0004114617710000059
wherein H represents a measurement matrix, R, P represents measurement noise covariance and optimal estimation covariance, p (·) represents a probability density function, exp [ · ] represents an exponential function with a natural constant e as a base, and subscript k represents an estimation time;
considering the maximum a posteriori estimate MAP concept, the a posteriori probability density function is equivalent to solving nonlinear regression parameters, equivalent to the filter update process as follows:
Figure BDA0004114617710000061
according to the newton-lavson algorithm, an iterative update procedure of the estimated state quantity x is constructed as an iterative form shaped as a nonlinear least square:
x i+1 ＝x i -(▽ 2 V(x i )) -1 ▽V(x i )＝(J T (x i )J(x i )) -1 J T (x i )r i (x i )
where i represents the number of iterations at the current estimated time, jacobian matrix V (x i )＝J T (x i )r(x i ) Heisen matrix ] 2 V(x i )≈J T (x i )J(x i )，
Figure BDA0004114617710000062
And obtaining an optimal estimation equation of the ICKF equation of the iterative volume Kalman filter through inverse matrix quotients:
Figure BDA0004114617710000063
wherein k=p xy,k|k-1 (P yy,k|k-1 ) -1 Representing a filter gain matrix; in the framework of a volumetric kalman filter, the covariance expression in the filter gain matrix K is replaced by uncertainty in the likelihood approximation, with:
Figure BDA0004114617710000064
Figure BDA0004114617710000065
re-writing to obtain an optimal estimation equation of the i-th iteration volume Kalman filter ICKF equation at the current estimation moment k:
Figure BDA0004114617710000066
s3: designing an iterative volume Kalman filter to simplify an iterative updating structure;
the simplified iterative update structure is as follows:
generating a volume point x j,k-1|k-1
Figure BDA0004114617710000067
Wherein, the number of the volume points of the m table，
Figure BDA0004114617710000068
Represents a basic volume point [1 ]]Representing n-dimensional unit directions
A complete full symmetry point set generated by full arrangement and changing of element symbols is performed;
Figure BDA0004114617710000069
priori prediction update
Calculating a one-step prediction state
Figure BDA00041146177100000610
And a priori estimated covariance->
Figure BDA00041146177100000611
Figure BDA00041146177100000612
Figure BDA00041146177100000613
Wherein Q is k Representing a system noise covariance;
Figure BDA0004114617710000071
posterior metrology update
Calculating a metrology estimation
Figure BDA0004114617710000072
Figure BDA0004114617710000073
Calculating cross covariance P xy,k|k-1 And measure covariance P yy,k|k-1
Figure BDA0004114617710000074
Figure BDA0004114617710000075
Loop computing ith iteration posterior state estimate x i+1
Figure BDA0004114617710000076
After the iteration times reach a set value N, the method comprises the following steps of
Figure BDA0004114617710000077
Calculating 1 posterior estimated covariance
Figure BDA0004114617710000078
S4: introducing penalty weight function to quickly reduce weight of outlier measurement
Considering constructing a robust kernel function, and replacing a square term of an original error in the traditional Kalman filtering by a nonlinear damping term of a robust estimation method; robust estimation object X established for measuring outliers * The following are provided:
Figure BDA0004114617710000079
wherein W is k E (0, 1) represents the extrinsic information penalty weight matrix at k time, θ (W) k ) Representing a penalty function of the penalty function,
Figure BDA00041146177100000710
the mahalanobis distance of the representation vector;
the GM loss function is chosen as a robust kernel function as follows:
Figure BDA00041146177100000711
wherein s is GM A shape control factor representing GM loss function;
due to robust estimation of the objective function X * For convex function, for calculating optimal punishment weight matrix, robust estimation objective function X * Calculating the bias guide, and taking minimum value points:
Figure BDA00041146177100000712
Figure BDA00041146177100000713
compared with the prior art, the invention has the following advantages and beneficial effects:
compared with the prior art, the single-axis rotation MEMS inertial navigation system of the unmanned ship provided by the invention adopts a single-axis rotation modulation technology to effectively eliminate the constant drift and scale factor errors of the gyroscopes and accelerometers perpendicular to the rotation axis direction, and then adopts a multi-source information fusion technology to effectively estimate the constant drift and scale factor errors of the gyroscopes and accelerometers parallel to the rotation axis direction on the basis of the external measurement information of a Global Navigation Satellite System (GNSS) and an underwater Doppler Velocimeter (DVL), finally, a simplified iteration update structure is introduced to meet the real-time performance and the weight of rapidly reducing outlier measurement by introducing a punishment weight function, so that the adaptability of the single-axis rotation MEMS strapdown inertial navigation system in a GNSS/DVL challenging environment is improved, the further research and development of the single-axis rotation MEMS strapdown inertial navigation anti-disturbance technology are promoted, and technical support is provided for the high-precision, high-reliability and high-efficiency operation of the unmanned ship under severe sea conditions.
Drawings
FIG. 1 is a perspective view of the overall structure of a single axis rotation modulating MEMS inertial navigation hardware system of the present invention.
Fig. 2 is a flowchart of a simplified iterative update structure of a robust iterative volume kalman filter in an embodiment of the invention.
Detailed Description
The technical scheme provided by the present invention will be described in detail with reference to the following specific examples, and it should be understood that the following specific examples are only for illustrating the present invention and are not intended to limit the scope of the present invention.
The invention provides an unmanned ship rotation modulation inertial navigation system suitable for a challenging environment. FIG. 1 is a diagram of the overall structure of a single axis rotation modulated MEMS inertial navigation hardware system of the present invention, comprising: a micro-electromechanical inertial unit (MEMS-IMU) 1, a servo motor 2, a servo driver 3, a single-shaft rotary table 4, a movable code ring 5, a navigation positioning resolving board card 6, an aviation plug 7 and a shell frame 8. The micro-electromechanical inertial unit integrates a triaxial MEMS gyroscope and an MEMS accelerometer, and the acceleration and angular rate information output by the micro-electromechanical inertial unit is subjected to zero offset compensation through a single-axis rotation modulation technology and a multi-source information fusion technology and then calculated to obtain the real-time attitude, speed and position information of the unmanned ship. The servo motor and the servo driver are used for driving the micro-electromechanical inertial unit to rotate reciprocally around the zenith axis, so that constant drift of the gyroscope and the accelerometer which are perpendicular to the direction of the rotating shaft is effectively eliminated, and the driver is used as a motor control end for adjusting parameters such as rotating speed, position, moment and the like of the motor. The navigation positioning resolving board card is used for receiving the output acceleration and angular velocity information, the speed and position information of an external global navigation satellite system and the speed information of an underwater Doppler velocimeter, and outputting navigation parameters after strapdown navigation resolving, combined navigation multisource information fusion and robust estimation are completed through an internal software algorithm. The single-shaft rotary table is used for placing a micro-electromechanical inertial unit (MEMS-IMU) and driven by a servo motor to rotate reciprocally around an antenna axis, and the movable code ring is used for connecting an electrical connection wire to prevent wiring winding caused by rotation of the rotary table. The aviation plug and the housing frame serve electrical information access and hardware equipment assembly, respectively.
The software algorithm arranged in the navigation positioning resolving board card is a robust estimation software algorithm suitable for the GNSS/DVL challenge environment, and is used for solving the problem that the measurement outlier of the global navigation satellite system and the underwater Doppler velocimeter in severe sea conditions affects the accurate estimation effect of the GNSS/DVL/SINS combined navigation parameters, and comprises the following steps:
s1: comprehensively considering MEMS strapdown inertial navigation, combined navigation characteristics and multi-error source coupling relation, and selecting attitude angle error [ delta phi ]]Speed error [ Deltav ]]Position error [ Δp ]]Zero offset [ epsilon ] of gyroscope]Error of scale factor of gyroscope [ delta K ] G ]Add zero offset [ -V)]Add scale factor error [ delta K ] A ]As the 24-dimensional correction state quantity x of the combined filter, the speed and position information [ v ] of the global navigation satellite system is selected GNSS ,p GNSS ]And velocity information of an underwater Doppler velocimeter [ v ] DVL ]Y is measured externally as a combined filter. Designing a strapdown inertial error differential equation f (x) under the nonlinear large misalignment angle condition:
Figure BDA0004114617710000091
wherein,,
Figure BDA0004114617710000092
f b representing gyroscope and add output, +.>
Figure BDA0004114617710000093
The rotation vector representing the rotation angular velocity vector of the earth, the navigation system, and the rotation vector of the earth system and the inertial system, respectively,/->
Figure BDA0004114617710000094
Representing the rotation calculation error of the navigation system, δg representing the gravity error,/->
Figure BDA0004114617710000095
Representing a transformation matrix of the carrier system into the navigation system, < >>
Figure BDA0004114617710000096
Representing a misalignment angle error matrix between a calculated navigation system and an ideal navigation system, R M 、R N H represents the principal radius of curvature of the meridian, the principal radius of curvature of the mortise and tenon circle, and altitudeDegree, L, represents the local latitude. />
Figure BDA0004114617710000097
Representing the velocity estimate, sec represents a secant trigonometric function.
S2: and introducing a maximum posterior estimation and nonlinear least squares regression concept to establish an iterative volume Kalman filter (ICKF) equation.
In the framework of a nonlinear sigma point filter, a posterior probability density function containing gaussian measurement noise is calculated:
Figure BDA0004114617710000098
where H represents the measurement matrix, R, P represents the measurement noise covariance and the optimal estimation covariance, p (·) represents the probability density function, exp [ · [ ], and]an exponential function with a base representing the natural constant e, a subscript k representing the estimated time,
Figure BDA0004114617710000099
a one-step predictive estimate of the state quantity from time k-1 to time k is represented.
Considering the maximum a posteriori estimation (MAP) concept, the posterior probability density function is equivalent to solving nonlinear regression parameters, equivalent to the filter update process as follows:
Figure BDA00041146177100000910
according to Newton-Raphson (Newton-Raphson) algorithm, the iterative update procedure of the estimated state quantity x is constructed as an iterative form shaped as a nonlinear least squares:
x i+1 ＝x i -(▽ 2 V(x i )) -1 ▽V(x i )＝(J T (x i )J(x i )) -1 J T (x i )r i (x i )
where i represents the number of iterations at the current estimated time, jacobian matrix V (x i )＝J T (x i )r(x i ) And Heisen matrix ] 2 V(x i )≈J T (x i )J(x i ) Further comprises
Figure BDA0004114617710000101
Figure BDA0004114617710000102
h(x k ) Is a nonlinear measurement differential equation.
And obtaining an optimal estimation equation of an iterative volume Kalman filter (ICKF) equation through inverse matrix quotients:
Figure BDA0004114617710000103
wherein k=p xy,k|k-1 (P yy,k|k-1 ) -1 Representing the filter gain matrix. In the framework of a volumetric kalman filter, the covariance expression in the filter gain matrix K is replaced by uncertainty in the likelihood approximation, with:
Figure BDA0004114617710000104
Figure BDA0004114617710000105
re-writing to obtain an optimal estimation equation of an ith iteration volume Kalman filter (ICKF) equation of the current estimation moment k:
Figure BDA0004114617710000106
s3: designing an iterative volume kalman filter simplifies the iterative update structure.
The large number of matrix accumulation multiplication operations generated in the iterative process increases the calculation pressure of the processor, and the real-time performance of navigation calculation needs to be met. Because the state posterior provides more information about the nonlinear measurement function, the solved nonlinear regression parameters are constrained on the high likelihood interval of the posterior probability density function to obtain a better approximation effect, and the prior information has little influence on the maximum posterior estimation. Therefore, the posterior estimation covariance and the prior prediction stage information are calculated only once to reduce the calculation amount of iterative update by considering that a plurality of iterative updates are introduced in the posterior filtering process at each filtering moment and only the state quantity is estimated in an iterative manner. The simplified iterative update structure is as follows:
Figure BDA00041146177100001014
generating a volume point x j,k-1|k-1
Figure BDA0004114617710000107
Wherein, the m table represents the number of the volume points,
Figure BDA0004114617710000108
represents a basic volume point [1 ]]Representing a complete set of full symmetry points generated by full arrangement of n-dimensional unit vectors and changing element symbols
Figure BDA0004114617710000109
Priori prediction update
Calculating a one-step prediction state
Figure BDA00041146177100001010
And a priori estimated covariance->
Figure BDA00041146177100001011
Figure BDA00041146177100001012
Figure BDA00041146177100001013
Wherein Q is k Representing the system noise covariance.
Figure BDA0004114617710000111
Posterior metrology update
Calculating a metrology estimation
Figure BDA0004114617710000112
Figure BDA0004114617710000113
Calculating cross covariance P xy,k|k-1 And measure covariance P yy,k|k-1
Figure BDA0004114617710000114
Figure BDA0004114617710000115
Wherein,,
Figure BDA0004114617710000116
a one-step predictive estimate of the observed quantity x from time k-1 to time k;
loop computing ith iteration posterior state estimate x i+1
Figure BDA0004114617710000117
After the iteration times reach a set value N, the method comprises the following steps of
Figure BDA0004114617710000118
Calculating 1 posterior estimated covariance
Figure BDA0004114617710000119
S4: the introduction of a penalty weight function rapidly reduces the weight of the outlier measurement.
Considering constructing a robust kernel function, a nonlinear damping term of a robust estimation method is adopted to replace a square term of an original error in the traditional Kalman filtering. Robust estimation object X established for measuring outliers * The following are provided:
Figure BDA00041146177100001110
wherein W is k E (0, 1) represents an extrinsic information penalty matrix at the moment k, wherein the closer the penalty is to 1, the better the extrinsic information quality is, and the closer the penalty is to 0, the greater the extrinsic information is affected by an outlier. θ (W) k ) A penalty function is represented for reducing extrinsic information weights containing outlier measurements to suppress their impact on the filter.
Figure BDA00041146177100001111
Representing the mahalanobis distance of the vector.
The Geman McClure (GM) loss function is selected as a robust kernel function, which comprises the following steps:
Figure BDA00041146177100001112
wherein s is GM The shape control factor representing the GM loss function determines the convergence speed and robust performance of the robust kernel function.
Due to robust estimation of the objective function X * For convex function, for calculating optimal punishment weight matrix, robust estimation objective function X * Calculating the bias guide, and taking minimum value points:
Figure BDA00041146177100001113
Figure BDA0004114617710000121
the penalty weight function is introduced into the simplified iterative updating structure of the iterative volume Kalman filter according to S3, and the calculated cross covariance P in the posterior measurement updating stage is calculated xy,k|k-1 And measure covariance P yy,k|k-1 After the step. The external information penalty weight matrix W is calculated in advance in each iteration posterior estimation state k To help the navigation filter quickly suppress the severe impact of outlier measurements on the system.
The robust iterative volume Kalman filter composed of S3 and S4 is shown in figure 2, the GM loss function is used for estimating regression coefficients, abnormal measured values can be restrained while the smoothness of the robust volume Kalman filter is ensured, and a more robust least square solution can be obtained. The influence of the rough difference on the navigation filter is restrained by quickly reducing the weight of outlier measurement, so that the aims of improving the positioning precision and the navigation parameter compensation effect are fulfilled, and the adaptability of the single-axis rotation MEMS strapdown inertial navigation system in a GNSS/DVL challenge environment is improved.
The technical means disclosed by the scheme of the invention is not limited to the technical means disclosed by the embodiment, and also comprises the technical scheme formed by any combination of the technical features. It should be noted that modifications and adaptations to the invention may occur to one skilled in the art without departing from the principles of the present invention and are intended to be within the scope of the present invention.

Claims (3)

1. The unmanned ship rotation modulation inertial navigation system is suitable for a challenging environment and is characterized by comprising a micro-electromechanical inertial unit MEMS-IMU, a servo motor, a servo driver, a single-shaft rotating table, a movable code ring, a navigation positioning resolving board card, an aviation plug and a shell frame; the micro-electromechanical inertial unit integrates a triaxial MEMS gyroscope and an MEMS accelerometer, and the acceleration and angular rate information output by the micro-electromechanical inertial unit is subjected to zero offset compensation through a single-axis rotation modulation technology and a multi-source information fusion technology and then is calculated to obtain real-time attitude, speed and position information of the unmanned ship; the servo motor and the servo driver are used for driving the micro-electromechanical inertial unit to reciprocally rotate around the zenith axis, so that constant drift of the gyroscope and the accelerometer which are perpendicular to the direction of the rotating shaft is eliminated, and scale factor errors are eliminated; the navigation positioning resolving board card is used for receiving the output acceleration and angular velocity information, the speed and position information of an external global navigation satellite system and the speed information of an underwater Doppler velocimeter, and outputting navigation parameters after completing strapdown navigation resolving, combined navigation multisource information fusion and robust estimation; the aviation plug and the shell frame are respectively used for electric information access and hardware equipment assembly.
2. The unmanned ship rotational modulation inertial navigation system adapted to challenging environments of claim 1, wherein the built-in software algorithm comprises the steps of:
s1: comprehensively considering MEMS strapdown inertial navigation, combined navigation characteristics and multi-error source coupling relation, and selecting attitude angle error [ delta phi ]]Speed error [ Deltav ]]Position error [ Δp ]]Zero offset [ epsilon ] of gyroscope]Error of scale factor of gyroscope [ delta K ] G ]Add zero offset [ -V)]Add scale factor error [ delta K ] A ]As the 24-dimensional correction state quantity x of the combined filter, the speed and position information [ v ] of the global navigation satellite system is selected GNSS ,p GNSS ]And velocity information of an underwater Doppler velocimeter [ v ] DVL ]Measuring y as the outside of the combined filter; designing a strapdown inertial error differential equation f (x) under the nonlinear large misalignment angle condition:
Figure FDA0004114617700000011
wherein,,
Figure FDA0004114617700000012
f b representing gyroscope and add output, +.>
Figure FDA0004114617700000013
The rotation vector representing the rotation angular velocity vector of the earth, the navigation system, and the rotation vector of the earth system and the inertial system, respectively,/->
Figure FDA0004114617700000014
Represents the rotation calculation error of the navigation system, δg represents the gravity error,
Figure FDA0004114617700000015
representing a transformation matrix of the carrier system into the navigation system, < >>
Figure FDA0004114617700000016
Representing a misalignment angle error matrix between a calculated navigation system and an ideal navigation system, R M 、R N H represents the principal radius of curvature of the meridian, the principal radius of curvature of the mortise and tenon circle, and the altitude, L represents the local latitude;
s2: introducing a maximum posterior estimation and nonlinear least square regression concept, and establishing an iterative volume Kalman filter ICKF equation;
in the framework of a nonlinear sigma point filter, a posterior probability density function containing gaussian measurement noise is calculated:
Figure FDA0004114617700000021
wherein H represents a measurement matrix, R, P represents measurement noise covariance and optimal estimation covariance, p (·) represents a probability density function, exp [ · ] represents an exponential function with a natural constant e as a base, and subscript k represents an estimation time;
considering the maximum a posteriori estimate MAP concept, the a posteriori probability density function is equivalent to solving nonlinear regression parameters, equivalent to the filter update process as follows:
Figure FDA0004114617700000022
according to the newton-lavson algorithm, an iterative update procedure of the estimated state quantity x is constructed as an iterative form shaped as a nonlinear least square:
x i+1 ＝x i -(▽ 2 V(x i )) -1 ▽V(x i )＝(J T (x i )J(x i )) -1 J T (x i )r i (x i )
where i represents the number of iterations at the current estimated time, jacobian matrix V (x i )＝J T (x i )r(x i ) Heisen matrix ] 2 V(x i )≈J T (x i )J(x i )，
Figure FDA0004114617700000023
And obtaining an optimal estimation equation of the ICKF equation of the iterative volume Kalman filter through inverse matrix quotients:
Figure FDA0004114617700000024
wherein k=p xy,k|k-1 (P yy,k|k-1 ) -1 Representing a filter gain matrix; in the framework of a volumetric kalman filter, the covariance expression in the filter gain matrix K is replaced by uncertainty in the likelihood approximation, with:
Figure FDA0004114617700000025
Figure FDA0004114617700000026
re-writing to obtain an optimal estimation equation of the i-th iteration volume Kalman filter ICKF equation at the current estimation moment k:
Figure FDA0004114617700000027
s3: designing an iterative volume Kalman filter to simplify an iterative updating structure;
the simplified iterative update structure is as follows:
(1) generating a volume point x j,k-1|k-1
Figure FDA0004114617700000028
Wherein, the m table represents the number of the volume points,
Figure FDA0004114617700000031
represents a basic volume point [1 ]]Representing a complete full symmetry point set generated by full arrangement of n-dimensional unit vectors and changing element symbols;
(2) priori prediction update
Calculating a one-step prediction state
Figure FDA0004114617700000032
And a priori estimated covariance->
Figure FDA0004114617700000033
Figure FDA0004114617700000034
Figure FDA0004114617700000035
Wherein Q is k Representing a system noise covariance;
(3) posterior metrology update
Calculating a metrology estimation
Figure FDA0004114617700000036
Figure FDA0004114617700000037
Calculating cross covariance P xy,k|k-1 And measure covariance P yy,k|k-1
Figure FDA0004114617700000038
Figure FDA0004114617700000039
Loop computing ith iteration posterior state estimate x i+1
Figure FDA00041146177000000310
After the iteration times reach a set value N, the method comprises the following steps of
Figure FDA00041146177000000311
Calculating 1 posterior estimated covariance
Figure FDA00041146177000000312
S4: introducing penalty weight function to quickly reduce weight of outlier measurement
Considering constructing a robust kernel function, and replacing a square term of an original error in the traditional Kalman filtering by a nonlinear damping term of a robust estimation method; robust estimation object X established for measuring outliers * The following are provided:
Figure FDA00041146177000000313
wherein W is k E (0, 1) represents the extrinsic information penalty weight matrix at k time, θ (W) k ) Representing a penalty function of the penalty function,
Figure FDA00041146177000000314
the mahalanobis distance of the representation vector;
the GM loss function is chosen as a robust kernel function as follows:
Figure FDA0004114617700000041
wherein s is GM A shape control factor representing GM loss function;
due to robust estimation of the objective function X * For convex function, for calculating optimal punishment weight matrix, robust estimation objective function X * Calculating the bias guide, and taking minimum value points:
Figure FDA0004114617700000042
Figure FDA0004114617700000043
3. a robust estimation method suitable for a GNSS/DVL challenge environment is used for outputting accurate navigation parameters of an unmanned ship rotation modulation inertial navigation system, and comprises the following steps:
s1: comprehensively considering MEMS strapdown inertial navigation, combined navigation characteristics and multi-error source coupling relation, and selecting attitude angle error [ delta phi ]]Speed error [ Deltav ]]Position error [ Δp ]]Zero offset [ epsilon ] of gyroscope]Error of scale factor of gyroscope [ delta K ] G ]Add zero offset [ -V)]Add scale factor error [ delta K ] A ]As the 24-dimensional correction state quantity x of the combined filter, the speed and position information [ v ] of the global navigation satellite system is selected GNSS ,p GNSS ]And underwater DopplerVelocity information v of velocimeter DVL ]Measuring y as the outside of the combined filter; designing a strapdown inertial error differential equation f (x) under the nonlinear large misalignment angle condition:
Figure FDA0004114617700000044
wherein,,
Figure FDA0004114617700000045
f b representing gyroscope and add output, +.>
Figure FDA0004114617700000046
The rotation vector representing the rotation angular velocity vector of the earth, the navigation system, and the rotation vector of the earth system and the inertial system, respectively,/->
Figure FDA0004114617700000047
Represents the rotation calculation error of the navigation system, δg represents the gravity error,
Figure FDA0004114617700000048
representing a transformation matrix of the carrier system into the navigation system, < >>
Figure FDA0004114617700000049
Representing a misalignment angle error matrix between a calculated navigation system and an ideal navigation system, R M 、R N H represents the principal radius of curvature of the meridian, the principal radius of curvature of the mortise and tenon circle, and the altitude, L represents the local latitude;
s2: introducing a maximum posterior estimation and nonlinear least square regression concept, and establishing an iterative volume Kalman filter ICKF equation;
in the framework of a nonlinear sigma point filter, a posterior probability density function containing gaussian measurement noise is calculated:
Figure FDA0004114617700000051
wherein H represents a measurement matrix, R, P represents measurement noise covariance and optimal estimation covariance, p (·) represents a probability density function, exp [ · ] represents an exponential function with a natural constant e as a base, and subscript k represents an estimation time;
considering the maximum a posteriori estimate MAP concept, the a posteriori probability density function is equivalent to solving nonlinear regression parameters, equivalent to the filter update process as follows:
Figure FDA0004114617700000052
according to the newton-lavson algorithm, an iterative update procedure of the estimated state quantity x is constructed as an iterative form shaped as a nonlinear least square:
x i+1 ＝x i -(▽ 2 V(x i )) -1 ▽V(x i )＝(J T (x i )J(x i )) -1 J T (x i )r i (x i )
where i represents the number of iterations at the current estimated time, jacobian matrix V (x i )＝J T (x i )r(x i ) Heisen matrix ] 2 V(x i )≈J T (x i )J(x i )，
Figure FDA0004114617700000053
And obtaining an optimal estimation equation of the ICKF equation of the iterative volume Kalman filter through inverse matrix quotients:
Figure FDA0004114617700000054
wherein k=p xy,k|k-1 (P yy,k|k-1 ) -1 Representing a filter gain matrix; in the framework of a volume Kalman filter, the covariance expression in the filter gain matrix K is approximated by uncertainty in the likelihoodQualitative substitutions, including:
Figure FDA0004114617700000055
Figure FDA0004114617700000056
re-writing to obtain an optimal estimation equation of the i-th iteration volume Kalman filter ICKF equation at the current estimation moment k:
Figure FDA0004114617700000057
s3: designing an iterative volume Kalman filter to simplify an iterative updating structure;
the simplified iterative update structure is as follows:
(4) generating a volume point x j,k-1|k-1
Figure FDA0004114617700000058
Wherein, the m table represents the number of the volume points,
Figure FDA0004114617700000061
represents a basic volume point [1 ]]Representing a complete full symmetry point set generated by full arrangement of n-dimensional unit vectors and changing element symbols;
(5) priori prediction update
Calculating a one-step prediction state
Figure FDA0004114617700000062
And a priori estimated covariance->
Figure FDA0004114617700000063
Figure FDA0004114617700000064
Figure FDA0004114617700000065
Wherein Q is k Representing a system noise covariance;
(6) posterior metrology update
Calculating a metrology estimation
Figure FDA0004114617700000066
Figure FDA0004114617700000067
Calculating cross covariance P xy,k|k-1 And measure covariance P yy,k|k-1
Figure FDA0004114617700000068
Figure FDA0004114617700000069
Loop computing ith iteration posterior state estimate x i+1
Figure FDA00041146177000000610
After the iteration times reach a set value N, the method comprises the following steps of
Figure FDA00041146177000000611
Calculating 1 posterior estimated covariance
Figure FDA00041146177000000612
S4: introducing penalty weight function to quickly reduce weight of outlier measurement
Considering constructing a robust kernel function, and replacing a square term of an original error in the traditional Kalman filtering by a nonlinear damping term of a robust estimation method; robust estimation object X established for measuring outliers * The following are provided:
Figure FDA00041146177000000613
wherein W is k E (0, 1) represents the extrinsic information penalty weight matrix at k time, θ (W) k ) Representing a penalty function of the penalty function,
Figure FDA00041146177000000614
the mahalanobis distance of the representation vector;
the GM loss function is chosen as a robust kernel function as follows:
Figure FDA0004114617700000071
wherein s is GM A shape control factor representing GM loss function;
due to robust estimation of the objective function X * For convex function, for calculating optimal punishment weight matrix, robust estimation objective function X * Calculating the bias guide, and taking minimum value points:
Figure FDA0004114617700000072
Figure FDA0004114617700000073
CN202310215059.1A 2023-03-08 2023-03-08 Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment Pending CN116380067A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310215059.1A CN116380067A (en) 2023-03-08 2023-03-08 Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310215059.1A CN116380067A (en) 2023-03-08 2023-03-08 Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment

Publications (1)

Publication Number Publication Date
CN116380067A true CN116380067A (en) 2023-07-04

Family

ID=86977879

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310215059.1A Pending CN116380067A (en) 2023-03-08 2023-03-08 Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment

Country Status (1)

Country Link
CN (1) CN116380067A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116772903A (en) * 2023-08-16 2023-09-19 河海大学 SINS/USBL installation angle estimation method based on iterative EKF

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116772903A (en) * 2023-08-16 2023-09-19 河海大学 SINS/USBL installation angle estimation method based on iterative EKF
CN116772903B (en) * 2023-08-16 2023-10-20 河海大学 SINS/USBL installation angle estimation method based on iterative EKF

Similar Documents

Publication Publication Date Title
CN109443379B (en) SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle
CN112097763B (en) Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN112284384A (en) Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity
Vasconcelos et al. INS/GPS aided by frequency contents of vector observations with application to autonomous surface crafts
CN104697520A (en) Combined navigation method based on integrated gyroscope free strapdown inertial navigation system and GPS
CN110763872A (en) Multi-parameter online calibration method for Doppler velocimeter
CN104931994A (en) Software receiver-based distributed deep integrated navigation method and system
CN116380067A (en) Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment
CN114459476B (en) Underwater unmanned submarine current measuring DVL/SINS integrated navigation method based on virtual speed measurement
CN112197765A (en) Method for realizing fine navigation of underwater robot
CN116007620A (en) Combined navigation filtering method, system, electronic equipment and storage medium
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN111722295A (en) Underwater strapdown gravity measurement data processing method
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN112284388A (en) Multi-source information fusion navigation method for unmanned aerial vehicle
CN114018262B (en) Improved derivative volume Kalman filtering integrated navigation method
CN113551669B (en) Combined navigation positioning method and device based on short base line
CN113434806B (en) Robust adaptive multi-model filtering method
CN114279443B (en) USBL (unified use line library) slant distance correction method based on maximum entropy self-adaptive robust estimation
CN115031728A (en) Unmanned aerial vehicle integrated navigation method based on tight combination of INS and GPS
CN114061621B (en) Large-misalignment-angle initial alignment method based on rotation modulation of strong tracking movable machine base
CN116337115B (en) Sonar-based method and system for calibrating inertial sensor
McKelvie et al. The evolution of the ship's inertial navigation system for the fleet Ballistic missile program
CN114166203B (en) Intelligent underwater robot multi-source combined navigation method based on improved S-H self-adaptive federal filtering

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