CN112229421A - Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation - Google Patents
Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation Download PDFInfo
- Publication number
- CN112229421A CN112229421A CN202010973054.1A CN202010973054A CN112229421A CN 112229421 A CN112229421 A CN 112229421A CN 202010973054 A CN202010973054 A CN 202010973054A CN 112229421 A CN112229421 A CN 112229421A
- Authority
- CN
- China
- Prior art keywords
- rotation
- matrix
- lie
- alignment
- representing
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 239000011159 matrix material Substances 0.000 claims abstract description 117
- 230000009466 transformation Effects 0.000 claims abstract description 16
- 238000013507 mapping Methods 0.000 claims abstract description 10
- 230000008569 process Effects 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000000354 decomposition reaction Methods 0.000 claims description 7
- 238000005259 measurement Methods 0.000 claims description 7
- 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 5
- 230000007547 defect Effects 0.000 abstract 1
- 238000004088 simulation Methods 0.000 description 10
- 238000004422 calculation algorithm Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 4
- 238000011160 research Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 239000011435 rock Substances 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
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 rough alignment method based on lie group optimal estimation, which adopts lie group description to replace the traditional quaternion description to realize the calculation of SINS attitude transformation, and utilizes a lie group differential equation to establish a linear initial alignment filtering model based on the lie group description. And according to the physical definition of the lie algebra, calculating the error lie algebra by utilizing the direction of the product of the prediction vector and the observation vector and the included angle between the prediction vector and the observation vector. 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 discloses a strap-down inertial navigation shaking base rough alignment method based on lie group description, and belongs to the technical field of navigation methods and application.
Background
Navigation is the process of properly guiding a carrier along a predetermined route to a destination with the required accuracy and within a specified time. The inertial navigation system calculates each navigation parameter of the carrier according to the output of the sensor of the inertial navigation system by taking Newton's second law as a theoretical basis. The autonomous navigation system is an autonomous navigation system, does not depend on external information when working, does not radiate any energy to the outside, has good concealment and strong interference resistance, and can provide complete motion information for a carrier all day long and all weather.
The early inertial navigation system is mainly based on platform inertial navigation, and with the maturity of inertial devices and the development of computer technology, a strapdown inertial navigation system with an inertial device and a carrier directly fixedly connected with each other begins to appear in the last 60 th century. Compared with platform inertial navigation, the strapdown inertial navigation system saves a complex entity stable platform and has the advantages of low cost, small volume, light weight, high reliability and the like. In recent years, a strapdown inertial navigation system is mature, the precision is gradually improved, and the application range is gradually expanded. The strapdown inertial navigation technology directly installs a gyroscope and an accelerometer on a carrier to obtain the acceleration and the angular velocity under a carrier system, and converts measured data into a navigation coordinate system through a navigation computer to complete navigation.
The research of the self-alignment process is of great significance in the strapdown inertial navigation system, and particularly the self-alignment method under the shaking base is the current research hotspot. Besides the lie group filtering estimation method, the self-alignment process can be completed by using methods such as singular value decomposition, quaternion Kalman and the like under the condition of shaking the base. These methods still have considerable drawbacks. Singular value decomposition is an optimization method based on matrix decomposition, and since the calculation process does not meet the operation rule of the lie group space, singular points are generated under the condition of a large misalignment angle, so that the alignment performance is influenced. Quaternion Kalman is a method for constructing a linear filtering model by using a pseudo measurement equation and completing an alignment task by using a Kalman filter, and although the method cannot generate singular points, the construction of the pseudo measurement equation also influences the convergence speed and the alignment precision. In the process of lie group filtering alignment, calculation errors are generated due to conversion between a lie algebra and a lie group space when a gain matrix and an error covariance matrix are constructed; the design principle of the filter is improved based on Kalman filtering, and the definition of innovation does not conform to the property of the lie group rotation matrix. And therefore has an effect on the alignment accuracy.
Aiming at the problems of the existing shaking base self-alignment method, the invention provides a novel optimal estimation algorithm for the plum blossom so as to further improve the performance of initial alignment by using the idea of optimal estimation. Due to the improvement of the lie group theory, the state quantity is ensured to meet the special orthogonal property at any time by improving the expression mode of innovation. The estimation model constructed based on the method avoids the problem of singular values in the traditional optimization method, and the iterative computation of each step conforms to the characteristics of the lie group rotation matrix. Compared with the method for self-aligning the lie group filtering, the method has the advantages that the advantage of the lie group filtering is kept, meanwhile, the aligning speed is improved, and the accuracy is improved. The feasibility of the algorithm is proved through simulation verification, and the method can be used as an upper-level substitute for the lie filtering to perform shaking base self-alignment.
Disclosure of Invention
Since the carrier is easily affected by various external interference factors during the initial alignment process, it is difficult to keep the carrier still during the alignment process. 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 attitude matrix is described by replacing quaternion with the lie group, so that the long alignment time of the traditional TRIAD method is avoided; (2) according to the physical definition of the lie algebra, the error lie algebra is calculated by utilizing the direction of the product of the prediction vector and the observation vector and the included angle between the prediction vector and the observation vector, 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 conventional q method (coarse alignment based on the Wahba problem) is avoided.
In order to achieve the purpose, the invention provides the following technical scheme:
the SINS strapdown inertial navigation system shaking base self-alignment method based on lie group optimal estimation is characterized by comprising the following steps of:
step (1): the SINS strapdown inertial navigation system carries out system preheating preparation, starts the system, and obtains the longitude lambda and the latitude L of the position of the carrier and the projection g of the local gravity acceleration under the navigation systemnCollecting the projection of the rotation angular rate information of the carrier system output by the gyroscope in the inertial measurement unit IMU relative to the inertial system on the carrier system according to the basic information
And carrier system acceleration information f output by accelerometerbEtc.;
step (2): preprocessing acquired data of the gyroscope and the accelerometer, and establishing a linear shaking base self-alignment system model based on the lie group description on the basis of a lie group 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 earth center is selected as an origin, the X axis is located in an equatorial plane and points to the original meridian from the earth center, the 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 earth rotation;
the geocentric inertial coordinate system i is characterized in that the geocentric inertial coordinate system i is obtained by selecting the geocenter as an origin, an X axis is located in an equatorial plane and points to the spring equinox from the geocenter, a Z axis points to the geographical arctic from the geocenter, and the X axis, the Y axis and the Z axis form a right-hand coordinate system;
a navigation coordinate system N, which is a geographical coordinate system representing the position of the carrier, selects the gravity center of the carrier-based aircraft as an origin, points to the east E on the X-axis, points to the north N on the Y-axis, and points to the sky U on the Z-axis; in the method, a navigation coordinate system is selected as a geographic coordinate system;
a carrier coordinate system b system which represents a three-axis orthogonal coordinate system of the strapdown inertial navigation system, wherein the gravity center of the carrier-based aircraft is selected as an origin, and an X axis, a Y axis and a Z axis respectively point to the right along a transverse axis, point to the front along a longitudinal axis and point to the up along a vertical axis of the carrier-based aircraft body;
an initial navigation coordinate system n (0) system which represents a navigation coordinate system of the SINS at the startup running time and keeps 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 the SINS at the starting-up running time and keeps static relative to an inertial space in the whole alignment process;
a navigation coordinate system n' system which represents an initial navigation coordinate system calculated by a lie group optimal estimation algorithm, wherein a rotation relation exists between the coordinate system and the real navigation coordinate system n system;
based on a lie group differential equation, establishing a linear self-alignment system model based on the lie group description:
according to the SINS strapdown inertial navigation system principle, the SINS shaking base self-alignment problem is converted into a posture estimation problem, the posture is converted into rotation transformation between two coordinate systems, and a navigation posture matrix is represented by a 3 multiplied by 3 orthogonal transformation matrix; the orthogonal transformation matrix conforms to the property of a special orthogonal group SO (n) of lie groups, and forms a three-dimensional rotation group SO (3):
wherein R ∈ SO (3) represents a specific navigation attitude matrix,
representing a 3 x 3 vector space, superscript T representing the transpose of the matrix, I representing a three-dimensional identity matrix, det (R) representing the determinant of matrix R;
the self-alignment attitude estimation problem of the shaking base is converted into a solving problem of an attitude matrix R based on the lie group description; navigating the attitude matrix according to the chain rule of the attitude matrix based on the lie group description
Decomposition is in the form of the product of three matrices:
wherein, t represents a time variable,
a pose matrix representing the current carrier frame relative to the current navigation frame,
representing an attitude matrix of an initial navigation system relative to a current navigation system, the initial attitude matrix
A pose matrix representing 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, attitude matrix
And
the time-varying update process is:
wherein ,
a pose matrix representing the initial vehicle system relative to the current vehicle system,
showing the projection of the rotation angular rate of the navigation system relative to the inertial system on the navigation system, and the condition of shaking the baseLower it is equal to the earth rotation angular rate
L represents the local latitude and the local latitude,
the angular rate of rotation of the carrier system representing the gyroscope output relative to the inertial system is projected onto the carrier system, the symbol (· ×) represents the operation of converting a three-dimensional vector into an antisymmetric matrix, and the operation rule is as follows:
as can be seen from the formulas (2) to (5),
and
calculated in real time from IMU sensor data, and
an attitude matrix representing an initial time, which does not change with time; thus, the attitude matrix during SINS self-alignment
Is converted into an initial attitude matrix based on the lie group description
Solving the problem of (1);
according to the strapdown inertial navigation shaking base rough alignment principle and the formula (2), a shaking base rough alignment model based on lie group description can be written in the following form:
wherein ,Vn(t) and Vb(t) represents the speed of the carrier in the system of n and the speed in the system of b, Vn(t) and Vb(t) can be obtained by the following formula:
and due to the initial rotation matrix
Is a constant matrix, whereby the rocking base coarse alignment model can be written as:
wherein R(tk) Represents tkOf time of day
And (3): solving error lie algebra according to physical definition of the lie algebra, and utilizing exponential mapping between the lie group and the lie algebra to make R (t)k) And (6) estimating.
According to the definition of the rotation matrix and the formula (9), Vn(t) and R (t)k)VbAnd (t) is the same vector under the n system, and the vector product is 0. However, due to the presence of errors, the estimated rotation matrix is compared to the actual rotation matrix R (t)k) There is a discrepancy between them, which results in an estimated rotation matrix
Is the attitude matrix from b to n', i.e.:
according to the chain rule, to compensate the estimated rotation matrix, we only need to calculate the rotation relationship between the n' system and the n system, i.e. we need to calculate
According to the rotary formula of rodregs:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aaT+(sinθ)a× (ll)
an arbitrary rotation matrix can be obtained from a corresponding rotation lie algebra Φ, and an arbitrary rotation lie algebra (rotation vector) Φ can be obtained from the rotation angle θ and the rotation axis a by the following equation:
φ＝θa (12)
further 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, so that the rotation relationship between n 'system and n system, i.e. the rotation relationship between n' system and n system
Equivalence and solution observation vector Vn(t) and the prediction vector
So that V is determined only by the equations (11) and (12)n(t) and 
the rotating shaft and the rotating angle can be determined
Vn(t) and 
rotational axis and angle and rotational relationship therebetweenAs shown in fig. 3 below.
Whereby the rotating shaft a can pass through the pair
The following are obtained by unitization:
the rotation angle θ can be obtained by the following equation:
error lie algebra
Can be written as:
the update equation for the optimal estimation of lie groups can then be established as follows:
and (4): solving attitude matrices required by a navigation system
Thereby accomplish and rock base self-alignment process:
according to the attitude change matrix obtained by solving in the previous step
And
information, the navigation attitude matrix can be solved through the formula (2), and SINS strapdown is completedThe inertial navigation system rocks the base self-alignment.
Compared with the prior art, the invention has the following advantages and beneficial effects:
(1) according to the invention, the initial attitude matrix is described by replacing quaternion with the lie group, and a non-collinear vector is not required to be used as observation, so that the problem of long alignment time of the traditional TRIAD method is solved;
(2) according to the physical definition of the lie algebra, the error lie algebra is calculated by utilizing the direction of the product of the prediction vector and the observation vector and the included angle between the prediction vector and the observation vector, 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 (coarse alignment based on the Wahba problem) is avoided, and the alignment precision is improved.
Drawings
FIG. 1 is a simplified overview of a strapdown inertial navigation system device.
FIG. 2 is a flow chart of a strapdown inertial navigation system.
FIG. 3Vn(t) and 
the rotating shaft and the rotating angle and the rotating relation graph between the two parts.
FIG. 4 is a graph of the results of a moving base alignment simulation.
Detailed Description
The invention relates to a self-alignment method design of a shaking base of an SINS strapdown inertial navigation system based on lie swarm optimal estimation, and the specific implementation steps of the invention are described in detail by combining the system flow chart of the invention:
the invention provides a shaking base self-alignment method of an SINS strapdown inertial navigation system based on lie group optimal estimation, which comprises the steps of firstly, acquiring real-time data of a sensor; processing the acquired data, and establishing a linear self-alignment system model based on the lie group description based on the lie group differential equation; estimating to obtain an initial attitude matrix based on the lie group description by using an optimal lie group estimation algorithm
And solving the attitude matrix
During the self-alignment period, the accurate initial attitude matrix is finally obtained through multiple estimation and calculation
And attitude matrix
The self-alignment process is completed.
Step 1: starting and initializing an SINS inertial navigation system, and obtaining longitude lambda and latitude L and local gravity acceleration g of the position of a carriernCollecting angular rate information output by a gyroscope in an inertial measurement unit IMU (inertial measurement Unit) according to basic information
And acceleration information f output by the accelerometerb。
Step 2: processing the acquired data of the gyroscope and the accelerometer, establishing a linear shaking base self-alignment system model based on the lie group description based on the lie group differential equation,
based on a lie group differential equation, establishing a linear self-alignment system model based on the lie group description:
according to the SINS strapdown inertial navigation system principle, the SINS shaking base self-alignment problem is converted into a posture estimation problem, the posture is converted into rotation transformation between two coordinate systems, and a navigation posture matrix is represented by a 3 multiplied by 3 orthogonal transformation matrix; the orthogonal transformation matrix conforms to the property of a special orthogonal group SO (n) of lie groups, and forms a three-dimensional rotation group SO (3):
wherein R ∈ SO (3) represents a specific navigation attitude matrix,
representing a 3 x 3 vector space, superscript T representing the transpose of the matrix, I representing a three-dimensional identity matrix, det (R) representing the determinant of matrix R;
the self-alignment attitude estimation problem of the shaking base is converted into a solving problem of an attitude matrix R based on the lie group description; navigating the attitude matrix according to the chain rule of the attitude matrix based on the lie group description
Decomposition is in the form of the product of three matrices:
wherein, t represents a time variable,
a pose matrix representing the current carrier frame relative to the current navigation frame,
representing an attitude matrix of an initial navigation system relative to a current navigation system, the initial attitude matrix
A pose matrix representing 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, attitude matrix
And
the time-varying update process is:
wherein ,
a pose matrix representing the initial vehicle system relative to the current vehicle system,
representing the projection of the angular rate of rotation of the navigational system relative to the inertial system on the navigational system, which is equal to the angular rate of rotation of the earth under the condition of shaking the base
L represents the local latitude and the local latitude,
the angular rate of rotation of the carrier system representing the gyroscope output relative to the inertial system is projected onto the carrier system, the symbol (· ×) represents the operation of converting a three-dimensional vector into an antisymmetric matrix, and the operation rule is as follows:
as can be seen from the formulas (2) to (5),
and
calculated in real time from IMU sensor data, and
an attitude matrix representing an initial time, which does not change with time; thus, the attitude matrix during SINS self-alignment
Is converted into an initial attitude matrix based on the lie group description
Solving the problem of (1);
according to the strapdown inertial navigation shaking base rough alignment principle and the formula (2), a shaking base rough alignment model based on lie group description can be written in the following form:
wherein ,Vn(t) and Vb(t) represents the speed of the carrier in the system of n and the speed in the system of b, Vn(t) and Vb(t) can be obtained by the following formula:
and due to the initial rotation matrix
Is a constant matrix, whereby the rocking base coarse alignment model can be written as:
wherein R(tk) Represents tkOf time of day
And (3): solving error lie algebra according to physical definition of the lie algebra, and utilizing exponential mapping between the lie group and the lie algebra to make R (t)k) And (6) estimating.
According to the definition of the rotation matrix and the formula (9), Vn(t) and R (t)k)VbAnd (t) is the same vector under the n system, and the vector product is 0. However, due to the presence of errors, the estimated rotation matrix is compared to the actual rotation matrix R (t)k) There is a discrepancy between them, which results in an estimated rotation matrix
Is the attitude matrix from b to n', i.e.:
according to the chain rule, to compensate the estimated rotation matrix, we only need to calculate the rotation relationship between the n' system and the n system, i.e. we need to calculate
According to the rotary formula of rodregs:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aaT+(sinθ)a× (27)
an arbitrary rotation matrix can be obtained from a corresponding rotation lie algebra Φ, and an arbitrary rotation lie algebra (rotation vector) Φ can be obtained from the rotation angle θ and the rotation axis a by the following equation:
φ＝θa (28)
further 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, so that the rotation relationship between n 'system and n system, i.e. the rotation relationship between n' system and n system
Equivalence and solution observation vector Vn(t) and the prediction vector
So that V is determined only by the equations (11) and (12)n(t) and 
the rotating shaft and the rotating angle can be determined
Vn(t) and 
the rotational axis and angle and the rotational relationship therebetween are shown in fig. 3 below.
Whereby the rotating shaft a can pass through the pair
The following are obtained by unitization:
the rotation angle θ can be obtained by the following equation:
error lie algebra
Can be written as:
the update equation for the optimal estimation of lie groups can then be established as follows:
and (4): solving an attitude matrix
And the attitude information is resolved,
in step (2), the SINS is self-aligned
Is converted into a pair
And will solve the problem
Decomposition is in the form of three matrix products:
solved according to formula (28), formula (31) and formula (46)
And
attitude matrix
The solving method is as follows:
and obtaining an attitude matrix according to the solution
And resolving attitude information.
The invention has the following beneficial effects:
(1) the method was subjected to simulation experiments under the following simulation conditions:
in the step (1), the simulation carrier is influenced by wind waves under the condition of shaking the base, the course angle psi, the pitch angle theta and the roll angle gamma of the simulation carrier periodically change, and the posture change conditions are as follows:
in step (1), the initial geographic position: east longitude 118 degrees, north latitude 40 degrees;
in the step (1), the output frequency of the sensor is 100 Hz;
in the step (1), the gyroscope drifts: the gyro constant drift on the three directional axes is 0.02 degree/h, and the random drift is 0.005 degree/h;
in the step (1), zero offset of the accelerometer: the constant bias of the accelerometer on three direction axes is 2 multiplied by 10-4g, randomly biasing to
In step (2), the rotation angular rate of the earth 7.2921158e-5rad/s；
In the step (2), the time interval T is 0.02 s;
in step (3), the initial value of the optimal estimation algorithm of the plum group
The simulation result of the method is as follows:
200s simulation is carried out, the estimation error of the attitude angle is taken as a measurement index, and the simulation result is shown in a figure (4). As can be seen from the figure, the pitch attitude completes alignment in about 30s, converging to 0.4'; the transverse rolling posture is aligned in about 35s and converged to 0.5'; the heading attitude is aligned around 45s and converges to 4.4'. According to the simulation result, the method can quickly and effectively complete the self-alignment task under the shaking base.
The invention adopts the lie group description to replace the traditional quaternion description to realize the calculation of SINS attitude transformation, and utilizes the lie group differential equation to establish a linear initial alignment filtering model based on the lie group description. Therefore, one-step direct self-alignment of SINS is realized, and the problems of non-uniqueness and non-linearity caused by the description of an initial attitude matrix by a traditional quaternion are avoided; the invention utilizes the left multiplication invariance of the lie group and the exponential mapping between the lie group and the lie algebra to carry out equivalent transformation on the state-related error, establishes an equivalent state-independent lie algebra filtering equation, avoids the influence of state-related noise on a filtering result, and effectively improves the alignment precision.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. It should be noted that, for a person skilled in the art, several modifications and variations can be made without departing from the principle of the present invention, and these modifications and variations should also be regarded as the protection scope of the present invention.
Claims (2)
1. The strapdown inertial navigation shaking base rough alignment method based on the optimal estimation of the lie group is characterized by comprising the following steps of:
step (1): the SINS strapdown inertial navigation system carries out system preheating preparation, starts the system, and obtains the longitude lambda and the latitude L of the position of the carrier and the projection g of the local gravity acceleration under the navigation systemnBasic information, collecting the projection of the rotation angular rate information of the carrier system output by the gyroscope in the inertial measurement unit IMU relative to the inertial system on the carrier system
And carrier system acceleration of accelerometer outputInformation fb；
Step (2): preprocessing acquired data of the gyroscope and the accelerometer, and establishing a linear shaking base self-alignment system model based on the lie group description on the basis of a lie group differential equation:
based on a lie group differential equation, establishing a linear self-alignment system model based on the lie group description:
according to the SINS strapdown inertial navigation system principle, the SINS shaking base self-alignment problem is converted into a posture estimation problem, the posture is converted into rotation transformation between two coordinate systems, and a navigation posture matrix is represented by a 3 multiplied by 3 orthogonal transformation matrix; the orthogonal transformation matrix conforms to the property of a special orthogonal group SO (n) of lie groups, and forms a three-dimensional rotation group SO (3):
wherein R ∈ SO (3) represents a specific navigation attitude matrix,
representing a 3 x 3 vector space, superscript T representing the transpose of the matrix, I representing a three-dimensional identity matrix, det (R) representing the determinant of matrix R;
the self-alignment attitude estimation problem of the shaking base is converted into a solving problem of an attitude matrix R based on the lie group description; navigating the attitude matrix according to the chain rule of the attitude matrix based on the lie group description
Decomposition is in the form of the product of three matrices:
wherein, t represents a time variable,
a pose matrix representing the current carrier frame relative to the current navigation frame,
representing an attitude matrix of an initial navigation system relative to a current navigation system, the initial attitude matrix
A pose matrix representing 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, attitude matrix
And
the time-varying update process is:
wherein ,
a pose matrix representing the initial vehicle system relative to the current vehicle system,
representing the projection of the angular rate of rotation of the navigation system relative to the inertial system on the navigation system, which is equal toAngular rate of rotation of the earth
L represents the local latitude and the local latitude,
the angular rate of rotation of the carrier system representing the gyroscope output relative to the inertial system is projected onto the carrier system, the symbol (· ×) represents the operation of converting a three-dimensional vector into an antisymmetric matrix, and the operation rule is as follows:
as can be seen from the formulas (2) to (5),
and
calculated in real time from IMU sensor data, and
an attitude matrix representing an initial time, which does not change with time; thus, the attitude matrix during SINS self-alignment
Is converted into an initial attitude matrix based on the lie group description
Solving the problem of (1);
according to the strapdown inertial navigation shaking base rough alignment principle and the formula (2), a shaking base rough alignment model based on lie group description can be written in the following form:
wherein ,Vn(t) and Vb(t) represents the speed of the carrier in the system of n and the speed in the system of b, Vn(t) and Vb(t) can be obtained by the following formula:
and due to the initial rotation matrix
Is a constant matrix, whereby the rocking base coarse alignment model is written as:
wherein R(tk) Represents tkOf time of day
And (3): solving error lie algebra according to physical definition of the lie algebra, and utilizing exponential mapping between the lie group and the lie algebra to make R (t)k) Carrying out estimation;
according to the definition of the rotation matrix and the formula (9), Vn(t) and R (t)k)Vb(t) is the same vector under n series, and the vector product is 0; however, due to errors, the estimated rotation matrix is compared to the actual rotation matrix R (t)k) There is a deviation between, which results in an estimated rotational momentMatrix of
Is the attitude matrix from b to n', i.e.:
according to the chain rule, to compensate the estimated rotation matrix, we only need to calculate the rotation relationship between the n' system and the n system, i.e. we need to calculate
According to the rotary formula of rodregs:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aaT+(sinθ)a× (11)
the arbitrary rotation matrix can be obtained from the corresponding rotation lie algebra Φ, and the arbitrary rotation lie algebra (rotation vector) Φ can be obtained from the rotation angle θ and the rotation axis a by the following formula:
φ＝θ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 coordinate systems, so that the rotation relationship between n 'system and n system, i.e. the rotation relationship between n' system and n system
Equivalence and solution observation vector Vn(t) and the prediction vector
So that V is determined only by the equations (11) and (12)n(t) and 
rotational axis and angle therebetween
Vn(t) and 
the rotating shaft, the rotating angle and the rotating relation between the two parts;
the rotating shaft a passes through the pair
The following are obtained by unitization:
the rotation angle θ is obtained by the following equation:
error lie algebra
Writing into:
the update equation for the optimal estimation of lie groups can then be established as follows:
2. the strap-down inertial navigation moving base initial alignment method based on lie group description as claimed in claim 1, wherein in step (3), the error lie algebra is solved according to the physical definition of the lie algebra, and the exponential mapping between the lie group and the lie algebra is used to perform R (t) mappingk) Carrying out estimation;
the step (3.1) separates the state-dependent noise by left-multiplication invariance of the lie group and exponential mapping between the lie group and the lie algebra as follows:
according to the definition of the rotation matrix and the formula (9), Vn(t) and R (t)k)Vb(t) is the same vector under n series, and the vector product is 0; however, due to errors, the estimated rotation matrix is compared to the actual rotation matrix R (t)k) There is a discrepancy between them, which results in an estimated rotation matrix
Is the attitude matrix from b to n':
according to the chain rule, to compensate the estimated rotation matrix, only the rotation relationship between the n 'system and the n system needs to be calculated, i.e. the rotation relationship between the n' system and the n system
According to the rotary formula of rodregs:
R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aaT+(sinθ)a× (18)
the arbitrary rotation matrix is obtained from the corresponding rotation lie number Φ, and the arbitrary rotation lie number Φ is obtained from the rotation angle θ 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 coordinate systems, so that the rotation relationship between n 'system and n system, i.e. the rotation relationship between n' system and n system
Equivalence and solution observation vector Vn(t) and the prediction vector
So that V is determined only by the equations (18) and (19)n(t) and 
the rotating shaft and the rotating angle therebetween are determined
Vn(t) and 
the rotating shaft, the rotating angle and the rotating relation between the two parts;
whereby the rotating shaft a passes through the pair
The following are obtained by unitization:
the rotation angle θ is obtained by the following equation:
error lie algebra
Writing into:
the update equation for the optimal estimation of lie groups 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 true CN112229421A (en) | 2021-01-15 |
| CN112229421B 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) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112833918A (en) * | 2021-02-08 | 2021-05-25 | 北京理工大学 | 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 (3)
| Title |
|---|
| RONG, H 等: "GAM-Based Mooring Alignment for SINS Based on An Improved CEEMD Denoising Method", 《SENSORS》 * |
| 徐浩;裴福俊;蒋宁;: "一种基于李群描述的深空探测器姿态估计方法", 深空探测学报, no. 01 * |
| 裴福俊 等: "基于空间域划分的分布式SLAM算法", 《***工程与电子技术》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112833918A (en) * | 2021-02-08 | 2021-05-25 | 北京理工大学 | 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 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112229421B (en) | 2023-08-11 |
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 | |
| CN110595503B (en) | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation | |
| CN107588771B (en) | Lei cluster description-based strapdown inertial navigation resolving method | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN104635251B (en) | A kind of INS/GPS integrated positionings determine appearance new method | |
| CN103033189B (en) | Inertia/vision integrated navigation method for deep-space detection patrolling device | |
| CN109931957B (en) | Self-alignment method of SINS strapdown inertial navigation system based on LGMKF | |
| CN105698822B (en) | Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced | |
| CN104713555A (en) | Autonomous vehicle navigation method for assisting orientation by applying omnimax neutral point | |
| CN111366148B (en) | Target positioning method suitable for multiple observations of airborne photoelectric observing and sighting system | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| CN109470241B (en) | Inertial navigation system with gravity disturbance autonomous compensation function and method | |
| CN111399023B (en) | Inertial basis combined navigation filtering method based on lie group nonlinear state error | |
| CN112880669B (en) | Spacecraft starlight refraction and single-axis rotation modulation inertial integrated navigation method | |
| CN102519485A (en) | Gyro information-introduced double-position strapdown inertial navigation system initial alignment method | |
| CN110058288A (en) | Unmanned plane INS/GNSS integrated navigation system course error modification method and system | |
| CN110926499B (en) | SINS strapdown inertial navigation system shaking base self-alignment method based on Liqun optimal estimation | |
| CN112229421A (en) | Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation | |
| CN111207773B (en) | Attitude unconstrained optimization solving method for bionic polarized light navigation | |
| CN110388942B (en) | Vehicle-mounted posture fine alignment system based on angle and speed increment | |
| CN110221331B (en) | Inertia/satellite combined navigation dynamic filtering method based on state transformation | |
| CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
| CN112212889A (en) | SINS strapdown inertial navigation system shaking base rough alignment method based on special orthogonal group optimal estimation | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor |
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 |