CN102840856B - A kind of gyroscope north finding method of dynamic rotary modulation - Google Patents

Abstract

The invention belongs to inertial survey technique or Geophysical Experiment and instrument field, be specifically related to the gyroscope north finding method of a kind of dynamic rotary modulation.Object is under carrier exists the condition of great-attitude angle, and without the need to leveling, without the need to knowing measuring point latitude when realize fast, accurately seek north and measure, improve geodetic operating speed.It uses with the turntable of angular velocity Ω uniform rotation, 1 gyroscope is installed by turntable, 2 accelerometers; First, 2 accelerometers are utilized to solve: the angle of pitch and roll angle; Then, gyroscope is utilized to solve geographic latitude L and course angle; After calculating initial value, the least square method algorithm of real-time recursion is adopted to improve precision.Advantage of the present invention be without the need to leveling, without the need to knowing measuring point latitude when realize fast, accurately seek north and measure, improve geodetic operating speed.

Description

A kind of gyroscope north finding method of dynamic rotary modulation

Technical field

The invention belongs to inertial survey technique or Geophysical Experiment and instrument field, relate generally to the gyroscope north finding method of a kind of dynamic rotary modulation.

Background technology

Chinese patent application CN90103687.0 discloses a kind of north finder of No. 15 Inst. No. 1 Design Inst., Ministry of Aeronautic and Astranantic Industry of China invention.This north finder adopts the flexible gyroscope and an accelerometer that are arranged on turntable as Detection theory device, the component of rotational-angular velocity of the earth on multiple position is detected by flexible gyroscope, in conjunction with the accelerometer output quantity of same position, calculate the angle of initial position and north orientation.Adopt driving stepper motor turntable to rotate, realize the sampling of many measuring positions.In the driving of stepper motor, adopt analog circuit board as the drive control module of stepper motor, adopt optical pulse generator and position memory device record with the use of, to determine zero-bit and other turned positions of single axle table.And when seeking north, this north finder all will carry out zero-bit and automatically adjust, operation of namely making zero at every turn.In addition, the zero-bit index line of this north finder is positioned on housing, therefore, seeks north mostly for carrier self, is rarely used in and seeks north to target.And realize indexing mechanism to control Inertial Measurement Unit and be parked in different positions in the short period of time respectively and carry out data acquisition, this is more difficult in Project Realization, therefore its to seek the north time longer.

The a kind of of China Patent No. ZL200610043115.4 open No.205 Inst., China Weapon Industry Co. invention adopts optical fibre gyro as the single axis fiber gyro north seeker of sensitive element, this north finder comprises the single axis fiber gyro as sensitive element, rotating shaft, direction line, stepper motor, single-chip microcomputer, seek north and resolve computing machine, optical fibre gyro, direction line is all connected in rotating shaft, stepper motor can drive axis of rotation, the output signal of optical fibre gyro at diverse location can be obtained with this, single-chip microcomputer mainly completes the control that countershaft rotates, seek north to resolve computing machine and mainly complete the sampling that fibre optic gyroscope exports data on four at equal intervals position, and the mathematical model utilizing it built-in processes sampled signal and calculates and seeks northern result, it both can complete simple seeks northern function, also can complete and the measurement of target link relative to the angle of north orientation.Because this north finder is only sampled the data of four positions, comparatively a kind of north finder decrease sampling location, therefore improve to a certain extent and seek northern speed.But minimizing sampling location, is difficult to reach higher north finding precision.And control gyro to be parked in respectively on four equally spaced diverse locations, then gather gyro data, same need cost be no less than 5 minutes seek the north time.And adopt direction line export seek northern result, precision also can not be too high, cannot with adopt electrooptical device special angular instrument---total powerstation is mentioned in the same breath.

In order to improve north finding precision further, shortening and seeking the north time, a kind of Dynamic High-accuracy rotation modulation of rising in recent years seeks northern technology.Its ultimate principle be computing machine control and motor drive under, Inertial Measurement Unit (IMU) is with constant angular velocity continuous rotation, computing machine gathers the data of gyroscope sensitive axes and the data of accelerometer simultaneously, the value reading angle-measuring equipment, as current angular, calculates position angle by north by seeking northern machine solution.Gyrostatic drift can be suppressed to the inverse of rotating speed doubly by the method, and eliminates the impact of gyroscope calibration factor on north finding precision, substantially increases gyrostatic north finding precision and the speed of equal index.Document [1], [2], [3], [4], [5], [6], [7] are the domestic part article delivered on publication at present.The continuous rotation that document [1,2] describes a kind of flexible gyroscope formation seeks northern case, analyzes, establish the mathematical model of Continuous rotary north-finder, and carried out simulation study the northern error of seeking under rotation status.Document [4] have studied a kind of algorithm of rotary laser gyroscope north searching instrument, and points out that document [1,2] does not provide gyrostatic precision index when emulating, and doubts the conclusion of its simulation study; Therefore document [4] thinks that the method is difficult to realize, then research Multiple station method realizes seeking north.Document [6], [7] are to utilizing optical fibre gyro to carry out the principle of Fast Dynamic North-Seeking and algorithm has carried out simulation study, and its index seeks north time 12min, north finding precision 0.2 °.Comprehensive above situation, shows domestic research of continuous rotation being modulated to north finding method, is also in the exploratory stage, more to its principles and methods, and carries out the also less of systematic study to it.

[1] Zou Xiangyang, Sun Qian, Chen Jiabin, Xu Jianhua. the north-seeking algorithm of Continuous rotary north-finder and signal transacting [J]. Beijing Institute of Technology's journal, 2004,24 (9): 801-807.

[2] Xu Jianhua, Xie Ling, Gao Yanan, Chen Jiabin. Filter Used in Revolution-Modulation North-Finder research [J]. Beijing Institute of Technology's journal, 2005,25 (80:718-721.

[3] An Daoqi. the development [D] of Continuous rotary north-finder. Beijing Institute of Technology's M Sc thesis, 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 quick Fast Dynamic North-Seeking algorithm research [J] of the monocycle based on optical fibre gyro. sensor and micro-system, 2007,26 (11): 61-64.

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

Summary of the invention

The object of the invention is the gyroscope north finding method of a kind of dynamic rotary modulation so that there is the condition of great-attitude angle at carrier under, and without the need to leveling, without the need to knowing measuring point latitude when realize fast, accurately seek north and measure, improve geodetic operating speed.

The present invention is achieved in that the gyroscope north finding method that a kind of dynamic rotary is modulated, wherein,

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

Turntable is that axle rotates with vertical direction; Described gyrohorizon is arranged in rack mounting apertures, and its responsive axis is orthogonal with rotating shaft, and when zero-bit crossed by turntable, the zero line of photoelectricity Circular gratings ruler reading head is pointed in gyro to measure coordinate system x-axis direction;

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

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

First, accelerometer is utilized to solve: the angle of pitch and roll angle;

When the instantaneous angular position of the relative stage body of Inertial Measurement Unit (IMU) is α=Ω t+ φ ₀time, wherein Ω is stage body rotating speed, φ ₀for initial phase, reading acceleration measuring value is (f _mx, f _my), after transforming to stage body coordinate system, the specific force in x-axis, y-axis 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, gyroscope is utilized to solve geographic latitude L and course angle Φ;

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

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

Due to :+(cos θ cos φ cosL+sin θ sinL) Ω _ecos α

If when attitude angle θ, γ are 0, 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 ₀) be multiplied by above formula two ends then integration can obtain:

$\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)$

Namely turntable can provide the outline value in orientation, geographical north after rotating a circle.

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

Linear equation is

Z _k＝H _kX _k-1+N _k

Wherein N _kfor measurement noise, H _kfor measurement matrix; Z _kfor measurement amount, X _k-1for amount to be estimated;

Observation equation is as follows:

$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

Employing least square method of recursion solves: [Δ φ Δ L E] ^t, E is gyroscope constant value drift;

(1) initialization X ₀, P ₀,

X ₀obtained by orientation, the geographical north outline value of rough north-seeking;

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

(3) recurrence calculation

$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)

?

[φ 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 the nonlinear equation about course angle and latitude, can not directly adopt least square method to solve, can first by nonlinear equation in initial value point Taylor expansion, obtain following formula, namely

${\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 is

Z _k＝H _kX _k-1+N _k

Employing least square method of recursion can in the hope of [Δ φ Δ L E] ^t, then by

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

Sought north value accurately.

Advantage of the present invention on calculation method, adopts initial value to estimate and accurately resolve to combine, initial value estimation block according to turntable revolve turn around time gyroscope and the output data acquisition quadrature demodulation of accelerometer, accurate north-seeking algorithm adopts the least square method algorithm of real-time recursion on the basis of rough north-seeking, reduce the systematic error of gyroscope, accelerometer, achieve position angle by north, local latitude that real-time resolving goes out measuring point, system is achieved fast, seek northern object in real time.

The invention solves following problem:

A. measure attitude of carrier angle with two accelerometers, and with this compensation data attitude of carrier error cause seek northern error, therefore under near-horizontal state, seeking north without the need to limiting gyro quick instrument sense axle, eliminating the levelling gear of the complexity of similar gyrostabilized platform;

B. can complete without the need to additional latitude signal and accurately seek north;

C. under continuous rotation state, seek north, the systematic error of gyro and accelerometer can be eliminated, thus can north finding precision be improved;

D. at the least square method algorithm adopting real-time recursion, reduce the systematic error of gyroscope, accelerometer, achieve real-time resolving and go out the position angle by north of measuring point, the object of local latitude;

E. the present invention was exportable data less than 30 seconds, and north finding precision is better than 10 ' (1 δ); Within every 12 seconds, refresh once result, precision increases in time and improves, and seek the north time 2 points, north finding precision is better than 1 '; Seek the north time 5 points, north finding precision 30 ", different north finding precisions and time can be selected as required in using.The above time is not containing preheating time (3 minutes).

Accompanying drawing explanation

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

Embodiment

Below in conjunction with the drawings and specific embodiments, the present invention is described further:

Be described in detail below in conjunction with embodiment:

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

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

The angular velocity of each axle of stage body is

${\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: at the rotational-angular velocity of the earth ω that n fastens _ien; Rotational-angular velocity of the earth constant: Ω _e.

The angular velocity of gyro sensitive axes sensing is

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

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

If acceleration measuring value is (f _mx, f _my), after transforming to stage body coordinate system, the specific force in x-axis, y-axis 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]---\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, (1) only containing course angle and latitude two unknown numbers in, utilize the gyro to measure of two positions just can calculate course angle and latitude, and continuous rotation measurement make calculation result more accurate.North finder principle of work that Here it is.

From said process, output from Gyroscope is modulated to the sinusoidal signal that initial phase is φ.The method solving φ has least square method, quadrature demodulation method.

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

For easy analysis, if when attitude angle θ, γ are 0, (1) can simplify, and its implication is: the angle of pitch and roll angle are 0, seeks for being meet accuracy requirement during rough north-seeking under the condition in north, 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 ₀) be multiplied by (5) two ends then integration can obtain:

$\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)$

Namely turntable can provide the outline value in orientation, geographical north after rotating a circle.

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

If linear equation is

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

Wherein N _kfor measurement noise, H _kfor measurement matrix; Z _kfor measurement amount, X _k-1for amount to be estimated.

Algorithm steps

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

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

(6) recurrence calculation

$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)

Due to P _kneed to carry out twice matrix inversion operation in the asking for of battle array, in order to avoid matrix inversion, adopt following formula recursion, P ₀selection carry out according to the known system of selection of least square method, wherein, Initial value choice is larger, and the ratio shared in estimated value in the early stage of new breath is larger, does not have the new breath that should provide based on new observed quantity when priori in the early stage.

$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 solve, can first by nonlinear equation in initial value point Taylor expansion, namely

${\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 is

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

Employing least square method of recursion can in the hope of [Δ φ Δ L E] ^t,

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

Seek in northern process, use display device to receive resolved data, show angle by north, local latitude, gyroscopic drift, seek north time, attitude angle, 30 seconds is exportable data, and within every 12 seconds, refresh once result, precision increases raising in time.

Claims (2)

1. a gyroscope north finding method for dynamic rotary modulation, is characterized in that:

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

Turntable is that axle rotates with vertical direction; Described gyrohorizon is arranged in rack mounting apertures, and its responsive axis is orthogonal with rotating shaft, and when zero-bit crossed by turntable, the zero line of photoelectricity Circular gratings ruler reading head is pointed in gyro to measure coordinate system x-axis direction;

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

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

First, accelerometer is utilized to solve: the angle of pitch and roll angle;

When the instantaneous angular position of the relative stage body of Inertial Measurement Unit IMU is α=Ω t+ φ ₀time, wherein Ω is stage body rotating speed, φ ₀for initial phase, reading acceleration measuring value is f _mx, f _my, after transforming to stage body coordinate system, the specific force in x-axis, y-axis 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, gyroscope is utilized to solve geographic latitude L and course angle Φ;

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

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

+(cosθcosφcosL+sinθsinL)Ω _ecosα

Due to: establish attitude angle θ, γ be 0 time, 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 ₀) be multiplied by above formula, then two ends integration can obtain:

$\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=\arctan (\hat{B}/\hat{A}),L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

Namely turntable can provide the outline value in orientation, geographical north after rotating a circle.

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

Linear equation is

Z _k＝H _kX _k-1+N _k

Wherein N _kfor measurement noise, H _kfor measurement matrix; Z _kfor measurement amount, X _k-1for amount to be estimated;

Observation equation is as follows:

$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

Employing least square method of recursion solves: [Δ φ Δ L E] ^t, E is gyroscope constant value drift;

(1) initialization X ₀, P ₀,

X ₀obtained by orientation, the geographical north outline value of rough north-seeking;

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

(3) recurrence calculation

$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)

?

[φ 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 the nonlinear equation about course angle and latitude, can not directly adopt least square method to solve, can first by nonlinear equation in initial value point Taylor expansion, obtain following formula, namely

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}(\mathrm{\φ}-{\mathrm{\φ}}_0)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}(L-L_0)+E$

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 is

Z _k＝H _kX _k-1+N _k

Employing least square method of recursion can in the hope of [Δ φ Δ L E] ^t, then by

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

Sought north value accurately.