CN106017452A - Dual gyro anti-disturbance north-seeking method - Google Patents

Abstract

The invention discloses a dual gyro anti-disturbance north-seeking method. The method comprises the following steps: 1) initializing a north-seeking system; 2) performing data acquisition for a gyro/accelerometer; 3) initially loading a pitch angle and a roll angle, constructing upward virtual information, realizing the construction of the all-attitude inertia device information; and 4) using an inertia aligning technology, calculating and outputting the information of an azimuth angle, and ending north seeking. During a north-seeking process, the normal operation of the north-seeking system is kept under extraneous disturbance condition, north seeking result is effective, north seeking under disturbance is really realized; mechanical structure parts of an indexing mechanism are omitted, the complex degree of a structure in the north-seeking system is reduced, the weight of the north-seeking system is mitigated; and the attitude and orientation information of carrier can be given with real-time.

Description

Double tops disturbance rejection north finding method

Technical field

The invention belongs to inertia and seek north technical field, be specifically related to the north finding method of a kind of double tops disturbance rejection.

Background technology

Gyroscope north searching system is one of important achievement of inertial technology application, and it determines north by revolutions sensitively To, round-the-clock can reliably work, do not affected by external environment condition such as magnetic field of the earth, landform, weather etc., can be the most any Realize automated north-seeking function in the case of outside azimuth information, measure local geographical north orientation.Current north-seeking system, most Use single gyro, rely on indexing mechanism to realize settling accounts corresponding north orientation after position rotates.Two positions can be divided into by turned position Method, four position methods, Multiple station method, and rotate debugging continuously.No matter using anything to seek north formula, north-seeking system is in the mistake used Journey is always affected by various disturbances, is mainly had fitful wind, the vibration of electromotor, personnel for vehicle-mounted north-seeking system disturbing source Walk about, the sedimentation etc. of ground, the disturbance of pedestal can seriously reduce the north finding precision of system.In order to reduce various disturbance to seeking north The impact of precision, the method used at present is all confined to inertia type instrument i.e. gyro, the filtering of accelerometer output valve and numeral Processing, these methods can effectively eliminate the high frequency components impact on north-seeking system.In actual applications, especially pedestal disturbance In when comprising bigger low-frequency noise, use and directly the method for the filtering of inertia type instrument output valve and digital processing often be can not get ratio Comparatively ideal effect.

Summary of the invention

For the deficiency existing for prior art, solved by the invention problematically, how to provide a kind of and disturb in existence Remain in the case of Dong normally work, export effective result, it is ensured that the north finding method of north finding precision.

For achieving the above object, the present invention uses following scheme:

Double tops disturbance rejection north finding method, specifically comprises the following steps that

Step 1: north-seeking system is initialized；

Step 2: gather X-axis, Y-axis optical fibre gyro exports information with accelerometer, demarcates the parameter of inertia device；Concrete next Say, 1) at a latitude it is known that and under the turntable demarcated, by gathering the X-axis of equally distributed 12 horizontal levels, Y-axis Gyro output data, calibrate 8 parameters in double optical fibre gyro error model；2) under turntable, by the sensitive positive and negative earth Acceleration of gravity information, under the conditions of gathering 6 kinds, X-axis, Y-axis accelerometer output data, calibrate dual acceleration meter error model In 8 parameters；

Step 3a: the angle of pitch and two parameter initial bindings the most of roll angle are crossed and judged, if being judged as NO, then The initial calculation bookbinding angle of pitch and two parameters of roll angle；If being judged as YES, then use the angle of pitch after updating and roll JIAOSHEN Number；

Step 3b: 16 parameters and the angle of pitch described in step 3a, two parameters one of roll angle will demarcated in step 2 With building sky to virtual information, i.e. sky is to gyro virtual information and sky to accelerometer virtual information, thus realizes full attitude The structure of inertia device information；

Step 4: utilize inertial system technique of alignment, computer azimuth angle, the angle of pitch, the information of roll angle；With the horizontal stroke calculated Roll angle described in roll angle and angle of pitch information updating step 3a and angle of pitch parameter；The azimuth information that output calculates, Seek north process to terminate.

Further, described step 4, before seeking north process and terminating, arranges Rule of judgment and carrys out repeated execution of steps 2～4 to obtain Satisfactory must seek north result；If being judged as YES, then seek north process and terminate, if being judged as NO, then return and perform step 2.

It addition, the collection position to gyro output data can be not limited to 12 in step 2, it is also possible to be 4,8 etc.. Owing to using linear regression to carry out fitting parameter, in the range of 0-360 °, data distribution is the most uniform, then the parameter of matching is the most accurate Really.Gathering position if increasing, one is time-consuming, and the collection position that two is too much has little significance for matching；If collection position Very few, then the parameters precision simulated is the highest.Considering factors above, the purpose of the present invention to be realized, gathering position should not Less than 4.

Further, in described step 2, the concrete scaling method of inertia device is as follows:

Corresponding attitude matrix is:

1) optical fibre gyro parameter calibration:

Double optical fibre gyro error models:

X-axis gyro:

Y-axis gyro:

Above in two formulas, W_bx, W_byIt is X-axis and Y-axis gyro output information, W respectively_0x, W_0yBe respectively X-axis and Y-axis gyro defeated Go out zero-bit,It is the gyro measured value to carrier angular velocity, K_xx, K_yyIt is X-axis and Y-axis gyro scale system respectively Number, k_xy, k_zx, k_xy, k_zyIt is gyro cross-coupling coefficient respectively；

At latitude it is known that and under the conditions of horizontal stationary, have following three formulas:

In above formula, ω_ieIt is rotational-angular velocity of the earth, for constant；(4) formula is brought into (2), (3) formula, has

W_bx=W_0x+K_xx(-ω_iesinψcosL)+k_yx(ω_iecosψcosL)+k_zx(ω_iesinL) (5)

W_by=W_0y+k_xy(-ω_iesinψcosL)+K_yy(ω_iecosψcosL)+k_zy(ω_iesinL) (6)

During demarcating gyro, remain horizontal stationary, demarcate azimuthal point and choose successivelyPhase X-axis gyro and the Y-axis gyro answered are output as

Build following two matrixes,

8 parameters in X matrix, are double optical fibre gyro error model (2), the parameter of (3) formula, so at a latitude It is known that and demarcate turntable under, by gather 12 horizontal levels gyro export data, above 8 parameters can be calibrated；

2) accelerometer is demarcated

Dual acceleration meter error model is as follows:

Above in two formulas, V_ax, V_ayIt is X-axis and Y-axis accelerometer output information, K respectively_ax0, K_ax0It is X-axis and Y-axis respectively Accelerometer output zero-bit,It is the accelerometer measured value to carrier specific force, K_axx, K_ayyBe respectively X-axis and Y-axis accelerometer calibration factor, k_axy, k_azx, k_axy, k_azyIt is accelerometer cross-coupling coefficient respectively；

Time accelerometer is demarcated, under quiescent conditions, choose X-axis successively, Y-axis, the positive negative G of Z axis accelerometer sensitive add Speed, corresponding X-axis accelerometer and Y-axis accelerometer are output as

Build following two matrixes,

8 parameters in X matrix, are the parameter of dual acceleration meter error model (8) formula, so under turntable, passing through Sensitive positive and negative terrestrial gravitation acceleration information, under the conditions of gathering 6 kinds, X-axis Y-axis accelerometer output data, above 8 can be calibrated Individual parameter.

Further, in described step 3b, it is as follows to the concrete construction method of virtual information:

1) sky builds to gyro virtual information

Have according to Eulerian equation:

Considering angular velocity dynamic constrained condition, in carrier orientation, angular velocity is zero, i.e.

Bring above formula into have, obtain attitude angular velocity constraint equation:

Arrangement obtains:

In view of there being following formula to set up

Knowing according to the error model in (2), (3), X-axis gyro and Y-axis gyro are output as

(14) are brought into (15), obtain:

Wherein

Owing to when seeking north, system is static in navigational coordinate system, navigational coordinate system relatively spherical coordinate system does not turn Dynamic, soIt is zero, the most northeastward under sky coordinate system

So that

2) sky builds to accelerometer virtual information

Have according to specific force equation:

Considering the dynamic constrained condition of carrier line motion, speed and acceleration in vertical direction are zero, and level side Upward velocity is 0, i.e.

Bring above formula into have

Arrange sky to acceleration information is:

Z axis accelerometer is output as:

Knowing according to the error model in (8) formula, X accelerometer and Y accelerometer are output as

(24) are brought into (25), obtain:

So that

The data that (19) and (27) construct according to above, just constitute complete Three Degree Of Freedom inertia device data.

Further, the method utilizing inertial system alignment computer azimuth angle in described step 4 is as follows:

The quaternary number form formula of attitude matrix is as follows

Wherein

Initial time,During seeking north, carrier is in geo-stationary, and does not has speed, Now For the gyro data in (19) formula, i.e.Right (29) formula uses the complete card solving method of quaternion differential equation can obtain with real-time updateWith

Owing to having

Bring (28) formula into (30) formula to obtain:

Arrangement obtains:

In formula, left kernel function and right kernel function are respectively as follows:

For solvingIt is converted into and seeks matrixMinimal eigenvalue characteristic of correspondence vector, will solve After arriving(28) formula of bringing into obtains newThe most available orientation, pitching, roll information；

Orientation:

Pitching: θ=asin (2 (q₂q₃+q₀q₁)) (34)

Roll:

Wherein

Further, in described step 3a, the angle of pitch is as follows with the method for the initial binding of roll angle:

Comparison (27) formula, does not consider that sky, to relevant parameter, has:

Horizontal accelerometer output information is now utilized to calculate the angle of pitch and roll angle:

In ripe disturbance rejection Initial Alignment Technique, and technique of alignment under inertial system, need three axles of full attitude Gyro output information, three axis accelerometer output information, after alignment, azimuth information is exactly the output knot that north-seeking system is final Really.At double tops disturbance rejection north finding method, utilize certain formula, construct sky (equal to angular rate information and acceleration information For virtual information), such virtual construct goes out full attitude Inertia information, the technique of alignment under recycling inertial system, it is possible to realize North technology is sought under disturbance rejection.

Compared to prior art, there is advantages that

1, it is an advantage of the current invention that to occur during Xun Bei fitful wind, engine luggine, personnel walk about, settlement of foundation etc. In the case of disturbance, when especially pedestal disturbance comprising bigger low-frequency noise, still ensure that north-seeking system is working properly, seek north Result is effective, and be truly realized under disturbance seeks north；

2, in tradition north finder system, indexing mechanism is relied on, it is achieved single gyro is in different time sections, the most sensitive mutual Rotational-angular velocity of the earth component information on orthogonal 4 positions carries out seeking north resolving；It is an advantage of the current invention that to eliminate indexing The mechanical structured members such as mechanism, reduce north-seeking system internal structure complexity, alleviate north-seeking system weight；

3, in tradition north finder system, azimuth information can only be provided after terminating finally seeking north；Advantages of the present invention It is possess attitude and the azimuth information that can provide carrier in real time.

Accompanying drawing explanation

Fig. 1 is the flow chart of double tops disturbance rejection north finding method embodiment.

Detailed description of the invention

During the demarcation and calculating of the present invention, all using sky, northeast coordinate system, 312 turns of sequences, i.e. around-Z axis rotation ψ (azimuth), then turns about the X axis θ (angle of pitch), finally rotates γ (roll angle), the latitude of inertia device calibration position around Y-axis Degree is L.Below in conjunction with the accompanying drawings the present invention is described in further detail, but embodiments of the present invention are not limited to this.

Embodiment:

Double tops disturbance rejection north finding method, as it is shown in figure 1, specifically comprise the following steps that

Step 1: north-seeking system is initialized.

Step 2: gather X-axis, Y-axis optical fibre gyro exports information with accelerometer, demarcates the parameter of inertia device；Concrete next Say, 1) at a latitude it is known that and under the turntable demarcated, by gathering the X-axis of equally distributed 12 horizontal levels, Y-axis Gyro output data, calibrate 8 parameters in double optical fibre gyro error model；2) under turntable, by the sensitive positive and negative earth Acceleration of gravity information, under the conditions of gathering 6 kinds, X-axis, Y-axis accelerometer output data, calibrate dual acceleration meter error mould 8 parameters in type.

Step 3a: the angle of pitch and two parameter initial bindings the most of roll angle are crossed and judged, if being judged as NO, then The initial calculation bookbinding angle of pitch and two parameters of roll angle；If being judged as YES, then use the angle of pitch after updating and roll JIAOSHEN Number.

Step 3b: 16 parameters and the angle of pitch described in step 3a, two parameters one of roll angle will demarcated in step 2 With building sky to virtual information, i.e. sky is to gyro virtual information and sky to accelerometer virtual information, thus realizes full attitude The structure of inertia device information.

Step 4: utilize inertia technique of alignment, computer azimuth angle, the angle of pitch, the information of roll angle；With the roll calculated Roll angle described in angle and angle of pitch information updating step 3a and angle of pitch parameter；The azimuth information that output calculates, seeks North process terminates；Terminate seek north before, can by arrange Rule of judgment come repeated execution of steps 2～4 with obtain meet the requirements Seek north result, such as will " seek north time < 3min " as Rule of judgment, if being judged as YES, Ze Xun Beiliu City journey terminates, if judgement It is no, then returns and perform step 2.

The following is data calculation formula in each step concrete to deduce and contrast with the test data of prior art:

(1) in described step 2, the concrete scaling method of inertia device is as follows:

Corresponding attitude matrix is:

1) optical fibre gyro parameter calibration:

Double optical fibre gyro error models:

X-axis gyro:

Y-axis gyro:

Above in two formulas, W_bx, W_byIt is X-axis and Y-axis gyro output information, W respectively_0x, W_0yBe respectively X-axis and Y-axis gyro defeated Go out zero-bit,It is the gyro measured value to carrier angular velocity, K_xx, K_yyIt is X-axis and Y-axis gyro scale system respectively Number, k_xy, k_zx, k_xy, k_zyIt is gyro cross-coupling coefficient respectively.

At latitude it is known that and under the conditions of horizontal stationary, have following three formulas:

In above formula, ω_ieIt is rotational-angular velocity of the earth, for constant.(4) formula is brought into (2), (3) formula, has

W_bx=W_0x+K_xx(-ω_iesinψcosL)+k_yx(ω_iecosψcosL)+k_zx(ω_iesinL) (5)

W_by=W_0y+k_xy(-ω_iesinψcosL)+K_yy(ω_iecosψcosL)+k_zy(ω_iesinL) (6)

During demarcating gyro, remain horizontal stationary, demarcate azimuthal point and choose successivelyPhase X-axis gyro and the Y-axis gyro answered are output as

Build following two matrixes,

8 parameters in X matrix, are double optical fibre gyro error model (2), the parameter of (3) formula, so at a latitude It is known that and demarcate turntable under, by gather 12 horizontal levels gyro export data, above 8 parameters can be calibrated.

2) accelerometer is demarcated

Dual acceleration meter error model is as follows:

Above in two formulas, V_ax, V_ayIt is X-axis and Y-axis accelerometer output information, K respectively_ax0, K_ax0It is X-axis and Y-axis respectively Accelerometer output zero-bit,It is the accelerometer measured value to carrier specific force, K_axx, K_ayyBe respectively X-axis and Y-axis accelerometer calibration factor, k_axy, k_azx, k_axy, k_azyIt is accelerometer cross-coupling coefficient respectively.

Time accelerometer is demarcated, under quiescent conditions, choose X-axis successively, Y-axis, the positive negative G of Z axis accelerometer sensitive add Speed, corresponding X-axis accelerometer and Y-axis accelerometer are output as

Build following two matrixes,

8 parameters in X matrix, are the parameter of dual acceleration meter error model (8) formula, so under turntable, passing through Sensitive positive and negative terrestrial gravitation acceleration information, under the conditions of gathering 6 kinds, X-axis Y-axis accelerometer output data, above 8 can be calibrated Individual parameter.

(2) in described step 3b, it is as follows to the concrete construction method of virtual information:

1) sky builds to gyro virtual information

Have according to Eulerian equation:

Considering angular velocity dynamic constrained condition, in carrier orientation, angular velocity is zero, i.e.

Bring above formula into have, obtain attitude angular velocity constraint equation:

Arrangement obtains:

In view of there being following formula to set up

Knowing according to the error model in (2), (3), X-axis gyro and Y-axis gyro are output as

(14) are brought into (15), obtain:

Wherein

Owing to when seeking north, system is static in navigational coordinate system, navigational coordinate system relatively spherical coordinate system does not turn Dynamic, soIt is zero, the most northeastward under sky coordinate system

So that

2) sky builds to accelerometer virtual information

Have according to specific force equation:

Considering the dynamic constrained condition of carrier line motion, speed and acceleration in vertical direction are zero, and level side Upward velocity is 0, i.e.

Bring above formula into have

Arrange sky to acceleration information is:

Z axis accelerometer is output as:

Knowing according to the error model in (8) formula, X accelerometer and Y accelerometer are output as

(24) are brought into (25), obtain:

So that

The data that (19) and (27) construct according to above, just constitute complete Three Degree Of Freedom inertia device data.

(3) method utilizing inertial system alignment computer azimuth angle in described step 4 is as follows:

The quaternary number form formula of attitude matrix is as follows

Wherein

Initial time,During seeking north, carrier is in geo-stationary, and does not has speed, Now For the gyro data in (19) formula, i.e.Right (29) formula uses the complete card solving method of quaternion differential equation can obtain with real-time updateWith

Owing to having

Bring (28) formula into (30) formula to obtain:

Arrangement obtains:

In formula, left kernel function and right kernel function are respectively as follows:

For solvingIt is converted into and seeks matrixMinimal eigenvalue characteristic of correspondence vector.Specifically solve Process is referred to Section 3.6 in publishing house of Tsing-Hua University " the C algorithms most in use procedure set second edition ": " Orthonormal Eigenvectors of Real Symmetric Matrices The Jacobi of eigen vector reaches a standard method ".After solving and obtain(28) formula of bringing into obtains newJust may be used Obtain orientation, pitching, roll information.

Orientation:

Pitching: θ=asin (2 (q₂q₃+q₀q₁)) (34)

Roll:

Wherein

(4) in described step 3a, the angle of pitch is as follows with the method for the initial binding of roll angle:

Comparison (27) formula, does not consider that sky, to relevant parameter, has:

Horizontal accelerometer output information is now utilized to calculate the angle of pitch and roll angle:

Before structure data, by (19) formula and (27) formula, it is to be appreciated that the 8 of gyro calibrating parameters, acceleration 8 calibrating parameters of degree meter, and the angle of pitch, roll angle, 18 parameters altogether.Wherein, front 16 parameters are at inertia device mark Fixed test can be calculated；Latter two parameter (angle of pitch, roll angle) needs north-seeking system initial calculation to bind, and follow-up bows The elevation angle, roll angle can utilize (34) formula and (35) formula result of calculation to participate in later stage computing.The angle of pitch and roll angle initial calculation Bookbinding utilizes above-mentioned formula to calculate.

(5) present invention and the contrast of tradition north finding method:

In traditional method north-seeking system, using flexible gyroscope 2 position to seek north technology, disturbance rejection north finding method uses double tops Double accelerometers seek north, and certain comparative test result is shown in Table 1.

Table 1 Data Comparison

Test data shows above, use the present invention double tops disturbance rejection north finding method not only in static test with biography System north-seeking system north finding precision is suitable；When existing in the case of external disturbance, tradition north-seeking system north finding precision is substantially reduced, defeated Go out result can not use at all, and use double tops disturbance rejection north finding method still to can ensure that north finding precision (1 σ < 0.1 °).

Finally illustrating, above example is only in order to illustrate technical scheme and unrestricted, although with reference to relatively The present invention has been described in detail by good embodiment, it will be understood by those within the art that, can be to the skill of the present invention Art scheme is modified or equivalent, and without deviating from objective and the scope of inventive technique scheme, it all should be contained at this In the middle of bright right.

Claims (6)

1. double tops disturbance rejection north finding method, it is characterised in that specifically comprise the following steps that

Step 1: north-seeking system is initialized；

Step 2: gather X-axis, Y-axis optical fibre gyro exports information with accelerometer, demarcates the parameter of inertia device；Specifically, 1) at a latitude it is known that and demarcate turntable under, by gather the X-axis of equally distributed 12 horizontal levels, the gyro of Y-axis Output data, calibrate 8 parameters in double optical fibre gyro error model；2) under turntable, by sensitive positive and negative terrestrial gravitation Acceleration information, under the conditions of gathering 6 kinds, X-axis, Y-axis accelerometer output data, calibrate in dual acceleration meter error model 8 parameters；

Step 3a: the angle of pitch and two parameter initial bindings the most of roll angle are crossed and judged, if being judged as NO, the most initially Calculate the bookbinding angle of pitch and two parameters of roll angle；If being judged as YES, then use the angle of pitch after updating and roll angle parameter；

Step 3b: 16 parameters and the angle of pitch described in step 3a, two parameter one isomorphisms of roll angle will demarcated in step 2 Building sky to virtual information, i.e. sky is to gyro virtual information and sky to accelerometer virtual information, thus realizes full attitude inertia The structure of device information；

Step 4: utilize inertial system technique of alignment, computer azimuth angle, the angle of pitch, the information of roll angle；With the roll angle calculated With the roll angle described in angle of pitch information updating step 3a and angle of pitch parameter；The azimuth information that output calculates, seeks north Process terminates.

Double tops disturbance rejection north finding method the most according to claim 1, it is characterised in that: described step 4 is seeking north process Before end, arrange Rule of judgment come repeated execution of steps 2～4 with obtain satisfactory seek north result；If being judged as YES, then seek North process terminates, if being judged as NO, then returns and performs step 2.

Double tops disturbance rejection north finding method the most according to claim 1, it is characterised in that: inertia device in described step 2 Concrete scaling method as follows:

Corresponding attitude matrix is:

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]---\left(1\right)$

1) optical fibre gyro parameter calibration:

Double optical fibre gyro error models:

X-axis gyro:

$W_{bx}=W_{0x}+K_{xx}{\mathrm{\&omega;}}_{ibx}^b+k_{yx}{\mathrm{\&omega;}}_{iby}^b+k_{zx}{\mathrm{\&omega;}}_{ibz}^b---\left(2\right)$

Y-axis gyro:

$W_{by}=W_{0y}+k_{xy}{\mathrm{\&omega;}}_{ibx}^b+K_{yy}{\mathrm{\&omega;}}_{iby}^b+k_{zy}{\mathrm{\&omega;}}_{ibz}^b---\left(3\right)$

Above in two formulas, W_bx, W_byIt is X-axis and Y-axis gyro output information, W respectively_0x, W_0yIt is X-axis and Y-axis gyro output zero respectively Position,It is the gyro measured value to carrier angular velocity, K_xx, K_yyIt is X-axis and Y-axis gyro calibration factor respectively, k_xy, k_zx, k_xy, k_zyIt is gyro cross-coupling coefficient respectively；

At latitude it is known that and under the conditions of horizontal stationary, have following three formulas:

$\begin{array}{c}{\mathrm{\&omega;}}_{ibx}^b=-{\mathrm{\&omega;}}_{ie}\sin \mathrm{\&psi;}\cos L\\ {\mathrm{\&omega;}}_{iby}^b={\mathrm{\&omega;}}_{ie}\cos \mathrm{\&psi;}\cos L\\ {\mathrm{\&omega;}}_{ibz}^b={\mathrm{\&omega;}}_{ie}\sin L\end{array}---\left(4\right)$

In above formula, ω_ieIt is rotational-angular velocity of the earth, for constant；(4) formula is brought into (2), (3) formula, has

W_bx=W_0x+K_xx(-ω_ie sinψcosL)+k_yx(ω_ie cosψcosL)+k_zx(ω_ie sinL) (5)

W_by=W_0y+k_xy(-ω_ie sinψcosL)+K_yy(ω_ie cosψcosL)+k_zy(ω_ie sinL) (6)

During demarcating gyro, remain horizontal stationary, demarcate azimuthal point and choose successivelyCorresponding X Axle gyro and Y-axis gyro are output as

$W_b={\left[\begin{array}{cccc}{W}_{bx0}& W_{bx1}& ...& W_{bx11}\\ W_{by0}& W_{by1}& ...& W_{by11}\end{array}\right]}_{12\&times;2}^T$

Build following two matrixes,

$\begin{array}{cc}A={\left[\begin{array}{cccc}1& -{\mathrm{\&omega;}}_{ie}\sin 0\cos L& {\mathrm{\&omega;}}_{ie}\cos 0\cos L& {\mathrm{\&omega;}}_{ie}\sin L\\ 1& -{\mathrm{\&omega;}}_{ie}\sin \frac{\mathrm{\&pi;}}{6}\cos L& {\mathrm{\&omega;}}_{ie}\cos \frac{\mathrm{\&pi;}}{6}\cos L& {\mathrm{\&omega;}}_{ie}\sin L\\ .& .& .& .\\ .& .& .& .\\ .& .& .& .\\ 1& -{\mathrm{\&omega;}}_{ie}\sin \frac{11\mathrm{\&pi;}}{6}\cos L& {\mathrm{\&omega;}}_{ie}\cos \frac{11\mathrm{\&pi;}}{6}\cos L& {\mathrm{\&omega;}}_{ie}\sin L\end{array}\right]}_{12\&times;4}& X={\left[\begin{array}{cc}{W}_{0x}& W_{0y}\\ K_{xx}& k_{xy}\\ k_{yx}& K_{yy}\\ k_{zx}& k_{zy}\end{array}\right]}_{4\&times;2}\end{array}$

$\left\{\begin{array}{c}AX=W_b\\ X={\left(A^{T}A\right)}^{-1}{A}^T{W}_b\end{array}\right.---\left(7\right)$

8 parameters in X matrix, are double optical fibre gyro error model (2), the parameter of (3) formula, so at a latitude Know, and under the turntable demarcated, export data by gathering the gyro of 12 horizontal levels, above 8 parameters can be calibrated；

2) accelerometer is demarcated

Dual acceleration meter error model is as follows:

$\begin{array}{c}{V}_{ax}=K_{ax0}+K_{axx}{f}_x^b+k_{ayx}{f}_y^b+k_{azx}{f}_z^b\\ V_{ay}=K_{ay0}+K_{axy}{f}_x^b+k_{ayy}{f}_y^b+k_{azy}{f}_z^b\end{array}---\left(8\right)$

Above in two formulas, V_ax, V_ayIt is X-axis and Y-axis accelerometer output information, K respectively_ax0, K_ax0It is X-axis and Y-axis acceleration respectively Degree meter output zero-bit,It is the accelerometer measured value to carrier specific force, K_axx, K_ayyIt is X-axis and Y-axis respectively Accelerometer calibration factor, k_axy, k_azx, k_axy, k_azyIt is accelerometer cross-coupling coefficient respectively；

Time accelerometer is demarcated, under quiescent conditions, choose X-axis, Y-axis, the acceleration of Z axis accelerometer sensitive positive negative G successively, Corresponding X-axis accelerometer and Y-axis accelerometer are output as

$V_a={\left[\begin{array}{cccccc}{V}_{ax\_x+g}& V_{ax\_x-g}& V_{ax\_y+g}& V_{ax\_y-g}& V_{ax\_z+g}& V_{ax\_z-g}\\ V_{ay\_x+g}& V_{ay\_x-g}& V_{ay\_y+g}& V_{ay\_y-g}& V_{ay\_z+g}& V_{ay\_z-g}\end{array}\right]}_{6\&times;2}^T$

Build following two matrixes,

$\begin{array}{cc}A={\left[\begin{array}{cccc}1& 1& 0& 0\\ 1& -1& 0& 0\\ 1& 0& 1& 0\\ 1& 0& -1& 0\\ 1& 0& 0& 1\\ 1& 0& 0& -1\end{array}\right]}_{6\&times;4}& X={\left[\begin{array}{cc}{K}_{ax0}& K_{ay0}\\ K_{axx}& K_{axy}\\ K_{ayx}& K_{ayy}\\ K_{azx}& K_{azy}\end{array}\right]}_{4\&times;2}\end{array}$

$\left\{\begin{array}{c}AX=V_a\\ X={\left(A^{T}A\right)}^{-1}{A}^T{V}_a\end{array}\right.---\left(9\right)$

8 parameters in X matrix, are the parameter of dual acceleration meter error model (8) formula, so under turntable, by sensitivity Positive and negative terrestrial gravitation acceleration information, under the conditions of gathering 6 kinds, X-axis Y-axis accelerometer output data, above 8 ginsengs can be calibrated Number.

Double tops disturbance rejection north finding method the most according to claim 3, it is characterised in that: in described step 3b, sky is to virtual The concrete construction method of information is as follows:

1) sky builds to gyro virtual information

Have according to Eulerian equation:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\frac{sin\mathrm{\&gamma;}}{cos\mathrm{\&theta;}}{\mathrm{\&omega;}}_{nbx}^b-\frac{cos\mathrm{\&gamma;}}{\cos \mathrm{\&theta;}}{\mathrm{\&omega;}}_{nbz}^b\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}={\mathrm{cos\&gamma;\&omega;}}_{nbx}^b+{\mathrm{sin\&gamma;\&omega;}}_{nbz}^b\\ \stackrel{\&CenterDot;}{\mathrm{\&gamma;}}={\mathrm{sin\&gamma;tan\&theta;\&omega;}}_{nbx}^b+{\mathrm{\&omega;}}_{nby}^b-{\mathrm{cos\&gamma;tan\&theta;\&omega;}}_{nbz}^b\end{array}\right.---\left(10\right)$

Considering angular velocity dynamic constrained condition, in carrier orientation, angular velocity is zero, i.e.

$\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=0---\left(11\right)$

Bring above formula into have, obtain attitude angular velocity constraint equation:

$\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\frac{sin\mathrm{\&gamma;}}{cos\mathrm{\&theta;}}{\mathrm{\&omega;}}_{nbx}^b-\frac{cos\mathrm{\&gamma;}}{cos\mathrm{\&theta;}}{\mathrm{\&omega;}}_{nbz}^b=0---\left(12\right)$

Arrangement obtains:

${\mathrm{\&omega;}}_{nbz}^b={\mathrm{tan\&gamma;\&omega;}}_{nbx}^b---\left(13\right)$

In view of there being following formula to set up

${\mathrm{\&omega;}}_{ibz}^b=h_{inz}^b+{\mathrm{\&omega;}}_{nbz}^b={\mathrm{\&omega;}}_{inz}^b+tan\mathrm{\&gamma;}({\mathrm{\&omega;}}_{ibx}^b-{\mathrm{\&omega;}}_{inx}^b)---\left(14\right)$

Knowing according to the error model in (2), (3), X-axis gyro and Y-axis gyro are output as

$\left[\begin{array}{c}{W}_{bx}\\ W_{by}\end{array}\right]-\left[\begin{array}{c}{W}_{0x}\\ W_{0y}\end{array}\right]=\left[\begin{array}{ccc}{K}_{xx}& k_{yx}& k_{zx}\\ k_{xy}& K_{yy}& k_{zy}\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_{ibx}^b\\ {\mathrm{\&omega;}}_{iby}^b\\ {\mathrm{\&omega;}}_{ibz}^b\end{array}\right]---\left(15\right)$

(14) are brought into (15), obtain:

$\begin{array}{c}\left[\begin{array}{c}{W}_{bx}\\ W_{by}\end{array}\right]-\left[\begin{array}{c}{W}_{0x}\\ W_{0y}\end{array}\right]=\left[\begin{array}{ccc}{K}_{xx}& k_{yx}& k_{zx}\\ k_{xy}& K_{yy}& k_{zy}\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_{ibx}^b\\ {\mathrm{\&omega;}}_{iby}^b\\ {\mathrm{\&omega;}}_{inz}^b+\tan \mathrm{\&gamma;}\left({\mathrm{\&omega;}}_{ibx}^b-{\mathrm{\&omega;}}_{inx}^b\right)\end{array}\right]\\ \left[\begin{array}{cc}{K}_{xx}+k_{zx}\tan \mathrm{\&gamma;}& k_{yx}\\ k_{xy}+k_{zy}\tan \mathrm{\&gamma;}& K_{yy}\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_{ibx}^b\\ {\mathrm{\&omega;}}_{iby}^b\end{array}\right]+\left[\begin{array}{c}{k}_{zx}\\ k_{zy}\end{array}\right]\left({\mathrm{\&omega;}}_{inz}^b-{\mathrm{tan\&gamma;\&omega;}}_{inx}^b\right)\end{array}---\left(16\right)$

Wherein

${\mathrm{\&omega;}}_{in}^b=C_n^b{\mathrm{\&omega;}}_{in}^n=C_n^b({\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)---\left(17\right)$

Owing to when seeking north, system is static in navigational coordinate system, navigational coordinate system relatively spherical coordinate system does not rotate, institute WithIt is zero, the most northeastward under sky coordinate system

${\mathrm{\&omega;}}_{in}^b=C_n^b{\mathrm{\&omega;}}_{in}^n=C_n^b{\mathrm{\&omega;}}_{ie}^n=C_n^b\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}_{ie}\cos L\\ {\mathrm{\&omega;}}_{ie}\sin L\end{array}\right]---\left(18\right)$

So that

$\left\{\begin{array}{c}\left[\begin{array}{c}{\mathrm{\&omega;}}_{ibx}^b\\ {\mathrm{\&omega;}}_{iby}^b\end{array}\right]={\left[\begin{array}{cc}{K}_{xx}+k_{zx}\tan \mathrm{\&gamma;}& k_{yx}\\ k_{xy}+k_{zy}\tan \mathrm{\&gamma;}& K_{yy}\end{array}\right]}^{-1}\&lsqb;\left[\begin{array}{c}{W}_{bx}\\ W_{by}\end{array}\right]-\left[\begin{array}{c}{W}_{0x}\\ W_{0y}\end{array}\right]-\left[\begin{array}{c}{k}_{zx}\\ k_{zy}\end{array}\right]\left({\mathrm{\&omega;}}_{inz}^b-{\mathrm{tan\&gamma;\&omega;}}_{inx}^b\right)\&rsqb;\\ {\mathrm{\&omega;}}_{ibz}^b={\mathrm{\&omega;}}_{inz}^b=\tan \mathrm{\&gamma;}\left({\mathrm{\&omega;}}_{ibx}^b-{\mathrm{\&omega;}}_{inx}^b\right)\end{array}\right.---\left(19\right)$

2) sky builds to accelerometer virtual information

Have according to specific force equation:

${\stackrel{\&CenterDot;}{v}}^n=C_b^n{f}^b-(2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)\&times;v^n+g^n---\left(20\right)$

Considering the dynamic constrained condition of carrier line motion, speed and acceleration in vertical direction are on zero, and horizontal direction Speed is 0, i.e.

${\stackrel{\&CenterDot;}{v}}_z^n=v^n=0---\left(21\right)$

Bring above formula into have

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{v}}_x^n\\ {\stackrel{\&CenterDot;}{v}}_y^n\\ {\stackrel{\&CenterDot;}{v}}_z^n\end{array}\right]=\left[\begin{array}{ccc}X& X& X\\ X& X& X\\ -sin\mathrm{\&gamma;}cos\mathrm{\&theta;}& sin\mathrm{\&theta;}& cos\mathrm{\&gamma;}cos\mathrm{\&theta;}\end{array}\right]\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]---\left(22\right)$

Arrange sky to acceleration information is:

${\stackrel{\&CenterDot;}{v}}_z^n=-{\mathrm{sin\&gamma;cos\&theta;f}}_x^b+{\mathrm{sin\&theta;f}}_y^b+{\mathrm{cos\&gamma;cos\&theta;f}}_z^b-g=0---\left(23\right)$

Z axis accelerometer is output as:

$f_z^b={\mathrm{tan\&gamma;f}}_x^b-\frac{tan\mathrm{\&theta;}}{\cos \mathrm{\&gamma;}}{f}_y^b+\frac{g}{cos\mathrm{\&gamma;}cos\mathrm{\&theta;}}---\left(24\right)$

Knowing according to the error model in (8) formula, X accelerometer and Y accelerometer are output as

$\left[\begin{array}{c}{V}_{ax}\\ V_{ay}\end{array}\right]-\left[\begin{array}{c}{K}_{ax0}\\ K_{ay0}\end{array}\right]=\left[\begin{array}{ccc}{K}_{axx}& k_{ayx}& k_{azx}\\ k_{axy}& K_{ayy}& k_{azy}\end{array}\right]\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]---\left(25\right)$

(24) are brought into (25), obtain:

So that

$\left\{\begin{array}{c}\left[\begin{array}{c}{f}_x^b\\ f_y^b\end{array}\right]=A^{-1}\&lsqb;\left[\begin{array}{c}{V}_{ax}\\ V_{ay}\end{array}\right]-\left[\begin{array}{c}{K}_{ax0}\\ K_{ay0}\end{array}\right]\left[\begin{array}{c}{K}_{azx}\\ K_{azy}\end{array}\right]\frac{g}{cos\mathrm{\&gamma;}cos\mathrm{\&theta;}}\&rsqb;\\ f_z^b={\mathrm{tan\&gamma;f}}_x^b-\frac{tan\mathrm{\&theta;}}{cos\mathrm{\&gamma;}}{f}_y^b+\frac{g}{\cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}}\end{array}\right.---\left(27\right)$

The data that (19) and (27) construct according to above, just constitute complete Three Degree Of Freedom inertia device data.

Double tops disturbance rejection north finding method the most according to claim 4, it is characterised in that: described step 4 utilizes inertia The method of system's alignment computer azimuth angle is as follows:

The quaternary number form formula of attitude matrix is as follows

$q_n^b\left(t\right)=q_n^{i_0}\left(t\right)\&CircleTimes;q_{i_0}^{i_{b0}}\&CircleTimes;q_{i_{b0}}^b\left(t\right)---\left(28\right)$

Wherein

$\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{i_0}^n\left(t\right)=\frac{1}{2}{q}_{i_0}^n\left(t\right)\&CircleTimes;{\mathrm{\&omega;}}_{in}^n\\ {\stackrel{\&CenterDot;}{q}}_{i_{b0}}^b\left(t\right)=\frac{1}{2}{q}_{i_{b0}}^b\left(t\right)\&CircleTimes;{\mathrm{\&omega;}}_{ib}^b\end{array}---\left(29\right)$

Initial time,During seeking north, carrier is in geo-stationary, and does not has speed, Now For the gyro data in (19) formula, i.e.

The complete card solving method that (29) formula uses quaternion differential equation can obtain with real-time update ArriveWith

Owing to having

$q_n^b\left(t\right)f^b\left(t\right)q_n^{b^{*}}\left(t\right)=g^n---\left(30\right)$

Bring (28) formula into (30) formula to obtain:

Arrangement obtains:

In formula, left kernel function and right kernel function are respectively as follows:

$\begin{array}{cc}M\left(\mathrm{\&beta;}\right)=\left[\begin{array}{cccc}{\mathrm{\&beta;}}_0& -{\mathrm{\&beta;}}_1& -{\mathrm{\&beta;}}_2& -{\mathrm{\&beta;}}_3\\ {\mathrm{\&beta;}}_1& {\mathrm{\&beta;}}_0& -{\mathrm{\&beta;}}_3& {\mathrm{\&beta;}}_2\\ {\mathrm{\&beta;}}_2& {\mathrm{\&beta;}}_3& {\mathrm{\&beta;}}_0& -{\mathrm{\&beta;}}_1\\ {\mathrm{\&beta;}}_3& -{\mathrm{\&beta;}}_2& {\mathrm{\&beta;}}_1& {\mathrm{\&beta;}}_0\end{array}\right]& M^{,}\left(\mathrm{\&alpha;}\right)=\left[\begin{array}{cccc}{\mathrm{\&alpha;}}_0& -{\mathrm{\&alpha;}}_1& -{\mathrm{\&alpha;}}_2& -{\mathrm{\&alpha;}}_3\\ {\mathrm{\&alpha;}}_1& {\mathrm{\&alpha;}}_0& {\mathrm{\&alpha;}}_3& -{\mathrm{\&alpha;}}_2\\ {\mathrm{\&alpha;}}_2& -{\mathrm{\&alpha;}}_3& {\mathrm{\&alpha;}}_0& {\mathrm{\&alpha;}}_1\\ {\mathrm{\&alpha;}}_3& {\mathrm{\&alpha;}}_2& -{\mathrm{\&alpha;}}_1& {\mathrm{\&alpha;}}_0\end{array}\right]\end{array};$

For solvingIt is converted into and seeks matrixMinimal eigenvalue characteristic of correspondence vector, will solve after obtaining 's(28) formula of bringing into obtains newThe most available orientation, pitching, roll information；

Orientation:

Pitching: θ=a sin (2 (q₂q₃+q₀q₁)) (34)

Roll:

Wherein

Double tops disturbance rejection north finding method the most according to claim 4, it is characterised in that: in described step 3a the angle of pitch with The method of the initial binding of roll angle is as follows:

Comparison (27) formula, does not consider that sky, to relevant parameter, has:

$\left[\begin{array}{c}{f}_x^b\\ f_y^b\end{array}\right]={\left[\begin{array}{cc}{K}_{axx}& k_{ayx}\\ k_{axy}& K_{ayy}\end{array}\right]}^{-1}\&lsqb;\left[\begin{array}{c}{V}_{ax}\\ V_{ay}\end{array}\right]-\left[\begin{array}{c}{K}_{ax0}\\ K_{ay0}\end{array}\right]\&rsqb;---\left(36\right)$

Horizontal accelerometer output information is now utilized to calculate the angle of pitch and roll angle:

$\begin{array}{c}\mathrm{\&theta;}=a\sin \left(-f_y^b/g\right)\\ \mathrm{\&gamma;}=a\sin \left(f_x^b/g\cos \mathrm{\&theta;}\right)\end{array}---\left(37\right).$