CN103743413B - Heeling condition modulated is sought northern instrument alignment error On-line Estimation and is sought northern error compensating method - Google Patents

Abstract

The present invention relates to a kind of heeling condition modulated and seek northern instrument alignment error On-line Estimation and seek northern error compensating method, it is characterized in that: step 1: set up the transformational relation between geographic coordinate system, carrier coordinate system, platform coordinate system, gyro coordinate system and four; Step 2: set up under heeling condition, turntable in the time of self vertical center axis constant speed rotary, output model when gyro exists alignment error angle; Step 3: set up under heeling condition, turntable in the time of self vertical center axis constant speed rotary, output model when accelerometer exists alignment error; Step 4: the output signal to gyro and accelerometer is processed, and obtain the inclination angle of carrier; Step 5: according in step 4, gyro, accelerometer data result after treatment being carried out to On-line Estimation to alignment error angle; Step 6: according to estimating and seek northern error processing the carrier inclined angle obtaining in the signal of rear gyro and step 4 in step 5, then course angle rough estimate evaluation is revised, obtained course angle accurately, complete the north of seeking of carrier.

Description

Heeling condition modulated is sought northern instrument alignment error On-line Estimation and is sought northern error compensating method

Technical field

The present invention relates to a kind of heeling condition modulated seeks northern instrument alignment error On-line Estimation and seeks northern error compensating method.

Background technology

In human society life practice, geographic orientation is indispensable information, seeks north and will find out carrier relatively exactlyThe position relation of the north orientation of ball, the i.e. course angle of definite carrier. Strapdown is sought north and is claimed again analytic expression to seek north, and it utilizes gyroDirectly revolutions angular velocity component sensitively, have be swift in response, advantages of simple structure and simple, directly application of advancedOptical gyroscope as the desirable northern inertance element of seeking. Strapdown is sought north finding method that north adopts and is mainly contained static state and seek north and dynamicallySeek north. Static state seeks that northern method comprises that unit is put, two positions, three positions, four positions and multiposition are sought north, wherein two positions andFour position north finding methods are more common. At present, the research of static state being sought to northern case is mature on the whole, and is seeking northern case, signal filterRipple, error analysis and compensation, engineering prototype development aspect have all obtained certain achievement.

It is emerging in recent years a kind of Fast Dynamic North-Seeking scheme that rotation modulation formula is sought north, seeking in northern process, and Inertial Measurement UnitAround the continuous constant revolution of its vertical center axis, utilize the output signal of gyro in horizontal plane or inclined plane to calculate with turntableCarrier azimuth. Compare tradition and seek northern case, static measurement is become kinetic measurement by it, utilizes the constant speed rotary pair of turntableThe output of gyro and accelerometer is modulated, and can effectively suppress gyroscope constant value drift, random drift and accelerometer and floatMove, met and sought northern rapidity and hi-Fix requirement. At present, correlation theory, technology and method are only confined to revolveBackoff algorithm when modulation system is sought northern general principle, heeling condition and data processing aspect, do not consider the actual north of seekingThe alignment error angle of middle gyro.

In practical engineering application, the variation of the external environment such as humidity, the temperature factor when seeking the work of northern instrument, gyroAnd the mechanical connection between turntable can be out of shape because of stressed variation, causes between gyro coordinate system and platform coordinate system and existsAlignment error angle, and the size of this error angle and external interference factor closely related, cannot be by the rule of definite variationDescribe. There is no at present concrete grammar and overcome the northern error of seeking being caused by alignment error angle, make to seek in practical application northern smartDegree is subject to serious restriction.

Summary of the invention

The object of the invention is to provide a kind of heeling condition modulated seek northern instrument alignment error On-line Estimation and seek northern error compensationMethod, can estimate the alignment error angle between gyro coordinate system and platform coordinate system, and carries out seeking northern errorCompensation, effectively improves north finding precision.

Realize the object of the invention technical scheme:

A kind of heeling condition modulated is sought northern instrument alignment error On-line Estimation and seeks northern error compensating method, it is characterized in that:

Step 1: set up turning between geographic coordinate system, carrier coordinate system, platform coordinate system, gyro coordinate system and fourChange relation;

Step 2: set up under heeling condition, turntable is in the time of self vertical center axis constant speed rotary, and gyro exists alignment errorOutput model when angle;

Step 3: set up under heeling condition, turntable is in the time of self vertical center axis constant speed rotary, and accelerometer exists to be installedOutput model when error;

Step 4: the output signal to gyro and accelerometer is processed, and obtain the inclination angle of carrier;

Step 5: according in step 4, gyro, accelerometer data result after treatment being carried out alignment error angle onlineEstimate;

Step 6: according to estimating and seek processing the carrier inclined angle obtaining in the signal of rear gyro and step 4 in step 5North error, then revises course angle rough estimate evaluation, obtains course angle accurately, completes the north of seeking of carrier.

In step 2, output model when gyro exists alignment error angle is,

ω_yi＝K_y{-[(cosγsinH+sinθsinγcosH)ω_N-cosθsinγω_H]sinα_i

+(ω_NcosθcosH+ω_Hsinθ)cosα_i

-η[(cosγsinH+sinθsinγcosH)ω_Ncosα_i-cosθsinγω_Hcosα_i

+(ω_NcosθcosH+ω_Hsinθ)sinα_i]}+ε_yi

The course angle that in formula, H is carrier, θ and γ are respectively the angle of pitch and the roll angle of carrier, and η is platform coordinate system p and gyroAlignment error angle between coordinate system g, angle position alpha_i＝Ω·t_i, wherein t_iRepresent that turntable turns to the time of i position,Ω is turntable CAV,For local latitude, ω_ieFor earth rotation angleSpeed, ω_yiRepresent the output valve of i position gyro in turntable rotation, K_yRepresent the constant multiplier of gyro, ε_yiExpression turnsThe gyroscopic drift of i position during platform rotates, i=1,2 ...., n.

In step 4, the output signal of gyro and accelerometer is processed, and is obtained the inclination angle of carrier, by asLower method realizes,

Order ${\mathrm{\Ω}}_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,{\mathrm{\Ω}}_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ Have

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y[{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}-{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-\mathrm{\η}({\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ})]\\ {\mathrm{\Ω}}_y=K_y\{{\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ}-\mathrm{\η}[{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}\left]\right\}\end{array}\right.$

Order $a_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,a_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ Have

$\left\{\begin{array}{c}\mathrm{\θ}\≈\arcsin \left(\frac{a_y}{g}\right)\\ \mathrm{\γ}\≈\arcsin \left(\frac{a_x}{g\cos \mathrm{\θ}}\right)\end{array}\right.$

In formula, g represents terrestrial gravitation acceleration, f_yiRepresent the output valve of i position accelerometer in turntable rotation,i＝1,2,....,n。

In step 5, the size that On-line Estimation goes out alignment error angle realizes by the following method,

$\mathrm{\η}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)d\sin \hat{H}-(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(b\cos \hat{H}-c\sin \hat{H})}{\left({d\cos \hat{H}+e}_{}\right)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}$

A=ω in formula_Hcosθsinγ,b＝ω_Ncosγ,c＝ω_Nsinθsinγ,d＝ω_Ncosθ,e＝ω_Hsinθ， $\hat{H}=\mathrm{atg}\frac{-{\mathrm{\Ω}}_x\cos \mathrm{\θ}-{\mathrm{\Ω}}_y\sin \mathrm{\γ}\sin \mathrm{\θ}+K_y{\mathrm{\ω}}_H\sin \mathrm{\γ}}{\cos \mathrm{\γ}({\mathrm{\Ω}}_y-K_y{\mathrm{\ω}}_H\sin \mathrm{\θ})}.$

In step 6, the correction of carrier heading is realized by the following method,

$\mathrm{\ΔH}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)(a-b\sin \hat{H}-c\cos \hat{H})+(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(d\cos \hat{H}+e)}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}$

$H=\hat{H}+\mathrm{\ΔH}$

N value is 3000.

The beneficial effect that the present invention has:

Gyro of the present invention and accelerometer as Inertial Measurement Unit with turntable around the continuous constant revolution of its vertical centre, gyroBe used for measuring carrier inclined interior rotational-angular velocity of the earth component, accelerometer is used for measuring the inclination angle of carrier, by buildingOutput model when gyro, accelerometer exist alignment error angle under vertical heeling condition, to gyro, accelerometer output letterNumber process, estimate in real time the alignment error angle of gyro, then compensate seeking northern error, complete and seek north. ThisInvention is estimated the alignment error angle between gyro coordinate system and platform coordinate system, revises and seeks northern result, can be remarkableImprove north finding precision.

Brief description of the drawings

Fig. 1 is method flow diagram of the present invention;

Fig. 2 is the northern resolution error curve map of seeking when the inventive method is ignored alignment error with tradition in embodiment.

Detailed description of the invention

As shown in Figure 1, step 1: set up geographic coordinate system, carrier coordinate system, platform coordinate system, gyro coordinate system andTransformational relation between four;

The definition of coordinate system: geographic coordinate system ox_ny_nz_nFor sky, northeast coordinate system, the origin of coordinates is in turntable planar central; CarryBody coordinate system ox_by_bz_b, the origin of coordinates overlaps with geographic coordinate system initial point, ox_bAxle and oy_bAxle in turntable plane, oz_bAxlePerpendicular to turntable plane and and ox_bAxle, oy_bAxle is right-handed helix, wherein ox_bAxle, oy_bAxle is not with the rotation of turntableRotation; Platform coordinate system ox_py_pz_pCoordinate origin overlaps with geographic coordinate system initial point, initial time platform coordinate systemox_py_pz_pWith carrier coordinate system ox_by_bz_bRespectively corresponding coincidence of x, y, z axle, when turntable rotates, ox_pAxle, oy_pAxleRotate with turntable; Gyro coordinate system ox_gy_gz_gThe origin of coordinates equally also in turntable planar central, with revolving of turntableThen rotate. When single shaft gyroscope north searching, the sensitive axes of gyro and oy_gAxle overlaps, the alignment error that now northern instrument is sought in consideration,The oz of gyro coordinate system g system_gThe oz of axle and platform coordinate system p system_pAxle overlaps completely, oy_gAxle and oy_pAxle, ox_gAxle withox_pBetween axle, there is alignment error angle η.

Geographic coordinate system n to the transfer matrix of carrier coordinate system b is

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos H-\sin \mathrm{\θ}\sin \mathrm{\γ}\sin H& \cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin H& \cos \mathrm{\θ}\cos H& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos H+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin H& \sin \mathrm{\γ}\sin H-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos H& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(1\right)$

In formula: the course angle that H is carrier, θ and γ are respectively the angle of pitch and the roll angle of carrier.

When turntable with CAV Ω around oz_bAxle (oz_pAxle) while rotating counterclockwise, angle position alpha_i＝Ω·t_i, wherein t_iTableShow that turntable turns to the time of i position, carrier coordinate system to the transfer matrix of platform coordinate system is

$C_b^p=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_i& \sin {\mathrm{\α}}_i& 0\\ -\sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i& 0\\ 0& 0& 1\end{array}\right]---\left(2\right)$

Alignment error angle η between platform coordinate system p and gyro coordinate system g is low-angle, so platform coordinate is tied to topSpiral shell coordinate system transfer matrix is

$C_p^g=\left[\begin{array}{ccc}1& \mathrm{\η}& 0\\ -\mathrm{\η}& 1& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

Step 2: set up under heeling condition, turntable is in the time of self vertical center axis constant speed rotary, and gyro exists alignment errorOutput model when angle;

Consider gyro constant multiplier and gyroscopic drift, have

${\mathrm{\ω}}^g=K{C}_p^g(C_b^p{C}_n^b\·{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\Ω})+\mathrm{\ϵ}---\left(4\right)$

Wherein, ω^gRepresent angular speed under gyro coordinate system, K=[K_xK_yK_z]^T,K_gx、K_gy、K_gzRepresent that gyro is eachThe constant multiplier of axle,Represent the projection of rotational-angular velocity of the earth in geographic coordinate system, Ω=[00 Ω]^TFor seeking northern instrumentThe angular velocity vector that indexing mechanism rotates, Ω is the angular speed size that turntable rotates, ε=[ε_xε_yε_z]^T，ε_x、ε_y、ε_zRepresent the gyroscopic drift of the each axle of gyro, [... ]^TThe transposition of representing matrix.

Due to sensitive axes and the oy of gyro_gAxle, therefore gyro is output as angular velocity omega under gyro coordinate system^gIn the projection of y axle.By formula (1), (2), (3) substitution formula (4), and get ω^gY axle component can obtain gyro and be output as

ω_yi＝K_y{-[(cosγsinH+sinθsinγcosH)ω_N-cosθsinγω_H]sinα_i

+(ω_NcosθcosH+ω_Hsinθ)cosα_i（5）

-η[(cosγsinH+sinθsinγcosH)ω_Ncosα_i-cosθsinγω_Hcosα_i

+(ω_NcosθcosH+ω_Hsinθ)sinα_i]}+ε_yi

The course angle that in formula, H is carrier, θ and γ are respectively the angle of pitch and the roll angle of carrier, and η is platform coordinate system p and gyroAlignment error angle between coordinate system g, angle position alpha_i＝Ω·t_i, wherein t_iRepresent that turntable turns to the time of i position,Ω is turntable CAV,For local latitude, ω_ieFor earth rotation angleSpeed, ω_yiThe output valve that represents i position gyro in turntable rotation, Ky represents the constant multiplier of gyro, ε_yi＝ε₀+ε_diRepresent the gyroscopic drift of i position in turntable rotation, ε₀Constant value drift, ε_diRandom drift, i=1,2 ...., n.

Step 3: set up under heeling condition, turntable is in the time of self vertical center axis constant speed rotary, and accelerometer exists to be installedOutput model when error;

In Used in Revolution-Modulation North-Finder, adopt an accelerometer on horizontal direction y axle under gyro coordinate system to measureThe inclination angle of carrier, the specific force that accelerometer records is at the f that is projected as of geographic coordinate systemⁿ, being projected as of gyro coordinate systemf^g, acceleration of gravity is projected as g under geographic coordinate systemⁿ. Have

f^g＝[f_xf_yf_z]^T（6）

gⁿ＝[00-g]^T（7）

Because carrier is static, ground velocity is in the projection of geographic coordinate systemThe relative earth of geographic coordinate system is satThe rotational angular velocity of mark systemHave according to inertial navigation fundamental equation

$\begin{array}{c}{f}^n={\stackrel{\·}{V}}_e^n+(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V_e^n-g^n\\ =-g^n\\ ={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T\end{array}---\left(8\right)$

Consider acceleration drift ▽=[▽_x▽_y▽_z]^T, according to Coordinate Conversion principle, have

$f^g=\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=C_p^g{C}_b^p{C}_n^b\·f^n+\▿---\left(9\right)$

By in formula (1), (2), (3), (8) substitution formula (9), and get f^g's_yAxle component is as the output of accelerometer

f_yi＝(gsinθ+ηgcosθsinγ)cosα_i+(gcosθsinγ-ηgsinθ)sinα_i+▽_y（10）

Wherein f_yiRepresent i position oy in turntable rotation_gThe specific force that the accelerometer of axle records, i=1,2 ...., n.

Step 4: the output signal to gyro and accelerometer is processed, and obtain the inclination angle of carrier;

(1) gyro output signal data processing

Turntable rotates a circle, and in rotary course, gyro output constantly changes. Choosing n symmetric position in a week obtainsGyro output sampled value, in the time that frequency n is enough large, we can adopt following approximate expression to carry out data processing.

$\left\{\begin{array}{c}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\sin {\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\sin \mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\cos {\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\cos \mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\sin }^2{\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}{\sin }^2\mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\cos }^2{\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}{\cos }^2\mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\sin {\mathrm{\α}}_i\cos {\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\sin \mathrm{\α}\cos \mathrm{\αd\α}=0\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ϵ}}_{\mathrm{di}}\sin {\mathrm{\α}}_i\≈0\\ \frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ϵ}}_{\mathrm{di}}\cos {\mathrm{\α}}_i\≈0\end{array}\right.---\left(12\right)$

Order ${\mathrm{\Ω}}_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,{\mathrm{\Ω}}_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ Have according to formula (5), (11), (12)

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y[{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}-{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-\mathrm{\η}({\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ})]\\ {\mathrm{\Ω}}_y=K_y\{{\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ}-\mathrm{\η}[{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}\left]\right\}\end{array}\right.---\left(13\right)$

, consider amount of calculation and computational accuracy here, according to practical experience repeatedly, choose n=3000.

(2) accelerometer output signal data processing

With the data processing to gyro output signal in (1), choose turntable middle n the symmetric position that rotate a circle and obtain accelerationDegree meter output sampled value, order $a_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,a_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ Have according to formula (10), (11)

$\left\{\begin{array}{c}{a}_x\≈g\cos \mathrm{\θ}\sin \mathrm{\γ}-\mathrm{\η}g\sin \mathrm{\θ}\\ a_y\≈g\sin \mathrm{\θ}+\mathrm{\η}g\cos \mathrm{\θ}\sin \mathrm{\γ}\end{array}\right.---\left(14\right)$

Here, in like manner formula (13), n=3000.

In real work, the tiltangleθ, the γ that seek northern instrument are low-angle, and alignment error angle η is also low-angle, therefore ignoresLittle quantifier-the η of second order gsin θ in formula (14) and η gcos θ sin γ, have

$\left\{\begin{array}{c}{a}_x\≈g\cos \mathrm{\θ}\sin \mathrm{\γ}\\ a_y\≈g\sin \mathrm{\θ}\end{array}\right.---\left(15\right)$

Can obtain the inclination angle of carrier according to formula (15)

$\left\{\begin{array}{c}\mathrm{\θ}\≈\arcsin \left(\frac{a_y}{g}\right)\\ \mathrm{\γ}\≈\arcsin \left(\frac{a_x}{g\cos \mathrm{\θ}}\right)\end{array}\right.---\left(16\right)$

Step 5: according in step 4, gyro, accelerometer data result after treatment being carried out alignment error angle onlineEstimate.

Ignore alignment error angle η, according to formula (13), carrier heading is carried out to rough estimate

$\hat{H}=\mathrm{atg}\frac{-{\mathrm{\Ω}}_x\cos \mathrm{\θ}-{\mathrm{\Ω}}_y\sin \mathrm{\γ}\sin \mathrm{\θ}+K_y{\mathrm{\ω}}_H\sin \mathrm{\γ}}{\cos \mathrm{\γ}({\mathrm{\Ω}}_y-K_y{\mathrm{\ω}}_H\sin \mathrm{\θ})}---\left(17\right)$

If course angle true value H and estimated valueBetween pass be

$H=\hat{H}+\mathrm{\ΔH}---\left(18\right)$

Wherein Δ H is in a small amount.

By in formula (18) substitution formula (13), and trigonometric function sinH, cosH are carried out to Taylor expansion

$\left\{\begin{array}{c}\sin H=\sin \hat{H}+\cos \hat{H}\·\mathrm{\ΔH}+o\left(\mathrm{\ΔH}\right)\\ \cos H=\cos \hat{H}-\sin \hat{H}\·\mathrm{\ΔH}+o\left(\mathrm{\ΔH}\right)\end{array}\right.---\left(19\right)$

Wherein $o\left(\mathrm{\ΔH}\right)=\underset{k=2}{\overset{\∞}{\mathrm{\Σ}}}\frac{{\sin }^{\left(k\right)}\left(\hat{H}\right)}{k!}{\left(\mathrm{\ΔH}\right)}^k$ It is high-order a small amount of.

By in formula (19) substitution formula (13), ignore the high-order of η and Δ H in a small amount, have

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y[a-b\sin \hat{H}-c\cos \hat{H}-(b\cos \hat{H}-c\sin \hat{H})\·\mathrm{\ΔH}-(d\cos \hat{H}+e)\·\mathrm{\η}]\\ {\mathrm{\Ω}}_y=K_y[d\cos \hat{H}+e-d\sin \hat{H}\·\mathrm{\ΔH}+(a-b\sin \hat{H}-c\cos \hat{H})\·\mathrm{\η}]\end{array}\right.---\left(20\right)$

Wherein, a=ω_Hcosθsinγ,b＝ω_Ncosγ,c＝ω_Nsinθsinγ,d＝ω_Ncosθ，e＝ω_Hsinθ。

According to formula (20), On-line Estimation is carried out in alignment error angle

$\mathrm{\η}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)d\sin \hat{H}-(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(b\cos \hat{H}-c\sin \hat{H})}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}---\left(21\right)$

Step 6: according to estimating and seek processing the carrier inclined angle obtaining in the signal of rear gyro and step 4 in step 5North error, then repairs course angle rough estimate evaluation, obtains course angle accurately, completes the north of seeking of carrier.

$\mathrm{\ΔH}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)(a-b\sin \hat{H}-c\cos \hat{H})+(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(d\cos \hat{H}+e)}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}---\left(22\right)$

$H=\hat{H}+\mathrm{\ΔH}---\left(23\right)$

Seek the azimuth that northern essence is to try to achieve according to the rotational-angular velocity of the earth component recording in platform coordinate system carrier.Wherein, Ω_x、Ω_yBe equivalent to the angular speed recording under gyro coordinate system, in the time that alignment error angle exists, gyro coordinate systemAlso not exclusively overlap with platform coordinate system. And seek northern instrument in real work, alignment error is appearance inevitably,Now use again Ω_x、Ω_yAsk the azimuth of carrier, will inevitably introduce and seek northern error, the present invention can be by estimating installationError angle and seek northern error, obtains course angle accurately, thereby avoids seeking northern error because of what alignment error caused.

In conjunction with specific embodiments, further the present invention will be described. When emulation, put aside gyro scale factor error,The factor such as position shifter error and variations in temperature of turntable. If rotational-angular velocity of the earth ω_ie=15.04107 °/h, local geographic latitudeAlignment error angle η=20', turntable rotational angular velocity Ω=1 °/s, tiltangleθ=5 of turntable °, γ=5 °,The constant value drift of optical fibre gyro is 0.01 °/h, and random drift is 0.01 °/h, and n=3000 is counted in sampling location. Can from Fig. 2To find out, rotate a circle in process seeking northern instrument, seek northern instrument continuous wave output carrier heading, traditional alignment error of ignoringTime seek northern resolution error scope for [0.3 ° 0.4 °], this shows, ignore alignment error and cause requiring high accuracy to seekIn north, have a strong impact on the final northern result of seeking. The inventive method can well address this problem, can by Fig. 2To find out that alignment error is after overcompensation, seek northern resolution error scope for [0.2 ° 0.15 °], when not considering alignment errorNorth finding precision is significantly improved, and illustrates that the inventive method has played good elimination effect to alignment error. From the present embodimentCan find out, in practical engineering application, this method can sought northern instrument long-term work cause alignment error in the situation that,What well compensation was caused by alignment error seeks northern error, improves and seeks northern precision.

Claims (5)

1. heeling condition modulated is sought northern instrument alignment error On-line Estimation and is sought a northern error compensating method, itsBe characterised in that:

Step 1: set up geographic coordinate system, carrier coordinate system, platform coordinate system, gyro coordinate system and four itBetween transformational relation;

Step 2: set up under heeling condition, turntable is in the time of self vertical center axis constant speed rotary, and gyro exists peaceOutput model when dress error angle;

Step 3: set up under heeling condition, turntable is in the time of self vertical center axis constant speed rotary, and accelerometer is depositedOutput model in the time of alignment error angle;

Step 4: the output signal to gyro and accelerometer is processed, and obtain the inclination angle of carrier; ToolBody is realized by the following method,

Order ${\mathrm{\Ω}}_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{yi}{\mathrm{sin\α}}_i,Q_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{yi}{\mathrm{cos\α}}_i,$ Have

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y\[{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}-{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-\mathrm{\η}({\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ})\]\\ {\mathrm{\Ω}}_y=K_y\{{\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ}-\mathrm{\η}\[{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}\]\}\end{array}\right.$

Order $a_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{yi}{\mathrm{sin\α}}_i,a_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{yi}{\mathrm{cos\α}}_i,$ Have

$\left\{\begin{array}{c}\mathrm{\θ}\≈\arcsin \left(\frac{a_y}{g}\right)\\ \mathrm{\γ}\≈\arcsin \left(\frac{a_x}{g\cos \mathrm{\θ}}\right)\end{array}\right.$

In formula, g represents terrestrial gravitation acceleration, f_yiRepresent the output of i position accelerometer in turntable rotationValue, i=1,2 ...., n;

Step 5: according in step 4, gyro, accelerometer data result after treatment being entered alignment error angleRow On-line Estimation;

Step 6: according to estimating processing the carrier inclined angle obtaining in the signal of rear gyro and step 4 in step 5Count out and seek northern error, then course angle rough estimate evaluation is revised, obtain course angle accurately, complete carrierSeek north.

2. heeling condition modulated according to claim 1 is sought northern instrument alignment error On-line Estimation and Xun BeiError compensating method, is characterized in that: in step 2, output model when gyro exists alignment error angle is

ω_yi＝K_y{-[(cosγsinH+sinθsinγcosH)ω_N-cosθsinγω_H]sinα_i

+(ω_NcosθcosH+ω_Hsinθ)cosα_i

-η[(cosγsinH+sinθsinγcosH)ω_Ncosα_i-cosθsinγω_Hcosα_i

+(ω_NcosθcosH+ω_Hsinθ)sinα_i]}+ε_yi

The course angle that in formula, H is carrier, θ and γ are respectively the angle of pitch and the roll angle of carrier, and η is platform coordinate systemAlignment error angle between p and gyro coordinate system g, angle position alpha_i＝Ω·t_i, wherein t_iRepresent that turntable turns toThe time of i position, Ω is turntable CAV, ForLocal latitude, ω_ieFor rotational-angular velocity of the earth, ω_yiRepresent the output valve of i position gyro in turntable rotation,K_yRepresent the constant multiplier of gyro, ε_yiRepresent the gyroscopic drift of i position in turntable rotation,i＝1,2,....,n。

3. heeling condition modulated according to claim 2 is sought northern instrument alignment error On-line Estimation and Xun BeiError compensating method, is characterized in that: in step 5, alignment error angle On-line Estimation realizes by the following method,

$\mathrm{\η}=\frac{(a-bsin\hat{H}-ccos\hat{H}-{\mathrm{\Ω}}_x/K_y)dsin\hat{H}-(dcos\hat{H}+e-{\mathrm{\Ω}}_y/K_y)(bcos\hat{H}-csin\hat{H})}{(dcos\hat{H}+e)d\sin \hat{H}+(a-bsin\hat{H}-ccos\hat{H})(bcos\hat{H}-csin\hat{H})}$

A=ω in formula_Hcosθsinγ,b＝ω_Ncosγ,c＝ω_Nsinθsinγ,d＝ω_Ncosθ,e＝ω_Hsinθ，

$\hat{H}=atg\frac{-{\mathrm{\Ω}}_{x}cos\mathrm{\θ}-{\mathrm{\Ω}}_{y}sin\mathrm{\γ}sin\mathrm{\θ}+K_y{\mathrm{\ω}}_{H}sin\mathrm{\γ}}{\cos \mathrm{\γ}({\mathrm{\Ω}}_y-K_y{\mathrm{\ω}}_H\sin \mathrm{\θ})}.$

4. heeling condition modulated according to claim 3 is sought northern instrument alignment error On-line Estimation and Xun BeiError compensating method, is characterized in that: the correction to carrier heading in step 6 realizes by the following method,

$\mathrm{\Δ}H=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)(a-b\sin \hat{H}-c\cos \hat{H})+(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(d\cos \hat{H}+e)}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}$

$H=\hat{H}+\mathrm{\Δ}H.$

According to claim 2-4 the heeling condition modulated described in any one to seek northern instrument alignment error onlineEstimate and seek northern error compensating method, it is characterized in that: n value is 3000.