CN113051757B - Strapdown inertial navigation generalized PSI angle error model construction method - Google Patents
Strapdown inertial navigation generalized PSI angle error model construction method Download PDFInfo
- Publication number
- CN113051757B CN113051757B CN202110312139.XA CN202110312139A CN113051757B CN 113051757 B CN113051757 B CN 113051757B CN 202110312139 A CN202110312139 A CN 202110312139A CN 113051757 B CN113051757 B CN 113051757B
- Authority
- CN
- China
- Prior art keywords
- error
- equation
- generalized
- error equation
- attitude
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- 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
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/02—Reliability analysis or reliability optimisation; Failure analysis, e.g. worst case scenario performance, failure mode and effects analysis [FMEA]
Abstract
The invention discloses a strapdown inertial navigation generalized PSI angle error model construction method, which decomposes a carrier attitude matrix into a carrier angular motion excitation item and a linear motion excitation item through coordinate system decomposition, wherein an error equation corresponding to the angular motion excitation item is an attitude error equation in the generalized PSI angle error model, and an error equation corresponding to the linear motion excitation item is a position error equation in the generalized PSI angle error model. On the premise of not introducing an approximate speed error term, the method has the advantage that an attitude angle error model is independent of speed and position errors; meanwhile, all error equation deductions strictly follow a chain multiplication rule of an attitude matrix (quaternion), so that the deduction process is clear on one hand, and the deduced error equation is more directly corrected in the inertial navigation calculation result in the filtering application on the other hand; in addition, the velocity error equation takes into accountThe error contained in the method improves the accuracy of the error model.
Description
Technical Field
The invention relates to the technical field of error analysis of a strapdown inertial navigation system, in particular to a strapdown inertial navigation generalized PSI angle error model construction method based on coordinate system decomposition.
Background
The inertial navigation system is an autonomous navigation system which does not depend on any external information and does not radiate energy to the outside, has the characteristics of good concealment and can work in various complex environments such as air, ground, underwater and the like, and is mainly divided into a platform type inertial navigation system and a strapdown type inertial navigation system. The strapdown inertial navigation system is developed on the basis of a platform type inertial navigation system, is a frameless system and consists of three rate gyros, three linear accelerometers and an airborne computer. The gyroscope and the accelerometer are directly and fixedly connected to the carrier and are respectively used for measuring angular velocity information and linear motion information of the carrier, and the onboard computer calculates the course, the attitude, the speed and the position of the carrier according to the measurement information. Because a complex electromechanical platform is omitted, the strapdown inertial navigation system has the advantages of simple structure, small volume, light weight, low cost, simple maintenance, high reliability and the like, and the fault-tolerant capability of the strapdown inertial navigation system can be improved through a redundancy technology; meanwhile, with the appearance of solid-state inertial devices such as laser gyroscopes and fiber-optic gyroscopes, the rapid development of computer technology and the increasing perfection of computing theory, the superiority of strapdown inertial navigation systems is gradually revealed.
The PSI angle error model of the strapdown inertial navigation system is a concept introduced from a platform type inertial navigation error equation and mainly relates to a carrier coordinate system b, a local real horizontal geographic coordinate system t, a calculation geographic coordinate system c and a calculation platform coordinate system p. In fact, the calculated geographic coordinate system c and the calculated platform coordinate system p are both calculated local horizontal coordinate systems, except that the excitation errors are not consistent, and the so-called PSI angle is the error between the c-system and the p-system. Due to the special definition of the c system and the p system, the physical meaning of the PSI angle is not clear, so that the attitude correction process in the filtering application is not intuitive. In addition, the introduction of the c system in the PSI angle error model ensures that the derivation process of the speed error equation assumesThe method does not contain errors, although the form of a speed error equation is simpler, the result is not strictly established, so that a PSI angle error model exists compared with a disturbance angle PHI error modelLess accurate problem.
Disclosure of Invention
Aiming at the problems, the invention provides a strapdown inertial navigation generalized PSI angle error model construction method based on coordinate system decomposition.
The invention protects a strapdown inertial navigation generalized PSI angle error model, the attitude error equation of which isThe position error equation isWherein θ [ - δ L δ λ cosL δ λ sinL] T The velocity error equation is
the invention also protects a construction method of the strapdown inertial navigation generalized PSI angle error model, which comprises the following steps:
the attitude error equation derivation comprises the following steps:
Step A4, combining the relation of quaternion and three-dimensional vector multiplication to obtain
in step A5, it is assumed that the angle between the calculated carrier coordinate system b' and the real carrier coordinate system b is a small angle, that is, the included angle isThe error quaternion may be approximated by δ q 1 =[1 α T /2] T Derivation ofThe equation of attitude error can be obtained
The position error equation derivation comprises the following steps:
Step B2, differentiating the above formula to obtainWherein the content of the first and second substances,
Step B5, under the assumption of small angle, the error quaternion can be approximated to be δ q 2 =[1 θ T /2] T Derived position error equation of
The derivation of the speed error equation comprises the following steps:
Step C2, differentiating the above formula and further calculating to obtain
Step C3, further calculating the available speed error equation according to the step C2
The carrier attitude matrix is decomposed into a carrier angular motion excitation term and a linear motion excitation term through coordinate system decomposition, wherein an error equation corresponding to the angular motion excitation term is an attitude error equation in the generalized PSI angular error model, and an error equation corresponding to the linear motion excitation term is a position error equation in the generalized PSI angular error model. On the premise of not introducing an approximate speed error term, the method has the advantage that an attitude angle error model is independent of speed and position errors, which is referred to in a 'broad sense'; meanwhile, all error equation deductions strictly follow a chain multiplication rule of an attitude matrix (quaternion), so that the deduction process is clear on one hand, and the deduced error equation is more directly corrected in the inertial navigation calculation result in the filtering application on the other hand; in addition, the velocity error equation takes into accountThe error contained in the method improves the accuracy of the error model.
Drawings
Fig. 1 is an application flow of the PSI angle error model established in the present invention in the combined filtering.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments. The embodiments of the present invention have been presented for purposes of illustration and description, and are not intended to be exhaustive or limited to the invention in the form disclosed. Many modifications and variations will be apparent to those of ordinary skill in the art. The embodiment was chosen and described in order to best explain the principles of the invention and the practical application, and to enable others of ordinary skill in the art to understand the invention for various embodiments with various modifications as are suited to the particular use contemplated.
The method for constructing the strapdown inertial navigation generalized PSI angle error model comprises the following steps of:
firstly, constructing a coordinate system
1. And a carrier coordinate system b, referred to as a b system for short, wherein the origin is the center of three axes of the inertial navigation system, the x axis points to the right of the carrier, and the y axis points to the front of the carrier, thereby forming a right-hand rectangular coordinate system Oxyz. And b is that the axial directions of the three shafts are respectively along the transverse rolling shaft, the pitching shaft and the yawing shaft of the carrier.
2. An inertial coordinate system i, i system for short, wherein an origin O is located at the earth centroid, coordinate axes are not rotated relative to an inertial space, an x axis and a y axis are located in the earth equatorial plane, the x axis points to the vernal point, and the z axis points to the earth rotation axis, so that a right-hand rectangular coordinate system Oxyz is formed.
3. And an earth coordinate system e, for short, an e system, wherein the origin is positioned at the center of the earth, the coordinate axis is fixedly connected with the earth, the z axis is along the direction of the rotation axis of the earth, and the x axis points to the intersection line of the Greenwich mean meridian plane and the equatorial plane of the earth, so that a right-hand rectangular coordinate system Oxyz is formed. The earth coordinate system rotates around the z-axis at an angular velocity omega relative to the inertial coordinate system ie And (4) rotating.
4. The navigation coordinate system N is called N system for short, and for a strapdown system, namely a local horizontal coordinate system, an original point is located at the center of mass of the carrier, and three coordinate axes respectively point to an east direction E, a north direction N and a vertical direction U (a vertical line of the local is in an upward direction). The angular rate of rotation of the navigational coordinate system relative to the terrestrial coordinate system depends on the motion of the center of mass of the vehicle relative to the earth.
5. Calculating a carrier coordinate system b ', which is called b' for short,
6. and calculating a navigation coordinate system n ', which is called n' system for short.
Since the inertial coordinate system i and the terrestrial coordinate system e are objectively present, they are assumed to contain no calculation error.
Definition ofThe cosine matrix of the direction of rotation from a to b is usually used to represent the rotation relationship between two coordinate systems. The transformation from one coordinate system to another coordinate system can be realized by three successive rotations around different coordinate axes, the angle of the three successive rotations being called euler rotation angle. The coordinate transformation can also be expressed as the product of the independent transformations of the three rotations.
1. Transformation between a carrier coordinate system b and a navigation coordinate system n
Let phi be the course angle of carrier motion, theta be the pitch angle, gamma be the roll angle, the navigation coordinate system is around z n Axially rotated in the negative direction by an angle phi, rotated about the x-axis by an angle theta, and then rotated about the y-axis b After the shaft rotates gamma, the shaft coincides with the carrier coordinate system, and then the conversion matrix is
2. Transformation between terrestrial coordinate system e and navigation coordinate system n
The longitude and latitude of a point P on the earth surface are lambda and L, then the navigation coordinate system at the point P can be obtained by rotating the earth coordinate system three times, and the rotation matrix between the points is
3. Transformation between earth coordinate system e and inertial coordinate system i
The coordinate conversion matrix of the earth coordinate system relative to the inertial coordinate system isWherein ω is ie The earth rotation angular velocity t is the time elapsed from the time (reference time 0) at which the inertial coordinate system coincides with the instantaneous spherical coordinate system.
Second, attitude coordinate system decomposition
According to the attitude matrix (quaternion) chain multiplication rule, the attitude decomposition form can be obtained
In the strapdown inertial navigation attitude calculation, the introduction of various errors causes the calculation to have an attitude quaternion containing errors asAccording to the attitude matrix chain multiplication rule, the attitude decomposition form can be obtained
Third, attitude error equation derivation
3. By substituting the formulae (5) and (6) into the formula (4)
4. The quaternion and three-dimensional vector multiplication satisfy the following relation
According to the formulas (8) and (9), the formula (7) can be further developed
(11) in the formula, the included angle between the calculated carrier coordinate system b' and the real carrier coordinate system b is assumed to be a small angle, that is, the included angle isUnder the small angle assumption, the error quaternion can be approximated to be δ q 1 =[1 α T /2] T (13) The differential equation of α obtained by simultaneous equations (10), (11) and (13) is
(14) The formula is an attitude error equation in the generalized PSI angle error model, and it can be seen that the attitude error alpha is only caused by the error of the gyroscope, which is consistent with the traditional PSI angle error model.
Fourth, position error equation derivation
2. Can be obtained by differentiating the formula (15)
3. by substituting the formulae (17) and (18) into the formula (16)
5. Under the small angle assumption, the error quaternion can be approximated as δ q 2 =[1 θ T /2] T (21) The differential equation of θ obtained by combining the expressions (20) and (21) is
(22) The formula is a position error equation in the generalized PSI angle error model and is given in the form of angle error and is related to the traditional longitude and latitude errorIs θ [ - δ L δ λ cosL δ λ sinL] T (23)。
Fifth, velocity error equation derivation
2. Differentiating the expression (26), and substituting the expressions (24) and (25) into one another
Substitution of formula (28) into formula (27)
(29) The formula is a speed error equation in the generalized PSI angle error model, and can be seen from the derivation process, the formula is not ignoredThe error contained in the method.
And sixthly, deducing an error model in a state transition matrix form, namely, sorting the error equation and writing the error equation into the state transition matrix form.
First, the derivation of the error vectors involved therein is expanded.
wherein R is M 、R N Respectively representing the radius of curvature of the earth meridian and the radius of curvature of the prime unit circle of the place where the carrier is located;
second, for terrestrial and marine applications, the altitude error is ignored, and the state vector is defined
M θv =M 3
fig. 1 is an application flow of the PSI angle error model established in the invention in combined filtering, and it can be known by referring to the above error equations that the generalized PSI error equation established in the invention contains fewer terms, has a simpler form, is convenient for kalman filtering to be implemented, and enables the derived error equation to more intuitively perform inertial navigation solution result correction in filtering application.
It is to be understood that the described embodiments are merely a few embodiments of the invention, and not all embodiments. All other embodiments, which can be derived by one of ordinary skill in the art and related arts based on the embodiments of the present invention without any creative effort, shall fall within the protection scope of the present invention.
Claims (3)
1. A strapdown inertial navigation generalized PSI angle error model construction method is characterized in that an attitude error equation of an error model isThe position error equation isWherein θ [ - δ L δ λ cosL δ λ sinL] T (ii) a The velocity error equation isWherein
Defining state vectorsThen its corresponding state space model isWherein the content of the first and second substances, M θv =M 3 ,
the attitude error equation derivation includes the steps of:
Step A4, combining the relation of quaternion and three-dimensional vector multiplication to obtainWherein the content of the first and second substances,
2. The strapdown inertial navigation generalized PSI angle error model building method of claim 1, wherein the position error equation derivation comprises the steps of:
Step B2, differentiating the above formula to obtainWherein the content of the first and second substances,
3. The strapdown inertial navigation generalized PSI angle error model building method of claim 1, wherein the velocity error equation derivation comprises the steps of:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110312139.XA CN113051757B (en) | 2021-03-23 | 2021-03-23 | Strapdown inertial navigation generalized PSI angle error model construction method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110312139.XA CN113051757B (en) | 2021-03-23 | 2021-03-23 | Strapdown inertial navigation generalized PSI angle error model construction method |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113051757A CN113051757A (en) | 2021-06-29 |
CN113051757B true CN113051757B (en) | 2022-08-09 |
Family
ID=76514727
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110312139.XA Active CN113051757B (en) | 2021-03-23 | 2021-03-23 | Strapdown inertial navigation generalized PSI angle error model construction method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113051757B (en) |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103256943A (en) * | 2013-04-26 | 2013-08-21 | 哈尔滨工程大学 | Compensation method for scale factor error in single-axial rotating strapdown inertial navigation system |
CN106767797B (en) * | 2017-03-23 | 2020-03-17 | 南京航空航天大学 | inertial/GPS combined navigation method based on dual quaternion |
CN109029454A (en) * | 2018-07-13 | 2018-12-18 | 哈尔滨工程大学 | A kind of abscissa system Strapdown Inertial Navigation System damping algorithm based on Kalman filtering |
CN110487301B (en) * | 2019-09-18 | 2021-07-06 | 哈尔滨工程大学 | Initial alignment method of radar-assisted airborne strapdown inertial navigation system |
-
2021
- 2021-03-23 CN CN202110312139.XA patent/CN113051757B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN113051757A (en) | 2021-06-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Yuan et al. | Error compensation of an optical gyro INS by multi-axis rotation | |
CN109813311B (en) | Unmanned aerial vehicle formation collaborative navigation method | |
CN109211269B (en) | Attitude angle error calibration method for double-shaft rotary inertial navigation system | |
CN108871326B (en) | A kind of single-shaft-rotation modulation inertia-astronomy deep integrated navigation method | |
CN112595350B (en) | Automatic calibration method and terminal for inertial navigation system | |
CN105929836B (en) | Control method for quadrotor | |
CN103076015A (en) | SINS/CNS integrated navigation system based on comprehensive optimal correction and navigation method thereof | |
CN110296719B (en) | On-orbit calibration method | |
CN103256928A (en) | Distributed inertial navigation system and posture transfer alignment method thereof | |
CN111207745B (en) | Inertial measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle | |
CN108592946B (en) | Inertial device drift online monitoring method based on two sets of rotary inertial navigation redundancy configuration | |
CN113587925B (en) | Inertial navigation system and full-attitude navigation resolving method and device thereof | |
CN112179340A (en) | Double-shaft rotation modulation method for redundancy configuration inertia measurement unit | |
CN111238469A (en) | Unmanned aerial vehicle formation relative navigation method based on inertia/data chain | |
CN112902956A (en) | Course initial value acquisition method for handheld GNSS/MEMS-INS receiver, electronic equipment and storage medium | |
CN112611394A (en) | Aircraft attitude alignment method and system under emission coordinate system | |
Whittaker et al. | Linearized analysis of inertial navigation employing common frame error representations | |
CN105928519B (en) | Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer | |
Wei et al. | Novel rotation scheme for dual-axis rotational inertial navigation system based on body diagonal rotation of inertial measurement unit | |
CN111060140B (en) | Polar region inertial navigation error obtaining method under earth ellipsoid model | |
CN113051757B (en) | Strapdown inertial navigation generalized PSI angle error model construction method | |
CN115574817B (en) | Navigation method and navigation system based on three-axis rotation type inertial navigation system | |
Hong et al. | Application of EKF for missile attitude estimation based on “SINS/CNS” integrated guidance system | |
CN109737960A (en) | Deformation of hull measurement method based on velocity plus angular rate matching | |
CN111649738B (en) | Method for calculating initial attitude of accelerometer under microgravity field |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |