CN1786666A - Method for initial aiming of arbitrary double position of strapdown inertial navigation system - Google Patents

Abstract

The invention relates to random two position aiming method of strapping inertial navigation system. It is the method used to confirm strapping inertial navigation system (SINS) initial appearance. Its features are rotated the SINS from the initial position to random, utilized the relation ship of the two position SINS output and rotational angular velocity of the earth and gravity acceleration to confirm SINS initial appearance and measure out gyroscope constant drift. The invention has high precision. And it is simple and practical, and can be applied in various strapping inertial navigation system.

Description

A kind of any two-position initial alignment method of strapdown inertial navigation system

Technical field

The initial attitude that the present invention relates to a kind of strapdown inertial navigation system SINS is determined method, can be used for various in the initial alignment of high-precision strapdown inertial navigation system.

Background technology

Strapdown inertial navigation system SINS is a kind of autonomous navigational system fully, it utilizes the carrier angular velocity information of gyro to measure and the acceleration information of accelerometer measures, on the basis of initial information, carry out integral operation, position, speed and attitude information can be provided continuously, in real time, has good concealment, be not subjected to advantages such as weather condition restriction, thereby be widely used in fields such as Aeronautics and Astronautics, navigation.According to the ultimate principle of SINS, SINS must obtain initial information before the beginning navigator fix, comprise initial position, speed and attitude.The initial position of SINS and speed are easier to obtain, and therefore how obtaining high-precision initial attitude information fast becomes the very important problem in inertial navigation field, and the process of calculating the SINS initial attitude is called initial alignment.

Initial alignment can be divided into two kinds of so-called autonomous type and controlled formulas, because independently be the inertial navigation system biggest advantage, so autonomous alignment also is the main direction of initial alignment development.Common autonomous alignment method is divided into unit again and puts and the two-position alignment methods, unit puts the initial alignment method and utilizes the information of gyroscope and accelerometer output can calculate the initial attitude of carrier, but since the existence unit of making of gyroscopic drift to put the alignment methods precision not high.The two-position is aimed at and to be utilized on two positions the information of gyroscope and accelerometer output can measure gyroscope constant value drift, has therefore improved the precision of aiming at.But two-position alignment methods in the past requires SINS around Z axle Rotate 180 degree or 90 degree, this just need be installed in SINS on the servo platform, utilize servo platform to realize the rotation of 180 degree or 90 degree, this uses for engineering and has brought inconvenience, and the precision of servo platform is not high, reduces the precision that the two-position is aimed at

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, provide a kind of SINS is rotated to the optional position around arbitrary axis, can determine the initial attitude of SINS, calculate the constant value drift of gyro simultaneously, both guarantee the precision of aiming at, greatly facilitated any two-position of the strapdown inertial navigation system initial alignment method of engineering practical application again.

Technical solution of the present invention is: a kind of any two-position initial alignment method of strapdown inertial navigation system, it is characterized in that SINS is rotated to any one position from initial position, utilize the output of SINS on two positions and the relation of rotational-angular velocity of the earth and acceleration of gravity, can determine the initial attitude of SINS, and can measure gyrostatic constant value drift, its concrete steps are as follows:

(1) the strapdown inertial navigation system preheating is prepared, concrete setup time is according to different systems and difference, SINS is ready, it is motionless as primary importance that maintenance SINS is still in initial position, and the data of collection gyro output in 2-5 minute and accelerometer output, if but the lower proper extension of the precision of SINS is adopted several times;

(2) utilize the relation of accelerometer output and acceleration of gravity, calculate the pitching angle theta of initial position ₁With roll angle γ ₁

(3) by any means SINS is rotated to any one position as the second place, and utilize the variable quantity α of the output calculating course angle of SINS self;

(4) keep SINS in second place transfixion, gather the data of gyro output in 2-5 minute and accelerometer output, utilize accelerometer output to calculate the pitching angle theta of second position ₂With roll angle γ ₂

(5) utilize the variable quantity of output data and the SINS attitude of SINS on two positions, calculate the course angle of the second place _ ₂With gyroscope constant value drift D _x, D _yAnd D _z

Principle of the present invention is: when SINS kept transfixion a position, the angular velocity of the output of three axles of angular velocity passage can be expressed as:

SINS is rotated to the another one optional position, can set up equation in like manner,

Utilize the output of accelerometer can determine the angle of pitch and the course angle θ of two positions ₁, γ ₁, θ ₂And γ ₂, utilize the output of gyro and adopt the hypercomplex number method can calculate two course angle variable quantity α between the position, because rotation time is shorter, so the error that gyroscope constant value drift causes is very little.6 equations that so any two-position alignment issues can be converted into that formula (12) and (13) form and _ ₂=_ ₁+ α 4 unknown numbers of totally 7 equation solutions _ ₁, D _x, D _yAnd D _zProblem.

The present invention's advantage compared with prior art is: the present invention has broken the traditional double position alignment need be with the constraint of SI NS around Z axle Rotate 180 degree or 90 degree, SINS is rotated to the optional position around arbitrary axis, can determine the initial attitude of SINS, calculate the constant value drift of gyro simultaneously, both guarantee the precision of aiming at, greatly facilitated the engineering practical application again.

Description of drawings

Fig. 1 is any two-position of the present invention initial alignment process flow diagram;

Fig. 2 be course angle _, the synoptic diagram of pitching angle theta and roll angle γ, Ox among the figure _ny _nz _nBe navigation coordinate system, i.e. the geographical coordinate system in sky, northeast, Ox _by _bz _bBe carrier coordinate system.Fig. 2 a represents that from navigation coordinate be Ox _ny _nz _nAround z _nThe axle be rotated counterclockwise _ with carrier coordinate system Ox _by _bz _bOverlap, _ be course angle; Fig. 2 b represents that from navigation coordinate be Ox _ny _nz _nAround x _nAxle is rotated counterclockwise θ and carrier coordinate system Ox _by _bz _bOverlap, θ is the angle of pitch; Fig. 2 c represents that from navigation coordinate be Ox _ny _nz _nAround y _nAxle is rotated counterclockwise γ and carrier coordinate system Ox _by _bz _bOverlap, γ is roll angle.

Embodiment

As shown in Figure 1, specific implementation method of the present invention is as follows:

1, the preparation of strapdown inertial navigation system

After the SINS start, enter standby condition, setup time is according to the inhomogeneity system and difference.

2, gather initial position, i.e. primary importance data

SINS is ready, and keeping SINS to be still in initial position is that primary importance is motionless, and gathers the data of gyro output in 2 minutes and accelerometer output, if but the lower proper extension of the precision of SINS is adopted several times.

3, calculate the pitching angle theta of SINS on the primary importance ₁With roll angle γ ₁, the definition of the angle of pitch and roll angle is shown in Fig. 2 b and Fig. 2 c.

Utilize the relation of accelerometer output and acceleration of gravity, calculate the pitching angle theta of initial position ₁With roll angle γ ₁, the formula of calculating is as follows:

${\mathrm{\θ}}_1=\arcsin \left(\frac{{f_{\mathrm{yo}}}^1}{g}\right)\·\·\·\left(14\right)$

${\mathrm{\γ}}_1=\arcsin \left(\frac{-{f_{\mathrm{xo}}}^1}{\cos {\mathrm{\θ}}_1\·g}\right)\·\·\·\left(15\right)$

Wherein, f _Xo ¹Be the X-axis specific force of SINS output on the 1st position, f _Yo ¹Be the Y-axis specific force of SINS output on the 1st position, g is an acceleration of gravity.

4, rotate SINS around arbitrary axis and arrive any position as the second place

SINS is rotated to any one position as the second place around turning axle arbitrarily, utilize output angle increment or the angular velocity information of SINS, adopt the hypercomplex number method to calculate the variable quantity α of course angle, calculation procedure is as follows:

(1) attitude during first position of initialization, pitching angle theta ₁With roll angle γ ₁Output by accelerometer calculates, make course angle _ ₁=0, calculate initial attitude matrix C _b ⁿWith hypercomplex number q, computing formula is as follows:

Order $C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

Then have:

$q_0=\±\frac{1}{2}\sqrt{1+T_{11}+T_{22}-T_{33}}$

$q_1=\±\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}$

$q_2=\±\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}$

$q_3=\±\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}$

(2) upgrade hypercomplex number and attitude matrix

$q(n+1)=\{(1-\frac{{\left({\mathrm{\Δ\θ}}_0\right)}^2}{8}+\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^2}{48}\left(\mathrm{\Δ\θ}\right))\}q\left(n\right)\·\·\·\left(17\right)$

Wherein,

$\mathrm{\Δ\θ}=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& {-\mathrm{\Δ\θ}}_x& 0\end{array}\right]$

${\mathrm{\Δ\θ}}_0=\sqrt{{\mathrm{\Δ\θ}}_x^2+{\mathrm{\Δ\θ}}_y^2+{\mathrm{\Δ\θ}}_z^2}$

Attitude matrix C _b ⁿMore new formula as follows:

$C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\·\·\·\left(18\right)$

(3) calculate the course angle variable quantity

$\mathrm{\α}={\mathrm{tg}}^{-1}\left(\frac{T_{21}}{T_{22}}\right)\·\·\·\left(19\right)$

Main value is judged as following table

T 22Symbol	The α symbol	The α true value	α place quadrant
				+	+	α	First quartile
-	+	α+π	Second quadrant

-	-	α+π	Third quadrant
				+	-	α+2π	Four-quadrant

5, gather second place data

Forward SINS to second position, treat that SINS is stable after, keep SINS in second place transfixion, and gather the data that gyro output in 2 minutes and accelerometer are exported, if but the lower proper extension of the precision of SINS was adopted several times as 5 minutes.

6, calculate the pitching angle theta of SINS on the second place ₂With roll angle γ ₂

Utilize the accelerometer of SINS on the second place to export the pitching angle theta that calculates second position ₂With roll angle γ ₂, computing formula is as follows:

${\mathrm{\θ}}_2=\arcsin \left(\frac{{f_{\mathrm{yo}}}^2}{g}\right)\·\·\·\left(20\right)$

${\mathrm{\γ}}_2=\arcsin \left(\frac{-{f_{\mathrm{xo}}}^2}{\cos {\mathrm{\θ}}_2\·g}\right)\·\·\·\left(21\right)$

Wherein, f _Xo ²Be the X-axis specific force of SINS output on the 2nd position, f _Yo ²Be the Y-axis specific force of SINS output on the 2nd position, g is an acceleration of gravity.

7, calculate course angle _ ₂With gyroscope constant value drift D

Utilize the variable quantity of output data and the SINS attitude of SINS on two positions, the course angle of the calculating second place _ ₂With gyroscope constant value drift D _x, D _yAnd D _z, wherein the definition of course angle calculation procedure shown in Fig. 2 a is as follows:

(1) three axial angle speed of SINS output model

Wherein, $\stackrel{\‾}{{\mathrm{\ω}}_o}={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{xo}}& {\mathrm{\ω}}_{\mathrm{yo}}& {\mathrm{\ω}}_{\mathrm{zo}}\end{array}\right]}^T$ Angular velocity for SINS output.

(2) set up two locational input and output equations

SINS has on first position the time:

SINS has on second position the time:

$+\left[\begin{array}{c}{D}_x-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{lat}\cos {\mathrm{\θ}}_2\sin {\mathrm{\γ}}_2\\ D_y+{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{lat}\sin {\mathrm{\θ}}_2\\ D_z+{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{lat}\cos {\mathrm{\θ}}_2\cos {\mathrm{\γ}}_2\end{array}\right]\·\·\·\left(24\right)$

Wherein, α is the course angle variable quantity, _ ₂=_ ₁+ α.

(3) calculate course angle _ ₂

_ ₂＝_ ₁+α (25)

Wherein, _ ₁Course angle during for first position, α is a position turns over from first position to the second angle, is defined as counterclockwise to just.

First position course angle _ ₁Computing formula be:

Wherein,

Wherein, ω _IeBe rotational-angular velocity of the earth, lat is a latitude,

m _x＝cosγ ₂cosα-sinθ ₂sinγ ₂sinα-cosγ ₁

n _x＝cosγ ₂sinα+sinθ ₂sinγ ₂cosα-sinθ ₁sinγ ₁

m _y＝cosθ ₂cosα-cosθ ₁

n _y＝cosθ ₂sinα

m _z＝sinγ ₂cosα+sinθ ₂cosγ ₂sinα-sinγ ₁

n _z＝sinγ ₂sinα-sinθ ₂cosγ ₂cosα-sinθ ₁cosγ ₁

ω _x′＝ω _xo ²-ω _xo ¹

ω _y′＝ω _yo ²-ω _yo ¹

ω _z′＝ω _zo ²-ω _zo ¹

Δ _x＝ω _iesin lat·(cosθ ₁sinγ ₁-cosθ ₂sinγ ₂)

Δ _y＝ω _iesin lat·sinθ ₂-sinθ ₁

Δ _z＝ω _iesin lat·(cosθ ₂cosγ ₂-cosθ ₁cosγ ₁)

θ ₁, γ ₁And θ ₂, γ ₂Be respectively the angle of pitch, the roll angle of the primary importance and the second place,

$\mathrm{\θ}=\arcsin \left(\frac{f_{\mathrm{yo}}}{g}\right)\·\·\·\left(31\right)$

$\mathrm{\γ}=\arcsin \left(\frac{-f_{\mathrm{xo}}}{\cos \mathrm{\θ}\·g}\right)\·\·\·\left(32\right)$

Wherein, f _XoBe the X-axis specific force of SINS output, f _YoBe the Y-axis specific force of SINS output, g is an acceleration of gravity.

Calculate _ ₁The time, main value be need carry out and determination methods such as following table judged:

cos_ 1Symbol	_ 1Symbol	_ 1True value	_ 1The place quadrant
				+	+	_ 1	First quartile
-	+	π-_ 1	Second quadrant
				-	-	π-_ 1	Third quadrant
+	-	_ 1	Four-quadrant

(4) gyroscope constant value drift D _x, D _yAnd D _zCalculating

D _x＝ω _xo-(ω _iecoslat sin_ _icosγ _i+ω _iecoslat sinθ _icos_ _isinγ _i-ω _iesin lat cosθ _isinγ _i) (33)

D _y＝ω _yo-(ω _iecos lat cos_ _i cosθ _i+ω _iesin lat sinθ _i) (34)

D _z＝ω _zo-(ω _iecos lat sin_ _i sinγ _i-ω _iecos lat sinθ _icos_ _icosγ _i+ω _iesin lat cosθ _icosγ _i) (35)

Wherein, i=x, y.

Claims (4)

1, a kind of any two-position initial alignment method of strapdown inertial navigation system is characterized in that may further comprise the steps:

(1) keep SINS at initial position as the primary importance transfixion, and gather the data that gyro output in 2-5 minute and accelerometer are exported;

(2) utilize the output of accelerometer and the relation of acceleration of gravity, calculate the pitching angle theta of initial position ₁With roll angle γ ₁

(3) by any means SINS is rotated to any one position as the second place, and utilize the variable quantity α of the output calculating course angle of SINS self;

(4) keep SINS in second place transfixion, gather the data of gyro output in 2-5 minute and accelerometer output, utilize accelerometer output to calculate the pitching angle theta of second position ₂With roll angle γ ₂

(5) utilize the variable quantity of output data and the SINS attitude of SINS on two positions, calculate the course angle of the second place _ ₂,, calculate gyroscope constant value drift D simultaneously as the angle, initial heading of system _x, D _yAnd D _z

2, any two-position initial alignment method of a kind of strapdown inertial navigation system according to claim 1, it is characterized in that: course angle variable quantity α is angular velocity or an angle increment information of utilizing SINS output in the described step (3), adopts the hypercomplex number method to calculate.

3, any two-position initial alignment method of a kind of strapdown inertial navigation system according to claim 1, it is characterized in that: the computing formula of second place course angle _ 2 in the described step (4) is as follows: course angle _ ₂

_ ₂＝_ ₁+α (1)

Wherein, _ ₁Course angle during for first position, α is a position turns over from first position to the second angle, is defined as counterclockwise to just;

First position course angle _ ₁Computing formula be:

Wherein,

Wherein, ω _IeBe rotational-angular velocity of the earth, lat is a latitude,

m _x＝cosγ ₂cosα-sinθ ₂sinγ ₂sinα-cosγ ₁

n _x＝cosγ ₂sinα+sinθ ₂sinγ ₂cosα-sinθ ₁sinγ ₁

m _y＝cosθ ₂cosα-cosθ ₁

n _y＝cosθ ₂sinα

m _z＝sinγ ₂cosα+sinθ ₂cosγ ₂sinα-sinγ ₁

n _z＝sinγ ₂sinα-sinθ ₂cosγ ₂cosα-sinθ ₁cosγ ₁

ω _x′＝ω _xo ²-ω _xo ¹

ω _y′＝ω _yo ²-ω _yo ¹

ω _z′＝ω _zo ²-ω _zo ¹

Δ _x＝ω _iesinlat·(cosθ ₁sinγ ₁-cosθ ₂sinγ ₂)

Δ _y＝ω _lesinlat·sinθ ₂-sinθ ₁

Δ _z＝ω _iesinlat·(cosθ ₂cosγ ₂-cosθ ₁cosγ ₁)

θ ₁, γ ₁And θ ₂, γ ₂Be respectively the angle of pitch, the roll angle of the primary importance and the second place,

${\mathrm{\θ}}_i=\arcsin \left(\frac{f_{\mathrm{yo}}^i}{g}\right)(i=x,y)---\left(7\right)$

${\mathrm{\γ}}_i=\arcsin \left(\frac{{-f}_{\mathrm{xo}}^i}{\cos {\mathrm{\θ}}_i\·g}\right)(i=x,y)---\left(8\right)$

Wherein, f _Xo ⁱBe the X-axis specific force of SINS output on i the position, f _Yo ⁱBe the Y-axis specific force of SINS output on i the position, g is an acceleration of gravity.

Calculate _ ₁The time, main value be need carry out and determination methods such as following table judged: cos_ ₁Symbol _ ₁Symbol _ ₁True value _ ₁The place quadrant + + _ ₁ First quartile - + π-_ ₁ Second quadrant - - π-_ ₁ Third quadrant + - _ ₁ Four-quadrant

4, any two-position initial alignment method of a kind of strapdown inertial navigation system according to claim 1 is characterized in that: the gyroscope constant value drift D in the described step (4) _x, D _yAnd D _zComputing formula as follows:

D _x＝ω _xo-(ω _iecoslatsin_ _icosγ _i+ω _iecoslatsinθ _icos_ _isinγ _i-ω _iesinlatcosθ _isinγ _i)(9)

D _y＝ω _yo-(ω _iecoslatcos_ _icosθ _i+ω _iesinlatsinθ _i)(10)

D _z＝ω _zo-(ω _iecoslatsin_ _isinγ _i-ω _iecoslatsinθ _icos_ _icosγ _i+ω _iesinlatcosθ _icosγ _i)(11)

Wherein, i=x, y.