Note: Descriptions are shown in the official language in which they were submitted.
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-.
. . ... - . -
_ _ _
--2--
__- ' ' '
~'~
~ '
' ' ' . ' -: . :' :
.
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
26
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-
28 --
.'' ' :
` ~ 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 .
27 -11-
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
27 -12-
__ ~___
,' , : '
~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., , ~ ~ ,
'. ' .;.
. .
.
1131053
1 ¦ and the scale factor may be estimated
21 ~J
5 ¦ ~ ~ TT (38
; ~ ¦ Similarly
6 1
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
12
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.
2G
27 ~21-
28 ___ _ ~-
``~1 . I
1131053
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.
21
22
23 -
24
27 ~~~~ -28-
~A ____
~,
