CN117537811A - Cross-polar region navigation switching method under optimized earth ellipsoid model - Google Patents
Cross-polar region navigation switching method under optimized earth ellipsoid model Download PDFInfo
- Publication number
- CN117537811A CN117537811A CN202311492702.1A CN202311492702A CN117537811A CN 117537811 A CN117537811 A CN 117537811A CN 202311492702 A CN202311492702 A CN 202311492702A CN 117537811 A CN117537811 A CN 117537811A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- error
- abscissa
- representing
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 113
- 238000006243 chemical reaction Methods 0.000 claims abstract description 69
- 230000008569 process Effects 0.000 claims abstract description 39
- 238000004364 calculation method Methods 0.000 claims abstract description 19
- 238000001914 filtration Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 79
- 239000013598 vector Substances 0.000 claims description 24
- 238000009434 installation Methods 0.000 claims description 21
- 230000008859 change Effects 0.000 claims description 10
- 230000005484 gravity Effects 0.000 claims description 5
- 230000009897 systematic effect Effects 0.000 claims description 5
- 230000014509 gene expression Effects 0.000 claims description 4
- 230000010355 oscillation Effects 0.000 abstract description 2
- 238000005094 computer simulation Methods 0.000 abstract 1
- 230000001360 synchronised effect Effects 0.000 abstract 1
- 230000009466 transformation Effects 0.000 abstract 1
- 230000006872 improvement Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/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
-
- 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
-
- 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/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
Abstract
The invention belongs to the technical field of inertial navigation, and discloses an optimized navigation switching method of a cross-polar region under an ellipsoidal earth model, which is suitable for navigation of a ship in a cross-polar region. The invention utilizes the characteristics of the Psi angle error model definition in a calculation coordinate system, optimizes the transverse navigation mechanical arrangement under the earth ellipsoid model, and constructs an inertia/velocimeter combined navigation filter. In addition, when the ship sails across the polar region, a transformation relation between navigation parameters and filtering parameters between an abscissa and an ordinate is constructed, so that synchronous switching of the navigation parameters and the filtering parameters is realized; the invention uses the earth ellipsoid model to reduce the system modeling error, and ensures the navigation precision during long voyage while avoiding complex parameter calculation. The invention can solve the problems of overshoot and oscillation error of the combined navigation filter in the navigation coordinate system conversion process, and enhances the transitional stability of the ship inertial navigation system from the non-polar region to the polar region.
Description
Technical Field
The invention belongs to the technical field of inertial navigation, relates to an inertial/velocimeter combined navigation method, and particularly relates to an optimized navigation switching method for a trans-regional navigation under an ellipsoidal model of the earth, which is suitable for trans-regional navigation of ships.
Background
The polar region, especially the arctic region, has important strategic value in aspects of resources, scientific research, navigation channels and the like, and each country accelerates strategic layout and research of the polar region. Inertial navigation has become an important navigation tool because it has excellent autonomy and is not affected by the severe environment of the polar region.
The traditional inertial navigation arrangement scheme is often used in low-and-medium latitude areas, however, due to the characteristics of high polar latitude and convergence of longitude lines, the problems of inherent singular points, loss of heading reference and the like of the traditional inertial navigation arrangement scheme occur in polar regions. The proposal of the transverse coordinate system can better solve the problem of polar region navigation, but although the method avoids the inherent singular point of the poles by redefining a new coordinate system, the inherent singular point still exists at the north-transverse poles and the south-transverse poles, so the requirement of full-latitude navigation can not be met. In addition, the initial transversal coordinate system is put forward by adopting an earth sphere model, so that model errors are brought, and the model is not suitable for long-endurance polar region navigation. After improvement, the transverse coordinate system mechanical arrangement of the ellipsoidal earth model is very complex, and some systematic error parameter analytical expressions related to the ellipsoidal earth model are tedious and complex, so that the calculation load is brought to the system calculation.
In order to realize navigation of long-endurance cross-polar region navigation of a ship, a common method is to combine the advantages of an abscissa and an geography coordinate system, construct an inertial navigation system capable of realizing the mutual conversion between navigation coordinate systems on the basis of an earth ellipsoid model, and the method inevitably involves the switching of navigation parameters and navigation system structures between the coordinate systems. For the integrated navigation system, the switching process can affect the continuity and consistency of the navigation filtering structure, so that the problems of abrupt change of the filter structure, increase of the filtering super-harmonic estimation error and the like are caused. Therefore, to achieve smooth switching of the coordinate system needs to be solved: 1. the mechanical arrangement of inertial navigation in a transverse coordinate system based on an ellipsoidal model of the earth is designed, which is simpler and less computationally intensive. 2. Conversion relation among navigation parameters under different navigation coordinate systems. 3. The conversion relation of error states and the conversion relation of covariance matrix involved in the integrated navigation system filter.
Aiming at the existing problems and oriented to long-endurance polar region navigation application, the invention provides an optimized polar region navigation switching method under an earth ellipsoid model, which is based on a Psi angle error model, builds more concise inertial navigation mechanical arrangement under an abscissa system, and builds a switching relation of navigation parameters and navigation system structures so as to maintain the continuity and consistency of combined navigation system parameters and filtering structures in the switching process; meanwhile, the complexity of the system scheme is reduced so as to reduce the system operation burden. The invention can improve the stability in the switching process of the integrated navigation system, can ensure the navigation precision while reducing the operation amount of the system, and has very important engineering significance.
Disclosure of Invention
In the prior art, hard switching is mostly adopted, and the influence on the continuity and consistency of the filtering structure of an inertial navigation system, particularly a combined navigation system, in the switching process is ignored, so that short-time estimation errors are increased. In addition, the mechanical arrangement is very complicated under the transverse coordinate system constructed based on the Phi angle error model under the earth ellipsoid model, and the parameter calculation is difficult, so that the system parameter switching scheme is also complicated, and the system operation burden is increased. The technical problem to be solved by the invention is as follows: aiming at the defects of the prior art, the invention provides an optimized cross-polar region navigation switching method under an ellipsoidal model of the earth, which is used for optimizing complex mechanical arrangement and lengthy parameter calculation schemes of a transverse coordinate system under the ellipsoidal model, and solves the problems of discontinuity and inconsistency of a filtering structure of a combined navigation system caused by navigation coordinate system switching in the process of entering and exiting polar regions of a ship so as to improve the stability of a filter in the switching process.
In order to solve the technical problems, the invention provides the following solutions:
a cross-region navigation switching method under an optimized earth ellipsoid model comprises the following steps:
(1) Defining a horizontal earth coordinate system, defining a horizontal pole, defining a horizontal longitude and a horizontal latitude, and determining a horizontal position representation mode: the origin of the horizontal earth coordinate system e' is positioned in the earth center, the X axis points to the north pole along the earth rotation axis, the Y axis points to the intersection point of the original meridian and the equator, and the Z axis passes through the intersection point of the east meridian of 90 degrees and the equator; defining (0 DEG, 90 DEG E) as a transverse north pole and (0 DEG, 90 DEG W) as a transverse south pole; defining a large ellipse formed by 0-degree warp yarns and 180-degree warp yarns as a transverse equator; defining a half major ellipse formed by 90 DEG E and 90 DEG W northern hemisphere parts as a 0 DEG transverse meridian, wherein the transverse primary meridian is the northern hemisphere part of a meridian where the geographic longitude 90 DEG E is located, and the transverse meridian is a contour line of a plane passing through a transverse pole intersecting with the earth surface; defining the intersection angle of the geographic normal line and the transverse equatorial plane of a point on the earth surface as the transverse latitude of the point; defining the intersection angle of the transverse meridian plane where the point is located and the transverse primary meridian plane as the transverse longitude; the position of the ship in the horizontal earth coordinate system is expressed as (L t ，λ t H), wherein L t Represents the latitude, lambda t Represents a horizontal longitude, h represents a height;
(2) Defining a horizontal geographic coordinate system: the origin of the transverse geographic coordinate system t is positioned at the center of the carrier, the Y axis points to the transverse north pole along the tangent of the transverse meridian, the Z axis points to the vertical direction perpendicular to the local horizontal plane, and the X axis, the Y axis and the Z axis form a right-hand coordinate system and are defined as transverse east-transverse north-vertical direction;
(3) The conversion relation between the coordinate systems is determined as follows:
determining a directional cosine matrix from the earth coordinate system e to the abscissa e' according to the abscissa definition in the step (1)The method comprises the following steps:
direction cosine matrix for determining earth coordinate system e to geographic coordinate system gThe method comprises the following steps:
wherein L represents the latitude of the ship, and lambda represents the longitude of the ship;
determining a directional cosine matrix of the abscissa e' to the abscissa t
Determining a directional cosine matrix from the geographic coordinate system g to the transverse geographic coordinate system t according to the chain rule
In the middle ofExpressed as a directional cosine matrix +.>Is a transpose of (2); sigma represents the angle between the abscissa t and the geographic g, and is specifically expressed as:
(4) Acquiring carrier posture, speed and position related information by utilizing inertial navigation, and determining a posture updating equation, a speed updating equation and a position updating equation under a horizontal geographic coordinate system, wherein the specific steps are as follows:
(4.1) determining an attitude update equation under the abscissa system:
in the method, in the process of the invention,a directional cosine matrix from the carrier coordinate system b to the transverse geographic coordinate system t; />Representing a projection of the rotational angular velocity of the carrier coordinate system b with respect to the inertial coordinate system i under the carrier coordinate system b; / >Representing a projection of a rotation angular velocity of the abscissa t with respect to the inertial coordinate i under the abscissa t;
wherein:representation of the groundProjection of the angular velocity of rotation of the spherical coordinate system e relative to the inertial coordinate system i in the abscissa t, +.>The projection of the rotation angular velocity of the abscissa t relative to the earth e under the abscissa t is expressed as:
wherein,representing projection of the rotation angular velocity of the earth in the earth coordinate system e, ω ie The magnitude of the rotation angular velocity of the earth; />A directional cosine matrix from the earth coordinate system e to the transverse geographic coordinate system t; />Representing a projection of the angular velocity of the geographic coordinate system g with respect to the earth coordinate system e under the geographic coordinate system g; />Representing a projection of a rotation angular velocity of the abscissa t with respect to the geographical g under the abscissa t;
and->The concrete steps are as follows:
wherein,and->Respectively representing the north speed and the east speed of the carrier under a geographic coordinate system; r is R M Representing the radius of the meridian at the carrier, R N The radius of the mortise circle at the carrier is expressed as follows:
wherein R is e Representing the radius of the long half axis of the earth, ρ representing the eccentricity of the earth;
determination of
In the method, in the process of the invention, And->The speed of the carrier in the east direction and the north direction under the horizontal geographic coordinate system t are respectively; />Representing the twist rate at the carrier in the abscissa system,/->And->The curvatures of the horizontal geographic east direction and the horizontal geographic north direction are respectively specifically expressed as:
(4.2) determining the velocity v in the abscissa t Is updated by the update equation:
in the formula, v t Representing the carrier speed in the abscissa t; f (f) b Representing the specific force represented under the carrier coordinate system b; g t Representing a gravity vector represented under a horizontal geographic coordinate system t;
(4.3) determining a location update equation under the abscissa system:
parameters described in step (4.1)The change is caused by the change of transverse longitude and transverse latitude, and is specifically expressed as:
comparing the parameters determined in step (4.1)Determining a transverse longitude and transverse latitude differential equation:
the altitude change is caused by the sky speed, and an altitude differential equation is determined:
in the method, in the process of the invention,the tangential velocity of the carrier in the horizontal geographic coordinate system t is represented;
(5) Determining a conversion relation among a calculation coordinate system, a platform coordinate system and a real navigation coordinate system:
determining a directional cosine matrix from the calculated coordinate system c to the platform coordinate system pThe method comprises the following steps:
determining a real navigation coordinate system t g Direction cosine matrix to platform coordinate system p The method comprises the following steps:
determining a real navigation coordinate system t g Direction cosine matrix to calculate geographic coordinate system cThe method comprises the following steps:
in which I 3×3 Representation ofA 3×3 identity matrix; psi is a drift error angle, phi is a posture error angle, and delta theta is a position error angle; the relation among the drift error angle, the attitude error angle and the position error angle is determined as follows:
φ＝ψ+δθ
(6) Determining a Kalman filtering model of a ship under a calculated horizontal geographic coordinate system, wherein the Kalman filtering model comprises the following steps of:
(6.1) determining a system state equation under the calculated abscissa system:
(6.1.1) determining a system error state under the calculated abscissa system:
x c' (t)＝[ψ c' δv c' δr c' ε b ▽ b δk δη δγ] T
wherein,representing the projection of the three-dimensional drift error angle vector under the calculated horizontal geographic coordinate system c ', wherein each component is the drift error angle of the east direction, the north direction and the sky direction under the calculated horizontal geographic coordinate system c';representing the projection of the three-dimensional speed error vector under the calculated horizontal geographic coordinate system c ', wherein each component is the speed error of the east direction, the north direction and the sky direction under the calculated horizontal geographic coordinate system c'; />Representing the projection of the three-dimensional position error vector under the calculated horizontal geographic coordinate system c ', wherein each component is the position error of the eastern direction, the northbound direction and the heaven direction under the calculated horizontal geographic coordinate system c'; />Zero offset vectors of the gyroscopes are represented, and each component is the zero offset of the X, Y, Z-axis gyroscopes; / >Zero offset vectors of the accelerometers are represented, and each component is the zero offset of the X, Y, Z-axis accelerometer; δk represents the error of the scale factor of the velocimeterThe method comprises the steps of carrying out a first treatment on the surface of the δη and δγ represent the pitch angle installation error and azimuth angle installation error of the velocimeter;
(6.1.2) determining and calculating the attitude, speed and position error equations of the inertial navigation system under the abscissa system:
in the method, in the process of the invention,representing the calculated earth rotation angular velocity in the abscissa c->Representing the angular velocity of the calculated abscissa c' relative to the earth e under the calculated abscissa c +.>Representing a directional cosine matrix from the carrier coordinate system b to the calculated abscissa coordinate system c', f c' Representing the calculated specific force represented under the abscissa c';
in the middle ofThe gyro error and the accelerometer error in the carrier coordinate system b are expressed as:
in the method, in the process of the invention,and->Noise representing gyroscopes and accelerometers, respectively;
wherein the differential equations of the gesture and the speed error are projected under a calculated abscissa system,are all known definite quantities, +.>Disturbance error term->0, in the differential equation of velocity error +.>Anddisturbance error term->And->All are 0, namely, error items related to the geographic position under the earth ellipsoid model do not exist;
(6.1.3) determining an error equation of gyro zero offset, accelerometer zero offset, velocimeter scale factor error, velocimeter pitch angle installation error and velocimeter azimuth angle installation error:
wherein τ ε And τ ▽ First order Markov correlation times, w, representing gyroscopes and accelerometers, respectively ε And w ▽ White gaussian noise representing gyroscopes and accelerometers, respectively;
(6.2) determining a speed observation equation of the velocimeter:
wherein,
in the method, in the process of the invention,representing a speed estimated value of the inertial navigation system and a speed output value of the velocimeter; v c' Representing the calculation of a velocity vector in the abscissa c';
(6.3) determining a system error state correction mode: the filtered system state vector is defined in the calculated abscissa system, and the system state is corrected to be defined in the abscissa system t:
φ t ＝ψ c' +δθ t
δv t ＝δv c' -δθ t ×v c'
in phi t Representing the projection of the attitude error angle phi in the transverse geographic coordinate system t; δv t Projection of the velocity error δv in a horizontal geographical coordinate system t;representing computed cross geographyA directional cosine matrix from the coordinate system c' to the transverse geographic coordinate system t; delta theta t Representing the projection of the position error angle δθ in the abscissa t, specifically expressed as:
in the method, in the process of the invention,δr respectively t North component, east component; δr t For projection of position error δr in the abscissa t, where longitude and latitude errors are converted into position error δr t Expressed as:
position error δr t The conversion into longitude and latitude errors is expressed as:
wherein the method comprises the steps ofAs the position error δr t An tangential component of (2);
(7) The method comprises the following specific steps of determining the conversion relation of system attitude, speed and position when a ship enters and leaves a polar region, and determining the conversion relation of an error state and a covariance matrix of a combined inertial/velocimeter combined navigation system:
and (7.1) when the navigation coordinate system of the entering polar region is switched to the horizontal geographic coordinate system, determining the conversion relation of the system gesture and the speed as follows:
in the method, in the process of the invention,a directional cosine matrix representing the carrier coordinate system b to the geographic coordinate system g; v g Representing carrier velocity in a geographic coordinate system;
the conversion relation of the positions is as follows:
and (7.2) when the navigation coordinate system of the driving-out polar region is switched to the geographic coordinate system, determining the conversion relation of the system gesture and the speed as follows:
in the method, in the process of the invention,a directional cosine matrix representing an abscissa t to an ordinate g;
determining a conversion relation of the position parameters:
(7.3) determining the error state conversion relation of the combined inertial/velocimeter combined navigation system, wherein the method comprises the following steps:
(7.3.1) determining the drift error angle ψ under the calculated abscissa system c' And calculating the drift error angle psi in the geographic coordinate system c Is a conversion relation of:
in the middle ofThe directional cosine matrix from the calculated geographic coordinate system c to the calculated transverse geographic coordinate system c' is specifically expressed as follows:
wherein,for the direction cosine matrix of the earth coordinate system e to the calculated geographical coordinate system c,/for the earth coordinate system e>A direction cosine matrix from the abscissa e 'to the calculated abscissa c';
(7.3.2) determining the velocity error δv in the calculated abscissa system c' And calculating velocity error δv in geographic coordinate system c Is a conversion relation of:
(7.3.3) determining the position error δr in the calculated abscissa c' And calculating position error delta r under geographic coordinate system c Is a conversion relation of:
(7.4) determining the conversion relation of the covariance matrix of the combined inertial/velocimeter combined navigation system, wherein the conversion relation comprises the following steps:
according to the step (7.3), when the ship enters the polar region, the conversion relationship of the error state is expressed as:
x c' (t)＝Φx c (t)
wherein x is c Representing a system error state under a calculated geographic coordinate system; phi represents the transition of the systematic error state from the calculated geographic coordinate system c to the calculated abscissac' is a conversion matrix with the following specific expression:
in the formula, diag {.cndot } is expressed as a block diagonal matrix; i 3×3 A 3×3 identity matrix; when a ship enters a polar region and is switched to the horizontal geographic coordinate system navigation, calculating a covariance matrix P of a system error state under a geographic coordinate system c c (t) and calculating a covariance matrix P of the systematic error states in the abscissa c c' The conversion relationship of (t) is expressed as:
in the method, in the process of the invention,representing the calculation of an error state estimate in the abscissa system,/>Representing calculating an error state estimation value under a geographic coordinate system;
when the ship leaves the polar region and switches to the geographic coordinate system for navigation, the error state and covariance matrix conversion relationship is expressed as follows:
x c (t)＝Φ- 1 x c' (t)，P c (t)＝Φ -1 P c' (t)Φ -T 。
furthermore, the combined navigation filter adopted by the invention adopts closed-loop feedback to the attitude error, the speed error, the position error, the gyroscope and the accelerometer zero offset of the system, the velocimeter scale factor error and the installation error adopt open-loop feedback, and the system error state correction of each closed-loop feedback is postponed by 0.
Further, if the carrier receives the position information of other sensors, including but not limited to GNSS position information, gravity matching position information, geomagnetic matching position information, the conversion relation is converted based on the received position informationOr->And carrying out correction and update.
Further, in the step (7), while the navigation coordinate system is switched, the velocity observables of the velocimeter are also switched, that is, the before-switching observables are projections of the velocity of the velocimeter under the calculated geographic coordinate system, and the after-switching observables are projections of the velocity of the velocimeter under the calculated horizontal geographic coordinate system.
In the step (7), the latitude is used as a threshold value for switching the navigation coordinate system, the latitude threshold value is selected according to practical situations, when the latitude of the position of the carrier is larger than the set threshold value, the navigation system is switched to the horizontal geographic coordinate system, and when the latitude of the position of the carrier is smaller than the threshold value, the navigation system is switched to the geographic coordinate system.
Furthermore, in the step (7), the states of the three systems including the gyro zero offset, the accelerometer zero offset, the velocimeter scale factor error, the velocimeter pitch angle installation error and the velocimeter azimuth angle installation error are kept consistent in different coordinate systems without conversion.
Compared with the prior art, the invention has the following advantages:
according to the invention, a more optimal transverse navigation arrangement scheme based on the earth ellipsoid model is constructed by using the characteristic that the earth ellipsoid model is more fit with the actual earth ellipsoid model and the Psi angle error model is defined in a calculation coordinate system, so that the system model is simplified, the model approximation error in the polar region is reduced, and the navigation precision is ensured. The scheme of combining the horizontal geographic coordinate system and the geographic coordinate system is adopted, so that inherent singular points of the polar region are avoided, and the practical requirement of full-latitude navigation is met; according to the requirement of cross-polar region navigation on the stability of the filter, an inertial/velocimeter combined navigation system navigation scheme is designed, the conversion relation between the error states of the filter and covariance matrixes under different coordinate systems is constructed, the problem of overshoot oscillation of the combined navigation system filter in the coordinate system switching process is solved, and smooth filtering is realized in the coordinate system switching process; the invention greatly reduces the calculation load of the system, reduces the errors of the system model and the calculation approximation errors, and improves the calculation efficiency of the system.
Drawings
Fig. 1 is a flowchart of a method provided in an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the following examples in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
As shown in fig. 1, a cross-region navigation switching method under an optimized ellipsoidal earth model is provided, and the specific implementation manner is as follows:
(1) Defining a horizontal earth coordinate system, defining a horizontal pole, defining a horizontal longitude and a horizontal latitude, and determining a horizontal position representation mode: the origin of the horizontal earth coordinate system e' is positioned in the earth center, the X axis points to the north pole along the earth rotation axis, the Y axis points to the intersection point of the original meridian and the equator, and the Z axis passes through the intersection point of the east meridian of 90 degrees and the equator; defining (0 DEG, 90 DEG E) as a transverse north pole and (0 DEG, 90 DEG W) as a transverse south pole; defining a large ellipse formed by 0-degree warp yarns and 180-degree warp yarns as a transverse equator; defining a half major ellipse formed by 90 DEG E and 90 DEG W northern hemisphere parts as a 0 DEG transverse meridian, wherein the transverse primary meridian is the northern hemisphere part of a meridian where the geographic longitude 90 DEG E is located, and the transverse meridian is a contour line of a plane passing through a transverse pole intersecting with the earth surface; defining the intersection angle of the geographic normal line and the transverse equatorial plane of a point on the earth surface as the transverse latitude of the point; defining the intersection angle of the transverse meridian plane where the point is located and the transverse primary meridian plane as the transverse longitude; the position of the ship in the horizontal earth coordinate system is expressed as (L t ，λ t H), wherein L t Represents the latitude, lambda t Represents a horizontal longitude, h represents a height;
(2) Defining a horizontal geographic coordinate system: the origin of the transverse geographic coordinate system t is positioned at the center of the carrier, the Y axis points to the transverse north pole along the tangent of the transverse meridian, the Z axis points to the vertical direction perpendicular to the local horizontal plane, and the X axis, the Y axis and the Z axis form a right-hand coordinate system and are defined as transverse east-transverse north-vertical direction;
(3) The conversion relation between the coordinate systems is determined as follows:
determining a directional cosine matrix from the earth coordinate system e to the abscissa e' according to the abscissa definition in the step (1)The method comprises the following steps:
direction cosine matrix for determining earth coordinate system e to geographic coordinate system gThe method comprises the following steps:
wherein L represents the latitude of the ship, and lambda represents the longitude of the ship;
determining a directional cosine matrix of the abscissa e' to the abscissa t
Determining a directional cosine matrix from the geographic coordinate system g to the transverse geographic coordinate system t according to the chain rule
In the middle ofExpressed as a directional cosine matrix +.>Is a transpose of (2); sigma represents the angle between the abscissa t and the geographic g, and is specifically expressed as:
(4) Acquiring carrier posture, speed and position related information by utilizing inertial navigation, and determining a posture updating equation, a speed updating equation and a position updating equation under a horizontal geographic coordinate system, wherein the specific steps are as follows:
(4.1) determining an attitude update equation under the abscissa system:
in the method, in the process of the invention,a directional cosine matrix from the carrier coordinate system b to the transverse geographic coordinate system t; />Representing a projection of the rotational angular velocity of the carrier coordinate system b with respect to the inertial coordinate system i under the carrier coordinate system b; />Representing a projection of a rotation angular velocity of the abscissa t with respect to the inertial coordinate i under the abscissa t;
wherein:representing the rotation of the earth coordinate system e relative to the inertial coordinate system iProjection of the angular velocity in the abscissa t, +.>The projection of the rotation angular velocity of the abscissa t relative to the earth e under the abscissa t is expressed as:
wherein,representing projection of the rotation angular velocity of the earth in the earth coordinate system e, ω ie The magnitude of the rotation angular velocity of the earth; />A directional cosine matrix from the earth coordinate system e to the transverse geographic coordinate system t; />Representing a projection of the angular velocity of the geographic coordinate system g with respect to the earth coordinate system e under the geographic coordinate system g; />Representing a projection of a rotation angular velocity of the abscissa t with respect to the geographical g under the abscissa t;
and->The concrete steps are as follows:
wherein,and->Respectively representing the north speed and the east speed of the carrier under a geographic coordinate system; r is R M Representing the radius of the meridian at the carrier, R N The radius of the mortise circle at the carrier is expressed as follows:
wherein R is e Representing the radius of the long half axis of the earth, ρ representing the eccentricity of the earth;
determination of
In the method, in the process of the invention,and->The speed of the carrier in the east direction and the north direction under the horizontal geographic coordinate system t are respectively; />Representing the twist rate at the carrier in the abscissa system,/->And->Respectively, in the horizontal direction of the east direction of the geographyAnd a curvature in the lateral geographic north direction, specifically expressed as:
(4.2) determining the velocity v in the abscissa t Is updated by the update equation:
in the formula, v t Representing the carrier speed in the abscissa t; f (f) b Representing the specific force represented under the carrier coordinate system b; g t Representing a gravity vector represented under a horizontal geographic coordinate system t;
(4.3) determining a location update equation under the abscissa system:
parameters described in step (4.1)The change is caused by the change of transverse longitude and transverse latitude, and is specifically expressed as:
comparing the parameters determined in step (4.1)Determining a transverse longitude and transverse latitude differential equation:
the altitude change is caused by the sky speed, and an altitude differential equation is determined:
in the method, in the process of the invention,the tangential velocity of the carrier in the horizontal geographic coordinate system t is represented;
(5) Determining a conversion relation among a calculation coordinate system, a platform coordinate system and a real navigation coordinate system:
Determining a directional cosine matrix from the calculated coordinate system c to the platform coordinate system pThe method comprises the following steps:
determining a real navigation coordinate system t g Direction cosine matrix to platform coordinate system pThe method comprises the following steps:
determining a real navigation coordinate system t g Direction cosine matrix to calculate geographic coordinate system cThe method comprises the following steps:
in which I 3×3 Representing 3×3A unit matrix; psi is a drift error angle, phi is a posture error angle, and delta theta is a position error angle; the relation among the drift error angle, the attitude error angle and the position error angle is determined as follows:
φ＝ψ+δθ
(6) Determining a Kalman filtering model of a ship under a calculated horizontal geographic coordinate system, wherein the Kalman filtering model comprises the following steps of:
(6.1) determining a system state equation under the calculated abscissa system:
(6.1.1) determining a system error state under the calculated abscissa system:
x c' (t)＝[ψ c' δv c' δr c' ε b ▽ b δk δη δγ] T
wherein,representing the projection of the three-dimensional drift error angle vector under the calculated horizontal geographic coordinate system c ', wherein each component is the drift error angle of the east direction, the north direction and the sky direction under the calculated horizontal geographic coordinate system c';representing the projection of the three-dimensional speed error vector under the calculated horizontal geographic coordinate system c ', wherein each component is the speed error of the east direction, the north direction and the sky direction under the calculated horizontal geographic coordinate system c'; />Representing the projection of the three-dimensional position error vector under the calculated horizontal geographic coordinate system c ', wherein each component is the position error of the eastern direction, the northbound direction and the heaven direction under the calculated horizontal geographic coordinate system c'; / >Zero offset vectors of the gyroscopes are represented, and each component is the zero offset of the X, Y, Z-axis gyroscopes; />Zero offset vectors of the accelerometers are represented, and each component is the zero offset of the X, Y, Z-axis accelerometer; δk represents the tachometer scale factor error;δη and δγ represent the pitch angle installation error and azimuth angle installation error of the velocimeter;
(6.1.2) determining and calculating the attitude, speed and position error equations of the inertial navigation system under the abscissa system:
in the method, in the process of the invention,representing the calculated earth rotation angular velocity in the abscissa c->Representing the angular velocity of the calculated abscissa c' relative to the earth e under the calculated abscissa c +.>Representing a directional cosine matrix from the carrier coordinate system b to the calculated abscissa coordinate system c', f c' Representing the calculated specific force represented under the abscissa c';
in the middle ofThe gyro error and the accelerometer error in the carrier coordinate system b are expressed as:
in the method, in the process of the invention,and->Noise representing gyroscopes and accelerometers, respectively;
wherein the differential equations of the gesture and the speed error are projected under a calculated abscissa system,are all known definite quantities, +.>Disturbance error term->0, in the differential equation of velocity error +. >Anddisturbance error term->And->All are 0, namely, error items related to the geographic position under the earth ellipsoid model do not exist;
(6.1.3) determining an error equation of gyro zero offset, accelerometer zero offset, velocimeter scale factor error, velocimeter pitch angle installation error and velocimeter azimuth angle installation error:
wherein τ ε Andfirst order Markov correlation times, w, representing gyroscopes and accelerometers, respectively ε And->White gaussian noise representing gyroscopes and accelerometers, respectively;
(6.2) determining a speed observation equation of the velocimeter:
wherein,
in the method, in the process of the invention,representing a speed estimated value of the inertial navigation system and a speed output value of the velocimeter; v c' Representing the calculation of a velocity vector in the abscissa c';
(6.3) determining a system error state correction mode: the filtered system state vector is defined in the calculated abscissa system, and the system state is corrected to be defined in the abscissa system t:
φ t ＝ψ c' +δθ t
δv t ＝δv c' -δθ t ×v c'
in phi t Representing the geographic position of the attitude error angle phiProjection in coordinate system t; δv t Projection of the velocity error δv in a horizontal geographical coordinate system t;representing a direction cosine matrix from the calculated abscissa c' to the abscissa t; delta theta t Representing the projection of the position error angle δθ in the abscissa t, specifically expressed as:
In the method, in the process of the invention,δr respectively t North component, east component; δr t For projection of position error δr in the abscissa t, where longitude and latitude errors are converted into position error δr t Expressed as:
δr t ＝[(R N +h)δλ t cosL t (R M +h)δL t δh] T
position error δr t The conversion into longitude and latitude errors is expressed as:
wherein the method comprises the steps ofAs the position error δr t An tangential component of (2);
(7) The method comprises the following specific steps of determining the conversion relation of system attitude, speed and position when a ship enters and leaves a polar region, and determining the conversion relation of an error state and a covariance matrix of a combined inertial/velocimeter combined navigation system:
and (7.1) when the navigation coordinate system of the entering polar region is switched to the horizontal geographic coordinate system, determining the conversion relation of the system gesture and the speed as follows:
in the method, in the process of the invention,a directional cosine matrix representing the carrier coordinate system b to the geographic coordinate system g; v g Representing carrier velocity in a geographic coordinate system;
the conversion relation of the positions is as follows:
and (7.2) when the navigation coordinate system of the driving-out polar region is switched to the geographic coordinate system, determining the conversion relation of the system gesture and the speed as follows:
in the method, in the process of the invention,a directional cosine matrix representing an abscissa t to an ordinate g;
determining a conversion relation of the position parameters:
(7.3) determining the error state conversion relation of the combined inertial/velocimeter combined navigation system, wherein the method comprises the following steps:
(7.3.1) determining the drift error angle ψ under the calculated abscissa system c' And calculating the drift error angle psi in the geographic coordinate system c Is a conversion relation of:
in the middle ofThe directional cosine matrix from the calculated geographic coordinate system c to the calculated transverse geographic coordinate system c' is specifically expressed as follows:
wherein,for the direction cosine matrix of the earth coordinate system e to the calculated geographical coordinate system c,/for the earth coordinate system e>A direction cosine matrix from the abscissa e 'to the calculated abscissa c';
(7.3.2) determining the velocity error δv in the calculated abscissa system c' And calculating velocity error δv in geographic coordinate system c Is a conversion relation of:
(7.3.3) determining the position error δr in the calculated abscissa c' And calculating position error delta r under geographic coordinate system c Is a conversion relation of:
(7.4) determining the conversion relation of the covariance matrix of the combined inertial/velocimeter combined navigation system, wherein the conversion relation comprises the following steps:
according to the step (7.3), when the ship enters the polar region, the conversion relationship of the error state is expressed as:
x c' (t)＝Φx c (t)
wherein x is c Representing a system error state under a calculated geographic coordinate system; phi represents a conversion matrix for converting the system error state from a calculated geographic coordinate system c to a calculated transverse geographic coordinate system c', and the specific expression is as follows:
in the formula, diag {.cndot } is expressed as a block diagonal matrix; i 3×3 A 3×3 identity matrix; when a ship enters a polar region and is switched to the horizontal geographic coordinate system navigation, calculating a covariance matrix P of a system error state under a geographic coordinate system c c (t) and calculating a covariance matrix P of the systematic error states in the abscissa c c' The conversion relationship of (t) is expressed as:
in the method, in the process of the invention,representing the calculation of an error state estimate in the abscissa system,/>Representing calculating an error state estimation value under a geographic coordinate system;
when the ship leaves the polar region and switches to the geographic coordinate system for navigation, the error state and covariance matrix conversion relationship is expressed as follows:
x c (t)＝Φ- 1 x c' (t)，P c (t)＝Φ -1 P c' (t)Φ -T 。
furthermore, the combined navigation filter adopted by the invention adopts closed-loop feedback to the attitude error, the speed error, the position error, the gyroscope and the accelerometer zero offset of the system, the velocimeter scale factor error and the installation error adopt open-loop feedback, and the system error state correction of each closed-loop feedback is postponed by 0.
Further, if the carrier receives the position information of other sensors, including but not limited to GNSS position information, gravity matching position information, geomagnetic matching position information, the conversion relation is converted based on the received position informationOr->And carrying out correction and update.
Further, in the step (7), while the navigation coordinate system is switched, the velocity observables of the velocimeter are also switched, that is, the before-switching observables are projections of the velocity of the velocimeter under the calculated geographic coordinate system, and the after-switching observables are projections of the velocity of the velocimeter under the calculated horizontal geographic coordinate system.
In the step (7), the latitude is used as a threshold value for switching the navigation coordinate system, the latitude threshold value is selected according to practical situations, when the latitude of the position of the carrier is larger than the set threshold value, the navigation system is switched to the horizontal geographic coordinate system, and when the latitude of the position of the carrier is smaller than the threshold value, the navigation system is switched to the geographic coordinate system.
Furthermore, in the step (7), the states of the three systems including the gyro zero offset, the accelerometer zero offset, the velocimeter scale factor error, the velocimeter pitch angle installation error and the velocimeter azimuth angle installation error are kept consistent in different coordinate systems without conversion.
The foregoing is only a preferred embodiment of the present invention, and is not intended to limit the present invention, and all technical solutions belonging to the present invention are within the scope of the present invention. Improvements and modifications and the like without departing from the principles of the invention are also considered within the scope of the invention.
Claims (6)
1. The cross-region navigation switching method under the optimized earth ellipsoid model is characterized by comprising the following steps of:
(1) Defining an abscissa-earth coordinate system, defining a transverse pole, defining a transverse longitude and a transverse latitude, determining Lateral position representation: the origin of the horizontal earth coordinate system e' is positioned in the earth center, the X axis points to the north pole along the earth rotation axis, the Y axis points to the intersection point of the original meridian and the equator, and the Z axis passes through the intersection point of the east meridian of 90 degrees and the equator; defining (0 DEG, 90 DEG E) as a transverse north pole and (0 DEG, 90 DEG W) as a transverse south pole; defining a large ellipse formed by 0-degree warp yarns and 180-degree warp yarns as a transverse equator; defining a half major ellipse formed by 90 DEG E and 90 DEG W northern hemisphere parts as a 0 DEG transverse meridian, wherein the transverse primary meridian is the northern hemisphere part of a meridian where the geographic longitude 90 DEG E is located, and the transverse meridian is a contour line of a plane passing through a transverse pole intersecting with the earth surface; defining the intersection angle of the geographic normal line and the transverse equatorial plane of a point on the earth surface as the transverse latitude of the point; defining the intersection angle of the transverse meridian plane where the point is located and the transverse primary meridian plane as the transverse longitude; the position of the ship in the horizontal earth coordinate system is expressed as (L t ，λ t H), wherein L t Represents the latitude, lambda t Represents a horizontal longitude, h represents a height;
(2) Defining a horizontal geographic coordinate system: the origin of the transverse geographic coordinate system t is positioned at the center of the carrier, the Y axis points to the transverse north pole along the tangent of the transverse meridian, the Z axis points to the vertical direction perpendicular to the local horizontal plane, and the X axis, the Y axis and the Z axis form a right-hand coordinate system and are defined as transverse east-transverse north-vertical direction;
(3) The conversion relation between the coordinate systems is determined as follows:
determining a directional cosine matrix from the earth coordinate system e to the abscissa e' according to the abscissa definition in the step (1)The method comprises the following steps:
direction cosine matrix for determining earth coordinate system e to geographic coordinate system gThe method comprises the following steps:
wherein L represents the latitude of the ship, and lambda represents the longitude of the ship;
determining a directional cosine matrix of the abscissa e' to the abscissa t
Determining a directional cosine matrix from the geographic coordinate system g to the transverse geographic coordinate system t according to the chain rule
In the middle ofExpressed as a directional cosine matrix +.>Is a transpose of (2); sigma represents the angle between the abscissa t and the geographic g, and is specifically expressed as:
(4) Acquiring carrier posture, speed and position related information by utilizing inertial navigation, and determining a posture updating equation, a speed updating equation and a position updating equation under a horizontal geographic coordinate system, wherein the specific steps are as follows:
(4.1) determining an attitude update equation under the abscissa system:
in the method, in the process of the invention,a directional cosine matrix from the carrier coordinate system b to the transverse geographic coordinate system t; />Representing a projection of the rotational angular velocity of the carrier coordinate system b with respect to the inertial coordinate system i under the carrier coordinate system b; / >Representing a projection of a rotation angular velocity of the abscissa t with respect to the inertial coordinate i under the abscissa t;
wherein: representing the projection of the angular velocity of rotation of the earth coordinate system e with respect to the inertial coordinate system i in the abscissa t,/->Representing rotational angular velocity of the abscissa t relative to the earth eProjection of the degree in the abscissa t is specifically expressed as:
wherein,representing projection of the rotation angular velocity of the earth in the earth coordinate system e, ω ie The magnitude of the rotation angular velocity of the earth; />A directional cosine matrix from the earth coordinate system e to the transverse geographic coordinate system t; />Representing a projection of the angular velocity of the geographic coordinate system g with respect to the earth coordinate system e under the geographic coordinate system g; />Representing a projection of a rotation angular velocity of the abscissa t with respect to the geographical g under the abscissa t;
and->The concrete steps are as follows:
wherein,and->Respectively representing the north speed and the east speed of the carrier under a geographic coordinate system; r is R M Representing the radius of the meridian at the carrier, R N The radius of the mortise circle at the carrier is expressed as follows:
wherein R is e Representing the radius of the long half axis of the earth, ρ representing the eccentricity of the earth;
determination of
In the method, in the process of the invention, And->The speed of the carrier in the east direction and the north direction under the horizontal geographic coordinate system t are respectively; />Representing in the horizontal geographical coordinate systemTwist rate at the carrier,/->And->The curvatures of the horizontal geographic east direction and the horizontal geographic north direction are respectively specifically expressed as:
(4.2) determining the velocity v in the abscissa t Is updated by the update equation:
in the formula, v t Representing the carrier speed in the abscissa t; f (f) b Representing the specific force represented under the carrier coordinate system b; g t Representing a gravity vector represented under a horizontal geographic coordinate system t;
(4.3) determining a location update equation under the abscissa system:
parameters described in step (4.1)The change is caused by the change of transverse longitude and transverse latitude, and is specifically expressed as:
comparing the parameters determined in step (4.1)Determining a transverse longitude and transverse latitude differential equation:
the altitude change is caused by the sky speed, and an altitude differential equation is determined:
in the method, in the process of the invention,the tangential velocity of the carrier in the horizontal geographic coordinate system t is represented;
(5) Determining a conversion relation among a calculation coordinate system, a platform coordinate system and a real navigation coordinate system:
determining a directional cosine matrix from the calculated coordinate system c to the platform coordinate system pThe method comprises the following steps:
determining a real navigation coordinate system t g Direction cosine matrix to platform coordinate system p The method comprises the following steps:
determining a real navigation coordinate system t g Direction cosine matrix to calculate geographic coordinate system cThe method comprises the following steps:
in which I 3×3 Representing a 3 x 3 identity matrix; psi is a drift error angle, phi is a posture error angle, and delta theta is a position error angle;
the relation among the drift error angle, the attitude error angle and the position error angle is determined as follows:
φ＝ψ+δθ
(6) Determining a Kalman filtering model of a ship under a calculated horizontal geographic coordinate system, wherein the Kalman filtering model comprises the following steps of:
(6.1) determining a system state equation under the calculated abscissa system:
(6.1.1) determining a system error state under the calculated abscissa system:
wherein,representing the projection of the three-dimensional drift error angle vector under the calculated horizontal geographic coordinate system c ', wherein each component is the drift error angle of the east direction, the north direction and the sky direction under the calculated horizontal geographic coordinate system c';representing the projection of the three-dimensional speed error vector under the calculated horizontal geographic coordinate system c ', wherein each component is the speed error of the east direction, the north direction and the sky direction under the calculated horizontal geographic coordinate system c'; />Representing the projection of the three-dimensional position error vector under the calculated horizontal geographic coordinate system c ', wherein each component is the position error of the eastern direction, the northbound direction and the heaven direction under the calculated horizontal geographic coordinate system c'; />Zero offset vectors of the gyroscopes are represented, and each component is the zero offset of the X, Y, Z-axis gyroscopes; / >Zero offset vectors of the accelerometers are represented, and each component is the zero offset of the X, Y, Z-axis accelerometer; δk represents the tachometer scale factor error; δη and δγ represent the pitch angle installation error and azimuth angle installation error of the velocimeter;
(6.1.2) determining and calculating the attitude, speed and position error equations of the inertial navigation system under the abscissa system:
in the method, in the process of the invention,representing the calculated earth rotation angular velocity in the abscissa c->Representing the angular velocity of the calculated abscissa c' relative to the earth e under the calculated abscissa c +.>Representing a directional cosine matrix from the carrier coordinate system b to the calculated abscissa coordinate system c', f c' Representing the calculated specific force represented under the abscissa c';
in the middle ofThe gyro error and the accelerometer error in the carrier coordinate system b are expressed as:
in the method, in the process of the invention,and->Noise representing gyroscopes and accelerometers, respectively;
(6.1.3) determining an error equation of gyro zero offset, accelerometer zero offset, velocimeter scale factor error, velocimeter pitch angle installation error and velocimeter azimuth angle installation error:
wherein τ ε Andfirst order Markov correlation times, w, representing gyroscopes and accelerometers, respectively ε And->White gaussian noise representing gyroscopes and accelerometers, respectively;
(6.2) determining a speed observation equation of the velocimeter:
wherein,
in the method, in the process of the invention,representing a speed estimated value of the inertial navigation system and a speed output value of the velocimeter; v c' Representing the calculation of a velocity vector in the abscissa c';
(6.3) determining a system error state correction mode: the filtered system state vector is defined in the calculated abscissa system, and the system state is corrected to be defined in the abscissa system t:
φ t ＝ψ c' +δθ t
δv t ＝δv c' -δθ t ×v c'
in phi t Representing the projection of the attitude error angle phi in the transverse geographic coordinate system t; δv t Projection of the velocity error δv in a horizontal geographical coordinate system t;representing a direction cosine matrix from the calculated abscissa c' to the abscissa t; delta theta t Representing the projection of the position error angle δθ in the abscissa t, specifically expressed as:
in the method, in the process of the invention,δr respectively t North component, east component; δr t For projection of position error δr in the abscissa t, where longitude and latitude errors are converted into position error δr t Expressed as:
δr t ＝[(R N +h)δλ t cosL t (R M +h)δL t δh] T
position error δr t The conversion into longitude and latitude errors is expressed as:
wherein the method comprises the steps ofAs the position error δr t An tangential component of (2);
(7) The method comprises the following specific steps of determining the conversion relation of system attitude, speed and position when a ship enters and leaves a polar region, and determining the conversion relation of an error state and a covariance matrix of a combined inertial/velocimeter combined navigation system:
And (7.1) when the navigation coordinate system of the entering polar region is switched to the horizontal geographic coordinate system, determining the conversion relation of the system gesture and the speed as follows:
in the method, in the process of the invention,a directional cosine matrix representing the carrier coordinate system b to the geographic coordinate system g; v g Representing carrier velocity in a geographic coordinate system;
the conversion relation of the positions is as follows:
and (7.2) when the navigation coordinate system of the driving-out polar region is switched to the geographic coordinate system, determining the conversion relation of the system gesture and the speed as follows:
in the method, in the process of the invention,a directional cosine matrix representing an abscissa t to an ordinate g;
determining a conversion relation of the position parameters:
(7.3) determining the error state conversion relation of the combined inertial/velocimeter combined navigation system, wherein the method comprises the following steps:
(7.3.1) determining the drift error angle ψ under the calculated abscissa system c' And calculating the drift error angle psi in the geographic coordinate system c Is a conversion relation of:
in the middle ofThe directional cosine matrix from the calculated geographic coordinate system c to the calculated transverse geographic coordinate system c' is specifically expressed as follows:
wherein,for the direction cosine matrix of the earth coordinate system e to the calculated geographical coordinate system c,/for the earth coordinate system e>Direction cosine matrix for abscissa e' to calculate abscissa c；
(7.3.2) determining the velocity error δv in the calculated abscissa system c' And calculating velocity error δv in geographic coordinate system c Is a conversion relation of:
(7.3.3) determining the position error δr in the calculated abscissa c' And calculating position error delta r under geographic coordinate system c Is a conversion relation of:
(7.4) determining the conversion relation of the covariance matrix of the combined inertial/velocimeter combined navigation system, wherein the conversion relation comprises the following steps:
according to the step (7.3), when the ship enters the polar region, the conversion relationship of the error state is expressed as:
x c' (t)＝Φx c (t)
wherein x is c Representing a system error state under a calculated geographic coordinate system; x is x c Representing a system error state under a calculated geographic coordinate system; phi represents a conversion matrix for converting the system error state from a calculated geographic coordinate system c to a calculated transverse geographic coordinate system c', and the specific expression is as follows:
in the formula, diag {.cndot } is expressed as a block diagonal matrix; i 3×3 A 3×3 identity matrix; when a ship enters a polar region and is switched to the horizontal geographic coordinate system navigation, calculating a covariance matrix P of a system error state under a geographic coordinate system c c (t) and calculating a covariance matrix P of the systematic error states in the abscissa c c' The conversion relationship of (t) is expressed as:
in the method, in the process of the invention,representing the calculation of an error state estimate in the abscissa system,/>Representing calculating an error state estimation value under a geographic coordinate system;
When the ship leaves the polar region and switches to the geographic coordinate system for navigation, the error state and covariance matrix conversion relationship is expressed as follows:
x c (t)＝Φ -1 x c' (t)，P c (t)＝Φ -1 P c' (t)Φ -T 。
2. the method for cross-polar region navigation switching under an optimized earth ellipsoid model according to claim 1, wherein the combined navigation filter adopts closed-loop feedback for attitude error, speed error, position error, gyro and accelerometer zero bias of the system, the tachometer scale factor error and installation error adopts open-loop feedback, and the system error state correction of each closed-loop feedback is postponed by 0.
3. The method of claim 1, wherein if the carrier receives the position information of the other sensors, the conversion relation is based on the received position informationOr->And carrying out correction and update.
4. The method of claim 1, wherein in the step (7), the navigation coordinate system is switched, and the velocity observables of the velocimeter are switched, that is, the pre-switching observables are projections of the velocity of the velocimeter in the calculated geographic coordinate system, and the post-switching observables are projections of the velocity of the velocimeter in the calculated transverse geographic coordinate system.
5. The method for switching navigation in a cross-polar region under an optimized ellipsoidal earth model as claimed in claim 1, wherein in the step (7), the switching of the navigation coordinate system takes latitude as a threshold, the selection of the latitude threshold is selected according to practical situations, when the latitude of the carrier is greater than a set threshold, the navigation system is switched to the horizontal geographic coordinate system, and when the latitude of the carrier is less than the threshold, the navigation system is switched to the geographic coordinate system.
6. The method for cross-polar region navigation switching under an optimized ellipsoidal earth model according to claim 1, wherein in the step (7), three system states of gyro zero bias, accelerometer zero bias, tachometer scale factor error, tachometer pitch angle installation error, and tachometer azimuth angle installation error are kept consistent in different coordinate systems without conversion.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311492702.1A CN117537811A (en) | 2023-11-10 | 2023-11-10 | Cross-polar region navigation switching method under optimized earth ellipsoid model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311492702.1A CN117537811A (en) | 2023-11-10 | 2023-11-10 | Cross-polar region navigation switching method under optimized earth ellipsoid model |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117537811A true CN117537811A (en) | 2024-02-09 |
Family
ID=89795060
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311492702.1A Pending CN117537811A (en) | 2023-11-10 | 2023-11-10 | Cross-polar region navigation switching method under optimized earth ellipsoid model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117537811A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115200574A (en) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | Polar region transverse combination navigation method under earth ellipsoid model |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108426575A (en) * | 2018-02-12 | 2018-08-21 | 中国人民解放军海军工程大学 | With the improved inertial navigation polar region transverse direction air navigation aid of ellipsoid model of globe |
| AU2020101525A4 (en) * | 2020-07-28 | 2020-09-03 | Xi`an University of Architecture and Technology | Inertial basis combined navigation method for indirect grid frame in polar zone |
| AU2020103939A4 (en) * | 2020-12-08 | 2021-02-11 | Xi'an University Of Architecture And Technology | Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework |
| CN115200574A (en) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | Polar region transverse combination navigation method under earth ellipsoid model |
| CN116222618A (en) * | 2023-03-11 | 2023-06-06 | 中国人民解放军国防科技大学 | Double-inertial navigation collaborative calibration method under polar environment |
| CN116481564A (en) * | 2023-03-11 | 2023-07-25 | 中国人民解放军国防科技大学 | Polar region double-inertial navigation collaborative calibration method based on Psi angle error correction model |
| CN116734887A (en) * | 2023-03-11 | 2023-09-12 | 中国人民解放军国防科技大学 | Polar region double-inertial navigation collaborative calibration method based on speed error correction model |
-
2023
- 2023-11-10 CN CN202311492702.1A patent/CN117537811A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108426575A (en) * | 2018-02-12 | 2018-08-21 | 中国人民解放军海军工程大学 | With the improved inertial navigation polar region transverse direction air navigation aid of ellipsoid model of globe |
| AU2020101525A4 (en) * | 2020-07-28 | 2020-09-03 | Xi`an University of Architecture and Technology | Inertial basis combined navigation method for indirect grid frame in polar zone |
| AU2020103939A4 (en) * | 2020-12-08 | 2021-02-11 | Xi'an University Of Architecture And Technology | Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework |
| CN115200574A (en) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | Polar region transverse combination navigation method under earth ellipsoid model |
| CN116222618A (en) * | 2023-03-11 | 2023-06-06 | 中国人民解放军国防科技大学 | Double-inertial navigation collaborative calibration method under polar environment |
| CN116481564A (en) * | 2023-03-11 | 2023-07-25 | 中国人民解放军国防科技大学 | Polar region double-inertial navigation collaborative calibration method based on Psi angle error correction model |
| CN116734887A (en) * | 2023-03-11 | 2023-09-12 | 中国人民解放军国防科技大学 | Polar region double-inertial navigation collaborative calibration method based on speed error correction model |
Non-Patent Citations (1)
| Title |
|---|
| 徐晓苏等: "基于横向地理坐标系的极区惯性导航方法研究", 《华中科技大学学报》, vol. 42, no. 12, 31 December 2014 (2014-12-31), pages 116 - 121 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115200574A (en) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | Polar region transverse combination navigation method under earth ellipsoid model |
| CN115200574B (en) * | 2022-07-03 | 2024-04-19 | 中国人民解放军国防科技大学 | Polar region transverse combined navigation method under earth ellipsoid model |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115200574B (en) | Polar region transverse combined navigation method under earth ellipsoid model | |
| CN112097763B (en) | Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination | |
| CN109211269B (en) | Attitude angle error calibration method for double-shaft rotary inertial navigation system | |
| CN117537811A (en) | Cross-polar region navigation switching method under optimized earth ellipsoid model | |
| CN111595348B (en) | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system | |
| CN101187567A (en) | Optical fiber gyroscope strap-down inertial navigation system initial posture determination method | |
| CN110457813B (en) | Virtual polar region method based on transverse geographic coordinate system | |
| CN103915673A (en) | Onboard A-E-C three-axis satellite communication antenna beam pointing-tracking control method | |
| CN108303079B (en) | Data smoothing method for underwater USBL reverse application | |
| CN106840194B (en) | A kind of Large azimuth angle linear alignment method | |
| CN103913163A (en) | Method for calculating A-E-C triaxial antenna coordinate of shipborne satellite communication earth station | |
| CN108426575A (en) | With the improved inertial navigation polar region transverse direction air navigation aid of ellipsoid model of globe | |
| CN116481564B (en) | Polar region double-inertial navigation collaborative calibration method based on Psi angle error correction model | |
| CN111380519B (en) | Navigation error correction method for ultrashort baseline/strapdown inertial navigation loose combination | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| CN116734887A (en) | Polar region double-inertial navigation collaborative calibration method based on speed error correction model | |
| CN109708670A (en) | The noise compensation method of polar region Transfer Alignment deflection deformation based on improved Sage-Husa adaptive Kalman filter | |
| CN113568442A (en) | Satellite alignment control system and method | |
| CN110873577B (en) | Underwater rapid-acting base alignment method and device | |
| CN110388942B (en) | Vehicle-mounted posture fine alignment system based on angle and speed increment | |
| CN117470234B (en) | Ship cross-polar region filtering switching method based on Psi angle error model | |
| CN117516518B (en) | Cross-polar region damping switching method under earth ellipsoid model | |
| CN117516520B (en) | Polar region optimal damping method under optimized earth ellipsoid model | |
| CN117470235B (en) | Optimized long-endurance cross-polar region damping switching method under earth ellipsoid model | |
| CN117470233A (en) | Combined navigation method of abscissa system under optimized earth ellipsoid model |
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 |