Chapter 3

Chng 2: GVHD: ThS.
Hunh Vn Kim
H thng IMU v v cc tnh ton lin quan SVTH: L Vn Mi

Page 37

CHNG 3 : H THNG IMU V CC TNH TON LIN QUAN
3.1 CC PHNG PHP M T NH V V PHNG HNG TRONG
KHNG GIAN:

S U, vic tm hiu cc l thuyt
khng gian 3 chiu l cn thit.
3.1.1 Phng php G ur :

c

S T-

V

T

ch
T-Bryan.

3.1.1.1 Gc Euler chnh xc ( proper Euler angles):
l giai ca 2 mt phng XY v xy
T ng hp ny gc Euler tun theo quy lut zyz
ta xoay h trc t tuy i theo cc tr ng z s
tip tc quay theo trc x s p tc quay theo trc z s

Chng 2: GVHD: ThS. Hunh Vn Kim

Page 38

3.1: nh Proper Euler
Trc c c k hiu bi mu lam, tr c k hi
C th ng hp ny da vo hnh 3.9
:
G nh l gc gia trc x( c ng
( k hiu N)
G nh l gc gia trc z ( c nh) v trc Z(
trc xoay)
G nh l gc gi c X(
trc xoay).
2
[-] R
[ ] [-/ /]
nh gc Euler t mt h trc bt k:
X XYZ
, Ta :
3
cos( ) Z | =
2
3
sin( ) 1 Z | =
2
cos( ) sin( ) Z o | =

2
2 3
cos( ) / 1 Z Z o =


Page 39

Hnh 3.2 Hnh chiu trc Z ln h trc chun

T Y ( hnh 3.11)

9 -

9 = :
3
3 3
cos( )sin( )
cos( )=Y / 1
Y
Z
|

Hnh 3.3 Hnh chiu trc Y ln h trc chun

Page 40

:
2
2 3
3
2
3 3
arccos( / 1 )
arccos( )
arccos( / 1 )
Z Z
Z
Y Z
o
|
=
=
=

3.1.1.2 Gc TaitBryan
YZ;
T tun tho quy ut zyx

Hnh 3.4 Gc Tait-
Tait- :
G v trc X
G gc gi v trc v trc Y
G f a tr .
Trong thc t ng ng dng Gc Tait- Bryan xc
nh thu ng ca h thng INS, v d c th l xc
nh v ng c c gi l h
trc t hng khng ( aircraft convention)


Page 41

Hnh 3.5 ng dng Gc Tait- Bryan trong hng khng
Roll l gc quay quanh trc x ( trc dc thn my bay)
Pitch l gc quay quanh trc y ( trc dc cnh my bay)
Heading (Yaw) l gc quay quanh trc z ( trc song song
vi trng lc)

3.1.1.3 Vn Gimbal Lock:

Hnh 3.6 Hi G P =9
Gimbal Lock l hi ng mt mt bc t do trong khng gian
3 chiu khi 2 trong 3 trc trng nhau ( hoc song song nhau) d n
h thng ch quay trong khng gian 2 chiu.
V d gi s ng hp Gc Tait-Bryan: nu gc Pitch
= 9 t ta c trc X s trng vi trc Z sau khi
thc hin xong gc quay quanh tr Y uay quanh trc X
th s gi c Z. Vt ch c quanh trc Y
v Z m thi.
y cc gc Picth v Roll s phi b gii hn trong tm t
( -/ /

Page 42

khc ph m ny c
c 1 cch duy nht l s dng h t khng gian 4 chiu (
Quaternion), s cp phn 3.1.3.

3.1.2 Phng php DCM ( Direct Cosine Matrix):
n 3.1.1

X Y Z
T

cos sin 0
( , ) sin cos 0 (3.1)
0 0 1
cos 0 sin
( , ) 0 1 0 (3.2)
sin 0 cos
1 0 0
( , ) 0 cos sin (3.3)
0 sin cos
R Z
R Y
R X

u u
u
u u
| | |
| |
(
(
=
(
(

(
(
=
(
(

(
(
=
(
(

T R(Z, ), R(Y, ) v R(X,) l t l cc ma trn gc xoay
quanh trc z, y, x theo T-
Bryan.
n xoay tng qut trong khng gian 3 chiu Euclide
nh :
cos sin 0 cos 0 sin 1 0 0
sin cos 0 0 1 0 0 cos sin
0 0 1 sin 0 cos 0 sin cos
xyz
R
u u
| |
u u | |
( ( (
( ( (
=
( ( (
( ( (


Page 43

Hay :
cos .cos cos .sin sin .sin .cos sin .sin cos .sin .cos
cos .sin cos .cos sin .sin .sin sin .cos cos .sin .sin
sin sin .cos cos .cos

xyz
R
u | | u | | u
u | | u | | u
u | u | u
+ + (
(
= + +
(
(

(3.4)

c s d v khng gian 3 chiu
trong openGL (s c 4).
Ma tr nh gc nghing da vo cc
thng s c t Accellerometer s cp mc 3.3.

3.1.3 Phng php Quatrnion:
cp phn u din theo gc Euler s
xy ra hi G n gii php khng gian 4
chiu. Chnh l h t Quaternion

- S W R
8
:
0 1 2 3
2
2 2 2 2
0 1 2 3
[ ]
1
T
q q q q q
q q q q q
=
= + + + =

:
0
1
2
3
cos( / 2)
sin( / 2) cos( )
sin( / 2) cos( )
sin( / 2) cos( )
x
y
z
q
q
q
q
o
o |
o |
o |
=
=
=
=

T o
cos( )
x
| , cos( )
y
| , cos( )
z
|

khn :
2 2
2 3 1 2 0 3 0 2 1 3
2 2
1 2 0 3 1 3 2 3 0 1
2 2
1 3 0 2 0 1 2 3 1 2
1 2( ) 2( . . ) 2( . . )
2( . . ) 1 2( ) 2( . . ) (3.5)
2( . . ) 2( . . ) 1 2( )
xyz
q q q q q q q q q q
R q q q q q q q q q q
q q q q q q q q q q
( + +
(
= + +
(
(
+ +

:

Page 44

2 2 2 2
0 1 2 3 1 2 0 3 0 2 1 3
2 2 2 2
1 2 0 3 0 1 2 3 2 3 0 1
2 2 2 2
1 3 0 2 0 1 2 3 0 1 2 3
2( . . ) 2( . . )
2( . . ) 2( . . ) (3.6)
2( . . ) 2( . . )
xyz
q q q q q q q q q q q q
R q q q q q q q q q q q q
q q q q q q q q q q q q
( + +
(
= + +
(
(
+ +

3.1.4 Chuyn i gia cc h trc ta :
3.1.4.1 T Gc Euler chuyn sang ma trn xoay:
Khi s d T-
nh ma trn xoay t gc bi c.

cos .cos cos .sin sin .sin .cos sin .sin cos .sin .cos
cos .sin cos .cos sin .sin .sin sin .cos cos .sin .sin
sin sin .cos cos .cos

xyz
R
u | | u | | u
u | | u | | u
u | u | u
+ + (
(
= + +
(
(

V c l
R :
P :
Y :

3.1.4.2 T ma trn xoay chuyn sang gc Euler:
Gi s ta c mt ma trn xoay bi c l

11 12 13
21 22 23
31 32 33
(3.7)
a a a
M a a a
a a a
(
(
=
(
(

T 7 ng nht 2 h s ca 2
ma tr c: a
13
= -sin ; tan = a
32
/ a
33 ;
tan = a
21
/ a
11
:

Page 45

Hnh 3.6 Bng tnh ton gi tr gc Euler t ma trn xoay

3.1.4.3 T gc Euler chuyn sang Quaternion

Vic chuy i t cc gc Euler bi c sang h t
Q nh theo cng thc sau
cos( ) cos( ) cos( ) sin( ) sin( ) sin( )
2 2 2 2 2 2
0
cos( ) cos( ) sin( ) sin( ) sin( ) cos( )
2 2 2 2 2 2
1
cos( ) sin( ) cos( ) sin( ) cos( ) sin( )
2
2 2 2 2 2 2
3
sin( ) cos( ) cos( ) cos( ) sin( ) sin( )
2 2 2 2 2 2
q
q
q
q
u | u |
u | u |
u | u |
u | u |
(
(

(
( =
(
+
(
(

(
(
(
(
(
(
(
(
(
(


Page 46

3.1.4.4 T Quaternion chuyn sang Euler:

ng nht h s 5 c ma trn chuyn
:
0 1 2 3
2 2
1 2
0 2 1 3
0 3 1 2
2 2
2 3
2( . . )
arctan
1 2( )
arcsin(2( . . ))
2( . . )
arctan
1 2( )
q q q q
q q
q q q q
q q q q
q q
|
u
+ (
(
+
(
(
(
= (
(
(
( +

(
( +

[-t /2, t
/] V
5
:

2 2
0 1 2 3 1 2
0 2 1 3
2 2
0 3 1 2 2 3
atan 2(2( . . ),1 2( ))
arcsin(2( . . ))
atan 2(2( . . ),1 2( ))
q q q q q q
q q q q
q q q q q q
|
u
( + + (
(
(
=
(
(
(
( + +

3.1.4.5 T Quaternion chuyn sang ma trn xoay:

T 5 6
3.1.4.6 T Ma trn xoay chuyn sang Quaternion:

C th chuy c khi chuyn qua Quaternion
Vic chuy i trc ti i phc tp.


Page 47

3.2 H THNG NH V QUN TNH INS:

S
nh v ng:
dead-reckoning. Dead reckoning l mt thut ng dng trong ngnh hng hi
truyn th ch nh v tr thuyn trn bin ca cc
hoa tiu. Nh i hoa tiu c th c v tr ca thuyn trong mt
khong thi gian nh nh nh b li v t ca thuy
c nhng cng ngi vt sp t tr c t
lin da vo kinh nghim v a l. Ti ngy nay, cng vi s pht trin ca khoa
hc k thut, dead-reckoning tr thnh m nh v cho cc h thng
P a trn trng thi hin ti ca h th n tc,
gia tc, v c th c trng thi k tip ca h thng. H
th nh v qun tnh INS chnh l mt ng d n hnh c
nh v dead-reckoning.
H th nh v qun tnh INS pht trin t nh 96 i khi
u cho s pht trin ny l ti Robert Goddard, mt nh tin phong trong
cng ngh tn la ca M. Kt qu thc nghim ca Robert Goddard vi mt h
u v INS trn th gi S c
ng d c tn l r c
hng khng dn dng v qun s, vn ti bin, tu ngm, cng ngh truyn thng,
v c trong cc ngnh khoa hc nghin cu v robot t hnh.


Page 48

3.2.1 Nguyn hot ng a IN
Nguyn l ho ng ca INS l tng hp cc tn hi c bi
m ng qun tnh IMU (Inertial cho bit
trng thi hin thi ca h th d -
ng trng thi k tip ca h thng. Cc tn hi c
bi IMU gm c vn tc gc v gia tc di ca h thng. IMU cu to t cc
cm bin gia tc (accelerometer) v cc cm bin gyro. Tn hiu gia tc di
c v bi accelerometer, cn tn hiu vn t i gyro.
t IMU v INS. IMU ch ng
nhng thng s ca h thng, cn INS th bao gm IMU v nhng thut ton
tng h nh trng thi ca h thng. Cm bi n tc
gc ca h th i vi mt h t tham chiu. Bng cch s dng
ng ti gc t u ca h thng, sau
ly tch phn gi tr vn t ng ca h
thng ti mi th m. Cm bin gia t c gia tc di ca h thng
trong mt h t tham chi ng l mt h t gn lin vi
mt vt chuy ng khi acce c gn c nh ln h thng v
chuy ng cng h thng.
Tuy nhin, b nh vn tc gc v gia t i vi
h t chuy ng gn vi h thng, ta hon ton c th c
gia tc di ca h thng trong h t qun tnh. Thc hin php tch phn
gia tc qun tnh ta c th c vn t a h thng, sau
n na ta s c v tr ca h thng trong h t qun
u ki c v tr, vn tc gc ban u ca
h thng trong h t qun tnh.
y, h th nh v qun tnh cung cp v tr, vn t ng
v vn tc gc ca h thng b n tc gc v gia tc di ca h
thng trong h t tham chi
S

3.2.2 Cu to a IN
H th nh v qun tnh INS gm cc acc
(hoc cc cm bi a h thng trong khng
gian g
nghing ca my bay khi ln hay xung, roll l gc nghing cnh ca
my bay, cn yaw l gc ca my bay quay quanh trc th ng.

Page 49

3.7: Y P R

c di s c gn ln ba trc ca
h t chuy ng ca h th S
my tnh s tnh ton r c trng thi hin ti ca h thng. Mt h thng
INS ho ng gn hay trn b m t ph c tch hp thm php hiu
chnh Schuler cho h thng c th lin tc ch v t khi di chuyn
(php hiu ch S c pht minh b S 9. Php hiu
ch S m bo cho nh i v S ta
gn lin v t cho d tm quay ca mt vt di chuyn trn b
m t b i v khng cn trng v t bi ng ca
a hnh. V l thuy c ng dng cho nhng h th ng
qun tinh ho ng gn b m
S ng thu
tng h nh trng thi ca h thng.

3.8: S Y P R

Page 50

3.2.2.1 Phn loi INS
H thng nh v ton c c cu to nn bi mt b IMU v
thut ton dead-reckoning. Ty thuc vo cch b tr cc cm bin gyro v
accelerometer m c th phn chia INS thnh hai loi: gimbaled system v
strapdown system.
i vi Gimbaled system, ba accelerome c sp xp theo ba
trc vung gc vi nhau v l t ch c- -ty
m b ng ca cc
i khi qu o ca h th i th cc
c c nh trn m u nn gi l gyro-stablized
platform u n m c ba gimbal, l nhng vng c th quay quanh
mt tr ng knh c c lin kt vi
nhau l t t ngoi vo trong sao cho mi gimbal c th quay quanh trc.
gimbal trong cng, ba cm bi c b tr theo cc tr
accelerometer, v c ch i b hin ng precession ( s thay i v
tr trc quay c m b ng ca cc
i. Loi cm bi ng s dng trong gimbaled system
ng l lo trc (intergrating rate gyro), v tn hiu ng
u khi
i trong h t qun tnh. Gimbal
i trong qu trnh chuy ng ca
h th c c nh trong mt mt phng
nn trn gimbal ny. Cu to ca gimbaled INS thc s rt phc tp,vic ch
t c kt c chnh xc cao l c
u ny d n vic gi thnh ca gimbaled INS rt cao.
Strap down INS c kt c n t chuyn
i gimbaled INS, bi v trong h thng strapdown khng h c
kt c T S c c
nh ngay trn kt cu ca h thng,. Tuy nhin trong strapdown INS, nhng
ng dng ca cng ngh thng tin trong x l tn hiu l rt quan trng, v
nhng linh kin strap down ng l cc cm bi n t, quang hc hay
bn dn. Trong strap down INS khng c thnh phn no l chuy ng v
mt v ng thi ca h thng trong h quy chiu gn lin vi h
th i cc cm bin gyro (m thng s trng thi c
chnh l vn tc gc quanh cc tr c s
d c di ca h thng theo cc trc trong h t chuyn
ng. Strap down INS tnh ton cc gi tr c kt hp vi php bi i
t c trng thi v v tr ca h thng.

Page 51

Vi nguyn l ng cc gimbal th gimbaled INS
gp phi sai s kh ln khi h th i qu o m t ngt m
nh kp thi, hi ng ny gi l gimbal lock i vi
strap down INS th khng xy ra hi ng trn.
T cp nht d liu c S
INS rt nhiu ln. Gimbaled INS c th ho ng tt vi t update l 50-
60 l/ i v ng l 2000 ln/ giy. V tm
a cc gyro s dng trong strap down INS l S

3.2.2.2 n v o ng qun tnh IM Inrtia asurnt unit

U
U

S

U

-reckoning.
U

R P Y
gyro.
T

gian.

3.9: Y P R

U


Page 52

t

U
U

GPS

U

3.3 CC CM IN C THNH MT N V O NG QN TNH IM

T
(
accellerometer) c cm
bin t ng( magnetometer).

3.3.1 C in Gyro
Thut ng gyroscope tc con quay hi chuyn xut hin t gia
th k XIX, v trong nhng thp nin g c s dng rng ri v
c thay th trn ton cu vi t gyro. Nhng l thuy u v con
quay hi chuy c p d gii thch v chuy ng ca mt vt
th T t. V dn d c pht trin v ng dng
rng ri trong nhi c bit l trong h th nh v qun
tnh INS. C th chia gyro thnh 3 loi vi nguyn l ho ng, cu to v
kh ng d : n.
Kh u trong s pht tri ng da trn nguyn l con
quay hi chuyn truyn th G c s dng nhiu trong h thng
S S i ca gyro quang hc, v g t l
s pht trin ca cc lo n ng dng bi cng ngh n t
MEMS (micro- n ng
dng nhiu trong strap down INS.


Page 53

3.3.1.1 Gyro
G ng gi bng thut ng rotating wheel) hot ng
da trn nguyn l bo ton moment ca mt vt th chuy ng quay
khi tng cc lc tc dng ln vt trit tiu.
Cu t n ca m m c m i hnh trn
ng bng mt s i trong mt b
lo c truyn chuy ng quay b ng ln si dy dn hi
treo dc theo trc ca bnh xe. Khi chuy ng quay th Gi A l moment
quay c I l moment qun tnh c i vi trc quay, w l
vn tc quay c c. Da trn nguyn l bo ton moment ta
c:
.
i i
r r ir
A I w =
i vi cu to d c moment qun tnh
c t t t ng vi cc
trc quay ca gimbal. u to c ng tm ca
i trng vi trng tm ca h gyro gimbal:
0 0
0 0
0 0
rx
r ry
rz
I
I I
I
(
(
=
(
(

Trong qu trnh ho ng, n ng thi ca h thng thay
i, th cc gimbal c th chuy ng quay quanh tr
gyro vn gi ng ca trc quay, v gi trng thi n
nh trong khng gian qun tnh. Cn gc quay ca cc gimbal c th
c bng cc thit b n lin vi trc quay ca gimbal, cc thit
b c gi bng thut ng pickoff.
S ng gimbal trong cu to c nh s bc t do
chuy ng quay ca gyro. V d c t do th c cu to
gm c hai gimbal. Nu trc quay ca mt gyro hai bc t do l thng
c gi l gyro th ng (vertical gyro), cn n c
quay nm trong mt phng nm ngang th g ng
(directional gyro), cn mt gyro c trc quay khng theo m
th no th gi l gyro t do (free gyro). S nh trong khng gian ba
chi i mt gyro th ng v m ng, hoc l hai
gyro t do. Mt lo m ng dng rng ri l gyro mt bc t
do, tc l cu to ch gm mt gimbal quay quanh trc m thi, loi gyro
c gi l intergrating rate gyro.
G i gyro truyn th c
ng dng nhiu trong h thng d ng ca tn la hay tu ngm. Gyro
yu l ng chuy ng ca h
thng th G y c s dng trong h gimbaled INS bi v
kt c t ph hp. Hu ht cc h th S u s dng kt hp
ba cm bin gyro loi rate intergrating mt bc t do
ng trong khng gian.

Page 54

3.3.1.2 Gyro quang (Optical gyro)
Nguyn l ho ng ca gyro quang da trn hiu ng
Sagnacc nh vt l h P G S 9
l hiu ng giao thoa i vn tc quay. Hiu ng
S i m t lu c tch ra
thnh hai v cho lan truyn trn cng mt qu o vi hai chi c
nhau, v qu ng th t thit b
thoa, gi l dng c f Tt b ny
c gn ln m c th quay th vng giao
thoa s b dch chuyn, d c trn mng quan st
s b dch chuy dch chuyn ty thuc dch chuyn
hay gc quay c .
Cu to c t sc phc tp v gm nhiu b phn
chuy ng, cn gyro quang th c cu t
khng c b phn no chuy ng. C hai lo c pht trin
v ng dng rng ri trong cng ngh
gyro vng laser v gyro cp quang.
Gyro vng laser (RLG-ring laser gyro):
M n c cu to l mt rnh kn c dng
hnh h ng l ba, bn, hay su c c khoan bn trong mt khi
th ng chuy ng ca mt hn h
helium v neon.
Cc phn t kh chuy ng pht ra nh sng khi b kch thch bi
ng theo
ng c chiu nhau trong rnh kn, mt chm sng chuy ng
theo chi ng h v chm sng cn li chuy c chiu
ng h. Ti trng thi cn bng ca khi thy tinh, th thi gian di
chuyn c ng l mt r
nhau, cho nn hai sng nh sng l cng pha nhau. Cn n i thy
t gc trong khng gian thi th t
m a b sai lch,
l s lm dch chuyn vn
giao thoa trn thit b quan st.

Page 55

3.10:

3.11: G
Gyro cp quang (Fiber optic gyro) :
Mc d s pht trin ca gyro vng laser l rt th
vi nhng nghin c ng lc cho nhng
thit k cho gyro cp quang.
c truyn trong s c qun thnh cun,
v chiu di t n 5km, chiu di ca si cp quang di hay

Page 56

ngn s nhy c T ng gyro cp
c nh, nh v cng sut b. Ngoi cun dy cp quang
th gyro cp quang cn c nhng thnh ph n sau: mt ngun sng,
mt b phn cc nh sng, mt b ng sng, mt b chnh pha
nh sng v mt cm bi pht hin nh sng.
Nguyn l ho ng c
c phn c c tch lm hai s
lan truy c nhau trong cun cp quang. B phn cc
m bo cho c hai lung sng ph c phn c cho t
lan truyn bng nhau. Ti v tr cn bng, gyro khng quay th khng c tn
hiu g cm bin quang b c pha nn trit tiu
nhau. Nu gyro quay mt gc th hai chm sng lch pha kt hp s to nn
mt chm sng kt h chm sng kt h c cm bin quang
chuy n p cng cao n
ln.

3.12: G

G t tri so vi gyro vng laser ch
n c th i nhi nhy khc nhau ty theo ng dng
bi chiu di cun cp quang c th thay th c. V cng v c
nh g c ng dng ngy cng rng ri trong cc
h th nh v ng.

Page 57

: G

3.3.1.3 Gyro rung (Vibrating gyro)
Vi gi thnh th n so vi cc loi gyro khc, gyro rung
c ng dng nhi c dn dng gii. Gyro rung ch yu
a xe trong lc di chuyn, v gyro rung ch
tr cho GPS, h th nh v chnh ca cc loi xe ny.
Cng ngh ch t u, vi cm bin v h th
n (MEMS- n nhanh
chng trn ton th gii, v c ng dng nhi sn xut ra
nhng thit b i s ng ln v gi thnh h. Cm bin
m bi n tc
gc c ti t ln trong nhiu ng d ng cm bi
u khin. Cc cm bi S ng
chnh xc cao v gi thnh rt thp. Tuy nhin kh
ng ca cm bin c th c nng cp nh cc cng c
l x l tn hiu.
Trong hu ht cc loi gyro rung, th thnh phn cm bi n
l m u gm mt kh c gi bng hai l xo.
Khi gi c gi bi hai l xo gi l kh ng chu lc
(proof mass). Proof mass c th ng mt trong hai ch , mt ch
khi khng c lc coriolis, gi l ch cn ch cn li l ch
th c f ng trn mt h th u
l T ng gyro rung ho ng ch th cp, bng
cch chuy n s ng ca proof mass, ta c th c vn
tc quay ca h thng. Vn tc cng l n p cng cao. Do proof
c cu to t vt li n cho nn gyro rung b ng rt
ln bi nhi . Vi cng ngh MEMS, hi c hn ch,
a gyro c nng cao.


Page 58

3.3.2 C in gia t
Mt gia tc k s dng qun tnh ca 1 v i gia
gia t ng hc trong khng gian qun tnh so vi gia tc tr ng .
Gia tc k dng con lc c cu to kh gi gm 1
vt n f c treo bi 1 l xo. Vt nng c th chuy ng
dc theo l xo. Con l ng gim ch hn ch nh
ng c ng. Hnh sau minh ha cho cu to 1 gia tc k con lc

3.14:

N c th l xo s bin dng. D bin dng
ca l xo m ta c th c gia tc ca h th t
ng ca gia tc k con lc.
Hi ng dng gia tc k i gia
tc k c kh c t n gia tc k dng con lc v h
khng c thnh phn no di chuyn, nh g t cun dy bn ngoi
proof mass. Nguyn l ho ng ca n l khi c dch chuyn nh ca
proof mass th s n trong cun dy, t t l n t
theo chi c l kh D c gia
tc chuy ng ca h thng b n chy trong cun dy.
Vi s pht trin ca cng ngh MEMS (microelectromechanical
systems) th cc loi gia tc k c sn xut hng lot vi gi thnh thp
v ch ng kh tt.


Page 59

3.3.3 Cm bin t trng:
Cm bin t ng l mt thit b ng
ca t c to ra trong phng th nghim hoc t t).
n v trong h th ng quc t l Tesla
v kh l nanotesla (nT). Trong thc t th ta
ng v l Gauss.
1 Gauss = 100 000 (nT )
T t l m ng th. N tha i theo rt nhiu
nguyn nhn khc nhau. T khong 20 000 nT
o v khong 80 000nT gn cc cc.
Cm bin t c chia ra lm 2 lo chnh l :
S : ng t ng.
Vector magnetom: ng thnh phn ca t ng
theo cc trc n kh ng ca thit
b.
3.4 CC TNH TON X L D LI T CC CM BIN:
T u gi s rng vt gn thit b c t tuyt
i l X
b
Y
b
Z
b
, trong khi t ca cm bin ( t i) l X
b
Y
b
Z
b
:
Mi quan h gia h trucj t tuy i v h trc t c xc
( theo cng thc 3.4) vi
T
xyz
R l ma trn chuyn ca
xyz
R :

'
'
'
b b
T
b xyz b
b b
X X
Y R Y
Z Z
( (
( (
=
( (
( (

' cos cos cos cos sin
' cos sin sin cos sin cos cos sin sin sin cos sin (3.7)
cos sin cos sin sin sin cos sin cos sin cos cos '
b b
b b
b b
X X
Y Y
Z Z
u u u
u | | | u | u |
u | | | u | u |
( ( (
( ( (
= +
( ( (
( ( ( + +

3.4.1 X l d liu t cm bin trng trng (accellerometer):

Page 60

Hnh 3.14 Tnh ton gc gc nghing (tilt) t accelerometer
T accelerometer ta c th c gc nghing ca h
thng b nh gc Roll v Pitch.
Trong h t i X
b
Y
b
Z
b
, ta c cc gi tr ng
hp cm bi t vung gc v a trng l :
X
b
= Y
b
=0 v Z
b
= 1 g ;
trc t tuy i ta s c cc gi tr l
Ax, Ay

, A
z
, cc gi tr c chun ha ( normalize).
Thay 7 c
cos cos cos cos sin 0
cos sin sin cos sin cos cos sin sin sin cos sin 0
cos sin cos sin sin sin cos sin cos sin cos cos 1
x
y
z
A
A
A
u u u
u | | | u | u |
u | | | u | u |
( ( (
(
( (
= +
(
( (
(
( ( + +

T suy ra l :
Gc Roll = = Arcsin( A
y
/ cos )
Gc Pitch = = Arcsin(- A
x
)
Ch rng A
x
, A
y
, A
z
c chu

2 2 2
1
x y z
A A A + + =

Nu biu thc ny khng tha th cm bin l khng tuyn tnh
hoc c sai s trong vic calib.

Page 61

3.4.2 X l d liu t cm bin t trng

Hnh 3.15 : Tnh ton gc xoay ( Yaw) t magnetomter
c gc nghing nh n cm
bin accellerometer. By gi ta s
(Yaw) theo cm bin t :
Gi Mx, My, Mz l hnh chiu c t
ln cc trc t c
trong h t tuy i.
Trong h t tuy i th r rng Yaw s :
Yaw = = / (3.8)
Nu thit b nghing theo cc gc Roll v Pitch th ta quy v h t
tuy :
'
' ( , ) ( , ) (3.9)
'
x x
T T
y y
z z
M M
M R X R Y M
M M
| u
( (
( (
=
( (
( (

Thay cc cng thc 3.2 v 3.3 vo 3.9 c cc kt qu:

x
= M
x
cos + M
y
sin

y
= M
x
sinsin + M
y
cos - M
z
sincos

z
= M
x
cossin + M
y
sin - M
z
coscos

Page 62

Cng th 8 ng h :
Yaw = = / u Mx >0 v My>0
Yaw = = 8 + / <
Yaw = =6 + / >
<=
Yaw = = 90
o
( n = <)
Yaw = = 270 ( n = >
Ch r ng hp cm bin gia t
c chu
2 2 2
' ' ' 1
x y z
M M M + + =

N u ny khng th u t cc ngun t
.
3.4.3 X l d liu t cm bin vn tc gc (Gyro):
Vic tnh ton s liu t Gyro l kh phc tp v n cho ra d liu
l vn t nh c gc quay th chng ta phi tch phn
theo thi gian.
tnh vn t i( vi phn) gc Euler
t G :

0 sin cos
1
0 cos cos sin cos
os
cos sin sin cos cos
x
y
z
t
t c
t
o
o
e | |
ou
| | | u e
o |
u | u | u
e
o|
o
(
(
(
(
(
(
(
(
=
(
(
(
(
(
(

(
(

Vi , t l gc Euler ti th m ly m
(t-1) . Gc Euler ti th m hin t :
( ) ( 1)
( ) ( 1) .
( ) ( 1)
t
t t
t t t
t
t t
t
o
o

ou
u u
o
| |
o|
o
(
(
( (
(
( (
(
= + A
( (
(
( ( (

(
(

Chapter 3

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Chapter 3

Uploaded by

Copyright:

Available Formats

Chng 2: GVHD: ThS.

You might also like