XAY DUNG HE DIEU KHIEN ROBOT TU HANH TREN CO SO LOGIC MO KET HOiP THONG TIN VE TINH GPS TRONG BAI TOAN DAN
DUONG TOAN CUC
Phan Qudc Thdng, Ngo Vdn Chung, Bui Mgnh Dung line vicn ky thugt qudn sir
Tdm tdt:
Bdi hao trinh hay ket qua nghien ciiu thur hiin hdi todn ddn duang todn cue cho robot lir hdnh dua tren phirang phdp dicu khien mix ket hgp vai cdc thong tin clic lieu thu tic hi thong thong tin vc tinh GPS rd la hdn dicn tir. Bdi bdo cimg giai thieu mgt so kit qua dgt dugrc cua nhom dc Idi Irong thirc te xdv dung hi thong robot tir hdnh. to chirc chuang trinh xir ly die lieu vd dieu khien. cimg cdc ket qud thir nghiem tren thirc dja dira tren phirang phdp diiu khien dirge neu.
Contract:
This paper presents some researched results in resolving problem of general guiding for self moving robot on the base of fuzzy logic in coordinating with data information from Global Positioning System - GPS and electronics compass. This paper also introduces results in building in practice a self-moving robot, organizing program processing data, and realizing experiments on the field based on the mentioned method.
I. DAT VAN DE
Thdi gian qua, da cd nhiing cdng trinh nghien ciru giai quyet bdi todn dan dudng cho robot tu hanh vugt dja hinh dua tren cac ly thuyet dieu khien hien dai va kinh dien. Ldi giai cho bai toan nay ngay cang dugc hoan thien va cdc ket qua dat dugc cung rat kha quan.
Tuy nhien, trong nhirng nam gan day, cung vdi su xuat hien cua nhieu ky thuat cong nghf tien tien nhu cdc bg vi xir ly hifn dai, cdc bd cam bien cdng nghf mdi, cdc bd thu thong tin ve tinh da cho phep md ra mdt hudng nghien cuu mdi nhdm xay dung cdc hf thong robot ngay cang thdng minh. Day ciing Id mdt van de chua dugc di sau nghien ciiu nhieu a ^
Vif t Nam. i Xuat phat tir thuc te tren, nhdm tdc ^ia de xuat gidi phdp xay dung robot tu hdnh tren dia
hinh sii dung ly thuyet dieu khien md, ket hgp vdi cac thdng tin vf tin GPS va la ban difn tir de giai quyet bai todn dan dudng todn cue cho robot tu hdnh.
II. BAI TOAN DIEU KHIEN ROBOT T l / HANH VA MO HINH NGHlfeN ClTU
Bai todn dieu khien robot tu hanh cd the chia thanh hai loai : bdi todn cue bd vd bai toan todn cue. O bai toan todn cue, ban dd dudng di cua robot co bdn da dugc xdc djnh tmdc. Khi
dd, van dk ddt ra la can xay • dgng hf thdng dieu khien de dan dudng robot tu hanh di dugc den diem dfch da xac dinh. Ddi vdi bai todn cue bo, mdi tmdng lam vifc cua robot ' khdng biet tmdc hodc chi biet dugc mdt phin. Khi do, robot can tu nhdn biet mdi tnrong ' thdng qua hf thdng cdc cam bien trang bj tren robot. Bai todn ddn dudng cue bd la nham dilu khien robot trdnh dugc cac vat can ngdu nhien hay bat ngof
1 • Bt' rtiii hu tui'u ( i P ^ -' L« Uiu \ni. ti>
•• I ^m I'icu
-I • iMuU \U\x d i i u ; (HlU tli)P\
I \>M ^^n^rtl ^y•*if '• Hop . V u L1iM*u '• I ' o u ; (0 I > " , S- r.»uti »lut tlou?
Hinh 1. Mo hinh robot tu hanh tren dudng di chuyin cua robot.
Bai bdo nay chiing tdi tap trung vao giai quyet bai toan dan dudng todn cue tren co sd ling dung ly thuyet dieu khien md kit hgp vdi cac thong tin ve tinh vd la ban difn tir. Bai toan dan dudng cue bd chiing tdi se dl cap trong mdt bai bao khac.
Md hinh nghien ciiu
De cd.the thir nghiem giai phdp dugc de xuit va danh gid kit qua nghien ciru, nhdm tdc gia da tien hanh xay xung robot tu hdnh vugt dja hinh trong dilu kien ngodi trdi, dja hinh gl ghe. Vi vay, ket cau ciia robot dugc chgn la: Bdnh dai ldm bdng xfch, dgng co truyin dgng bdng xfch, be mdy va hf truyin ddng rat chdc chdn. Qk truyin dgng cho bdnh xe phdi cd hai dgng CO DC cdng suat dii Idn de dilu khien hai bdnh xfch phai vd trdi.
HI. THI/C HIEN THirC TE HE T H 6 N G DIEU KHltN TR^N CO Sd LOGIC M 6
Xay dung bg dieu khiln md:
Qua qud trinh thir nghifm thuc tl, chiing tdi nhan thiy cd thi nhan dugc tfn hieu vl vj tri tuang ddi on djnh tir thilt bj GPS khi tiln hdnh do tai cdc dilm cdch nhau khoang 10m (do sai sl cua thiet bj thu GPS). Vi vay, thuat todn dan dudng cho robot dugc xay d\mg nhu sau:
Tmdc het khoang cdch di chuyen ciia robot dugc xdc djnh nhd siir d\ing bd encorder.
Sau mdi 10m, lai xdc djnh vj trf cua robot dk hifu chinh hudng di chuyin tilp theo cho robot.
Trong pham vi 10m hifu chinh hudng di cho robot dung la bdn sl.
A(xA.yA): Vj tri' hifn tai cua robot.;
B(xB,yB): Vj tri ciia diem muc tieu.
a: gdc Ifch ciia AB so vdi phuang Bdc (tfnh dua vdo vj trf cua A vd B).
P: gdc Ifch hudng hifn tai cua robot so vdi phuang Bdc.
9=P-a: gdc Ifch cua robot so vdi AB.
M&hoa: Tuang iing vdi lugng vdo la gdc sai lech d ta se dieu chinh hudng di ciia robot thdng
Bac B
Hinh 2. Hirdng di chuyin cua robot
DL
•« .«:
npiilvariilile'DsIa'
20 30 i ; 59 M output v i n M TCPIiar
53 1 «
Hinh 3: Md hoa sai so goc diu vao 6 Hinh 4: Md hoa dap irng dgng co* trai (phai) gua viec dilu chinh cdng suat cua 2 ddng ca.
6 md hda thdnh cdc tap: {am Idn (AL); am vira (AV); khdng (K); duang vira (DV); duang Idn (DL)}.
Ddp iimg (cdng suit) ciia ddng co trdi vd ddng co phdi dugc md hda :{Diing (D); Tien Cham (TC); Tiln nhanh (TN)}.
Luatm&i Ta xay dung cdc luat md theo bang dudi day
AND
Am vira Khdng Duong vira Duong Idn
Tien nhanh Tien nhanh Tien cham
Dirng
Tien cham Tien nhanh Tien nhanh Tien nhanh
B
Lu^t hgp thdnh vd gidi met. Trong bdi toan ndy, ta sir dung luat hgp thdnh max-min va giai md bdng phuang phdp diem trgng tam.
Sau khi robot di dugc doan dudng 10m, ta se xdc djnh lai vj tri ciia robot. Gid sii khi do robot d vj trf A'(xA;yA)- Khi dd doan dudng cin di tilp theo la A*B vd ta se tfnh dugc cac gid trj gdc Ifch d, d vd d mdi.
R = 10 m
Hinh 5: C^c tinh huong di chuyin
Xir ly thong tin GPS kit hgp thong tin La bdn difn tir dieu khien robot tir hdnh trong bdi todn ddn du&ng toan cue:
Bg thu GPS dugc su dung trong de tdi Id MagellanR eXplorist"""" 500 thuc hifn tmyen thdng vdi vi dieu khien ARM LPC2294 qua cdng ndi tilp RS232 bang giao thiic NMEA. Giao thirc NMEA bao gdm cdc cau, mdi cau bdt dau vdi mdt tir de chi ra loai dii lifu, theo sau se Id cdc dii lieu chi tiet. Mdi cau khdc nhau thi chiia cdc thdng tin khdc nhau.
Mdi cau trong giao thiirc NMEA diu bdt diu vdi ky tu '$' vd ket thiic bang ky tu xudng ddng, quay trd ve diu ddng vd khdng dugc ddi qud 80 ky tu. Du lifu chura trong mdt ddng dan le va dugc ngan cdch vdi nhau bdi cdc diu phdy ( ','). Dii lifu dan
thuin chi Id cdc ky tu ASCII. Trong vifc tmyln thdng vdi bg ,.. . ^ . -».. ». 1 thu GPS thi bd vi dieu khien se thuc hifn tim trong diu ra cua
GPS cau chua cdc du lifu cin thiet (d day cin thdng tin vl kinh do, vT do) dk giup cho robot th\rc hifn nhifm vu.
Tdm tdt doan chuang trinh giao tilp giua vi dilu khiln va GPS dk tim ra cau chua thSng tin ve kinh do, vT dg tir dd xdc djnh tga dg cua robot:
nivc h i ^ rtu cin cua miy anh
STOP
Hinh 6. Luu dd thuat toan robot ty* hanh theo
quy dao dfnh san
int ii.tih_kmh_do(voi<t) {
cli.ir roado[5];
im i.j;
i=0:
while (buf_kinlido[i] 1-= .'j i t + : i++;
J-^+J for (j=0; }<
{
toado[j] = buf_.kinhd.o[i];
t+-s-;
}
i = aioiftoadov.
j = strleiifbuf_kiiihclo) - 2:
if (buf_kinlicio[j]> '5') i++:
return i:
im tirth vi do(void)
1
chav iMdo[.5]:
int i.j:
i=0:
while (buf_vido[i] ! - '.') i-n-:
i++-:
for (J--0: }<-l: j-h+) {
toado[j] - buf.xidoti]:
)
i - aioi(toado):
j = iirIen(buf_vido) - 2;
if fbuf_\ido[ji> '5') i++:
return i:
v o i d tin.h toa d o ( v o i d ) int ttd:
char kttd;
timcau:
w h i l e (kttd!= 'S')kttd = GetChar();
kttd = GetCharO;
kttd = GetCharO;
kttd = GetCharO;
if (kt1d!='R') goto timcau;
for O t d = l : t t d c = 3 . t t d ^ - ) {
w h i l e (GetCharO!='.');
k n d = GetCharO:
}
b u f _ v i d o [ 0 ] = kttd;
for ( t t d = l : t t d < = 8 ; t t d — ) buf_vido[ttd] = GetCharO;
for ( t t d = l ; t t d < = 2 ; t t d — ) {
w h i l e CGetcharO!=','):
kttd = GetCharO;
}
b u f _ k i n l i d o [ 0 ] = kttd;
for ( t t d = l ; t t d < = 9 ; t t d — ) buf_kinhdo[ttd] = Getchar(
v i d o = t i n h _ v i _ d o O ; k i n h d o = tinh_kinh_doO;
}
I Sau khi xdc djnh dilm xuit phdt vd dilm dfch ciia robot tren bdn I dd thi tga dg cdc diem nay se dugc truyen xudng cho robot va dugc B' luu vao bd nhd ROM. Can cii vdo tga do diem xuit phdt vd dilm dfch se xdc djnh dugc gdc Ifch ciia hudng di chuyen so vdi hudng Bdc. La bdn sd CMPS03 cho biet gdc Ifch hifn tai cua la bdn so vdi phuang Bac.
Khi di chuyen vdi quang dudng ddi se xay ra sai Ifch vj tn. Gid su robot di chuyen tir diem A den diem B, cd hudng chuyen ddng Id A-B. Neu vi mdt ly do nao do, nhu nhieu tu, dudng bj nghieng...thi xe se di sai dudng sang B'. Gidi phdp cho tinh hudng ndy Id chia quang dudng di thdnh cdc doan nhd (khoang 10m mdt) vd robot tien hanh hifu chinh sau moi quang dudng ndy.
ffinh 7: Tmh hulng Nhu tnnh bay d hinh 8, doan dudng Ifch qui dao robot can di tir diem A den diem B.
LPC2294 se tfnh ra hudng can di, sau do chia nhd doan AB thdnh cac doan ABl, B1B2, B2B3 vd B3B, vd tfnh ngugc lai toa do ciia cdc dilm Bl, B2, B3. Dfch den dau tien cua robot la B l ,
Gid su robot bj Ifch vj tn' sang B l ' nhu da trinh bdy d phin tren, robot se tu tfnh lai hudng di mdi Bl-B vd qud trtrih cu thi lap lai. Nhu vay, quy dao cua robot cd the hai zich zdc
nhimg dfch den van bao dam sai sd nhd nhat. • • y, yj
Tdm tdt thuat tdan md hda guc Ifch giua hudng hifn tai jj^^h 8: Chia nho quang dudng ciia robot va hudng can di: di cia robot
XB,yB
int AL (signed iiuS deta ) // am Ion im DV isigned bnS deta) // duong \'ua
(
if (deu<=-30) itftiimi.lOOi;
L: ((deta>-30 )<&:&i,deta<-1 ?) >
reiiim(.(intX<.-l5-(.float;ideD)'*100/15));
if(e>=-15) jrlum(P);
if (deta<=0 I i-etumlOi;
if ((dcta>0>&<Sc(dei:a<=15j) Ktum (,(int)(.(floai;ideta"^ 100/13 Vi;
if ((deta> 15)&&(deta< 30))
return ((intX(30-<floai)deta)'*lOO/l5)>:
if (deta>=30) retumtO);
1
IV. THlT N G H I E M THV'C T ^ v A M O T S 6 K 6 T Q U A
Sa do khoi hf thong da du^c xdy dimg:
La b&n s6 O-IPSOB
N'liv thu GPS
Ci.c cam bi£'n
— •
Modul thu phit khdng dAy
Khoi xijr 1^- trung tdm ( Vi di^u khi«n
' L P C 2 2 ^ ;
J
'
M ^ h c6ng suA'c dieu kfai^n d^ng c a D C
Hinh 9: Sa do khii hf thing dieu khiln robot
Hinh 9 gidi thifu sa dd khdi he thdng dilu khiln robot dugc nhdm tdc gia xay dung.
Robot dugc dieu khiln vd gidm sdt thong qua phin mim vilt tren nin Visual Basic c6 giao difn dieu khiln dugc trinh bdy d hinh 10. Hnh
11 gidi thifu robot chay tren th\rc dja.
Nhdm dk tdi da tien hdnh thii nghifm robot tren san van dgng, ban dd khu vvc san v|n dpng dugc cdi ddt tren may ti'nh dilu khien khdng day tir xa de chgn dudng di cho robot vd quan sat boat dgng ciia robot.
Thu nghifm phin dan dudng khdng cd vat can tren dudng vdi dudng di dugc chgn cd dang hinh chir "L", tire Id mgt nua dudn^ bao quanh san van dgng. Ket qua Id trong 5 lan tien hanh thu nghifm chi'nh thuc de lay ket qua ddnh gia thi trong ca 5 lan robot deu di ve dfch.
LI .1 [ , — W
r u n IN-; TrlMi niFi KIUF^N RI^BOT
".iy^lTMJt'.ffU'i.'i; "-jra I
IfkUt l l J J I W i
["in'•.tssaaa •
Hinh 10. Giao difn dieu khiln robot Hinh 11: Robot chay tren thuc dia
Bdng nhung lan thir nghiem, kit qud vd sai sd cua nidi lan Lan thu
1 2 3 4 5
Ket qua Ve tdi dfch Ve tdi dfch Ve tdi dfch Ve tdi dfch Ve tdi dfch
Sai sd
Trong ban kfnh 3,3m Trong bdn kfnh 2,7m Trong bdn kfnh 3,6m Trong bdn kfnh 2,2m Trong bdn kfnh 2,8m
Sai sd % 1,8%
1,5%
2%
1,2%
1,5%
Tir kit qua thir nghiem tren dja hinh thgc te, nhdm dk tdi nhan thiy:
- Qua trinh dilu khien hau nhu ludn bdo dam ve dfch.
Sai sd vl dfch Id khd nhd, thudng ndm trong pham vi vdi phan trdm neu di tren cu ly khoang vdi tram met. Sai sd tuyft ddi ndm trong vdng vdi met.
Cd nhilu ylu td anh hudng len dg chfnh xdc dilu khiln vl dfch, trong d6, do chi'nh xdc cua bd thu tfn hifu GPS Id lit quan trgng. Dk tdi su dung bg GPS gid re phd thdng tren thj trudng vdi do chfnh xdc thip. Ngodi dg chi'nh xdc ciia bd thu GPS, vifc bao ddm can bdng mat ngang cua la ban difn tir ciing cd dnh hudng ddng kl ciia dg chi'nh xdc dilu khiln robot.
V. KET LUAN
Mdt sd kit qua chfnh dat dugc ciia nhdm dl tdi cd thi tdm tdt nhu sau:
- Tren ca sd ung dgng ly thuylt logic md, dl tdi da xay dung thuat tdan ddn dudng todn cue dilu khiln robot tu hdnh vugt dja hinh kit hgp vdi thdng tin dir lifu thu dugc tii hf thdng ve tin GPS vd la ban dien tir.
Xay dung md hinh thuc te Id robot tu hdnh vugt dja hinh sii dung cdc hf vi dieu khien ARM 32 bit.
Tiln bdnh thu nghifm tren thuc dja cho robot tir hdnh trinh tdi myc tieu dugc xdc djnh tren san van ddng va ddnh gid do chi'nh xdc cua cdc lin thii nghifm. Kit qud thu dugc Id rat kha quan. Cdc lin thu diu cho kit qud robot ludn tu ddng tiln dugc din muc tieu vdi do chi'nh xac va tin cay khd cao.
Tuy nhien cdc kit qua dat dugc mdi thu nghifm trong mdi trudng cua mdt san van ddng.
Tmdng hgp mdi Uirdng cd nhilii ylu to phiic tap hom rit cin dugc tiep tuc diu tu nghien ciiu vl luat hgp thdnh tren ca sd kit hgp nhilu mf nh dk cung nhu gidi thuat dilu khiln dk robot cd thi xii ly dugc cdc tinh hudng khdc nhau trong thuc tl.
TAI L I | U THAM KHAO
[1]. Pham Xuan Minh, Nguyen Doan Phudc Ly thuyet dilu khiln md. NXBKH&KT 2006
[2].^guyln Nhu Phong. Ly thuylt md vd un^ dung, NXB KH&KT 2005
[3], Nguyen Thuang Ngd. Ly thuylt dilu khiln tu dgng thdng thudng va hifn dai.
NXBKH&KT 2005
[4] Passino K.M. & S. Yurkovich, Fuzzy Control, Addison Wesley, 1998.
[5] tanaka K. and H.O. Wang, Fuzzy Control Systems Design And Analysis, John Wiley
& Sons, Inc., 2001.
[6] Cdc trang web vl Fuzzy logic.