Chng 2:

H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

CHNG 3 : H THNG IMU V CC TNH TON LIN QUAN

111Equation Chapter 1 Section 13.1
CC PHNG PHP M T NH V V
PHNG HNG TRONG KHNG GIAN:
tinh toan cung nh giai thich nguyn ly hoat ng cua h thng inh
vi quan tinh INS va n vi o lng quan tinh IMU, vic tm hiu cac ly thuyt
khng gian 3 chiu la cn thit.
3.1.1 Phng php Goc Euler :
Cac goc Euler la 3 goc c inh nghia bi Leonhard Euler xac inh hng
cua m t i tng. xac inh hng trong khng gian Euclide 3 chiu, 3 tham
s c oi hoi. Chung co th c chon theo nhiu cach khac nhau, va cac goc
Euler la m t trong s o.
Cac goc Euler thay th cho ba chuyn ng quay kt hp, di chuyn h truc tham
chiu n m t h truc ta ang xet. Hay noi m t cach khac, bt k m t hng nao
trong khng gian Euclide 3 chiu cung co th c xac inh bng s kt hp cua
3 chuyn ng xoay thanh phn (chuyn ng xoay quanh m t truc c ban), va
tng t nh th, ma tr n xoay t h truc c inh tham chiu n h truc ta ang
xet cung co th c phn tich thanh 3 ma tr n xoay thanh phn.
Khng tinh n vi c xet du cua chuyn ng quay cung nh vi c di chuyn cac
h truc tham chiu, co tt ca 12 quy c khac nhau trong vi c kt hp chuyn
ng quay, t o la cac quy c v goc khac nhau. M t trong s chung c goi
la goc Euler chinh xac. S con lai c goi la goc Tait-Bryan. i luc chung u
c goi chung la goc Euler.
Vi c xac inh cac goc la thu c nhom nao phu thu c vao cach inh nghia ng
c s dung h tr vi c xac inh cac goc nay. Co th s dung ng c s la
giao cua 2 m t phng tng ng (2 m t phng trung nhau khi cac goc bng 0).
Tuy nhin cung co th xac inh da vao 2 m t phng trc giao (2 m t phng
vung goc nhau khi cac goc bng 0). Cac goc c xac inh bng cach chon th
nht c goi la cac goc Euler chinh xac. Con vi cach chon th hai, cac goc
c goi la goc Nautical, goc Cardan, ho c la goc Tait-Bryan.
3.1.1.1 Goc Euler chnh xc ( proper Euler angles):
ng c s la giai cua 2 mt phng XY va xy
Trong trng hp nay goc Euler tun theo quy lut zyz, nghia la ta xoay
h truc toa tuyt i theo cac truc tng ng z s c goc , tip tuc
quay theo truc x s c goc , va tip tuc quay theo truc z s c goc .

Page 37

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.1: Cach xac inh Cac goc Proper Euler

Truc c inh c ki hiu bi mau lam, truc xoay c ki hiu la mau o
Co th xac inh goc Euler chinh xac trong trng hp nay da vao hnh 3.9
nh sau:
Goc c xac inh la goc gia truc x( c inh) va ng
c s ( ki hiu N)
Goc c xac inh la goc gia truc z ( c inh) va truc
Z( truc xoay)
Goc c xac inh la goc gia ng c s ( N ) va truc X(
truc xoay).
Lu y rng, cac goc va co modulo la 2, dai gia tri nm trong
khoang [-,]. Ring goc co modulo la , dai gia tri nm trong khoang
[0, ] ho c la [-/2, /2]
Cach xac inh goc Euler t mt h truc bt k:
Xet m t h truc toa vi cac vector n vi (X,Y,Z) nh trong
hnh 3.10, Ta thy rng:
cos( ) Z 3

sin( ) 1 Z 32
cos( ) sin( ) Z 2

Page 38

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

cos( ) Z 2 / 1 Z32

Hnh 3.2 Hnh chiu truc Z ln h truc chun

Ta cung chiu vector Y tng t nh th ( hnh 3.11). Ln
u tin chiu ln m t phng cua truc z va ng c s N, goc
chiu nay s la (90 - ) va cos( 90 ) = sin nn ta co:
cos( )sin( ) Y3
cos( )=Y3 / 1 Z 3

Page 39

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.3 Hnh chiu truc Y ln h truc chun

Cui cung, s dung ham chuyn i ngc ta s co c
cac goc Euler:

arccos( Z 2 / 1 Z 32 )
arccos( Z 3 )
arccos(Y3 / 1 Z32 )
3.1.1.2

Gc TaitBryan

ng c s l giao gia 2 mt phng xy v YZ;

Trng hp ny gc Euler tun theo quy lut zyx, ngha l
khi quay h trc ta tuyt i theo trc ta c gc , tip
tc quay theo trc y ta c gc v tip tc quay theo trc x
ta c gc

Page 40

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.4 Gc Tait-Bryan (ng c s y c k hiu mu vng)

C th xc nh gc Tait-Bryan da vo hnh 3.12 nh sau:

Goc la goc gia ng c s va truc X

Goc la goc gia ng c s va truc va truc Y
Goc alf goc gia tri y va ng c s .
Trong thc t ngi ta thng ng dung Goc Tait- Bryan xac
inh thuc tinh va inh hng cua h thng INS, vi du cu th la xac
inh vi tri va phng hng cua may bay, i klhi c goi la h
truc toa hang khng ( aircraft convention)

Hnh 3.5 ng dung Goc Tait- Bryan trong hang khng

Roll la goc quay quanh truc x ( truc doc thn may bay)
Pitch la goc quay quanh truc y ( truc doc canh may bay)
Heading (Yaw) la goc quay quanh truc z ( truc song song
vi trong lc)
3.1.1.3 Vn Gimbal Lock:

Page 41

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.6 Hin tng Gimbal khi Pitch =9 0

Gimbal Lock la hin tng mt mt bc t do trong khng
gian 3 chiu khi 2 trong 3 truc trung nhau ( hoc song song nhau) dn
n h thng ch quay trong khng gian 2 chiu.
Vi du gia s trong trng hp Goc Tait-Bryan: nu goc Pitch
= 90 khi o theo th t ta co truc X s trung vi truc Z sau khi
thc hin xong goc quay quanh truc Y , khi o khi quay quanh truc X
th s ging nh quay quanh truc Z. Vt ch quay c quanh truc Y
va Z ma thi.
Nh vy cac goc Picth va Roll s phai bi gii han trong tm
t ( -/2 , /2).
khc phuc nhc im nay cua phng phap goc Euler,
ch co 1 cach duy nht la s dung h toa khng gian 4 chiu
( Quaternion), s c cp phn 3.1.3.
3.1.2

Phng php DCM ( Direct Cosine Matrix):

Nh a noi phn 3.1.1, cac goc Euler co th xac inh 3 chuyn ng xoay
thanh phn a m t h truc tham chiu n h truc ma ta ang xet. Cac
chuyn ng thanh phn la chuyn ng quanh m t truc, co th la X, Y, ho c Z.
Tng ng vi cac chuyn ng nay, ta co ma tr n xoay thanh phn c inh
nghia nh sau.

Page 42

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

cos
R( Z , ) sin
0

sin
cos
0

0
0
1

cos
R(Y , ) 0
sin

0 sin
1
0
0 cos
0
0
1

R( X , ) 0 cos sin
0 sin cos

(3.1)

(3.2)

(3.3)

Trong o R(Z, ), R(Y, ) v R(X,) ln lt la cac ma trn goc xoay

quanh truc z, y, x theo cac goc tng ng , , theo phng phap goc TaitBryan.
Khi o ta co ma trn xoay tng quat trong khng gian 3 chiu Euclide nh
sau:

Rxyz

cos
sin
0

sin
cos
0

0
0
1

cos
0

sin

0 sin
1
0
0 cos

1
0
0 cos
0 sin

0

sin

cos

Hay :

Rxyz

cos .cos
cos .sin
sin

cos .sin sin .sin .cos

cos .cos sin .sin .sin
sin .cos

sin .sin cos .sin . cos

sin .cos cos .sin .sin

cos .cos
(3.4)

y cung la phng trnh c s dung v khng gian 3 chiu

trong openGL (s cp trong chng 4).
Ma trn nay con c dung xac inh goc nghing da vao cac
thng s oc t Accellerometer s cp muc 3.3.
3.1.3 Phng php Quaternion:
Nh a cp phn 3.1.1.3, phng phap biu din theo goc Euler s
xay ra hin tng Gimbla Lock nn ngi ta a nghi n giai phap khng gian 4
chiu. Chinh la h toa Quaternion
Page 43

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

y la h thng s c phat trin m r ng t h thng s phc. No c

inh nghia ln u tin bi nha toan hoc ngi Ai-len Sir William Rowan
Hamilton vao nm 1843, va c ng dung phn tich khng gian 3 chiu.
M t quaternion n vi co th c inh nghia nh sau:
q [q0 q1 q2 q3 ]T
2

q q02 q12 q22 q32 1

Chung ta co th kt hp quaternion vi m t chuyn ng quay quanh
m t truc nh sau:
q0 cos( / 2)
q1 sin( / 2) cos( x )
q2 sin( / 2) cos( y )
q3 sin( / 2) cos( z )

Trong cac cng thc trn, la goc quay (gia tri tinh bng radian cua
cos( x ) cos( y ) cos( z )
goc quay) va
,
,
la cac cosine inh hng
cua truc quay. Khi o ma tr n quay cua chuyn ng quay nay trong
khng gian 3 chiu Euclide c xac inh nh sau:
1 2( q22 q32 ) 2( q1.q2 q0 .q3 ) 2(q0 .q2 q1.q3 )

Rxyz 2( q1.q2 q0 .q3 ) 1 2(q12 q32 ) 2( q2 .q3 q0 .q1 )

(3.5)
2
2
2(q1.q3 q0 .q2 ) 2( q0 .q1 q2 .q3 ) 1 2( q1 q2 )

ho c la:
q02 q12 q22 q32

2(q1.q2 q0 .q3 )

Rxyz 2( q1.q2 q0 .q3 )

2(q1.q3 q0 .q2 )

q q q q

3.1.4

2
0

2
1

2
2

2
3

2(q0 .q1 q2 .q3 )

2(q0 .q2 q1.q3 )

2( q2 .q3 q0 .q1 )
q02 q12 q22 q32

(3.6)

Chuyn i gia cc h trc ta :

3.1.4.1 T Goc Euler chuyn sang ma trn xoay:
Khi s dung phng phap goc Tait-Bryan ta co phng trnh 3.4
xac inh ma trn xoay t goc bit trc.

Page 44

Chng 2:
H thng IMU v v cc tnh ton lin quan

Rxyz

cos .cos
cos .sin
sin

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

cos .sin sin .sin .cos

cos .cos sin .sin .sin
sin .cos

sin .sin cos .sin . cos

sin .cos cos .sin .sin

cos .cos

Vi quy c la
Roll : la goc
Pitch : la goc
Yaw : la goc
3.1.4.2 T ma trn xoay chuyn sang goc Euler:
Gia s ta co mt ma trn xoay bit trc la
a11
M a21
a31

a12
a22
a32

a13
a23
a33

(3.7)

T phng trnh 3.4 va phng trnh 3.7. ng nht 2 h s cua 2

ma trn ta c: a13 = -sin ; tan = a32 / a33 ; tan = a21 / a11 :

Page 45

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.6 Bang tinh toan gia tri goc Euler t ma trn xoay
3.1.4.3 T goc Euler chuyn sang Quaternion
Vic chuyn i t cac goc Euler bit trc sang h toa
Quaternion c xac inh theo cng thc sau

cos( 2 ) cos( 2 ) cos( 2 ) sin( 2 )sin( 2 )sin( 2 )

q0
cos( ) cos( ) sin( ) sin( ) sin( ) cos( )

2
2
2
2
2
2
q1

cos( ) sin( ) cos( ) sin( ) cos( )sin( )

q 2
2
2
2
2
2
2

q3

sin( ) cos( ) cos( ) cos( )sin( )sin( )

2
2
2
2
2
2

3.1.4.4 T Quaternion chuyn sang Euler:

Page 46

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

ng nht h s 2 phng trnh 3.4 va 3.5 ta c ma trn chuyn

i nh sau:
2(q0 .q1 q2 .q3 )

arctan

1 2( q12 q22 )

arcsin(2( q .q q .q ))
0 2
1 3

2(
q
.
q

q
0 3
1 .q2 )
arctan

1 2( q22 q32 )

Chu y la arctan va arcsin ch cho gia tri trong khoang t [- /2,

/2]. Vi th ta phai thay cac ham arctan bng ham atan2 co th
bao quat ht khng gian Euclide 3 chiu. Khi o cng thc 2.15 tr
thanh:
atan 2(2( q0 .q1 q2 .q3 ),1 2( q12 q22 ))

arcsin(2(q0 .q2 q1.q3 ))

2
2

atan 2(2( q0 .q3 q1.q2 ),1 2(q2 q3 ))

3.1.4.5 T Quaternion chuyn sang ma trn xoay:

Theo phng trnh 3.5 va 3.6
3.1.4.6 T Ma trn xoay chuyn sang Quaternion:
Co th chuyn qua goc Euler trc khi chuyn qua Quaternion
Vic chuyn i trc tip tng i phc tap.

Page 47

Chng 2:
H thng IMU v v cc tnh ton lin quan

3.2

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

H THNG NH V QUN TNH INS:

co th tm hiu v h thng inh vi quan tinh INS, trc ht ta cn bit v m t thu t

ng thng dung c s dung trong linh vc inh vi va do ng: dead-reckoning. Dead
reckoning la mt thut ng dung trong nganh hang hai truyn thng, dung ch phng phap
xac inh vi tri thuyn trn bin cua cac hoa tiu. Nhng ngi hoa tiu co th tin oan c vi
tri cua thuyn trong mt khoang thi gian nht inh nh vao b lai va tc cua thuyn, do
o co th c lng c nhng chng ngai vt sp ti trn ng i, hay vi tri cua t lin
da vao kinh nghim v ia ly. Ti ngay nay, cung vi s phat trin cua khoa hoc k thut, deadreckoning tr thanh mt phng phap inh vi cho cac h thng do ng. Phng phap nay da
trn trang thai hin tai cua h thng nh vn tc, gia tc, vi tri, hng co th c lng c
trang thai k tip cua h thng. H thng inh vi quan tinh INS chinh la mt ng dung in hnh
cua phng phap inh vi dead-reckoning.
H thng inh vi quan tinh INS phat trin t nhng nm 1960. Ngi khi u cho s
phat trin nay la tin si Robert Goddard, mt nha tin phong trong cng ngh tn la cua M.
Kt qua thc nghim cua Robert Goddard vi mt h gyroscope th s a khi dy trao lu
nghin cu v INS trn th gii. INS c ng dung u tin trong linh vc tn la vu tru, sau
o m rng qua cac linh vc hang khng dn dung va qun s, vn tai bin, tau ngm, cng ngh
truyn thng, va ca trong cac nganh khoa hoc nghin cu v robot t hanh.

Page 48

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

3.2.1 Nguyn ly hoat ng cua INS

Nguyn ly hoat ng cua INS la tng hp cac tin hiu o c bi mt c cu o

lng quan tinh IMU (Inertial measurement units), cho bit trang thai hin thi cua h thng,
sau o s dung phng phap dead-reckoning c lng trang thai k tip cua h thng. Cac
tin hiu o c bi IMU gm co vn tc goc va gia tc dai cua h thng. IMU cu tao t cac
cam bin gia tc (accelerometer) va cac cam bin gyro. Tin hiu gia tc dai c oc v bi
accelerometer, con tin hiu vn tc goc c o bi gyro. y chung ta nn phn bit IMU va
INS. IMU ch ong vai tro la o lng nhng thng s cua h thng, con INS th bao gm IMU
va nhng thut toan tng hp va xac inh trang thai cua h thng. Cam bin gyro o vn tc
goc cua h thng i vi mt h toa tham chiu. Bng cach s dung phng va hng tai gc
toa nh la trang thai ban u cua h thng, sau o ly tich phn gia tri vn tc goc o c, ta
co c phng hng cua h thng tai mi thi im. Cam bin gia tc o c gia tc dai cua
h thng trong mt h toa tham chiu, va thng thng la mt h toa gn lin vi mt vt
chuyn ng khi accelerometer c gn c inh ln h thng va chuyn ng cung h thng.

Tuy nhin, bng cach xac inh vn tc goc va gia tc dai i vi h toa chuyn
ng gn vi h thng, ta hoan toan co th xac inh c gia tc dai cua h thng trong h toa
quan tinh. Thc hin phep tich phn gia tc quan tinh ta co th xac inh c vn tc quan
tinh cua h thng, sau o tich phn ln na ta s thu c vi tri cua h thng trong h toa quan
tinh, nhng vi iu kin la ta xac inh c vi tri, vn tc goc ban u cua h thng trong h
toa quan tinh.

Nh vy, h thng inh vi quan tinh cung cp vi tri, vn tc, hng va vn tc goc
cua h thng bng cach o vn tc goc va gia tc dai cua h thng trong h toa tham chiu.
u im cua h thng inh vi quan tinh INS la khng cn cac tin hi u tham chiu t bn ngoai
trong vi c xac inh vi tri, hng, va v n tc cua h thng m t khi no a c khi ng xong.
3.2.2 Cu tao cua INS

H thng inh vi quan tinh INS gm cac accelerometer o goc (hoc cac cam bin
gyro) o cac goc nghing cua h thng trong khng gian gm co yaw, pitch va roll. Nh trong
hnh bn di th pitch la goc nghing cua may bay khi chuc ln hay xung, roll la goc nghing
canh cua may bay, con yaw la goc cua may bay quay quanh truc thng ng.

Page 49

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.7: Cac goc Yaw, Pitch va Roll

Con cac accelerometer o gia tc dai s c gn ln ba truc cua h toa chuyn

ng o cac gia tc dai tng ng cua h thng. Sau o may tinh s tinh toan ra c trang
thai hin tai cua h thng. Mt h thng INS hoat ng gn hay trn b mt trai t phai c
tich hp thm phep hiu chnh Schuler cho h thng co th lin tuc ch v tm trai t khi di
chuyn (phep hiu chnh Schuler c phat minh bi Schuler nm 1923. Phep hiu chnh
Schuler am bao cho nhng tinh toan i vi INS la ung trong h toa gn lin vi tm trai
t cho du tm quay cua mt vt di chuyn trn b mt trai t bi thay i va khng con trung
vi tm trai t bi anh hng cua ia hnh. Va ly thuyt nay a c ng dung cho nhng h
thng do ng quan tinh hoat ng gn b mt trai t nh trong tau thuy, may bay). Nh
v y INS th bao gm h thng cac cam bin o lng va nhng thut toan tng hp va xac
inh trang thai cua h thng.

Hnh 3.8: S thay i trang thai cua may bay ng vi cac goc Yaw, Pitch va Roll
3.2.2.1

Phn loai INS

Page 50

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

H thng inh vi toan cu c cu tao nn bi mt b IMU va thut toan deadreckoning. Tuy thuc vao cach b tri cac cam bin gyro va accelerometer ma co th phn chia
INS thanh hai loai: gimbaled system va strapdown system.

i vi Gimbaled system, ba accelerometer c sp xp theo ba truc vung goc

vi nhau va ln lt ch theo cac phng bc-nam, ng-ty va hng li tm khoi tm trai t.
am bao phng hng cua cac accelerometer khng i khi qu ao cua h thng thay i
th cac accelerometer c c inh trn mt c cu nn goi la gyro-stablized platform. C cu
nn o gm co ba gimbal, la nhng vong co th quay quanh mt truc i qua ng kinh cua
vong, va cac gimbal nay c lin kt vi nhau ln lt t ngoai vao trong sao cho mi gimbal
co th quay quanh truc. gimbal trong cung, ba cam bin gyro c b tri theo cac truc tng
t nh accelerometer, va co chc nng loai bo hin tng precession ( s thay i vi tri truc quay
cua cac gimbal), do o am bao cho phng hng cua cac gimbal khng i. Loai cam bin
gyro thng s dung trong gimbaled system thng la loai gyro tich phn n truc (intergrating
rate gyro), va tin hiu ng ra c a vao mt ng c servo cua gimbal tng ng iu
khin sao cho phng hng cua gimbal khng i trong h toa quan tinh. Gimbal trong cung
lun co phng hng khng i trong qua trnh chuyn ng cua h thng, cac accelerometer
va cac gyro c c inh trong mt mt phng nn trn gimbal nay. Cu tao cua gimbaled INS
thc s rt phc tap,vic ch tao c kt cu gimbal at c chinh xac cao la cc k kho
khn, iu nay dn n vic gia thanh cua gimbaled INS rt cao.

Strap down INS co kt cu c khi n gian hn, it phn t chuyn ng hn so

vi gimbaled INS, bi v trong h thng strapdown khng h co kt cu gimbal. Trong strap
down INS, cac accelerometer va gyro c c inh ngay trn kt cu cua h thng,. Tuy nhin
trong strapdown INS, nhng ng dung cua cng ngh thng tin trong x ly tin hiu la rt quan
trong, va nhng linh kin strap down thng la cac cam bin in t, quang hoc hay ban dn.
Trong strap down INS khng co thanh phn nao la chuyn ng v mt vt ly, nhng trang thai
cua h thng trong h quy chiu gn lin vi h thng c o bi cac cam bin gyro (ma thng
s trang thai o c y chinh la vn tc goc quanh cac truc tng ng). Cac accelerometer
c s dung o gia tc dai cua h thng theo cac truc trong h toa chuyn ng. Strap
down INS tinh toan cac gia tri o c kt hp vi phep bin i toa suy ra c trang thai
va vi tri cua h thng.

Vi nguyn ly n inh phng hng cac gimbal th gimbaled INS gp phai sai s
kha ln khi h thng thay i qu ao mt cach t ngt ma cac gimbal cha n inh kip thi,
hin tng nay goi la gimbal lock. i vi strap down INS th khng xay ra hin tng trn.

Tc cp nht d liu cua strap down INS nhanh hn gimbaled INS rt nhiu
ln. Gimbaled INS co th hoat ng tt vi tc update la 50-60 ln/ giy, nhng i vi strap
down bnh thng la 2000 ln/ giy. Va tm o cua cac gyro s dung trong strap down INS ln
hn trong gimbaled INS.
3.2.2.2

n vi o lng qun tnh IMU (Inertial measurement unit)

Page 51

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

n vi o lng quan tinh la m t thit bi i n t c dung o va cho ra cac

gia tri v n tc, hng, gia tc trong trng cua m t phng ti n chuyn ng, in hnh la may
bay. IMU s dung s kt hp gia cac cam bin gia tc va cam bin gyro. IMU thng c
dung iu khin qua trnh chuyn n g cua cac phng ti n v n chuyn ho c robot t hanh.

n vi o lng quan tinh la thanh phn chinh cua h thng inh vi quan tinh
INS c s dung trong hu ht cac may bay, tau thuy, tau ngm, phi thuyn vu tru, cac phng
ti n v n chuyn ng b cung nh la trong robot t hanh. Cac d li u thu th p c t cac
cam bin trong IMU cho phep may tinh co th xac inh vi tri cua phng ti n chuyn ng, s
dung phng phap tinh toan c bit n di tn goi dead-reckoning.

IMU hoat n g bng cach xac inh gia tri hi n tai cua gia tc khi s dung m t
ho c nhiu cam bin gia tc. No cung phat hi n nhng thay i cua cac goc quay nh Roll,
Pitch va Yaw s dung m t ho c nhiu cam bin gyro.

Trong h thng inh vi, cac d li u o c t cam bin s c a vao may

tinh tinh toan va cho ra vi tri hi n tai da trn v n tc va thi gian.

Hnh 3.9: Cac goc Yaw, Pitch va Roll trong h thng inh vi quan tinh

Nhc im c ban nht cua IMU trong vi c inh vi o la chung thng bi li

tich luy. Bi v h thng dn ng lin tuc thm vao nhng thay i c phat hi n vao vi tri
a c tinh toan trc o, bt k m t sai s nao trong qua trnh o lng, du la nho, cung s
c tich luy dn dn. iu nay dn n vi c tri cac ai lng tinh toan, ho c m t s thay i
khac bi t ngay cang tng gia gia tri vi tri ma h thng tinh toan va gia tri thc t cua thit bi.

IMU thng ch la m t thanh phn cua h thng inh vi. Cac h thng khac s
c s dung lam chinh xac cac gia tri sai l ch ma IMU mc phai trong qua trnh chuyn
ng cua thit bi, in hnh nh h thng inh vi GPS, cam bin lc trong trng, cam bin v n
tc bn ngoai ( bu s tri v n tc), h thng o khi ap hi u chnh chinh xac gia tri cao ,
va m t cam bin la ban i n t giup xac inh t trng.

Cu truc cua IMU thng thng bao gm m t khi vi 3 cam bin gia tc va 3
cam bin gyro. Cac cam bin gia tc c t sao cho cac truc o cua chung trc giao vi nhau.
Chung o gia tc quan tinh, in hnh la gia tc trong trng. Cac cam bin gyro cung c t
trong h trc giao tng t, o vi tri goc quay co tham chiu n nhng h thng phi hp a
c chon.
3.3

CC CM BIN CU THNH MT N V O LNG QUN TNH IMU

Page 52

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

M t n vi o lng quan tinh co th c tao nn bi nhiu cam bin khac nhau

nhm em lai chinh xac cao cho vi c inh vi. Tuy nhin, m t n vi o lng quan tinh c
ban s bao gm cam bin gia tc ( accellerometer) va cam bin gyro nh a trnh bay trn va
co th co cam bin t trng( magnetometer).

3.3.1

Cam bin Gyro

Thut ng gyroscope tc con quay hi chuyn xut hin t gia th k XIX, va

trong nhng thp nin gn y no c s dung rng rai va c thay th trn toan cu vi t
gyro. Nhng ly thuyt ban u v con quay hi chuyn c ap dung giai thich v chuyn
ng cua mt vt th quay nh Trai t. Va dn dn gyro a c phat trin va ng dung rng rai
trong nhiu linh vc, va c bit la trong h thng inh vi quan tinh INS. Co th chia gyro thanh
3 loai vi nguyn ly hoat ng, cu tao va kha nng ng dung khac nhau nh sau: gyro c, gyro
quang, va gyro in. Khi u trong s phat trin la gyro c, hoat ng da trn nguyn ly con
quay hi chuyn truyn thng. Gyro c c s dung nhiu trong h thng gimbaled INS. Sau
o la s ra i cua gyro quang hoc, va gn y nht la s phat trin cua cac loai gyro in ng
dung bi cng ngh vi c in t MEMS (micro-electromechanical systems), gyro quang va
gyro in ng dung nhiu trong strap down INS.

3.3.1.1 Gyro c
Gyro c ( hay thng goi bng thut ng rotating wheel) hoat ng da trn nguyn ly
bao toan moment cua mt vt th chuyn ng quay khi tng cac lc tac dung ln vt trit tiu.

Cu tao c ban cua mt gyro c gm co mt ia kim loai hnh tron c treo l

lng bng mt si dy an hi trong mt b khung. ia kim loai c truyn chuyn ng quay
bng cach tac ng ln si dy dan hi treo doc theo truc cua banh xe. Khi chuyn ng quay th
Goi A la moment quay cua ia gyro, I la moment quan tinh cua ia i vi truc quay, w la vn tc
quay cua ia quanh truc. Da trn nguyn ly bao toan moment ta co:

Ari I r .wiri
i vi cu tao dang ia tron ta co th tinh c moment quan tinh cua
ia trong h toa t tai tm ia va cac truc quanh tng ng vi cac
truc quay cua gimbal. Lu y la trong cu tao cua gyro c, trong tm cua
ia xoay phai trung vi trong tm cua h gyro gimbal:
0
I rx 0
I r 0 I ry 0

0
0 I rz
Trong qua trnh hoat ng, nu nh trang thai cua h thng thay i, th
cac gimbal co th chuyn ng quay quanh truc, nhng ma ia gyro vn
Page 53

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

gi nguyn phng hng cua truc quay, va gi trang thai n inh trong
khng gian quan tinh. Con goc quay cua cac gimbal co th o c bng
cac thit bi o goc gn lin vi truc quay cua gimbal, cac thit bi nay c
goi bng thut ng pickoff.
S lng gimbal trong cu tao cua gyro quy inh s bc t do chuyn
ng quay cua gyro. Vi du nh gyro co hai bc t do th co cu tao gm co
hai gimbal. Nu truc quay cua mt gyro hai bc t do la thng ng th no
c goi la gyro thng ng (vertical gyro), con nu nh truc quay nm
trong mt phng nm ngang th goi la gyro inh hng (directional gyro),
con mt gyro co truc quay khng theo mt phng cu th nao th goi la
gyro t do (free gyro). S n inh trong khng gian ba chiu oi hoi mt
gyro thng ng va mt gyro inh hng, hoc la hai gyro t do. Mt loai
gyro c co tm ng dung rng rai la gyro mt bc t do, tc la cu tao ch
gm mt gimbal quay quanh truc ma thi, loai gyro nay thng c goi
la intergrating rate gyro.
Gyro c la loai gyro truyn thng, co chinh xac cao, va c ng dung
nhiu trong h thng dn ng cua tn la hay tau ngm. Gyro c co
chc nng chu yu la n inh phng hng chuyn ng cua h thng
thng. Gyro c chu yu c s dung trong h gimbaled INS bi v kt
cu c khi rt phu hp. Hu ht cac h thng INS u s dung kt hp ba
cam bin gyro loai rate intergrating mt bc t do n inh phng
hng trong khng gian.
3.3.1.2 Gyro quang (Optical gyro)
Nguyn ly hoat ng cua gyro quang da trn hiu ng Sagnac,c nha
vt ly hoc ngi Phap Georges Sagnac tm ra nm 1913, la hiu ng giao
thoa anh sang tng quan vi vn tc quay. Hiu ng Sagnac c o bi
mt vong o giao thoa. Mt lung sang c tach ra thanh hai va cho lan
truyn trn cung mt qu ao vi hai chiu trai ngc nhau, va qu ao o
la mt vong kin, ng thi cung la mt thit bi o giao thoa, goi la dung cu
o giao thoa vong (ring interferometer). Thit bi nay c gn ln mt gia
co th quay c. Khi gia quay th vong giao thoa s bi dich chuyn,
dn n vn giao thoa thu c trn mang quan sat s bi dich chuyn, va
dich chuyn cua vn tuy thuc vao dich chuyn hay goc quay cua gia
.
Cu tao cua gyro c la ht sc phc tap va gm nhiu b phn chuyn
ng, con gyro quang th co cu tao n gian hn va hu nh khng co b
phn nao chuyn ng. Co hai loai gyro quang c phat trin va ng dung
rng rai trong cng ngh do ng chinh xac cao, o la gyro vong laser
va gyro cap quang.
Gyro vng laser (RLG-ring laser gyro):

Page 54

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Mt gyro vong laser c ban co cu tao la mt ranh kin co dang hnh hoc
(thng la ba, bn, hay sau canh) c khoan bn trong mt khi thuy tinh.
Cac ranh nay la ng chuyn ng cua mt hn hp khi, nh la helium va
neon.
Cac phn t khi chuyn ng phat ra anh sang khi bi kich thich bi in
trng trong ranh kin. Chum sang c tach i va chuyn ng theo hai
hng ngc chiu nhau trong ranh kin, mt chum sang chuyn ng theo
chiu kim ng h va chum sang con lai chuyn ng ngc chiu kim
ng h. Tai trang thai cn bng cua khi thuy tinh, th thi gian di chuyn
cua hai chum sang i ht quang ng la mt ranh kin la nh nhau, cho
nn hai song anh sang la cung pha nhau. Con nu nh khi thuy tinh quay
i mt goc trong khng gian thi thi gian hai chum sang i ht mt ranh kin
khng con nh nhau na bi quang ng i co s sai lch, va song anh
sang cung bi lch pha nhau. Nh th s lam dich chuyn vn giao thoa trn
thit bi quan sat.

Hnh 3.10: Cu tao cua gyro vong laser

Page 55

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.11: Gyro vong laser thc t

Gyro cp quang (Fiber optic gyro) :
Mc du s phat trin cua gyro vong laser la rt thanh cng, nhng vi
nhng nghin cu trong linh vc cap quang a tao ng lc cho nhng
thit k cho gyro cap quang.
nh sang c truyn trong si cap quang c qun thanh cun, va chiu
dai ti a co th ln n 5km, chiu dai cua si cap quang dai hay ngn s
thay i nhay cua gyro cap quang. Thng thng gyro cap quang co
kich thc nho, nh va cng sut be. Ngoai cun dy cap quang th gyro
cap quang con co nhng thanh phn c ban sau: mt ngun sang, mt b
phn cc anh sang, mt b chia i lung sang, mt b chnh pha anh sang
va mt cam bin quang phat hin anh sang.
Nguyn ly hoat ng cua gyro cap quang hoan toan tng t nh gyro
vong laser, anh sang sau khi c phn cc va c tach lam hai s lan
truyn theo hai hng ngc nhau trong cun cap quang. B phn cc am
bao cho ca hai lung sang phai c phn cc ng u cho tc lan
truyn bng nhau. Tai vi tri cn bng, gyro khng quay th khng co tin
hiu g cam bin quang bi v hai chum sang la ngc pha nn trit tiu
nhau. Nu gyro quay mt goc th hai chum sang lch pha kt hp s tao nn
mt chum sang kt hp, cng chum sang kt hp c cam bin quang
chuyn thanh in ap tng ng. in ap cang cao nu nh goc quay cang
ln.

Page 56

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.12: Cac thanh phn c ban cua Gyro cap quang
Gyro cap quang co c tinh vt tri so vi gyro vong laser ch no co th
cho phep ta o vi nhiu nhay khac nhau tuy theo ng dung bi chiu
dai cun cap quang co th thay th c. Va cung vi kich thc nho gon
nn gyro cap quang c ng dung ngay cang rng rai trong cac h thng
inh vi va do ng.

Hnh 3.13: Gyro cap quang thc t

3.3.1.3 Gyro rung (Vibrating gyro)
Vi gia thanh thp hn hn so vi cac loai gyro khac, gyro rung c ng
dung nhiu trong linh vc dn dung gii. Gyro rung chu yu c dung
o goc heading cua xe trong luc di chuyn, va gyro rung ch ong vai tro h
tr cho GPS, h thng inh vi chinh cua cac loai xe nay.
Cng ngh ch tao cac vi c cu, vi cam bin va h thng vi c in
(MEMS- micro electromechanical system) ang phat trin nhanh chong
trn toan th gii, va ngay cang c ng dung nhiu san xut ra nhng
thit bi o quan tinh vi san lng ln va gia thanh ha. Cam bin gyro rung
Page 57

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

vi c (micromechanical vibrating gyro) hay cam bin o vn tc goc co

tim nng rt ln trong nhiu ng dung nh la nhng cam bin c s trong
linh vc do ng va iu khin. Cac cam bin MEMS thng khng co
chinh xac cao va gia thanh rt thp. Tuy nhin kha nng hoat ng cua
cam bin co th c nng cp nh cac cng cu khac nh cac b loc x
ly tin hiu.
Trong hu ht cac loai gyro rung, th thanh phn cam bin c ban la mt c
cu gm mt khi c gi bng hai lo xo.
Khi gia c gi bi hai lo xo goi la khi lng chiu lc (proof mass).
Proof mass co th dao ng mt trong hai ch , mt ch khi khng
co lc coriolis, goi la ch c s con ch con lai la ch th cp khi
proof mass dao ng trn mt h thng ang quay va chiu lc coriolis.
Thng thng gyro rung hoat ng ch th cp, bng cach chuyn i
ap in s dao ng cua proof mass, ta co th o c vn tc quay cua h
thng. Vn tc cang ln th in ap cang cao. Do proof mass c cu tao
t vt liu ap in cho nn gyro rung bi anh hng rt ln bi nhit . Vi
cng ngh MEMS, hin tng nay a c han ch, do o tinh nng cua
gyro rung cang c nng cao.

3.3.2

Cam bin gia tc

Mt gia tc k s dung quan tinh cua 1 vt o s thay i gia gia tc
ng hoc trong khng gian quan tinh so vi gia tc trong trng .
Gia tc k dang con lc co cu tao kha ging nh gyro rung, gm 1 vt
nng (proof mass) c treo bi 1 lo xo. Vt nng co th chuyn ng doc
theo lo xo. Con lc c t vao mi trng giam chn han ch anh
hng cua rung ng. Hnh sau minh hoa cho cu tao 1 gia tc k con lc

Page 58

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Hnh 3.14: Cu tao gia tc k con lc

Nu nh co 1 gia tc th lo xo s bin dang. Da trn bin dang cua lo
xo ma ta co th tinh c gia tc cua h thng. o la nguyn ly hoat ng
cua gia tc k con lc.
Hin nay, ngi ta thng dung gia tc k vong kin, la m t loai gia tc k
co kha nng lam vic tt hn hn gia tc k dang con lc va hu nh khng
co thanh phn nao di chuyn, nh gn thm m t cun dy bn ngoai proof
mass. Nguyn ly hoat ng cua no la khi co dich chuyn nho cua proof
mass th s sinh ra 1 dong in trong cun dy, tao m t lc in t theo
chiu ngc lai kh i chuyn ng. Do o, co th suy ra c gia tc
chuyn ng cua h thng bng cach o dong in chay trong cun dy.
Vi s phat trin cua cng ngh MEMS (microelectromechanical systems)
th cac loai gia tc k c san xut hang loat vi gia thanh thp va cht
lng kha tt.
3.3.3 Cam bin t trng:
Cam bin t trng la mt thit bi dung do cng va hng cua t
trng ( c tao ra trong phong thi nghim hoc t trng trai t).
n vi trong h thng o lng quc t la Tesla. Nhng y la n vi kha
ln nn ngi ta dung n vi nanotesla (nT). Trong thc t th ta thng
o t trng vi n vi la Gauss.
1 Gauss = 100 000 (nT )
Page 59

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

T trng trai t la mt trng th. No thay i theo rt nhiu nguyn

nhn khac nhau. T trng trai t co cng khoang 20 000 nT xich
ao va khoang 80 000nT gn cac cc.
Cam bin t trng c chia ra lam 2 lao chinh la :
Scalar magnetometer : o tng cng t trng.
Vector magnetometer: o tng thanh phn cua t trng
theo cac truc, lin quan n kha nng inh hng cua thit
bi.
3.4

CC TNH TON X L D LIU S B T CC CM BIN:

Trong cac tinh toan di y ta iu gia s rng vt gn thit bi co toa tuyt
i la XbYbZb , trong khi toa cua cam bin ( toa tng i) la XbYbZb:
Mi quan h gia h trucj tao tuyt i va h truc toa tng i c xac

RT xyz
inh nh sau ( theo cng thc 3.4) vi

R
la ma trn chuyn cua

X b '

Y ' RT
xyz
b
Zb '

cos cos
X b '
Y ' cos sin sin cos sin
b

Z b '
cos sin cos sin sin
3.4.1

xyz

X b
Y b
b
Z
cos cos
cos cos sin sin sin
sin cos sin cos sin

X ly d liu t cam bin trng trng (accellerometer):

Hnh 3.14 Tinh toan goc goc nghing (tilt) t accelerometer

Page 60

sin
cos sin
cos cos

X b
Y b (3.7)
b
Z

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

T accelerometer ta co th xac inh c goc nghing cua h thng bng

cach xac inh goc Roll va Pitch.
Trong h toa tng i XbYbZb , ta co cac gia tri trong trng hp cam
bin t vung goc vi phng cua trong lc nh sau:
Xb = Yb =0 va Zb = 1 g ;
Nhng trong h truc toa tuyt i ta s o c cac gia tri la
Ax, Ay , Az, cac gia tri nay a c chun hoa ( normalize).
Thay vao phng trnh 3.7 ta c
Ax
cos cos

Ay cos sin sin cos sin

A
cos sin cos sin sin
z

cos cos
cos cos sin sin sin
sin cos sin cos sin

sin
cos sin

cos cos

T y ta c th suy ra l :
Gc Roll = = Arcsin( Ay / cos )
Gc Pitch = = Arcsin(- Ax )
Ch rng

Ax , Ay , Az c chun ha, ngha l

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

3.4.2 X ly d liu t cam bin t trng

Hnh 3.15 : Tinh toan goc xoay ( Yaw) t magnetomter

Nh trnh bay trn ta a xac inh c goc nghing nh n cam bin
accellerometer. By gi ta s xac inh goc xoay theo phng ngang (Yaw)
theo cam bin t trng. Cach xac inh c trnh bay nh sau:

Page 61

0
0
1

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

Goi Mx, My, Mz la hnh chiu cua vector cng t trng B ln cac
truc t tng i va Mx, My, Mz la hnh la cac gia tri o c trong
h toa tuyt i.
Trong h toa tuyt i th r rang Yaw s c tinh nh sau:
Yaw = = arctan( My /Mx) (3.8)
Nu thit bi nghing theo cac goc Roll va Pitch th ta quy v h to tuyt
i nh sau:
M 'x

T
T
M ' y R ( X , ) R (Y , )
M '

x
M

M
y (3.9)

M
z

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

Mx = Mxcos + Mysin
My = Mx sinsin + Mycos - Mzsincos
Mz = Mx cossin + Mysin - Mzcoscos
Cng thc 3.8 c cc trng hp nh sau:
Yaw = = arctan( My /Mx) ( nu Mx >0 va My>0
Yaw = = 180 + arctan (My/Mx) ( nu Mx <0)
Yaw = =360 + arctan ( My/Mx) ( nu Mx > 0 va
My<=0)
Yaw = = 90o ( nu Mx = 0 va My <0)
Yaw = = 270 ( nu Mx = 0 va My >0)
Chu y rng tng t nh trng hp cam bin gia tc ta cung co Mx, My,
Mz cung c chun hoa, nghia la
M ' 2 x M ' 2 y M '2 z 1
Nu iu ny khng tha ngha l c nhiu t cc
ngun t trng khc .

3.4.3 X ly d liu t cam bin vn tc goc (Gyro):

Vic tinh toan s liu t Gyro la kha phc tap v no cho ra d liu la vn
tc goc nn xac inh c goc quay th chung ta phai tich phn theo thi
gian.
Cac phng trnh dung tinh vn tc thay i( vi phn) goc Euler t Gyro
nh sau:

Page 62

Chng 2:
H thng IMU v v cc tnh ton lin quan

GVHD: ThS. Hunh Vn Kim

SVTH: L Vn Mi

t

1
t
cos

0
0

sin
cos cos

cos

sin sin

cos
sin cos
cos cos

x

y

z

Vi , , ln lt la goc Euler tai thi im ly mu trc o

(t-1) . Goc Euler tai thi im hin tai c xac inh nh sau:

(t ) (t 1) t
(t )

(t 1) t .t
(t ) (t 1)

Page 63