CN1908584A - Method for determining initial status of strapdown inertial navigation system - Google Patents

Abstract

The initial attitude determination method for SINS comprises: rotating the SINS from initial position to any one position round arbitrary 3D axis; according to SINS output on first position and the relation of earth rotational angular velocity and acceleration of gravity, determining the initial attitude primarily; then, using Kalman filter to estimate the gyro constant drift and other values for more accurate initial attitude. This invention improves observability and precision.

Description

The method of determining a gesture of initial strapdown inertial navigation system

FIELD

The present invention relates to a method of determining the initial posture strapdown inertial navigation system (SINS), can be used in various high-precision initial attitude strapdown inertial navigation system to determine, especially for non-airborne SINS indexing mechanism during installation, or indexing means is not high precision, limited angle circumstances.

Background technique

The basic principles of the strapdown inertial navigation system (of SINS) is based on the laws with respect to inertial space Newtonian proposed mechanics, the use of a gyroscope, a line motion relative to inertial space accelerometers measure vector and angular motion parameters at a given motion initial conditions , integral calculation performed by a computer, continuous, provide position, velocity and attitude information in real time. SINS entirely on their own inertia sensitive element, not dependent on any external information measuring navigation parameters, so it has good concealment, from weather conditions, no loss of signal, without interference, etc., is a fully autonomous, all-weather the navigation system has been widely used in aviation, aerospace, marine and other fields. The basic principle of the SINS, SINS initial information must be obtained before calculation navigation solution, including the initial position, velocity and attitude. SINS initial position and velocity information is more readily available initial posture, attitude determination accuracy after initial is the initial accuracy of navigation SINS enters the working state. Therefore, fast and accurate initial attitude determination is a very important step when SINS work.

Initial posture conventional strapdown inertial navigation system can be divided into coarse alignment and determining the fine alignment in two stages. Coarse alignment stage is the base under static conditions, a single position or two positions (two positions or any two fixed positions) of the gyroscope and accelerometer output is introduced directly into the computer, calculating the initial attitude vector. When using this method, the error is often ignored and the external disturbance factors gyroscope and accelerometer, however, these factors can lead to errors, so the initial attitude calculation accuracy. In particular, when using a gyro on two fixed positions and accelerometer output calculated in claim SINS rotation about the Z axis by 90 degrees or 180 degrees, which requires to be installed on a SINS indexing mechanism, the indexing mechanism using rotated 180 degrees or 90 degrees to achieve, the use of very inconvenient when the project, and the precision indexing mechanism is not high, the rotational angle can not be accurately measured, decrease the accuracy of determining the initial attitude. Due to limited to the specific project SINS mounting position, there is a rotation angle of 180 degrees or can not meet the requirement of 90, this time, two conventional fixed initial posture position determining method will not be applied. Further, when the gyros by any two positions and the accelerometer output, calculate the arc sine function, a gyroscope and accelerometer itself error and a measurement error and the like, is likely to cause the calculation results instability occurs imaginary phenomenon, and therefore often need multiple collection and calculation of any gyro and accelerometer outputs two positions, the selection results of the calculation, the average value, as the initial attitude of the carrier. Thus, the initial attitude determination time will be doubled, and initial pose calculation accuracy can not be guaranteed.

Fine alignment stage is carried out in the rough alignment based on the use of modern control theory state space method, the data output of the gyroscope and accelerometer are processed. When the processing of data in a single location, the azimuth angle of considerable misalignment poor, slow convergence takes longer, and a gyroscope and accelerometer errors can not be observed, therefore, can not be a good estimate of not increase the initial attitude accuracy purposes, can not be achieved calibration of gyroscopes and accelerometers while the initial attitude calculation. When the two fixed positions of the data processing, as already described, there is a limitation of the rotational angle position of the two conventional fixed initial attitude determination method, and the higher accuracy of the indexing means, particularly engineering applications often can not.

SUMMARY

The technical problem of the present invention are: to overcome the deficiencies of the prior art by providing an accurate method for determining the initial posture, convenience strapdown inertial navigation system.

Technical Solution The present invention is as follows: initial gesture of strapdown inertial navigation system determination method, the following steps: (1) SINS warming up, depending on the specific preparation time different systems; (2) SINS After completion of the preparation, SINS held in the initial position (referred to as a first position) stationary, and gathering the gyro output accelerometer output;

(3) the use of accelerometer output, and the relationship between the gravitational acceleration of the Earth from the gyro output angle rate relationship is calculated heading angle 1 a first position, the roll angle and the pitch angle θ1 γ1; (4) without indexing mechanism, the rotation of SINS by any method from a first position the three-dimensional space about any axis to any position (referred to as second position), SINS held stationary in the second position, the gyro output and capture accelerometer output; (5 ) using the Kalman filtering technique, 1, θ1 and γ1 as an initial parameter, the data on two positions gyroscopes and accelerometers output processing. SINS position due to the change, change in the error model SINS system matrix, thereby SINS system observability is improved, the filtering effect is improved, a better estimate of the geographic coordinate system of a computing second position n 'the real geographic coordinates the error angle (simply referred to as misalignment angle φx, φy and Phi] z) gyro drift constant, the accelerometer-based offset between the constant n; (6) using the estimated φx, φy and calculate the true geographic coordinates Phi] z n and calculating a geographic coordinate system transformation matrix Cn'n between n '. The incremental gyro output angle or angular velocity information, using quaternion calculating 'transformation matrix Cbn between' b n coordinate system and geographic coordinates calculated vector. And a Cn'n Cbn ', the calculated transformation matrix Cbn coordinate b between the support and the true geographic coordinate system n, the heading angle recalculated by the second position Cbn 2, pitch angle and roll angle θ2 γ2, the as an initial gesture of SINS.

The principles of the present invention is: when SINS remain stationary at a certain position, the gravitational acceleration and the accelerometer output the gyro output angle from the Earth rate the following relationship: fx1fy1fz1 = Cnb00g ... (1)]]> & omega; x1 & omega; y1 & omega; z1 = Cnb0 & omega; iecosL & omega; iesinL ... (2)]]> wherein, fx1, fy1 and fz1 and ωx1, ωy1 and ωz1 respectively SINS position of the X-axis, Y-axis and Z-axis output therefor specific force and angular rate; G is the gravitational acceleration; ωie angular rate from the earth, which is projected on the east, north, days direction of Ω = [0 ωiecosL ωiesinL]; L is the local latitude; Cnb navigation coordinate system into a vector coordinate system transformation matrix, Cnb = (Cbn) T = C11C13C13C21C12C23C31C32C33.]]> Cnb can be expressed as the heading angle 1 this position, the pitch angle and roll angle γ1 expression θ1, the use of (1) and (2 ) can be calculated 1, β1 and γ1 at this location. It is calculated as follows: a (1), (2), we have: C13 = fx1g]]> C23 = fy1g]]> C33 = fz1g]]> C12 = & omega; x1 & omega; iecosL-fx1tanLg]]> C22 = & omega; y1 & omega; iecosL-fy1tanLg ... (3)]]> C32 = & omega; z1 & omega; iecosL-fz1tanLg]]> C11 = C22C33-C23C32C21 = -C12C33 + C13C32C31 = C12C23-C13C22 heading 1, pitch angle θ1 and transverse calculated roll angle of the primary values ​​of γ1: If the heading angle 1, pitch angle and roll angle range θ1 is defined as γ1 [0,2π], [-Π, + π], then 1, θ1 and γ1 true value can be determined as follows.

θ1 = θ1 master (5) Determined by the formula (5) 1, θ1 and γ1 is the heading angle SINS In this position, the pitch angle and roll angle.

Because (1) and (2) the offset is not considered an accelerometer, gyroscope drift and measurement errors gyroscope and an accelerometer output, the obtained angle [phi] 1 heading, pitch angle and roll angle θ1 can γ1 accurately describe the real angular relationship between the vehicle coordinate system and the local geographic coordinate system. Thus, it should be based on this, the further use of modern estimation theory initial attitude misalignment angle estimated from the random errors and random interference.

Kalman filtering techniques, will 1, θ1 and γ1 as the initial parameter can be estimated after calculating the geographic coordinate system n 'and the error angle between the actual geographic coordinate system n, the correction Cbn', obtain a more accurate initial attitude. However, when the Kalman filter process accelerometer and gyroscope output of a single position, the system is not completely observed, wherein the two accelerometers error and a gyroscope can not be observed, so poor is estimated to improve the reach Objective initial attitude accuracy. SINS present invention from a first position by the three-dimensional space around the shaft is rotated to any arbitrary position, i.e. a position change SINS, vary SINS error model system matrix, thereby improving observability SINS system, Kalman filtering to improve the effects of , to better estimate the misalignment angle gyro, accelerometer errors. Using the estimated angular misalignment of the transformation matrix Cbn 'is corrected to obtain a more accurate heading angle, pitch angle and roll angle.

Advantage over the prior art that the present invention is: (1) The present invention breaks the traditional two fixed positions need to calculate the initial attitude of the indexing rotation restricting mechanism SINS 90 degrees or 180 degrees about the Z axis, but without the need for transfer bit means, the SINS about an arbitrary axis to an arbitrary position, the three-dimensional space is a two-position any initial pose calculation. Two fixed positions avoiding a deterioration in accuracy due to the initial posture calculating means indexing accuracy is not high, resulting, i.e., improve the accuracy of calculation of the initial pose. Further, the SINS about an arbitrary axis to an arbitrary position but also greatly facilitate the practical engineering application.

(2) The present invention utilizes a gyroscope and accelerometer data output from two positions, improving observability SINS system, the Kalman filter to improve the filtering effect, a better estimate of the misalignment angle, and the gyro constant drift after the accelerometer constant offset, corrected by the posture matrix estimated misalignment angle, obtain a more accurate heading angle, pitch angle and roll angle.

(3) The present invention can be done at the same initial pose calculation drift measurement and calibration tasks, not only improve the accuracy of the initial posture calculated by the system, and calibration accuracy is also improved, compensation of SINS navigation state, can effectively improve the navigation and positioning accuracy.

BRIEF DESCRIPTION

A flowchart of a method of determining the initial posture in FIG. 1 of the present invention; FIG. 2 is a heading angle [phi], a schematic view of the pitch angle θ and roll angle γ, and FIG Oxnynzn navigation coordinate system, i.e. Northeast days geographic coordinate system, Oxbybzb carrier Coordinate System. Wherein Figure 2a shows the navigation frame from the axis zn Oxnynzn rotated counterclockwise about [phi] coincides with the vector coordinates Oxbybzb,  is the heading angle; Figure 2b shows the xn axis is rotated counterclockwise about the θ coordinate system with the support from the navigation coordinate system Oxnynzn Oxbybzb overlap, θ is the pitch angle; FIG. 2c shows the navigation frame about the shaft rotates counterclockwise Oxnynzn yn coordinates Oxbybzb gamma] coincides with the carrier, γ is the roll angle.

Detailed ways

As shown in FIG. 1, the specific embodiment of the method of the present invention are as follows: 1, after preparation SINS strapdown inertial navigation system to boot into the ready state.

2, a first data acquisition SINS ready position, holding the lower accuracy if SINS data acquisition time may be extended SINS initial position (referred to as a first position) stationary, collecting 5 minutes gyro output and an accelerometer output, .

3, a first position and posture computing the navigation frame is taken as the Northeast days geographic coordinate system, at a first angular position [phi] 1 heading, pitch angle and roll angle θ1 defined γ1 in FIG. 2a, 2b and 2c.

Accelerometer output the gravitational acceleration and the gyroscope output of the Earth's angular rate the following relationship: fx1fy1fz1 = Cnb00g ... (6)]]> & omega; x1 & omega; y1 & omega; z1 = Cnb0 & omega; iecosL & omega; iesinL ... (7) ]]> where, fx1, fy1 and fz1 and ωx1, ωy1 and ωz1 SINS respectively the first position of the X-axis, the specific force and angular rate Y axis and Z axis output; G is the acceleration of gravity; ωie earth since angular rate, which is projected on the east, north, days direction of Ω = [0 ωiecosL ωiesinL], L represents the local latitude; Cnb the navigation system to the transformation matrix carrier system, can be written as: Cnb = (Cbn) T . = C11C13C13C21C12C23C31C32C33]]> of (6), (7), we have: C13 = fx1g]]> C23 = fy1g]]> C33 = fz1g]]> C12 = & omega; x1 & omega; iecosL-fx1tanLg]]> C22 = & omega; y1 & omega; iecosL-fy1tanLg ... (8)]]>

C32 = & omega; z1 & omega; iecosL-fz1tanLg]]> C11 = C22C33-C23C32C21 = -C12C33 + C13C32C31 = C12C23-C13C22 heading 1, the main value of the pitch angle θ1 calculated roll angle γ1 and formula is: Main θ1 = arcsin (C23) (9) If the heading angle 1, pitch angle and roll angle range θ1 is defined as γ1 [0,2π], [-Π, + π], then 1, θ1 and γ1 true value can be determined as follows.

θ1 = θ1 primary (10) Determined by the formula (10) 1, θ1 and γ1 is the heading angle SINS in the first position, the pitch angle and roll angle.

4, the second location data collection SINS rotation about three spatial axes by any arbitrary method from an initial position to any position (referred to as second position), SINS held stationary in the second position, the gyro output collected 5 minutes and accelerometer output.

5, the data filtering process is performed two positions will 1, θ1 and γ1 as the initial parameters by Kalman filtering technique, two data positions gyroscopes and accelerometers output processed accurately estimated angular misalignment φx, φy and Phi] z (Northeast days under the geographic coordinate system is φE, φN and phi] U) and the constant drifts of the gyroscope, accelerometer bias constant.

(. 1) is determined to establish the initial posture SINS navigation error model coordinate system is taken as the Northeast days geographic coordinate system, the position and vertical velocity error is omitted, the accelerometer and gyroscope errors considered random white noise process plus the offset, then the SINS error model: & delta; V & CenterDot; E & delta; V & CenterDot; N & phi; & CenterDot; E & phi; & CenterDot; N & phi; & CenterDot; U & dtri; & CenterDot; x & dtri; & CenterDot; y & epsiv; & CenterDot; x & epsiv; & CenterDot; y & epsiv; & CenterDot; z = 02 & Omega; U0- g0T11T12000-2 & Omega; U0g00T21T22000000 & Omega; U- & Omega; N00T11T12T1300- & Omega; U0000T21T22T2300- & Omega; N0000T31T32T3300000000000000000000000000000000000000000000000000 & delta; VE & delta; VN & phi; E & phi; N & phi; U & dtri; x & dtri; y & epsiv; x & epsiv; y & epsiv; z ... (11)]]> formula subscripts E, N, U represent the east, north and days. Of the Earth's angular rate ωie in the east, north and the projection of heaven direction Ω = [0 ωiecosL ωiesinL] = [0 ΩNΩU], L represents the local latitude; subscripts x, y, z is a vector coordinate system; Tij (i = 1,2,3; j = 1,2,3) is the attitude matrix Cbn the elements, Cbn = {Tij} i = 1,2,3;. j = 1,2,3]]> (2) SINS initial posture determination establishing a Kalman filter model for a strapdown inertial navigation system, taking into account the random deviations random gyro drift error and the accelerometer, the (11) the correction equation in the following form: [X & CenterDot; a (t) X & CenterDot; b (t)] = FTi05 & times; 505 & times; 5 [Xa (t) Xb (t)] + [W & prime; (t) 05 & times; 1] = AiX (t) + W (t) ... (12)]]> wherein, W (t) is white Gaussian noise N (O, Q); and a state variable Xa = [δVEδVNφEφNφU] T, Xb = [xyεxεyεz] T; random noise state vector W & prime; (t) = w & delta; VEw & delta; VNw & phi; Ew & phi; Nw & phi; UT,]]> wherein, δVE, δVN respectively velocity error east and north, φE, φN horizontal misalignment angle; phi] U azimuth misalignment angle; x, y acceleration the count value of the bias is often random, εx, εy, εz gyroscope random constant drift; 05 × 5 05 × 1 and are of a specified dimension Zero matrix; Content F and representatives of Ti as follows: Ti = T11T12000T21T2200000T11T12T1300T21T22T2300T31T32T33,]]> F = 02 & Omega; U0-g0-2 & Omega; U0g00000 & Omega; U- & Omega; N00- & Omega; U0000 & Omega; N00]]> (12) formula Jie for initial attitude determination system of equations of the Kalman filter model. In order to apply the Kalman filter optimal estimation of the state vector, the need to establish a system of observation equations. Select two horizontal velocity error for the concept of measurement, systematic observation equation established for the Z (t) = 10000000000100000000XaXb + & eta; E & eta; N = HX (t) + & eta; (t) ... (13)]]> wherein, η (t) is an observation noise vector system, Gaussian white noise process N (O, R) is.

(3) establishing a discrete Kalman filter system model according to the above equations and observation equations, the discrete Kalman filter equations may be established as follows.

Filter equations: Gain equation: Kk = Pk, k-1HkT [HkPk, k-1HkT + Rk] -1 ... (15)]]> prediction error variance equation: Pk, k-1 = & Phi; k, k-1Pk-1 & Phi ; k, k-1T + Qk-1 ... (16)]]> filtering error variance equation: Pk = [I-KkHk] Pk, (17) k-1 in the formula, Φk, k-1 discretization the state transition matrix (matrix system), Q, R are the covariance matrix of the observation noise and system noise.

(4) filtering the initial condition X (0) = 010 × 1; P (0), Q and R values ​​corresponding to medium accuracy SINS follows:

P (0) = diag {(0.3m / s) 2, (0.3m / s) 2, (30 ') 2, (10 ") 2, (10") 2, (100μg) 2, (100μg) 2 , (0.1 °) 2, (0.1 °) 2, (0.1 °) 2}; Q = diag {(100μg) 2, (100μg) 2, (0.1 °) 2, (0.1 °) 2, (0.1 °) 2,0,0,0,0,0}; R = diag {(0.1m / s) 2, (0.1m / s) 2}.

6, calculates a gyro random constant drifts and accelerometer random constant offset filter estimated x, y accelerometer that is a random bias constant, εx, εy, εz gyro random constant drift.

7, calculating and computing vector coordinates b geographic coordinate system n 'transformation matrix Cbn between' may be utilized or angular increments of angular velocity information output from the gyro is calculated Cbn quaternion method ', the following calculation steps: (1) using the heading angle [phi] 1, the pitch attitude angle θ1 when the roll angle γ1 and initialize the first position, to calculate the initial transformation matrix Cbn 'and quaternion q, is calculated as follows: So Cbn & prime; = T11T12T13T21T22T23T31T32T33]]> there are: q0 = & PlusMinus; 121 + T11 + T22-T33]]> q1 = & PlusMinus; 121 + T11 + T22-T33]]> q2 = & PlusMinus; 121 + T11 + T22-T33 ]]> q3 = & PlusMinus; 121 + T11 T22-T33]]> (2) + update quaternion q0, q1, q2, q3, and transformation matrix Cbn '

q (n + 1) = {(1 - (& Delta; & theta; 0) 28 + (& Delta; & theta; 0) 4384) I + (12 - (& Delta; & theta; 0) 248) (& Delta; theta &;)} q (n) ... (19)]]> wherein, & Delta; & theta; = 0- & Delta; & theta; x- & Delta; & theta; y- & Delta; & theta; z & Delta; & theta; x0 & Delta; & theta; z- & Delta; & theta ; y & Delta; & theta; y- & Delta; & theta; z0 & Delta; & theta; x & Delta; & theta; z & Delta; & theta; y- & Delta; & theta; x0]]> & Delta; & theta; 0 = & Delta; & theta; x2 + & Delta; & theta; y2 + & Delta; & theta; z2]]> transformation matrix Cbn 'the following update equations: Cbn & prime; = q02 + q12-q22-q322 (q1q2-q0q3) 2 (q1q3 + q0q2) 2 (q1q2 + q0q3) q02-q12 + q22 -q322 (q2q3-q0q1) 2 (q1q3-q0q2) 2 (q2q3 + q0q1) q02-q12-q22 + q32 ... (20)]]> 8, computes the heading 2, pitch angle and roll angle θ2 conversion matrix Cn'n γ2 Kalman filter estimated φE, φN, φU calculate the true geographic coordinate system and calculating a geographic coordinate system n n 'between. Calculated based Cn'n and (20) Cbn ', calculates a transformation matrix Cbn coordinate b between the support and the true geographic coordinate system n, then the heading angle is calculated from the second position Cbn 2, and a pitch angle θ2 roll angle γ2, as the initial attitude of SINS. The calculation steps are as follows: (1) calculate the conversion real geographic coordinate system n and calculating a geographic coordinate system n 'between the matrix Cnn'Cn & prime; n = 1- & phi; U & phi; N & phi; U1- & phi; E- & phi; N & phi; E1 ... (21)]]> (2) calculate the transform matrix CbnCbn coordinate b between the support and the true geographic coordinate system n = Cn & prime; nCbn & prime; ... (22)]]> (3) computes the heading 2, pitch angle and roll angle θ2 heading angle γ2 2, defined pitch angle and roll angle θ2 γ2 in FIG. 2a, 2b and 2c.

The (22) is calculated by the equation Cbn referred to as

Cbn = T11T12T13T21T22T23T31T32T33 ... (23)]]> And because Thus, from (23) and (24), [phi] 2 may be determined heading angle, pitch angle and the roll angle θ2 main value γ2, i.e. If the heading angle 2, pitch angle and roll angle θ2 γ2 are defined as the range [0,2], [-Π, + π]. Then, 2, θ2 and γ2 true value is determined by the following formula: θ2 = θ2 primary (26) Determined by the equation (26) 2, θ2 and γ2 is the heading angle SINS in the second position, the pitch angle and roll angle, enters it as a navigation operating state SINS initial posture.

Claims (1)

An initial posture strapdown inertial navigation system determining method, comprising the steps of: (1) SINS preheating ready, SINS held in the initial position, referred to as a first position, stationary, collecting the gyro output and accelerometer output; (2) the accelerometer output relationship and the relationship between the gravitational acceleration of the Earth from the gyro output angle rate, calculate the heading angle 1 a first position, the roll angle and the pitch angle θ1 [gamma] l; ( 3) from a first position about the SINS three-dimensional space is rotated to any arbitrary axis position, called the second position, a second position at SINS held stationary, and gathering the gyro output accelerometer output; (4) Kalman filtering the 1, θ1 and γ1 as an initial parameter, the data on two positions gyroscopes and accelerometers output processing, calculating estimated geographic coordinate system between the position of the second 'real geographic coordinate system n n the error angle, referred to as the misalignment angle φx, φy and φz and constant drift gyroscope, accelerometer bias constant; using (5) of the Kalman filter estimated φx, φy and φz calculate the true geographic coordinates N are calculated based geographic coordinate system n 'conversion matrix between Cn'n The incremental gyro output angle or angular velocity information, quaternion methods, vectors, and calculating the geographic coordinates b n coordinate system calculated' between the the transformation matrix Cbn ', then the heading angle is calculated from the second position Cbn 2, pitch angle and roll angle θ2 γ2, as the initial attitude of SINS.