CN110595503A - Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation - Google Patents
Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation Download PDFInfo
- Publication number
- CN110595503A CN110595503A CN201910718946.4A CN201910718946A CN110595503A CN 110595503 A CN110595503 A CN 110595503A CN 201910718946 A CN201910718946 A CN 201910718946A CN 110595503 A CN110595503 A CN 110595503A
- Authority
- CN
- China
- Prior art keywords
- matrix
- initial
- lie group
- representing
- alignment
- 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
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 shaking base self-alignment method of an SINS strapdown inertial navigation system based on lie group optimal estimation, which adopts the lie group description to replace the traditional quaternion description to realize the calculation of SINS attitude transformation, utilizes the lie group differential equation to establish a linear initial alignment filtering model based on the lie group description, and designs the initial attitude matrix required by navigation determined by the lie group optimal estimation method. The initial attitude matrix is directly and optimally estimated by adopting a lie group optimal estimation algorithm, SO that the initial attitude estimation problem is converted into the optimal estimation problem of an SO (3) group, one-step direct self-alignment of SINS is realized, and the alignment time is greatly shortened; the problems of non-uniqueness and non-linearity caused by the fact that the initial attitude matrix is described by the traditional quaternion are solved, and the alignment precision is effectively improved; the problem that the orthogonality of the rotation matrix cannot be guaranteed due to additive errors and calculation errors in the lie group filtering method is solved. The invention has practical value in practical engineering.
Description
Technical Field
The invention discloses a shaking base self-alignment method of an SINS strapdown inertial navigation system based on lie group optimal estimation, 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 problems of non-uniqueness and non-linearity of the traditional quaternion description method are avoided; (2) the linear self-alignment filtering model based on lie group description is established by utilizing the lie group differential equation, so that the one-step direct self-alignment process of the SINS is realized, and compared with the existing two-step alignment method, the alignment time can be greatly shortened and the alignment precision can be improved; (3) the SINS strapdown inertial navigation system shaking base self-alignment method based on lie group optimal estimation can effectively avoid the complex expression problem and a large amount of calculation errors generated by the conversion of quaternions to attitude matrixes in the existing quaternion Kalman filtering method, and can avoid the problem that the orthogonality of a rotation matrix cannot be ensured due to additive errors and calculation errors in the lie group filtering method.
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 informationAnd 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; according to the baseChain rule of lie group described attitude matrix, navigation attitude matrixDecomposition 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 matrixA 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 matrixAndthe time-varying update process is:
wherein the content of the first and second substances,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 baseL 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),andcalculated in real time from IMU sensor data, andan attitude matrix representing an initial time, which does not change with time; thus, the attitude matrix during SINS self-alignmentIs converted into an initial attitude matrix based on the lie group descriptionSolving the problem of (1);
accelerometer measurement information f under base shaking conditionbExpressed as:
wherein, gbShowing the projection of the local gravitational acceleration under the carrier system, δ fbWhich is indicative of other noise, is,showing the projection of the centripetal acceleration caused by the shaking under the carrier system,the IMU sensor data is obtained through real-time calculation, and the calculation process is as follows:
wherein r isbThe distance from the installation position of the inertial sensor to the axis of the shaking base is represented;
will acceleration of gravity gbMeasuring information f from an accelerometerbSeparating to obtain:
according to the SINS inertial navigation philosophy and lie chain law, gnAnd gbThere is the following relationship between:
and (3) performing item shifting and sorting operations on the formula (9) to obtain:
equation (10) is simplified as:
wherein the content of the first and second substances,representing the projection of the gravitational acceleration under the initial navigation system,representing a projection of gravitational acceleration under an initial carrier system;
integrating equation (11) over [0, t ] yields:
wherein the content of the first and second substances,representing acceleration of gravityThe corresponding velocity vector is set to be,representing acceleration of gravityA corresponding velocity vector;
equation (12) can be simplified as:
wherein
Equation (12) relates to the initial attitude matrixThe mathematical equation of (a) is,andcalculated from the sensor output; given in equation (12)Andthe method is in a continuous form, discretization is needed in the actual calculation process, and a linear measurement equation of the shaking base self-alignment system is established after discretization as follows:
since the attitude matrix will be solvedIs converted into solving the initial attitude matrixAnd is not limited toTo conform to the constant matrix of lie group characteristics, the linear state equation for establishing the rocking base self-aligning system is as follows:
Rk＝Rk-1 (17)
according to the content, the solution problem of the attitude matrix is converted into the solution problem under the inertial coordinate system at the initial moment, and a shaking base self-alignment system equation with a plum cluster structure is established and expressed as follows:
and (3): according to the optimal estimation algorithm of the lie group, directly carrying out the initial attitude matrix based on the description of the lie groupAnd (3) carrying out optimal estimation:
the one-step prediction of the state matrix is represented as:
wherein the content of the first and second substances,one-step prediction of an initial attitude matrix representing time k, Rk-1A posteriori estimation of an initial attitude matrix representing the time k-1;
the estimated value of the measurement vector is expressed as:
wherein the content of the first and second substances,representing velocity vector at time kIs determined by the estimated value of (c),a one-step prediction of the initial attitude matrix representing time k,calculating the data output by the sensor;
in the process of transforming the coordinate system, the existence of errors can cause the rotation relationship between the navigation coordinate system n' obtained by calculation and the real navigation coordinate system n; θ represents the rotation angle of the phase difference between the n' system and the n system, and using the vector product operation property, θ (k) at the k time can be expressed as:
wherein the content of the first and second substances,representing velocity vector at time kIs determined by the estimated value of (c),andcalculating the data output by the sensor;
according to the right-hand rule, theta (k) is the projection of the rotation angle under the n' system, and theta (k) is projected under the b system to obtain the measurement innovation of the optimal estimation algorithm of the lie groupComprises the following steps:
according to the principle of the SINS strapdown inertial navigation system based on lie group description, the state update equation of the lie group optimal estimation algorithm can be written as follows:
wherein R iskRepresenting a posterior estimate of the initial pose matrix at time k, exp () representing an exponential mapping from lie algebraic space to lie group space,representing a pose update matrix based on the lie group structure,representing vectorsThe antisymmetric matrix form of (a);
the self-alignment algorithm of the shaking base of the SINS strapdown inertial navigation system based on the lie group optimal estimation is summarized as follows:
and (4): solving attitude matrices required by a navigation systemThereby accomplish and rock base self-alignment process:
according to the attitude change matrix obtained by solving in the previous stepAndand (3) information, namely solving a navigation attitude matrix through a formula (2) to finish 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) the invention adopts the lie group to describe the initial attitude matrixCan effectively avoidInitial attitude matrix free of traditional quaternion descriptionNon-uniqueness and non-linearity of acoustics.
(2) The method adopts the lie group method to establish the linear system model, realizes the one-step direct self-alignment process of the SINS, and compared with the traditional two-step alignment method, not only can greatly shorten the alignment time, but also can improve the alignment precision.
(3) The SINS strapdown inertial navigation system shaking base self-alignment method based on the lie group optimal estimation can effectively avoid the complex expression problem and a large amount of calculation errors generated by the conversion of quaternions to attitude matrixes in the traditional quaternion Kalman filtering method, and can avoid the problem that the orthogonality of the rotation matrixes cannot be ensured due to additive errors and calculation errors in the lie group filtering method.
Drawings
FIG. 1 is a schematic diagram of a strapdown inertial navigation system
FIG. 2 flow chart of strapdown inertial navigation system
FIG. 3 is a schematic diagram of a rotation relationship between a navigation coordinate system and a body coordinate system
FIG. 4 is a schematic diagram of the rotation angle between the calculated coordinate system and the real coordinate system
FIG. 5 is a flow chart of an optimal estimation method for lie groups
FIG. 6 is a diagram of a self-aligned simulation result of a wobble base
FIG. 7 is a graph of the results of a self-alignment comparison experiment of a rocking base
FIG. 8 is a schematic view of a page of the upper computer collecting real navigation data
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 obtaining real-time data of a sensor(ii) a 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 algorithmAnd solving the attitude matrixDuring the self-alignment period, the accurate initial attitude matrix is finally obtained through multiple estimation and calculationAnd attitude matrixThe 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 informationAnd 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,
step (2.1): by passingUpdating computations
Attitude matrix chain rule and attitude matrix based on lie group descriptionThe decomposition is as follows:
due to the fact thatRepresents the angular velocity of rotation of the navigation system relative to the inertial system, andthe change is usually very slow, and the solving process is updated according to the attitude matrix based on the lie group description, tk-1Time tkAttitude matrix of time of dayThe approximation is:
wherein the content of the first and second substances,
according to equations (25) - (27), the attitude matrixThe iterative process is approximated as:
step (2.2): angular rate information output by a gyroscopeUpdating computations
tk-1Time tkAttitude matrix of time of dayThe approximation is:
wherein, according to a double-subsample rotation vector method, the following can be obtained:
wherein, Delta theta1And Δ θ2Respectively representing two adjacent half-sample periodsOutputting the calculated angle increment by the gyroscope;
the attitude matrix according to equations (29) - (30)The iterative process is approximated as:
step (2.3): establishing initial rotation matrix based on lie group descriptionThe self-aligning system model mode is as follows:
accelerometer measurement information f under base shaking conditionbExpressed as:
will acceleration of gravity gbMeasuring information f from an accelerometerbMiddle dividerAnd (4) separating to obtain:
according to the SINS inertial navigation philosophy and lie chain law, gnAnd gbThere is the following relationship between:
integrating equation (34) over [0, t ], and obtaining after sorting:
substituting the formula (33) into the formula (35) to obtain:
equation (36) reduces to:
wherein
Equation (37) relates to the initial attitude matrixThe mathematical equation of (a) is,andall in a continuous form, solved by an integral iterative algorithmAnddiscrete value at time kAnd
time kThe approximation is:
wherein, T is the sampling period,the unit matrix is represented by a matrix of units,the calculation is iterated by the formula (28);
due to δ fbMuch smaller than the acceleration of gravity gbIn a discrete process, δ fbConsidering the minimum error amount, equation (39) is therefore approximated as:
wherein the content of the first and second substances,
time kThe approximation is:
wherein the content of the first and second substances,the calculation is iterated by the formula (31);
according to the formula, a linear measurement equation of the shaking base self-alignment system is established as follows:
due to the matrix of the postureIs converted into an initial attitude matrixSolve the problem, andto conform to the constant matrix of lie group characteristics, the linear state equation for establishing the rocking base self-aligning system is:
Rk＝Rk-1 (44)
according to the content, the solution problem of the attitude matrix is converted into the solution problem under the inertial coordinate system at the initial moment, and a shaking base self-alignment system equation with a plum cluster structure is established and expressed as follows:
and (3): direct estimation using lie group optimal estimation algorithm
The plum cluster optimal estimation algorithm estimates the shaking base self-alignment model, and the whole process is as follows:
wherein the content of the first and second substances,for one-step prediction of the initial attitude matrix at time k, Rk-1For a posteriori estimation of the initial attitude matrix at time k-1,is an estimated value of the measurement vector at the time k, theta (k) is a projection of the rotation angle between the system n ' and the system n ' under the system n ',for the b is the measured innovation of the optimal estimation algorithm of the next lie group,updating the matrix, R, for lie group structure based poseskA posteriori estimation of the initial attitude matrix at time k, i.e. as calculated
And (4): solving an attitude matrixAnd the attitude information is resolved,
in step (2), the SINS is self-alignedIs converted into a pairAnd will solve the problemDecomposition is in the form of three matrix products:
solved according to formula (28), formula (31) and formula (46)Andattitude matrixThe solving method is as follows:
and obtaining an attitude matrix according to the solutionAnd 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:
300s simulation is carried out, the estimation error of the attitude angle is taken as a measurement index, and the simulation result is shown in figure (6). As can be seen from the figure, the pitch attitude completes alignment around 120s, converging to 0.42'; the transverse rolling posture is aligned in about 120s and converged to 0.48'; the heading attitude completes alignment around 150s, converging to 1.2'. According to the simulation result, the method can quickly and effectively complete the self-alignment task under the shaking base.
(2) The shaking base self-alignment method of the SINS strapdown inertial navigation system based on the optimal estimation of the lie group provided by the invention is verified through a real experiment. In a real experiment, no external auxiliary information is provided, and the experiment lasts for 300 s. The upper navigation computer controls a navigation system, actual three-axis attitude information with course accuracy of 0.1 degree and attitude accuracy of 0.05 degree is collected at a data updating rate of 100HZ and a baud rate of 115200bps, and the carrier attitude information obtained by resolving is compared with the high-accuracy real carrier attitude information obtained in the step, so that the feasibility and the effectiveness of the method and the system are proved.
The system experiment results are as follows:
a real experiment was performed for 300s, and the estimation error of the attitude angle was used as an index for measuring the alignment accuracy, and the result is shown in fig. 7. As can be seen in the figure, the pitch attitude completes alignment at about 65s, converging to-6.82'; the rolling gesture is aligned in about 50s and converged to 4.37'; the heading pose is aligned around 120s, converging to 23.58'. The method can quickly and effectively complete the self-alignment task under the shaking base, and compared with a quaternion Kalman filtering method, a singular value decomposition method and a lie group filtering method, the method is small in overshoot, high in convergence speed and good in filtering precision.
The method converts the shaking base self-alignment problem into the optimal estimation problem of the initial rotation matrix, adopts the method based on the lie group to describe the initial attitude matrix, and can effectively avoid the problems of non-uniqueness and non-linearity caused by the description of the initial attitude matrix based on the quaternion method in the prior art; the method utilizes the lie group differential equation to establish the one-step alignment linear system model, and compared with the traditional two-step alignment method, the method can greatly shorten the self-alignment time and improve the alignment precision; the invention adopts the lie group optimal estimation method, can avoid the complex expression problem and a large amount of calculation errors generated by the conversion of quaternion to attitude matrix in the quaternion Kalman filtering process, and can avoid the problem that the orthogonality of the rotation matrix can not be ensured due to additive errors and calculation errors in the lie group Kalman filtering process.
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 (5)
1. 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 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 systemAnd carrier system acceleration information f output by accelerometerb；
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 descriptionDecomposition 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 matrixA 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 matrixAndthe time-varying update process is:
wherein the content of the first and second substances,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 baseL 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),andcalculated in real time from IMU sensor data, andan attitude matrix representing an initial time, which does not change with time; thus, the attitude matrix during SINS self-alignmentIs converted into an initial attitude matrix based on the lie group descriptionSolving the problem of (1);
accelerometer measurement information f under base shaking conditionbExpressed as:
wherein, gbShowing the projection of the local gravitational acceleration under the carrier system, δ fbWhich is indicative of other noise, is,showing the projection of the centripetal acceleration caused by the shaking under the carrier system,the IMU sensor data is obtained through real-time calculation, and the calculation process is as follows:
wherein r isbThe distance from the installation position of the inertial sensor to the axis of the shaking base is represented;
will acceleration of gravity gbMeasuring information f from an accelerometerbSeparating to obtain:
according to the SINS inertial navigation philosophy and lie chain law, gnAnd gbThere is the following relationship between:
and (3) performing item shifting and sorting operations on the formula (9) to obtain:
equation (10) is simplified as:
wherein the content of the first and second substances,representing the projection of the gravitational acceleration under the initial navigation system,representing a projection of gravitational acceleration under an initial carrier system;
integrating equation (11) over [0, t ] yields:
wherein the content of the first and second substances,representing acceleration of gravityThe corresponding velocity vector is set to be,representing acceleration of gravityA corresponding velocity vector;
equation (12) can be simplified as:
wherein
Equation (12) relates to the initial attitude matrixThe mathematical equation of (a) is,andcalculated from the sensor output; given in equation (12)Andthe method is in a continuous form, discretization is needed in the actual calculation process, and a linear measurement equation of the shaking base self-alignment system is established after discretization as follows:
since the attitude matrix will be solvedIs converted into solving the initial attitude matrixAnd is not limited toTo conform to the constant matrix of lie group characteristics, the linear state equation for establishing the rocking base self-aligning system is as follows:
Rk＝Rk-1 (17)
according to the content, the solution problem of the attitude matrix is converted into the solution problem under the inertial coordinate system at the initial moment, and a shaking base self-alignment system equation with a plum cluster structure is established and expressed as follows:
and (3): according to the optimal estimation algorithm of the lie group, directly carrying out the initial attitude matrix based on the description of the lie groupAnd (3) carrying out optimal estimation:
the one-step prediction of the state matrix is represented as:
wherein the content of the first and second substances,one-step prediction of an initial attitude matrix representing time k, Rk-1A posteriori estimation of an initial attitude matrix representing the time k-1;
the estimated value of the measurement vector is expressed as:
wherein the content of the first and second substances,representing velocity vector at time kIs determined by the estimated value of (c),a one-step prediction of the initial attitude matrix representing time k,calculating the data output by the sensor;
in the process of transforming the coordinate system, the existence of errors can cause the rotation relationship between the navigation coordinate system n' obtained by calculation and the real navigation coordinate system n; θ represents the rotation angle of the phase difference between the n' system and the n system, and using the vector product operation property, θ (k) at the k time can be expressed as:
wherein the content of the first and second substances,representing velocity vector at time kIs determined by the estimated value of (c),andcalculating the data output by the sensor;
according to the right-hand rule, theta (k) is the projection of the rotation angle under the n' system, and theta (k) is projected under the b system to obtain the measurement innovation of the optimal estimation algorithm of the lie groupComprises the following steps:
according to the principle of the SINS strapdown inertial navigation system based on lie group description, the state update equation of the lie group optimal estimation algorithm can be written as follows:
wherein R iskRepresenting a posterior estimate of the initial pose matrix at time k, exp () representing an exponential mapping from lie algebraic space to lie group space,representing a pose update matrix based on the lie group structure,representing vectorsThe antisymmetric matrix form of (a);
the self-alignment algorithm of the shaking base of the SINS strapdown inertial navigation system based on the lie group optimal estimation is summarized as follows:
and (4): solving attitude matrices required by a navigation systemThereby accomplish and rock base self-alignment process:
according to the attitude change matrix obtained by solving in the previous stepAndand (3) information, namely solving a navigation attitude matrix through a formula (2) to finish self-alignment of the SINS strapdown inertial navigation system shaking base.
2. The base of claim 1The SINS strapdown inertial navigation system shaking base self-alignment method based on lie swarm optimal estimation is characterized in that the SINS strapdown inertial navigation system is started and initialized in the step (1), and the longitude lambda and the latitude L of the position where a carrier is located and the projection g of the local gravity acceleration under a navigation system are obtainednBasic 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 systemAnd carrier system acceleration information f output by accelerometerb。
3. The method for self-aligning the shaking base of the SINS strapdown inertial navigation system based on lie optimal estimation according to claim 1, wherein in the step (2), a linear shaking base self-alignment system model based on the lie description is established based on the lie differential equation;
step (2.1): by passingUpdating computations
Attitude matrix chain rule and attitude matrix based on lie group descriptionThe decomposition is as follows:
due to the fact thatRepresents the angular velocity of rotation of the navigation system relative to the inertial system, andthe change is usually very slow, and the solving process is updated according to the attitude matrix based on the lie group description, tk-1Time tkAttitude matrix of time of dayThe approximation is:
wherein the content of the first and second substances,
according to equations (25) - (27), the attitude matrixThe iterative process is approximated as:
step (2.2): angular rate information output by a gyroscopeUpdating computations
tk-1Time tkAttitude matrix of time of dayThe approximation is:
wherein, according to a double-subsample rotation vector method, the following can be obtained:
wherein, Delta theta1And Δ θ2Respectively representing two adjacent half-sample periodsOutputting the calculated angle increment by the gyroscope;
the attitude matrix according to equations (29) - (30)The iterative process is approximated as:
step (2.3): establishing initial rotation matrix based on lie group descriptionThe self-aligning system model mode is as follows:
accelerometer measurement information f under base shaking conditionbExpressed as:
will acceleration of gravity gbMeasuring information f from an accelerometerbSeparating to obtain:
according to the SINS inertial navigation philosophy and lie chain law, gnAnd gbThere is the following relationship between:
integrating equation (34) over [0, t ], and obtaining after sorting:
substituting the formula (33) into the formula (35) to obtain:
equation (36) reduces to:
wherein
Equation (37) relates to the initial attitude matrixThe mathematical equation of (a) is,andall in a continuous form, solved by an integral iterative algorithmAnddiscrete value at time kAnd
time kThe approximation is:
wherein, T is the sampling period,the unit matrix is represented by a matrix of units,the calculation is iterated by the formula (28);
due to δ fbMuch smaller than the acceleration of gravity gbIn a discrete process, δ fbConsidering the minimum error amount, equation (39) is therefore approximated as:
wherein the content of the first and second substances,
time kThe approximation is:
wherein the content of the first and second substances,the calculation is iterated by the formula (31);
according to the formula, a linear measurement equation of the shaking base self-alignment system is established as follows:
due to the matrix of the postureIs converted into an initial attitude matrixSolve the problem, andto conform to the constant matrix of lie group characteristics, the linear state equation for establishing the rocking base self-aligning system is:
Rk＝Rk-1(44)
according to the content, the solution problem of the attitude matrix is converted into the solution problem under the inertial coordinate system at the initial moment, and a shaking base self-alignment system equation with a plum cluster structure is established and expressed as follows:
4. the method for self-aligning the swaying base of SINS strapdown inertial navigation system based on lie group optimal estimation according to claim 1, wherein the step (3) uses the lie group optimal estimation algorithm to directly estimate
The plum cluster optimal estimation algorithm estimates the shaking base self-alignment model, and the whole process is as follows:
wherein the content of the first and second substances,for one-step prediction of the initial attitude matrix at time k, Rk-1For a posteriori estimation of the initial attitude matrix at time k-1,is an estimated value of the measurement vector at the time k, theta (k) is a projection of the rotation angle between the system n ' and the system n ' under the system n ',for the b is the measured innovation of the optimal estimation algorithm of the next lie group,updating the matrix, R, for lie group structure based poseskA posteriori estimation of the initial attitude matrix at time k, i.e. as calculated
5. The method for self-aligning the swaying base of SINS strapdown inertial navigation system based on lie group optimal estimation according to claim 1, wherein the attitude matrix is solved in the step (4)And resolving attitude information;
in step (2), the SINS is self-alignedIs converted into a pairAnd will solve the problemDecomposition is in the form of three matrix products:
solved according to formula (28), formula (31) and formula (47)Andattitude matrixThe solving method is as follows:
and obtaining an attitude matrix according to the solutionAnd resolving attitude information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910718946.4A CN110595503B (en) | 2019-08-05 | 2019-08-05 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910718946.4A CN110595503B (en) | 2019-08-05 | 2019-08-05 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110595503A true CN110595503A (en) | 2019-12-20 |
| CN110595503B CN110595503B (en) | 2021-01-15 |
Family
ID=68853609
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910718946.4A Active CN110595503B (en) | 2019-08-05 | 2019-08-05 | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110595503B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111399023A (en) * | 2020-04-20 | 2020-07-10 | 中国人民解放军国防科技大学 | Inertial basis combined navigation filtering method based on lie group nonlinear state error |
| CN112033440A (en) * | 2020-08-20 | 2020-12-04 | 哈尔滨工业大学 | Method for achieving latitude-free initial alignment under swing base based on gradient descent optimization |
| CN112212889A (en) * | 2020-09-16 | 2021-01-12 | 北京工业大学 | SINS strapdown inertial navigation system shaking base rough alignment method based on special orthogonal group optimal estimation |
| CN112229421A (en) * | 2020-09-16 | 2021-01-15 | 北京工业大学 | Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation |
| CN113108781A (en) * | 2021-04-01 | 2021-07-13 | 东南大学 | Improved coarse alignment algorithm applied to unmanned ship during traveling |
| CN114636357A (en) * | 2022-03-31 | 2022-06-17 | 北京中科宇航技术有限公司 | Aiming test design method for vertical turntable shaking state |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103245360A (en) * | 2013-04-24 | 2013-08-14 | 北京工业大学 | Autocollimation method of carrier aircraft rotating type strapdown inertial navigation system under shaking base |
| CN103727940A (en) * | 2014-01-15 | 2014-04-16 | 东南大学 | Gravity acceleration vector fitting-based nonlinear initial alignment method |
| CN106871928A (en) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | Strap-down inertial Initial Alignment Method based on Lie group filtering |
| CN107588771A (en) * | 2017-08-28 | 2018-01-16 | 北京工业大学 | Strap-down inertial calculation method based on Lie group description |
| CN109931955A (en) * | 2019-03-18 | 2019-06-25 | 北京工业大学 | Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group |
| CN109931957A (en) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | SINS self-alignment method for strapdown inertial navigation system based on LGMKF |
-
2019
- 2019-08-05 CN CN201910718946.4A patent/CN110595503B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103245360A (en) * | 2013-04-24 | 2013-08-14 | 北京工业大学 | Autocollimation method of carrier aircraft rotating type strapdown inertial navigation system under shaking base |
| CN103727940A (en) * | 2014-01-15 | 2014-04-16 | 东南大学 | Gravity acceleration vector fitting-based nonlinear initial alignment method |
| CN106871928A (en) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | Strap-down inertial Initial Alignment Method based on Lie group filtering |
| CN107588771A (en) * | 2017-08-28 | 2018-01-16 | 北京工业大学 | Strap-down inertial calculation method based on Lie group description |
| CN109931955A (en) * | 2019-03-18 | 2019-06-25 | 北京工业大学 | Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group |
| CN109931957A (en) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | SINS self-alignment method for strapdown inertial navigation system based on LGMKF |
Non-Patent Citations (5)
| Title |
|---|
| HAO XU等: "An In-Motion Initial Alignment Algorithm for SINS Using Adaptive Lie Group Filter", 《PROCEEDINGS OF THE 37TH CHINESE CONTROL CONFERENCE》 * |
| ROBIN CHHABRA等: "A generalized exponential formula for forward and differential kinematics of open-chain multi-body systems", 《MECHANISM AND MACHINE THEORY》 * |
| SILVERE BONNABEL等: "Non-linear observer on Lie Groups for left-invariant dynamics with right-left equivariant output", 《PROCEEDINGS OF THE 17TH WORLD CONGRESS THE INTERNATIONAL FEDERATION OF AUTOMATIC CONTROL》 * |
| 李益群等: "基于李群谱配点法的卫星姿态仿真", 《导航定位与授时》 * |
| 梁青琳: ",基于李群描述的捷联惯导***初始对准方法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111399023A (en) * | 2020-04-20 | 2020-07-10 | 中国人民解放军国防科技大学 | Inertial basis combined navigation filtering method based on lie group nonlinear state error |
| CN112033440A (en) * | 2020-08-20 | 2020-12-04 | 哈尔滨工业大学 | Method for achieving latitude-free initial alignment under swing base based on gradient descent optimization |
| CN112212889A (en) * | 2020-09-16 | 2021-01-12 | 北京工业大学 | SINS strapdown inertial navigation system shaking base rough alignment method based on special orthogonal group optimal estimation |
| CN112229421A (en) * | 2020-09-16 | 2021-01-15 | 北京工业大学 | Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation |
| CN112229421B (en) * | 2020-09-16 | 2023-08-11 | 北京工业大学 | Strapdown inertial navigation shaking base coarse alignment method based on optimal estimation of Litsea group |
| CN113108781A (en) * | 2021-04-01 | 2021-07-13 | 东南大学 | Improved coarse alignment algorithm applied to unmanned ship during traveling |
| CN114636357A (en) * | 2022-03-31 | 2022-06-17 | 北京中科宇航技术有限公司 | Aiming test design method for vertical turntable shaking state |
| CN114636357B (en) * | 2022-03-31 | 2023-11-10 | 北京中科宇航技术有限公司 | Aiming test design method aiming at shaking state of vertical turntable |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110595503B (en) | 2021-01-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110595503B (en) | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation | |
| 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 | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN107588771B (en) | Lei cluster description-based strapdown inertial navigation resolving method | |
| CN109931957B (en) | Self-alignment method of SINS strapdown inertial navigation system based on LGMKF | |
| Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
| CN103743414B (en) | Initial Alignment Method between the traveling of vehicle-mounted SINS assisted by a kind of speedometer | |
| CN106767797B (en) | inertial/GPS combined navigation method based on dual quaternion | |
| CN104165640A (en) | Near-space missile-borne strap-down inertial navigation system transfer alignment method based on star sensor | |
| CN103913181A (en) | Airborne distribution type POS (position and orientation system) transfer alignment method based on parameter identification | |
| CN104713555A (en) | Autonomous vehicle navigation method for assisting orientation by applying omnimax neutral point | |
| CN110567455B (en) | Tightly-combined navigation method for quadrature updating volume Kalman filtering | |
| CN110926499A (en) | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN104374405A (en) | MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering | |
| CN109211230A (en) | A kind of shell posture and accelerometer constant error estimation method based on Newton iteration method | |
| CN109059914A (en) | A kind of shell roll angle estimation method based on GPS and least squares filtering | |
| Lambert et al. | Visual odometry aided by a sun sensor and inclinometer | |
| 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 | |
| CN109211231A (en) | A kind of shell Attitude estimation method based on Newton iteration method | |
| CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
| CN116105730A (en) | Angle measurement-only optical combination navigation method based on cooperative target satellite very short arc observation | |
| CN112229421A (en) | Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation |
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 |