CN102840856A - Dynamically rotary modulated north-seeking method for gyroscope - Google Patents

Abstract

The invention belongs to the field of an inert measuring technique or geophysical experiment and instrument, in particular to a dynamically rotary modulated north-seeking method for a gyroscope. The dynamically rotary modulated north-seeking method for the gyroscope provided by the invention aims to realize fast and precise north-seeking measurement under the condition that a carrier has a large attitude angle without leveling or knowing the latitude of a measuring point, so that the speed of geodesic operation is improved. A rotary table which rotates uniformly at an angular speed Omega is used. The rotary table is provided with the gyroscope and two accelerometers. The method comprises the steps of: first, solving an angle of pitch and a roll angle by the two accelerometers; then, solving the geographic latitude L and a course angle by the gyroscope; and after calculating an initial value, improving the precision by using a real time recursive least square method. The dynamically rotary modulated north-seeking method for the gyroscope provided by the invention has the advantages that fast and precise north-seeking measurement can be realized under the condition without leveling or knowing the latitude of the measuring point, so that the speed of geodesic operation is improved.

Description

A kind of gyroscope north finding method of dynamic rotation modulation

Technical field

The invention belongs to inertia measurement technology or Geophysical Experiment and instrument field, relate generally to a kind of gyroscope north finding method of dynamic rotation modulation.

Background technology

One Chinese patent application CN90103687.0 discloses the invention of China No. 15 Inst. No. 1 Design Inst., Ministry of Aeronautic and Astranantic Industry a kind ofly seeks northern appearance.This is sought northern appearance and adopts a flexible gyroscope being installed on the turntable and an accelerometer as detecting components and parts; Detect rotational-angular velocity of the earth at a plurality of locational components through flexible gyroscope; In conjunction with the accelerometer output quantity of same position, calculate the angle of initial position and north orientation.Adopt the step motor drive turntable to rotate, realize the sampling of many measuring positions.In the driving of stepper motor, adopt the drive control module of analog circuit board as stepper motor, adopt optical pulse generator and position memory device record to be used, with zero-bit and other turned positions of confirming single axle table.And when seeking north, this is sought northern appearance and all will carry out zero-bit and adjust the operation of promptly making zero automatically at every turn.In addition, this zero-bit index line of seeking northern appearance is positioned on the housing, therefore, mostly is used for the north of seeking of carrier self, seldom is used for target is sought north.And realize that indexing mechanism control Inertial Measurement Unit is parked in different positions in the short period of time respectively and carries out data acquisition, this is difficult on Project Realization, thus its to seek the north time longer.

A kind of single axis fiber gyro north seeker that adopts optical fibre gyro as sensitive element of the open No.205 Inst., China Weapon Industry Co. of Chinese patent ZL200610043115.4 invention; This is sought northern appearance and comprises the single axis fiber gyro as sensitive element; Rotating shaft; The direction index line; Stepper motor; Single-chip microcomputer; Seek north and resolve computing machine; Optical fibre gyro; The direction index line all is connected in the rotating shaft; Stepper motor can drive rotating shaft and rotate; Can obtain the output signal of optical fibre gyro with this at diverse location; Single-chip microcomputer is mainly accomplished the control that countershaft rotates; Seeking north resolves computing machine and mainly accomplishes fibre optic gyroscope four uniformly-spaced samplings of output data on the position; And utilize its built-in mathematical model that sampled signal is handled and calculated and seek northern result; It both can accomplish the simple northern function of seeking, and also can accomplish and the measurement of target link with respect to the angle of north orientation.Because this seeks only the sample data of four positions of northern appearance, before a kind of minimizing of seeking northern appearance the sampling location, therefore improved the northern speed of seeking to a certain extent.But the minimizing sampling location is difficult to the north finding precision that reaches higher.And the control gyro is parked in respectively on four equally spaced diverse locations, gathers gyro data again, needs cost to be no less than 5 minutes seeking the north time equally.And adopt the output of direction index line to seek northern result, precision can be too not high yet, can't with adopt electrooptical device special-purpose angular instrument---total powerstation is mentioned in the same breath.

In order further to improve north finding precision, shorten and seek the north time, rising in recent years a kind of Dynamic High-accuracy rotation modulation seek northern technology.Its ultimate principle is under the drive of computer control and motor; Inertial Measurement Unit (IMU) rotates with constant angular velocity continuously; Computing machine is gathered the data of gyroscope sensitive axes and the data of accelerometer simultaneously; The value that reads angle-measuring equipment is calculated position angle by north as current angle by seeking northern machine solution.The inverse that this method can be suppressed to gyrostatic drift rotating speed doubly and has been eliminated the influence of gyroscope calibration factor to north finding precision, has improved the gyrostatic north finding precision and the speed of equal index greatly.Document [1], [2], [3], [4], [5], [6], [7] are the domestic part articles of on publication, delivering at present.Document [1,2] has been introduced a kind of continuous rotation of flexible gyroscope formation and has been sought northern case, and the northern error of seeking under the rotation status is analyzed, and has set up the mathematical model that continuous rotary is sought northern appearance, and has carried out simulation study.Document [4] has been studied a kind of algorithm of rotary laser gyroscope north searching instrument, and points out that document [1,2] does not provide the precision of gyroscope index when emulation, doubt the conclusion of its simulation study; Therefore document [4] thinks that this method is difficult to realize, then research multiposition method realizes seeking north.Simulation study has been carried out to principle and the algorithm that utilizes optical fibre gyro dynamically to seek north in document [6], [7], and its index is to seek north time 12min, 0.2 ° of north finding precision.Comprehensive above situation shows domestic research to continuous rotation modulation north finding method, also is in the exploratory stage, more to its principle introduction, and it is carried out the also less of systematic study.

[1] Zou Xiangyang, Sun Qian, Chen Jiabin, Xu Jianhua. continuous rotary is sought northern algorithm of seeking of northern appearance and signal Processing [J]. Beijing Institute of Technology's journal, 2004,24 (9): 801-807.

[2] Xu Jianhua, Xie Ling, Gao Yanan, Chen Jiabin. the rotation modulation system is sought northern appearance filtering technique research [J]. Beijing Institute of Technology's journal, 2005,25 (80:718-721.

[3] An Daoqi. continuous rotary is sought the development [D] of northern appearance. the Master degree candidate of Beijing Institute of Technology paper, 2006.

[4] Zhang Si incites somebody to action, Qin Shiqiao, Wang Shengshu. rotary laser gyroscope north searching instrument algorithm research [J]. and airborne weapon, 2006, (1): 12-14,40.

[5] Wang Changping. rotary laser gyroscope north searching instrument error modeling and simulation study [D]. graduate school of National University of Defense Technology item bachelorship paper, 2005.

[6] Liu Dongbo, Liu Jianye, Lai Jizhou. the monocycle based on optical fibre gyro is dynamically sought northern algorithm research [J] fast. sensor and micro-system, 2007,26 (11): 61-64.

[7] Liu Dongbo. the optical fibre gyro quick high accuracy is sought northern technology and experimental study [D]. Nanjing Aero-Space University, 2007.

Summary of the invention

The gyroscope north finding method that the objective of the invention is a kind of dynamic rotation modulation so that exist under the condition of great-attitude angle at carrier, and need not leveling, need not to know that realizing fast, accurately seeking north under the situation of measuring point latitude measures, and improves the geodetic operating speed.

The present invention is achieved in that a kind of gyroscope north finding method of dynamic rotation modulation, wherein,

Use is installed 1 gyroscope, 2 accelerometers with the turntable of angular velocity Ω uniform rotation on the turntable;

Turntable is the axle rotation with the vertical direction; Said gyrohorizon is installed in the support mounting hole, its responsive axis and rotating shaft quadrature, and turntable is when crossing zero-bit, and gyro to measure coordinate system x direction of principal axis points to the zero line of photoelectricity circle grating ruler reading head;

2 accelerometers 3 along the direction of two sensitive axes of twin shaft gyro, are vertically mounted in mutually perpendicular two mounting holes in single axle table support top respectively;

If sky, northeast coordinate system (OXnYnZn) (n system); The Xb axle of rotation stage body coordinate system (OXbYbZb) (b system) points to the grating zero-bit, and the Yb axle points to 90 ° of directions of grating, and the Zb axle points to turning axle; The Xm axle of gyro to measure coordinate system (OXmYmZm) (m system) points to accelerometer 1, and the Ym direction is pointed to accelerometer 2, and the Zm axle points to turning axle; Local geographic latitude is L; Pitching/roll/the course angle of seeking northern appearance stage body is respectively θ, γ, φ, and solution procedure comprises the steps:

At first, utilize accelerometer to find the solution: the angle of pitch and roll angle;

When the intermittent angle position of the relative stage body of Inertial Measurement Unit (IMU) is α=Ω t+ φ ₀The time, wherein Ω is the stage body rotating speed, φ ₀Be initial phase, reading the acceleration measuring value is (f _Mx, f _My), transform to the stage body coordinate system after, the specific force on x axle, the y axle is:

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{mx}}\cos \mathrm{\α}-f_{\mathrm{my}}\sin \mathrm{\α}\\ f_{\mathrm{mx}}\sin \mathrm{\α}+f_{\mathrm{my}}\cos \mathrm{\α}\end{array}\right]$

Then

θ＝sin ^-1(f _by/g)

γ＝sin ^-1(-f _bx/g/cosθ)

Then, utilize gyroscope to find the solution geographic latitude L and course angle Φ;

What rough north-seeking scheme of the present invention adopted is exactly the quadrature demodulation method, and its mathematical principle is:

ω _g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω _esinα

Because :+(cos θ cos φ cosL+sin θ sinL) Ω _eCos α

If attitude angle θ, γ are at 0 o'clock, can be reduced to:

ω _g＝(sinφcosα+cosφsinα)Ω _ecosL

＝Ω _ecosL·sin(α+φ)

Order: A=Ω _eCosLcos φ, B=Ω _eCosLsin φ,

Then have:

ω _g＝(Asinα+Bcosα)

Use sin α, cos α (α=Ω t+ φ respectively ₀) multiply by the following formula two ends then integration can get:

$\left\{\begin{array}{c}\hat{A}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\\ \hat{B}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\end{array}\right.$

So have: ${\widehat{\mathrm{\φ}}}_d=\mathrm{atc}\tan (\hat{B}/\hat{A}),$ $L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

It is the summary value that can provide the orientation, geographical north after turntable rotates a circle.

The gyroscope north finding method of aforesaid a kind of dynamic rotation modulation, wherein, after calculating initial value, adopt the least square method algorithm of real-time recursion to improve precision:

Linear equation does

Z _k＝H _kX _k-1+N _k

N wherein _kBe measurement noise, H _kFor measuring matrix; Z _kBe measurement amount, X _K-1For treating estimator;

Observation equation is following:

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X _k＝[φ-φ ₀?L-L ₀?E] ^T＝[Δφ?ΔL?E] ^T

The employing least square method of recursion is found the solution: [Δ φ Δ L E] ^T, E is a gyroscope constant value drift;

(1) initialization X ₀, P ₀,

X ₀Orientation, geographical north summary value by rough north-seeking obtains;

(2) according to observation equation, the calculating observation matrix H _kWith observed quantity Z _k, and the weighting coefficient W of definite new observed quantity _k

(3) recursion is calculated

$P_k=P_{k-1}-\frac{P_{k-1}{H}_k{H}_k^T{P}_{k-1}}{W_k^{-1}+H_k^T{P}_{k-1}{H}_k}$

K _k＝P _kH _kW _k

X _k＝X _k-1+K _k(Z _k-H _kX _k-1)

Get

[φ?L?E] ^T＝[φ ₀?L ₀?0] ^T+[Δφ?ΔL?E] ^T

Because formula

ω _g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω _esinα+(cosθcosφcosL+sinθsinL)Ω _ecosα

Be nonlinear equation, can not directly adopt least square method to find the solution, can earlier nonlinear equation be put Taylor expansion at initial value, obtain following formula, promptly about course angle and latitude

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}& \left(\mathrm{\φ}-{\mathrm{\φ}}_0\right)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}& (L-L_0)+E\end{array}$

Therefore can make

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X _k＝[φ-φ ₀?L-L ₀?E] ^T＝[Δφ?ΔL?E] ^T

The linear equation that can be similar to does

Z _k＝H _kX _k-1+N _k

The employing least square method of recursion can be in the hope of [Δ φ Δ L E] ^T, then by

[φ?L?E] ^T＝[φ ₀?L ₀?0] ^T+[Δφ?ΔL?E] ^T

Sought the north value accurately.

Advantage of the present invention is on calculation method, to adopt the initial value estimation to combine with accurately resolving; The initial value estimation block revolves according to turntable that the output data of gyroscope and accelerometer adopts quadrature demodulation when turning around; Accurately seek northern algorithm adopts real-time recursion on the basis of rough north-seeking least square method algorithm; Reduce the systematic error of gyroscope, accelerometer; Realized that real-time resolving goes out position angle by north, the local latitude of measuring point, the system that makes has realized seeking fast, in real time the purpose in north.

The invention solves following problem:

A. measure the attitude of carrier angle with two accelerometers, and with this compensation data the attitude of carrier error cause seek northern error, therefore need not to limit the quick appearance sense of gyro axle seeks north under the level of approximation state, has cancelled the levelling gear of the complicacy of similar gyrostabilized platform;

B. need not to add the latitude signal can accomplish and accurately seek north;

C. seeking north under the rotation status continuously, can eliminate the systematic error of gyro and accelerometer, thereby can improve north finding precision;

D. at the least square method algorithm that adopts real-time recursion, reduce the systematic error of gyroscope, accelerometer, realized that real-time resolving goes out the position angle by north of measuring point, the purpose of local latitude;

E. the present invention got final product output data less than 30 seconds, and north finding precision is better than 10 ' (1 δ); Refreshed once result in per 12 seconds, precision increases in time and improves, and seeks the 2 minutes time of north, and north finding precision is better than 1 '; Seek the 5 minutes time of north, north finding precision 30 ", can select different north finding precisions and time as required in the use.The above time does not contain preheating time (3 minutes).

Description of drawings

Fig. 1 is to use the hardware platform structural representation of the gyroscope north finding method of a kind of dynamic rotation modulation of the present invention.

Embodiment

Below in conjunction with accompanying drawing and specific embodiment the present invention is done further explanation:

To combine embodiment to be elaborated below:

As shown in Figure 1, this is for seeking northern appearance main machine structure synoptic diagram; If sky, northeast coordinate system (OXnYnZn) (n system); The Xb axle of rotation stage body coordinate system (OXbYbZb) (b system) points to grating zero-bit (with this as the turntable zero-bit), and the Yb axle points to 90 ° of directions of grating, Zb axle sensing turning axle; The Xm axle of gyro to measure coordinate system (OXmYmZm) (m system) points to accelerometer 1, and the Ym direction is pointed to accelerometer 2, and the Zm axle points to turning axle; Local geographic latitude is L; Pitching/roll/the course angle of seeking northern appearance stage body is respectively θ, γ, φ.

When the intermittent angle position of the relative stage body of Inertial Measurement Unit (IMU) is α=Ω t+ φ ₀(wherein Ω is the stage body rotating speed, φ ₀Be initial phase) when rotating.

Each angular velocity of stage body does

${\mathrm{\ω}}_m=T_b^m{T}_n^b{\mathrm{\ω}}_{\mathrm{ien}}=\left[\begin{array}{c}[(-\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ})\cos \mathrm{\α}-\left(\cos \mathrm{\θ}\cos \mathrm{\φ}\right)\sin \mathrm{\α}]{\mathrm{\Ω}}_e\cos L\\ ...............................+[(-\sin \mathrm{\γ}\cos \mathrm{\θ})\cos \mathrm{\α}-\left(\sin \mathrm{\θ}\right)\sin \mathrm{\α}]{\mathrm{\Ω}}_e\sin L\\ [(-\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ})\sin \mathrm{\α}+\left(\cos \mathrm{\θ}\cos \mathrm{\φ}\right)\cos \mathrm{\α}]{\mathrm{\Ω}}_e\cos L\\ ..........................+[+(-\sin \mathrm{\γ}\cos \mathrm{\θ})\sin \mathrm{\α}+\left(\sin \mathrm{\θ}\right)\sin \mathrm{\α}]{\mathrm{\Ω}}_e\sin L\\ (-\sin \mathrm{\γ}\sin \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}){\mathrm{\Ω}}_e\cos L+\left(\cos \mathrm{\γ}\cos \mathrm{\θ}\right){\mathrm{\Ω}}_e\sin L\end{array}\right]$

Transformation matrix of coordinates:

The rotational-angular velocity of the earth ω that fastens at n _IenRotational-angular velocity of the earth constant: Ω _e

The angular velocity of gyro sensitive axes sensing does

ω _g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω _esin?α+(cosθcosφcosL+sinθsinL)Ω _ecosα (1)

This is the expression formula of gyroscope sensitive axes sensing signal, and it is relevant with the rotation angle and the geographic latitude of the attitude angle of stage body, course angle, the relative stage body of gyro sensitive axes, and wherein, attitude angle (θ, γ) can utilize accelerometer measures to obtain.

If the acceleration measuring value is (f _Mx, f _My), transform to the stage body coordinate system after, the specific force on x axle, the y axle does

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{mx}}\cos \mathrm{\α}-f_{\mathrm{my}}\sin \mathrm{\α}\\ f_{\mathrm{mx}}\sin \mathrm{\α}+f_{\mathrm{my}}\cos \mathrm{\α}\end{array}\right]---\left(2\right)$

Then

θ＝sin ^-1(f _by/g)

γ＝sin ^-1(-f _bx/g/cosθ) (3)

After utilizing (3) to try to achieve the attitude angle of stage body, only contain two unknown numbers of course angle and latitude in (1), utilize the gyro to measure of two positions just can calculate course angle and latitude, to resolve the result more accurate and continuous wheel measuring is feasible.Here it is seeks northern appearance principle of work.

Can know that by said process output from Gyroscope is modulated to the sinusoidal signal that initial phase is φ.The method of finding the solution φ has least square method, quadrature demodulation method.

What rough north-seeking scheme of the present invention adopted is exactly the quadrature demodulation method, and its mathematical principle is:

Be easy analysis, establish attitude angle θ, γ is at 0 o'clock, (1) can simplify, and its implication is: the angle of pitch and roll angle are 0, and seeking when being used for rough north-seeking under the condition in north is to meet accuracy requirement, and can improve computing speed:

ω _g＝(sinφcosα+cosφsinα)Ω _ecosL (4)

＝Ω _ecosL·sin(α+φ)

Order: A=Ω _eCosLcos φ, B=Ω _eCosLsin φ,

Then (4) are rewritten as:

ω _g＝(Asinα+Bcosα) (5)

Use sin α, cos α (α=Ω t+ φ respectively ₀) multiply by (5) two ends then integration can get:

$\left\{\begin{array}{c}\hat{A}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\\ \hat{B}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\end{array}\right.---\left(6\right)$

So have: ${\widehat{\mathrm{\φ}}}_d=\mathrm{atc}\tan (\hat{B}/\hat{A}),$ $L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

It is the summary value that can provide the orientation, geographical north after turntable rotates a circle.

What the essence of invention was sought northern case employing is the least square method algorithm of real-time recursion, and its mathematical principle is:

If linear equation does

Z _k＝H _kX _k-1+N _k (7)

N wherein _kBe measurement noise, H _kFor measuring matrix; Z _kBe measurement amount, X _K-1For treating estimator.

Algorithm steps

(4) initialization X ₀, P ₀, X ₀Obtain by rough north-seeking;

(5) according to observation equation, the calculating observation matrix H _kWith observed quantity Z _k, and the weighting coefficient W of definite new observed quantity _k

(6) recursion is calculated

$P_k{=[P_{k-1}^{-1}+H_k{W}_k{H}_k^T]}^{-1}$

K _k＝P _kH _kW _k

X _k＝X _k-1+K _k(Z _k-H _kX _k-1) (8)

Because P _kNeed carry out twice matrix inversion operation in the asking for of battle array,, adopt the following formula recursion, P for fear of matrix inversion ₀Selection get final product according to the known system of selection of least square method, wherein, initial value is selected big more, new breath shared ratio in estimated value in the early stage is big more, the new breath that does not have in the early stage should provide with new observed quantity under the situation of priori is main.

$P_k=P_{k-1}=\frac{P_{k-1}{H}_k{H}_k^T{P}_{k-1}}{W_k^{-1}+H_k^T{P}_{k-1}{H}_k}$

K _k＝P _kH _kW _k

X _k＝X _k-1+K _k(Z _k-H _kX _k-1) (9)

Because formula (1) is the nonlinear equation about course angle and latitude, can not directly adopt least square method to find the solution, can earlier nonlinear equation be put Taylor expansion at initial value, promptly

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}& \left(\mathrm{\φ}-{\mathrm{\φ}}_0\right)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}& (L-L_0)+E\end{array}$

Therefore can make

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X _k＝[φ-φ ₀?L-L ₀?E] ^T＝[Δφ?ΔL?E] ^T (10)

The linear equation that can be similar to does

Z _k＝H _kX _k-1+N _k (11)

The employing least square method of recursion can be in the hope of [Δ φ Δ L E] ^T,

[φ?L?E] ^T＝[φ ₀?L ₀?0] ^T+[Δφ?ΔL?E] ^T (12)

Seek in the northern process, use display device to receive resolved data, show angle by north, local latitude, gyroscopic drift, seek north time, attitude angle, got final product output data in 30 seconds, refreshed once result in per 12 seconds, precision increases raising in time.

Claims (2)

1. one kind is dynamically rotated the gyroscope north finding method of modulating, and it is characterized in that:

Use is installed 1 gyroscope, 2 accelerometers with the turntable of angular velocity Ω uniform rotation on the turntable;

Turntable is the axle rotation with the vertical direction; Said gyrohorizon is installed in the support mounting hole, its responsive axis and rotating shaft quadrature, and turntable is when crossing zero-bit, and gyro to measure coordinate system x direction of principal axis points to the zero line of photoelectricity circle grating ruler reading head;

2 accelerometers 3 along the direction of two sensitive axes of twin shaft gyro, are vertically mounted in mutually perpendicular two mounting holes in single axle table support top respectively;

If sky, northeast coordinate system (OXnYnZn) (n system); The Xb axle of rotation stage body coordinate system (OXbYbZb) (b system) points to the grating zero-bit, and the Yb axle points to 90 ° of directions of grating, and the Zb axle points to turning axle; The Xm axle of gyro to measure coordinate system (OXmYmZm) (m system) points to accelerometer 1, and the Ym direction is pointed to accelerometer 2, and the Zm axle points to turning axle; Local geographic latitude is L; Pitching/roll/the course angle of seeking northern appearance stage body is respectively θ, γ, φ, and solution procedure comprises the steps:

At first, utilize accelerometer to find the solution: the angle of pitch and roll angle;

When the intermittent angle position of the relative stage body of Inertial Measurement Unit IMU is α=Ω t+ φ ₀The time, wherein Ω is the stage body rotating speed, φ ₀Be initial phase, reading the acceleration measuring value is (f _Mx, f _My), transform to the stage body coordinate system after, the specific force on x axle, the y axle is:

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{mx}}\cos \mathrm{\α}-f_{\mathrm{my}}\sin \mathrm{\α}\\ f_{\mathrm{mx}}\sin \mathrm{\α}+f_{\mathrm{my}}\cos \mathrm{\α}\end{array}\right]$

Then

θ＝sin ^-1(f _by/g)

γ＝sin ^-1(-f _bx/g/cosθ)

Then, utilize gyroscope to find the solution geographic latitude L and course angle Φ;

What rough north-seeking scheme of the present invention adopted is exactly the quadrature demodulation method, and its mathematical principle is:

ω _g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω _esinα

Because :+(cos θ cos φ cosL+sin θ sinL) Ω _eCos α

If attitude angle θ, γ are at 0 o'clock, can be reduced to:

ω _g＝(sinφcosα+cosφsinα)Ω _ecosL

＝Ω _ecosL·sin(α+φ)

Order: A=Ω _eCosLcos φ, B=Ω _eCosLsin φ,

Then have:

ω _g＝(Asinα+Bcosα)

Use sin α, cos α (α=Ω t+ φ respectively ₀) multiply by following formula, then two ends then integration can get:

$\left\{\begin{array}{c}\hat{A}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\\ \hat{B}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\end{array}\right.$

So have: ${\widehat{\mathrm{\φ}}}_d=\mathrm{atc}\tan (\hat{B}/\hat{A}),$ $L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

It is the summary value that can provide the orientation, geographical north after turntable rotates a circle.

2. the gyroscope north finding method of a kind of dynamic rotation modulation as claimed in claim 1 is characterized in that: after calculating initial value, adopt the least square method algorithm of real-time recursion to improve precision:

Linear equation does

Z _k＝H _kX _k-1+N _k

N wherein _kBe measurement noise, H _kFor measuring matrix; Z _kBe measurement amount, X _K-1For treating estimator;

Observation equation is following:

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X _k＝[φ-φ ₀?L-L ₀?E] ^T＝[Δφ?ΔL?E] ^T

The employing least square method of recursion is found the solution: [Δ φ Δ L E] ^T, E is a gyroscope constant value drift;

(1) initialization X ₀, P ₀,

X ₀Orientation, geographical north summary value by rough north-seeking obtains;

(2) according to observation equation, the calculating observation matrix H _kWith observed quantity Z _k, and the weighting coefficient W of definite new observed quantity _k

(3) recursion is calculated

$P_k=P_{k-1}-\frac{P_{k-1}{H}_k{H}_k^T{P}_{k-1}}{W_k^{-1}+H_k^T{P}_{k-1}{H}_k}$

K _k＝P _kH _kW _k

X _k＝X _k-1+K _k(Z _k-H _kX _K-1)

{。##.##1},

[φ?L?E] ^T＝[φ ₀?L ₀?0] ^T+[Δφ?ΔL?E] ^T

Because formula

ω _g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω _esinα+(cosθcosφcosL+sinθsinL)Ω _ecosα

Be nonlinear equation, can not directly adopt least square method to find the solution, can earlier nonlinear equation be put Taylor expansion at initial value, obtain following formula, promptly about course angle and latitude

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}& \left(\mathrm{\φ}-{\mathrm{\φ}}_0\right)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}& (L-L_0)+E\end{array}$

Therefore can make

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X _k＝[φ-φ ₀?L-L ₀?E] ^T＝[Δφ?ΔL?E] ^T

The linear equation that can be similar to does

Z _k＝H _kX _k-1+N _k

The employing least square method of recursion can be in the hope of [Δ φ Δ L E] ^T, then by

[φ?L?E] ^T＝[φ ₀?L ₀?0] ^T+[Δφ?ΔL?E] ^T

Sought the north value accurately.

Images