CN112229421B - Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group - Google Patents
Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group Download PDFInfo
- Publication number
- CN112229421B CN112229421B CN202010973054.1A CN202010973054A CN112229421B CN 112229421 B CN112229421 B CN 112229421B CN 202010973054 A CN202010973054 A CN 202010973054A CN 112229421 B CN112229421 B CN 112229421B
- Authority
- CN
- China
- Prior art keywords
- rotation
- matrix
- vector
- alignment
- lie
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 241001081179 Litsea Species 0.000 title description 2
- 235000012854 Litsea cubeba Nutrition 0.000 title description 2
- 239000011159 matrix material Substances 0.000 claims abstract description 111
- 230000009466 transformation Effects 0.000 claims abstract description 10
- 238000013507 mapping Methods 0.000 claims abstract description 9
- 230000008569 process Effects 0.000 claims description 12
- 238000006243 chemical reaction Methods 0.000 claims description 10
- 238000000354 decomposition reaction Methods 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000005259 measurement Methods 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 5
- 238000007781 pre-processing Methods 0.000 claims description 2
- 238000002360 preparation method Methods 0.000 claims description 2
- 230000001419 dependent effect Effects 0.000 claims 1
- 238000001914 filtration Methods 0.000 abstract description 11
- 238000004364 calculation method Methods 0.000 abstract description 4
- 230000007547 defect Effects 0.000 abstract 1
- 238000004088 simulation Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 239000000969 carrier Substances 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000036039 immunity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000035800 maturation Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
The invention discloses a strapdown inertial navigation shaking base coarse alignment method based on a Lignu optimal estimation, which adopts Lignu description to replace traditional quaternion description to realize calculation of SINS posture transformation, and utilizes a Lignu differential equation to establish a Lignu description-based linear initial alignment filtering model. And calculating error lie algebra by using the direction of vector product of the predictive vector and the observation vector and the included angle between the predictive vector and the observation vector according to the physical definition of lie algebra. And compensating the initial rotation matrix by using the mapping relation between the SO (3) group and the lie algebra. The method avoids the defect that the TRIAD method needs non-collinear vector observation, improves the alignment speed, does not have the non-convex problem existing in the method based on the Wahba problem, improves the alignment precision, and is more suitable for practical engineering application.
Description
Technical Field
The invention relates to a strapdown inertial navigation shaking base coarse alignment method based on a plum blossom description, and belongs to the technical field of navigation methods and applications.
Background
Navigation is a process of guiding a carrier to a destination with a required accuracy by correctly guiding the carrier along a predetermined route. And the inertial navigation system calculates various navigation parameters of the carrier according to the output of the sensor by taking Newton's second law as a theoretical basis. The autonomous navigation system does not depend on external information or radiate any energy to the outside during working, has good concealment and strong immunity, and can provide complete motion information for carriers all the day and all the weather.
Early inertial navigation systems were mainly platform inertial navigation, and along with the maturation of inertial devices and the development of computer technology, strapdown inertial navigation systems in which the inertial devices and carriers are directly and fixedly connected began to appear in the last 60 th century. Compared with platform inertial navigation, the strapdown inertial navigation system omits a complex entity stable platform and has the advantages of low cost, small volume, light weight, high reliability and the like. In recent years, strapdown inertial navigation systems are mature, the precision is gradually improved, and the application range is gradually expanded. The strapdown inertial navigation technology directly installs the gyroscope and the accelerometer on the carrier to obtain the acceleration and the angular velocity under the carrier system, and the navigation computer converts the measured data into a navigation coordinate system to complete navigation.
The research of the self-alignment process plays an important role in the strapdown inertial navigation system, and particularly, the self-alignment method under the shaking base is a current research hotspot. Besides the Liqun filter estimation method, the self-alignment process can be completed by adopting singular value decomposition, quaternion Kalman and other methods under the condition of shaking the base. These methods still have a non-negligible disadvantage. Singular value decomposition is an optimization method based on matrix decomposition, and because the calculation process of the singular value decomposition does not meet the operation rule of the Liqun space, singular points can be generated under the condition of larger misalignment angle, and the alignment performance is affected. The quaternion Kalman is a method for constructing a linearization filtering model by using a pseudo-measurement equation and completing an alignment task by using a Kalman filter, and the method does not generate singular points, but the construction of the pseudo-measurement equation also has an influence on convergence speed and alignment precision. In the alignment process of the Liqun filtering, calculation errors are generated due to conversion between Liqun algebra and Liqun space when a gain matrix and an error covariance matrix are constructed; the design principle of the filter is based on the improvement of Kalman filtering, and the definition of the innovation does not accord with the property of the Liqun rotation matrix. And thus has an effect on alignment accuracy.
Aiming at the problems of the existing shaking base self-alignment method, the invention provides a new plum-group optimal estimation algorithm by referring to the thought of optimal estimation in order to further improve the initial alignment performance. Thanks to the perfection of the Leu theory, the state quantity is ensured to meet the special orthogonal property at any time by improving the expression mode of the innovation. The estimation model constructed based on the method avoids the singular value problem in the traditional optimization method, and the iterative computation of each step accords with the characteristics of the Liqun rotation matrix. Compared with the self-alignment method of the Liqun filtering, the self-alignment method of the Liqun filtering improves the alignment speed and improves the precision while maintaining the advantages of the Liqun filtering. The simulation verification proves the feasibility of the algorithm, and the algorithm can be used as an upper-level substitute for the filtering of the Liu group to perform self-alignment of the shaking base.
Disclosure of Invention
Since the carrier is easily affected by various external interference factors during the initial alignment, it is difficult to keep static during the alignment. Therefore, the self-alignment algorithm under the shaking base has high research significance and application value. The invention aims to solve the problems of the existing shaking base self-alignment method: (1) According to the invention, the initial gesture matrix is described by replacing quaternions with the Li group, so that the long alignment time of the traditional TRIAD method is avoided; (2) According to the invention, by utilizing the direction of vector multiplication of a prediction vector and an observation vector and the included angle between the prediction vector and the observation vector according to the physical definition of the lie algebra, the error lie algebra is calculated, and the mapping relation between the so (3) group and the lie algebra is utilized to compensate the initial rotation matrix. The non-convex problem of the traditional q method (rough alignment based on the Wahba problem) is avoided.
In order to achieve the above purpose, the present invention provides the following technical solutions:
the SINS strapdown inertial navigation system shaking base self-alignment method based on the optimal estimation of the Liqun is characterized by comprising the following steps of:
step (1): the SINS strapdown inertial navigation system performs system preheating preparation, starts the system, and obtains longitude lambda and latitude L of the position of the carrier and projection g of local gravity acceleration under the navigation system n Basic information such as rotation angle rate information of a carrier system output by a gyroscope in an inertial measurement unit IMU relative to an inertial system is acquired and projection of rotation angle rate information of the carrier system on the carrier system is acquiredAnd carrier system acceleration information f output by accelerometer b Etc.;
step (2): preprocessing the acquired data of the gyroscope and the accelerometer, and establishing a linear shaking base self-alignment system model based on the Liqun description based on a Liqun differential equation:
the coordinate system in the detailed description of the method is defined as follows:
the earth coordinate system e is characterized in that the center of the earth is selected as an origin, an X axis is positioned in an equatorial plane and points to the primary meridian from the earth center, a Z axis points to the geographic north pole from the earth center, and the X axis, the Y axis and the Z axis form a right-hand coordinate system and rotate along with the autorotation of the earth;
the earth center inertial coordinate system i system is characterized in that the earth center is selected as an origin, an X axis is positioned in an equatorial plane and points from the earth center to a spring point, a Z axis points from the earth center to a geographic north pole, and the X axis, the Y axis and the Z axis form a right-hand coordinate system;
the navigation coordinate system N is used for representing a geographic coordinate system of the position of the carrier, the gravity center of the carrier-based aircraft is selected as an origin, the X axis points to the east E, the Y axis points to the north N, and the Z axis points to the sky U; in the method, a navigation coordinate system is selected as a geographic coordinate system;
the carrier coordinate system b is used for representing a three-axis orthogonal coordinate system of the strapdown inertial navigation system, the gravity center of the carrier aircraft is selected as an origin, and the X axis, the Y axis and the Z axis are respectively directed rightward along the transverse axis of the carrier aircraft body, forward along the longitudinal axis and upward along the vertical axis;
an initial navigation coordinate system n (0) system which represents a navigation coordinate system of the SINS at the starting-up running time and is kept static relative to an inertial space in the whole alignment process;
an initial carrier coordinate system b (0) system, which represents a carrier coordinate system of SINS at the starting-up running time and is kept stationary relative to an inertial space in the whole alignment process;
the navigation coordinate system n' system represents an initial navigation coordinate system calculated by a Liqun optimal estimation algorithm, and a rotation relationship exists between the initial navigation coordinate system and a real navigation coordinate system n system;
based on the differential equation of the Liqun, establishing a linear self-aligned system model based on Liqun description:
according to the SINS strapdown inertial navigation system principle, the SINS shaking base self-alignment problem is converted into a gesture estimation problem, gesture conversion is rotation conversion between two coordinate systems, and a navigation gesture matrix is represented by a 3 x 3 orthogonal conversion matrix; the orthogonal transformation matrix accords with the property of a special orthogonal group SO (n) of the plum group, and forms a three-dimensional rotation group SO (3):
wherein R epsilon SO (3) represents a specific navigation gesture matrix,a vector space of 3×3, a superscript T denotes a transpose of the matrix, I denotes a three-dimensional identity matrix, det (R) denotes a determinant of the matrix R;
the shaking base self-alignment posture estimation problem is converted into a solving problem of a posture matrix R based on the Liqun description; according to a chain rule of gesture matrixes based on Liqun description, the navigation gesture matrixesThe decomposition is in the form of the product of three matrices:
wherein t represents a time variable,an attitude matrix representing the current carrier system relative to the current navigation system,>representing the pose matrix of the initial navigation system relative to the current navigation system, the initial pose matrix +.>Representing the pose matrix of the initial carrier system relative to the initial navigation system,>representing a posture matrix of the current carrier system relative to the initial carrier system;
from the lie group differential equation, the pose matrix and />The updating process with time is as follows:
wherein ,representing the pose matrix of the initial carrier system relative to the current carrier system,>representing the projection of the rotation angular rate of the navigation system relative to the inertial system on the navigation system, which is equal to the rotation angular rate of the earth under shaking the base conditions L represents the local latitude>The projection of the rotation angular rate of the carrier system with respect to the inertial system, representing the gyroscope output, on the carrier system, is denoted by the symbol (·×) representing the operation of converting a three-dimensional vector into an antisymmetric matrix, the operation rules being as follows:
as can be seen from formulas (2) - (5), and />Calculated from IMU sensor data in real time, whereas +.>A gesture matrix representing an initial time, which does not change over time; thus, the gesture matrix during SINS self-alignment>Is converted into an initial gesture matrix based on the Liqun description>Solving a problem;
according to the strapdown inertial navigation shake base coarse alignment principle and the formula (2), a shake base coarse alignment model based on the Liqun description can be written into the following form:
wherein ,Vn(t) and Vb (t) represents the speed of the carrier in the n-system and the speed of the carrier in the b-system, V n(t) and Vb (t) can be obtained by the following formula:
and due to the initial rotation matrixIs a constant matrix whereby the sloshing base coarse alignment model can be written as follows:
wherein R(tk ) Representing t k Time of day
Step (3): solving for error lie algebra based on the physical definition of lie algebra and using the exponential mapping pair R (t) between lie groups and lie algebra k ) An estimation is made.
V according to the definition of the rotation matrix and equation (9) n (t) and R (t) k )V b (t) is the same vector in the n system, and the vector product is 0. However, due to the presence of errors, the estimated rotation matrix is compared with the actual rotation matrix R (t k ) With deviations between them, which results in an estimated rotation matrixIs a posture matrix from b series to n' series, namely:
according to the chain law, we only need to calculate the rotation relationship between the n' series and the n series, i.e. to compensate the estimated rotation matrix
According to the rotation formula of rodgers:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa T +(sinθ)a × (11)
any rotation matrix can be found by starting the corresponding rotation lie algebra phi, and any rotation lie algebra (rotation vector) phi can be found from the rotation angle theta and the rotation axis a by:
φ＝θa (12)
further according to the basic principle of coordinate system transformation, the rotational relationship between two coordinate systems is equivalent to the rotational relationship of the same vector under two systems, thereby the rotational relationship between n' and n systems, i.eEquivalent and solving of the observation vector V n (t) and prediction vector->The rotational relationship between them is such that V is determined according to the formulae (11) and (12) n(t) and />The rotation axis and the rotation angle between the two can be determined +.>V n(t) and />The rotational axis and angle of rotation therebetween are shown in fig. 3 below.
Whereby the rotation axis a can pass through the pair ofThe unit is obtained by:
the rotation angle θ can be obtained by the following equation:
thus error lie algebraCan be written as:
the updated equation for the optimal estimate of the lie group can then be established as follows:
step (4): gesture matrix needed for solving navigation systemThereby completing the self-alignment process of the shaking base:
according to the gesture change matrix obtained by solving in the previous step and />And (3) information, solving a navigation attitude matrix through the formula (2), and completing the self-alignment of the SINS strapdown inertial navigation system shaking base.
Compared with the prior art, the invention has the following advantages and beneficial effects:
(1) According to the invention, the initial gesture matrix is described by replacing quaternions by the Li group, and a non-collinear vector is not needed to be used as observation, so that the problem of long alignment time of the traditional TRIAD method is avoided;
(2) According to the invention, the error lie algebra is calculated by utilizing the direction of vector multiplication of the prediction vector and the observation vector and the included angle between the prediction vector and the observation vector according to the physical definition of the lie algebra, and the mapping relation between the so (3) group and the lie algebra is utilized to compensate the initial rotation matrix. The non-convex problem existing in the traditional q method (rough alignment based on the Wahba problem) is avoided, and the alignment precision is improved.
Drawings
FIG. 1 is a general schematic diagram of a strapdown inertial navigation system device.
FIG. 2 is a flow chart of a strapdown inertial navigation system.
FIG. 3V n(t) and and a rotation shaft and a rotation angle and a rotation relation diagram. In the figure, by->To V n The rotation axis alpha determined by the rotation of (c) can be obtained by right hand rule. According to the right rule, it can be judged +.>And alpha are in the same direction.
FIG. 4 is a diagram of the alignment simulation results of the movable base.
Detailed Description
The invention relates to a SINS strapdown inertial navigation system shaking base self-alignment method design based on a Liqun optimal estimation, and the specific implementation steps of the invention are described in detail below with reference to a system flow chart of the invention:
the SINS strapdown inertial navigation system shaking base self-alignment method based on the optimal estimation of the plum cluster comprises the steps of firstly obtaining real-time data of a sensor; processing the acquired data, and establishing a linear self-alignment system model based on the Liqun description based on the Liqun differential equation; using a Liqun optimal estimation algorithm to estimate and obtain an initial gesture matrix based on Liqun descriptionAnd solving the gesture matrix->During self-alignment, the accurate initial posture matrix is finally obtained through multiple estimation solutions>And gesture matrix->The self-alignment process is completed.
Step 1: starting and initializing an SINS inertial navigation system to obtain longitude lambda, latitude L and local gravity acceleration g of the position of a carrier n Basic information such as angular rate information output by gyroscopes in Inertial Measurement Unit (IMU) is collectedAnd accelerometer output acceleration information f b 。
Step 2: processing the acquired data of the gyroscope and the accelerometer, establishing a linear shaking base self-alignment system model based on the Liqun description based on the Liqun differential equation,
based on the differential equation of the Liqun, establishing a linear self-aligned system model based on Liqun description:
according to the SINS strapdown inertial navigation system principle, the SINS shaking base self-alignment problem is converted into a gesture estimation problem, gesture conversion is rotation conversion between two coordinate systems, and a navigation gesture matrix is represented by a 3 x 3 orthogonal conversion matrix; the orthogonal transformation matrix accords with the property of a special orthogonal group SO (n) of the plum group, and forms a three-dimensional rotation group SO (3):
wherein R epsilon SO (3) represents a specific navigation gesture matrix,a vector space of 3×3, a superscript T denotes a transpose of the matrix, I denotes a three-dimensional identity matrix, det (R) denotes a determinant of the matrix R;
the shaking base self-alignment posture estimation problem is converted into a solving problem of a posture matrix R based on the Liqun description;according to a chain rule of gesture matrixes based on Liqun description, the navigation gesture matrixesThe decomposition is in the form of the product of three matrices:
wherein t represents a time variable,an attitude matrix representing the current carrier system relative to the current navigation system,>representing the pose matrix of the initial navigation system relative to the current navigation system, the initial pose matrix +.>Representing the pose matrix of the initial carrier system relative to the initial navigation system,>representing a posture matrix of the current carrier system relative to the initial carrier system;
from the lie group differential equation, the pose matrix and />The updating process with time is as follows:
wherein ,representing the pose matrix of the initial carrier system relative to the current carrier system,>representing the projection of the rotation angular rate of the navigation system relative to the inertial system on the navigation system, which is equal to the rotation angular rate of the earth under shaking the base conditions L represents the local latitude>The projection of the rotation angular rate of the carrier system with respect to the inertial system, representing the gyroscope output, on the carrier system, is denoted by the symbol (·×) representing the operation of converting a three-dimensional vector into an antisymmetric matrix, the operation rules being as follows:
as can be seen from formulas (2) - (5), and />Calculated from IMU sensor data in real time, whereas +.>A gesture matrix representing an initial time, which does not change over time; thus, the gesture matrix during SINS self-alignment>Is converted into an initial gesture matrix based on the Liqun description>Solving a problem;
according to the strapdown inertial navigation shake base coarse alignment principle and the formula (2), a shake base coarse alignment model based on the Liqun description can be written into the following form:
wherein ,Vn(t) and Vb (t) represents the speed of the carrier in the n-system and the speed of the carrier in the b-system, V n(t) and Vb (t) can be obtained by the following formula:
and due to the initial rotation matrixIs a constant matrix whereby the sloshing base coarse alignment model can be written as follows:
wherein R(tk ) Representing t k Time of day
Step (3): solving the error lie algebra according to the physical definition of the lie algebra, and utilizing the relation between the lie group and the lie algebraIs an exponential mapping of R (t) k ) An estimation is made.
V according to the definition of the rotation matrix and equation (9) n (t) and R (t) k )V b (t) is the same vector in the n system, and the vector product is 0. However, due to the presence of errors, the estimated rotation matrix is compared with the actual rotation matrix R (t k ) With deviations between them, which results in an estimated rotation matrixIs a posture matrix from b series to n' series, namely:
according to the chain law, we only need to calculate the rotation relationship between the n' series and the n series, i.e. to compensate the estimated rotation matrix
According to the rotation formula of rodgers:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa T +(sinθ)a × (27)
any rotation matrix can be found by starting the corresponding rotation lie algebra phi, and any rotation lie algebra (rotation vector) phi can be found from the rotation angle theta and the rotation axis a by:
φ＝θa (28)
further according to the basic principle of coordinate system transformation, the rotational relationship between two coordinate systems is equivalent to the rotational relationship of the same vector under two systems, thereby the rotational relationship between n' and n systems, i.eEquivalent and solving of the observation vector V n (t) and prediction vector->The rotational relationship between, and thereforeAs long as V is determined according to the formula (11) and the formula (12) n(t) and />The rotation axis and the rotation angle between the two can be determined +.>V n(t) and />The rotational axis and angle of rotation therebetween are shown in fig. 3 below.
Whereby the rotation axis a can pass through the pair ofThe unit is obtained by:
the rotation angle θ can be obtained by the following equation:
thus error lie algebraCan be written as:
the updated equation for the optimal estimate of the lie group can then be established as follows:
step (4): solving an attitude matrixAnd the pose information is calculated,
in step (2), the SINS is self-alignedThe solution problem of (2) is converted into p->Solve the problem and will->The decomposition is in the form of three matrix products:
solving according to the formulas (28), (31) and (46) and />Gesture matrixThe solving mode of (2) is as follows:
and according to the gesture matrix obtained by solvingAnd calculating the gesture information.
The beneficial effects of the invention are as follows:
(1) The method is subjected to simulation experiments under the following simulation conditions:
in the step (1), under the condition of shaking the base, the simulated carrier receives the influence of wind and waves, and the course angle psi, the pitch angle theta and the roll angle gamma of the simulated carrier are periodically changed, and the posture change conditions are as follows:
in step (1), an initial geographic location: 118 degrees of east longitude and 40 degrees of north latitude;
in the step (1), the output frequency of the sensor is 100Hz;
in step (1), gyroscope drift: the constant drift of the gyroscopes on the three direction axes is 0.02 degrees/h, and the random drift is 0.005 degrees/h;
in step (1), the accelerometer is zero biased: constant value bias of accelerometer in three direction axes is 2 x 10 -4 g, randomly bias to
In step (2), the earth rotation angular rate 7.2921158e -5 rad/s；
In the step (2), the time interval T is 0.02s;
in the step (3), initial values of the optimal estimation algorithm of the Liqun are obtained
The simulation result of the method is as follows:
200s simulation was performed, and the simulation result was shown in fig. 4, with the estimated error of the attitude angle as a measurement index. As can be seen from the figure, the pitch attitude is aligned at about 30s, converging to 0.4'; the roll gesture completes alignment at about 35s, converging to 0.5'; the heading pose completes alignment at about 45s and converges to 4.4'. As shown by simulation results, the self-alignment task under the shaking base can be completed rapidly and effectively.
According to the invention, the traditional quaternion description is replaced by the Liqu description to realize the calculation of SINS posture transformation, and a Liqu differential equation is utilized to establish a linear initial alignment filtering model based on Liqu description. Therefore, the SINS can be directly and automatically aligned in one step, and the problems of non-uniqueness and non-linearity caused by the description of the initial gesture matrix by the traditional quaternion are avoided; the invention uses the left multiplication invariance of the Liqun and the index mapping between the Liqun and the Liqun to carry out equivalent transformation on the state related error, establishes an equivalent state independent Liqun filtering equation, avoids the influence of state related noise on the filtering result and effectively improves the alignment precision.
The foregoing is merely a preferred embodiment of the present invention and is not intended to limit the present invention. It should be noted that modifications and variations can be made by those skilled in the art without departing from the principles of the present invention, which are also considered as being within the scope of the present invention.
Claims (2)
1. The strapdown inertial navigation shaking base coarse alignment method based on the optimal estimation of the Liqun is characterized by comprising the following steps of:
step (1): the SINS strapdown inertial navigation system performs system preheating preparation, starts the system, and obtains longitude lambda and latitude L of the position of the carrier and projection g of local gravity acceleration under the navigation system n Basic information is acquired, and projection of rotation angle rate information of a carrier system output by a gyroscope in an inertial measurement unit IMU relative to an inertial system on the carrier system is acquiredAnd carrier system acceleration information f output by accelerometer b ；
Step (2): preprocessing the acquired data of the gyroscope and the accelerometer, and establishing a linear shaking base self-alignment system model based on the Liqun description based on a Liqun differential equation:
based on the differential equation of the Liqun, establishing a linear self-aligned system model based on Liqun description:
according to the SINS strapdown inertial navigation system principle, the SINS shaking base self-alignment problem is converted into a gesture estimation problem, gesture conversion is rotation conversion between two coordinate systems, and a navigation gesture matrix is represented by a 3 x 3 orthogonal conversion matrix; the orthogonal transformation matrix accords with the property of a special orthogonal group SO (n) of the plum group and forms a three-dimensional rotation group SO (3):
wherein R epsilon SO (3) represents a specific navigation gesture matrix,a vector space of 3×3, a superscript T denotes a transpose of the matrix, I denotes a three-dimensional identity matrix, det (R) denotes a determinant of the matrix R;
the shaking base self-alignment posture estimation problem is converted into a solving problem of a posture matrix R based on the Liqun description; according to a chain rule of gesture matrixes based on Liqun description, the navigation gesture matrixesThe decomposition is in the form of the product of three matrices:
wherein t represents a time variable,an attitude matrix representing the current carrier system relative to the current navigation system,>representing the pose matrix of the initial navigation system relative to the current navigation system, the initial pose matrix +.>Representing the pose matrix of the initial carrier system relative to the initial navigation system,>representing a posture matrix of the current carrier system relative to the initial carrier system;
from the lie group differential equation, the pose matrix and />The updating process with time is as follows:
wherein ,representing the pose matrix of the initial carrier system relative to the current carrier system,>representing the projection of the rotation angular rate of the navigation system relative to the inertial system on the navigation system, which is equal to the rotation angular rate of the earth under shaking the base conditions +.> L represents the local latitude>The projection of the rotation angular rate of the carrier system with respect to the inertial system, representing the gyroscope output, on the carrier system, is denoted by the symbol (·×) representing the operation of converting a three-dimensional vector into an antisymmetric matrix, the operation rules being as follows:
as can be seen from formulas (2) - (5), and />Calculated from IMU sensor data in real time, whereas +.>A gesture matrix representing an initial time, which does not change over time; thus, the gesture matrix during SINS self-alignment>Is converted into an initial gesture matrix based on the Liqun description>Solving a problem;
according to the strapdown inertial navigation shake base coarse alignment principle and the formula (2), a shake base coarse alignment model based on the Liqun description can be written into the following form:
wherein ,Vn(t) and Vb (t) represents the speed of the carrier in the n-system and the speed of the carrier in the b-system, V n(t) and Vb (t) can be obtained by the following formula:
and due to the initial rotation matrixIs a constant matrix whereby the sloshing base coarse alignment model is written as follows:
wherein R(tk ) Representing t k Time of day
Step (3): solving for error lie algebra based on the physical definition of lie algebra and using the exponential mapping pair R (t) between lie groups and lie algebra k ) Estimating;
v according to the definition of the rotation matrix and equation (9) n (t) and R (t) k )V b (t) is the same vector in the n system, and the vector product is 0; however, due to errors, the estimated rotation matrix is compared with the actual rotation matrix R (t k ) With deviations between them, which results in an estimated rotation matrixIs a posture matrix from b series to n' series, namely:
to compensate the estimated rotation matrix according to the chain law, we only need to calculate the rotation relationship between n' series and n series, i.e
According to the rotation formula of rodgers:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa T +(sinθ)a × (11)
any rotation matrix can be found by starting the corresponding rotation lie algebra phi, and any rotation lie algebra (rotation vector) phi is found by the rotation angle theta and the rotation axis a by:
φ＝θa (12)
according to the basic principle of coordinate system transformation, the rotation relationship between two coordinate systems is equivalent to the rotation relationship of the same vector under two systems, thus the rotation relationship between n' system and n system, namelyEquivalent and solving of the observation vector V n (t) and prediction vector->The rotational relationship between them is such that V is determined according to the formulae (11) and (12) n(t) and />The rotation axis and the rotation angle between them are determined->V n(t) and />The rotation shaft and the rotation angle and the rotation relation between the two;
the rotating shaft a passes through the pairThe unit is obtained by:
the rotation angle θ is obtained by the following equation:
thus error lie algebraWriting:
the updated equation for the optimal estimate of the lie group can then be established as follows:
2. the method of initial alignment of a strapdown inertial navigation mobile base based on a lie group description of claim 1, wherein in step (3), an error lie algebra is solved according to a physical definition of the lie algebra, and an exponential mapping pair R (t k ) Estimating;
step (3.1) separates state-dependent noise by the left-hand invariance of the lie group and the exponential mapping between the lie group and the lie algebra by:
v according to the definition of the rotation matrix and equation (9) n (t) and R (t) k )V b (t) is the same vector in the n system, and the vector product is 0; however, due to errors, the estimated rotation matrix is compared with the actual rotation matrix R (t k ) With deviations between them, which results in an estimated rotation matrixIs the pose matrix of b-series to n' -series:
according to the chain law, to compensate the estimated rotation matrix, only the rotation relationship between n' series and n series is calculated, i.e
According to the rotation formula of rodgers:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa T +(sinθ)a × (18)
the arbitrary rotation matrix is obtained by starting the corresponding rotation lie algebra phi, and the arbitrary rotation lie algebra phi is obtained by the rotation angle theta and the rotation axis a by the following formula:
φ＝θa (19)
according to the basic principle of coordinate system transformation, the rotation relationship between two coordinate systems is equivalent to the rotation relationship of the same vector under two systems, thus the rotation relationship between n' system and n system, namelyEquivalent and solving of the observation vector V n (t) and prediction vector->The rotational relationship between them is thus determined according to the formulae (18) and (19) as long as V n(t) and />The rotation axis and the rotation angle between them are determined +.>V n(t) and />The rotation shaft and the rotation angle and the rotation relation between the two;
whereby the rotation axis a passes through the pair ofThe unit is obtained by:
the rotation angle θ is obtained by the following equation:
thus error lie algebraWriting:
the updated equation for the optimal estimate of the lie group can then be established as follows:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010973054.1A CN112229421B (en) | 2020-09-16 | 2020-09-16 | Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010973054.1A CN112229421B (en) | 2020-09-16 | 2020-09-16 | Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112229421A CN112229421A (en) | 2021-01-15 |
| CN112229421B true CN112229421B (en) | 2023-08-11 |
Family
ID=74107568
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010973054.1A Active CN112229421B (en) | 2020-09-16 | 2020-09-16 | Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112229421B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112833918B (en) * | 2021-02-08 | 2022-12-13 | 北京理工大学 | High-rotation body micro inertial navigation aerial alignment method and device based on function iteration |
| CN113503893A (en) * | 2021-06-02 | 2021-10-15 | 北京自动化控制设备研究所 | Initial alignment algorithm of moving base inertial navigation system |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2013059989A1 (en) * | 2011-10-25 | 2013-05-02 | 国防科学技术大学 | Motion alignment method of inertial navigation system |
| CN106871928A (en) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | Strap-down inertial Initial Alignment Method based on Lie group filtering |
| CN109931957A (en) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | SINS self-alignment method for strapdown inertial navigation system based on LGMKF |
| CN110595503A (en) * | 2019-08-05 | 2019-12-20 | 北京工业大学 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
| CN110702143A (en) * | 2019-10-19 | 2020-01-17 | 北京工业大学 | Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description |
| CN110926499A (en) * | 2019-10-19 | 2020-03-27 | 北京工业大学 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
-
2020
- 2020-09-16 CN CN202010973054.1A patent/CN112229421B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2013059989A1 (en) * | 2011-10-25 | 2013-05-02 | 国防科学技术大学 | Motion alignment method of inertial navigation system |
| CN106871928A (en) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | Strap-down inertial Initial Alignment Method based on Lie group filtering |
| CN109931957A (en) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | SINS self-alignment method for strapdown inertial navigation system based on LGMKF |
| CN110595503A (en) * | 2019-08-05 | 2019-12-20 | 北京工业大学 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
| CN110702143A (en) * | 2019-10-19 | 2020-01-17 | 北京工业大学 | Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description |
| CN110926499A (en) * | 2019-10-19 | 2020-03-27 | 北京工业大学 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
Non-Patent Citations (1)
| Title |
|---|
| 基于空间域划分的分布式SLAM算法;裴福俊 等;《***工程与电子技术》;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112229421A (en) | 2021-01-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110702143B (en) | Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description | |
| CN106871928B (en) | Strap-down inertial navigation initial alignment method based on lie group filtering | |
| CN107588771B (en) | Lei cluster description-based strapdown inertial navigation resolving method | |
| CN110595503B (en) | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN106052716B (en) | Gyro error online calibration method based on starlight information auxiliary under inertial system | |
| CN105698822B (en) | Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced | |
| CN109931957B (en) | Self-alignment method of SINS strapdown inertial navigation system based on LGMKF | |
| CN106767797B (en) | inertial/GPS combined navigation method based on dual quaternion | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| CN108592943B (en) | Inertial system coarse alignment calculation method based on OPREQ method | |
| CN112229421B (en) | Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group | |
| CN110926499B (en) | SINS strapdown inertial navigation system shaking base self-alignment method based on Liqun optimal estimation | |
| CN104374388A (en) | Flight attitude determining method based on polarized light sensor | |
| CN105910606A (en) | Direction adjustment method based on angular velocity difference | |
| CN110058288A (en) | Unmanned plane INS/GNSS integrated navigation system course error modification method and system | |
| Lambert et al. | Visual odometry aided by a sun sensor and inclinometer | |
| CN109211230A (en) | A kind of shell posture and accelerometer constant error estimation method based on Newton iteration method | |
| CN111207773B (en) | Attitude unconstrained optimization solving method for bionic polarized light navigation | |
| CN116448145A (en) | Navigation attitude determination method based on polarization vector space difference | |
| CN116644264A (en) | Nonlinear bias-invariant Kalman filtering method | |
| CN113916226B (en) | Minimum variance-based interference rejection filtering method for integrated navigation system | |
| Chang et al. | Pseudo open-loop unscented quaternion estimator for attitude estimation | |
| CN112212889A (en) | SINS strapdown inertial navigation system shaking base rough alignment method based on special orthogonal group optimal estimation | |
| CN113532479B (en) | Liqun multiplicative Kalman filtering-based strapdown inertial navigation system motion initial alignment method |
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 |