CN101893445A - Fast Initial Alignment Method for Low Precision Strapdown Inertial Navigation System in Swing State - Google Patents

Abstract

The purpose of the present invention is to provide a fast initial alignment method for a low-precision strapdown inertial navigation system in a rocking state. The steps are as follows: determine the initial position parameters of the carrier; collect the data output by the accelerometer and gyroscope, use the second-order leveling and orientation estimation method to complete the rough alignment of the strapdown inertial navigation system, and initially determine the attitude of the carrier; estimate the length of the lever arm , calculate the disturbance acceleration caused by the lever arm effect, and compensate the output of the accelerometer; establish the Kalman filter state equation and the Kalman filter measurement equation; estimate the misalignment angle of the system, and use it at the end of fine alignment Correct the strapdown attitude matrix of the system to complete the initial alignment. The invention eliminates the influence of the lever-arm effect error on the initial alignment of the strapdown inertial navigation system; shortens the alignment time of the system and improves the alignment accuracy of the system, thereby comprehensively improving the initial alignment of the low-precision strapdown inertial navigation system performance.

Description

Low-accuracy strapdown inertial navigation system fast initial alignment method under the swinging condition

Technical field

What the present invention relates to is the alignment methods that is used for marine navigation.

Background technology

Along with the continuous development of inertial technology, use the inertia device formation strapdown attitude system of low precision and research focus and the developing direction that integrated navigation system has become navigation field.The purpose of strapdown inertial navitation system (SINS) initial alignment is to determine the attitude of carrier at initial time, and initial alignment is a gordian technique of strapdown inertial navitation system (SINS), and it directly affects the precision and the reaction time of inertial navigation system.For the strapdown inertial navitation system (SINS) of low precision, the precision of inertial measurement cluster is not high, is difficult to finish effectively autoregistration, particularly alignment of orientation.

The naval vessel can be in swinging condition because of the interference of fitful wind wave in mooring, except including the rotational-angular velocity of the earth component, also comprising the information of waving on naval vessel in the gyrostatic output; Because the oscillating motion center that carrier is departed from the installation site of inertial measurement cluster not only includes in the output of accelerometer outside the gravitational acceleration component, is also comprising the disturbing acceleration that is caused by lever arm effect.Therefore, the initial alignment problem of researching and solving low-accuracy strapdown inertial navigation system under the swinging condition has great importance.

Summary of the invention

The object of the present invention is to provide low-accuracy strapdown inertial navigation system fast initial alignment method under the swinging condition that can effectively improve the alignment precision of low-accuracy strapdown inertial navigation system initial alignment under the swinging condition and shorten the aligning time.

The object of the present invention is achieved like this:

Low-accuracy strapdown inertial navigation system fast initial alignment method under the swinging condition of the present invention is characterized in that:

(1) determines the initial position parameters of carrier by GPS, comprise longitude and latitude;

(2) gather the data of accelerometer and gyroscope output and the data that collect are handled, adopt second order leveling and DOA estimation method to finish the coarse alignment of strapdown inertial navitation system (SINS), the preliminary attitude of determining carrier, be pitch angle θ, roll angle γ and course angle ψ, set up the initial strapdown matrix of strapdown inertial navitation system (SINS)

$C_b^{n^{\′}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

(3) at first, according to the angular speed of gyroscope output with and the attitude angle information estimation lever arm length of carrier: at two different moment t ₁And t ₂Accelerometer is respectively measured one group of specific force value

With Utilize the different strapdown matrixes that obtain that constantly resolve

With

It is transformed into the calculating geographic coordinate system, obtains

$C_{b1}^{n^{\′}}{f}_1^b=-g_1^{n^{\′}}+C_{b1}^{n^{\′}}\mathrm{\δ}{f}_1^b$

$C_{b2}^{n^{\′}}{f}_2^b=-g_2^{n^{\′}}+C_{b2}^{n^{\′}}\mathrm{\δ}{f}_2^b$

Wherein

Be disturbing acceleration,

Be the distance between the oscillating motion center of the installation site of inertial measurement cluster and carrier, Be the angular velocity of gyroscope output,

Be corresponding derivative,

Be respectively t ₁, t ₂Acceleration of gravity constantly, and

Order $M=\left\{\right[{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]+[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right\},$ Promptly

$\mathrm{\δ}{f}^b={\mathrm{Mr}}_P^b,$

Obtain

$C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b=(C_{b1}^n{M}_1-C_{b2}^n{M}_2)r_P^b,$

Obtaining lever arm length is

$r_P^b={(C_{b1}^n{M}_1-C_{b2}^n{M}_2)}^{-1}(C_{b1}^n{f}_{b1}^b-C_{b2}^n{f}_2^b);$

Secondly, calculate the disturbing acceleration that lever arm effect causes:

$\mathrm{\δ}{f}^b={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×r_P^b+{\mathrm{\ω}}_{\mathrm{ib}}^b\×({\mathrm{\ω}}_{\mathrm{ib}}^b\×r_P^b)=\left[\begin{array}{ccc}-({\mathrm{\ω}}_{{\mathrm{ib}}_y}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_y}^2)\end{array}\right]\left[\begin{array}{c}{r}_{P_x}^b\\ r_{P_y}^b\\ r_{P_z}^b\end{array}\right];$

Then, the output to accelerometer compensates:

$f^b=f_0^b-\mathrm{\δ}{f}^b$

Wherein

Be the specific force of compensation preacceleration meter output,

Specific force for the output of compensation post-acceleration meter;

(4) at first, setting up with velocity error and attitude error is the Kalman filtering state equation of state variable:

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW},$

Wherein X is the state variable of system, and A and B are respectively the state-transition matrix and the noise matrix of system, and W is a system noise, and the state variable of system is

$X={\left[\begin{array}{cccccccccc}\mathrm{\δ}{V}_e& \mathrm{\δ}{V}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$

System noise is

$W={\left[\begin{array}{cccccccccc}{W}_{{\&dtri;}_x}& W_{{\&dtri;}_y}& W_{{\mathrm{\&epsiv;}}_x}& W_{{\mathrm{\&epsiv;}}_y}& W_{{\mathrm{\&epsiv;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HSA00000180273200031.png,HSA00000180273200032.png"\; state="\{\{state\}\}">z</figure-callout>}}}& 0& 0& 0& 0& 0\end{array}\right]}^T,$

δ V wherein _e, δ V _nRepresent east orientation, north orientation velocity error respectively, φ _e, φ _nAnd φ _uRepresent east orientation, north orientation and orientation misalignment respectively, Represent X-axis, Y-axis accelerometer bias respectively, ε _x, ε _yAnd ε _zRepresent X-axis, Y-axis and Z axle gyroscope constant value drift respectively,

Be respectively X-axis, Y-axis accelerometer bias random white noise,

With Represent X-axis, Y-axis and Z axle gyroscope constant value drift random white noise respectively, make the strapdown matrix For

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$

The state-transition matrix of system is

$A=\left[\begin{array}{cccc}{A}_{11}& A_{12}& C_{2\&times;2}& 0_{3\&times;3}\\ A_{21}& A_{22}& 0_{3\&times;2}& C_b^{n^{\&prime;}}\\ 0_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;2}& 0_{2\&times;3}\\ 0_{3\&times;2}& 0_{3\&times;3}& 0_{3\&times;2}& 0_{3\&times;3}\end{array}\right],$

Wherein

$A_{12}=\left[\begin{array}{ccc}0& -(C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b)& C_{21}{f}_x^b+C_{22}{f}_y^b+C_{23}{f}_z^b\\ C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b& 0& -(C_{11}{f}_x^b+C_{12}{f}_y^b+C_{13}{f}_z^b)\end{array}\right],$

ω in the formula _IeBe rotational-angular velocity of the earth, R _eBe earth radius,

Be the specific force of compensation post-acceleration meter output,

Be the angular velocity of gyroscope output, the system noise matrix is

$B=\left[\begin{array}{ccc}{C}_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;5}\\ 0_{3\&times;2}& C_b^{n^{\&prime;}}& 0_{3\&times;5}\\ 0_{5\&times;2}& 0_{5\&times;3}& 0_{5\&times;5}\end{array}\right],$

Wherein

$C_{2\&times;2}=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\end{array}\right],$

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right];$

Secondly, setting up with velocity error and course error is the Kalman filtering measurement equation of measurement amount:

Z＝HX+V，

Wherein Z is the measurement amount of system, and H is the measurement matrix of system, and V is the measurement noise battle array of system, and the amount of system is measured as

Z＝[δV _e?δV _n?δψ] ^T，

Wherein δ ψ=ψ '-ψ is the course error angle, the course angle that ψ ' resolves for strapdown inertial navitation system (SINS), and ψ is for to carry out revised course angle to the output of magnetic course transmitter, and the measurement matrix of system is

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& k_1& k_2& 1& 0& 0& 0& 0& 0\end{array}\right],$

K wherein ₁=-tan θ sin ψ, k ₂=-tan θ cos ψ, θ, ψ are respectively the pitch angle and the course angle of carrier;

(5) utilize kalman filter method to estimate the misalignment of system, and come the strapdown attitude matrix of update the system with it the finish time at fine alignment:

The transition matrix that calculates between geographic coordinate system n ' and the local geographic coordinate system n is

$C_{n^{\&prime;}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 1& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& -{\mathrm{\&phi;}}_e& 1\end{array}\right],$

Transition matrix between carrier coordinate system b and the local geographic coordinate system n is

$C_b^n=C_{n^{\&prime;}}^n{C}_n^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}& C_{13}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right],$

According to

Determine attitude of carrier, promptly the main value of pitch angle θ, the roll angle γ of carrier and course angle ψ is respectively

θ _Main=arcsinC ' ₃₂,

The field of definition of roll angle is (0 a °, 360 °), and the field of definition of pitch angle is (90 °, 90 °), and the field of definition of course angle is (180 °, 180 °), and the true value that obtains attitude of carrier is

θ=θ _Main,

Initial alignment is finished.

Advantage of the present invention is: compensated the disturbing acceleration that lever arm effect causes, eliminated the influence of lever arm effect error to the strapdown inertial navitation system (SINS) initial alignment; The increase course error is observed quantity, having overcome traditional is the problem of long, estimation effect difference orientation misalignment estimated time of static-base alignment method of observed quantity with the velocity error, shortened the aligning time of system, improve the alignment precision of system, thereby improved the performance of low-accuracy strapdown inertial navigation system initial alignment comprehensively.

Description of drawings

Fig. 1 is a process flow diagram of the present invention;

Fig. 2 is a lever arm effect error schematic diagram of the present invention;

Fig. 3 is the evaluated error curve of east orientation misalignment in the specific embodiment of the invention;

Fig. 4 is the evaluated error curve of north orientation misalignment in the specific embodiment of the invention;

Fig. 5 is the evaluated error curve of orientation misalignment in the specific embodiment of the invention.

Embodiment

For example the present invention is done description in more detail below in conjunction with accompanying drawing:

In conjunction with Fig. 1～2, low-accuracy strapdown inertial navigation system fast initial alignment method is divided following steps under the swinging condition of the present invention:

(1) determines the initial position parameters of carrier by GPS, comprise longitude and latitude;

(2) gather the data of accelerometer and gyroscope output and the data that collect are handled, adopt second order leveling and DOA estimation method to finish the coarse alignment of strapdown inertial navitation system (SINS), the preliminary attitude of determining carrier, be pitch angle θ, roll angle γ and course angle ψ, set up the initial strapdown matrix of strapdown inertial navitation system (SINS)

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right];$

(3) at first, according to the angular speed of gyroscope output with and the attitude angle information estimation lever arm length of carrier: at two different moment t ₁And t ₂Accelerometer is respectively measured one group of specific force value

With

Utilize the different strapdown matrixes that obtain that constantly resolve

With

It is transformed into the calculating geographic coordinate system, obtains

$C_{b1}^{n^{\&prime;}}{f}_1^b=-g_1^{n^{\&prime;}}+C_{b1}^{n^{\&prime;}}\mathrm{\&delta;}{f}_1^b$

$C_{b2}^{n^{\&prime;}}{f}_2^b=-g_2^{n^{\&prime;}}+C_{b2}^{n^{\&prime;}}\mathrm{\&delta;}{f}_2^b$

Wherein Be disturbing acceleration, Be the distance between the oscillating motion center of the installation site of inertial measurement cluster and carrier,

Be the angular velocity of gyroscope output,

Be corresponding derivative,

Be respectively t ₁, t ₂Acceleration of gravity constantly, and

Order $M=\left\{\right[{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{\mathrm{ib}}^b\&times;]+[{\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\left]\right[{\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\left]\right\},$ Promptly

$\mathrm{\&delta;}{f}^b={\mathrm{Mr}}_P^b,$

Obtain

$C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b=(C_{b1}^n{M}_1-C_{b2}^n{M}_2)r_P^b,$

Obtaining lever arm length is

$r_P^b={(C_{b1}^n{M}_1-C_{b2}^n{M}_2)}^{-1}(C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b);$

Secondly, calculate the disturbing acceleration that lever arm effect causes:

$\mathrm{\&delta;}{f}^b={\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{\mathrm{ib}}^b\&times;r_P^b+{\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;({\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r_P^b)=\left[\begin{array}{ccc}-({\mathrm{\&omega;}}_{{\mathrm{ib}}_y}^2+{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_y}-{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_z}& {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_y}\\ {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_y}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_z}& -({\mathrm{\&omega;}}_{{\mathrm{ib}}_x}^2+{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\&omega;}}_{{\mathrm{ib}}_y}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}-{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_x}\\ {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}-{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_y}& {\mathrm{\&omega;}}_{{\mathrm{ib}}_y}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_x}& -({\mathrm{\&omega;}}_{{\mathrm{ib}}_x}^2+{\mathrm{\&omega;}}_{{\mathrm{ib}}_y}^2)\end{array}\right]\left[\begin{array}{c}{r}_{P_x}^b\\ r_{P_y}^b\\ r_{P_z}^b\end{array}\right];$

Then, the output to accelerometer compensates:

$f^b=f_0^b-\mathrm{\&delta;}{f}^b$

Wherein

Be the specific force of compensation preacceleration meter output,

Specific force for the output of compensation post-acceleration meter;

(4) at first, setting up with velocity error and attitude error is the Kalman filtering state equation of state variable:

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}+\mathrm{BW},$

Wherein X is the state variable of system, and A and B are respectively the state-transition matrix and the noise matrix of system, and W is a system noise, and the state variable of system is

$X={\left[\begin{array}{cccccccccc}\mathrm{\&delta;}{V}_e& \mathrm{\&delta;}{V}_n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T,$

System noise is

$W={\left[\begin{array}{cccccccccc}{W}_{{\&dtri;}_x}& W_{{\&dtri;}_y}& W_{{\mathrm{\&epsiv;}}_x}& W_{{\mathrm{\&epsiv;}}_y}& W_{{\mathrm{\&epsiv;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HSA00000180273200031.png,HSA00000180273200032.png"\; state="\{\{state\}\}">z</figure-callout>}}}& 0& 0& 0& 0& 0\end{array}\right]}^T,$

δ V wherein _e, δ V _nRepresent east orientation, north orientation velocity error respectively, φ _e, φ _nAnd φ _uRepresent east orientation, north orientation and orientation misalignment respectively, Represent X-axis, Y-axis accelerometer bias respectively, ε _x, ε _yAnd ε _zRepresent X-axis, Y-axis and Z axle gyroscope constant value drift respectively, Be respectively X-axis, Y-axis accelerometer bias random white noise,

With

Represent X-axis, Y-axis and Z axle gyroscope constant value drift random white noise respectively, make the strapdown matrix

For

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$

The state-transition matrix of system is

$A=\left[\begin{array}{cccc}{A}_{11}& A_{12}& C_{2\&times;2}& 0_{3\&times;3}\\ A_{21}& A_{22}& 0_{3\&times;2}& C_b^{n^{\&prime;}}\\ 0_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;2}& 0_{2\&times;3}\\ 0_{3\&times;2}& 0_{3\&times;3}& 0_{3\&times;2}& 0_{3\&times;3}\end{array}\right],$

Wherein

$A_{12}=\left[\begin{array}{ccc}0& -(C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b)& C_{21}{f}_x^b+C_{22}{f}_y^b+C_{23}{f}_z^b\\ C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b& 0& -(C_{11}{f}_x^b+C_{12}{f}_y^b+C_{13}{f}_z^b)\end{array}\right],$

ω in the formula _IeBe rotational-angular velocity of the earth, R _eBe earth radius,

Be the specific force of compensation post-acceleration meter output,

Be the angular velocity of gyroscope output, the system noise matrix is

$B=\left[\begin{array}{ccc}{C}_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;5}\\ 0_{3\&times;2}& C_b^{n^{\&prime;}}& 0_{3\&times;5}\\ 0_{5\&times;2}& 0_{5\&times;3}& 0_{5\&times;5}\end{array}\right],$

Wherein

$C_{2\&times;2}=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\end{array}\right],$

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right];$

Secondly, setting up with velocity error and course error is the Kalman filtering measurement equation of measurement amount:

Z＝HX+V，

Wherein Z is the measurement amount of system, and H is the measurement matrix of system, and V is the measurement noise battle array of system, and the amount of system is measured as

Z＝[δV _e?δV _n?δψ] ^T，

Wherein δ ψ=ψ '-ψ is the course error angle, the course angle that ψ ' resolves for strapdown inertial navitation system (SINS), and ψ is for to carry out revised course angle to the output of magnetic course transmitter, and the measurement matrix of system is

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& k_1& k_2& 1& 0& 0& 0& 0& 0\end{array}\right],$

K wherein ₁=-tan θ sin ψ, k ₂=-tan θ cos ψ, θ, ψ are respectively the pitch angle and the course angle of carrier;

(5) utilize kalman filter method to estimate the misalignment of system, and come the strapdown attitude matrix of update the system with it the finish time at fine alignment:

The transition matrix that calculates between geographic coordinate system n ' and the local geographic coordinate system n is

$C_{n^{\&prime;}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 1& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& -{\mathrm{\&phi;}}_e& 1\end{array}\right],$

Transition matrix between carrier coordinate system b and the local geographic coordinate system n is

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}& C_{13}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right],$

According to

Determine attitude of carrier, promptly the main value of pitch angle θ, the roll angle γ of carrier and course angle ψ is respectively

θ _Main=arcsinC ' ₃₂,

The field of definition of roll angle is (0 a °, 360 °), and the field of definition of pitch angle is (90 °, 90 °), and the field of definition of course angle is (180 °, 180 °), and the true value that obtains attitude of carrier is

θ=θ _Main,

Initial alignment is finished.

Utilize under the swinging condition of the present invention low-accuracy strapdown inertial navigation system fast initial alignment method to carry out emulation experiment:

The three-axis swinging motion of carrier is that its mathematical model is respectively around the motion with sinusoidal rule of pitch axis, axis of roll and course axle:

$\left\{\begin{array}{c}\mathrm{\&theta;}={\mathrm{\&theta;}}_m\sin (2\mathrm{\&pi;}/T_{\mathrm{\&theta;}}+{\mathrm{\&phi;}}_{\mathrm{\&theta;}})\\ \mathrm{\&gamma;}={\mathrm{\&gamma;}}_m\sin (2\mathrm{\&pi;}/T_{\mathrm{\&gamma;}}+{\mathrm{\&phi;}}_{\mathrm{\&gamma;}})\\ \mathrm{\&psi;}={\mathrm{\&psi;}}_m\sin (2\mathrm{\&pi;}/T_{\mathrm{\&psi;}}+{\mathrm{\&phi;}}_{\mathrm{\&psi;}})+{\mathrm{\&psi;}}_0\end{array}\right.$

Wherein: θ _m, γ _m, ψ _mBe respectively the amplitude of waving of carrier pitching, rolling, yawing; T _θ, T _γ, T _ψBe respectively the rolling period of pitching, rolling, yawing; φ _θ, φ _γ, φ _ψBe initial phase; ψ ₀Be the angle, initial heading.During emulation, θ _m=6 °, γ _m=6 °, ψ _m=5 °; T _θ=7s, T _γ=9s, T _ψ=8s; The initial heading is ψ ₀Get 30 °; Initial phase φ _θ, φ _r, φ _yBe 0.

Simulation time: 300s;

The initial position of carrier: 45.7796 ° of north latitude, 126.6705 ° of east longitudes;

Equatorial radius: R _e=6378393.0m;

Rotational-angular velocity of the earth: ω _Ie=7.2921158e-5rad/s;

Earth surface acceleration of gravity: g ₀=9.78049m/s ²

Lever arm length: $r_P^b={\left[\begin{array}{ccc}0.3m& 0.4m& 0.2m\end{array}\right]}^T;$

Gyroscope constant value drift: 0.1 °/h;

Gyroscope Random Drift: 0.01 °/h;

Accelerometer bias: 10 ^-4G;

Accelerometer random deviation: 10 ^-5G;

The precision of magnetic course transmitter: 0.1 °;

Attitude error angle after coarse alignment finishes: 0.5 °, 0.5 °, 1 °;

The graph of errors that utilizes east orientation misalignment, north orientation misalignment and orientation misalignment that the method for the invention obtains is respectively as Fig. 3, Fig. 4 and shown in Figure 5.The result shows, under swinging condition, adopts method of the present invention can shorten estimated time of orientation misalignment effectively, improves the alignment precision of low-cost strapdown inertial navitation system (SINS) initial alignment.

Claims (1)

1. low-accuracy strapdown inertial navigation system fast initial alignment method under the swinging condition is characterized in that:

(1) determines the initial position parameters of carrier by GPS, comprise longitude and latitude;

(2) gather the data of accelerometer and gyroscope output and the data that collect are handled, adopt second order leveling and DOA estimation method to finish the coarse alignment of strapdown inertial navitation system (SINS), the preliminary attitude of determining carrier, be pitch angle θ, roll angle γ and course angle ψ, set up the initial strapdown matrix of strapdown inertial navitation system (SINS)

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right];$

(3) at first, according to the angular speed of gyroscope output with and the attitude angle information estimation lever arm length of carrier: at two different moment t ₁And t ₂Accelerometer is respectively measured one group of specific force value

With Utilize the different strapdown matrixes that obtain that constantly resolve

With

It is transformed into the calculating geographic coordinate system, obtains

$C_{b1}^{n^{\&prime;}}{f}_1^b=-g_1^{n^{\&prime;}}+C_{b1}^{n^{\&prime;}}\mathrm{\&delta;}{f}_1^b$

$C_{b2}^{n^{\&prime;}}{f}_2^b=-g_2^{n^{\&prime;}}+C_{b2}^{n^{\&prime;}}\mathrm{\&delta;}{f}_2^b$

Wherein

Be disturbing acceleration,

Be the distance between the oscillating motion center of the installation site of inertial measurement cluster and carrier,

Be the angular velocity of gyroscope output,

Be corresponding derivative,

Be respectively t ₁, t ₂Acceleration of gravity constantly, and

Order $M=\left\{\right[{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{\mathrm{ib}}^b\&times;]+[{\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\left]\right[{\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\left]\right\},$ Promptly

$\mathrm{\&delta;}{f}^b={\mathrm{Mr}}_P^b,$

Obtain

$C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b=(C_{b1}^n{M}_1-C_{b2}^n{M}_2)r_P^b,$

Obtaining lever arm length is

$r_P^b={(C_{b1}^n{M}_1-C_{b2}^n{M}_2)}^{-1}(C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b);$

Secondly, calculate the disturbing acceleration that lever arm effect causes:

$\mathrm{\&delta;}{f}^b={\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{\mathrm{ib}}^b\&times;r_P^b+{\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;({\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r_P^b)=\left[\begin{array}{ccc}-({\mathrm{\&omega;}}_{{\mathrm{ib}}_y}^2+{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_y}-{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_z}& {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_y}\\ {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_y}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_z}& -({\mathrm{\&omega;}}_{{\mathrm{ib}}_x}^2+{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\&omega;}}_{{\mathrm{ib}}_y}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}-{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_x}\\ {\mathrm{\&omega;}}_{{\mathrm{ib}}_x}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}-{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_y}& {\mathrm{\&omega;}}_{{\mathrm{ib}}_y}{\mathrm{\&omega;}}_{{\mathrm{ib}}_z}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_x}& -({\mathrm{\&omega;}}_{{\mathrm{ib}}_x}^2+{\mathrm{\&omega;}}_{{\mathrm{ib}}_y}^2)\end{array}\right]\left[\begin{array}{c}{r}_{P_x}^b\\ r_{P_y}^b\\ r_{P_z}^b\end{array}\right];$

Then, the output to accelerometer compensates:

$f^b=f_0^b-\mathrm{\&delta;}{f}^b$

Wherein

Be the specific force of compensation preacceleration meter output,

Specific force for the output of compensation post-acceleration meter;

(4) at first, setting up with velocity error and attitude error is the Kalman filtering state equation of state variable:

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}+\mathrm{BW},$

Wherein X is the state variable of system, and A and B are respectively the state-transition matrix and the noise matrix of system, and W is a system noise, and the state variable of system is

$X={\left[\begin{array}{cccccccccc}\mathrm{\&delta;}{V}_e& \mathrm{\&delta;}{V}_n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T,$

System noise is

$W={\left[\begin{array}{cccccccccc}{W}_{{\&dtri;}_x}& W_{{\&dtri;}_y}& W_{{\mathrm{\&epsiv;}}_x}& W_{{\mathrm{\&epsiv;}}_y}& W_{{\mathrm{\&epsiv;}}_z}& 0& 0& 0& 0& 0\end{array}\right]}^T,$

δ V wherein _e, δ V _nRepresent east orientation, north orientation velocity error respectively, φ _e, φ _nAnd φ _uRepresent east orientation, north orientation and orientation misalignment respectively,

Represent X-axis, Y-axis accelerometer bias respectively, ε _x, ε _yAnd ε _zRepresent X-axis, Y-axis and Z axle gyroscope constant value drift respectively,

Be respectively X-axis, Y-axis accelerometer bias random white noise,

With

Represent X-axis, Y-axis and Z axle gyroscope constant value drift random white noise respectively, make the strapdown matrix

For

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$

The state-transition matrix of system is

$A=\left[\begin{array}{cccc}{A}_{11}& A_{12}& C_{2\&times;2}& 0_{3\&times;3}\\ A_{21}& A_{22}& 0_{3\&times;2}& C_b^{n^{\&prime;}}\\ 0_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;2}& 0_{2\&times;3}\\ 0_{3\&times;2}& 0_{3\&times;3}& 0_{3\&times;2}& 0_{3\&times;3}\end{array}\right],$

Wherein

$A_{12}=\left[\begin{array}{ccc}0& -(C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b)& C_{21}{f}_x^b+C_{22}{f}_y^b+C_{23}{f}_z^b\\ C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b& 0& -(C_{11}{f}_x^b+C_{12}{f}_y^b+C_{13}{f}_z^b)\end{array}\right],$

ω in the formula _IeBe rotational-angular velocity of the earth, R _eBe earth radius,

Be the specific force of compensation post-acceleration meter output,

Be the angular velocity of gyroscope output, the system noise matrix is

$B=\left[\begin{array}{ccc}{C}_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;5}\\ 0_{3\&times;2}& C_b^{n^{\&prime;}}& 0_{3\&times;5}\\ 0_{5\&times;2}& 0_{5\&times;3}& 0_{5\&times;5}\end{array}\right],$

Wherein

$C_{2\&times;2}=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\end{array}\right],$

$C_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right];$

Secondly, setting up with velocity error and course error is the Kalman filtering measurement equation of measurement amount:

Z＝HX+V，

Wherein Z is the measurement amount of system, and H is the measurement matrix of system, and V is the measurement noise battle array of system, and the amount of system is measured as

Z＝[δV _e?δV _n?δψ] ^T，

Wherein δ ψ=ψ '-ψ is the course error angle, the course angle that ψ ' resolves for strapdown inertial navitation system (SINS), and ψ is for to carry out revised course angle to the output of magnetic course transmitter, and the measurement matrix of system is

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& k_1& k_2& 1& 0& 0& 0& 0& 0\end{array}\right],$

K wherein ₁=-tan θ sin ψ, k ₂=-tan θ cos ψ, θ, ψ are respectively the pitch angle and the course angle of carrier;

(5) utilize kalman filter method to estimate the misalignment of system, and come the strapdown attitude matrix of update the system with it the finish time at fine alignment:

The transition matrix that calculates between geographic coordinate system n ' and the local geographic coordinate system n is

$C_{n^{\&prime;}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 1& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& -{\mathrm{\&phi;}}_e& 1\end{array}\right],$

Transition matrix between carrier coordinate system b and the local geographic coordinate system n is

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}=\left[\begin{array}{ccc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}& C_{13}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right],$

According to

Determine attitude of carrier, promptly the main value of pitch angle θ, the roll angle γ of carrier and course angle ψ is respectively

θ _Main=arcsinC ' ₃₂,

The field of definition of roll angle is (0 a °, 360 °), and the field of definition of pitch angle is (90 °, 90 °), and the field of definition of course angle is (180 °, 180 °), and the true value that obtains attitude of carrier is

θ=θ _Main,

Initial alignment is finished.

Images