You are on page 1of 19

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

Idegenvezet robotplatform fejlesztse


nll laboratrium BME IIT

Ksztettk:

Wantuch Viktor Rozslay Mikls

FC557R P34UAU

Konzulens: Kszlt:

Dr. Kiss Blint docens 2013. mjus 23.

FC557R | P34UAU

-1-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

Tartalomjegyzk:

1. Tmakirs, specifikci........................................................................................... 3. oldal 1.1. Tmakirs ...................................................................................................... 3. oldal 1.2. Rszletes specifikci .................................................................................... 3. oldal 2. Implementci elmleti alapjai ................................................................................ 4. oldal 2.1. Az idegenvezet mobilis robot, mint llapotgp............................................ 4. oldal 2.2. Az implementci elmleti, blokkos realizcija .......................................... 5. oldal 3. Implementci a gyakorlatban ................................................................................. 6. oldal 3.1. A legmagasabb szint ...................................................................................... 6. oldal 3.2. Plyakvets, odometria ................................................................................ 6. oldal 3.2.1. Pozci meghatrozsa: pos_2.vi...................................................... 7. oldal 3.2.2. Mozgs: move.vi .............................................................................. 8. oldal 3.2.3. Egyenes halads: egyenes_2.vi......................................................... 9. oldal 3.2.4. Kanyarods: kanyar_2.vi .................................................................. 10. oldal 3.2.5. Szgszmtsok .................................................................................... 11. oldal 3.2.5.1. Kanyarods irnynak meghatrozsa: forgasirany.vi ........... 11. oldal 3.2.5.2. A szgek 360-os modulo szmtsa: absTeta.vi ................... 12. oldal 3.2.5.3. Tangens rtk normlsa: tan_szog.vi .................................... 13. oldal 3.2.5.4. Szgtartomny vizsglata: tartomany.vi................................. 14. oldal 3.2.6. Klmn-szrs: cs5kalman.vi ........................................................... 15. oldal 3.3. Plyatervezs, kikerls ................................................................................. 16. oldal 3.3.1. A plya megadsa: path.vi ................................................................ 16. oldal 3.3.2. tkzselkerls kikerlssel: crash_avoid.vi ................................. 17. oldal 4. Eredmnyek, kvetkeztetsek, tovbbi lehetsgek ................................................ 18. oldal Nyilatkozat ................................................................................................................. 19. oldal Hivatkozsok ................................................................................................................ 19. oldal

FC557R | P34UAU

-2-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

1. Tmakirs, specifikci 1.1. Tmakirs A feladat egy laborban vagy killtsi standon mkdtethet kerekeken gurul mobilis robot fejlesztse, amely a rteleptett multimdis eszkzkkel mutatja be a labor/killts elemeit s kzben rzkeli a krnyezetben tallhat mozg embereket s akadlyo kat. A feladat a robot konstrukcijnak kidolgozsa, az rzkel s hajtsi rendszer megtervezse s megvalstsa, a fedlzeti szablyozsok implementlsa. 1.2. Rszletes specifikci A National Instruments oktatsi s kutatsi cllal kifejlesztett DaNI mobilis platform programozsa LabVIEW (Laboratory Virtual Instrumentation Engineering Workbench) magas szint rendszerfejleszti krnyezetben.1 A robotnak a kvetkez funkcikkal kell rendelkeznie: felismernie sajt helyt halads elre meghatrozott plyn az tjba es akadlyok kikerlse a plya meghatrozott pontjain jelzs kldse a fogad szmtgpnek, mely vezrli a labor/killts elemeit bemutat multimdis megjelent eszkzket Az idei flvben ezek kzl megvalstott funkcik: felismeri a sajt helyt a kiindulsi ponthoz kpest kpes elre meghatrozott plyn haladni kikerli az tjba es akadlyokat amennyiben nem tud tovbbhaladni, megll

FC557R | P34UAU

-3-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

2. Implementci elmleti alapjai 2.1. Az idegenvezet mobilis robot, mint llapotgp A robot viselkedst tekintve felfoghat llapotgpknt, ami a bemen jelek illetve bels programjnak hatsra vltja llapotait, vagy hajtja vgre az adott llapotban elrt feladatokat.

START

Prezentci, akci

Prezentci, akci befejezdtt Folytathat a navigci (tvlati fejleszts)

Megadott pozci elrse

Navigci
Navigci nem folytathat

Idleges, vagy vgleges lells

Nem kvetik Kvetik

Krnyezet management
(tvlati fejleszts)

STOP

Az idegenvezet mobilis robot llapotgrfja

A robotot irnyt klnbz LabVIEW VI-ok a fenti llapotgrfot valstjk meg.

FC557R | P34UAU

-4-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

2.2. Az implementci elmleti, blokkos realizcija A LabVIEW nyelvhez igazod blokkos elmleti ttekints albb lthat:

Szenzor management

Odometria

Akci management Motor management

Vezrls Plyatervezs + Kikerls Plya kvets

Az idegenvezet robot blokkos realizcija

FC557R | P34UAU

-5-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3. Implementci a gyakorlatban 3.1. A legmagasabb szint

A vgrehajts alapveten balrl jobbra trtnik, az error vezetk a VI-ok lefutsnak sorrendjt szabja meg. A subVI-ok lersa albb olvashat. 3.2. Plyakvets, odometria Az implementci ezen szakasza egy, a laborban vagy killtson bell elre meghatrozott plya kvetsnek megvalstsa. Teht a mozg egysg pozcijnak s orientcijnak meghatrozst kell elvgezni a mrt mennyisgek alapjn, ami jelen esetben a robot kt hajtott kerekn elhelyezett inkrementlis adk jele. Ezzel prhuzamosan pedig a plya kvetst megvalst beavatkoz jelekkel kell elltni, ami esetnkben a kerekek sebessgnek megadsa. A robot pozcija: A koordinta rendszer kzppontja a robot kiindulsi pontja, ekkor a pozitv y irny a robot hossztengelyvel prhuzamosan elre, a pozitv x irny a hossztengelyre merlegesen jobbra mutat.

FC557R | P34UAU

-6-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.1. Pozci meghatrozsa: pos_2.vi

Implementlt feladat: A robot pozcijnak meghatrozsa az elz itercibl szrmaz helykoordintk s szg illetve a kerksebessgek alapjn. Bemenetek: t vR vL xi yi tetat i Kimenetek: xi+1 yi+1 tetati+1 eltelt id (sec) jobb kerk motorjnak sebessge (rad/sec) bal kerk motorjnak sebessge (rad/sec) jelenlegi pozci x koordintja (mter) jelenlegi pozci y koordintja (mter) jelenlegi pozci szge a kiindulshoz kpest (rad) a kvetkez iterciban add x koordinta (mter) a kvetkez iterciban add y koordinta (mter) a kvetkez iterciban add szg (rad)

Mkdse: A kerksebessgekbl s az eltelt idbl megtett utat szmtunk, s ezt adjuk hozz a helykoordintkhoz.

FC557R | P34UAU

-7-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.2. Mozgs: move.vi

Implementlt feladat: A robot szksges kzs- s differencilis mdus sebessgnek meghatrozsa ahhoz, hogy a jelenlegi pozcijbl a kvnt pozci fel haladjon. Bemenetek: x to go y to go teta xi yi Kimenetek: diff common a kvnt clpozci x koordintja (mter) a kvnt clpozci y koordintja (mter) a robot jelenlegi szge (rad) jelenlegi pozci x koordintja (mter) jelenlegi pozci y koordintja (mter) a szksges differencilis mdus sebessg (rad/sec) a szksges kzs mdus sebessg (rad/sec)

Mkdse: A kapott jelenlegi s kvnt helykoordintk illetve jelenlegi szg informcii elszr a kanyarodst majd az egyenes haladst vgrehajt blokkokhoz jutnak el. A robot elszr irnyba fordul, majd egyenesen halad, korrigl, melynek oka, hogy kzeli pontokat gy tudja elrni, illetve gy nem bann alak plyn halad. A kanyarodst s az egyenes haladst vgrehajt blokkok mkdse az adott subVI-ok lersban olvashat. A move.vi kiadja a szksges sebessginformcikat.

FC557R | P34UAU

-8-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.3. Egyenes halads: egyenes_2.vi

Implementlt feladat: A robot szksges kzs mdus sebessgnek meghatrozsa. Bemenetek: x to go y to go xi yi Kimenetek: common a kvnt clpozci x koordintja (mter) a kvnt clpozci y koordintja (mter) jelenlegi pozci x koordintja (mter) jelenlegi pozci y koordintja (mter) a szksges kzs mdus sebessg (rad/sec)

Mkdse: A kvnt pozci s a jelenlegi pozci eltrse alapjn lltja a robot kzs mdus sebessgt. Amennyiben a robot 5 cm-nl tvolabb van a cltl, gy 8 m/s sebessggel halad, ellenkez esetben megll.

FC557R | P34UAU

-9-

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.4. Kanyarods: kanyar_2.vi

Implementlt feladat: meghatrozsa. Bemenetek: x to go y to go teta xi yi Kimenetek: diff

robot

szksges

differencilis

mdus

sebessgnek

a kvnt clpozci x koordintja (mter) a kvnt clpozci y koordintja (mter) a robot jelenlegi szge (rad) jelenlegi pozci x koordintja (mter) jelenlegi pozci y koordintja (mter) a szksges differencilis mdus sebessg (rad/sec)

Mkdse: A robot differencilis mdus sebessgt hatrozza meg ketts szgtartomny hasznlatval, melynek szksgessgt a kvetkez meggondols adja: A robot orientcija sosem tkletes, gy bizonyos trs szgtartomnyra kell vizsglnunk az orientcit. Amennyiben egyetlen tartomnyra vizsglnnk, gy miutn azon bellre kerlt a robot, rgtn el is indulna a nmileg hibs irnyba, s ismt tlpn a megengedett trst. A ketts szgtartomny clja, hogy az orientciba llsnl szigorbb, haladskor nagyobb trs szgtartomnyban tartjuk a robotot. Amint a robot irnyba llt, nem forog tovbb, s a jelen VI notificationt kld az egyenesen mozgat VI-nak. A VI hasznlja a tan_szog, forgasirany, tartomany VI-okat, ezek lersa a kvetkez alfejezetben olvashat.

FC557R | P34UAU

- 10 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.5. Szgszmtsok 3.2.5.1. Kanyarods irnynak meghatrozsa: forgasirany.vi

Implementlt feladat: A robot jelenlegi s kvnt kvetkez szge alapjn meghatrozza, hogy a forgst pozitv vagy negatv forgsirnnyal vgezve gyorsabb-e a vgrehajts. Bemenetek: szogrol szogre Kimenetek: pozitivF A jelenlegi szg, amirl el szeretnnk forogni (rad) A kvnt szg, ahova el szeretnnk forogni (rad) Pozitv (anti-clockwise) irny forgs szksges-e (bool)

Mkdse: Egymsba gyazott CASE szerkezetekkel vizsglja, hogy a szgek illetve irnyok milyen viszonyban llnak egymshoz kpest, s merre rdemes forogni.

FC557R | P34UAU

- 11 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.5.2. A szgek 360-os modulo szmtsa: absTeta.vi

Implementlt feladat: A robot folyamatosan ismeri szgt a kiindulsi pozcijhoz kpest. Ez tbbszri krbeforgs esetn zavar lenne, ugyanis pldul kt teljes kr megttele utn br irnyt tekintve 0-ot vrnnk 720-ot jelezne. Az absTeta.vi a bemenetn kapott szget 0 s 360 kz normlja. Bemenetek: tetain Kimenetek: tetaout teta szg (lehet >2) (rad) teta szg <= 2 normlssal (rad)

Mkds: A bemeneti szget kt egymsba gyazott CASE szerkezettel normljuk. Kvl elszr megvizsgljuk, hogy 02 tartomnyba esik-e, majd ellenkez esetben addig vonunk le belle vagy adunk hozz 2-t, amg az eredmny <=2 nem lesz.

FC557R | P34UAU

- 12 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.5.3. Tangens rtk normlsa: tan_szog.vi

Implementlt feladat: Az arcus tangens fggvny ltal szolgltatott szgbl 0 s 360 kz normlt rtket szmt. Bemenetek: x to go y to go tan xi yi Kimenetek: teta to go Mkdse: szmtsa. a kvnt clpozci x koordintja (mter) a kvnt clpozci y koordintja (mter) az arcus tangens fggvny ltal szolgltatott szg (rad) jelenlegi pozci x koordintja (mter) jelenlegi pozci y koordintja (mter) a robottal elrni kvnt szg (rad)

A koordintartkekbl sknegyed meghatrozsa, majd ez alapjn szg

FC557R | P34UAU

- 13 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.5.4. Szgtartomny vizsglata: tartomany.vi

Implementlt feladat: Megvizsglja, hogy a robot jelenlegi orientcijnak eltrse a kvnt orientcitl bell esik-e a trsen. Bemenetek: teta to go teta hatar Kimenetek: in? a robottal elrni kvnt szg (rad) a robot jelenlegi szge (rad) a ketts szgtartomny megfelel trse (0,2 vagy 0,4 rad) (rad) igazsgrtk, jelzi, hogy az elrt trsi tartomnyon bell vagyunk-e (bool)

Mkdse: A 360-ra normlt szgeket tbb relci elvgzsvel veti ssze, majd egy bool rtk formjban kiadja, hogy a kvnt tartomnyon bell vagyunk-e.

FC557R | P34UAU

- 14 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.2.6. Klmn-szrs: cs5kalman.vi

Implementlt feladat: Klmn-szrs megvalstsa a Szakirny laboron elvgzettek szerint, nhny apr mdostssal. (Jelen megoldsban a szr teta szget vr kerkadatok helyett.)

FC557R | P34UAU

- 15 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.3. Plyatervezs, kikerls 3.3.1. A plya megadsa: path.vi

Implementlt feladat: A robot plyjnak N pontjt egy 2 sorbl s N oszlopbl ll tblzatban troljuk. A path.vi ezen tblzat s az aktulis pozci ismeretben kiadja a kvetkez elrend pont koordintit. Bemenetek: Array xi yi iter distance Kimenetek: x to go y to go iter out az rintend N db pont x-y koordinta prjainak mtrixa (2 sor, N oszlop) jelenlegi pozci x koordintja (mter) jelenlegi pozci y koordintja (mter) futindex, jelli, hnyadik pont fel igyeksznk (0N-1) az ultrahangos tvolsgrzkel ltal mrt tvolsg (mter) az adott itercis fzisban elrni kvnt x koordinta (mter) az adott itercis fzisban elrni kvnt y koordinta (mter) futindex, amit visszacsatolunk shiftregiszterrel

Mkdse: A VI sorra adja ki az ppen kvetkezknt elrend koordintkat. Amennyiben az adott elrend pont 5 cm-es krnyezetbe r a robot, gy a tblzat kvetkez oszlopra lpnk, ezltal az j clkoordinta a kvetkez szmpr ltal megadott pont lesz. Amennyiben tkzs veszlye ll fenn, gy notificationt kld a trgykikerls menedzsmentnek.

FC557R | P34UAU

- 16 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

3.3.2. tkzselkerls kikerlssel: crash_avoid.vi

Implementlt feladat: Kzeli trgy szlelse esetn az tkzs elkerlse elre meghatrozott plyn trtn kikerlssel. Bemenetek: x to go y to go adatok koord in Kimenetek: x to go y to go koord out az adott itercis fzisban elrni kvnt x koordinta, ha nem lenne tkzs (mter) az adott itercis fzisban elrni kvnt y koordinta, ha nem lenne tkzs (mter) hol vagyunk (xi, yi, tetai rtkekbl kpzett tmb) 3 pont, ahov a kikerls sorn el kell menjen az adott itercis fzisban elrni kvnt x koordinta, az tkzs elkerlsre (mter) az adott itercis fzisban elrni kvnt y koordinta, az tkzs elkerlsre (mter) a kvetkez iterciban visszashiftelt koordintk

Mkdse: Amennyiben a VI olyan zenetet kap, hogy egy objektumhoz olyan kzel van a robot, ami tkzst okozna, a robotot egy tglalap alak plyra irnytja, gy kerli ki az akadlyt. Ms szavakkal: a robotnak hrom kzbeiktatott pont bejrsra ad utastst. Amennyiben kikerls kzben jabb tkzs kvetkezne be, a robot megll. Ez a legmagasabb szint VI-ban lett megvalstva.

FC557R | P34UAU

- 17 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

4. Eredmnyek, kvetkeztetsek, tovbbi lehetsgek A fent ismertetett VI-okat valstottuk meg s tltttk fel a DaNI robotra. A tesztels sorn a kvetkezket tapasztaltuk: a robot j kzeltssel, kis hibval elri a szmra kijellt koordintkat, az eltrs nem haladja meg az 5 centimtert haladsa nem tkletesen egyenes, menet kzben tbbszr korrigl a tblzatosan megadott plya pontjait kielgt mdon bejrja

A kvetkez kzvetlen tovbbfejlesztsi lehetsgek adottak a projekt kapcsn: Folytathat navigci: tbbszri akadlyba tkzs esetn a plya folytats lehetsgnek figyelse, az akadly elhrulst kveten a plya folytatsa. Kvets management: a ltogatk mozgsnak figyelse s jelzs kldse illetve meglls abban az esetben, ha nem kvetik a robotot. Ehhez szksges az ultrahangos rzkel idrl-idre trtn htrafordtsa, vagy egy tovbbi rzkel felszerelse. Prezentci tartsa megadott pontokban: a robot kls audiovizulis megjelent eszkznek kld jelet a megfelel pozcikban, s egy helyben ll, mialatt a megjelent eszkzn a prezentci fut. Folyamatosabb mozgs megvalstsa azzal, hogy a robot egyszerre fordul s halad egyenesen. A clponttl tvol nagy kzs mdus, kis differencilis mdus sebessggel (akr folyamatos szgkorrekcival), a clponthoz kzel pedig kis kzs mdus, nagy differencilis mdus sebessggel mozog, ezzel elkerlhet a bann alak plya, illetve az, hogy nem tall be a clpontba amellett, hogy nem kell kacszva mozognia a robotnak (felvltva forognia s elre mennie).

FC557R | P34UAU

- 18 -

2012/13 II. flv

nll laboratrium | BMEVIIIA355

BME IIT

Idegenvezet robotplatform fejlesztse

NYILATKOZAT Wantuch Viktor s Rozslay Mikls kijelentjk, hogy jelen tanulmny a sajt munknk, elksztshez meg nem engedett segdeszkzket nem vettnk ignybe. Kelt: 2013. mjus 23.

Hivatkozsok: bortkpek http://www.ni.com/white-paper/11564/en http://www.labviewdeveloper.net/about-livewires.html Mobilis robot plyatervezse s irnytsa (Egyetemi segdlet, BME IIT, 2012)

[1]

FC557R | P34UAU

- 19 -

2012/13 II. flv