CA1131053A - Low cost self aligning strapdown attitude and heading reference system - Google Patents

Abstract

LOW COST SELF ALIGNING STRAPDOWN ATTITUDE AND
HEADING REFERENCE SYSTEM
ABSTRACT OF THE DISCLOSURE
Apparatus including two two-degree-of-freedom gyroscopes and at least two accelerometers mounted on a turn-table on a vehicle. The turntable can be positioned about a vehicle vertical axis in one or the other of two positions 180 degrees apart for alignment. After alignment, the turntable is caged into its zero degree position, and the instruments thereafter operate in a strap-down mode with the yaw, roll and pitch angles of the vehicle computed by computer mechanisms which are sensitive to signals from the gyroscopes and accelerometers.

Description

FXELD. OF THE I~VENTIO~
This invention pertains to automatic heading reference apparatus utilizing gyroscopes and accelerometers strapped down to a supporting vehicle and computing means to produce signals in earth-fixed coordinatesO

` . BACKGROU~D OF THE INVE~TIO~
Previously known reference apparatus require an . . external heading reference, usually magnetic, to establish : and maintain heading, the accuracy heing limited to that of the external source. Prior art includes:
(1) A platform is supported on gimbals relative to the vehicle, and the platform is held locally level by signals from gyroscopes and accelerometers-.
. . ... - . -_ _ _ __- ' ' ' ~'~

~ ' ' ' ' . ' -: . :' :

.

31~53 (~.) A xi.n~le ~Jy~oscope is usc~cl and is suspcllded on giml:)al.s wit~l its spi.n a~ls vert:LcaL or horizonia:~. Lt is he~ld in place, and i-ts Outpllt sigllals are used to produce usable si~nals.
(3) Angular rates are supplied indixec-tly through gimbal resolvers.
sRIEF SU~lMARY OF THE INVENTION
. sroadly speaking the present invention provides a method for determinlng the initial angu].ar orientation of a Cartesi.an set oE rate sensor and acce].erome-ter axes relative .;
-to a set oE reEerence-axes and for determining the blas errors :':
of the ra-te sensors and acceleromete.rs, comprising: positioning ~;
the sensing axes of the ra-te sensors and accelerometers i.n a fi.rst precle-iermined position desiynated zero degrees and r~ad:i.ng the outputs of -the ra-te sensors and accelerometers;
turning the rate se~isors and accelerometers ].80 de~rees about one instrument axis; re-reading -the output signal.s of the xate sensors and-accelexometexs; adding and subtxacting the output signals i.n l:ile 180 degree pos:ition -to and from the signa].s .in ~; 20 -the zero degree position to ob-taln sum and diff2rence signals ~ whlch:are related -to -t'ne angles between the two sets of :~
`:~ coordi.nates and the bias errors and scale factors of the rate sensors and acce].erometexs.
~ The above method may be carr.ied out by way of a :; heading and attitude xeference system fox use i.n a vehicle having pitch, rol]. and yaw axes designed x, y and z, respec-tively, comprising: at least two angular rate sensoxs mounted on the ~: vehicle wi.th thei.x sensing axes parallel to x, y and z; a-t . : least two accelerometers mounted on the vehicle wi-ih -theix _~

:..
.: ~ ; . . . ~, ~ :
.

1~1L3~(~53 sensirlcJ axes para~ k?1 to x and y; means for init:i.all.y storing si.gna]s frorii .. the rate sensors and accel.erometers when the vehicle is suhstantiall.y stati.onary; means for turni.ng the angular rate sensors dnd the acceLerometers ~' - L80 degrees about the z axis; means :Eor combini.ng si.gnals from the rate sensors '; and accelerome-ters after their turnlng 180 degress about the z axi.s wi.th the . stored signals to produce sum and di.fference signals; and means for combining the sum and diiference signals to produce accelerometer and rate sensor initia] bias signals, rate sensor scale fact,or signals, and signals indicative of the initi.a] a-ttit,ude angles between the x, y and z axes and a reference set of coordinates.
~, The apparatus of this .invention uses a turntabl.e which is pivoted ::
, for rotation about the yaw or azimuth "Z" axis in the vehicle. The -turntable is motor driven between a predetermi.ned zero degree position and a 1.80 degree ~'~ position by a motor and gear d:rive. ~et:ents at the zero arld 130 degree ~ ' posi.tions prec.ise].y position the turntab].e. ..
~: ~ Positi.oned UpOIl the turntab1.e are the rate sensors r which may be two-degree-oE--freedom gyroscopes, alld the al, least two accelerometers. '~
The gyroscopes are aligned to c~enerate angular rate signal.s about x and y axes no.rmal to the z axi.s and about the æ axis. The accelerometers are 20~ aligned to rneasure acce].eration in the direction of the ~ and y axes.
~ :
Optionally a thi.rd accelerometer measures acceleratioll al.ong tlle z axis.
Gyroscope biasing errors and the initial tilt of the gyroscopes re].ative to gravi.ty are Eirst determined by measuring the outputs of the sensors of the gyroscopes and the accelerometers first in a zero, then i.n a 180 degree rotation, wi.th the turntable Eirst in the zero then i.n the 1~0 degree pOS:itiOII.

-3~-:: '. ~ : :
. , - , ::

; : : : : ~ .
.: :' ' .. ..
- :: . : . , . , -.: ": . :' , :: ., , . :

1~ ~
~3~L~5;~
. ,, , . .:
. .
After initial alignment, the outputs of the gyroscope

2 and accelerometer sensors are delivered to computing means to subtract out errors in the signals and to resolve the signals into earth coordinates. .
The resolved signals may then be used either by an :
operator or an autopilot to control a vehicle such as a. :
. . q helicopter, airplane, tank or truck.
8 It is therefore an object of this invention to produce..
9 signals which are measures of angular rate and angular position 10 - of a vehicle relative to an earth-fixed set of rectangular .- 11 coordinates. . . . ;
. . . .
12 : . . It is a more specific object of this in~ention to :~
~ 13 achieve the above named objects where the arth fixed set of : 1~ coordinates are north-south, east-west and vertical~ -Other objects will become apparent from the following 16 description taken together with the accompanying drawlngs.
~ . 17 . . . It is also an object of this invention to supply an .~ lB accurate self aligning attitude and heading reference system , 19 at low cost, reg`ardless of initial starting temperature.
`~ 20. . ~ ~ .
21 . . ` .. . .. `. . . . . - ~ `

23 . . .
; 2~ . .
. 25 . . . :~
26 . .
____ .
27 - -4- .. , 28 ____ ~~~ ~

~.: , .: , . . . . ..

~3~ 5;3~

13~IEF D135CRIPTION OF THE DR~WINGS
2 Figure 1 shows a plan view of a turntable mounted for rotation about one axis relative to a vehicle and the gyroscopes, accelerometers and temperature sensor mounted 5 thereon; -6 Figure 2Ais a schematic diagram of an alternative 7. embodiment of the invention; . ' .
, 8 Figure 2B. is a schematic diagram of a preferred : embodiment of the invention;
10 Figures 3A, 3B, 3C show a schematic rotation of .
11 coordinates through a set of Euler angles, 12 ' Figure 4 is a block diagram of a timer used in this 13 invention during àligl~ent, 14 'Figure 5 is a block diagram of means for producing .initial bias signals and acceleration signals for the x and y .
16 .accelerometers, 17 , Figure 6 is a block diagram of means Eor producing 18 initial ~ and ~ signals;
19 Figure 7 is a block diagram of means for producin~
20 an initial gyroscope biasing signal for one gyroscope, .
21 Figure 8 is a means for producing a scale factor 22 signal for th,e z,axis sensor on one of the gyroscopes, 23 Figure 9 is a means for producing sum and difference 24 signals for the two gyroscopes in di~ferent turntable positions 25 during alignment; ' : 2~ . , , ' , .
. 27 ___ 5 -____ .,, ~ ' 13iO~3 1 ¦ Figure 10 is a means for producing a signal of the 2 ¦ ~alue o ~ during alignment, 5 ¦ Figure 11 is a means for producing a ~ompensated ¦ signal of the angular rate signal about the x or y axis;
5 ¦ . Fisure 12 is a means for producing a compensated ¦ signal of the acceleration of the vehicle along its x or y 7 ¦ axis:
8 ¦ Figure 13 is a means for producing updated signals `
9 ¦ for interchanglng signals between vehicle oriented coordinates 10 ¦ and earth-fixed coordinates, 11 ¦ Figure 14 is a mechani2ation for pr~ducing compensated 12 ¦ ~ , ~ and Y signals using the apparatus of Figure 13, and 13 ¦ Figure 15 is a block diagram of the cut-out logic 14 ¦ of ~'igure 14. . - ::
15 1 .

17 1 ` `
18 I . . . .
19 ~ '`' ' .
. ' :~
21 .
22 . .
23 . . .
24 . ` .
.
,~ , ' . ,, .
27 ____ -6- `

:,:

11310~3 ~1 1 1 ¦ DETAILED DESCRIPTION OF THE I~VENTION
21 The apparatus of the invention comprises two two-axes 31 angular rate sensors 10, 12 such as two-degree-of-freedom 41 syroscopes, each for generating angular rate signals which are 5¦ measures of angular rate about two perpendicular ax~sO At least .
~¦ two, and preferably three, linèal acceleration measuring devices, 7 ¦ 16, 18, 20 such as accelerometers for senerating signals which 8¦ are measures of lineal acceleration, are positioned with their 9 ¦ sensing axes forming an orthogonal set of axes. The rate Q ¦ sensors 10, 12 and accelerometers 16, 18, 20 are fixedly mounted 11 ¦ on a turntable 22 having a rotation axis 24~ The accelerometers 12 ¦ 16, 18, 20 are positioned with the sensing.a~is of accelerometer 13 1 20 defining a axis parallel to the axis 24~ The sensing axes 1~ ~ of each of the.angular rate sensors 10, 12 are parallel to the sensing axes of accelerometers 16, 18 and 200 .
16 The turntable 22 can be turnea about the axis 24 17 relative to the supporting vehicle 30. The periphery of the : :~
18 turntable 22 has gear teeth 32 thereon to engage a spur gear 34 19 which is driven by a motor 33~ Detents 36, 38 are positioned 2~ on op~osite ends of a diameter of the turntable 22. A flexible 21 pawl 40 has a roller 42 on the end thereof to roll on the 22 periphery of the turntable 22 and to fit into the V-shaped 23 detents 36, 38 to hold the turntable in each o~ two precisely 24 aligned positions 180 degrees apart. A raised tab 44 engages :~
micro-switches 46, 48 to stop the drive motor 33 of 26 spur gear 34 when the roller 42 engages the detents 36, 38. .
27 ____. ... 7 2~ ~ . .

, .. ~ . .

~. :

Il. . ' I' ~3~3~L~S3 1 A right-handed set o~ orthogonal axes is defined 2 in the vehicle 30 with the z axis coinciding with axis 24.

3 Customarily the x, y and z axes are called the pitch, roll and yaw axes of the vehicle. ~:
. The sensing axes of the gyroscopes 10, 12 and the : ~ accelerometers 16, 18, 20 are initial'y oriented as shown in 7 Figure ~B.
8 The sensing axis ~ZlO is parallel to the z axis. . .
Wlth the turntable 22 as shown, its spin axis SAlo and its ~0 ~ther sensing axis ~x10 are parallel to the y and x axes 11 respectively. - .
` 12 .
: ~3 : ~
14 . .
15 The sensing axis ~Z12 is parallel to the z ax s.
lô With the turntable 22 as shown, its other sensing axis ~Y12 .
:~ 17 and its spin axis SA12 are paral.lel to the y and x axes, ~: . 18 respectively~ .
19 In an alternative embodiment (shown in Figure 2A), 20 the spin axis SA12 is parallel to the z axis With the turn- ::~
21 table in a first position, its sensing axes ~xl2 ana ~Y12 ar~
22 parallel to the x and y axes, respectively .245 .
2~ .
- 27 ____ -8-2a ----~ :

Il 1131053 1 With the turn-table positioned as shown in Figures 2B
2 and 2A, the accelerometer 16 senses acceleration Ax in the 3 x direction, and the accelerometer 12 senses acceleration Ay n the y direction, and the accelerometer 20 senses acceleration Az in the z direction.
6 A ~ symbol over another symboi indicates that the 7 othex symbol represents a measured signal.
8 The x, y, z or pitch, roll and yaw axes of the vehicle 3 do not, in general, coincide with the E-W, N-S and local vertical directions. To change measured signals in the vehicle 11 coordinate system into components in a se~ond set o~ coordinates, ¦
12 one may transform through a set of Euler angles. The Euler 13 angle transformation is shown in ~igures 3A,3B and 3C. The 14 first Euler angle rotation of coordinates, shot~n in Figure 3A, is about the z axls through an angle ~ to define a first 16 intermediate right handed set of orthogonal coordinates x', y' 17 and z. The second Euler angle rotation of coordinates, shown 18 in Fi~ure 3B, is about the x' axls through an angle ~ to define 1~ a second intermediate right handed set of orthogonal coordinates xl, y", z"~ The third Euler angle rotation of coordinates, 21 shown in Figure 3C, is about the y" axis through an angle ~4 27 ___ - _9_ 28 ____ 1 to define the right handed set of orthogonal coordinates x"' , 2 y", z"' which correspond to the directi.ons, east, north and vertical~

4 The transformation between the x, y, z axes and the

5 x'll, y-', z"~ axes is a matrix [ T j made up of sines and cosines

6 of ~ and ~.

7 Thus:

19 Il [~]= [p ~ -1 [~
11 Where ~x' ~y~z are si~nals which are measured by either rate 12 sensor 10 or 12. ~ H is the horizontal, north~directed, y"' 13 component of the earth's rotation rate, and~fLv is the locally 14 vertical z"' component o the earth's rotation rate. Equation (1) may also be written:

7 ~ [J~H ¦ I P ] ~ L~) 19 Similarly the measured acceleratiQns ax, ~y~ ~z may be transformed from one coordinate system to the other.

22 ¦ [ ~ ~ I az ~ (2) ¦
24 It is convenient to express the accelerations in units o "g", the acceleration of gravity~

27 ~~~~ ~ -10-.'' ' :

` ~ 53 1 When the equations are tr~s~ormed with the turntable 2 22 in its zero degree position and the supporting vehicle 30 . 3 at rest ~. 4 - AX = -cos p Sin ~ ~ ~x (3) 5 . ~y .= Sin ~ ~ B (4~
= Cos~ Cos ~~ ~ B . (5) . 7 where B~, B~, ~ are the bias errors of the accelerometer.
When the turntable is turned to its 180 degree position, and the vehicle 30 still at rest, . x 180 Cos ~ Sin ~ ~ B - (6) 11 . y 180 = -Sin ~ ~ By . (7j .
12 - . az 180 ~ Cos ~ ~os ~' ~ B (8) 15Taking the diferences of equations (3) and (6) and . .
- 14 of equations (4) and (73, .
~ ~x = -2 Cos p Sin ~' (9) .
16 . ~y = 2 Sin ~ ~10) ..
Taking the sums of equations (3) and ~6~ and equations 18 (4) and (7), .
19 ax - 2 x (11) 20 . ~ay = 2 By (12) 21 One may then determine ~ and ~ from equ~tions (9) and ~103, 2~~ = Sin -1 ( ~ 2 - ~ (13) 24 ~ ~ Sin ~ L ) (14) 25 .
~8 .

28 __~
. . ~ Y' .

~ ~ ' ,ll . I
~L~3~3 .

i From equations (11) and (12), ~x and ~y may be determined X ~ (15) 4 By~ 63 6 From equations (5~ or (8) ~z ~ ~ (or C~ ) -Cos ~ Cos~ - (173

8 Where the.~f symbol means an estimated signal.
With the turntable 22 in its zero degree position, the ~yroscopes 10 and 12 measure, 11 ~\ .
- (Cos ~ Sin ~ Sin~ ~ Sin ~ Cos ~ (18) 13 - (Cos ~ Sin Qx~o Cos ~ Cos~ -Ax Sin ~ Cos ~ Sin 1~ . ~ ~x ' - ' . ;
17 ~ CO~ ~ Cos ~ ) ~ H + ~Sin ~ L V +My Sin~ ~1 ~ Qy Cos ~ Cos ~ -Ay Sin ~ Cos ~ Sin ~ ~YJæ
21 ~ = (Sin ~ Sin ~ -Cos ~ Sin~ Cos ~ ~ H (20 224 ~ (Cos~ Cos ~ )_nLV + Mz C~s ~ Cos ~ Qz Cos~ Sin ~ ~Az Sin ~ Cos ~ Cos~
2~ - ~ ~ z~0 __ ~___ ,' , : ' ~L~3~53 l ~ - (Sin ~ Sin ~ -Cos ~ Sin ~ Cos ~ ) Q~ (21 (Cos~ Cos ~ Mz Cos ~ Cos 3 ~ Qz Sin~ -Az Cos ~. Sin ~ CQS ~

S ~ ~, z ' :
6 Where M is a mass unbalance drift coefficient for mass unbalance 7 of the designated gyroscope 10 or 12 in the direction of the 8 designated axis. .
A is the anisoelastic dri~t coefficient due to the anisoelasticity of the designated gyroscope 10 or 12 about the-designated axis;
11 .
12 . Q is the quadrature drift coefficient which occurs 13 only ~n a dry tuned rotor gyroscope due to mass unbalance :~
14 coupling into the quadrature axis; .
~ is the non-acceleration sensitive dri~t error in the 16 gyroscope about the designated axis~
Wi.th the turntable 22 in its 180 degree position, 17 ~ (Cos ~ Sin ~ Sin ~ ~ Sin ~ Cos ~ H (22`
+ (Cos ~ Sin ~ L~Lv + Mx ~ Cos~ Sin~

+ Qx Cos~ Cos ~ -Ax Sin ~ Cos~ Sin 21 . ~ ~ x :~
22 ~y ~ -(Cos ~ Cos~ H ~(Sin ~ ) ~ v Y z ~ (23) 23 . ~ Qy Cos ~ Cos ~ -Ay Sin ~ Cos ~ Sin ~
24 + ~ y ~ . .
..
2~ .
-27 __ _ -13-28 _~__ Il .
I' " ~3~ i;3 .

. . .
1 ~ ~ (Sin ~ Sin ~ -Cos ~ Sin ~ Cos ~ H ~2~-`
2 J8~ + (Cos~ Cos~ ) ~ + Mz Cos ~ Cos -~ Qz Cos ~ Sin ~ -Az Sin ~ Cos~ Cos~
4 ~ ~ zJO

~ ~0~ - ~Sin ~ Sin ~ -Cos ~ Sin~ Cos ~ H (25 7 ~ ~ (Cos~ Cos ~ ) ~ v Mz Cos ~ Cos ~ .
8 - Qz Sin~ +Az Cos ~ Sin ~ Cos ~

~ ~z . ~' ., .
11 ~Taking the differences and sums of equations (18) . 12 and (22) and equa~tions (19) and (23), 13 ~ ~ = 2 (Cos ~ Sin ~ Sin ~ + Sin ~ Cos ~ ~ H (26, 14 ~ (cos~ sin ~ )~L v ~ 2~o COS~ si ~ : ~

16 ~ XJo 2QX~o C05~ Cos~ - 2Ax Sin~ Cos ~ Sin 1 (27 ; -17 ~ 2~o ~ :
18 . . .
= 2 (Cos ~ Cos ~ ) ~ H ~ 2 ~Sin~ ) ~ v (28,~
+ 2 My Sin~
2 ~ ~ I = 2 Qy ~ Cos ~ Cos y - 7 Ay Sin ~ Cos ~ Sln ~ 29, 23 ~ 2 ~Y~2 .
~ ., z7 ____ - -14- ~- :
28 ____ ~7 ;

.

~ 1310s3 1 Combining equations (26) and (28) in ~ ( ~ + Myl~J ] I ¦

~ It should be noted that the gyroscope drift coefficients Q and 7 ~ do not appear in equation (30) because they have cancelled 8 in the difference equations (26) and (28). Also note that - Mx and My ~ are for different gyroscopes, and errors in knowledge of the values are expected to be uncorrelated so ll that their effects on the estimate of ~ is in a Root Sum - 12 Squared sense rather than a direct sense.
13 The drift parameters of the gyroscopes may be 14 estimated from equations t27) and (29). ~ .
Take the :sum of equations (20) and t24) and of 16 .equations (21) and (25).
: 17 . . ~ ~ = 2 (Sin ~ Sin ~ - ~os ~ Sin ~ Cos ~ ~H 31`-~
~8 + 2 (Cos~ Cos ~ v ~ 2 Mz Cos~ Cos 19 ~ 2 ~z~ -. .
21 ~ ~ = 2 (Sin ~ Sin y - Cos ~ Sin ~ Cos Y ) ~ 32) 23 l~2 (Cos~ Cos ~ ~ v ~ 2 Mz Cos ~ Cos - 2~ .Z12 .
2~ .
-27 ____ 28 ____ -15-: ~ , , . ~L~IL3~63S3 1 From which estimates of the dri~t terms can be made.

3 (~*z ~Mz Cos~ Cos~ 2J - (Sin~ Si.n~ -Cos~ Sin~ Cos ~)~ (33' ~, _ (Cos ~ Cos Y )~v 6 ~ . ' ., 7 ~z +Mz Cos~ CosY) = ~ tSin~ Sin~ -Cos ~Sin~ Cos ~)~ (34'.
8' ,- _ (Cos ~ CoS ~ v :~, . 9 . . ':
'10 . .
11 Since the~instruments are rotated through a known angle of .
12 180 degrees about the z axis, the scale factor forC0z and 15 ZJ2 may be eStimated, .
14 ~ T .
15 . ~ K~lo ~zlo dt = n ( 35,.
16. . .
17 ~zjo ~z/o ~t) due to earth rate ~L~T~t) (36 i8 and~ (t) is the rate of rotation of the turntable 22 'TT
19 abo,ut th,e,,z axis.. during rotat,ion from zero to 180 degrees. - :.
Assum ~ TT~t~ is a constant~ ~by making ~ear 34 21 turn at a constant speed~.
22 .
22~ Kz ~ ~z dt - Kz~o ~ QTT¦ ( 37 ~ ~

. .
2~ l ' ~7 ____ _ -16- :
28 . .

~ , i., , ~ ~ , '. ' .;.
. .

.

1 ¦ and the scale factor may be estimated 21 ~J

5 ¦ ~ ~ TT (38 ; ~ ¦ Similarly 8 ~ ~r ~ ( 39,

9 ¦~ote that the preferred orientation also provides

10 ¦ two sources o~ 1l azimuth" body rate ( ~) measurements about the

11 ¦ z axis, thereby permitting averaging or optimally mixing or

12 1 even selecting to improve performance. This is important

13 ¦ because the azimuth angle (~) is not readily bounded as the

14 ¦ pitch and roll angles may be by use o~ the x and y accelero-

15 ¦ meters 16 and 18.

16 1 . Alignment with the alternate gyroscope orientation

17 ¦ o~ Figure 2A is now considered.
1~ ¦ The acceleration measurements are the same as in 19 ¦ equations (3) through;~8), and the difference and sum of 20 ¦ equations (~) through (12) are the s~ne. Equations (13) through 21 ¦ (17) are also the same.
22 ¦ For the zero degree position of the turntable 22, 23 ~ = (Cos ~ Sin ~ Sin ~ ~ Sin ~ Cos ~ )J~H (40 24 I _ (Cos~ Sin ~ )lLv ~ Mx Co~ Sin ~
+ Qx Sin~ ~ Ax Sin ~ Cos ~ Cos~ + ~ x 27 ___- -17-28 ____ :- .. .
,., , : ': ' . ', , ~ ~L3~3 ~1 1 1 ¦ ~y~ = ~Cos ~ Cos ~ )~*~ + (Sin ~ v + My Sin ~ (41:
3 I - Qy Cos~ Sin ~ -Ay Cos~ Sin d~ cos~ + ~y I The equations for~ and Q are the same as equations (18) ¦ and (20), respectively.
6 ¦ When the turntable 22 is turned to the 180 degree ¦ positio~ , ¦ ~/~ - ~(Cos ~ Sin~ Sin ~ + Sin ~ Cos ~ H (42 9 I + (Cos~ Sin ~ )QV + Mx ~ Cos ~ Sin~
10 ¦ . ~ - Q~ Sin~ - A Sin ~ Cos ~ Cos~ +~ x 11 1 ~ - . . j~
12 ¦ ~ = -(Cos,~ Cos~ )~ H ~ (Sin ~ ~ ~ v . y/~ ~ (4 13 ¦ + Qy Cos ~ Sin ~ + Ay Cos ~ Sin ~ Cos ~ -; 1 ~ ~ Yl~
16 .The equations for ~ and ~ are the same as 17 equations (22) and (24), respectively.

18 . Forming the sum and difference equations ~rom equations ~40) and (42) and equations (41) and (43~.
~ GQ~ - 2 (Cos ~ Sin ~ Sin ~+ Sin ~ Cos ~ )lLH . (4a : 21 - 2 (Sin ~ Sin ~ ~ v ~ 2 Mx Cos~ Sin 22 QxJ~ Sin ~ ~ 2 Ax ~ Sin~ Cos ~ Cos ~

224 ~ 2 ~ (45`;

2~
~7 ____ ~8 ---- ~18-.

ll . I I

I ~L3~S3 1 ¦ ~y = 2 lCos ~ Cos ~ ~2 H ~ 2 (Sin ~ ) ~1 v ( G~
X ¦ . ~12 n ~ 2 Qy Cos ~ Sin y 3 I - 2 ~y Cos ~ Sin y Cos Y

5 I ~Y12 - 2 ~ Y12 ~7`
6 I . . ~ .
¦ Using equations t44~ and (46), an estimate of azimuth angle about the z axis is made:

9 ¦ X = Tan ~ - Tan~ Sin Y (~
10 I ~ Cos Y l , 11 ¦ 212 + Cos~ SinY (Qv~Mxl2) -Qxl2 Sin~ - xl2 Sin ~Cos ~CosYl 1 12 ¦ 2 Sin ~(Qv~ Y12) ~ Y12 Cos ~SinY ~ Y12 Cos ~Sin ~ CosYJ ¦
13.1 .. . .
14 ¦ Similarly yyroscope bias drift estimates may be made from .
15 ¦ equations (~$3 and t47) 16 1 ~xl2 = 212 .
8 ~ ~Y12 50 20 ¦ Equation (33) is also valid for this mechanization.
21 1 An estLmate for Kz can also be made as in e~uation ~2 I (39)~ .
23 ¦ Note from equation (481 that this second configuration 24 ¦ produces error terms which could become important during alignment when pitch and roll angles, ~ and y become significant (for 2~ I .
I ____ 27 ~ 19-Z8 . . ~-. ', , `

.- ~ .
, ~3~3 1 example, greater than six degrees). Also note that only one 2 gyroscope 10 measures ~, but two gyroscopes 10 and 12 measure ~ ~ . Hence in this embodiment errors in ~ cannot be reduced by 4 combining or selecting, but errors in ~ can be reduced by combining. However, this redundancy feature is not important 6 in ~ because the accelerometers can produce an independent 7 measure of~ ~
8 The abo~e equations and description implement the alignment of the apparatus of this invention.
It is likely, in a preferred embodiment of the invention, 11 that a general purpose digital computer or processor would be 12 used to ~eceive the output signals of the gyroscopes and the 13 accelerometer. Those output signals would either be in digital 14 form or be converted to digital form. The computer would then produce output signals, probably in digital form.
16 For purposes of explanation the computer functions 17 have been shown in FIGS. 4-15 in block form. One may consider 1~ the blocks to be portions of a general purpose computer,

19 software for a computer, or an analog computer.
FIG. 4 shows a timer 59 which may be started with a 21 start signal or energized from a switch. Initially, the timer 22 should enable the turntable motor to place the turntable 22 in 23 its zero degree position and enable the computers of FIGS 5 2~ and 9 to store the output si~nals of the gyros and accelerometers in the storage memories 60, 62. The timer 59 then sends a 26 signal to the turntable motor to index the turntable 22 into 27 ~ 20-28 ____ ~9 ;~' , .

11 ,.

~3~!L()S3 ., 1 its 180 degree position. The tlmer 59 then enables the sto~age 2 memories 60, 62 to deliver their stored signals to the various summers 64, 66, 68, 70, 72, 74, 76, 78, 80, 82 ~or adding or 4 subtracting the outputs of the sensors on the gyroscopes 10, 12 and the accelerometers 16, 18, 20. The sum and difference signals of FIGo 5 are then stored in storage memories 84, 86, 88, 90 for future use. The outputs of summers 72, 74, 76, 78, 8 80, 82, if desired, also may be entered into storage (not shown).
9 ~ Equations (13) and (17~ are mechanized in FIG. 6 The ~/2 input is obtained from FIG. 5. The az~ is obtained 11 from the ~emory 60 of FI~. 5. The ~ signal is delivered by 12 an adder 92 receiving Y/2 signals from memory 90 and sin ~
13 ¦ signals from sin generator 94. The output of the~adder 92 is 1~ ¦ then integrated by integrator 96 to produce the ~ signal.
16 ¦ Sin ~ and cos ~ signals are then produced by sin generator 94 16 I and cos generator 98.
17 ¦ X/2 is divided by cos ~ through a divider 100, 18 ¦ and the resultant signal is delivere~d to an adder 102. The 19 ¦ adder 102 also receives input from sin gen~rator 104. The -

20 ¦ output of adder 102 is integrated by integrator 106 to produce I ,v ~

21 ¦ an output signal ~ . The ~ signal is delivered to sin generator

22 104 and cos generator 108 to produce sin ~ and cos ~ signals.
Cos generators 98 and integrator 108 are connected 2~ into multiplier 110 with adder signaL 112. The output signal 2~ ~ is stored by mèmory 114.

27 ~21-28 ___ _ ~-``~1 . I

1 ¦ In FI~. 7 the cos ~, sin ~ and cos ~ ou~puts of Figs. 6 and 10 are 2 ,I connected into multiplier 116 whose output is connected into 3 I adder 118. The sin ~ input to multiplier 120 is obtained ¦ from sin generator 122 of FIG. 10. The sin ~ input to 6 ¦ multiplier 120 is from sin generator 104 of FIG. 6. The output 6 of multiplier 120 is subtracted in summer 118.
For a given latitude, known a priori, at the position 3 of alignment, one can generate a signal proportional to the 9 horizontal component of earth's rotation at that latitude. --The signal is then delivered to multiplier 124 where it is 11 multiplied by the output of adder 118. The adder 126 receives 12 the output of muLitplier 124, the output of multiplier 128 and -13 the output of adder 80 of FIG. 9 to mechanize equation 33. The 14 cos ~ and cos ~ inputs to mulitiplier 128 are obtained from FIG. 60 The Qy input to multiplier 128 is the calculated vertical 16 component of earth's rotatlon for the particular Xnown latitude 17 where the alignment occurs.
18 Equation 34 may be mechanized the same as equation 33~
19 FIG. 8 mechanizes equation ~38), and equation (39) may æo he mechanized in a similar fashion. The output of adder 80 of 21 FI~. 9 is delivered to an adder 130. A signal which is a ; 22 measure of the known angular velocity of turntable 22 is added ~3 into adder 130. The output of adder 130 is divided into ~ by 2~ the divider 132 to produce a ~ signal.
2~ FIG. 10 mechanizes equation 30. The sin ~, cos ~, 26 sin b', cos ~' inputs are from FIG. 6. The Q v input is known 27 ~~~~~ -22-2~ 1 -____ :

. . .. .
.
. ~ . .

~L131053 1 ¦ from knowledge of the local lati-tude at the initial calibration 2 ¦ e Mx10~ ~Y12~ inputs are known t~onstants of the 3 ~ gyroscopes. The YI2/2 and the XlO/2 inputs are ~rom FIG. 9. The sin ~ , cos ~ terms are delivered to 5 1 divider 134 to produce a tan ~ signal. The tan ~ signal is 6 ¦ ~multiplied in multiplier 136 by the sin y signal. The output 7 of mulitplier 136 is delivered to adder 138.
8 The Mx and Q~ signals are added in addt-~r 140 and 9 ;the sum signal is delivered to multiplier 142 where i~ is multiplied by cos ~ and sln~ ~ . The output o~ multiplier 14 11 is added in adder 144 to ~ X~72. The output of adder 14~ is 12 delivered to divider 146. -13 The ~y and Qv sit~lals are added in adder 147 and ~4 the sum signal is delivered to multiplier 150 where it is ~ multiplied by sin~ . The output of multiplier 150 is added, 16 in adder 152, to~Y12~2, and the sum oit~nal is delivered to divider 146. The 17 ou~put of divider 146 is delivered to adder 138 and h~ce to multiplier 148. A
18 ¦ aos ~ over cos y signal is produced in divider 150. The signal from 150 is de-19 livered to multiplier 148~ and the output of multiplierl48is delivered to adder A tan ~ signal is delivered by tan generator 154 to adder 152.
21 The output of adder 152 is integrated by integrator 156 to 22 produce an ~ signal. Tan ~ and sin ~ signals are produced ~3 by tan generator 154 and Sin generator 122, respectively~
24 FIGS. 11-15 are mechanizations of the heading reference 2~ of this invention in its operative mode.
~B FIG. 11 shows a typical computer which continuously ~ removes various s, Q, A and M bias errors from the output .

.
` ll~L3~5i3 1 signals of the gyroscopes. uJ is an output from a gyroscope 10 2 sensor. SF P
3 scale factor which is known. The temperature sensor 50 adjacentto¦
4 the gyroscopes and accelerometers produces a temperature signal wl1ich modifies the scale factor in a kno~l way. These signals ~ are delivered to multiplier 160, and the produced signal is Y delivered to adder 162.
A temperature sensitive correction factor is delivered 9 from multiplier 164 to adder 162. The ~ is delivered from a circuit substantially identical to FIG. 12 but with ax and Bx 11 inputs. Mx is known, and the temperature signal comes from 12 sensor 50.- -13 The multiplier 166 receives known signals Ax ~ Qx 14 ~x ~ which are ~nown contants of gyroscope 10. The a and 1~ ay signals come from circuits like FIG. 12. The ~ and ~ signals are updated pitch and roll angle signals from FIG. 14.
17 Multiplier 166 has a ~lultiplying factor which is temperature 18 sensitive in a known function o temperature. -19 The initial bias corr~ctions of equation ~27) during ali~nment, from FIG. 9, are added in adder 168 from the 21 updated signal output of multiplier 166. Storage means (not 22 shown) may be needed to hold the signal of FIGo 9~

23 The output of addex 168 is subtracted in adder 162 to produce ~x signal which is the sum of the component of earth rate about the x axis and the relative angular rate about the x 26 axis.
2q ____- -24-28 ~____ ' 1~3~
~ . .' 1 ¦ A ~ircuit similar to that o~ FIG. 11 also may be used 2 ¦ to calculate ~ and ~ .
3 ¦ There is a circuit like FIG. 12 for each x, y ¦ accelerometer channel. The accelerometer signal is delivered 5 ¦ to a temperature sensitive multiplier 170. The scale factor ~ ¦ KSF is known, and temperature s1gnals are received from sensor 50.
7 ¦ Bias signals such as By are delivered from FIG. 5 to a temperature 8 ¦ sensitive multiplier which receives termperature signals from 9 ¦ sensor 50. The outputs of multipliers 170, 172 are added in adder 173 to produce an ay signal which has a component due to 11 gravity unless ~= 0, plus a true acceleration signal. I ;
12 FIGS. 1'3 and 15 are portions of FIG~ 14.
13 In FIG. 13, the known latitude signal (which may be 14 obtained by any technique) is delivered to sin and cosine 16 generators 174 and 176. An Q signal proportional to earth ~ rotation is also delive'red to generators 174, 176~ The n sin 17 and Q cos ~` outputs of generator 174, 176 are delivered to 18 sin, cos matrix mechanization 178. ~le mechanization ]78 ¦
19 mechanizes ,three equations having sines and cosines therein and represented by a matrix [P] . The matrix [P] terms are 2~ delivered from time dela~ block 190. The outputs of mechanization ' 22 178 are n X~ n y and n z signals, the components of earth's 23 rotation about axes x, y, z~ The outputs of 178 are delivered 2~ to adders 182, 184, 186 where they are subtracted out of the sensed signals w x~ ~Jyl ~z from,E'IG. 11. The corrected signals 2~ are delivered to the matrix updating mc-chanis-n 188 which updates ~7 ~ 25-. . ,. ; ~

:

lQ~

1 I the info~mation in matrix mechanization blocks 178 and 180. The 2 il~ block 180 ~ech~nizes matrix e~uation [~] and block 178 mechanizes [P]T' 4 ¦ The up-dating block 188 receives [P] signals from 5 ¦ block 190 and performs matrix multiplication as indicated in ¦ block 188 to produce an updating increment for each term of r ¦ [P] and [p~r The incremental output of block 188 is added 8 ¦ to [P] in adder 192. The output.of adder 192 is time delayed by 190, and the updated matrix terms are delivered to blocks 10 1 178, 180 and 188. .
11 ¦ Blocks 194, 196, 198 of F.igure 14 together form FIG. 13. The 12 ¦ adders 182,-184, 186 correspond to the same adders in FIG. 13, .:
13 ¦ and the outputs ~ ffrom the Euler angle resolver 206 of 14 ¦ FIG. 13 correspond to the same outputs in FIG. 14.
15 I ~dditional feedback circuitry to adders 182, 184 to 16 ¦ stabilize the mechanization is sho~ in FIG. 14. The feedback 17 ¦ loops, in turn may he cut out in accordance ~v-ith logic built 18 into blocks 200, 202~ That logic is shown in FIG. 15.
19 . In FIG. 14 the ~ signal is delivered to the cut out logic 200, and the ~ signal is delivered to logic 202~
21 Signal ay has subtracted therefrom in adder 210 a 22 gravity cDmponent g sin ~ from sin generator 212 which, in turn, 23 receives a ~ signal from block 194. The output of adder 210 ~ is delivered to logic 200 and to adder 214. The output of adder 214 is integrated by integrator 216 and a part of the 26 output signal is ~ed bacX through scaler 218 to adder 214~ The ~7 _____ -26-2$ ____ ~ 3~L~S3 1 output of inteyrator 216 is further scaled by scaler 222 and fed back through cutout switch 220 to adder 182.
The ~ output of 194 is delivered through a cos . ~ generator 230 to a multiplier 232~ The ~ output of block 196 is delivered th~ough (g times) sin generabor 234 to ~he m~ltiplier 232.
. The output o~ multiplier 232 is a gravity term ~hich is : 7 subtracted in adder 236 from the signal a . The output of :...... . adder 236, labeled ~ax, is delivered to cut out logic 202 and 9 -to adder 238. The output of adder 238 is integrated by integrato~
240, and the output is scaIed by scaler 242 and fed back to adder 11 238. .
12 The outp~t of integrator 240 is also scaled by scaler 13 244 to deliver a scaled feedbac~ signal through cut out switch .
14 246 to adder 184. -15 rrhe wz10 and.~z12 outputs are added by adder 186 ~:
~ 16 to 2 ~, and ~he output is multiplied by 1/2 in multiplier 250 : 17 to produce an averaged signal w~ich is delivered to block 198, 18 .which is part of FIG. 13, to produce an ~ si~nal. .
19 The cut out logic is shown in FIGo 15. The symbols ~OL~ ~aXOLI~oL and ~ayOL are predetermined thresholds at which the various loops open.
22 The symbolS ~L~ ~ax~L~ ~CL YCL

24 at which the loops re-close, and they are slightly lower than the corresponding open loop thresholds to prevent relay 2~ chattering.
27 ____ 2~ :
~s ____ 1 In summary the apparatus of this invention is a heading and attitude reference unit which uses strapped down 3 gyroscopes and accelerometers together with accelerometers to generate accurate vehicle attitude and heading as well as vehicle angular rates.
6 - It should also be noted that the particular errors 7 due to M, Q, A and ~ are peculiar to dry tuned flexure ~ suspended rotor gyroscopes. Other ~inds of gyroscopes as ; well as other angular rate sensors could be used. For example, nuclear magnetic resonance gyroscopes and laser gyroscopes 11 could he used. Other kinds of gyroscopes and rate sensors 12 would, of course, have their own error sourees and those 13 errox sources could be identified by the initial sensing 14 with the turntable first in one position then turned 180 degrees.
- Although the invention has been described in detail 17 above, it is intended that the invention shall not be limited - 19 by that description alone but in combination with the appended claLms.

27 ~~~~ -28-~A ____ ~,

Claims (12)

THE EMBODIMENTS OF THE INVENTION IN WHICH AN EXCLUSIVE
PROPERTY OR PRIVILEGE IS CLAIMED ARE DEFINED AS FOLLOWS:

1. A heading and attitude reference system for use in a vehicle having a pitch, roll and yaw axes designed x, y and z, respectively, comprising:
at least two angular rate sensors mounted on said vehicle with their sensing axes parallel to x, y and z;
at least two acccelerometers mounted on said vehicle with their sensing axes parallel to x and y;
means for initially storing signals from said rate sensors and accelerometers when said vehicle is substantially stationary;
means for turning said angular rate sensors and said accelerometers 180 degrees about said z axis;
means for combining signals form said rate sensors and accelerometers after their turning 180 degrees about said z axis with said stored signals to produce sum and difference signals; and means for combining said sum and difference signals to produce accelerometer and rate sensor initial bias signals, rate sensor scale factor signals, and signals indicative of the initial attitude angles between said x, y and z axes and a reference set of coordinates.

2. Apparatus as recited in claim 1 in which said means for turning said angular rate sensors and said accelerometers 180 degrees about said z axis comprises a turntable mounted upon said vehicle with its turn axis parallel to said z axis and said turntable.

3. Apparatus as recited in Claim 2 in which said means for combining said sum and difference signals comprises:
means for producing a signal which is a measure of the initial pitch angle in response to the said difference signal of the said y axis accelerometer;
means for producing a signal which is a measure of the initial roll angle in response to said pitch angle signal and said difference signal of said x axis accelerometer;
means for producing a signal which is a measure of x axis accelerometer bias in response to said sum signal of said x axis accelerometer;
means for producing a signal which is a measure of y axis accelerometer bias in response to said sum signal of said y axis accelerometer:
means for producing a signal which is a measure of the initial drift parameters about the x axis of a first said rate sensor in response to said sum signal of rotation rate of said first rate sensor about the x axis and to signals which are a measure of earth rate;
means for producing a signal which is a measure of the initial drift parameters about the y axis of a second said rate sensor in response to said sum signal of that second said I rate sensor about the y axis and to said earth rate signals;

Claim 3 continued means for producing a signal which is a measure of the initial yaw angle in response to said roll and pitch angle signals, to said difference signals of the angular rate about the x axis of one said rate sensor and of the angular rate about the y axis of the other said rate sensor, to a signal which is a measure of a component of angular velocity due to earth's rotation, and to signals which are measures of the acceleration sensitive drift coefficient of both said rate sensors; and means for producing a signal which is a measure of the initial drift rate of said rate sensors about the z axis in response to said pitch, roll and yaw signals, to said component of earth's rate signal, and to said sum signals of the angular rate of each respective rate sensor about its z axis.

4. Apparatus as recited in claim 3 and further comprising means for producing a signal which is a measure of the scale factors about the z axis of said rate sensors in response to signals which measure the rotation of said turntable, and said sum signals about the z axis of rate sensors.

5. Apparatus as recited in claim 4 in which said signals are responsive to instrument temperature and further comprising means for generating a temperature signal and for modifying said other signals in response to said temperature signal.

6. Apparatus recited in claim 5 and further comprising means for storing said signals for use during the operative mode of said apparatus.

7. Apparatus as recited in claim 6 and further comprising:
means for continuously modifying the angular rate signal about said x axis from said first rate sensor to remove instrument drift rate errors;
means for continuously modifying the angular rate signal about said y axis from said second rate sensor to remove instrument drift rate errors;
means for resolving earth's rate signals into components about said x, y and z axes;
means for subtracting said earth's rate signal components from the respective modified angular rate signals;
means for producing direction signals which are measures of the rotation of said instruments about said reference coordinate axes including means responsive to said initial pitch, roll and yaw angle signals, continuously to update means for producing direction signals, and means for resolving said updated signals to produce updated pitch, roll and yaw angle signals.

8. Apparatus as recited in Claim 7 and further comprising feedback means for bounding errors in the pitch and roll angle signals.

9. Apparatus as recited in Claim 8 in which said bounding signals are pitch and roll signals obtained in response to said accelerometer signals.

10. Apparatus as recited in Claim 9 and further comprising logic means for selectively disconnecting and reconnecting said bounding signals in response to the magnitude of said pitch and roll angles and to vehicle acceleration signals.

11. Apparatus as recited in Claim 10 in which said bounding means is disconnected in said x channel in response to either the pitch angle signal or the y axis vehicle acceleration signal exceeding predetermined thresholds;
said bounding means is disconnected in said y channel in response to either the roll angle signal or the x axis vehicle acceleration signal exceeding predetermined thresholds;
said x channel bounding means is reconnected in response to both the pitch angle signal and the y axis vehicle acceleration signal being below predetermined thresholds; and said y channel bounding means is reconnected in response to both the roll signal and the x axis vehicle acceleration signal being below predetermined thresholds.

12. A method for determining the initial angular orientation of a Cartesian set of rate sensor and accelerometer axes relative to a set of reference axes and for determining the bias errors of said rate sensors and accelerometers, comprising:
positioning the sensing axes of said rate sensors and accelerometers in a first predetermined position designated zero degrees and reading the outputs of said gyroscopes and accelerometers:
turning said rate sensors and accelerometers 180 degrees about one instrument axis re-reading the output signals of said rate sensors and accelerometers;
adding and subtracting the output signals in the 180 degree position to and from said signals in the zero degree position to obtain sum and difference signals which are related to the angles between the two sets of coordinates and the bias errors and scale factors of the rate sensors and accelerometers.
.