DONG QUAN SU SU DUNG THIET BI MEMS-INS BANG BO LOG KALMAN MO'
Le Himg Urn "\ Pham Hdi An ''>, Ngiiyin Vil *^, Vu Minh Khiem''\ Pham Thanh Hd'^
"^ Dgi hoc GTVT.''^ Vien Tir dgng hda KTQS, '^ Vien Do luong VN Tdm tdt:
Bdi bdo dua ra phuvng phdp ddnh gid dg nghieng mat phdng b? ciia phuong tien codgng quern su tren ca so sir dung cdm biin gia tdc vd con quay vi co dien tu. De ddnh ^id dg nghieng hdng hen logi cdm bien tren. mgt bg Igc Kalman kit hgp logic md dugrc de xudt. Cac kit qud thue nghiem se kiem tra ddnh gid bg Igc ndy.
Abstract:
This article relates to methods for asses.sment the tilt of the base plane of the military vehicle by using acceleration sensors and micro-electromechanical gyroscope. To evaluate the tilt using these two types of sensors, a Kalman filter combining fuzzy logic is proposed.
The experimental results will evaluate this filter.
I. DAT VAN DE
Trong tdc chidn hien dai, djch thudng td chirc tdn cdng dudng khdng vdi cac loai hda lyc manh nhu ten lira hdnh trinh, mdy bay tdm thdp kdt hgp vdi cdc he thong trinh sat^ phat hien muc tieu nhdm tieu diet nhanh cdc muc tieu xdc djnh. Dieu ndy yeu cau cac he thdng phong khdng tdm thdp cua ta phdi dugc tfch hgp tren cdc phuong tien co ddng nhu xe hang ndng, banh hoi hodc banh xi'ch,... Tren xe bao gdm cdc he thdng trinh sat, bat bam muc tieu, ngam bdn,... De co the nhanh chong tieu diet muc tieu, thi mdt vdn de vd ciing quan trgng la ta phai xdc dinh dugc cdc gdc nghieng cua mat phdng be Idp ddt tren than xe, tir dd dua vao lam cac tham sd ddu vdo cho cdc thudt todn ban ddn, ket hgp he thdng bat bam muc tieu tfnh toan cac phan tir bdn.
Khdi do ludng qudn tfnh MEMS-INS Idp ddt tren phuang tien (gdm gia toe kd va con quay) dugc sii dung de tfnh todn do nghieng mat phdng be. Trong he thong dan dudng quan tfnh truyen thdng, cac gid trj goc nghieng se dugc tfnh qua phep ti'ch phan tdc dg gdc nhan dugc tir ddu ra ciia con quay. Vdi thidt bj MEMS cd sai sd ngau nhien Idn thi phuong phap ndy khdng phu hgp. Do phep tfch phdn, gid tri gdc tfnh dugc tir con quay se bi phan ky theo thdi gian, tuy nhidn gid trj ddu ra ciia con quay lai dn djnh vdi nhieu, va nd rdt tdt vdi cac ung dyng trong thdi gian ngdn. Cdc gid trj gdc do dugc tir gia tdc kd cd sai sd khdng bi phdn ky theo thdi gian do phep tfnh tfch phan thi lai chju dnh hudmg ciia lyc gia toe trgng tnrdng tren cd 03 true tga do. Vdi dac trung nhieu khdc nhau ciia gia tdc kd va con quay nhu vay,4)ai bao dd xudt mdt thudt todn dp dung bd Igc Kalman trgn dir lieu ddu ra cua chung dd tang do chfnh xdc khi ddnh gid do nghieng mat phdng be cua phuang tien co dgng quan sy. Thuat toan nay se hieu chinh gid trj ma tr^n sai sd phdp do R^ dk ban chd sai sd do dnh hudng qua tnnh ddng bgc cua phuang tien tdi ddu ra ciia gia tdc kd bdng bd didu khidn md TSK vdi phuong phap gidi md trung binh tam.
II. MO HINH BO LOC KALMAN CHO VIEC TINH T O A N DO CAN BANG 2.1 Mo hinh xac dinh goc nghieng ciia mat phdng be
Bdi bao su dung phuang phdp gdc Euler tfnh gid'tri do nghieng mat phdng be. Cac goc Euler bao gdm gdc chiie ngoc 6, gdc nghieng (j>. va goc hudng y/
y\e)
Hinh 1. He toa do gan lien (b) va cac goc Euler
Ma trdn COSIN chi phuang tir he tga do gdn lidn (/?) sang he tga do dan dudng (n) dugc tfnh bdng cdng thiic dudi day [I], [3].
C; =(C':,f=R,{-yr)R,(-e)R,{-<P)
^cOcy/ -c^iif-^cipsQcxif s^y/-\-c^6cy/ ' c6s\f/ cef)cy/ + c(ps9sii/ -s0ci/r + c0sdsi/r -s6 S0C0 cejJcO vdi R: phep quay, s: sin, c: cos.
Mdi lien he giua gdc Euler va gia tri ddu ra ciia con quay tren moi true dugc bidu didn bdng cdng thiic dudi day:
(2)
^.,"
^.v 0).
—
'f
0 0
+ RM)
"o"
^
0
^R,{(P)R^.{G)
"0"
0 y^
Thay thd ma trdn cac phep quay R^.(^),R^.(ip),R.(i/r) vdo cdng thue (2) ta dugc
^
1 sin ^ tan 0 cos ^ tan 6 0 cos^ -sinip 0 sin ^ sec ^ cos ^ sec ^
CO.
CO
y
CO.
(3)
vdi (o^, (Oy, 0)^. cac gia tri ddu ra ciia con quay tren cdc true x, y, z-
De tfnh dugc do nghieng mat phang be, ta chi quan tdm den gdc chiic ngdc vd goc nghieng dugc bieu dien qua cdc cdng thue dudi day:
(^6;^.+fi;_jSin^tan^ + fi;. cos^tan^ (4)
^ cOy cos ip-a),sin^ (5) Cac cdng thue nay se dugc su dung trong md hinh he thdng cua bd Igc Kalman.
Gdc hudng yr cua phuang tien dugc tfnh theo cdng thiic sau:
.i«fe=fl;^,sin^sec^ + 6;,cos^sec^ (6) Trong bai todn tfnh do nghieng, ta se khdng quan tdm den thdnh phan goc hudng y/
2.2 Cau true ciia bo loc Kalman md:
Bg Igc Kalman md ddnh gid gia tri do nghieng dugc cho trong hinh 2, d day dau ra cua con quay dugc su dyng lam md hinh he thdng, do nd khdng chiu dnh hudng Idn bdi tdc ddng do qua trinh ddng bgc cua phuang tien. Dau ra cua gia tdc ke sir dung lam md hinh do ludng do chiu anh hudng bdi nhidu trong he thdng. Nhu vay, khi phuang tien chuyen ddng, bg Igc Kalman cdn hieu chinh dd sd do ciia con quay tin cay ban ciia gia tdc ke. Phuang phdp lap luan logic md TSK se duoc dp dung de lien tuc hieu chinh bd Igc. Khi chuyen ddng cua phuang tien dugc do ludng bdi cac thiet bi gia tdc vd con quay, bd dieu khien md se quyet dinh tham sd Rk tuong ung de dua vdo bd Igc Kalman.
Con quay
Gia tdc kc
6.(1)
Md hinh he thdng
BQ loe Kalman
Md hinh do ludng
0,0
Bp md danh gia/?
Thdng so dieu chinh
6. (/>
Hinh 2. C^u true ciia bp loe Kalman md d^nh gi^ dg nghieng mat phSng be ciia phuong tien cff dgng quan sy
2.3 Mo hinh he thdng:
Trong bai todn ndy, cdc bidn ddi ciia ^6c Euler se dugc sir dung Idm cdc bien trang thdi [3].
Cac bidn trang thdi ndy dugc xdc djnh bdng cdc cdng thirc dudi day:
x, =-sin^ (7) Xj =s\x\(pcos6 (8)
A-3=COS^COS^ ( 9 )
Md hinh he thdng ciia bd loc Kalman nhdn duoc tir phdp dao hdm cac phuong tnnh (4) vd (5)
4 = - ^ o s 9 = ft;.x2 - fi;,.A-3 (10) -^ = ^os(Z)cos^-(^in<^sin^ = -ft;,A-, +co^Xj^ (11) jSi^=-<^in^cos(9-^os(Z)sin^ = ft;^A, -co^x^^ (12) Cdc phuang trinh tren cd the dugc vidt lai theo dang ma trdn vector dudi day:
4 A A
=
0
- 0) .^
0),
CO .^
0
-0)^
- OL
CO
0
-h w(r) (13)
vdi w(f): Id nhieu qua tnnh phan bd Gausse vdi trung binh 0.
Md hinh qud trinh ndy chi sir dung dau ra cua con quay vd cac phuong trinh tuyen U'nh dan gidn, do vdy bd Igc chi chiu dnh hudng cua sai sd con ^uay, nhung nd se khdng hieu qua khi cd nhidu ben ngoai tdc dgng do khdng dugc bii trir sai sd.
2.4 Mo hinh do ludng:
Ddu ra cua gia tdc kd dugc sir dyng lam cdc phuang trinh do ludng, cac ddu ra ndy dugc idu dien trong he tga do gdn lien {b) bdng phuong tnnh (14) dudi day:
i& -ru bidu aien trong
f''=^^aJ',,,xv'-g-'^
+^ s i n ^ - ^ s i n ^ c o s ^ - ^ c o s ^ c o s ^
(14)
•^ qw
-qw y&. -qu + pv vdi ll, V, w: gid tri van tdc tren mdi tmc tga do,
/*, A i&: gia tdc theo 03 true tga do.
Cdc gid tri « V, w f, A 1& khdng do ludng tryc tidp dugc tir ddu ra ciia gia tdc kd ma phai lay tu cac cam bien ben ngoai khac, do vdy ta bd cdc sd hang nay trong cdng thirc (14) dd thu dugc trang thai on dinh cua khoi do ludng INS trong phuang trinh dudi day
fy
f:
-sin<9 sin(Z)cos(9 COS0COS.6
= -8
•^i
-VT
. • ^ ? .
(L5) Neu nhu cac thanh phan van tdc va gia tdc tren tdn lai ta se coi nhu la nhieu.
Them vao dd de tuyen tfnh hoa md hinh do ludng nay, chimg ta sir dung rang budc sau:
.Vf + .v; -I- .vj = sin' ^ + sin" ^cos' 6 + cos" (^eos' 9 -1 Tir rang budc nay, cdng thue (15) se dugc vidt lai nhu sau:
(16)
/ v
/ :
I
/ I ( A - , , A , , . V , ) =
-8'\
-8^2
- 8'h (17)
I'l' + -VJ -f- A-;
Ma trdn do ludng H dugc chi ra trong cdng thirc (18) dudi day:
8
HJJi =
dx 0 0 2A,
-8 0
2 A ,
0 -8
2A-3
(18)
Cdc phuang trinh do ludng nay dugc coi nhu khdng chju dnh hudng ciia cdc thdnh phdn lyc gia tdc khdc ngoai tnur lyc trgng tmdng. Trong thye td, khi phuang tien chuydn ddng, md hinh do ludng nay se khdng cdn ddng tin cay. Dk khdc phuc nhugc didm nay, cdn hidu chinh tham sd cua bd Igc Kalman dya tren bd ddnh gid md.
III. THIET KE HE M 6 DANH GIA GIA TRI MA TRAN HIEP BIEN DO LlTdNG Md hinh md Takagi-Sugeno la md hinh md rdt hiru dyng vdi vide sir dung cdc tap dii lieu vdo/ra cho tmdc [2]. Thdng qua tap mdu cdc gid tri do ludng cua gia tdc kd vd con quay thu thdp dugc trong cdc qud trinh ddng bgc khdc nhau ta se xdy dyng dugc bd didu khidn nid tuong ung. Trong bai todn ndy, cdc ludt md se dugc su dung dk thay ddi gid tri ma trdn hiep bien dg ludng R.
Do lgi ciia bd Igc Kalman dugc tfnh theo cdng thue dudi day:
Kt — PL H t /?; T ty-i (19)
Qua cdng thurc tren ta thdy rang gid tri cua R ty Id nghich vdi do Igi K, vdi gia tri R Idn thi tdc do udc lugng cham ban vd bd Igc gan nhu khdng diing den cdc gid tri do ludng dugc trong khi tin tudng ban vdo md hinh qud trinh vdi gid tri K nhd. He thdng chiing ta thiet ke d day se tin tudng hem vdo cac gid tri dau ra cua con quay dugc sii dung trong md hinh qua trinh trong qud trinh ddng hge. Nhu vay gid tri cua R phdi Idn khi phuang tien chuyen ddng. Qua trinh dgng hge ciia phuang tien dugc danh gia qua tdc do gdc vd gia tdc cua nd. Khi gid tri cua tdc do gdc va gia tdc Idn, gid tri R phdi dugc tang len do chuyen dgng ciia phuang tien. Ddnh gia do Idn ciia tdc do gdc va gia tdc, ta dua ra 02 chi sd ddng bgc trong do chi tidu Ajnorm dai dien cho dgng bgc cua gia tdc ke vd Omegajionn la ciia con quay.
A_nonn = {\ff^_-g)lg (20) Omega _norni =\cdl^^ (21) Cdc chi tieu ddng bgc nay dong vai trd ddu vdo cua he md ddnh gid R, se dugc chia ra hdm
02 tap md LARGE va SMALL the hien hai miirc do dgng bgc Idn vd nhd.
Ddu ra la 04 luat md dudi ddy
1. Neu gid tn A_norm nhd, gid tri Omega_norm nhd thi z = 0
2. Ndu gid tri Ajionn nhd, gid trj Omega_norm Idn thi z = a^a + a^/3 + AJ
FI'S v'.iri.jllf <
-)!' ',<•' I Ku.i
1-rivrtii I
'yv
)iMrao-rpOfm
infiiil vmitiM "A-norin" nr<ul variiibH •Oiwgo.rwrm"
Hinh 3. Tap md' chi so dpng hge cua gia toe ke va con quay Sir dung ddu ra ciia cac'luat md nay, gid trj R se dugc thay ddi bdng ludt sau:
/? = ^A-,Z-+A-2)/,,, 0^ (22)
V 0 y^
Mdi quan hd giua gia trj ddu vao/dau ra dugc chi ra trong hinh 3 vdi «/ = 3.5, 02 - 8, c; = 0.5, cdc gia trj* nay dugc nit ra bdng thye nghiem. Trong bdi todn ndy vdi cdc thiet bi dan dudng quan tfnh cua hang Analog Device da chgn thi gia trj thi'ch hgp Id ki = 10 vd ki - 400.
IV. KET QUA THU'C NGHIEM
Thye nghiem dugc tridn khai trdn tren thiet bi cua hang Analog Devices vdi con guay ADIS16209 va gia tdc do nghieng ADIS16354. Cac thidt bj dugc gdn cd djnh vdo 01 khdi do ludng gdn tren ban xoay trong phdng thf nghiem theo gdc td cd bg do gdc vdi sai sd Imrad.
-F^r^iffit^^-f
Hinh 4. Con quay ADIS16354 va gia toe ke ADIS162q9
Tir bg dir lieu thu thdp tren cdc dja hinh khdc nhau vdi tdc do chuyen dgng khdc nhau, chuong trinh md phdng tren MATLAB-SIMULINK se khdo sdt dii lieu dua ra cac vector ham thudc ciia bd md. Cac vector ham thudc ndy chu yeu phu thudc vao dac tfnh ky thuat ciia thiei bi vd kinh nghiem chuyen gia. Do chuong trinh md phdng cd tdc do cham, de kiem tra bg loc Kalrhan md rdng md de xudt, chiing tdi da cdi ddt lap trinh bdng ngdn ngir Visual C dd kiem tra cdc kdt qud thye nghiem nhdn dugc tir chuong trinh md phdng. Cdc chi tieu dua ra danh gid so sdnh Id cdc gid tri do nghieng nhan dugc tir con quay, tii gia tdc kd va tir bg Igc Kalman md de xudt.
Kiem tra thye nghiem khi cho khdi do ludng qudn tfnh ndm tren bdn xoay trong phdng ihi nghiem (gdc chiic thye td 0.4 do), so sdnh cdc gid tri thu dugc ta cd cac kdt qua cua gdc chiic trong khodng sau: tir gia tdc kd (-0.5) H- ( L l ) , tir con quay (-0.2) -H (0.8), tir bg Igc Kalman mo (0.3) ^ (0.5). Vdi goc nghieng ciing thu dugc kdt qud tuong ty.
'5-1
0 6 c chuc tvr d-iu ra cu.i pA toe kc
!! ;
•; ' ' J ; • , , ' 'i • • ! i ; , f v ! ' H I ' ' : [ • ' ' I ) • " 111.!' ;;;' i , r,. ;i •
I I
i ; • u
0 ( k diuc til J.Jit IJ vUa ..L.II qu.,^
i . A fl
'
I'T'»'!"'fi'i'';'•''.'
, 1^ I I |i
'•''''if''',/'ii'i i.'iuiiisnii'i' l i i n
K 1
12 t i H )fl Dl 2J ::*> ."fl «
- - - V,
10 12 14 1& )|) :H) 22 24 26 28 3
Hinh 5. Cac gia trj gdc chuc tuong li-ng tinh dugc tir dau ra ciia con quay, ciia gia tdc ke va ciia bg loc Kalman md
V. KET LUAN
Trong bai bdo nay, mdt thudt todn tfnh todn do nghieng than xe ciia phuang tien co ddng quan sy dp dung bd Igc Kalman md dugc de xudt. De thidt ke bd Igc Kalman, cdc bidn trang thai da dugc dinh nghTa su dung mdi lien quan giiia cdc goc Euler va tdc do goc. Tir cdc bidn trang thai, md hinh qua trinh dugc bieu dien thanh cdc cdng thue tuyen tfnh, vd trong md hinh do ludng cdc ddu ra cua gia tdc ke dugc su dung tryc tiep ma khdng qua cdc phep chuyen ddi khac. Do md hinh do ludng chiu dnh hudng cua cdc nhieu gia tdc khi vat the chuyen dgng, de
f * *
hieu chinh cdc sai sd nay, mgt thudt todn thfch nghi tham sd dugc cdi ddt su dyng Id gic md de dua ra gid tri Rk hgp ly tuong ung vdi qud trinh dgng bgc chuyen dgng.
T A I LIEU THAM KHAO
[1] Le Himg Lan, Nguyen Quang Hung, Pham Hdi An (2008), "Tich hgp dd lieu da cdm bien trong ddnh gid hudmg chuyen dgng phuong tien giao thong mat ddt dua tren cdc cdm bien qudn tinh su dung he chuyen gia mo". Tap chf nghien cuu khoa hge vd cdng nghe quan sy, Tmng tam KHKT&CNQS, tr 87-93.
[2] T. Takagi and M. Sugeno. (1983) "Derivation of fuzzy control rules from human operator's control actions" Proc. of the IFAC Symp. on Fuzzy Infonnation, Knowledge Representation and Decision Analysis, pages 55-60, July 1983.
[3] Salychev, "Inertial Systems in Navigation and Geophysics", Bauman MSTU Press, 1998.
