Avtonomni komobilni lesistemi snavozila Gregor.Klančar Andrej..Zdešar UL Sašo....Blažič FE Igor...Škrjanc Univerza v Ljubljani Fakulteta za elektrotehniko Avtonomni mobilni sistemi: kolesna vozila Gregor Klančar Andrej Zdešar Sašo Blažič Igor Škrjanc Ljubljana, 2021 ____________________________________________________ Kataložni zapis o publikaciji (CIP) pripravili v Narodni in univerzitetni knjižnici v Ljubljani COBISS.SI-ID 79116035 ISBN 978-961-243-422-9 (PDF) ____________________________________________________ URL: http://msc.fe.uni-lj.si/ams-kv/ Copyright © 2021 Založba FE. All rights reserved. Razmnoževanje (tudi fotokopiranje) dela v celoti ali po delih brez predhodnega dovoljenja Založbe FE prepovedano. Založnik: Založba FE, Ljubljana Izdajatelj: Fakuleta za elektrotehniko, Ljubljana Urednik: prof. dr. Sašo Tomažič Recenzenta: prof. dr. Borut zupančič, prof. dr. Rihard Karba 1. elektronska izdaja Predgovor V knjigi so zbrani bistveni algoritmi, ki so potrebni za izvedbo avtonomnih mobilnih sistemov. Knjiga vsebuje pregled obstoječe (raziskovalno aktualne) teorije kot tudi številne avtorske raziskovalne prispevke s področja avtonomnih mobilnih sistemov. Čeprav se knjiga osredotoča na obravnavo algoritmov za kolesna vozila, se lahko z ustreznimi prilagoditvami velik delež predstavljenega materiala uporabi tudi za druge vrste mobilnih sistemov in tudi na mnogih drugih področjih. Knjiga je po tematikah razdeljana na sedem poglavij, ki jih bralec lahko bere linearno ali pa se osredotoči le na izbrana poglavja s tematikami, ki ga zanimajo. Po uvodnem poglavju 1, ki podaja nekaj osnovnih pojmov in kratek zgodovinski pregled, sledi poglavje 2, ki obravnava modeliranje kinematike gibanja raznovrstnih kolesnih mehanizmov in tudi dinamični model mobilnega sistema z omejitvami. Nato so v poglavju 3 predstavljeni različni pristopi vodenja kolesnih mobilnih sistemov in pristopi načrtovanja poti v poglavju 4. V poglavju 5 je podan pregled senzorjev, ki se uporabljajo v mobilnih sistemih, pri čemer so predstavljene tudi transformacije koordinatnih sistemov. Poglavje 6 pokriva stohastičnost v mobilnih sistemih in obravnava ocenjevanje pošumljenih stanj z Bayesovim in Kalmanovim filtrom ter s filtrom delcev. Na koncu se poglavje 7 dotakne še agentov in večagentnih sistemov, z opisi nekaj praktičnih primerov uporabe. Teorija je podprta z mnogimi primeri z rešitvami, ki so opremljeni z izvlečki programov v Matlabu. Le-te lahko bralec tudi preizkusi in uporabi pri praktičnem delu. Na spletni strani http://msc.fe.uni-lj.si/ams-kv je na voljo elektronska izdaja knjige in dodatni material, ki knjigo dopolnjuje. Poleg elektronske knjige lahko bralec v mapo src s spletne strani prenese vse programe, ki so predstavljeni v knjigi. Ime m-datoteke pri izvlečku programa nato deluje kot povezava do celotnega programa. Knjiga je namenjena vsakomur, ki ga zanima področje avtonomnih mobilnih siste- mov z raziskovalnega in/ali praktičnega stališča. Zaželeno je vsaj nekaj osnovnega znanja iz matematičnega modeliranja in simulacij dinamičnih sistemov, teorije regulacij, digitalnega vodenja sistemov, optimizacije, statistike in verjetnosti ter programiranja. Knjiga se lahko uporabi tudi kot gradivo pri predmetih na IV dodiplomskem ali podiplomskem študiju, ki obravnavajo mobilno robotiko. Tako je primerno gradivo za študente 2. letnika na podiplomskem študiju 2. stopnje Elektrotehnike na Univerzi v Ljubljani, Fakulteti za elektrotehniko. Za nastanek knjige so zaslužni sodelavci Laboratorija za avtomatiko in kibernetiko (vključno z bivšimi sodelavci, tudi pod prejšnjimi imeni laboratorija) na Univerzi v Ljubljani, Fakulteti za elektrotehniko. Velika zahvala gre prof. dr. Rihardu Karbi in prof. dr. Borutu Zupančiču za temeljit pregled dela in številne koristne komentarje. Posebna zahvala gre prof. dr. Dragu Matku, ki je področje mobilnih sistemov vpeljal v laboratorij. Hvala as. dr. Matevžu Bošnaku za njegov prispevek na področju avtonomnih mobilnih sistemov. Hvala vsem študentom, raziskovalnim partnerjem in tehničnemu osebju na Univerzi v Ljubjani, Fakulteti za elektrotehniko. Hvala študentki Valentini Stanić za pomoč pri pripravi knjige v slovenščini. Hvala raziskovalcem z institucij po vsem svetu, s katerimi smo sodelovali pri najrazličnejših projektih, ki so vplivali na pripravo te knjige. Hvala tudi Javni agenciji za raziskovalno dejavnost Republike Slovenije, ki je podprla izvedbo mnogih raziskovalnih in aplikativnih projektov. Ljubljana G. Klančar, A. Zdešar, S. Blažič, I. Škrjanc September, 2021 Kazalo 1 Uvod v mobilne sisteme 1 1.1 Roboti . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Mobilnost . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.3 Kolesa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.4 Avtonomni mobilni sistemi . . . . . . . . . . . . . . . . . . . . . . 4 1.5 Kratka zgodovina . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2 Modeliranje gibanja mobilnih sistemov 15 2.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 2.2 Kinematika kolesnih mobilnih sistemov . . . . . . . . . . . . . . . 16 2.2.1 Diferencialni pogon . . . . . . . . . . . . . . . . . . . . . . 17 2.2.2 Kolesni pogon . . . . . . . . . . . . . . . . . . . . . . . . . 22 2.2.3 Trikolesni pogon . . . . . . . . . . . . . . . . . . . . . . . 24 2.2.4 Tricikel s priklopnikom . . . . . . . . . . . . . . . . . . . . 24 2.2.5 Avtomobilski (Ackermannov) pogon . . . . . . . . . . . . 26 2.2.6 Sinhroni pogon . . . . . . . . . . . . . . . . . . . . . . . . 27 2.2.7 Večsmerni pogon . . . . . . . . . . . . . . . . . . . . . . . 28 2.2.8 Gosenični pogon . . . . . . . . . . . . . . . . . . . . . . . 32 2.3 Omejitve gibanja . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 2.3.1 Holonomične omejitve . . . . . . . . . . . . . . . . . . . . 34 2.3.2 Neholonomične omejitve . . . . . . . . . . . . . . . . . . . 34 2.3.3 Integrabilnost omejitev . . . . . . . . . . . . . . . . . . . 35 2.3.4 Vektorska polja, porazdelitev, Liejevi oklepaji . . . . . . . 35 2.3.5 Vodljivost kolesnih mobilnih robotov . . . . . . . . . . . . 45 2.4 Dinamični model mobilnega sistema z omejitvami . . . . . . . . . 49 VI Kazalo 2.4.1 Predstavitev dinamičnega modela mobilnega sistema z omejitvami v prostoru stanj . . . . . . . . . . . . . . . . . 50 2.4.2 Kinematični in dinamični model robota z diferencialnim pogonom . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 3 Vodenje kolesnih mobilnih sistemov 61 3.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 3.2 Vodenje v referenčno lego . . . . . . . . . . . . . . . . . . . . . . 62 3.2.1 Vodenje orientacije . . . . . . . . . . . . . . . . . . . . . . 63 3.2.2 Vodenje gibanja naprej . . . . . . . . . . . . . . . . . . . 65 3.2.3 Osnovni pristopi . . . . . . . . . . . . . . . . . . . . . . . 69 3.3 Vodenje po referenčni trajektoriji . . . . . . . . . . . . . . . . . . 86 3.3.1 Osnovni pristopi k vodenju po referenčni trajektoriji . . . 87 3.3.2 Razčlenitev vodenja na predkrmiljenje in povratno zanko 90 3.3.3 Povratnozančna linearizacija . . . . . . . . . . . . . . . . 92 3.3.4 Izpeljava kinematičnega modela pogreška vodenja pri sle- denju referenčne trajektorije . . . . . . . . . . . . . . . . . 97 3.3.5 Linearni regulator . . . . . . . . . . . . . . . . . . . . . . 98 3.3.6 Načrtovanje vodenja na osnovi funkcij Ljapunova . . . . . 103 3.3.7 Načrtovanje mehkega vodenja Takagi-Sugeno v okviru li- nearnih matričnih neenačb . . . . . . . . . . . . . . . . . 118 3.3.8 Modelno prediktivno vodenje . . . . . . . . . . . . . . . . 121 3.3.9 Vodenje na podlagi optimizacije z rojem delcev . . . . . . 127 3.3.10 Vodenje mobilnega sistema s pristopom vodenja na osnovi slike . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 3.4 Ocena optimalnega profila hitrosti za znano pot . . . . . . . . . . 142 4 Načrtovanje poti 155 4.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155 4.1.1 Okolica robota . . . . . . . . . . . . . . . . . . . . . . . . 156 4.1.2 Načrtovanje poti . . . . . . . . . . . . . . . . . . . . . . . 156 4.1.3 Konfiguracija in konfiguracijski prostor . . . . . . . . . . . 157 4.1.4 Matematični zapis oblike in lege ovire v okolici . . . . . . 158 4.2 Predstavitev okolja za načrtovanje poti . . . . . . . . . . . . . . . 159 4.2.1 Predstavitev z grafi . . . . . . . . . . . . . . . . . . . . . . 159 4.2.2 Razcep na celice . . . . . . . . . . . . . . . . . . . . . . . 160 4.2.3 Zemljevid cest . . . . . . . . . . . . . . . . . . . . . . . . 166 4.2.4 Potencialno polje . . . . . . . . . . . . . . . . . . . . . . . 171 4.2.5 Načrtovanje poti z metodami vzorčenja prostora . . . . . 174 Kazalo VII 4.3 Preprosti algoritmi načrtovanja poti — algoritmi tipa hrošč . . . 180 4.3.1 Algoritem Hrošč0 . . . . . . . . . . . . . . . . . . . . . . . 181 4.3.2 Algoritem Hrošč1 . . . . . . . . . . . . . . . . . . . . . . . 181 4.3.3 Algoritem Hrošč2 . . . . . . . . . . . . . . . . . . . . . . . 183 4.4 Metode iskanja poti v grafu . . . . . . . . . . . . . . . . . . . . . 186 4.4.1 Iskanje v širino . . . . . . . . . . . . . . . . . . . . . . . . 187 4.4.2 Iskanje v globino . . . . . . . . . . . . . . . . . . . . . . . 188 4.4.3 Iterativno iskanje v globino . . . . . . . . . . . . . . . . . 189 4.4.4 Dijkstrov algoritem . . . . . . . . . . . . . . . . . . . . . . 190 4.4.5 Algoritem A ? . . . . . . . . . . . . . . . . . . . . . . . . . 192 4.4.6 Pohlepno iskanje najprej najboljši . . . . . . . . . . . . . . 195 5 Senzorji v mobilnih sistemih 201 5.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201 5.2 Transformacije koordinatnih sistemov . . . . . . . . . . . . . . . 202 5.2.1 Orientacija in rotacija . . . . . . . . . . . . . . . . . . . . 202 5.2.2 Translacija in rotacija . . . . . . . . . . . . . . . . . . . . 211 5.2.3 Kinematika rotacijskih koordinatnih sistemov . . . . . . . 212 5.2.4 Projekcijska geometrija . . . . . . . . . . . . . . . . . . . 214 5.3 Metode merjenja lege . . . . . . . . . . . . . . . . . . . . . . . . . 224 5.3.1 Relativno določanje lege . . . . . . . . . . . . . . . . . . . 224 5.3.2 Merjenje smeri gibanja . . . . . . . . . . . . . . . . . . . . 232 5.3.3 Aktivne značke in globalne meritve pozicije . . . . . . . . 233 5.3.4 Navigacija z uporabo značilk okolja . . . . . . . . . . . . 245 5.3.5 Ujemanje modelov okolja — zemljevidi . . . . . . . . . . . 271 5.4 Senzorji . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 272 5.4.1 Karakteristike senzorjev . . . . . . . . . . . . . . . . . . . 272 5.4.2 Klasifikacija senzorjev . . . . . . . . . . . . . . . . . . . . 274 6 Nedeterminističnost v mobilnih sistemih 279 6.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279 6.2 Osnove verjetnosti . . . . . . . . . . . . . . . . . . . . . . . . . . 280 6.2.1 Diskretna slučajna spremenljivka . . . . . . . . . . . . . . 280 6.2.2 Zvezna slučajna spremenljivka . . . . . . . . . . . . . . . 282 6.2.3 Bayesovo pravilo . . . . . . . . . . . . . . . . . . . . . . . 285 6.3 Ocenjevanje stanj . . . . . . . . . . . . . . . . . . . . . . . . . . . 290 6.3.1 Motnje in šum . . . . . . . . . . . . . . . . . . . . . . . . 290 6.3.2 Ocena konvergence in pristranskosti . . . . . . . . . . . . 290 VIII Kazalo 6.3.3 Spoznavnost . . . . . . . . . . . . . . . . . . . . . . . . . 291 6.4 Bayesov filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293 6.4.1 Markovove verige . . . . . . . . . . . . . . . . . . . . . . . 293 6.4.2 Ocenjevanje stanj iz opazovanj . . . . . . . . . . . . . . . 294 6.4.3 Ocenjevanje stanj iz opazovanj in akcij . . . . . . . . . . . 300 6.4.4 Primer lokalizacije . . . . . . . . . . . . . . . . . . . . . . 306 6.4.5 Zaznavanje okolja . . . . . . . . . . . . . . . . . . . . . . 307 6.4.6 Gibanje v okolju . . . . . . . . . . . . . . . . . . . . . . . 313 6.4.7 Lokalizacija v okolju . . . . . . . . . . . . . . . . . . . . . 319 6.5 Kalmanov filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324 6.5.1 Kalmanov filter v matrični obliki . . . . . . . . . . . . . . 331 6.5.2 Razširjeni Kalmanov filter . . . . . . . . . . . . . . . . . . 338 6.5.3 Druge različice Kalmanovega filtra . . . . . . . . . . . . . 362 6.6 Filter delcev . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 362 7 Agenti in večagentni sistemi 375 7.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375 7.2 Večagentni sistemi . . . . . . . . . . . . . . . . . . . . . . . . . . 375 7.3 Agenti . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 377 7.4 Arhitektura in delovanje agentov . . . . . . . . . . . . . . . . . . 378 7.4.1 Kognitivni agenti . . . . . . . . . . . . . . . . . . . . . . . 378 7.4.2 Odzivni agenti . . . . . . . . . . . . . . . . . . . . . . . . 379 7.4.3 Hibridni agenti . . . . . . . . . . . . . . . . . . . . . . . . 380 7.4.4 Odzivno vedenjski agenti . . . . . . . . . . . . . . . . . . 381 7.4.5 Osnovne vedenjske arhitekture . . . . . . . . . . . . . . . 382 7.4.6 Ostale delitve agentov in večagentnih sistemov . . . . . . 385 7.5 Primeri uporabe večagentnih sistemov . . . . . . . . . . . . . . . 386 7.5.1 Robotski nogomet — avtonomna igra kolesnih robotov . . 386 7.5.2 Vožnja vozil v formaciji . . . . . . . . . . . . . . . . . . . 394 7.5.3 Avtomatsko vodeni vozički . . . . . . . . . . . . . . . . . . 400 7.5.4 Večagentno planiranje vožnje transportnih vozil . . . . . . 419 1 Uvod v mobilne sisteme 1.1 Roboti Beseda robot izvira iz družine slovanskih jezikov. V poljščini beseda “robota” pomeni delo, medtem ko je v češčini ali slovenščini bolj arhaična in pomeni “suženjsko delo” ali corvée. Znani češki pisatelj Karel Čapek je sestavil in uporabil besedo “robot” v svoji drami R.U.R. - Rossumovi Univerzalni Roboti. Z njo je opisal umetnega človeka, ki bi ga danes lahko poimenovali kiborg ali android (slika 1.1). Zaradi velikega uspeha drame, je besedo prevzela večina svetovnih jezikov. Medtem ko je beseda robot nastala pred manj kot sto leti, je sama ideja o mehanskem bitju veliko starejša. V Grški mitologiji najdemo mnogo bitij, kjer ima vsako svoj namen. Spartoi so oboroženi mitološki vojaki, ki so se razvili iz zmajevih zob, katere je posejal Kadmos. Vojaki so Kadmosu pomagali pri izgradnji Kadmeje, tj. trdnjave v Tebah. Talos, ki ga je ustvaril Hefajst, je bil ogromen bronast avtomat, namenjen zaščiti Evrope na Kreti pred pirati in napadalci. Grški bog kovačev in obrtnikov Hefajst je zaslužen tudi za nekatere druge mehanske strukture, ki so bile realizirane. Avtomate lahko najdemo tudi v starodavnih judovskih, kitajskih in indijskih legendah. Skozi celotno zgodovino je prisotna ideja o mehanskem avtomatu, ki je podoben ljudem ali živalim; v 19. in 20. stoletju pa je postala res priljubljena. V 20. stoletju se je pojavil priljubljen medij, ki je upodobil in oživel robote – film. Nekatere ideje v literaturi in filmu so bile v času nastanka označene kot znanstvena fantastika, kasneje pa je ta fikcija postala resničnost. Vendar pa roboti niso samo stvar fikcije. Zelo zgodnji iznajditelji so skušali ustvari mehanski avtomat. Grški matematik Arhitas naj bi v 4. stoletju p. n. št. 2 Uvod v mobilne sisteme Slika 1.1: Scena iz dramske predstave R.U.R. , ki prikazuje tri robote [Fotografija v javni domeni (https://commons.wikimedia.org/wiki/File%3ACapek_play.jpg)] oblikoval in zgradil prvo umetno letečo napravo na lasten pogon. Ta mehanska ptica na parni pogon naj bi bila zmožna preleteti okoli 200 metrov. Leonardo da Vinci je v svoji obširni dediščini zapustil mnogo mehanskih načrtov. S pomočjo grobih skic iz Leonardovih zapiskov je Rosheim [1] rekonstruiral programirljiv voziček (slika 1.2), ki je služil kot podlaga Leonardovim izumom, med katerimi sta bila tudi robotski lev in vitez. Z razmahom industrijske revolucije je tehnološki napredek pripeljal do razcveta avtomatizacije, ki je postopoma vodila do današnje mobilne robotike. 1.2 Mobilnost Beseda mobilnost izhaja iz latinske besede z istim pomenom “m¯ obilis”. Večina živalskih vrst ima sposobnost lokomocije, tj. premikanja organizma iz enega mesta na drugo. Medtem ko nekatere živali za premikanje uporabljajo pasivne sisteme (npr. s pomočjo gibanja vode ali zraka), so druge razvile bolj ali manj napredne mehanizme za aktivno gibanje. Ene živali se gibajo v tridimenzionalnem prostoru (plavanje v vodi, letenje po zraku, premikanje po tleh), druge bolj ali manj sledijo dvodimenzionalni površini vode ali tal, tretje pa so zmožne združiti različne načine gibanja. V okviru mobilnih robotov nas zanimajo sistemi, ki se lahko premikajo z uporabo svojega sistema za lokomocijo. Slednji pogosto posnemajo gibanje človeka ali določene živali. Posnemanje bioloških sistemov običajno uspešno rešuje nekatere tehnične težave, ki se pojavijo med načrtovanjem gibanja umetnega sistema. Drug pomemben vidik mobilnega sistema je avtonomija, saj lahko pri gibanju pride do prevelike oddaljenosti od človeškega operaterja. Tako mora imeti sistem določeno stopnjo avtonomnosti, da se lahko premika po prostoru brez pomoči 1.2. Mobilnost 3 Slika 1.2: Model kolesa in programirljivega vozička, zgrajen na podlagi zapiskov Leonarda da Vincija Slika 1.3: Radijsko krmiljen električni čoln Nikole Tesle 4 Uvod v mobilne sisteme (a) (b) Slika 1.4: 5200 let staro leseno kolo z osjo, ki so ju našli na Ljubljanskem barju, je glede na starost kot tudi tehnološko dovršenost eno izmed najpomembnejših predmetov svetovne kulturne dediščine (premer kolesa meri 70 cm, dolžina osi pa 120 cm) [Muzej in galerije mesta Ljubljane, avtor M. Paternoster] operaterja oz. da na daljavo sprejema njegove ukaze. Nikola Tesla je konec 19. stoletja prvi oblikoval in sestavil radijsko voden električni čoln (slika 1.3). Od 20. stoletja nivo avtonomnosti nenehno narašča, vendar človek še vedno na določenem nivoju upravlja obstoječe mobilne sisteme. 1.3 Kolesa Čeprav se lahko gibljejo tudi zelo primitivne živalske vrste, ni samoumevno razviti umetni sistem, ki je sposoben posnemati gibanje živali. Medtem ko koles in podobnih struktur ni mogoče najti v živalskem svetu, vozila s kolesi omogočajo energetsko učinkovito gibanje po tleh. Površina tal mora biti dovolj gladka, čeprav se lahko ustrezno zgrajena kolesna vozila premikajo tudi po neravnem terenu, stopnicah ipd. Ni znano, kje in kdaj so izumili kolo; uveljavljeno prepričanje je, da so prva kolesa uporabili približno 4000 let p. n. št. v Mezopotamiji in so se od tam razširila po celem svetu. Nekateri strokovnjaki ocenjujejo, da so kolo izumili v prazgodovinski Evropi. Najstarejše ohranjeno leseno kolo z osjo je staro 5200 let in je bilo odkrito v Sloveniji na Ljubljanskem barju (slika 1.4). 1.4 Avtonomni mobilni sistemi Mobilni sistemi niso (fizično) povezani oziroma vpeti v okolico in se lahko poljubno premikajo v določenem območju. Glede na okolje, v katerem se premikajo, jih lahko razvrstimo v tri skupine: 1.4. Avtonomni mobilni sistemi 5 • Kopenski mobilni sistemi. Med njimi najdemo različne vrste mobilnih platform, kot so mobilna vozila s kolesi ali gosenicami, roboti z nogami (humanoidi in roboti, ki posnemajo hojo živali) ter roboti, ki posnemajo druge načine živalskega premikanja (npr. kačje lezenje). Kopenske mo- bilne sisteme s kolesi ali gosenicami brez operaterja imenujemo kopenska brezpilotna vozila (UGV, angl. unmanned ground vehicles). • Zračni mobilni sistemi. Ta skupina je sestavljena iz mobilnih siste- mov, ki letijo v določenem zračnem prostoru (letala, helikopterji, droni, rakete in leteči sistemi, ki posnemajo letenje živali). Če letijo brez pilota jih imenujemo brezpilotna zračna vozila (UAV, angl. unmanned aerial vehicles). • Vodni in podvodni mobilni sistemi. V tej skupino uvrščamo različne vrste ladij, čolnov, podmornic, avtonomnih podvodnih vozil (AUV, angl. autonomous underwater vehicles) ipd. V knjigi bomo obravnavali samo kolesna mobilna vozila, čeprav se lahko z ustreznimi prilagoditvami velik delež predstavljenega materiala uporabi tudi za druge vrste mobilnih sistemov. Mobilne sisteme obravnavamo kot avtonomne, če so sposobni avtonomnega gibanja v svoji okolici. Avtonomija mora biti zagotovljena • z energijskega vidika – robot nosi vir energije, • z vidika odločanja – robot se je sposoben odločati in izvajati ustrezne akcije. V praksi to pomeni, da mobilni sistem sprejema ukaze človeškega operaterja glede na stopnjo avtonomnosti, ki mu je vgrajena. Sistem nato poskuša izvesti ukazane naloge in ustrezne “podnaloge” na nižjih nivojih. V primeru predvidljivih okoliščin se naloga izvede v določenem časovnem intervalu. Glede na stopnjo avtonomnosti robota lahko operater izvede naslednje tipične ukaze: Želene hitrosti koles. Robot sprejema ukaze, ki predstavljajo želene hitrosti koles. Osnovni algoritmi vodenja z ustreznimi senzorji (običajno so to rotacijski dajalniki) omogočajo želeno vrtenje koles glede na ukaz. Želene translacijske in kotne hitrosti robota. Računalniški program, ki deluje na robotu, pozna njegov kinematični model in lahko izračuna ustrezne hitrosti koles, da doseže želene hitrosti robota. Želena pot ali trajektorija robota. Robot lahko v svoji okolici ugotovi in vodi svojo lego, ki je običajno določena kot skupna informacija o poziciji in orientaciji glede na izbrani koordinatni sistem. Na tej ravni najdemo lokalizacijo robota s pomočjo različnih senzorjev, nameščenih na robotu ali v okolju, s katerimi poiščemo najboljši približek lege robota. Pri vodenju se pojavijo tudi težave zaradi nelinearnosti sistema, napačnih informacij senzorjev, zdrsa koles, slabih modelov, zakasnitev itd. 6 Uvod v mobilne sisteme Želeno delovanje v znanem okolju z morebitnimi ovirami. Robot mora izvesti opravilo v znanem okolju z nekaj (statičnimi ali dinamičnimi) ovirami. Na tej ravni je robot sposoben načrtovati svojo pot, oz. jo ponovno zasnovati v primeru pojava ovir, ki preprečujejo izpolnitev operacije. Želena operacija v neznanem okolju. Robot ne pozna svoje okolice, zato mora sočasno izvajati algoritme za določanje položaja in graditi zemljevid svoje okolice – tovrsten pristop je znan kot SLAM (angl. simultaneous localisation and mapping). Želena naloga. Robot prevzame nalogo, ki jo mora izpolniti v okolju, kjer lahko sodeluje z drugimi roboti ali agenti. Robot potrebuje določeno stopnjo razumevanja svojih nalog, poznati pa mora tudi njihove prioritete, da lahko prekine neko nalogo in/ali prevzame nalogo z višjo prioriteto. Številna pravila odločanja morajo biti vgrajena v robota. Recimo robot preveri stanje energije v svojih akumulatorjih in jih po potrebi napolni. Roboti na prvih dveh (zgoraj opisanih) nivojih niso avtonomni. Obstajajo tudi druge razvrstitve, pri čemer je pomembno, da razumemo naloge in inteligenco robota, ki jih ima na določeni ravni. Glavni mehanski in elektronski sestavni deli avtonomnega mobilnega robota so: • mehanska konstrukcija: togi in gibljivi sestavni deli (telo, kolesa, gosenice, noge itd.), • aktuatorski pogon: električni motorji (DC, koračni motor, servomotor itd.), • senzorji: rotacijski dajalniki, senzorji bližine in razdalje, inercialna navi-gacijska enota, globalni navigacijski satelitski sistem (GNSS, angl. Global navigation satellite system) itd., • računalniki: mikrokrmilniki, prenosni osebni računalnik, vgrajeni sistemi itd., • napajalna enota: baterije, sončne celice itd., • elektronika: elektronika za pogon motorjev, meritve senzorjev, distribu-cija moči in telekomunikacijska elektronika. Knjiga obravnava algoritme, potrebne za obdelavo podatkov senzorjev in pogon motorjev s ciljem zagotavljana avtonomnosti mobilnega robota. Kolesni mobilni roboti imajo več dobrih lastnosti, zaradi katerih so privlačni za uporabo. Mobilni roboti omogočajo dostop do okolij, nevarnih za ljudi (npr. minska polja, radioaktivna okolja, globokomorske raziskave itd.), in oddaljenih ali nedostopnih okolij (npr. raziskovanje zunajzemeljskih planetov, “nano roboti” v medicini itd.). Hkrati lahko namesto človeka izvajajo naloge, ki so zanj fizično 1.4. Avtonomni mobilni sistemi 7 zahtevne. Uvedba avtomatizacije, robotike in mobilnih sistemov omogoča tudi večjo produktivnost, boljšo kakovost izdelka ali storitve ter zmanjša stroške dela. Dandanes se mobilni sistemi uporabljajo v številnih aplikacijah na različnih področjih, ki se zaradi hitrega tehnološkega razvoja nenehno širijo. Nepopoln seznam aplikacij kolesnih mobilnih robotov vključuje: • medicinske storitve, kot je pomoč pri operacijah, opravljanje laboratorijskih analiz (npr. v situacijah, kjer je nevarnost okužbe), • aplikacije čiščenja (sesanje tal, pometanje in pomivanje v domovih ali velikih zgradbah, čiščenje oken), • aplikacije v kmetijstvu, kot je avtomatizirano obiranje sadja, sajenje, košnja trave, • gozdna dela, čiščenje gozdov, • prodaja blaga široke potrošnje, • pregled in nadzor nevarnih področij (detekcija in deaktivacija min na minskih poljih, pregled jedrskih reaktorjev, čiščenje kanalizacijskih cevi), • vesoljske aplikacije (sateliti, pregled in servisiranje satelitov, raziskovanje planetov), • pomorske aplikacije (roboti za postavljanje kablov in pregledovanje mor- skega dna), • roboti za nakladanje in razkladanje blaga ali materiala iz letal, ladij ter tovornjakov, • vojaški roboti (izvidniški roboti, letala in razni avtopilotski izstrelki), • varnostni roboti (za nadzor skladišč in stavb), • pomoč starejšim in invalidnim osebam (avtonomni invalidski vozički, roboti za rehabilitacijo), • potrošniške aplikacije (robotski hišni ljubljenčki, robotski nogomet), • sistemi v raziskovalnih ustanovah, namenjeni učenju in razvoju novih algoritmov. Nekaj od predhodno naštetih področij uporablja tudi že avtonomne mobilne sisteme (AMS), torej robote, ki se lahko samostojno gibljejo v okolici med opravljanjem različnih opravil. Med temi sistemi prevladujejo kolesni avtonomni sistemi kot tudi letalni avtonomni sistemi. Število praktičnih aplikacij AMS in tudi komercialno dostopnih sistemov se povečuje. Uporaba avtonomni sesalnikov, kosilnic in podobnih sistemov je že vsakdanjost. Pogosta je tudi uporaba AMS v tovarnah, bolnišnicah in distribucijskih centrih za dostavo. Obetajoče so tudi 8 Uvod v mobilne sisteme bodoče aplikacije v kmetijstvu in javnem transportu, kjer številni raziskovalni centri razvijajo kmetijske robote in samovozeča vozila, ki bodo kmalu zaživela v uporabi. Najdemo lahko tudi številne druge aplikacije na področjih kot so: vojska za izvidniške namene, misije v vesolju za planetarna raziskovanja, pri naravnih nesrečah za iskanje in reševanje in na področju varovanja. Mobilni avtonomni sistemi predstavljajo hitro razvijajoče področje raziskav in razvoja, zato se bo v bližnji prihodnosti pojavilo še veliko novih aplikacij, ki nam trenutno niso tako očitne. Da dosežemo želeno avtonomijo in novo funkcionalnost, morajo ti sistemi združevati številne tehnologije in opremo. Ključne tehnologije, ki so predstavljene v nadaljevanju te knjige so: modeliranje, vodenje, planiranje, senzorika. lokalizacija in sistemi odločanja. Predvidevanje prihodnosti je že od nekdaj zahtevna naloga. Današnje tehnologije in aplikacije so bile še pred desetletjem težko predstavljive. Predvideva se, da bodo v bližnji prihodnosti avtonomni kolesni mobilni roboti postali še bolj nepogrešljivi v vsakdanjem življenju: v tovarnah prihodnosti bodo sodelovali z ljudmi, nam pomagali pri domačih opravilih, nas peljali po cesti, reševali življenja (v reševalnih misijah) in še veliko več. V naslednjem poglavju je prikazan kratek pregled zgodovine, oz. kako nas je tehnološki razvoj pripeljal do trenutne točke. 1.5 Kratka zgodovina Poglavje predstavlja nekaj pomembnih mejnikov v zgodovini kolesnih mobilnih robotov [2]. Poudarek je na aplikacijah, vendar so omenjeni tudi nekateri tehnološki dosežki, ki so pomembno vplivali na področje mobilne robotike. 1898 Nikola Tesla je na sejmu elektronike v dvorani Madison Square Garden v New Yorku demonstriral brezžično radijsko vodeno plovilo [3], ki je eden izmed njegovih patentiranih izumov [4]. 1939–1945 Med drugo svetovno vojno so v Nemčiji razvili avtopilotski raketi V-1 [5] in V-2 [6]. Hkrati je Američan Norbert Wiener razvijal sistem za avtomatsko ciljanje protiletalskega orožja [7]. 1948–1949 W. Grey Walter je ustvaril avtonomna robota imenovana Elmer in Elsie [8], ki sta bila podobna želvam in zmožna slediti svetlobnemu viru (tj. fotodioda), zaznavati ovire (kontaktno stikalo) in se izogibati oviram. 1961–1963 Univerza Johns Hopkins je razvila mobilnega robota Beast [9], ki je lahko taval po belih hodnikih in iskal črne stenske vtičnice za polnjenje svojih baterij. 1966–1972 Raziskovalni inštitut Stanford je razvijal robota Shakey [10], ki je vseboval kamero, sonar, senzorje za zaznavanje trka in brezžično 1.5. Kratka zgodovina 9 povezavo. To je bil prvi robot za splošno rabo, ki je znal načrto- vati svoje akcije. Rezultati projekta vključujejo razvoj iskalnega algoritma A ? , Houghovo transformacijo in graf vidljivosti. 1969 Predstavljena in patentirana prva robotska kosilnica MowBot [11]. 1970 Sovjetska zveza je na Luni uspešno izkrcala prvi lunarni rover Lunokhod 1, ki je bil daljinsko voden z Zemlje ter nosil več kamer in drugih senzorjev. V 301 dneh delovanja je rover prevozil približno 10 km, posnel več kot 25 000 slik in naredil več analiz tal [12]. 1973 Sovjetska zveza je na Luni izkrcala drugi lunarni rover Lunokhod 2. Med štirimesečno misijo je rover prepotoval 39 km, kar je do leta 2014 veljalo za najdaljšo prepotovano razdaljo izven Zemlje [13]. 1976 Nasini vesoljski plovili brez posadke Viking 1 in Viking 2 (vsako sestavljeno iz vesoljskega plovila in pristajalnika) sta vstopili v Marsovo obrito, nekoliko dni kasneje pa so pristajalniki mehko pristali na površini Marsa [14]. 1977 Francoski laboratorij za analizo in arhitekturo sistemov (LAAS) je začel z razvojem mobilnega robota Hilare 1 [15], ki je bil opremljen z ultrazvočnimi in laserskimi pregledovalniki razdalj ter kamero na robotski roki. 1979 Vozilo Stanford (angl. Stanford cart) (začetni model predstavljen leta 1962) je bilo zmožno vizualne navigacije po progi z ovirami [16]. 1982 Na voljo je bil prvi model iz serije komercialnih robotov HERO, ki so bili namenjeni predvsem za domačo in izobraževalno rabo [17]. 1986 Ekipa pod vodstvom Ernsta Dietera Dickmannsa [18] je razvila robotski avto VaMoRs, ki se je lahko sam vozil po ulicah brez prometa s hitrostjo do 90 km / h. 1995 Na tržišču se je pojavil cenovno ugoden mobilni robot Pioneer za izobraževalne in raziskovalne namene [19]. 1996 Organiziran je bil prvi robotski nogometni turnir, leto kasneje pa je bila ustanovljena FIRA (angl. Federation of international robot-soccer association) [20]. 1996–1997 NASA je v okviru projekta Mars Pathfinder na Mars poslala rover Sojourner [21], ki je sprejemal ukaze iz Zemlje ter se je lahko samostojno peljal po vnaprej določeni poti in se pri tem izogibal nevarnim situacijam. 2002 Na tržišču se je pojavil prvi model robotskega sesalnika Roomba za domačo rabo [22]. 10 Uvod v mobilne sisteme 2004 Marsova roverja dvojčka Spirit in Opportunity sta pristala na Marsu [23]. Rover Spirit se je leta 2009 zagozdil, rover Opportunity pa je še vedno aktiven in je leta 2014 podrl rekord za najdaljšo zunajzemeljsko prepotovano razdaljo, ki ga je postavil Lunokhod 2. 2004 Prvo tekmovanje DARPA Grand Challenge je potekalo v puščavi Mojave (ZDA). Nobeno avtonomno vozilo ni dokončalo 240 km dolge proge [24]. 2005 Na drugem tekmovanju DARPA Grand Challenge je avtonomno vozilo Stanley iz Univerze v Stanfordu prvo dokončalo progo. Še štiri druga vozila (od 23) so uspešno opravila nalogo [25]. 2007 Organizirano je bilo tekmovanje DARPA Urban Grand Challenge, kjer je šest avtonomnih vozil uspešno prevozilo progo v urbanem okolju. Zahtevano je bilo upoštevanje vseh prometnih pravil ter uspešno vključevanje v promet [26]. 2009 Izšla je prvotna različica robotskega operacijskega sistema ROS 0.4 (angl. Robot operating system) [27]. 2009 Google je začel (na kalifornijskih avtocestah) preizkušati svojo tehnologijo avtonomne vožnje s predelanim avtom Toyota Prius [28]. 2010 V izzivu VisLab Intercontinental Autonomous Challenge [29] so štiri avtonomna vozila brez pomoči človeka opravila skoraj 6000 km dolgo potovanje od Parme v Italiji do Šanghaja na Kitajskem. 2012 Na Marsu je uspešno pristal Nasin robotski rover Curiosity [30], ki je še vedno aktiven. 2014 Google je razkril nov prototip avtonomnega vozila brez volana in pedalov [28]. 2015 Podjetje Tesla v določenih modelih svojih električnih vozil omogoči sisteme za avtonomno vožnjo. V petih letih skupno število kilome- trov, ki jih prevozijo lastniki vozil v avtonomnem načinu delovanja (druga stopnja avtonomnosti), preseže 5 milijard [31]. 2016 Od junija je flota samovoznih vozil podjetja Google v avtonomnem načinu skupaj prevozila 2 777 585 km [32]. 2016 Zagonsko podjetje comma.ai, ki ga je ustanovil George Hotz, izda prvo delujočo različico odprtokodne programske opreme za razvoj avtonomne vožnje, ki v osnovi temelji na uporabi kamere za zazna- vanje okolja. 2017 Izide prva verzija odprtokodnega simulacijskega okolja CARLA za razvoj algoritmov za avtonomno vožnjo v urbanem okolju [33]. 1.5. Kratka zgodovina 11 2018 Podjetje Waymo vzpostavi storitev avtonomnega prevoza oseb za končne uporabnike v kraju Phoenix (Arizona) [34]. 2019 Podjetje Waymo je s svojimi avtonomnimi vozili prevozilo več kot 10 milijonov kilometrov v resničnem svetu (kar je več kot 200-krat okoli Zemlje oz. 10-krat do Lune in nazaj) in več kot 10 milijard kilometrov v simulacijskem okolju (kar je več kot pot, ki jo Zemlja napravi okoli Sonca v 10 letih) [35]. 2021 Na Marsu je marca v kraterju Jezero (v bližini dolin Neretva vallis in Sava vallis) pristal Nasin rover Perserverance s helikopterjem Ingenuity na krovu [36]. Rover je opremljen s 23 kamerami, od tega se jih 9 uporablja za navigacijo, detekcijo ovir in planiranje poti pri avtonomni vožnji. Dva meseca kasneje je na Marsu pristal še kitajski rover Žurong [37]. 12 Uvod v mobilne sisteme Literatura [1] M. Rosheim. Leonardo’s Lost Robots. Springer Berlin Heidelberg, 2006. [2] Mobile robot. https://en.wikipedia.org/wiki/Mobile_robot, 2020. Dostopano: 5. 10. 2020. [3] Nikola Tesla. https://en.wikipedia.org/wiki/Nikola_Tesla, 2020. Dostopano: 5. 10. 2020. [4] N. Tesla. Method of and apparatus for controlling mechanism of moving vessels or vehicles, 1898. US Patent 613,809. [5] V-1 flying bomb. https://en.wikipedia.org/wiki/V-1_flying_bomb, 2020. Dostopano: 5. 10. 2020. [6] V-2 rocket. https://en.wikipedia.org/wiki/V-2_rocket, 2020. Dostopano: 5. 10. 2020. [7] D. Jerison in D. Stroock. Norbert Wiener. Notices of the AMS, zv. 42, št. 4, str. 430–438, 1995. [8] William Grey Walter. https://en.wikipedia.org/wiki/William_Grey_Walter, 2020. Dostopano: 5. 10. 2020. [9] D. P. Watson in D. H. Scheidt. Autonomous systems. Johns Hopkins APL technical digest, zv. 26, št. 4, str. 368–376, 2005. [10] N. J. Nilsson. Shakey the robot. Tehn. por., SRI International, 1984. [11] S. L. Bellinger. Self-propelled random motion lawnmower, 1972. US Patent 3,698,523. [12] I. Karachevtseva, J. Oberst in sod. Cartography of the Lunokhod-1 landing site and traverse from LRO image and stereo-topographic data. Planetary and Space Science, zv. 85, str. 175–187, 2013. [13] Lunokhod 2. https://en.wikipedia.org/wiki/Lunokhod_2, 2020. Dostopano: 5. 10. 2020. [14] Viking project information. http://nssdc.gsfc.nasa.gov/planetary/viking.html, 2020. Dostopano: 5. 10. 2020. [15] G. Giralt, R. Chatila in M. Vaisset. An integrated navigation and motion control system for autonomous multisensory mobile robots. V Autonomous robot vehicles, str. 420–443. Springer, 1990. [16] H. P. Moravec. The Stanford cart and the CMU rover. Proceedings of the IEEE, zv. 71, str. 872–884, 1983. [17] HERO (robot). https://en.wikipedia.org/wiki/HERO_(robot), 2020. Dostopano: 5. 10. 2020. [18] E. D. Dickmanns. Dynamic vision for perception and control of motion. Springer Science & Business Media, 2007. [19] Pioneer 3 - Robots: Your guide to the world of robotics. https://robots.ieee.org/ robots/pioneer, 2020. Dostopano: 5. 10. 2020. [20] FIRA RoboworldCup Official Website. https://www.firaworldcup.org, 2020. Dostopano: 5. 10. 2020. [21] NASA – Mars Pathfinder and Sojourner. http://www.nasa.gov/mission_pages/ mars-pathfinder/, 2019. Dostopano: 5. 10. 2020. [22] iRobot history. http://www.irobot.com/About-iRobot/Company-Information/History. aspx, 2020. Dostopano: 5. 10. 2020. Literatura 13 [23] NASA Mars Exploration Rovers – Spirit and Opportunity. http://www.nasa.gov/mission_ pages/mer/index.html, 2018. Dostopano: 5. 10. 2020. [24] DARPA Grand Challenge 2004. https://archive.darpa.mil/grandchallenge04, 2014. Dostopano: 5. 10. 2020. [25] DARPA Grand Challenge 2005. https://archive.darpa.mil/grandchallenge05, 2014. Dostopano: 5. 10. 2020. [26] DARPA Urban Challenge. https://archive.darpa.mil/grandchallenge, 2014. Dostopano: 5. 10. 2020. [27] ROS.org, history. http://www.ros.org/history/, 2020. Dostopano: 5. 10. 2020. [28] Waymo. https://waymo.com, 2020. Dostopano: 5. 10. 2020. [29] The VisLab Intercontinental Autonomous Challenge. https://vislab.it/viac, 2020. Dostopano: 5. 10. 2019. [30] NASA Mars Science Laboratory – Curiosity. https://www.nasa.gov/mission_pages/msl/ index.html, 2020. Dostopano: 5. 10. 2020. [31] Lex Fridman Tesla Vehicle Deliveries and Autopilot Mileage Statistics. https:// lexfridman.com/tesla-autopilot-miles-and-vehicles, 2020. Dostopano: 1. 3. 2021. [32] Google Self-Driving Car Project: Monthly Report, June 2016. https: //static.googleusercontent.com/media/www.google.com/en//selfdrivingcar/files/ reports/report-0616.pdf, 2016. Dostopano: 18. 7. 2016. [33] A. Dosovitskiy, G. Ros in sod. CARLA: An open urban driving simulator. V Proceedings of the 1st Annual Conference on Robot Learning, str. 1–16. 2017. [34] Waymo Story. https://waymo.com/company/#story, 2021. Dostopano: 1. 3. 2021. [35] TechCrunch Waymo has now driven 10 billion autonomous miles in simulation. https://techcrunch.com/2019/07/10/ waymo-has-now-driven-10-billion-autonomous-miles-in-simulation, 2019. Do- stopano: 1. 3. 2021. [36] NASA Mars Perseverance Rover. https://www.nasa.gov/perseverance, 2021. Dostopano: 1. 3. 2021. [37] CNSA Probe makes historic landing on Mars. http://www.cnsa.gov.cn/english/ n6465652/n6465653/c6812005/content.html, 2021. Dostopano: 25. 5. 2021. 2 Modeliranje gibanja mobilnih sistemov 2.1 Uvod Človek že več tisoč let izkorišča prednosti kolesnega pogona. Osnovna zgradba prazgodovinskega dvokolesnega vozička (slika 2.1) je enaka tisti v modernih avtomobilih in kolesnih robotih. V tem poglavju je predstavljeno modeliranje gibanja različnih kolesnih mobilnih sistemov. Dobljeni model se lahko uporabi v različne namene. V knjigi ga bomo večinoma uporabljali za načrtovanje strategij lokomocije sistema. Lokomocija je proces gibanja avtonomnega sistem z enega mesta na drugo. Modeli gibanja lahko opisujejo kinematiko robota, kjer nas zanima matemati- čen zapis gibanja brez upoštevanja sil in navorov, ki v splošnem tako gibanje povzročijo. Kinematični model opisuje geometrijske relacije v sistemu, to so relacije med vhodnimi parametri in vedenjem sistema, ki jih podajajo stanja sistema. Kinematični model opisuje hitrosti sistema in je predstavljen z množico diferencialnih enačb prvega reda. Dinamični model pa opisuje gibanje sistema zaradi sil, ki delujejo nanj. Tovrstni model vključuje fizikalne veličine, kot so sile, energije, masa sistema, vztrajnost in hitrosti. Opisi dinamičnih modelov so podani z diferencialnimi enačbami drugega reda. Pri načrtovanju gibanja kolesnih mobilnih robotov običajno uporabimo kine- matične modele, medtem ko za druge (bolj kompleksne) sisteme, kot so zračna 16 Modeliranje gibanja mobilnih sistemov Slika 2.1: Dvokolesni voziček [Muzej in galerije mesta Ljubljane, slikar: I. Rehar] plovila, zračni in nožni roboti, hitra kolesna vozila ipd., uporabljamo dinamične modele gibanja. 2.2 Kinematika kolesnih mobilnih siste- mov Obstaja več različnih kinematičnih modelov: • Notranja kinematika pojasnjuje relacije med notranjimi spremenljivkami sistema (npr. kako vrtenje koles vpliva na gibanje vozila). • Zunanja kinematika opisuje pozicijo in orientacijo vozila glede na referenčni koordinatni sistem. • Direktna kinematika modelira stanja sistema kot funkcijo vhodov (hi- trosti koles, gibanje sklepov, zasuk krmilnega kolesa itd.), inverzna ki- nematika pa se uporablja za načrtovanje gibanja, torej podaja vhode v sistem, ki so potrebni za doseg želenega stanja. • Omejitve gibanja se tipično pojavijo, ko ima sistem manj vhodnih spre- menljivk kot prostostnih stopenj (neholonomične omejitve). Holonomične omejitve omejujejo dosegljivost določenih stanj sistema, medtem ko neholo- nomične omejitve omejijo smeri možnih premikov sistema (kolesa robota se lahko vrtijo le v smeri njihove orientacije). Število prostostnih stopenj je minimalno število stanj s katerimi lahko opišemo konfiguracijo sistema. V nadaljevanju sledi nekaj primerov določitve notranje kinematike kolesnih 2.2. Kinematika kolesnih mobilnih sistemov 17 Ym Yg Xm j y x Xg Slika 2.2: Vozilo v ravnini mobilnih robotov. Lega robota v ravnini je podana z vektorjem stanj   x( t) q( t) =  y( t)   ϕ( t) v globalnih koordinatah ( Xg, Yg), kot je prikazano na sliki 2.2. Premični koordinatni sistem ( Xm, Ym) je pripet na mobilnega robota. Relacija med globalnim in premičnim koordinatnim sistemom (zunanja kinemtika) je podana z vektorjem translacije [ x, y] T in rotacijsko matriko   cos ϕ sin ϕ 0 R( ϕ) = − sin ϕ cos ϕ 0   0 0 1 Kolesni mobilni robot se giblje s pomočjo koles, ki se vrtijo zaradi trenja med njimi in podlago. Pri zmernih hitrostih običajno predpostavimo model idealnega kotaljenja koles, kjer se lahko kolo premika le zaradi rotacije (kotaljenja), brez zdrsov v smeri kotaljenja ali pravokotno na smer kotaljenja. Vsako kolo se lahko prosto vrti okoli lastne osi, torej obstaja točka, ki leži na presečišču vseh osi koles. Ta točka se imenuje trenutni center rotacije (ICR, angl. instantaneous center of rotation) ali trenutni center ukrivljenosti (ICC, angl. instantaneous center of curvature) in določa točko, okoli katere vsa kolesa krožijo z enako krožno hitrostjo ω glede na ICR. Za nadaljnje branje si lahko pogledate [1–3]. 2.2.1 Diferencialni pogon Diferencialni pogon je zelo preprost in zato precej pogosto uporabljen mehanizem pogona, predvsem pri manjših vozilih ali mobilnih robotih. Vozilo s takim 18 Modeliranje gibanja mobilnih sistemov ICR w Ym Y v g L R v Xm j y vR L r x Xg Slika 2.3: Kinematika diferencialnega pogona pogonom ima ponavadi eno ali dve dodatni podporni kolesi (angl. castor ), ki podpirata vozilo in preprečujeta njegovo prevračanje. Kolesi diferencialnega pogona sta vpeti na skupno os, hitrost vrtenja vsakega kolesa pa je poljubna in gnana s svojim motorjem. Glede na sliko 2.3 sta vhodni (regulirni) spremenljivki hitrost desnega kolesa vR( t) in hitrost levega kolesa vL( t). Ostale spremenljivke na sliki 2.3 so: r – radij kolesa, L – razdalja med kolesoma in R( t) – trenutni radij trajektorije vožnje vozila oz. razdalja med središčem vozila (središčna točka med kolesoma) in točko ICR. V vsakem časovnem trenutku imata obe kolesi enako kotno hitrost ω( t) okrog ICR vL( t) ω( t) = R( t) − L 2 vR( t) ω( t) = R( t) + L 2 od koder izrazimo ω( t) in R( t) kot vR( t) − vL( t) ω( t) = L L vR( t) + vL( t) R( t) = 2 vR( t) − vL( t) Tangencialna hitrost vozila je vR( t) + vL( t) v( t) = ω( t) R( t) = 2 Obodni hitrosti koles sta vL( t) = rωL( t) in vR( t) = rωR( t), kjer sta ωL( t) in ωR( t) kotni hitrosti levega in desnega kolesa okoli njune osi. Upoštevajoč navedene 2.2. Kinematika kolesnih mobilnih sistemov 19 relacije lahko zapišemo notranjo kinematiko (v lokalnih koordinatah) kot       ˙ x r r m( t) vX ( t) " # m 2 2 ωL( t)  ˙ y      m( t) = vY ( t) = 0 0 (2.1)    m    ωR( t) ˙ ϕ( t) ω( t) − r r L L Zunanja kinematika robota (v globalnih koordinatah) pa je     ˙ x( t) cos ϕ( t) 0 " # v( t)  ˙ y( t) = sin ϕ( t) 0 (2.2)     ω( t) ˙ ϕ( t) 0 1 kjer sta v( t) in ω( t) vhodni (regulirni) spremenljivki. Model (2.2) lahko s pomočjo Eulerjeve integracijske metode zapišemo v diskretni obliki (2.3), ki je veljavna za diskretne čase vzorčenja t = kTs, k = 0 , 1 , 2 , . . . , kjer je Ts čas vzorčenja x( k + 1) = x( k) + v( k) Ts cos ϕ( k) y( k + 1) = y( k) + v( k) Ts sin ϕ( k) (2.3) ϕ( k + 1) = ϕ( k) + ω( k) Ts Direktna in inverzna kinematika Lego robota v trenutku t dobimo z integracijo kinematičnega modela, kar imenujemo odometrija (angl. odometry, dead reckoning). Določitev lege robota s podanimi vhodnimi spremenljivkami imenujemo direktna kinematika t Z x( t) = v( t) cos ϕ( t) d t 0 t Z y( t) = v( t) sin ϕ( t) d t (2.4) 0 t Z ϕ( t) = ω( t) d t 0 Če med časi vzorčenja predpostavimo konstantni hitrosti v in ω, lahko integracijo v enačbah (2.4) izračunamo numerično z uporabo Eulerjeve metode. Dobimo direktno kinematiko x( k + 1) = x( k) + v( k) Ts cos ϕ( k) y( k + 1) = y( k) + v( k) Ts sin ϕ( k) ϕ( k + 1) = ϕ( k) + ω( k) Ts 20 Modeliranje gibanja mobilnih sistemov Z uporabo trapezne integracijske metode dobimo bolj točen rezultat numerične integracije ω( k) T s x( k + 1) = x( k) + v( k) Ts cos ϕ( k) + 2 ω( k) T s y( k + 1) = y( k) + v( k) Ts sin ϕ( k) + 2 ϕ( k + 1) = ϕ( k) + ω( k) Ts V primeru uporabe eksaktne integracije pa je direktna kinematika v( k) x( k + 1) = x( k) + (sin ( ϕ( k) + ω( k) Ts) − sin ϕ( k)) ω( k) v( k) y( k + 1) = y( k) − (cos ( ϕ( k) + ω( k) Ts) − cos ϕ( k)) ω( k) ϕ( k + 1) = ϕ( k) + ω( k) Ts kjer integriramo znotraj intervala vzorčenja in za hitrosti v in ω predvidimo sledeče spremembe stanj Z ( k+1) Ts Z ( k+1) Ts ∆ x( k) = v( k) cos ϕ( t) d t = v( k) cos ( ϕ( k) + ω( k)( t − kTs)) d t kTs kTs Z ( k+1) Ts Z ( k+1) Ts ∆ y( k) = v( k) sin ϕ( t) d t = v( k) sin ( ϕ( k) + ω( k)( t − kTs)) d t kTs kTs Zapis inverzne kinematike je bolj zahtevna naloga, saj moramo določiti ustrezne vhode, da se bo robot peljal v želeno lego ali po želeni trajektoriji. Mobilni roboti so običajno izpostavljeni neholonomičnim omejitvam (poglavje 2.3), ki onemogočajo poljubne smeri vožnje. Obstaja tudi več možnih rešitev (poti) za doseg želene lege. Preprosta rešitev inverzne kinematike je možna, če dovolimo le premo gibanje vozila ( vR( t) = vL( t) = vR =⇒ ω( t) = 0, v( t) = vR) ali le kroženje na mestu ( vR( t) = − vL( t) = vR =⇒ ω( t) = 2 vR , v( t) = 0) s konstantnimi hitrostmi. Za L kroženje na mestu se enačbe gibanja (2.4) poenostavijo v x( t) = x(0) y( t) = y(0) (2.5) 2 vRt ϕ( t) = ϕ(0) + L za premo gibanje pa se enačbe gibanja (2.4) poenostavijo v x( t) = x(0) + vRt cos ϕ(0) y( t) = y(0) + vRt sin ϕ(0) (2.6) ϕ( t) = ϕ(0) Možna strategija gibanja je usmeritev vozila proti ciljni legi z rotacijo, nato sledi prema vožnja proti cilju, na koncu pa poravnava dejanske orientacije vozila 2.2. Kinematika kolesnih mobilnih sistemov 21 z želeno (ciljno) orientacijo. Zahtevane vhodne spremenljivke za vsako fazo (rotacija, premo gibanje, rotacija) se lahko enostavno izrazijo iz (2.5) in (2.6). Če predpostavimo diskretno notacijo, kjer sta hitrosti vR( k) in vL( k) konstantni znotraj intervala vzorčenja Ts in se lahko spreminjata le v časovnih trenutkih t = kTs, lahko zapišemo enačbe gibanja robota. Za kroženje na mestu ( vR( k) = − vL( k)) imamo x( k + 1) = x( k) y( k + 1) = y( k) (2.7) 2 vR( k) Ts ϕ( k + 1) = ϕ( k) + L in za premo gibanje ( vR( k) = vL( k)) x( k + 1) = x( k) + vR( k) Ts cos ϕ( k) y( k + 1) = y( k) + vR( k) Ts sin ϕ( k) (2.8) ϕ( k + 1) = ϕ( k) Za želeno gibanje vozila znotraj intervala vzorčenja t ∈ [ kTs, ( k + 1) Ts) lahko za vsak vzorec časa izračunamo inverzno kinematiko tako, da izrazimo vhodne spremenljivke iz (2.7) in (2.8). Kot smo že omenili, obstaja več različnih gladkih poti, ki pripeljejo vozilo v želeno lego, kar otežuje izvedbo inverzne kinematike. Inverzna kinematika pa je enostavna, če imamo predpisano želeno gladko trajektorijo ( x( t), y( t)), ki ji mora vozilo slediti tako, da je njegova orientacija vedno tangentna na trajektorijo. Trajektorija je definirana v časovnem intervalu t ∈ [0 , T ]. Ob predpostavki, da je začetna lega vozila na želeni trajektoriji ter imamo idealen kinematični model, lahko izračunamo potrebne regulirne veličine (vhode) v kot v( t) = ±p ˙ x 2( t) + ˙ y 2( t) (2.9) kjer predznak določa želeno smer vožnje (+ za vožnjo naprej, – za vzvratno vožnjo). Kot tangente v vsaki točki na trajektoriji je določen z ϕ( t) = atan2 ( ˙ y( t) , ˙ x( t)) + lπ (2.10) kjer l ∈ {0 , 1} definira želeno smer vožnje (0 za vožnjo naprej in 1 za vzvratno vožnjo). Funkcija atan2 ( y, x) je štirikvadrantna razširitev funkcije arctan yx arctan y ; x > 0   x    arctan y + π ; x < 0 in y ≥ 0  x    arctan y − π ; x < 0 in y < 0 atan2 ( y, x) = x (2.11) π  ; x = 0 in y > 0  2    − π ; x = 0 in y < 0   2   nedoločeno ; x = 0 in y = 0 22 Modeliranje gibanja mobilnih sistemov Y ICR g w R a v j y d x Xg Slika 2.4: Kinematika kolesnega pogona Z odvajanjem (2.10) po času dobimo kotno hitrost vozila ω( t) ˙ x( t)¨ y( t) − ˙ y( t)¨ x( t) ω( t) = = v( t) κ( t) (2.12) ˙ x 2( t) + ˙ y 2( t) kjer je κ( t) ukrivljenost trajektorije. Z uporabo relacij (2.9) in (2.12) ter pred-pisane referenčne poti vozila ( x( t), y( t)) lahko izračunamo potrebni regulirni veličini v( t) in ω( t). Potrebna pogoja pri načrtovanju poti sta, da je pot dvakrat odvedljiva in da je tangencialna hitrost različna od nič ( v( t) 6= 0). Če je pri nekem času t tangencialna hitrost v( t) = 0, se robot vrti na mestu s krožno hitrostjo ω( t). Kota ϕ( t) ne moremo določiti iz enačbe (2.9), torej mora biti podan eksplicitno. Prikazano inverzno kinematiko za znano trajektorijo lahko uporabimo pri vodenju kot predkrmiljenje, ki je dodatek povratnozančnemu vodenju za odpravo motenj, vplivov zaradi netočnega modela kinematike in začetnih pogreškov lege vozila [4]. 2.2.2 Kolesni pogon Kolesni pogon, prikazan na sliki 2.4, ima krmilno kolo s kotom krmiljenja α in se kotali s kotno hitrostjo ωs (pogon na prednje kolo). Točka ICR je določena s presečiščem osi prednjega in zadnjega kolesa. V danem trenutku kolo kroži okoli ICR s kotno hitrostjo ω, radijem R in razdaljo med kolesoma d π d R( t) = d tan − α( t) = 2 tan α( t) Krmilno kolo kroži okoli ICR s kotno hitrostjo ω, zato lahko zapišemo vs( t) vs( t) ω( t) = ˙ ϕ( t) = = sin α( t) p d 2 + R 2( t) d kjer je vs( t) = ωs( t) r obodna hitrost in r radij krmilnega kolesa. 2.2. Kinematika kolesnih mobilnih sistemov 23 Notranja kinematika vozila (v koordinatnem sistemu robota) je določena z ˙ xm( t) = vs( t) cos α( t) ˙ ym( t) = 0 (2.13) vs( t) ˙ ϕ( t) = sin α( t) d zunanja kinematika pa z ˙ x( t) = vs( t) cos α( t) cos ϕ( t) ˙ y( t) = vs( t) cos α( t) sin ϕ( t) vs( t) ˙ ϕ( t) = sin α( t) d oziroma v matrični obliki     ˙ x( t) cos ϕ( t) 0 " # v( t)  ˙ y( t) = sin ϕ( t) 0 (2.14)     ω( t) ˙ ϕ( t) 0 1 kjer je v( t) = vs( t) cos α( t) in ω( t) = vs( t) sin α( t). d Kolesni pogon na zadnje kolo Običajno imajo vozila (kolo, tricikel in nekateri avtomobili) pogon na zadnja kolesa. V tem primeru sta regulirni veličini hitrost zadnjega kolesa vr( t) in kot krmiljenja sprednjega (krmilnega) kolesa α( t). Notranjo kinematiko lahko enostavno izpeljemo iz (2.13), kjer upoštevamo vr( t) = vs( t) cos α( t) ˙ xm( t) = vr( t) ˙ ym( t) = 0 vr( t) ω( t) = ˙ ϕ( t) = tan α( t) d in zunanja kinematika je ˙ x( t) = vr( t) cos ϕ( t) ˙ y( t) = vr( t) sin ϕ( t) (2.15) vr( t) ˙ ϕ( t) = tan α( t) d oziroma v matrični obliki     ˙ x( t) cos ϕ( t) 0 " # vr( t)  ˙ y( t) = sin ϕ( t) 0     ω( t) ˙ ϕ( t) 0 1 kjer je ω( t) = vr( t) tan α( t). d 24 Modeliranje gibanja mobilnih sistemov Direktna in inverzna kinematika Z upoštevanjem (2.14) lahko zapišemo direktno kinematiko kolesa s sprednjim pogonom z (2.4), podobno kot smo zapisali pri diferencialnem pogonu. V splošnem je inverzno kinematiko zelo težko rešiti, lahko pa problem precej poenostavimo z vpeljavo strategije gibanja z dvema osnovnima načinoma premika. Prvi način predstavlja premo gibanje v smeri naprej ( α( t) = 0), drugi način pa kroženje na mestu ( α( t) = ± π ). Pri premem gibanju se hitrosti vozila 2 poenostavijo v v( t) = vs( t) in ω( t) = 0. Z vstavitvijo teh hitrosti v (2.14) in diskretizacijo dobimo sledeče enačbe gibanja x( k + 1) = x( k) + vs( k) Ts cos ϕ( k) y( k + 1) = y( k) + vs( k) Ts sin ϕ( k) (2.16) ϕ( k + 1) = ϕ( k) V primeru kroženja na mestu pa se hitrosti vozila poenostavijo v v( t) = 0 in ω( t) = vs( t) . Z vstavitvijo teh hitrosti v (2.14) in diskretizacijo dobimo sledeči d model gibanja x( k + 1) = x( k) y( k + 1) = y( k) (2.17) vs( t) ϕ( k + 1) = ϕ( k) + Ts d Regulirni veličini (vhoda v sistem) lahko določimo iz (2.16) in (2.17) za želeno gibanje med časi vzorčenja. 2.2.3 Trikolesni pogon Trikolesni pogon, prikazan na sliki 2.5, ima enako kinematiko kot kolesni pogon ˙ x( t) = vs( t) cos α( t) cos ϕ( t) ˙ y( t) = vs( t) cos α( t) sin ϕ( t) (2.18) vs( t) ˙ ϕ( t) = sin α( t) d kjer velja v( t) = vs( t) cos α( t), ω( t) = vs( t) sin α( t) in v d s je obodna hitrost krmilnega kolesa. Trikolesni pogon je pogosto uporabljen v mobilni robotiki, ker tri kolesa zagotavljajo stabilnost vozila v vertikalni smeri in tako pomožna podporna kolesa niso potrebna. 2.2.4 Tricikel s priklopnikom Kinematika tricikla je opisana v poglavju 2.2.3. Za priklopnik določimo točko 2.2. Kinematika kolesnih mobilnih sistemov 25 ICR Yg w R a v j y d x Xg Slika 2.5: Kinematika trikolesnega pogona Yg ICR Yg w R a v L j y d R 2 b ICR 2 x Xg Slika 2.6: Kinematika tricikla s priklopnikom ICR 2, ki leži na presečišču zadnje osi tricikla in osi priklopnika. Kotna hitrost, s katero kolesa priklopnika krožijo okoli točke ICR 2, je v( t) vs( t) cos α( t) vs( t) cos α( t) sin β( t) ω 2( t) = = = = ˙ β( t) R 2( t) R 2( t) L 26 Modeliranje gibanja mobilnih sistemov in končna kinematika vozila na sliki 2.6 je določena z ˙ x( t) = vs( t) cos α( t) cos ϕ( t) ˙ y( t) = vs( t) cos α( t) sin ϕ( t) vs( t) ˙ ϕ( t) = sin α( t) d ˙ vs( t) cos α( t) sin β( t) β( t) = L 2.2.5 Avtomobilski (Ackermannov) pogon Avtomobilski pogon uporablja Ackermannov princip krmiljenja, čigar osnovna ideja je, da ima notranje kolo (tisto, ki je bližje točki ICR) večji zasuk krmiljenja kot zunanje. To omogoča vozilu, da se vrti okoli središčne točke na osi zadnjih koles. Posledično ima notranje kolo manjšo obodno hitrost kot zunanje. Ackermannovo krmiljenje omogoča vrtenje zadnjih koles brez zdrsov, zato točka ICR leži na premici, ki gre skozi zadnjo os. Ta krmilni mehanizem omogoča manjšo obrabo pnevmatik. Na sliki 2.7 je levo kolo na zunanji strani, desno pa Yg d a L vL v y a R l/2 vR R w ICR x Xg Slika 2.7: Shema Ackermannovega pogona na notranji. Orientacijo prednjih krmilnih koles lahko določimo iz π R + l tan − α 2 L = 2 d π R − l tan − α 2 R = 2 d 2.2. Kinematika kolesnih mobilnih sistemov 27 od koder izrazimo kote krmiljenja π R + l α 2 L = − arctan 2 d π R − l α 2 R = − arctan 2 d Notranje in zunanje zadnje kolo krožita okoli točke ICR z enako kotno hitrostjo ω, torej sta njuni obodni hitrosti l vL = ω R + 2 l vR = ω R − 2 Ackermannov kinematični pogon je primeren za modeliranje gibanja večjih vozil. Model gibanja lahko opišemo tudi z uporabo kinematike tricikla (2.18), kjer uporabimo povprečen Ackermannov kot krmiljenja α = π − arctan R . Inverzna 2 d kinematika Ackermannovega pogona je zahtevna in presega namen tega dela. 2.2.6 Sinhroni pogon Vozilo s sinhronim pogonom lahko vsa svoja kolesa sinhrono krmili okoli vertikalne osi (v danem trenutku imajo vsa kolesa enako orientacijo). Tipično ima vozilo s sinhronim pogonom tri kolesa, ki so razporejena simetrično (v enakostraničnem trikotniku) okoli središča vozila, kot je prikazano na sliki 2.8. Vsa kolesa so Yg w w v v j y w v x Xg Slika 2.8: Sinhroni pogon krmiljena sinhrono, torej so njihove osi vrtenja zmeraj vzporedne in zato se točka ICR nahaja v neskončnosti. Vozilo lahko neposredno spreminja orientacijo koles, kar predstavlja tretje stanje v vektorju stanj (2.19). Regulirne veličine so hitrost krmiljenja koles ω in njihova obodna hitrost v. 28 Modeliranje gibanja mobilnih sistemov Kinemtika vozila s sinhronim pogonom je podobna kinematiki difencialnega pogona     ˙ x( t) cos ϕ( t) 0 " # v( t)  ˙ y( t) = sin ϕ( t) 0 (2.19)     ω( t) ˙ ϕ( t) 0 1 kjer sta v( t) in ω( t) regulirni spremenljivki ali vhoda, ki ju lahko neodvisno spreminjamo (to pri diferencialnem pogonu ni možno). Direktna in inverzna kinematika Direktno kinematiko dobimo z integracijo kinematičnega modela (2.19). t Z x( t) = v( t) cos ϕ( t) d t 0 t Z y( t) = v( t) sin ϕ( t d t 0 t Z ϕ( t) = ω( t) d t 0 Splošna rešitev inverzne kinematike ni možna, ker obstaja več rešitev za doseg želene lege. Inverzna kinematika je enostavno rešljiva v posebnem primeru, kjer se vozilo vrti na mestu ali pa premo giblje v smeri trenutne orientacije (brez rotacije). Ko se robot določen časovni interval ∆ t vrti na mestu s konstantno krožno hitrostjo ω, se njegova orientacija spremeni za ω∆ t. V primeru premega gibanja s konstantno hitrostjo v, ki traja ∆ t, se vozilo premakne za v∆ t v smeri trenutne orientacije. 2.2.7 Večsmerni pogon V predhodno opisanih kinematičnih modelih so bila uporabljena preprosta kolesa, ki se lahko vrtijo (kotalijo) le v smeri njihove orientacije (npr. diferencialni pogon). Tovrstna preprosta kolesa imajo samo eno možno smer kotaljenja. Da omogočimo večsmerno kotaljenje, potrebujemo bolj kompleksno konstrukcijo koles. Primer takega kolesa je kolo Mecanum ali švedsko kolo (slika 2.9), ki ima po obodu razvrščenih več valjčkov. Osi pasivnih valjčkov niso vzporedne z osjo glavnega kolesa, ampak so običajno pod kotom γ = 45°. To omogoča različne smeri gibanja, ki izhajajo iz poljubne kombinacije smeri vrtenja glavnega kolesa in pasivnih valjčkov. Drug primer kompleksnega kolesa je kolo omni, ki omogoča večsmerno gibanje, podobno kot kolo Mecanum. Kolo omni (slika 2.10) ima na svojem obodu nameščene pasivne valjčke, katerih os je pravokotna glede na os kolesa. 2.2. Kinematika kolesnih mobilnih sistemov 29 Slika 2.9: Kolo Mecanum z valjčki, nameščenimi po obodu kolesa. Vsak valjček ima os vrtenja pod kotom 45° glede na ravnino koles in pod kotom 45° glede na linijo, vzporedno z osjo kolesa. Slika 2.10: Kolo omni s šestimi “prostovrtečimi” se valjčki, ki so razporejeni po obodu kolesa. Kolo se lahko vrti in bočno drsi (vrtijo se valjčki). Kinematika štirikolesnega večsmernega pogona Priljubljena štirikolesna platforma Mecanum, prikazana na sliki 2.11, ima kolesa z levo in desnosučnimi valjčki, kjer sta diagonalni kolesi istega tipa. To omogoča vozilu, da se premika v poljubni smeri s poljubno rotacijo, kar dosežemo s spreminjanjem hitrosti in smeri vrtenja glavnih koles. Če se vsa štiri kolesa vrtijo v isti smeri (z isto hitrostjo), se vozilo giblje naprej ali vzvratno. Ko pa se glavni kolesi na eni strani platforme vrtijo v nasprotni smeri kot kolesi na drugi strani platforme, bo le-ta krožila. Bočno gibanje platforme dosežemo tako, da se kolesi na eni diagonali vrtijo v nasprotni smeri kot kolesi na drugi diagonali. Kombinacija opisanih gibanj omogoča gibanje platforme v poljubni smeri s poljubno rotacijo. Inverzno notranjo kinematiko štirikolesnega pogona Mecanum na sliki 2.12 lahko zapišemo na naslednji način. Hitrost sprednjega kolesa (v koordinatnem sistemu robota) pridobimo iz hitrosti glavnega kolesa v 1( t) in hitrosti pasivnih valjčkov vR( t). V nadaljevanju bomo izpustili zapis s časovno odvisnostjo, da dobimo bolj kompaktne in enostavne enačbe (npr. v 1( t) = v 1). Skupna hitrost koles v smereh 30 Modeliranje gibanja mobilnih sistemov Slika 2.11: Osnovne smeri gibanja večsmernega pogona s štirimi kolesi Mecanum, ki se lahko vrtijo naprej ali nazaj ym v R v 1 v 2 g=45° v R d y l xm j v R v 4 v v R 3 x Slika 2.12: Štirikolesna platforma Mecanum xm in ym v koordinatnem sistemu robota je vm 1 x = v 1 + vR cos π = v in 4 1 + vR √2 vm 1 y = vR sin π = vR √ , od koder pridobimo hitrost (sprednjega) glavnega kolesa 4 2 v 1 = vm 1 x − vm 1 y. Hitrost sprednjega kolesa v koordinatnem sistemu robota lahko izrazimo tudi s translacijsko hitrostjo robota vm = p ˙ x 2 + ˙ y 2 in njegovo m m kotno hitrostjo ˙ ϕ kot vm 1 x = ˙ xm − ˙ ϕd in vm 1 y = ˙ ym + ˙ ϕl (pomen razdalj d in l je mogoče razbrati iz slike 2.12). Iz slednjih relacij lahko izrazimo hitrost glavnega kolesa s hitrostjo robota kot v 1 = ˙ xm − ˙ ym − ( l + d) ˙ ϕ. Podobne enačbe lahko zapišemo za v 2, v 3 in v 4. Torej je inverzna kinematika v lokalnih koordinatah  v    1 1 −1 − l − d   ˙ xm  v 2 1 1 − l − d   =    ˙ y  m (2.20)  v  1 −1 l + d     3   ˙ ϕ v 4 1 1 l + d ki jo lahko zapišemo v matrični obliki kot v = J ˙ q m, kjer je v T = [ v 1 , v 2 , v 3 , v 4] T in q T = [ x m m, ym, ϕ] T . Za izračun inverzne kinematike v globalnih koordinatah moramo obravnavati 2.2. Kinematika kolesnih mobilnih sistemov 31 rotacijsko matriko R L , ki predstavlja orientacijo lokalnih koordinat glede na G globalne (q m = R L q) G   cos ϕ sin ϕ 0 R L = −  G sin ϕ cos ϕ 0   0 0 1 in jo upoštevamo kot v = J R L ˙ q. G Iz notranje inverzne kinematike v = J ˙ q m (2.20) dobimo direktno notranjo kinematiko ˙ q m = J+v, kjer je J+ = J T J−1 J T psevdoinverz matrike J. Direktno notranjo kinematiko platforme Mecanum s štirimi kolesi zapišemo kot       v 1 ˙ xm 1 1 1 1 1  v 2  ˙ y      m = −1 1 −1 1   4    v  3 ˙ ϕ −1 −1 1 1   ( l+ d) ( l+ d) ( l+ d) ( l+ d) v 4 Direktno kinematko v globalnih koordinatah pa dobimo z ˙ q = R L T J +v. G Kinematika trikolesnega večsmernega pogona Na sliki 2.13 je prikazana priljubljena večsmerna konfiguracija za trikolesni pogon. Njegovo inverzno kinematiko (v globalnih koordinatah) dobimo z upoštevanjem y v 1 g=90° q2 j v 2 x q3 R v 3 Slika 2.13: Trikolesni večsmerni pogon ( θ 2 = 120°, θ 3 = 240°) p translacijske hitrosti robota v = ˙ x 2 + ˙ y 2 in njegove kotne hitrosti ˙ ϕ. Hitrost prvega kolesa v 1 = v 1 t + v 1 r je sestavljena iz translacije v 1 t = − ˙ x sin ϕ + ˙ y cos ϕ in orientacije v 1 r = R ˙ ϕ. Torej je skupna hitrost prvega kolesa enaka v 1 = − ˙ x sin ϕ+ ˙ y cos ϕ+ R ˙ ϕ. Podobno je ob upoštevanju kota (v globalnih koordinatah) drugega kolesa ϕ+ θ 2 njegova hitrost enaka v 2 = − ˙ x sin( ϕ+ θ 2)+ ˙ y cos( ϕ+ θ 2)+ R ˙ ϕ; hitrost tretjega kolesa pa je v 3 = − ˙ x sin( ϕ + θ 3) + ˙ y cos( ϕ + θ 3) + R ˙ ϕ. Inverzna 32 Modeliranje gibanja mobilnih sistemov kinematika trikolesnega pogona v globalnih koordinatah je       v 1 − sin ϕ cos ϕ R ˙ x  v      2 = − sin( ϕ + θ 2) cos( ϕ + θ 2) R ˙ y (2.21)       v 3 − sin( ϕ + θ 3) cos( ϕ + θ 3) R ˙ ϕ kar zapišemo v matrični obliki kot v = J ˙ q. Včasih je bolj priročno voditi robota v njegovih lokalnih koordinatah, ki jih pridobimo z upoštevanjem transformacije rotacije v = J R L T ˙ q G m Direktno kinematiko v globalnih koordinatah dobimo s pomočjo inverzne kine- matike (2.21) kot ˙ q = Sv, kjer je S = J −1       ˙ x − sin θ 1 − sin( θ 1 + θ 2) − sin( θ 1 + θ 3) v 1 2  ˙ y  =  cos θ    1 cos( θ 1 + θ 2) cos( θ 1 + θ 3) v 2   3     ˙ ϕ 1 1 1 v 2 R 2 R 2 R 3 2.2.8 Gosenični pogon Gibanje goseničnega pogona (slika 2.14) lahko približno opišemo s kinematiko diferencialnega pogona     ˙ x( t) cos ϕ( t) 0 " # v( t)  ˙ y( t) = sin ϕ( t) 0     ω( t) ˙ ϕ( t) 0 1 Diferencialni pogon predpostavlja idealno kotaljenje koles s točkastim dotikom kolesa in podlage, kar pa ne drži v primeru goseničnega pogona. Gosenični pogon ima večjo kontaktno površino med kolesi in tlemi, torej morajo za spremembo smeri gibanja gosenice (oz. kolesa) drseti. To jim omogoča premikanje po zahtevnejšem terenu, kjer so ostala kolesna vozila manj uspešna. Koeficient zdrsa Ym Y v g L v Xm j w y vR x Xg Slika 2.14: Gosenični pogon med gosenicami in podlago ni konstanten, saj je odvisen od kontakta s tlemi oz. 2.3. Omejitve gibanja 33 vrste podlage. Zato je odometrija (direktna kinematika) še manj zanesljiva za ocenjevanje lege robota v primerjavi z diferencialnim pogonom. 2.3 Omejitve gibanja Pri gibanju kolesnega mobilnega robota se srečujemo z dinamičnimi in kinematič- nimi omejitvami. Dinamične omejitve izvirajo iz dinamičnega modela sistema, ki ima omejeno odzivnost (pospeševanje) zaradi svoje vztrajnosti (mase) in omejitev motornega pogona (npr. omejen navor motorja zaradi njegovih zmogljivosti ali preprečevanja podrsavanja koles). Kinematične omejitve pa izvirajo iz konstrukcije robota in njegovega kinematičnega modela. Zanimive so predvsem kinematične omejitve, ki jih ločimo na holonomične in neholonomične omejitve. Neholonomične omejitve omejujejo možne smeri premika mobilnega robota [5]. Holonomične omejitve pa se nanašajo le na dimenzijo opisa sistema s posplošenimi koordinatami, zato lahko z njihovo pomočjo odstranimo odvečne posplošene koordinate, ki so odvisne od drugih. Nek sistem je holonomičen, če nima kinematičnih omejitev ali pa vsebuje samo holonomične omejitve, zato nima omejitev v smeri gibanja. Neholonomični sistem pa vsebuje neholonomične omejitve, torej se ne more premikati v poljubni smeri (npr. avtomobil se lahko premika le v smeri vrtenja koles, ne more pa se premikati bočno). Za holonomične sisteme lahko določimo nabor neodvisnih posplošenih koordinat, ki določajo prostor, v katerem so možne poljubne smeri gibanja. V neholonomičnih sistemih temu ni tako, saj gibanje v vsakem trenutku ni poljubno, temveč je dovoljeno le gibanje, ki ustreza neholonomičnim omejitvam. Za holonomične sisteme torej velja, da so njihova stanja neposredno odvisna od konfiguracije notranjih spremenljivk (zasuki koles, koti sklepov). V primeru neholonomičnih sistemov to ne drži, saj vrnitev notranjih spremenljivk v začetno konfiguracijo ne zagotavlja tudi vrnitve sistema v začetno stanje (pozicijo in orientacijo). Posplošeno lahko rečemo, da je izhodno stanje neholonomičnih sistemov odvisno od opravljene poti (zaporedje notranjih spremenljivk). V nadaljevanju bomo obravnavali mehanske sisteme, katerih konfiguracijo (lega sistema v okolju in odnosi med deli sistema) lahko opišemo z vektorjem posplošenih koordinat q. Pri podani trajektoriji q( t) določimo vektor posplošenih hitrosti ˙ q( t). Holonomične omejitve izrazimo v obliki enačb, ki povezujejo posplošene koordi- nate. Te enačbe lahko uporabimo za izločitev nekaterih posplošenih spremenljivk, da dobimo manjši prostor posplošenih spremenljivk, potrebnih za opis sistema. Neholonomične omejitve pa ne zmanjšujejo dimenzije prostora posplošenih spre- menljivk temveč samo dimenzije prostora možnih posplošenih hitrosti. Neholono- mične omejitve torej vplivajo na problem načrtovanja poti. V zvezi z njimi se pojavljajo naslednja vprašanja: 34 Modeliranje gibanja mobilnih sistemov • Kako ugotoviti, ali je kinematična omejitev neholonomična? Če je omejitev integrabilna, se lahko enačba, ki vsebuje hitrostne parametre (odvode posplošenih koordinat), prevede v holonomično omejitev. • Ali neholonomična omejitev omejuje množico dostopnih konfiguracij (tj. lege sistema)? Z uporabo orodij teorije vodenja lahko pridemo do preprostih pogojev, pod katerimi neholonomične omejitve ne vplivajo na območje dosegljivih leg. • Kako zgraditi generator izvedljivih oz. možnih poti za robota z neholono- mičnimi omejitvami? 2.3.1 Holonomične omejitve Holonomične omejitve so vezane na posplošene koordinate (stanja) sistema. Za sistem z n posplošenimi koordinatami q = [ q 1 , . . . , qn] T je holonomična omejitev izražena kot f (q) = f ( q 1 , . . . , qn) = 0 (2.22) kjer je f gladka funkcija z zveznimi odvodi. Ta omejitev določa podmnožico vseh možnih konfiguracij v posplošenih koordinatah (delovni prostor), ki zadostujejo omejitvi (2.22) (zmanjša število prostostnih stopenj sistema). Z upoštevanjem (2.22) lahko namreč izločimo določeno posplošeno koordinato (izrazimo jo lahko z n − 1 ostalimi koordinatami). V splošnem imamo lahko m holonomičnih omejitev ( m < n). Če so omejitve linearno neodvisne, določajo ( n − m)–dimenzionalni “podprostor”, ki je dejanski delovni prostor sistema (sistem ima n − m prostostnih stopenj). 2.3.2 Neholonomične omejitve Neholonomične omejitve omejujejo možne hitrosti ali smeri gibanja sistema. Zapišemo jih v obliki f (q , ˙ q) = f ( q 1 , . . . , qn, ˙ q 1 , . . . , ˙ qn) = 0 (2.23) kjer je f gladka funkcija z zveznimi odvodi in ˙ q vektor hitrosti sistema v posplo- šenih koordinatah. V primeru, da sistem nima omejitev (2.23), se lahko giblje v poljubnih smereh. Kinematična omejitev (2.23) je holonomična, če je integrabilna, kar pomeni, da lahko hitrosti ˙ q 1 , . . . , ˙ qn izločimo iz enačbe (2.23) in zapišemo omejitev v obliki (2.22). Če omejitev (2.23) ni integrabilna, je neholonomična. Če obstaja m linearno neodvisnih neholonomičnih omejitev v obliki (2.23), je prostor dostopnih hitrosti ( n − m)–dimenzionalen. Neholonomična omejitev torej omeji dovoljene hitrosti sistema. Za primer lahko vzamemo dvokolesnega robota 2.3. Omejitve gibanja 35 (invalidski voziček), ki se lahko premika v smeri trenutne orientacije koles, v bočni smeri (pravokotno na kolesa) pa ne. Predpostavimo, da so omejitve linearne v odvisnosti od ˙ q = [ ˙ q 1 , . . . , ˙ qn)] T . Potem lahko (2.23) zapišemo kot   ˙ q 1 h i . f (q , ˙ q) = a T (q) ˙ q =   a . 1(q) . . . an(q)  .  = 0   ˙ qn kjer je a(q) vektor členov omejitve (nedovoljena smer pomika). V kolikor imamo m neholonomičnih omejitev, lahko njihove člene zapišemo v matriko omejitev   a T (q) 1 . A(q) =  .   .    a T (q) m in vse neholonomične omejitve sistema podamo v matrični obliki A(q) ˙ q = 0 Nadalje določimo matriko dosegljivih smeri gibanja sistema ( m omejitev določa n − m dosegljivih smeri) h i S(q) = s1(q) s2(q) . . . s n− m(q) Ta matrika podaja kinematični model sistema, za katerega velja ˙ q( t) = S(q)v( t) (2.24) kjer je v( t) regulirni vektor (glejte kinamatični model (2.2)). Produkt matrike omejitev A in kinematične matrike S je ničelna matrika AS = 0 2.3.3 Integrabilnost omejitev Da ugotovimo, ali je določena omejitev neholonomična (oz. hitrostna), moramo preveriti, če jo je mogoče integrirati in s tem prevesti v holonomično omejitev. V kolikor to ni mogoče, je omejitev neholonomična. 2.3.4 Vektorska polja, porazdelitev, Liejevi okle- paji V trenutnem času t in stanju q dobimo možne smeri premikov iz določene točke prostora q z linearno kombinacijo vektorskih polj v matriki dosegljivih smeri S. 36 Modeliranje gibanja mobilnih sistemov Porazdelitev tako podaja dosegljiv “podprostor” iz določene točke prostora q s premiki, ki predstavljajo linearno kombinacijo vektorskih polj (stolpci matrike S). Vektorska polja so odvodi posplošenih koordinat, torej predstavljajo hitrosti ali smeri možnih premikov v prostoru. Vektorsko polje je zvezno odvedljiva preslikava, ki vsaki točki prostora priredi natanko določen vektor. Prikaz določitve dosegljivih vektorskih polj je podan v primeru 2.1. Primer 2.1 Za robota z diferencialnim pogonom s kinematičnim modelom (2.2) določite dosegljive hitrosti (smeri premikov) in omejitve gibanja. Rešitev Vektorski polji dosegljivih hitrosti (smeri premikov) sta     cos ϕ 0 s     1(q) = sin ϕ s2(q) = 0 (2.25)     0 1 kar pomeni, da so možne smeri premika v danem trenutku, ko se nahajamo v legi q, podane z linearno kombinacijo ˙ q = u 1s1(q) + u 2s2(q) (2.26) kjer sta u 1 in u 2 poljubni realni števili, ki predstavljata regulirni veličini. Enačba (2.26) je le preurejen zapis kinematičnega modela (2.2). V kolikor vektorskih polj s i nimamo podanih, jih lahko določimo iz znanih omejitev a j, kjer upoštevamo ortogonalnost smeri omejitev in smeri gibanja, torej s i⊥a j. Iz slike 2.3 lahko določimo omejitev s smerjo, v kateri se robot ne more premikati, to je bočno na kolesa. Edina omejitev je   − sin ϕ a(q) =  cos ϕ    0 kar je ravno pravokotno na vektor možnega premika s1(q) (translacijski premik v smeri kotaljenja koles) in na vektor s2(q) (rotacija okoli osi pravokotne na ravnino gibanja). V kolikor porazdelitev določenih vektorskih polj definira celoten prostor, je vsebovana (angl. involutive). Če osnovna porazdelitev ni vsebovana, lahko določimo množico novih vektorskih polj, ki so linearno neodvisna od vektorskih polj osnovne porazdelitve. Nova vektorska polja (smeri pomika) lahko dobimo s končnim številom preklopov osnovnih smeri pomika, kjer dolžino pomikov 2.3. Omejitve gibanja 37 limitiramo proti 0. Nove smeri lahko določimo s pomočjo Liejevih oklepajev, ki predstavljajo operacijo nad dvema vektorskima poljema, kot bo kasneje podano z enačbo (2.28). Praktičen primer uporabe je paralelno parkiranje avtomobila (tudi vozila z diferencialnim pogonom), kjer ni možen neposreden bočni premik na parkirno mesto zaradi neholonomičnih omejitev sistema (kolesa ne drsijo bočno). Kljub temu pa lahko dosežemo premik v stran z zaporedno kombinacijo gibanja naprej in nazaj ter zasukov, kar prikazuje primer 2.2. Primer 2.2 Predstavite manever za paralelno parkiranje vozila z diferencialnim pogonom. Rešitev Z uporabo osnovnih smeri pomika, definiranih z vektorskimi polji s1(q) in s2(q) (glejte enačbo (2.25)), in začetnim stanjem (lega vozila) q0 = q(0) določimo novo stanje sistema. Začnemo v stanju q0 = q(0) in se za kratek čas ε gibljemo v smeri s1, nato v smeri s2 za čas ε, nato v smeri −s1 za čas ε in na koncu v smeri −s2 za čas ε, kjer je dosežena končna lega vozila. To lahko matematično zapišemo kot q(4 ε) = φ−s2 φ−s1 ( φ s2 ( φ s1 (q ε ε ε ε 0))) kar predstavlja nelinearno diferencialno enačbo, katere rešitev (integracija kinematičnega modela) lahko aproksimiramo z razvojem v Taylerjevo vrsto (za podrobnosti glejte [1]) kot ∂ s ∂ s q 2 1 (4 ε) = q0 + ε 2 s1(q0) − s2(q0) + O( ε 3) (2.27) ∂ q ∂ q kjer so parcialni odvodi ovrednoteni v q0, O( ε 3) pa predstavlja prispevek višjih odvodov, ki je za kratke čase ε zanemarljiv. Dobljeni končni premik manevra parkiranja je tako dolžine ε 2 v smeri vektorja, ki je podan v oklepaju in predstavlja operacijo Liejev oklepaj, definiran v enačbi (2.28). Manever paralelnega parkiranja lahko predstavimo z eksperimentom. Predpo- stavimo, da je začetna lega robota q0 = [0 , 0 , 0] T . V prvem koraku izvajanja manevra pridemo v točko       0 cos 0 ε q       1 = q0 + ε s1 = 0 + ε sin 0 = 0       0 0 0 v drugem koraku izvedemo rotacijo       ε 0 ε q       2 = q1 + ε s2 = 0 + ε 0 = 0       0 1 ε 38 Modeliranje gibanja mobilnih sistemov v tretjem koraku imamo translacijo v negativni smeri       ε cos ε ε − ε cos ε q       3 = q2 − ε s1 = 0 − ε sin ε = − ε sin ε       ε 0 ε in v zadnjem koraku rotacijo v negativni smeri       ε − ε cos ε 0 ε − ε cos ε q       4 = q3 − ε s2 = − ε sin ε − ε 0 = − ε sin ε       ε 1 0 Na sliki 2.15 so prikazani izračunani premiki in vmesne točke. j q 0 y q 4 q 3 3 O(e ) 2 q 2 e [s s , ] 1 2 q 3 y q 0 q q 4 2 q q 1 1 x x Slika 2.15: Manever paralelnega parkiranja v delovnem prostoru (levo), kjer je orientacija predstavljena z osjo z, in pogled na manever od zgoraj (desno) Opazimo lahko, da končni premik v stran ni neposredno izvedljiv (v enem koraku) z možnimi smermi gibanja s1 in s2, je pa izvedljiv v več korakih z njuno kombinacijo. Dobljen končni premik ni točno v bočni smeri zaradi končnega časa ε in člena O( ε 3) v relaciji (2.27). V kolikor so premiki majhni s kratkim časom trajanja ε → 0, je dobljen premik točno v stran, torej je končna lega   0 q   4 = − ε 2   0 Kot smo videli v primeru 2.2, lahko s pomočjo Liejevih oklepajev določimo nove smeri gibanja, ki jih osnovna porazdelitev ne dovoljuje. Te nove smeri lahko dosežemo s končnim številom neskončno kratkih premikov v smereh vektorskih 2.3. Omejitve gibanja 39 polj v osnovni porazdelitvi. Liejevi oklepaji so operacija nad vektorskima poljema i(q) in j(q), katere rezultat je novo vektorsko polje [i , j]. Definiramo ga kot ∂ j ∂ i [i , j] = i − j (2.28) ∂ q ∂ q kjer sta  ∂i 1 ∂i 1 . . . ∂i 1  ∂ q1 ∂ q2 ∂ q n ∂i 2 ∂i 2 ∂ i  . . . ∂i 2  ∂ q1 ∂ q2 ∂ q =  n   . . .  ∂ q .  .. .. . . ..    ∂in ∂in . . . ∂in ∂ q1 ∂ q2 ∂ q n  ∂j 1 ∂j 1 . . . ∂j 1  ∂ q1 ∂ q2 ∂ q n ∂j 2 ∂j 2 ∂ j  . . . ∂j 2  ∂ q1 ∂ q2 ∂ q =  n   . . .  ∂ q .  .. .. . . ..    ∂jn ∂jn . . . ∂jn ∂ q1 ∂ q2 ∂ q n Porazdelitev je vsebovana, če z Liejevimi oklepaji ne moremo pridobiti novih linearno neodvisnih vektorskih polj. Vsebovana porazdelitev je zaprta znotraj Liejevih oklepajev [6] in sistem je popolnoma holonomičen, torej nima omejitev gibanja ali pa so vse omejitve holonomične. Ta ugotovitev izhaja iz Frobeniusovega teorema [7]: Če je osnovna porazdelitev vsebovana, je sistem holonomičen in vse morebitne omejitve sistema so integrabilne. Predpostavimo, da je iz m omejitev k omejitev holonomičnih in ( m − k) neholonomičnih. Glede na Frobeniusov teorem obstajajo tri možnosti za k [8]: • k = m: dimenzija vsebovane porazdelitve (število linearno neodvisnih vektorskih polj) je enaka dimenziji osnovne porazdelitve ( n − m). • 0 < k < m: imamo k integrabilnih omejitev, torej lahko iz opisa sistema izločimo k posplošenih koordinat. V tem primeru je dimenzija vsebovane porazdelitve enaka n − k. • k = 0: dimenzija vsebovane porazdelitve je n (dimenzija prostora) in vse omejitve so neholonomične. Primer 2.3 Določite omejitve gibanja in smeri možnih premikov za primer enojnega kolesa, ki se kotali po podlagi. Določite število prostostnih stopenj sistema ter število in vrsto njegovih omejitev. 40 Modeliranje gibanja mobilnih sistemov Rešitev Kinematika enojnega kolesa je enaka kot pri diferencialnem pogonu (kolesi damo skupaj), prikazan na sliki 2.3 s kinematičnim modelom 2.2. Omejene in možne smeri gibanja smo že določili v primeru 2.1. Imamo samo eno hitrostno omejitev, ki ne omejuje dosegljivosti leg q v prostoru, zato ima sistem tri prostostne stopnje. Da je omejitev res hitrostna (neholonomična), lahko pokažemo z uporabo Liejevih oklepajev in Frobeniusovega teorema. Najprej določimo Liejev oklepaj za dosegljivi vektorski polji s1 in s2 (2.25) ∂ s ∂ s s 2 1 3 = [s1 , s2] = s1 − s2 ∂ q ∂ q         0 0 0 cos ϕ 0 0 − sin ϕ 0 = 0 0 0 sin ϕ − 0 0 cos ϕ  0         0 0 0 0 0 0 0 1   sin ϕ = − cos ϕ   0 Dobimo novo smer možnega pomika, ki je linearno neodvisna od vektorskih polj s1 in s2 ter je zato ni v osnovni porazdelitvi. Posledično lahko sklepamo, da je omejitev neholonomična, dimenzija vsebovane porazdelitve pa je 3 (sistem ima 3 prostostne stopnje), kar je enako dimenziji sistema. Hkrati z dodatnimi Liejevimi oklepaji ([s1 , s3], [s2 , s3]) ni možno določiti novih linearno neodvisnih vektorskih polj. 2.3. Omejitve gibanja 41 Primer 2.4 Določite omejitve gibanja in smeri možnih premikov za primer avtomobila brez krmilnega mehanizma (kolesa so fiksno vpeta), prikazan na sliki 2.16. Določite število in vrsto omejitev ter število prostostnih stopenj sistema. Yg j ( ) x,y Xg Slika 2.16: Avtomobil brez krmilnega mehanizma 42 Modeliranje gibanja mobilnih sistemov Rešitev Vozilo se ne more premikati bočno niti vrteti (njegova orientacija ϕ se ne spreminja), torej imamo naslednje smeri omejitev gibanja     sin ϕ 0 a     1(q) = − cos ϕ a2(q) = 0     0 1 in omejitvi gibanja a T(q) ˙ q = ˙ x sin ϕ − ˙ y cos ϕ = 0 1 a T(q) ˙ q = ˙ ϕ = 0 2 ki sta integrabilni, kar lahko preprosto pokažemo s tem, da ju integriramo ϕ = ϕ 0 ( x − x 0) sin ϕ − ( y − y 0) cos ϕ = 0 Vozilo se lahko premika le v smeri vektorskega polja   cos ϕ s   1 = sin ϕ   0 Ker sta obe omejitvi holonomični, je porazdelitev vektorskega polja s1 vsebovana, kar pomeni, da je sistem holonomičen in ima eno prostostno stopnjo. Z Liejevimi oklepaji ne moremo določiti novih vektorskih polj (smeri premikov), saj imamo le eno vektorsko polje možnega premika. Primer 2.5 V programskem okolju Matlab izvedite simulacijo platforme z diferencialnim pogonom. Parametri vozila so: računski korak Ts = 0 , 033 s, čas simulacije 10 s, polmer kolesa r = 0 , 04 m, razdalja med kolesoma L = 0 , 08 m. Naloge: 1. Analitično izračunajte in izvedite simulacijo poti, ki jo opravi robot, če je začetno stanje q(0) = [ x, y, ϕ] T = [0 , −0 , 5 , 0] T in so vhodi robota (v treh primerih): • v( t) = 0 , 5 m / s, ω( t) = 0 rad / s; 2.3. Omejitve gibanja 43 • v( t) = 1 m / s, ω( t) = 2 rad / s; • kotni hitrosti koles ωL( t) = 24 rad / s, ωR( t) = 16 rad . s Kakšni sta kotni hitrosti koles? Ali sta izračunana in simulirana pot enaki? Zakaj ne? 2. Z uporabo odometrije izvedite lokalizacijo vozila pri konstantnih vhodnih hitrostih v( t) = 0 , 5 m / s, ω( t) = 1 rad / s. Primerjajte ocenjeno in simulirano pot robota. Dobljene rezultate ovrednotite tudi v primeru idealne situacije, tj. brez šuma in pogreška modeliranja. 3. Izvedite lokalizacijo iz drugega vprašanja z upoštevanjem začetnega po- greška stanja. Predpostavite, da je pravo začetno stanje neznano, in za lokalizacijo uporabite drugačna začetna stanja kot pri simulaciji. 4. Izberite si pot, sestavljeno iz dveh ali treh daljic, kjer začetna točka sovpada z začetno lego robota. Izračunajte potrebno zaporedje vhodov za vožnjo robota po tej poti (čas simulacije je 10 sekund). 5. Izberite poljubno trajektorijo, definirano kot časovna parametrična funkcija ( x( t) = f ( t) in y( t) = g( t), f in g sta gladki funkciji). Izračunajte potrebne vhode za sledenje predpisani trajektoriji. Kaj se zgodi, če na začetku robot ni na tej poti? 6. Simulirajte manever paralelnega parkiranja, opisan v primeru 2.2. Izberite ustrezen ε ter konstantni hitrosti v in ω. 7. Za dano začetno stanje q(0) = [0 , −0 , 5 , 0] T izračunajte kinematično matriko S in matriko omejitev A. Rešitev Osnovna koda za simulacijo diferencialnega pogona je podana v programu 2.1. Kodo lahko priredimo in dobimo želene rešitve. Program 2.1 ./src/mdl/example_diff_drive.m 1 r = 0.04; % Radij kolesa 2 L = 0.08; % Dol ž ina osi med kolesoma 3 Ts = 0.03; % Ra č unski korak 4 t = 0: Ts :10; % Č as simulacije 5 q = [4; 0.5; pi /6]; % Za č etna lega 6 7 for k = 1: length ( t ) 8 wL = 12; % H i t r o s t l e v e g a k o l e s a 9 wR = 1 2 . 5 ; % H i t r o s t d e s n e g a k o l e s a 44 Modeliranje gibanja mobilnih sistemov 10 v = r / 2 * ( wR + wL ); % H i t r o s t r o b o t a 11 w = r / L *( wR - wL ); % K o t n a h i t r o s t r o b o t a 12 dq = [ v * cos ( q ( 3 ) + Ts * w / 2 ) ; v * sin ( q ( 3 ) + Ts * w / 2 ) ; w ]; 13 q = q + Ts * dq ; % I n t e g r a c i j a 14 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č je [ - pi , pi ] 15 end Primer 2.6 V programskem okolju Matlab izvedite simulacijo platforme s trikolesnim po- gonom na zadnjih kolesih. Parametri vozila so: računski korak Ts = 0 , 033 s, čas simulacije 10 s, polmer kolesa r = 0 , 2 m, razdalja med zadnjima kolesoma L = 0 , 08 m ter razdalja med sprednjim in zadnjima kolesoma D = 0 , 07 m. Naloge: 1. Analitično izračunajte in izvedite simulacijo poti, ki jo opravi robot, če je začetno stanje q(0) = [ x, y, ϕ] T = [0 , −0 , 5 , 0] T in so vhodi robota (v dveh primerih): • v( t) = 0 , 5 m / s, α( t) = 0 • v( t) = 1 m / s, α( t) = π 6 Kakšne so kotne hitrosti koles? Ali sta izračunana in simulirana pot enaki? Zakaj ne? 2. Z uporabo odometrije izvedite lokalizacijo vozila pri konstantnih vhodih v( t) = 0 , 5 m / s, α( t) = π . Primerjajte ocenjeno in simulirano pot robota. 6 Dobljene rezultate ovrednotite tudi v primeru idealne situacije, tj. brez šuma in pogreška modeliranja. 3. Izvedite lokalizacijo iz druge naloge z upoštevanjem začetnega pogreška stanja. Predpostavite, da je pravo začetno stanje neznano, in za lokalizacijo uporabite drugačna začetna stanja kot pri simulaciji. 4. Izberite si pot, sestavljeno iz dveh ali treh daljic, kjer začetna točka sovpada z začetno lego robota. Izračunajte potrebno zaporedje vhodov za vožnjo robota po tej poti (čas simulacije je 10 sekund). 5. Izberite poljubno trajektorijo, definirano kot časovno parametrična funkcija ( x( t) = f ( t) in y( t) = g( t), f in g sta gladki funkciji). Izračunajte potrebne vhode za sledenje predpisani trajektoriji. Kaj se zgodi, če na začetku robot ni na tej poti? 2.3. Omejitve gibanja 45 Rešitev Osnovna koda za simulacijo trikolesnega pogona je podana v programu 2.2. Kodo lahko priredimo in dobimo želene rešitve. Program 2.2 ./src/mdl/example_tricycle_drive.m 1 D = 0.07; % Razdalja med prednjim kolesom in zadnjo osjo 2 Ts = 0.03; % Ra č unski korak 3 t = 0: Ts :10; % Č as simulacije 4 q = [4; 0.5; pi /6]; % Za č etna lega 5 6 for k = 1: length ( t ) 7 v = 0 . 5 ; % H i t r o s t r o b o t a 8 a l p h a = 0 . 0 4 * ( 1 + sin ( k * Ts * pi / 2 ) ) ; % O r i e n t a c i j a p r e d n j e g a k o l e s a 9 w = v / D * tan ( a l p h a ); % K o t n a h i t r o s t r o b o t a 10 dq = [ v * cos ( q ( 3 ) + Ts * w / 2 ) ; v * sin ( q ( 3 ) + Ts * w / 2 ) ; w ]; 11 q = q + Ts * dq ; % I n t e g r a c i j a 12 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č je [ - pi , pi ] 13 end 2.3.5 Vodljivost kolesnih mobilnih robotov Pred načrtovanjem vodljivosti sistema se moramo vprašati: Ali lahko robot doseže katerokoli točko q v delovnem prostoru z izvajanjem svojih možnih manevrov? Odgovor na vprašanje je povezan z vodljivostjo sistema. Če je sistem vodljiv, lahko doseže poljubno konfiguracijo q s kombiniranjem razpoložljivih manevrov gibanja. Če ima robot kinematične omejitve, jih moramo analizirati in ugotoviti, ali vplivajo na vodljivost sistema. Robot z neholonomičnimi omejitvami se lahko premika le v svojih osnovnih smereh gibanja (osnovni manevri, kot sta vožnja naravnost in kroženje). Vendar lahko tak robot z združevanjem osnovnih mane- vrov še vedno doseže želeno konfiguracijo q. Nova smer gibanja se razlikuje od vsake osnovne smeri ali linearne kombinacije osnovnih smeri, kot je prikazano v (2.24). Vodljivost robota določimo z analizo vsebovane porazdelitve, pridobljene z zaporednimi operacijami Liejevih oklepajev v osnovnih smereh gibanja (s1, s2, s3 itd.) na naslednji način ([7, 9–12]) {s1 , s2 , s3 , [s1 , s2] , [s1 , s3] , [s2 , s3] , . . . , [s1 , [s1 , s2]] , [s1 , [s1 , s3]] , . . . } Če ima vsebovana porazdelitev rang n ( n je dimenzija vektorja q), lahko robot v delovnem prostoru doseže katerokoli točko q in je zato vodljiv. Sledeča izjava je znana kot Chowov izrek: Sistem je vodljiv, če je rang njegove vsebovane porazdelitve glede na Liejeve oklepaje enak dimenziji delovnega prostora. Torej je robot vodljiv, če so vse njegove omejitve neholonomične in je rang njegove vsebovane porazdelitve enak n. Ta test vodljivosti je namenjen nelinearnim sistemom, vendar ga je možno uporabiti tudi za linearne sisteme ˙ q = Aq + B u, 46 Modeliranje gibanja mobilnih sistemov kjer sta osnovni vektorski polji s1 = Aq in s2 = B. Izračun njegove vsebovane porazdelitve vodi v Kalmanov test za vodljivost linearnih sistemov h i rang B AB A2B . . . = n Vsebovano porazdelitev pridobimo z zaporednim izračunavanjem Liejevih okle- pajev nad osnovnimi in predhodno določenimi novimi smermi gibanja. Število zaporednih stopenj (nivojev izračunov) določa indeks težavnosti manevriranja kolesnega mobilnega robota [13]. Višji kot je ta indeks, več osnovnih manevrov je potrebnih za doseg želene smeri gibanja. Primer 2.7 Pokažite, da je diferencialni pogon vodljiv. Rešitev Diferencialni pogon ima tri ( n = 3) stanja q = [ x, y, ϕ] ter dve osnovni smeri gibanja s1 in s2     cos ϕ 0 s     1(q) = sin ϕ s2(q) = 0     0 1 Nova smer gibanja, pridobljena z Liejevim oklepajem, je ∂ s ∂ s s 2 1 3 = [s1 , s2] = s1 − s2 ∂ q ∂ q   sin ϕ = − cos ϕ   0 Z izračunom Liejevih oklepajev iz poljubne kombinacije vektorskih polj s1, s2 in s3 ne dobimo nove linearno neodvisne smeri gibanja, zato je porazdelitev [s1 , s2 , s3] vsebovana (zaprta z Liejevimi oklepaji) in ima rang h i rang s 1 s2 s3 = n = 3 torej je sistem vodljiv. Primer 2.8 Določite omejitve gibanja in smeri možnih premikov za primer vozila (slika 2.17) s pogonom na zadnjih kolesih. Vozilo upravljamo z obodno hitrostjo zadnjih koles v in kotno hitrostjo krmilnega mehanizma γ. 2.3. Omejitve gibanja 47 Določite število in vrsto omejitev, kinematični model ter število prostostnih stopenj sistema. Ali je sistem vodljiv? Yg a g j ( ) x,y v d Xg Slika 2.17: Vozilo s pogonom na zadnji osi, ki ga upravljamo s hitrostjo zadnjih koles v in kotno hitrostjo krmilnega mehanizma γ Rešitev Vozilo opišemo s štirimi stanji q = [ x, y, ϕ, α] T , saj sta regulirni veličini hitrost t v in kotna hitrost krmilnega mehanizma γ. Četrto stanje ( α = R γ d t) opisuje 0 trenutno stanje krmila. Velja opomniti, da je kinematika obravnavanega vozila podobna kinematiki kolesa z zadnjim pogonom (2.15), le da ima slednja samo tri stanja, saj je zasuk krmilnega mehanizma že določen z enim od izhodov (2.15). Iz slike 2.17 vidimo, da se vozilo ne more premikati v smeri bočno na kolesa, torej sta vektorja omejitev gibanja  sin ϕ   sin( α + ϕ)  − cos ϕ − cos( α + ϕ) a     1(q) = a2(q) =  0   − d cos α      0 0 Vektorski polji osnovnih smeri gibanja sta določeni s smerjo kotaljenja zadnjih koles (smer gibanja, ko je γ = 0 in konstanten zasuk krmilnega kolesa α) ter 48 Modeliranje gibanja mobilnih sistemov vrtenjem krmilnega kolesa (smer gibanja, ko je v = 0)  cos ϕ  0  sin ϕ  0 s     1 = s2 =  1 tan α 0  d    0 1 Kinematika vozila je podana z  cos ϕ 0 " # " # h i v  sin ϕ 0 v ˙ q = s   1 s2 = γ  1 tan α 0 γ  d  0 1 kar je podobno kinematiki (2.15), le da imamo še dodatno stanje α in vhod γ = ˙ α. Sistem je torej opisan s štirimi stanji ter ima dve omejitvi in dva vhoda. Želimo imeti štiri linearno neodvisne smeri gibanja s i. Dve že določa kinematični model, zato poskušamo z Liejevimi oklepaji določiti še preostali dve s3 = [s1 , s2] ∂ s2 ∂ s1 = s1 − s2 ∂ q ∂ q 0 0 0 0  cos ϕ  0 0 − sin ϕ 0  0 0 0 0 0  sin ϕ  0 0 cos ϕ 0  0 =     −     0 0 0 0  1 tan α 0 0 0 1  0    d   d cos2 α    0 0 0 0 0 0 0 0 0 1  0   0  =    −1   d cos2 α  0 in s4 = [s1 , s3] ∂ s3 ∂ s1 = s1 − s3 ∂ q ∂ q 0 0 0 0   cos ϕ  0 0 − sin ϕ 0   0  0 0 0 0   sin ϕ  0 0 cos ϕ 0   0  =     −     0 0 0 −2 sin α   1 tan α 0 0 0 1   −1   d cos3 α   d   d cos2 α   d cos2 α  0 0 0 0 0 0 0 0 0 0  − sin ϕ  d cos2 α cos ϕ   =  d cos2 α   0    0 2.4. Dinamični model mobilnega sistema z omejitvami 49 Vse štiri dobljene smeri s i, i = 1 , . . . , 4 so linearno neodvisne, zato ima sistem štiri prostostne stopnje in obe omejitvi sta neholonomični. Rang vsebovane porazdelitve je 4, kar je enako številu prostostnih stopenj sistema. Vozilo je torej vodljivo. 2.4 Dinamični model mobilnega sistema z omejitvami Kinematični model opisuje le statično transformacijo hitrosti robota ( psevdohitro-sti) v osnovni koordinatni sistem, podan s posplošenimi koordinatami. Dinamični model gibanja mehanskega sistema pa podaja dinamične zakonitosti, kot je gibanje sistema pod vplivom zunanjih sil in vztrajnosti sistema. Z uporabo Lagrangeove formulacije, ki je še posebej primerna za opis mehanskih sistemov [14], lahko določimo dinamični model sistema d ∂ L ∂ L ∂P − + + gk + τd = fk (2.29) d t ∂ ˙ q k k ∂qk ∂ ˙ qk kjer indeks k opisuje posplošene koordinate qk ( k = 1 , . . . , n). Z L je označena razlika med kinetično in potencialno energijo sistema, imenovana Lagrangian, P je močnostna funkcija zaradi trenja in dušenja v sistemu, gk označuje sile zaradi gravitacije, τd predstavlja vse neznane motnje, ki jih z enačbo 2.29 nismo k zajeli v modelu, fk pa je posplošena sila (zunanji vplivi na sistem), povezana s posplošeno koordinato qk. Enačba (2.29) velja samo za sisteme brez omejitev gibanja, torej za sisteme, ki imajo n prostostnih stopenj in so brez hitrostnih omejitev. Za sisteme s kinematičnimi omejitvami lahko zapišemo dinamične enačbe gibanja z uporabo Lagrangeovih multiplikatorjev [15] m d ∂ L ∂ L ∂P X − + + gk + τd = fk − λjajk (2.30) d t ∂ ˙ q k k ∂qk ∂ ˙ qk j=1 kjer je m število linearno neodvisnih omejitev gibanja, λj Lagrangeov multiplikator, povezan z j-to omejitveno enačbo, ajk ( j = 1 , . . . , m, k = 1 , . . . , n) pa koeficienti omejitev. Končni nabor enačb vsebuje n + m diferencialnih in algebrajskih enačb ( n Lagrangeovih enačb in m omejitvenih enačb) z n + m neznankami ( n posplošenih koordinat qk in m Lagrangeovih multiplikatorjev λj). Enačbe so diferencialne v smislu posplošenih koordinat in algebrajske glede na Lagrangeove multiplikatorje. Splošni dinamični model (2.30) mehanskega sistema z omejitvami lahko zapišemo v matrični obliki M (q) ¨ q + V (q , ˙ q) + F ( ˙ q) + G(q) + τ d = E(q)u − A T (q)λ (2.31) 50 Modeliranje gibanja mobilnih sistemov Tabela 2.1: Pomen matrik v dinamičnem modelu (2.31) Oznaka Opis q vektor posplošenih koordinat (dimenzije n × 1) M (q) pozitivno definitna matrika mas in vztrajnosti (dimenzije n × n) V (q , ˙ q) vektor Coriolisovih in centrifugalnih členov (dimenzije n × 1) F ( ˙ q) vektor sil trenja in dušenja (dimenzije n × 1) G(q) vektor gravitacijskih sil in navorov (dimenzije n × 1) τ d vektor neznanih motenj, vključno z dinamiko, ki ni zajeta v modelu (dimenzije n × 1) E(q) matrika preslikav aktuatorskega prostora v prostor posplošenih spremenljivk (dimenzije n × r) u vektor vhodov (dimenzije r × 1) A T (q) matrika kinematičnih omejitev (dimenzije m × n) λ vektor omejitvenih sil (Lagrangeovi multiplikatorji) (dimenzije m × 1) kjer je pomen matrik opisan v tabeli 2.1. 2.4.1 Predstavitev dinamičnega modela mobilnega sistema z omejitvami v prostoru stanj V nadaljevanju bomo izpeljali dinamični model sistema z m kinematičnimi omejitvami v prostoru stanj. Nadalje bomo izvedli delno linearizacijo sistema, opisanega v prostoru stanj, z vpeljavo nelinearne povratnozančne relacije [6]. Dobljeni sistem bomo zapisali kot kinematični model drugega reda. Dinamični sistem z m kinematičnimi omejitvami zapišemo kot M (q) ¨ q + V (q , ˙ q) + F ( ˙ q) + G(q) = E(q)u − A T (q)λ (2.32) kjer smo vpliv neznanih motenj na sistem τ d iz (2.31) zanemarili. Kinematični model gibanja pa podaja enačba ˙ q = S(q)v( t) (2.33) Dinamični model (2.32) in kinematični model (2.33) lahko združimo v enoten zapis v prostoru stanj. Poenoteno obravnavo neholonomičnih in holonomičnih omejitev lahko najdemo v [16], kjer so holonomične omejitve izražene v diferencialni obliki (s hitrostmi) kot neholonomične. Zaradi enostavnosti zapisa bomo v nadaljevanju izpustili odvisnost od q. Če odvajamo relacijo (2.33) po času, dobimo ¨ q = ˙ Sv + S ˙v 2.4. Dinamični model mobilnega sistema z omejitvami 51 Dobljen izraz vstavimo v (2.32) ter zamenjamo posplošene koordinate q s psevdohitrostmi v. Tako dobimo M ˙ Sv + M S ˙v + V + F + G = Eu − A T λ (2.34) Prisotnost Lagrangeovih multiplikatorjev λ zaradi omejitev gibanja lahko izločimo z upoštevanjem relacij AS = 0 in S T A T = 0. Z množenjem enačbe (2.34) z matriko S T dobimo skrčeno obliko dinamičnega modela S T M ˙ Sv + S T M S ˙v + S T V + S T F + S T G = S T Eu s čimer smo izločili Lagrangeove multiplikatorje λ. Z vpeljavo zamenjav f M = S T M S, e V = S T M ˙ Sv + S T (V + F + G) in e E = S T E lahko pregledneje zapišemo f M ˙v + e V = e Eu (2.35) od koder izrazimo vektor psevdopospeškov ˙ v ˙ v = f M −1 e Eu − e V Če je nadalje izpolnjen pogoj det S T E 6= 0, kar v večini realističnih primerov je, lahko iz enačbe (2.35) izrazimo vhod v sistem u = e E−1 f M ˙v + e V (2.36) Z razširitvijo vektorja stanj s psevdohitrostmi x = [q T v T ] T in zapisom sistema v splošni nelinearni obliki ˙ x = f (x) + g(x)u (člen f (x) vsebuje nelinearno odvisnost od stanj) dobimo zapis sistema v prostoru stanj " # " # Sv 0 n× r ˙ x = + u (2.37) − f M −1 e V f M −1 e E kjer je r število vhodov v vektorju u. Dimenzija vektorja stanj x je (2 n − m) × 1. Z inverznim modelom (2.36) lahko za želene psevdopospeške sistema izračunamo potreben vhod v sistem. Z uporabo izračunanih vhodov v sistemu (2.37) dobimo naslednji skupni model " # " # Sv 0 ˙ x = + n×( n− m) u z 0( n− m)×1 I( n− m)×( n− m) kjer u z predstavlja psevdopospeške sistema. Izraz (2.36) lahko torej uporabimo za izračun napovedanih vhodov sistema. Te vhode lahko pri vodenju uporabimo samostojno (odprtozančno vodenje) ali pa v kombinaciji z zaprtozančnim vodenjem ter tako dobimo regulacijo s predkrmiljenjem. 2.4.2 Kinematični in dinamični model robota z diferencialnim pogonom Vozilo z diferencialnim pogonom na sliki 2.3 ima kolesa, gnana s pomočjo dveh elektromotorjev. Predpostavimo, da je težišče robota v njegovem geometrijskem 52 Modeliranje gibanja mobilnih sistemov Y t g l v w j y m,J t r b r x Xg Slika 2.18: Vozilo z diferencialnim pogonom središču. Masa vozila brez koles je mv, masa koles pa je mw. Vozilo se giblje po ploskvi; njegov vztrajnostni moment okoli osi z označimo kot Jc (os z je pravokotna na ploskev) in vztrajnostni moment koles kot Jw. V praksi je običajno masa koles veliko manjša od mase ohišja vozila, zato lahko uporabimo skupno maso m in vztrajnost J . Gibanje vozila opišemo s tremi posplošenimi koordinatami q = [ x, y, ϕ], vhod v sistem pa predstavlja navor na levo in desno kolo ( τl, τr prikazano na sliki 2.18). Kinematični model in omejitve Kinematični model vozila (2.2) je     ˙ x cos ϕ 0 " # v  ˙ y  = sin ϕ 0     ω ˙ ϕ 0 1 neholonomična omejitev gibanja pa je (vozilo se ne more premikati pravokotno na smer vrtenja koles) − ˙ x sin ϕ + ˙ y cos ϕ = 0 kar pomeni, da stolpci kinematične matrike predstavljajo dosegljive smeri premikov   cos ϕ 0 S = sin ϕ 0   0 1 in matrika koeficientov omejitev je h i A = − sin ϕ cos ϕ 0 2.4. Dinamični model mobilnega sistema z omejitvami 53 Dinamični model Dinamični model izpeljemo z Lagrangeovo formulacijo m d ∂ L ∂ L ∂P X − + = fk − λjajk (2.38) d t ∂ ˙ qk ∂qk ∂ ˙ qk j=1 kjer smo iz relacije (2.30) izpustili člen, povezan z vplivi neznanih motenj τd . k Podobno ne upoštevamo sil in navorov zaradi gravitacije gk, ker se vozilo vozi po ploskvi in je tako potencialna energija konstantna (brez izgube posplošenosti lahko predpostavimo WP = 0). Skupno kinetično energijo sistema lahko opišemo z relacijo m J WK = ˙ x 2 + ˙ y 2 + ˙ ϕ 2 2 2 Ker je potencialna energija sistema WP = 0, je Lagrangian enak m J L = WK − WP = ˙ x 2 + ˙ y 2 + ˙ ϕ 2 2 2 Hkrati zanemarimo še vpliv dušenja in trenja pri kotaljenju koles ( P = 0). Sile in navori v enačbi (2.38) so d ∂ L = m¨ x d t ∂ ˙ x d ∂ L = m¨ y d t ∂ ˙ y d ∂ L = J ¨ ϕ d t ∂ ˙ ϕ in ∂ L = 0 ∂x ∂ L = 0 ∂y ∂ L = 0 ∂ϕ Za dinamični model lahko glede na Lagrangeovo formulacijo (2.38) zapišemo sledeče diferencialne enačbe m¨ x − λ 1 sin ϕ = Fx m¨ y + λ 1 cos ϕ = Fy J ¨ ϕ = T r je radij kolesa. Rezultanta sil na levem in desnem kolesu je F = 1 ( τ r r + τl). Sila v smeri osi x je Fx = 1 ( τ ( τ r r + τl) cos ϕ, v smeri osi y pa Fy = 1 r r + τl) sin ϕ. Navor, ki deluje na vozilo, je T = L ( τ 2 r r − τl), pri čemer je L razdalja med kolesi. 54 Modeliranje gibanja mobilnih sistemov Imamo torej model 1 m¨ x − λ 1 sin ϕ − ( τr + τl) cos ϕ = 0 r 1 m¨ y + λ 1 cos ϕ − ( τr + τl) sin ϕ = 0 r L J ¨ ϕ − ( τr − τl) = 0 2 r Dobljeni dinamični model lahko zapišemo v matrični obliki M (q) ¨ q + V (q , ˙ q) + F ( ˙ q) = E(q)u − A T (q)λ kjer so matrike   m 0 0 M =  0 m 0   0 0 J   cos ϕ cos ϕ 1 E =  sin ϕ sin ϕ r   L − L 2 2 h i A = − sin ϕ cos ϕ 0 " # τ u = r τl ostale matrike pa so ničelne. Model v prostoru stanj (2.37), ki vključuje kinematiko in dinamiko, je določen z matrikami (glejte poglavje 2.4.1) " # m 0 f M = 0 J " # 0 e V = 0 " # 1 1 1 e E = r L − L 2 2 od koder lahko glede na enačbo (2.37) zapišemo sistem v prostoru stanj v obliki ˙ x = f (x) + g(x)u, kjer je vektor stanj določen z x = [q T , v T ] T . Dobimo  ˙ x  v cos ϕ  0 0   ˙ y   v sin ϕ  0 0  " #       τr  ˙ ϕ =  ω  +  0 0        τ  l ˙ v   0   1 1       mr mr  ˙ ω 0 L − L 2 J r 2 J r Inverzni model sistema določimo glede na relacijo (2.36) kot " # " # τ ˙ vmr r + ˙ ωJr = 2 L τ ˙ vmr l − ˙ ωJr 2 L 2.4. Dinamični model mobilnega sistema z omejitvami 55 Z uporabo inverznega modela lahko za določene hitrosti in pospeške robota izračunamo potrebna navora na obe kolesi. Izračunana vhoda lahko uporabimo za odprtozančno vodenje ali bolje v kombinaciji z zaprtozančnim vodenjem (regulacija s predkrmiljenjem). Primer 2.9 Zapišite kinematični in dinamični model za vozilo z diferencialnim pogonom, prikazano na sliki 2.19. Modela izrazite s koordinatama masnega središča ( xc, yc), ki sta od geometrijskega središča ( x, y) oddaljeni za razdaljo d. Y t g l v d y w c j y m,J t r b r x xc Xg Slika 2.19: Vozilo z diferencialnim pogonom z masnim središčem ( xc, yc) in geometrijskim središčem ( x, y) Rešitev Če upoštevamo transformacijo med geometrijskim in masnim središčem x = xc − d cos ϕ in y = yc − d sin ϕ ter njuna časovna odvoda ˙ x = ˙ xc + d ˙ ϕ sin ϕ in ˙ y = ˙ yc − d ˙ ϕ cos ϕ, ki ju vstavimo v kinematični model (slika 2.3), sta končni kinematični model in kinematična omejitev masnega središča     ˙ xc cos( ϕ) − d sin( ϕ) " # v  ˙ y    c = sin( ϕ) d cos( ϕ)     ω ˙ ϕ 0 1 − ˙ xc sin ϕ + ˙ yc cos ϕ − ˙ ϕd = 0 Lagrangian za masno središče je L = m ˙ x 2 + ˙ y 2 + J ˙ ϕ 2. Glede na (2.38) je 2 c c 2 dinamični model 56 Modeliranje gibanja mobilnih sistemov 1 m¨ xc − λ 1 sin ϕ = ( τr + τl) cos ϕ r 1 m¨ yc + λ 1 cos ϕ = Fy = ( τr + τl) sin ϕ r L J ¨ ϕ − λ 1 d = 2 r in matrike modela   m 0 0 M =  0 m 0   0 0 J   cos ϕ cos ϕ 1 E =  sin ϕ sin ϕ r   L − L 2 2 h i A = − sin ϕ cos ϕ − d   cos( ϕ( t)) − d sin( ϕ) S = sin( ϕ( t)) d cos( ϕ)    0 1 Glede na (2.37) je skupna predstavitev sistema v prostoru stanj  ˙ x      c v cos ϕ − ωd sin ϕ 0 0  ˙ yc  v sin ϕ + ωd cos ϕ  0 0  " #       τr  ˙ ϕ  =  ω  +  0 0        τ  l ˙ v   dω 2   1 1       mr mr  ˙ ω − dvωm L − L md 2+ J 2 r( d 2 m+ J ) 2 r( d 2 m+ J ) Primer 2.10 Zapišite kinematični in dinamični model vozila iz primera 2.9, kjer masno in geometrijsko središče nista enaka. Modela izrazite s pomočjo koordinat geometrijskega središča ( x, y), saj je to bolj ustrezno, če se masno središče vozila spreminja z različnim tovorom. 2.4. Dinamični model mobilnega sistema z omejitvami 57 Rešitev Kinematični model in kinematična omejitev geometrijskega središča sta     ˙ x cos( ϕ) 0 " # v  ˙ y  = sin( ϕ) 0     ω ˙ ϕ 0 1 − ˙ xc sin ϕ + ˙ yc cos ϕ = 0 Lagrangian je L = m ˙ x 2 + ˙ y 2 + J ˙ ϕ 2 kjer je masno središče izraženo z geome- 2 c c 2 trijskim. Končni model v prostoru stanj je  ˙ x  v cos ϕ  0 0   ˙ y   v sin ϕ  0 0  " #       τr  ˙ ϕ =  ω  +  0 0        τ  l ˙ v   dω 2   1 1       mr mr  L−2 d sin(2 ϕ) − L−2 d sin(2 ϕ) ˙ ω C 2 r( d 2 m+ J ) 2 r( d 2 m+ J ) kjer je C = − dωm( ˙ x cos ϕ+ ˙ y sin ϕ− v cos(2 ϕ)− dω sin(2 ϕ)) . md 2+ J Primer 2.11 V programskem okolju Matlab izračunajte potrebne navore za gibanje mobilnega robota iz primera 2.9 po referenčni trajektoriji xr = 1 , 1 + 0 , 7 sin(2 π/ 30), yr = 0 , 9 + 0 , 7 sin(4 π/ 30) brez uporabe senzorjev (odprtozančno vodenje). S pomočjo simulacije izračunajte navore robota z uporabo inverznega dinamičnega modela in prikažite dobljeno trajektorijo sistema. Parametri robota so: m = 0 , 75 kg, J = 0 , 001 kgm2, L = 0 , 075 m, r = 0 , 024 m in d = 0 , 01 m. Rešitev Glede na (2.36) je inverzni model robota  2 2  " # 2 r( ˙ ω( md + J)+ dω m) τ r( ˙ vm− dω m) r + = 2 L  2 2  τ 2 r( ˙ ω( md + J)+ dω m) l r( ˙ vm− dω m) − 2 L Iz referenčne točke izračunamo referenčno hitrost vr in referenčno kotno hitrost ωr ter njuna časovna odvoda ˙ vr in ˙ ωr. Koda Matlab je podana v programu 2.3. Rezultati simulacije pa so prikazani na slikah 2.20 in 2.21. 58 Modeliranje gibanja mobilnih sistemov Program 2.3 ./src/mdl/example_dynamic_model.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 4 % Referenca 5 freq = 2* pi /30; 6 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 7 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 8 d d x R e f = - f r e q ^ 2 * 0 . 7 * sin ( f r e q * t ); d d y R e f = -4* f r e q ^ 2 * 0 . 7 * sin (2* f r e q * t ); 9 dddxRef = - freq ^3*0.7* cos ( freq * t ); dddyRef = -8* freq ^3*0.7* cos (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na t r a j e k t o r i j a 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 dvRef = ( dxRef .* ddxRef + dyRef .* ddyRef )./ vRef ; 14 dwRef = ( dxRef .* dddyRef - dyRef .* dddxRef )./ vRef .^2 - 2.* wRef .* dvRef ./ vRef ; 15 16 q = [ qRef (: ,1); vRef (1); wRef (2)]; % Za č etna lega robota 17 m = 0.75; J = 0.001; L = 0.075; r = 0.024; d = 0.01; % Parametri robota 18 19 for k = 1: length ( t ) 20 % I z r a č un n a v o r o v iz t r a j e k t o r i j e in i n v e r z n e g a m o d e l a 21 v = v R e f ( k ); w = w R e f ( k ); dv = d v R e f ( k ); dw = d w R e f ( k ); 22 tau = [( r *( dv * m - d * w * m * w ) ) / 2 + ( r *( dw *( m * d ^2+ J ) + d * w * m * v ))/ L ; ... 23 ( r *( dv * m - d * w * m * w ) ) / 2 - ( r *( dw *( m * d ^2+ J ) + d * w * m * v ))/ L ]; 24 25 % S i m u l a c i j a k i n e m a t i č n e g a in d i n a m i č n e g a m o d e l a g i b a n j a r o b o t a 26 phi = q ( 3 ) ; v = q ( 4 ) ; w = q ( 5 ) ; 27 F = [ v * cos ( phi ) - d * w * sin ( phi ); ... 28 v * sin ( phi ) + d * w * cos ( phi ); ... 29 w ; ... 30 d * w ^2; ... 31 -( d * w * v * m )/( m * d ^2 + J )]; 32 G = [0 , 0; ... 33 0 , 0; ... 34 0 , 0; ... 35 1/( m * r ) , 1/( m * r ); ... 36 L / ( 2 * r *( m * d ^2 + J )) , - L / ( 2 * r *( m * d ^2 + J ) ) ] ; 37 dq = F + G * tau ; % M o d e l v p r o s t o r u s t a n j 38 q = q + dq * Ts ; % E u l e r j e v a i n t e g r a c i j a 39 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č je [ - pi , pi ] 40 end 2.4. Dinamični model mobilnega sistema z omejitvami 59 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 2.20: Izračunana pot robota (polna krivulja) in referenčna pot (črtkana krivulja) 1 0.5 ] 0 [NmTr−0.5 −1 0 5 10 15 20 25 t [s] 1 0.5 ] 0 [NmTl−0.5 −1 0 5 10 15 20 25 t [s] Slika 2.21: Navora na kolesi 60 Modeliranje gibanja mobilnih sistemov Literatura [1] H. Choset, K. Lynch in sod. Principles of robot motion: theory, algorithms, and implementations. MIT Press, illustrate izd., 2005, 603 str. [2] B. Siciliano in O. Khatib. Springer Handbook of Robotics. Springer, 2008, 1611 str. [3] G. Dudek in M. Jenkin. Computational Principlec of Mobile Robotics. Cambridge University press, 2010. [4] G. Klančar in I. Škrjanc. Tracking-error model-based predictive control for mobile robots in real time. Robotics and Autonomous Systems, zv. 55, št. 6, str. 460–469, 2007. [5] I. Kolmanovsky in N. H. McClamroch. Developments in Nonholonomic Control Problems. IEEE Control Systems, zv. 15, št. 6, str. 20–36, 1995. [6] A. De Luca in G. Oriolo. Modelling and Control of Nonholonomic Mechanical Systems. V J. Angeles in A. Kecskeméthy (Uredniki), Kinematics and Dynamics of Multi-Body Systems, CISM International Centre for Mechanical Sciences, str. 277–342. Springer, Vienna, 1995. [7] J. C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, New York, 1991. [8] N. Sarkar, X. Yun in V. Kumar. Control of mechanical systems with rolling constraints application to dynamic control of mobile robots. The International Journal of Robotics Research, zv. 13, št. 1, str. 55–69, 1994. [9] R. Hermann in A. J. Krener. Nonlinear Controllability and Observability. IEEE Transaction on Automatic Control, zv. 22, št. 5, str. 728–740, 1977. [10] R. W. Brockett. Asymptotic stability and feedback stabilization. V R. S. Millman in H. J. Sussmann (Uredniki), Differential Geometric Control Theory, str. 181–191. Birkhuser, Boston, MA, 1983. [11] J. P. Laumond. Robot Motion Planning and Control, Lecture Notes in Control and Information Sciences, zv. 229. Springer, 1998. [12] W. S. Levine. The Control Handbook, Second Edition . Electrical Engineering Handbook. CRC Press, 2010. [13] A. De Luca, G. Oriolo in M. Vendittelli. Control of Wheeled Mobile Robots: An Experimental Overview. V S. Nicosia, B. Siciliano, A. Bicchi in P. Valigi (Uredniki), Ramsete, Lecture Notes in Control and Information Sciences, zv. 270, str. 181–226. Springer Berlin Heidelberg, 2001. [14] O. Egeland in J. T. Gravdahl. Modeling and Simulation for Automatic Control . Marine Cybernetics, Trondheim, Norway, 2002. [15] Z. Li in J. F. Canny. Nonholonomic Motion Planning. Kluwer Academic Publishers, Boston, Dordrecht, London, 1993. [16] X. Yun. State Space Representation of Holonomic and Nonholonomic Constraints Resulting from Rolling Contacts. V IEEE International Conference on Robotics and Automation, str. 2690–2694. IEEE, Nagoya, 1995. 3 Vodenje kolesnih mobilnih sistemov 3.1 Uvod Vodenje kolesnih mobilnih robotov v okolju brez ovir lahko izvedemo z vodenjem od začetne do končne lege (klasično sledilno vodenje, kjer niso predpisana vmesna stanja trajektorije) ali pa preko sledenja referenčni trajektoriji. V primeru neholonomičnih kolesnih mobilnih sistemov se izkaže, da je bolj težavno vodenje v referenčno lego, kot pa sledenje referenčni trajektoriji, ki povezuje začetno in ciljno lego. Za uspešno vodenje (tako klasično vodenje kot sledenje trajektoriji) moramo uporabiti nezvezen ali časovno spremenljiv regulator [1], ker je sistem, ki ga vodimo, nelinearen in časovno spremenljiv. Nadalje mora robot pri gibanju upoštevati neholonomične omejitve, torej njegova pot ne more biti poljubna. Dodatni razlog za uporabo sledenja trajektoriji se skriva v dejstvu, da se mobilni robot pogosto giblje v prostoru z omejitvami, ovirami in raznimi zahtevami, ki v določeni meri predpisujejo želeno pot do cilja. Neholonomične sisteme je smiselno voditi z dvoprostostnim regulatorjem, sestavljen iz predkrmiljenja (angl. feedforward) in povratne zanke (angl. feedback). Predkrmiljenje se izračuna iz referenčne trajektorije, kjer določimo oz. predhodno izračunamo potrebne vhode v sistem, da bo le-ta sledil referenčni trajektoriji brez povratnih informacij senzorjev (odprtozančno vodenje). To- vrstno vodenje ni praktično uporabno, saj ni odporno na motnje v delovanju sistema in pogreške začetnih stanj. Zato je potrebno vključiti še povratno zanko (zaprtozančni del). Dvoprostostni regulator je naraven in prikladen za vodenje 62 Vodenje kolesnih mobilnih sistemov neholonomičnih mehanskih sistemov, zato bo uporabljen v večini prikazanih primerov v nadaljevanju. Kolesni mobilni roboti so dinamični sistemi, kjer je za želeno gibanje platforme potreben ustrezen navor na kolesa. Zato morajo algoritmi za vodenje gibanja upoštevati dinamične lastnosti sistema. Običajno se to težavo reši s pomočjo kaskadnih regulacijskih shem z zunanjim regulatorjem za vodenje hitrosti in notranjim za vodenje navora (sila, tok motorja itd.). Zunanji regulator določa potrebne hitrosti sistema, da sistem doseže referenčno lego ali sledi referenčni trajektoriji. Notranji (hitrejši) regulator pa izračuna potrebne navore (sila, tok motorja itd.), da doseže hitrosti sistema, ki jih določa zunanji regulator. Notranji regulator mora biti dovolj hiter, da dodatni fazni zamik ne povzroča težav. Pri večini primerov je notranji regulator navora že vgrajen v mobilnega robota, uporabnik pa z vodenjem določa želene hitrosti sistema glede na njegovo kinematiko. Preostali del tega poglavja je razdeljen na različne pristope vodenja za doseg referenčne lege ter pristope vodenja sledenja referenčni trajektoriji. Prvi bodo vključevali osnovno idejo in nekaj preprostih primerov uporabe na različnih platformah, slednji pa bodo obravnavani bolj podrobno, saj so ti pristopi bolj naravni za kolesne mobilne robote, ki se vozijo v okoljih z znanimi ovirami. 3.2 Vodenje v referenčno lego V nadaljevanju bomo predstavili osnovne pristope k vodenju mobilnega robota v referenčno lego, ki jo določata pozicija in orientacija. V tem primeru pot ali trajektorija do referenčne lege ni predpisana, zato se lahko robot vozi do cilja po katerikoli izvedljivi poti. To pot lahko eksplicitno določimo in jo med vožnjo tudi sproti prilagajamo ali pa jo podamo implicitno z izvedbo algoritma vodenja v referenčno lego. Običajno sta podani samo začetna (ali trenutna) in končna (ali referenčna) lega s poljubno potjo med njima, kar odpira nove možnosti npr. za izbiro “optimalne” poti. Pri izbiri poti moramo upoštevati vse omejitve – kinematične, dinamične in okoljske. To nam običajno še vedno omogoča izbiro neskončno mnogo poti, kjer pa izberemo tisto, ki upošteva tudi dodatna merila, kot so čas, dolžina, ukrivljenost, poraba energije ipd. V splošnem je načrtovanje poti zahtevna naloga, zato v tem razdelku ne bomo upoštevali teh vidikov. V nadaljevanju bo vodenje v referenčno lego razdeljeno na dve ločeni nalogi: vodenje orientacije in vodenje gibanja naprej. Ne moremo ju obravnavati ločeno, potrebno je uporabiti kombinacijo, kar privede do več regulacijskih shem za doseg referenčne lege. Ti pristopi so splošni in jih je mogoče uporabiti pri različnih kinematikah mobilnih robotov. V tem poglavju ju bomo ponazorili s primeri na diferencialnemu in Ackermannovemu pogonu. 3.2. Vodenje v referenčno lego 63 3.2.1 Vodenje orientacije Pri ravninskem gibanju je za izvedbo gibanja z želeno orientacijo potrebno vodenje orientacije. Vodenje orientacije je pomembno tudi zaradi prisotnosti neholonomičnih omejitev, ki onemogočajo premik kolesnega robota v določenih smereh. Čeprav orientacije ni možno voditi neodvisno od gibanja naprej, lahko na težavo pogledamo tudi z vidika klasičnega vodenja, kar nam pokaže, kako ojačenja regulatorja vplivajo na klasična merila uspešnosti pri povratnozančnem vodenju orientacije. Predpostavimo, da je orientacija kolesnega robota v nekem času t enaka ϕ( t), referenčna ali želena orientacija pa ϕref ( t). Pogrešek vodenja lahko določimo kot eϕ( t) = ϕref ( t) − ϕ( t) V vsakem sistemu vodenja je potrebna regulirna veličina, ki lahko spremeni ali vpliva na regulirano veličino, kar je v našem primeru orientacija. Cilj vodenja je izničiti pogrešek vodenja. Običajno mora rešitev hitro konvergirati proti 0, pri čemer morajo biti upoštevane nekatere dodatne zahteve, kot so poraba energije, obremenitev aktuatorja ter robustnost sistema ob prisotnosti motenj, šuma, parazitske dinamike itd. Običajno načrtovanje vodenja začnemo z modelom sistema, ki ga želimo voditi. V nadaljevanju bomo zapisali kinematični model, natančneje enačbo za opis njegove orientacije. Vodenje orientacije diferencialnega pogona Kinematiko diferencialnega pogona podaja (2.2), kjer tretja enačba določa potek orientacije ˙ ϕ( t) = ω( t) (3.1) Z vidika vodenja enačba (3.1) opisuje sistem z regulirno veličino ω( t) in ima integrirni značaj (njegov pol leži v izhodišču kompleksne ravnine s). Znano je, da lahko preprost proporcionalni regulator izniči pogrešek pri vodenju integrirnega procesa. Regulacijski zakon zapišemo kot ω( t) = K( ϕref ( t) − ϕ( t)) (3.2) kjer je ojačenje regulatorja K poljubna pozitivna konstanta. Regulacijski zakon (3.2) kaže, da je kotna hitrost platforme ω( t) sorazmerna pogrešku orientacije robota. S pomočjo enačb (3.1) in (3.2) lahko zapišemo dinamiko regulacijske zanke za orientacijo ˙ ϕ( t) = K ( ϕref ( t) − ϕ( t)) kar določa zaprtozančno prenosno funkcijo vodenega sistema φ( s) 1 Gcl( s) = = φ 1 ref ( s) s + 1 K 64 Vodenje kolesnih mobilnih sistemov kjer sta φ( s) in φref ( s) Laplaceovi transformaciji ϕ( t) in ϕref ( t). Prenosna funkcija Gcl( s) je prvega reda, torej se orientacija eksponentno približuje konstantni referenci (s časovno konstanto T = 1 ) in ima ojačenje 1, zato v ustaljenem K stanju ni pogreška orientacije. Regulator (3.2) torej povzroči, da se zaprtozančna prenosna funkcija obnaša kot sistem prvega reda. Včasih je zaželena prenosna funkcija drugega reda Gcl( s) = φ( s) , saj omogoča več svobode pri načrtovanju poteka med prehodnim φref ( s) pojavom. Razvoj regulatorja začnemo z definiranjem kotnega pospeška platforme ˙ ω( t), ki je sorazmeren pogrešku orientacije robota ˙ ω( t) = K( ϕref ( t) − ϕ( t)) (3.3) Dobljen regulirani sistem ˙ ω = ¨ ϕ( t) = K( ϕref ( t) − ϕ( t)) s prenosno funkcijo φ( s) K Gcl( s) = = φref ( s) s 2 + K √ je sistem drugega reda z lastno frekvenco ωn = K in koeficientom dušenja ζ = 0. Tak sistem je mejno stabilen, njegovi oscilatorni odzivi pa so nesprejemljivi. Dušenje sistema dosežemo z dodatnim členom v regulatorju (3.3) ˙ ω( t) = K 1( ϕref ( t) − ϕ( t)) − K 2 ˙ ϕ( t) (3.4) kjer sta K 1 in K 2 poljubni pozitivni ojačenji regulatorja. Z upoštevanjem (3.1) in (3.4) dobimo zaprtozančno prenosno funkcijo φ( s) K 1 Gcl( s) = = φref ( s) s 2 + K 2 s + K 1 √ kjer je ωn = K 1 lastna frekvenca, ζ = K 2 √ koeficient dušenja in sta s 2 K 1 , 2 = 1 − p ζωn ± jωn 1 − ζ 2 zaprtozančna pola. Vidimo, da z izbiro K 1 in K 2 vplivamo na primerno dušenje zaprtozančnega sistema. Vodenje orientacije Ackermannovega pogona Regulacijo orientacije Ackermannovega pogona lahko zasnujemo podobno kot v primeru diferencialnega pogona. Edina razlika se pojavi zaradi drugačnega kinematičnega modela (2.15) za orientacijo, ki ga zapišemo kot vr( t) ˙ ϕ = tan ( α( t)) (3.5) d Regulirna veličina α je sorazmerna velikosti pogreška orientacije α( t) = K( ϕref ( t) − ϕ( t)) (3.6) 3.2. Vodenje v referenčno lego 65 S pomočjo (3.5) in (3.6) zapišemo diferencialno enačbo, s katero lahko opišemo dinamiko pogreška orientacije vr( t) ˙ ϕ( t) = tan ( K( ϕref ( t) − ϕ( t))) d Sistem je torej nelinearen. Za majhne kote α( t) in konstantno hitrost zadnjih koles vr( t) = V dobimo linearni približek modela V ˙ ϕ( t) = K ( ϕref ( t) − ϕ( t)) d ki ga lahko zapišemo kot prenosno funkcijo φ( s) 1 Gcl( s) = = φ d ref ( s) s + 1 V K Podobno kot v primeru diferencialnega pogona se pogrešek orientacije ekspo- nentno približuje 0 (pri konstantni referenčni orientaciji) in s časovno konstanto T = d . V K Če je želena povratnozančna prenosna funkcija (orientacije) drugega reda, lahko uporabimo podoben pristop kot pri diferencialnem pogonu. Regulacijski zakon, ki ga podaja enačba (3.4), lahko uporabimo tudi na Ackermannovemu pogonu ˙ α( t) = K 1( ϕref ( t) − ϕ( t)) − K 2 ˙ ϕ( t) (3.7) Ob predpostavki, da je hitrost konstantna ( vr( t) = V ) ter so koti α majhni, zapišemo linearno aproksimacijo enačbe (3.5) kot V ˙ ϕ( t) = α( t) (3.8) d Kot α( t) iz (3.8) vstavimo v (3.7) V V ¨ ϕ( t) = K 1 ( ϕref ( t) − ϕ( t)) − K 2 ˙ ϕ( t) d d Tako dobimo zaprtozančno prenosno funkcijo φ( s) K V 1 G d c( s) = = φ V V ref ( s) s 2 + K 2 s + K d 1 d q Dobljeni odziv orientacije robota z lastno frekvenco ω V n = K 1 dušimo s d q koeficientom dušenja ζ = K 2 V . 2 dK 1 3.2.2 Vodenje gibanja naprej Z vodenjem gibanja naprej mislimo na algoritme, ki določajo translatorno hitrost mobilnega robota v( t), da dosežejo določen cilj vodenja. Vendar za vodenje mobilnega robota ne moremo uporabiti samo vodenja gibanja naprej. Kot primer vzemimo diferencialni pogon. Z vodenjem orientacije, kjer spreminjamo kotno 66 Vodenje kolesnih mobilnih sistemov hitrost ω( t), robot brez težav doseže ciljno orientacijo. S pomočjo vodenja naprej pa robot v splošnem ne more doseči želene pozicije, razen v primeru, ko je robot že na začetku usmerjen proti cilju. To pomeni, da je vodenje gibanja naprej neločljivo povezano z vodenjem orientacije. Torej je potrebno za doseg cilja voditi translatorno in kotno hitrost. V primeru sledenja trajektoriji, le-ta bolj ali manj določa hitrost, medtem ko se pri vodenju v referenčno lego hitrost zmanjša, ko se robot približa cilju. Primerna izbira regulatorja je proporcinalna odvisnost hitrosti glede na razdaljo do referenčne točke ( xref ( t) , yref ( t)) q v( t) = K ( xref ( t) − x( t))2 + ( yref ( t) − y( t))2 (3.9) Referenčni položaj je lahko konstanten ali pa se spreminja glede na neko referenčno trajektorijo. Regulator (3.9) ima vsekakor nekaj omejitev, pa tudi v primeru zelo velikih ali zelo majhnih razdalj do referenčne točke je potrebna posebna obravnava: • Če je razdalja do referenčne točke velika, je velika tudi regulirna veličina (3.9). Priporočljivo je omejiti regulirno veličino za največjo hitrost. V praksi omejitve narekujejo omejitve pogona, vozne razmere podlage, ukrivljenost poti itd. • Če je razdalja do referenčne točke zelo majhna, lahko robot prevozi refe- renčno točko (zaradi šuma ali nepopolnega modela vozila). Ko se robot oddaljuje od referenčne točke, se razdalja povečuje in robot pospešuje v skladu z enačbo (3.9). S to težavo se bomo spoprijeli po združitvi regulatorja gibanja naprej in regulatorja orientacije. Hitrost je neločljivo povezana s pospeškom. Slednji je v praktičnih situacijah vedno omejen zaradi omejenih sil in navorov, ki jih proizvajajo aktuatorji. To je potrebno upoštevati pri načrtovanju vodenja gibanja naprej. Ena od možnosti je omejitev pospeška. Običajno je dovolj, da na izhodu regulatorja v( t) uporabimo nizkoprepustni filter, preden se regulirna veličina posreduje robotu v obliki signala v∗( t). V ta namen lahko uporabimo najpreprostejši filter prvega reda z enosmernim ojačenjem 1, podan z diferencialno enačbo τf ˙ v∗( t) + v∗( t) = v( t) ali z enakovredno prenosno funkcijo V ∗( s) 1 Gf ( s) = = V ( s) τf s + 1 kjer je τf časovna konstanta filtra. V primeru 3.1 je uporabljen preprost algoritem vodenja, ki pripelje robota z Ackermannovim pogonom v referenčno točko. Algoritem vsebuje tako vodenje orientacije kot vodenje gibanja naprej. 3.2. Vodenje v referenčno lego 67 Primer 3.1 Napišite algoritem za vodenje trikolesnega robota s pogonom na zadnjem paru koles na referenčno pozicijo xref = 4 m in yref = 4 m. Robota vodimo z zasukom prednjega krmilnega kolesa α in s hitrostjo pogonskih koles vr. Medosna razdalja je d = 0 , 1 m, začetna lega vozila pa [ x(0) , y(0) , ϕ(0)] = [1 m , 0 m , − π]. Algoritem vodenja mora upoštevati omejitvi vozila vmax = 0 , 8 m / s in αmax = π . 4 Napišite algoritem vodenja in ga preizkusite na simulaciji kinematike vozila z uporabo Eulerjeve integracijske metode. Rešitev V tem primeru je mogoče hkrati voditi orientacijo in gibanje naprej. Zapis rešitve v programskem okolju Matlab je podan v programu 3.1. Rezultati simulacije so prikazani na slikah 3.1 in 3.2, iz katerih je razvidno, da vozilo doseže referenčno točko in se tam ustavi. Na sliki 3.2 so regulirne veličine omejene s fizičnimi omejitvami vozila. Program 3.1 ./src/ctr/example_ackerman_control_point.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 d = 0.1; % Medosna razdalja 4 xyRef = [4; 4]; % Referen č ni polo ž aj 5 q = [1; 0; - pi ]; % Za č etna lega 6 7 for k = 1: length ( t ) 8 p h i _ r e f = a t a n 2 ( x y R e f (2) - q (2) , x y R e f (1) - q ( 1 ) ) ; % R e f e r e n č na o r i e n t a c i j a 9 q R e f = [ x y R e f ; p h i _ r e f ]; 10 11 e = q R e f - q ; % P o g r e š ek 12 13 % R e g u l a t o r 14 v = 0 . 3 * s q r t ( e ( 1 ) ^ 2 + e ( 2 ) ^ 2 ) ; 15 a l p h a = 0 . 2 * e ( 3 ) ; 16 17 % O m e j i t v e v o z i l a 18 if abs ( a l p h a ) > pi /4 , a l p h a = pi /4* s i g n ( a l p h a ); end 19 if abs ( v ) >0.8 , v = 0 . 8 * s i g n ( v ); end 20 21 % S i m u l a c i j a g i b a n j a r o b o t a 22 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; v / d * tan ( a l p h a )]; 23 n o i s e = 0 . 0 0 ; % S p r e m e n i s t a n d a r d n o d e v i a c i j o š uma ( npr . 0 . 0 0 1 ) 24 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 25 end 68 Vodenje kolesnih mobilnih sistemov 5 4 Cilj 3 ] [m 2 y 1 Start 0 −1 0 2 4 6 x [m] Slika 3.1: Pot robota do ciljne pozicije iz primera 3.1 0.8 0.6 s]/ [m 0.4 v 0.2 00 5 10 15 20 25 30 t [s] 0.8 0.6 ] [rad 0.4 α 0.2 00 5 10 15 20 25 30 t [s] Slika 3.2: Regulirni signali iz primera 3.1 3.2. Vodenje v referenčno lego 69 Primer 3.2 Za diferencialni pogon rešite enako nalogo kot v primeru 3.1, pri čemer je največja hitrost vozila vmax = 0 , 8 m / s. Rešitev Ustrezno prilagodite regulator iz primera 3.1 in popravite kinematiko v simulaciji. 3.2.3 Osnovni pristopi V nadaljevanju bomo predstavili nekaj uporabnih pristopov k vodenju kolesnega mobilnega robota v referenčno lego. Omenjeni pristopi na različne načine zdru- žujejo predhodno predstavljeno vodenje orientacije in vodenje gibanja naprej (poglavji 3.2.1 in 3.2.2) ter tako omogočajo uspešno vodenje kolesnih mobilnih robotov v referenčno lego. Ponazorjeni bodo na robotu z diferencialnim pogonom, lahko pa jih prilagodimo tudi drugim vrstam kolesnih robotov. Vodenje v referenčno pozicijo V tem primeru mora robot priti v referenčno (končno) pozicijo, pri tem pa ni predpisana končna orientacija, torej je lahko poljubna. Da robot prispe do referenčne točke, moramo nenehno voditi njegovo orientacijo v smeri proti referenčni točki. To smer označimo z ϕr (slika 3.3) in jo lahko enostavno določimo s pomočjo geometrijskih relacij yref − y( t) ϕr( t) = arctan xref − x( t) Vodenje kotne hitrosti ω( t) je tako določeno kot ω( t) = K 1( ϕr( t) − ϕ( t)) (3.10) kjer je K 1 pozitivno ojačenje regulatorja. Osnovno vodenje je podobno kot v primeru 3.1 in je prikazano na sliki 3.3. Najprej s pomočjo enačbe (3.9) določimo translatorno hitrost robota q v( t) = K 2 ( xref ( t) − x( t))2 + ( yref ( t) − y( t))2 (3.11) Kot smo že omenili, je potrebno v začetni fazi omejiti največjo hitrost zaradi številnih fizičnih omejitev. Upoštevati moramo zlasti omejitve hitrosti in pospeška. Regulacijski zakon (3.11) skriva tudi potencialno nevarnost, ko se robot približa ciljni legi. Ukaz za hitrost (3.11) je vedno pozitiven, zato se lahko robot med 70 Vodenje kolesnih mobilnih sistemov j r ( x , y ) ref ref D j ( x, y) Slika 3.3: Vodenje v referenčno lego zaviranjem proti končni poziciji pomotoma zapelje preko nje. Težava je v tem, da se bo takrat regulirna veličina za hitrost povečala, ker se poveča razdalja med robotom in referenco (robot se oddaljuje od cilja). Druga težava je v tem, da prečkanje referenčne točke prav tako obrne referenčno orientacijo, kar vodi do hitrega vrtenja robota. Obstaja nekaj preprostih rešitev: • Ko robot prevozi referenčno točko, se pogrešek orientacije nenadoma spre- meni za 180°. Zato bo algoritem preveril, ali njegova absolutna vrednost presega 90°. Pogrešek orientacije se bo nato povečal ali zmanjšal za 180° (tako da se nahaja v intervalu [−180° , 180°]), preden vstopi v regulator. Poleg tega izhod regulatorja (3.11) spremeni svoj predznak. Torej nadgrajeni različici regulacijskih zakonov (3.10) in (3.11) zaobideta omenjene težave eϕ( t) = ϕr( t) − ϕ( t) ω( t) = K 1 arctan(tan( eϕ( t))) (3.12) q v( t) = K 2 ( xref ( t) − x( t))2 + ( yref ( t) − y( t))2sgn(cos( eϕ( t))) • Ko robot doseže določeno okolico referenčne točke, se konča faza približe- vanja in regulirne veličine za hitrosti postanejo ničelne. Ta mehanizem za popolno zaustavitev vozila je potrebno uporabiti tudi pri spremenjenem regulacijskem zakonu (3.12), zlasti v primeru šumnih meritev. Vodenje v referenčno lego z vpeljavo vmesne točke Naslednji algoritem vodenja je enostaven za izvedbo, saj uporablja preprost regulator, določen z (3.11) in (3.10), ki robota pripelje v želeno referenčno točko. Ker imamo poleg ( xref ( t) , yref ( t)) tudi zahtevo za referenčno smer ϕref , moramo dodati vmesno točko, ki bo oblikovala trajektorijo tako, da dobimo pravilno 3.2. Vodenje v referenčno lego 71 j t ( x , y ) t t dtol r j ref ( x , y ) ref ref j ( ) x, y Slika 3.4: Vodenje v referenčno lego z vpeljavo vmesne točke končno orientacijo. Vmesna točka ( xt, yt) je od referenčne točke oddaljena za razdaljo r, pri čemer smer od vmesne točke proti referenčni sovpada z referenčno orientacijo, kot prikazuje slika 3.4. Vmesno točko določimo z xt = xref − r cos ϕref yt = yref − r sin ϕref Algoritem vodenja je sestavljen iz dveh faz. V prvi fazi vodimo robota proti vmesni točki. Ko se ji dovolj približa (kar preverja pogoj p( x − xt)2 + ( y − yt)2 < dtol), algoritem preide v drugo fazo, kjer vodimo robota proti referenčni točki. Ta pristop zagotavlja, da robot pride na referenčno pozicijo z zahtevano orientacijo (v referenčni legi je možen zelo majhen pogrešek orientacije). Možne so različne variacije algoritma in vpeljava več vmesnih točk za boljše delovanje. Predstavljen algoritem je zelo enostaven in uporaben na mnogih področjih. Glede na aplikacijo je potrebno izbrati ustrezno razdaljo r in tolerančno področje dtol. Primer 3.3 Za robota z diferencialnim pogonom napišite algoritem vodenja v referenčno lego [ xref , yref , ϕref ] = [4 m , 4 m , 0°] z vpeljavo vmesne točke. Poiščite ustrezne vrednosti parametrov r in dtol. Začetna lega vozila je [ x(0) , y(0) , ϕ(0)] = [1 m , 0 m , 100°]. Preizkusite algoritem vodenja s pomočjo simulacije kinematike vozila z diferencialnim pogonom. 72 Vodenje kolesnih mobilnih sistemov Rešitev Matlab koda možne rešitve je podana v programu 3.2. Rezultati simulacije so prikazani na slikah 3.5 in 3.6. Program 3.2 ./src/ctr/example_diff_control_intermediate_point.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :15; % Č as simulacije 3 r = 0.5; % Razdalja vmesne to č ke od cilja 4 dTol = 0.05; % Toleran č na razdalja od vmesne to č ke za preklop 5 qRef = [4; 4; 0]; % Referen č na lega 6 q = [1; 0; 100/180* pi ]; % Za č etna lega 7 8 % Vmesna to č ka 9 xT = qRef (1) - r * cos ( qRef (3)); 10 yT = qRef (2) - r * sin ( qRef (3)); 11 12 state = 0; % Vodenje proti : 0 - vmesni to č ki , 1 - referen č ni to č ki 13 for k = 1: length ( t ) 14 D = s q r t (( q R e f (1) - q ( 1 ) ) ^ 2 + ( q R e f (2) - q ( 2 ) ) ^ 2 ) ; 15 if D < d T o l % U s t a v i t e v v bli ž ini c i l j a 16 v = 0; 17 w = 0; 18 e l s e 19 if s t a t e ==0 20 d = s q r t (( xT - q ( 1 ) ) ^ 2 + ( yT - q ( 2 ) ) ^ 2 ) ; 21 if d < dTol , s t a t e = 1; end 22 23 p h i T = a t a n 2 ( yT - q (2) , xT - q ( 1 ) ) ; 24 e P h i = p h i T - q ( 3 ) ; 25 e l s e 26 e P h i = q R e f (3) - q ( 3 ) ; 27 end 28 e P h i = w r a p T o P i ( e P h i ); 29 30 % R e g u l a t o r 31 v = D * 0 . 8 ; 32 w = e P h i *5; 33 end 34 35 % S i m u l a c i j a g i b a n j a r o b o t a 36 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 37 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 38 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 39 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 40 end 3.2. Vodenje v referenčno lego 73 5 Cilj Vmesna točka 4 3 ] [m 2 y 1 Start 0 −1 0 2 4 6 x [m] Slika 3.5: Pot robota do ciljne lege z uporabo vmesne točke iz primera 3.3 4 3 s]/ [m 2 v 1 00 5 10 15 t [s] 0 s]−1 /−2 [rad ω−3 −4 0 5 10 15 t [s] Slika 3.6: Regulirni signali iz primera 3.3 74 Vodenje kolesnih mobilnih sistemov j t j r r a j ref ( x , y ) ref ref D b j ( x, y) Slika 3.7: Vodenje v referenčno lego z vpeljavo vmesne usmeritve Vodenje v referenčno lego z vpeljavo vmesne usmeritve Robot se mora pripeljati iz začetne v referenčno lego, kjer sta podani pozi- cija ( xref , yref ) in orientacija ϕref . Ideja algoritma z vpeljavo vmesne usmeritve je prikazana na sliki 3.7 [2]. Najprej določimo pravokotni trikotnik in postavimo referenčno točko v oglišče s pravim kotom. Kateta, dolžine D = +p( xref ( t) − x( t))2 + ( yref ( t) − y( t))2, povezuje trenutni položaj robota z referenčnim in določa orientacijo ϕr, ki kaže od robota proti cilju. Druga kateta ima določeno dolžino r > 0, ki jo izberemo sami. Kot med kateto D in hipotenuzo označimo z β( t). Pri tem pristopu imata ključno vlogo dva kota, ki ju določimo kot α( t) = ϕr( t) − ϕref (arctan + r ; α( t) > 0 β( t) = D − arctan r ; sicer D Upoštevamo, da imata kota α( t) in β( t) vedno enak predznak (v primeru na sliki 3.7 sta oba pozitivna). Če je α ob določenem času 0, je takrat β nepomemben in je lahko njegov znak poljuben. Velike (absolutne) vrednosti α nakazujejo, da vožnja naravnost do referenčne točke ni dobra ideja, saj bo pogrešek orientacije v referenčni legi velik. Zato je potrebno zmanjšati (absolutno) vrednost kota α. To dosežemo z vpeljavo vmesne usmeritve, ki je premaknjena iz ϕr, premik pa je vedno stran od referenčne orientacije ϕref ; če referenčna orientacija kaže desno (z vidika robota), se robot približa referenčni poziciji z leve strani in obratno. Medtem ko se med približevanjem cilju (absolutna) vrednost kota α( t) običajno zmanjšuje, se (absolutna) vrednost kota β( t) povečuje z manjšanjem oddaljenosti od reference. To bomo izkoristili pri načrtovanju algoritma vodenja. Podobno kot v primeru vodenja z vpeljavo vmesne točke je algoritem sestavljen iz 3.2. Vodenje v referenčno lego 75 dveh faz. V prvi fazi (kjer je | α( t)| velik) vodimo orientacijo robota proti vmesni usmeritvi ϕt( t) = ϕr( t) + β( t) (omenjen primer je prikazan na sliki 3.7). V drugi fazi, ko postaneta kota α in β enaka, trenutna referenčna orientacija preide v ϕt( t) = ϕr( t) + α( t). Torej v prvem delu robota vodimo v smeri referenčne pozicije, v drugem delu pa poskrbimo, da se robot pripelje na referenčno pozicijo z referenčno orientacijo. Preklop med fazama je izveden brez nezveznega skoka, saj sta obe usmeritvi v času prehoda enaki. Regulacijski zakon za orientacijo zapišemo kot ( α( t) ; | α( t)| < | β( t)| eϕ( t) = ϕr( t) − ϕ( t) + β( t) ; sicer ω( t) = Keϕ( t) Vidimo, da trenutna referenčna usmeritev nikoli ne kaže proti referenčni točki, ampak je vedno rahlo zamaknjena. Zamik je izbran tako, da gre kot α( t) proti 0. To pomeni, da referenčna usmeritev kaže proti referenčni točki in robot bo prispel do nje s pravilno referenčno orientacijo. Upoštevamo, da ta algoritem velja tudi za negativne vrednosti kotov α in β. Paziti moramo le, da so vsi koti v intervalu (− π, π]. Translatorna hitrost je določena podobno kot v prejšnjem poglavju. Primer 3.4 Za robota z diferencialnim pogonom napišite algoritem vodenja do referenčne lege [ xref , yref , ϕref ] = [4 m , 4 m , 0°] z vpeljavo vmesne usmeritve. Poiščite ustrezno vrednost parametra r. Začetna lega vozila je [ x(0) , y(0) , ϕ(0)] = [1 m , 0 m , 100°]. Preizkusite algoritem na simulaciji kinematičnega modela. Rešitev Matlab koda možne rešitve je predstavljena v programu 3.3. Rezultati simulacije so prikazani na slikah 3.8 in 3.9. Program 3.3 ./src/ctr/example_diff_control_intermediate_direction.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :15; % Č as simulacije 3 r = 0.2; % Parameter razdalje 4 qRef = [4; 4; 0]; % Referen č na lega 5 q = [1; 0; 100/180* pi ]; % Za č etna lega 6 7 for k = 1: length ( t ) 8 % I z r a č u n a j z a m i k z a r a d i r e f e r e n č ne u s m e r i t v e 9 p h i R = a t a n 2 ( q R e f (2) - q (2) , q R e f (1) - q ( 1 ) ) ; 10 D = s q r t (( q R e f (1) - q ( 1 ) ) ^ 2 + ( q R e f (2) - q ( 2 ) ) ^ 2 ) ; 11 76 Vodenje kolesnih mobilnih sistemov 12 a l p h a = w r a p T o P i ( p h i R - q R e f ( 3 ) ) ; 13 b e t a = a t a n ( r / D ); 14 if alpha <0 , b e t a = - b e t a ; end 15 16 % C o n t r o l l e r 17 if abs ( a l p h a ) < abs ( b e t a ) 18 e P h i = w r a p T o P i ( p h i R - q (3) + a l p h a ); % D r u g i del 19 e l s e 20 e P h i = w r a p T o P i ( p h i R - q (3) + b e t a ); % P r v i del 21 end 22 v = D * 0 . 8 ; 23 w = e P h i *5; 24 25 % S i m u l a c i j a g i b a n j a r o b o t a 26 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 27 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 28 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 29 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č je [ - pi , pi ] 30 end 5 Cilj 4 3 ] [m 2 y 1 Start 0 −1 0 2 4 6 x [m] Slika 3.8: Pot robota do ciljne lege z vpeljano vmesno usmeritvijo iz primera 3.4 3.2. Vodenje v referenčno lego 77 4 3 s]/ [m 2 v 1 00 5 10 15 t [s] 0 s]−1 / [rad−2 ω−3 −40 5 10 15 t [s] Slika 3.9: Regulirni signali iz primera 3.4 Vodenje po odsekoma zvezni poti, določena s premico in krožnim lokom Pot, sestavljena iz daljic in krožnih lokov, je znana kot najkrajša možna pot za robote z Ackermannovim pogonom, kjer polmer krožnic predstavlja najkrajši možni polmer zavoja vozila [3–5]. Takšna pot je najkrajša tudi za robota z diferencialnim pogonom, kjer je najkrajši polmer kroga omejen na nič, kar pomeni, da se lahko robot vrti na mestu. Osnovna ideja algoritma je prikazana na sliki 3.10. Najprej skozi referenčno točko narišemo krožnico s polmerom R, ki je tangencialen na referenčno orientacijo. O primerni dolžini polmera R bomo razpravljali kasneje. Obstajata dve rešitvi – izberemo krožnico, katere središče je bližje robotu. Ta krožnica oz. natančneje določen lok predstavlja drugi del načrtovane poti. Robot najprej sledi premici, ki se v bližini referenčne točke tangencialno poveže na krožnico. Ponovno obstajata dve rešitvi tangente in izberemo tisto, ki daje pravilno smer vožnje po krožnem loku; smer vožnje po loku je določena z referenčno orientacijo (v primeru na sliki 3.10 se bo robot peljal v smeri urinega kazalca). Rešitev preprosto izberemo s preverjanjem predznakov vektorskih produktov radialnih vektorjev (od središča krožnice do potencialne tangentne točke na njej) in tangencialnih vektorjev (od robota do potencialne tangentne točke na krožnici). 78 Vodenje kolesnih mobilnih sistemov j t ( x , y ) ref ref j ref ( x , y ) t t R D j ( x, y) Slika 3.10: Vodenje po odsekoma zvezni poti, sestavljeni iz premice in krožnega loka Cilj prvega dela algoritma je vodenje robota proti točki ( xt, yt), kjer se premica dotika krožnega loka. Vzemimo preprost regulator orientacije ω( t) = K( ϕt( t) − ϕ( t)) kjer je ϕt( t) = arctan yt− y( t) . Ko je razdalja do vmesne točke dovolj majhna, se xt− x( t) začne druga faza, ki vključuje vožnjo vzdolž trajektorije. Regulator spremenimo v v( t) ω( t) = + K( ϕtang( t) − ϕ( t)) R kjer je R polmer kroga, v( t) želena translatorna hitrost in ϕtang( t) smer tangente na krožni lok v trenutni poziciji robota. Prvi del regulatorja predstavlja predkrmiljenje, ki zagotavlja vožnjo robota po krožnem loku s polmerom R, drugi del pa povratno zanko, ki popravlja regulacijske pogreške. Da dosežemo večjo robustnost, se referenčna pot izračuna v vsakem računskem koraku regulacijske zanke, kar zagotovi, da je robot vedno na referenčni premici ali krožnici. Končna prevožena pot se zato nekoliko razlikuje od idealne poti, sestavljene iz daljice in krožnega loka. Omenjena razlika v gibanju nastane zaradi neujemanja začetnih pogojev (orientacija robota se ne ujema popolnoma s tangento), šuma in motenj (zdrs koles ipd.). Referenčno pot je v realnem času razmeroma enostavno določiti. Sama pot je zvezna, vendar zahtevani vhodi niso. Zaradi prehoda iz premice na krožni lok kotna hitrost robota hipoma skoči iz nič na v( t) . V praksi to ni možno zaradi R omejenega pospeška robota, zato se pri tem prehodu pojavi nekaj sledilnega pogreška. Takšno vodenje je primerno za primere, ko mora robot priti v referenčno lego po najkrajši poti poljubne oblike (npr. robotski nogomet). Pri robotih z omejenim 3.2. Vodenje v referenčno lego 79 polmerom zavoja (npr. Ackermannov pogon) je najkrajša pot do ciljne lege, ko je parameter R enak najkrajšemu polmeru zavoja robota. Seveda pa to vodi v visoke vrednosti radialnih pospeškov, zato je morda zaželena večja vrednost R. Primer 3.5 Za robota z diferencialnim pogonom napišite algoritem vodenja v referenčno lego [ xref , yref , ϕref ] = [0 m , 0 m , 0°]. Polmer krožnice naj bo R = 0 , 4 m. Začetna lega vozila je [ x(0) , y(0) , ϕ(0)] = [−3 m , −3 m , 100°]. Preizkusite algoritem na simulaciji kinematičnega modela. Rešitev Čeprav je osnovna ideja algoritma vodenja dokaj preprosta, je izvedba nekoliko bolj zapletena, saj je potrebno izračunati ustrezna središča krožnice in tangentne točke na njej ter nekatere druge parametre. Možna rešitev je podana v programu 3.4, rezultati simulacije pa so prikazani na slikah 3.11 in 3.12. Program 3.4 ./src/ctr/example_diff_control_line_circle.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :15; % Č as simulacije 3 r = 0.2; % Parameter razdalje 4 qRef = [4; 4; 0]; % Referen č na lega 5 q = [1; 0; 100/180* pi ]; % Za č etna lega 6 7 aMax = 5; % Maksimalni pospe š ek 8 vMax = 0.4; % Maksimalna hitrost 9 accuracy = vMax * Ts ; % To č nost 10 curveZone = 0.6; % Radij 11 Rr = 0.99* curveZone /2; % Radij 12 slowDown = false ; v = 0; vDir = 1; w = 0; % Za č etna stanja 13 X = [0 , 1; -1 , 0]; % Pomo ž na matrika : a . ’* X * b = a (1)* b (2) - a (2)* b (1) 14 15 for k = 1: length ( t ) 16 fin = [ cos ( q R e f ( 3 ) ) ; sin ( q R e f ( 3 ) ) ] ; 17 D = q R e f ( 1 : 2 ) ; % C i l j n a to č ka 18 S = q ( 1 : 2 ) ; % P o l o ž aj r o b o t a 19 M = ( D + S ) / 2 ; 20 Ov = [ cos ( q ( 3 ) ) ; sin ( q ( 3 ) ) ] ; % V e k t o r o r i e n t a c i j e 21 SDv = D - S ; % V e k t o r SD 22 l2 = n o r m ( SDv ); % R a z d a l j a 23 24 if s l o w D o w n 25 v = v - a M a x * Ts ; if v < 0 , v = 0; end 26 w = 0; 27 e l s e 28 if fin . ’* X * SDv > SDv . ’* X * fin 29 Ps = D - Rr * X . ’* fin ; % C e n t e r k r o g a 30 e l s e 31 Ps = D - Rr * X * fin ; % C e n t e r k r o g a 32 end 33 34 l = n o r m ( Ps - S ); 80 Vodenje kolesnih mobilnih sistemov 35 if l < c u r v e Z o n e /2 36 Dv = fin ; 37 e l s e 38 d = s q r t ( sum (( S - Ps ) . ^ 2 ) - Rr ^ 2 ) ; 39 a l p h a = a t a n ( Rr / d ); 40 phi = w r a p T o 2 P i ( a t a n 2 ( Ps (2) - S (2) , Ps (1) - S ( 1 ) ) ) ; 41 U1 = S + d *[ cos ( phi + a l p h a ); sin ( phi + a l p h a )]; 42 U2 = S + d *[ cos ( phi - a l p h a ); sin ( phi - a l p h a )]; 43 if (( U1 - S ). ’* X *( Ps - U1 )) * ( fin . ’* X *( Ps - D )) >= 0 44 D = U1 ; 45 e l s e 46 D = U2 ; 47 end 48 M = ( D + S ) / 2 ; 49 SDv = D - S ; 50 Dv = SDv /( n o r m ( SDv )+ eps ); 51 end 52 53 if l2 > a c c u r a c y % Č e p o l o ž aj ni d o s e ž en 54 v = v + a M a x * Ts ; if v > vMax , v = v M a x ; end 55 56 Ev = X *( D - S ); 57 DTv = X * Dv ; 58 if abs ( DTv . ’* X * Ev ) < 0 . 0 0 0 0 0 1 % P o j d i n a r a v n o s t 59 g a m m a = 0; 60 Sv = SDv /( n o r m ( SDv )+ eps ); 61 e l s e % Go on a c i r c l e 62 C = DTv * Ev . ’* X *( D - M )/( DTv . ’* X * Ev ) + D ; % C e n t e r k r o g a 63 if SDv . ’* X * Dv > 0 , a = 1; e l s e a = -1; end 64 Sv = a * X *( C - S ); 65 Sv = Sv /( n o r m ( Sv )+ eps ); 66 g a m m a = a * a c o s ( Dv . ’* Sv ); 67 if a * Sv . ’* X * Dv < 0 , g a m m a = a *2* pi - g a m m a ; end 68 l = abs ( g a m m a * n o r m ( S - C )); % Dol ž ina k r i v u l j e 69 end 70 71 if v > eps 72 if Ov . ’* Sv < 0 , v D i r = -1; e l s e v D i r = 1; end % U s m e r j e n o s t 73 e P h i = a c o s ( v D i r * Sv . ’* Ov ); % K o t n i p o g r e š ek 74 if v D i r * Ov . ’* X * Sv < 0 , e P h i = - e P h i ; end 75 dt = l / v ; if dt < 0 . 0 0 0 0 1 , dt = 0 . 0 0 0 0 1 ; end 76 w = g a m m a / dt + e P h i / dt * 1 0* (1 - exp ( - l2 / 0 . 1 ) ) ; % K o t n a h i t r o s t 77 e l s e 78 w = 0; 79 end 80 e l s e 81 s l o w D o w n = t r u e ; 82 end 83 end 84 u = [ v D i r * v ; w ]; % T a n g e n c i a l n a in k o t n a h i t r o s t 85 86 % S i m u l a c i j a g i b a n j a r o b o t a 87 dq = [ u ( 1 ) * cos ( q ( 3 ) ) ; u ( 1 ) * sin ( q ( 3 ) ) ; u ( 2 ) ] ; 88 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 89 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 90 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 91 end 3.2. Vodenje v referenčno lego 81 5 Cilj 4 3 ] [m 2 y 1 Start 0 −1 0 2 4 6 x [m] Slika 3.11: Pot robota do ciljne lege na podlagi premic in krožnic iz primera 3.5 0.4 0.3 s]/ [m 0.2 v 0.1 00 5 10 15 t [s] 0 s]/−0.5 [rad ω −1 −1.50 5 10 15 t [s] Slika 3.12: Regulirni signali iz primera 3.5 82 Vodenje kolesnih mobilnih sistemov Ti+1 v Ti d r R vn Slika 3.13: Vodenje po odsekoma zvezni poti, določeni s sekvenco točk. Referenčna pot med sosednjima točkama je daljica, ki ti dve točki povezuje. Od vodenja v referenčno lego do vodenja po referenčni poti Pogosto je cilj vodenja določen z zaporedjem referenčnih točk, ki naj bi jih robot prevozil. V tem primeru ne govorimo več o vodenju v referenčno lego, temveč je skozi te točke speljana referenčna pot. Za povezave med posameznimi točkami so pogosto uporabljene daljice. Cilj vodenja je, da robot prispe v vsako referenčno točko s pravilno orientacijo ter se samodejno odpelje v naslednjo referenčno točko. Ta pristop je enostaven za izvedbo in običajno zadostuje za uporabo v praksi. Njegova pomanjkljivost je nezveznost med sosednjimi daljicami, zato se tam pojavi skokovit pogrešek sledenja. Pot je določena z zaporedjem točk T i = [ xi, yi] T , kjer je i ∈ 1 , 2 , . . . , n in n število točk. Na začetku mora robot slediti prvemu segmentu (daljica med točkama T1 −−−→ in T2) in tako priti do T2 z orientacijo, ki jo določa vektor T1T2. Ko doseže konec tega segmenta, začne slediti naslednjemu (med točkama T2 in T3) in tako naprej. Slika 3.13 prikazuje aktualen segment med točkama T i in T i+1 z označenimi veličinami. Vektor v = T i+1 − T i = [∆ x, ∆ y] T predstavlja smer segmenta, vektor r = R − T i pa smer od točke T i proti središču robota R. Vektor v n = [∆ y, −∆ x] je pravokoten na vektor v. Robot mora slediti trenutnemu segmentu, medtem ko je projekcija vektorja r na vektor v znotraj intervala, ki ga določata točki T i in T i+1. Ta pogoj lahko 3.2. Vodenje v referenčno lego 83 izrazimo na naslednji način (Sledi trenutnemu segmentu (T i, T i+1) če 0 < u < 1 Sledi naslednjemu segmentu (T i+1 , T i+2) če u > 1 kjer je u skalarni produkt v T r u = v T v Spremenljivka u nam torej pove, ali je trenutni segment še vedno aktualen ali pa je potreben prehod na naslednjega. Pravokotna razdalja med robotom in segmentom (daljico) je določena z normalnim vektorjem v n v T r d = n pv T v n n Normiramo razdaljo d z dolžino daljice ter dobimo normirano pravokotno razdaljo dn med robotom in daljico v T r d n n = v T v n n Ko je robot na segmentu (daljici), je normirana razdalja dn nič. Ko pa je robot na desni strani segmenta (glede na vektor v), je dn pozitivna in obratno. Normirana razdalja dn se uporablja za določanje želene smeri vožnje robota. Če je robot na daljici ali v njeni neposredni bližini, ji mora slediti. Če pa je robot daleč stran od daljice, se mora voziti pravokotno nanjo, da (čim hitreje) prispe do nje. Referenčno smer vožnje v nekem trenutku lahko določimo kot ϕref = ϕlin + ϕrot kjer je ϕlin = atan2 (∆ y, ∆ x) (štirikvadratna inverzna funkcija tangens je definirana v (2.11)) smer daljice in ϕrot = arctan ( K 1 dn) popravek dodatne referenčne rotacije, ki robotu omogoča, da doseže daljico. Ojačenje K 1 spreminja občutljivost dodatnega referenčnega kota ϕrot glede na dn. Ker je ϕref pridobljen s seštevanjem dveh kotov, je potrebno poskrbeti, da bo v veljavnem območju [− π, π] (cikličnost kota). Zaenkrat smo določili referenčno smer, ki ji mora robot slediti, kar lahko dose- žemo z uporabo primernega regulacijskega algoritma. Regulacijski pogrešek je opredeljen kot eϕ = ϕref − ϕ kjer je ϕ orientacija robota. Iz pogreška orientacije s pomočjo proporcionalnega regulatorja izračunamo kotno hitrost robota ω = K 2 eϕ kjer je K 2 proporcionalno ojačenje. Podobno lahko izvedemo tudi PID regulator, kjer z integracijskim členom povečujemo hitrost približevanja robota daljici (vse manjši eϕ), z diferencialnim členom pa zmanjšamo oscilacije, ki nastanejo zaradi dodanega integracijskega člena. Translatorno hitrost robota v lahko vodimo z osnovnimi pristopi, obravnavanimi v prejšnjih poglavjih. 84 Vodenje kolesnih mobilnih sistemov Primer 3.6 Napišite algoritem vodenja za robota z diferencialnim pogonom, ki naj prevozi zaporedje daljic, ki jih definirajo točke T1 = [3 , 0], T2 = [6 , 4], T3 = [3 , 4], T4 = [3 , 1] in T5 = [0 , 3]. Poiščite ustrezne vrednosti parametrov K 1 in K 2. Začetna lega vozila je [ x(0) , y(0) , ϕ(0)] = [5 m , 1 m , 108°]. Preizkusite algoritem na simulaciji kinematičnega modela. Rešitev Matlab koda možne rešitve je podana v programu 3.5. Referenčna pot in dejanska trajektorija gibanja robota sta prikazani na sliki 3.14, regulirni signali pa na sliki 3.15. Program 3.5 ./src/ctr/example_diff_point_sequence.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 T = [3 , 0; 6 , 4; 3 , 4; 3 , 1; 0 , 3]. ’; % To č ke referen č nih daljic 4 q = [5; 1; 0.6* pi ]; % Za č etna lega 5 6 i = 1; % Indeks prve to č ke 7 for k = 1: length ( t ) 8 % R e f e r e n č na d a l j i c a 9 dx = T (1 , i +1) - T (1 , i ); 10 dy = T (2 , i +1) - T (2 , i ); 11 12 v = [ dx ; dy ]; % U s m e r i t v e n i v e k t o r r e f e r e n č ne d a l j i c e 13 vN = [ dy ; - dx ]; % V e k t o r o r t o g o n a l n e u s m e r i t v e 14 r = q ( 1 : 2 ) - T (: , i ); 15 u = v . ’* r /( v . ’* v ); 16 17 if u >1 && i < s i z e ( T ,2) -1 % P o g o j za p r e k l o p na n a s l e d n j o d a l j i c o 18 i = i + 1; 19 dx = T (1 , i +1) - T (1 , i ); 20 dy = T (2 , i +1) - T (2 , i ); 21 v = [ dx ; dy ]; 22 vN = [ dy ; - dx ]; 23 r = q ( 1 : 2 ) - T (: , i ); 24 end 25 26 dn = vN . ’* r /( vN . ’* vN ); % N o r m i r a n a o r t o g o n a l n a r a z d a l j a 27 28 p h i L i n = a t a n 2 ( v (2) , v ( 1 ) ) ; % U s m e r i t e v p r e m i c e d a l j i c e 29 p h i R o t = a t a n (5* dn ); % Č e smo d a l e č od p re m ic e , p o t e m je p o t r e b e n 30 % d o d a t e n zasuk , da se u s m e r i m o p r o t i p r e m i c i . Č e smo na l e v i strani , 31 % se o b r n e m o v s m e r i u r i n e g a k a za lc a , s i c e r v o b r a t n i s m e r i . 32 % Oja č e n j e 5 p o v e č a ob č u t l j i v o s t . 33 34 p h i R e f = w r a p T o P i ( p h i L i n + p h i R o t ); 35 36 % K o t n i p o g r e š ek 37 e P h i = w r a p T o P i ( p h i R e f - q ( 3 ) ) ; 38 39 % R e g u l a t o r 40 v = 0 . 4 * cos ( e P h i ); 3.2. Vodenje v referenčno lego 85 41 w = 3* e P h i ; 42 43 % S i m u l a c i j e g i b a n j a r o b o t a 44 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 45 n o i s e = 0 . 0 0 ; % SL P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 46 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 47 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 48 end 5 4 3 ] [m 2 y 1 Start 0 −1 0 2 4 6 x [m] Slika 3.14: Referenčna pot in dejansko gibanje robota iz primera 3.6 86 Vodenje kolesnih mobilnih sistemov 0.4 s] 0.2 / [mv 0 −0.2 0 5 10 15 20 25 30 t [s] 5 s]/ 0[rad ω −5 0 5 10 15 20 25 30 t [s] Slika 3.15: Regulirni signali iz primera 3.6 Poskusite dopolniti kodo, da bo robot lahko vozil tudi vzvratno, če je | eϕ| > π . 2 3.3 Vodenje po referenčni trajektoriji V mobilni robotiki je pot krivulja, ki jo mora robot prevoziti v prostoru po- splošenih koordinat. Če je pot parametrizirana po času, torej gibanje po poti je sinhronizirano s časom, govorimo o trajektoriji. Kadarkoli je načrt gibanja robota znan vnaprej, lahko (referenčno) trajektorijo robota zapišemo kot časovno funkcijo v prostoru posplošenih koordinat: q ref ( t) = [ xref ( t) , yref ( t) , ϕref ( t)] T . Iz praktičnih razlogov je trajektorija vedno definirana na končnem časovnem intervalu t ∈ [0 , T ], kar pomeni, da ima referenčna trajektorija začetno in končno točko. Vodenje po referenčni trajektoriji je mehanizem, ki zagotavlja, da je trajektorija robota q( t) kljub morebitnim težavam čim bolj podobna referenčni q ref ( t). 3.3. Vodenje po referenčni trajektoriji 87 3.3.1 Osnovni pristopi k vodenju po referenčni trajektoriji Pri načrtovanju vodenja si najprej predstavljamo referenčno trajektorijo kot referenčno pozicijo, ki se v vsakem računskem koraku regulatorja premakne na trenutno točko referenčne trajektorije ( xref ( t) , yref ( t)). V ta namen uporabimo vodenje do referenčne pozicije z regulacijskima zakonoma (3.10) in (3.11). Pozorni moramo biti na to, da se robot čimbolj približa namišljeni referenčni točki. Pri majhni hitrosti in šumni meritvi pozicije se lahko zgodi, da se meritev pozicije robota znajde pred trajektorijo. Zato je v takšnih situacijah izredno pomembno, da pravilno ukrepamo, npr. z uporabo posodobljenega regulacijskega zakona (3.12). Ta pristop je nekoliko problematičen zaradi dejstva, da je tu povratna zanka bolj obremenjena in so zato potrebna sorazmerno velika ojačenja regulatorja, da bi bili regulacijski pogreški majhni. Posledično je omenjen pristop dovzeten za motnje v regulacijski zanki. Zatorej je koristno vpeljati predkrmiljenje, kar bo predstavljeno v poglavju 3.3.2. Primer 3.7 Trikolesni robot s pogonom na zadnjih kolesih iz primera 3.1 naj bo voden tako, da sledi referenčni trajektoriji xref = 1 , 1+0 , 7 sin( 2 πt ) in y ). 30 ref = 0 , 9 + 0 , 7 sin( 4 πt 30 Začetna lega vozila je [ x(0) , y(0) , ϕ(0)] = [1 , 1 , 0 , 8 , 0]. Napišite dva algoritma vodenja in ju preizkusite na simulaciji kinematičnega modela: • Prvi algoritem naj uporabi osnovna regulacijska zakona (3.10) in (3.11). • Drugi algoritem naj uporabi nadgrajen regulacijski zakon (3.12). Rešitev S spreminjanjem vrednosti spremenljivke UpgradedLaw lahko vodenje preklopimo med t. i. osnovnim načinom, podanim s (3.10) in (3.11), ter nadgrajenim, ki ga podaja (3.12). Rezultati primera 3.7 so prikazani na slikah 3.16 in 3.17. V prikazanem primeru osnovni in nadgrajeni regulacijski zakon delujeta enako. Matlab koda je podana v programu 3.6. Program 3.6 ./src/ctr/example_tracking_simple_control.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 d = 0.1; % Razdalja med prednjo in zadnjo osjo 88 Vodenje kolesnih mobilnih sistemov 4 q = [1.1; 0.8; 0]; % Za č etna lega 5 6 % Referen č na t r a j e k t o r i j a 7 freq = 2* pi /30; 8 xRef = 1.1 + 0.7* sin ( freq * t ); 9 yRef = 0.9 + 0.7* sin (2* freq * t ); 10 11 % Oja č enja r egu lat orj a 12 Kphi = 2; 13 Kv = 5; 14 15 u p g r a d e d C o n t r o l = true ; % Ta nastavitev se lahko spremeni na false 16 for k = 1: length ( t ) 17 % R e f e r e n c a 18 p h i R e f = a t a n 2 ( y R e f ( k ) - q (2) , x R e f ( k ) - q ( 1 ) ) ; 19 q R e f = [ x R e f ( k ); y R e f ( k ); p h i R e f ]; 20 21 % P o g r e š ek g l e d e na t r e n u t n o r e f e r e n č no to č ko 22 e = q R e f - q ; % P o g r e š ek po x , y in k o t u 23 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 24 25 % R e g u l a t o r 26 a l p h a = e ( 3 ) * K p h i ; % R e g u l a c i j a u s m e r i t v e ( o s n o v n a ) 27 v = s q r t ( e ( 1 ) ^ 2 + e ( 2 ) ^ 2 ) * Kv ; % K r m i l j e n j e ( o s n o v n o ) 28 if u p g r a d e d C o n t r o l 29 % % Č e e (3) ni v o b m o č ju [ - pi /2 , pi /2] , je p o t r e b n o pri š t e t i +/ - pi 30 % k e (3) in h i t r o s t m o r a o b r n i t i p r e d z n a k 31 v = v * s i g n ( cos ( e ( 3 ) ) ) ; % S p r e m e m b a p r e d z n a k a h i t r o s t i , č e je p o t r e b n o 32 e (3) = a t a n ( tan ( e ( 3 ) ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 33 a l p h a = e ( 3 ) * K p h i ; % R e g u l a c i j a u s m e r i t v e ( n a d g r a d n j a ) 34 end 35 36 % M e h a n s k e o m e j i t v e r o b o t a 37 if abs ( a l p h a ) > pi /4 , a l p h a = pi /4* s i g n ( a l p h a ); end 38 if abs ( v ) >0.8 , v = 0 . 8 * s i g n ( v ); end 39 40 % S i m u l a c i j a g i b a n j a r o b o t a 41 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; v / d * tan ( a l p h a )]; 42 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 43 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 44 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 45 end 3.3. Vodenje po referenčni trajektoriji 89 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.16: Preprosto vodenje po referenčni trajektoriji Ackermannovega pogona. Črtkana krivulja prikazuje referenčno pot, polna krivulja pa dejansko pot. 0.6 s]/0.4 [mv0.2 00 5 10 15 20 25 30 t [s] 1 0.5 ] [rad 0 α−0.5 −10 5 10 15 20 25 30 t [s] Slika 3.17: Regulirni signali preprostega vodenja po referenčni trajektoriji Ackermannovega pogona 90 Vodenje kolesnih mobilnih sistemov 3.3.2 Razčlenitev vodenja na predkrmiljenje in povratno zanko Vodenje po referenčni trajektoriji je pomembno tako s praktičnega kot teoretič- nega vidika, saj se po Brockettovem pogoju neholonomični sistemi ne morejo asimptotično stabilizirati okoli težišča z uporabo gladke (tj. zvezno odvedljive) časovno nespremenljive povratne zanke [1]. Dobljeni rezultat je mogoče enostavno preveriti na diferencialnem pogonu ter ga razširiti na druge kinematike, vključno z Ackermannovo. Najprej preverimo, ali je sistem brez lezenja (stanja se ne spreminjajo, če ni vzbujanja). Ker je pri diferencialnem pogonu brez vodenja ( v = 0, ω = 0) odvod vektorskega polja ˙ q( t) enak 0, je to sistem brez lezenja. Brockett [1] je dokazal, da mora imeti tak sistem enako število vhodov in stanj, da ga lahko stabiliziramo z uporabo zvezne časovno nespremenljive povratne zanke. V primeru diferencialnega pogona je ta pogoj očitno kršen, zato je potrebno poiskati druge vrste povratnih zank. Kljub temu so popolnoma neholonomični sistemi brez lezenja še vedno vodljivi v nelinearnem smislu, zato je možno izvesti asimptotično stabilizacijo z uporabo časovno spremenljivih, nezveznih ali hibri-dnih regulacijskih zakonov. Omejitvi, ki jo določa Brockettov pogoj, se lahko izognemo z uvedbo drugačne strukture vodenja. V primeru vodenja po referenčni trajektoriji se zelo pogosto uporablja dvoprostostni regulator, kjer en del pripada predkrmiljenju, drugi pa povratni zanki. Pred uvedbo predkrmiljenja in povratne zanke moramo določiti še eno pomembno lastnost sistema. Sistem je diferencialno plosk (angl. differentially flat), če obstaja nabor t. i. ploskih izhodov ter so lahko vsa stanja in vhodi sistema zapisani kot funkcije teh ploskih izhodov in končnega števila njihovih časovnih odvodov. To pomeni, da morata obstajati nelinearni funkciji f x in f u, ki izpolnjujeta d p x = f x(z f , ż f , ¨ z f , . . . z f ) d tp d p u = f u(z f , ż f , ¨ z f , . . . z f ) d tp kjer vektorji x, u in z f predstavljajo stanja sistema, vhode in ploske izhode, medtem ko je p končno celo število. Potrebno je omeniti, da morajo biti ploski izhodi funkcije stanj sistema, njegovih vhodov in končnega števila njihovih (vhodnih) odvodov. To pomeni, da so v splošnem ploski izhodi fiktivni – niso podobni dejanskim izhodom. V primeru kinematičnega modela diferencialnega pogona, ki ga podaja (2.2), sta ploska izhoda dejanska izhoda sistema x in y. Oba vhoda (hitrosti) in 3.3. Vodenje po referenčni trajektoriji 91 tretje stanje (orientacija robota) so lahko predstavljeni kot funkciji x in y ter njuni odvodi. Vemo, da si lahko ˙ x in ˙ y predstavljamo kot kartezični koordinati translatorne hitrosti robota, zato ju uporabimo v izračunu hitrosti s pomočjo Pitagorovega izreka p v( t) = ˙ x 2( t) + ˙ y 2( t) (3.13) Zaradi neholonomičnih omejitev se kolesni robot z diferencialnim pogonom vedno vozi v smeri svoje orientacije, kar pomeni, da je tangenta orientacije enaka količniku kartezičnih komponent translatorne hitrosti ˙ y( t) ϕ( t) = arctan (3.14) ˙ x( t) Kotna hitrost ω( t) je določena kot časovni odvod orientacije ϕ( t), ki jo podaja enačba (3.14) d ˙ y( t) ˙ x( t)¨ y( t) − ˙ y( t)¨ x( t) ω( t) = arctan = (3.15) d t ˙ x( t) ˙ x 2( t) + ˙ y 2( t) in ni definirana le v primeru, ko je translatorna hitrost enaka 0. Kartezični koordinati pozicije x in y sta prav tako ploska izhoda kinematičnega modela kolesnega pogona na zadnje kolo, ki ga podaja enačba (2.15). Prvi dve enačbi tega kinematičnega modela sta enaki kot pri diferencialnem pogonu (2.2), zato lahko orientacijo in hitrost zadnjega kolesa izračunamo s pomočjo (3.13) in (3.14). Tretja enačba v sistemu enačb (2.15) je vr( t) ˙ ϕ = tan ( α( t)) d od koder sledi d ˙ ϕ( t) α( t) = arctan (3.16) vr( t) Vhod α( t) lahko zapišemo z ploskimi izhodi in njihovimi odvodi z vstavitvijo izrazov (3.13) in (3.15) v (3.16) namesto vr( t) in ˙ ϕ( t) d ( ˙ x( t)¨ y( t) − ˙ y( t)¨ x( t)) α( t) = arctan ( ˙ x 2( t) + ˙ y 2( t))(3 / 2) Zgornja analiza nam poda pomembno ugotovitev, da je Ackermannov pogon strukturno enak diferencialnemu. Če je določena regulirna veličina { v( t) , ω( t)} uporabljena na robotu z diferencialnim pogonom, je dobljena trajektorija enaka, kot če bi regulirno veličino ( vr( t) , α( t)) = ( v( t) , arctan dω( t) ) uporabili na v( t) robotu z Ackermannovim pogonom. Večina primerov v nadaljevanju obravnava robota z diferencialnim pogonom. Te rezultate je torej mogoče enostavno razširiti na robote z Ackermannovim pogonom in tudi na nekatere druge kinematične strukture. Če je sistem plosk, lahko vse sistemske spremenljivke izrazimo iz ploskih izhodov brez integracije. Koristna posledica tega dejstva je, da se lahko na podlagi referenčne trajektorije analitično izračunajo zahtevane regulirne veličine. V primeru 92 Vodenje kolesnih mobilnih sistemov kolesnega robota z diferencialnim pogonom enačbi (3.13) in (3.15) podajata formule za izračun referenčnih hitrosti vref ( t) in ωref ( t) iz referenčne trajektorije, ki jo podajata xref ( t) in yref ( t) q vref ( t) = ˙ x 2 ( t) + ˙ y 2 ( t)2 (3.17) ref ref ˙ xref ( t)¨ yref ( t) − ˙ yref ( t)¨ xref ( t) ωref ( t) = (3.18) ˙ x 2 ( t) + ˙ y 2 ( t) ref ref Podobne formule lahko pridobimo tudi za druge kinematične strukture, ki so ploski sistemi. Enačbi (3.17) in (3.18) podajata odprtozančno vodenje, ki zagotavlja, da se v idealnem primeru, kadar kinematični model robota natančno opisuje gibanje ter ni motenj, merskih napak in pogreška začetne lege, robot vozi po referenčni trajektoriji. Teh predpostavk nikoli popolnoma ne izpolnimo, zato je potrebno tudi povratnozančno vodenje. V teh primerih sta referenčni hitrosti iz enačb (3.17) in (3.18) uporabljeni v predkrmiljenju regulacijskega zakona, medtem ko je za povratnozančno vodenje mogoče uporabiti širok spekter regulacijskih zakonov. Nekatere od njih bomo obravnavali tudi v nadaljevanju. 3.3.3 Povratnozančna linearizacija Ideja povratnozančne linearizacije je uvedba transformacije (običajno sistemskega vhoda), ki linearizira sistem med novim vhodom in izhodom. Ker je novi sistem linearen je možna uporaba kateregakoli od obstoječih linearnih načrtovalnih postopkov vodenja. Najprej moramo zagotoviti, da je sistem diferencialno plosk [6, 7]. V razdelku 3.3.2 smo pokazali, da je veliko kinematičnih struktur ploskih. Nato je postopek načrtovanja povratnozančne linearizacije sledeč: • Izbrati moramo ustrezne ploske izhode. Njihovo število naj bo enako številu sistemskih vhodov. • Ploske izhode odvajamo, za dobljene odvode pa je potrebno preveriti funkcijsko odvisnost od vhodov sistema. Ta korak ponavljamo, dokler se vsi vhodi (ali njihovi odvodi) ne pojavijo v odvodih ploskih izhodov. Če lahko iz tega sistema enačb izrazimo vse vhode (natančneje njihove najvišje odvode), lahko preidemo na naslednji korak. • Rešimo sistem enačb za najvišje odvode posameznih vhodov. Za pridobitev dejanskih vhodov sistema je potrebno na njihovih odvodih uporabiti verigo integratorjev. Po drugi strani pa odvodi ploskih izhodov služijo kot novi vhodi v sistem. • Ker je dobljeni sistem linearen, lahko na teh novih vhodih uporabimo širok nabor možnih regulacijskih zakonov. 3.3. Vodenje po referenčni trajektoriji 93 V primeru kolesnega mobilnega robota z diferencialnim pogonom sta ploska izhoda x( t) in y( t). Njun prvi odvod glede na kinematični model (2.2) je ˙ x = v cos ϕ ˙ y = v sin ϕ V prvih odvodih se pojavi le translatorna hitrost v, zato ponovno odvajamo ¨ x = ˙ v cos ϕ − v ˙ ϕ sin ϕ ¨ y = ˙ v sin ϕ + v ˙ ϕ cos ϕ V drugih odvodih pa sta prisotni obe hitrosti ( v in ω = ˙ ϕ). Zdaj je sistem enačb preurejen tako, da so drugi odvodi ploskih izhodov opisani kot funkcije najvišjih odvodov posameznih vhodov (v tem primeru sta to ˙ v in ω) " # " # " # " # ¨ x cos ϕ − v sin ϕ ˙ v ˙ v = = F ¨ y sin ϕ v cos ϕ ω ω Uvedemo matriko F , ki je nesingularna, če je v 6= 0. Sistem enačb je torej mogoče rešiti za ˙ v in ω " # " # " # " # ˙ v ¨ x cos ϕ sin ϕ ¨ x = F −1 = (3.19) ω ¨ y − sin ϕ cos ϕ ¨ y v v Rešitev ω iz enačbe (3.19) je dejanski vhod robota, medtem ko je treba rešitev ˙ v integrirati, preden jo lahko uporabimo kot vhod. Novo pridobljeni linearni sistem ima vhoda [ u 1 , u 2] T = [¨ x, ¨ y] T in stanja z = [ x, y, ˙ x, ˙ y] T (kinematični model (2.2) ima tri stanja, četrto je posledica dodatnega integratorja). Dinamiko novega sistema lahko priročno opišemo z zapisom v prostoru stanj  ˙ x 0 1 0 0  x 0 0 " #  ¨ x 0 0 0 0  ˙ x 1 0 u 1   =     +   (3.20)  ˙ y 0 0 0 1  y 0 0 u         2 ¨ y 0 0 0 0 ˙ y 0 1 ali v matrični obliki kot ż = Az + Bu (3.21) Sistem (3.21) je vodljiv, ker ima matrika vodljivosti h i Q c = B AB (3.22) polni rang in zato regulator stanj obstaja za poljubno izbran karakteristični polinom zaprte zanke. Dodatna zahteva je zasnova regulacijskega zakona, da bo robot sledil referenčni trajektoriji. Pri ploskih sistemih je za ploske izhode podana referenčna trajektorija, v tem primeru je to xref ( t) in yref ( t). Potem je mogoče zlahka pridobiti referenco za stanje sistema z ref ( t) = [ xref , ˙ xref , yref , ˙ yref ] T in vhod sistema u ref = [¨ xref , ¨ xref ] T . Enačbo (3.21) lahko zapišemo tudi za referenčne signale ż ref = Az ref + Bu ref (3.23) 94 Vodenje kolesnih mobilnih sistemov Predkrmiljenje ¨ xref , ¨ yref Referenčna stanja xref , yref , ˙ x v ref , ˙ yref Regulator ˙ v x, y, ϕ F −1 ω Robot K u 1 = ¨ x u 2 = ¨ y x, y, ˙ x, ˙ y Ocenjevalnik stanj Slika 3.18: Povratnozančna linearizacija za sledenje referenci Pogrešek med dejanskimi in referenčnimi stanji je opredeljen kot ˜ z = z − z ref . Če odštejemo (3.23) od (3.21), dobimo ˙˜ z = A ˜ z + B(u − u ref ) (3.24) Enačba (3.24) opisuje dinamiko pogreška stanj. Ta dinamika mora biti stabilna in primerno hitra. Dinamiko zaprte zanke lahko dosežemo s predpisanimi zaprtozančnimi poli. Kot smo že pokazali, je par (A , B) vodljiv in tako lahko s pravilno izbiro konstantne matrike ojačenj regulatorja K (dimenzije 2 × 4) dosežemo poljubne lokacije zaprtozančnih polov na levi strani kompleksne ravnine s. Enačbo (3.24) lahko preuredimo kot ˙˜ z = (A−BK) ˜ z+BK ˜ z+B(u−u ref ) = (A−BK) ˜ z+B(K ˜ z+u−u ref ) (3.25) Če je zadnji člen enačbe (3.25) enak 0, pogreški stanj konvergirajo proti 0 s predpisano dinamiko, ki jo podaja zaprtozančna matrika (A − BK). Da bo zadnji izraz 0, moramo definirati sledeči regulacijski zakon u( t) = K(z ref ( t) − z( t)) + u ref ( t) (3.26) Shematski prikaz celotnega sistema vodenja je podan na sliki 3.18. Parametre regulatorja (matriko ojačenj K) lahko določimo z metodo premikanja polov s pomočjo Ackermannove formule, ki jo najdete v klasičnih knjigah s področja teorije regulacij [8]. Zaradi posebne oblike matrik A in B v (3.20), kjer u 1 vpliva samo na stanji z 1 in z 2 ter u 2 vpliva samo na stanji z 3 in z 4, ima matrika ojačenj regulatorja posebno obliko " # k K = 1 k 2 0 0 0 0 k 3 k 4 Regulacijski zakon (3.26) je torej mogoče popolnoma razčleniti u 1( t) = ¨ x( t) = k 1( xref ( t) − x( t)) + k 2( ˙ xref ( t) − ˙ x( t)) + ¨ xref ( t) (3.27) u 2( t) = ¨ y( t) = k 3( yref ( t) − y( t)) + k 4( ˙ yref ( t) − ˙ y( t)) + ¨ yref ( t) 3.3. Vodenje po referenčni trajektoriji 95 Predlagan pristop zahteva, da so vsa stanja znana. Medtem ko običajno izme- rimo x in y, njihovih odvodov ne. Odvode sicer lahko ocenimo z numeričnim odvajanjem, vendar to povečuje šum in se ga zato v praksi izogibamo. Na voljo sta dve rešitvi: • Neizmerjena stanja lahko ocenijo opazovalniki stanj. • Če izmerimo orientacijo robota ϕ, lahko izračunamo odvode kot ˙ x = v cos ϕ, ˙ y = v sin ϕ. Praktična uporaba tega pristopa je prikazana v primeru 3.8. Primer 3.8 Vodite vozilo z diferencialnim pogonom, da sledi referenčni trajektoriji xref = 1 , 1 + 0 , 7 sin( 2 πt ) in y ). Računski korak je T 30 ref = 0 , 9 + 0 , 7 sin( 4 πt 30 s = 0 , 033 s. Začetna lega je [ x(0) , y(0) , ϕ(0)] = [1 , 1 , 0 , 8 , 0]. V Matlab kodi izvedite algoritem, predstavljen v tem razdelku, in grafično prikažite rezultate. Rešitev Koda je predstavljena v programu 3.7, rezultati primera 3.8 pa so prikazani na slikah 3.19 in 3.20. V tem pristopu se ne pojavijo težave periodične orientacije (ni potrebno preslikati kotov na interval (− π, π]). To izhaja iz dejstva, da se orientacija vedno pojavi znotraj trigonometričnih funkcij, ki so same po sebi periodične. Program 3.7 ./src/ctr/example_tracking_feedback_lin.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 4 % Referenca 5 freq = 2* pi /30; 6 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 7 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 8 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 9 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; 10 uRef = [ ddxRef ; ddyRef ]; 11 12 q = [ xRef (1)+.05; yRef (1) -0.1; 0]; % Za č etna lega 13 z1 = [ q (1); dxRef (1)]; % Za č etno stanje [x , x ’] 14 z2 = [ q (2); dyRef (1)]; % Za č etno stanje [y , y ’] 15 v = sqrt ( z1 (2)^2+ z2 (2)^2); % Za č etno stanje hitrosti 16 17 % Matrike l i n e a r i z i r a n e g a sistema 18 A = [0 , 1; 0 , 0]; B = [0; 1]; C = [1 , 0]; 19 % Regulator stanj 20 desPoles = [ -2 -1 i ; -2+1 i ]; % Ž eleni zaprtozan č ni poli 21 K = acker (A , B , desPoles ); % Oja č enje regularja po metodi premikanja polov 96 Vodenje kolesnih mobilnih sistemov 22 23 for k = 1: length ( t ) 24 % R e f e r e n č na s t a n j a 25 z R e f 1 = [ x R e f ( k ); d x R e f ( k )]; 26 z R e f 2 = [ y R e f ( k ); d y R e f ( k )]; 27 28 % P o g r e š ek in r e g u l a t o r 29 ez1 = z R e f 1 - z1 ; 30 ez2 = z R e f 2 - z2 ; 31 uu = [ d d x R e f ( k ); d d y R e f ( k )] + [ K * ez1 ; K * ez2 ]; 32 33 % I z r a č un r e g u l i r n i h s i g n a l o v 34 F = [ cos ( q (3)) , - v * sin ( q ( 3 ) ) ; ... 35 sin ( q (3)) , v * cos ( q ( 3 ) ) ] ; 36 vv = F \ uu ; % T r a n s l a t o r n i p o s p e š ek in k o t n a h i t o s t 37 v = v + Ts * vv ( 1 ) ; % I n t e g r a c i j a t r a n s l a t o r n e g a p o s p e š ka 38 u = [ v ; vv ( 2 ) ] ; % R e g u l i r n a s i g n a l a 39 40 % S i m u l a c i j a g i b a n j a r o b o t a 41 dq = [ u ( 1 ) * cos ( q ( 3 ) ) ; u ( 1 ) * sin ( q ( 3 ) ) ; u ( 2 ) ] ; 42 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 43 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 44 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 45 46 % I z r a č un s t a n j na p o d l a g i z n a n e ( i z m e r j e n e ) o r i e n t a c i j e in h i t r o s t i 47 z1 = [ q ( 1 ) ; u ( 1 ) * cos ( q ( 3 ) ) ] ; 48 z2 = [ q ( 2 ) ; u ( 1 ) * sin ( q ( 3 ) ) ] ; 49 end 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.19: Vodenje po referenčni trajektoriji diferencialnega pogona na podlagi povratnozančne linearizacije iz primera 3.8 (referenca je označena s črtkano krivuljo) 3.3. Vodenje po referenčni trajektoriji 97 0.4 s] 0 / .3 [mv0.2 0.10 5 10 15 20 25 t [s] 6 s] 4 / 2[rad ω 0 −20 5 10 15 20 25 t [s] Slika 3.20: Regulirni signali vodenja po referenčni trajektoriji diferencialnega pogona na podlagi povratnozančne linearizacije iz primera 3.8 3.3.4 Izpeljava kinematičnega modela pogreška vodenja pri sledenju referenčne trajekto- rije Da rešimo problem vodenja, običajno izvedemo ustrezno transformacijo koordinat robota. Pozicijski pogrešek je ponavadi podan v lokalnem koordinatnem sistemu (robota), poravnan s pogonskim mehanizmom, in izražen kot odstopanje virtu- alnega referenčnega robota od dejanskega robota, kar prikazuje slika 3.21. Na sliki 3.21 so predstavljeni tudi vsi dobljeni pogreški: ex podaja pogrešek v smeri vožnje, ey podaja pogrešek v pravokotni smeri in eϕ podaja pogrešek orientacije. Opisani pristop je bil prvič uporabljen v [9]. Pogrešek lege e( t) = [ ex( t) , ey( t) , eϕ( t)] T je določen z dejansko lego q( t) = [ x( t) , y( t) , ϕ( t)] T resničnega robota in referenčno lego q ref ( t) = [ xref ( t) , yref ( t) , ϕref ( t)] T virtualnega referenčnega robota     ex( t) cos( ϕ( t)) sin( ϕ( t)) 0  e    y ( t) = − sin( ϕ( t)) cos( ϕ( t)) 0 (q ref ( t) − q( t)) (3.28)     eϕ( t) 0 0 1 98 Vodenje kolesnih mobilnih sistemov ( x , y ref ref) j ref ey ex e j j j= r - ef j ( x, y) Slika 3.21: Prikaz pozicijskega pogreška v lokalnih koordinatah Ob predpostavki, da imata dejanski in referenčni robot enak kinematični model, ki ga podaja (2.2), in ob upoštevanju transformacije (3.28), lahko model pogreška lege zapišemo na naslednji način       ˙ ex cos eϕ 0 " # −1 ey vref  ˙ e      y = sin eϕ 0 + 0 − ex u (3.29)     ω   ref ˙ eϕ 0 1 0 −1 kjer sta vref in ωref linearna in kotna referenčna hitrost, podani z (3.17) in (3.18). Regulator določa vhod u = [ v, ω] T . Zelo pogosto [10] je regulirna veličina u razčlenjena kot " # " # v v u ref cos eϕ + vf b = = (3.30) ω ωref + ωfb kjer sta vfb in ωfb povratnozančna (regulacijska) signala, ki bosta določena kasneje, vref cos eϕ in ωref pa sta signala predkrmiljenja, čeprav je tehnično gledano vref cos eϕ moduliran s pogreškom orientacije, ki izvira iz izhoda. Po drugi strani pa vref cos eϕ postane “pravo” predkrmiljenje, ko je pogrešek orientacije enak 0. Če vstavimo regulirno veličino (3.30) v (3.29), dobimo model sledilnega pogreška ˙ ex = ωref ey − vfb + eyωfb ˙ ey = − ωref ex + vref sin eϕ − exωfb (3.31) ˙ eϕ = − ωfb Cilj vodenja je izničiti pogreške modela sledilnega pogreška (3.31) z ustreznima regulirnima veličinama vfb in ωfb. S tem se bomo ukvarjali v nadaljevanju. 3.3.5 Linearni regulator Model pogreška (3.31) je nelinearen. V tem razdelku ga bomo linearizirali, kar omogoča uporabo linearne regulacije. Linearizacija mora potekati okoli 3.3. Vodenje po referenčni trajektoriji 99 ravnotežne točke, zato izberemo točko ničelnega pogreška ( ex = ey = 0 , eϕ = 0), ki je logična ravnotežna točka modela (3.31), če sta tudi obe hitrosti povratne zanke enaki 0 ( vfb = 0, ωfb = 0). Linearizacijo modela (3.31) okoli točke ničelnega pogreška zapišemo v obliki         ˙ ex 0 ωref 0 ex −1 0 " # vfb  ˙ e        y = − ωref 0 vref ey + 0 0 (3.32)         ωfb ˙ eϕ 0 0 0 eϕ 0 −1 Ta linearen sistem je časovno spremenljiv, ker sta vref ( t) in ωref ( t) časovno odvisni. Sistem (3.32) je predstavitev dinamičnega sistemskega pogreška v prostoru stanj, kjer so vsa stanja (v tem primeru pogreški) dostopna. Povratna zanka iz stanj je torej mogoča (rezultira v uspešno vodenje), če je sistem vodljiv. Ob predpostavki, da sta vref in ωref konstantni (referenčna pot je sestavljena iz daljic in krožnih lokov), lahko enostavno dokažemo, da je matrika vodljivosti (3.22) polnega ranga in lahko vse pogreške izničimo z regulatorjem stanj. V primeru da vref in ωref nista konstantni, je sistem še vedno vodljiv, če je katerikoli od referenčnih signalov različen od 0. Tovrstna analiza pa je veliko bolj zapletena. Zaradi posebne strukture sistema (3.32) se pogosto uporablja linearni regulator stanj s preprosto obliko matrike ojačenj   " # " # ex vfb kx 0 0 =  e  y ω   f b 0 ky kϕ eϕ Vidimo, da se pogrešek v smeri vožnje popravi za vfb, medtem ko se pogreški v orientaciji in bočnih smereh popravijo z ωfb. Ojačenja regulatorja ( kx, ky, kϕ) je mogoče določiti s poskušanjem, z njihovo optimizacijo na modelu sistema, z metodo premikanja polov itd. V nadaljevanju so ojačenja regulatorja določena z metodo premikanja polov tako, da poli sistema ležijo na ustreznih lokacijah v kompleksni ravnini s. Sistem ima tri pole, torej je vsaj en pol realen, druga dva pa lahko izberemo, da sta konjugirano kompleksna. Predpostavimo, da so želene lege zaprtozančnih polov s 1 = −2 ζωn in s 2 , 3 = − p ζωn ± ωn 1 − ζ 2. Lastna frekvenca ωn > 0 in koeficient dušenja 0 < ζ < 1 sta parametra, ki ju lahko nastavimo tako, da dosežemo zadovoljivo dušenje in hiter prehodni pojav. Če karakteristični polinom zaprtozančnega sistema     0 ωref 0 −1 0 " # kx 0 0 s I     3×3 − − ωref 0 vref − 0 0     0 k y kϕ 0 0 0 0 −1 primerjamo z želenim ( s + 2 ζωn)( s 2 + 2 ζωns + ω 2 ) n 100 Vodenje kolesnih mobilnih sistemov lahko dobimo rešitev za ojačenja regulatorja [6] kx = kϕ = 2 ζωn ω 2 − ω 2 ( t) (3.33) n ref ky( t) = vref ( t) Upoštevamo, da mora biti ωn večja od največje vrednosti | ωref ( t)|. Ojačenja regulatorja (3.33) se praktično ne uporabljajo, ker ky( t) postane izjemno velik, ko je referenčna hitrost vref ( t) majhna. To težavo odpravimo s časovno spremenljivo lastno frekvenco ωn. Ker je smiselno prilagoditi čas umiritve prehodnega pojava q glede na referenčne hitrosti, se zdi primerna izbira: ωn( t) = ω 2 ( t) + gv 2 ( t), ref ref g > 0. Po ponovitvi podobnega postopka (kot zgoraj), dobimo naslednja ojačenja regulatorja q kx( t) = kϕ( t) = 2 ζ ω 2 ( t) + gv 2 ( t) ref ref ky( t) = gvref ( t) V okviru algoritmov vodenja, predstavljenih v tem poglavju, moramo izpostaviti dve pripombi: • Regulacijski zakoni so zasnovani na podlagi lineariziranih modelov. Line- ariziran model je veljaven le v bližini delovne točke (v tem primeru je to točka ničelnega pogreška) in pri velikih regulacijskih pogreških njegova učinkovitost morda ne bo takšna, kot je bila pričakovana. • Če imamo opravka z linearnim, a časovno spremenljivim sistemom, nekateri rezultati linearnih časovno nespremenljivih sistemov niso več veljavni. Tu je potrebno omeniti, da je sistem morda nestabilen, četudi vsi poli ležijo na (fiksnih) lokacijah na levi strani kompleksne ravnine s. Kljub omenjenim možnim težavam se linearni regulacijski zakoni v praksi pogosto uporabljajo zaradi njihove enostavnosti, razmeroma enostavne prilagoditve ter sprejemljive zmogljivosti in robustnosti. Simulacija uporabe je podana v primeru 3.9. Primer 3.9 Vodite vozilo z diferencialnim pogonom, da sledi referenčni trajektoriji xref = 1 , 1 + 0 , 7 sin( 2 πt ) in y ). Računski korak je T 30 ref = 0 , 9 + 0 , 7 sin( 4 πt 30 s = 0 , 033 s, začetna lega pa [ x(0) , y(0) , ϕ(0)] = [1 , 1 , 0 , 8 , 0]. Zapišite predstavljen algoritem z ojačenji regulatorja, ki jih podaja (3.33), in grafično prikažite rezultate. 3.3. Vodenje po referenčni trajektoriji 101 Rešitev Matlab koda je podana v programu 3.8. Rezultati simulacije so prikazani na slikah 3.22 in 3.23, kjer je prikazano dobro sledenje. Program 3.8 ./src/ctr/example_tracking_linear_control.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Reference trajectory 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 for k = 1: length ( t ) 16 e = [ cos ( q (3)) , sin ( q (3)) , 0; ... 17 - sin ( q (3)) , cos ( q (3)) , 0; ... 18 0 , 0 , 1 ] * ( q R e f (: , k ) - q ); % V e k t o r p o g r e š ka 19 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 20 21 % T r e n u t n i r e f e r e n č ni v h o d i 22 v R e f = u R e f (1 , k ); 23 w R e f = u R e f (2 , k ); 24 25 % R e g u l a t o r 26 eX = e ( 1 ) ; eY = e ( 2 ) ; e P h i = e ( 3 ) ; 27 z e t a = 0 . 9 ; % P a r a m e t e r za n a s t a v l j a n j e 28 g = 85; % P a r a m e t e r za n a s t a v l j a n j e 29 Kx = 2* z e t a * s q r t ( w R e f ^2+ g * v R e f ^ 2 ) ; 30 K p h i = Kx ; 31 Ky = g * v R e f ; 32 % Oja č e n j a so l a h k o t u d i k o n s t a n t n a , npr .: Kx = K p h i = 3; Ky = 30; 33 34 % R e g u l a t o r : k r m i l j e n j e in r e g u l a c i j a 35 v = v R e f * cos ( e ( 3 ) ) + Kx * e ( 1 ) ; 36 w = w R e f + Ky * e (2) + K p h i * e ( 3 ) ; 37 38 % S i m u l a c i j a g i b a n j a r o b o t a 39 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 40 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 41 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 42 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 43 end 102 Vodenje kolesnih mobilnih sistemov 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.22: Vodenje diferencialnega pogona po referenčni trajektoriji iz primera 3.9 (referenca je označena s črtkano krivuljo) 0.8 0.6 s]/ [m 0.4 v 0.2 00 5 10 15 20 25 t [s] 10 s]/ 5 [rad ω 0 0 5 10 15 20 25 t [s] Slika 3.23: Regulirni signali vodenja diferencialnega pogona po referenčni trajektoriji iz primera 3.9 3.3. Vodenje po referenčni trajektoriji 103 3.3.6 Načrtovanje vodenja na osnovi funkcij Lja- punova Kot smo že omenili, je model pogreška (3.31) sam po sebi nelinearen. Nelinearne sisteme je najbolje voditi z nelinearnim regulatorjem, ki med načrtovanjem vodenja upošteva vse lastnosti sistema. Teorija, ki temelji na funkcijah Ljapunova, se pogosto uporablja za reševanje težav pri stabilizaciji nelinearnega sistema. V našem primeru bomo (asimptotično) stabilnost modela pogreška (3.31) analizirali glede na različne regulacijske zakone. Stabilnost Ljapunova Na kratko je predstavljena druga metoda Ljapunova, ki zagotavlja zadostne pogoje za (asimptotično) stabilnost ravnotežnih točk nelinearnega dinamičnega sistema ˙ x = f (x), x ∈ R. Najprej predpostavimo, da ravnotežje leži v x = 0. Pristop temelji na pozitivno definitnih skalarnih funkcijah V (x) : n R → R, za katere velja V (x) = 0, če je x = 0, in V (x) > 0, ko je x 6= 0. Stabilnost ravnotežne točke preverimo z odvodom funkcije V . Pomembno je, da dobimo odvod kot rešitev diferencialne enačbe sistema ˙ ∂V ∂V V = ˙ x = f (x) ∂ x ∂ x Če velja ˙ V ≤ 0 ( ˙ V je negativno semidefinitna funkcija), je ravnotežje (lokalno) stabilno. Če pa velja ˙ V < 0, razen pri x = 0 ( ˙ V je negativno definitna funkcija), je ravnotežje (lokalno) asimptotično stabilno. Ko je lim|x|→∞ V (x) = ∞, so rezultati globalni. Zato pristop temelji na iskanju funkcij z navedenimi lastnostmi, ki jih imenujemo funkcije Ljapunova. Za kandidata običajno izberemo kvadratno funkcijo Ljapunova in če je možno pokazati, da je njen odvod negativen ali vsaj nič, je sistem stabilen. Klasična razlaga funkcij Ljapunova temelji na energiji sistema. Če se energija disipativnega sistema uporablja kot funkcija Ljapunova, se njegova energija ne more povečati (odvod funkcije ni pozitiven). Posledično ostanejo vsi signali omejeni in lahko potrdimo stabilnost sistema. Vendar je potrebno poudariti, da funkcija Ljapunova morda ni povezana z energijo sistema. Predvsem pa je v okviru nelinearnosti “stabilnost sistema” napačen termin. Namesto tega je potrebno analizirati stabilnost ravnotežnih točk ali bolj splošno stabilnost invariantnih množic. Možno je najti sisteme, v katerih obstajajo tako stabilne kot nestabilne ravnotežne točke. 104 Vodenje kolesnih mobilnih sistemov Načrtovanje vodenja v okviru stabilnosti Ljapunova V nadaljevanju bomo pokazali, kako lahko teorem stabilnosti Ljapunova upo- rabimo za namen načrtovanja vodenja. Naš nelinearni sistem (3.31) ima tri stanja z ravnotežno točko pri e = 0. Želimo oblikovati vodenje, ki bo to točko stabiliziralo; če je možno, naj bo točka asimptotično stabilna, kar pomeni, da bi se vse trajektorije sčasoma približale referenčni in tam ostale za vedno. Najočitnejši kandidat za funkcijo Ljapunova je vsota treh kvadratov pogreškov ky 1 V (e) = ( e 2 + e 2) + e 2 2 x y 2 ϕ to si lahko razlagamo kot uravnoteženo vsoto kvadratov pogreškov razdalje in orientacije. Zaradi različnih enot moramo dodati pozitivno konstanto ky, vendar se bo pozneje pokazalo, da ta konstanta igra pomembno vlogo pri zasnovi regulacijskega zakona. Časovni odvod funkcije V je ˙ V = kyex ˙ ex + kyey ˙ ey + eϕ ˙ eϕ vendar je potrebno ta odvod ovrednotiti glede na rešitve modela (3.31), kar pomeni, da moramo vpeljati odvode pogreška iz (3.31) ˙ V (e) = kyex ( ωref ey − vfb + eyωfb) + + kyey (− ωref ex + vref sin eϕ − exωfb) + eϕ (− ωfb) (3.34) = − kyexvfb + kyvref ey sin eϕ − eϕωfb Osnovna ideja vodenja, ki temelji na metodi Ljapunova, je ustrezna izbira regulacijskega zakona, ki zagotovi, da je odvod funkcije Ljapunova negativen. V tem primeru je precej očitno, kako lahko izvedemo regulacijski algoritem. Člen − kyexvfb v (3.34) bo negativen, če bo linearna hitrost vfb proporcionalna pogrešku ex, saj je kvadrat pogreška e 2 pozitiven. Podobno bo člen − e x ϕωf b v (3.34) vedno negativen, če bo kotna hitrost ωfb proporcionalna pogrešku eϕ. S primerno modifikacijo kotne hitroste ωfb lahko dosežemo še, da izničimo člen kyvref ey sin eϕ v (3.34). S tem zagotovimo, da je odvod funkcije Ljapunova (3.34) negativen. Regulacijski zakon, ki to izpolnjuje, je vfb = kxex sin e (3.35) ϕ ωfb = kyvref ey + kϕeϕ eϕ Ta regulacijski zakon je dobro znan in uveljavljen [6, 11]. Uvedemo predlagano vodenje (3.35) in ˙ V postane ˙ V = − kxkye 2 − k (3.36) x ϕe 2 ϕ Ojačenja vodenja so pozitivna, kasneje pa se bo pokazalo, da sta lahko kx in kϕ poljubni enakomerno zvezni pozitivni funkciji, medtem ko mora biti ky pozitivna konstanta. Odvod funkcije Ljapunova očitno ni pozitiven, ker pa je ocenjen na nič pri ex = 0, eϕ = 0, ne glede na ey, je odvod negativen in semidefiniten; tudi 3.3. Vodenje po referenčni trajektoriji 105 ravnotežje je stabilno. To pomeni, da bo pogrešek ostal omejen, vendar nismo dokazali njegove konvergence proti 0. Analiza konvergence pogreška je bistveno težja, zato moramo uvesti nekaj doda- tnih matematičnih orodij. Pomembno vlogo bodo igrale norme signalov. Norma L p funkcije x( t) je definirana kot Z ∞ 1 /p k x k = | x( τ )| p d τ p 0 kjer je | · | (skalarna) dolžina vektorja. Če zgornji integral obstaja (je končen), funkcija x( t) pripada L p. Omejitev p na neskončnost zagotavlja zelo pomemben razred funkcij L∞, t. i. omejene funkcije. Za dokazovanje stabilnosti regulacijskih zakonov bomo uporabili dve zelo znani lemi. Prva je Barbălatova lema, druga pa je njena izpeljava. Obe lemi sta vzeti iz [12]. Lema 3.1 (Barbălatova lema). Če lim R t t→∞ f ( τ ) d τ obstaja in je končna ter 0 je f ( t) enakomerno zvezna funkcija, potem velja lim t→∞ f ( t) = 0 . Lema 3.2. Če velja f, ˙ f ∈ L∞ in f ∈ L p za določene p ∈ [1 , ∞) , potem f ( t) → 0 ko t → ∞ . Zdaj smo pripravljeni obravnavati problem konvergence pogreška v (3.31). Zaradi (3.36) je ˙ V ≤ 0, zato funkcija Ljapunova ne narašča in ima limito lim t→∞ V ( t). Posledično so stanja modela (3.31) omejena ex, ey, eϕ ∈ L∞ Poleg tega iz (3.35) izhaja, da so regulirni signali omejeni, iz (3.31) pa da so omejeni odvodi pogreškov vfb, ωfb, ˙ ex, ˙ ey, ˙ eϕ ∈ L∞ kjer smo upoštevali, da so vref , ωref , kx in kϕ omejeni. Slednje velja v primeru ploskih referenčnih trajektorij ( xref , yref , ϕref ). Da dokažemo asimptotično stabilnost modela (3.31), najprej izračunamo integral ˙ V iz (3.36) Z ∞ Z ∞ Z ∞ ˙ V dt = V (∞) − V (0) = − kxkye 2 d t − k d t x ϕe 2 ϕ 0 0 0 Ker je V pozitivno definitna funkcija, velja naslednja neenakost Z ∞ Z ∞ Z ∞ Z ∞ V (0) ≥ kxkye 2 d t + k d t ≥ k k e 2 d t + k e 2 d t x ϕe 2 ϕ x y x ϕ ϕ 0 0 0 0 kjer sta uvedeni spodnji meji funkcij kx( t) in kϕ( t) kx( t) ≥ k > 0 x kϕ( t) ≥ k > 0 ϕ 106 Vodenje kolesnih mobilnih sistemov Iz (3.3.6) izhaja, da pogreška ex( t) in eϕ( t) pripadata L2. Na podlagi leme 3.2 je možno enostavno pokazati, da pogreška ex( t) in eϕ( t) konvergirata proti 0. Ker obstaja limita lim t→∞ V ( t), potem obstaja tudi lim t→∞ ey( t). Videli smo, da je razmeroma enostavno prikazati konvergenco pogreškov ex( t) in eϕ( t) proti 0. Tudi pogoji za konvergenco so dokaj blagi – ojačenja regulatorja in referenčne trajektorije morajo biti omejeni. Konvergenco ey proti 0 pa je težje dokazati, saj so zahteve veliko težje dosegljive, kot bo prikazano v nadaljevanju. Poleg tega, da so ojačenja regulatorja enakomerno zvezna, morajo biti referenčne hitrosti neprestano vzbujene, torej vref in ωref ne smeta limitirati proti 0. Zato bomo obravnavali dva primera. V prvem predpostavimo vref 9 0, v drugem pa ωref 9 0. Predpostavimo, da je lim t→∞ vref ( t) 6= 0. Uporaba leme 3.1 na ˙ eϕ( t) iz (3.31) zagotavlja, da lim t→∞ ˙ eϕ( t) = 0, saj limita lim t→∞ eϕ( t) obstaja in je končna, odvod ˙ eϕ( t) pa je enakomerno zvezen. Slednje velja zaradi (3.31), če je ωfb enakomerno zvezna. Enakomerno zveznost funkcije f ( t) na [0 , ∞) preverimo tako, da pogledamo, ali velja f, ˙ f ∈ L∞. Prej smo dokazali, da sta ey in eϕ enakomerno zvezna, medtem ko sta ojačenje regulatorja kϕ in referenčna hitrost vref enakomerno zvezna ob predpostavki iz (3.35), da je odvod ˙ eϕ( t) tudi enakomerno zvezen. Tako smo dokazali, da lim t→∞ ˙ ey( t) = 0 velja (kar je enako lim t→∞ ωfb( t) = 0). Konvergenca ey proti 0 izhaja iz (3.35) e sin eϕ ϕ → 0 , kϕ ∈ L∞ , ωf b → 0 ⇒ ky vref e e y → 0 ϕ k sin eϕ y vref e → 1 , k e y → 0 , sin eϕ y > 0 , vref → 0 ⇒ ey → 0 ϕ eϕ Zdaj predpostavimo lim t→∞ ωref ( t) 6= 0. Spet je potrebno zagotoviti, da velja lim t→∞ ωfb = 0. Kot smo že pokazali, to drži, če sta vref in kϕ enakomerno zvezna. Nato se Barbălatova lema (lema 3.1) uporabi na ˙ ex v (3.31). Za ex, ey in ωfb smo tudi že pokazali, da so enakomerno zvezni. Ob predpostavki, da je kx enakomerno zvezen, sta tudi vfb in ωref enakomerno zvezni. To dokazuje izraz lim t→∞ ˙ ex( t) = 0. Podobno lahko sklepamo, da zadnja dva izraza v enačbi (3.31) za ˙ ex limitirata proti 0, ko gre t proti neskončnosti. Posledično gre tudi produkt ωref ey proti 0. Ker je ωref neprestano vzbujena in ne gre proti 0, mora iti ey proti 0. Še enkrat je potrebno poudariti, da je za konvergenco ex in eϕ potrebna le omejenost vref ali ωref . Precej težja naloga je voditi ey na 0. To dosežemo z neprestanim vzbujanjem vref ali ωref . Vsi rezultati so veljavni globalno, kar pomeni, da je konvergenca zagotovljena ne glede na začetno lego. Primer 3.10 Vodite vozilo z diferencialnim pogonom, da sledi referenčni trajektoriji xref = 1 , 1 + 0 , 7 sin( 2 πt ) in y ). Računski korak je T 30 ref = 0 , 9 + 0 , 7 sin( 4 πt 30 s = 0 , 033 s, začetna lega pa je [ x(0) , y(0) , ϕ(0)] = [1 , 1 , 0 , 8 , 0]. V Matlab kodi izvedite 3.3. Vodenje po referenčni trajektoriji 107 predstavljeni algoritem vodenja, preizkusite različna ojačenja in grafično prikažite rezultate. Rešitev Koda je podana v programu 3.9. Rezultati simulacije primera 3.10 so prikazani na slikah 3.24 in 3.25, kjer je ponazorjeno dobro sledenje. Upoštevamo, da se za kx( t) in kϕ( t) lahko uporabi poljubna pozitivna funkcija. V tem primeru smo izbrali takšne funkcije, da dobimo enak linearni model sistema kot v primeru linearnega regulatorja (primer 3.9). Regulacijski zakoni niso enaki razen v mejnih primerih ( eϕ → 0). Tako je oblika prehoda podobna referenčni trajektoriji, ne glede na referenčne hitrosti. Program 3.9 ./src/ctr/example_tracking_nonlinear_control.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na t r a j e k t o r i j a 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 for k = 1: length ( t ) 16 e = [ cos ( q (3)) , sin ( q (3)) , 0; ... 17 - sin ( q (3)) , cos ( q (3)) , 0; ... 18 0 , 0 , 1 ] * ( q R e f (: , k ) - q ); % V e k t o r p o g r e š ka 19 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 20 21 % T r e n u t n i r e f e r e n č ni v h o d i 22 v R e f = u R e f (1 , k ); 23 w R e f = u R e f (2 , k ); 24 25 % R e g u l a t o r 26 z e t a = 0 . 9 ; % P a r a m e t e r za n a s t a v l j a n j e 27 g = 85; % P a r a m e t e r za n a s t a v l j a n j e 28 Kx = 2* z e t a * s q r t ( w R e f ^2 + g * v R e f ^ 2 ) ; 29 K p h i = Kx ; 30 Ky = g ; 31 % Oja č e n j i Kx in K p h i sta l a h k o t u d i k o n s t a n t n i . 32 % Ta o b l i k a o m o g o č a , da je du š e n j e v p r e h o d n e m p o j a v u 33 % n e o d v i s n o od r e f e r e n č nih h i t r o s t i . 34 35 % R e g u l a t o r : k r m i l j e n j e in r e g u l a c i j a 36 v = v R e f * cos ( e ( 3 ) ) + Kx * e ( 1 ) ; 37 w = w R e f + Ky * v R e f * s i n c ( e ( 3 ) / pi )* e (2) + K p h i * e ( 3 ) ; 38 39 % S i m u l a c i j a g i b a n j a r o b o t a 108 Vodenje kolesnih mobilnih sistemov 40 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 41 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 42 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 43 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 44 end 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.24: Nelinearno vodenje po referenčni trajektoriji vozila z diferencialnim pogonom iz primera 3.9 (referenca je označena s črtkano krivuljo) 3.3. Vodenje po referenčni trajektoriji 109 0.8 0.6 s]/ [m 0.4 v 0.2 00 5 10 15 20 25 t [s] 10 s]/ 5 [rad ω 0 0 5 10 15 20 25 t [s] Slika 3.25: Regulirni signalni nelinearnega vodenja po referenčni trajektoriji vozila z diferencialnim pogonom iz primera 3.9 Razvoj periodičnega regulacijskega zakona Težava sledenja je očitno periodičnost glede na orientacijo. To je mogoče opaziti iz kinematičnega modela z uporabo poljubne regulirne veličine in poljubnega začetnega pogoja, ki podajata določeno trajektorijo robota. Če isto regulirno veličino uporabimo na dejanskem robotu in se začetni pogoj razlikuje od prejšnjega samo za večkratnik 2 π, dobimo enak odziv za x( t) in y( t); tudi ϕ( t) se od prejšnje rešitve razlikuje za isti večkratnik 2 π. Periodična narava se mora odražati tudi v uporabljenemu regulacijskemu zakonu. To pomeni, da moramo poiskati tak regulacijski zakon, ki je periodičen glede na pogrešek orientacije eϕ (perioda je 2 π) in zagotavlja, da je konvergenca pogreška lege e enaka [0 0 2 kπ] T ( k ∈ Z). Tako zmanjšamo vse običajne probleme s preslikavo orientacije na (− π, π]. Ti problemi lahko v določenih aplikacijah postanejo kritični okoli ±180°, npr. pri uporabi opazovalnika za oceno lege robota iz zakasnelih meritev. Velja opomniti, da so določeni regulacijski zakoni periodični v smislu predhodne diskusije, npr. regulacijski zakon povratnozančne linearizacije, ki ga podajata (3.19) in (3.27). Očitno bi morale biti funkcije, uporabljene v razdelku o analizi konvergence, periodične tudi glede na eϕ. To pomeni, da imajo te funkcije več lokalnih minimumov in zato ne izpolnjujejo pogojev za klasične funkcije Ljapunova. Čeprav je analiza 110 Vodenje kolesnih mobilnih sistemov stabilnosti podobna direktni metodi Ljapunova (druga metoda Ljapunova), s to teorijo stabilnosti konvergenca ni dokazana, ker pri našem pristopu ni potrebno, da e konvergira proti nič. Kljub temu bomo v tem poglavju funkcije, uporabljene za analizo konvergence, še vedno imenovali “funkcije Ljapunova”. Naš cilj je spraviti pozicijski pogrešek na nič, medtem ko pogrešek orientacije konvergira proti poljubnemu večkratniku 2 π. Da to dosežemo, bomo uporabili funkcijo Ljapunova, ki je periodična glede na eϕ (z osnovno periodo 2 π). Najprej bo koncept prikazan na eni funkciji Ljapunova, kasneje pa ga bomo posplošili. Prvi kandidat za funkcijo Ljapunova je izbran kot 2 tan eϕ V = ky e 2 + e 2 + 1 2 2 x y 2 1 2 kjer je ky pozitivna konstanta. Njen odvod, upoštevajoč enačbe (3.31), pa je ˙ V = kyex ( ωref ey − vfb + eyωfb) + tan eϕ + k 2 y ey (− ωref ex + vref sin eϕ − exωf b) − 2 ωfb cos2 eϕ (3.37) 2 tan eϕ = − k 2 y exvf b + ky vref ey sin eϕ − 2 ωfb cos2 eϕ 2 Če uporabimo regulacijski zakon vfb = kxex eϕ ωfb = kyvref ey cos4 + kϕ sin eϕ 2 kjer sta kx in kϕ pozitivno omejeni funkciji, je odvod ˙ V iz enačbe (3.37) enak 2 tan eϕ ˙ V = − k 2 xky e 2 − k x ϕ 1 2 Nato lahko sledimo istim korakom kot v analizi regulacijskega zakona (3.35) in ugotovimo, da ex in tan eϕ konvergirata proti 0 (to pomeni, da velja e 2 ϕ → 2 kπ, k ∈ Z) v primeru omejenih ojačenj regulatorja in omejene trajektorije. Konvergenca ey proti 0 se lahko zaključi tudi po dolgotrajni analizi, če so izpolnjeni isti pogoji kot v primeru regulacijskega zakona (3.35). Primer 3.11 Vodite vozilo z diferencialnim pogonom, da sledi referenčni trajektoriji xref = 1 , 1+ 0 , 7 sin( 2 πt ) in y ). Računski korak je T 30 ref = 0 , 9 + 0 , 7 sin( 4 πt 30 s = 0 , 033 s, začetna lega pa [ x(0) , y(0) , ϕ(0)] = [1 , 1 , 0 , 8 , 0]. V Matlab kodi izvedite predstavljeni algoritem vodenja, preizkusite različna ojačenja regulatorja in grafično prikažite rezultate. 3.3. Vodenje po referenčni trajektoriji 111 Rešitev Koda je predstavljena v programu 3.10. Rezultati simulacije so prikazani na slikah 3.26 in 3.27, kjer je ponazorjeno dobro sledenje. Program 3.10 ./src/ctr/example_tracking_periodic_control.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na t r a j e k t o r i j a 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 for k = 1: length ( t ) 16 e = [ cos ( q (3)) , sin ( q (3)) , 0; ... 17 - sin ( q (3)) , cos ( q (3)) , 0; ... 18 0 , 0 , 1 ] * ( q R e f (: , k ) - q ); % V e k t o r p o g r e š ka 19 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 20 21 % T r e n u t n i r e f e r e n č ni v h o d i 22 v R e f = u R e f (1 , k ); 23 w R e f = u R e f (2 , k ); 24 25 % R e g u l a t o r 26 eX = e ( 1 ) ; eY = e ( 2 ) ; e P h i = e ( 3 ) ; 27 z e t a = 0 . 9 ; % P a r a m e t e r za n a s t a v l j a n j e 28 g = 85; % P a r a m e t e r za n a s t a v l j a n j e 29 Kx = 2* z e t a * s q r t ( w R e f ^2+ g * v R e f ^ 2 ) ; 30 K p h i = Kx ; 31 Ky = g ; 32 % Oja č e n j i Kx in K p h i sta l a h k o t u d i k o n s t a n t n i . 33 % Ta o b l i k a o m o g o č a , da je du š e n j e v p r e h o d n e m p o j a v u 34 % n e o d v i s n o od r e f e r e n č nih h i t r o s t i . 35 36 % R e g u l a t o r : k r m i l j e n j e in r e g u l a c i j a 37 v = v R e f * cos ( e ( 3 ) ) + Kx * eX ; 38 w = w R e f + Ky * v R e f *( cos ( e P h i / 2 ) ) ^ 4 * eY + K p h i * sin ( e P h i ); 39 40 % S i m u l a c i j a g i b a n j a r o b o t a 41 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 42 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 43 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 44 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 45 end 112 Vodenje kolesnih mobilnih sistemov 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.26: Nelinearno vodenje po referenčni trajektoriji vozila z diferencialnim pogonom iz primera 3.11 (referenca je označena s črtkano krivuljo) 0.8 0.6 s]/ [m 0.4 v 0.2 00 5 10 15 20 25 t [s] 6 s] 4 / [rad 2 ω 0 0 5 10 15 20 25 t [s] Slika 3.27: Regulirni signali nelinearnega vodenja po referenčni trajektoriji vozila z diferencialnim pogonom iz primera 3.11 3.3. Vodenje po referenčni trajektoriji 113 Dobro znan regulacijski zakon Kanayama [10] lahko analiziramo v predlaganem okvirju. Rezultat izbire funkcije Ljapunova e sin ϕ 2 V = ky e 2 + e 2 + 1 2 2 x y 2 1 2 in uporabe regulacijskega zakona, predlaganega v [10] (v drugem izrazu za ωfb je bil tudi tretji faktor | vref |, ki ga lahko vključimo v kϕ) vfb = kxex (3.38) ωfb = kvref ey + kϕ sin eϕ je stabilen sistem pogreškov, kjer se lahko konvergenca vseh pogreškov prikaže pod enakimi pogoji kot prej. Upoštevamo, da poleg stabilnih ravnotežij pri eϕ = 2 kπ, k ∈ Z obstaja tudi nestabilno (odbijajoče se) ravnotežje pri eϕ = (2 k + 1) π, k ∈ Z. Okvir za zasnovo periodičnega zakona vodenja je predstavljen v [13]. Velja omeniti, da je precej preprosto razširiti predlagane tehnike na zasnovo vodenja za simetrična vozila, ki se lahko med normalnim delovanjem premikajo naprej in nazaj. V tem primeru morajo biti funkcije Ljapunova periodične s periodo π na eϕ. Model pogreška sistema s štirimi stanji Zdaj se bomo lotili istega problema kot v prejšnjem poglavju. Z vidika vodenja pogosto želimo slediti vsaki legi robota, ki se razlikuje od referenčne za večkratnik kota 360°. Model (3.31) ne olajša omenjenega problema, ker je običajno potrebno pogrešek orientacije izničiti z uporabo (3.31). V tem poglavju je predstavljen kinematični model sistema, kjer so vse lege, ki se v orientaciji razlikujejo za večkratnik kota 360°, predstavljene kot ena lega. To lahko dosežemo z razširitvijo vektorja stanj za en element. Spremenljivko ϕ( t) iz prvotnega kinematičnega modela (2.2) zamenjata dve novi spremenljivki s( t) = sin( ϕ( t)) in c( t) = cos( ϕ( t)). Njuna odvoda sta ˙ s( t) = cos( ϕ( t)) ˙ ϕ( t) = c( t) ω( t) ˙ c( t) = − sin( ϕ( t)) ˙ ϕ( t) = − s( t) ω( t) Tako dobimo nov kinematični model  ˙ x  c 0  " #  ˙ y  s 0  v ˙ q =   =    ˙ s 0 c  ω     ˙ c 0 − s 114 Vodenje kolesnih mobilnih sistemov Novi pogreški so opredeljeni kot ex = c( xref − x) + s( yref − y) ey = − s( xref − x) + c( yref − y) (3.39) es = sin( ϕref − ϕ) = c sin ϕref − s cos ϕref ecos = cos( ϕref − ϕ) = c cos ϕref + s sin ϕref Po odvajanju enačb (3.39) in nekaj manipulacij dobimo naslednji sistem ˙ ex = vref ecos − v + eyω ˙ ey = vref es − exω ˙ es = ωref ecos − ecosω ˙ ecos = − ωref es + esω Tako kot v (3.30) bosta tudi v tem regulacijskem zakonu uporabljena v = vref ecos + vfb in ω = ωref + ωfb. Cilj vodenja je voditi ex, ey in es proti 0. Spremenljivka ecos je pridobljena kot kosinus pogreška orientacije in jo je treba voditi proti 1. Zato bo nov pogrešek definiran kot ec = ecos − 1 in tako je končni model sistemskega pogreška ˙ ex = ωref ey − vfb + eyωfb ˙ ey = − ωref ex + vref es − exωfb (3.40) ˙ es = − ecωfb − ωfb ˙ ec = esωfb Na podlagi pristopa Ljapunova bomo razvili regulator, ki doseže asimptotično stabilnost modela pogreška (3.40). Zelo neposredna ideja je uporaba sledeče funkcije Ljapunova k 1 V 0 = e 2 + e 2 + e 2 + e 2 (3.41) 2 x y 2 s c Zanimivo, ta funkcija Ljapunova vodi do regulacijskega zakona (3.38). Vendar bo tukaj predlagana nekoliko bolj kompleksna funkcija, ki kot poseben primer vključuje tudi funkcijo (3.41). Za doseg cilja vodenja je predlagan naslednji kandidat za funkcijo Ljapunova k 1 V = e 2 + e 2 + e 2 + e 2 (3.42) 2 x y 2 1 + ec s c a kjer sta k > 0 in a > 2 konstanti. Upoštevamo, da je [−2 , 0] območje funkcije ec = cos( ϕref − ϕ) − 1 in zato a − 2 ec 0 < ≤ 1 + ≤ 1 a a 1 a (3.43) 1 ≤ ≤ 1 + ec a − 2 a Zaradi (3.43) je funkcija V 0 (3.41) spodnja meja funkcije V (3.42), pa tudi V izpolnjuje pogoje za funkcijo Ljapunova. Vloga člena (1 + ec ) bo pojasnjena a kasneje. Funkcijo V lahko poenostavimo na naslednji način e 2 + e 2 = e 2 + ( e s c s cos − 1)2 = 2 − 2 ecos = −2 ec (3.44) 3.3. Vodenje po referenčni trajektoriji 115 Upoštevajoč enačbe modela pogreška (3.40) in (3.44), je odvod V (3.42) enak − 1 e ˙ 1 sωf b(−2 ec) V = − ke a xvf b + kvref ey es + (−2 esωfb) + 2 1 + ec 2 a 2 1 + ec a ! ωfb = − kexvfb + es kvref ey − 1 + ec 2 a Da bo ˙ V negativno semidefinitna, predlagamo sledeči regulacijski zakon vfb = kxex ec 2 ec 2 n (3.45) ωfb = kvref ey 1 + + kses 1 + a a kjer sta funkciji kx( t) in ks( t) pozitivni za n ∈ Z. Iz praktičnih razlogov je n majhno število (običajno izberemo −2, −1, 0, 1 ali 2). Z upoštevanjem regulacijskega zakona (3.45) postane funkcija ˙ V ˙ ec 2 n−1 V = − kkxe 2 − k 1 + (3.46) x se 2 s a Ponovno je preprosto prikazati konvergenco ex in es na podlagi (3.46). Prikaz konvergenc ey in ec pa je spet nekoliko zahtevnejši [14]. Primer 3.12 Vodite vozilo z diferencialnim pogonom, da sledi referenčni trajektoriji xref = 1 , 1 + 0 , 7 sin( 2 πt ) in y ). Računski korak je T 30 ref = 0 , 9 + 0 , 7 sin( 4 πt 30 s = 0 , 033 s, začetna lega pa je [ x(0) , y(0) , ϕ(0)] = [1 , 1 , 0 , 8 , 0]. V Matlab kodi izvedite predstavljeni algoritem vodenja ter preizkusite različna ojačenja in dodatne parametre vodenja ( a in n). Rešitev Matlab koda je navedena v programu 3.11. Rezultati simulacije so prikazani na slikah 3.28 in 3.29, kjer je predstavljeno dobro sledenje. Program 3.11 ./src/ctr/example_tracking_four_state_control.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 116 Vodenje kolesnih mobilnih sistemov 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na lega 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 for k = 1: length ( t ) 16 e = [ cos ( q (3)) , sin ( q (3)) , 0; ... 17 - sin ( q (3)) , cos ( q (3)) , 0; ... 18 0 , 0 , 1 ] * ( q R e f (: , k ) - q ); % V e k t o r p o g r e š ka 19 eX = e ( 1 ) ; eY = e ( 2 ) ; % P o g r e š ek po r a z d a l j i 20 eS = sin ( e ( 3 ) ) ; e C o s = cos ( e ( 3 ) ) ; eC = e C o s - 1; % K o t n i p o g r e š ek 21 22 % T r e n u t n i r e f e r e n č ni v h o d i 23 v R e f = u R e f (1 , k ); 24 w R e f = u R e f (2 , k ); 25 26 % R e g u l a t o r 27 z e t a = 0 . 9 ; % P a r a m e t e r za n a s t a v l j a n j e 28 g = 85; % P a r a m e t e r za n a s t a v l j a n j e 29 a = 10; % P a r a m e t e r za n a s t a v l j a n j e 30 n = 2; % P a r a m e t e r za n a s t a v l j a n j e ( c e l o š t e v i l o ) 31 Kx = 2* z e t a * s q r t ( w R e f ^2+ g * v R e f ^ 2 ) ; 32 Ks = Kx ; 33 K = g ; 34 % Oja č e n j i Kx in Ks sta l a h k o t u d i k o n s t a n t n i . 35 % Ta o b l i k a o m o g o č a , da je du š e n j e v p r e h o d n e m p o j a v u 36 % n e o d v i s n o od r e f e r e n č nih h i t r o s t i . 37 38 % R e g u l a t o r : k r m i l j e n j e in r e g u l a c i j a 39 v = v R e f * cos ( e ( 3 ) ) + Kx * eX ; 40 w = w R e f + K * v R e f * eY * ( 1 + eC / a )^2 + Ks * eS * ( 1 + eC / a ) ^ ( 2 * n ); 41 42 % S i m u l a c i j a g i b a n j a r o b o t a 43 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 44 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 45 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 46 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 47 end 3.3. Vodenje po referenčni trajektoriji 117 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.28: Nelinearno vodenje po referenčni trajektoriji vozila z diferencialnim pogonom iz primera 3.12 (referenca je označena s črtkano krivuljo) 0.8 0.6 s]/ [m 0.4 v 0.2 00 5 10 15 20 25 t [s] 6 s] 4 / [rad 2 ω 0 0 5 10 15 20 25 t [s] Slika 3.29: Regulirni signalni nelinearnega vodenja po referenčni trajektoriji vozila z diferencialnim pogonom iz primera 3.12 118 Vodenje kolesnih mobilnih sistemov 3.3.7 Načrtovanje mehkega vodenja Takagi-Sugeno v okviru linearnih matričnih neenačb Kot smo že poudarili, je model pogreška (3.31) nelinearen. Modeli Takagi-Sugeno (TS) opisujejo dinamiko nelinearnih sistemov. V tem poglavju bo model (3.31) zapisan v obliki modela Takagi-Sugeno, ki omogoča zasnovo vodenja kot t. i. paralelno porazdeljena kompenzacija v okviru linearnih matričnih neenačb (LMI, angl. linear matrix inequality). Mehki model pogreška Takagi-Sugeno kolesnega mobilnega robota z diferencialnim pogonom Modeli Takagi-Sugeno (TS) imajo svoje korenine v mehki (angl. fuzzy) logiki, kjer je model podan v obliki pravil če-potem (angl. if-then). Model TS je lahko predstavljen tudi v bolj kompaktni obliki [15] r ˙ X ξ ( t) = hi (z ( t)) (A iξ ( t) + B i u ( t)) i=1 r X υ ( t) = hi (z ( t)) (C iξ ( t)) i=1 kjer je ξ ( t) ∈ n p q R vektor stanj, υ ( t) ∈ R izhodni vektor in z ( t) ∈ R pogojni vektor, odvisen od vektorja stanj (ali neke druge veličine), A i, B i, C i pa so konstantne matrike. Nelinearne utežnostne funkcije hi (z ( t)) so vse nenegativne in takšne, da velja P r h i=1 i (z ( t)) = 1 za poljuben z( t). Za vsak nelinearen model je mogoče najti enakovreden mehki model TS v kompaktnem območju spremenljivke prostora stanj z razdelitvijo nelinearnega področja, kjer se vsak omejeni nelinearni izraz razgradi v konveksno kombinacijo njegovih meja. Število pravil r je povezano s številom nelinearnosti modela, kot bo prikazano v nadaljevanju. V tem poglavju bomo izkoristili dejstvo, da so v primeru modela pogreška kolesnega robota nelinearne funkcije znane vnaprej, kar omogoča uporabo prej omenjenega koncepta. Nelinearen model sledilnega pogreška (3.31) bo torej prepisan v enakovredno matrično obliko         ˙ ex 0 ωref 0 ex −1 ey " # sin e vfb  ˙ e  − ϕ      y = ωref 0 vref ey + 0 − ex (3.47)    eϕ      ωfb ˙ eϕ 0 0 0 eϕ 0 −1 3.3. Vodenje po referenčni trajektoriji 119 V tem modelu se pojavijo štiri omejene nelinearne funkcije: ω sin eϕ ref , vref (ali eϕ z drugačno oznako vr sinc( ϕ)), ey in ex. Tako dobimo pogojni vektor  ω  ref  v  z ref ( t)sinc ( eϕ( t)) ( t) =    e   y ( t)  ex( t) Najprej bomo v linearnem smislu analizirali vodljivost modela (3.47). V bližini točke, v kateri je pogrešek sistema enak nič, je sistem (3.47) vodljiv, če je vref različen od 0 in | eϕ| drugačen od π ali pa če je ωref različna od 0. V praktičnih primerih ωref pogosto prečka 0, zato vref ne more biti enak 0 in | eϕ| ne more biti enak π. Da preprečimo izgubo vodljivosti in se osredotočimo na določeno kompaktno območje prostora pogreška, so potrebne naslednje predpostavke ω ≤ ω ref ref ≤ ¯ ωref | eϕ| ≤ ¯ eϕ < π, 0 < v ≤ v sinc (¯ e ref ref ≤ ¯ vref ⇒ vref ϕ) ≤ vref sinc ( eϕ) ≤ ¯ vref | ey| ≤ ¯ ey | ex| ≤ ¯ ex Meje od vref in ωref so pridobljene iz dejanske referenčne trajektorije, medtem ko so meje sledilnega pogreška izbrane na podlagi (predhodno) znanih informacij. Pomembno je, da so te meje nižje od pogreška zaradi merilnega šuma, začetnih pogreškov itd. Meje iz (3.3.7) označujemo kot z in ¯ z j j , j = 1 , 2 , 3 , 4. V sistemu so 4 nelinearnosti, zato je število pravil če-potem r enako 24 = 16. Model TS (3.47) je ˙ e ( t) = Az( t)e ( t) + Bz( t)u fb ( t)     0 ε 1 0 −1 ε 3 i i A     i = − ε 1 0 ε 2 B i = 0 − ε 4  i i   i  0 0 0 0 −1 Indeks i gre poljubno skozi vsa oglišča hiperkocke, ki jo definira (3.3.7). Običajno se uporabi binarno naštevanje (0 ; i ≤ r i 2 1 = 1 ; sicer (0 ; i − r i 1 ≤ r i 2 4 2 = 1 ; sicer (0 ; i − r i 1 − r i 2 ≤ r i 2 4 8 3 = 1 ; sicer (0 ; i − r i 1 − r i 2 − r i 3 ≤ r i 2 4 8 16 4 = 1 ; sicer 120 Vodenje kolesnih mobilnih sistemov Potem je εj v (3.3.7) opredeljen kot i εj = z + i ; i = 1 , 2 , . . . , 16 , j = 1 , 2 , 3 , 4 i j j ¯ zj − zj Na koncu pa določimo še pripadnostne funkcije hi hi(z) = w 1 ( z ( z ( z ( z i 1 1) w 2 i 2 2) w 3 i 3 3) w 4 i 4 4) i = 1 , 2 , . . . , 16 zj − zj wj ( z , wj ( z ( z 1 j ) = j ) = 1 − wj j ) j = 1 , 2 , 3 , 4 ¯ z 0 1 j − zj Model TS (3.3.7) modela sledilnega pogreška predstavlja natančen model sistema (3.47), torej pri tem pristopu model TS ne deluje kot aproksimator, ampak upošteva vse znane nelinearnosti v sistemu. Tako je izraz (3.3.7) zelo primeren za naloge načrtovanja in analize, kot bo prikazano v nadaljevanju. Vodenje kolesnega mobilnega robota z diferencialnim pogo- nom z uporabo paralelne porazdeljene kompenzacije Za stabilizacijo modela TS (3.3.7) se uporablja paralelna porazdeljena kompenzacija (PDC, angl. parallel distributed compensation) [16] r X u fb ( t) = − hi (z ( t)) F i e ( t) = −Fz( t) e ( t) i=1 Problem stabilizacije z uporabo PDC je dobro znan. Zaradi posebne strukture, v kateri imata model naprave in regulator enake pripadnostne funkcije, je mogoče temu nelinearnemu sistemu prilagoditi določena orodja za analizo in načrtovanje linearnih sistemov. Še posebej pomembna je možnost formalne in neposredne obravnave stabilnosti sistema. V grobem je sistem, ki ga opisujeta (3.3.7) in (3.3.7), asimptotično stabilen, če je (A i − B i F j) Hurwitzeva matrika za vsak i in j, kar pomeni, da vsi njeni poli ležijo na levi polovici kompleksne ravnine s. Število matrik, potrebnih za analizo, zelo hitro narašča, zato uporabimo sistematičen pristop. Dokaj kmalu so ugotovili, da je LMI popolno orodje za to nalogo [17]. Parametri naprave so podani v obliki matrik A i in B i, zato je mogoče najti tak nabor parametrov regulatorja F j, ki asimptotično stabilizirajo sistem. Prvotni pristop je preveč konzervativen, saj ne upošteva posebnih lastnosti sistema, kot je oblika pripadnostnih funkcij ipd. Izvirne pogoje LMI je možno omiliti. Prilagoditev rezultata glede na [18] je: Model TS (3.3.7) je mogoče stabilizirati z regulacijskim zakonom PDC (3.3.7), če obstajajo matrike M i (i = 1 , 2 , . . . , r) in X > 0 , tako da veljata naslednja pogoja LMI Υ ii < 0 ; i = 1 , 2 , . . . , r 2 Υ ii + Υ ij + Υ ji < 0 i, j = 1 , 2 , . . . , r, i 6= j r − 1 kjer je Υ ij = XA T +A B T −B i i X − M T j i i M j . Ojačenja F i regulacijskega zakona PDC (3.3.7) podaja F i = M i X−1 . 3.3. Vodenje po referenčni trajektoriji 121 3.3.8 Modelno prediktivno vodenje Modelno prediktivno vodenje (MPC, angl. model-based predictive control) temelji na naprednih metodah, ki jih je mogoče uporabiti na različnih področjih; tudi v mobilni robotiki, kjer je referenčna trajektorija znana vnaprej. Uporaba predik-tivnih pristopov vodenja v mobilni robotiki se običajno nanaša na lineariziran (lahko tudi nelinearen) kinematični ali dinamični model za napovedovanje stanj sistema. Znane so številne uspešne implementacije v kolesni mobilni robotiki, kot so posplošeno prediktivno vodenje v [19], prediktivno vodenje s Smithovim prediktorjem za upravljanje časovne zakasnitve sistema v [20], MPC na osnovi linearnega časovno spremenljivega sistema v [21], nelinearni prediktivni regulator z modelom sistema v obliki večplastnega nevronskega omrežja v [22] in mnoge druge. Rešitve regulacijskih zakonov so v večini pristopov pridobljene z optimizacijo cenilke. Drugi pristopi pridobijo regulacijski zakon kot analitično rešitev, ki je računsko učinkovita in jo je mogoče enostavno uporabiti pri hitrih izvedbah v realnem času [23]. To poglavje obravnava mobilnega robota z diferencialnim pogonom in vodenje po referenčni trajektoriji, ki mora biti zvezna in dvakrat zvezno odvedljiva funkcija časa. Za napovedovanje se uporablja linearni dinamični model pogreška, ki ga pridobimo z linearizacijo sistema okoli referenčne trajektorije. Regulator zmanjšuje razliko med napovedanim sledilnim pogreškom robota in referenčnim pogreškom z definirano želeno dinamiko. Modelne strategije vodenja združujejo rešitev predkrmiljenja in akcija povratne zanke v vhodnem vektorju u, kar zapišemo kot " # v u ref cos eϕ + vf b = u ff + u fb = ωref + ωfb kjer se vhodni vektor predkrmiljenja u ff = [ vref cos eϕ ωref ] T izračuna iz referenčne trajektorije z uporabo relacij (3.17) in (3.18). Vhodni vektor povratne zanke je u fb = [ vfb ωfb] T , kar je izhod MPC regulatorja. Problem vodenja upošteva linearen dinamični model sledilnega pogreška (3.32), ki ga lahko na kratko zapišemo kot ˙ e = A c( t)e + B c u fb (3.48) kjer sta A c( t) in B c matriki zveznega modela prostora stanj in e je sledilni pogrešek v lokalnih koordinatah robota, ki so določene s transformacijo (3.28) in prikazane na sliki 3.21. Diskretno modelno prediktivno vodenje Modelno prediktivno vodenje (MPC), predstavljeno v [23], je zasnovano za diskretne čase, zato je potrebno zapisati model (3.48) v diskretni obliki ˙ e( k + 1) = A( k)e( k) + Bu fb( k) (3.49) 122 Vodenje kolesnih mobilnih sistemov kjer je A( k) ∈ n n n m R × R , n je število spremenljivk stanj, in B ∈ R × R , m je število vhodnih spremenljivk. Diskretni matriki A( k) in B dobimo na naslednji način A( k) = I + A c( t) Ts B = B cTs kar je zadosten približek za kratek računski korak Ts. Glavna ideja MPC je izračunati optimalne akcije vodenja, ki minimizirajo kri- terijsko funkcijo, določeno v intervalu predikcijskega horizonta h. Kriterijska funkcija je kvadratna cenilka h X J (u fb, k) = T ( k, i)Q( k, i) + u T ( k + i − 1)Ru f b f b( k + i − 1) (3.50) i=1 sestavljena iz prihodnjega referenčnega sledilnega pogreška e r( k + i), napovedanega sledilnega pogreška e( k + i| k), razlike med omenjenima pogreškoma ( k, i) = er( k + i) − e( k + i| k) in prihodnje akcije u fb( k + i − 1), kjer i označuje i-ti korak napovedi ( i = 1 , . . . , h); Q in R sta utežnostni matriki. Za napoved stanja e( k + i| k) se uporabi model pogreška (3.49), kot sledi e( k + 1| k) = A( k)e( k) + Bu fb( k) e( k + 2| k) = A( k + 1)e( k + 1| k) + Bu fb( k + 1) ... (3.51) e( k + i| k) = A( k + i − 1)e( k + i − 1| k) + Bu fb( k + i − 1) ... e( k + h| k) = A( k + h − 1)e( k + h − 1| k) + Bu fb( k + h − 1) Napovedi e( k + i| k) v (3.51) so preurejene tako, da so odvisne od trenutnega pogreška e( k), trenutnih in prihodnjih vhodov u fb( k+ i−1) ter matrik A( k+ i−1) in B. Napoved izhoda modela v trenutku h lahko potem zapišemo kot e( k + h| k) = Π h−1A( k + j)e( k)+ j=1 h X + Π h−1A( k + j) B u j= i f b( k + i − 1) + Bu f b( k + h − 1) i=1 Prihodnji referenčni pogrešek (e r( k + i)) določa, kako naj se zmanjša sledilni pogrešek, ko robot ni na trajektoriji. Določimo lahko, da naj se prihodnji referenčni pogrešek eksponentno zmanjša od trenutnega sledilnega pogreška e( k) kot e r( k + i) = A i e( k) r za i = 1 , . . . , h. Dinamiko referenčnega pogreška določa matrika referenčnega modela A r. 3.3. Vodenje po referenčni trajektoriji 123 Glede na (3.51) in (3.3.8) je določen vektor predikcijskega pogreška sledenja robota h i T E∗( k) = e T ( k + 1| k) e T ( k + 2| k) . . . e T ( k + h| k) pri čemer je E∗ podan za celoten interval opazovanja ( h), kjer je regulirni vektor h i T U fb( k) = u T ( k) u T ( k + 1) . . . u T ( k + h − 1) (3.52) f b f b f b in Λ( k, i) = Π h−1A( k + j) = A( k + h − 1)A( k + h − 2) . . . A( k + i + 1)A( k + i) j= i Vektor predikcijskega pogreška sledenja robota lahko zapišemo v strnjeni obliki E∗( k) = F ( k)e( k) + G( k)U fb( k) kjer je h i T F ( k) = A T ( k) A T ( k)A T ( k + 1) . . . Λ T ( k, 0) in  B  0 . . . 0  ..  A( k + 1)B B . . . .  G( k) =    .. .. . . ..   . . . .    Λ( k, 1)B Λ( k, 2)B . . . B pri čemer sta dimenziji F ( k) in G( k) enaki ( nh × n) ter ( nh × mh). Vektor referenčnega sledilnega pogreška je h i T E∗( k) = r e T ( k + 1) e T ( k + 2) . . . e T ( k + h) r r r ki se v strnjeni obliki zapiše kot E∗( k) = F r r e( k) kjer je h F T T i T r = A T A2 . . . A h r r r matrika dimenzije ( nh × n). Optimalni vhodni vektor (3.52) dobimo z numerično ali analitično optimizacijo funkcije (3.50). V nadaljevanju bo izpeljana analitična rešitev. Kriterijska funkcija (3.50) se v matrični obliki glasi J (U fb) = (E∗ − E∗) T Q (E∗ − E∗) + U T R U r r f b f b (3.53) minimum (3.53) pa je izražen kot ∂J = −2 Q G T E∗ + 2G T Q E∗ + 2 R U fb = 0 ∂ U r f b 124 Vodenje kolesnih mobilnih sistemov Dobimo rešitev za optimalni vhodni vektor kot U fb( k) = G T Q G + R−1 G T Q (F r − F ) e( k) (3.54) kjer sta utežnostni matriki naslednji Q    0 . . . 0 R 0 . . . 0  0 Q . . . 0   0 R . . . 0  Q =      . . . .  R =  . . . .   . . .. . . ..   . . .. . . ..      0 0 . . . Q 0 0 . . . R Rešitev (3.54) vsebuje vhodne vektorje uT ( k + i − 1) za celoten interval napovedi f b ( i = 1 , . . . , h). Akcijo povratne zanke v trenutku k uveljavimo tako, da na robotu uporabimo samo prvi vektor uT ( k) (prve m vrstice od U f b f b( k)). Rešitev je pridobljena analitično, zato omogoča hitre izvedbe v realnem času, kar morda ni možno, če uporabimo numerično optimizacijo funkcije (3.50). Primer 3.13 Izvedite modelno prediktivno vodenje, podano v (3.54), za vodenje robota z diferencialnim pogonom po trajektoriji. Referenčna trajektorija in robot sta določena v primeru 3.9. Predikcijski horizont je h = 4, matrika referenčnega modela je A r = I3×3 · 0 , 65, utežnostni matriki pa sta   4 0 0 Q = 0 40 0  R = I2×2 · 10−3   0 0 0 , 1 Rešitev Z uporabo MPC izračunamo povratnozančni del krmilnega signala u fb( k) in ga uporabimo na robotu skupaj s predkrmiljenjem u ff ( k). V programu 3.12) so podane možne rešitve in rezultati vodenja po referenčni trajektoriji. Pridobljeni rezultati simulacije so prikazani na slikah 3.30 in 3.31. Program 3.12 ./src/ctr/example_tracking_mpc.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 3.3. Vodenje po referenčni trajektoriji 125 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na t r a j e k t o r i j a 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 for k = 1: length ( t ) -4 16 e = [ cos ( q (3)) , sin ( q (3)) , 0; ... 17 - sin ( q (3)) , cos ( q (3)) , 0; ... 18 0 , 0 , 1 ] * ( q R e f (: , k ) - q ); % V e k t o r p o g r e š ka 19 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 20 21 A1 = [1 , Ts * u R e f (2 , k ) , 0; - Ts * u R e f (2 , k ) , 1 , Ts * u R e f (1 , k ); 0 ,0 ,1]; 22 A2 = [1 , Ts * u R e f (2 , k +1) , 0; - Ts * u R e f (2 , k +1) , 1 , Ts * u R e f (1 , k + 1 ) ; 0 ,0 ,1]; 23 A3 = [1 , Ts * u R e f (2 , k +2) , 0; - Ts * u R e f (2 , k +2) , 1 , Ts * u R e f (1 , k + 2 ) ; 0 ,0 ,1]; 24 A4 = [1 , Ts * u R e f (2 , k +3) , 0; - Ts * u R e f (2 , k +3) , 1 , Ts * u R e f (1 , k + 3 ) ; 0 ,0 ,1]; 25 B = [ - Ts , 0; 0 , 0; 0 , - Ts ]; 26 27 Z = z e r o s (3 ,2); 28 Hm = [ B , Z , Z , Z ; ... 29 A1 * B , B , Z , Z ; ... 30 A1 * A2 * B , A1 * B , B , Z ; ... 31 A1 * A2 * A3 * B , A1 * A2 * B , A1 * B , B ]; 32 Fm = [ A1 , A1 * A2 , A1 * A2 * A3 , A1 * A2 * A3 * A4 ]. ’; 33 34 ar = 0 . 6 5 ; 35 Ar = eye ( 3 ) * ar ; % D i n a m i k a r e f e r e n č n e g a p o g r e š ka 36 H = 0; 37 Fr = [ Ar ^( H +1) , Ar ^( H +2) , Ar ^( H +3) , Ar ^( H + 4) ]. ’; 38 39 % Ute ž n o s t n e m a t r i k e 40 Qt = d i a g ( r e p m a t ( [ 1 ; 40; 0.1] , 4 , 1 ) ) ; 41 Rt = d i a g ( r e p m a t ( [ 0 . 0 0 1 ; 0 .0 01 ] , 4 , 1 ) ) ; 42 43 % I z r a č un o p t i m a l n i h r e g u l i r n i h s i g n a l o v 44 K K g p c = ( Hm . ’* Qt * Hm + Rt )\( Hm . ’* Qt *( Fr - Fm )); 45 KK = K K g p c ( 1 : 2 , : ) ; % I z b i r a t r e n u t n i h oja č enj r e g u l a t o r j a 46 47 v = KK * e ; 48 uF = [ u R e f (1 , k )* cos ( e ( 3 ) ) ; u R e f (2 , k )]; 49 u = v + uF ; 50 51 v M A X = 1; w M A X = 15; % M a k s i m a l n i h i t r o s t i 52 if abs ( u (1)) > vMAX , u (1) = s i g n ( u ( 1 ) ) * v M A X ; end 53 if abs ( u (2)) > wMAX , u (2) = s i g n ( u ( 2 ) ) * w M A X ; end 54 55 % S i m u l a c i j a g i b a n j a r o b o t a 56 dq = [ u ( 1 ) * cos ( q ( 3 ) ) ; u ( 1 ) * sin ( q ( 3 ) ) ; u ( 2 ) ] ; 57 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 58 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 59 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 60 end 126 Vodenje kolesnih mobilnih sistemov 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.30: Rezultati vodenja, pridobljeni z eksplicitnim regulatorjem MPC (referenca je označena s črtkano krivuljo) 1 s]/ [m 0.5 v 00 5 10 15 20 25 t [s] 10 s]/ 5 [rad ω 0 0 5 10 15 20 25 t [s] Slika 3.31: Vzbujanje robota izračunano z uporabo eksplicitnega MPC-ja (črtkana krivulja predstavlja le signal predkrmiljenja, brez regulirnega signala) 3.3. Vodenje po referenčni trajektoriji 127 3.3.9 Vodenje na podlagi optimizacije z rojem delcev Vodenje mobilnega robota lahko interpretiramo tudi kot optimizacijski problem, kjer je potrebno v vsakem računskem koraku regulacijske zanke najti najboljšo rešitev med vsemi možnimi rešitvami v iskalnem prostoru. Tako vodenje nima eksplicitne strukture, kar pomeni, da regulacijskega zakona ni mogoče podati kot funkcijo preslikave stanj sistema v akcije vodenja. Optimalno rešitev, ki minimalizira nekatere kriterijske funkcije, najdemo z uporabo iterativnega opti-mizacijskega algoritma, kot so Newtonove metode, metode gradientnega sestopa ali pa stohastične metode, kot so genetski algoritmi, optimizacija z rojem delcev (PSO, angl. particle swarm optimization) ipd. Če kriterijska funkcija ni konveksna za problem minimizacije, je lahko večina optimizacijskih algoritmov ujetih v lokalnem minimumu, kar pa ni optimalna rešitev. Verjetnost pojava take rešitve lahko zmanjšamo z uporabo stohastične optimizacije, kjer je vzorec iskanja do neke mere naključen. Osnovna ideja PSO izhaja iz družabnega vedenja malih živali, kot so jate ptic ali rib [24, 25]. PSO uporablja skupino (roj) delcev, kjer vsak delec predstavlja svojo hipotetično rešitev. Vsak delec i je opisan s parametričnim vektorjem x i, ki določa njegov položaj v parametričnem prostoru, in inkrementalnim vektorjem v i, ki določa njegovo hitrost v parametričnem prostoru. Med optimizacijo se populacija vseh potencialnih rešitev posodablja glede na kriterijsko funkcijo, ki določa merilo kakovosti. Vsak delec spremlja svoje parametre in si zapomni njihove (doslej) najboljše vrednosti pBest i skupaj s pripadajočo kriterijsko funkcijo Ji = f (pBest i). Med optimizacijo je shranjen tudi (doslej) najboljši parametrični vektor za celoten roj pBest i. V lokalni različici PSO pa se za neko okolico delcev spremlja najboljši parametrični vektor gBest vsakega delca (okolica se določi s topologijo obroča, k najbližjimi delci ipd. [26]). Za lokalne različice PSO je manj verjetno, da bodo ujete v lokalnem minimumu. V nadaljevanju je razložena osnovna (globalna) različica PSO. V vsakem račun- skem koraku regulacijske zanke delci posodobijo svoje kognitivno in socialno vedenje glede na naslednja pravila v i ← ω v i + c 1rand(0 , 1)(pBest i − x i) + c 2rand(0 , 1)(gBest − x i) (3.55) x i ← x i + v i kjer je ω faktor vztrajnosti, c 1 samozavedna konstanta in c 2 socialna konstanta. Poleg tega je rand(0 , 1) vektor enakomerno razporejenih vrednosti v območju (0 , 1). Dimenzije vektorjev v (3.55) so enake dimenziji iskalnega prostora. 128 Vodenje kolesnih mobilnih sistemov Parametri ω, c 1 in c 2 so pozitivni nastavitveni parametri. Osnovna koda PSO je podana v algoritmu 1. Algorithm 1 Optimizacija z rojem delcev Inicializacija: for vsak delec i = 1 , . . . , N do Naključno inicializiraj položaje delcev x i znotraj meja parametričnega prostora. Naključno inicializiraj hitrost delcev v i ali jih nastavi na nič. Nastavi pBest i = x i. end for Optimizacija: Jbest = ∞ repeat for vsak delec i = 1 , . . . , N do Izračunaj trenutno kriterijsko funkcijo Ji = f (x i) za vsak delec. Shrani najboljše parametre if Ji < f (pBest i) then pBest i = x i end if if f (pBest i) < Jbest then gBest = pBest i Jbest = f (gBest) end if end for for vsak delec i = 1 , . . . , N do Posodobi hitrost in položaj delca v i ← ω v i + c 1rand(0 , 1)(pBest i − x i) + c 2rand(0 , 1)(gBest − x i) Preveri, ali je hitrost v i izvedljiva: x i ← x i + v i end for until največje število iteracij ali izpolnjen kriterij za konvergenco Primer 3.14 S pomočjo PSO določite regulator za robota in trajektorijo iz primera 3.9. PSO uporabite v vsakem računskem koraku t = kTs, da najdete najboljše regulirne veličine (translatorna in kotna hitrost), ki pripeljejo robota čim bližje trenutnemu referenčnemu položaju xref ( t), yref ( t). Nato izračunajte predikcijo lege robota glede na njegovo kinematiko in predlagano rešitev delcev (akcija regulatorja). Cenilka je sestavljena iz sledilnega pogreška e( t) = [ ex( t) , ey( t) , eϕ( t)] T in povratne zanke u fb = [ vfb, ωfb] T . Optimalno vodenje minimizira cenilko 3.3. Vodenje po referenčni trajektoriji 129 J ( t) = e( t) T Qe( t) + u T Ru f b f b, kjer sta Q in R diagonalni utežnostni matriki, ki se uporabljata za nastavitev delovanja regulatorja. Rešitev Regulacijski zakon vključuje podobne akcije predkrmiljenja in povratne zanke kot v primeru 3.9. Akcijo predkrmiljenja izračunamo iz znane trajektorije, medtem ko akcijo povratne zanke določimo z uporabo PSO. Sledilni pogrešek je izražen v lokalnih koordinatah robota (glejte sliko 3.21), saj je s tem optimizacija bolj učinkovita, zaradi bolje razklopljenega zaprtozančnega sistema. Pogrešek v lokalni x koordinati je lahko preprosto kompenziran s translatorno hitrostjo, pogrešek v y in ϕ pa s kotno hitrostjo. To namreč ne velja pri uporabi globalnega sledilnega pogreška, zaradi nelinearne transformacije rotacije v (3.28). V programu 3.13 je podana Matlab koda možne rešitve. Pridobljeni rezultati simulacije so prikazani na slikah 3.32 in 3.33. Rezultati vodenja so podobni kot v primeru 3.13, vendar je računska zahtevnost algoritma precej večja. Rezultati primera 3.14 niso deterministični, ker PSO pri optimizaciji uporablja naključno porazdeljene delce, da najde najboljše regulirne veličine v vsakem računskem koraku. Zato se lahko pridobljene trajektorije mobilnega robota do neke mere razlikujejo, še posebej v začetni prehodni fazi dokler robot ne doseže reference. Kljub temu so rezultati večine simulacijskih tekov primerljivi z rezultati drugih predstavljenih determinističnih regulatorjev (npr. iz primera 3.9). Program 3.13 ./src/ctr/example_tracking_pso.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na t r a j e k t o r i j a 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 vMax = 1; wMax = 15; % Omejitve hitrosti 16 17 % I n i c i a l i z a c i j a roja delcev 18 iterations = 20; % Š tevilo iteracij 19 omega = 0.5*0.5; % Faktor vz tra jno sti 20 c1 = 0.5*1; % Sam oz ave dna konstanta 21 c2 = 0.5*1; % Socialna konstanta 22 N = 25; % Velikost roja delcev 23 swarm = zeros ([2 , N ,4]); 130 Vodenje kolesnih mobilnih sistemov 24 uBest = [0; 0]; 25 26 for k = 1: length ( t ) -1 27 % Za č e t n i p o l o ž aji d e l c e v 28 s w a r m (: ,: ,1) = r e p m a t ( uBest , 1 , N ) + d i a g ( [ 0 . 1 ; 3 ] ) * r a n d n (2 , N ); 29 s w a r m (: ,: ,2) = 0; % Za č e t n e h i t r o s t i d e l c e v 30 s w a r m (1 ,: ,4) = 1 0 0 0 ; % N a j b o l j š a v r e d n o s t k r i t e r i j s k e f u n k c i j e 31 32 for i t e r = 1: i t e r a t i o n s % I t e r a t i v n o i s k a n j e o p t i m a l n e re š i t v e s PSO 33 % V r e d n o t e n j e p a r a m e t r o v d e l c e v 34 for i = 1: N 35 % I z r a č un n o v e p r e d v i d e n e l e g e r o b o t a na p o d l a g i p a r a m e t r o v 36 % i - t e g a d e l c a ( v h o d n i h h i t r o s t i ) in p r i m e r j a v a p r e d v i d e n e 37 % l e g e z r e f e r e n č no l e g o . 38 vwi = s w a r m (: , i , 1 ) ; 39 ui = vwi + u R e f (: , k ); % R e g u l a c i j a in k r m i l j e n j e 40 qk = q ; % T r e n u t n a l e g a r o b o t a 41 % P r e d i k c i j a l e g e r o b o t a na p o d l a g i p a r a m e t r o v d e l c e v ( h i t r o s t i ) 42 qk = qk + Ts *[ cos ( qk (3)) , 0; sin ( qk (3)) , 0; 0 , 1]* ui ; 43 qk (3) = w r a p T o P i ( qk ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 44 e = [ cos ( qk (3)) , sin ( qk (3)) , 0; ... 45 - sin ( qk (3)) , cos ( qk (3)) , 0; ... 46 0 , 0 , 1 ] * ( q R e f (: , k +1) - qk ); % P o g r e š ek 47 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 48 Qt = d i a g ( [ 4 ; 80; 0 . 1 ] ) ; Rt = d i a g ( [ 1 ; 1 ] * 0 . 0 0 0 1 ) ; % Ute ž i 49 J = e . ’* Qt * e + vwi . ’* Rt * vwi ; % K r i t e r i j s k a f u n k c i j a 50 if J < s w a r m (1 , i ,4) % Č e je n o v i p a r a m e t e r b o l j š i , p o s o d o b i : 51 s w a r m (: , i ,3) = s w a r m (: , i , 1 ) ; % v r e d n o s t i p a r a m e t r o v ( v in w ) 52 s w a r m (1 , i ,4) = J ; % in n a j b o l j š o v r e d n o s t k r i t e r i j s k e f u n k c i j e . 53 end 54 end 55 [~ , g B e s t ] = min ( s w a r m (1 ,: ,4)); % P a r a m e t r i g l o b a l n o n a j b o l j š ega d e l c a 56 57 % P o s o d o b i t e v p a r a m e t r o v s h i t r o s t n i m i v e k t o r j i 58 a = o m e g a * s w a r m (: ,: ,2) + ... 59 c1 * r a n d (2 , N ) . * ( s w a r m (: ,: ,3) - s w a r m (: ,: ,1)) + ... 60 c2 * r a n d (2 , N ) . * ( r e p m a t ( s w a r m (: , gBest ,3) , 1 , N ) - s w a r m (: ,: ,1)); 61 % M a k s i m a l n a s p r e m e m b a p a r a m e t r o v , p o s p e š ek : a M a x =3 == > 3* Ts = 0 . 1 62 a (1 , a (1 ,:) >0.1) = 0 . 1 ; a (1 , a (1 ,:) < -0.1) = -0.1; 63 % M a k s i m a l n a s p r e m e m b a p a r a m e t r o v , k o t n i p o s p e š ek : a M a x =60 == > 60* Ts =2 64 a (2 , a (1 ,:) >2) = 2; a (2 , a (1 ,:) < -2) = -2; 65 66 v = s w a r m (: ,: ,1) + a ; % P o s o d o b i t e v h i t r o s t i 67 % O m e j i t e v h i t r o s t i z o h r a n j a n j e m u k r i v l j e n o s t i 68 [ m , ii ] = max ([ v (1 ,:)/ v M a x ; v (2 ,:)/ w M a x ; o n e s (1 , N ) ] ) ; 69 i = ii = = 1 ; v (1 , i ) = s i g n ( v (1 , i ))* v M a x ; 70 v (2 , i ) = v (2 , i )./ m ( i ); 71 i = ii = = 2 ; v (2 , i ) = s i g n ( v (2 , i ))* w M a x ; 72 v (1 , i ) = v (1 , i )./ m ( i ); 73 74 s w a r m (: ,: ,2) = a ; % P o s o d o b i t e v h i t r o s t i d e l c e v ( p o s p e š ki ) 75 s w a r m (: ,: ,1) = v ; % P o s o d o b i t e v p o l o ž a j e v d e l c e v ( h i t r o s t i ) 76 end 77 78 % V z m e m e m o n a j b o l j š i d e l e c za i z r a č un r e g u l i r n i h s i g n a l o v 79 u B e s t = s w a r m (: , gBest , 1 ) ; 80 u = u B e s t + u R e f (: , k ); % R e g u l a c i j a in k r m i l j e n j e 81 82 % O m e j i t v e h i t r o s t i 83 if abs ( u (1)) > vMax , u (1) = s i g n ( u ( 1 ) ) * v M a x ; end 84 if abs ( u (2)) > wMax , u (2) = s i g n ( u ( 2 ) ) * w M a x ; end 85 86 % S i m u l a c i j a g i b a n j a r o b o t a 3.3. Vodenje po referenčni trajektoriji 131 87 dq = [ u ( 1 ) * cos ( q ( 3 ) ) ; u ( 1 ) * sin ( q ( 3 ) ) ; u ( 2 ) ] ; 88 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 89 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 90 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 91 end 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.32: Pridobljeni rezultati vodenja na podlagi optimizacije z rojem delcev (referenca je označena s črtkano krivuljo) 132 Vodenje kolesnih mobilnih sistemov 1 s] 0.5 / [mv 0 −0.50 5 10 15 20 25 t [s] 20 s] 10 / [rad ω 0 −10 0 5 10 15 20 25 t [s] Slika 3.33: Akcija vodenja, izračunana na podlagi optimizacije z rojem delcev (črtkana krivulja predstavlja le signal predkrmiljenja, brez regulirnega signala) PSO je splošni algoritem, ki ga je mogoče uporabiti v številnih aplikacijah, dokler računska zahtevnost in čas, potrebna za izračun rešitve, ustrezata dejanskim realno-časovnim zahtevam vodenega sistema. Modelno prediktivno vodenje z uporabo optimizacije z ro- jem delcev Reševanje optimizacije kriterijske funkcije v MPC lahko opravi tudi PSO, če le-ta išče najboljše parametre (akcije regulatorja) za predikcijski interval ( t, t + hTS), kjer je h horizont. Če ima sistem m = 2 regulirni veličini, mora optimizacija najti m · h optimalnih parametrov, kar hitro lahko postane računsko zahtevno in posledično problematično za sisteme s kratkim računskim korakom regulacijske zanke. Vseeno pa obstaja nekaj možnosti za zmanjšanje računskega časa. Ena možnost je, da v predikcijskem horizontu predpostavimo konstantne regulirne veličine. Če se akcija regulatorja v relativno kratkem času horizonta bistveno ne spremeni, lahko v intervalu horizonta predpostavimo konstantne regulirne veličine. To pomeni, da je potrebno optimizirati le m parametrov namesto m · h. Druga možnost je zmanjšanje števila potrebnih iteracij pri optimizaciji za vsak računski korak regulacijske zanke. To lahko storimo z inicializacijo delcev okoli optimalne rešitve iz prejšnjega časovnega vzorca. Tako bi bilo potrebno manj 3.3. Vodenje po referenčni trajektoriji 133 iteracij za konvergenco delcev. V primeru 3.15 je podana možna rešitev za implementacijo MPC na PSO. Primer 3.15 Razširite primer 3.14 na modelni prediktivni regulator s predikcijskim horizontom h = 3. Rešitev Predstavljena rešitev predvideva, da je akcija regulatorja (povratnozančni del) v intervalu predikcijskega horizonta konstantna u fb( t + ( i − 1) TS) = u fb. Akcija predkrmiljenja (u ff ) je pridobljena iz znane trajektorije, medtem ko se akcija povratne zanke izračuna z uporabo PSO. Iz trenutne lege robota je h-koračna predikcija pridobljena s pomočjo kinematičnega modela robota in akcij regulatorja u( t + ( i − 1) TS) = u fb + u ff ( t + ( i − 1) TS) ( i = 1 , . . . , h) kot ˆ q( t + iTS) = f (ˆ q ( t + ( i − 1) TS) , u ( t + ( i − 1) TS)) kjer je začetno stanje enako ˆ q( t) = q( t) in i = 1 , . . . , h. Akcijo regulatorja dobimo z optimizacijo cenilke znotraj horizonta h X J ( t + hTS) = e( t + iTS) T Qe( t + iTS) + u T Ru f b f b i=1 kjer je e(·) sledilni pogrešek v lokalnih koordinatah. Optimalna akcija regulatorja se izračuna z uporabo PSO, kot je predstavljeno v Matlab kodi v programu 3.14. Rezultati simulacije so prikazani na slikah 3.34 in 3.35. Sledilni pogrešek je nekoliko manjši, pa tudi začetni prehodni pojav je boljši kot v primeru 3.14. Program 3.14 ./src/ctr/example_tracking_pso_mpc.m 1 Ts = 0.033; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [1.1; 0.8; 0]; % Za č etna lega 4 5 % Referenca 6 freq = 2* pi /30; 7 x R e f = 1.1 + 0 . 7 * sin ( f r e q * t ); y R e f = 0.9 + 0 . 7 * sin (2* f r e q * t ); 8 d x R e f = f r e q * 0 . 7 * cos ( f r e q * t ); d y R e f = 2* f r e q * 0 . 7 * cos (2* f r e q * t ); 9 ddxRef = - freq ^2*0.7* sin ( freq * t ); ddyRef = -4* freq ^2*0.7* sin (2* freq * t ); 10 qRef = [ xRef ; yRef ; atan2 ( dyRef , dxRef )]; % Referen č na t r a j e k t o r i j a 11 vRef = sqrt ( dxRef .^2+ dyRef .^2); 12 wRef = ( dxRef .* ddyRef - dyRef .* ddxRef )./( dxRef .^2+ dyRef .^2); 13 uRef = [ vRef ; wRef ]; % Referen č ni vhodi 14 15 vMax = 1; wMax = 15; % Omejitve hitrosti 16 17 % I n i c i a l i z a c i j a roja delcev 18 iterations = 20; % Š tevilo iteracij 134 Vodenje kolesnih mobilnih sistemov 19 omega = 0.5*0.5; % Faktor vz tra jno sti 20 c1 = 0.5*1; % Sam oza ved na konstanta 21 c2 = 0.5*1; % Socialna konstanta 22 N = 25; % Velikost roja delcev 23 swarm = zeros ([2 , N ,4]); 24 uBest = [0; 0]; 25 26 H = 3; % Dol ž ina p r e d i k c i j s k e g a horizonta 27 28 for k = 1: length ( t ) - H 29 % Za č e t n i p o l o ž aji d e l c e v 30 s w a r m (: ,: ,1) = r e p m a t ( uBest , 1 , N ) + d i a g ( [ 0 . 1 ; 3 ] ) * r a n d n (2 , N ); 31 s w a r m (: ,: ,2) = 0; % Za č e t n e h i t r o s t i d e l c e v 32 s w a r m (1 ,: ,4) = 1 0 0 0 ; % N a j b o l j š a v r e d n o s t k r i t e r i j s k e f u n k c i j e 33 34 for i t e r = 1: i t e r a t i o n s % I t e r a t i v n o i s k a n j e o p t i m a l n e re š i t v e s PSO 35 % V r e d n o t e n j e p a r a m e t r o v d e l c e v 36 for i = 1: N 37 % I z r a č un n o v e p r e d v i d e n e l e g e r o b o t a na p o d l a g i p a r a m e t r o v 38 % i - t e g a d e l c a ( v h o d n i h h i t r o s t i ) in p r i m e r j a v a p r e d v i d e n e 39 % l e g e z r e f e r e n č no l e g o . 40 vwi = s w a r m (: , i , 1 ) ; 41 ui = vwi + u R e f (: , k ); % R e g u l a c i j a in k r m i l j e n j e 42 qk = q ; % T r e n u t n a l e g a r o b o t a 43 % P r e d i k c i j a l e g e r o b o t a na p o d l a g i p a r a m e t r o v d e l c e v ( h i t r o s t i ) 44 J = 0; 45 for h = 1: H 46 qk = qk + Ts *[ cos ( qk (3)) , 0; sin ( qk (3)) , 0; 0 , 1]* ui ; 47 qk (3) = w r a p T o P i ( qk ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 48 e = [ cos ( qk (3)) , sin ( qk (3)) , 0; ... 49 - sin ( qk (3)) , cos ( qk (3)) , 0; ... 50 0 , 0 , 1 ] * ( q R e f (: , k + h ) - qk ); % P o g r e š ek 51 e (3) = w r a p T o P i ( e ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 52 Qt = d i a g ( [ 4 ; 80; 0 . 1 ] ) ; Rt = d i a g ( [ 1 ; 1 ] * 0 . 0 0 0 1 ) ; % Ute ž i 53 J = J + e . ’* Qt * e + vwi . ’* Rt * vwi ; % K r i t e r i j s k a f u n k c i j a 54 end 55 if J < s w a r m (1 , i ,4) % Č e je n o v i p a r a m e t e r b o l j š i , p o s o d o b i : 56 s w a r m (: , i ,3) = s w a r m (: , i , 1 ) ; % v r e d n o s t i p a r a m e t r o v ( v in w ) 57 s w a r m (1 , i ,4) = J ; % in n a j b o l j š o v r e d n o s t k r i t e r i j s k e f u n k c i j e . 58 end 59 end 60 [~ , g B e s t ] = min ( s w a r m (1 ,: ,4)); % P a r a m e t r i g l o b a l n o n a j b o l j š ega d e l c a 61 62 % P o s o d o b i t e v p a r a m e t r o v s h i t r o s t n i m i v e k t o r j i 63 a = o m e g a * s w a r m (: ,: ,2) + ... 64 c1 * r a n d (2 , N ) . * ( s w a r m (: ,: ,3) - s w a r m (: ,: ,1)) + ... 65 c2 * r a n d (2 , N ) . * ( r e p m a t ( s w a r m (: , gBest ,3) , 1 , N ) - s w a r m (: ,: ,1)); 66 % M a k s i m a l n a s p r e m e m b a p a r a m e t r o v , p o s p e š ek : a M a x =3 == > 3* Ts = 0 . 1 67 a (1 , a (1 ,:) >0.1) = 0 . 1 ; a (1 , a (1 ,:) < -0.1) = -0.1; 68 % M a k s i m a l n a s p r e m e m b a p a r a m e t r o v , k o t n i p o s p e š ek : a M a x =60 == > 60* Ts =2 69 a (2 , a (1 ,:) >2) = 2; a (2 , a (1 ,:) < -2) = -2; 70 71 v = s w a r m (: ,: ,1) + a ; % P o s o d o b i t e v h i t r o s t i 72 % O m e j i t e v h i t r o s t i z o h r a n j a n j e m u k r i v l j e n o s t i 73 [ m , ii ] = max ([ v (1 ,:)/ v M a x ; v (2 ,:)/ w M a x ; o n e s (1 , N ) ] ) ; 74 i = ii = = 1 ; v (1 , i ) = s i g n ( v (1 , i ))* v M a x ; 75 v (2 , i ) = v (2 , i )./ m ( i ); 76 i = ii = = 2 ; v (2 , i ) = s i g n ( v (2 , i ))* w M a x ; 77 v (1 , i ) = v (1 , i )./ m ( i ); 78 79 s w a r m (: ,: ,2) = a ; % P o s o d o b i t e v h i t r o s t i d e l c e v ( p o s p e š ki ) 80 s w a r m (: ,: ,1) = v ; % P o s o d o b i t e v p o l o ž a j e v d e l c e v ( h i t r o s t i ) 81 end 3.3. Vodenje po referenčni trajektoriji 135 82 83 % V z m e m e m o n a j b o l j š i d e l e c za i z r a č un r e g u l i r n i h s i g n a l o v 84 u B e s t = s w a r m (: , gBest , 1 ) ; 85 u = u B e s t + u R e f (: , k ); % R e g u l a c i j a in k r m i l j e n j e 86 87 % O m e j i t v e h i t r o s t i 88 if abs ( u (1)) > vMax , u (1) = s i g n ( u ( 1 ) ) * v M a x ; end 89 if abs ( u (2)) > wMax , u (2) = s i g n ( u ( 2 ) ) * w M a x ; end 90 91 % S i m u l a c i j a g i b a n j a r o b o t a 92 dq = [ u ( 1 ) * cos ( q ( 3 ) ) ; u ( 1 ) * sin ( q ( 3 ) ) ; u ( 2 ) ] ; 93 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 94 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 95 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 96 end 1.6 1.4 1.2 ] 1 [my 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 x [m] Slika 3.34: Pridobljeni rezultati vodenja z uporabo MPC in PSO (referenca je označena s črtkano krivuljo) 136 Vodenje kolesnih mobilnih sistemov 1 s]/0[m.5 v 0 0 5 10 15 20 25 t [s] 15 s] 10 / 5 [rad ω 0 −50 5 10 15 20 25 t [s] Slika 3.35: Akcija regulatorja, izračunana z uporabo MPC in PSO (črtkana krivulja predstavlja le signal predkrmiljenja, brez regulirnega signala) 3.3.10 Vodenje mobilnega sistema s pristopom vodenja na osnovi slike V tem poglavju bo predstavljeno vodenje na osnovi slike (VS, angl. visual servoing), ki se pogosto uporablja v mobilni robotiki. Glavni poudarek je na razvoju mobilnega robotskega sistema, ki lahko avtonomno opravi podano nalogo le na podlagi vizualnih informacij od kamere, nameščene na mobilnem sistemu. Pri vodenju na osnovi slike se za določitev regulacijskega pogreška uporabljajo t. i. značilke. Značilke so neodvisne spremenljivke, ki opisujejo določen vizualni signal, torej lahko z njimi opišemo točke, črte, kroge, območje objekta, kote med črtami itd. Regulacijski pogrešek splošne regulacijske sheme pri vodenju na osnovi slike lahko zapišemo kot razliko med vektorjem želenih značilk x ref ( t) in vektorjem trenutnih značilk x(y( t) , ζ) [27] e( t) = x ref ( t) − x(y( t) , ζ) (3.56) K-ti element vektorja značilk x(y( t) , ζ) ∈ K R v (3.56) je pridobljen iz meritev na podlagi slike y( t) (npr. pozicija, območje ali oblika vzorcev na sliki) in dodatnega časovno nespremenljvega znanja o sistemu ζ (npr. notranji parametri 3.3. Vodenje po referenčni trajektoriji 137 kamere, znane lastnosti opazovane scene, omejitve sistema). Eden od ključnih izzivov pri načrtovanju vodenja na osnovi slike je ustrezna opredelitev vektorja značilk, saj se ta vektor uporablja za določanje regulacijskega pogreška. Če je vektor značilk neustrezno določen, lahko obstaja več situacij, kjer je regulacijski pogrešek najmanjši — problem lokalnih minimumov. Nekatere značilke so primerne samo za določene vrste gibanja (npr. samo translacija) in so lahko popolnoma neprimerne za nekatere druge vrste gibanja (npr. translacija z rotacijo). Če značilke niso skrbno izbrane, lahko v nekaterih situacijah pride do neželenih, nepotrebnih ali celo nepričakovanih ukrepov vodenja — težava, znana kot samovoljnost kamere (angl. camera retreat). Tekom sledenja lahko nekatere značilke zapustijo vidno polje kamere, kar lahko onemogoči dokončanje naloge vodenja na osnovi slike, zato mora algoritem vodenja preprečiti tovrstne situacije. To lahko doseže z uporabo alternativnih značilk, če se nekatere neizogibno izgubijo, ali z oceno lokacije značilk, ki so začasno izven vidnega polja. Glede na definicijo vektorja značilk x lahko vodenja na osnovi slike razvrstimo v tri glavne kategorije [28]: položajno vodenje na osnovi slike (PBVS, angl. position-based visual servoing), direktno vodenje na osnovi slike (IBVS, angl. image-based visual servoing) in hibridno vodenje na osnovi slike (HVS, angl. hybrid visual servoing). V primeru PBVS je regulacijski pogrešek opredeljen kot razlika med želeno in trenutno lego robota v tridimenzionalnem delovnem prostoru. V strukturi vodenja PBVS se kamera uporablja za oceno tridimenzionalnih položajev objektov in robota iz slike, ki je projekcija realnega okolja. Zato je mogoče tudi tukaj neposredno uporabiti vse do zdaj predstavljene načine vodenja. Običajno je mogoče s PBVS doseči optimalne premike, vendar ta pristop zahteva točno kalibracijo kamere, drugače ni možno odpraviti pravega pogreška vodenja. Po drugi strani je regulacijski pogrešek pri IBVS definiran neposredno v dvodimenzionalnem prostoru slike (npr. kot razlika med slikovnima točkama), zato ima točnost kalibracije kamere manjši vpliv na IBVS. Vendar pa IBVS običajno dosega manj optimalne trajektorije, kot jih je mogoče doseči s pristopom PBVS. IBVS je tudi bolj dovzeten za težave, ki jih povzroča samovoljnost kamere (primer kjer se kamera z rotacijo bliža cilju, ga preseže v normalni smeri in se nato vrača nazaj), če značilke niso pravilno izbrane. Pristopi IBVS so še posebej zanimivi, saj omogočajo opredelitev naloge neposredno na sliki in regulacijski pristop, znan kot nauči-s-prikazom (angl. teach-by-showing). HVS skuša združiti dobre lastnosti regulacijskih shem PBVS in IBVS. Nekatere regulacijske sheme preklapljajo med PBVS in IBVS v skladu s kriterijem preklopa, ki na podlagi trenutnih stanj sistema izbere najprimernejši način vodenja [29]. V nekaterih drugih HVS je regulacijski pogrešek sestavljen iz značilk dvodimenzionalnega slikovnega prostora kot tudi značilk tridimenzionalnega delovnega prostora [30, 31]. Še posebej zahtevno je vodenje na osnovi slike neholonomičnih sistemov [32–35]. Sheme vodenja na osnovi slike se včasih uporabljajo v kombinaciji z nekaterimi dodatnimi senzorji [36], ki lahko podajo dodatne podatke ali olajšajo obdelavo slike. V [27, 37] so avtorji predstavili splošni pristop načrtovanja vodenja na osnovi 138 Vodenje kolesnih mobilnih sistemov slike, ki ga je mogoče uporabiti ne glede na njegovo kategorijo. Vzemimo primer, ko je vektor referenčnih značilk v (3.56) časovno nespremenljiv (x ref ( t) = x ref = const.). Običajno se uporabi hitrostni regulator. V tem primeru lahko zapišemo vhodni vektor kot kombinacijo vektorja translatorne hitrosti v( t) in vektorja kotne hitrosti ω( t) v kombiniranem vektorju u T ( t) = [v T ( t) , ω T ( t)] ∈ M R . V splošnem sta tako vektor translatorne hitrosti kot vektor kotne hitrosti nekega objekta (v tridimenzionalnem prostoru) sestavljena iz treh hitrosti: v T ( t) = [ vx( t) , vy( t) , vz( t)] in ω T ( t) = [ ωx( t) , ωy( t) , ωz( t)]. Vendar pa imata v kolesni mobilni robotiki ta dva vhodna vektorja običajno nekaj ničelnih elementov, saj se mobilni robot v normalnih voznih razmerah premika po ravnini in ne pričakujemo prevračanja ali dvigovanja. Razmerje med hitrostjo premika u T ( t) in hitrostjo spreminjanja značilk ˙ s T ( t) lahko zapišemo kot ˙ x( t) = L( t)u( t) (3.57) kjer je matrika L( t) ∈ K M R × R znana kot interakcijska matrika. Iz relacij (3.56) in (3.57) lahko določimo hitrost spreminjanja pogreška ˙ e( t) = −L( t)u( t) (3.58) Regulirni signal u( t) mora minimizirati pogrešek e. Če je zaželeno eksponentno padanje pogreška e v obliki ˙ e( t) = − g e( t), g > 0, lahko izpeljemo sledeči regulacijski zakon u( t) = g L†( t)e( t) (3.59) kjer je L†( t) Moore-Penroseov psevdoinverz interakcijske matrike L( t). Če ima matrika L( t) poln rang, je njen psevdoinverz enak L†( t) = (L T ( t)L( t))−1L T ( t). V primeru da je interakcijska matrika kvadratna ( K = M ) in ni singularna (det(L( t)) 6= 0), se izračun psevdoinverza v (3.59) poenostavi v običajno inverzno matriko L−1( t). V praksi je znana le približna vrednost prave interakcijske matrike. Zato lahko v regulacijskem zakonu (3.59) uporabimo samo oceno interakcijske matrike b L( t) ali njenega psevdoinverza c L†( t) u( t) = g c L†( t)e( t) (3.60) Oceno psevdoinverzne interakcijske matrike c L†( t) lahko določimo z linearizacijo sistema okoli njegovega trenutnega stanja (c L†( t) = b L†( t)) ali okoli njegovega t želenega (referenčnega) stanja (c L†( t) = b L† ( t)). Včasih lahko uporabimo tudi ref kombinacijo v obliki c L†( t) = 1 ( 2 b L ref ( t) + b L t( t))† [37]. Če vstavimo (3.60) v (3.58), dobimo diferencialno enačbo zaprtozančnega sistema ˙ e( t) = − g L( t)c L†( t)e( t) (3.61) Funkcijo Ljapunova V ( t) = 1 e T ( t)e( t) lahko uporabimo za preverjanje stabilnosti 2 zaprtozančnega sistema (3.61). Odvod funkcije Ljapunova je ˙ V ( t) = − g e T ( t)L( t)c L†( t)e( t) 3.3. Vodenje po referenčni trajektoriji 139 Zadosten pogoj za globalno stabilnost sistema (3.61) je izpolnjen, če je matrika L( t)c L†( t) pozitivno definitna. V primeru, da je število opazovanih značilk K enako številu regulirnih veličin M , torej K = M , ter imata matriki L( t) in c L†( t) poln rang, je zaprtozančni sistem (3.61) stabilen, če le ocena matrike c L†( t) ni pregroba [27]. Vendar je potrebno opozoriti, da v primeru IBVS ni preprosto izbrati ustreznih značilk v slikovnem prostoru, ki odpravijo problem vodenja v delovnem prostoru. Primer 3.16 Kamera C je nameščena na kolesnem mobilnem robotu R z diferencialnim pogonom na t R = [0 , 0 , 0 , 5] T z orientacijo R R = R C C x(90°)R y (−90°)R x(45°). Parametri kamere so (model kamere je opisan v poglavju 5.2.4): αxf = αyf = 300, γ = 0 (brez striga), središče slike pa je v središču slike z dimenzijo 1024 krat 768. Načrtajte IBVS, ki vodi mobilnega robota iz začetne lege [ x(0) , y(0) , ϕ(0)] = [1 m , 0 m , 100°] do ciljne pozicije xref = 4 m in yref = 4 m. Zavoljo poenostavitve predpostavimo, da je na sliki vidno središče vrtenja mobilnega robota. Rešitev Kamera opazuje prizor pred mobilnim robotom. Ker je kamera nameščena brez možnosti zasuka okoli središča optične osi, uporabimo razklopljeno vodenje (angl. decoupled control) neposredno na podlagi slikovnega pogreška med opazovano ciljno pozicijo in sliko središča rotacije robota. Možna implementacija rešitve v Matlabu je prikazana v programu 3.15. Dobljena pot mobilnega robota je prikazana na sliki 3.36, pot opazovanega cilja na sliki pa je prikazana na 3.37. Regulirni signali so prikazani na sliki 3.38. Rezultati potrjujejo uporabnost IBVS. Robot doseže ciljno lego, saj je opazovana značilka (cilj) ves čas vidna (slika 3.37). Program 3.15 ./src/ctr/example_diff_vs_point.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :15; % Č as simulacije 3 r = 0.5; % Razdalja vmesne to č ke od cilja 4 dTol = 1; % Toleran č na razdalja od vmesne to č ke za preklop 5 qRef = [4; 4; 0]; % Referen č na lega 6 q = [1; 0; 100/180* pi ]; % Za č etna lega 7 8 % Kamera 9 alphaF = 300; % % alpha *f , v px / m 10 s = [1024; 768]; % Dimenzije zaslona , v px 11 c = s /2; % Opti č no sredi š če , v px 12 S = [ alphaF , 0 , c (1); 0 , alphaF , c (2); 0 , 0 , 1]; % Notranji model kamere 13 RL2C = rotX ( pi /2)* rotY ( - pi /2)* rotX ( pi /4); tL2C =[ 0;0 ;0. 5] ; % Lega kamere 14 % Simulacija kamere 15 p0P = S * RL2C . ’*([0; 0; 0] - tL2C ); p0P = p0P / p0P (3); 16 RW2L = rotZ ( - q (3)); tW2L = [ q (1:2); 0]; 140 Vodenje kolesnih mobilnih sistemov 17 pP = S * RL2C . ’*( RW2L . ’*([ qRef (1:2); 0] - tW2L ) - tL2C ); pP = pP / pP (3); 18 19 u = [0; 0]; 20 for k = 1: length ( t ) 21 if pP (1) <0 || pP (2) <0 || pP (1) > s (1) || pP (2) > s (2) % N e v i d n a zna č i l k a 22 u = [0; 0]; % S l e d e n a zna č i l k a je i z g u b l j e n a 23 e l s e 24 D = s q r t ( sum (( pP (1:2) - p0P ( 1 : 2 ) ) . ^ 2 ) ) ; 25 if D < d T o l % U s t a v i t e v v bli ž ini c i l j a 26 u = [0; 0]; 27 e l s e 28 u = [0 , 0 . 0 0 2 ; 0.005 , 0 ] * ( p0P (1:2) - pP ( 1 : 2 ) ) ; 29 end 30 end 31 32 % S i m u l a c i j a g i b a n j a r o b o t a 33 dq = [ u ( 1 ) * cos ( q ( 3 ) ) ; u ( 1 ) * sin ( q ( 3 ) ) ; u ( 2 ) ] ; 34 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 35 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 36 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 37 38 % S i m u l a c i j a k a m e r e 39 R W 2 L = r o t Z ( - q ( 3 ) ) ; t W 2 L = [ q ( 1 : 2 ) ; 0]; 40 pP = S * R L 2 C . ’*( R W 2 L . ’*([ q R e f ( 1 : 2 ) ; 0] - t W 2 L ) - t L 2 C ); pP = pP / pP ( 3 ) ; 41 end 5 4 Cilj 3 ] [m 2 y 1 Start 0 −1 0 2 4 6 x [m] Slika 3.36: Pot pridobljena z IBVS iz primera 3.16 3.3. Vodenje po referenčni trajektoriji 141 x [px] 0 1024 0 ] [pxy 768 Slika 3.37: Pot opazovane značilke (cilja) v slikovnem prostoru iz primera 3.16 1 s]/ [m 0.5 v 00 5 10 15 t [s] 0 s]−0.5 / [rad −1 ω−1.5 −20 5 10 15 t [s] Slika 3.38: Regulirna signala IBVS iz primera 3.16 142 Vodenje kolesnih mobilnih sistemov 3.4 Ocena optimalnega profila hitrosti za znano pot Kolesni mobilni roboti se morajo pogosto voziti po obstoječi vnaprej določeni poti (npr. po cesti ali koridorju), ki je prostorsko določena z nekim parametrom u kot xp( u), yp( u), u ∈ [ uSP , uEP ]. Za vožnjo robota po tej poti je potrebno določiti želeni hitrostni profil, če mora robot v najkrajšem času priti od neke začetne točke (SP) do neke končne točke (EP) in pri tem upoštevati zmožnosti robota ipd. Za vožnjo robota po takšni poti mora biti njegova pozicija odvisna od časa x( t), y( t) z določenim hitrostnim profilom v( t), ω( t), kot bo prikazano v nadaljevanju. Predpostavimo poseben primer u = t, kjer je referenčna pot v bistvu trajektorija z implicitno podanim hitrostnim profilom. Robot mora voziti po trajektoriji z referenčnima hitrostma v( t) = vref ( t) in ω( t) = ωref ( t), izračunani iz referenčne trajektorije z uporabo relacij (3.17) in (3.18). Pri načrtovanju želenega hitrostnega profila za prostorsko podano pot, je potrebno najti razpored u = u( t). Načrtovani hitrostni profil mora biti skladen z omejitvami robota, kot sta največja hitrost in pospešek, ki ju lahko proizvajajo motorji ali pa ki zagotavljata varno vožnjo brez vzdolžnega in bočnega drsenja koles. Referenčne hitrosti so podobno kot v (3.17) in (3.18), izražene kot q v( t) = x 0 ( u ( t))2 + y 0 ( u ( t))2 ˙ u ( t) = v p p p ( u) ˙ u ( t) (3.62) 0 00 0 00 x ( u( t)) y ( u( t)) − y ( u( t)) x ( u( t)) p p p p ω( t) = ˙ u( t) = ωp ( u) ˙ u ( t) x 0 ( u( t))2 + y 0 ( u( t))2 p p in ukrivljenost je 0 00 0 00 x ( u( t)) y ( u( t)) − y ( u( t)) x ( u( t)) p p p p κ( t) = = κp ( u) x 0 ( u( t))2 + y 0 ( u( t))23 / 2 p p kjer črtice označujejo odvode po u, pike pa odvode po t. Časovni odvodi poti 0 upoštevajo razpored u( t) na način d x( t) = d xp d u = x ˙ u ( t), d y( t) = d yp d u = d t d u d t p d t d u d t 0 y ˙ u ( t). p Glavna ideja načrtovanja hitrostnega profila je povzeta iz [38], ki se določi glede na omejitev idealnega kotaljenja. To pomeni, da so regulirne hitrosti enake dejanskim hitrostim robota (brez drsenja koles), kar dosežemo z omejitvijo celotnega dovoljenega pospeška q a = a 2 + a 2 t r kjer sta d v at = ar = vω = v 2 κ (3.63) d t 3.4. Ocena optimalnega profila hitrosti za znano pot 143 tangencialni in radialni pospešek. Največja vrednost pospeška, ki preprečuje drsenje, je določena s silo trenja Ffric Ffric mgcfric aMAX = = = gcfric m m kjer je m masa vozila, g gravitacijski pospešek in cfric koeficient trenja. Največji tangencialni pospešek aMAXt in radialni pospešek aMAXr se zaradi konstrukcije vozila ponavadi razlikujeta in ju je mogoče eksperimentalno oceniti. Pomembno je, da se skupni pospešek nahaja znotraj elipse a 2 a 2 t + r ≤ 1 (3.64) a 2 a 2 M AXt M AXr ali pa, v primeru časovno optimalnega načrtovanja, na njenem robu. V zavojih na poti mora robot zaradi večjih radialnih pospeškov voziti počasneje. Zato za oceno razporeda u( t) najprej na poti označimo prelomne točke (TP, angl. turning points), kjer je absolutna vrednost ukrivljenosti lokalno največja. Parameter u je v intervalu [ uSP , uEP ]. Pozicije TP so označene z u = uT P i, kjer je i = 1 , . . . , nT P in nT P je število TP. V TP je translatorna hitrost lokalno najmanjša, tangencialni pospešek naj bi bil 0, radialni pospešek pa je največji. Tangencialno hitrost v TP lahko glede na (3.63) izračunamo kot r aMAXr vp( uT P i) = (3.65) | κ( uT P i)| Pred in po TP se lahko robot premika hitreje, ker je ukrivljenost manjša kot v TP. Zato mora robot pred vsako TP upočasniti ( u < uT P i) in po njej pospešiti ( u > uT P i) v skladu z omejitvijo pospeška (3.64). Iz (3.62) sledi, da sta v( t) in vp( u) v vsaki fiksni točki na poti sorazmerni s časovno odvisnim proporcionalnim faktorjem ˙ u( t). Najkrajši izvedljiv hitrostni profil je torej določen z odvodom razporeda ˙ u( t), kjer minimiziramo največje hitrostne profile, ki izpolnjujejo omejitve pospeška, kot je opisano v nadaljevanju. Radialni in tangencialni pospešek sta izražena iz (3.63) z upoštevanjem (3.62) kot 0 0 ar( t) = x ( u)2 + y ( u)2 κ ( u) κ p p p ( u) ˙ u 2 ( t) = v 2 p p ( u) ˙ u 2 ( t) (3.66) 0 00 0 00 x ( u) x ( u) + y ( u) y ( u) q p p p p at( t) = ˙ u 2( t) + x 0 ( u)2 + y 0 ( u)2 ¨ u 2 ( t) q p p x 0 ( u)2 + y 0 ( u)2 p p (3.67) d vp ( u) = ˙ u 2 ( t) + vp ( u) ¨ u ( t) d u kjer je zaradi krajšega zapisa izpuščena odvisnost od časa za u( t). Z mejnim primerom (3.64) in (3.66) iz (3.67) dobimo optimalno diferencialno enačbo razporeda s 0 00 0 00 1 ( x 02 + y 02) κ 2 ˙ u 4 x x + y y p p p p p p p ¨ u = ± aMAXt − − ˙ u 2 (3.68) x 02 + y 02 a 2 x 02 + y 02 p p M AXr p p 144 Vodenje kolesnih mobilnih sistemov Rešitev diferencialne enačbe najdemo z eksplicitno simulacijo z integracijo iz TP naprej in nazaj po času. Pri pospeševanju uporabimo pozitiven predznak, pri upočasnjevanju pa negativen predznak. Začetna pogoja u( t) in ˙ u sta določena s položajem vseh TP uT P i in z (3.66), ki pove, da je največja dovoljena vrednost radialnega pospeška v TP enaka s aMAXr ˙ u| T P i = (3.69) x 0 ( u ( u p T P i)2 + y 0 p T P i)2 κp ( uT P i) Prikazan je le pozitivni začetni pogoj (3.69), ker mora biti u( t) strogo naraščajoča funkcija. Diferencialne enačbe rešujemo, dokler ne najdemo kršitve omejitve pospeška ali pa u zapusti interval [ uSP , uEP ]. Kršitev se običajno pojavi pri (pospešenem) premikanju iz trenutne TP i proti naslednji TP ( i − 1 ali i + 1) in vrednost translatorne hitrosti močno preseže največjo dovoljeno vrednost, ki jo določa trajektorija. Rešitev diferencialne enačbe (3.68) je tako sestavljena iz segmentov ˙ u okoli vsake TP ˙ ul = ˙ ul( u) ; u ∈ [ u , u l l] , l = 1 , . . . , nT P (3.70) kjer je ˙ ul = ˙ ul ( u ( t)) odvod razporeda, odvisen od u in u , u l l pa so meje l-tega segmenta. Tu so segmenti v (3.70) podani kot funkcije od u, čeprav je simulacija funkcije (3.68) izvedena v času, ker časovni zamik (čas, potreben za prihod v TP) ni znan. Kar je znano na tej točki, je relativni časovni interval, ki ustreza rešitvi vsakega segmenta ˙ ul. Rešitev časovno optimalnega hitrostnega profila, pridobljena s pospeševanjem na meji zdrsa (maksimalno pospeševanje) celotne trajektorije, je možna, če unija intervalov pokriva celoten interval zanimanja [ uSP , uEP ] ⊆ S nT P [ u , u l=1 l l]. Dejanski ˙ u najdemo z minimizacijo posameznih maksimalnih profilov v okolici TP-jev ˙ u = min ˙ ul( u) (3.71) 1≤ l≤ nT P Absolutni čas, ki ustreza razporedu u( t), dobimo iz pretvorbe ˙ u ( u ( t)) = d u , da d t dobimo dt = ˙ u−1( u) du in izvedemo integracijo Z uEP d u t = = t( u) u ˙ u( u) SP kjer ˙ u( u) ne sme postati 0. V našem primeru je iskana časovno optimalna rešitev, zato je hitrost (tudi ˙ u( u)) vedno večja od 0. Izraz ˙ u( u) = 0 bi namreč pomenil, da je hitrost enaka nič, ko bi se sistem ustavil – to pa ne more voditi do časovno optimalne rešitve. Začetne (v SP) in končne (v EP) hitrosti se lahko obravnavajo na naslednji način. SP in EP obravnavamo kot običajne TP, kjer so začetne hitrosti vSP , vp( uSP ), vEP , vp( uEP ) znane, zato lahko izračunamo začetne pogoje za ˙ uSP = vSP , vp( uSP ) ˙ uEP = vEP . Če so ti začetni pogoji za SP ali EP večji od rešitve za ˙ u vp( uEP ) (upoštevajoč vse TP), rešitev ne obstaja, ker bi bilo nemogoče priti skozi prvo 3.4. Ocena optimalnega profila hitrosti za znano pot 145 TP, tudi če robot maksimalno (popolnoma) zavira. Če rešitev za dane hitrosti v SP in EP obstaja, vključimo tudi segmente ˙ u, izračunane za SP in EP v (3.70) in (3.71). Iz izračunanega razporeda u( t), ˙ u( t), je referenčna trajektorija podana kot x( t) = xp ( u ( t)) in y( t) = yp ( u ( t)) ter referenčna hitrost z (3.17) in (3.18). Primer 3.17 Izračunajte optimalni hitrostni profil, ki bo omogočil najkrajši čas potovanja po poti xp( u) = cos( u), yp( u) = sin(2 u) (na sliki 3.39), kjer je u ∈ [0 , 2 π], ter upošteval največji tangencialni aMAXt = 2 m / s2 in radialni pospešek aMAXr = 4 m / s2. 1 0.5 ] [m 0 y −0.5 −1 −1 −0.5 0 0.5 1 x [m] Slika 3.39: Pot . Izračunajte razpored u( t) in hitrostna profila v( u), v( t). Rešitev Za izračun optimalnega razporeda u( t) uporabimo algoritem, predstavljen v tem poglavju. Najprej moramo izračunati vse TP z začetnimi pogoji, ki so podani v (3.65) in (3.69), nato pa simulirati rešitve za vsako TP z uporabo (3.68). Končno minimiziramo časovni odvod razporeda glede na (3.71). 146 Vodenje kolesnih mobilnih sistemov Možna izvedba rešitve je podana v programu 3.16 (zanemarite dele kode za omejevanje hitrosti, ki bi jih lahko ovrednotili, če bi bila spremenljivka velCnstr nastavljena na true). Optimalna določitev razporeda za primer 3.17 je prikazana na slikah od 3.40 do 3.42. Iz slike 3.40 je razvidno, da je rešitev za vsako TP integrirana vse do točke, kjer so kršene omejitve pospeška, kar je označeno s tanko linijo. Končna rešitev je označena z odebeljeno krivuljo. Program 3.16 ./src/ctr/example_velocity_profile_planning.m 1 % Definicija t r a j e k t o r i j e v obliki funkcij 2 x = @ ( u ) cos ( u ); y = @ ( u ) sin (2* u ); % Pot 3 dx = @ ( u ) - sin ( u ); dy = @ ( u ) 2* cos (2* u ); % P r v i o d v o d 4 ddx = @ ( u ) - cos ( u ); ddy = @ ( u ) -4* sin (2* u ); % Drugi odvod 5 v = @ ( u ) sqrt ( dx ( u ).^2 + dy ( u ).^2); % T a n g e n c i a l n a hitrost 6 w = @ ( u ) ( dx ( u ).* ddy ( u ) - dy ( u ).* ddx ( u ))./( dx ( u ).^2+ dy ( u ).^2); % Kotna hitrost 7 kappa = @ ( u ) w ( u )./ v ( u ); % U k r i v l j e n o s t 8 dv = @ ( u ) ( dx ( u ).* ddx ( u ) + dy ( u ).* ddy ( u ))./ v ( u ); 9 10 u = 0:0.001:2* pi ; % Č as 11 arMax = 4; atMax = 2; % Omejitvi pospe š ka 12 vSP = 0.2; vEP = 0.1; % Za č etna in kon č na hitrost 13 uSP = u (1); uEP = u ( end ); % Za č etni in kon č ni polo ž aj 14 uTP = []; % To č ke zavojev 15 for i = 2: length ( u ) -1 % Dolo č itev to č k zavojev 16 if all ( abs ( k a p p a ( u ( i ))) > abs ( k a p p a ( u ([ i -1 , i + 1 ] ) ) ) ) 17 uTP = [ uTP , u ( i )]; 18 end 19 end 20 up0 = sqrt ( arMax ./ abs ( v ( uTP ).* w ( uTP ))) ; % Odvodi v to č kah zavojev 21 22 velCnstr = false ; % Omogo č i omejitev hitrosti 23 if velCnstr 24 v M a x = 1 . 5 ; % O m e j i t e v h i t r o s t i 25 for i = 1: l e n g t h ( uTP ) % P r i l a g o d i uTP g l e d e na h i t r o s t n o o m e j i t e v 26 vvu = v ( uTP ( i )); vvt = vvu * up0 ( i ); 27 if abs ( vvt ) > vMax , up0 ( i ) = abs ( v M a x / vvu ); end 28 end 29 % D o d a j z a h t e v o za za č e t n o in kon č no h i t r o s t 30 uTP = [ uSP , uTP , uEP ]; up0 = [ vSP / v ( uSP ) , up0 , vEP / v ( uEP )]; 31 end 32 33 Ts = 0.001; % Ra č unski korak 34 N = length ( uTP ); ts = cell (1 , N ); us = cell (1 , N ); ups = cell (1 , N ); 35 for i = 1: N % Zanka č ez vse to č ke zavojev 36 uB = uTP ( i ); upB = up0 ( i ); tB = 0; 37 uF = uTP ( i ); upF = up0 ( i ); tF = 0; 38 uBs = [ ] ; u p B s = []; tBs = []; uFs = []; u p F s = [ ] ; tFs = []; % S p o m i n 39 goB = t r u e ; goF = t r u e ; 40 41 w h i l e goB || goF 42 % I n t e g r a c i j a n a z a j od to č ke z a v o j a 43 if uB > uSP && goB 44 dxT = dx ( uB ); dyT = dy ( uB ); 45 d d x T = ddx ( uB ); d d y T = ddy ( uB ); 46 vT = v ( uB )* upB ; wT = w ( uB )* upB ; k a p p a T = k a p p a ( uB ); 47 arT = vT * wT ; atT = a t M a x * s q r t (1 - ( arT / a r M a x ) ^ 2 ) ; 48 49 if v e l C n s t r && abs ( vT ) > v M a x 3.4. Ocena optimalnega profila hitrosti za znano pot 147 50 upB = v M a x / v ( uB ); upp = - dv ( uB )* upB ^2/ v ( uB ); 51 e l s e i f abs ( arT ) - a r M a x > 0 . 0 0 1 52 arT = a r M a x ; atT = 0; upp = 0; goB = f a l s e ; 53 e l s e 54 atT = - r e a l ( atT ); 55 upp = r e a l ( - a t M a x * s q r t ( 1 / ( dxT ^2 + dyT ^2) - ... 56 ( dxT ^2 + dyT ^ 2 ) * k a p p a T ^2* upB ^4/ a r M a x ^2) - ... 57 ( dxT * d d x T + dyT * d d y T )/( dxT ^2 + dyT ^2) * upB ^ 2 ) ; 58 end 59 60 uBs = [ uBs ; uB ]; u p B s = [ u p B s ; upB ]; tBs = [ tBs ; tB ]; % S h r a n j e v a n j e 61 tB = tB + Ts ; 62 uB = uB - upB * Ts ; % E u l e r j e v a i n t e g r a c i j a 63 upB = upB - upp * Ts ; % E u l e r j e v a i n t e g r a c i j a 64 e l s e 65 goB = f a l s e ; 66 end 67 68 % I n t e g r a c i j a n a p r e j of to č ke z a v o j a 69 if uF < uEP && goF 70 dxT = dx ( uF ); dyT = dy ( uF ); 71 d d x T = ddx ( uF ); d d y T = ddy ( uF ); 72 vT = v ( uF )* upF ; wT = w ( uF )* upF ; k a p p a T = k a p p a ( uF ); 73 arT = vT * wT ; atT = a t M a x * s q r t (1 - ( arT / a r M a x ) ^ 2 ) ; 74 75 if v e l C n s t r && abs ( vT ) > v M a x 76 upF = v M a x / v ( uF ); upp = - dv ( uF )* upF ^2/ v ( uF ); 77 e l s e i f abs ( arT ) - a r M a x > 0 . 0 0 1 78 arT = a r M a x ; atT = 0; upp = 0; goF = f a l s e ; 79 e l s e 80 atT = r e a l ( atT ); 81 upp = r e a l (+ a t M a x * s q r t ( 1 / ( dxT ^2 + dyT ^2) - ... 82 ( dxT ^2 + dyT ^ 2 ) * k a p p a T ^2* upF ^4/ a r M a x ^2) - ... 83 ( dxT * d d x T + dyT * d d y T )/( dxT ^2 + dyT ^2) * upF ^ 2 ) ; 84 end 85 86 uFs = [ uFs ; uF ]; u p F s = [ u p F s ; upF ]; tFs = [ tFs ; tF ]; % S h r a n j e v a n j e 87 tF = tF + Ts ; 88 uF = uF + upF * Ts ; % E u l e r j e v a i n t e g r a c i j a 89 upF = upF + upp * Ts ; % E u l e r j e v a i n t e g r a c i j a 90 e l s e 91 goF = f a l s e ; 92 end 93 end 94 95 ts { i } = [ tBs ; tB + tFs (2: end )]; 96 us { i } = [ f l i p u d ( uBs ); uFs (2: end )]; 97 ups { i } = [ f l i p u d ( u p B s ); u p F s (2: end )]; 98 end 99 100 % Iskanje minimuma med vsemi profili 101 usOrig = us ; 102 for i = 1: N -1 103 d = ups { i +1} - i n t e r p 1 ( us { i } , ups { i } , us { i + 1 } ) ; 104 j = f i n d ( d (1: end - 1 ) . * d (2: end ) <0 , 1); % ups { i } je p r i b l i ž no e n a k ups { i +1} 105 % I s k a n j e b o l j n a t a n č n e g a u - ja , k j e r sta p r o f i l a ups { i } in ups { i +1} e n a k a 106 uj = us { i + 1 } ( j ) + ( us { i + 1 } ( j +1) - us { i + 1 } ( j ) ) / ( d ( j +1) - d ( j ))*(0 - d ( j )); 107 rob = i n t e r p 1 ( us { i } , ups { i } , uj ); 108 109 k e e p = us { i } < uj ; 110 us { i } = [ us { i }( k e e p ); uj ]; ups { i } = [ ups { i }( k e e p ); rob ]; 111 k e e p = us { i +1} > uj ; 112 us { i +1} = [ uj ; us { i + 1 } ( k e e p )]; ups { i +1} = [ rob ; ups { i + 1 } ( k e e p )]; 148 Vodenje kolesnih mobilnih sistemov 113 end 114 115 % K o n s t r u k c i j a kon č ne re š itve 116 tt = interp1 ( usOrig {1} , ts {1} , us {1}); uu = us {1}; uup = ups {1}; 117 for i = 2: N 118 ti = i n t e r p 1 ( u s O r i g { i } , ts { i } , us { i }); 119 tt = [ tt ; ti + tt ( end ) - ti ( 1 ) ] ; 120 uu = [ uu ; us { i } + uu ( end ) - us { i } ( 1 ) ] ; 121 uup = [ uup ; ups { i }]; 122 end 123 vv = v ( uu ).* uup ; 1.8 1.6 s] 1.4 ˙[1/u 1.2 T P1 T P2 T P3 T P4 1 0.80 1 2 3 4 5 6 u [1] Slika 3.40: Optimalno določen razpored, ki poišče najmanjši profil ˙ u vseh prelo- mnih točk (TP) 6 4 [1]u 2 00 1 2 3 4 5 6 t [s] Slika 3.41: Optimalni razpored u( t), ki je nelinearna funkcija, čeprav je v tem primeru videti skoraj linearna 3.4. Ocena optimalnega profila hitrosti za znano pot 149 2.5 2 s]/ [m 1.5 v 1 0.50 1 2 3 4 5 6 t [s] 2.5 2 s]/ [m 1.5 v 1 0.50 1 2 3 4 5 6 u [1] Slika 3.42: Optimalni hitrosti v( u) in v( t) Primer 3.18 Razširite primer 3.17, da vključuje tudi zahteve za začetno in končno hitrost: vSP = 0 , 2 m / s in vEP = 0 , 1 m / s. Poleg tega upoštevajte, da je največja hitrost omejena na vMAX = 1 , 5 m / s. Izračunajte razpored u( t) in profile hitrosti v( u), v( t). Rešitev Kodo iz primera 3.17 je mogoče spremeniti tako, da vsebuje dodatne zahteve. Zahteve za začetno in končno hitrost se obravnavajo podobno kot v drugih TP. SP in EP sta obravnavani kot novi TP, katerih začetni pogoji so uSP = 0, uEP = 2 π, ˙ uSP = vSP in ˙ u . v EP = vEP p( uSP ) vp( uEP ) Omejitve hitrosti se upoštevajo, če je spremenljivka velCnstr v programu 3.16 nastavljena na true. Optimalna določitev razporeda za primer 3.18 je prikazana na slikah 3.43 – 3.45. 150 Vodenje kolesnih mobilnih sistemov 1.5 T P2 T P3 T P4 T P5 1 s] ˙[1/u 0.5 SP EP 00 1 2 3 4 5 6 u [1] Slika 3.43: Optimalno določen razpored, ki poišče najmanjši ˙ u vseh prelomnih točk (TP) 6 4 [1]u 2 0 1 2 3 4 5 6 7 8 t [s] Slika 3.44: Optimalni razpored u( t), ki je nelinearna funkcija, čeprav je v tem primeru videti skoraj linearna 3.4. Ocena optimalnega profila hitrosti za znano pot 151 1.5 s] 1 / [mv0.5 0 1 2 3 4 5 6 7 8 t [s] 1.5 s] 1 / [mv0.5 00 1 2 3 4 5 6 u [1] Slika 3.45: Optimalni hitrosti v( u) in v( t) 152 Vodenje kolesnih mobilnih sistemov Literatura [1] R. W. Brockett. Asymptotic stability and feedback stabilization. V R. S. Millman in H. J. Sussmann (Uredniki), Differential Geometric Control Theory, str. 181–191. Birkhuser, Boston, MA, 1983. [2] M. Bowling in M. Veloso. Motion control in dynamic multi-robot environments. V IEEE International Symposium on, CIRA ’99, str. 168–173. IEEE, Monterey, CA, 1999. [3] L. E. Dubins. On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents. American Journal of Mathematics, zv. 79, št. 3, str. 497–516, 1957. [4] J. A. Reeds in L. A. Shepp. Optimal paths for a car that goes both forward and backward. Pacific Journal of Mathematics, zv. 145, št. 2, str. 367–393, 1990. [5] C. C. de Wit in O. J. Sordalen. Exponential stabilization of mobile robots with nonholonomic constraints. Proceedings of the 30th IEEE Conference on Decision and Control, zv. 37, št. 11, str. 1791–1797, 1991. [6] A. De Luca, G. Oriolo in M. Vendittelli. Control of Wheeled Mobile Robots: An Experimental Overview. V S. Nicosia, B. Siciliano, A. Bicchi in P. Valigi (Uredniki), Ramsete, Lecture Notes in Control and Information Sciences, zv. 270, str. 181–226. Springer Berlin Heidelberg, 2001. [7] A. Zdešar. Simulacijsko okolje za analizo in sintezo algoritmov vodenja na osnovi digitalne slike. Bsc, Univerza v Ljubljani, 2010. [8] B. Zupančič. Zvezni regulacijski sistemi. Fakulteta za elektrotehniko, 2010. [9] Y. Kanayama, A. Nilipour in C. A. Lelm. A locomotion control method for autonomous vehicles. V Robotics and Automation, 1988. Proceedings., 1988 IEEE International Conference on, zv. 2, str. 1315–1317. 1988. [10] Y. Kanayama, Y. Kimura in sod. A stable tracking control method for an autonomous mobile robot. V Proceedings of the 1990 IEEE International Conference on Robotics and Automation, str. 384–389. IEEE, Cincinnati, OH, 1990. [11] C. Samson. Time-varying Feedback Stabilization of Car-like Wheeled Mobile Robots. International Journal of Robotics Research, zv. 12, št. 1, str. 55–64, 1993. [12] P. A. Ioannou in J. Sun. Robust Adaptive Control. št. let. 1 v Control theory. PTR Prentice-Hall, 1996. [13] S. Blažič. On Periodic Control Laws for Mobile Robots. IEEE Transactions on Industrial Electronics, zv. 61, št. 7, str. 3660–3670, 2014. [14] S. Blažič. A novel trajectory-tracking control law for wheeled mobile robots. Robotics and Autonomous Systems, zv. 59, št. 11, str. 1001–1007, 2011. [15] T. Takagi in M. Sugeno. Fuzzy identification of systems and its applications to modeling and control. IEEE Transactions on Systems, Man, and Cybernetics, zv. SMC-15, št. 1, str. 116–132, 1985. [16] K. Tanaka in M. Sano. A robust stabilization problem of fuzzy control systems and its application to backing up control of a truck-trailer. Fuzzy Systems, IEEE Transactions on, zv. 2, št. 2, str. 119–134, 1994. [17] H. O. Wang, K. Tanaka in M. F. Griffin. An approach to fuzzy control of nonlinear systems: stability and design issues. IEEE Transactions on Fuzzy Systems, zv. 4, št. 1, str. 14–23, 1996. [18] H. D. Tuan, P. Apkarian in sod. Parameterized linear matrix inequality techniques in Literatura 153 fuzzy control system design. IEEE Transactions on fuzzy systems, zv. 9, št. 2, str. 324–332, 2001. [19] A. Ollero in O. Amidi. Predictive path tracking of mobile robots. application to the CMU navlab. V Proceedings of 5th International Conference on Advanced Robotics, Robots in Unstructured Environments, ICAR, zv. 91, str. 1081–1086. 1991. [20] J. E. Normey-Rico, J. Gómez-Ortega in E. F. Camacho. A Smith-predictor-based genera-lised predictive controller for mobile robot path-tracking. Control Engineering Practice, zv. 7, št. 6, str. 729–740, 1999. [21] F. Kuhne, W. F. Lages in J. M. G. da Silva Jr. Model predictive control of a mobile robot using linearization. V Proceedings of mechatronics and robotics, str. 525–530. 2004. [22] D. Gu in H. Hu. Neural predictive control for a car-like mobile robot. Robotics and Autonomous Systems, zv. 39, št. 2, str. 73–86, 2002. [23] G. Klančar in I. Škrjanc. Tracking-error model-based predictive control for mobile robots in real time. Robotics and Autonomous Systems, zv. 55, št. 6, str. 460–469, 2007. [24] J. Kennedy in R. Eberhart. Particle Swarm Optimization. V Proceedings of IEEE International Conference on Neural Networks, str. 1942–1948. IEEE, 1995. [25] J. Kennedy in R. Eberhart. Swarm Intelligence. Morgan Kaufmann, 2001. [26] J. Kennedy in R. Mendes. Neighborhood topologies in fully informed and best-of-neighborhood particle swarms. IEEE Transactions on Systems, Man, and Cybernetics, Part C: Applications and Reviews, zv. 36, št. 4, str. 515–519, 2006. [27] F. Chaumette in S. Hutchinson. Visual servo control, part I: basic approaches. IEEE Robotics and Automation Magazine, zv. 13, št. 4, str. 82–90, 2006. [28] P. I. Corke. Visual control of robots: high-performance visual servoing. Wiley, New York, 1996, 353 str. [29] N. R. Gans in S. A. Hutchinson. Stable visual servoing through hybrid switched-system control. IEEE Transactions on Robotics, zv. 23, št. 3, str. 530–540, 2007. [30] E. Malis, F. Chaumette in S. Boudet. 2-1/2-D visual servoing. IEEE Transactions on Robotics and Automation, zv. 15, št. 2, str. 238–250, 1999. [31] E. Malis in F. Chaumette. 2 1/2 D visual servoing with respect to unknown objects through a new estimation scheme of camera displacement. International Journal of Computer Vision, zv. 37, št. 1, str. 79–97, 2000. [32] D. Fontanelli, P. Salaris in sod. Unicycle-like Robots with Eye-in-Hand Monocular Cameras : From PBVS towards IBVS. V G. Chesi in K. Hashimoto (Uredniki), Visual Servoing via Advanced Numerical Methods, Lecture Notes in Control and Information Sciences, str. 335–360. Springer, London, 2010. [33] A. De Luca, G. Oriolo in P. R. Giordano. Image-based visual servoing schemes for nonholonomic mobile manipulators. Robotica, zv. 25, št. 02, str. 131–145, 2007. [34] G. L. Mariottini. Image-based Visual Servoing for Mobile Robots: the Multiple View Geometry Approach. Phd, University of Siena, 2005. [35] G. Chesi in K. Hashimoto. Visual Servoing via Advanced Numerical Methods. Springer-Verlag, Berlin Heidelberg, 2010, 398 str. [36] A. Cherubini in F. Chaumette. Visual navigation of a mobile robot with laser-based collision avoidance. The International Journal of Robotics Research, zv. 32, št. 2, str. 189–205, 2012. [37] F. Chaumette in S. Hutchinson. Visual servo control, part II: advanced approaches. IEEE Robotics and Automation Magazine, zv. 14, št. 1, str. 109–118, 2007. 154 Vodenje kolesnih mobilnih sistemov [38] M. Lepetič, G. Klančar in sod. Time optimal path planning considering acceleration limits. Robotics and Autonomous Systems, zv. 45, št. 3, str. 199–210, 2003. 4 Načrtovanje poti 4.1 Uvod Načrtovanje poti od točke A do točke B, hkratno izogibanje oviram in upošteva- nje sprememb v okolju so za človeka enostavne naloge, medtem ko kolesnemu mobilnemu robotu predstavljajo izziv, ki ga mora (vsaj delno) premagati, da postane avtonomen. Robot s pomočjo senzorjev z določeno negotovostjo zaznava prostor okoli sebe in tako izdeluje ali dopolnjuje svoj zemljevid okolice. Za izračun premikov do cilja se s pomočjo algoritmov odloča in načrtuje potrebne akcije. Pri tem je potrebno upoštevati dinamične in kinematične omejitve robota. Načrtovanje poti se uporablja za reševanje problemov na različnih področjih, od preprostega načrtovanja poti znotraj znanega okolja do določitve ustreznega zaporedja premikov za doseg cilja. Načrtovanje poti je pogosto omejeno na vnaprej zgrajena okolja in okolja, ki jih lahko vnaprej dovolj dobro opišemo. Načrtovanje poti se lahko uporablja v okoljih, ki so v celoti ali delno poznana, ter v popolnoma neznanih okoljih, kjer zaznane informacije določajo želeno gibanje robota. Načrtovanje poti v znanih okoljih je aktualno področje raziskovanja, ki je temelj kompleksnejših primerov, kjer okolje ni znano vnaprej. V nadaljevanju so predstavljene najpogosteje uporabljene metode načrtovanja poti za kolesne mobilne robote. Za nadaljnje branje o metodah načrtovanja poti glejte [1–3]. Najprej podajmo definicije nekaj osnovnih pojmov pri načrtovanju poti. 156 Načrtovanje poti goal goal planned path start start space with obstacles a) b) Slika 4.1: (a) Okolica robota z ovirami ter začetno in ciljno konfiguracijo; (b) ena izmed možnih poti od začetne do ciljne konfiguracije 4.1.1 Okolica robota Robot se giblje v okolju, ki je sestavljeno iz prostega območja in območja z ovirami (slika 4.1). V prostem območju se nahajata začetna in ciljna konfiguracija – množica parametrov, ki določa robota v prostoru. Parametri običajno vključujejo pozicijo in orientacijo robota, lahko pa tudi zasuke v njegovih sklepih. Število teh parametrov je enako številu prostostnih stopenj robota. Okolje, ki vsebuje premikajoče se ovire, imenujemo dinamično okolje, okolje, ki se časovno ne spreminja, pa statično okolje. Pri znanem okolju je pozicija ovir vnaprej znana. V nasprotnem primeru govorimo o neznanem okolju. 4.1.2 Načrtovanje poti Načrtovanje poti je proces iskanja zvezne poti, ki bo robota pripeljala od začetne do ciljne konfiguracije, tako da bo njegova celotna pot ležala v prostem območju, kot je prikazano na sliki 4.1. Pri načrtovanju poti mobilni sistem uporablja zemljevid okolja, ki je shranjen v njegovem spominu. Stanje (ali konfiguracija) podaja možno lego mobilnega robota v okolju. Predstavimo ga lahko kot točko v konfiguracijskem prostoru, ki vključuje vsa možna stanja robota. Robot lahko preide iz enega stanja v drugo s pomočjo različnih akcij. Ustrezno pot opišemo z zaporedjem akcij, ki vodijo robota od začetne konfiguracije (oz. stanja) skozi nekaj vmesnih konfiguracij, potrebnih za doseg ciljne konfiguracije (oz. stanja). Izbira akcije v trenutnem ali naslednjem stanju, je odvisna od izbranega algoritma načrtovanja poti. Ta se glede na cenilko (kriterijsko funkcijo) odloči, katero je naslednje najprimernejše stanje iz množice ustreznih stanj. Cenilka je običajno določena z merjenjem razdalje, npr. najkrajša evklidska razdalja do ciljne točke. 4.1. Uvod 157 Začetno in ciljno stanje pogosto povezuje več poti, ali pa pot sploh ne obstaja. Običajno obstaja več izvedljivih poti, na katerih robot ne trči z ovirami. Izbor zožimo z dodatnimi zahtevami ali kriteriji, ki določajo želeno optimalnost: • dolžina poti naj bo najkrajša, • ustrezna pot naj bo tista, ki jo robot lahko prevozi v najkrajšem možnem času, • pot naj bo čim bolj oddaljena od ovir, • pot naj bo gladka, brez ostrih zavojev, • pot naj upošteva omejitve gibanja robota (primer neholonomičnosti, kjer v danem trenutku niso možne vse smeri vožnje). 4.1.3 Konfiguracija in konfiguracijski prostor Stanje mobilnega sistema v nekem okolju imenujemo konfiguracija in jo opišemo z n podatki, ki predstavljajo vektor stanj q = [ q 1 , . . . , qn] T , kjer je n število prostostnih stopenj. Stanje q je točka v n-dimenzionalnem prostoru, ki ga imenujemo konfiguracijski prostor Q (angl. configuration space) in predstavlja vse možne konfiguracije mobilnega sistema glede na njegov kinematični model. Del konfiguracijskega prostora, ki predstavlja ovire Oi, označimo z Qobst = S O i i. Torej je prosti del okolja brez ovir Qfree enak Qfree = Q − Qobst in predstavlja prostor, kjer lahko mobilni sistem načrtuje svoje gibanje. Predpostavimo, da imamo robota krožne oblike, ki je zmožen le translacij v ravnini, torej ima dve prostostni stopnji q = [ x, y] T . Njegovo konfiguracijo lahko obravnavamo točkovno in jo enostavno predstavimo s točko njegovega centra x, y. Konfiguracijski prostor Q pa določimo s pomikanjem robota ob oviri, tako da je ves čas v stiku z njo, kar prikazuje slika 4.2. Pri tem točka centra robota, ki določa njegovo pozicijo, opiše mejo med Qfree in Qobs. Na ta način razširimo dimenzije ovir za znano dimenzijo robota (njegov radij), da lahko robota obravnavamo kot točko. Še en primer konfiguracijskega prostora za trikotnega robota in ovire je prikazan na sliki 4.3. Robot se lahko premika samo v smereh x in y (q = [ x, y] T ). Če za robota na sliki 4.3 predpostavimo, da je zmožen tudi rotacije, ima njegova konfiguracija tri dimenzije q = [ x, y, ϕ] T , njegov konfiguracijski prostor pa je kompleksnejši. Poenostavljeno lahko konfiguracijski prostor določimo tako, da obliki robota očrtamo krog, ki ima središče v točki centra robota. Dobljeni prostor Qfree je v tem primeru nekoliko manjši kot dejanski prosti prostor, ker ima očrtan krog večjo površino kot robot. Vendar pa to poenostavi problem načrtovanja poti. 158 Načrtovanje poti q Q q q goal free goal goal q q qstart start start Qobs a) b) c) Slika 4.2: (a) Krožni robot v okolju z oviro, z označeno začetno in ciljno konfiguracijo, (b) določitev konfiguracijskega prostora in (c) načrtovanje poti, ki okolje (a) prevede v konfiguracijski prostor, kjer je robot predstavljen s točko svojega centra Qfree q q q start start start Qobs a) b) c) Slika 4.3: (a) Pravokotna ovira in trikotni robot s točko, ki določa njegovo konfiguracijo q, (b) določitev konfiguracijskega prostora, (c) prosti konfiguracijski prostor Qfree in prostor ovir Qobs 4.1.4 Matematični zapis oblike in lege ovire v okolici Za izračun konfiguracijskega prostora robota in uporabo nadaljnjih poenostavitev okolja je potreben matematični opis oblike in lege ovir v prostoru. Najpogostejša pristopa za opis ovir sta: predstavitev meje s pomočjo oglišč in predstavitev s polravninami. Predstavitev ovir z zapisom meje s pomočjo oglišč Ovira na tlorisu predstavlja m-strani poligon, ki ima m oglišč. Mejo ovire lahko zapišemo z nizanjem oglišč v obratni smeri urinega kazalca, pri čemer so luknje v ovirah in okoliška stena zapisane v nasprotni smeri, tj. v smeri urinega kazalca (slika 4.4). To velja tako za zapis konveksnih kot tudi nekonveksnih poligonov. Predstavitev ovir s presekom polravnin Konveksni poligon z m oglišči lahko zapišemo kot unijo m polravnin, kjer je vsaka določena s svojo enačbo premice f ( x, y) ≤ 0 oz. ravnine f ( x, y, z) ≤ 0 (tridimenzionalne ovire). Slika 4.5 prikazuje primer tovrstnega opisa peterokotnika. 4.2. Predstavitev okolja za načrtovanje poti 159 Slika 4.4: Primer opisa ovire z nizom oglišč: luknja ovire je opisana z nizom v smeri urinega kazalca, zunanja meja ovire pa z nizom v nasprotni smeri. Leva stran vsakega usmerjenega linearnega segmenta pripada oviri (zasenčena površina). Slika 4.5: Primer opisa ovire s polravninami Nekonveksne like in like z luknjami opišemo s pomočjo operacij nad množicami, npr. unija, presek, razlika množic itd. 4.2 Predstavitev okolja za načrtovanje poti Pred samim načrtovanjem poti moramo okolje predstaviti na poenoten matema- tičen način, primeren za obdelavo z algoritmi iskanja poti. 4.2.1 Predstavitev z grafi Konfiguracijski prostor je sestavljen iz prostega območja, ki predstavlja vse možne konfiguracije (stanja) mobilnega sistema, in območja z ovirami. Če prosto območje skrčimo in ga predstavimo s podmnožico konfiguracij (npr. središča področij ali celic), ki vključujejo začetne in ciljne konfiguracije ter želeno število vmesnih konfiguracij s prehodi med njimi, dobimo graf prehajanja stanj. Stanja v grafu so prikazana s krogi in jih imenujemo vozlišča grafa, povezave 160 Načrtovanje poti 0.5 0.5 0.3 0.5 0.3 0.2 1,4 0.2 1,4 2 2 1 1 0,8 0,8 0.3 0.7 0.3 0.7 1,1 0.5 1,1 0.5 1.2 1.2 a) b) Slika 4.6: Primer (a) uteženega grafa in (b) usmerjenega uteženega grafa med njimi pa s črtami, ki predstavljajo akcije, potrebne za prehod med stanji. Graf je utežen, če vsaki povezavi pripišemo neko utež ali ceno, ki je potrebna za izvršitev akcije pri prehodu med stanjema te povezave. V usmerjenem grafu (angl. directed graph) pa povezave označimo še s smerjo. V usmerjenem grafu je cena odvisna od smeri prehoda, medtem kot je v neusmerjenem grafu možen prehod v obeh smereh. Slika 4.6 prikazuje utežen in usmerjen utežen graf. Iskanje poti v grafu prehajanja stanj je možno z različnimi algoritmi iskanja, kot je A ? , Dijkstrov algoritem itd. 4.2.2 Razcep na celice Okolje lahko razdelimo na celice, ki predstavljajo enostavne geometrijske like. Celice so konveksne, saj mora vsaka daljica, ki povezuje poljubni konfiguraciji v celici, v celoti ležati znotraj celice. Po razcepu okolja na celice lahko izvedemo graf prehajanja stanj, kjer so stanja določene točke v celici (npr. težišča), povezave med stanji (vozlišča grafa) pa so možne le med sosednjimi celicami s skupnim robom ali ogliščem. Natančen razcep na celice Razcep okolja na celice je natančen, če celice v celoti ležijo ali v prostem območju ali v območju z ovirami. Natančen razcep je “brezizguben”, saj je unija vseh prostih celic enaka prostemu konfiguracijskemu prostoru Qfree. Primer natančnega razcepa na celice je navpičen razcep, prikazan na sliki 4.7. V tem primeru se z navidezno navpično črto pomikamo čez okolico od leve proti desni (meji). Vsakič, ko prečkamo oglišče katerega izmed večkotnikov, ustvarimo navpično mejo med celicama, ki lahko poteka samo navzgor, samo navzdol ali pa gor in dol od oglišča. Kompleksnost tega pristopa je močno odvisna od geometrije okolice. V enostavnih okolicah bo število celic in povezav med njimi majhno. Z večanjem števila poligonov (ovir) in njihovih oglišč, narašča tudi število celic. Natančen razcep na celice lahko predstavimo z grafom prehajanja stanj, kjer so vozlišča središča celic, prehodi med središči celic pa gredo skozi točke na središču 4.2. Predstavitev okolja za načrtovanje poti 161 start 4 5 11 1516 12 9 6 13 1 17 10 2 3 14 goal 8 7 4 5 9 11 12 15 16 1 6 10 13 17 2 3 7 8 14 Slika 4.7: Navpičen razcep na celice (zgoraj) in pripadajoč graf (spodaj) z vrisano potjo med začetno in ciljno konfiguracijo (odebeljena črta) 162 Načrtovanje poti start start cilj cilj a) b) Slika 4.8: Približen razcep na celice je sestavljen iz dveh korakov: (a) na okolico položimo mrežo celic enake velikosti in (b) označimo celice kot proste ali zasedene (zasenčene celice) mej med celicami. Približen razcep na celice Razcep okolja na celice je približen, ko posamezna celica vsebuje tako prosti konfiguracijski prostor kot tudi oviro ali del nje. Celice, ki vsebujejo vsaj del ovire, označimo kot zasedene, ostale pa so proste. Večinoma se uporablja razcep na celice enakih velikosti, kjer dobimo mrežo zasedenosti (angl. occupancy grid) (slika 4.8). Center vsake celice (na sliki 4.8 so proste celice pobarvane z belo barvo) je v grafu predstavljen kot vozlišče. Povezave med celicami so možne v štirih ali osmih smereh, odvisno od tega, ali je dovoljeno prehajanje v diagonalni smeri. Omenjen pristop je zelo enostaven za uporabo, vendar lahko zaradi konstantne velikosti celic pride do izgube informacij o okolici (ta razcep ni “brezizguben”); npr. ovire se povečajo in obstoječi ozki prehodi med njimi lahko izginejo pri približnem razcepu na celice. Glavna pomanjkljivost tega pristopa je poraba pomnilnika, ki je za večja okolja velika, ne glede na to, ali so okolja enostavna ali kompleksna. Do manjše izgube informacij pri majhni porabi pomnilnika pride pri uporabi spremenljive velikosti celic. Na okolje položimo eno celico, ki ga popolnoma pokrije. Če je celotna celica v prostem območju ali območju z ovirami, ostane takšna kot je. Če pa je le del nje pokrit z oviro, celico razdelimo na 4 manjše celice. Postopek, imenovan štiriško drevo (angl. quadtree), ponavljamo, dokler ni dosežena želena resolucija. Dobljeno razdelitev okolja na celice (slika 4.9) lahko prav tako pretvorimo v graf stanj. Približen razcep na celice je enostavnejši od natančnega, vendar lahko zaradi izgube informacij vodi do problema, ko proces načrtovanja poti ne najde rešitve, čeprav le-ta obstaja. 4.2. Predstavitev okolja za načrtovanje poti 163 start cilj Slika 4.9: Približen razcep na celice z uporabo spremenljive velikosti celic — štiriško drevo. Proste celice so označene z belo barvo, zasedene pa so zasenčene Primer 4.1 Napišite program, ki z uporabo štiriškega drevesa razgradi okolje s pravokotnimi ovirami. Program 4.1 ustvari naključno okolje z ovirami; v programu je določena tudi funkcija za izračun štiriškega drevesa. Funkcija sprejme ovire, dimenzijo okolja in želeno globino (število delitev) štiriškega drevesa ter vrne štiriško drevo v obliki Matlab strukture. Program 4.1: Implementacija štiriškega drevesa ./src/pth/example_quad_tree.m 1 bb = [0 , 16 , 0 , 12]; % Dimenzija okolja : xa , xb , ya , yb 2 N = 10; % Š tevilo ovir 3 minDim = [0.1; 0.1]; % Minimalne dimenzije ovire : xMin in yMin 4 maxDim = [2; 2]; % Maksimalne dimenzije ovire : xMax in yMax 5 % Naklju č ni zemljevid ovir , ogli š č a v stolpcu : x1 , y1 , x2 , y2 , x3 , y3 , x4 , y4 6 obst = zeros (8 , N ); 7 for i = 1: N 8 p = [ bb ( 1 ) ; bb ( 3 ) ] + [ d i f f ( bb ( 1 : 2 ) ) ; d i f f ( bb ( 3 : 4 ) ) ] . * r a n d (2 ,1); 9 phi = 2* pi * r a n d (); d = m i n D i m /2 + ( maxDim - m i n D i m ) / 2 * r a n d (); 10 R = [ cos ( phi ) , - sin ( phi ); sin ( phi ) , cos ( phi )]; 11 v = r e p m a t ( p , 1 , 4) + R *([ -1 , 1 , 1 , -1; -1 , -1 , 1 , 1 ] . * r e p m a t ( d , 1 , 4 ) ) ; 12 o b s t (: , i ) = r e s h a p e ( v , [] , 1); 13 end 14 15 tree = quadTree ( obst , bb , 4); % Izdelava š tiri š kega drevesa globine 4 164 Načrtovanje poti Rešitev Možna izvedba algoritma s štiriškim drevesom je podana v Matlab kodi v pro- gramu 4.2. Rezultat algoritma na naključnem zemljevidu je prikazan na sliki 4.10. Program 4.2: Razcep na celice s štiriškim drevesom ./src/pth/quadTree.m 1 function tree = quadTree ( obst , bb , level ) 2 % I z d e l a v a š t i r i š k e g a d r e v e s a $ t r e e $ g l o b i n e $level >=0 $ o k o l i o v i r $ o b s t $ 3 % ( v s a k s t o l p e c v s e b u j e o g l i š č a o v i r e : x1 , y1 , x2 , y2 , . . . ) in 4 % za o k o l j e d i m e n z i j $ b b $ ( xa , xb , ya , yb ) 5 m i n D i m = [ d i f f ( bb ( 1 : 2 ) ) ; d i f f ( bb ( 3 : 4 ) ) ] / 2 ^ l e v e l ; % M i n i m a l n a v e l i k o s t c e l i c e 6 % O s n o v n o v o z l i š č e 7 t r e e ( 1 ) . l e a f = t r u e ; % % Ali je c e l i c a kon č no v o z l i š č e ? 8 t r e e ( 1 ) . f r e e = f a l s e ; % Ali je c e l i c a z a s e d e n a ? 9 t r e e ( 1 ) . b o u n d s = bb ; % M e j e c e l i c e : xa , xb , ya , yb 10 t r e e ( 1 ) . c e n t e r = [ m e a n ( bb ( 1 : 2 ) ) ; m e a n ( bb ( 3 : 4 ) ) ] ; % C e n t e r c e l i c e 11 12 id = 1; k = 2; 13 w h i l e id < k 14 o c c u p i e d = i s O c c u p i e d ( t r e e ( id ). bounds , o b s t ); 15 16 d = [ d i f f ( t r e e ( id ). b o u n d s ( 1 :2 )) , d i f f ( t r e e ( id ). b o u n d s ( 3 : 4 ) ) ] / 2 ; 17 if o c c u p i e d && d (1) > m i n D i m ( 1 ) / 2 % R a z d e l i t e v c e l i c e na š t i r i n o v e c e l i c e 18 t r e e ( id ). l e a f = f a l s e ; 19 t r e e ( id ). f r e e = f a l s e ; 20 21 b = t r e e ( id ). b o u n d s ; 22 bs = [ b (1) , b ( 1 ) + d (1) , b (3) , b ( 3 ) + d ( 2 ) ; ... 23 b ( 1 ) + d (1) , b (2) , b (3) , b ( 3 ) + d ( 2 ) ; ... 24 b (1) , b ( 1 ) + d (1) , b ( 3 ) + d (2) , b ( 4 ) ; ... 25 b ( 1 ) + d (1) , b (2) , b ( 3 ) + d (2) , b ( 4 ) ] ; 26 for i = 1:4 27 t r e e ( k ). l e a f = t r u e ; 28 t r e e ( k ). f r e e = f a l s e ; 29 t r e e ( k ). b o u n d s = bs ( i , : ) ; 30 t r e e ( k ). c e n t e r = [ m e a n ( bs ( i , 1 : 2 ) ) ; m e a n ( bs ( i , 3 : 4 ) ) ] ; 31 k = k + 1; 32 end 33 e l s e i f ~ o c c u p i e d 34 t r e e ( id ). f r e e = t r u e ; 35 end 36 id = id + 1; 37 end 38 39 % I z d e l a v a v i d l j i v o s t n e g a g r a f a 40 a = z e r o s (2 , l e n g t h ( t r e e ) * 4 ) ; l e a f s = z e r o s (1 , l e n g t h ( t r e e )); 41 for i = 1: l e n g t h ( t r e e ) 42 a (: , i *4 -3: i *4) = t r e e ( i ). b o u n d s ([1 , 2 , 2 , 1; 3 , 3 , 4 , 4 ] ) ; 43 l e a f s ( i ) = t r e e ( i ). l e a f ; 44 end 45 o f f s e t = [ -1 , 1 , 1 , -1; -1 , -1 , 1 , 1 ] / 2 . * r e p m a t ( minDim , 1 , 4); 46 for i = 1: l e n g t h ( t r e e ) 47 t r e e ( i ). n e i g h b o u r s = []; 48 if t r e e ( i ). l e a f 49 b = t r e e ( i ). b o u n d s ([1 , 2 , 2 , 1; 3 , 3 , 4 , 4]) + o f f s e t ; 50 c = r e s h a p e ( i n p o l y g o n ( a (1 ,:) , a (2 ,:) , b (1 ,:) , b (2 ,:)) , 4 , [ ] ) ; 51 t r e e ( i ). n e i g h b o u r s = s e t d i f f ( f i n d ( any ( c ).* l e a f s ) , i ); 52 end 53 end 4.2. Predstavitev okolja za načrtovanje poti 165 54 end 55 56 function occupied = isOccupied ( bounds , obst ) 57 o c c u p i e d = f a l s e ; 58 pb = b o u n d s ([1 , 2 , 2 , 1 , 1; 3 , 3 , 4 , 4 , 3 ] ) ; 59 for j = 1: s i z e ( obst , 2) % S p r e h o d č ez vse o v i r e 60 pa = r e s h a p e ( o b s t (: , j ) , 2 , [ ] ) ; N = s i z e ( pa , 2); 61 ina = i n p o l y g o n ( pa (1 ,:) , pa (2 ,:) , pb (1 ,:) , pb (2 ,: ) ); 62 inb = i n p o l y g o n ( pb (1 ,:) , pb (2 ,:) , pa (1 ,:) , pa (2 ,: ) ); 63 if any ( ina ) || any ( inb ) % Ali so o g l i š č a v o v i r i ali c e l i c i ? 64 o c c u p i e d = t r u e ; b r e a k ; 65 e l s e % Ali so kak š na p r e s e č i š č a r o b o v ? 66 for k = 1: s i z e ( pb , 2) -1 % S p r e h o d č ez vse m e j n e r o b o v e 67 for i = 1: N % S p r e h o d č ez r o b o v e o v i r 68 a1 = [ pa (: , i ); 1]; a2 = [ pa (: , mod ( i , N ) + 1 ) ; 1]; 69 b1 = [ pb (: , k ); 1]; b2 = [ pb (: , k + 1 ) ; 1]; 70 pc = c r o s s ( c r o s s ( a1 , a2 ) , c r o s s ( b1 , b2 )); % P r e s e č i š č e 71 if abs ( pc (3)) > eps 72 pc = pc / pc ( 3 ) ; 73 da = a2 - a1 ; ca = pc - a1 ; ea = ( ca . ’* da )/( da . ’* da ); 74 db = b2 - b1 ; cb = pc - b1 ; eb = ( cb . ’* db )/( db . ’* db ); 75 if eb > eps && eb <1 && ea > eps && ea <1 76 o c c u p i e d = t r u e ; b r e a k ; 77 end 78 end 79 end 80 if o c c u p i e d , b r e a k ; end 81 end 82 end 83 if o c c u p i e d , b r e a k ; end 84 end 85 end 166 Načrtovanje poti 12 10 8 y 6 4 2 00 5 10 15 x Slika 4.10: Razcep okolja z naključnimi ovirami s pomočjo štiriškega drevesa. Na štiriško drevo je položena mreža, ki povezuje središča vseh sosednjih celic. 4.2.3 Zemljevid cest Zemljevid cest, sestavljen iz črt, krivulj in njihovih stičišč, podaja povezljivost prostega območja okolja. Proces načrtovanja poti mora povezati začetno in ciljno točko z obstoječimi povezavami na zemljevidu in poiskati povezano zaporedje cest. Postavitev cest je odvisna od geometrije okolice. Cilj je uporabiti najmanjše število cest, ki robotu omogočajo dostop do kateregakoli dela prostega območja. V nadaljevanju so predstavljeni trije načini izdelave zemljevida cest: graf vidljivosti, Voronojev graf in triangulacija prostora. Graf vidljivosti Graf vidljivosti je sestavljen iz vseh možnih povezav med dvema ogliščema, ki v celoti ležita v prostem območju. To pomeni, da so za vsako oglišče vzpostavljene povezave z vsemi (drugimi) oglišči, ki so vidna z njegove pozicije. Pri tem začetno in ciljno točko obravnavamo kot dodatni oglišči. Povezave ustvarimo tudi med sosednjima ogliščema istega poligona. Primer grafa vidljivosti je prikazan na sliki 4.11a. Pot, dobljena s pomočjo grafa vidljivosti, je najkrajša možna, saj so ceste speljane kar se da blizu oviram. Da rešimo problem trka robota in 4.2. Predstavitev okolja za načrtovanje poti 167 start start cilj cilj a) b) Slika 4.11: Zemljevid cest: (a) graf vidljivosti in (b) Voronojev graf ovir, lahko le-te povečamo vsaj za polmer robota, kot je opisano v podpoglavju 4.1.3. Grafi vidljivosti so dokaj enostavni za uporabo, vendar z večanjem števila ovir in njihove kompleksnosti narašča število cestnih povezav in vozlišč, zato se manjša njihova učinkovitost. Grafe vidljivosti lahko poenostavimo z odstranitvijo redundantnih povezav, ki jih je možno nadomestiti z obstoječo krajšo povezavo. Voronojev graf Voronojev graf (slika 4.11b) je sestavljen iz odsekov cest, ki so najbolj oddaljeni od ovir. To pomeni, da je povezava med dvema ovirama enaka razdalji do obeh ovir. Osnovni Voronojev graf (diagram) je opredeljen za ravnino z n točkami (npr. točkovna ovira). Ravnina se razdeli na n območij, katerih meje sestavljajo zemljevid. Vsako območje ima natanko eno izvorno točko in velja, da so vse točke znotraj določenega območja bližje svoji izvorni točki, kot katerikoli drugi izvorni točki (drugih območij). Na sliki 4.11 je prikazan primer splošnega Voronojevega grafa za ravninsko okolje, v katerem so ovire poljubnih oblik (trikotnik, pravokotnik, mnogokotnik itd.). Tu je prostor razdeljen na območja, kjer ima vsako območje točno eno izvorno oviro. Vsaka točka znotraj določenega območja je bližje izvorni oviri kot katerikoli drugi oviri. Vožnja po takšni cesti zmanjšuje tveganje za trk robota z ovirami, kar je zaželeno, ko je lega robota znana z neko negotovostjo zaradi merilnega šuma ali vodenja. Zemljevid okolja, zgrajen iz poligonov, vsebuje tri značilne Voronojeve krivulje, kot je prikazano na sliki 4.12. Voronojeva krivulja, ki ima enako razdaljo med: • dvema ogliščema (premica), • dvema robovoma (ista premica), • ogliščem in daljico (parabola). V cestno omrežje povežemo še začetno in ciljno konfiguracijo. Tako dobimo graf, po katerem iščemo rešitev. 168 Načrtovanje poti Slika 4.12: Tipične Voronojeve krivulje Kot smo že omenili, ta pristop maksimira oddaljenost robota do ovir, vendar pridobljena dolžina poti še zdaleč ni optimalna (najkrajša). Robot s senzorji oddaljenosti (npr. z ultrazvočnim ali laserskim pregledovalnikom razdalj) lahko z enostavno regulacijo, kjer se giba na enaki oddaljenosti od vseh okoliških ovir, sledi cestam po Voronojevem grafu. Roboti z bližinskimi tipali ali senzorji za kratke razdalje pa imajo pri tem pristopu probleme z lokalizacijo, ki jih pri grafu vidljivosti ne bi imeli. Primer 4.2 Z uporabo Matlab funkcije voronoi izračunajte Voronojev graf za okolje na sliki 4.11. Koordinate okolja (o) in objektov (o1, o2, o3) so podane v programu 4.3. Program 4.3: Oglišča ovir v okolju ./src/pth/param_map.m 1 o = 1 0 0 0 * [ 0 . 0 1 4 9 , 0 . 0 6 9 3 ; ... 2 1 .6 2 28 , 0 . 0 6 7 9 ; ... 3 1 .6 2 41 , 1 . 0 8 6 7 ; ... 4 0 .0 11 2 , 1 . 0 8 5 4 ] ; 5 o1 = 1000*[0.4263 , 0.4569; ... 6 0 .6 1 44 , 0 . 6 8 5 7 ; ... 7 0 .3 09 7 , 0 . 9 4 1 4 ; ... 8 0 .1 19 0 , 0 . 7 1 2 6 ] ; 9 o2 = 1000*[0.8151 , 0.2079; ... 10 1 .0 88 5 , 0 . 3 0 0 8 ; ... 11 0 .9 6 44 , 0 . 6 5 7 4 ; ... 12 0 .8 75 3 , 0 . 6 2 7 8 ; ... 13 0 .9 7 06 , 0 . 3 5 7 3 ; ... 14 0 .7 83 8 , 0 . 2 9 2 7 ] ; 15 o3 = 1000*[1.3319 , 0.4865; ... 16 1 .4 72 3 , 0 . 5 6 5 9 ; ... 17 1 .3 84 5 , 0 . 7 1 1 2 ] ; 18 % Prosto obmo č je je na desni strani od ovire 19 obstacles = { flipud ( o ) , o1 , o2 , o3 }; Rešitev S pomočjo Matlab funkcije voronoi lahko narišemo Voronojev graf za seznam točk. Okolje na sliki 4.11 vključuje tudi like z robovi, ki so opisani z daljicami, zato ni možno neposredno uporabiti funkcije voronoi. Vsako daljico lahko poljubno natančno predstavimo z diskretno množico (enakomerno) porazdeljenih pomožnih točk. S pomočjo funkcije voronoi lahko nato izračunamo aproksi- 4.2. Predstavitev okolja za načrtovanje poti 169 macijo Voronojevega grafa, kot je prikazano na sliki 4.13. Poleg iskanih robov med ovirami dobimo s tem postopkom tudi veliko dodatnih robov, ki so na sliki 4.13 označeni črtkano. Gre za robove, ki ločujejo pomožne točke, niso v prostem prostoru ali pa so posledica nekonveksnosti ovir. Program 4.4 vsebuje filtrirne mehanizme, ki odstranijo vse dodatne robove, tako da dobimo končno aproksimacijo Voronojevega grafa — polno izvlečeni robovi na sliki 4.13. Slika 4.13: Aproksimacija Voronojevega grafa z uporabo Matlab funkcije voronoi in pomožnih točk za predstavitev robov Program 4.4 ./src/pth/example_voronoi.m 1 param_map ; 2 3 % Opis daljic z mno ž ico pomo ž ni /( vmesnih ) to č k 4 dMin = 50; 5 points = []; obst = []; 6 B = length ( obstacles ); 7 for i = 1: B 8 ob = o b s t a c l e s { i }; 9 M = s i z e ( ob , 1); 10 for j = 1: M 11 k = mod ( j , M ) + 1 ; % j +1 12 d = s q r t (( ob ( j ,1) - ob ( k , 1 ) ) ^ 2 + ( ob ( j ,2) - ob ( k , 2 ) ) ^ 2 ) ; 13 n = c e i l ( d / d M i n ) + 1 ; 14 x = l i n s p a c e ( ob ( j ,1) , ob ( k ,1) , n ). ’; 15 y = l i n s p a c e ( ob ( j ,2) , ob ( k ,2) , n ). ’; 16 p o i n t s = [ p o i n t s ; x (1: end -1) y (1: end - 1 ) ] ; 17 end 170 Načrtovanje poti 18 o b s t = [ o b s t ; o b s t a c l e s { i } ( [ 1 : end , 1 ] ,: ) ; nan ( 1 ,2 ) ]; 19 end 20 21 % Izra č un V o r o n o i j e v i h daljic iz mno ž ice pomo ž nih to č k 22 [ vx , vy ] = voronoi ( points (: ,1) , points (: ,2)); 23 24 % O d s t a n j e v a n j e pomo ž nih ( niso v prostem prostoru ) V o r o n o i j e v i h robov 25 s = false (1 , size ( vx , 2)); 26 for j = 1: size ( vx , 2) 27 in = i n p o l y g o n ( vx (: , j ) , vy (: , j ) , o b s t (: ,1) , o b s t ( : , 2) ) ; 28 s ( j ) = all ( in = = 1 ) ; 29 end 30 ux = vx (: , s ); uy = vy (: , s ); % Pribli ž ni V oro no ije vi robovi 31 32 % O d s t r a n j e v a n j e nazaklju č enih robov pri konveksnih objektih 33 r = []; 34 for j = 1: length ( ux (:)) 35 c = l e n g t h ( f i n d (( ux (:) - ux ( j ) ) . ^ 2 + ( uy (:) - uy ( j ) ) . ^ 2 < eps )); 36 if c ==1 37 a = j ; % O d k r i t a r o b n a to č ka na n e z a k l j u č e n e m r o b u 38 w h i l e t r u e % I s k a n j e v s e h to č k na n e z a k l j u č e n e m r o b u 39 if mod ( a , 2) == 0 , b = a -1; e l s e b = a +1; end 40 r = [ r , max ( a , b ) / 2 ] ; 41 c = f i n d (( ux (:) - ux ( b ) ) . ^ 2 + ( uy (:) - uy ( b ) ) . ^ 2 < eps ); 42 if l e n g t h ( c ) >2 , b r e a k ; end 43 a = c ( c ~= b ); 44 end 45 end 46 end 47 ux (: , r ) = []; uy (: , r ) = []; 48 49 % Izris 50 plot ([ vx ; nan (1 , size ( vx ,2))] , [ vy ; nan (1 , size ( vy ,2))] , ’b : ’ ); hold on ; 51 plot ( points (: ,1) , points (: ,2) , ’b . ’ ); 52 plot ([ ux ; nan (1 , size ( ux ,2))] , [ uy ; nan (1 , size ( uy ,2))] , ’g - ’ , ’ LineWidth ’ , 1); 53 axis equal tight ; Triangulacija prostora Pri triangulaciji prostora je okolje razcepljeno na trikotne celice. Čeprav obstajajo različni algoritmi triagulacije, je dober pristop, ki preprečuje ozke trikotnike, še vedno aktualen raziskovalni problem [2]. Eden od možnih algoritmov je Delaunayeva triangulacija prostora, ki je dual Voronojevega grafa. V Delaunayevem grafu središče vsakega trikotnika (središče očrtanega kroga) sovpada z vsakim ogliščem Voronojevega poligona. Primer delaunayeve triangulacije prostora je prikazan na sliki 4.14. Iz pristopov, ki brezizgubno upodobijo okolico, je s pomočjo kasneje predstavljenih algoritmov možno v končnem času dobiti informacijo, ali iskana pot obstaja ali ne. Pravimo, da so ti pristopi popolni (angl. complete). 4.2. Predstavitev okolja za načrtovanje poti 171 start cilj Slika 4.14: Delaunayeva triangulacija prostora, kjer nekateri robovi trikotnika sovpadajo z robovi ovir Primer 4.3 Uporabite Delaunayevo triangulacijo prostora za okolje na sliki 4.11. Točke, ki določajo meje ovir, so navedene v programu 4.3. Rešitev Za znana oglišča lahko v programskem okolju Matlab s pomočjo funkcije DelaunayTri izračunamo Delaunayevo triangulacijo prostora, s funkcijo triplot pa jo narišemo, kot je prikazano v programu 4.5. Program 4.5: Delaunayeva triangulacija prostora ./src/pth/example_delaunay.m 1 param_map ; 2 3 points = cell2mat ( obstacles (:)); 4 dt = d e l a u n a y T r i a n g u l a t i o n ( points ); 5 triplot ( dt , ’b - ’ ); axis equal tight ; 4.2.4 Potencialno polje Okolica je predstavljena s potencialnim poljem, ki ga lahko razumemo kot namišljeno višino. Ciljna točka je na dnu, višina polja pa narašča z oddaljenostjo od ciljne točke ter je na ovirah še višja. Postopek načrtovanja poti si lahko 172 Načrtovanje poti start start cilj cilj Slika 4.15: Potencialno polje za znano ciljno točko (zgoraj) ter ekvipotencialne krivulje in izračunana pot za dve začetni točki (spodaj), kjer izračunana pot doseže cilj (levo) in kjer je pot ujeta v nekonveksno oviro (desno) predstavljamo kot premikanje žoge, ki se kotali po hribu navzdol do cilja v dolini, kar je prikazano na sliki 4.15. Potencialno polje je vsota privlačnega polja ciljne točke Uattr(q) in odbojnega polja ovir Urep(q) U (q) = Uattr(q) + Urep(q) (4.1) Ciljna točka je globalni minimum potencialnega polja. Privlačni potencial Uatr(q) v (4.1) je lahko določen tako, da je sorazmeren kva-dratu evklidske razdalje do ciljne točke D(q , q goal) = p( x − xgoal)2 + ( y − ygoal)2 kot 1 Uattr(q) = kattr D 2(q , q goal) 2 kjer je katr pozitivna konstanta. 4.2. Predstavitev okolja za načrtovanje poti 173 Odbojni potencial Urep(q) naj bo zelo velik v bližini ovir, z oddaljenostjo od ovir D(q , q obst) pa se zmanjšuje. Njegova vrednost je nič, ko je D(q , q obst) večja od mejne vrednosti D 0. Odbojni potencial je lahko predstavljen kot  2 1 1  krep − 1 ; D(q) ≤ D 0 U 2 D(q , q obst) D 0 rep(q) = (4.2) 0 ; D(q) > D 0 kjer je krep pozitivna konstanta, D(q , q obst) pa razdalja do najbližje točke na najbližji oviri. Za opis poti od začetne do ciljne točke mora robot slediti negativnemu gradientu potencialnega polja (−∇ U (q)). Primer 4.4 Določite negativni gradient potencialnega polja (4.1). Rešitev Negativni gradient privlačnega polja (4.2.4) je " # 1 2( x − x −∇ U goal) attr (q) = − kattr = kattr(q goal − q) 2 2( y − ygoal) in kaže v smeri od lege robota q do cilja q goal, njegova dolžina pa je proporcionalna razdalji od q do q goal. Določimo še negativni gradient odbojnega polja 4.2 za primer, ko je D(q) ≤ D 0 1 1 −1 −∇ Urep(q) = − krep − ∇ Dobst = Dobst D 0 D 2 obst 1 1 1 = krep − (q − q obst) Dobst D 0 D 3 obst kjer je Dobst = D(q , q obst) = p( x − xobst)2 + ( y − yobst)2. Vidimo, da smer odbojnega polja vedno kaže stran od ovire, njegova jakost pa se zmanjšuje z oddaljenostjo od ovire. Za primer, ko je Dobst > D 0, pa je odbojni gradient −∇ Urep(q) = 0. Pri predstavitvi okolja z uporabo potencialnega polja lahko robot preprosto doseže ciljno točko s sledenjem negativnega gradienta potencialnega polja, ki ga eksplicitno izračunamo iz znane pozicije robota. Glavna pomanjkljivost tega pristopa je možnost ujetosti robota (tresenje – angl. jittering behaviour ) v lokalnem minimumu. Do tega lahko pride, če okolje vsebuje kakršnokoli nekonveksno oviro (slika 4.15) in začne robot oscilirati med več enako oddaljenimi točkami od ovire. Načrtovanje poti z uporabo potencialnega polja se lahko uporabi 174 Načrtovanje poti za izračun referenčne poti, ki ji mora robot slediti, ali pa pri vodenju gibanja za usmerjanje robota v trenutni smeri negativnega gradienta. Primer 4.5 Izračunajte potencialno polje iz slike 4.15 za okolico z ovirami. Koordinate ovir so podane v programu 4.3. Rešitev Potencialno polje izračunamo s pomočjo enačb (4.1) – (4.2), kjer je pot izračunana kot integral negativnega gradienta iz primera 4.4. 4.2.5 Načrtovanje poti z metodami vzorčenja pro- stora Do sedaj predstavljene metode za predstavitev okolja za namen planiranja poti so zahtevale znano eksplicitno predstavitev prostega konfiguracijskega prostora. Z večanjem dimenzije konfiguracijskega prostora postanejo te metode preveč časovno potratne, zato lahko v teh primerih uporabimo metode vzorčenja prostora. Pri načrtovanju poti z vzorčenjem prostora (angl. sampling-based path planning) se naključno zajemajo točke iz okolja (konfiguracije robota), nato se s pomočjo zaznavanja trka preverja, če le-te ležijo v prostem območju [4, 5]. Iz množice zajetih točk in povezav med njimi, ki prav tako v celoti ležijo v prostem območju, poiščemo pot med znano začetno in želeno ciljno točko. Pri metodah vzorčenja prostora ni potreben izračun prostega konfiguracijskega prostora Qfree, ki pri kompleksni postavitvi ovir in visokih prostostnih stopnjah postane časovno zamuden, ampak ga predstavimo z naključnimi vzorci ter neodvisno od geometrije okolice najdemo rešitev za širok spekter problemov. Prav tako se izognemo velikemu številu celic, ki ga dobimo pri opisu z razcepom na celice, ter zamudni implementaciji in računanju, ki spremljata uporabo natančnega razcepa na celice. Zaradi vključitve stohastičnega mehanizma (naključni sprehod, angl. random walk) v nekatere algoritme, npr. v RPP (angl. random path planner ), ko se iz točke, v kateri smo ujeti, rešimo s pomočjo premika po naključnih vzorcih iz prostega območja, močno omilimo problem lokalnih minimumov, ki nas pesti pri uporabi potencialnega polja. Da bi zaznavanje trka zavzemalo čim manj računskega časa, ga preverjamo samo za ovire, ki so dovolj blizu, da bi lahko trčile z robotom. Robot in ovire so lahko omejeni z enostavnimi liki, tako da kompleksnejše preverjanje trka (med pravo obliko robota in oblikami ovir) izvajamo samo, ko se dva lika prekrivata. V tem primeru lahko trk zaznavamo na hierarhičen način, kjer večji lik, ki obkroža 4.2. Predstavitev okolja za načrtovanje poti 175 Slika 4.16: Razdelitev večjega lika na dva manjša pri hierarhičnem zaznavanju trka robota celotnega robota, zamenjamo z dvema manjšima, ki obkrožata vsak svojo polovico robota, kot je to prikazano na sliki 4.16. Če se kateri izmed likov prekriva z oviro, ga ponovno razdelimo na dva manjša ustrezna lika. S tem nadaljujemo dokler ne izključimo ali potrdimo trka oz. dosežemo želene resolucije. Tovrstne pristope delimo na tiste, ki so primerni za enkratno iskanje poti, in tiste, ki so primerni za večkratno iskanje poti. Pri prvih želimo čim hitreje poiskati pot med eno začetno in eno ciljno točko, zato se osredotočimo na dele okolice, ki obetajo rešitev. V drevesno strukturo sproti dodajajmo nove točke in povezave dokler ne najdemo rešitve. Pri drugih pristopih pa se pred samim načrtovanjem poti izvede enkraten postopek izdelave neusmerjenega grafa oz. zemljevida cest, ki predstavlja povezanost prostega območja in s pomočjo katerega lahko nato rešimo problem načrtovanja poti za več poljubnih parov začetnih in ciljnih točk. V nadaljevanju sta opisana predstavnika iz obeh skupin. Metoda hitro-rastočega naključnega drevesa Metoda hitro-rastočega naključnega drevesa (angl. rapidly-exploring random tree) je metoda iskanja poti med eno začetno in eno ciljno točko [4]. Metoda v vsaki iteraciji doda nove povezave v smeri od naključnih točk proti najbližjim točkam, ki so že v grafu (drevesu). V prvi iteraciji algoritma začetna konfiguracija qi predstavlja drevo (povezan graf). V vsaki naslednji iteraciji se naključno izbere konfiguracija qrand in iz obstoječega grafa poišče najbližje vozlišče qnear. V smeri od qnear proti qrand se na vnaprej določeni razdalji ε izračuna kandidat za novo vozlišče qnew. Če sta qnew in povezava od qnear do qnew v prostem območju, je qnew novo vozlišče in njegova povezava z qnear je dodana v graf. Postopek je prikazan na sliki 4.17. Iskanje se zaključi po določenem številu iteracij (npr. sto iteracij) ali ko je dosežena določena verjetnost (npr. 10%). Takrat se namesto zajema novega 176 Načrtovanje poti e qnear qnew qrand Slika 4.17: Metoda hitro-rastočega naključnega drevesa — razširitev grafa z novo točko qnew v smeri naključno vzorčene točke qrand naključnega vzorca izbere ciljna točka za katero se preveri, ali jo je mogoče povezati z grafom [6]. Takšno drevo se hitro razširi na neraziskana območja, kar lahko vidimo na sliki 4.18. Ta metoda ima samo dva parametra: velikost koraka ε in želena ločljivost ali število iteracij, ki določata pogoje za zaključek algoritma. Zato je vedenje algoritma hitro-rastočega naključnega drevesa dosledno in njegova analiza preprosta. (a) (b) (c) Slika 4.18: Drevo, zgrajeno z metodo hitro-rastočega naključnega drevesa, hitro napreduje v neraziskano prosto območje. Slike z leve proti desni prikazujejo drevesa z 20, 100 in 1000 vozlišči. 4.2. Predstavitev okolja za načrtovanje poti 177 Primer 4.6 Izvedite algoritem hitro-rastočega naključnega drevesa, ki bo ustvaril drevo za dvodimenzionalno prosto območje, velikosti 10 m × 10 m, kjer je parameter ε = 0 , 2 m. Rešitev Za podobne rezultate, kot na sliki 4.18, lahko uporabite Matlab kodo iz programa 4.6. Program 4.6 ./src/pth/example_rrt.m 1 xi = [5 , 5]; % Za č etna k o n f i g u r a c i j a 2 D = 0.2; % Razdalja do novega vozli š č a 3 maxIter = 1000; 4 M = [ xi ]; % Zemljevid 5 6 j = 1; 7 while j < maxIter 8 x R a n d = 10* r a n d (1 ,2); % N a k l j u č na k o n f i g u r a c i j a 9 d M i n = 1 0 0 ; i M i n = 1; % I s k a n j e n a j b l i ž je to č ke v z e m l j e v i d u M 10 for i = 1: s i z e ( M ,1) 11 d = n o r m ( M ( i ,:) - x R a n d ); 12 if d < d M i n 13 d M i n = d ; 14 i M i n = i ; 15 end 16 end 17 18 x N e a r = M ( iMin , : ) ; 19 v = x R a n d - x N e a r ; 20 x N e w = x N e a r + v / n o r m ( v )* D ; % I z r a č un n o v e to č ke 21 22 con = [ x N e a r ; x N e w ]; 23 M = [ M ; x N e w ]; 24 j = j + 1; 25 26 l i n e ( con (: ,1) , con (: ,2) , ’ C o l o r ’ , ’ b ’ ); 27 end Primer 4.7 Razširite primer 4.6, da vključuje tudi preproste ovire (npr. krožne ovire). Rešitev Predpostavite okolje z enostavnimi ovirami v obliki krogov z znanimi položaji in premeri. Preverite, ali v prostem območju ležita kandidat za novo vozlišče qnew 178 Načrtovanje poti cilj cilj start start a) b) Slika 4.19: Metoda verjetnostnega zemljevida poti: (a) faza učenja in (b) faza iskanja poti in daljica, ki povezuje qnear in qnew. Metoda verjetnostnega zemljevida poti Metoda verjetnostnega zemljevida poti (angl. probabilistic roadmap) je način iskanja poti med več začetnimi in več ciljnimi točkami, ki poteka v dveh fazah [7]. Prva je faza učenja, v kateri se izdela povezan zemljevid cest ali neusmerjen graf prostega območja (slika 4.19a), druga pa faza iskanja, v kateri se trenutni par začetne in ciljne točke poveže z grafom in se s pomočjo iskalnih algoritmov poišče pot (slika 4.19b). V fazi učenja izdelamo zemljevid cest, ki je na začetku prazna množica, potem pa se napolni z vozlišči s ponavljanjem v nadaljevanju naštetih korakov. Naključno izbrano konfiguracijo qrand, ki leži v prostem območju, dodamo v zemljevid in določimo vozlišča Qn za razširitev zemljevida. To lahko storimo tako, da izberemo K najbližjih sosednjih vozlišč ( Qn) ali pa vsa sosednja vozlišča, katerih oddaljenost od qrand je manjša od vnaprej določenega parametra D (slika 4.20). V prvem oz. prvih korakih morda ne najdemo sosednjih vozlišč. Nato dodamo v zemljevid vse enostavne povezave od qrand do vozlišč iz Qn, ki v celoti ležijo v prostem območju. S tem postopkom nadaljujemo, dokler zemljevid ne vsebuje želenega števila vozlišč N . V fazi iskanja začetno in ciljno točko preko prostega območja povežemo s čim bliž- jima možnima vozliščema iz zemljevida in nato z iskalnim algoritmom poiščemo pot med njima. Za ti dve fazi ni nujno, da ju izvedemo ločeno. Lahko ju ponavljamo, dokler nimamo dovolj vozlišč za odkritje rešitve. Če ni možno najti rešitve, zemljevid razširimo z novimi vozlišči in povezavami, dokler ne dobimo izvedljive rešitve. 4.2. Predstavitev okolja za načrtovanje poti 179 D qrand Qn Slika 4.20: Pri metodi verjetnostnega zemljevida poti dodamo v zemljevid vse možne povezave od naključnih točk qrand do sosednjih vozlišč Qn Slika 4.21: Pri preizkusu mostu sta izbrani dve naključni bližnji točki, ki določata daljico. Če se srednja točka nahaja v prostem območju, zunanji dve pa znotraj ovir, je srednja točka dodana kot vozliče na zemljevidu. Na ta način se iterativno približujemo čim bolj ustrezni predstavitvi prostega območja. Metoda je zelo učinkovita pri robotih z velikim številom prostostnih stopenj, vendar ima težave pri iskanju povezave med dvema območjema preko ozkih prehodov. To lahko premagamo z dodajanjem vozlišč s pomočjo preizkusa mostu (angl. bridge test), v katerem izberemo tri naključne “blizuležeče” točke na daljici (slika 4.21). Če sta krajni točki v trku z ovirami, srednja pa ne, potem srednjo točko vključimo v zemljevid kot vozlišče ter jo nato skušamo na enak način kot ostale povezati s sosednjimi vozlišči. Z združitvijo preizkusa mostu in enakomernega vzorčenja [8] v “hibridno strategijo vzorčenja” lahko dobimo manjše zemljevide poti, ki bolj učinkovito pokrivajo prosto območje ter ohranjajo dobre povezave preko ozkih prehodov. 180 Načrtovanje poti Primer 4.8 Izvedite algoritem verjetnostnega zemljevida poti za dvodimenzionalno prosto območje, velikosti 10 m × 10 m. Poskusite razširiti algoritem za okolje z ovirami. Rešitev Rešitev je podana v programu 4.7. Program 4.7 ./src/pth/example_prm.m 1 D = 1; % Parameter razdalje 2 maxIter = 200; 3 M = []; % Zemljevid 4 5 j = 1; 6 while j <= maxIter 7 x R a n d = 10* r a n d (1 ,2); % N a k l j u č na k o n f i g u r a c i j a 8 M = [ M ; x R a n d ]; 9 con = []; % P o v e z a v e 10 for i = 1: s i z e ( M ,1) % I s k a n j e p o v e z a v do s o s e d n j i h v o z l i š č 11 d = n o r m ( M ( i ,:) - x R a n d ); 12 if d < D && d > eps % D o d a j a n j e p o v e z a v od x R a n d do s o s e d a 13 con = [ con ; xRand , M ( i , : ) ] ; 14 end 15 end 16 j = j + 1; 17 18 l i n e ( x R a n d (1) , x R a n d (2) , ’ C o l o r ’ , ’ r ’ , ’ M a r k e r ’ , ’ . ’ ); 19 for i = 1: s i z e ( con ,1) 20 l i n e ( con ( i ,[1 ,3]) , con ( i ,[2 ,4]) , ’ C o l o r ’ , ’ b ’ ); 21 end 22 end 4.3 Preprosti algoritmi načrtovanja poti — algoritmi tipa hrošč Algoritmi tipa hrošč (angl. bug algorithm) so najbolj enostavni algoritmi planiranja poti. Za planiranje ne potrebujejo zemljevida okolice, zato so primerni, ko zemljevid okolice ni znan ali pa se okolica stalno spreminja in tudi ko ima mobilna platforma zelo omejeno moč računanja. Ti algoritmi uporabljajo le lokalno informacijo o okolju, pridobljeno iz senzorjev (npr. senzor razdalje), in globalno podan cilj, ne potrebujejo pa globalnega znanja v obliki zemljevida okolja. Njihovo delovanje sestoji iz dveh enostavnih vzorcev obnašanja: gibanje po ravni liniji proti cilju in sledenje obrisu ovire. Mobilni roboti, ki uporabljajo te algoritme, se lahko izogibajo oviram in pre- 4.3. Preprosti algoritmi načrtovanja poti — algoritmi tipa hrošč 181 cilj cilj start start Slika 4.22: Algoritem Hrošč0 (varianta vedno zavij levo) uspešno najde pot do cilja (leva slika) in neuspešno (desna slika) mikajo proti cilju. Tovrstni algoritmi imajo majhno porabo spomina, vendar je lahko najdena pot daleč od optimalne. Algoritmi tipa hrošč so bili najprej implementirani v [9], temu pa so sledile številne izboljšave kot v [10–12]. V nadaljevanju so predstavljeni trije osnovni algoritmi tipa hrošč. 4.3.1 Algoritem Hrošč0 Algoritem Hrošč0 deluje v naslednjih dveh korakih: 1. V ravni liniji se premika proti cilju, dokler ne naleti na oviro ali cilj. 2. Če naleti na oviro, vedno zavije levo (oz. vedno desno, če je tako določeno v algoritmu) in sledi obrisu ovire, dokler ni možno ponovno nadaljevati proti cilju. Primer delovanja algoritma Hrošč0 je prikazan na sliki 4.22. 4.3.2 Algoritem Hrošč1 Algoritem Hrošč1 uporablja glede na Hrošč0 nekaj več spomina in zahteva malo več računanja, saj v vsaki iteraciji izračuna evklidsko razdaljo do cilja in si zapomni najbližjo točko na obodu ovire do cilja. Njegovo delovanje podajata koraka: 1. V ravni liniji se premika proti cilju, dokler ne naleti na oviro ali cilj. 2. Če naleti na oviro, ob oviri zavije levo in sledi celotnemu obrisu ovire ter ves čas meri evklidsko razdaljo do cilja. Ko ponovno prispe do točke, kjer 182 Načrtovanje poti cilj cilj start start cilj start Slika 4.23: Algoritem Hrošč1 najde pot do cilja v obeh primerih na zgornjih slikah. V najslabšem primeru je njegova pot za 3 obsega vseh ovir daljša od 2 evklidske razdalje med začetno in ciljno točko. Algoritem zna ugotoviti, kdaj cilj ni dosegljiv (primer na spodnji sliki). je naletel na oviro, gre po krajši poti ob obodu ovire do točke, ki je bila najbliže cilju. Nato gre po ravni liniji proti cilju. Primer delovanja algoritma Hrošč1 je prikazan na sliki 4.23. Dobljena pot ni optimalna; v najslabšem primeru je za 3 obsega vseh ovir do 2 cilja daljša kot evklidska razdalja med začetno in ciljno točko. Algoritem za vsako oviro, na katero naleti na poti do ciljne točke, najde samo eno točko naleta na oviro in samo eno točko, v kateri zapusti obod ovire. Tako na nobeno oviro ne naleti več kot enkrat in zaradi tega nikoli ne ustvari ciklov med istimi ovirami. Ko algoritem naleti na isto oviro več kot enkrat, je to znak, da je znotraj ovire ujeta ali začetna ali ciljna točka. Takrat se algoritem konča, saj ne obstaja nobena izvedljiva pot do cilja (spodnji primer na sliki 4.23). 4.3. Preprosti algoritmi načrtovanja poti — algoritmi tipa hrošč 183 cilj cilj start start Slika 4.24: Algoritem Hrošč2 sledi glavni liniji do cilja. Na isto oviro lahko naleti večkrat, zato pride do kroženja. Algoritem Hrošč2 zna prepoznati nedosegljiv cilj. 4.3.3 Algoritem Hrošč2 Algoritem Hrošč2 se vedno poskuša premikati po glavni liniji, tj. daljici, ki povezuje začetno in ciljno točko. Deluje s ponavljanjem naslednjih korakov: 1. Robot naj se premika po glavni liniji, dokler ne naleti na oviro ali ciljno točko. V ciljni točki se iskanje zaključi. 2. Če je robot naletel na oviro, sledi obodu ovire toliko časa, da doseže glavno linijo, kjer je evklidska razdalja do cilja manjša kot evklidska razdalja do točke, kjer je (zadnjič) naletel na oviro. Čeprav se zdi algoritem Hrošč2 veliko bolj učinkovit kot Hrošč1 (leva slika 4.24), ne zagotavlja, da bo robot samo enkrat naletel na določeno oviro. Pri nekaterih postavitvah in oblikah ovir po prostoru lahko Hrošč2 dolgo časa po nepotrebnem kroži, preden prispe do ciljne točke, kar je prikazano na desni sliki 4.24. Algoritem lahko razbere, da ciljne točke ni možno doseči, če večkrat v isti točki naleti na isto oviro. Iz primerjave algoritmov Hrošč1 in Hrošč2 lahko zaključimo sledeče: • Hrošč1 je bolj temeljit algoritem iskanja, saj preišče vse možnosti pred izvršitvijo naslednjega koraka, • Hrošč2 je požrešen algoritem, saj izbere prvo obetavno opcijo, • v večini primerov je Hrošč2 bolj učinkovit kot Hrošč1, toda • Hrošč1 ima bolj predvidljivo delovanje. 184 Načrtovanje poti Primer 4.9 Izvedite načrtovanje poti z algoritmom Hrošč0 za mobilnega robota z diferenci- alnim pogonom. Predpostavite, da zemljevid okolja ni na voljo in robot pozna samo svojo trenutno lego, ciljno točko in trenutno razdaljo do cilja (meritev senzorja). Glede na algoritem Hrošč0 bi moral robot zapeljati proti cilju, če je dovolj oddaljen od katerekoli ovire (npr. več kot 0 . 2 m), in slediti oviri, če je blizu ovire. Napišite kodo svoje implementacije algoritma s pomočjo programa 4.8, ki že omogoča simulacijo gibanja robota in meritev senzorjev. Okolje in primer pridobljene poti robota sta prikazana na sliki 4.25. Program 4.8 ./src/pth/example_bug0.m 1 Ts = 0.03; % Ra č unski korak 2 t = 0: Ts :30; % Č as simulacije 3 q = [0; 0; 0]; % Za č etna lega 4 goal = [4; 4]; % Ciljna lega 5 % Ovire 6 obstacles {1} = flipud ([ -1 -1; 7 -1; 7 5; -1 5]); 7 obstacles {2} = [0.5 1; 4 1]; 8 obstacles {3} = [3 3.5; 3 2.5; 5 2.5; 3 2.5]; 9 obst = []; 10 for i = 1: length ( obstacles ) 11 o b s t = [ o b s t ; o b s t a c l e s { i } ( [ 1 : end , 1 ] ,: ) ; nan ( 1 ,2 ) ]; 12 end 13 14 for k = 1: length ( t ) 15 % R a z d a l j a do n a j b l i ž je o v i r e in u s m e r i t e v d a l j i c e 16 [ dObst , ~ , z ] = n e a r e s t S e g m e n t ( q (1:2). ’ , o b s t ); 17 p h i O b s t = a t a n 2 ( o b s t ( z +1 ,2) - o b s t ( z ,2) , o b s t ( z +1 ,1) - o b s t ( z , 1 ) ) ; 18 19 % Sem p r i d e r e g u l a c i j s k i a l g o r i t e m ... 20 21 % S i m u l a c i j a g i b a n j a r o b o t a 22 dq = [ v * cos ( q ( 3 ) ) ; v * sin ( q ( 3 ) ) ; w ]; 23 n o i s e = 0 . 0 0 ; % P a r a m e t e r za n a s t a v l j a n j e š uma ( npr . 0 . 0 0 1 ) 24 q = q + Ts * dq + r a n d n (3 ,1)* n o i s e ; % E u l e r j e v a i n t e g r a c i j a 25 q (3) = w r a p T o P i ( q ( 3 ) ) ; % Z a p i s k o t a v o b m o č ju [ - pi , pi ] 26 end 4.3. Preprosti algoritmi načrtovanja poti — algoritmi tipa hrošč 185 5 4 Cilj 3 ] [m 2 y 1 0 Start −1 0 2 4 6 x [m] Slika 4.25: Načrtovanje poti in vodenje z algoritmom Hrošč0 za vožnjo robota do cilja ob izogibanju oviram Rešitev Možna implementacija rešitve, ki ustvari pot na sliki 4.25, je podana v programu 4.9 — kodo vstavite v označeno vrstico programa 4.8. Oblika dobljene poti je odvisna tudi od uporabljenega regulacijskega algoritma za vodenje robota. Program 4.9 ./src/pth/script_bug0.m 1 % Regulacija na podlagi razdalje do ovire 2 if dObst >0.2 % Vo ž nja proti cilju 3 p h i R e f = a t a n 2 ( g o a l (2) - q (2) , g o a l (1) - q ( 1 ) ) ; 4 e P h i = w r a p T o P i ( p h i R e f - q ( 3 ) ) ; 5 d G o a l = s q r t ( sum (( goal - q ( 1 : 2 ) ) . ^ 2 ) ) ; 6 g = [ d G o a l /2 , 1]; % Oja č e n j i r e g u l a t o r j a 7 else % Vo ž nja okoli ovire po desni 8 p h i R e f = w r a p T o P i ( p h i O b s t + pi * 0 ) ; % Pri š t e j t e pi za vo ž njo po l e v i 9 e P h i = w r a p T o P i ( phiRef - q ( 3 ) ) ; 10 g = [0.4 , 5]; % Oja č e n j i r e g u l a t o r j a 11 end 12 % Enostavni regulator za d i f e r e n c i a l n i pogon 13 v = g (1)* abs ( cos ( ePhi )); 14 w = g (2)* ePhi ; 15 v = min ([ v , 0.5]); 186 Načrtovanje poti Primer 4.10 Z razširitvijo primera 4.9 izvedite tudi algoritma Hrošč1 in Hrošč2. Rešitev Za implementacijo algoritma Hrošč1 lahko prilagodite program 4.9 iz primera 4.9, kjer je glavno vedenje sestavljeno iz dveh delov. Prvo vedenje (vožnja proti cilju) ostaja nespremenjeno, drugo pa je treba spremeniti. Shranite začetni položaj, kjer robot najprej zazna oviro. Vozite se okoli ovire in izmerite razdaljo do cilja ter si zapomnite najbližjo točko. To izvajajte, dokler robot ne pride v shranjeni začetni položaj ali vsaj dovolj blizu. Vrnite se na najbližjo zapomnjeno točko. Podobno lahko program 4.9 iz primera 4.25 prilagodite algoritmu Hrošč2. Shranite začetni položaj, kjer robot najprej zazna oviro. Robot naj kroži okoli ovire, dokler ne prečka glavne linije. Če je točka prečkanja bližje cilju kot izhodiščni točki, pelje proti cilju; v nasprotnem še naprej kroži okoli ovire. 4.4 Metode iskanja poti v grafu Ko imamo okolje z ovirami ustrezno predstavljeno z grafom (npr. prostor stanj, razcep na celice, zemljevid cest), lahko uporabimo enega izmed algoritmov, ki poiščejo pot od začetne do ciljne konfiguracije. V nadaljevanju je podanih nekaj znanih algoritmov iskanja poti v grafu. V splošnem začnemo iskanje tako, da preverimo, ali je začetno vozlišče hkrati tudi ciljno vozlišče. Ponavadi to ne drži, zato razširimo iskanje na vozlišča, ki sledijo sedanjemu vozlišču. Na podlagi izbranega algoritma (in vrednosti cenilke) izberemo enega izmed sosednjih vozlišč. Če izbrano vozlišče ni ciljno, raziskujemo naslednja vozlišča, ki sledijo temu novemu vozlišču. Postopek nadaljujemo, dokler ne najdemo rešitve ali dokler ne preiščemo celotnega grafa. Pri iskanju v grafu vodimo sezname vozlišč, ki smo jih med iskanjem že obiskali, s čimer preprečimo, da bi večkrat obiskali isto vozlišče. Tako imenovana živa vozlišča, iz katerih lahko nadaljujemo iskanje, shranimo na seznam odprtih vozlišč. Mrtva vozlišča nimajo naslednikov ali pa smo jih že preverili in jih shranimo na seznam zaprtih vozlišč. Od strategije algoritma je odvisno, po kakšnem zaporedju izbiramo vozlišča za razširitev območja iskanja. Seznam odprtih vozlišč razvrstimo glede na določen kriterij in ob izbiranju naslednjega vozlišča za razširitev iskanja vzamemo prvo s seznama, torej tisto, ki najbolj ustreza razvrščevalnemu kriteriju (ima najmanjšo vrednost glede na kriterij). 4.4. Metode iskanja poti v grafu 187 Slika 4.26: Algoritem iskanja v širino najprej razišče najbližja vozlišča. Trenutno vozlišče je označeno s puščico, odprta (živa) vozlišča so označena s svetlo sivimi, zaprta (mrtva) vozlišča s črnimi in nepreverjena vozlišča z belimi krogi. Na začetku je na seznamu odprtih vozlišč Q samo začetno vozlišče. Izračunamo vozlišča, ki sledijo začetnemu, in jih zapišemo na seznam odprtih vozlišč, začetno vozlišče pa damo na seznam zaprtih vozlišč. Nato iskanje razširimo s prvim vozliščem na odprtem seznamu tako, da izračunamo njegove naslednike. Tako so na odprtem seznamu preostali nasledniki (razen prvega že raziskanega vozlišča) in pravkar izračunani nasledniki prej izbranega vozlišča. Postopek je prikazan na sliki 4.26, kjer so odprta vozlišča prikazana z svetlo sivimi krogi in lahko vidimo, zakaj se ta vozlišča med iskanjem v drevesni strukturi imenujejo listi. Zaprta vozlišča so prikazana s črnimi krogi, nepreverjena vozlišča pa z belimi. Ločimo neinformirane in informirane algoritme iskanja po grafu. Neinformi-rani algoritmi posedujejo samo informacije, ki so podane z definicijo problema (slepo iskanje po stanjih oz. vozliščih). Graf pregledujejo sistematično in ne razlikujejo med bolj ali manj obetavnimi vozlišči. Informirano ali hevristično iskanje vsebuje dodatne informacije o vozliščih, zato je zmožen razlikovati med bolj in manj obetavnimi vozlišči in posledično je lahko algoritem učinkovitejši. 4.4.1 Iskanje v širino Iskanje v širino (BFS, angl. breadth-first search) je algoritem neinformiranega iskanja. Najprej raziščemo najbolj plitva vozlišča, torej vozlišča, ki so najbliže začetnemu vozlišču. Vsa vozlišča, do katerih lahko dostopamo v k korakih, obiščemo prej kot katerokoli vozlišče, do katerega pridemo v k + 1 korakih, kar je prikazano na sliki 4.26. Seznam odprtih vozlišč Q razvrščamo po metodi prvi noter, prvi ven (FIFO, angl. first in, first out): na novo odprta vozlišča dodajamo na konec seznama Q, vozlišča za razširjanje iskanja pa jemljemo z začetka seznama. Algoritem je popoln, saj pri končnem faktorju razvejitve najde rešitev, če le-ta obstaja. Če pa je možnih več rešitev, najde tisto, ki je najmanj korakov oddaljena od začetnega vozlišča. To ne pomeni, da je najdena rešitev hkrati tudi optimalna, saj ni nujno, da imajo vsi prehodi med vozlišči enako ceno. Poraba spomina in računski čas sta pri tej metodi velika, saj z razvejanostjo drevesa eksponentno naraščata. 188 Načrtovanje poti cilj Slika 4.27: Iskanje v globino ima majhno porabo spomina, saj hranimo le liste (svetlo siva) in na poti razširjena vozlišča (temno siva). Prečrtana vozlišča so odstranjena iz spomina. 4.4.2 Iskanje v globino Iskanje v globino (DFS, angl. depth-first search) je algoritem neinformiranega iskanja, kjer iščemo v globino. Vozlišče, ki je najbolj oddaljeno od začetnega, razširimo in iskanje na ta način nadaljujemo v globino, dokler neko vozlišče nima več nobenih naslednikov. Takrat iskanje nadaljujemo z naslednjim najglobljim vozliščem, katerega nasledniki še niso bili raziskani, kot je to prikazano na sliki 4.27 Seznam odprtih vozlišč Q obravnavamo kot sklad (angl. stack), ki ga razvrščamo po metodi zadnji noter, prvi ven (angl. last in, first out – LIFO): na novo odprta vozlišča dodajamo na začetek seznama Q, od koder tudi jemljemo vozlišča za razširjanje iskanja. Algoritem iskanja v globino ni popoln, saj bi v primeru neomejene globine (neskončno število vej, ki se ne končajo) neskončno časa raziskoval samo eno vejo grafa. Temu se lahko izognemo tako, da iskanje omejimo do določene globine, kjer pa je možno, da ima rešitev večjo globino in jo zato algoritem ne najde. Prav tako algoritem ni optimalen, saj najdena pot ni nujno tudi najkrajša. Iskanje v globino ima majhno porabo spomina, saj mora hraniti samo pot od začetnega do trenutnega vozlišča in vmesna neraziskana vozlišča, s katerimi še 4.4. Metode iskanja poti v grafu 189 Globina 0 korakov 1 korak 2 koraka 3 koraki cilj Slika 4.28: Prikaz iterativnega poglabljanja iskanja v globino do globine treh korakov. Prečrtana vozlišča so odstranjena iz spomina. Algoritem se zaključi v tretjem koraku, kjer najde ciljno vozlišče. nismo nadaljevali iskanja. Ko raziščemo neko vozlišče in vse njegove naslednike, lahko to vozlišče prenehamo hraniti v spominu. 4.4.3 Iterativno iskanje v globino Iterativno iskanje v globino (IDDFS, angl. iterative deepening depth-first search) združuje prednosti iskanja v širino in iskanja v globino. Algoritem po korakih veča globino, do katere raziskujemo z iskanjem v globino, dokler ne najdemo ciljnega vozlišča. Najprej izvedemo iskanje v globino za vozlišča oddaljena nič korakov od začetnega. V primeru, da ne najdemo ciljnega vozlišča, iskanje v globino ponovimo za vozlišča, oddaljena en korak od začetnega vozlišča itd. Tako se iskanje izvaja tudi v širino. Algoritem ima majhno porabo spomina, je popoln, saj najde rešitev, če le-ta obstaja, in optimalen, ker najde najkrajšo pot, če so vse cene prehodov enake ali ne naraščajo z globino vozlišča. Če imajo vsa vozlišča približno enak faktor razvejitve tudi večkratno računanje stanj ni pretirano potratno, saj je večina vozlišč na dnu drevesa in jih algoritem obišče le občasno. 190 Načrtovanje poti 4.4.4 Dijkstrov algoritem Dijkstrov algoritem je neinformiran algoritem za iskanje najkrajše poti od enega začetnega vozlišča do vseh preostalih vozlišč v grafu [13]. Prvotno ga je zasnoval Edsger Dijkstra [14], kasneje pa so ga razširili z raznimi prilagoditvami. V primeru iskanja poti med eno začetno in eno ciljno točko deluje neučinkovito, zaradi izračunavanja optimalnih poti do vseh vozlišč, zato lahko določimo, da se konča, ko izračuna najkrajšo želeno pot. Algoritem zagotovo najde najkrajšo pot od začetnega do ciljnega vozlišča, saj temelji na računanju cene poti od začetnega do trenutnega vozlišča, ki jo imenujmo cena-do-sem. Ceno poti do trenutno obravnavanega vozlišča izračunamo kot vsoto cene celotne poti do vozlišča, iz katerega smo prišli do trenutnega vozlišča, in cene povezave med njima. V primeru več najkrajših poti (z isto ceno) algoritem vrne eno in ni pomembno katero. Za izvajanje algoritma je potrebno označiti povezave med vozlišči in jim določiti ceno. Za vsako obiskano vozlišče shranjujemo ceno trenutno najkrajše poti do njega ( cena-do-sem) in povezave, po kateri smo prišli do trenutnega vozlišča. Pri iskanju vodimo tudi seznam odprtih in zaprtih vozlišč. Na začetku je v seznamu odprtih vozlišč samo začetno vozlišče, katerega cena-do-sem je nič in brez predhodne povezave (ni prejšnjega vozlišča). Seznam zaprtih vozlišč je prazen. Nato ponavljamo naslednje korake, ki so ilustrirani na sliki 4.29 1. Iz seznama odprtih vozlišč vzamemo prvo vozlišče; to naj bo trenutno vozlišče. Seznam naj bo urejen naraščajoče glede na ceno-do-sem, kjer je prvo vozlišče tisto z najmanjšo ceno-do-sem. 2. Vsem vozliščem, do katerih lahko pridemo iz trenutnega vozlišča in niso na seznamu zaprtih vozlišč, izračunamo ceno-do-sem kot vsoto cene-do-sem trenutnega vozlišča in cene vmesne povezave. 3. Izračunamo in shranimo ceno-do-sem ter povezavo do trenutnega vozlišča za vsa vozlišča, ki še nimajo shranjenih teh informacij. 4. Če je v prejšnjem koraku katero izmed teh vozlišč že imelo shranjeno ceno-do-sem in ustrezno povezavo v grafu iz katere od prejšnjih iteracij, ti dve ceni primerjamo in kot končen podatek shranimo manjšo ceno in ustrezno povezavo. 5. Vozlišča dodamo na seznam odprtih vozlišč in ga uredimo po naraščajoči vrednosti cene-do-sem. Takšen seznam, ki ga imenujemo vrsta s prednostjo (angl. priority queue), omogoča, da hitreje najdemo vozlišče z najmanjšo vrednostjo cene-do-sem kot pa z iskanjem po neurejenem seznamu. Trenutno vozlišče premaknemo na seznam zaprtih vozlišč. 4.4. Metode iskanja poti v grafu 191 0 0 3 3 A B 3,A A B 3,A 2 6 5 2 6 5 C D C D 5 5 4 4 1 2 1 1 6,A 2 1 2,A 2,A 6,A E F E 4 4 F 4,A 4,A 3,C 0 0 3 A 3 B 3,A A B 3,A 2 6 5 2 6 5 C D 5 C D 4 5 4 1 2 1 2,A 6,A 1 2 1 2,A 5,E 5,E E 4 F E 4 F 3,C 7,E 3,C 7,E 0 0 3 A B 3,A 3 A B 3,A 2 6 5 2 6 5 C D 5 4 C D 5 4 1 2 1 2,A 5,E 1 2 1 2,A 5,E E 4 F E 4 F 3,C 7,E 3,C 6,D 6,D Slika 4.29: Dijkstrov algoritem za iskanje najkrajše poti med začetnim vozliščem A in ostalimi vozlišči. Trenutno oglišče je označeno s sivim kvadratom, njegovi nasledniki pa s puščicami. Cene poti so označene ob povezavah. Ob vozliščih je označena cena-do-sem in povezava do predhodnega vozlišča. Odprta vozlišča so označena s svetlo sivo, zaprta vozlišča pa s temno sivo. Primer: Zanima nas najkrajša pot med vozliščema A in F. Vidimo, da je CenaF − D− E− C− A = 6, pot pa gre skozi vozlišča A → C → E → D → F . 192 Načrtovanje poti V osnovni verziji naj bi se Dijkstrov algoritem končal, ko je seznam odprtih vozlišč prazen in rezultat vsebuje najkrajše poti iz začetnega v vsa ostala vozlišča. Če potrebujemo samo najkrajšo pot od začetnega do končnega vozlišča, iskanje zaključimo, ko dodamo ciljno vozlišče na seznam zaprtih vozlišč. Ustrezno pot dobimo tako, da vzvratno sledimo in izpišemo po katerih povezavah smo prišli do ciljnega vozlišča, gledamo torej povezave v smeri od ciljnega do začetnega vozlišča. Najprej pogledamo povezavo od ciljnega vozlišča do predhodnega, nato sledimo povezavi do predhodega vozlišča, kjer prav tako preberemo in izpišemo po kateri povezavi smo prišli do tja. S tem nadaljujemo, dokler ne dosežemo začetnega vozlišča. Na koncu seznam izpisanih povezav le še obrnemo. Dijkstrov algoritem je popoln (če pot obstaja, jo najde) in optimalen (najdena pot je najkrajša), če so vse uteži (cene) povezav večje od nič. 4.4.5 Algoritem A ? A ? (prebrano kot a zvezdica ali a star ) je informiran algoritem iskanja, saj vsebuje dodatno informacijo ali hevristiko. Hevristika je ocena cene poti od trenutnega vozlišča do cilja, zaradi česar lahko algoritem razlikuje med bolj ali manj obetavnimi vozlišči in je učinkovitejši pri iskanju rešitve. Algoritem za vsako vozlišče izračuna vrednost izbrane hevristične funkcije, ki predstavlja oceno cene, potrebne za pot od tega vozlišča do cilja, imenovana cena-do-cilja. Hevristična funkcija je lahko evklidska razdalja, razdalja Manhattan (vsota premikov v navpični in vodoravni smeri) ali kakšna druga primerna funkcija. Tekom izvajanja algoritem za vsako vozlišče računa ceno-celotne-poti tako, da za določeno vozlišče sešteje ceno-do-sem in ceno-do-cilja. Hkrati vodi tudi seznama odprtih in zaprtih vozlišč. Algoritem deluje tako, da je na začetku v seznamu odprtih vozlišč samo začetno vozlišče, katerega cena-do-sem je nič in nima predhodne povezave, seznam zaprtih vozlišč pa je prazen. V nadaljevanju se ponavljajo naslednji koraki, ki so ilustrirani na sliki 4.30: 1. Iz seznama odprtih vozlišč vzamemo prvo vozlišče, imenovano trenutno vozlišče. Seznam je urejen naraščajoče glede na ceno-celotne-poti, kjer ima prvo vozlišče najmanjšo ceno-celotne-poti. 2. Vsem vozliščem, do katerih lahko pridemo iz trenutnega vozlišča, izraču- namo • ceno-do-cilja, • ceno-do-sem kot vsoto cene-do-sem trenutnega vozlišča in vmesne povezave ter • ceno-celotne-poti kot vsoto cene-do-sem in cene-do-cilja. 4.4. Metode iskanja poti v grafu 193 3. Za vsako izmed teh vozlišč, ki še nima shranjene izračunane cene-do-sem, ustrezne vmesne povezave od trenutnega vozlišča, cene-do-cilja in cene-celotne-poti, shranimo vse naštete vrednosti. 4. Če je v prejšnjem koraku katero izmed teh vozlišč že imelo shranjene izračunane vrednosti iz katere od prejšnjih iteracij, primerjamo obe ceni-do-sem in kot končen podatek shranimo manjšo skupaj s pripadajočo povezavo in ustrezno ceno-celotne-poti. 5. Vozlišča, katerih vrednosti smo računali prvič, dodamo na seznam odprtih vozlišč. Vozlišča, ki smo jim posodobili vrednosti in so že bila na seznamu odprtih vozlišč, jih tam tudi obdržimo. Vozlišča, ki so bila na seznamu zaprtih vozlišč in katerih vrednosti smo posodobili (do njega smo našli pot z manjšo ceno-do-sem), premaknemo na seznam odprtih vozlišč, ki ga nato uredimo po naraščajoči vrednosti cene-celotne-poti. Trenutno vozlišče premaknemo na seznam zaprtih vozlišč. Na prvi sliki 4.30 je trenutno vozlišče hkrati tudi začetno ter so izbrana njegova naslednja vozlišča, ki so dosegljiva iz štirih smeri: levo, desno, gor in dol. Za vsa ta naslednja vozlišča je cena-do-sem enaka 1, saj so samo en korak stran od začetnega vozlišča, ceno-do-cilja pa določa razdalja Manhattan (hevristika) iz naslednjega do ciljnega vozlišča, ki jo lahko izmerimo preko ovire. Vsota obeh cen je cena-celotne-poti. Vozlišča, ki sledijo, imajo s puščico označeno povezavo do trenutnega vozlišča (označeno s krogom). Odprti seznam vozlišč tako vsebuje ta štiri naslednja vozlišča, zaprti seznam vozlišč pa vsebuje samo začetno vozlišče. Na drugi sliki 4.30 iz odprtega seznama izberemo za trenutno vozlišče tisto, ki ima najmanjšo ceno-celotne-poti (v tem primeru je enaka 6). Trenutno vozlišče ima samo enega naslednika, saj druge celice blokira ovira ali pa se nahajajo na zaprtem seznamu. Cena-do-sem za naslednje vozlišče je 2, ker se nahaja dva koraka stran (razdalja Manhattan) od začetnega vozlišča, in cena-do-cilja je 8. Trenutno vozlišče premaknemo na zaprti seznam, naslednje vozlišče pa na odprti seznam. Na tretji sliki 4.30 izberemo trenutno vozlišče kot vozlišče iz odprtega seznama, ki ima najmanjšo ceno-celotne-poti (v našem primeru je 6). Algoritem ponavlja korake dokler ne doseže ciljnega vozlišča. Algoritem A ? zagotavlja optimalnost najdene poti v grafu, v kolikor je hevristika ( cena-do-cilja) optimistična, kar pomeni, da je cena-do-cilja za vsako vozlišče manjša ali kvečjemu enaka pravi ceni-do-cilja. Algoritem zaključimo, ko dodamo ciljno vozlišče na seznam zaprtih vozlišč. Algoritem A ? je popoln, saj vedno najde pot, če le-ta obstaja, pri uporabi optimistične hevristike pa je tudi optimalen. Slabost A ? je velika poraba spomina. V primeru, da so vse cene-do-cilja enake nič, je delovanje algoritma A ? enako Dijkstrovemu. Na sliki 4.31 je prikazana primerjava delovanja Dijkstrovega algoritma in A ? . 194 Načrtovanje poti 1+7=8 1+7=8 2+6=8 1+7=8 2+6=8 1+7=8 1+5=6 1+7=8 1+5=6 1+7=8 1+5=6 0 0 0 2+6=8 1+5=6 1+5=6 1+5=6 2+4=6 1+7=8 2+6=8 1+7=8 2+6=8 1+7=8 2+6=8 1+7=8 1+5=6 1+7=8 1+5=6 1+7=8 1+5=6 0 0 0 2+6=8 2+6=8 2+6=8 1+5=6 1+5=6 1+5=6 3+5=8 3+3=6 3+5=8 3+3=6 4+2=6 3+5=8 3+3=6 4+2=6 5+1=6 2+4=6 2+4=6 2+4=6 3+5=8 3+5=8 4+4=8 3+5=8 4+4=8 5+3=8 1+7=8 2+6=8 1+7=8 2+6=8 1+7=8 1+5=6 1+7=8 1+5=6 0 0 2+6=8 6+2=8 2+6=8 6+2=8 1+5=6 1+5=6 3+5=8 3+3=6 4+2=6 5+1=6 6+0=6 3+5=8 3+3=6 4+2=6 5+1=6 6+0=6 2+4=6 2+4=6 3+5=8 4+4=8 5+3=8 6+2=8 3+5=8 4+4=8 5+3=8 6+2=8 Slika 4.30: Trenutno vozlišče je označeno s krogom. Začetno vozlišče je zeleno, končno vozlišče oranžno, odprta vozlišča so svetlo siva, zaprta vozlišča temno siva in ovire so črne. Možne so štiri smeri prehodov: levo, desno, gor in dol. V vsaki celici (vozlišče) je označena smer do trenutnega vozlišča in cena poti, ki je vsota cene-do-sem in cene-do-cilja. Za izračun cene je uporabljena razdalja Manhattan. Najdeno pot razberemo s sledenjem povezavam, označene so s puščicami. 1 1+7=8 2+6=8 2 2 3 4 5 6 1+7=8 1+5=6 1 0 1 5 6 0 2+6=8 6+2=8 2 6 7 1 1+5=6 3+5=8 3+3=6 4+2=6 5+1=6 6+0=6 3 2 3 4 5 6 2+4=6 4 3 4 5 6 3+5=8 4+4=8 5+3=8 6+2=8 5 4 5 6 6 5 6 Slika 4.31: Primerjava algoritmov iskanja poti: Dijkstrov (levo) in A ? (desno). Oba najdeta najkrajšo pot do cilja, vendar A ? porabi precej manj iteracij zaradi uporabe dodatne informacije (hevristike) pri iskanju poti. 4.4. Metode iskanja poti v grafu 195 Slika 4.32: Primerjava algoritmov iskanja poti: Pohlepno iskanje najprej-najboljši (levo) in A ? (desno). Pot, ki jo najdemo s pohlepnim iskanjem na način najprej najboljši, ni vedno optimalna. Odprta vozlišča so označena s svetlo sivo, zaprta vozlišča pa s temno sivo. 4.4.6 Pohlepno iskanje najprej najboljši Pohlepno iskanje na način najprej najboljši (GBFS, angl. greedy best-first search) je informiran oz. hevrističen algoritem. Seznam odprtih vozlišč razvrščamo po naraščajoči ceni-do-cilja. Tako v vsaki iteraciji razširimo iskanje na tisto odprto vozlišče, ki je najbliže cilju (ima najmanjšo ceno do cilja), saj predvidevamo, da bomo tako najhitreje dosegli cilj. Vendar najdena celotna pot ni nujno optimalna (najkrajša), kot prikazuje slika 4.32, saj algoritem upošteva le ceno od trenutnega do ciljnega vozlišča in ga ne zanima cena do trenutnega vozlišča. Posledično tudi ni pomembno, ali je hevristika optimistična ali ne, kot je bilo pomembno pri algoritmu A ? . Primer 4.11 Izvedite načrtovanje poti z algoritmom A ? v okolju na sliki 4.32. Primerjajte dobljene rezultate z rezultati na sliki 4.32. Za izračun razdalje uporabite razdaljo Manhattan, medtem ko za hevristiko ( cena-do-cilja) uporabite razdaljo Manhattan ali evklidsko razdaljo ter tudi primerjajte njune rezultate. Nato prilagodite kodo za pohlepno iskanje najprej najboljši. Rešitev V splošnem je zaželena uporaba algoritma A ? . Možna izvedba algoritma je prikazana v programu 4.10, kjer smo za izračun cene-do-sem uporabili razdaljo Manhattan, za izračun hevristike pa evklidsko razdaljo. 196 Načrtovanje poti Program 4.10: Implementacija algoritma A ? ./src/pth/AStarBase.m 1 classdef AStarBase < handle 2 properties 3 map = []; % Z e m l j e v i d : 0 - prosto , 1 - o v i r a 4 o p e n = []; c l o s e d = []; s t a r t = []; g o a l = []; act = []; p a t h = []; 5 end 6 7 methods 8 f u n c t i o n p a t h = f i n d ( obj , start , g o a l ) % s t a r t =[ is ; js ] , g o a l =[ ig ; jg ] 9 obj . s t a r t = s t a r t ; obj . g o a l = g o a l ; obj . p a t h = []; 10 obj . c l o s e d = []; % P r a z e n z a p r t i s e z n a m 11 obj . o p e n = s t r u c t ( ’ id ’ , start , ’ src ’ , [0; 0] , ’ g ’ , 0 , ... 12 ’ h ’ , obj . h e u r i s t i c ( s t a r t )); % Za č e t n i o d p r t i s e z n a m 13 14 if obj . map ( s t a r t (1) , s t a r t ( 2 ) ) ~ = 0 || obj . map ( g o a l (1) , g o a l ( 2 ) ) ~ = 0 15 p a t h = []; r e t u r n ; % Pot ne o b s t a j a ! 16 end 17 18 w h i l e t r u e % I s k a n j e 19 if i s e m p t y ( obj . o p e n ) , b r e a k ; end % Pot ni b i l a n a j d e n a :( 20 21 obj . act = obj . o p e n ( 1 ) ; % V o z l i š č e z u r e j e n e g a o d p r t e g a s e z n a m a 22 obj . c l o s e d = [ obj . closed , obj . act ]; % d a m o na z a p r t i s e z n a m 23 obj . o p e n = obj . o p e n (2: end ); % in ga o d s t r a n i m o z o d p r t e g a s e z n a m a . 24 25 if obj . act . id ( 1 ) = = obj . g o a l (1) && obj . act . id ( 2 ) = = obj . g o a l (2) 26 % Pot o b s t a j a :) Poi š č emo pot s p o m o č jo z a p r t e g a s e z n a m a ... 27 p = obj . act . id ; obj . p a t h = [ p ]; ids = [ obj . c l o s e d . id ]; 28 w h i l e sum ( abs ( p - s t a r t ) ) ~ = 0 % S l e d i m o s t a r š em do s t a r t a 29 p = obj . c l o s e d ( ids (1 ,: )= = p (1) & ids ( 2 ,: ) == p ( 2 ) ) . src ; 30 obj . p a t h = [ p , obj . p a t h ]; 31 end 32 b r e a k ; 33 end 34 35 n e i g h b o u r s = obj . g e t N o d e N e i g h b o u r s ( obj . act . id ); 36 for i = 1: s i z e ( n e i g h b o u r s , 2) 37 n = n e i g h b o u r s (: , i ); 38 % V o z l i š č e d o d a m o na o d p r t i seznam , č e ni ž e na z a p r t e m 39 % s e z n a m u in ni o v i r a 40 ids = [ obj . c l o s e d . id ]; z = ids (1 ,: )= = n (1) & ids ( 2 ,: ) == n ( 2 ) ; 41 if i s e m p t y ( f i n d ( z , 1)) && ~ obj . map ( n (1) , n ( 2 ) ) 42 obj . a d d N o d e T o O p e n L i s t ( n ); 43 end 44 end 45 end 46 p a t h = obj . p a t h ; 47 end 48 49 f u n c t i o n a d d N o d e T o O p e n L i s t ( obj , i ) 50 g = obj . act . g + obj . c o s t ( i ); % C e n a p o t i 51 % P r e v e r i m o , č e je v o z l i š č e ž e na o d p r t e m s e z n a m u 52 ids = [ obj . o p e n . id ]; s = []; 53 if ~ i s e m p t y ( ids ) 54 s = sum ( abs ( ids - r e p m a t ( i , 1 , s i z e ( ids , 2 ) ) ) ) = = 0 ; 55 end 56 if i s e m p t y ( f i n d ( s , 1)) % D o d a m o v o z l i š č e na o d p r t i s e z n a m 57 n o d e = s t r u c t ( ’ id ’ , i , ’ src ’ , obj . act . id , ... 58 ’ g ’ , g , ’ h ’ , obj . h e u r i s t i c ( i )); 59 obj . o p e n = [ obj . open , n o d e ]; 60 e l s e % P o s o d o b i m o v o z l i š č e na o d p r t e m s e zn am u , č e ima b o l j š o c e n o 4.4. Metode iskanja poti v grafu 197 61 if g < obj . o p e n ( s ). g 62 obj . o p e n ( s ). g = g ; 63 obj . o p e n ( s ). src = obj . act . id ; 64 end 65 end 66 % U r e d i m o o d p r t i s e z n a m 67 [~ , i ] = s o r t r o w s ([[ obj . o p e n . g ]+[ obj . o p e n . h ]; obj . o p e n . h ]. ’ , [1 ,2 ]) ; 68 obj . o p e n = obj . o p e n ( i ); 69 end 70 71 f u n c t i o n n = g e t N o d e N e i g h b o u r s ( obj , a ) 72 n = [ a (1) -1 , a (1) , a (1) , a ( 1 ) + 1 ; a (2) , a (2) -1 , a (2)+1 , a ( 2 ) ] ; 73 [ h , w ] = s i z e ( obj . map ); 74 n = n (: , n (1 ,:) >=1 & n (1 ,:) <= h & n (2 ,:) >=1 & n (2 ,:) <= w ); % M e j e 75 end 76 77 f u n c t i o n g = c o s t ( obj , a ) 78 g = sum ( abs ( a - obj . act . id )); % M a n h a t t a n s k a r a z d a l j a 79 end 80 81 f u n c t i o n h = h e u r i s t i c ( obj , a ) 82 h = s q r t ( sum (( a - obj . g o a l ) . ^ 2 ) ) ; % E v k l i d s k a r a z d a l j a 83 end 84 end 85 end V programu 4.11 je prikazana uporaba algoritma A ? iz programa 4.10. Stolpci izhodne spremenljivke path predstavljajo urejen seznam celic (vozlišč), ki vodijo od začetka do cilja. Za uporabo različnih hevristik je potrebna minimalna sprememba algoritma (npr. razdalja Manhattan, ki je bila uporabljena za pridobitev poti, prikazana na sliki 4.32). Program 4.11: Uporaba algoritma A ? ./src/pth/example_astar_base.m 1 map = zeros (14 , 14); % Zemljevid 2 map (5:10 ,[4 11]) = 1; map (5 ,4:11) = 1; % Ovire 3 4 astar = AStarBase (); 5 astar . map = map ; 6 path = astar . find ([11; 6] , [4; 10]) p a t h = C o l u m n s 1 t h r o u g h 13 11 11 11 11 11 11 11 10 9 8 7 6 5 6 7 8 9 10 11 12 12 12 12 12 12 12 C o l u m n s 14 t h r o u g h 16 4 4 4 12 11 10 Algoritem A ? se lahko prevede v pohlepno iskanje najprej najboljši, če je cena-do-sem nastavljena na nič. 198 Načrtovanje poti Primer 4.12 Razširite primer 4.11 za razcep okolja s pomočjo štiriškega drevesa in poiščite optimalno pot z algoritmom A ? . Rešitev Za razširitev algoritma A ? iz programa 4.10 s štiriškim drevesom je potrebnih le nekaj manjših sprememb. V tem primeru niso vse celice enako velike, zato so med njimi različne razdalje in vsaka celica ima drugačno število sosedov. Torej moramo spremeniti način določanja sosednjih celic. Izvedba algoritma za razgradnjo s pomočjo štiriškega drevesa v programu 4.2 ustvari graf vidljivosti tako, da za vsako celico v štiriškem drevesu poišče vse sosednje celice. Za določitev cene-do-sem lahko izračunamo razdaljo med celicami kot evklidsko razdaljo med središči celic (glejte sliko 4.10). Literatura 199 Literatura [1] H. Choset, K. Lynch in sod. Principles of robot motion: theory, algorithms, and implementations. MIT Press, illustrate izd., 2005, 603 str. [2] S. M. LaValle. Planning algorithms. Cambridge University Press, 2006, 1007 str. [3] B. Siciliano in O. Khatib. Springer Handbook of Robotics. Springer, 2008, 1611 str. [4] S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. TR 98-11. Tehn. por., Computer Science Dept., Iowa State Univ., Iowa, 2013. [5] S. M. LaValle in J. J. Kuffner. Randomized kinodynamic planning. International Journal of Robotics Research, zv. 20, št. 5, str. 278–400, 2001. [6] J. J. Kuffner in S. M. LaValle. RRT-Connect: An Efficient Approach to Single-Query Path Planning. V IEEE International Conference on Robotics and Automation (ICRA 2000), str. 1–7. IEEE, 2000. [7] L. E. Kavraki, P. Svestka in sod. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, zv. 12, št. 4, str. 566–580, 1996. [8] Z. Sun, D. Hsu in sod. Narrow Passage Sampling for Probabilistic Roadmap Planning. IEEE Transactions on Robotics, zv. 21, št. 6, str. 1105–1115, 2005. [9] V. Lumelsky in P. Stepanov. Dynamic Path Planning for a Mobile Automaton with Limited Information on the Environment. IEEE Transactions on Automatic Control, zv. 31, št. 11, str. 1058–1063, 1986. [10] A. Sankaranarayanan in M. Vidyasagar. A new path planning algorithm for moving a point object amidst unknown obstacles in a plane. V IEEE Conference on Robotics and Automation, str. 1930–1936. IEEE, 1990. [11] I. Kamon in E. Rivlin. Sensory-Based Motion Planning with Global Proofs. IEEE Transactions on robotics, zv. 13, št. 6, str. 814–821, 1997. [12] S. Laubach in J. Burdick. An autonomous Sensor-Based Path-Planner for Planetary Microrovers. V IEEE Conference on Robotics and Automation, str. 347–354. IEEE, 1999. [13] K. Mehlhorn in P. Sanders. Algorithms and Data Structures: The Basic Toolbox. Springer, 2008, 300 str. [14] E. W. Dijkstra. A note on two problems in connection with graphs. Numerische Mathe-matik, zv. 1, str. 269–271, 1959. 5 Senzorji v mobilnih sistemih 5.1 Uvod Kolesni mobilni roboti zaznavajo okolje s pomočjo senzorjev, kar jim omogoča avtonomno delovanje v okolju. Senzorji se uporabljajo za obvladovanje negotovosti in motenj, ki so vedno prisotne v okolju in v vseh robotskih podsistemih, kot je negotovost zemljevida okolice, neznani model gibanja, neznana dinamika itd. Negotovi so tudi izidi akcij (premiki mobilnega robota), zaradi neidealnih aktuatorjev. Primarni namen senzorjev torej je, da zmanjšajo ali odpravijo te negotovosti in s tem omogočijo ocenjevanje stanj mobilnega robota kot tudi izgleda okolice. Običajno z enim senzorjem ni možna elegantna rešitev, zlasti za uporabo v zaprtih prostorih, za (dovolj) natančno in robustno merjenje želenih informacij, npr. leg robota. Lega je potrebna za lokalizacijo robota, kar pa predstavlja enega izmed največjih izzivov v mobilni robotiki. Zato je potrebna uporaba več senzorjev, kjer s pomočjo metod zlivanja (fuzije) njihovih informacij dobimo bolj kakovostne in robustne informacije. Ocena lege robota običajno združuje relativne in absolutne senzorje. Relativni senzorji podajajo informacije relativno glede na koordinate mobilnega robota, medtem ko so meritve absolutnih senzorjev podane v nekem globalnem koordinatnem sistemu (npr. zemeljske koordinate). S pomočjo senzorjev lahko mobilni robot zaznava stanje okolice, pri čemer je potrebno informacije senzorjev predhodno ustrezno analizirati in interpretirati. Meritve v realnem okolju se spreminjajo dinamično (npr. spremembe osvetlitve, 202 Senzorji v mobilnih sistemih različna absorpcija zvoka ali svetlobe na površinah). Pogrešek merjenja pogosto statistično modeliramo z neko gostoto verjetnosti, za katero običajno predpostavimo, da je simetrična ali celo normalna. Vendar ta predpostavka ni vedno pravilna; npr. ultrazvočni merilnik razdalje lahko zaradi večkratnih odbojev (od oddajnika do sprejemnika) izmeri preveliko razdaljo. V nadaljevanju so na kratko opisane transformacije koordinatnih sistemov, ki so potrebne za pravilno predstavitev senzorskih meritev robotu in za oceno relevantnih informacij v koordinatnem sistemu robota. Sledi poglavje, v katerem so pojasnjene glavne metode za lokalizacijo, ki se uporabljajo za oceno lege robota v okolici z uporabo izbranih senzorjev. Na koncu je podan kratek pregled pogosto uporabljenih senzorjev v mobilni robotiki. 5.2 Transformacije koordinatnih sistemov Senzorji običajno niso nameščeni v središču robota ali izhodišču njegovega ko- ordinatnega sistema. Njihova pozicija in orientacija na robotu sta opisani s translacijskim vektorjem in rotacijo glede na koordinatni sistem robota. S po- močjo transformacij lahko izmerjene veličine v koordinatnem sistemu senzorja prevedemo v koordinatni sistem robota. S transformacijami lahko izrazimo izmerjeni smerni vektor (npr. pospeškometer, magnetometer) ali izmerjene pozicijske koordinate (npr. laserski pregledovalnik razdalj, kamera) v koordinatnem sistemu robota. Poleg tega se mobilni roboti premikajo po prostoru, zato lahko njihovo lego ali premike opišemo z ustreznimi transformacijami. V nadaljevanju je podan kratek splošni pregled transformacij za tridimenzionalni prostor, čeprav sta v mobilni robotiki običajno dovolj dve dimenziji (npr. ravninsko gibanje, ki ga opisujeta dve translaciji in ena rotacija). Najprej bo opisana transformacija rotacije, nato pa še translacije. 5.2.1 Orientacija in rotacija Orientacijo nekega lokalnega koordinatnega sistema (npr. senzor) glede na referenčni koordinatni sistem (npr. robot) opisuje rotacijska matrika R   u 1 u 2 u 3 R =  v  1 v 2 v 3   w 1 w 2 w 3 kjer so enotski vektorji lokalnega koordinatnega sistema u, v in w izraženi v referenčnem koordinatnem sistemu kot u = [ u 1 , u 2 , u 3] T , v = [ v 1 , v 2 , v 3] T , w = [ w 1 , w 2 , w 3] T in velja u×v = w. Vrstice matrike R so komponente lokalnih enotskih vektorjev vzdolž referenčnih enotskih vektorjev x, y in z. Elementi 5.2. Transformacije koordinatnih sistemov 203 matrike R so kosinusi kotov med posameznimi osmi obeh koordinatnih sistemov, zato jo imenujemo matrika smernih kosinusov (DCM, angl. direction cosine matrix) ali rotacijska matrika. Ker so vektorji u, v in w ortonormalni, sta inverz in transponiranka matrike R enaki (det R = 1 in R−1 = R T ). Rotacijska matrika ima 9 parametrov za opis treh stopenj prostosti, vendar ti parametri niso medsebojno neodvisni, ampak so definirani s šestimi omejitvenimi relacijami (vsota kvadratov elementov poljubne vrstice ali stolpca matrike R je 1 in skalarni produkt poljubnih dveh vrstic ali stolpcev matrike R je 0). Vektor v L v lokalnem koordinatnem sistemu izrazimo z vektorjem v G v globalnem koordinatnem sistemu s pomočjo rotacije v L = R L v G G Rotacijska matrika R L torej transformira vektor iz globalnega G v lokalni L G koordinatni sistem. Osnovne transformacije rotacije dobimo z vrtenjem okoli osi x, y in z z osnovnimi rotacijskimi matrikami   1 0 0 R   x( ϕ) = 0 cos( ϕ) sin( ϕ) (5.1)   0 − sin( ϕ) cos( ϕ)   cos( θ) 0 − sin( θ) R   y ( θ) = 0 1 0 (5.2)   sin( θ) 0 cos( θ)   cos( ψ) sin( ψ) 0 R   z ( ψ) = − sin( ψ) cos( ψ) 0 (5.3)   0 0 1 kjer R os( kot) označuje rotacijo okoli osi za dani kot. Zaporedne rotacije opišemo s produktom rotacijskih matrik, kjer je pomemben vrstni red rotacij. Z rotacijsko matriko opišemo orientacijo togega telesa, vendar so v nekaterih primerih primernejše druge predstavitve, zato bomo v nadaljevanju opisali še dve dodatni obliki: Eulerjeve kote in kvaternione. Eulerjevi koti Eulerjevi koti opisujejo orientacijo togega telesa z vrtenjem okoli osi x, y in z. Ti koti so označeni kot: • ϕ – nagib ali kot valjanja (angl. roll), okoli osi x, • θ – naklon ali kot prevračanja (angl. pitch), okoli osi y, • ψ – zasuk ali kot sukanja (angl. yaw ali heading), okoli osi z. 204 Senzorji v mobilnih sistemih Možnih je 12 različnih kombinacij treh osnovnih rotacij okoli osi x, y in z [1]. Najpogosteje se uporablja kombinacija 3-2-1, kjer dobimo orientacijo lokalnega koordinatnega sistema glede na referenčni koordinatni sistem tako, da iz začetne lege, kjer sta oba koordinatna sistema poravnana, izvedemo rotacije lokalnega koordinatnega sistema v naslednjem zaporedju: 1. rotacija okoli osi z za kot sukanja ψ, 2. rotacija okoli na novo pridobljene osi y za kot prevračanja θ, 3. rotacija okoli na novo pridobljene osi x za kot valjanja ϕ. Rotacijska matrika te transformacije je R = R x( ϕ)R y( θ)R z( ψ) =   cos θ cos ψ cos θ sin ψ − sin θ = sin ϕ sin θ cos ψ − cos ϕ sin ψ sin ϕ sin θ sin ψ + cos ϕ cos ψ sin ϕ cos θ   cos ϕ sin θ cos ψ + sin ϕ sin ψ cos ϕ sin θ sin ψ − sin ϕ cos ψ cos ϕ cos θ in predstavitev z Eulerjevimi koti je R 23 ϕ = arctan R 33 θ = − arcsin( R 13) (5.4) R 12 ψ = arctan R 11 Parametrizacija z uporabo Eulerjevih kotov ni redundantna (trije parametri za tri prostostne stopnje). Njena pomanjkljivost pa je singularnost pri θ = π/ 2, kjer imata rotaciji okoli osi z in x enak učinek (sovpadeta). Ta učinek izgube prostostne stopnje je v klasičnih letalskih žiroskopih znan kot problem blokade kardanskega sklopa (angl. gimbal lock). Pri zapisu rotacije z Eulerjevimi koti se ta singularnost pojavi zaradi deljenja s cos θ (glejte (5.23) v poglavju 5.2.3). Kvaternioni Kvaternioni predstavljajo orientacijo v tridimenzionalnem prostoru z uporabo štirih parametrov in ene omejitvene enačbe. So brez problema singularnosti, ki se je pojavil pri predstavitvi z Eulerjevimi koti. Matematično gledano so kvaternioni nekomutativna razširitev kompleksnih števil, ki jih zapišemo kot q = q 0 + q 1i + q 2j + q 3k kjer za kompleksne elemente i, j in k velja i2 = j2 = k2 = ijk = −1; q 0 je skalarni del kvaterniona in q 1i + q 2j + q 3k je vektorski del. Norma kvaterniona je določena z √ q kqk = qq∗ = q 2 + q 2 + q 2 + q 2 0 1 2 3 5.2. Transformacije koordinatnih sistemov 205 kjer je q∗ konjugiran kvaternion, ki ga izračunamo kot q∗ = q 0 − q 1i − q 2j − q 3k. Inverzno vrednost kvaterniona določimo s pomočjo njegove konjugirane vrednosti in norme q∗ q−1 = kqk2 Z enotskimi kvaternioni lahko parametriziramo rotacijo v prostoru q 0 = cos ∆ ϕ/ 2 q 1 = e 1 sin ∆ ϕ/ 2 (5.5) q 2 = e 2 sin ∆ ϕ/ 2 q 3 = e 3 sin ∆ ϕ/ 2 kjer je e T = [ e 1 , e 2 , e 3] enotski vektor osi vrtenja in ∆ ϕ kot zasuka okoli te osi. Za enotske kvaternione velja q 2 + q 2 + q 2 + q 2 = 1. 0 1 2 3 Transformacija q v 0 = q−1 ◦ q v ◦ q zavrti vektor v = [ x, y, z] T , podan s kvaternionom q v = x i + y j + z k (ali enakovredno q v = [0 , x, y, z] T ), okoli osi e za kot ∆ ϕ v vektor v0 = [ x 0 , y 0 , z 0] T , izražen s kvaternionom q v 0 = x 0i + y 0j + z 0k kjer ◦ označuje produkt kvaternionov, definiran v (5.6) in (5.7). Druga prednost kvaternionov je relativno enostavna kombinacija zaporednih rotacij. Kvaternion, ki ustreza produktu dveh rotacijskih matrik, je enak produktu obeh kvaternionov [1]. Imamo kvaterniona q = q 0 + q 1i + q 2j + q 3k in q0 = q 0 + q 0 i + q 0 j + q 0 k 0 1 2 3 Če nek vektor zavrtimo iz njegove začetne orientacije za zasuk q0, nato pa še za zasuk q, je celoten zasuk vektorja q00 = q0 ◦ q = ( q 0 q q q q 0 0 − q 01 1 − q 02 2 − q 03 3) + i( q 0 q q q q 1 0 + q 02 3 − q 03 2 + q 00 1) (5.6) + j(− q 0 q q q q 1 3 + q 02 0 + q 03 1 + q 00 2) + k( q 0 q q q q 1 2 − q 02 1 + q 03 0 + q 00 3) ali v vektorsko-matrični obliki  q 00  q   q 0  0 0 − q 1 − q 2 − q 3 0  q 00  q 1 q 0 q 3 − q 2  q 0   1  =    1 (5.7)  q 00  q   q 0   2   2 − q 3 q 0 q 1   2 q 00 q q 0 3 3 q 2 − q 1 q 0 3 206 Senzorji v mobilnih sistemih Povezava med kvaternionom in predstavitvijo z rotacijsko matriko je podana z   q 2 + q 2 − q 2 − q 2 2( q 0 1 2 3 0 q 3 + q 1 q 2) 2( q 1 q 3 − q 0 q 2) R(q) =  2( q  1 q 2 − q 3 q 0) q 2 − q 2 + q 2 − q 2 2( q 0 q 1 + q 2 q 3) (5.8)  0 1 2 3  2( q 0 q 2 + q 1 q 3) 2( q 2 q 3 − q 0 q 1) q 2 − q 2 − q 2 + q 2 0 1 2 3 ali v obratni smeri 1 p q 0 = 1 + R 11 + R 22 + R 33 (5.9) 2 1 q 1 = ( R 23 − R 32) 4 q 0 1 q 2 = ( R 31 − R 13) 4 q 0 1 q 3 = ( R 12 − R 21) (5.10) 4 q 0 Če je q 0 v enačbi (5.9) blizu nič, je pretvorba iz rotacijske matrike v kvaternion (enačbe (5.9) – (5.10)) singularna. V tem primeru lahko izračunamo kvaternion z uporabo ekvivalentne oblike (enačbe (5.11) – (5.12)) brez numeričnih problemov 1 q 0 = ( R 32 − R 23) (5.11) 4 T q 1 = T 1 q 2 = ( R 12 + R 21) 4 T 1 q 3 = ( R 13 + R 31) (5.12) 4 T √ kjer je T = 1 1 + R 2 11 − R 22 − R 33. Povezavo med kvaternioni in Eulerjevimi koti (notacija 3-2-1 ) dobimo z matrikami R x( ϕ), R y( θ) in R z( ψ), kar ustreza kvaternionom [cos( ϕ/ 2) + i sin( ϕ/ 2)], [cos( θ/ 2) + j sin( θ/ 2)] in [cos( ψ/ 2) + k sin( ψ/ 2)]. Kvaternion za rotacijo 3-2-1 je q = [cos( ψ/ 2) + k sin( ψ/ 2)][cos( θ/ 2) + j sin( θ/ 2)][cos( ϕ/ 2) + i sin( ϕ/ 2)] ali v vektorski obliki cos( ϕ/ 2) cos( θ/ 2) cos( ψ/ 2) + sin( ϕ/ 2) sin( θ/ 2) sin( ψ/ 2) sin( ϕ/ 2) cos( θ/ 2) cos( ψ/ 2) − cos( ϕ/ 2) sin( θ/ 2) sin( ψ/ 2) q =   cos( ϕ/ 2) sin( θ/ 2) cos( ψ/ 2) + sin( ϕ/ 2) cos( θ/ 2) sin( ψ/ 2)   cos( ϕ/ 2) cos( θ/ 2) sin( ψ/ 2) − sin( ϕ/ 2) sin( θ/ 2) cos( ψ/ 2) Obratna transformacija se glasi 2( q 1 q 0 + q 2 q 3) ϕ = arctan q 2 − q 2 − q 2 + q 2 0 1 2 3 θ = − arcsin (2( q 1 q 3 − q 2 q 0)) 2( q 3 q 0 + q 1 q 2) ψ = arctan q 2 + q 2 − q 2 − q 2 0 1 2 3 5.2. Transformacije koordinatnih sistemov 207 Primer 5.1 Koordinatni sistem senzorja (magnetometer) je glede na koordinatni sistem robota zasukan. Naj bo koordinatni sistem senzorja lokalen (L), koordinatni sistem robota pa globalen (G). Orientacija senzorja glede na robota je opisana z dvema rotacijama. Na začetku sta oba koordinatna sistema poravnana, nato (L) zavrtimo okoli osi x za kot αx = 90° in nato še enkrat okoli nove osi y za kot αy = 45°. 1. Kakšna je orientacija senzorja, izražena z rotacijsko matriko R L , glede G na koordinatni sistem robota? Določite Eulerjeve kote (notacija 3-2-1 ) in kvaternion q L, ki opisujejo to transformacijo. G 2. Magnetometer meri smerni vektor v = [0 , 0 , 1] T magnetnega polja. Kakšna je predstavitev tega vektorja v koordinatah robota? Rešitev 1. Končno orientacijo opisuje skupna rotacijska matrika, kjer je pomemben vrstni red množenja R L = R G y ( αy )R x( αx) =     cos( αy) 0 − sin( αy) 1 0 0 =  0 1 0  0 cos( α  x) sin( αx)     sin( αy) 0 cos( αy) 0 − sin( αx) cos( αx) (5.13)   0 , 7071 0 , 7071 0 =  0 0 1   0 , 7071 −0 , 7071 0 Vrstice v matriki R L predstavljajo komponente novih osi senzorja, izražene G v koordinatnem sistemu robota, kar je prikazano grafično na sliki 5.1. y Rx Ry x y z y x x z z Slika 5.1: Prikaz rotacij v enačbi (5.13) 208 Senzorji v mobilnih sistemih 2. Eulerjeve kote za notacijo 3-2-1 izračunamo iz matrike R = R L G R 23 ϕ = arctan( ) = 90° R 33 θ = − arcsin( R 13) = 0° (5.14) R 12 ψ = arctan( ) = 45° R 11 kjer dobimo rotacijsko matriko z R L = R G x( ϕ)R y ( θ)R z ( ψ), katere osnovne rotacije R x( ϕ), R y( θ) in R z( ψ) so določene v enačbi (5.3) — glejte sliko 5.2. y R R z y R y x y x x x y z x z z Slika 5.2: Prikaz rotacij z Eulerjevimi koti (5.14) 3. Kvaternion q L dobimo s produktom kvaternionov zaporednih rotacij G q L = q G x ◦ q y kjer je q x, glede na enačbo (5.5), definiran s kotom zasuka ∆ ϕx = 90° okoli rotacijske osi e x = [1 , 0 , 0] T in q y s kotom zasuka ∆ ϕy = 45° okoli rotacijske osi e y = [0 , 1 , 0] T  cos ∆ ϕ    x/ 2 0 , 7071  e  0 , 7071 q x 1 sin ∆ ϕx/ 2     x = =  e   0   x 2 sin ∆ ϕx/ 2   ex 3 sin ∆ ϕx/ 2 0  cos ∆ ϕ    z / 2 0 , 9239  e   0  q y 1 sin ∆ ϕy / 2     y = =  e  0 , 3827  y 2 sin ∆ ϕy / 2   ey 3 sin ∆ ϕy/ 2 0 Končni kvaternion (glejte definicijo produkta kvaternionov v (5.7)) ima obliko 0 , 6533 0 , 6533 q L = q   G x ◦ q y = 0 , 2706   0 , 2706 kar ustreza kotu zasuka ∆ ϕ = 2 arccos( q 0) = 2 arccos(0 , 6533) = 98 , 41° okoli rotacijske osi e = 1∆ [ q 1 , q 2 , q 3] T = [0 , 8630 , 0 , 3574 , 0 , 3574] T . sin ϕ 2 5.2. Transformacije koordinatnih sistemov 209 4. Merjeni smerni vektor v L = [0 , 0 , 1] T zapišemo v koordinatah robota kot v G = R G v L L = [0 , 707 11 , −0 , 707 11 , 0] T −1 kjer je R G = (R L ) = (R L ) T . L G G Enako dobimo s kvaternioni, kjer je vektor po rotaciji enak −1 q v = (q G) ◦ q ◦ q G G L vL L −1 ter velja q v = [0 , v T ] T in q G = (q L) (glejte 5.2.1). Produkt kvaternio-L L L G nov je definiran v (5.7). Dobimo 0 , 6533 0  0 , 6533   0  0 , 6533 0 −0 , 6533  0 , 7071  q         v = ◦ ◦ = G 0 , 2706 0 −0 , 2706 −0 , 7071         0 , 2706 1 −0 , 2706 0 kar ustreza vektorju v G = [0 , 7071 , −0 , 7071 , 0] T (upoštevamo le vektorski del kvaterniona). Primer 5.2 Na začetku sta globalni koordinatni sistem G in lokalni koordinatni sistem L poravnana, nato pa se L zavrti okoli osi x za kot αx = 90° in nato ponovno okoli nove osi z za kot αz = 90°. 1. Kakšna je orientacija koordinatnega sistema L, izražena z rotacijsko matriko (R L ), glede na koordinatni sistem G? Določite Eulerjeve kote (notacija G 3-2-1 ) in kvaternion q L, ki opisujejo to transformacijo. G 2. Vektor v globalnih koordinatah je v G = [0 , 0 , 1] T . Kakšna je predstavitev tega vektorja v lokalnih koordinatah? Rešitev 1. Za rotacijo zapišemo rotacijsko matriko (na sliki 5.3 je še grafična predstavitev te rotacije)   0 0 1 R L = −  (5.15) G 1 0 0   0 −1 0 210 Senzorji v mobilnih sistemih y Rx Rz y y x z x x z z Slika 5.3: Prikaz rotacije (5.15) 2. Eulerjeve kote (notacija 3-2-1 ) ocenimo iz matrike R L G R 23 ϕ = arctan = nedefinirano R 33 θ = − arcsin ( R 13) = −90° R 12 ψ = arctan = nedefinirano R 11 Opazimo, da je θ = −90°, kar pomeni, da je parametrizacija z Eulerjevimi koti singularna in sta zato ϕ ter ψ nedefinirana. Z uporabo Eulerjevih kotov torej ne moremo zapisati te orientacije (rotacije). 3. Kvaternion q L je G q L = q G x ◦ q z kjer je 0 , 7071 0 , 7071 q   x =  0    0 0 , 7071  0  q   z =  0    0 , 7071 Torej je  0 , 5   0 , 5  q L = q   G x ◦ q z = −0 , 5   0 , 5 kar ustreza zasuku za kot ∆ ϕ = 2 arccos(0 , 5) = 120° okoli rotacijske osi e = 1∆ [ q 1 , q 2 , q 3] T = [0 , 5774 , −0 , 5774 , 0 , 5774] T . sin ϕ 2 5.2. Transformacije koordinatnih sistemov 211 4. Vektor v G = [0 , 0 , 1] T je v lokalnih koordinatah zapisan kot v L = R L v G G = [1 , 0 , 0] T ali s kvaternioni 0 −1 1 q   v = (q L) ◦ q ◦ q L = L G vG G 0   0 kjer je vektorski del enak v L = [1 , 0 , 0] T . 5.2.2 Translacija in rotacija Za posplošeno predstavitev bomo označili koordinatni sistem senzorja kot L (lokalne koordinate) in koordinatni sistem robota kot G (globalne koordinate). Lokacija senzorja glede na koordinatni sistem robota je opisana s translacijskim vektorjem t G = [ t . Translacija t G opisuje L x, ty , tz ] in rotacijsko matriko R L G L pozicijo izhodišča lokalnega koordinatnega sistema v globalnih koordinatah in rotacijska matrika R L opisuje orientacijo lokalnega koordinatnega sistema G glede na globalni koordinatni sistem (robota). Točko p G, podano v globalnih koordinatah, lahko opišemo z lokalnimi koordinatami z uporabo transformacije p L = R L p G G − t G L njena inverzna transformacija pa je podana z p T G = (R L )−1p = R L p G L + t G L G L + t G L Primer 5.3 Robot ima laserski pregledovalnik razdalj (LPR), ki izmeri položaj najbližje točke ovire p L = [1 , 0 , 5 , 0 , 4] T m v koordinatah senzorja. Ima tudi magnetometer, ki ob istem času izmeri vektor zemeljskega magnetnega polja v T = [22 , 1 , 42] nT. L LPR je glede na (globalni) koordinatni sistem robota premaknjen za t1 = [0 , 1 , 0 , 0 , 25] T in zasukan za 30° okoli osi z. Magnetometer pa je premaknjen za t2 = [0 , 0 , 1 , 0 , 2] in zasukan za Eulerjeve kote ϕ = 0°, θ = 10°, ψ = 20° (notacija 3-2-1 ). 1. Katere so najbližje koordinate točke ovire v koordinatnem sistemu robota? 2. Kakšen je vektor magnetnega polja v koordinatnem sistemu robota? 212 Senzorji v mobilnih sistemih Rešitev 1. Rotacijska matrika je     cos(30°) sin(30°) 0 0 , 866 0 , 5 0 R L = −  =  −  G sin(30°) cos(30°) 0 0 , 5 0 , 8660 0     0 0 1 0 0 1 Točka, izražena v koordinatah robota, pa je p G = (R L ) T p G L + t1 = [0 , 716 , 0 , 933 , 0 , 65] T 2. Pri transformaciji vektorja je pomembna le rotacija, tako da so kompo- nente magnetnega vektorja v koordinatah robota dobljene z rotacijsko transformacijo   0 , 9254 0 , 3368 −0 , 1736 R L = R −  G x(0°)R y (10°)R z (20°) = 0 , 3420 0 , 9397 0   0 , 1632 0 , 0594 0 , 9848 v G = (R L ) T v G L = [26 , 8705 , 10 , 8443 , 37 , 5417] T 5.2.3 Kinematika rotacijskih koordinatnih siste- mov V tem poglavju bomo pokazali, kako je orientacija togega telesa, ki je predsta- vljena s kvaternionom ali rotacijsko matriko, povezana s kotnimi hitrostmi okoli lokalnih osi togega telesa. Togo telo se vrti okrog svojih osi x, y in z s kotnimi hitrostmi ωx, ωy in ωz, zato se orientacija togega telesa (npr. robota ali senzorske enote) spreminja glede na referenčni koordinatni sistem. Rotacijska kinematika izražena s kvaternioni Časovno odvisnost rotacije togega telesa (podana z diferencialno enačbo) lahko izpeljemo iz definicije produkta dveh kvaternionov (5.7). Če je orientacija togega telesa q( t) v času t znana, lahko njegovo orientacijo v času t + ∆ t zapišemo kot q( t + ∆ t) = q( t) ◦ ∆q( t) (5.16) kjer ∆q( t) podaja spremembo orientacije togega telesa iz q( t) v q( t + ∆ t). Z drugimi besedami, ∆q( t) je orientacija telesa v času t + ∆ t glede na njegovo orientacijo v času t. Do končne orientacije telesa q( t + ∆ t) torej pridemo tako, da najprej zavrtimo telo za rotacijo q( t) glede na nek referenčni koordinatni sistem 5.2. Transformacije koordinatnih sistemov 213 in nato za rotacijo ∆q( t) glede na q( t). Tako s pomočjo enačbe (5.5) zapišemo ∆q( t) kot  cos ∆ ϕ/ 2   ex sin ∆ ϕ/ 2 ∆q( t) =    e   y sin ∆ ϕ/ 2 ez sin ∆ ϕ/ 2 kjer je e( t) = [ ex, ey, ez] T os rotacije, izražena v lokalnih koordinatah togega telesa v času t in ∆ ϕ je kot zasuka v časovnem intervalu ∆ t. Ob predpostavki, da sta e( t) in ∆ ϕ konstantna v časovnem intervalu ∆ t, lahko preoblikujemo produkt kvaternionov (5.16) s pomočjo (5.7) kot   0 − e  x − ey − ez  ∆ ϕ ∆ ϕ  e  q x 0 ez − ey ( t + ∆ t) = cos I + sin   q( t) (5.17)  2 2  e    y − ez 0 ex  ez ey − ex 0 kjer je I enotska matrika dimenzij 4 × 4. Za kratke intervale ∆ t lahko upoštevamo aproksimacijo ∆ ϕ ≈ p ω 2 2 2 x + ωy + ωz ∆ t, kjer je ω( t) = [ ωx, ωy, ωz] T vektor trenutnih kotnih hitrosti, ki ga lahko zapišemo tudi v obliki ω( t) = p ω 2 2 2 x + ωy + ωz e. Za majhne kote lahko (5.17) aproksimiramo z ∆ tΩ q( t + ∆ t) ≈ I + q( t) 2 kjer je  0 − ω  x − ωy − ωz  ω  Ω x 0 ωz − ωy =    ω   y − ωz 0 ωx  ωz ωy − ωx 0 Diferencialno enačbo, ki opisuje orientacijo togega telesa, dobimo z limitiranjem ∆ t proti nič dq q( t + ∆ t) − q( t) 1 = lim = Ωq (5.18) d t ∆ t→0 ∆ t 2 kjer so kotne hitrosti v Ω podane v koordinatnem sistemu togega telesa. Rotacijska kinematika izražena z rotacijsko matriko Izpeljimo še diferencialno enačbo za predstavitev orientacije togega telesa, ki jo podaja rotacijska matrika. Podobno kot v (5.16) lahko zapišemo R( t + ∆ t) = ∆R( t)R( t) (5.19) kjer je R( t) orientacija togega telesa v času t, R( t + ∆ t) orientacija togega telesa v času t + ∆ t in ∆R( t) sprememba orientacije (orientacija telesa v času t = t+∆ t) glede na orientacijo v času t. Sprememba orientacije ∆R( t) je definirana kot R t+∆ t Ω0 d t ∆R( t) = e t (5.20) 214 Senzorji v mobilnih sistemih kjer je   0 ωz − ωy Ω0 = − ω  z 0 ωx   ωy − ωx 0 in ω( t) = [ ωx, ωy, ωz] T je vektor trenutnih kotnih hitrosti telesa. Ob predpostavki, da je Ω0 konstantna matrika v časovnem intervalu ∆ t, lahko aproksimiramo izraz R t+∆ t Ω0( t) d t ≈ Ω0∆ t = B. Eksponent v (5.20) razvijemo t v Taylorjevo vrsto ∆R( t) = e B = B2 B3 = I + B + + + . . . = 2! 3! B2 σ 2B σ 2B2 σ 4B (5.21) = I + B + − − + + . . . = 2! 3! 4! 5! sin σ 1 − cos σ = I + B + B2 σ σ 2 kjer je I enotska matrika dimenzij 3 × 3 in σ = ∆ t p ω 2 2 2 x + ωy + ωz . Za majhne kote σ lahko (5.21) aproksimiramo z ∆R( t) = I + B = I + Ω0∆ t kar lahko uporabimo za izračun rotacijske matrike (5.19) v času t + ∆ t R( t + ∆ t) = (I + Ω0∆ t) R( t) Končno diferencialno enačbo dobimo z limitiranjem ∆ t proti nič dR R( t + ∆ t) − R( t) = lim = Ω0R (5.22) d t ∆ t→0 ∆ t Zavoljo popolnosti je podana tudi enakovredna parametrična predstavitev rotacije z Eulerjevimi koti (notacija 3-2-1 ) ˙ ϕ = ωx + ωy sin ϕ tan θ + ωz cos ϕ tan θ ˙ θ = ωy cos ϕ − ωz sin ϕ (5.23) ˙ ωy sin ϕ + ωz cos ϕ Ψ = cos θ kjer lahko vidimo, da postane notacija (prva in tretja enačba (5.23)) singularna pri θ = ± π/ 2. 5.2.4 Projekcijska geometrija Projekcija je preslikava prostora z N > 0 dimenzijami v prostor z M < N dimenzijami. Običajno se pri projekcijski preslikavi nekaj informacije nepovratno izgubi. Če pa je na voljo več projekcij objekta iz različnih vidnih kotov, je v 5.2. Transformacije koordinatnih sistemov 215 nekaterih primerih možno rekonstruirati opazovani objekt v N -dimenzionalnem prostoru. Dve najbolj osnovni projekciji sta: perspektivična projekcija in vzporedna projekcija (linearna preslikava z goriščem v neskončnosti). Glede na model kamere z luknjico, se 3D-točka p T = [ x C C yC zC ], podana v koor- dinatnem sistemu kamere C, preslika v 2D-točko p T = [ x P P yP ] v koordinatnem sistemu slike P kot sledi (glejte sliko 5.4) 1 p = Sp P C (5.24) zC kjer p predstavlja točko p T = [ x = P P P yP ] v homogenih koordinatah, tj. pT P [ x 3 3 P yP 1]. Matrika S ∈ R × R opisuje notranji model kamere   αxf γ cx S =  0 α  y f cy (5.25)   0 0 1 Intrinzični (notranji) parametri kamere vsebovani v S so: goriščna razdalja f ; faktorja skaliranja αx in αy v horizontalni in vertikalni smeri; optično središče na sliki ( cx, cy) in strig γ. Parametre modela kamere se običajno določi ali popravi s postopkom kalibracije kamere. Perspektivični model kamere (5.24) ne nelinearen, ker vsebuje člen z−1 (inverz razdalje do točke v smeri osi z v koordinatnem C sistemu kamere C). Čeprav je perspektivična transformacija invariantna za točke, premice in splošne stožnice (stožnice so dvodimenzionalne krivulje, ki nastanejo pri preseku stožca z ravnino) — tj. točke se preslikajo v točke, premice v premice in stožnice v stožnice —, je slika prizora nekoliko popačena predstavitev opazovanega prizora. V splošnem velja, da se koti med premicami in razmerja med razdaljami ne ohranjajo (tj. vzporedne premice se v splošnem ne preslikajo v vzporedne premice). V nekaterih posebnih postavitvah kamere je perspektivično projekcijo možno zadovoljivo aproksimirati z ustreznim linearnim modelom [2] — kar lahko poenostavi kalibracijo kamere. Model kamere (5.24) lahko zapišemo tudi v obliki p ∝ Sp P C Slika (projekcija) opazovanega objekta nastane na zaslonu za optičnim središčem kamere (na goriščni razdalji f v smeri negativne osi zC od optičnega središča) in je obrnjena za 180° ter pomanjšana. Pri vizualizaciji projekcije kamere si lahko zamislimo, da nastane ne-obrnjena slika pred optičnim središčem kamere (na pozitivni osi zC na enaki razdalji od optičnega središča kot prava slika), kot je prikazano na sliki 5.4. Primer 5.4 Notranji model kamere (5.25) ima naslednje parametre: αxf = αyf = 1000, brez striga ( γ = 0) in optično središče je na sredini slike, ki je dimenzij 1024 krat 768. Preslikajte naslednjo množico 3D-točk, ki so podane v koordinatnem sistemu 216 Senzorji v mobilnih sistemih Resnična slikovna ravninaK. s. slike P K. s. kamere Virtualna slikovna ravnina C p 3D-objekt P p P p Optično središče Center slike Optična os ( cx, cy) K. s. slike P Svetovni k. s. W Slika 5.4: Projekcija pri modelu kamere z luknjico kamere, na zaslon kamere (v koordinatni sistem slike) p T ∈ {[−1 1 4] , [1 1 5] , [0 − 1 4] , [−1 1 − 4] , [4 1 5]} C Rešitev Projekcija 3D-točk na sliko z enačbo (5.24) podana v programu 5.1. Rezultati so prikazani tudi grafično na levi strani slike 5.5. Opazimo, da četrta točka ni prikazana na sliki, ker je izven omejene slikovne ravnine (zaslona). Nahaja se za kamero, in torej ni vidna. To je posledica dejstva, da matematični model (5.24) ne upošteva omejitev vidnega polja kamere. Zatorej moramo izvesti dodatna preverjanja, da zagotovimo, da se na slikovno ravnino preslikajo le točke v vidnem polju kamere: tj. projicirane točke morajo biti znotraj meja slikovne ravnine in pred kamero. Zatorej se na slikovni ravnini v resnici pojavijo le prve tri točke (glejte desno stran slike 5.5). Program 5.1: Rešitev primera 5.4 ./src/sen/example_projection.m 1 % Notranji parametri kamere in velikost zaslona 2 alphaF = 1000; % alpha *f , v px / m 3 s = [1024; 768]; % Velikost zaslona , v px 4 c = s /2; % Opti č no sredi š č e na sliki , v px 5 S = [ alphaF , 0 , c (1); 0 , alphaF , c (2); 0 , 0 , 1]; % Model kamere 6 7 % Mno ž ica 3 D to č k v k o o r d i n a t n e m sistemu kamere 8 pC = [ -1 1 4; 1 1 5; 0 -1 4; -1 1 -4; 4 1 5]. ’; 9 10 % Projekcija to č k na slikovno ravnino 11 pP = ( S * pC )./ repmat ( pC (3 ,:) , 3 , 1) 5.2. Transformacije koordinatnih sistemov 217 pP = 262 712 512 762 1 3 1 2 634 584 134 134 584 1 1 1 1 1 0 1024 0 1024 0 0 pP,3 pP,4 pP,3 p pP,2 pP,2 P,1 pP,1 768 768 Slika 5.5: Projicirane točke na slikovno ravnino iz primera 5.4. Na levi so točke, ki so preslikane z matematičnim modelom (5.24) in na desni so točke, ki so v resnici vidne na slikovni ravnini. Jasno je, da je mogoče s preslikavo (5.24) enolično preslikati vsako točko v 3D-prostoru na slikovno ravnino, kar pa ne velja za inverzno preslikavo. Ker perspektivična preslikava povzroči izgubo informacije o globini prizora, lahko vsako točko na slikovni ravnini preslikamo le v poltrak (žarek) v 3D-prostoru, če ni na voljo nobene dodatne informacije. Prizor lahko rekonstruiramo, če lahko nekako pridobimo podatek o globini. Obstaja veliko metod, ki omogočajo 3D- rekonstrukcijo in temeljijo na uporabi globinskih kamer, strukturirane svetlobe, dodatni svetlobnih namigov, gibanja itd. Položaj točke v 3D-prostoru lahko ocenimo tudi na podlagi več (najmanj dveh) projekcij 3D-točke iz različnih pogledov. 3D-rekonstrukcija je torej možna z uporabo stereo kamere. Geometrija več pogledov Geometrija več pogledov ni pomembna le zato, ker omogoča rekonstrukcijo opazovanega prizora, temveč tudi zaradi lastnosti, ki jih lahko s pridom izkoristimo pri razvoju algoritmov strojnega vida (npr. pri iskanju parov točk med slikami in pri ocenjevanju lege kamere na podlagi pogleda kamere). Predpostavimo, da rotacijska matrika R C 1 in translacijski vektor t C 1 opisujeta relativno lego med C 2 C 2 dvema kamerama (slika 5.6). Če optični središči obeh kamera ne sovpadata (t C 1 6= 0), lahko po kratki matematični manipulaciji pridemo do izraza (5.27) C 2 218 Senzorji v mobilnih sistemih Svetovni k. s. W Epipolarna ravnina p C Epipolarna premica točke p P 1 K. s. slike P 1 K. s. F p P 2 p slike P 2 P 1 F T p P 1 p P 2 K. s. K. s. kamere C 2 kamere C 1 R C 1 , t C 1 C 2 C 2 Slika 5.6: Perspektivična geometrija pogledov dveh kamer (predpostavljamo, da sta notranja modela kamer enaka) p C 1 = R C 1p (5.26) C 2 C 2 + t C 1 C 2 S−1 p ∝ R C 1S−1 p + t C 1 P 1 C 2 P 2 C 2 [t C 1 ] ∝ [t C 1 ] S−1 p C 2 ×S−1 p ×R C 1 P 1 C 2 C 2 P 2 0 = pT S− T [t C 1 ]×R C 1 S−1 p P 1 C 2 C 2 P 2 0 = pT F p (5.27) P 1 P 2 Vektorski produkt vektorjev a T = [ a 1 a 2 a 3] in b T = [ b 1 b 2 b 3] smo zapisali kot a × b = [a]×b, kjer je [a]× antisimetrična matrika   0 − a 3 a 2 [a]× =  a  3 0 − a 1   − a 2 a 1 0 Matrika F je znana kot fundamentalna matrika in opisuje epipolarno omejitev (5.27): točka p leži na premici F p na prvi sliki in točka p leži na premici P 1 P 2 P 2 F T p na drugi sliki. Pomembna je tudi relacija p T Ep P C 1 C 1 2 = 0, kjer je ma- trika E = [t C 1 ] na področju strojnega vida znana kot esencialna matrika. C 2 ×R C 1 C 2 Povezava med esencialno in fundamentalno matriko je E = S T F S. Epipolarno omejitev lahko izkoristimo za izboljšanje iskanja parov točk med slikama istega prizora iz dveh zornih kotov, če je znana medsebojna lega med kalibriranima kamerama. Ker mora korespondenčni par točke p na prvi sliki P 1 ležati na premici F T p na drugi sliki, se iskanje para na 2D-ravnini slike skrči na P 1 iskanje vzdolž 1D-premice (epipolarne premice). Zato je možna velika pohitritev iskanja parov točk in tudi iskanje parov je lahko bolj robustno, saj lahko zavržemo pare, ki ne zadostijo epipolarni omejitvi. 5.2. Transformacije koordinatnih sistemov 219 Primer 5.5 Z dvema kamerama opazujemo množico 3D-točk v prostoru. Medsebojna lega kamer je podana z rotacijsko matriko R C 1 = R C 2 x(30◦)R y (60◦) in translacijskim vektorjem t C 1 = [4 − 1 2] T . Notranji parametri modela (5.25) obeh kamer so C 2 enaki: αxf = αyf = 1000, brez striga ( γ = 0) in optična os poteka skozi središče slike z dimenzijami 1024 krat 768. Množica točk, ki nastane na zaslonu prve kamere, je p T ∈ {[262 634] , [762 634] , [512 134] , [443 457] , [412 284]} P 1 Množica točk, ki nastane na zaslonu druge kamere, je p T ∈ {[259 409] , [397 153] , [488 513] , [730 569] , [115 214]} P 1 Točke, ki nastanejo na slikovnih ravninah obeh kamer, so prikazane na sliki 5.7. Določite vse možne korespondenčne pare točk, ki zadostijo epipolarni omejitvi. Za vse najdene korespondenčne pare točk rekonstruirajte položaj 3D-točk v prostoru glede na koordinatna sistema obeh kamer. 0 1024 0 1024 0 0 pP1,3 pP2,2 p p P2,5 P1,5 p pP P 2,1 1,4 pP2,3 p p P2,4 P p 1,1 P1,2 768 768 Slika 5.7: Točke na dveh slikovnih ravninah iz primera 5.5 Rešitev Pri iskanju parov točk med dvema slikama iz kamer, katerih medsebojna lega je znana, velja, da morajo projekcije točk, ki pripadajo isti 3D-točki, zadostiti epipolarni omejitvi (5.27). Enačba (5.27) je oblike l T p = 0, tj. homogena točka pT = [ x y 1] leži na premici l T = [ a b c], saj je ax + by + c = 0 implicitna oblika enačbe premice. S pomočjo epipolarne omejitve (5.27) lahko torej najdemo premico l P 2 na drugi sliki za vsako točko p na prvi sliki, in obratno, premico P 1 l P 1 za vsako točko p . Epipolarne premice za prvo in drugo sliko so prikazane P 2 na sliki 5.8. Glede na sliki 5.7 in 5.8 lahko grafično najdemo štiri možne pare točk. 220 Senzorji v mobilnih sistemih 0 1024 0 1024 0 0 l l P2 P ,5 1,2 lP l 2,3 l P P2 1 ,4 ,1 lP1,3 lP1,4 lP l 2 P ,1 2,2 768 768 Slika 5.8: Točke in epipolarne premice na dveh slikah iz primera 5.5 Implementacija algoritma za iskanje parov točk je podana v programu 5.2 (funkciji rotX in rotY sta Matlabovi implementaciji enačb (5.1) in (5.2)). Pari točk so zbrani v indeksni matriki pairs. Program 5.2: Iskanje parov točk iz primera 5.5 ./src/sen/example_fundamental.m 1 % Notranji parametri kamere in velikost zaslona 2 alphaF = 1000; % alpha *f , v px / m 3 s = [1024; 768]; % Velikost zaslona , v px 4 c = s /2; % Opti č no sredi š č e na sliki , v px 5 S = [ alphaF , 0 , c (1); 0 , alphaF , c (2); 0 , 0 , 1]; % Model kamere 6 7 % Lega med kamerama 8 R = rotX ( pi /6)* rotY ( pi /3); t = [4; -1; 2]; 9 10 % Mno ž ica to č k 11 pP1 = [262 , 634; 762 , 634; 512 , 134; 443 , 457; 412 , 284]. ’; 12 pP2 = [259 , 409; 397 , 153; 488 , 513; 730 , 569; 115 , 214]. ’; 13 14 N1 = size ( pP1 , 2); N2 = size ( pP2 , 2); % Š tevilo to č k 15 16 % F u n d a m e n t a l n a matrika 17 tx = [0 , -t (3) , t (2); t (3) , 0 , -t (1); -t (2) , t (1) , 0]; 18 F = S . ’\ tx * R / S ; 19 20 epsilon = 1e -2; % Toleranca napake po razdalji 21 pP1 = [ pP1 ; ones (1 , N1 )]; pP2 = [ pP2 ; ones (1 , N2 )]; % To č ke v homogenih ko ord in ata h 22 23 % Epipolarne premice v k . s . P1 , ki pripadajo to č kam v k . s . P2 24 lP1 = F * pP2 ; 25 % Epipolarne premice v k . s . P2 , ki pripadajo to č kam v k . s . P1 26 lP2 = F . ’* pP1 ; 27 28 % Iskanje parov to č k ( upo š tevanje epipolarne omejitve ) 29 pairs = []; 30 for i = 1: N1 31 d = abs ( lP2 (: , i ). ’* pP2 ); 32 k = f i n d ( d < e p s i l o n ); 33 if ~ i s e m p t y ( k ) , p a i r s = [ pairs , [ i ; k ( 1 ) ] ] ; end 34 end 35 pairs 5.2. Transformacije koordinatnih sistemov 221 p a i r s = 1 2 3 5 3 4 2 1 Singularni primeri Enačba (5.27) postane singularna, če optični središči obeh kamer sovpadata. V primeru, če je t C 1 nič, lahko iz (5.26) izpeljemo relacijo C 2 p ∝ SR C 1S−1 p (5.28) P 1 C 2 P 2 Do podobne oblike enačbe pridemo tudi v primeru, če se vse točke v 3D-prostoru nahajajo le na eni ravnini. Brez izgube na splošnosti lahko predpostavimo, da je to ravnina zW = 0 p ∝ S[r C 1 r C 1 t C 1 ][r C 2 r C 2 t C 2]−1S−1 p (5.29) P 1 W, 1 W, 2 W W, 1 W, 2 W P 2 kjer R C = [r C r C r C ]. Preslikava ravnine v svetovnih koordinatah na W W, 1 W, 2 W, 3 slikovno ravnino je p ∝ S[r C r C t C ] p (5.30) P W, 1 W, 2 W W kjer v tem primeru velja pT = [ x W W yW 1]. Enačbe (5.28), (5.29) in (5.30) imajo vse podobno obliko: p 0 ∝ H p. Matrika H je na področju strojnega vida znana kot homografija. 3D-rekonstrukcija V primeru stereo kamere je položaj 3D-točke možno določiti na podlagi obeh projekcij (slik) točke. Postopek zahteva določitev korespondenčnega para točk na slikah, ki ustrezata 3D-točki v prostoru, kar je eden izmed fundamentalnih problemov na področju strojnega vida. Ko je korespondenčni par točk najden in če je znana lege med obema kamerama (translacija med kamerama ne sme biti nič) ter če sta znana še notranja modela obeh kamer, lahko določimo položaj točke v 3D-prostoru. Če sta oba notranja modela kamer enaka, lahko ocenimo globino točke v koordinatnih sistemih obeh kamer ( zC 1 in zC 2), če rešimo naslednji sistem enačb (npr. z uporabo metode najmanjših kvadratov) S−1 p z − R C 1 S−1 p z (5.31) P C C 1 1 C 2 P 2 2 = t C 1 C 2 Primer 5.6 Za vse najdene korespondenčne pare točk na slikah iz primera 5.5 določite položaje točk v 3D-prostoru glede na koordinatni sistem prve in druge kamere. 222 Senzorji v mobilnih sistemih Rešitev Položaje točk v 3D-prostoru lahko določimo, če rešimo sistem enačb (5.31) za vsak korespondenčni par točk na slikah iz primera 5.5. Rešitev vstavimo v inverzno transformacijo enačbe (5.24). Implementacija rešitve v okolju Matlab je podana v programu 5.3. Položaji točk v 3D-prostoru glede na koordinatni sistem prve in druge kamere so prirejeni spremenljivkam pC1 in pC2. Program 5.3: Rekonstrukcija točk v 3D-prostoru iz primera 5.6 ./src/sen/example_reconstruct.m 1 % R e k o n s t r u k c i j a 2 M = size ( pairs , 2); 3 pC1 = zeros (3 , M ); 4 for i = 1: M 5 a = p a i r s (1 , i ); b = p a i r s (2 , i ); 6 c1 = S \ pP1 (: , a ); 7 c2 = - R *( S \ pP2 (: , b )); 8 psi = [ c1 , c2 ]\ t ; 9 pC1 (: , i ) = psi ( 1 ) * c1 ; 10 end 11 pC2 = R . ’*( pC1 - repmat (t , 1 , M )); 12 pC1 , pC2 pC1 = - 0. 9 98 9 0 . 9 9 9 4 0 - 0. 2 99 9 0 . 9 9 8 9 0 . 9 9 9 4 - 1. 00 0 5 - 0 .2 9 99 3 . 9 9 5 6 3 . 9 9 7 8 4 . 0 0 1 8 2 . 9 9 9 4 pC2 = - 0. 13 7 2 0 . 8 6 3 8 -0 . 49 8 8 - 1. 0 97 3 0 . 7 3 3 3 0 . 7 3 2 7 - 1. 0 01 3 0 . 1 0 6 6 5 . 6 9 3 0 3 . 9 6 3 5 4 . 3 3 0 8 4 . 3 3 1 6 Problem rekonstrukcije se poenostavi v primeru kanonične postavitve stereo kamere, kjer je prva kamera glede na drugo le premaknjena vzdolž osi x za razdaljo b (kot je prikazano na sliki 5.9). V tem primeru postanejo epipolarne premice vzporedne in epipolarna premice točke p P 1 gre tudi skozi to točko (ne le točko p P 2 na drugi sliki). V primeru digitalnih slik to pomeni, da se par točke na prvi sliki nahaja na drugi sliki v isti vrstici kot na prvi sliki. Predpostavimo, da so parametri modelov (5.25) obeh kamer enaki: αx = αy = α in γ = 0. Položaj točke v 3D-prostoru glede na koordinatni sistem prve kamere lahko dobimo iz obeh njenih projekcij (slik) b h i p T = − − (5.32) C x c c 1 d P 1 x yP 1 y αf kjer smo uvedli dispariteto d = xP − 1 xP 2. Dispariteta vsebuje informacijo o globini, kot je razvidno iz zadnjega elementa v vektorju (5.32): zC 1 = αfbd−1. Za vse točke, ki se nahajajo pred kamero, je dispariteta pozitivna: d ≥ 0. 5.2. Transformacije koordinatnih sistemov 223 Epipolarna premica Koordinatni sisrem slike P 1 Koordinatni sistem slike P 2 p P 1 p P 2 b Center slike Slika 5.9: Kanonična postavitev stereo kamere Primer 5.7 Predpostavimo kanonično postavitev stereo kamere z razdaljo med kamerama b = 0 , 2 in naslednjimi notranjimi parametri obeh kamer: αxf = αyf = 1000, cx = 512 in cy = 384. Določite položaj 3D-točke, ki se projicira v točko p T = [351 522] na prvi sliki in v točko p T = [236 522] na drugi sliki. P 1 P 2 Rešitev Ker imamo opravka s kanonično postavitvijo stereo kamere, lahko rešitev dobimo direktno z uporabo (5.32) (glejte program 5.4). Program 5.4: Rekonstrukcija 3D-točke iz primera 5.7 ./src/sen/example_reconstruct0.m 1 % Notranji parametri kamere in velikost zaslona 2 alphaF = 1000; % alpha *f , v px / m 3 c = [512; 384]; % Opti č no sredi š č e na sliki , v px 4 S = [ alphaF , 0 , c (1); 0 , alphaF , c (2); 0 , 0 , 1]; % Model kamere 5 6 b = 0.2; % Razdalja med kamerama , v m 7 8 % Pari to č k 9 pP1 = [351; 522]; 10 pP2 = [236; 522]; 11 12 % 3D - r e k o n s t r u k c i j a 13 d = pP1 (1) - pP2 (1); 14 pC1 = b / d *[ pP1 - c ; alphaF ] pC1 = - 0. 28 0 0 0 . 2 4 0 0 1 . 7 3 9 1 224 Senzorji v mobilnih sistemih 5.3 Metode merjenja lege V nadaljevanju bomo predstavili glavne metodologije uporabe senzorjev za oce- njevanje lege robota v okolju. Ti pristopi lahko merijo relativno spremembo lege glede na predhodno določeno lego ali pa absolutno lego glede na nek referenčen koordinatni sistem. 5.3.1 Relativno določanje lege Relativno določanje lege (angl. dead reckoning) ocenjuje trenutno lego robota s pomočjo prejšnje znane lege in izmerjenih relativnih premikov iz prejšnje lege. Ti premiki ali inkrementi gibanja (razdalja in orientacija) se izračunajo iz izmerjenih hitrosti v pretečenem času in smeri gibanja. Za te pristope je skupna uporaba integracije poti za oceno trenutne lege, zato se običajno pojavijo različni pogreški (pogrešek integracijske metode, merilni pogrešek, pristranskost, šum itd.). V mobilni robotiki sta najpogosteje uporabljena pristopa odometrija in inercialni navigacijski sistem. Odometrija Odometrija ocenjuje lego robota s pomočjo integracije premikov, ki jih lahko izmerimo ali pridobimo iz uporabljenih regulirnih veličin za gibanje. V mobilni robotiki običajno pridobimo relativne premike iz osnih senzorjev (npr. inkrementalni enkoder), ki so pritrjeni na osi koles robota. Z uporabo notranjega kinematičnega modela (glejte (2.1) za diferencialni pogon) so meritve zasuka koles povezane s spremembami pozicije in orientacije mobilnega robota. Spremembe pozicije in orientacije v določenem časovnem intervalu med zaporednima meritvama lahko izrazimo s hitrostmi robota. V nekaterih primerih se lahko kotne hitrosti koles izmerijo neposredno ali pa jih izrazimo iz znanih reguliranih hitrosti (predpostavimo, da so hitrostni regulatorji točni). Vzemimo robota z diferencialnim pogonom, ki ima na kolesih nameščena inkre- mentalna enkoderja. Senzorja merita relativno spremembo zasuka levega ∆ αL( t) in desnega kolesa ∆ αR( t) glede na (prejšnjo) orientacijo v času t− = t − ∆ t. Če predpostavimo idealno vrtenje koles (brez zdrsov koles ipd.), je njuna prevožena razdalja ∆ dL( t) = ∆ αL( t) R ∆ dR( t) = ∆ αR( t) R kjer je R polmer koles. Z uporabo notranjega kinematičnega modela (2.1) sta sprememba orientacije in prevožena razdalja (premik) ∆ dR( t) − ∆ dL( t) ∆ ϕ( t) = L ∆ dR( t) + ∆ dL( t) ∆ d( t) = 2 5.3. Metode merjenja lege 225 kjer je L razdalja med kolesoma. Lego robota lahko ocenimo iz njegovih izmerjenih hitrosti s pomočjo integracije zunanjega kinematičnega modela (2.2) ali iz vsote izračunane pozicije robota in sprememb orientacije. Z uporabo trapezne metode integracije dobimo ocenjeno lego robota ∆ ϕ( t) x( t) = x( t−) + ∆ d( t) cos ϕ( t−) + 2 ∆ ϕ( t) y( t) = y( t−) + ∆ d( t) sin ϕ( t−) + 2 ϕ( t) = ϕ( t−) + ∆ ϕ( t) Vendar pa zaradi integralne narave odometrije pride do kumulativnega pogreška (lezenja), ki ga v glavnem delimo na sistematične in nedeterministične pogreške. Sistematični pogreški se pojavijo zaradi kinematičnih nepravilnosti (npr. napačen podatek za polmer koles) ter (ne)točnosti uporabljene integracijske metode in meritve (neznana pristranskost). Nedeterministični pogreški pa so posledica zdrsa koles, šuma meritve ipd. Zato je odometrija samostojno uporabna le za kratkoročno ocenjevanje pri znani začetni legi. Pogosteje se uporablja v kombinaciji z meritvami absolutnih senzorjev za napovedovanje in filtriranje absolutnih meritev lege. Tako dobimo boljše ocene lege. V kolesnih mobilnih sistemih za odometrijo pogosto uporabljamo senzorje, ki jih pritrdimo na os kolesa (npr. inkrementalni optični dajalniki, potenciometri) in merijo kot zasuka ali kotno hitrost. Inercialna navigacija Inercialni navigacijski sistem (INS) je samostojna tehnika za oceno lege, orientacije in hitrosti vozila s pomočjo relativnega merjenja položaja. INS vključuje senzorje gibanja (pospeškometer) in merilnike zasuka (žiroskop), kjer sta pozicija in orientacija vozila ocenjeni glede na znano začetno lego. Meritev pospeškometra in žiroskopa predstavljata tridimenzionalna vektorja pospeškov in kotnih hitrosti v prostoru. Za oceno lege in orientacije robota je potrebna dvojna integracija meritve pospeška in enojna integracija meritve kotne hitrosti. Uporaba integracije je glavni vzrok za položajni pogrešek v INS, saj se ob integraciji akumulirajo konstantni (sistematični) pogreški (lezenje senzorja, slaba kalibracija itd.). Zaradi stalnega pogreška napaka ocenjevanja pozicije narašča kvadratično s časom, napaka ocene orientacije pa narašča linearno s časom. Poleg tega je napaka ocenjevanja pozicije odvisna od napake ocenjevanja orientacije, ker pospeškometer meri celotni pospešek, torej tudi gravitacijo. Da lahko ocenimo pospešek vozila, moramo od meritve odšteti gravitacijski pospešek, kar zahteva natančno poznavanje orientacije vozila. Vsak pogrešek orientacije (nagnjena podlaga) povzroči napačni navidezni pogrešek zaradi gravitacije, kar je še posebej problematično, ker so pospeški vozila običajno veliko manjši od 226 Senzorji v mobilnih sistemih gravitacije. Pri majhnih pospeških povzroča dodatne težave relativno velik šum (majhna vrednost razmerja signal-šum). Ti vplivi so jasno razvidni iz modela meritve pospeškometra, ki je sestavljen iz translacijskega pospeška vozila a, gravitacije g = [0 , 0 , 0 , 981] T in radialnega pospeška a T m = R w (a + R w g + ω × v) + a i e bias + a noise (5.33) kjer je a m izmerjeni pospešek v lokalnih koordinatah senzorja, R w rotacijska e matrika med koordinatnim sistemom Zemlje (ECEF) in globalnim koordinatnim sistemom, v katerem sledimo legi, R w rotacijska matrika iz lokalnega koordi-i natnega sistema INS v globalni koordinatni sistem, ω kotna hitrost v globalnih koordinatah, v translacijska hitrost, a bias pristranskost pospeška in a noise šum. Orientacijo ocenimo s pomočjo meritev žiroskopa. Model meritve žiroskopa je ω m = ω i + ω bias + ω noise (5.34) kjer je ω m vektor izmerjene kotne hitrosti, ω i pravi vektor kotnih hitrosti telesa v lokalnih koordinatah, ω bias je pristranskost senzorja in ω noise šum senzorja. Oceno orientacije INS-enote dobimo s pomočjo ocenjenih enotskih kotnih hitrosti iz (5.34) kot ω i = ω m − ω bias. Pristranski del se ponavadi oceni sproti s pomočjo nekega ocenjevalnika (npr. Kalmanov filter) ali pa predpostavimo, da je konstanten za kratke ocenjevalne intervale, kakovostni žiroskop in začetno kalibracijo (ocena hitrosti ω bias). Vendar se pristranskost spreminja s časom, zato je slednji pristop primeren samo za kratkoročno uporabo. Najenostavnejšo kalibracijo izvedemo s povprečenjem N meritev žiroskopa, medtem ko držimo INS-enoto v konstantnem položaju (ω P N bias = 1 ω N i=1 m, ko ω i = 0). S pomočjo rotacijske kinematike za kvaternione (5.18) dobimo relativno oceno orientacije enote INS glede na začetno orientacijo kot dq i ( t) 1 w = Ω( t)q i ( t) d t 2 w t Z dq i ( t) q i ( t) = w d t + q i (0) (5.35) w d t w 0 kjer je q i ( t) kvaternion, ki opisuje rotacijo iz globalnega koordinatnega sistema w w v koordinanti sistem INS-enote i, q i (0) začetna orientacija w  0 − ω  x − ωy − ωz  ω  Ω x 0 ωz − ωy ( t) =    ω   y − ωz 0 ωx  ωz ωy − ωx 0 in ω T i( t) = [ ωx, ωy , ωz ] T . Rotacijsko matriko R i = R w pridobimo iz razmerja w i (5.8). V (5.35) lahko uporabimo numerično integracijo (5.17). Da lahko ocenimo lego INS-enote, izračunamo translacijski pospešek v globalnih koordinatah iz (5.33) a = R w(a g − ω × v i m − a bias) − R w e 5.3. Metode merjenja lege 227 kjer je ω = R wω i i vektor kotne hitrosti v globalnih koordinatah, izraz za pristran- skost a bias pa se ocenjuje sproti s pomočjo nekega ocenjevalnika (npr. Kalmanov filter). Potrebna je tudi ustrezna kalibracija pospeškometra. Oceno hitrosti v( t) in pozicije x( t) dobimo z integracijo t Z v( t) = a( t) d t + v(0) 0 t Z x( t) = v( t) d t + x(0) 0 Primer 5.8 Izvedite simulacijo izmerjenega pospeška in kotnih hitrosti za robota z diferencialnim pogonom in INS-enoto, ki se vozi po krivulji x( t) = cos( t), y( t) = sin(2 t) in z( t) = 0 (v globalnem koordinatnem sistemu). Čas simulacije je 6 s, računski korak pa 1 ms. INS-enota je usmerjena tako, da je njena x-os tangenta na trajektorijo, y-os je pravokotna na x-os in z-os je poravnana z z-osjo globalnega koordinatnega sistema. Iz meritev ocenite pozicijo in orientacijo INS-enote v globalnem koordinatnem sistemu. Poleg tega upoštevajte pristranskost senzorjev in šum ter opazujte, kako vplivata na ocenjeno lego INS-enote. Rešitev V simulaciji dobimo kotne hitrosti INS-enote iz razmerja (5.22) in jih uporabimo za matriko kotnih hitrosti Ω0 = dR Ω0R T . Simulacije meritev pospeškometra in d t žiroskopa so modelirane s pomočjo (5.33) in (5.34) ter prikazane na sliki 5.10. 228 Senzorji v mobilnih sistemih 10 10 2 ]s 2 ] / − [m 0 [s 0 a x ω x −100 2 4 6 −100 2 4 6 t [s] t [s] 10 10 2 ]s 2 ] / − [m 0 [s 0 a y ω y −100 2 4 6 −100 2 4 6 t [s] t [s] 20 10 2 ]s 2 ] / 10 − 0 [m [s a z ω z 00 2 4 6 −100 2 4 6 t [s] t [s] Slika 5.10: Simulacija meritev pospeška (levi stolpec) in kotne hitrosti (desni stolpec) INS-enote Ocenjena pozicija in orientacije INS-enote v idealnem primeru brez šuma in pristranskosti sta prikazani na sliki 5.11. 5.3. Metode merjenja lege 229 3 2 z [m] 1 0 1 1 0 0 −1 −1 y [m] x −2 [m] −2 (a) 1 1 q 0 0 q 1 0 −10 2 4 6 −10 2 4 6 t [s] t [s] 1 1 q 3 0 q 4 0 −10 2 4 6 −10 2 4 6 t [s] t [s] (b) Slika 5.11: Ocenjena pozicija (a) in orientacija (b) INS-enote iz meritev pospe- škometra in žiroskopa v idealnih razmerah, brez šuma in pristranskosti (prava vrednost je označena s polno krivuljo, ocena pa s črtkano krivuljo). Na koncu simulacije lahko opazimo manjši pozicijski pogrešek zaradi numerične integracije. Ocenjena lega v primeru šumnih meritev in vključene pristranskosti pa je prika- zana na sliki 5.12, kjer je mogoče opaziti hitro rast pozicijskega pogreška. 230 Senzorji v mobilnih sistemih 3 2 z [m] 1 0 1 1 0 0 −1 −1 y [m] x −2 [m] −2 (a) 1 1 q 0 0 q 1 0 −10 2 4 6 −10 2 4 6 t [s] t [s] 1 1 q 3 0 q 4 0 −10 2 4 6 −10 2 4 6 t [s] t [s] (b) Slika 5.12: Ocenjena pozicija (a) in orientacija (b) INS-enote iz meritev pospe- škometra in žiroskopa z upoštevanim šumom in pristranskostjo senzorja (prava vrednost je označena s polno krivuljo, ocena pa s črtkano krivuljo). Ker pri- stranskost ni kompenzirana, se pojavi velik pogrešek ocene pozicije in majhen pogrešek ocene orientacije. V programu 5.5 je podana koda za simulacijo INS-enote in oceno njene lege (funkcije rotX, rotY in rotZ so Matlabove izvedbe funkcij (5.1), (5.2) in (5.3)). 5.3. Metode merjenja lege 231 Program 5.5 ./src/sen/example_inertial_sensors_navigation.m 1 biasA = [1; 1; 1]*0.02; biasW = [1; 1; 1]*0.04; % Sistemati č na napaka senzorja 2 SigmaA = 0.1; SigmaW = 0.05; % Š um senzorja 3 4 nSteps = 6000; dT = 0.001; t = 0; % Š tevilo vzorcev in velikost koraka 5 6 for k = 1: nSteps 7 % S i m u l a c i j a g i b a n j a s e n z o r j a : p r a v a l e g a in p r a v i p o s p e š ki 8 x = [ cos ( t ); sin (2* t ); 0]; % L e g a 9 v = [ - sin ( t ); 2* cos (2* t ); 0]; % H i t r o s t 10 a = [ - cos ( t ); -4* sin (2* t ); 0]; % P o s p e š ek 11 12 fi = [0; 0; a t a n 2 ( v (2) , v ( 1 ) ) ] ; % E u l e r j e v i k o t i g l e d e na s v e t o v n i k . s . 13 dfi = [0; 0; ( v ( 1 ) * a (2) - v ( 2 ) * a ( 1 ) ) / ( v ( 1 ) ^ 2 + v ( 2 ) ^ 2 ) ] ; % O d v o d 14 Rx = r o t X ( fi ( 1 ) ) ; Ry = r o t Y ( fi ( 2 ) ) ; Rz = r o t Z ( fi ( 3 ) ) ; 15 dRx = [0 , 0 , 0; 0 , - sin ( fi (1)) , cos ( fi ( 1 ) ) ; 0 , - cos ( fi (1)) , - sin ( fi ( 1 ) ) ] ; 16 dRy = [ - sin ( fi (2)) , 0 , - cos ( fi ( 2 ) ) ; 0 , 0 , 0; cos ( fi (2)) , 0 , - sin ( fi ( 2 ) ) ] ; 17 dRz = [ - sin ( fi (3)) , cos ( fi (3)) , 0; - cos ( fi (3)) , - sin ( fi (3)) , 0; 0 , 0 , 0]; 18 R = Rx * Ry * Rz ; 19 dR = dRx * Ry * Rz * dfi (1) + Rx * dRy * Rz * dfi (2) + Rx * Ry * dRz * dfi ( 3 ) ; 20 q = d c m 2 q u a t ( R ). ’; % K v a t e r n i o n med s v e t o v n i m k . s . in s e n z o r j e m 21 22 % M e r i t v e ž i r o s k o p a 23 O m e g a = dR * R . ’; % Z a p i s o d v o d a k o t n e h i t o r s t i v m a t r i č ni o b l i k i 24 wb = -[ O m e g a (3 ,2); O m e g a (1 ,3); - O m e g a (1 ,2 )] ; % K o t n e h i t r o s t i 25 26 % M e r i t v e p o s p e š k o m e t r a 27 a g D y n = a ; % D i n a m i č ni p o s p e š ek v s v e t o v n e m k . s . 28 a g G r a v = [0; 0; 9 . 8 1 ] ; % G r a v i t a c i j a 29 R e a r t h = eye ( 3 ) ; % G l o b a l n i k . s . s o v p a d a s k . s . Z e m l j e 30 wg = R . ’* wb ; % P r a v e k o t n e h i t r o s t i v s v e t o v n e m k . s . 31 w g S k e w = [0 - wg (3) wg ( 2 ) ; wg (3) 0 - wg ( 1 ) ; - wg (2) wg (1) 0]; 32 vg = v ; % H i t r o s t i v s v e t o v n e m k . s . 33 34 % I z m e r j e n e k o t n e h i t r o s t i in p o s p e š ki 35 w b M e a = wb + b i a s W + r a n d n (3 ,1)* S i g m a W ; 36 a b M e a = R *( a g D y n + R e a r t h * a g G r a v + w g S k e w * vg ) + b i a s A + r a n d n (3 ,1)* S i g m a A ; 37 38 % I n e r c i a l n a n a v i g a c i j a 39 if k ==1 % I n i c i a l i z a c i j a 40 q E s t = q ; x E s t = x ; v E s t = v ; % I n i c i a l i z a c i j a za č e t n i h v r e d n o s t i 41 e l s e % P o s o d o b i t e v 42 % Ž i r o s k o p 43 wx = w b M e a ( 1 ) ; wy = w b M e a ( 2 ) ; wz = w b M e a ( 3 ) ; 44 O M E G A = [ 0 - wx - wy - wz ; ... 45 wx 0 wz - wy ; ... 46 wy - wz 0 wx ; ... 47 wz wy - wx 0]; 48 d Q e s t = 0 . 5 * O M E G A * q E s t ; 49 q E s t = q E s t + d Q e s t * dT ; % I n t e g r a c i j a k v a t e r n i o n o v 50 q E s t = q E s t / n o r m ( q E s t ); % N o r m i r a n j e k v a t e r n i o n o v 51 % P o s p e š ek 52 a g G r a v = [ 0; 0; 9 . 8 1 ] ; % G r a v i t a c i j a 53 R_ = q u a t 2 d c m ( q E s t . ’); % L e g a s e n z o r j a g l e d e na s v e t o v n i k . s . 54 R e a r t h = eye ( 3 ) ; % S v e t o v n i k . s . s o v p a d a s k . s . Z e m l j e 55 wg_ = R_ . ’*[ wx ; wy ; wz ]; % K o t n e h i t r o s t i v s v e t o v n e m k . s . 56 w g S k e w _ = [0 - wg_ (3) wg_ ( 2 ) ; wg_ (3) 0 - wg_ ( 1 ) ; - wg_ (2) wg_ (1) 0]; 57 A e s t = R_ . ’* a b M e a - R e a r t h * a g G r a v - w g S k e w _ * v E s t ; % O c e n a p o s p e š ka 58 v E s t = v E s t + A e s t * dT ; % O c e n a h i t r o s t i 59 x E s t = x E s t + v E s t * dT ; % O c e n a l e g e 60 end 232 Senzorji v mobilnih sistemih 61 t = t + dT ; 62 end 5.3.2 Merjenje smeri gibanja Sistemi za merjenje smeri podajajo informacijo o orientaciji vozila v prostoru, oz. v katero smer je vozilo usmerjeno. Za oceno smeri običajno uporabimo več senzorjev, kot so magnetometer, žiroskop in pospeškometer. Njihove informacije so vgrajene v senzorske sisteme za oceno smeri (npr. žiroskop, kompas ali inklinometer). Magnetometer in pospeškometer zagotavljata absolutne meritve tridimenzionalnih smernih vektorjev zemeljskega magnetnega polja (jakost in smer) ter smernega vektorja gravitacije Zemlje (če senzorska enota ne pospešuje). Za zmanjšanje šuma meritve in izboljšanje točnosti, so v ocenjevalne filtre vključene tudi relativne meritve žiroskopa. Za oceno orientacije senzorske enote glede na nek referenčni koordinatni sistem (npr. fiksni zemeljski koordinatni sistem) sta potrebna vsaj dva smerna vektorja. Senzorsko enoto lahko sestavljata magnetometer in pospeškometer. Merilni model magnetometra zapišemo kot b m = R i R w b w e true + b bias + b noise kjer je b m izmerjeno magnetno polje v koordinatnem sistemu senzorja, b true magnetno polje Zemlje v zemeljskem koordinatnem sistemu za neko mesto na Zemlji, rotacijski matriki pa sta definirani enako kot v (5.33). Magnetno polje b true lahko aproksimiramo kot konstanto za nekaj majhnih površin na Zemlji (npr. 100 km2). Model pospeškometra je podan v (5.33). V primeru enakomernega gibanja kaže smerni vektor meritve v smeri gravitacije in z-osi fiksnega zemeljskega koordinatnega sistema. Orientacija senzorske enote glede na fiksni zemeljski koordinatni sistem je opisana z rotacijsko matriko R i = R wT R w, ki jo dobimo z zapisom matrike vektorjev osi e i e zemeljskega koordinatnega sistema, izraženih v lokalnem koordinatnem sistemu. V primeru mirovanja pospeškometer izmeri smerni vektor, ki kaže od središča Zemlje proti poziciji INS. Ta smer torej določa z-os zemeljskega koordinatnega sistema in je izražena v lokalnem (senzorskem) koordinatnem sistemu kot a z m d = ka m k Smer Zemljine x-osi, izražena v lokalnih koordinatah, je določena s komponento vektorja magnetometra, ki je pravokotna na z d b x m × z d d = kb m × z d k 5.3. Metode merjenja lege 233 kjer × označuje vektorski produkt. Smer proti severu določa y-os Zemlje, izraženo v lokalnem koordinatnem sistemu kot y d = z d × x d Dobljena rotacijska matrika je R i = [x e d, y d, z d] kjer je rotacijska matrika med globalnim in zemeljskimi koordinatnim sistemom T enaka R i = R wT = R i R wT . To orientacijo lahko opišemo tudi s kvaternioni w i e e s pomočjo enačb (5.9) – (5.10) ali z Eulerjevimi koti s pomočjo relacije (5.4). Točna ocena orientacije vozila je pomembna tudi pri izvedbi odometrije ali iner-cialne navigacije za zmanjšanje pogreška orientacije in posledično tudi pogreška ocene pozicije. Zato v absolutnih meritvah smeri gibanja pogosto uporabimo relativne meritve v koraku korekcije ocenjevalnikov (npr. Kalmanovega filtra). 5.3.3 Aktivne značke in globalne meritve pozi- cije Lokalizacija v okolici je možna tudi z opazovanjem značk, ki se nahajajo na znanih pozicijah v okolici. Značke so lahko naravne, če so že del okolice (npr. luči na stropu, brezžični oddajniki itd.), ali pa umetne, če so nameščene v okolico za namen lokalizacije (npr. radijski oddajniki, ultrazvočni ali infrardeči oddajniki, v zemljo zakopane žice za robotske kosilnice, GPS-sateliti itd.). Glavna prednost uporabe aktivnih značk je preprosta, robustna in hitra lokalizacija. Vendar so stroški za njihovo namestitev, delovanje in vzdrževanje relativno visoki. Za oceno pozicije ali lege sistema se običajno uporablja triangulacija ali trila-teracija. Trilateracija s pomočjo izmerjenih razdalj do več oddajnikov (značk) oceni pozicijo sprejemnika, ki je nameščen na vozilo. Zelo znan tovrstni pristop je globalni pozicijski sistem, kjer so aktivne značke sateliti na znanih lokacijah v vesolju. Triangulacija pa uporablja izmerjene kote do treh ali več značk (npr. svetlobni vir) na znanih lokacijah. Osnovna ideja triangulacije je ponazorjena na sliki 5.13, kjer robot meri relativne kote αi glede na aktivne značke. Predpostavimo tri značke, kot je prikazano na sliki 5.13. Trenutna lega robota q = [ x, y, ϕ] T in izmerjeni koti αj ( j = 1 , 2 , 3) so povezani z naslednjimi relacijami ym 1 − y tan( α 1 + ϕ) = xm 1 − x ym 2 − y tan( α 2 + ϕ) = (5.36) xm 2 − x ym 3 − y tan( α 3 + ϕ) = xm 3 − x 234 Senzorji v mobilnih sistemih ( x , y m2 m2) ( x , y m3 m3) a ( x , y m1 m ) 1 3 a2 a1 j y x Slika 5.13: Lokalizacija robota s pomočjo triangulacije, kjer so izmerjeni relativni koti αi do značk Rešitev triangulacije dobimo z rešitvijo enačb (5.36) za q = [ x, y, ϕ] T . Osnovna ideja trilateracije je prikazana na sliki 5.17, kjer so trenutna pozicija robota, izmerjene razdalje do značk in njihove pozicije povezane z naslednjim sistemom enačb d 2 = ( x 1 m 1 − x)2 + ( ym 1 − y)2 d 2 = ( x (5.37) 2 m 2 − x)2 + ( ym 2 − y)2 d 2 = ( x 3 m 3 − x)2 + ( ym 3 − y)2 V nadaljevanju bomo obravnavali nekaj primerov trilateracije in triangulacije. Primer 5.9 Robot je opremljen s senzorjem, ki meri smeri do aktivnih značk. V okolici so tri aktivne značke na znanih lokacijah m1 = [ xm 1 , ym 1] T = [0 , 0] T , m2 = [ xm 2 , ym 2] T = [5 , 3] T in m3 = [ xm 3 , ym 3] T = [1 , 5] T . Pri trenutni legi robota q = [ x, y, ϕ] T so izmerjene smeri podane kot relativni koti α 1 = −2 , 7691, α 2 = −0 , 3585 in α 3 = 1 , 4277. Kakšna je trenutna lega robota q? Možnih je več rešitev tega triangulacijskega problema, pri čemer bomo v nada- ljevanju predstavili dve možnosti. V prvem delu je uporabljena optimizacija z rojem delcev (PSO), ki smo jo predstavili v poglavju 3.3.9. V drugem delu pa je uporabljen priljubljen geometrijski algoritem, ki temelji na presečišču krožnih lokov. 5.3. Metode merjenja lege 235 Rešitev A Trenutna lega robota q = [ x, y, ϕ] T in izmerjeni koti αj ( j = 1 , 2 , 3) so povezani z enačbami (5.36). Naloga algoritma PSO je najti neznanke ( x, y in ϕ), da so relacije (5.36) veljavne. Vsaka pozicija delca predstavlja eno od možnih rešitev (q i) in med optimizacijami se množica delcev posodobi v smislu bolj optimalnih rešitev. Merilo optimalnosti rešitve i-tega delca je podana na naslednji način 3 X Ji = f (q i) = ( αj − ˆ αj)2 j=1 kjer je ˆ αj simulacija meritve i-tega delca, ki jo dobimo iz (5.36) ymj − y ˆ αj = arctan − ϕ xmj − x Upoštevajte, da mora funkcija arctan vrniti pravilen kot v območju (− π, π] (v programskem okolju Matlab se za ta namen lahko uporabi funkcijo atan2). Pravilna rešitev je q = [2 , 2 , 5 , π/ 6] T , katere koda je podana v programu 5.6, končna situacija pa je prikazana na sliki 5.14. Program 5.6 ./src/sen/example_triangulation_pso.m 1 m = [0 , 0; 5 , 3; 1 , 5]. ’; % Zna č ke 2 r0 = [2; 2.5; pi /6]; % Prava lega robota , ki ni znana . 3 4 % Izmerjeni koti 5 alpha = wrapToPi ( atan2 ( m (2 ,:) - r0 (2) , m (1 ,:) - r0 (1)) - r0 (3)); 6 7 % Uporaba metode rojenja delcev ( PSO ) 8 iterations = 50; % Š tevilo iteracij 9 omega = 0.5; % Faktor vz tra jno st i 10 c1 = 0.5; % S amo zav ed na konstanta 11 c2 = 0.5; % Socialna konstanta 12 N = 25; % Velikost roja delcev 13 14 % Za č etni polo ž aji delcev 15 swarm = zeros ([3 , N ,4]); 16 swarm (1 ,: ,1) = 3 + randn (1 , N ); % Za č etne vrednosti x 17 swarm (2 ,: ,1) = 3 + randn (1 , N ); % Za č etne vrednosti y 18 swarm (3 ,: ,1) = 0 + randn (1 , N ); % Za č etne vrednosti fi 19 swarm (: ,: ,2) = 0; % Za č etne hitrosti delcev 20 swarm (1 ,: ,4) = 1000; % Najbolj š a vrednost k ri ter ijs ke funkcije 21 22 for iter = 1: iterations % Iterativno iskanje optimalne re š itve s PSO 23 % V r e d n o t e n j e p a r a m e t r o v d e l c e v 24 for i = 1: N 25 % I z r a č un n o v e g a p r e d v i d e n e g a k o t a na p o d l a g i i - t e g a d e l c a 26 p E s t = s w a r m (: , i , 1 ) ; % O c e n j e n i p a r a m e t r i d e l c a ( x , y , fi ) 27 28 % P r i m e r j a v a p r e d v i d e n i h k o t o v z i z m e r j e n i m i k o t i 29 a l p h a E s t = w r a p T o P i ( a t a n 2 ( m (2 ,:) - p E s t (2) , m (1 ,:) - p E s t (1)) - p E s t ( 3 ) ) ; 30 31 % I z r a č un k r i t e r i j s k e f u n k c i j e 32 c o s t = ( a l p h a E s t - a l p h a )*( a l p h a E s t - a l p h a ). ’; 236 Senzorji v mobilnih sistemih 33 if cost < s w a r m (1 , i ,4) % Č e je n o v i p a r a m e t e r b o l j š i , p o s o d o b i : 34 s w a r m (: , i ,3) = s w a r m (: , i , 1 ) ; % v r e d n o s t i p a r a m e t r o v ( x , y , in fi ) 35 s w a r m (1 , i ,4) = c o s t ; % in n a j b o l j š o v r e d n o s t k r i t e r i j s k e f u n k c i j e . 36 end 37 end 38 [~ , g B e s t ] = min ( s w a r m (1 ,: ,4)); % P a r a m e t r i g l o b a l n o n a j b o l j š ega d e l c a 39 40 % P o s o d o b i t e v p a r a m e t r o v s h i t r o s t n i m i v e k t o r j i 41 s w a r m (: ,: ,2) = o m e g a * s w a r m (: ,: ,2) + ... 42 c1 * r a n d (3 , N ) . * ( s w a r m (: ,: ,3) - s w a r m (: ,: ,1)) + ... 43 c2 * r a n d (3 , N ) . * ( r e p m a t ( s w a r m (: , gBest ,3) , 1 , N ) - s w a r m (: ,: ,1)); 44 s w a r m (: ,: ,1) = s w a r m (: ,: ,1) + s w a r m (: ,: ,2); 45 end 46 47 r = swarm (: , gBest ,1) % Re š itev , najbolj š a ocena lege 5 M3 4 α 3 3 α 2 M2 y 2 α 1 1 0 M1 −2 0 2 4 6 x Slika 5.14: Rešitev problema triangulacije iz primera 5.9 z optimizacijo roja delcev (PSO). Začetne lege delcev so označene s pika-črtica, končne lege pa s krog-črta. Rešitev B Obstaja mnogo analitičnih rešitev triangulacije, od katerih se najpogosteje uporablja presek krožnih lokov. Opisali bomo osnovno idejo principa in uporabili končno analitično rešitev za izračun lege robota. Celotno izpeljavo algoritma lahko najdete v [3]. 5.3. Metode merjenja lege 237 Algoritem temelji na treh krogih, kjer je vsak krog definiran s tremi točkami: par značk m i, m j, ( i, j = 1 , 2 , 3, i 6= j) in pozicija robota x, y (glejte sliko 5.15). ( x , y m2 m ) 2 a23 a12 ( x , y m1 m1) ( x , y m3 m3) a3 a2 a1 j y x Slika 5.15: Lokalizacija robota s triangulacijo na podlagi preseka krožnih lokov Par značk je povezan s položajem robota z dvema daljicama, med katerima je kot αij = αj − αi. Središča in polmeri teh treh krogov so " # " #! x 1 ( y c cij mi − ymj cot αij ) ij = = m i + m j + ycij 2 ( xmj − xmi cot αij) km i − m j k rij = 2 sin αij Ker velja α 13 = α 12 + α 23, sta samo dva od teh kotov neodvisna in njuna pripadajoča kroga (za α 12 in α 23) sta ( x − x 12)2 + ( y − y 12)2 = r 2 (5.38) 12 ( x − x 23)2 + ( y − y 23)2 = r 223 Presek obeh krogov (5.38) je rešitev za pozicijo robota. Za več podrobnosti glede pridobitve analitične rešitve, glejte [3]; tukaj je navedena le končna rešitev. Novi začasni koordinatni sistem je definiran tako, da je m2 v njegovem izhodišču in m3 leži na njegovi x-osi, kar omogoča lažjo pridobitev rešitve. Rotacijska matrika od referenčnega do začasnega koordinatnega sistema je " # cos β − sin β R = sin β cos β 238 Senzorji v mobilnih sistemih kjer je β = tan ym 3− ym 2 . Značke, izražene v začasnih koordinatah ( ¯ m x i = m 3− xm 2 [¯ xmi, ¯ ymi] T ), dobimo s transformacijo ¯ m i = R−1(m i − m2). Presečišče krogov v časovnih koordinatah (¯ x, ¯ y) je " # " # ¯ x 1 − η cot α 12 1 = ¯ xm 3 ¯ y 1 − η 2 − η kjer je η = ¯ xm 3−¯ xm 1−¯ ym 1 cot α 12 . Rešitev v referenčnih koordinatah je ¯ xm 3 cot α 23−¯ ym 1+¯ xm 1 cot α 12 " # " # x ¯ x = m2 + R y ¯ y in orientacija robota ym 1 − y ϕ = arctan − α 1 xm 1 − x Celoten algoritem za pridobitev rešitve je podan v Matlab kodi v programu 5.7. Rešitev je grafično prikazana na sliki 5.16. Program 5.7 ./src/sen/example_triangulation.m 1 m = [0 , 0; 5 , 3; 1 , 5]. ’; % Polo ž aji treh zna č k 2 r0 = [2; 2.5; pi /6]; % Prava lega robota , ki ni znana . 3 4 % Izmerjeni koti 5 alpha = wrapToPi ( atan2 ( m (2 ,:) - r0 (2) , m (1 ,:) - r0 (1)) - r0 (3)); 6 7 % T r i a n g u l a c i j a : izra č un lege na podlagi izmerjenih kotov 8 f = atan2 ( m (2 ,3) - m (2 ,2) , m (1 ,3) - m (1 ,2)); 9 S = [ cos ( f ) - sin ( f ); sin ( f ) cos ( f )]; % Rotacija za koo rdi na tni sistem v m2 10 m_ = S . ’*( m - repmat ( m (: ,2) ,1 ,3)); % Preslikani polo ž aji zna č k 11 12 cta = cot ( alpha (2) - alpha (1)); 13 ctb = cot ( alpha (3) - alpha (2)); 14 ni = ( m_ (1 ,3) - m_ (1 ,1) - m_ (2 ,1)* cta )/( m_ (1 ,3)* ctb - m_ (2 ,1)+ m_ (1 ,1)* cta ); 15 p_ = m_ (1 ,3)*(1 - ni * ctb )/(1+ ni ^2)*[1; - ni ]; 16 17 % Re š itev 18 p = m (: ,2) + S * p_ % Polo ž aj 19 fi = wrapToPi ( atan2 ( m (2 ,1) - p (2) , m (1 ,1) - r0 (1)) - alpha (1)) % O rie nt aci ja p = 2 . 0 0 0 0 2 . 5 0 0 0 fi = 0 . 5 2 3 6 5.3. Metode merjenja lege 239 5 M3 4 α 3 3 α 2 M2 y 2 α 1 1 0 M1 −2 0 2 4 6 x Slika 5.16: Rešitev problema triangulacije iz primera 5.9 z direktnim pristopom Primer 5.10 Robot je opremljen s senzorjem, ki meri razdaljo do aktivnih značk na podlagi merjenja časa potovanja signala od značke (oddajnik) do robota s sprejemnikom. Imamo tri aktivne značke na znanih lokacijah m1 = [ xm 1 , ym 1] T = [0 , 0] T , m2 = [ xm 2 , ym 2] T = [5 , 3] T in m3 = [ xm 3 , ym 3] T = [1 , 5] T . Pri trenutni poziciji robota r = [ x, y] T so izmerjene razdalje d 1 = 3 , 2016 m, d 2 = 3 , 0414 m in d 3 = 2 , 6926 m, kot je prikazano na sliki 5.17. 240 Senzorji v mobilnih sistemih ( x , y m2 m2) ( x , y m1 m ) 1 d2 d1 ( ) x, y d3 ( x , y m3 m3) Slika 5.17: Lokalizacija robota s trilateracijo, kjer so merjene razdalje do značk di Kakšna je trenutna pozicija robota r? Rešitev Naloga trilateracijskega algoritma je najti neznano pozicijo x, y, tako da so relacije (5.37) veljavne. To lahko storimo s PSO kot v primeru 5.9, kjer je potrebno izvedbo dopolniti z relacijami (5.37) pri izračunu kriterijske funkcije. Rešitev (5.37) lahko najdemo tudi analitično. Na voljo imamo več različnih algoritmov, tukaj pa je podana enostavna rešitev. Od prve in druge enačbe v (5.37) odštejemo tretjo ter dobimo d 2 − d 2 = ( x 1 3 m 1 − x)2 − ( xm 3 − x)2 + ( ym 1 − y)2 − ( ym 3 − y)2 d 2 − d 2 = ( x 2 3 m 2 − x)2 − ( xm 3 − x)2 + ( ym 2 − y)2 − ( ym 3 − y)2 Enačbi preuredimo v 2( xm 3 − xm 1) x + 2( ym 3 − ym 1) y = d 2 − d 2 − x 2 + x 2 − y 2 + y 2 1 3 m 1 m 3 m 1 m 3 2( xm 3 − xm 2) x + 2( ym 3 − ym 2) y = d 2 − d 2 − x 2 + x 2 − y 2 + y 2 2 3 m 2 m 3 m 2 m 3 ter tako dobimo linearni enačbi v odvisnosti od x in y, ki ju lahko zapišemo v 5.3. Metode merjenja lege 241 obliki Ar = b, kjer sta " # 2( x A m = 3 − xm 1) 2( ym 3 − ym 1) 2( xm 3 − xm 2) 2( ym 3 − ym 2) in " # d 2 − d 2 − x 2 + x 2 − y 2 + y 2 b = 1 3 m 1 m 3 m 1 m 3 d 2 − d 2 − x 2 + x 2 − y 2 + y 2 2 3 m 2 m 3 m 2 m 3 od koder izračunamo neznano pozicijo r = A−1b S tem dobimo pravilno rešitev r = [2 , 2 , 5] T , katere Matlab koda je podana v programu 5.8. Program 5.8 ./src/sen/example_trilateration.m 1 m = [0 , 0; 5 , 3; 1 , 5]. ’; % Polo ž aji zna č k 2 r0 = [2; 2.5; pi /6]; % Prava lega robota , ki ni znana . 3 4 % Izmerjene razdalje do zna č k 5 d = sqrt (( m (1 ,:) - r0 (1)).^2+( m (2 ,:) - r0 (2)).^2); 6 7 % T r i l a t e r a c i j a : iskanje lege robota glede na izmerjene razdalje 8 N = size (m ,2); A = zeros (N -1 ,2); b = zeros (N -1 ,1); 9 for i = 1: N -1 10 A ( i ,:) = 2*[ m (1 , N ) - m (1 , i ) , m (2 , N ) - m (2 , i )]; 11 b ( i ) = d ( i )^2 - d ( N )^2 - m (1 , i )^2 + m (1 , N )^2 - m (2 , i )^2 + m (2 , N ) ^ 2 ; 12 end 13 14 r = A \ b % Izra č unan polo ž aj r = 2 . 0 0 0 0 2 . 5 0 0 0 Primer 5.11 V primeru 5.10 so bile izmerjene razdalje točne, kar je nerealna predpostavka. Običajno so v meritvah prisotni šumi in druge motnje, zato je potreben predoločen sistem z več kot tremi značkami, da se minimizira pogrešek ocene pozicije. Za ponazoritev uporabimo n = 4 značke na lokacijah m1 = [ xm 1 , ym 1] T = [0 , 0] T , m2 = [ xm 2 , ym 2] T = [5 , 3] T , m3 = [ xm 3 , ym 3] T = [1 , 5] T in m4 = [ xm 4 , ym 4] T = [2 , 4] T . Izmerjene razdalje s šumom so d 1 = 3 , 2297 m, d 2 = 3 , 0697 m, d 3 = 2 , 7060 m in d 4 = 1 , 4759 m. Ocenite trenutno pozicijo robota r. 242 Senzorji v mobilnih sistemih Rešitev Predoločen sistem z n aktivnimi značkami, ki minimizira povprečno kvadratno napako kAr − bk, dobimo na naslednji način. Matrika A je   2( xmn − xm 1) 2( ymn − ym 1)  2( x  mn − xm 2) 2( ymn − ym 2) A =    . .   .. ..    2( xmn − xmn−1) 2( ymn − ymn−1) in vektor b je   d 2 − d 2 − x 2 + x 2 − y 2 + y 2 1 n m 1 mn m 1 mn  d 2 − d 2 − x 2 + x 2 − y 2 + y 2  2 n m 2 mn m 2 mn b =    .   ..    d 2 − d 2 − x 2 + x 2 − y 2 + y 2 n−1 n mn−1 mn mn−1 mn Rešitev dobimo z uporabo psevdoinverza r = (A T A)−1A T b Za dano razdaljo je rešitev r = [1 , 9873 , 2 , 5337] T . Matlab koda implementacije je podana v programu 5.9, rešitev pa je prikazana na sliki 5.18. Program 5.9 ./src/sen/example_trilateration_noise.m 1 m = [0 , 0; 5 , 3; 1 , 5; 2 , 4]. ’; % Polo ž aji zna č k 2 3 % Izmerjene razdalje do zna č k 4 d = [3.2297 , 3.0697 , 2.7060 , 1.4759]; 5 6 % T r i l a t e r a c i j a : iskanje lege robota glede na izmerjene razdalje 7 N = size (m ,2); A = zeros (N -1 ,2); b = zeros (N -1 ,1); 8 for i = 1: N -1 9 A ( i ,:) = 2*[ m (1 , N ) - m (1 , i ) , m (2 , N ) - m (2 , i )]; 10 b ( i ) = d ( i )^2 - d ( N )^2 - m (1 , i )^2 + m (1 , N )^2 - m (2 , i )^2 + m (2 , N ) ^ 2 ; 11 end 12 13 r = A \ b % Izra č unan polo ž aj r = 1 . 9 8 7 3 2 . 5 3 3 7 5.3. Metode merjenja lege 243 5 M3 4 d M4 3 d4 d 3 2 M2 y 2 1 d1 0 M1 −2 0 2 4 6 x Slika 5.18: Rešitev problema trilateracije iz primera 5.11 Globalni pozicijski sistem Najpogosteje uporabljen princip trilateracije za lokalizacijo je globalni pozicijski sistem (GPS, angl. Global positioning system). Sateliti predstavljajo aktivne značke, ki pošiljajo kodiran signal GPS sprejemni postaji, katere pozicijo je potrebno oceniti s trilateracijo. Sateliti imajo zelo točno atomsko uro ter znane pozicije, določene s Keplerjevimi elementi in drugimi dvovrstičnimi parametri. Obstaja več GPS-sistemov: Navstar iz ZDA, Glonass iz Rusije in Galileo iz Evrope. GPS-sistem Navstar sestoji iz najmanj 24 satelitov, ki dvakrat dnevno obkrožijo Zemljo na višini 20 200 km. GPS se zdi zelo priročen senzorski sistem za lokalizacijo, vendar ima nekatere omejitve, ki jih je potrebno upoštevati pri uporabi v mobilnih sistemih. Ovire, kot so drevesa, hribi in zgradbe, blokirajo GPS-signal in onemogočijo sprejem. Zaradi večkratnih odbojev pa lahko pride do interference signalov in posledično napačne ocene razdalje. Vseeno gre za zelo zmogljiv sistem, ki dosega točnost okoli 5 m oz. celo 1 cm za diferencialne sisteme z dodatnim sprejemnikom v referenčni postaji. Lokalizacijo z uporabo GPS lahko razložimo na enostaven način. Sprejemnik meri čas potovanja signala iz določenega satelita. Čas potovanja je razlika med časom sprejema tr in časom oddaje tt. Signal potuje s svetlobno hitrostjo c, 244 Senzorji v mobilnih sistemih satelit 1 satelit 2 d 1 GPS-sprejemnik d 2 satelit 3 d 3 d 4 satelit 4 Slika 5.19: GPS lokalizacija zahteva sprejem od najmanj štirih satelitov za oceno pozicije GPS-sprejemnika in časovne zakasnitve zato se lahko izračuna razdalja med sprejemnikom in satelitom. Vendar pa ura sprejemnika ni tako natančna kot atomska ura na satelitih, zato se pojavi neznana časovna pristranskost ali pogrešek razdalje, ki je enak za vse razdalje do satelitov. Torej mora GPS sprejemnik oceniti 4 parametre: svojo tridimenzionalno pozicijo ( x, y, z) in časovno pristranskost tb. Okoli vsakega satelita narišemo sfero (tj. površina krogle), katere polmer določa izmerjena razdalja. Presek dveh sfer je krožnica, presek treh sfer pa sta dve točki, v katerih se lahko nahaja sprejemnik. Zato potrebujemo vsaj še eno sfero, da zanesljivo ocenimo pozicijo sprejemnika. Če se sprejemnik nahaja na površju Zemlje, jo lahko obravnavamo kot četrto sfero, s katero izločimo pravilno točko, pridobljeno iz preseka treh satelitskih sfer. V idealnem primeru bi bili trije satelitski sprejemniki dovolj. Ampak kot smo že omenili, je ura sprejemnika netočna, kar povzroča neznano časovno pristranskost tb. Posledično presek štirih sfer (tri od satelitov in ena od Zemlje) ni točka ampak območje. Za oceno časa tb in manjši pogrešek lokalizacije je potreben sprejem četrtega satelita, kar pomeni, da so za GPS-lokalizacijo potrebni vsaj štirje sateliti, kot je prikazano na sliki 5.19. GPS lokalizacija mora rešiti naslednji sistem enačb 5.3. Metode merjenja lege 245 p d 1 = c( tr 1 − tt 1 − td) = ( x 1 − x)2 + ( y 1 − y)2 + ( z 1 − z)2 p d 2 = c( tr 2 − tt 2 − td) = ( x 2 − x)2 + ( y 2 − y)2 + ( z 2 − z)2 p d 3 = c( tr 3 − tt 3 − td) = ( x 3 − x)2 + ( y 3 − y)2 + ( z 3 − z)2 p d 4 = c( tr 4 − tt 4 − td) = ( x 4 − x)2 + ( y 4 − y)2 + ( z 4 − z)2 kjer so neznanke pozicija sprejemnika x, y, z in časovna zakasnitev sprejemnika td. Za i-ti satelit so znane vrednosti pozicija ( xi, yi, zi), čas sprejema tri, čas prenosa tti in hitrost svetlobe c. 5.3.4 Navigacija z uporabo značilk okolja Značilke so podmnožica vzorcev, ki jih je mogoče robustno razbrati iz neobde-lanih meritev senzorja ali drugih podatkov. Značilke so lahko premice, daljice, krogi, pike, robovi, vogali in drugi vzorci. Zaznavanje značilk v okolju se lahko uporablja za namene lokalizacije (ocene lege) mobilnega robota in gradnjo zemljevida. V primeru dvodimenzionalnega laserskega pregledovalnika razdalj lahko iz dobljenih podatkov (razdalja in kot) izločimo značilke v obliki daljic. Linijske značilke v lokalnem koordinatnem sistemu mobilnega robota lahko nato primerjamo z globalnim zemljevidom okolice, ki je prav tako predstavljen z nizom linij, da bi določili lego mobilnega robota na zemljevidu. Dandanes se v mobilni robotiki uporabljajo različni senzorji, med katerimi je najpopularnejša kamera. V zadnjih letih so bili razviti številni algoritmi strojnega vida za zaznavanje slikovnih značilk, ki se lahko uporabijo tudi za merjenje lege robota v okolici. Pristopi, ki temeljijo na značilkah, običajno vsebujejo naslednje korake: detekcija značilk, opis značilk in ujemanje značilk. V fazi detekcije značilk se obdelujejo neobdelani podatki za določitev lokacij značilk. Za opis zaznane značilke se obi- čajno uporablja območje okoli njene lokacije, nato se lahko uporabijo deskriptorji (opisi značilk) za iskanje podobnih značilk (faza ujemanja značilk). Značilke se nahajajo na znanih lokacijah, zato lahko njihovo opazovanje izboljša znanje o lokaciji mobilnega robota (manjša negotovost lokacije). Seznam značilk z njihovimi lokacijami se imenuje zemljevid, ki je lahko predhodno shranjen v pomnilniku ali pa se gradi sproti med lokalizacijo — pristop, ki to omogoča, se imenuje SLAM (angl. simultaneous localization and mapping). Prvi pristop je metodološko enostavnejši, a hkrati nepraktičen, še posebej za večja okolja, saj zahteva uporabo nekega referenčnega sistema lokalizacije ali pa je potrebno ročno zapisati zaznane značilke. Glavna ideja drugega pristopa pa je možnost lokalizacije iz opazovanih značilk, ki so že na zemljevidu, in shranjevanje novo opaženih značilk na podlagi zaznane lokacije. Za zanesljivo zaznavanje značilk in kar se da točno lokalizacijo robota je priporočljiva metoda relativnega določanja položaja – odometrija. V primeru težko razpoznavnih značilk (npr. debla dreves v sadovnjaku ali zaznavanje daljic v stavbah) so za identifikacijo opazovanih značilk potrebne približne informacije o lokaciji robota. Približna lokacija robota v trenutnem času je pridobljena iz lokacije v prejšnjem času in napovedi odometrije za relativno gibanje od prejšnje do trenutne lokacije. 246 Senzorji v mobilnih sistemih Značilke so lahko naravne, če so že del okolja, ali umetne, če so izdelane posebej za namen lokalizacije. Naravne značilke v strukturiranih okoljih (običajno v zaprtih prostorih) so stene, talne plošče, luči, vogali ipd., v nestrukturiranih okoljih (običajno na prostem) pa so to drevesna debla, prometni znaki itd. Umetne značilke so narejene izključno za namen preproste in robustne lokalizacije (barvne oznake, črtne kode, talne linije itd.). Običajno pridobivanje značilk zahteva nekaj obdelave podatkov senzorjev, da bi dobili bolj kompaktno, informativno in abstraktno predstavitev trenutnega pogleda senzorja (linijska predstavitev proti množici točk). V nekaterih primerih lahko uporabimo tudi neobdelane meritve senzorja (npr. slika kamere) za proces lokalizacije s korelacijo pogleda senzorja in shranjenega zemljevida. Pogosto se uporabljajo vizualne značilke, ki jih je mogoče zaznati z nekaterimi slikovnimi senzorji. Ena najpreprostejših in najbolj uporabljenih značilk je premica, ki jo lahko v okolju zazna kamera ali laserski merilnik razdalj. Premica kot značilka V lokalizaciji je premica pogosta izbira za značilko, saj gre za preprosto geometrijsko obliko. Uporabimo jo lahko za opisovanje notranjega ali zunanjega okolja (stene, ploski predmeti, cestne proge itd.). S primerjavo trenutno opazovanih parametrov značilk in parametrov predhodno znanega zemljevida okolja lahko ocenimo lego robota. V ta namen se pogosto uporablja laserski merilnik razdalj, ki meri oblak točk odboja v okolju. Iz tega izmerjenega oblaka točk se s pomočjo različnih namenskih algoritmov ocenijo (običajno dvodimenzionalni) parametri premic. Postopek prilagajanja premici navadno zahteva dva koraka: prvi je identifikacija rojev, ki jih je mogoče predstaviti s premico, drugi pa je ocena parametrov prilagajanja premici za vsak roj, recimo z metodo najmanjših kvadratov. Običajno se ta dva koraka izvajata iterativno. Algoritem razcepi-in-združi Zelo priljubljen algoritem za obdelavo podatkov laserskega pregledovalnika razdalj je razcepi-in-združi (angl. split-and-merge) [4, 5], ki je preprost za izvedbo, ima nizko računsko zahtevnost in dobro zmogljivost. Algoritem zahteva paketne podatke, ki so iterativno razdeljeni na roje, kjer je vsak roj opisan z linearnim prototipom (premica za dvodimenzionalne podatke). Algoritem se lahko uporabi le za urejene podatke, kjer zaporedni vzorci podatkov pripadajo isti premici (podatki iz laserskega pregledovalnika razdalj so običajno urejeni). Sprva vsi vzorci podatkov pripadajo enemu roju, katerega parametri linearnega prototipa (premica) so prepoznani. Roj se nato razdeli pri vzorcu, ki ima največjo razdaljo od prototipa in je ta razdalja večja od praga dsplit. Izbira vrednosti dsplit je odvisna od šuma podatkov in mora biti večja od pričakovanega merilnega 5.3. Metode merjenja lege 247 pogreška zaradi šuma (npr. tri standardne deviacije). Ko se rojenje zaključi, se združijo kolinearni roji. Ta korak je neobvezen in običajno ni potreben pri urejenih podatkovnih vrstah. Za vsak roj j ( j = 1 , . . . , m) zapišemo linearni prototip v normalni obliki [z T ( k) , 1]θ j = 0 (5.39) kjer je z( k) = [ x( k) , y( k)] T k-ti vzorec ( k = 1 , . . . , n; n je število vseh vzorcev), ki leži na premici, določeni s parametri θ. Za roj j, ki vsebuje vzorce kj = 1 , . . . , nj, lahko s pomočjo singularnega razcepa ocenimo vektor parametrov θ j. Regresijska matrika   z T (1) 1 . . ψ =  . .   . .    z T ( nj) 1 določa množico homogenih enačb v matrični obliki ψθ j = 0, kjer je potrebno oceniti parametre prototipa θ j v smislu minimizacije najmanjših kvadratov. Rešitev predstavlja lastni vektor (p r = [ px, py, pp] T ) regresijske matrike ψ T ψ, ki pripada najmanjši lastni vrednosti (izračunana z uporabo singularnega razcepa). Parametre prototipa v normalni obliki dobimo z normalizacijo p r p θ r j = ± q p 2 + p 2 x y kjer je izbrani predznak nasproten od tretjega parametra p r ( pp). Za podatkovni vzorec z( k), ki ni v prototipu j, izračunamo ortogonalno razdaljo d j ( k) = [z T ( k) , 1]θ j V primeru dvodimenzionalnih podatkov se lahko linearni prototip alternativno oceni s povezovanjem prvega in zadnjega podatkovnega vzorca v roju. To ni opti- malno v smislu najmanjših kvadratov, vendar zmanjšuje računsko kompleksnost in zagotavlja, da se vzorec, ki definira razcep, ne pojavi v prvem ali zadnjem podatkovnem vzorcu. Prikaz prilagoditve podatkov laserskega pregledovalnika razdalj na premice je podan v primeru 5.12. Primer 5.12 Za podatke laserskega pregledovalnika razdalj, ki so sestavljeni iz 180 točk odboja (glejte sliko 5.20 in program 5.10), ocenite roje premic, ki najbolje opisujejo meritve. Rojenje poteka z uporabo praga razdalje dsplit = 0 , 06 m. Program 5.10: Podatki laserskega pregledovalnika razdalj ./src/sen/script_laserscandata.m 248 Senzorji v mobilnih sistemih 1 data = [ -0 -2149 38 -2158 76 -2166 110 -2092 137 -1962 162 -1851 ... 2 185 -1761 222 -1809 255 -1817 289 -1822 324 -1835 358 -1840 ... 3 393 -1849 428 -1853 464 -1862 502 -1873 539 -1879 577 -1887 ... 4 617 -1898 656 -1905 697 -1915 738 -1921 780 -1929 824 -1942 ... 5 860 -1931 873 -1872 885 -1814 898 -1763 911 -1714 957 -1726 ... 6 1 0 0 1 -1734 1 0 3 5 -1722 2 4 0 7 -3852 2 4 3 1 -3743 2 4 5 2 -3635 2 4 7 6 ... 7 -3536 2 4 9 7 -3437 2 5 1 9 -3342 2 5 3 9 -3250 2 5 5 8 -3158 2 5 7 6 -3070 ... 8 2 5 9 6 -2986 2 6 1 4 -2903 2 6 3 0 -2821 2 6 4 9 -2744 2 6 6 4 -2664 2 6 8 3 ... 9 -2591 2 6 9 8 -2516 2 7 1 5 -2445 2 7 3 0 -2373 2 7 4 2 -2301 2 7 5 9 -2234 ... 10 2 7 7 4 -2167 2 7 9 0 -2102 2 8 0 2 -2036 2 8 1 7 -1973 2 8 3 1 -1910 2 8 4 5 ... 11 -1847 2 8 5 7 -1785 2 8 6 9 -1724 2 8 8 5 -1666 2 8 9 8 -1606 2 9 0 8 -1546 ... 12 2 9 2 4 -1490 2 9 3 6 -1432 1 1 4 6 -535 1 1 4 9 -512 1 1 5 5 -490 1 1 6 0 -469 ... 13 1 1 6 4 -447 1 1 6 9 -425 1 1 7 4 -404 1 1 7 8 -383 1 1 8 2 -361 1 1 8 8 -341 ... 14 1 1 9 0 -319 1 1 9 5 -298 1 1 9 9 -277 1 2 0 5 -256 1 2 1 0 -235 1 2 1 4 -214 ... 15 1 2 2 0 -193 1 2 2 2 -172 1 2 2 7 -151 1 2 3 1 -129 1 2 3 8 -108 1 2 4 1 -87 ... 16 1 2 4 8 -65 1 2 5 3 -44 1 2 5 4 -22 1 2 5 9 0 1 2 6 5 22 1 2 6 8 44 1 2 7 7 67 ... 17 1 2 7 9 89 1 2 8 7 113 1 2 9 1 136 1 2 9 5 159 1 3 0 0 183 1 3 0 6 207 1 3 1 2 ... 18 231 1 3 1 5 256 1 3 2 0 280 1 3 2 8 307 1 1 7 8 294 1 1 3 3 304 1 0 9 2 313 ... 19 1 0 5 0 321 1 0 1 4 329 977 336 944 344 915 351 885 357 856 363 ... 20 829 369 805 375 778 380 756 385 735 391 713 395 694 401 674 ... 21 405 652 408 637 413 617 416 600 420 582 423 566 427 552 432 ... 22 537 435 520 436 507 441 493 444 478 446 463 447 452 452 439 ... 23 455 426 456 414 459 401 461 391 466 380 469 1 9 2 0 2 4 5 7 1 9 4 4 ... 24 2 5 8 0 1 9 7 6 2 7 2 0 2 0 1 1 2 8 7 2 2 0 4 4 3 0 3 0 2 0 8 1 3 2 0 4 2 1 1 5 3 3 8 5 2 0 4 2 ... 25 3 3 9 9 1 9 7 1 3 4 1 5 1 9 0 3 3 4 3 3 1 8 3 2 3 4 4 5 1 7 6 4 3 4 6 2 1 6 9 4 3 4 7 4 1 6 2 9 ... 26 3 4 9 3 1 5 6 1 3 5 0 6 1 4 9 5 3 5 2 2 1 4 2 8 3 5 3 3 1 3 6 2 3 5 4 9 1 2 9 7 3 5 6 4 1 2 3 1 ... 27 3 5 7 5 1 1 6 7 3 5 9 3 1 1 0 2 3 6 0 3 187 654 175 655 164 656 153 662 ... 28 141 662 130 667 118 668 106 671 94 670 83 677 71 678 60 681 ... 29 48 683 36 686 24 691 12 6 9 0] . ’ ; 30 x = data (2:2: end )/1000; 31 y = data (1:2: end -1)/1000; Rešitev Predstavljena je enostavna izvedba algoritma, ki izračuna parametre premic vsakega roja v smislu najmanjših kvadratov. Če je potrebno roj razdeliti in se pojavi delitveni vzorec (ki definira razcep) kot prva ali zadnja točka v roju, potem roja ni mogoče razcepiti. V tem primeru ponovno izračunamo parametre premic, da se prilegajo le prvemu in zadnjemu vzorcu, ter izvedemo razcep. Izvedba ocene premic je podana v programu 5.11, podatki laserskega pregledovalnika razdalj pa so v programu 5.10. Ocenjene daljice so prikazane na sliki 5.20. Program 5.11 ./src/sen/example_lines_sandm.m 1 X = [x , y ]; % Meritve laserskega merilnika razdalj 2 [N , M ] = size ( X ); 3 4 % Init 5 C = 50; % Maksimalno š tevilo rojev 6 clusters = 1; % Zadnji aktivni roj 7 dMin = 0.06; % Prag razdalje za deljenje roja 8 5.3. Metode merjenja lege 249 9 s i z e O f C l u s t e r = zeros (C ,1); % Š tevilo to č k v rojih 10 c l u s t e r B o u n d s = zeros (C ,2); % Indeksi mejnih to č k v rojih 11 c l u s t e r P a r a m s = zeros (C , M +1); % Parametri rojev 12 s p l i t C l u s t e r = zeros (C ,1); % Zastavica za deljenje 13 14 % Na za č etku so vse to č ke v enem roju 15 s i z e O f C l u s t e r ( clusters ,1) = N ; 16 c l u s t e r B o u n d s ( clusters ,:)= [1 , N ]; % To č ke so urejene 17 s p l i t C l u s t e r ( clusters ,1) = 1; % Za č etni roj lahko delimo 18 19 exit = false ; 20 while ~ exit 21 e x i t = t r u e ; 22 t m p L a s t C l u s t e r = c l u s t e r s ; 23 for i = 1: t m p L a s t C l u s t e r 24 if s p l i t C l u s t e r ( i ) 25 p0 = c l u s t e r B o u n d s ( i , 1 ) ; % Za č e t n a to č ka v r o j u 26 p1 = c l u s t e r B o u n d s ( i , 2 ) ; % Kon č na to č ka v r o j u 27 28 % O c e n a p a r a m e t r o v r o j a v s m i s l u n a j m a n j š ih k v a d r a t o v 29 Psi = [ X ( p0 : p1 ,:) , o n e s ( p1 - p0 + 1 ,1 ) ]; 30 [~ , ~ , V ] = svd ( Psi ); 31 t h e t a E s t = V (: ,3); 32 % P r e s l i k a v a p r e m i c e ax + by + c =0 v n o r m a l n o o b l i k o 33 s = - s i g n ( t h e t a E s t ( 3 ) ) ; if s ==0 , s = 1; end 34 mi = 1/ s q r t ( t h e t a E s t ( 1 ) ^ 2 + t h e t a E s t ( 2 ) ^ 2 ) * s ; 35 T h e t a = t h e t a E s t * mi ; 36 37 % O c e n i t e v e n o s t a v n i h p a r a m e t r o v p r e m i c e ( an p o d l a g i za č e t n e in 38 % kon č ne to č ke ). Ti p a r a m e t r i se u p o r a b i j o , ko je to č ka d e l j e n j a 39 % na m e j i r o j a 40 if abs ( X ( p1 ,1) - X ( p0 , 1 ) ) < 1 0 0 * eps % V e r t i k a l n a p r e m i c a 41 a = 1; b = 0; c = - X (1 ,1); 42 e l s e 43 a = ( X ( p1 ,2) - X ( p0 , 2 ) ) / ( X ( p1 ,1) - X ( p0 , 1 ) ) ; 44 b = -1; 45 c = - a * X ( p0 ,1) + X ( p0 , 2 ) ; 46 end 47 % P r e s l i k a v a p r e m i c e ax + by + c =0 v n o r m a l n o o b l i k o 48 t h e t a E s t = [ a ; b ; c ]; 49 s = - s i g n ( t h e t a E s t ( 3 ) ) ; if s ==0 , s = 1; end 50 mi = 1/ s q r t ( t h e t a E s t ( 1 ) ^ 2 + t h e t a E s t ( 2 ) ^ 2 ) * s ; 51 T h e t a 0 = t h e t a E s t * mi ; 52 53 % S h r a n j e v a n j e o p t i m a l n i h p a r a m e t r o v 54 c l u s t e r P a r a m s ( i ,:) = T h e t a . ’; 55 ind = p0 : p1 ; 56 XX = X ( ind , : ) ; 57 58 % I z r a č un r a z d a l j e na p o d l a g i p r v e in z a d n j e 59 % to č ke v r o j u ( e n o s t a v n a p r e m i c a ) 60 dik = [ XX , o n e s ( s i z e ( XX , 1 ) , 1 ) ] * T h e t a 0 ; 61 [ dd0 , iii ] = max ( abs ( dik )); ii0 = ind ( iii ); 62 63 % I z r a č un r a z d a l j e od p r e m i c e v s m i s l u n a j m a n j š ih k v a d r a t o v 64 dik = [ XX , o n e s ( s i z e ( XX , 1 ) , 1 ) ] * T h e t a ; 65 [ dd , iii ] = max ( abs ( dik )); ii = ind ( iii ); 66 67 % D e l j e n j e r o j a 68 d o S p l i t = 0; 69 if dd > d M i n && ( ii - p0 ) >=2 && ( p1 - ii ) >=1 % O p t i m a l n e p r e m i c e 70 if c l u s t e r s < C 71 i i F i n = ii ; % L o k a c i j a d e l j e n j a 250 Senzorji v mobilnih sistemih 72 d o S p l i t = 1; 73 c l u s t e r P a r a m s ( i ,:) = T h e t a . ’; 74 end 75 e l s e i f dd0 > d M i n && ( ii0 - p0 ) >=2 && ( p1 - ii0 ) >=1 % E n o s t a v n e p r e m i c e 76 if c l u s t e r s < C 77 i i F i n = ii0 ; % L o k a c i j a d e l j e n j a 78 d o S p l i t = 1; 79 c l u s t e r P a r a m s ( i ,:) = T h e t a 0 . ’; 80 end 81 e l s e 82 s p l i t C l u s t e r ( i ) = 0; 83 end 84 85 86 if d o S p l i t ==1 && c l u s t e r s < C 87 % D e l j e n j e r o j a v r o j a A in B 88 c l u s t e r s = c l u s t e r s + 1; % Nov roj 89 % P r v a in z a d n j a to č ka v r o j u A 90 c l u s t e r B o u n d s ( i , 1 ) ; 91 c l u s t e r B o u n d s ( i ,2) = iiFin -1; 92 s p l i t C l u s t e r ( i ) = 1; 93 % P r v a in z a d n j a to č ka v r o j u B 94 c l u s t e r B o u n d s ( c l u s t e r s ,1) = i i F i n +1; 95 c l u s t e r B o u n d s ( c l u s t e r s ,2) = p1 ; 96 s p l i t C l u s t e r ( c l u s t e r s ) = 1; 97 98 e x i t = f a l s e ; 99 end 100 end 101 end 102 end 3 2 ] [my 1 0−4 −2 0 2 4 x [m] Slika 5.20: Podatki LRF in identični roji premic z uporabo algoritma razcepi-in-združi Samorazvijajoče se rojenje premic Podobno kot pri algoritmu razcepi-in- združi lahko ocenimo premice tudi v primeru podatkovnih vrst. Rojenje se izvaja sproti in se iterativno posodobi, ko prispejo novi podatki. Primer preprostega in 5.3. Metode merjenja lege 251 računalniško učinkovitega algoritma je samorazvijajoče se rojenje premic [6]. V nadaljevanju bomo na kratko opisali njegove glavne korake. Zapišemo j-ti prototip, ki modelira podatke z( kj) ( kj = 1 , . . . , nj) v j-tem roju (z( kj) − µ j) T · p j = 0 kjer je µ j( kj) srednja vrednost podatkov v j-tem roju, ki se posodobi v vsaki iteraciji (ko je na voljo nov vzorec) kot k 1 µ j − 1 j ( kj ) = µ j( kj − 1) + z( kj) kj kj in p j je normalni vektor j-tega prototipa, ki ga izračunamo iz kovariančne matrike j-tega roja (za dvodimenzionalne podatke) " # σ 2 σ 2 Σ 11 12 j ( kj ) = σ 2 σ 2 21 22 kot lastni vektor pri pripadajoči najmanjši lastni vrednosti Σ j( kj) h i T  θ −1  √ √ ; | λ 1| ≤ | λ 2| p 1+ θ 2 1+ θ 2 j = h i T  1 √ θ √ ; | λ  1| > | λ 2| 1+ θ 2 1+ θ 2 kjer so θ in lastne vrednosti λ 1 in λ 2 določene z − σ 2 p + σ 2 + σ 4 + σ 4 − 2 σ 2 σ 2 + 4 σ 4 θ = 11 22 11 22 11 22 12 2 σ 212 λ 1 = σ 2 − θσ 2 22 12 1 + θ 2 λ 2 = σ 2 − θσ 2 + σ 2 22 12 θ 12 Kovariančna matrika se posodablja iterativno kj − 2 1 Σ j( kj) = Σ j( kj − 1) + (z( kj) − µ j( kj − 1)) (z( kj) − µ j( kj − 1)) T kj − 1 kj Trenutni vzorec z( k) je potrebno razvrstiti v enega od obstoječih prototipov j ( j ∈ {1 , . . . , m}). To se izvede z izračunom ortogonalne razdalje dj( k) od vsakega j-tega prototipa dj( k) = |(z( k) − µ j) T p j| Če je dj( k) = 0, podatkovni vzorec leži na j-tem linearnem prototipu. Vzorec spada v j-ti roj, če je razdalja dj( k) za j-ti roj najmanjša ter hkrati manjša od vnaprej določenega praga dmin ( dj( k) < dmin). V [6] je predlagano robustno rojenje, kjer je dmin sproti ocenjen iz podatkov j-tega roja. Osnovna ideja algoritma rojenja je prikazana na sliki 5.21. Lahko ga uporabimo sproti za urejene podatkovne vrste ali pa paketne podatke vzorcev (kot razcepi-in-združi). Rezultati razvrstitve primera 5.12 so računsko manj zahtevni za podobno kakovost rojenja kot pri algoritmu razcepi-in-združi. 252 Senzorji v mobilnih sistemih Start Inicializacija prvega roja z vsaj tremi vzorci in ocena parametrov roja (normalni vektor in srednja vrednost) Za vzorec k Izračun razdalje d( k) do j-tega roja Dodaj vzorec v roj in posodobi normalni vektor ter srednjo vrednost Da dj( k) < dmin Shrani vzorec Vzorec pripada trenutnemu roju Ne Ali so Da Ali so Da Dodaj nov roj in na voljo 3 podatki oceni normalni vektor novi vzorci? konsistentni? ter srednjo vrednost Ne Ne Slika 5.21: Princip samorazvijajočega se rojenja za pretočne podatke, kjer roje določajo prototipi premic 5.3. Metode merjenja lege 253 Primer 5.13 Za podatke laserskega pregledovalnika razdalj, ki so sestavljeni iz 180 točk odboja (glejte sliko 5.20 in začetek rešitve za koordinate točk), ocenite roje premic, ki najbolje opišejo meritve. Rojenje poteka s pomočjo algoritma razvijajočega se rojenja premic. Rešitev Trenutni vzorec pripada j-temu roju, če je njegova razdalja dj( k) do premice roja manjša od praga dmin ( dj( k) < dmin). Prag dmin je lahko konstanta ali pa ga, kot v tem primeru, ocenjujemo sproti. Prag razdalje dmin se izračuna iz ocenjene variance razdalje roja σj( kj) (varianca razdalje vzorcev od premice). Rekurzivna 2 d √ ocena variance je σ j ( k) j ( kj ) = σj ( kj − 1) kj −2 + in prag je d σ k min = κmax j , j −1 kj kjer je κmax = 7 nastavitveni parameter. Izvedba ocene premic je podana v programu 5.12 (podatki laserskega pregledovalnika razdalj so opredeljeni v programu 5.10). Ocenjene daljice so prikazane na sliki 5.22. Program 5.12 ./src/sen/example_straight_lines.m 1 X = [x , y ]; % Meritve laserskega merilnika razdalj 2 kappaMax = 7; % Prag za faktor ort ogo nal ne razdalje 3 cosPhiTh = cos (10/180* pi ); % Prag za za č etno k o l i n e a r n o s t roja 4 5 [n , m ] = size ( X ); dimension = m ; 6 % Parametri 7 N r _ c l o u d _ m a x = 20; C u r r e n t _ c l u s t = 1; 8 9 N r _ p o i n t s _ i n _ c l o u d = zeros ( Nr_cloud_max ,1); % Vektor , ki dolo č a š tevilo 10 % to č k v oblaku , kjer vsaka vrstica pripada drugemu oblaku 11 M_ of_ cl oud s = zeros ( Nr_cloud_max , dimension ); % Matrika - vrstice pripadajo 12 % razli č nim oblakom in stolpci vsebujejo elemente vhodnega vektorja 13 V_ of_ cl oud s = zeros ( Nr_cloud_max , dimension ^2); 14 V a r D _ o f _ c l o u d = zeros ( Nr_cloud_max ,1); % Varianca razdalje to č ke 15 % od roja 16 M_dist = zeros ( Nr_cloud_max ,1); % Razdalje med to č kami v roju 17 E i g _ o f _ c l o u d s = zeros ( Nr_cloud_max , dimension ); 18 E i g L a t _ o f _ c l o u d = zeros ( Nr_cloud_max , dimension ); 19 P o i n t s _ i n _ b u f f e r = zeros (6 , dimension ); % Shranimo do 6 vzorcev 20 S t a r t E n d X _ p o i n t s _ i n _ c l o u d = zeros ( Nr_cloud_max ,2); 21 22 % I n i c i a l i z a c i j a 23 % Prvi roj ima tri to č ke ( č e so le - te kolinearne ) 24 N r _ p o i n t s _ i n _ c l o u d ( Current_clust ,1) = 3; 25 N r _ p o i n t s _ i n _ b u f f e r = 0; 26 27 N = 3; % Za č etno š tevilo to č k 28 XX = X (1: N ,:); 29 M = sum ( XX )/ N ; 30 31 dXX = XX - repmat (M ,N ,1); 254 Senzorji v mobilnih sistemih 32 Vmat = dXX ’* dXX /( N -1); % Kovarian č na matrika vzorcev 33 34 theta = (( Vmat (1 ,1)^2 - 2* Vmat (1 ,1)* Vmat (2 ,2) + 4* Vmat (1 ,2)^2 ... 35 + V m a t ( 2 , 2 ) ^ 2 ) ^ ( 1 / 2 ) - V m a t (1 ,1) + V m a t ( 2 , 2 ) ) / ( 2 * V m a t ( 1 , 2) ); 36 lam1 = Vmat (2 ,2) - theta * Vmat (1 ,2); 37 lam2 = Vmat (2 ,2) - theta * Vmat (1 ,2) + (1 + theta ^2)/ theta * Vmat (1 ,2); 38 if norm ( lam2 ) > norm ( lam1 ) % Izberi glavni lastni vektor 39 p = [1/ s q r t ( ( 1 + t h e t a ^2)) , t h e t a / s q r t ( ( 1 + t h e t a ^ 2 ) ) ] ; 40 else 41 p = [ t h e t a / s q r t ( ( 1 + t h e t a ^2)) , -1/ s q r t ( ( 1 + t h e t a ^ 2 ) ) ] ; 42 end 43 p = p / sqrt ( p *p ’); % Prvi lastni vektor ( n o r m a l i z i r a n ) 44 pL = [ p (1 ,2) , -p (1 ,1)]; % Drugi lastni vektor ( n o r m a l i z i r a n ) 45 46 % Razdalja od premice 47 sumD = 0; 48 for i = 1: N 49 s u m D = s u m D + ((( X ( i ,:) - M )* pL ’ ) / ( pL * pL ’ ) ) ^ 2 ; 50 end 51 varD = sumD /( N -1); 52 53 % Povpre č na razdalja med to č kami 54 d1 = sqrt ( ( X (1 ,:) - X (2 ,:))*( X (1 ,:) - X (2 ,:)) ’ ); 55 d2 = sqrt ( ( X (3 ,:) - X (2 ,:))*( X (3 ,:) - X (2 ,:)) ’ ); 56 d_med_toc = ( d1 + d2 )/2; 57 58 % S h r a n j e v a n j e 59 M_ of_ clo ud s ( Current_clust ,:) = M ; 60 V_ of_ clo ud s ( Current_clust ,:) = [ Vmat (1 ,:) , Vmat (2 ,:)]; 61 E i g _ o f _ c l o u d s ( Current_clust ,:) = p ; 62 E i g L a t _ o f _ c l o u d ( Current_clust ,:) = pL ; 63 V a r D _ o f _ c l o u d ( Current_clust ,1) = varD ; 64 M_dist ( Current_clust ,1) = d_med_toc ; 65 S t a r t E n d X _ p o i n t s _ i n _ c l o u d ( Current_clust ,1) = 1; 66 S t a r t E n d X _ p o i n t s _ i n _ c l o u d ( Current_clust ,2) = 3; 67 68 for k = 4: n % Sprehod č ez vse vzorce 69 % I z r a č un r a z d a l j e do t r e n u t n e g a v z o r c a 70 pL = E i g L a t _ o f _ c l o u d ( C u r r e n t _ c l u s t , : ) ; 71 M = M _ o f _ c l o u d s ( C u r r e n t _ c l u s t , : ) ; 72 V m a t = [ V _ o f _ c l o u d s ( C u r r e n t _ c l u s t , 1 : 2 ) ; V _ o f _ c l o u d s ( C u r r e n t _ c l u s t , 3 : 4 ) ] ; 73 v a r D = V a r D _ o f _ c l o u d ( C u r r e n t _ c l u s t , 1 ) ; 74 75 d = abs (( X ( k ,:) - M )* pL ’); 76 77 if N r _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ) > 1 78 d _ m e d _ t o c = s q r t (( X ( k ,:) - X ( k - 1 , : ) ) * ( X ( k ,:) - X ( k -1 ,:)) ’); 79 end 80 81 % N o v a to č ka p r i p a d a roju , č e je n j e n a r a z d a l j a do r o j a d o v o l j 82 % m a j h n a in so v z o r c i d o v o l j s k u p a j 83 if d < k a p p a M a x * s q r t ( v a r D ) && d _ m e d _ t o c < 2* M _ d i s t ( C u r r e n t _ c l u s t ,1) 84 S t a r t E n d X _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ,2) = k ; 85 N r _ p o i n t s _ i n _ b u f f e r = 0; 86 j = N r _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ) + 1 ; % P o v e č a n j e v z o r c e v v r o j u 87 M o l d = M _ o f _ c l o u d s ( C u r r e n t _ c l u s t , : ) ; 88 dXM = X ( k ,:) - M o l d ; 89 M = M o l d + dXM / j ; % R e k u r z i v n i i z r a č un s r e d n j e v r e d n o s t i 90 91 % K o v a r i a n č na m a t r i k a p o d a t k o v 92 V m a t = V m a t *( j - 2 ) / ( j -1) + 1/ j * dXM ’* dXM ; 93 94 t h e t a = (( V m a t (1 ,1 )^ 2 - 2* V m a t (1 ,1)* V m a t (2 ,2) + 4* V m a t ( 1 , 2) ^ 2 ... 5.3. Metode merjenja lege 255 95 + V m a t ( 2 , 2 ) ^ 2 ) ^ ( 1 / 2 ) - V m a t (1 ,1) + V m a t ( 2 , 2 ) ) / ( 2 * V m a t ( 1 , 2) ); 96 l a m 1 = V m a t (2 ,2) - t h e t a * V m a t (1 ,2); 97 l a m 2 = V m a t (2 ,2) - t h e t a * V m a t (1 ,2) + (1 + t h e t a ^ 2 ) / t h e t a * V m a t (1 ,2); 98 if n o r m ( l a m 2 ) > n o r m ( l a m 1 ) % I z b e r i g l a v n i l a s t n i v e k t o r 99 p = [1/ s q r t ( ( 1 + t h e t a ^2)) , t h e t a / s q r t ( ( 1 + t h e t a ^ 2 ) ) ] ; 100 e l s e 101 p = [ t h e t a / s q r t ( ( 1 + t h e t a ^2)) , -1/ s q r t ( ( 1 + t h e t a ^ 2 ) ) ] ; 102 end 103 p = p / s q r t ( p * p ’); % P r v i l a s t n i v e k t o r ( n o r m a l i z i r a n ) 104 pL = [ p (1 ,2) , - p ( 1 ,1 ) ]; % D r u g i l a s t n i v e k t o r ( n o r m a l i z i r a n ) 105 106 % R e k u r z i v n i i z r a č un v a r i a n c e r a z d a l j e 107 d = abs ( ( X ( k ,:) - M )* pL ’); 108 v a r D o l d = V a r D _ o f _ c l o u d ( C u r r e n t _ c l u s t , 1 ) ; 109 v a r D = v a r D o l d *( j - 2 ) / ( j - 1 ) + d ^2/ j ; 110 111 % S h r a n j e v a n j e 112 M _ o f _ c l o u d s ( C u r r e n t _ c l u s t ,:) = M ; 113 V _ o f _ c l o u d s ( C u r r e n t _ c l u s t ,:) = [ V m a t (1 ,:) , V m a t (2 , : )] ; 114 E i g _ o f _ c l o u d s ( C u r r e n t _ c l u s t ,:) = p ; 115 E i g L a t _ o f _ c l o u d ( C u r r e n t _ c l u s t ,:) = pL ; 116 N r _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ) = j ; 117 V a r D _ o f _ c l o u d ( C u r r e n t _ c l u s t ,1) = v a r D ; 118 e l s e % N o v a to č ka ne p r i p a d a r o j u - u s t v a r i m o nov roj 119 N r _ p o i n t s _ i n _ b u f f e r = N r _ p o i n t s _ i n _ b u f f e r + 1; 120 P o i n t s _ i n _ b u f f e r ( N r _ p o i n t s _ i n _ b u f f e r , :) = X ( k , : ) ; 121 122 % N o v i roj m o r a i m e t i 3 k o n s i s t e n t n e v z o r c e 123 if N r _ p o i n t s _ i n _ b u f f e r >= 3 124 XX = P o i n t s _ i n _ b u f f e r (1: N r _ p o i n t s _ i n _ b u f f e r , : ) ; 125 M = sum ( XX )/ N r _ p o i n t s _ i n _ b u f f e r ; 126 dXX = XX - r e p m a t ( M , N r _ p o i n t s _ i n _ b u f f e r , 1 ) ; 127 V m a t = dXX ’* dXX /( N r _ p o i n t s _ i n _ b u f f e r - 1 ) ; 128 129 t h e t a = (( V m a t (1 ,1 )^ 2 - 2* V m a t (1 ,1)* V m a t (2 ,2) + 4* V m a t ( 1 , 2) ^ 2 ... 130 + V m a t ( 2 , 2 ) ^ 2 ) ^ ( 1 / 2 ) - V m a t (1 ,1) + V m a t ( 2 , 2 ) ) / ( 2 * V m a t ( 1 , 2) ); 131 l a m 1 = V m a t (2 ,2) - t h e t a * V m a t (1 ,2); 132 l a m 2 = V m a t (2 ,2) - t h e t a * V m a t (1 ,2) + (1 + t h e t a ^ 2 ) / t h e t a * V m a t (1 ,2); 133 if n o r m ( l a m 2 ) > n o r m ( l a m 1 ) % I z b e r i g l a v n i l a s t n i v e k t o r 134 p = [1/ s q r t ( ( 1 + t h e t a ^2)) , t h e t a / s q r t ( ( 1 + t h e t a ^ 2 ) ) ] ; 135 e l s e 136 p = [ t h e t a / s q r t ( ( 1 + t h e t a ^2)) , -1/ s q r t ( ( 1 + t h e t a ^ 2 ) ) ] ; 137 end 138 p = p / s q r t ( p * p ’); % P r v i l a s t n i v e k t o r ( n o r m a l i z i r a n ) 139 pL = [ p (1 ,2) , - p (1 , 1 )] ; % D r u g i l a s t n i v e k t o r ( n o r m a l i z i r a n ) 140 141 % T e s t i r a n j e k o n s i s t e n t n o s t i v z o r c e v 142 s u m D = 0; 143 for i = 1: N r _ p o i n t s _ i n _ b u f f e r 144 d = abs (( XX ( i ,:) - M )* pL ’); 145 s u m D = s u m D + d ^2; 146 end 147 v a r D = s u m D /( N r _ p o i n t s _ i n _ b u f f e r - 1 ) ; 148 % P o p r a v e k , č e je za č e t n a v a r i a n c a p r e m a j h n a 149 if v a r D < m e a n ( V a r D _ o f _ c l o u d (: ,1)) 150 v a r D = m e a n ( V a r D _ o f _ c l o u d (: , 1 )) ; 151 end 152 153 d1 = s q r t (( XX (1 ,:) - XX ( 2 , : ) ) * ( XX (1 ,:) - XX (2 ,:)) ’); 154 d2 = s q r t (( XX (3 ,:) - XX ( 2 , : ) ) * ( XX (3 ,:) - XX (2 ,:)) ’); 155 156 vv1 = XX (1 ,:) - XX (2 ,:); 157 vv2 = XX (2 ,:) - XX (3 ,:); 256 Senzorji v mobilnih sistemih 158 c o s P h i = vv1 * vv2 ’/( n o r m ( vv1 )* n o r m ( vv2 )); 159 160 if d < k a p p a M a x * s q r t ( v a r D ) && abs ( d2 - d1 ) < min ( d1 , d2 ) && cosPhi > c o s P h i T h 161 % V z o r c i so k o n s i s t e n t n i in p r i m e r n o r a z m a k n j e n i 162 C u r r e n t _ c l u s t = C u r r e n t _ c l u s t + 1; 163 M _ o f _ c l o u d s ( C u r r e n t _ c l u s t ,:) = M ; 164 V _ o f _ c l o u d s ( C u r r e n t _ c l u s t ,:) = [ V m a t (1 ,:) , V m a t ( 2 , :) ]; 165 E i g _ o f _ c l o u d s ( C u r r e n t _ c l u s t ,:) = p ; 166 E i g L a t _ o f _ c l o u d ( C u r r e n t _ c l u s t ,:) = pL ; 167 N r _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ) = 3; 168 V a r D _ o f _ c l o u d ( C u r r e n t _ c l u s t ,1) = v a r D ; 169 S t a r t E n d X _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ,1) = k - 2; 170 S t a r t E n d X _ p o i n t s _ i n _ c l o u d ( C u r r e n t _ c l u s t ,2) = k ; 171 d _ m e d _ t o c = ( d1 + d2 ) / 2 ; 172 M _ d i s t ( C u r r e n t _ c l u s t ,1) = d _ m e d _ t o c ; 173 end 174 N r _ p o i n t s _ i n _ b u f f e r = 0; % B r i s a n j e m e d p o m n i l n i k a 175 e l s e % Č a k a n j e na v s a j tri v z o r c e v m e d p o m n i l n i k u 176 end 177 end 178 end 3 2 ] [my 1 0−4 −2 0 2 4 x [m] Slika 5.22: Podatki laserskega pregledovalnika razdalj in prepoznani roji premic z uporabo algoritma samorazvijajočega se rojenja premic Houghova transformacija Houghova transformacija [7] je zelo uporaben pristop za oceno geometrijskih “primitivov”, ki se večinoma uporabljajo pri obdelavi slik. Vhodni podatki se zapišejo v parametrični prostor (npr. parametri premice), kjer maksimumi podajo število in parametre premic. Algoritem zahteva kvantizacijo parametričnega prostora. Fina kvantizacija poveča točnost, vendar je računsko in spominsko zahtevna. Da bi se izognili kvantizaciji in povečali točnost Houghove transformacije, je bilo izvedenih več raziskav, npr. naključnostna Houghova transformacija ali adaptivne izvedbe, obravnavane v [8, 9]. 5.3. Metode merjenja lege 257 Houghova transformacija lahko zanesljivo oceni roje ob prisotnosti osamelcev. V osnovni različici sta parametra premice α in d definirana z linearnim prototipom (5.39), kjer je θ j = [cos α, sin α, − d]. Normalna parametra premice, ki se običajno nahajata v območju − π < α ≤ π in dmin < d ≤ dmax, sta predstavljena v akumulatorju s kvantizacijo do N diskretnih vrednosti za α in M diskretnih vrednosti za d. Za vsak vzorec podatkov x( k), y( k) in vse možne vrednosti α( n) = − π + πn , N n ∈ {1 , . . . , N }, se izračunajo rešitve parametra d( n). Vsak par α( n), d( n) predstavlja možno premico, ki vsebuje vzorec x( k), y( k). Za vsak izračunan parameter se ustrezna lokacija v akumulatorju poveča za 1. Ko so vsi vzorci podatkov obdelani, so celice akumulatorja z najvišjimi vrednostmi iskani roji premic. Za pravilno izbiro kvantizacije parametričnega prostora in vrednosti praga je potrebno nekaj predhodnega znanja, da dobimo ustrezne maksimume v akumulatorju. Primer 5.14 Za podatke laserskega pregledovalnika razdalj, sestavljenih iz 180 točk odboja (glejte sliko 5.24 in začetek rešitve za koordinate točk), ocenite roje premic, ki najbolje opišejo meritve. Rojenje se izvede s pomočjo Houghove transformacije, kjer sta normalna parametra premice α in d kvantizirana na 720 diskretnih vrednosti. Rešitev Možna rešitev v programskem okolju Matlab je predstavljena v programu 5.13 (podatki laserskega pregledovalnika razdalj so podani v programu 5.10), kjer je za iskanje maksimumov v akumulatorju uporabljena funkcija houghpeaks. Pridobljeni akumulator je prikazan na sliki 5.23, razpoznani roji pa na sliki 5.24. 258 Senzorji v mobilnih sistemih 350 300 250 200 d 150 100 50 00 100 200 300 α Slika 5.23: Akumulator Houghove transformacije, kjer sta α ∈ (− π, π] in d ∈ [0 , 4 , 5] kvantizirana na 720 diskretnih vrednosti 3 2 ] [my 1 0−4 −2 0 2 4 x [m] Slika 5.24: Podatki laserskega pregledovalnika razdalj in razpoznani roji premic z uporabo Houghove transformacije Program 5.13 ./src/sen/example_lines_hough.m 1 [x , y ]; % Meritve laserskega merilnika razdalj 2 N = length ( x ); 3 5.3. Metode merjenja lege 259 4 dAlpha = pi /180; % K v a n t i z a c i j s k i kot 5 nAlpha = round (2* pi / dAlpha ); 6 nDist = nAlpha ; % K v a n t i z a c i j s k a razdalja 7 lutDist = zeros (N , nAlpha ); % P r e s l i k o v a l n a tabela za razdaljo 8 for i = 1: N % Za vsako to č ko izra č unamo premice v obmo č ju kota alpha 9 for j = 1: n A l p h a 10 a l p h a = ( j - 1 ) * dAlpha - pi ; 11 % R a z d a l j a od k o o r d i n a t n e g a i z h o d i š č a 12 d = x ( i )* cos ( a l p h a )+ y ( i )* sin ( a l p h a ); 13 if d <0 14 if alpha > pi , a l p h a = a l p h a - pi ; 15 e l s e a l p h a = a l p h a + pi ; end 16 jj = r o u n d (( a l p h a + pi )/ d A l p h a ); 17 l u t D i s t ( i , jj ) = - d ; 18 e l s e 19 l u t D i s t ( i , j ) = d ; 20 end 21 end 22 end 23 24 % Dolo č itev obmo č ja za parameter razdalje 25 minLutDist = min ( lutDist (:)); 26 maxLutDist = max ( lutDist (:)); 27 dDist = ( maxLutDist - minLutDist )/ nDist ; % K v a n t i z a c i j s k a razdalja 28 29 % Akumulator parametri č nega prostora 30 A = zeros ( nDist , nAlpha ); 31 for i = 1: N % Sprehod č ez vse to č ke 32 for j = 1: n A l p h a 33 k = r o u n d (( l u t D i s t ( i , j ) - m i n L u t D i s t )/ d D i s t ) + 1 ; 34 if k > nDist , k = n D i s t ; end 35 A ( k , j ) = A ( k , j ) + 1 ; 36 end 37 end 38 H = A (2: nAlpha ,:); % Akumulator 39 40 % Dolo č itev maksimumov v s k u l u l a t o r j u ( najbolj verjetne premice ) 41 nLines = 7; % Š tevilo n a j v e r j e t n e j š ih premic 42 peaks = h o u g h p e a k s ( H , nLines , ’ t h r e s h o l d ’ , 3 , ’ N H o o d S i z e ’ , [31 , 3 1 ] ) ; 43 % Parametri premice : razdalja od izhodi š č a in naklon premice 44 distAlpha = [ peaks (: ,1). ’* dDist + minLutDist ; peaks (: ,2). ’* dAlpha - pi ]; Slikovne značilke Številne dobre lastnosti kamere, računske zmogljivosti sodobnih računalnikov in napredek pri razvoju algoritmov omogočajo uporabo kamere za reševanje problemov v robotiki. Kamera se lahko uporablja za zaznavanje, prepoznavanje in sledenje opazovanih objektov v vidnem polju kamere, saj so slike projekcije tridimenzionalnih objektov v okolju (glejte poglavje 5.2.4). Digitalna slika je dvodimenzionalen diskreten signal, predstavljen z matriko kvantiziranih števil, ki predstavljajo prisotnost ali odsotnost svetlobe, jakost svetlobe (osvetljenost), barvo ali kakšno drugo veličino. Glavne vrste slik so: barvna (slika 5.25a), sivinska (slika 5.25b) in binarna (slika 5.25c). Pri strojnem vidu sivinske slike običajno zadostujejo za iskanje vzorcev, ki niso odvisni od barve. Binarne slike 260 Senzorji v mobilnih sistemih (a) (b) 10000 5000 requencyF 0 0 255 Bins (c) (d) Slika 5.25: Primer (a) barvne, (b) sivinske in (c) binarne slike ter (d) histogram sivinske slike so rezultat segmentacije slike in se uporabljajo za maskiranje vsebine. Eden od najpreprostejših načinov segmentacije slike je upragovljanje, pri čemer so slikovni elementi s sivinsko vrednostjo nad pragom označeni z logično 1, vsi ostali pa so nastavljeni na logično 0. Upragovljanje sivinske slike 5.25b s pragom 70 privede do binarne slike 5.25c. Za določitev najustreznejše vrednosti praga lahko pogostost sivinskih vrednosti na sliki predstavimo v histogramu. Slika 5.25d prikazuje histogram z 256 intervali, ki ustrezajo 256 nivojem sivinske slike 5.25b. V zadnjih letih so bili razviti številni algoritmi strojnega vida, ki omogočajo sledenje objektom na podlagi slike. V ta namen je vsebina slike običajno predstavljena z značilkami slike. Značilke so lahko območja slike s podobnimi lastnostmi (npr. podobna barva), oznake z določenim vzorcem ali nekatere druge značilnosti slike (npr. robovi, vogali, črte). V določenih situacijah lahko v okolje vstavimo umetne značke, ki omogočajo hitro in zanesljivo sledenje značilkam (npr. barvne značke ali matrične črtne kode za sledenje mobilnim robotom pri nogometu). Kadar to ni mogoče, je potrebno značilke izločiti iz slike (neprilagojene) scene. V nekaterih aplikacijah je mogoče uporabiti preprosto barvno segmentacijo (npr. odkrivanje rdečih jabolk v sadovnjaku). V zadnjih letih je bilo razvitih več pristopov za izločanje naravnih lokalnih značilk slike, ki so invariantne za nekatere transformacije in popačenja slike. 5.3. Metode merjenja lege 261 R G B H S V Slika 5.26: RGB in HSV komponente barvne slike 5.25a Barvne značilke Zaznavanje območij slike s podobno barvo ni trivialna naloga zaradi nehomogene osvetlitve, senc in odsevov. Sledenje z uporabo barvnih značilk je običajno uporabljeno samo v okoljih, kjer je mogoče vzpostaviti nadzorovano osvetlitev ali pa je možno barvo predmeta, ki mu sledimo, dovolj dobro razločiti od ostalih predmetov v okolju. Barva v digitalni sliki je ponavadi predstavljena s tremi barvnimi komponentami, tj. rdečo, zeleno in modro, kar je znano kot barvni model RGB. Določeno barvo dobimo s kombinacijo teh treh barvnih komponent, kjer posamezno komponento filtriramo skozi rdeč, zelen ali moder barvni filter. Druga dva barvna prostora, ki se pogosto uporabljata pri strojnem vidu, sta HSL (angl. hue-saturation-lightness) in HSV (angl. hue-saturation-value). Barvni prostor HSV omogoča bolj naraven opis in boljšo segmentacijo barv kot pa prostor RGB. Vrednosti v območju [0 , 255] iz barvnega prostora RGB lahko pretvorimo v barvni prostor HSV z  0 ; M − m = 0     60 G− B mod 360 ; M = R H = M − m 60 B− R ; M = G  M − m    60 R− G ; M = B M − m (5.40) (0 ; M = 0 S = M − m ; sicer M M V = 255 kjer je M = max{ R, G, B} in m = min{ R, G, B}. V (5.40) se nasičenost S in vrednost V gibljeta v območju [0 , 1], barvni odtenek H pa v območju [0 , 360). Matematična operacija x mod y predstavlja ostanek pri deljenju števila x s številom y. Na sliki 5.26 so prikazane RGB in HSV komponente barvne slike 5.25a. Barvni histogrami se lahko uporabijo za segmentacijo območij določene barve. 262 Senzorji v mobilnih sistemih Modra H S V 300 150 Frekvenca 0 1 16 32 1 16 32 1 16 32 Oranžna H S V 300 150 Frekvenca 0 1 16 32 1 16 32 1 16 32 Rdeča H S V 300 150 Frekvenca 0 1 16 32 1 16 32 1 16 32 Rumena H S V 300 150 Frekvenca 0 1 16 32 1 16 32 1 16 32 Les H S V 300 150 Frekvenca 0 1 16 32 1 16 32 1 16 32 Slika 5.27: Histogrami različnih barvnih predlog v barvnem prostoru HSV 5.3. Metode merjenja lege 263 HSV histogrami (z 32 intervali) različnih predlog s podobno barvo so prikazani na sliki 5.27 (barvna področja so vzeta s slike 5.25a). Histograme vsake predloge je mogoče projicirati nazaj na izvirno sliko, saj je za vsak slikovni element na sliki nastavljena frekvenca intervala, ki pripada vrednosti slikovnega elementa v ustreznem barvnem kanalu na sliki. Nastale sivinske slike lahko združimo kot linearno kombinacijo barvnih kanalov. Rezultati povratne projekcije HSV-histograma modre, oranžne, rdeče, rumene barve ter lesa so prikazani v prvem stolpcu na sliki 5.28. V dobljeni sliki sivinski nivoji predstavljajo merilo podobnosti slikovnih elementov barvni predlogi. Preden je slika upragovljena (glejte tretji stolpec na 5.28), je mogoče dodatno filtrirati sliko. Sliko lahko zgladimo z dvodimenzio-nalnim Gaussovim filtrom (glejte drugi stolpec na 5.28), da odstranimo nekaj šumnih vrhov. Upragovljeno binarno sliko lahko filtriramo z npr. morfološkim filtrom, da odstranimo ali zapolnimo nekatera povezana območja. Nastalo masko slike (glejte četrti stolpec na sliki 5.28) lahko uporabimo za filtriranje zaznanih območij na prvotni sliki (glejte zadnji stolpec na sliki 5.28). Več o algoritmih za obdelavo slik je med drugim na voljo v [10]. V praktičnih izvedbah segmentacije slike na podlagi barv se nekateri procesi filtriranja zavoljo hitrejšega delovanja preskočijo, kar pa zmanjša točnost. V primeru, da so barvne vrednosti vseh slikovnih komponent znotraj določene spodnje in zgornje meje, se vsak slikovni element na vhodni sliki šteje za del predmeta. Na sliki 5.27 lahko opazimo, da vrednosti barvnega odtenka in nasičenosti omogočata preprosto barvno segmentacijo. Vendar je potrebno poudariti, da je barvna segmentacija občutljiva na spreminjajoče se svetlobne pogoje v prostoru. Čeprav nekateri pristopi lahko kompenzirajo nehomogene pogoje osvetlitve [11], se barvna segmentacija slike najpogosteje uporablja le v okoliščinah, kjer je mogoče doseči ustrezne pogoje osvetlitve. Rezultat segmentacije slike je binarna slika, v kateri so slikovni elementi, ki ne pripadajo objektu (ozadje), postavljeni na 0. Obstaja lahko več ali samo eno povezano območje. Če obstaja več povezanih območij, je potrebno uporabiti ustrezen algoritem, da najdemo pozicije in oblike teh objektov [11]. Za bolj robustno iskanje povezanih območij lahko uvedemo nekatere omejitve, ki zavrnejo območja glede na določen pogoj (npr. velikost območja). Če se opazovan objekt bistveno razlikuje od okolice in ga je mogoče zanesljivo zaznati kot eno območje na segmentirani sliki, lahko pozicijo in obliko tega območja opišemo s slikovnimi momenti. Definicija osnovnega slikovnega momenta binarne digitalne slike I( x, y) ∈ {0 , 1} je X X mp,q = xpyqI( x, y) x y kjer sta p in q pozitivni celi števili ( p + q je red momenta). Moment m 0 , 0 predstavlja maso objekta, ki je v primeru binarne slike enaka njegovi površini. Da v binarni sliki najdemo središče objekta ( x 0 , y 0), lahko uporabimo momente ničtega in prvega reda m 10 m 01 x 0 = y 0 = m 00 m 00 264 Senzorji v mobilnih sistemih Slika 5.28: Detekcija barvnih značilk. Vsaka vrstica predstavlja drugačno barvo. Koraki od levega do desnega stolpca: povratna projekcija histograma barvne predloge, glajenje, upragovljanje, dodatno filtriranje in maskiranje slike Za opis orientacije in oblike objekta lahko uporabimo središčne momente X X µp,q = ( x − x 0) p( y − y 0) qI( x, y) x y ki so invariantni za transformacije v sliki. Središčne momente lahko uporabimo za prilagoditev elipsoida na zaznani objekt na sliki. Veliko polos elipse a in malo polos b lahko dobimo iz lastnih vrednosti λa in λb ( λa ≥ λb) matrike Q, ki je sestavljena iz središčnih momentov drugega reda " # µ Q = 2 , 0 µ 1 , 1 µ 1 , 1 µ 0 , 2 Osi elipse sta √ √ 2 λa 2 λb a = b = m 00 m 00 Orientacijo velike polosi elipse podaja kot 1 2 µ 1 , 1 θ = arctan 2 µ 2 , 0 − µ 0 , 2 Na sliki 5.29 so uporabljeni osnovni in središčni momenti, da se elipsoid prilagodi na binarno oznako (rezultat segmentacije oranžne barve na sliki 5.28). Način za pridobitev parametrov elipsoida je prikazan v programu 5.14, kjer so središčni momenti izpeljani iz osnovnih. Vidimo, da lahko momente slike uporabimo za opis preprostih značilk in določitev njihovih lokacij na sliki. V [12] je opredeljena množica momentov, ki so invariantni za skaliranje, translacijo in rotacijo. 5.3. Metode merjenja lege 265 m1,0 0 m0,0 640 0 m0,1 m0,0 b θ a 480 Slika 5.29: Prilagoditev elipsoida na masko, ki je pridobljena kot rezultat se- gmentacije oranžne barve Program 5.14 ./src/sen/example_moments.m 1 im = imread ( ’ c o l o u r _ o r a n g e _ m a s k . bmp ’ ) >128; % Binarna slika 2 3 [x , y ] = meshgrid (1: size ( im ,2) , 1: size ( im ,1)); 4 5 % Osnovni momenti 6 m00 = sum ( sum ( ( x .^0).*( y .^0).* double ( im ) )); 7 m10 = sum ( sum ( ( x .^1).*( y .^0).* double ( im ) )); 8 m01 = sum ( sum ( ( x .^0).*( y .^1).* double ( im ) )); 9 m11 = sum ( sum ( ( x .^1).*( y .^1).* double ( im ) )); 10 m20 = sum ( sum ( ( x .^2).*( y .^0).* double ( im ) )); 11 m02 = sum ( sum ( ( x .^0).*( y .^2).* double ( im ) )); 12 13 % Povr š ina , x in y 14 area = m00 15 x0 = m10 / m00 16 y0 = m01 / m00 17 18 % Centralni momenti 19 u00 = m00 ; 20 u11 = m11 - x0 * m01 ; % u11 = m11 - y * m10 ; 21 u20 = m20 - x0 * m10 ; 22 u02 = m02 - y0 * m01 ; 23 24 % Elipsa 25 v = eig ([ u20 , u11 ; u11 , u02 ]); % Lastne vrednosti 26 a = 2* sqrt ( v (2)/ u00 ) % Velika polos 27 b = 2* sqrt ( v (1)/ u00 ) % Mala polos 28 theta = atan2 (2* u11 , u20 - u02 )/2 % Usm erj eno st a r e a = 1 4 9 5 8 x0 = 266 Senzorji v mobilnih sistemih Slika 5.30: Umetne značke ID2 ID2 ID3 ID3 ID1 ID1 Slika 5.31: Zaznane umetne značke v dveh pogledih kamere 2 4 8 . 1 9 5 1 y0 = 3 5 9 . 7 1 0 1 a = 7 1 . 7 6 8 4 b = 6 6 . 6 5 8 5 t h e t a = 0 . 8 7 9 5 Značke z umetnimi vzorci Uvedba umetnih značk predstavlja minimalno prilagoditev okolja, ki lahko znatno poenostavi nekatere naloge strojnega vida, npr. sledenje objektom, ocena lege kamere itd. Trije primeri umetnih značk so prikazani na sliki 5.30. Vzorci značk so običajno zasnovani tako, da jih je mogoče zanesljivo in natančno zaznati. Poleg tega je v vzorcu lahko zapisana oznaka (ID) značke, zato je mogoče sledenje in prepoznava več značk v zaporedju slik. Eden od priljubljenih algoritmov za zaznavanje umetnih značk je ArUco [13, 14]. Slika 5.31 prikazuje zaznane umetne značke iz slike 5.30 v dveh pogledih kamere. Algoritmi za zaznavanje umetnih značk običajno obsegajo korak lokalizacije značke, v katerem se določi pozicija, orientacija in velikost značke. Ta postopek mora robustno razločiti samo prave značke od preostalih objektov v okolju. Ko so lokacije značk na sliki znane, je mogoče določiti projekcijo med zaznano značko in njenim lokalnim koordinatnim sistemom. Na podlagi vzorca značke v transformiranem lokalnem koordinatnem sistemu se določi njena oznaka (ID). 5.3. Metode merjenja lege 267 Nekateri vzorci umetnih značk so zasnovani tako, da omogočajo popravljanje vzorcev, zaradi česar je njihovo prepoznavanje bolj zanesljivo in hkrati tudi robustno na šum in okluzije. Naravne lokalne slikovne značilke Slike vsebujejo značilke, ki so same po sebi prisotne v okolju. Skozi leta so bili razviti številni algoritmi strojnega vida, ki omogočajo samodejno zaznavanje lokalnih značilk na slikah. Nekateri pomembni algoritmi za detekcijo lokalnih značilk so SIFT (angl. scale invariant feature transform) [15], SURF (angl. speeded-up robust features) [16], MSER (angl. maximally stable extremal regions) [17], FAST (angl. features from accelerated segment test) [18], AGAST (angl. adaptive and generic accelerated segment test) [19] idr. Večina teh algoritmov je vključena v odprtokodno knjižnico za računalniški vid OpenCV [20, 21]. V robotskih aplikacijah je pomembna lastnost opisovanja slik z značilkami ter učinkovitost algoritma, da omogoča delovanje v realnem času. Možni so različni pristopi za detekcijo lokalnih značilk (lokalizacija značilk), opis lokalnih značilk (predstavitev lastnosti značilk) in ujemanje značilk. Cilj detekcije značilk je odkrivanje in lokalizacija točk zanimanja (lokalna območja), običajno za zmanjšanje dimenzije slike. Običajno značilke detektiramo na sivinskih slikah. Obstaja več vrst slikovnih vzorcev: robovi (npr. Cannyev filter, Sobelov operator in Robertsov operator), vogali (npr. Harrisov detektor oglišč, Hessianova matrika, FAST), mehurčki (npr. Laplaceov operator z Gaussovim filtrom, razlika Gaussovih filtrov, MSER). Za zaznavanje značilk različnih velikosti, je slika običajno predstavljena v ločljivostnem prostoru [15]. Detekcija značilk mora biti invariantna na nekatere vrste transformacij in popačenj slike, da je možno ponovljivo odkrivanje značilk v več pogledih istega prizora. Običajno je zaželeno, da so značilke invariantne za translacijo, rotacijo, skaliranje, glajenje, osvetlitev in šum. Značilke bi morale biti robustne tudi za nekatere delne okluzije. Zaželena lastnost značilk je torej lokalnost. V mobilni robotiki je običajno zahtevana točna zaznava zadosti velikega števila značilk, saj je to predpogoj za točno in robustno izvajanje algoritmov, ki so odvisni od detektiranih značilk (npr. ocena lege mobilnega robota na podlagi značilk). Primer zaznanih značilk v dveh pogledih kamere istega prizora je prikazan na sliki 5.32. Vsaka značilka je določena s pozicijo, orientacijo in velikostjo. Namen opisa značilke je opisati vsako značilko glede na lastnosti slikovnega vzorca okoli nje na način, ki omogoča prepoznavo istih značilk na več slikah (če se te značilke ponovno pojavijo na drugih slikah). Lokalni vzorec okoli značilke (npr. območje, označeno s kvadratom na sliki 5.32) se uporablja za določitev njenega ustreznega deskriptorja, ki je običajno predstavljen kot vektor značilke. Lokalni deskriptorji značilk morajo biti razpoznavni na način, da je možno točno razpoznati značilke ne glede na spremembe v okolju (npr. spremembe osvetlitve, okluzije). Čeprav je mogoče lokalne slikovne značilke primerjati neposredno z npr. konvolucijo območij lokalnih značilk, ta pristop nima ustrezne izrazitosti pa tudi računsko je zelo zahteven. Vektorji deskriptorjev značilk ponavadi predstavljajo minimalno predstavitev značilk, ki omogoča ustrezno raven izrazitosti določene 268 Senzorji v mobilnih sistemih Slika 5.32: Zaznane značilke na dveh slikah istega prizora značilke za zanesljivo ujemanje z drugimi. Kateri detektor in način opisa značilke se uporabljata, je odvisno od posamezne aplikacije. Eksperimentalno primerjavo različnih detektorjev značilk lahko najdete v npr. [22]. Primer dveh sklopov vektorjev opisov značilk, izločene iz leve in desne slike na 5.32, je grafično predstavljen na sliki 5.33. Ujemanje značilk je eden osnovnih problemov v strojnem vidu. Številni algoritmi strojnega vida, kot je ocenjevanje globine prizora, tridimenzionalna rekonstrukcija scene na podlagi slik, ocena lege kamere in drugi, so odvisni od ustreznih ujemanj značilk med več slikami. Značilke se lahko ujemajo s primerjavo razdalj med deskriptorji značilk (vektorji). Odvisno od vrste deskriptorja se lahko uporabijo različne mere razdalje. Za dejanske vrednosti dekriptorjev se običajno uporablja Evklidska razdalja ali razdalja Manhattan, za binarne deskriptorje značilk pa Hammingova razdalja. Ujemanje opisov značilk ne zagotavlja vedno ustreznih ujemanj zaradi nenatančne lokalizacije značilk, njihove popačitve ter ponavljajočih se vzorcev. Zato je potrebno uporabiti ustrezno tehniko ujemanja značilk, ki lahko odpravi zmotne pare. Glede na dve množici deskriptorjev značilk A = { ai} i=1 , 2 ,...,N in B = { b A j } j=1 , 2 ,...,NB mora postopek ujemanja najti ustrezne (ujemajoče se) pare značilk iz obeh množic. Za vsako značilko v množici A je lahko najbližja značilka v množici B (glede na izbrano mero razdalje za primerjavo med značilkami) možni kandidat ujemanja. To je v splošnem surjektivna preslikava, saj ima lahko neka značilka iz množice B več kot samo eno ujemajočo se značilko v množici A. V normalnih pogojih mora imeti funkcija iz ene množice samo eno ujemanje v drugi množici. Zato običajno izvedemo dvosmerno iskanje, kjer za vsako značilko iz množice A poiščemo naj- bližjo značilko iz množice B in obratno, ter upoštevamo samo pare, ki se ujemajo v obeh smereh. Če je razdalja med najboljšim in drugim najboljšim ujemanjem premajhna (pod izbranim pragom), moramo par značilk zavrniti, saj obstaja velika verjetnost napake pri ujemanju. Prav tako zavrnemo par značilk, če je razdalja med izbranimi deskriptorji značilk nad določenim pragom, ker to pomeni preveliko neenakost značilk. Na sliki 5.33 so prikazana ujemanja med dvema množicama deskriptorjev značilk. Čeprav so bile uporabljene zgoraj omenjene tehnike filtriranja, niso odstranjena vsa napačna ujemanja, kot je razvidno iz slik 5.3. Metode merjenja lege 269 Slika 5.33: Dvosmerna ujemanja med deskriptorji značilk leve in desne slike na sliki 5.32. Vsak vodoravni trak predstavlja vrednosti opisnega vektorja s 64 elementi, ki je predstavljen kot vzorec z različnimi stopnjami intenzitete sive barve. 270 Senzorji v mobilnih sistemih T T T T F F T T T T T T T F T T T T F T T T F F T T Slika 5.34: Najdeni pari značilk, ki temeljijo na ujemanju deskriptorjev (glejte sliko 5.33). Večina parov značilk je pravih ( T ), nekateri pa so tudi zmotni ( F ). 5.34 in 5.35. Tudi če je samo nekaj osamelcev, lahko ti pomembno vplivajo na rezultate ocenjevalnih algoritmov, ki domnevajo, da se značilke pravilno ujemajo. Nekatere ujemajoče se značilke ne bodo ustrezale sistemskim omejitvam; npr. če par značilk ne izpolnjuje epipolarne omejitve ali če se rekonstruirana tridimenzionalna točka od para značilk pojavi za kamero, moramo par zavrniti. Več omejitev lahko najdete v [23]. V kolesni mobilni robotiki se lahko uvedejo dodatne omejitve, osnovane na predvidenih stanjih robota iz znanih dejanj in kinematičnega modela. Za filtriranje ujemajočih se parov značilk, ki niso fizično možni, lahko v postopek ujemanja vključimo model nekaterih geometrijskih omejitev. V ta namen je potrebno uporabiti robusten postopek prilagajanja modela. Običajno se uporablja metoda RANSAC (angl. random sample consensus) [24], ki omogoča Slika 5.35: Pari ujemajočih se točk 5.3. Metode merjenja lege 271 prileganje modela podatkom, čeprav je veliko osamelcev. Algoritem RANSAC lahko pojasnimo na problemu prilagajanja premice. V začetnem koraku se iz podatkov naključno izbere najmanjše število Nmin točk, ki so potrebne za prilagoditev modela. V primeru premice sta potrebni le dve točki ( Nmin = 2). Model (premica) se nato namesti na izbrani točki. Okoli nastavljenega modela (premice) je izbrano neko območje zaupanja, znotraj katerega se prešteje število vseh podatkovnih točk. Nato se med podatki izbere drugih Nmin točk in postopek se ponovi. Po nekaj iteracijah je za najboljši model izbran model z največjim številom točk znotraj območja zaupanja. V zadnjem koraku se model ponovno prilagodi na vse točke v ustreznem območju zaupanja po metodi najmanjših kvadratov. 5.3.5 Ujemanje modelov okolja — zemljevidi Zemljevid je predstavitev okolja na osnovi nekaterih parametrov značilk, ki so lahko točke odboja laserskega pregledovalnika razdalj, množica daljic, ki predstavljajo robove ovir, množica slikovnih značilk, ki predstavljajo predmete v okolju in podobno. Problem lokalizacije lahko predstavimo kot optimalno ujemanje dveh zemljevidov: lokalnega in globalnega zemljevida. Lokalni zemljevid, ki ga dobimo iz trenutnih meritev senzorjev, predstavlja del okolja, ki ga je mogoče neposredno opazovati (meriti) iz trenutne lege robota (npr. trenutne točke laserskega pregledovalnika razdalj). Globalni zemljevid je shranjen v notranjem pomnilniku mobilnega sistema in predstavlja znano ali že obiskano območje v okolju. S primerjavo obeh zemljevidov lahko določimo ali izboljšamo trenutno oceno lege mobilnega robota. Ko se mobilni sistem giblje v okolju, se lahko globalni zemljevid razširja in sproti posodablja z lokalnim zemljevidom tako, da se trenutne informacije senzorjev, ki predstavljajo prej neodkrita mesta, dodajo globalnemu zemljevidu. Pristop, ki omogoča tovrstno sprotno gradnjo zemljevida, je SLAM (angl. simultaneous localization and mapping). Njegovi osnovni koraki so opisani v algoritmu 2. Algorithm 2 Osnovni koraki algoritma SLAM Določanje ujemanja lokalnega in globalnega zemljevida na podlagi njunih ujemajočih se parov značilk. Ocena trenutne lege mobilnega robota na podlagi ujemanja lokalnega in glo- balnega zemljevida. Lokalizacija na osnovi kombinacije odometrije in primerjave zemljevidov. Posodobitev globalnega zemljevida s prej neopaženim delom okolja (nove značilke so dodane kot nova stanja na zemljevidu). Primer zemljevida, pridobljen s kombinacijo meritev laserskega pregledovalnika razdalj in podatkov odometrije z algoritmom SLAM, je prikazan na sliki 5.36. SLAM je eden od osnovnih pristopov v kolesni mobilni robotiki. Nekateri pogosti 272 Senzorji v mobilnih sistemih Soba 3 Soba 2B Soba 2A Soba 1 Hodnik Slika 5.36: Zemljevid notranjega okolja, zgrajen iz meritev laserskega pregledovalnika razdalj in podatkov odometrije izzivi pri lokalizaciji mobilnih robotov so: • Inicializacije lege mobilnega robota ob zagonu ni mogoče določiti z veliko verjetnostjo. Če začetna lega ni znana, moramo rešiti problem globalne lokalizacije. • Problem ugrabljenega robota se pojavi, ko se med delovanjem mobil- nega robota njegova (resnična) lega v okolju hipno spremeni (npr. mobilni robot je prestavljen na novo lokacijo ali pa ponovno vklopljen na drugi lokaciji). Robustni algoritmi lokalizacije so sposobni obnoviti in oceniti pravo lego robota. • Zagotavljanje ustrezne ocene lege med gibanjem mobilnega robota. V ta namen se uporabljajo podatki odometrije in meritve absolutnih sen- zorjev. Najpogosteje uporabljeni algoritmi za reševanje problema SLAM so razširjeni Kalmanov filter, Bayesov filter in filter delcev (glejte poglavje 6). 5.4 Senzorji V nadaljevanju je podan kratek pregled najpogosteje uporabljenih senzorjev v kolesni mobilni robotiki, njihovih značilnostih in razvrstitev. 5.4.1 Karakteristike senzorjev Delovanje senzorjev, njihova kakovost in lastnosti so določene z različnimi karak-teristikami. Najpogostejše so opisane v nadaljevanju. 5.4. Senzorji 273 Območje določa zgornjo mejo ( ymax) in spodnjo mejo ( ymin) uporabljene veli- čine, ki jo je mogoče izmeriti. Zgornja in spodnja meja ponavadi nista simetrični. Senzorje moramo uporabljati v navedenem območju, saj lahko v nasprotnem primeru pride do poškodbe senzorja. Dinamično območje je skupno območje od najnižje do največje vrednosti območja. Lahko je podano kot razlika Rdyn = ymax − ymin ali pogosteje kot razmerje (v decibelih) Rdyn = A log ymax , kjer je A = 10 za meritve, povezane z ymin močjo, in A = 20 za ostale. Ločljivost je najmanjša sprememba merjene veličine, ki jo senzor še zazna. Če ima senzor analogno-digitalni pretvornik, je ločljivost senzorja običajno enaka ločljivosti pretvornika (npr. za 10-bitni A/D in 5 V senzorsko območje je resolucija 5 V 210 ). Občutljivost je sprememba izhodne vrednosti senzorja na enoto veličine, ki jo merimo (npr. senzor razdalje, ki ima na izhodu napetost). Občutljivost je lahko konstantna v celotnem območju senzorja (linearnost) ali pa se spreminja (nelinearnost). Linearnost je lastnost senzorja, kjer je njegov izhod linearno odvisen (proporcionalen) od merjene veličine v celotnem območju. Linearni senzor ima konstantno občutljivost v celotnem območju. Histereza se nanaša na lastnost, da je izhodna trajektorija senzorja (ali njena odvisnost od vhoda) drugačna v primeru, ko se vhod senzorja povečuje ali zmanjšuje. Pasovna širina se nanaša na frekvenco, s katero lahko senzor zagotavlja meritve (v Hz). Je najvišja frekvenca, pri kateri izmerimo samo 70 , 7 % prave vrednosti. Točnost je določena s pričakovanim merilnim pogreškom, ki je razlika med izmerjeno m in pravo vrednostjo v. Točnost izračunamo iz relativnega merilnega pogreška kot točnost = 1 − | m− v| . v Natančnost je stopnja ponovljivosti meritve senzorja pri enaki pravi vrednosti merjene veličine. Pri večkratni meritvi iste prave vrednosti, izhod realnega senzorja poda obseg vrednosti. Natančnost je povezana z varianco meritve. Sistematični pogrešek ali deterministični pogrešek povzročajo nekateri dejavniki, ki jih je mogoče napovedati ali modelirati (pristranskost, temperaturno lezenje, kalibracija senzorja, popačenje zaradi leče kamere itd.). Naključni pogrešek ali nedeterministični pogrešek je nepredvidljiv, kar lahko opišemo le z gostoto verjetnosti (npr. normalna porazdelitev). Ta pogrešek se običajno imenuje šum in je označen kot razmerje signal-šum (SNR, angl. signal-to-noise ratio). 274 Senzorji v mobilnih sistemih 5.4.2 Klasifikacija senzorjev Kolesni mobilni robot lahko meri svoje notranje stanje s pomočjo proprioceptivnih senzorjev ali zunanje stanje okolja z uporabo eksteroceptivnih senzorjev. Primer proprioceptivne meritve je pozicija in orientacija robota, zasuk koles ali krmilnega mehanizma, kotna hitrost koles, stanje akumulatorjev, temperatura ipd. Primer eksteroceptivne meritve pa je razdalja do ovire, slikovni zajem s kamero, mikrofon, kompas, globalni pozicijski sistem (GPS) in drugi. Senzorji za zaznavanje okolja se uporabljajo za načrtovanje poti, zaznavanje ovir, gradnjo zemljevida itd. Ti senzorji so aktivni, če v okolje oddajajo energijo (elektromagnetno valovanje) in meritve prejmejo odziv okolja (laserski merilniki razdalj, ultrazvočni merilniki, kamera z integrirano osvetlitvijo itd.). Senzorji so pasivni, če prejmejo energijo, ki je že del okolja. Pasivni senzorji so torej vsi, ki niso aktivni (kamera brez osvetlitve, kompas, žiroskop, pospeškometer itd.). V tabeli 5.1 so navedeni najpogosteje uporabljeni senzorji v mobilni robotiki glede na njihovo uporabo. Dodatno je podan kratek opis njihove uporabe, namen (proprioceptivni – PC ali eksteroceptivni – EC) in emitirana energija (aktivna – A ali pasivna – P). 5.4. Senzorji 275 Tabela 5.1: Klasifikacija senzorjev v mobilni robotiki glede na njihovo uporabo, namen (proprioceptivni (PC) / eksteroceptivni (EC)) in oddano energijo (aktivna (A) / pasivna (P)) Klasifikacija Uporabnost Senzorji PC/EC A/P Taktilni detekcija trkov, kontaktna stikala EC P in haptični varnostni izklop, taktilni odbijači EC P senzorji bližina, rotacija optične bariere EC A koles ali motorja merilniki bližine EC P/A kontaktne letve EC P Osni rotacija koles inkrementalni enkoderji PC A senzorji ali motorja, absolutni enkoderji PC A orientacija sklepov, potenciometer PC P lokalizacija z tahogenerator PC P odometrijo Merilniki orientacija v žiroskop PC P smeri referenčnem k. s., magnetometer EC P gibanja lokalizacija, kompas EC P inercialna navigacija inklinometer EC P Merilniki inercialna navigacija pospeškometer EC P hitrosti dopplerjev radar EC A kamera EC P Oddajniki sledenje objektu, IR-oddajnik EC A lokalizacija WiFi-oddajnik EC A RF-oddajnik EC A ultrazvočni oddajnik EC A GPS EC A/P Merilniki merjenje razdalje ultrazvočni senzor EC A na osnovi do ovire, laserski merilnik razdalj EC A časa časa preleta, kamera EC P/A lokalizacija Merilniki identifikacija, kamera EC P/A na osnovi detekcija objektov, globinska kamera EC A strojnega sledenje objektom, stereo kamera EC P/A vida lokalizacija, RFID EC A segmentacija radar EC A optična triangulacija EC A 276 Senzorji v mobilnih sistemih Literatura [1] J. R. Wertz. Spacecraft Attitude Determination and Control. D. Reidel Publishing Company, Dordrecht, 1978, 858 str. [2] F. Chaumette in S. Hutchinson. Visual servo control, part I: basic approaches. IEEE Robotics and Automation Magazine, zv. 13, št. 4, str. 82–90, 2006. [3] J. M. Font-Llagunes in J. A. Batlle. Consistent triangulation for mobile robot localization using discontinuous angular measurements. Robotics and Autonomous Systems, zv. 57, št. 9, str. 931–942, 2009. [4] V. Nguyen, S. Gächter in sod. A comparison of line extraction algorithms using 2D range data for indoor mobile robotics. Autonomous Robots, zv. 23, št. 2, str. 97–111, 2007. [5] T. Pavlidis in S. L. Horowitz. Segmentation of plane curves. IEEE Transactions on Computers, zv. 23, št. 8, str. 860–870, 1974. [6] G. Klančar in I. Škrjanc. Evolving principal component clustering with a low run-time complexity for LRF data mapping. Applied soft computing, zv. 35, str. 349–358, 2015. [7] D. H. Ballard. Generalizing the hough transform to detect arbitrary shapes. Pattern Recognition, zv. 13, št. 2, str. 111–122, 1981. [8] L. Xu in E. Oja. Randomized hough transform (RHT): Basic mechanisms, algorithms, and computational complexities. CVGIP: Image Understanding, zv. 57, št. 2, str. 131–154, 1993. [9] J. Basak in A. Das. Hough transform network: Learning conoidal structures in a connec-tionist framework. IEEE Transactions on Neural Networks, zv. 13, št. 2, str. 381–392, 2002. [10] D. Forsyth in J. Ponce. Computer vision: a modern approach. Prentice Hall, illustrate izd., 2003, 693 str. [11] G. Klančar, M. Kristan in sod. Robust and efficient vision system for group of cooperating mobile robots with application to soccer robots. ISA transactions, zv. 43, št. 3, str. 329–342, 2004. [12] P. I. Corke. Visual control of robots: high-performance visual servoing. Wiley, New York, 1996, 353 str. [13] S. Garrido-Jurado, R. Munoz-Salinas in sod. Automatic generation and detection of highly reliable fiducial markers under occlusion. Pattern Recognition, zv. 47, št. 6, str. 2280–2292, 2014. [14] S. Garrido-Jurado, R. Munoz-Salinas in sod. Generation of fiducial marker dictionaries using mixed integer linear programming. Pattern Recognition, zv. 51, str. 481–491, 2016. [15] D. G. Lowe. Distinctive image features from scale-invariant keypoints. International Journal of Computer Vision, zv. 60, št. 2, str. 91–110, 2004. [16] H. Bay, A. Ess in sod. Speeded-up robust features (SURF). Computer Vision and Image Understanding, zv. 110, št. 3, str. 346–359, 2008. [17] J. Matas, O. Chum in sod. Robust wide-baseline stereo from maximally stable extremal regions. Image and Vision Computing, zv. 22, št. 10, str. 761–767, 2004. [18] E. Rosten in T. Drummond. Fusing points and lines for high performance tracking. Tenth IEEE International Conference on Computer Vision (ICCV’05) Volume 1, zv. 2, str. 1508–1515, 2005. [19] E. Mair, G. D. Hager in sod. Adaptive and Generic Corner Detection Based on the Literatura 277 Accelerated Segment Test. V Computer Vision — ECCV 2010, str. 183–196. Springer Berlin Heidelberg, 2010. [20] The OpenCV reference manual: release 2.4.3, 2012. [21] G. Bradski in A. Kaehler. Learning OpenCV. O?Reilly, 1. izd., 2008, 555 str. [22] K. Mikolajczyk in C. Schmid. A performance evaluation of local descriptors. IEEE Transactions on Pattern Analysis and Machine Intelligence, zv. 27, št. 10, str. 1615–1630, 2005. [23] B. Cyganek in J. P. Siebert. An introduction to 3D computer vision techniques and algorithms. John Wiley & Sons, Chichester, UK, 2009, 483 str. [24] M. A. Fischler in R. C. Bolles. Random sample consensus: a paradigm for model fitting with applicatlons to image analysis and automated cartography. Communications of the ACM, zv. 24, št. 6, str. 381–395, 1981. 6 Nedeterminističnost v mobilnih sistemih 6.1 Uvod V realnem svetu so deterministični dogodki zelo redki (skoraj ne obstajajo). Poglejmo si nekaj primerov. Predstavljajmo si primer merjenja hitrosti vozila. Nikoli ne moremo z absolutno natančnostjo določiti prave hitrosti vozila, saj imajo merilniki določeno negotovost (občutljivost, končna ločljivost, omejeno merilno območje, fizikalne omejitve ipd.). Dodatno so meritve senzorjev podvržene šumu in motnjam iz okolice, učinkovitost senzorja se običajno sčasoma spremeni ter senzorji lahko odpovejo ali se pokvarijo. Vsi ti dejavniki omejujejo koristno informacijo (o veličini, ki jo merimo). Do podobnih zaključkov lahko pridemo tudi pri aktuatorjih. V primeru motornega pogona zaradi šuma, trenja, obrabe, neznane obremenitve ali mehanske okvare ni mogoče določiti prave kotne hitrosti pri danem vzbujanju. Poleg tega so nekateri algoritmi sami po sebi negotovi. Običajno dajo algoritmi rezultat z omejeno natančnostjo, ki je zadovoljiva za dani problem, saj je potrebno rezultat pridobiti v določenem časovnem roku. Sistemi v mobilni robotiki pogosto delujejo v realnem času, kjer je hitrost računanja pomembnejša od absolutne natančnosti. Zaradi omejene procesne zmogljivosti številni algoritmi niso izvedljivi v realnem času z želenimi odzivnimi časi brez zmanjšanja natančnosti. Tudi delovno okolje mobilnih agentov (kolesnih mobilnih robotov) je negotovo. Negotovost je običajno nižja v strukturiranih okoljih (npr. stavbe pravilnih oblik) 280 Nedeterminističnost v mobilnih sistemih in višja v časovno spremenljivih dinamičnih okoljih (npr. domovi, cestni promet, bližina ljudi, gozdovi itd.). Pri razvoju avtonomnega kolesnega mobilnega sistema moramo upoštevati pro- bleme negotovosti in jih uspešno rešiti. 6.2 Osnove verjetnosti Naj bo X slučajna spremenljivka in x vrednost, ki jo lahko X zavzame. Če je prostor vzorcev slučajne spremenljivke X množica s končnim številom vrednosti (npr. metanje kovanca ima le dva možna izida), je X diskretna slučajna spremenljivka. V primeru, da je njen prostor vzorcev množica realnih števil (npr. teža kovanca), je X zvezna slučajna spremenljivka. V tem poglavju je podan kratek pregled osnovnih konceptov verjetnostnega računa. Poglobljeno obravnavo tematike najdete v številnih učbenikih o statistiki (npr. [1]). 6.2.1 Diskretna slučajna spremenljivka Diskretna slučajna spremenljivka X ima končni ali števni prostor vzorcev, ki vsebuje vse možne vrednosti slučajne spremenljivke X. Verjetnost, da diskretna slučajna spremenljivka X zavzame vrednost x, je P ( X = x) ali zapisano krajše P ( x) = P ( X = x). Zaloga vrednosti verjetnostne funkcije P ( x) je znotraj omejenega intervala med 0 in 1 za vsako vrednost v prostoru vzorcev X, zapišemo P ( x) ∈ [0 , 1] ∀ x ∈ X. Vsota verjetnosti vseh možnih vrednosti slučajne spremenljivke X je enaka X P ( x) = 1 (6.1) x∈ X Verjetnost dveh (ali več) dogodkov, ki se pojavljajo skupaj (npr. slučajna spremenljivka X zavzame vrednost x in slučajna spremenljivka Y zavzame vrednost y), je opisana z verjetnostjo produkta P ( x, y) = P ( X = x, Y = y) (6.2) Če sta slučajni spremenljivki X in Y neodvisni, je verjetnost produkta (6.2) preprosto produkt posameznih verjetnosti P ( x, y) = P ( x) P ( y) Pogojna verjetnost P ( x| y) podaja verjetnost, da slučajna spremenljivka X zavzame vrednost x, če je predhodno znano, da ima slučajna spremenljivka Y 6.2. Osnove verjetnosti 281 0.4 1/n P(x) P(x) 0.2 ... 0 x 0 1 x2 x3 x4 x1 x2 xn x x (a) (b) Slika 6.1: (a) Primer diskretne porazdelitve in (b) enakomerna diskretna porazdelitev (vsota višin vseh stolpcev je enaka 1) vrednost y. Če je P ( y) > 0, lahko določimo pogojno verjetnost z P ( x, y) P ( x| y) = (6.3) P ( y) V primeru neodvisnih slučajnih spremenljivk X in Y je izračun (6.3) trivialen P ( x, y) P ( x) P ( y) P ( x| y) = = = P ( x) P ( y) P ( y) Eden od temeljnih rezultatov verjetnostnega računa je teorem popolne verje- tnosti X P ( x) = P ( x, y) (6.4) y∈ Y V primeru, da je na voljo pogojna verjetnost, lahko teorem (6.4) podamo v drugi obliki X P ( x) = P ( x| y) P ( y) = p T ( x| Y )p( Y ) (6.5) y∈ Y V (6.5) smo uvedli diskretno porazdelitev, ki je definirana kot stolpični vektor p( Y ) h i T p( Y ) = P ( y 1) P ( y 2) . . . P ( yN ) kjer je N ∈ N število vseh možnih stanj slučajne spremenljivke Y . Podobno je stolpični vektor p( x| Y ) definiran kot h i T p( x| Y ) = P ( x| y 1) P ( x| y 2) . . . P ( x| yN ) Diskretne porazdelitve se lahko prikažejo s histogrami (slika 6.1). V skladu z (6.1) je vsota višin vseh stolpcev v histogramu enaka 1. Če so vsa stanja slučajne spremenljivke enako verjetna, lahko slučajno spremenljivko opišemo z enakomerno porazdelitvijo (slika 6.1b). 282 Nedeterminističnost v mobilnih sistemih p(m) 1/d p(x) p(x) P(X < a) P(X < a) 0 0 0 m a µ −d/2 µ a µ +d/2 x x (a) (b) Slika 6.2: (a) Primer gostote verjetnosti in (b) enakomerna gostota verjetnosti (integral površine pod krivuljo je enak 1) 6.2.2 Zvezna slučajna spremenljivka Območje zvezne slučajne spremenljivke je (bodisi končen bodisi neskončen) interval realnih števil. V zveznem primeru velja P ( X = x) = 0, ker ima zvezna slučajna spremenljivka X neskončni prostor vzorcev. Zato je uvedena gostota verjetnosti p( x) (angl. probability density function), ki ima omejeno območje med 0 in 1, torej je p( x) ∈ [0 , 1]. Verjetnost, da zvezna slučajna spremenljivke X zavzame vrednost manjšo od a je a Z P ( X < a) = p( x) d x −∞ Primera gostote verjetnosti sta prikazana na sliki 6.2. Podobno kot velja relacija (6.1) za diskretno slučajno spremenljivko, je integral gostote verjetnosti celotnega prostora vzorcev zvezne slučajne spremenljivke X enak +∞ Z P (−∞ < X < +∞) = p( x) d x = 1 −∞ Podobna razmerja, ki veljajo za diskretno slučajno spremenljivko (predstavljena v poglavju 6.2.1), se lahko razširijo tudi na gostoto verjetnosti. Nekatere pomembne relacije za diskretne in zvezne slučajne spremenljivke so vzporedno prikazane v tabeli 6.1. Porazdelitve slučajne spremenljivke so pogosto opisane z različnimi statističnimi parametri. Srednja vrednost zvezne slučajne spremenljivke X je določena kot matematično upanje +∞ Z µX = E{ X} = xp( x) d x −∞ 6.2. Osnove verjetnosti 283 Tabela 6.1: Izbrane enačbe verjetnostnega računa za diskretno in zvezno slučajno spremenljivko Opis Diskretna slučajna spr. Zvezna slučajna spr. +∞ Z X Polna verjetnost P ( x) = 1 p( x) d x = 1 x∈ X −∞ +∞ Z X Teorem polne verje- P ( x) = P ( x, y) p( x) = p( x, y) d y tnosti y∈ Y −∞ +∞ Z X P ( x) = P ( x| y) P ( y) p( x) = p( x| y) p( y) d y y∈ Y −∞ +∞ Z X Srednja vrednost µX = xP ( x) µX = xp( x) d x x∈ X −∞ +∞ Z X Varianca σ 2 = ( x − µ = ( x − µ X X )2 P ( x) σ 2 X X )2 p( x) d x x∈ X −∞ Eden od osnovnih parametrov, ki opisujejo obliko porazdelitve, je varianca +∞ Z σ 2 = var{ X} = E( X − E{ X})2 = ( x − µ X X )2 p( x) d x −∞ Ti lastnosti je mogoče določiti tudi za diskretne slučajne spremenljivke in so podane v tabeli 6.1. Srednja vrednost µ in varianca σ 2 sta edina parametra, ki sta potrebna za enoličen zapis normalne porazdelitve (slika 6.3). Normalna porazdelitev je ena izmed najpomembnejših gostot verjetnosti in je predstavljena z Gaussovo funkcijo 1 ( x− µ)2 p( x) = √ e− 12 σ 2 2 πσ 2 V večdimenzionalnem primeru, ko je slučajna spremenljivka vektor x, ima normalna porazdelitev naslednjo obliko − 1 p(x) = det (2 πΣ) 2 e− 12 (x−µ) T Σ−1(x−µ) kjer je Σ kovariančna matrika. Primer dvodimenzionalne Gaussove funkcije je prikazan na sliki 6.4. Kovariančna matrika je simetrična — element v vrstici i in stolpcu j je kovarianca cov{ Xi, Xj} med slučajnima spremenljivkama Xi in Xj. Kovarianca cov{ X, Y } je merilo linearnega razmerja med slučajnima spremenljivkama X in Y σ 2 = cov{ X, Y } = E{( X − µ XY X )( Y − µY )} = +∞ +∞ Z Z (6.6) = ( X − µX )( Y − µY ) p( x, y) d x d y −∞ −∞ 284 Nedeterminističnost v mobilnih sistemih p( µ) p(x) P( µ − σ < X < µ + σ) ≈ 0.68 0 µ − σ µ µ + σ x Slika 6.3: Normalna porazdelitev (gostota verjetnosti) p( µ X, µ Y ) p(x, y) 0 µ Y + σ Y µ X + σ X µ Y µ X µ Y − σ Y µ y X − σ X x Slika 6.4: Dvodimenzionalna Gaussova gostota verjetnosti kjer p( x, y) predstavlja gostoto verjetnosti produkta X in Y . Relacijo (6.6) lahko poenostavimo na σ 2 = E{ XY } − µ XY X µY (6.7) Če sta slučajni spremenljivki X in Y neodvisni, velja E{ XY } = E{ X}E{ Y } in kovarianca (6.7) ima vrednost 0: σ 2 = 0. Obratno ne velja — če je kovarianca XY nič, to ne pomeni, da sta slučajni spremenljivki neodvisni. Kovarianca dveh enakih slučajnih spremenljivk je varianca cov{ X, X} = var{ X} = σ 2 . X 6.2. Osnove verjetnosti 285 6.2.3 Bayesovo pravilo Bayesovo pravilo je eden od temeljnih stebrov teorije verjetnosti in ima veliko uporabno vrednost v mobilni robotiki. V zveznem prostoru je podano kot p( y| x) p( x) p( x| y) = (6.8) p( y) in za diskreten prostor kot P ( y| x) P ( x) P ( x| y) = (6.9) P ( y) Bayesovo pravilo omogoča, da izračunamo težko določljivo verjetnost na podlagi verjetnosti, ki jo je lažje določiti. Primer 6.1 Predstavimo uvodni primer uporabe Bayesovega pravila (6.8) v mobilnih sistemih. Naj slučajna spremenljivka X predstavlja stanje mobilnega sistema, ki ga želimo oceniti (npr. razdalja mobilnega sistema od ovire) na podlagi meritve Y , ki je stohastično odvisna od slučajnega stanja X. Ker je izid meritve negotov, želimo izvedeti, kakšna je gostota verjetnosti ocenjenega stanja X = x na podlagi meritve Y = y. Rešitev Zanima nas gostota verjetnosti p( x| y), ki jo lahko izračunamo iz (6.8). Gostota verjetnosti p( x) vsebuje znanje o slučajni spremenljivki X pred opravljeno meritvijo y, zato jo imenujemo napovedna ocena (angl. a-priori estimate). Pogojna gostota verjetnosti p( x| y) podaja znanje o slučajni spremenljivki X po opravljeni meritvi in je zato znana tudi kot trenutna ocena (angl. a posteriori estimate). Gostota verjetnosti p( y| x) vsebuje informacijo o vplivu stanja X na meritev Y , zato predstavlja model senzorja (npr. porazdelitev merjenja razdalje do ovire, če je mobilni robot na določeni razdalji od ovire). Gostota verjetnosti p( y) vsebuje porazdelitev meritve y, torej zaupanje v opravljeno meritev, in jo lahko določimo s teoremom popolne verjetnosti p( y) = R p( y| x) p( x) d x. Zato lahko trenutno oceno p( x| y) pridobimo s pomočjo znanega statističnega modela senzorja ( p( y| x)) in napovedne ocene (gostote verjetnosti stanja p( x)). Primer 6.2 Do ciljne lokacije vodijo tri različne poti. Mobilni sistem izbere prvo pot v sedmih od desetih primerov, drugo pot samo v enem od desetih, tretjo pa v enem od 286 Nedeterminističnost v mobilnih sistemih petih primerov. Na prvi poti ima 5 %, na drugi 10 % in na tretji 8 % verjetnost naleta na oviro. 1. Kakšna je verjetnost, da mobilni sistem na poti do cilja naleti na oviro? 2. Mobilni sistem je naletel na oviro. Kakšna je verjetnost, da se je to zgodilo na prvi poti? 6.2. Osnove verjetnosti 287 Rešitev Označimo prvo, drugo in tretjo pot z A 1, A 2 in A 3; oviro na katerikoli poti pa z B. Verjetnost, da izberemo določeno pot je: P ( A 1) = 0 , 7, P ( A 2) = 0 , 1, P ( A 3) = 0 , 2. Verjetnost naleta na oviro na prvi poti je P ( B| A 1) = 0 , 05, na drugi poti P ( B| A 2) = 0 , 1 in na tretji poti P ( B| A 3) = 0 , 08. 1. Verjetnost, da mobilni sistem naleti na oviro, je P ( B) = P ( B| A 1) P ( A 1) + P ( B| A 2) P ( A 2) + P ( B| A 3) P ( A 3) = = 0 , 05 · 0 , 7 + 0 , 1 · 0 , 1 + 0 , 08 · 0 , 2 = = 0 , 061 2. Verjetnost, da mobilni sistem obtiči na prvi poti, lahko izračunamo s pomočjo Bayesovega pravila (6.9): P ( B| A 1) P ( A 1) P ( A 1| B) = = P ( B) P ( B| A 1) P ( A 1) = = P ( B| A 1) P ( A 1) + P ( B| A 2) P ( A 2) + P ( B| A 3) P ( A 3) 0 , 05 · 0 , 7 = = 0 , 05 · 0 , 7 + 0 , 1 · 0 , 1 + 0 , 08 · 0 , 2 = 0 , 5738 Z Matlab kodo v programu 6.1 lahko izračunamo verjetnosti, da mobilni robot obtiči na katerikoli poti. Program 6.1 ./src/prb/example_three_paths.m 1 % Verjetnost izbire prve , druge ali tretje poti : 2 % p ( A ) = [ P ( A1 ) , P ( A2 ) , P ( A3 )] 3 p_A = [0.7 0.1 0.2] 4 % Verjetnost ovire na prvi , drugi in tretji poti : 5 % p ( B | A ) = [ P ( B | A1 ) , P ( B | A2 ) , P ( B | A3 )] 6 p_BA = [0.05 0.1 0.08] 7 288 Nedeterminističnost v mobilnih sistemih 8 % Verjetnost ovire : P ( B ) 9 P_B = p_BA * p_A . ’ 10 11 % Verjetnost zastoja na prvi , drugi in tretji poti : 12 % p ( A | B ) = [ P ( A1 | B ) , P ( A2 | B ) , P ( A3 | B )] 13 p_AB = ( p_BA .* p_A )./ P_B p_A = 0 . 7 0 0 0 0 . 1 0 0 0 0 . 2 0 0 0 p _ B A = 0 . 0 5 0 0 0 . 1 0 0 0 0 . 0 8 0 0 P_B = 0 . 0 6 1 0 p _ A B = 0 . 5 7 3 8 0 . 1 6 3 9 0 . 2 6 2 3 Primer 6.3 Mobilni robot za čiščenje tal je opremljen s senzorjem za zaznavanje umazanije, ki lahko zaznava čistočo tal pod mobilnim robotom (slika 6.5). Čistilne krtače Senzor za detekcijo umazanije Sesalna enota Kolo Koš za smeti Mobilni robot za čiščenje tal [pogled od spodaj navzgor] Slika 6.5: Mobilni robot za čiščenje tal Na podlagi odčitkov senzorja želimo ugotoviti, ali so tla pod robotom čista ali ne, zato ima diskretna slučajna spremenljivka dve možni vrednosti. Verjetnost, da so tla čista, je 40 %. O senzorju za zaznavanje umazanije lahko trdimo naslednje: če so tla čista, senzor to pravilno zazna v 80 % primerov; in če so tla umazana, senzor to pravilno zazna v 90 % primerov. V primeru čistih tal je verjetnost nepravilne meritve majhna, saj bo senzor napačno določil stanje tal v enem od petih primerov. V primeru umazanih tal je verjetnost nepravilne meritve še manjša, saj bo senzor napačno zaznal samo v enem od desetih primerov. Kakšna 6.2. Osnove verjetnosti 289 je verjetnost, da so tla čista, če senzor zazna, da so le-ta čista? Rešitev Označimo stanje tal — čista ( clean) ali umazana ( dirty) — z diskretno slu- čajno spremenljivko X ∈ { clean, dirty} in meritev senzorja s slučajno spremenljivko Z ∈ { clean, dirty}. Verjetnost, da so tla čista, torej zapišemo kot P ( X = clean) = 0 , 4 in model meritve senzorja lahko matematično predstavimo kot P ( Z = clean| X = clean) = 0 , 8 P ( Z = dirty| X = dirty) = 0 , 9 Uvedimo krajši zapis P ( x) = P ( X = clean) = 0 , 4 P (¯ x) = P ( X = dirty) = 1 − P ( x) = 0 , 6 P ( z| x) = P ( Z = clean| X = clean) = 0 , 8 P (¯ z| x) = P ( Z = dirty| X = clean) = 1 − P ( z| x) = 0 , 2 P (¯ z|¯ x) = P ( Z = dirty| X = dirty) = 0 , 9 P ( z|¯ x) = P ( Z = clean| X = dirty) = 1 − P (¯ z|¯ x) = 0 , 1 S pomočjo Bayesovega pravila (6.9) določimo P ( x| z) P ( z| x) P ( x) P ( x| z) = P ( z) Nadalje lahko s pomočjo teorema popolne verjetnosti izračunamo verjetnost, da senzor zazna tla kot čista P ( z) = P ( z| x) P ( x) + P ( z|¯ x) P (¯ x) = = 0 , 8 · 0 , 4 + 0 , 1 · 0 , 6 = = 0 , 38 Tako je iskana verjetnost enaka 0 , 8 · 0 , 4 P ( x| z) = = 0 , 8421 0 , 38 Dobljena verjetnost, da senzor zazna čista tla kot čista, je visoka. Vendar pa obstaja več kot 15 % verjetnost, da so tla dejansko umazana, zato lahko mobilni sistem napačno domneva, da čiščenje ni potrebno. Če mobilni robot v takšnem primeru ne izvede čiščenja, ostanejo tla umazana. 290 Nedeterminističnost v mobilnih sistemih 6.3 Ocenjevanje stanj Ocenjevanje stanja je proces, v katerem so prava stanja sistema ocenjena na podlagi izmerjenih podatkov in predhodnega znanja o sistemu. Tudi če je mo- žno stanja sistema neposredno izmeriti, izmerjeni podatki običajno vsebujejo šum in druge motnje. Zaradi tega so neobdelani izmerjeni podatki običajno neprimerni za nadaljnjo uporabo brez ustreznega filtriranja (npr. neposredna uporaba pri izračunu pogreška vodenja). V številnih primerih je možno oceniti stanja sistema, tudi če stanja niso neposredno merljiva. To lahko izvedemo, če je sistem spoznaven. Zato moramo pred ocenjevanjem stanj preveriti spoznavnost sistema. Najpomembnejši lastnosti algoritmov za ocenjevanje stanja sta: ocena konvergence in ocenjevalna pristranskost. V tem poglavju bomo opisali nekaj praktičnih vidikov, ki jih je potrebno upoštevati pred izvedbo določenega algoritma za ocenjevanje stanj. 6.3.1 Motnje in šum Vso neupoštevano sistemsko dinamiko — kot so nemerljivi signali in pogreški modeliranja — lahko razumemo kot sistemske motnje. Pod predpostavko linear- nosti lahko vse motnje predstavimo v enem samem členu n( t), ki ga prištejemo pravemu signalu y 0( t) y( t) = y 0( t) + n( t) Motnje razvrstimo v več razredov: visokofrekvenčni kvazistacionarni stohastični signali (npr. merilni šum), nizkofrekvenčni nestacionarni signali (npr. lezenje), periodični signali ali kakšen drug tip signalov (npr. špice, osamelci). Eden od najpomembnejših stohastičnih signalov je beli šum. Frekvenčni spekter in porazdelitev signala sta najpomembnejši lastnosti, ki opisujeta signal. Porazdelitev signala poda verjetnost, s katero amplituda zavzame določeno vrednost. Najpogostejši porazdelitvi signalov sta enakomerna porazdelitev in Gaussova (normalna) porazdelitev (glejte poglavje 6.2). Frekvenčni spekter signala predstavlja soodvisnost signala v vsakem trenutku, ki je povezan s porazdelitvijo frekvenčnih komponent signala. V primeru belega šuma so vse frekvenčne komponente enakomerno porazdeljene, zato je vrednost signala v vsakem časovnem trenutku neodvisna od prejšnjih vrednosti signala. 6.3.2 Ocena konvergence in pristranskosti Kot smo že omenili, ocenjevanje stanj podaja ocene notranjih stanj glede na izmerjene vhodno-izhodne signale, razmerje med spremenljivkami (modela sis- tema), nekatere statistične lastnosti signalov (npr. varianca) in druge informacije o sistemu. Vse te podatke je treba združiti tako, da dobimo točno in natančno oceno notranjih stanj. Na žalost so meritve, model in vnaprej znane lastnosti 6.3. Ocenjevanje stanj 291 signalov same po sebi negotove zaradi šuma, motenj, parazitske dinamike, napač- nih predpostavk o modelu sistema in drugih virov napak. Zato se ocene stanj običajno razlikujejo od dejanskih stanj. Vse zgoraj omenjene težave povzročajo določeno stopnjo negotovosti signalov. Matematično lahko problem rešimo v stohastičnem okolju, kjer se signali obrav- navajo kot slučajne spremenljivke. Signali so tako predstavljeni s pripadajočimi gostotami verjetnosti ali pa s srednjo vrednostjo in varianco. V stohastičnem okolju je pomembno vprašanje kakovost določene ocene stanja. Zlasti je potrebno analizirati konvergenco ocene stanja glede na pravo vrednost. Zastaviti si moramo dve pomembni vprašanji: 1. Ali je matematično upanje ocene enako pravi vrednosti? Če to drži, je ocena nepristranska. Ocena je konsistentna, če se izboljša s časom (večji interval opazovanja) in konvergira k pravi vrednosti s podaljševanjem intervala opazovanja proti neskončnosti. 2. Ali varianca pogreška ocene konvergira proti 0 , ko gre čas opazovanja proti neskončnosti? Če to drži in je ocena konsistentna, je ocena konsistentna v srednjekvadratični vrednosti. To pomeni, da se z naraščanjem časa opazovanja točnost in natančnost ocen izboljšuje (vse ocene so v bližini prave vrednosti). Na zgornji vprašanji lahko razmeroma enostavno odgovorimo, če so predpostavke o sistemu preproste (popoln model sistema, Gaussov šum itd.). Pri obravnavi zahtevnejšega problema je zelo težko najti odgovore in analitične rešitve niso več možne. Vendar se moramo zavedati, da ocenjevalniki stanj podajajo zadovoljive rezultate, če niso kršene nekatere pomembne predpostavke. Zato je izjemno pomembno, da pred uporabo določenega algoritma ocenjevanja stanj preberemo drobni tisk, saj lahko v nasprotnem primeru algoritem poda ocene, ki so daleč od dejanskih stanj. Problem je, da se tega ponavadi sploh ne zavedamo. 6.3.3 Spoznavnost Stanja, ki jih je potrebno oceniti, so običajno skrita, saj navadno informacije o njih niso neposredno dostopne. Stanja lahko ocenimo na podlagi meritev izhodov sistema, ki so neposredno ali posredno odvisni od stanj. Preden izvedemo algoritem ocenjevanja, moramo odgovoriti na naslednje vprašanje: Ali lahko enolično ocenimo stanja v končnem času na podlagi opazovanja izhodov sistema? Odgovor na to vprašanje podaja spoznavnost sistema. Če je sistem spoznaven, lahko ocenimo stanja z opazovanjem njegovih izhodov. Sistem je le delno spoznaven, če lahko ocenimo samo podmnožico stanj sistema, pri čemer preostalih stanj morda ne bomo ocenili. V primeru popolnoma spoznavnega sistema so vsa stanja in izhodi povezani. 292 Nedeterminističnost v mobilnih sistemih Preden začnemo z opredelitvijo spoznavnosti, moramo vpeljati koncept neraz- poznavnih stanj (angl. indistinguishable state). Predstavljajmo si splošen nelinearen sistem v obliki (x ∈ n m l R , u ∈ R in y ∈ R ) ˙ x( t) = f (x( t) , u( t)) (6.10) y( t) = h(x( t)) Stanji x0 in x1 sta nerazpoznavni, če za vsak dopusten vhod u( t) v končnem časovnem intervalu t ∈ [ t 0 , t 1] dobimo enake izhode [2] y( t, x0) ≡ y( t, x1) ∀ t ∈ [ t 0 , t 1] Množica vseh nerazpoznavnih stanj iz stanja x0 je označena kot I(x0). Sedaj lahko rečemo, da je sistem spoznaven pri x0, če množica nerazpoznavnih stanj I(x0) vsebuje samo stanje x0, torej I(x0) = {x0}. Sistem je spoznaven, če množica nerazpoznavnih stanj I(x) vsebuje samo stanje x za vsako stanje x v definicijskem območju, torej I(x) = {x} ∀ x. Spoznavnost ne pomeni, da je ocena stanja x opazovanega izhoda možna za vsak vhod u( t), t ∈ [ t 0 , t 1]. V določenih primerih je za razlikovanje med stanji potreben dolg čas opazovanja. Poznamo različne načine spoznavnosti [2]: lokalna spoznavnost ali močnejši koncept spoznavnosti, šibka spoznavnost ali oslabljeni koncept spoznavnosti in lokalna šibka spoznavnost. V primeru avtonomnih linearnih sistemov so vse tri oblike spoznavnosti enakovredne. Preverjanje spoznavnosti splošnih nelinearnih sistemov (6.10) zahteva napredno matematično analizo. Lokalno šibko spoznavnost sistema lahko preverimo s preprostim algebraičnim testom. V ta namen vpeljemo operator Liejev odvod kot časovni odvod izhodne funkcije h vzdolž trajektorije sistema x ∂ h(x) L f [h(x)] = f (x) ∂ x Vzemimo avtonomen sistem (6.10) (u( t) = 0 za vsak t). Lokalno šibko spoznavnost preverimo tako, da (večkrat) odvajamo sistemski izhod y, dokler se ne poveča rang matrike Q (definicija sledi v nadaljevanju) y = h(x) = L 0 [h(x)] f ∂ h(x) dx ∂ h(x) ˙ y = = f (x) = L f [h(x)] = L 1 [h(x)] ∂ x d t ∂ x f ∂ ∂ h(x) ¨ y = f (x) f (x) = L f [ L f [h(x)]] = L 2 [h(x)] ∂ x ∂ x f (6.11) ... d i y = Li [h(x)] d ti f 6.4. Bayesov filter 293 Časovne odvode izhoda sistema (6.11) lahko zapišemo v matriko L(x)  L 0 [h(x)] f  L 1 [h(x)] f   L(x) =  L 2 [h(x)]  f  (6.12)  .   ..    Li [h(x)] f Rang matrike Q(x0) = ∂ L(x)| ∂ x x0 določa lokalno šibko spoznavnost sistema pri x0. Če je rang matrike Q(x0) enak številu stanj, torej rang(Q(x0)) = n, naj bi sistem zadostoval spoznavnostnemu pogoju za rang pri x0, ki je zadosten, ne pa potreben pogoj za lokalno šibko spoznavnost sistema pri x0. Če je za vsak x iz definicijskega območja izpolnjen spoznavnostni pogoj za rang, je sistem lokalno šibko spoznaven. Bolj podrobna študija na temo spoznavnosti je na voljo v [2–4]. Pogoj za rang spoznavnosti se lahko poenostavi za časovno nespremenljive linearne sisteme. Za sistem z n stanji v obliki ˙ x( t) = Ax( t) + Bu( t), y( t) = Cx( t) so Liejevi odvodi (6.12) L 0 [h(x)] = Cx( t) f L 1 [h(x)] = C(Ax( t) + Bu( t)) f (6.13) L 2 [h(x)] = CA(Ax( t) + Bu( t)) f ... Delno odvajanje Liejevih odvodov (6.13) privede do Kalmanove spoznavnostne matrike Q h i Q T = C T A T C T . . . (A T ) n−1C T Sistem je spoznaven, če ima spoznavnostna matrika n neodvisnih vrstic, torej je rang spoznavnostne matrike enak številu stanj rang(Q) = n 6.4 Bayesov filter 6.4.1 Markovove verige Obravnavali bomo sisteme, za katere lahko predpostavimo, da je stanje vse- bovano. To pomeni, da se vse informacije o sistemu v danem trenutku lahko pridobijo iz stanj sistema. Sistem lahko opišemo na podlagi trenutnih stanj, kar je lastnost Markovovega procesa. Na sliki 6.6 je prikazan skriti Markovov proces, kjer stanja niso neposredno dostopna, ampak jih lahko samo ocenimo iz meritev, 294 Nedeterminističnost v mobilnih sistemih x x k- 1 k zk- 1 zk Slika 6.6: Skriti Markovov proces. Meritev zk je stohastično odvisna od trenutnega stanja xk, ki ni neposredno dostopno in je odvisno od prejšnjega stanja xk−1 x x k- 1 k u u k- 2 k-1 zk- 1 zk Slika 6.7: Skriti Markovov proces z zunanjimi vhodi. Meritev zk je stohastično odvisna od trenutnega stanja xk. Stanje xk ni neposredno dostopno ter je odvisno od prejšnjega stanja xk−1 in trenutno aktualnega vhoda uk−1. ki so stohastično odvisne od trenutnih vrednosti stanj. Zaradi tega je trenutno stanje sistema odvisno le od prejšnjega stanja in ne celotne zgodovine stanj p( xk| x 0 , . . . , xk−1) = p( xk| xk−1) Podobno je tudi meritev neodvisna od celotne zgodovine stanj sistema, če je znano le trenutno stanje p( zk| x 0 , . . . , xk) = p( zk| xk) Struktura skritega Markovovega procesa, kjer je trenutno stanje xk odvisno od prejšnjega stanja xk−1 in vhoda sistema uk−1, je prikazana na sliki 6.7. Vhod uk−1 je trenutno aktualen vhod, ki vpliva na notranja stanja od trenutka k − 1 do k. 6.4.2 Ocenjevanje stanj iz opazovanj Bayesov filter predstavlja najbolj splošen algoritem za izračun porazdelitve. Bayesov filter je močno statistično orodje, ki ga lahko uporabimo za oceno lokacije (stanj sistema) ob prisotnosti sistemskih in merilnih negotovosti [5]. Porazdelitev stanja po merjenju p( x| z) lahko ocenimo, če sta znana statistični model senzorja p( z| x) (porazdelitev izida meritve ob poznanem stanju) in porazdelitev meritve p( z). To smo predstavili v primeru 6.3 za diskretno slučajno spremenljivko. 6.4. Bayesov filter 295 Poglejmo si primer ocenjevanja stanja x, ko imamo več zaporednih meritev z. Želimo oceniti porazdelitev p( xk| z 1 , . . . , zk) stanja x v trenutku k, pri čemer upoštevamo zaporedje vseh meritev do aktualnega trenutka. Bayesovo formulo lahko podamo v rekurzivni obliki p( zk| xk, z 1 , . . . , zk−1) p( xk| z 1 , . . . , zk−1) p( xk| z 1 , . . . , zk) = p( zk| z 1 , . . . , zk−1) kar lahko krajše zapišemo kot p( zk| xk, z 1: k−1) p( xk| z 1: k−1) p( xk| z 1: k) = (6.14) p( zk| z 1: k−1) Pomen posameznih členov v (6.14) je: • p( xk| z 1: k) je ocenjena porazdelitev stanja v trenutku k, posodobljenega z merilnimi podatki, • p( zk| xk, z 1: k−1) je porazdelitev meritve v trenutku k, če poznamo trenutno stanje xk in pretekle meritve do trenutka k − 1, • p( xk| z 1: k−1) je napovedana porazdelitev stanja na osnovi preteklih meritev, • p( zk| z 1: k−1) je porazdelitev opravljene meritve (zaupanje v opravljeno meritev) v trenutku k. Nadalje velja, da je trenutna meritev zk v (6.14) neodvisna od preteklih meritev z 1: k−1 (stanje je vsebovano, Markovov proces), če poznamo stanje sistema xk p( zk| xk, z 1: k−1) = p( zk| xk) Zato se enačba (6.14) poenostavi v p( zk| xk) p( xk| z 1: k−1) p( xk| z 1: k) = (6.15) p( zk| z 1: k−1) Izpeljava enačbe (6.15) je podana v primeru 6.4. Primer 6.4 Za vajo izpeljite (6.15) iz (6.14), pri čemer predpostavite vsebovano stanje ( p( zk| xk, z 1: k−1) = p( zk| xk)). 296 Nedeterminističnost v mobilnih sistemih Rešitev Izpeljava porazdelitve (6.15) p( z 1: k| xk) p( xk) p( xk| z 1: k) = = p( z 1: k) p( zk, z 1: k−1| xk) p( xk) = = p( zk, z 1: k−1) p( zk| z 1: k−1 , xk) p( z 1: k−1| xk) p( xk) = = p( zk| z 1: k−1) p( z 1: k−1) p( zk| z 1: k−1 , xk) p( xk| z 1: k−1) p( z 1: k−1) p( xk) = = p( zk| z 1: k−1) p( z 1: k−1) p( xk) p( zk| z 1: k−1 , xk) p( xk| z 1: k−1) = = p( zk| z 1: k−1) p( zk| xk) p( xk| z 1: k−1) = p( zk| z 1: k−1) Rekurzivna enačba (6.15) za posodobitev stanja na podlagi preteklih meritev vsebuje tudi predikcijo p( xk| z 1: k−1). Postopek ocenjevanja stanja lahko razdelimo na predikcijski in korekcijski korak. Predikcijski korak Predikcijo p( xk| z 1: k−1) določimo kot Z p( xk| z 1: k−1) = p( xk| xk−1 , z 1: k−1) p( xk−1| z 1: k−1) d xk−1 kar lahko poenostavimo s predpostavko, da je stanje vsebovano Z p( xk| z 1: k−1) = p( xk| xk−1) p( xk−1| z 1: k−1) d xk−1 (6.16) kjer p( xk| xk−1) predstavlja porazdelitev prehajanja med stanji in p( xk−1| z 1: k−1) je popravljena porazdelitev ocenjenega stanja iz prejšnjega časovnega trenutka. Korekcijski korak Ocenjena porazdelitev stanj po opravljeni meritvi v trenutku k in izračunana napovedana porazdelitev stanj v predikcijskem koraku je p( zk| xk) p( xk| z 1: k−1) p( xk| z 1: k) = (6.17) p( zk| z 1: k−1) Verjetnost p( zk| z 1: k−1), ki opisuje merilno zaupanje, lahko določimo s pomočjo relacije Z p( zk| z 1: k−1) = p( zk| xk) p( xk| z 1: k−1) d xk 6.4. Bayesov filter 297 Najverjetnejša ocena stanj Kako lahko ocenjeno porazdelitev p( xk| z 1: k) uporabimo pri oceni najverjetnejšega stanja xk? Najbolj verjetna ocena stanja (matematično upanje) E{ xk} je podana kot vrednost, ki minimizira povprečni kvadratni pogrešek meritve Z E{ xk} = xkp( xk| z 1: k) d xk Ocenimo lahko tudi vrednost xk , ki maksimira trenutno verjetnost p( x max k | z 1: k ) xk = max p( x max k | z 1: k ) xk Primer 6.5 Imamo senzor iz primera 6.3. Kakšna je verjetnost čistih tal, če senzor v trenutku k = 2 ponovno zazna čista tla? Rešitev V primeru 6.3 so bila v trenutku k = 1 tla čista, senzor pa je tudi zaznal, da so čista ( z 1 = clean). Izračunali smo pogojno verjetnost 0 , 8 · 0 , 4 P ( x 1| z 1) = = 0 , 8421 0 , 38 V naslednjem trenutku k = 2 senzor vrne z 2 = clean z verjetnostjo pravilne zaznave senzorja P ( z 2| x 2) = 0 , 8 in z verjetnostjo napačne zaznave senzorja P ( z 2|¯ x 2) = 0 , 1 (predpostavljamo časovno nespremenljivo karakteristiko senzorja). Najprej ovrednotimo predvideno verjetnost P ( x 2| z 1), torej da so tla umazana glede na prejšnjo meritev. V enačbi (6.16) zamenjamo integracijo z vsoto in dobimo X P ( xk| z 1: k−1) = P ( xk| xk−1) P ( xk−1| z 1: k−1) xk−1∈ X Za zdaj predpostavimo, da mobilni sistem lahko le zazna stanje tal in ne more vplivati nanj, torej ne izvaja čiščenja tal in tal tudi ne more umazati. Tako je verjetnost prehajanja stanja preprosto P ( x 2| x 1) = 1 in P ( x 2|¯ x 1) = 0. Dobimo torej P ( x 2| z 1:1) = P ( x 2| x 1) P ( x 1| z 1) + P ( x 2|¯ x 1) P (¯ x 1| z 1) = = 1 · 0 , 8421 + 0 · 0 , 1579 = = 0 , 8421 kar je logičen rezultat. Če namreč nimamo meritve z 2, nismo pridobili novih informacij o sistemu. Zato ima stanje tal enako verjetnost v trenutku k = 2 kot v prejšnjem trenutku k = 1. 298 Nedeterminističnost v mobilnih sistemih Z upoštevanjem razmerja (6.17) lahko v korekcijskem koraku združimo meritev s trenutno oceno P ( z 2| x 2) P ( x 2| z 1:1) P ( x 2| z 1:2) = = P ( z 2| z 1:1) 0 , 8 · 0 , 8421 = P( z 2| z 1:1) kjer moramo izračunati vrednost normirnega člena, ki podaja verjetnost čistih tal v trenutku k = 2. Faktor lahko določimo z vsoto vseh možnih kombinacij stanj, ki vodijo do trenutne meritve zk ob upoštevanju izidov preteklih meritev z 1: k−1 X P ( zk| z 1: k−1) = P ( zk| xk) P ( xk| z 1: k−1) xk∈ X V našem primeru zapišemo P ( z 2| z 1) = P ( z 2| x 2) P ( x 2| z 1) + P ( z 2|¯ x 2) P (¯ x 2| z 1) = = 0 , 8 · 0 , 8421 + 0 , 1 · 0 , 1579 = = 0 , 6895 Verjetnost, da so tla čista, če je senzor dvakrat zapored zaznal čista tla, je P ( x 2| z 1:2) = 0 , 9771 Primer 6.6 Za primer 6.3 določite, kako se spremeni verjetnost stanja, če so tla čista, senzor pa opravi tri meritve z 1:3 = ( clean, clean, dirty). Rešitev Prvi dve pogojni verjetnosti smo že izračunali v primeru 6.3 in 6.5, tretja pa je P ( z 3| x 3) P ( x 3| z 1:2) P ( x 3| z 1:3) = = P ( z 3| z 1:2) 0 , 2 · 0 , 9771 = = 0 , 2 · 0 , 9771 + 0 , 9 · (1 − 0 , 9771) = 0 , 9046 kjer je P ( Z = dirty| X = clean) = 1 − P ( Z = clean| X = clean). Verjetnosti stanj v treh zaporednih meritvah, opravljenih v časovnih trenutkih i = 1 , 2 , 3, so P ( xk| z 1: i) = (0 , 8421 , 0 , 9771 , 0 , 9046) 6.4. Bayesov filter 299 Porazdelitev trenutnega stanja za trenutke k = 1 , 2 , 3 je prikazana na sliki 6.8. Implementacija rešitve v programskem okolju Matlab je prikazana v programu 6.2. Program 6.2 ./src/prb/example_nocleaning.m 1 % Verjetnost č istih ( clean ) in umazanih ( dirty ) tal 2 P_Xc = 0.4 % P ( X = c l e a n ) 3 P_Xd = 1 - P_Xc % P ( X = dirty ) 4 % Pogojna verjetnost merjenja č isto č e 5 P_ZcXc = 0.8 % P ( Z = c l e a n | X = c l e a n ) 6 P_ZdXc = 1 - P_ZcXc % P ( Z = dirty | X = clean ) 7 P_ZdXd = 0.9 % P ( Z = d i r t y | X = d i r t y ) 8 P_ZcXd = 1 - P_ZdXd % P ( Z = clean | X = dirty ) 9 10 disp ( ’ Korak k = 1: Z = clean ’) 11 % Verjetnost meritve v primeru , da zaznamo č ista tla 12 P_Zc_k1 = P_ZcXc * P_Xc + P_ZcXd * P_Xd 13 % Verjetnost č istih tal po opravljeni meritvi ( Bayesovo pravilo ) 14 P_XcZc_k1 = P_ZcXc * P_Xc / P_Zc_k1 15 P_XdZc_k1 = 1 - P_XcZc_k1 ; 16 17 disp ( ’ Korak k = 2: Z = clean ’) 18 % Verjetnost meritve v primeru , da zaznamo č ista tla 19 P_Zc_k2 = P_ZcXc * P_XcZc_k1 + P_ZcXd * P_XdZc_k1 20 % Verjetnost č istih tal po opravljeni meritvi ( Bayesovo pravilo ) 21 P_XcZc_k2 = P_ZcXc * P_XcZc_k1 / P_Zc_k2 22 P_XdZc_k2 = 1 - P_XcZc_k2 ; 23 24 disp ( ’ Korak k = 3: Z = dirty ’) 25 % Verjetnost meritve v primeru , da zaznamo umazana tla 26 P_Zd_k3 = P_ZdXc * P_XcZc_k2 + P_ZdXd * P_XdZc_k2 27 % Verjetnost č istih tal po opravljeni meritvi ( Bayesovo pravilo ) 28 P_XcZd_k3 = P_ZdXc * P_XcZc_k2 / P_Zd_k3 29 P_XdZd_k3 = 1 - P_XcZd_k3 ; P _ X c = 0 . 4 0 0 0 P _ X d = 0 . 6 0 0 0 P _ Z c X c = 0 . 8 0 0 0 P _ Z d X c = 0 . 2 0 0 0 P _ Z d X d = 0 . 9 0 0 0 P _ Z c X d = 0 . 1 0 0 0 K o r a k k = 1: Z = c l e a n P _ Z c _ k 1 = 0 . 3 8 0 0 P _ X c Z c _ k 1 = 0 . 8 4 2 1 K o r a k k = 2: Z = c l e a n P _ Z c _ k 2 = 0 . 6 8 9 5 P _ X c Z c _ k 2 = 0 . 9 7 7 1 K o r a k k = 3: Z = d i r t y P _ Z d _ k 3 = 300 Nedeterminističnost v mobilnih sistemih 0 . 2 1 6 0 P _ X c Z d _ k 3 = 0 . 9 0 4 6 Z = clean Z = clean Z = dirty 1 |Z) eancl0.5 = P(X 0 1 2 3 k Slika 6.8: Porazdelitev trenutnega stanja v treh trenutkih iz primera 6.6 6.4.3 Ocenjevanje stanj iz opazovanj in akcij V poglavju 6.4.2 smo ocenjevali stanje sistema samo na podlagi opazovanja okolice. Običajno pa akcije mobilnega sistema vplivajo na okolico, torej lahko spreminjajo tudi stanja sistema (npr. mobilni sistem se premakne, izvaja čiščenje itd.). Vsaka akcija mobilnega sistema ima neko lastno negotovost, zato izid akcije ni determinističen, ampak je podan z neko verjetnostjo. Gostota verjetnosti p( xk| xk−1 , uk−1) opisuje verjetnost prehoda iz prejšnjega v naslednje stanje pri znani akciji. Akcija uk−1 nastopi v trenutku k − 1 in vpliva na sistem do trenutka k, zato jo običajno poimenujemo kar trenutna akcija. V splošnem akcije v okolici povečujejo stopnjo negotovosti našega znanja o okolici, meritve v okolici pa običajno zmanjšujejo stopnjo negotovosti. Poglejmo si, kako akcije in meritve vplivajo na naše znanje o stanjih. Želimo ugotoviti gostoto verjetnosti p( xk| z 1: k, u 0: k−1), kjer so z meritve in u akcije. Kot v poglavju 6.4.2 lahko zapišemo Bayesov teorem p( zk| xk, z 1: k−1 , u 0: k−1) p( xk| z 1: k−1 , u 0: k−1) p( xk| z 1: k, u 0: k−1) = (6.18) p( zk| z 1: k−1 , u 0: k−1) kjer so: • p( xk| z 1: k, u 0: k−1) je ocena porazdelitve stanja v trenutku k, posodobljena z znanimi meritvami in opravljenimi akcijami, • p( zk| xk, z 1: k−1 , u 0: k−1) je porazdelitev meritve v trenutku k, če poznamo trenutno stanje xk, opravljene akcije in pretekle meritve do trenutka k − 1, 6.4. Bayesov filter 301 • p( xk| z 1: k−1 , u 0: k−1) je napoved porazdelitve stanja na osnovi preteklih meritev in opravljenih akcij do trenutka k, • p( zk| z 1: k−1 , u 0: k−1) je porazdelitev opravljene meritve (zaupanje v opravljeno meritev) v trenutku k. Indeksi akcij so v razponu od 0 do k − 1, ker akcije v preteklih trenutkih vplivajo na obnašanje stanja sistema v trenutku k. Nadalje velja, da lahko trenutno meritev zk v (6.18) opišemo le z znanim stanjem sistema xk, saj pretekle meritve in akcije ne prinašajo dodatnih informacij o sistemu (stanje je vsebovano, Markovov proces) p( zk| xk, z 1: k−1 , u 0: k−1) = p( zk| xk) Zato lahko (6.18) poenostavimo v p( zk| xk) p( xk| z 1: k−1 , u 0: k−1) p( xk| z 1: k, u 0: k−1) = (6.19) p( zk| z 1: k−1 , u 0: k−1) Rekurzivno pravilo za posodobitev verjetnosti ocene stanja (6.19) na podlagi preteklih meritev in akcij vključuje tudi predikcijo p( xk| z 1: k−1 , u 0: k−1), kjer porazdelitev ocene stanja napovemo na podlagi preteklih meritev z 1: k−1 in vseh akcij u 0: k−1. Tako lahko postopek ocenjevanja stanj razdelimo na predikcijski in korekcijski korak. V predikcijskem koraku še ni znana zadnja meritev, je pa znana trenutna akcija, zato lahko na podlagi modela sistema napovemo porazdelitev stanj. Ko je na voljo nova meritev, izvedemo še korekcijski korak. Predikcija Predikcijo p( xk| z 1: k−1 , u 0: k−1) lahko ovrednotimo s teoremom popolne verjetnosti Z p( xk| z 1: k−1 , u 0: k−1) = p( xk| xk−1 , z 1: k−1 , u 0: k−1) p( xk−1| z 1: k−1 , u 0: k−1) d xk−1 kjer zaradi vsebovanosti stanja velja p( xk| xk−1 , z 1: k−1 , u 0: k−1) = p( xk| xk−1 , uk−1) nadalje ugotovimo, da najnovejša akcija uk−1 ni potrebna za oceno stanja v prejšnjem trenutku p( xk−1| z 1: k−1 , u 0: k−1) = p( xk−1| z 1: k−1 , u 0: k−2) Zapišemo končni izraz za izračun predikcije Z p( xk| z 1: k−1 , u 0: k−1) = p( xk| xk−1 , uk−1) p( xk−1| z 1: k−1 , u 0: k−2) d xk−1 (6.20) kjer je p( xk| xk−1 , uk−1) porazdelitev prehajanja med stanji in p( xk−1| z 1: k−1 , u 0: k−2) korekcijska ocena porazdelitve stanja iz prejšnjega trenutka. 302 Nedeterminističnost v mobilnih sistemih Korekcija Ocena stanj, ko je na voljo meritev v trenutku k in predhodno izračunana predikcija, je p( zk| xk) p( xk| z 1: k−1 , u 0: k−1) p( xk| z 1: k, u 0: k−1) = (6.21) p( zk| z 1: k−1 , u 0: k−1) Verjetnost p( zk| z 1: k−1 , u 0: k−1), ki predstavlja zaupanje v opravljeno meritev, pa je Z p( zk| z 1: k−1 , u 0: k−1) = p( zk| xk) p( xk| z 1: k−1 , u 0: k−1) d xk Splošni algoritem za Bayesov filter Splošna oblika Bayesovega filtra je podana v psevdokodi v algoritmu 3. Pogojna verjetnost korekcijskega koraka p( xk| z 1: k, u 0: k−1), ki daje oceno porazdelitve stanja na podlagi znanih akcij in meritev, je znana kot zaupanje in jo zapišemo kot bel( xk) = p( xk| z 1: k, u 0: k−1) pogojno verjetnost predikcije p( xk| z 1: k−1 , u 0: k−1) pa označimo kot belp( xk) = p( xk| z 1: k−1 , u 0: k−1) Bayesov filter ocenjuje porazdelitev ocene stanja. V trenutku, ko je znana informacija o trenutni akciji uk−1, lahko izvedemo predikcijski korak in ko je na voljo tudi nova meritev, lahko izvedemo korekcijski korak. Vpeljemo še normirni faktor η = 1 = 1 . Algoritem za Bayesov filter (algoritem 3) α p( zk| z 1: k−1 ,u 0: k−1) najprej izvede predikcijo, nato pa še korekcijo, ki je ustrezno normirana. Algorithm 3 Bayesov filter function Bayesov_filter( bel( xk−1), uk−1, zk) α ← 0 for all xk do belp( xk) ← R p( xk| xk−1 , uk−1) bel( xk−1) d xk−1 bel 0( xk) ← p( zk| xk) belp( xk) α ← α + bel 0( xk) end for for all xk do bel( xk) ← 1 bel 0( x α k ) end for return bel( xk) end function Kot je razvidno iz algoritma 3, moramo rešiti integral, da določimo porazdelitev v predikcijskem koraku. Izvedba algoritma je torej omejena na enostavne zvezne 6.4. Bayesov filter 303 primere, kjer je možna eksplicitna rešitev integrala, in diskretne primere s končnim številom stanj, kjer lahko integral zamenja vsota. Primer 6.7 Mobilni robot je opremljen s senzorjem, ki lahko zazna, ali so tla čista ali ne ( Z ∈ { clean, dirty}), ter čistilnim sistemom (komplet krtač, vakuumska črpalka in posoda za prah) za čiščenje tal. Čiščenje se izvaja samo v primeru, ko robot meni, da je tla potrebno očistiti ( U ∈ { clean, null}). Zanima nas, ali so tla čista ali ne ( X ∈ { clean, dirty}). Začetna verjetnost (zaupanje), da so tla čista, je bel( X 0 = clean) = 0 , 5 Veljavnost meritev senzorja je podana s statističnim modelom senzorja P ( Zk = clean| Xk = clean) = 0 , 8 P ( Zk = dirty| Xk = clean) = 0 , 2 P ( Zk = dirty| Xk = dirty) = 0 , 9 P ( Zk = clean| Xk = dirty) = 0 , 1 Verjetnosti izida, če se robot odloči za čiščenje tal, je P ( Xk = clean| Xk−1 = clean, Uk−1 = clean) = 1 P ( Xk = dirty| Xk−1 = clean, Uk−1 = clean) = 0 P ( Xk = clean| Xk−1 = dirty, Uk−1 = clean) = 0 , 8 P ( Xk = dirty| Xk−1 = dirty, Uk−1 = clean) = 0 , 2 V kolikor pa čistilni sistem ni aktiviran, lahko domnevamo naslednje verjetnosti izida P ( Xk = clean| Xk−1 = clean, Uk−1 = null) = 1 P ( Xk = dirty| Xk−1 = clean, Uk−1 = null) = 0 P ( Xk = clean| Xk−1 = dirty, Uk−1 = null) = 0 P ( Xk = dirty| Xk−1 = dirty, Uk−1 = null) = 1 Predpostavimo, da mobilni sistem najprej opravi akcijo in šele nato prejme meritev. Določite zaupanje belp( xk) na osnovi opravljene akcije (predikcija) in zaupanje bel( xk) na osnovi meritve bel( xk) (korekcija) za naslednje sekvence opravljenih akcij in zaznanih meritev k Uk−1 Zk 1 null dirty 2 clean clean 3 clean clean 304 Nedeterminističnost v mobilnih sistemih Rešitev Za boljšo preglednost bomo označevali Xk ∈ { clean, dirty} kot Xk ∈ { xk, ¯ xk}, Zk ∈ { clean, dirty} kot Zk ∈ { zk, ¯ zk} in Uk ∈ ( clean, null) kot Uk ∈ { uk, ¯ uk}. Uporabimo algoritem 3. Za trenutek k = 1, ko je opravljena akcija ¯ u 0 = null, lahko določimo predikcijo zaupanja, da so tla čista X belp( x 1) = P ( x 1| x 0 , ¯ u 0) bel( x 0) = x 0∈ X = P ( x 1|¯ x 0 , ¯ u 0) bel(¯ x 0) + P ( x 1| x 0 , ¯ u 0) bel( x 0) = = 0 · 0 , 5 + 1 · 0 , 5 = = 0 , 5 in predikcijo zaupanja, da so tla umazana X belp(¯ x 1) = P (¯ x 1| x 0 , ¯ u 0) bel( x 0) = x 0∈ X = P (¯ x 1|¯ x 0 , ¯ u 0) bel(¯ x 0) + P (¯ x 1| x 0 , ¯ u 0) bel( x 0) = = 1 · 0 , 5 + 0 · 0 , 5 = = 0 , 5 Ker nismo izvedli nobene akcije, so verjetnosti stanja nespremenjene. Glede na meritev ¯ z 1 = dirty lahko določimo korekcijo zaupanja bel( x 1) = ηp(¯ z 1| x 1) belp( x 1) = η 0 , 2 · 0 , 5 = η 0 , 1 in bel(¯ x 1) = ηp(¯ z 1|¯ x 1) belp(¯ x 1) = η 0 , 9 · 0 , 5 = η 0 , 45 Ocenimo še normirni faktor η 1 η = = 1 , 82 0 , 1 + 0 , 45 in končne vrednosti zaupanj bel( x 1) = 0 , 182 bel( ¯ x 1) = 0 , 818 Postopek ponovimo še za trenutek k = 2, kjer je u 1 = clean in z 2 = clean belp( x 2) = 0 , 8364 belp(¯ x 2) = 0 , 1636 bel( x 2) = 0 , 9761 bel(¯ x 2) = 0 , 0239 in trenutek k = 3, kjer je u 2 = clean in z 3 = clean belp( x 3) = 0 , 9952 belp(¯ x 3) = 0 , 0048 bel( x 3) = 0 , 9994 bel(¯ x 3) = 0 , 0006 Rešitev primera 6.7 v programskem okolju Matlab je podana v programu 6.3. 6.4. Bayesov filter 305 Program 6.3 ./src/prb/example_cleaning.m 1 % Notacija : X === X ( k ) , X ’ === X (k -1) 2 disp ( ’ Za č etno zaupanje v č ista in umazana tla ’) 3 bel_Xc = 0.5; % bel ( X = clean ) 4 bel_X = [ bel_Xc 1 - bel_Xc ] % bel ( X = clean ) , bel ( X = dirty ) 5 6 disp ( ’ Pogojne ver jet no sti meritev senzorja ’) 7 P_ZcXc = 0.8; % P ( Z = c l e a n | X = c l e a n ) 8 P_ZdXc = 1 - P_ZcXc ; % P ( Z = dirty | X = clean ) 9 P_ZdXd = 0.9; % P ( Z = d i r t y | X = d i r t y ) 10 P_ZcXd = 1 - P_ZdXd ; % P ( Z = clean | X = dirty ) 11 p_ZX = [ P_ZcXc , P_ZcXd ; ... 12 P_ZdXc , P _ Z d X d ] 13 14 disp ( ’ Pogojne ver jet no sti v primeru č i š č enja ’) 15 P_XcXcUc = 1; % P ( X = c l e a n | X ’= clean , U ’= c l e a n ) 16 P_XdXcUc = 1 - P_XcXcUc ; % P ( X = dirty |X ’= clean ,U ’= clean ) 17 P_XcXdUc = 0.8; % P ( X = c l e a n | X ’= dirty , U ’= c l e a n ) 18 P_XdXdUc = 1 - P_XcXdUc ; % P ( X = dirty |X ’= dirty ,U ’= clean ) 19 p_ZXUc = [ P_XcXcUc , P_XdXcUc ; ... 20 P _ X c X d U c , P _ X d X d U c ] 21 22 disp ( ’ Pogojne verjetnosti , č e ni nobene akcije ’) 23 P_XcXcUn = 1; % P ( X = c l e a n | X ’= clean , U ’= n u l l ) 24 P_XdXcUn = 1 - P_XcXcUn ; % P ( X = dirty |X ’= clean ,U ’= null ) 25 P_XcXdUn = 0; % P ( X = c l e a n | X ’= dirty , U ’= n u l l ) 26 P_XdXdUn = 1 - P_XcXdUn ; % P ( X = dirty |X ’= dirty ,U ’= null ) 27 p_ZXUn = [ P_XcXcUn , P_XdXcUn ; ... 28 P _ X c X d U n , P _ X d X d U n ] 29 30 U = { ’ null ’ , ’ c l e a n ’ , ’ c l e a n ’ }; 31 Z = { ’ dirty ’ , ’ clean ’ , ’ clean ’ }; 32 for k =1: length ( U ) 33 f p r i n t f ( ’ P r e d i k c i j s k i k o r a k : U (% d )=% s \ n ’ , k -1 , U { k }) 34 if s t r c m p ( U ( k ) , ’ c l e a n ’ ) 35 b e l p _ X = b e l _ X * p _ Z X U c 36 e l s e 37 b e l p _ X = b e l _ X * p _ Z X U n 38 end 39 40 f p r i n t f ( ’ K o r e k c i j s k i k o r a k : Z (% d )=% s \ n ’ , k , Z { k }) 41 if s t r c m p ( Z ( k ) , ’ c l e a n ’ ) 42 b e l _ X = p _ Z X ( 1 , :) .* b e l p _ X ; 43 e l s e 44 b e l _ X = p _ Z X ( 2 , :) .* b e l p _ X ; 45 end 46 b e l _ X = b e l _ X / sum ( b e l _ X ) 47 end Za č e t n o z a u p a n j e v č i s t a in u m a z a n a tla b e l _ X = 0 . 5 0 0 0 0 . 5 0 0 0 P o g o j n e v e r j e t n o s t i m e r i t e v s e n z o r j a p _ Z X = 0 . 8 0 0 0 0 . 1 0 0 0 0 . 2 0 0 0 0 . 9 0 0 0 P o g o j n e v e r j e t n o s t i v p r i m e r u č i š č e n j a p _ Z X U c = 1 . 0 0 0 0 0 0 . 8 0 0 0 0 . 2 0 0 0 P o g o j n e v e r j e t n o s t i , č e ni n o b e n e a k c i j e 306 Nedeterminističnost v mobilnih sistemih p _ Z X U n = 1 0 0 1 P r e d i k c i j s k i k o r a k : U ( 0 ) = n u l l b e l p _ X = 0 . 5 0 0 0 0 . 5 0 0 0 K o r e k c i j s k i k o r a k : Z ( 1 ) = d i r t y b e l _ X = 0 . 1 8 1 8 0 . 8 1 8 2 P r e d i k c i j s k i k o r a k : U ( 1 ) = c l e a n b e l p _ X = 0 . 8 3 6 4 0 . 1 6 3 6 K o r e k c i j s k i k o r a k : Z ( 2 ) = c l e a n b e l _ X = 0 . 9 7 6 1 0 . 0 2 3 9 P r e d i k c i j s k i k o r a k : U ( 2 ) = c l e a n b e l p _ X = 0 . 9 9 5 2 0 . 0 0 4 8 K o r e k c i j s k i k o r a k : Z ( 3 ) = c l e a n b e l _ X = 0 . 9 9 9 4 0 . 0 0 0 6 6.4.4 Primer lokalizacije Na preprostem primeru si poglejmo princip lokalizacije, ki predstavlja osnovno idejo algoritma za lokalizacijo Monte Carlo. Mobilni robot se premika v okolju in zaznava svojo lego s senzorjem. Algoritem za lokalizacijo mora na podlagi meritev senzorja in izvedenih premikov določiti lego robota v okolju. Pri tem uporabimo teorem popolne verjetnosti in Bayesovo pravilo, ki predstavljata osnovo za izvedbo Bayesovega filtra. V nadaljevanju je predstavljen proces zaznavanja ob prisotnosti negotovosti senzorja, ki mu sledi proces izvajanja akcije ob prisotnosti negotovosti aktuatorja. Mobilni robot izvaja akcije z namenom spreminjanja stanja v okolju (lega mobilnega sistema se spreminja z njegovim premikanjem). Primer 6.8 Mobilni robot se vozi po krožni poti v smeri naprej ali nazaj. Pot je sestavljena iz končnega števila svetlih in temnih ploščic v naključnem vrstnem redu, kjer širina ploščice ustreza širini poti. Mobilni sistem lahko namestimo na katerokoli oštevilčeno ploščico. Brez izgube splošnosti predpostavimo, da je pot sestavljena iz petih ploščic, kot je prikazano na sliki 6.9. 6.4. Bayesov filter 307 x 4 x 3 Akcija premik naprej ( u) Senzor barve ( z) Mobilni robot ( x) x 5 x 2 Pot x 1 Slika 6.9: Pot v okolju je sestavljena iz petih črnih in belih ploščic. Mobilni sistem se lahko premika med njimi in zaznava barvo trenutne ploščice. Vsaka ploščica predstavlja celico, v katero lahko postavimo mobilni sistem, torej imamo diskretno predstavitev okolja. Mobilni sistem pozna zemljevid okolice, torej pozna zaporedje svetlih in temnih ploščic, vendar ne ve, v kateri celici se nahaja. Začetno zaupanje v pozicijo mobilnega sistema je podano z enakomerno porazdelitvijo, saj je vsaka celica enako verjetna. Mobilni sistem ima senzor za zaznavanje svetlih in temnih ploščic, vendar so njegove meritve negotove. Mobilni sistem se lahko premika za želeno število ploščic v smeri naprej ali nazaj, vendar je gibanje samo po sebi negotovo (lahko se premakne premalo ali preveč). Na omenjenem primeru bosta razložena procesa zaznavanja okolja in gibanja v okolju. 6.4.5 Zaznavanje okolja S pomočjo meritve, izvedene v okolici, lahko izboljšamo oceno stanja X (npr. lokacija) v tem okolju. Predstavljajmo si, da sredi noči hodimo v spanju in tavamo po hiši. Ko se zbudimo, lahko s svojimi čuti (vid, dotik itd.) ugotovimo, kje se nahajamo. Začetno znanje o okolici lahko matematično opišemo s porazdelitvijo p( x). To porazdelitev lahko izboljšamo ob nastopu nove meritve z (z zaupanjem p( z| x)), če je verjetnost po opravljeni meritvi p( x| z). To lahko dosežemo z Bayesovim 308 Nedeterminističnost v mobilnih sistemih pravilom p( x| z) = bel( x) = p( z| x) p( x) . Verjetnost p( z| x) predstavlja statistični p( z) model senzorja, bel( x) pa je zaupanje v oceno stanja po opravljeni meritvi. V procesu zaznavanja se torej izvede korekcijski korak Bayesovega filtra. Primer 6.9 Za primer 6.8 predpostavimo, da mobilni sistem zazna temno celico Z = dark. Mobilni sistem pravilno zazna temno celico z verjetnostjo 0 , 6, napako pa naredi z verjetnostjo 0 , 2, kjer svetlo celico razpozna kot temno. Tako zapišemo p( Z = dark| X = xd) = 0 , 6 d ∈ {3 , 4} p( Z = dark| X = xb) = 0 , 2 b ∈ {1 , 2 , 5} kjer indeks b označuje svetle celice, indeks d pa temne. Na začetku mobilni sistem ne pozna svoje pozicije, kar opišemo z enakomerno porazdelitvijo P ( X = xi) = bel( xi) = 0 , 2, i ∈ {1 , . . . , 5}. Kakšna je porazdelitev lokacije po opravljeni meritvi? Rešitev Želimo določiti porazdelitev pogojne verjetnosti p( X 1| Z = dark), kar je zaupanje v oceno stanja po opravljeni meritvi. Želeno verjetnost lahko določimo s pomočjo korekcijskega koraka Bayesovega filtra p( Z = dark| X p 1) ? p( X 1) ( X 1| Z = dark) = = P ( Z = dark) [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2] T ? [0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = = P ( Z = dark) [0 , 04 , 0 , 04 , 0 , 12 , 0 , 12 , 0 , 04] T = P ( Z = dark) kjer operator ? predstavlja množenje istoležnih elementov vektorja. Izračunati moramo verjetnost zaznave temne celice P ( Z = dark). Zato je potrebno oceniti polno verjetnost, tj. verjetnost zaznave temne celice ob upoštevanju vseh celic X P ( Z = dark) = P ( Z = dark| X 1 = xi) P ( X 1 = xi) = i = p T ( Z = dark| X 1)p( X 1) = = [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = = 0 , 36 Iskana (posteriorna) porazdelitev je torej p( X 1| Z = dark) = [0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T 6.4. Bayesov filter 309 od koder lahko sklepamo, da se mobilni robot s trikrat večjo verjetnostjo nahaja v celici 3 ali 4, kot pa v preostalih treh. Porazdelitve so grafično prikazane na sliki 6.10, rešitev tega primera pa je podana v programu 6.4. Program 6.4 ./src/prb/example_sensing.m 1 disp ( ’ P o r a z d e l i t e v meritev senzorja p ( Z = dark | X ) ’) 2 p_ZdX = [0.2 0.2 0.6 0.6 0.2] 3 disp ( ’ P o r a z d e l i t e v meritev senzorja p ( Z = bright | X ) ’) 4 p_ZbX = 1 - p_ZdX 5 disp ( ’ Za č etna p o r a z d e l i t e v p ( X ) ’) 6 p_X = ones (1 ,5)/5 7 8 disp ( ’ Verjetnost detekcije temne celice P ( Z = dark ) ’) 9 P_Zd = p_ZdX * p_X . ’ 10 11 disp ( ’ P o r a z d e l i t e v p ( X | Z = dark ) ’) 12 p_XZd = p_ZdX .* p_X / P_Zd P o r a z d e l i t e v m e r i t e v s e n z o r j a p ( Z = d a r k | X ) p _ Z d X = 0 . 2 0 0 0 0 . 2 0 0 0 0 . 6 0 0 0 0 . 6 0 0 0 0 . 2 0 0 0 P o r a z d e l i t e v m e r i t e v s e n z o r j a p ( Z = b r i g h t | X ) p _ Z b X = 0 . 8 0 0 0 0 . 8 0 0 0 0 . 4 0 0 0 0 . 4 0 0 0 0 . 8 0 0 0 Za č e t n a p o r a z d e l i t e v p ( X ) p_X = 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 V e r j e t n o s t d e t e k c i j e t e m n e c e l i c e P ( Z = d a r k ) P _ Z d = 0 . 3 6 0 0 P o r a z d e l i t e v p ( X | Z = d a r k ) p _ X Z d = 0 . 1 1 1 1 0 . 1 1 1 1 0 . 3 3 3 3 0 . 3 3 3 3 0 . 1 1 1 1 310 Nedeterminističnost v mobilnih sistemih p(Z = dark) 0.5 0 x1 x2 x3 x4 x5 p(X) 0.5 0 x1 x2 x3 x4 x5 p(X|Z = dark) 0.5 0 x1 x2 x3 x4 x5 Slika 6.10: Porazdelitve iz primera 6.9 Primer 6.10 Za primer 6.9 odgovorite na naslednja vprašanja: 1. Ali lahko večkratne zaporedne meritve izboljšajo oceno pozicije mobilnega robota (robot med meritvami miruje)? 2. Kakšna je porazdelitev pozicije mobilnega robota, če robot dvakrat zapored zazna ploščico kot temno? 3. Kakšna je porazdelitev pozicije mobilnega robota, če robot najprej zazna ploščico kot temno nato pa kot svetlo? 4. Kakšna je porazdelitev pozicije mobilnega robota, če robot najprej zazna ploščico kot temno, nato kot svetlo, nato pa spet kot temno? Rešitev 6.4. Bayesov filter 311 1. Večkratne zaporedne meritve lahko izboljšajo oceno pozicije mobilnega robota, če je verjetnost pravilne meritve večja od verjetnosti napake pri meritvi. 2. Če senzor zazna celico kot temno dvakrat zapored, je porazdelitev p( X 2| Z 1 = dark, Z 2 = dark) = p( X 2| z 1 , z 2) = p( z 2| X 2) ? p( X 2| z 1) = = P ( z 2| z 1) [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2] T ? [0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T = P ( z 2| z 1) kjer je j-ti element v porazdelitvi p( X 2| z 1) podan s P ( X 2 = xj| z 1) = p T ( X 2 = xj| X 1)p( X 1| z 1) = P ( X 1 = xj| z 1), ker nimamo vpliva na stanja (glejte primer 6.5), ampak jih le merimo (opazujemo). Pogojna verjetnost v imenovalcu je X P ( z 2| z 1) = P ( z 2| X 2 = xi) P ( X 2 = xi| z 1)) = xi = p T ( z 2| X 2)p( X 2| z 1)) = = [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2][0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T = = 0 , 4667 Končna rešitev je [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2] T ? [0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T p( X 2| z 1 , z 2) = = [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2][0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T = [0 , 0476 , 0 , 0476 , 0 , 4286 , 0 , 4286 , 0 , 0476] T 3. Svetla celica je pravilno zaznana z verjetnostjo p( Z = bright| X = bright) = 1 − p( Z = dark| X = bright) = 0 , 8 in nepravilno z verjetnostjo p( Z = bright| X = dark) = 1 − p( Z = dark| X = dark) = 0 , 4. Drugo meritev lahko izvedemo na osnovi porazdelitve p( X 2| Z 1 = dark) p( X 2| Z 1 = dark, Z 2 = bright) = p( X 2| z 1 , z 2) = [0 , 8 , 0 , 8 , 0 , 4 , 0 , 4 , 0 , 8] T ? [0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T = P ( Z 2 = bright| Z 1 = dark) P ( Z 2 = bright| Z 1 = dark) = X = P ( Z 2 = bright| X 2 = xi) P ( X 2 = xi| Z 1 = dark) = xi = p T ( Z 2 = bright| X 2)p( X 2| Z 1 = dark) = = [0 , 8 , 0 , 8 , 0 , 4 , 0 , 4 , 0 , 8][0 , 11 , 0 , 11 , 0 , 33 , 0 , 33 , 0 , 11] T = 0 , 533 p( X 2| Z 1 = dark, Z 2 = bright) = [0 , 167 , 0 , 167 , 0 , 25 , 0 , 25 , 0 , 167] T 312 Nedeterminističnost v mobilnih sistemih 4. Porazdelitev stanja po treh meritvah je p( X 3| Z 1 = dark, Z 2 = bright, Z 3 = dark) = = [0 , 083 , 0 , 083 , 0 , 375 , 0 , 375 , 0 , 083] T V programu 6.5 je prikazana Matlab koda rešitve. Porazdelitev trenutnega stanja za tri časovne trenutke je grafično predstavljena na sliki 6.11. Program 6.5 ./src/prb/example_sensing_ans34.m 1 p_ZdX = [0.2 0.2 0.6 0.6 0.2]; 2 p_ZbX = 1 - p_ZdX ; 3 p_X = ones (1 ,5)/5; 4 5 disp ( ’ Verjetnost detekcije temne celice P ( Z1 = dark ) ’) 6 P_z1 = p_ZdX * p_X . ’ 7 disp ( ’ P o r a z d e l i t e v p ( X1 | Z1 = dark ) ’) 8 p_Xz1 = p_ZdX .* p_X / P_z1 9 10 disp ( ’ Verjetnost detekcije svetle celice P ( Z2 = bright | Z1 = dark ) ’) 11 P_z2 = p_ZbX * p_Xz1 . ’ 12 disp ( ’ P o r a z d e l i t e v p ( X2 | Z1 = dark , Z2 = bright ) ’) 13 p_Xz2 = p_ZbX .* p_Xz1 / P_z2 14 15 disp ( ’ Verjetnost detekcije temne celice P ( Z3 = dark | Z1 = dark , Z2 = bright ) ’) 16 P_z3 = p_ZdX * p_Xz2 . ’ 17 disp ( ’ P o r a z d e l i t e v p ( X3 | Z1 = dark , Z2 = bright , Z3 = dark ) ’) 18 p_Xz3 = p_ZdX .* p_Xz2 / P_z3 V e r j e t n o s t d e t e k c i j e t e m n e c e l i c e P ( Z1 = d a r k ) P _ z 1 = 0 . 3 6 0 0 P o r a z d e l i t e v p ( X1 | Z1 = d a r k ) p _ X z 1 = 0 . 1 1 1 1 0 . 1 1 1 1 0 . 3 3 3 3 0 . 3 3 3 3 0 . 1 1 1 1 V e r j e t n o s t d e t e k c i j e s v e t l e c e l i c e P ( Z2 = b r i g h t | Z1 = d a r k ) P _ z 2 = 0 . 5 3 3 3 P o r a z d e l i t e v p ( X2 | Z1 = dark , Z2 = b r i g h t ) p _ X z 2 = 0 . 1 6 6 7 0 . 1 6 6 7 0 . 2 5 0 0 0 . 2 5 0 0 0 . 1 6 6 7 V e r j e t n o s t d e t e k c i j e t e m n e c e l i c e P ( Z3 = d a r k | Z1 = dark , Z2 = b r i g h t ) P _ z 3 = 0 . 4 0 0 0 P o r a z d e l i t e v p ( X3 | Z1 = dark , Z2 = bright , Z3 = d a r k ) p _ X z 3 = 0 . 0 8 3 3 0 . 0 8 3 3 0 . 3 7 5 0 0 . 3 7 5 0 0 . 0 8 3 3 6.4. Bayesov filter 313 p(X1|Z1 = dark) 0.4 0.2 0 x1 x2 x3 x4 x5 p(X2|Z1 = dark,Z2 = bright) 0.4 0.2 0 x1 x2 x3 x4 x5 p(X3|Z1 = dark,Z2 = bright,Z3 = dark) 0.4 0.2 0 x1 x2 x3 x4 x5 Slika 6.11: Porazdelitve trenutnega stanja v treh trenutkih iz primera 6.10 6.4.6 Gibanje v okolju Gibanje mobilnih sistemov v okolju je izvedeno s pomočjo aktuatorjev (npr. motorna kolesa) in regulacijskega sistema (algoritem). Pri vsakem gibanju je prisotna manjša ali večja negotovost, zato gibanje mobilnega sistema povečuje negotovost stanja mobilnega sistema (lege) v okolju. Predstavljajmo si, da stojimo v znanem okolju. Z zaprtimi očmi naredimo nekaj korakov. Približno vemo, kako velike korake smo naredili in tudi v katero smer, zato si lahko predstavljamo, kje v okolici se nahajamo. Vendar pa dolžine naših korakov niso natančno znane, prav tako težko ocenimo smeri korakov, zato se naše znanje o legi v prostoru sčasoma zmanjšuje, saj naredimo vedno več korakov. V primeru gibanja brez zaznavanja stanj preko meritev lahko enačbo (6.20) preuredimo +∞ Z p( xk| u 0: k−1) = p( xk| xk−1 , uk−1) p( xk−1| u 1: k−2) d xk−1 −∞ Zaupanje v novo stanje p( xk| u 0: k−1) je odvisno od zaupanja v prejšnjem trenutku p( xk−1| u 0: k−2) in pogojne verjetnosti prehoda med stanji p( xk| xk−1 , uk−1). Porazdelitev p( xk| , u 0: k−1) določimo z integracijo (ali seštevanjem v diskretnem 314 Nedeterminističnost v mobilnih sistemih primeru) vseh možnih verjetnosti prehodov p( xk| xk−1 , uk−1) iz predhodnih stanj xk−1 v stanje xk pri poznani akciji uk−1. Primer 6.11 Za primer 6.8 predpostavimo, da je začetna pozicija mobilnega robota v prvi celici ( X 0 = x 1), torej lahko začetno stanje podamo s porazdelitvijo p( X 0) = [1 , 0 , 0 , 0 , 0]. Mobilni sistem se lahko premika med celicami, kjer je izid akcije premika pravilen v 80 %. V 10 % se robot premakne za eno celico premalo in v 10 % za eno preveč, kot je potrebno. To lahko opišemo z naslednjimi verjetnostmi prehoda med stanji P ( Xk = xi| Xk−1 = xj, Uk−1 = u) = 0 , 8 za i = j + u P ( Xk = xi| Xk−1 = xj, Uk−1 = u) = 0 , 1 za i = j + u − 1 P ( Xk = xi| Xk−1 = xj, Uk−1 = u) = 0 , 1 za i = j + u + 1 Mobilni robot se mora premakniti za dve celici v nasprotni smeri urinega kazalca ( U 0 = 2). Določite zaupanje v pozicijo mobilnega sistema po premiku. Rešitev Porazdelitev (zaupanje) po premiku lahko določimo tako, da izračunamo ver- jetnosti pozicije mobilnega robota v vsaki celici (popolna verjetnost). Mobilni sistem lahko prispe v prvo celico samo iz celice 3 (premik preveč), celice 4 (pravi premik) in celice 5 (premik premalo). Tako dobimo porazdelitev prehoda v prvo celico p( X 1 = x 1| X 0 , U 0 = 2) = [0 , 0 , 0 , 1 , 0 , 8 , 0 , 1] T . Po premiku se mobilni sistem nahaja v prvi celici z verjetnostjo X P ( X 1 = x 1| U 0 = 2) = P ( X 1 = x 1| X 0 = xi, U 0 = 2) P ( X 0 = xi) = xi = p T ( X 1 = x 1| X 0 , U 0 = 2)p( X 0) = = [0 , 0 , 0 , 1 , 0 , 8 , 0 , 1][1 , 0 , 0 , 0 , 0] T = 0 Verjetnost, da se mobilni sistem po premiku nahaja v drugi celici, je X P ( X 1 = x 2| U 0 = 2) = P ( X 1 = x 2| X 0 = xi, U 0 = 2) P ( X 0 = xi) = xi = p T ( X 1 = x 2| X 0 , U 0 = 2)p( X 0) = = [0 , 1 , 0 , 0 , 0 , 1 , 0 , 8][1 , 0 , 0 , 0 , 0] T = 0 , 1 Podobno lahko izračunamo še verjetnosti za ostale celice P ( X 1 = x 3| U 0 = 2) = [0 , 8 , 0 , 1 , 0 , 0 , 0 , 1][1 , 0 , 0 , 0 , 0] T = 0 , 8 P ( X 1 = x 4| U 0 = 2) = [0 , 1 , 0 , 8 , 0 , 1 , 0 , 0][1 , 0 , 0 , 0 , 0] T = 0 , 1 P ( X 1 = x 5| U 0 = 2) = [0 , 0 , 1 , 0 , 8 , 0 , 1 , 0][1 , 0 , 0 , 0 , 0] T = 0 6.4. Bayesov filter 315 Zato je zaupanje v pozicijo po premiku p( X 1| U 0 = 2) = [0 , 0 , 1 , 0 , 8 , 0 , 1 , 0] T Porazdelitev trenutnega (a posteriori) stanja je grafično predstavljena na sliki 6.12. V programu 6.6 je prikazana Matlab koda rešitve. Program 6.6 ./src/prb/example_motion.m 1 disp ( ’ Za č etno zaupanje p ( X0 ) ’ ); 2 p_X0 = [1 0 0 0 0] 3 4 P_xxu_null = 0.8; % P ( X = i |X ’= j ,U ’= u ) , i = j + u 5 P_xxu_less = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j +u -1 6 P_xxu_more = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j + u +1 7 8 disp ( ’ Zaupanje p ( X1 | U0 =2) ’ ); 9 p_xXu = [0 0 P_xxu_more P_xxu_null P_xxu_less ]; % Za U =2 10 p_Xu = zeros (1 ,5); 11 for i =1:5 12 p _ X u ( i ) = p _ x X u * p _ X 0 . ’; 13 p _ x X u = p _ x X u ([ end 1: end - 1 ] ) ; 14 end 15 p_X1 = p_Xu Za č e t n o z a u p a n j e p ( X0 ) p _ X 0 = 1 0 0 0 0 Z a u p a n j e p ( X1 | U0 =2) p _ X 1 = 0 0 . 1 0 0 0 0 . 8 0 0 0 0 . 1 0 0 0 0 Zaupan je p(X0) 1 0.5 0 x1 x2 x3 x4 x5 Zaupan je p(X1|U0 = 2) 1 0.5 0 x1 x2 x3 x4 x5 Slika 6.12: Porazdelitvi trenutnega stanja v dveh trenutkih iz primera 6.11 316 Nedeterminističnost v mobilnih sistemih Primer 6.12 Kakšno je zaupanje v pozicijo mobilnega robota, če robot po premiku iz primera 6.11 izvede premik za eno celico v nasprotni smeri urinega kazalca ( U 1 = 1)? Rešitev Porazdelitev (zaupanje) po premiku ponovno določimo tako, da za vsako celico iz-računamo verjetnost pozicije mobilnega robota v vsaki celici (popolna verjetnost). V primeru premika za eno celico je prva celica dosegljiva iz celice 1 (premik premalo), celice 4 (premik preveč) in celice 5 (pravi premik). Mobilni sistem lahko prispe v drugo celico iz celic 1, 2 ter 5 in tako naprej. Po opravljenem premiku lahko izračunamo naslednje verjetnosti P ( X 2 = x 1| U 0 = 2 , U 1 = 1) = [0 , 1 , 0 , 0 , 0 , 1 , 0 , 8][0 , 0 , 1 , 0 , 8 , 0 , 1 , 0] T = 0 , 01 P ( X 2 = x 2| U 0 = 2 , U 1 = 1) = [0 , 8 , 0 , 1 , 0 , 0 , 0 , 1][0 , 0 , 1 , 0 , 8 , 0 , 1 , 0] T = 0 , 01 P ( X 2 = x 3| U 0 = 2 , U 1 = 1) = [0 , 1 , 0 , 8 , 0 , 1 , 0 , 0][0 , 0 , 1 , 0 , 8 , 0 , 1 , 0] T = 0 , 16 P ( X 2 = x 4| U 0 = 2 , U 1 = 1) = [0 , 0 , 1 , 0 , 8 , 0 , 1 , 0][0 , 0 , 1 , 0 , 8 , 0 , 1 , 0] T = 0 , 66 P ( X 2 = x 5| U 0 = 2 , U 1 = 1) = [0 , 0 , 0 , 1 , 0 , 8 , 0 , 1][0 , 0 , 1 , 0 , 8 , 0 , 1 , 0] T = 0 , 16 Torej je zaupanje v pozicijo po drugem premiku p( X 2| U 0 = 2 , U 1 = 1) = [0 , 01 , 0 , 01 , 0 , 16 , 0 , 66 , 0 , 16] T kar je prikazano na sliki 6.13. Opazimo, da je mobilni sistem najverjetneje v celici 4. Vendar pa porazdelitev nima tako izrazitega maksimuma, kot pred izvedbo drugega premika (primerjajte srednjo s spodnjo porazdelitvijo na sliki 6.13). To je v skladu z izjavo, da vsak premik poveča negotovost stanj v okolici. Implementacija rešitve v programskem okolju Matlab je prikazana v programu 6.7. Program 6.7 ./src/prb/example_motion2.m 1 disp ( ’ Za č etno zaupanje p ( X0 ) ’) 2 p_X0 = [1 0 0 0 0] 3 4 P_xxu_null = 0.8; % P ( X = i |X ’= j ,U ’= u ) , i = j + u 5 P_xxu_less = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j +u -1 6 P_xxu_more = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j + u +1 7 8 disp ( ’ Zaupanje p ( X1 | U0 =2) ’ ); 9 p_xXu = [0 0 P_xxu_more P_xxu_null P_xxu_less ]; % Za U =2 10 p_Xu = zeros (1 ,5); 11 for i =1:5 12 p _ X u ( i ) = p _ x X u * p _ X 0 . ’; 13 p _ x X u = p _ x X u ([ end 1: end - 1 ] ) ; 14 end 6.4. Bayesov filter 317 15 p_X1 = p_Xu 16 17 disp ( ’ Zaupanje p ( X2 | U1 =1) ’ ); 18 p_xXu = [ P_xxu_less 0 0 P_xxu_more P_xxu_null ]; % Za U =1 19 p_Xu = zeros (1 ,5); 20 for i =1:5 21 p _ X u ( i ) = p _ x X u * p _ X 1 . ’; 22 p _ x X u = p _ x X u ([ end 1: end - 1 ] ) ; 23 end 24 p_X2 = p_Xu Za č e t n o z a u p a n j e p ( X0 ) p _ X 0 = 1 0 0 0 0 Z a u p a n j e p ( X1 | U0 =2) p _ X 1 = 0 0 . 1 0 0 0 0 . 8 0 0 0 0 . 1 0 0 0 0 Z a u p a n j e p ( X2 | U1 =1) p _ X 2 = 0 . 0 1 0 0 0 . 0 1 0 0 0 . 1 6 0 0 0 . 6 6 0 0 0 . 1 6 0 0 Zaupan je p(X0) 1 0.5 0 x1 x2 x3 x4 x5 Zaupan je p(X1|U0 = 2) 1 0.5 0 x1 x2 x3 x4 x5 Zaupan je p(X2|U1 = 1) 1 0.5 0 x1 x2 x3 x4 x5 Slika 6.13: Porazdelitve stanj v treh časovnih trenutkih iz primera 6.12 Primer 6.13 Mobilni robot iz primera 6.11 se na začetku nahaja v prvi celici p( X 0) = [1 , 0 , 0 , 0 , 0]. Nato se v vsakem časovnem trenutku premakne za eno celico v nasprotni smeri urinega kazalca. 318 Nedeterminističnost v mobilnih sistemih 1. Kakšno je zaupanje v pozicijo mobilnega sistema po desetem premiku? 2. H kateri vrednosti konvergira zaupanje po neskončnem številu premikov? Rešitev 1. Zaupanje v stanje po desetih premikih je p( X 10| U 0:9) = [0 , 29 , 0 , 22 , 0 , 13 , 0 , 13 , 0 , 22] T 2. Po neskončnem številu premikov dobimo enakomerno porazdelitev, kjer so vse celico enako verjetne p( X∞| U 0:∞) = [0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T Te rezultate smo potrdili tudi v programskem okolju Matlab (program 6.8). Rezultati so grafično prikazani na sliki 6.14. Program 6.8 ./src/prb/example_motion3.m 1 disp ( ’ Za č etno zaupanje p ( X0 ) ’) 2 p_X0 = [1 0 0 0 0] 3 4 P_xxu_null = 0.8; % P ( X = i |X ’= j ,U ’= u ) , i = j + u 5 P_xxu_less = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j +u -1 6 P_xxu_more = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j + u +1 7 8 p_X = p_X0 ; 9 for k =1:1000 10 p _ x X u = [ P _ x x u _ l e s s 0 0 P _ x x u _ m o r e P _ x x u _ n u l l ]; % Za U =1 11 p _ X u = z e r o s (1 ,5); 12 for i = 1 : 5 13 p _ X u ( i ) = p _ x X u * p_X . ’; 14 p _ x X u = p _ x X u ([ end 1: end - 1 ] ) ; 15 end 16 p_X = p _ X u ; 17 if k = = 1 0 18 d i s p ( ’ Z a u p a n j e p ( X10 | U9 =1) ’ ); 19 p _ X 1 0 = p_X 20 e l s e i f k = = 1 0 0 0 21 d i s p ( ’ Z a u p a n j e p ( X 1 0 0 0 | U 9 9 9 =1) ’ ); 22 p _ X 1 0 0 0 = p_X 23 end 24 end Za č e t n o z a u p a n j e p ( X0 ) p _ X 0 = 1 0 0 0 0 Z a u p a n j e p ( X10 | U9 =1) 6.4. Bayesov filter 319 p _ X 1 0 = 0 . 2 9 4 9 0 . 2 2 4 3 0 . 1 2 8 3 0 . 1 2 8 3 0 . 2 2 4 3 Z a u p a n j e p ( X 1 0 0 0 | U 9 9 9 =1) p _ X 1 0 0 0 = 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 Zaupan je p(X0) 1 0.5 0 x1 x2 x3 x4 x5 Zaupan je p(X10|U9 = 1) 1 0.5 0 x1 x2 x3 x4 x5 Zaupan je p(X1000|U999 = 1) 1 0.5 0 x1 x2 x3 x4 x5 Slika 6.14: Porazdelitve trenutnega stanja v treh časovnih trenutkih iz primera 6.13 6.4.7 Lokalizacija v okolju Ko mobilni robot pozna zemljevid okolja, lahko oceni svojo lokacijo v okolju, tudi če ne pozna svoje začetne lokacije. Lokacijo mobilnega sistema lahko natančno določimo s porazdelitvijo. Proces ugotavljanja lokacije v okolju imenujemo lokalizacija. Lokalizacija združuje proces zaznavanja (meritev) in akcije (premik). Kot smo že omenili, meritve v okolju povečujejo znanje o lokaciji, gibanje mobilnega sistema v okolju pa to znanje zmanjšuje. Lokalizacija je postopek, pri katerem mobilni sistem stalno posodablja porazdelitev, ki predstavlja njegovo znanje o svoji lokaciji v okolju. Maksimum porazdelitve (če obstaja) predstavlja najverjetnejšo lokacijo mobilnega sistema. Pri procesu lokalizacije v bistvu izvajamo Bayesov filter (algoritem 3), ki združuje procesa premikanja in zaznavanja. 320 Nedeterminističnost v mobilnih sistemih Primer 6.14 Mobilni sistem se premika v okolici iz primera 6.8, kjer najprej izvede premik, nato pa zaznava okolico. Njegova začetna lega ni znana, kar lahko opišemo z enakomerno porazdelitvijo p( X 0) = bel( X 0) = [0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2]. Premik za uk celic v nasprotni smeri urinega kazalca je točen v 80 %, v 10 % pa je za eno celico prekratek ali predolg p( Xk = xi| Xk−1 = xj, Uk−1 = uk−1) = 0 , 8 za i = j + uk−1 p( Xk = xi| Xk−1 = xj, Uk−1 = uk−1) = 0 , 1 za i = j + uk−1 − 1 p( Xk = xi| Xk−1 = xj, Uk−1 = uk−1) = 0 , 1 za i = j + uk−1 + 1 Mobilni robot pravilno zazna temno celico z verjetnostjo 0 , 6, svetlo celico pa pravilno zazna z verjetnostjo 0 , 8. To lahko v matematični obliki zapišemo kot P ( Z = dark| X = dark) = 0 , 6 P ( Z = bright| X = dark) = 0 , 4 P ( Z = bright| X = bright = 0 , 8 P ( Z = dark| X = bright = 0 , 2 V vsakem časovnem trenutku dobi mobilni robot ukaz za premik za eno celico v nasprotni smeri urinega kazalca ( uk−1 = 1). Zaporedje prvih treh meritev je z 1:3 = [ bright, dark, dark]. 1. Kakšno je zaupanje v prvem trenutku k = 1? 2. Kakšno je zaupanje v drugem trenutku k = 2? 3. Kakšno je zaupanje v tretjem trenutku k = 3? 4. V kateri celici se mobilni robot nahaja z največjo verjetnostjo po tretjem koraku? Rešitev Po vsakem premiku izvedemo predikcijski korak Bayesovega filtra (algoritem 3), po meritvi (zaznavi) pa korekcijski korak. 1. Predikcijski korak izvedemo na osnovi izvedenega premika. Mali xi, i ∈ {1 , . . . , 5}, označuje, da se lokacija (stanje) mobilnega sistema nahaja v 6.4. Bayesov filter 321 celici i, veliki Xk pa označuje vektor vseh možnih stanj v trenutku k X belp( X 1 = x 1) = P ( X 1 = x 1| X 0 = xi, u 0) bel( X 0 = xi) = xi = p T ( X 1 = x 1| X 0 , u 0)bel( X 0) = = [0 , 1 , 0 , 0 , 0 , 1 , 0 , 8][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = 0 , 2 belp( X 1 = x 2) = [0 , 8 , 0 , 1 , 0 , 0 , 0 , 1][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = 0 , 2 belp( X 1 = x 3) = [0 , 1 , 0 , 8 , 0 , 1 , 0 , 0][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = 0 , 2 belp( X 1 = x 4) = [0 , 0 , 1 , 0 , 8 , 0 , 1 , 0][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = 0 , 2 belp( X 1 = x 5) = [0 , 0 , 0 , 1 , 0 , 8 , 0 , 1][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = 0 , 2 Torej je celotna porazdelitev (zaupanje) predikcijskega koraka bel p( X 1) = [0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T Na osnovi meritve se oceni korekcijski korak Bayesovega filtra bel( X 1 = x 1) = η p( Z 1 = bright| x 1) belp( X 1 = x 1) = η 0 , 8 · 0 , 2 = η 0 , 16 bel( X 1 = x 2) = η p( Z 1 = bright| x 2) belp( X 1 = x 2) = η 0 , 8 · 0 , 2 = η 0 , 16 bel( X 1 = x 3) = η p( Z 1 = bright| x 3) belp( X 1 = x 3) = η 0 , 4 · 0 , 2 = η 0 , 08 bel( X 1 = x 4) = η p( Z 1 = bright| x 4) belp( X 1 = x 4) = η 0 , 4 · 0 , 2 = η 0 , 08 bel( X 1 = x 5) = η p( Z 1 = bright| x 5) belp( X 1 = x 5) = η 0 , 8 · 0 , 2 = η 0 , 16 Ko upoštevamo še normirni faktor 1 η = = 1 , 56 0 , 16 + 0 , 16 + 0 , 08 + 0 , 08 + 0 , 16 dobimo posodobljeno porazdelitev (zaupanje) bel( X 1) = [0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T Enak rezultat lahko dobimo iz p T ( Z 1 = bright| X 1) ? bel T ( X 1) bel p ( X 1) = = p T ( Z 1 = bright| X 1)bel p( X 1) [0 , 8 , 0 , 8 , 0 , 4 , 0 , 4 , 0 , 8] ? [0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] = = [0 , 8 , 0 , 8 , 0 , 4 , 0 , 4 , 0 , 8][0 , 2 , 0 , 2 , 0 , 2 , 0 , 2 , 0 , 2] T = [0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T 2. Postopek iz prvega primera ponovimo na zadnjem rezultatu, da dobimo zaupanje v stanje v trenutku k = 1. Najprej ponovimo predikcijski korak belp( X 2 = x 1) = [0 , 1 , 0 , 0 , 0 , 1 , 0 , 8][0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T = 0 , 237 belp( X 2 = x 2) = [0 , 8 , 0 , 1 , 0 , 0 , 0 , 1][0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T = 0 , 25 belp( X 2 = x 3) = [0 , 1 , 0 , 8 , 0 , 1 , 0 , 0][0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T = 0 , 237 belp( X 2 = x 4) = [0 , 0 , 1 , 0 , 8 , 0 , 1 , 0][0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T = 0 , 138 belp( X 2 = x 5) = [0 , 0 , 0 , 1 , 0 , 8 , 0 , 1][0 , 25 , 0 , 25 , 0 , 125 , 0 , 125 , 0 , 25] T = 0 , 138 322 Nedeterminističnost v mobilnih sistemih Celotna porazdelitev za predikcijski korak je bel p( X 2) = [0 , 237 , 0 , 25 , 0 , 237 , 0 , 138 , 0 , 138] T za korekcijski korak pa [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2] T ? [0 , 237 , 0 , 25 , 0 , 237 , 0 , 138 , 0 , 138] T bel( X 2) = = [0 , 2 , 0 , 2 , 0 , 6 , 0 , 6 , 0 , 2][0 , 237 , 0 , 25 , 0 , 237 , 0 , 138 , 0 , 138] T = [0 , 136 , 0 , 143 , 0 , 407 , 0 , 236 , 0 , 079] T 3. Podobno kot v prejšnjih dveh primerih lahko dobimo porazdelitev oz. zaupanje za trenutek k = 3 bel p( X 3) = [0 , 1 , 0 , 131 , 0 , 167 , 0 , 363 , 0 , 237] T bel( X 3) = [0 , 048 , 0 , 063 , 0 , 245 , 0 , 528 , 0 , 115] T 4. Po tretjem koraku se mobilni robot najverjetneje nahaja v četrti celici, z verjetnostjo 52 , 8 %. Druga najverjetnejša celica je tretja celica, kjer se nahaja z verjetnostjo 24 , 5 %. Zaupanja stanj za vse tri časovne trenutke so grafično predstavljena na sliki 6.15. V programu 6.9 je prikazana Matlab koda rešitve. Program 6.9 ./src/prb/example_localization.m 1 disp ( ’ Za č etno zaupanje p ( X0 ) ’) 2 bel_X0 = ones (1 ,5)/5 3 4 P_xxu_null = 0.8; % P ( X = i |X ’= j ,U ’= u ) , i = j + u 5 P_xxu_less = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j +u -1 6 P_xxu_more = 0.1; % P ( X = i |X ’= j ,U ’= u ) , i = j + u +1 7 8 p_ZdX = [0.2 0.2 0.6 0.6 0.2]; % p ( Z = dark | X ) 9 p_ZbX = 1 - p_ZdX ; % p ( Z = b r i g h t | X ) 10 11 bel_X = bel_X0 ; 12 for k =1:3 13 % P r e d i k c i j s k i k o r a k 14 p _ x X u = [ P _ x x u _ l e s s 0 0 P _ x x u _ m o r e P _ x x u _ n u l l ]; % Za U =1 15 b e l p _ X = z e r o s (1 ,5); 16 for i = 1 : 5 17 b e l p _ X ( i ) = p _ x X u * b e l _ X . ’; 18 p _ x X u = p _ x X u ([ end 1: end - 1 ] ) ; 19 end 20 21 % K o r e k c i j s k i k o r a k 22 if k ==1 23 b e l _ X = p _ Z b X .* b e l p _ X ; 24 e l s e 25 b e l _ X = p _ Z d X .* b e l p _ X ; 26 end 27 b e l _ X = b e l _ X / sum ( b e l _ X ); 28 6.4. Bayesov filter 323 29 if k ==1 30 d i s p ( ’ Z a u p a n j i b e l p _ X 1 in b e l _ X 1 ’ ) 31 b e l p _ X 1 = b e l p _ X 32 b e l _ X 1 = b e l _ X 33 e l s e i f k ==2 34 d i s p ( ’ Z a u p a n j i b e l p _ X 2 in b e l _ X 2 ’ ) 35 b e l p _ X 2 = b e l p _ X 36 b e l _ X 2 = b e l _ X 37 e l s e i f k ==3 38 d i s p ( ’ Z a u p a n j i b e l p _ X 3 in b e l _ X 3 ’ ) 39 b e l p _ X 3 = b e l p _ X 40 b e l _ X 3 = b e l _ X 41 d i s p ( ’ N a j m a n j do n a j b o l j v e r j e t e n p o l o ž aj ’ ) 42 [ m , mi ] = s o r t ( b e l _ X ) 43 end 44 end Za č e t n o z a u p a n j e p ( X0 ) b e l _ X 0 = 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 Z a u p a n j i b e l p _ X 1 in b e l _ X 1 b e l p _ X 1 = 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 0 . 2 0 0 0 b e l _ X 1 = 0 . 2 5 0 0 0 . 2 5 0 0 0 . 1 2 5 0 0 . 1 2 5 0 0 . 2 5 0 0 Z a u p a n j i b e l p _ X 2 in b e l _ X 2 b e l p _ X 2 = 0 . 2 3 7 5 0 . 2 5 0 0 0 . 2 3 7 5 0 . 1 3 7 5 0 . 1 3 7 5 b e l _ X 2 = 0 . 1 3 5 7 0 . 1 4 2 9 0 . 4 0 7 1 0 . 2 3 5 7 0 . 0 7 8 6 Z a u p a n j i b e l p _ X 3 in b e l _ X 3 b e l p _ X 3 = 0 . 1 0 0 0 0 . 1 3 0 7 0 . 1 6 8 6 0 . 3 6 3 6 0 . 2 3 7 1 b e l _ X 3 = 0 . 0 4 8 4 0 . 0 6 3 3 0 . 2 4 5 0 0 . 5 2 8 4 0 . 1 1 4 9 N a j m a n j do n a j b o l j v e r j e t e n p o l o ž aj m = 0 . 0 4 8 4 0 . 0 6 3 3 0 . 1 1 4 9 0 . 2 4 5 0 0 . 5 2 8 4 mi = 1 2 5 3 4 324 Nedeterminističnost v mobilnih sistemih belp(X1) bel(X1) 0.5 0.5 0 x 0 1 x2 x3 x4 x5 x1 x2 x3 x4 x5 belp(X2) bel(X2) 0.5 0.5 0 x 0 1 x2 x3 x4 x5 x1 x2 x3 x4 x5 belp(X3) bel(X3) 0.5 0.5 0 x 0 1 x2 x3 x4 x5 x1 x2 x3 x4 x5 Slika 6.15: Porazdelitev trenutnega stanja v treh trenutkih iz primera 6.14 6.5 Kalmanov filter Kalmanov filter [6] je eden najpomembnejših algoritmov za ocenjevanje in napovedovanje stanj, ki se uporablja v številnih aplikacijah na različnih inženirskih področjih, tudi v avtonomnih mobilnih sistemih. Zasnovan je kot ocenjevalnik stanj linearnih sistemov, kjer lahko signali sistema vsebujejo šum. Algoritem sestavljata dva (tipična) koraka, predikcijski in korekcijski korak, ki se izvedeta v vsakem časovnem trenutku. V predikcijskem koraku napovemo najnovejše stanje skupaj z njegovimi negotovostmi. Ko je nova meritev na voljo, se izvede korekcijski korak, kjer se stohastična meritev utežno združi s napovedno oceno stanja, pri čemer imajo manj negotove vrednosti večjo utež. Algoritem je rekur- ziven in omogoča sprotno ocenjevanje trenutnega stanja sistema ob upoštevanju negotovosti sistema in meritve. Klasičen Kalmanov filter predvideva normalno porazdeljene šume, torej je poraz- delitev šuma Gaussova funkcija 1 ( x− µ)2 p( x) = √ e− 12 σ 2 2 πσ 2 kjer je µ srednja vrednost (matematično upanje) in σ 2 varianca. Gaussova funkcija je unimodalna (levo in desno od (edinega) maksimuma funkcija monotono 6.5. Kalmanov filter 325 0.4 0.3 p(x) 0.2 0.1 0 0 5 10 15 x Slika 6.16: Primer porazdelitve zvezne spremenljivke x (polna krivulja), aproksimacija z Gaussovo funkcijo (črtkana krivulja) in aproksimacija s histogramom (pikčasta krivulja) pada proti 0) za razliko od splošnih porazdelitev, ki so običajno večmodalne (obstaja več lokalnih maksimumov). Pri predpostavki, da je porazdelitev zve- znih spremenljivk unimodalna, lahko Kalmanov filter uporabimo za optimalno ocenjevanje stanj. V kolikor pa niso vse spremenljivke unimodalne, je ocena dobljenih stanj suboptimalna. Poleg tega je vprašljiva tudi konvergenca ocene k pravi vrednosti. Bayesov filter nima omenjenih problemov, vendar je njegova uporabnost omejena na enostavna zvezna ali diskretna okolja s končnim številom stanj. Na sliki 6.16 je prikazan primer zvezne porazdelitve, ki ni unimodalna. Zvezna porazdelitev je aproksimirana z Gaussovo funkcijo in histogramom (prostor je razdeljen na diskretne intervale). Aproksimacija z Gaussovo funkcijo se uporablja pri Kalmanovem filtru, histogram pa pri Bayesovem filtru. Bistvo korekcijskega koraka (glejte Bayesov filter (6.21)) je združevanje informacij iz dveh neodvisnih virov, to je iz meritve senzorja in napovedi stanja na osnovi predhodnih ocen stanja. Na primeru 6.15 si poglejmo, kako lahko optimalno združimo dve neodvisni oceni iste spremenljivke x, če poznamo vrednost in varianco (zaupanje) obeh virov. Primer 6.15 Imamo dve neodvisni oceni spremenljivke x. Vrednost prve ocene je x 1 z varianco σ 2, vrednost druge ocene pa je x . Kakšna je optimalna linearna 1 2 z varianco σ 2 2 kombinacija teh dveh ocen, ki predstavljata oceno stanja ˆ x z minimalno varianco? Rešitev Ocena optimalne vrednosti spremenljivke x je linearna kombinacija dveh meritev ˆ x = ω 1 x 1 + ω 2 x 2 326 Nedeterminističnost v mobilnih sistemih kjer sta parametra ω 1 in ω 2 iskani uteži, ki izpolnjujeta pogoj ω 1 + ω 2 = 1. Optimalni vrednosti uteži minimizirata varianco σ 2 optimalne ocene ˆ x. Varianca ocene je torej σ 2 = E(ˆ x − E{ˆ x})2 = E( ω 1 x 1 + ω 2 x 2 − E{ ω 1 x 1 + ω 2 x 2})2 = E( ω 1 x 1 + ω 2 x 2 − ω 1E{ x 1} − ω 2E{ x 2})2 n = E ( ω 1 ( x 1 − E{ x 1}) + ω 2 ( x 2 − E{ x 2}))2o n o = E ω 2 ( x ( x 1 1 − E{ x 1})2 + ω 2 2 2 − E{ x 2})2 + 2 ω 1 ω 2( x 1 − E{ x 1})( x 2 − E{ x 2}) = ω 2E( x E( x 1 1 − E{ x 1})2 + ω 2 2 2 − E{ x 2})2 + 2 ω 1 ω 2E{( x 1 − E{ x 1})( x 2 − E{ x 2})} = ω 2 σ 2 + ω 2 σ 2 + 2 ω 1 1 2 2 1 ω 2E{( x 1 − E{ x 1})( x 2 − E{ x 2})} Ker sta spremenljivki x 1 in x 2 neodvisni, sta neodvisni tudi razliki x 1 − E{ x 1} in x 2 − E{ x 2} ter velja E{( x 1 − E{ x 1})( x 2 − E{ x 2})} = 0. Torej lahko zapišemo σ 2 = ω 2 σ 2 + ω 2 σ 2 1 1 2 2 ali po uvedbi ω 2 = ω in ω 1 = 1 − ω σ 2 = (1 − ω)2 σ 2 + ω 2 σ 2 1 2 Iščemo vrednost uteži ω, ki minimizira varianco, in jo lahko pridobimo s pomočjo odvoda variance ∂ σ 2 = −2(1 − ω) σ 2 + 2 ωσ 2 = 0 ∂ω 1 2 kjer je rešitev σ 2 ω = 1 σ 2 + σ 2 1 2 Končna ocena (z minimalno varianco) je σ 2 x 1 + σ 2 x 2 ˆ x = 2 1 (6.22) σ 2 + σ 2 1 2 najmanjša varianca ocene pa σ 2 σ 2 1 1 −1 σ 2 = 1 2 = + (6.23) σ 2 + σ 2 σ 2 σ 2 1 2 1 2 Dobljeni rezultati potrjujejo, da vir z manjšo varianco (večje zaupanje) bolj prispeva h končni oceni in obratno. 6.5. Kalmanov filter 327 Primer 6.16 V določenem trenutku je podana začetna ocena stanja x = 2 z varianco σ 2 = 4. Nato s senzorjem izmerimo vrednost stanja z = 4 z varianco senzorja σ 2 = 1. z Gaussovi porazdelitvi stanja in meritve sta prikazani na sliki 6.17. 0.4 p(x) p(z|x) p(z|x) 0.3 0.2 p(x), 0.1 0−6 −4 −2 0 2 4 6 8 10 x, z Slika 6.17: Porazdelitev stanja (črtkana krivulja) in meritve (krivulja črta-pika) Kakšna je posodobljena optimalna ocena stanja, ki združuje informacijo predhodne ocene stanja in trenutne meritve? Kakšna je porazdelitev posodobljene optimalne ocene stanja? Rešitev Na podlagi slike 6.17 lahko predvidimo, da bo srednja vrednost posodobljenega stanja x 0 bližje srednji vrednosti meritve, ker ima le-ta manjšo varianco (negotovost). Z upoštevanjem (6.22) dobimo posodobljeno oceno stanja σ 2 x + σ 2 z x 0 = z = 3 , 6 σ 2 + σ 2 z Varianca posodobljene ocene σ 02 je manjša od obeh predhodnih varianc, saj integracija informacij predhodne ocene in meritve zmanjšuje negotovost posodobljene ocene. Varianca posodobljene ocene, izračunana s pomočjo (6.23), je 1 1 −1 σ 02 = + = 0 , 8 σ 2 σ 2 z in standardna deviacija je p σ 0 = σ 02 = 0 , 894 Posodobljena porazdelitev p( x| z) stanja po opravljeni korekciji na osnovi meritve je prikazana na sliki 6.18. 328 Nedeterminističnost v mobilnih sistemih 0.4 p(x) p(x|z) p(z|x) 0.3 p(x|z) p(z|x), 0.2 0.1 p(x), 0−6 −4 −2 0 2 4 6 8 10 x, z Slika 6.18: Porazdelitev začetnega stanja (črtkana krivulja), meritve (krivulja črta-pika) in posodobljenega stanja (polna krivulja) Za izpeljavo algoritma rekurzivnega ocenjevanja stanja uporabimo ugotovitve iz primera 6.15. V vsakem časovnem trenutku s pomočjo senzorja pridobimo novo meritev stanja z( k) = x( k) + n( k), kjer je n( k) šum meritve. Predpostavimo, da je varianca meritve σ 2( k) znana. Posodobljena optimalna ocena stanja je z kombinacija prejšnje ocene stanja ˆ x( k) in trenutne meritve z( k) ˆ x( k + 1) = (1 − ω) ˆ x( k) + ω( k) z( k) = ˆ x( k) + ω( z( k) − ˆ x( k)) Varianca posodobljenega stanja je σ 2( k) σ 2( k) σ 2( k + 1) = z = (1 − ω) σ 2( k) σ 2( k) + σ 2( k) z kjer je σ 2( k) ω = σ 2( k) + σ 2( k) z Glede na podano začetno oceno stanja x, ˆ x(0) in njeno varianco σ 2(0) lahko opti- malno združimo meritve z(1) , z(2) , . . . , z( k) tako, da ocenimo trenutno vrednost stanja in njegovo varianco. To predstavlja osnovno idejo korekcijskega koraka Kalmanovega filtra. Predikcijski korak Kalmanovega filtra podaja napoved stanja ob znani vhodni akciji. Izhodiščna ocena stanja ˆ x( k) ima porazdelitev z varianco σ 2( k). Na enak način ima akcija u( k), ki izvede premik iz stanja x( k) v x( k + 1), porazdelitev (negotovost premika) σ 2 ( k). Na primeru 6.17 si poglejmo vrednost stanja in u njegove variance po izvedeni akciji (premiku). 6.5. Kalmanov filter 329 Primer 6.17 Imamo izhodiščno oceno stanja ˆ x( k) z varianco σ 2( k). Nato izvedemo akcijo u( k), ki predstavlja neposreden premik stanja z negotovostjo (varianco) σ 2( k). u Kakšna je vrednost ocene stanja in njene negotovosti po premiku? Rešitev Posodobljena ocena stanja po premiku je ˆ x( k + 1) = ˆ x( k) + u( k) in negotovost te ocene je n σ 2( k + 1) = E (ˆ x( k + 1) − E{ˆ x( k + 1)})2o n = E (ˆ x( k) + u( k) − E{ˆ x( k) + u( k)})2o n = E ((ˆ x( k) − E{ˆ x( k)}) + ( u( k) − E{ u( k)}))2o (6.24) = E(ˆ x( k) − E{ˆ x( k)})2 + ( u( k) − E{ u( k)})2 + + E{2(ˆ x( k) − E{ˆ x( k)})( u( k) − E{ u( k)})} = σ 2( k) + σ 2 ( k)+ u + E{2 (ˆ x( k) − E{ˆ x( k)})( u( k) − E{ u( k)}))} Ker sta ˆ x in u neodvisna, velja E{2 (ˆ x( k) − E{ˆ x( k)}) ( u( k) − E{ u( k)})} = 0 in (6.24) se poenostavi v σ 2( k + 1) = σ 2( k) + σ 2( k) u Algoritem za poenostavljeno izvedbo Kalmanovega filtra Kalmanov filter za enostaven primer z enim stanjem je podan v algoritmu 4, kjer veličine z oznako (·) k| k−1 predstavljajo ocenjene vrednosti iz predikcijskega koraka in veličine z oznako (·) k| k vrednosti iz korekcijskega koraka. Zavoljo boljše preglednosti uporabimo zapis u( k − 1) = uk−1 in z( k) = zk. Kalmanov filter je sestavljen iz dveh korakov (predikcija in korekcija), ki se izvajata eden za drugim v zanki. V predikcijskem koraku uporabimo samo znano akcijo in določimo (napovemo) vrednost stanja v naslednjem časovnem trenutku. Torej iz začetnega zaupanja določimo novo zaupanje, katerega negotovost je večja od začetne. V korekcijskem koraku pa uporabimo meritev za izboljšanje napovedanega zaupanja tako, da ima ocena novega (popravljenega) stanja manjšo negotovost kot prejšnje zaupanje. V obeh korakih sta potrebna samo dva vhoda: v predikcijskem koraku morata biti znana vrednost predhodnega stanja ˆ xk−1| k−1 330 Nedeterminističnost v mobilnih sistemih Algorithm 4 Kalmanov filter za eno stanje function 2 2 Kalman_filter(ˆ xk−1| k−1, uk−1, zk, σ 2 , σ , σ ) k−1| k−1 uk−1 z k Predikcijski korak: ˆ xk| k−1 ← ˆ xk−1| k−1 + uk−1 σ 2 ← σ 2 + σ 2 k| k−1 k−1| k−1 uk−1 Korekcijski korak: 2 σ ω k| k−1 k ← σ 2 2 k| k−1+ σz k ˆ xk| k ← ˆ xk| k−1 + ωk( zk − ˆ xk| k−1) σ 2 ← (1 − ω k| k k ) σ 2 k| k−1 return ˆ xk| k, σ 2 k| k end function in izvedena akcija uk−1, v predikcijskem koraku pa napovedano stanje ˆ xk| k−1 in meritev zk. Podana mora biti tudi varianca premika stanja σ 2 , varianca k−1| k−1 vhodne akcije σ 2 2 u in varianca meritve σ . k−1 z k Primer 6.18 Imamo mobilnega robota, ki se lahko premika samo v eni dimenziji. Njegova začetna pozicija je neznana (slika 6.19). Predpostavimo začetno pozicijo ˆ x 0 = 3 z veliko varianco σ 2 = 100 (dejanske pozicije x 0 0 = 0 ne poznamo). Mobilni robot se v vsakem trenutku k = 0 , . . . , 4 premakne za u 0:4 = (2 , 3 , 2 , 1 , 1) enot, nato pa izvedemo meritve pozicije robota z 1:5 = (2 , 5 , 7 , 8 , 9) v trenutkih k = 1 , . . . , 5. Pomik in meritev sta motena z normalno porazdeljenim belim šumom, kar lahko opišemo s konstantno negotovostjo pomika σ 2 = 2 in u negotovostjo meritve σ 2 = 4. z x=? z u x 0 1 2 3 4 5 6 7 8 9 Slika 6.19: Lokalizacija mobilnega robota v enodimenzionalnem prostoru z neznano začetno pozicijo Kakšna je ocenjena pozicija robota in negotovost te ocene? 6.5. Kalmanov filter 331 Rešitev Dani problem lokalizacije mobilnega robota rešimo z izvajanjem algoritma 4. V prvem časovnem trenutku ( k = 1) najprej izračunamo predikcijo stanja in varianco ˆ x 1|0 = ˆ x 0|0 + u 0 = 3 + 2 = 5 σ 2 = σ 2 + σ 2 = 100 + 2 = 102 1|0 0|0 u nato pa izvedemo korekcijski korak Kalmanovega filtra v prvem časovnem tre- nutku k = 1 in dobimo σ 21|0 102 ω 1 = = = 0 , 962 σ 2 + σ 2 102 + 4 1|0 z ˆ x 1|1 = ˆ x 1|0 + ω 1( z 1 − ˆ x 1|0) = 5 + 0 , 962(2 − 5) = 2 , 113 σ 2 = (1 − ω = (1 − 0 , 962)102 = 3 , 849 1|1 1) σ 2 1|0 Predikcijo in korekcijo izvedemo še za ostale časovne trenutke. Za predikcijske korake dobimo ˆ x 1:5|0:4 = (5 , 00 , 5 , 11 , 7 , 05 , 8 , 02 , 9 , 01) σ 2 = (102 , 5 , 85 , 4 , 38 , 4 , 09 , 4 , 02) 1:5|0:4 in za korekcijske korake ˆ x 1:5|1:5 = (2 , 11 , 5 , 05 , 7 , 02 , 8 , 01 , 9 , 01) σ 2 = (3 , 85 , 2 , 38 , 2 , 09 , 2 , 02 , 2 , 01) 1:5|1:5 Dobljeni rezultati kažejo, da lahko pozicijo mobilnega robota določimo po ne- kaj korakih z negotovostjo 2, kar ustreza negotovosti predikcije in meritve −1 1 + 1 = 2 , 01. Negotovost predikcijske ocene pozicije konvergira proti σ 2 5 σ 2 |4 z 4, kar je v skladu z negotovostjo korekcije iz prejšnjega časovnega trenutka in meritve σ 2 + σ 2 = 4 , 02. 4|4 u 6.5.1 Kalmanov filter v matrični obliki Sisteme z več vhodi, stanji in izhodi lahko za večjo preglednost podamo v matrični obliki. Splošni linearni sistem zapišemo v prostoru stanj kot x( k + 1) = Ax( k) + Bu( k) + F w( k) (6.25) z( k) = Cx( k) + v( k) kjer je x vektor stanj, u je vhodni vektor (akcija), z je izhodni vektor (meritev), A je sistemska matrika, B je vhodna matrika, F je vhodna matrika šuma, C 332 Nedeterminističnost v mobilnih sistemih je izhodna matrika, w( k) je vektor procesnega šuma in v je vektor izhodnega (merilnega) šuma. V kolikor se šum w doda vhodu sistema u, velja F = B. Predpostavimo, da sta procesni šum w( k) in merilni šum v( k) medsebojno neodvisna bela šuma z ničelno srednjo vrednostjo in kovariančnima matrikama Q k = Ew( k)w T ( k) in R k = Ev( k)v T ( k) . Porazdelitev stanj x, ki so motena z belim Gaussovim šumom, podamo v matrični obliki − 1 p(x) = det (2 π P ) 2 e− 12 (x−µ) T P −1(x−µ) kjer je P kovariančna matrika napake ocene stanj. Kalmanov filter predstavlja pristop za filtriranje in ocenjevanje zveznih stanj linearnih sistemov, ki so motena z normalnim šumom. Porazdelitev šuma je podana z Gaussovo funkcijo (Gaussov šum). Vhodni in merilni šum vplivata na notranja stanja sistema, ki jih želimo oceniti. V primeru linearnega modela sistema je tudi filtriran šum preko linearnega modela (npr. od vhodov do stanj) Gaussov šum. Torej mora biti sistem linearen, saj to zagotavlja Gaussovo porazdelitev šuma na stanjih, kar je izhodišče pri izpeljavi Kalmanovega filtra. Kalmanov filter bo konvergiral k pravi oceni stanj le v primeru linearnih sistemov, ki so moteni z Gaussovim šumom. Kalmanov filter za linearni sistem (6.25) ima predikcijski korak ˆ x k| k−1 = A ˆ x k−1| k−1 + Bu k−1 (6.26) P k| k−1 = AP k−1| k−1A T + F Q k−1F T in korekcijski korak K −1 k = P k| k−1C T CP k| k−1C T + R k ˆ x (6.27) k| k = ˆ x k| k−1 + K k(z k − C ˆ x k| k−1) P k| k = P k| k−1 − K k CP k| k−1 V predikcijskem koraku določimo napovedno oceno ˆ x k| k−1, ki temelji na predho- dni oceni ˆ x k−1| k−1, dobljeni iz meritev do trenutka k − 1, in vhodu u( k − 1). V korekcijskem koraku pa izračunamo trenutno oceno ˆ x k| k, ki temelji na meritvah do trenutka k. Korekcijo stanja izvedemo z izračunom razlike med dejansko in ocenjeno meritvijo z k − C ˆ x k| k−1. Ta razlika je znana tudi kot inovacija ali residuum meritve. Celotna korekcija stanja se izračuna kot produkt Kalmanovega ojačenja K k in inovacije. Predikcijski korak lahko ovrednotimo vnaprej, medtem ko čakamo na novo meritev v trenutku k. Opazimo podobnost matričnega zapisa (6.26) in (6.27) z zapisom v algoritmu 4. Izpeljimo izraz za kovariančno matriko napake ocene stanj v predikcijskem koraku 6.5. Kalmanov filter 333 Kalmanovega filtra P k| k−1 = E(x k − ˆ x k| k−1)(x k − ˆ x k| k−1) T = covx k − ˆ x k| k−1 = covAx k−1 + Bu k−1 + F w k−1 − A ˆ x k−1| k−1 − Bu k−1 = covAx k−1 + F w k−1 − A ˆ x k−1| k−1 = covA(x k−1 − ˆ x k−1| k−1) + F w k−1 = covA(x k−1 − ˆ x k−1| k−1) + cov{F w k−1} = E(A(x k−1 − ˆ x k−1| k−1))(A(x k−1 − ˆ x k−1| k−1)) T + + E(F w k−1)(F w k−1) T = E(A(x k−1 − ˆ x k−1| k−1)(x k−1 − ˆ x k−1| k−1) T A T + + EF w k−1w T F T k−1 = AP k−1| k−1A T + F Q k−1F T kjer smo v šesti vrstici upoštevali, da je procesni (vhodni) šum w k v trenutku k neodvisen od napake ocene stanj v prejšnjem trenutku (x k−1 − ˆ x k−1| k−1). Izpeljimo še izraz za kovariančno matriko napake ocene stanj v korekcijskem delu Kalmanovega filtra P k| k = E(x k − ˆ x k| k)(x k − ˆ x k| k) T = covx k − ˆ x k| k = covx k − ˆ x k| k−1 − K k(z k − C ˆ x k| k−1) = covx k − ˆ x k| k−1 − K k(Cx k + v k − C ˆ x k| k−1) = cov(I − K k C )(x k − ˆ x k| k−1) − K k v k = cov(I − K k C)(x k − ˆ x k| k−1) + cov{K k v k} = (I − K k C)P k| k−1(I − K k C) T + K k R k K T k kjer smo v šesti vrstici upoštevali, da je merilni šum v k nekoreliran z ostalimi členi. Dobljeni izraz za kovariančno matriko P k| k je splošen in ga lahko uporabimo za poljubno ojačenje K k. Vendar pa izraz za P k| k v (6.27) velja le za optimalno ojačenje (Kalmanovo ojačenje), ki minimizira povprečni kvadratni n 2o pogrešek korekcije E x k − ˆ x k| k . To je ekvivalentno minimizaciji vsote vseh diagonalnih elementov kovariančne matrike korekcije P k| k. Splošni izraz za P k| k lahko razširimo in preuredimo P k| k = (I − K k C)P k| k−1(I − K k C) T + K k R k K T k = P k| k−1 − K k CP k| k−1 − P k| k−1C T K T + K + K k k C P k| k−1C T K T k k R k K T k = P k| k−1 − K k CP k| k−1 − P k| k−1C T K T + K k k (C P k| k−1C T + R k )K T k = P k| k−1 − K k CP k| k−1 − P k| k−1C T K T + K k k S k K T k kjer S k = CP k| k−1C T + R k predstavlja kovariančno matriko inovacije (S k = covz k − C ˆ x k| k−1 ). Vsota diagonalnih členov P k| k je minimalna, ko je odvod 334 Nedeterminističnost v mobilnih sistemih P k| k po K k enak 0 ∂ P k| k = −2(CP ∂ K k| k−1) T + 2K k S k = 0 k kar vodi do optimalnega ojačenja v (6.27) K k = P k| k−1C T S−1 = P k k| k−1C T (C P k| k−1C T + R k )−1 Kovariančno matriko korekcije pri optimalnem ojačenju lahko izpeljemo, če optimalno ojačenje z desne strani pomnožimo s S k K T in vstavimo v izraz za k P k| k P k| k = P k| k−1 − K k CP k| k−1 − P k| k−1C T K T + K k k S k K T k = P k| k−1 − K k CP k| k−1 − P k| k−1C T K T + P k k| k−1C T K T k = P k| k−1 − K k CP k| k−1 Primer 6.19 Mobilni robot se vozi po ravnini in meri svojo pozicijo z GPS-om desetkrat v sekundi (čas vzorčenja Ts = 0 , 1 s). Meritev položaja je motena z Gaussovim šumom, ki ima varianco 10 m2. Robot se premika s hitrostjo 1 m / s v smeri x in s hitrostjo 0 m / s v smeri y. Varianca Gaussovega šuma hitrosti je 0 , 1 m2 / s2. Na začetku opazovanja se mobilni robot nahaja v izhodišču x = [0 , 0] T , vendar je naša ocena začetne pozicije ˆ x = [3 , 3] T z začetno varianco " # 10 0 P0 = 0 10 Kakšen je časovni potek ocene pozicije in njene variance? Rešitev Do rešitve lahko pridemo s pomočjo simulacije v okolju Matlab. Določimo model gibanja robota, kjer ocena stanja predstavlja pozicijo robota na ravnini ˆ x k| k−1 = [ xk, yk] T , vhod u = [ vx, vy] T pa predstavlja hitrost robota v smereh x in y. Torej je model za predikcijo stanja sistema " # " # 1 0 T ˆ x s 0 k| k−1 = ˆ x k−1| k−1 + u 0 1 0 Ts Model meritve pozicije z GPS-om pa je " # 1 0 ˆ z k = ˆ x k| k−1 0 1 Matlab koda rešitve je podana v programu 6.10, rezultati simulacije pa so grafično prikazani na slikah 6.20, 6.21 in 6.22. Rezultati simulacije potrjujejo, da 6.5. Kalmanov filter 335 dobljena ocena pozicije konvergira k pravi oceni pozicije robota iz napačne začetne ocene. Rezultati so pričakovani, saj je srednja vrednost merjenega šuma nič. Vendar se varianca ocene s časom zmanjšuje in pade veliko pod varianco meritve (kinematičnemu modelu zaupamo bolj). Kalmanov filter omogoča optimalno združitev podatkov iz različnih virov (notranji kinematični model in zunanji model meritve), če so znane variance teh virov. Bralci lahko prosto eksperimentirate s parametri sistema in opazujete potek variance ocene in stopnjo konvergence. Program 6.10: Rešitev primera 6.19 ./src/prb/example_kf1.m 1 % Linearni model sistema v prostoru stanj 2 Ts = 0.1; % Ra č unski korak 3 A = [1 0; 0 1]; 4 B = [ Ts 0; 0 Ts ]; 5 C = [1 0; 0 1]; 6 F = B ; % Š um je dodan na vhod . 7 8 xTrue = [0; 0]; % Prava za č etna vrednost stanj 9 x = [3; 3]; % Za č etna ocena stanj 10 P = diag ([10 10]); % Varianca za č etne ocene stanj 11 Q = diag ([1 1]/10); % Varianca š uma modela gibanja 12 R = diag ([10 10]); % Varianca š uma meritev GPS 13 14 % Zanka 15 N = 150; 16 for k = 1: N 17 u = [1; 0]; % U k a z i za g i b a n j e 18 19 % S i m u l a c i j a p r a v e g a p o l o ž aja r o b o t a in p r a v i h m e r i t e v 20 x T r u e = A * x T r u e + B * u + F * s q r t ( Q )* r a n d n (2 , 1); 21 z T r u e = C * x T r u e + s q r t ( R )* r a n d n (2 , 1); 22 23 % O c e n a p o l o ž aja na p o d l a g i z n a n i h u k a z o v in m e r i t e v 24 % %% P r e d i k c i j a 25 x P r e d = A * x + B * u ; 26 P P r e d = A * P * A . ’ + F * Q * F . ’; 27 28 % %% K o r e k c i j a 29 K = P P r e d * C . ’/( C * P P r e d * C . ’ + R ); 30 x = x P r e d + K *( z T r u e - C * x P r e d ); 31 P = P P r e d - K * C * P P r e d ; 32 end 336 Nedeterminističnost v mobilnih sistemih 8 6 4 2 y 0 −2 −4 −6 −8 0 5 10 15 20 x Slika 6.20: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija z meritvami (pikice na pikčasti krivulji) iz primera 6.19. Končna pozicija robota je označena s krogom. 20 20 ] 10 ] 10 [m [m x y 0 0 −10 −10 0 5 10 15 0 5 10 15 t [s] t [s] Slika 6.21: Prava pozicija (črtkana krivulja) in ocena pozicije (polna krivulja) mobilnega robota na podlagi meritev (pikčasta krivulja) iz primera 6.19 6.5. Kalmanov filter 337 6 6 2 ] 4 2 ] 4 [m [m (x) (y) var 2 var 2 0 0 0 5 10 15 0 5 10 15 t [s] t [s] Slika 6.22: Časovni potek variance pozicije mobilnega robota iz primera 6.19 338 Nedeterminističnost v mobilnih sistemih 6.5.2 Razširjeni Kalmanov filter Kalmanov filter je razvit za linearne sisteme, vse motnje in šumi pa morajo biti opisljivi z (normalno) Gaussovo porazdelitvijo. Šum z Gaussovo porazdelitvijo je invarianten za linearne transformacije. Če Gaussov šum transformiramo z linearno funkcijo, je dobljeni šum še vedno Gaussov, spremenili so se samo njegovi parametri, ki jih lahko eksplicitno izračunamo iz znane linearne funkcije. Ravno zaradi tega je Kalmanov filter računsko učinkovit. V primeru transformacije vhodnega Gaussovega šuma z nelinearno funkcijo, izhodni šum ni več Gaussov, čeprav ga lahko še vedno aproksimiramo z Gaussovim šumom. Če je katerokoli prehajanje stanja ali izhodna enačba sistema nelinearna funkcija, osnovni Kalmanov filter ne zagotavlja več optimalne ocene stanja. Problem nelinearnosti lahko rešimo z uporabo razširjenega Kalmanovega filtra (EKF, angl. extended Kalman filter ), kjer nelinearnosti modela aproksimiramo z lokalnimi linearnimi modeli. Lokalni linearni model pridobimo iz nelinearnega sistema s pomočjo linearizacije (razvoj v Taylorjevo vrsto prvega reda) okoli trenutne ocene stanja. Z linearizacijo dobimo občutljivostne matrike (Jacobijeve matrike) za trenutne vrednosti ocenjenih stanj in meritev. Dobljeni linearni model omogoča izračun približka šuma, ki ni nujno Gaussov, z Gaussovo porazdelitvijo. Uporaba linearizacije pri modeliranju šuma omogoča računsko učinkovito izvaja- nje razširjenega Kalmanovega filtra, kar je razlog za njegovo pogosto uporabo v praksi. Točnost linearne aproksimacije je odvisna od variance šuma (pri velikih negotovostih ali amplitudah šuma je linearni približek slabši, saj je signal (morda) izven linearnega območja) in stopnje nelinearnosti. Zaradi pogreška, ki ga v sistem vnaša linearizacija, se lahko konvergenca filtra poslabša ali pa ocena sploh ne konvergira k pravi rešitvi. Nelinearni sistem lahko zapišemo v splošni obliki x k = f (x k−1 , u k−1 , w k−1) (6.28) z k = h (x k) + v k kjer se lahko šum w k pojavi na vhodu sistema ali pa vpliva neposredno na stanja. Razširjeni Kalmanov filter za nelinearni sistem (6.28) je podan s predikcijskim korakom ˆ x k| k−1 = f ˆ x k−1| k−1 , u k−1 (6.29) P k| k−1 = AP k−1| k−1A T + F Q k−1F T in korekcijskim korakom K −1 k = P k| k−1C T CP k| k−1C T + R k ˆ x (6.30) k| k = ˆ x k| k−1 + K k(z k − ˆ z k) P k| k = P k| k−1 − K k CP k| k−1 V predikcijskem koraku (6.29) uporabimo nelinearni model prehajanja stanj za izračun ocene predikcije stanja. Za izračun kovariančne matrike šuma iz 6.5. Kalmanov filter 339 modela stanj (6.28) določimo Jacobijevo matriko A, ki opisuje prehajanje šuma iz prejšnjih na trenutna stanja, in Jacobijevo matriko F , ki opisuje širjenje šuma od vhodov na stanja ∂ f A = ∂ ˆ x (ˆx k−1| k−1 , u k−1) ∂ f F = ∂ w (ˆx k−1| k−1 , u k−1) V korekcijskem koraku (6.30) določimo oceno meritve na podlagi predikcijske ocene stanja ˆ z k = h ˆ x k| k−1 . Določimo tudi Jacobijevo matriko C, ki opisuje širjenje šuma iz stanj na izhode (meritve) ∂ h C = (6.31) ∂ x (ˆx k| k−1) Kovariančni matriki šuma sta Q k = Ew( k)w T ( k) in R k = Ev( k)v T ( k) . V mnogih aplikacijah so uporabili razširjeni Kalmanov filter za reševanje problema lokalizacije kolesnih mobilnih robotov [7, 8] in gradnje zemljevida [9]. Primer 6.20 Kolesni mobilni robot z diferencialnim pogonom se premika po ravnini. Vhoda sta translatorna hitrost vk in kotna hitrost ωk, ki sta motena z Gaussovim šumom z variancama var{ vk} = 0 , 1 m2 / s2 in var{ ωk} = 0 , 1 rad2 / s2. Mobilni robot ima senzor, s katerim lahko izmeri razdaljo do značke, ki se nahaja v izhodišču globalnega koordinatnega sistema. Robot je opremljen tudi s kompa-som, ki omogoča merjenje orientacije mobilnega robota (odklon). Predstavljena situacija je prikazana na sliki 6.23. Meritev razdalje je motena z Gaussovim šumom z varianco 0 , 5 m2, meritev kota pa z Gaussovim šumom z varianco 0 , 3 rad2. Na začetku opazovanja je prava lega robota x0 = [1 , 2 , π/ 6] T , ocenjena lega robota pa je ˆ x0 = [3 , 0 , 0] T z začetno varianco   9 0 0 P   0 = 0 9 0   0 0 0 , 6 Kakšen je časovni potek ocene lege mobilnega robota ( ˆ x k| k−1 = [ xk, yk, ϕk] T ) in varianca ocene, če ob vsakem času vzorčenja Ts = 0 , 1 s pošljemo robotu ukaz u = [ vk, ωk] T = [0 , 5 , 0 , 5] T ? 340 Nedeterminističnost v mobilnih sistemih 4 3 ϕ 2 y 1 d 0 M −1 −2 0 2 4 x Slika 6.23: Postavitev, predstavljena v primeru 6.20. Mobilni robot ima senzor za merjenje razdalje do značke M (nameščena v izhodišču globalnega koordinatnega sistema) in kompas za določitev orientacije robota v globalnem koordinatnem sistemu (odklon). Rešitev Do rešitve lahko pridemo s pomočjo simulacije v okolju Matlab. Določimo model premikanja mobilnega robota. V tem primeru je kinematični model nelinearen   Tsvk−1 cos( ϕk−1) ˆ x   k| k−1 = ˆ x k−1| k−1 + Tsvk−1 sin( ϕk−1)   Tsωk−1 Model meritve razdalje in kota pa je "p # x 2 + y 2 ˆ z k k k = ϕk Matlab koda rešitve je predstavljena v programu 6.11. Rezultati simulacije so prikazani na slikah 6.24 – 6.28. Rezultati potrjujejo, da ocenjena lega konvergira k pravi legi mobilnega robota, čeprav je bila prvotna ocena pristranska. Členi inovacije se ustalijo okoli ničle. 6.5. Kalmanov filter 341 Program 6.11: Izvedba rešitve primera 6.20 ./src/prb/example_ekf1default.m 1 Ts = 0.1; % Ra č unski korak 2 xTrue = [1; 2; pi /6]; % Prava za č etna lega 3 x = [3; 0; 0]; % Ocena za č etne lege 4 P = diag ([9 9 0.6]); % Za č etna kovarian č na matrika ocene lege 5 Q = diag ([0.1 0.1]); % Kovarian č na matrika š uma modela gibanja 6 R = diag ([0.5 0.3]); % Kovarian č na matrika š uma merjenja razdalje in 7 % k o t a 8 en abl eN ois e = 1; % Omogo č i š um : 0 ali 1 9 N = 300; % Š tevilo s i m u l a c i j s k i h korakov 10 11 % Zanka 12 for k = 1: N 13 u = [ 0 . 5 ; 0 . 5 ] ; % U k a z i ( t r a n s l a t o r n a in k o t n a h i t r o s t ) 14 u N o i s y = u + s q r t ( Q )* r a n d n (2 , 1)* e n a b l e N o i s e ; 15 16 % S i m u l a c i j a p r a v i h s t a n j ( l e g e ) r o b o t a 17 x T r u e = x T r u e + Ts *[ u N o i s y ( 1 ) * cos ( x T r u e ( 3 ) ) ; ... 18 u N o i s y ( 1 ) * sin ( x T r u e ( 3 ) ) ; ... 19 u N o i s y ( 2 ) ] ; 20 x T r u e (3) = w r a p T o P i ( x T r u e ( 3 ) ) ; 21 22 % S i m u l a c i j a m e r i t e v s š u m o m 23 z T r u e = [ s q r t ( x T r u e ( 1 ) ^ 2 + x T r u e ( 2 ) ^ 2 ); ... 24 x T r u e ( 3 ) ] + s q r t ( R )* r a n d n (2 , 1)* e n a b l e N o i s e ; 25 z T r u e (1) = abs ( z T r u e ( 1 ) ) ; 26 z T r u e (2) = w r a p T o P i ( z T r u e ( 2 ) ) ; 27 28 % %% P r e d i k c i j a ( o c e n a l e g e in h i t r o s t i g l e d e na z n a n e v h o d e ) 29 x P r e d = x + Ts *[ u ( 1 ) * cos ( x ( 3 ) ) ; ... 30 u ( 1 ) * sin ( x ( 3 ) ) ; ... 31 u ( 2 ) ] ; 32 x P r e d (3) = w r a p T o P i ( x P r e d ( 3 ) ) ; 33 34 % J a c o b i j e v e m a t r i k e 35 A = [1 0 - Ts * u ( 1 ) * sin ( x ( 3 ) ) ; ... 36 0 1 Ts * u ( 1 ) * cos ( x ( 3 ) ) ; ... 37 0 0 1]; 38 F = [ Ts * cos ( x ( 3 ) ) 0; ... 39 Ts * sin ( x ( 3 ) ) 0; ... 40 0 Ts ]; 41 P P r e d = A * P * A . ’ + F * Q * F . ’; 42 43 % O c e n j e n e m e r i t v e 44 z = [ s q r t ( x P r e d ( 1 ) ^ 2 + x P r e d ( 2 ) ^ 2 ) ; ... 45 x P r e d ( 3 ) ] ; 46 47 % %% K o r e k c i j a 48 d = s q r t ( x P r e d ( 1 ) ^ 2 + x P r e d ( 2 ) ^ 2 ) ; 49 C = [ x P r e d ( 1 ) / d x P r e d ( 2 ) / d 0 ; . . . 50 0 0 1]; 51 K = P P r e d * C . ’/( C * P P r e d * C . ’ + R ); 52 i n o v = z T r u e - z ; 53 i n o v (2) = w r a p T o P i ( i n o v ( 2 ) ) ; 54 x = x P r e d + K * i n o v ; 55 P = P P r e d - K * C * P P r e d ; 56 end 342 Nedeterminističnost v mobilnih sistemih 5 4 3 y 2 1 0 M −2 0 2 4 6 x Slika 6.24: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija iz primera 6.20 6.5. Kalmanov filter 343 4 ] 2 [mx 0 0 5 10 15 20 25 30 t [s] 4 ] 2 [my 0 0 5 10 15 20 25 30 t [s] 4 2 ] [rad 0 ϕ−2 −40 5 10 15 20 25 30 t [s] Slika 6.25: Ocenjena lega (polna krivulja) in pravo stanje (črtkana krivulja) mobilnega robota z začetnim neničelnim pogreškom ocene iz primera 6.20 6 4 2 ] 4 ] [m 0 d [rad 2 α−2 0 −4 0 10 20 30 0 10 20 30 t [s] t [s] Slika 6.26: Meritvi razdalje in kota iz primera 6.20 344 Nedeterminističnost v mobilnih sistemih 10 10 0.2 2 ] 2 ] 0.15 [m [m 2 ] 5 5 0.1 (x) (y) [rad ϕ var var 0.05 0 0 0 0 10 20 30 0 10 20 30 0 10 20 30 t [s] t [s] t [s] Slika 6.27: Variance ocene lege robota iz primera 6.20 2 1 tion 0 Innova−1 −20 5 10 15 20 25 30 t [s] Slika 6.28: Časovni potek inovacije iz primera 6.20 Če stanja sistema niso neposredno merljiva (kot v tem primeru), se pojavi vprašanje spoznavnosti sistema. Analiza spoznavnosti lahko običajno zagotovi zadostne pogoje za spoznavnost sistema. Vendar moramo analizo opraviti pred načrtovanjem ocenjevanja stanj, saj nam to lahko pomaga pri izbiri ustrezne množice merilnih signalov, ki omogočajo ocenjevanje. Spoznavnost sistema lahko preverimo z uporabo naprednih matematičnih orodij ali pa izberemo preprost grafični pristop, ki temelji na definiciji nerazpoznavnih stanj (glejte poglavje 6.3.3). Analiza spoznavnosti sistema iz tega primera je prikazana na sliki 6.29, iz katere je razvidno, da so stanja sistema običajno razpoznavna, razen v nekaterih posebnih primerih (slika 6.29d). Če stanja sistema opazujemo dovolj dolgo in so regulirne veličine mobilnega robota ustrezno vzbujene, je sistem spoznaven. 6.5. Kalmanov filter 345 4 4 R2 2 R2 2 R1 y R3 R1 y 0 R3 0 M M −2 −2 −4 −2 0 2 4 −4 −2 0 2 4 x x (a) Tri začetne lege robotov, ki imajo enako (b) Posodobljena situacija s slike (a) po smer in oddaljenost od značke tem, ko so roboti prepotovali enako razdaljo v smeri naprej. Z vidika meritev so lege robotov razpoznavne. 4 4 R2 R2 2 2 R3 y R3 y R1 0 R1 0 M M −2 −2 −4 −2 0 2 4 −4 −2 0 2 4 x x (c) Tri začetne lege robotov, ki imajo enako (d) Posodobljena situacija s slike (c) po tem, smer in oddaljenost od značke – poseben ko so roboti prepotovali enako razdaljo v primer smeri naprej. Z vidika meritev je robot v legi 1 nerazpoznaven od robota v legi 2, robot v legi 3 pa je razpoznaven od ostalih dveh. 4 R2 2 y R3 R1 0 M −2−4 −2 0 2 4 x (e) Posodobljena situacija s slike (c) po tem, ko so roboti prepotovali isto neravno pot. Z vidika meritev so vsi roboti razpoznavni. Slika 6.29: Analiza spoznavnosti sistema iz primera 6.20 346 Nedeterminističnost v mobilnih sistemih Primer 6.21 Mobilni robot iz primera 6.20 naj ima malce drugačen senzor, ki meri razdaljo in kot do značke v izhodišču globalnega koordinatnega sistema. Meritve kota so znotraj intervala α ∈ [− π, π]. Meritev razdalje je motena z Gaussovim šumom z varianco 0 , 5 m2, meritev kota pa z Gaussovim šumom z varianco 0 , 3 rad2. 4 3 2 α y 1 d 0 M −1 −2 0 2 4 x Slika 6.30: Postavitev iz primera 6.21. Mobilni robot ima senzor za merjenje razdalje in kota do značke M , ki se nahaja v izhodišču globalnega koordinatnega sistema. Kakšen je časovni potek ocene lege robota ( ˆ x k| k−1 = [ xk, yk, ϕk] T ) in varianca ocene, če ob vsakem času vzorčenja Ts = 0 , 1 s pošljemo robotu ukaz u = [ vk, ωk] T = [0 , 5 , 0 , 5] T ? Rešitev Do rešitve lahko pridemo s pomočjo simulacije v okolju Matlab. Določimo model gibanja mobilnega robota. Kinematični model kolesnega mobilnega robota je enak kot v primeru 6.20   Tsvk−1 cos( ϕk−1) ˆ x   k| k−1 = ˆ x k−1| k−1 + Tsvk−1 sin( ϕk−1)   Tsωk−1 6.5. Kalmanov filter 347 Model meritve razdalje in kota je " p # x 2 + y 2 ˆ z k k k = atan2 (0 − yk, 0 − xk) − ϕk pri čemer je štirikvadratna inverzna funkcija tangens definirana v (2.11). Matlab koda rešitve je podana v programu 6.12. Rezultati simulacije, prikazani na slikah 6.31 – 6.35, kažejo, da ocenjena stanja konvergirajo k pravi legi robota. Čeprav je ta primer podoben primeru 6.20, se lahko zgodi, da ocena ne konvergira k pravi legi robota, če so okoljski pogoji nekoliko drugačni. Primer konvergence k napačni rešitvi prikazujejo slike 6.36 – 6.40. Ker sta oba izhoda senzorja (razdalja in kot) relativni meritvi, ocenjena stanja morda ne konvergirajo k pravi rešitvi, čeprav je inovacija (razlika med meritvijo in predikcijo meritve) blizu ničelne vrednosti (slika 6.40). Program 6.12: Izvedba rešitve iz primera 6.21 ./src/prb/example_ekf2default.m 1 Ts = 0.1; % Ra č unski korak 2 xTrue = [1; 2; pi /6]; % Prava za č etna lega 3 x = [3; 0; 0]; % Ocena za č etne lege 4 P = diag ([9 9 0.6]); % Za č etna kovarian č na matrika ocene lege 5 Q = diag ([0.1 0.1]); % Kovarian č na matrika š uma modela gibanja 6 R = diag ([0.5 0.3]); % Kovarian č na matrika š uma merjenja razdalje in 7 % k o t a 8 en abl eNo ise = 1; % Omogo č i š um : 0 ali 1 9 N = 300; % Š tevilo s i m u l a c i j s k i h korakov 10 11 % Loop 12 for k = 1: N 13 u = [ 0 . 5 ; 0 . 5 ] ; % U k a z i ( t r a n s l a t o r n a in k o t n a h i t r o s t ) 14 u N o i s y = u + s q r t ( Q )* r a n d n (2 , 1)* e n a b l e N o i s e ; 15 16 % S i m u l a c i j a p r a v i h s t a n j ( l e g e ) r o b o t a 17 x T r u e = x T r u e + Ts *[ u N o i s y ( 1 ) * cos ( x T r u e ( 3 ) ) ; ... 18 u N o i s y ( 1 ) * sin ( x T r u e ( 3 ) ) ; ... 19 u N o i s y ( 2 ) ] ; 20 x T r u e (3) = w r a p T o P i ( x T r u e ( 3 ) ) ; 21 22 % S i m u l a c i j a m e r i t e v s š u m o m ( r a z d a l j a in kot ) 23 z T r u e = [ s q r t ( x T r u e ( 1 ) ^ 2 + x T r u e ( 2 ) ^ 2 ) ; ... 24 a t a n 2 (0 - x T r u e (2) , 0 - x T r u e (1)) - x T r u e ( 3 ) ] + ... 25 s q r t ( R )* r a n d n (2 , 1)* e n a b l e N o i s e ; 26 z T r u e (1) = abs ( z T r u e ( 1 ) ) ; 27 z T r u e (2) = w r a p T o P i ( z T r u e ( 2 ) ) ; 28 29 % %% P r e d i k c i j a ( o c e n a l e g e in h i t r o s t i g l e d e na z n a n e v h o d e ) 30 x P r e d = x + Ts *[ u ( 1 ) * cos ( x ( 3 ) ) ; ... 31 u ( 1 ) * sin ( x ( 3 ) ) ; ... 32 u ( 2 ) ] ; 33 x P r e d (3) = w r a p T o P i ( x P r e d ( 3 ) ) ; 34 35 % J a c o b i j e v e m a t r i k e 36 A = [1 0 - Ts * u ( 1 ) * sin ( x ( 3 ) ) ; ... 37 0 1 Ts * u ( 1 ) * cos ( x ( 3 ) ) ; ... 38 0 0 1]; 348 Nedeterminističnost v mobilnih sistemih 39 F = [ Ts * cos ( x ( 3 ) ) 0; ... 40 Ts * sin ( x ( 3 ) ) 0; ... 41 0 Ts ]; 42 P P r e d = A * P * A . ’ + F * Q * F . ’; 43 44 45 % O c e n j e n e m e r i t v e 46 z = [ s q r t ( x P r e d ( 1 ) ^ 2 + x P r e d ( 2 ) ^ 2 ) ; ... 47 a t a n 2 (0 - x P r e d (2) , 0 - x P r e d ( 1 ) ) - x P r e d ( 3 ) ] ; 48 z (2) = w r a p T o P i ( z ( 2 ) ) ; 49 50 % %% K o r e k c i j a 51 d = s q r t ( x P r e d ( 1 ) ^ 2 + x P r e d ( 2 ) ^ 2 ) ; 52 C = [ x P r e d ( 1 ) / d x P r e d ( 2 ) / d 0; ... 53 - x P r e d ( 2 ) / d ^2 x P r e d ( 1 ) / d ^2 -1]; 54 K = P P r e d * C . ’/( C * P P r e d * C . ’ + R ); 55 i n o v = z T r u e - z ; 56 57 % I z b i r a p r i m e r n e i n o v a c i j e , z a r a d i š uma in c i k l i č n o s t i k o t a 58 i n o v (2) = w r a p T o P i ( i n o v ( 2 ) ) ; 59 60 x = x P r e d + K * i n o v ; 61 P = P P r e d - K * C * P P r e d ; 62 end 5 4 3 y 2 1 0 M −2 0 2 4 6 x Slika 6.31: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija iz primera 6.21 6.5. Kalmanov filter 349 4 ] 2[mx 0 0 5 10 15 20 25 30 t [s] 4 ] 2[my 0 0 5 10 15 20 25 30 t [s] 4 2 ] [rad 0 ϕ−2 −40 5 10 15 20 25 30 t [s] Slika 6.32: Ocena lege (polna krivulja) in pravo stanje (črtkana krivulja) mobilnega robota z začetnim neničelnim pogreškom ocene iz primera 6.21 6 4 2 ] 4 ] [m 0 d [rad 2 α−2 0 −4 0 10 20 30 0 10 20 30 t [s] t [s] Slika 6.33: Meritvi razdalje in kota iz primera 6.21 350 Nedeterminističnost v mobilnih sistemih 4 4 0.4 2 ] 3 2 ] 3 0.3 [m [m 2 ] 0 (x) 2 (y) 2 [rad .2 ϕ var 1 var 1 0.1 0 0 0 0 10 20 30 0 10 20 30 0 10 20 30 t [s] t [s] t [s] Slika 6.34: Variance ocene lege robota iz primera 6.21 2 1 tion 0 Innova−1 −20 5 10 15 20 25 30 t [s] Slika 6.35: Časovni potek inovacije iz primera 6.21 6.5. Kalmanov filter 351 5 4 3 y 2 1 0 M −2 0 2 4 6 x Slika 6.36: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija iz primera 6.21 (reprezentativni primer) 352 Nedeterminističnost v mobilnih sistemih 4 3 ] [m 2 x 1 0 0 5 10 15 20 25 30 t [s] 4 3 ] [m 2 y 1 0 0 5 10 15 20 25 30 t [s] 4 2 ] [rad 0 ϕ−2 −40 5 10 15 20 25 30 t [s] Slika 6.37: Ocena lege (polna krivulja) in pravo stanje (črtkana krivulja) mobilnega robota z začetnim neničelnim pogreškom ocene iz primera 6.21 (reprezentativni primer) 6 4 2 ] 4 ] [m 0 d [rad 2 α−2 0 −4 0 10 20 30 0 10 20 30 t [s] t [s] Slika 6.38: Meritvi razdalje in kota iz primera 6.21 (reprezentativni primer) 6.5. Kalmanov filter 353 4 4 0.4 2 ] 3 2 ] 3 [m [m 2 ] 0.3 (x) 2 (y) 2 [rad ϕ var 0.2 1 var 1 0 0 0.1 0 10 20 30 0 10 20 30 0 10 20 30 t [s] t [s] t [s] Slika 6.39: Variance ocene lege robota iz primera 6.21 (reprezentativni primer) 2 1 tion 0 Innova−1 −2 0 5 10 15 20 25 30 t [s] Slika 6.40: Časovni potek inovacije iz primera 6.21 (reprezentativni primer) Grafični prikaz spoznavnosti na sliki 6.41 omogoča vpogled v razloge za pristranskost ocene. Analiza kaže, da obstajajo stanja, ki so z vidika meritev nerazpoznavna, ne glede na regulirne veličine. Poleg tega za vsako meritev obstaja neskončna množica stanj, zato sistem ni spoznaven. Pristranskost ocene lahko odpravimo s hkratnim opazovanjem več značk, saj nam to zagotovi dovolj informacij za izbiro ustrezne rešitve, kot je prikazano v primeru 6.22. 354 Nedeterminističnost v mobilnih sistemih 4 4 R2 R2 2 2 R3 y R3 y R1 0 R1 0 M M −2 −2 −4 −2 0 2 4 −4 −2 0 2 4 x x (a) Tri začetne lege robotov z enako odda- (b) Posodobljena situacija s slike (a) po ljenostjo od značke tem, ko so roboti prepotovali enako razdaljo v smeri naprej. Z vidika meritev so lege robotov razpoznavne. 4 4 R2 R2 2 2 y R3 y R3 R1 0 R1 0 M M −2 −2 −4 −2 0 2 4 −4 −2 0 2 4 x x (c) Tri začetne lege robotov, ki imajo enak (d) Posodobljena situacija s slike (c) po tem, kot in oddaljenost do značke — poseben ko so roboti prepotovali enako razdaljo v primer smeri naprej. Z vidika meritev so vse tri lege robotov nerazpoznavne. 4 R2 R1 2 y 0 M R3 −2−4 −2 0 2 4 x (e) Posodobljena situacija s slike (c) po tem, ko so roboti prepotovali enako neravno pot. Z vidika meritev so vse tri lege robotov nerazpoznavne. Slika 6.41: Analiza spoznavnosti sistema iz primera 6.21 6.5. Kalmanov filter 355 Primer 6.22 Nadgradimo primer 6.21 tako, da lahko robot hkrati opazuje dve prostorsko ločeni znački, ki sta postavljeni na xM 1 = 0, yM 1 = 0 in xM 2 = 5, yM 2 = 5. Vsi ostali podatki so enaki kot v primeru 6.21. 5 M2 4 d2 3 y α 2 2 α 1 1 d1 0 M1 −2 0 2 4 6 x Slika 6.42: Postavitev iz primera 6.22. Mobilni robot ima senzor za merjenje kota in oddaljenosti od značk M 1 in M 2. Rešitev Prilagoditi moramo le korekcijski del algoritma. Vektor meritve sedaj vsebuje štiri elemente  p( x  M 1 − xk )2 + ( yM 1 − yk )2 atan2 ( y  ˆ z M 1 − yk , xM 1 − xk ) − ϕk   k =  p( x   M 2 − xk )2 + ( yM 2 − yk )2  atan2 ( yM 2 − yk, xM 2 − xk) − ϕk razdaljo in kot do prve značke ter razdaljo in kot do druge značke. Določimo izhodno matriko C (glejte (6.31)) z linearizacijo okoli trenutne predikcijske ocene stanja ( xk, yk)  x  k yk 0 d 1 d 1 − yk xk −1 C =  d 21 d 2 1   x  k yk  0  d d  2 2  − yk xk −1 d 2 2 d 2 2 356 Nedeterminističnost v mobilnih sistemih q q kjer je d 1 = ( xM 1 − xk)2 + ( yM 1 − yk)2 in d 2 = ( xM 2 − xk)2 + ( yM 2 − yk)2. Določimo tudi kovariančno matriko šuma meritve kot 0 , 5 0 0 0   0 0 , 3 0 0  R =    0 0 0 , 5 0    0 0 0 0 , 3 Matlab koda rešitve je podana v programu 6.13. Rezultati simulacije, predstavljeni na slikah 6.43 – 6.47, potrjujejo, da ocenjena stanja konvergirajo k pravi legi robota. Program 6.13: Rešitev primera 6.22 ./src/prb/example_ekf3default.m 1 Ts = 0.1; % Ra č unski korak 2 xTrue = [1; 2; pi /6]; % Prava za č etna lega 3 x = [3; 0; 0]; % Ocena za č etne lege 4 P = diag ([9 9 0.6]); % Za č etna kovarian č na matrika ocene lege 5 Q = diag ([0.1 0.1]); % Kovarian č na matrika š uma modela gibanja 6 R = diag ([0.5 0.3]); % Kovarian č na matrika š uma merjenja razdalje in 7 % k o t a 8 en abl eNo is e = 1; % Omogo č i š um : 0 ali 1 9 N = 300; % Š tevilo s i m u l a c i j s k i h korakov 10 marker = [0 0; 5 5]; % Polo ž aji zna č k 11 12 % Zanka 13 for k = 1: N 14 u = [ 0 . 5 ; 0 . 5 ] ; % U k a z i ( t r a n s l a t o r n a in k o t n a h i t r o s t ) 15 u N o i s y = u + s q r t ( Q )* r a n d n (2 , 1)* e n a b l e N o i s e ; 16 17 % S i m u l a c i j a p r a v i h s t a n j ( l e g e ) r o b o t a 18 x T r u e = x T r u e + Ts *[ u N o i s y ( 1 ) * cos ( x T r u e ( 3 ) ) ; ... 19 u N o i s y ( 1 ) * sin ( x T r u e ( 3 ) ) ; ... 20 u N o i s y ( 2 ) ] ; 21 x T r u e (3) = w r a p T o P i ( x T r u e ( 3 ) ) ; 22 23 % S i m u l a c i j a m e r i t e v s š u m o m ( r a z d a l j a in kot ) 24 z T r u e = []; 25 for m = 1: s i z e ( marker , 1) 26 d i s t = s q r t (( m a r k e r ( m ,1) - x T r u e ( 1 ) ) ^ 2 + ( m a r k e r ( m ,2) - x T r u e ( 2 ) ) ^ 2 ) ; 27 a l p h a = a t a n 2 ( m a r k e r ( m ,2) - x T r u e (2) , m a r k e r ( m ,1) - x T r u e (1)) - x T r u e ( 3 ) ; 28 zz = [ d i s t ; a l p h a ] + s q r t ( R )* r a n d n (2 , 1)* e n a b l e N o i s e ; 29 zz (1) = abs ( zz ( 1 ) ) ; 30 zz (2) = w r a p T o P i ( zz ( 2 ) ) ; 31 z T r u e = [ z T r u e ; zz ]; 32 end 33 34 % %% P r e d i k c i j a ( o c e n a l e g e in h i t r o s t i g l e d e na z n a n e v h o d e ) 35 x P r e d = x + Ts *[ u ( 1 ) * cos ( x ( 3 ) ) ; ... 36 u ( 1 ) * sin ( x ( 3 ) ) ; ... 37 u ( 2 ) ] ; 38 x P r e d (3) = w r a p T o P i ( x P r e d ( 3 ) ) ; 39 40 % J a c o b i j e v e m a t r i k e 41 A = [1 0 - Ts * u ( 1 ) * sin ( x ( 3 ) ) ; ... 42 0 1 Ts * u ( 1 ) * cos ( x ( 3 ) ) ; ... 6.5. Kalmanov filter 357 43 0 0 1]; 44 F = [ Ts * cos ( x ( 3 ) ) 0; ... 45 Ts * sin ( x ( 3 ) ) 0; ... 46 0 Ts ]; 47 P P r e d = A * P * A . ’ + F * Q * F . ’; 48 49 % %% K o r e k c i j a 50 z = []; 51 C = []; 52 for m = 1: s i z e ( marker ,1) 53 d i s t = s q r t (( m a r k e r ( m ,1) - x P r e d ( 1 ) ) ^ 2 + ( m a r k e r ( m ,2) - x P r e d ( 2 ) ) ^ 2 ) ; 54 a l p h a = a t a n 2 ( m a r k e r ( m ,2) - x P r e d (2) , m a r k e r ( m ,1) - x P r e d (1)) - x P r e d ( 3 ) ; 55 zz = [ d i s t ; a l p h a ]; 56 zz (2) = w r a p T o P i ( zz ( 2 ) ) ; 57 z = [ z ; zz ]; 58 59 % M a t r i k a C za k o r e k c i j o 60 c = [ x P r e d ( 1 ) / d i s t x P r e d ( 2 ) / d i s t 0; ... 61 - x P r e d ( 2 ) / d i s t ^2 x P r e d ( 1 ) / d i s t ^2 -1]; 62 C = [ C ; c ]; 63 end 64 65 % K o v a r i a n č na m a t r i k a m e r i t e v 66 RR = d i a g ( r e p m a t ([ R (1 ,1) R (2 ,2)] , 1 , s i z e ( marker , 1 ) ) ) ; 67 K = P P r e d * C . ’/( C * P P r e d * C . ’ + RR ); 68 69 i n o v = z T r u e - z ; 70 % I z b i r a p r i m e r n e i n o v a c i j e , z a r a d i š uma in c i k l i č n o s t i k o t a 71 for m = 1: s i z e ( marker , 1) 72 i n o v (2* m ) = w r a p T o P i ( i n o v (2* m )); 73 end 74 75 x = x P r e d + K *( i n o v ); 76 P = P P r e d - K * C * P P r e d ; 77 end 358 Nedeterminističnost v mobilnih sistemih 5 M2 4 3 y 2 1 0 M1 −2 0 2 4 6 x Slika 6.43: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija iz primera 6.22 6.5. Kalmanov filter 359 4 ] 2[mx 0 0 5 10 15 20 25 30 t [s] 4 ] 2[my 0 0 5 10 15 20 25 30 t [s] 4 2 ] [rad 0 ϕ−2 −40 5 10 15 20 25 30 t [s] Slika 6.44: Ocena lege (polna krivulja) in pravo stanje (črtkana krivulja) mobilnega robota z začetnim neničelnim pogreškom ocene iz primera 6.22 8 4 6 2 ] ] [m 4 0 d [rad α 2 −2 0 −4 0 10 20 30 0 10 20 30 t [s] t [s] Slika 6.45: Meritve razdalje in kota iz primera 6.22 360 Nedeterminističnost v mobilnih sistemih 4 4 0.2 2 ] 3 2 ] 3 [m [m 2 ] 2 2 (x) (y) [rad ϕ 0.1 var 1 var 1 0 0 0 0 10 20 30 0 10 20 30 0 10 20 30 t [s] t [s] t [s] Slika 6.46: Variance ocene lege robota iz primera 6.22 2 tion 0 Innova−2 −40 5 10 15 20 25 30 t [s] Slika 6.47: Časovni potek inovacije iz primera 6.22 Ponovno lahko grafično preverimo spoznavnost sistema. Tudi v tem primeru lahko najdemo posebno situacijo, kjer so vsa stanja nerazpoznavna, kar prikazuje slika 6.48. Na prvi pogled so obravnavana stanja z vidika meritev nerazpoznavna, ker pa predpostavljamo, da merilni podatki vsebujejo tudi oznako (ID) značk, ima ta posebna situacija razpoznavna stanja in sistem je spoznaven. 6.5. Kalmanov filter 361 4 M2 4 M2 R2 y y R1 2 R2 R1 2 M1 M1 0 0 −2 0 2 4 6 −2 0 2 4 6 x x (a) Dve posebni legi, ki sta z vidika meritev (b) Posodobljena situacija s slike (a) po simetrični enakih prepotovanih relativnih poteh glede na obe začetni legi. Z vidika meritev sta obe legi razpoznavni, če merilni podatki vsebujejo oznako (ID) značk. Slika 6.48: Analiza spoznavnosti sistema iz primera 6.22 362 Nedeterminističnost v mobilnih sistemih 6.5.3 Druge različice Kalmanovega filtra Poleg Kalmanovega filtra za linearne sisteme in razširjenega Kalmanovega filtra za nelinearne sisteme [10] obstaja vrsta izpeljank. Nepristranski Kalmanov filter (UKF, angl. unscented Kalman filter ) se obi- čajno uporablja v sistemih z izrazito nelinearnostjo, kjer razširjeni Kalmanov filter morda ne zagotavlja zadovoljivih rezultatov. V tem primeru se kovariančne matrike statistično ocenijo na podlagi manjše množice vhodnih točk, ki se preslikajo preko nelinearne funkcije ter se nato uporabijo za oceno srednje vrednosti in kovariančne matrike. Te točke, znane kot sigma točke, so razpršene okoli ocenjene vrednosti po nekem algoritmu (običajno je 2 n + 1 točk za n dimenzij). Informacijski filter uporablja informacijsko matriko in informacijski vektor namesto kovariančne matrike in ocene stanj. Informacijska matrika predstavlja inverz kovariančne matrike, informacijski vektor pa je produkt informacijske matrike in ocene vektorja stanj. Informacijski filter je dualen Kalmanovemu filtru, kjer je korekcijski korak računsko bistveno enostavnejši (le matrična vsota), vendar je predikcijski korak računsko bolj zahteven. Kalman-Bucyjev filter je oblika Kalmanovega filtra za zvezne sisteme. 6.6 Filter delcev Do zdaj smo uporabljali Bayesov filter za sisteme z diskretnim prostorom stanj s končnim številom vrednosti, ki jih spremenljivke stanj lahko zavzamejo. Če želimo uporabiti Bayesov filter za zvezne spremenljivke, lahko te spremenljivke kvantiziramo na končno število vrednosti. Tovrstna izvedba Bayesovega filtra za zvezne spremenljivke stanj je splošno znana kot histogramski filter. Za zvezne spremenljivke stanj in eksplicitno rešitev Bayesovega filtra (6.21) moramo rešiti enačbo p(x k|z1: k, u0: k−1) = p(z Z (6.32) k |x k ) p(x k|x k−1 , u k−1) p(x k−1|z1: k−1 , u0: k−2) dx k−1 p(z k|z1: k−1 , u0: k−1) kjer smo predpostavili, da so stanja vsebovana in da obravnavamo Markovov proces (glejte poglavje 6.2). Trenutna porazdelitev stanj (6.32) je potrebna za izračun najverjetnejše ocene stanj (matematično upanje) Z E ˆ x k| k = x k| k · p(x k|z1: k, u0: k−1) dx k| k Eksplicitna rešitev (6.32) je možna le za omejen nabor primerov, kjer predpostavimo Gaussovo porazdelitev in linearnost sistema, kar vodi v Kalmanov filter. V primeru nelinearnih sistemov lahko nelinearnost sistema (v modelu 6.6. Filter delcev 363 gibanja, aktuatorja in/ali senzorja) lineariziramo, kar nas privede do razširjenega Kalmanovega filtra. Filter delcev (angl. particle filter ) je bolj splošen pristop, kjer nista potrebna Gaussova porazdelitev in linearnost sistema. Osnovna ideja je, da trenutno oceno porazdelitve stanj (6.21) po opravljeni meritvi aproksimiramo z množico N delcev. Vsak delec v množici predstavlja vrednost ocenjenega stanja x i , ki je naključno k vzorčena iz porazdelitve (simulacija Monte Carlo). Vsak delec podaja svojo hipotezo o dejanskem stanju sistema. Porazdelitev je opisana s pomočjo množice naključno generiranih delcev, torej gre za neparametrični opis porazdelitve, ki ni omejen samo na Gaussove porazdelitve. Opis porazdelitve z delci omogoča modeliranje nelinearnih transformacij šuma (model aktuatorja in/ali senzorja). Torej lahko z delci opišemo porazdelitev šuma, ki se iz vhodov ali izhodov sistema prenaša preko nelinearnih funkcij na stanja sistema. Algoritem 5 predstavlja osnoven princip filtra delcev. Algorithm 5 Algoritem za filter delcev. Funkcijo Filter_delcev kličemo v vsakem časovnem trenutku s trenutnimi vhodi in meritvami ter predhodno oceno stanja. function Filter_delcev(ˆ xk−1| k−1, uk−1, zk) Inicializacija: if k > 0 then Inicializacija množice N delcev x i na osnovi naključnega vzorčenja k porazdelitve p(x0). end if Predikcija: Transformiraj (premakni) vsak delec ˆ x i na osnovi modela premika k−1| k−1 in znanega vhoda u k−1, kateremu dodaj naključno vrednost glede na lastnosti šuma, ki je del modela premika. Model premika podaja p(x k|x k−1 , u k−1). Dobljena predikcija je množica delcev ˆ x i . k| k−1 Korekcija: Za vsak delec ˆ x i oceni vrednost meritve, ki bi jo sistem izmeril, če k| k−1 bi njegovo stanje ustrezalo stanju delca. Glede na opravljeno meritev in primerjavo z ocenjenimi meritvami delcev oceni pomembnost delcev. Nato določi nov nabor delcev glede na njihovo pomembnost z naključnim izborom delcev z verjetnostjo, ki je proporcionalna njihovi pomembnosti, torej p(z k| ˆ x i ). Bolj verjetni delci so izbrani večkrat, manj verjetni delci pa k| k−1 manjkrat. Ocena filtriranega stanja ˆ x k| k je enaka povprečni vrednosti vseh delcev. end function V začetnem koraku moramo določiti začetno populacijo delcev, ˆ x i za i ∈ 1 , . . . , N , 0 364 Nedeterminističnost v mobilnih sistemih katere raztros je odvisen od zaupanja (porazdelitve) p(x0) v začetno stanje sistema. V kolikor začetno stanje ni znano, je porazdelitev uniformna in so delci enakomerno (enako verjetno) razporejeni po celotnem prostoru stanj. V predikcijskem koraku izračunamo novo stanje za vsak delec glede na podan vhod sistema. Dobljenim ocenam stanj za vsak delec dodamo naključno vrednost šuma, ki ga pričakujemo na vhodu sistema. Tako dobimo predikcijo stanja za vsak delec ˆ x i . Dodani šum zagotavlja, da se delci razpršijo, saj to omogoča k| k−1 oceno prave vrednosti ob prisotnosti različnih motenj. V korekcijskem koraku ovrednotimo pomembnost delcev tako, da za vsak delec (iz njegovih ocenjenih stanj) izračunamo odstopanje dejanske meritve z k od ocenjene meritve delca ˆ z i . Razlika med dejansko in ocenjeno meritvijo je splošno znana k kot inovacija in se lahko oceni za vsak delec kot innov i = z k k − ˆ z ik katere vrednost je manjša za bolj verjetne delce. Na podlagi inovacije lahko določimo pomembnost vsakega delca oz. verjetnost p(z k| ˆ x i ), ki predstavlja utež wi od i-tega delca. Utež lahko določimo z k| k−1 k Gaussovo porazdelitvijo kot − 1 wi = det (2 π R) 2 e− 12 (innov i ) T R−1(innov i ) k k k kjer je R kovariančna matrika meritve. Zelo pomemben korak v procesu uporabe filtra delcev je vzorčenje po po- membnosti (angl. importance sampling) glede na uteži wi . Množica N delcev k je naključno vzorčena tako, da je verjetnost izbire določenega delca iz množice sorazmerna njegovi uteži wi . Torej so delci z večjo utežjo izbrani večkrat kot k delci z manjšo utežjo, oz. delci z najmanjšo utežjo ne smejo biti nikoli izbrani. Pristop izbora nove množice delcev lahko izvedemo na več načinov. Enega od njih bomo predstavili v nadaljevanju: • Uteži delcev wi normiramo z vsoto vseh uteži P N wi in tako dobimo k i=1 k wi nove uteži wni = k . k P N wi i=1 k • Kumulativno seštejemo normirane uteži, da dobimo kumulativne uteži wci = P i wni , kot prikazuje slika 6.49. k j=1 k • Naključno izberemo N števil med 0 in 1 (iz enakomerne porazdelitve) in preverimo, katere uteži pripadajo izbranim številom. Torej primerjamo kumulativne uteži wci in naključno generirana števila. Glede na sliko k 6.49 so delci z večjimi utežmi bolj verjetno izbrani (te uteži na sliki 6.49 zavzemajo več prostora). • Izbrane delce uporabimo v korekcijskem koraku za oceno trenutne vrednosti stanja ˆ x k| k = P N wi ˆ x i . i=1 k k| k−1 6.6. Filter delcev 365 wc N 1 i = wn å i 1 = wn N wn 5 wn 4 wn 3 wn 2 0 wn 1 Slika 6.49: Vzorčenje po pomembnosti v korekcijskem koraku filtra delcev. Uteži delcev nanizamo eno za drugo in nato normiramo tako, da je vsota vseh delcev enaka 1. Bolj verjetni delci zavzamejo več prostora na enotskem intervalu in obratno. Novo populacijo delcev določimo tako, da naključno izberemo (z enakomerno porazdelitvijo) N števil iz enotskega intervala in pogledamo, katerim delcem pripadajo. V kolikor sistem miruje (trenutno stanje je enako preteklemu), je priporočeno, da ne vzorčimo po pomembnosti, ampak uporabimo kar stari nabor delcev in jim le prilagodimo nove uteži, kot je predstavljeno v [11]. V primeru 6.23 je prikazan primer uporabe filtra delcev. Primer 6.23 Uporabite filter delcev za ocenjevanje najverjetnejšega stanja v primeru 6.22. Pri implementaciji uporabimo N = 300 delcev. Vsi drugi podatki so enaki kot v primeru 6.22. Rešitev Matlab koda rešitve je podana v programu 6.14, rezultati simulacije pa so prikazani na slikah 6.50 in 6.51. Program 6.14: Implementacija rešitve primera 6.23 ./src/prb/example_pf1default.m 1 Ts = 0.1; % Ra č unski korak 2 xTrue = [1; 2; pi /6]; % Prava za č etna lega 366 Nedeterminističnost v mobilnih sistemih 3 x = [3; 0; 0]; % Ocena za č etne lege 4 P = diag ([9 9 0.6]); % Za č etna kovarian č na matrika ocene lege 5 Q = diag ([0.1 0.1]); % Kovarian č na matrika š uma modela gibanja 6 R = diag ([0.5 0.3]); % Kovarian č na matrika š uma merjenja razdalje in 7 % k o t a 8 en abl eN ois e = 1; % Omogo č i š um : 0 ali 1 9 N = 300; % Š tevilo s i m u l a c i j s k i h korakov 10 marker = [0 0; 5 5]; % Polo ž aji zna č k 11 12 % I n i c i a l i z a c i j a delcev 13 nParticles = 300; 14 xP = repmat ( xTrue , 1 , nParticles ) + diag ([4 4 1])* randn (3 , nParticles ); 15 W = ones ( nParticles , 1)/ nParticles ; % Vsi delci so enako verjetni 16 17 % Zanka 18 for k = 1: N 19 u = [ 0 . 5 ; 0 . 5 ] ; % U k a z i ( t r a n s l a t o r n a in k o t n a h i t r o s t ) 20 u _ s u m = u + s q r t ( Q )* r a n d n (2 , 1)* e n a b l e N o i s e ; 21 22 % S i m u l a c i j a p r a v i h s t a n j ( l e g e ) r o b o t a 23 x T r u e = x T r u e + Ts *[ u _ s u m ( 1 ) * cos ( x T r u e ( 3 ) ) ; ... 24 u _ s u m ( 1 ) * sin ( x T r u e ( 3 ) ) ; ... 25 u _ s u m ( 2 ) ] ; 26 x T r u e (3) = w r a p T o P i ( x T r u e ( 3 ) ) ; 27 28 % S i m u l a c i j a m e r i t e v s š u m o m ( r a z d a l j a in kot ) 29 z T r u e = []; 30 for m = 1: s i z e ( marker , 1) 31 d i s t = s q r t (( m a r k e r ( m ,1) - x T r u e ( 1 ) ) ^ 2 + ( m a r k e r ( m ,2) - x T r u e ( 2 ) ) ^ 2 ) ; 32 a l p h a = a t a n 2 ( m a r k e r ( m ,2) - x T r u e (2) , m a r k e r ( m ,1) - x T r u e (1)) - x T r u e ( 3 ) ; 33 zz = [ d i s t ; a l p h a ] + s q r t ( R )* r a n d n (2 , 1)* e n a b l e N o i s e ; 34 zz (1) = abs ( zz ( 1 ) ) ; 35 zz (2) = w r a p T o P i ( zz ( 2 ) ) ; 36 z T r u e = [ z T r u e ; zz ]; 37 end 38 39 % P r e d i k c i j a 40 for p = 1: n P a r t i c l e s 41 % D e l c i se p r e m i k a j o g l e d e na m o d e l š uma 42 un = u + s q r t ( Q )* r a n d n (2 , 1 ) * 1 ; 43 xP (: , p ) = xP (: , p ) + Ts *[ un ( 1 ) * cos ( xP (3 , p )); ... 44 un ( 1 ) * sin ( xP (3 , p )); ... 45 un ( 2 ) ] ; 46 xP (3 , p ) = w r a p T o P i ( xP (3 , p )); 47 end 48 49 % K o r e k c i j a 50 for p = 1: n P a r t i c l e s 51 % O c e n j e n a m e r i t e v za v s a k d e l e c 52 z = []; 53 for m = 1: s i z e ( marker , 1) 54 d i s t = s q r t (( m a r k e r ( m ,1) - xP (1 , p ) ) ^ 2 + ( m a r k e r ( m ,2) - xP (2 , p ) ) ^ 2 ) ; 55 a l p h a = a t a n 2 ( m a r k e r ( m ,2) - xP (2 , p ) , m a r k e r ( m ,1) - xP (1 , p )) - xP (3 , p ); 56 zz = [ d i s t ; a l p h a ]; 57 zz (1) = abs ( zz ( 1 ) ) ; 58 zz (2) = w r a p T o P i ( zz ( 2 ) ) ; 59 z = [ z ; zz ]; 60 end 61 62 I n n o v = z T r u e - z ; % I z r a č un i n o v a c i j e 63 64 % I z b i r a p r i m e r n e i n o v a c i j e , 65 % z a r a d i š uma in c i k l i č n o s t i k o t a 6.6. Filter delcev 367 66 for m = 1: s i z e ( marker , 1) 67 iii = z T r u e (2* m ) - ( z (2* m ) + [0; 2* pi ; -2* pi ]); 68 [ tmp , i n d e x ] = min ( abs ( iii )); 69 I n n o v (2* m ) = iii ( i n d e x ); 70 end 71 72 % Ute ž i d e l c e v ( v e r j e t n o s t i d e l c e v ) 73 % K o v a r i a n č na m a t r i k a m e r i t e v 74 RR = d i a g ( r e p m a t ( d i a g ( R ) , s i z e ( marker , 1) , 1 ) ) ; 75 W ( p ) = exp ( -0.5* I n n o v . ’* inv ( RR )* I n n o v ) + 0 . 0 0 0 1 ; 76 end 77 78 i N e x t G e n e r a t i o n = o b t a i n N e x t G e n e r a t i o n O f P a r t i c l e s ( W , n P a r t i c l e s ); 79 xP = xP (: , i N e x t G e n e r a t i o n ); 80 81 % N o v a o c e n a s t a n j je p o v p r e č je v s e h d e l c e v 82 x = m e a n ( xP , 2); 83 x (3) = w r a p T o P i ( x ( 3 ) ) ; 84 % U s m e r i t e v r o b o t a je d o l o č ena z n a j b o l j v e r j e t n i m d e l c e m 85 % n a m e s t o s p o v p r e č nim k o t o m v s e h d e l c e v . 86 [ gg , ggi ] = max ( W ); 87 x (3) = xP (3 , ggi ); 88 end Program 6.15: Funkcija, uporabljena v programih 6.14 in 6.16 ./src/prb/obtainNextGenerationOfParticles.m 1 function i N e x t G e n e r a t i o n = o b t a i n N e x t G e n e r a t i o n O f P a r t i c l e s (W , nParticles ) 2 % I z b i r a g l e d e na ute ž i d e l c e v 3 CDF = c u m s u m ( W )/ sum ( W ); 4 i S e l e c t = r a n d ( n P a r t i c l e s , 1); % N a k l j u č ne š t e v i l k e 5 % I n d e k s i n o v i h d e l c e v 6 C D F g = [0; CDF ]; 7 i n d g = [1; (1: n P a r t i c l e s ). ’]; 8 i N e x t G e n e r a t i o n _ f l o a t = i n t e r p 1 ( CDFg , indg , iS e le ct , ’ l i n e a r ’ ); 9 i N e x t G e n e r a t i o n = r o u n d ( i N e x t G e n e r a t i o n _ f l o a t + 0 . 5 ) ; % Z a o k r o ž e v a n j e i n d e k s o v 10 end 368 Nedeterminističnost v mobilnih sistemih 5 M2 4 3 y 2 1 0 M1 −2 0 2 4 6 x Slika 6.50: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija ter generirani delci v zadnjem koraku simulacije primera 6.23 6.6. Filter delcev 369 4 ] 2 [mx 0 0 5 10 15 20 25 30 t [s] 4 ] 2 [my 0 0 5 10 15 20 25 30 t [s] 4 2 ] [rad 0 ϕ−2 −40 5 10 15 20 25 30 t [s] Slika 6.51: Ocena lege (polna krivulja) in pravo stanje (črtkana krivulja) mobilnega robota z začetnim neničelnim pogreškom ocene iz primera 6.23 370 Nedeterminističnost v mobilnih sistemih V primeru 6.23 merimo kote in razdalje do značk, pri čemer moramo upoštevati omejitev kota. Primer lokalizacije, kjer merimo le razdalje do značk za oceno lege mobilnega robota, je prikazan v primeru 6.24. Primer 6.24 Uporabite filter delcev za oceno najverjetnejšega stanja v primeru 6.23, vendar upoštevajte le merjenje razdalj do značk. V implementaciji uporabite N = 500 delcev. Vsi ostali podatki so enaki kot v primeru 6.23. Rešitev Matlab koda rešitve je prikazana v programu 6.16. Rezultati simulacije so prikazani na slikah 6.52 in 6.53, od koder je razvidno, da ocenjena vrednost konvergira k pravi vrednosti podobno kot v primeru 6.23. Program 6.16: Implementacija rešitve primera 6.24 ./src/prb/example_pf2default.m 1 Ts = 0.1; % Ra č unski korak 2 xTrue = [1; 2; pi /6]; % Prava za č etna lega 3 x = [3; 0; 0]; % Ocena za č etne lege 4 P = diag ([9 9 0.6]); % Za č etna kovarian č na matrika ocene lege 5 Q = diag ([0.1 0.1]); % Kovarian č na matrika š uma modela gibanja 6 R = diag ([0.5 0.3]); % Kovarian č na matrika š uma merjenja razdalje in 7 % k o t a 8 en abl eN ois e = 1; % Omogo č i š um : 0 ali 1 9 N = 300; % Š tevilo s i m u l a c i j s k i h korakov 10 marker = [0 0; 5 5]; % Polo ž aji zna č k 11 R = R (1 ,1); % Le merjenje razdalje 12 13 % I n i c i a l i z a c i j a delcev 14 nParticles = 500; 15 xP = repmat ( xTrue , 1 , nParticles ) + diag ([4 4 1])* randn (3 , nParticles ); 16 W = ones ( nParticles , 1)/ nParticles ; % Vsi delci so enako verjetni 17 18 % Zanka 19 for k = 1: N 20 u = [ 0 . 5 ; 0 . 5 ] ; % U k a z i ( t r a n s l a t o r n a in k o t n a h i t r o s t ) 21 u _ s u m = u + s q r t ( Q )* r a n d n (2 , 1)* e n a b l e N o i s e ; 22 23 % S i m u l a c i j a p r a v i h s t a n j ( l e g e ) r o b o t a 24 x T r u e = x T r u e + Ts *[ u _ s u m ( 1 ) * cos ( x T r u e ( 3 ) ) ; ... 25 u _ s u m ( 1 ) * sin ( x T r u e ( 3 ) ) ; ... 26 u _ s u m ( 2 ) ] ; 27 x T r u e (3) = w r a p T o P i ( x T r u e ( 3 ) ) ; 28 29 % S i m u l a c i j a m e r i t e v s š u m o m ( r a z d a l j a ) 30 z T r u e = []; 31 for m = 1: s i z e ( marker , 1) 32 d i s t = s q r t (( m a r k e r ( m ,1) - x T r u e ( 1 ) ) ^ 2 + ( m a r k e r ( m ,2) - x T r u e ( 2 ) ) ^ 2 ) ; 33 zz = [ d i s t ] + s q r t ( R )* r a n d n (1 , 1)* e n a b l e N o i s e ; 34 zz (1) = abs ( zz ( 1 ) ) ; 35 z T r u e = [ z T r u e ; zz ]; 36 end 37 6.6. Filter delcev 371 38 % P r e d i k c i j a 39 for p = 1: n P a r t i c l e s 40 % D e l c i se p r e m i k a j o g l e d e na m o d e l š uma 41 un = u + s q r t ( Q )* r a n d n (2 , 1 ) * 1 ; 42 xP (: , p ) = xP (: , p ) + Ts *[ un ( 1 ) * cos ( xP (3 , p )); ... 43 un ( 1 ) * sin ( xP (3 , p )); ... 44 un ( 2 ) ] ; 45 xP (3 , p ) = w r a p T o P i ( xP (3 , p )); 46 end 47 48 % K o r e k c i j a 49 for p = 1: n P a r t i c l e s 50 % O c e n j e n a m e r i t e v za v s a k d e l e c 51 z = []; 52 for m = 1: s i z e ( marker , 1) 53 d i s t = s q r t (( m a r k e r ( m ,1) - xP (1 , p ) ) ^ 2 + ( m a r k e r ( m ,2) - xP (2 , p ) ) ^ 2 ) ; 54 zz = [ d i s t ]; 55 zz (1) = abs ( zz ( 1 ) ) ; 56 z = [ z ; zz ]; 57 end 58 59 I n n o v = z T r u e - z ; % I z r a č un i n o v a c i j e 60 61 % Ute ž i d e l c e v ( v e r j e t n o s t i d e l c e v ) 62 % K o v a r i a n č na m a t r i k a m e r i t e v 63 RR = d i a g ( r e p m a t ( d i a g ( R ) , s i z e ( marker , 1) , 1 ) ) ; 64 W ( p ) = exp ( -0.5* I n n o v . ’* inv ( RR )* I n n o v ) + 0 . 0 0 0 1 ; 65 end 66 67 i N e x t G e n e r a t i o n = o b t a i n N e x t G e n e r a t i o n O f P a r t i c l e s ( W , n P a r t i c l e s ); 68 xP = xP (: , i N e x t G e n e r a t i o n ); 69 70 % N o v a o c e n a s t a n j je p o v p r e č je v s e h d e l c e v 71 x = m e a n ( xP , 2); 72 x (3) = w r a p T o P i ( x ( 3 ) ) ; 73 % U s m e r i t e v r o b o t a je d o l o č ena z n a j b o l j v e r j e t n i m d e l c e m 74 % n a m e s t o s p o v p r e č nim k o t o m v s e h d e l c e v . 75 [ gg , ggi ] = max ( W ); 76 x (3) = xP (3 , ggi ); 77 end 372 Nedeterminističnost v mobilnih sistemih 5 M2 4 3 y 2 1 0 M1 −2 0 2 4 6 x Slika 6.52: Dejanska (črtkana krivulja) in ocenjena (polna krivulja) trajektorija ter generirani delci v zadnjem koraku simulacije primera 6.24 6.6. Filter delcev 373 4 ] 2 [mx 0 0 5 10 15 20 25 30 t [s] 4 ] 2 [my 0 0 5 10 15 20 25 30 t [s] 4 2 ] [rad 0 ϕ−2 −40 5 10 15 20 25 30 t [s] Slika 6.53: Ocena lege (polna krivulja) in pravo stanje (črtkana krivulja) mobilnega robota z začetnim neničelnim pogreškom ocene iz primera 6.24 Filter delcev je implementacija Bayesovega filtra za zvezne sisteme (zvezni prostor stanj), ki omogoča opis nelinearnih sistemov in lahko upošteva poljubno porazdelitev šuma. V primeru sistemov večjih dimenzij postane računsko precej zahteven, saj je za ustrezno konvergenco filtra potrebno veliko število delcev. Število potrebnih delcev narašča z dimenzijo prostora stanj. Dobra lastnost filtra delcev je robustnost ter zmožnost rešitve problema globalne lokalizacije in ugrabitve robota. Pri problemu globalne lokalizacije je začetna lega (vrednost stanj) neznana, zato se lahko mobilni robot nahaja kjerkoli v prostoru. Pri problemu ugrabitve pa je robot premaknjen (ugrabljen) na poljubno novo lokacijo. Robustni lokalizacijski algoritmi so sposobni rešiti te težave. 374 Nedeterminističnost v mobilnih sistemih Literatura [1] D. C. Montgomery in G. C. Runger. Applied Statistics and Probability for Engineers. John Wiley & Sons, Englewood Cliffs, New Jersey, 2003, 720 str. [2] R. Hermann in A. J. Krener. Nonlinear Controllability and Observability. IEEE Transaction on Automatic Control, zv. 22, št. 5, str. 728–740, 1977. [3] W. Respondek. Introduction to geometric nonlinear control; linearization, observability, decoupling, ICTP Lecture Notes Series, zv. 8, str. 173–222. ICTP, 2002. [4] A. Martinelli. Nonlinear unknown input observability: Analytical expression of the observable codistribution in the case of a single unknown input. V 2015 Proceedings of the Conference on Control and its Applications, str. 9–15. 2015. [5] D. Fox, J. Hightower in sod. Bayesian filtering for location estimation. IEEE Pervasive Computing, zv. 2, št. 3, str. 24–33, 2003. [6] R. E. Kalman. A New Approach to Linear Filtering and Prediction Problems. Transactions of the ASME-Journal of Basic Engineering, zv. 82, št. Series D, str. 35–45, 1960. [7] L. Teslić, I. Škrjanc in G. Klančar. Using a LRF sensor in the Kalman-filtering-based localization of a mobile robot. ISA transactions, zv. 49, št. 1, str. 145–153, 2010. [8] L. Teslić, I. Škrjanc in G. Klančar. EKF-based localization of a wheeled mobile robot in structured environments. Journal of Intelligent & Robotic Systems, zv. 62, št. 2, str. 187–203, 2010. [9] G. Klančar, L. Teslić in I. Škrjanc. Mobile-robot pose estimation and environment mapping using an extended Kalman filter. International Journal of Systems Science, zv. 45, št. 12, str. 2603–2618, 2014. [10] B. D. O. Anderson in J. B. Moore. Optimal Filtering. Prentice-Hall, Englewood Cliffs, New Jersey, 1979, 367 str. [11] S. Thrun, W. Burgard in D. Fox. Probabilistic robotics. MIT Press, 4. izd., 2005, 647 str. 7 Agenti in večagentni sistemi 7.1 Uvod Eden od načinov reševanja določenih nalog je vpeljava agenta ali entitete, tj. smiselno zaključena celota, ki je zmožna sama bolj ali manj uspešno reševati določen problem. Agenti so lahko fizični (robot) in vplivajo na stvarni svet, ali pa virtualni (simulacije, programske komponente) in vplivajo na virtualno okolje. Agenti, ki delujejo v nekem okolju, sestavljajo večagentni sistem. Večagentni sistemi podajajo principe za gradnjo kompleksnih sistemov s pomočjo agentov in mehanizmov za koordinacijo delovanja neodvisnih agentov. Osnovno vodenje ali delovanje agenta je potrebno, ne pa tudi zadostno, za usklajeno delovanje skupine agentov pri doseganju skupnega cilja. Vodenje večagentnega sistema je tako vedno kombinacija učinkovitega delovanja na nivoju osnovnih agentov in ustreznega sodelovanja med njimi. V nadaljevanju je podanih nekaj definicij ter klasifikacij agentov in večagentnih sistemov. 7.2 Večagentni sistemi Večagentni sistemi so razmeroma mlada veda na področju umetne inteligence. Pristopi večagentnih sistemov posegajo na področje porazdeljene umetne inte- 376 Agenti in večagentni sistemi ligence in umetnega življenja. Namen prvega je razvoj organizacije sistemov, ki so zmožni reševati probleme z razmišljanjem, drugi pa skuša modelirati žive organizme, zato preučuje tudi zmožnost preživetja in prilagajanja v običajno sovražnem okolju. Porazdeljeni sistemi so v računalništvu zelo dobro uvelja- vljeni (večprocesorski sistemi), medtem ko metode za koordinacijo več agentov v robotiki pridobivajo na priljubljenosti. Večagentni sistem je sestavljen iz več popolnoma ali delno avtonomnih agentov in podaja mehanizme za koordinacijo njihovega delovanja. Agenti izkazujejo neko obnašanje, ki ga pogosto določajo enostavna pravila in na katerega vpliva komunikacija z drugimi agenti ter interakcije z okoljem in objekti v okolju. Izziv večagentnih sistemov je predvsem sodelovanje več agentov, kar dosežemo z izvedbo nekaj korakov za zagotovitev njihove sinhronizacije, komunikacije (neposredno: sistem sporočil, skupne tabele ipd.; posredno: opazovanje ostalih, sklepanje) in pogajanj o delitvi dela. Večagentni sistem, prikazan na sliki 7.1, lahko torej opredelimo kot sistem, ki v splošnem vsebuje • okolje, • množico pasivnih objektov, • množico agentov (aktivni objekti v okolici) ter • množico odnosov in metod interakcije agentov z objekti okolice. Slika 7.1: Prikaz večagentnega sistema, kjer agent zaznava in vpliva na okolje ter ostale agente v okolju Večagentne sisteme, kjer so edini objekti agenti ter okolje ni definirano, imenujemo komunikacijski večagentni sistemi. V tem primeru odnosi med agenti predstavljajo omrežje, v katerem je vsak agent povezan z ostalimi. Taki sistemi so pogosti na področju porazdeljene umetne inteligence (DAI), kjer so agenti tipično programski moduli. 7.3. Agenti 377 V kolikor pa so agenti situirani v okolju ter komunikacija poteka le posredno preko zaznavanja in delovanja na okolje, imamo izključno situiran večagentni sistem. Splošni večagentni sistemi imajo lastnosti obeh omenjenih skrajnih oblik. 7.3 Agenti Čeprav stroga, uveljavljena definicija agenta ne obstaja, lahko rečemo, da agenta predstavlja entiteta v nekem okolju, ki lahko okolje tudi zazna in v njem deluje, ima cilje, znanje iz določenega področja ter zmožnost odločanja. Agent ima senzorje, s katerimi zaznava okolje (npr. senzor bližine zazna oviro), aktuatorje, s katerimi vpliva na okolje (npr. kolesni pogon premakne robota in/ali odrine oviro), ter znanje o okolju, v katerem deluje in mu omogoča, da s pomočjo informacije iz senzorjev upravlja svoje aktuatorje za doseg cilja (npr. doseg želene lokacije). Naštejmo nekaj lastnosti, ki opisujejo fizičnega ali virtualnega agenta: • zmožnost delovanja v okolju, • zmožnost komunikacije z ostalimi agenti, • ima nabor svojih teženj in ciljev, • ima dostop do virov (napajanje, CPU, spomin, informacije), • ima zmožnost zaznavanja svoje okolice (do določene mere), • ima svojo (delno) predstavitev okolice, ali pa je sploh nima, • lahko se reproducira, • njegovo delovanje stremi k dosegu ciljev, kjer uporablja vire, svoja zna- nja, zaznave senzorjev, svojo predstavitev okolice (oz. znanje o njej) in komunikacijo. Pomembna lastnost agenta je avtonomnost, kar pomeni, da agent ni upravljan preko operaterja ali drugega agenta, ampak je sposoben samostojnega delovanja glede na lastne cilje in situacije, v katerih se znajde. Avtonomni agent ima tudi dostop do lastnih virov, kot so napajanje, pomnilnik, informacije, itd. Agentovo zaznavanje je omejeno z lastnostmi lastnih senzorjev, zato ima le delno predstavitev okolja, saj ne more zaznavati vsega dogajanja v okolju. Na voljo so mu samo lokalne informacije, torej tiste v dosegu njegovih senzorjev, zato so večagentni sistemi večinoma decentralizirani (obnašanje agentov ni centralno nadzorovano). Agent ima svoje trenutno stanje v okolju predstavljeno s spremenljivkami, njegovo delovanje pa je odvisno od stanja, v katerem se nahaja. Več ko ima možnih stanj, na več različnih načinov lahko deluje. Agenti v večagentnem sistemu se lahko 378 Agenti in večagentni sistemi razlikujejo po lastnostih, obnašanju, virih, zmožnosti predstavitev, sposobnosti pomnjenja dogodkov in interpretaciji razpoložljivih informacij. Agent mora imeti sposobnost prilagajanja. Hkrati mora biti njegovo vgrajeno znanje fleksibilno, da ga lahko dopolnjuje s spreminjanjem določenih parametrov. Možni so tudi določeni algoritmi, ki slonijo na evoluciji živih bitij, ter ostali algoritmi strojnega učenja (genetski algoritmi, nevronske mreže, učenje z nagra-jevanjem). Uspeh teh metod pogojuje dejstvo, da je problem umetne inteligence pogosto kombinacijsko preveč kompleksen, da bi bil rešljiv v realnem času. Zato se inteligenca agenta skoraj vedno sestoji le iz dveh virov: znanja, pridobljenega na osnovi lastnih izkušenj (učenje, adaptiranje), ter vgrajenega znanja. Agent ima za razliko od ostalih programov in objektno orientiranega programira- nja naslednje lastnosti: • zaznava okolje, v katerem se nahaja, • ima sposobnost interakcije z ostalimi agenti in (najpomembnejše) • na poti k izpolnjevanju lastnih ciljev se odloča in deluje samoiniciativno. Objekti so pasivni elementi, ki nimajo možnosti izbire svojega delovanja, temveč delujejo le na zunanjo iniciativo. 7.4 Arhitektura in delovanje agentov Klasičen in uveljavljen način vodenja v mobilni robotiki in avtomatiki (od leta 1985 dalje) temelji na načelu zaznaj-planiraj-ukrepaj (SPA, angl. sense-plan-act). Sistem najprej s senzorji pridobi informacijo iz okolja, nato pa zgradi model z uporabo pridobljene informacije in načrta, oz. izračuna naslednji korak. Agent mora torej ugotoviti, kako naj se z vgrajeno strategijo odzove na zaznane podatke. Na koncu agent ukrepa in izvede akcijo. SPA poteka v iteracijah – po zaznavanju, planiranju in ukrepanju se celoten cikel ponovi. SPA je osnova avtomatskega vodenja, kjer se poskuša postopno zmanjševati pogrešek med želenim in dejanskim stanjem mobilnega (ali kateregakoli drugega) sistema (slika 7.2). 7.4.1 Kognitivni agenti Kognitivni agenti (angl. deliberative agents) delujejo po principu SPA. Ko agent zazna okolico s pomočjo modela sveta (simbolična predstavitev okolice), naredi načrt za izvedbo akcije. Agent določi načrt reševanja problema (slika 7.3) korak za korakom (interpretacija zaznav senzorjev, modeliranje, odločanje, planiranje, izvedba opravil, upravljanje 7.4. Arhitektura in delovanje agentov 379 okolje senzorji aktuatorji zaznaj planiraj ukrepaj (meri, oceni) (izračunaj) (izvedi) Slika 7.2: Osnovni način upravljanja agenta (SPA) aktuatorjev) na osnovi svojega zaznavanja okolja. Vsak tak agent ima običajno bazo podatkov in znanje, potrebno za reševanje problemov. V nepredvidljivem dinamičnem okolju agent z izključno kognitivnimi sposobnostmi ni učinkovit, saj njegov načrt reševanja problemov ne more predvideti sprememb okolja in bi ga moral nenehno spreminjati. okolje senzorji aktuatorji čanje percepcija modeliranje odlo planiranje izvedba opravil Slika 7.3: Kognitivni agent naredi načrt reševanja problema na osnovi zaznav Kognitivni agenti imajo nedvomno prednost v statičnih in poznanih okoljih. V primeru nepričakovanih dogodkov, glede na njihov model sveta, pa lahko odpovejo. Potrebujejo precej natančen model sveta (npr. zemljevid), ki ga je pogosto težko dobiti in vzdrževati. Za svoje delovanje potrebujejo veliko procesno moč, kar se lahko odraža v počasnem odzivu na spremembe v oklici. 7.4.2 Odzivni agenti Odzivni agenti (slika 7.4) so zmožni povezati svoje zaznavanje okolja z akcijami vodenja (različno vnaprej definirano obnašanje) in s tem kar najbolje izvršiti 380 Agenti in večagentni sistemi naloge brez gradnje internega modela okolja (kar je sorodno obnašanju manjših živali, npr. mravelj). Delujejo torej po principu zaznaj-deluj brez uporabe simbolične predstavitve okolja (modela sveta) in planiranja. Zanašajo se le na eno ali več enostavnih pravil, ki neposredno povežejo zaznave senzorjev z akcijami. V osnovi odzivni agenti nimajo stanj (ne shranjujejo nekaterih preteklih podatkov), so brez spomina in internega modela okolice, nimajo možnosti planiranja akcij vnaprej ter niso zmožni učenja. Njihova prednost je ravno v njihovi preprostosti, kar jim omogoča hiter (trenuten) odziv. okolje odzivna pravila odziv 1 odziv 2 senzorji odziv 3 aktuatorji ... odziv N Slika 7.4: Odzivni agent reagira na zaznave brez načrtovanja 7.4.3 Hibridni agenti V nepredvidljivem dinamičnem okolju so primernejši hibridni agenti, ki združujejo dobre lastnosti odzivnih in kognitivnih agentov. Obstajajo agenti, ki nimajo celotne ali obsežne simbolične predstavitve sveta okoli njih, ampak si zapomnijo le nekaj pomembnejših parametrov, kar jim lahko pomaga pri boljši asociaciji zaznav z akcijami ali izvedbi bolj dovršene akcije (npr. agent si lahko zapomni, da je v bližini stene). Nadalje ima lahko agent zmožnost adaptacije. To pomeni, da spreminja vzorce delovanja (obnašanje) in se prilagaja spreminjajočim se razmeram glede na svoje prejšnje izkušnje. Z drugimi besedami lahko temu rečemo tudi učenje. Za učenje na individualni ravni mora imeti agent spomin – torej pri popolnoma odzivnih agentih to ni mogoče. Obstaja tudi prilagodljivost na ravni sistema, ki je mogoča tudi pri večagentnih sistemih, sestavljenih iz odzivnih agentov. Če se agenti v sistemu lahko reprodu-cirajo, se lahko poveča število tistih agentov, ki so bolj primerni za novonastale razmere. 7.4. Arhitektura in delovanje agentov 381 7.4.4 Odzivno vedenjski agenti Njihovo delovanje je enako delovanju odzivnih agentov, le da namesto enostavnih pravil uporabljajo vloge. Vloge predstavljajo module, ki se izvajajo paralelno (slika 7.5), torej ima vsak modul dostop do senzorjev in lahko neposredno upravlja z aktuatorji. Vsaka vloga vsebuje neko znanje v obliki algoritmov vodenja, ki agentu omogočajo primerno delovanje v določeni situaciji (sledenje steni, iskanje predmeta, izogibanje oviri, prihod v začetni položaj itd.). Podajmo nekaj lastnosti vedenjskih agentov: • vsebujejo različne vloge za doseg ali sledenje ciljem, • vhodne informacije prejmejo vloge od senzorjev in drugih vlog ter posredu- jejo ukaze aktuatorjem, • vloge so lahko kompleksne in sestavljene iz različnih akcij (akcije: stop, naprej, levo itd.; vloge: sledenje cilju, izogibanje oviri). Ker se vloge izvajajo hkrati in neodvisno, so taki agenti primerni za aplikacije v realnem času. Vloge imajo lahko stanja (si zapomnijo zgodovino), model okolice in zmožnost planiranja vnaprej, kar omogoča izvedbo učinkovitih vlog. okolje odzivna pravila vloga 1 vloga 2 senzorji aktuatorji vloga 3 ... vloga N Slika 7.5: Vedenjski agent se odziva na zaznave z izvajanjem vlog Primer 7.1 Poglejmo si primer izvedbe enostavnega kognitivnega agenta in odzivnega agenta. Agent je mobilni robot, ki želi iti skozi zaklenjena vrata. Rešitev 382 Agenti in večagentni sistemi Kognitivni agent lahko planira svoje delovanje, torej bo zgradil načrt v več zaporednih korakih v obliki: Načrt odpiranja vrat: Grem do mesta, kjer je spravljen ključ, vzamem ključ, grem do vrat, odprem vrata s ključem. Odzivni agent pa se brez načrtovanja ali razmišljanja odzove na situacije iz okolja. Njegovo obnašanje omogoča skupek enostavnih pravil Pi oz. vlog: P1: Če sem pred vrati in imam ključ, potem odprem vrata. P2: Če sem pred vrati in nimam ključa, potem poskusim odpreti vrata. P3: Če se vrata ne odprejo in nimam ključa, potem grem iskat ključ. P4: Če iščem ključ in vidim ključ pred sabo, potem vzamem ključ in grem proti vratom. Vidimo, da kognitivni agent zgradi načrt, medtem ko ima odzivni agent že predhodno vgrajena pravila. Kognitivni agent bo gotovo odprl vrata hitreje, z manj akcijami, saj lahko predvidi zaporedje potrebnih akcij. Odzivni agent pa bo najprej šel do vrat in nato ugotovil, da nima ključa in da ga mora iti iskat. Vendar je odzivni agent bolj robusten: če so vrata odprta, jih bo odprl takoj, ne da bi šel po ključ. Kognitivni agent pa bo šel najprej po ključ, saj njegov model ne predvideva možnosti, da so vrata mogoče že odprta. 7.4.5 Osnovne vedenjske arhitekture Možnih je več načinov izvedbe arhitekture odzivno vedenjskih agentov. Osnovni arhitekturi sta tekmovalna in vsebovana shema. Tekmovalna shema Tekmovalna shema (angl. competitive architecture, motor schema architecture) je princip, ki ga je vpeljal Arkin [1]. Vloga sestoji iz sheme percepcije, ki procesira vhode iz senzorjev in jih posreduje motornim shemam. Tekmovalne ali motorne sheme generirajo izhode za vodenje, ki določajo premikanje robota, da doseže cilj. Gre za to, da več konkurenčnih vlog (shem) posreduje svoje ukaze (hitrost, smer gibanja itd.) agentu, ukazi posameznih vlog pa so z uporabo potencialnega 7.4. Arhitektura in delovanje agentov 383 polja predstavljeni kot vektorji, ki so normirani glede na percepcijo in motorno shemo vloge. Prispevki posameznih vlog se nato združijo v končen ukaz, ki je posredovan aktuatorju agenta. Druga možnost pa je, da se izmed vseh vlog izbere le najbolj uspešno (uspešnost se oceni na podlagi določenih parametrov). V osnovi gre za privlačna in odbojna polja. Predstavljajmo si primer, kjer agenta privlači cilj (vektor smeri vožnje je v smeri cilja), hkrati pa ga odbija od ovire (vektor želene smeri vožnje kaže stran od ovire). Bliže je agent oviri, bolj prevladuje odbojni vektor smeri in privlačni vektor proti cilju se zmanjšuje. Končna usmeritev je vektorska vsota teh dveh normiranih vektorskih polj. Primer 7.2 Poglejmo si primer vedenjskega agenta, katerega vloge so organizirane v tekmo- valno oz. motorno shemo. Imamo preprostega raziskovalnega robota, ki raziskuje okolje in ob zaznavi predmeta gre ponj. Ko mu zmanjka energije, gre napolnit akumulatorje. Nabor vlog za izvedbo delovanja agenta povežemo v strukturo, kot nakazuje slika 7.6. agent koordinator Raziskuj okolje senzorji Pojdi in primi objekt aktuatorji S Polni baterije okolje Slika 7.6: Tekmovalna shema 384 Agenti in večagentni sistemi Vsebovana shema Vsebovana shema (angl. subsumption architecture) je način dekompozicije inteli-gentnega obnašanja agenta v več preprostih vlog, ki so organizirane po nivojih glede na prioriteto. Posamezne nivoje lahko zgradimo z uporabo končnih av- tomatov. Pojem je vpeljal Rodney Brooks [2]. Vse vloge se izvajajo hkrati in prejemajo informacijo od senzorjev. Vloge z višjo prioriteto posredujejo ukaze aktuatorjem. Tu velja opomniti, da določene vloge (opravila) lahko onemogočijo ali spremenijo percepcijo, pa tudi povozijo akcije nižjih prioritetnih vlog. Vloge lahko onemogočijo svoje delovanje (onemogočijo vhode ali izhode), če na podlagi senzorjev ni izpolnjen pogoj za njihovo izvajanje. Vloga z višjo prioriteto lahko onesposobi vloge z nižjo prioriteto. Vloga z najvišjo prioriteto, ki ostane aktivna, določa novo akcijo. Vloge z višjimi prioritetami (v višjih slojih) so bolj abstraktne in lahko popolnoma dosežejo cilj. Višje vloge vključujejo funkcije nižjih vlog. Vloge z nižjimi prioritetami (nižji sloji) pa so preprostejše in hitro odzivne (refleksi). Primer 7.3 Poglejmo si primer vedenjskega agenta, katerega vloge so organizirane v vsebovano shemo. Raziskovalnega robota z nekoliko nadgrajeno funkcionalnostjo primera 7.6 podaja slika 7.7. Nabor vlog za izvedbo delovanja agenta povežemo v vsebovano strukturo, kjer so vloge razdeljene v nivoje. Vloge v višjih nivojih lahko onemogočijo vloge v nižjih nivojih, kar nakazujejo krogci. 7.4. Arhitektura in delovanje agentov 385 agent Izogibanje ovir Polnjenje akumulatorjev Optimizacija poti Gradnja zemljevida Raziskuj Prijemanje objekta senzorji Naklučno tavanje aktuatorji okolje Slika 7.7: Vsebovana shema 7.4.6 Ostale delitve agentov in večagentnih sis- temov Večagentne sisteme lahko obravnavamo glede na štiri lastnosti agentov: • granulacija agentov (fina ali groba), • raznolikost znanja agentov (splošno ali specializirano), • znanje agenta (konstruktivno ali tekmovalno, ekipno ali hierarhično, statično ali dinamično spreminjanje vloge) in • komunikacija agentov (oglasna deska ali sporočila, malo ali veliko komuni- kacije, neposredna ali posredna vsebina). 386 Agenti in večagentni sistemi Ponavadi imajo agenti grobo granulacijo in visoko stopnjo komunikacije, v ostalih lastnostih pa se razlikujejo. Skupina sodelujočih agentov je pri reševanju kompleksnega problema lahko bolj prilagodljiva in ekonomična kot en sam zmogljivejši agent, če le uspemo učinkovito rešiti oz. zagotoviti njihovo koordinacijo. Dejstvo pa je, da ni smiselno pretiravati z večanjem števila agentov pri opravljanju nekega opravila, ker je v tem primeru vloženo preveč energije v njihovo koordinacijo, komunikacijo in pogajanja. Isto opravilo lahko enako ali bolj učinkovito opravi tudi manj agentov. 7.5 Primeri uporabe večagentnih siste- mov Področje uporabe večagentnih sistemov je zelo široko. Tako imamo aplikacije v avtomatizaciji proizvodnje (avtomobilska proizvodnja, avtonomni vozički v skladiščih) in robote skavte (nevarna območja). Nekatere aplikacije pošiljajo robote skavte v izvidnico, kjer le-ti med seboj sodelujejo, raziskujejo in kartirajo teren. To so lahko simulacije ali pa resnične aplikacije (vojska, vesolje, nevarni tereni: globina, vulkani, minska polja itd.). Modele večagentnih sistemov lahko uporabljamo za simulacijo transporta in prometa, za raziskovanje potrošniških in finančnih trgov, preučevanje razširjanja epidemij, optimizacijo proizvodnje in logistike, simulacijo premikov bojnih enot na bojišču in simulacijo socialnih mrež. Večagentni sistemi se uporabljajo tudi v filmski industriji za simulacijo velikih množic ljudi. V filmih, kot so Avatar, Gospodar prstanov, King Kong ipd., je bil uporabljen programski paket MASSIVE (angl. Multiple agent simulation system in virtual environment). Nekateri modeli so enostavnejši in zajemajo le bistvene lastnosti sistemov, drugi pa so kompleksnejši in preverjeni tudi za uporabo podatkov iz realnega sveta. S pomočjo razvoja specialne programske opreme za modeliranje večagentnih sistemov in povečevanjem računske moči računalnikov je možno ustvarjati vedno bolj napredne večagentne aplikacije, s katerimi pridemo do točnejših rezultatov in ugotovitev na raznovrstnih strokovnih področjih. 7.5.1 Robotski nogomet — avtonomna igra ko- lesnih robotov Prikazan je primer, kako napisati program za kolesne robote, da ti lahko avto- nomno igrajo nogomet. Vsak robot je predstavljen kot agent, ki lahko okolje zaznava in v njem deluje v skladu s svojimi cilji in znanjem, ki ga o okolici ima. Delovanje agentov je izvedeno s pomočjo predstavljenih arhitektur delovanja (odzivne, kognitivne, hibridne in vedenjske). Vsak agent ima zakodirano znanje, potrebno za izvedbo osnovnih opravil, kot so vožnja v želeno lego, streljanje žoge 7.5. Primeri uporabe večagentnih sistemov 387 Slika 7.8: Poligon za robotski nogomet (levo) in diagram izvedbe vodenja (desno) v želeno smer, branjenje gola in podobno. Ker pri igri nogometa sodeluje več agentov je potrebno zagotoviti ustrezno koordinacijo njihovega delovanja. Slednje je zagotovljeno z vsebovano shemo in s predpisanimi prioritetami izvajanja vlog glede na želeno strategijo igre. V nadaljevanju so prikazane osnovne izvedbe vlog (npr. vratar, napadalec, obrambni igralec itd.), ki jih posamezni agenti lahko izvajajo. Pri tem so ključni algoritmi, za izvedbo gibanja dvokolesnih robotskih vozil po trajektoriji, v želeno lego in za izogibanja ovir. Te algoritme lahko nato uporabimo za izvedbo vlog agentov (npr. vratar, napadalec). Vloge lahko na podlagi zaznanih informacij iz senzorjev določijo ustrezne akcije. Primer je lahko vratar, ki se mora premakniti na ustrezno pozicijo, da brani strele žoge ali napadalec, ki mora znati priti do žoge in jo ustreliti (potisniti) v smeri nasprotnikovega gola. Vloge pa se morajo znati tudi izogibati oviram, ki so lahko drugi igralci ali ograja igrišča. Nekatere vloge so povsem odzivne druge ali pa vsebujejo tudi elemente planiranja (kognitivne vloge). Primer slednjega je prediktivno delovanje za bolj učinkovito in hitrejše prestrezanje gibajoče žoge. Slika 7.8 prikazuje testni poligon, ki sestoji iz dvokolesnih robotov (deset robotov za dve moštvi) kockaste oblike s stranico 7.5 cm, igrišča velikosti 2 . 2 × 1 . 8 m, barvne kamere in osebnega računalnika. Kamera je nameščena nad igriščem in omogoča sledenje objektov (žoge in igralcev) na podlagi barvne informacije [3]. Osnovno delovanje posameznega agenta (robota) prikazuje levi del slike 7.8. Na podlagi trenutnih lokacij objektov in izbrane strategije delovanja program agentom dodeli ustrezne vloge. Vloga med drugim vsebuje algoritem za določitev želene smeri in hitrosti gibanja agenta. Algoritem vodenja nato določi referenčne ukaze za translatorno in kotno hitrost, ki se robotu pošljejo preko brezžične povezave. Robot nato s pomočjo internega regulatorja PID doseže želene hitrosti vrtenja koles. Omenjeni cikel se izvaja s frekvenco 30 ali 60 Hz. 388 Agenti in večagentni sistemi j ref ( x , y ref r ) ef ( x , y g g) j ( ) x, y Slika 7.9: Algoritem vodenja napadalnega robota, ki ustreli žogo proti golu Podrobnejše delovanje sistema je opisano v treh podpoglavjih. V prvem so opisani algoritmi za izvedbo gibanja. V drugem delu predstavimo vloge za izvedbo odzivno vedenjske arhitekture delovanja posameznega agenta. V zadnjem podpoglavju pa je predstavljen večagentni sistem s kooperativnim delovanjem upoštevajoč strategijo igre. Izvedba algoritmov za različne vloge Za izvedbo želenega delovanja agentov je potrebno napisati ustrezne algoritme vodenja. Ti algoritmi bodo npr. napadalcu omogočili, da se žogi približa s prave strani in jo potisne v smeri gola. Podobno se mora vratar voziti v liniji pred golom in prestreči strele na gol. V nadaljevanju predstavimo nekaj osnovnih vlog uporabljenih v igri robotskega nogometa. Vloga napadalec Osnovni način delovanja napadalca (oz. agenta z vlogo napadalec) je, da pride do žoge in jo potisne v smeri gola. Torej referenčna lokacija vsebuje pozicijo žoge ( xref , yref ) in referenčno orientacijo ( ϕref ), ki je v smeri želene smeri gibanja žoge po trku (glejte sliko 7.9). Za strel proti golu ( xg, yg) definiramo referenčno orientacijo kot ϕref = atan2 ( yg − yref , xg − xref ) kjer je atan2 ( y, x) štirikvadrantna razširitev funkcije arctan y . Za izvedbo vodex nja lahko uporabimo enega od prikazanih algortimov v poglavju 3.2.3. V primeru, ko se žoga premika, je vodenje proti trenutni poziciji žoge ( xb, yb), kot ga prikazuje slika 7.9, manj učinkovito. Boljše delovanje lahko dosežemo, če 7.5. Primeri uporabe večagentnih sistemov 389 ( x , y b b) vb j b a j ref g d ( x , y ( x , y g g) ref r ) ef b v ( ) x, y Slika 7.10: Predikcija gibanja žoge in ocena točke, kjer robot žogo lahko prestreže. Predpostavimo premočrtno gibanje robota in konstantno hitrost v. izračunamo prihodnjo pozicijo žoge, kjer jo prestreže. V splošnem je izračun predikcije žoge za poljubno gibanje robota zahteven problem. Zato tu predpostavimo, da se robot lahko giblje premočrtno, kot prikazuje slika 7.10. Relativni kot α med smerjo kotaljenja žoge in povezavo z robotom lahko ocenimo iz skalarnega produkta [ vb cos ϕb, vb sin ϕb] T · [ x − xb, y − yb] T = cos α v p b ( x − xb)2 + ( y − yb)2 Nadalje z uporabo sinusnega izreka vb = v določimo še kot β in kot γ = sin β sin α π − α − β. Končno lahko z uporabo sinusnega izreka d = vbt izračunamo sin γ sin β potreben čas, ki ga robot rabi, da doseže predvideno pozicijo žoge ( xref , yref ). d sin β t = vb sin γ Predikcija pozicija žoge oziroma referenčna pozicija je xref = xb + vbt cos ϕb yref = yb + vbt sin ϕb Izračunana referenčna točka bo dosegljiva le, če bo robot na začetku usmerjen proti žogi in bo hkrati želena smer strela žoge ϕref podobna začetni orientaciji robota ϕ. Slednje je redko izpolnjeno saj je dejanska pot robota večinoma daljša od direktne linije, ki jo pri izračunu predpostavimo. Robot v splošnem tudi ni obrnjen proti predvideni referenčni točki in končna smer strela mora biti v smeri gola. Upoštevajoč približno oceno dejanske razdelje vožnje l do referenčne lege xref , yref , ϕref lahko prilagodimo hitrost vožnje robota vn = l , da ta doseže t izračunano predikcijo žoge v predvidenem času t. Vloga napadalec s premočrtnim enakomerno pospešenim strelom 390 Agenti in večagentni sistemi ( x , y b b) ( x , y g g) vb j b j ref ( x , y ref r ) ef j ( ) x, y Slika 7.11: Premočrtni, enakomerno pospešeni strel na gol. Robot na začetku miruje in je usmerjen proti golu. Potrebno pospeševanje in predikcijo pozicije žoge je potrebno izračunati. Gibajočo žogo lahko natančno ustrelimo na gol, če je začetna orientacija robota v smeri gola xg, yg in je gibanje robota enakomerno pospešeno. Razmere so prikazane na sliki 7.11. Neznano referenčno pozicijo ( xref , yref ) lahko izračunamo iz trenutne znane pozicije žoge in njenega gibanja (hitrost vb in smer ϕb) xref = xb + vbt cos ϕb (7.1) yref = yb + vbt sin ϕb oziroma jo lahko določimo tudi iz pozicije robota xref = x + ( xg − x) p (7.2) yref = y + ( yg − y) p kjer je t čas, ko robot lahko doseže žogo s predpisanim premočrtnim gibanjem in p je ustrezen parameter. Z upoševanjem enačb (7.1) in (7.2) lahko napišemo sledečo matrično relacijo " # " # " # xg − x − vb cos ϕb p xb − x = yg − y − vb sin ϕb t yb − y ki je krajše predstavljena kot Au = b. Določimo njeno rešitev u = A−1b. Rešitev je veljavna, če je 0 < p < 1, kar pomeni, da je točka prestrezanja žoge med robotom in golom. Referenčno točko določimo kot xref = xb + vbt cos ϕb yref = yb + vbt sin ϕb 7.5. Primeri uporabe večagentnih sistemov 391 T ( x , y ) ref ref Y j v b b ( x , y ) b b ( ) x, y X T Slika 7.12: Gibanje robota pri vlogi vratar in potreben pospešek vožnje a( t) upoštevajoč trenutno hitrost vožnje v( t) dolo- čimo iz 2 p( xref − x)2 + ( yref − y)2 − v( t) a( t) = . t 2 V kolikor je vloga uspešna (0 < p < 1) robotu ukažemo novo translatorno hitrost gibanja v (( k + 1) Ts) = v( kTs) + a( kTs) Ts, kjer je v(( k + 1) T s) hitrost v naslednjem trenutku, v( kTs) trenutna hitrost, Ts je računski korak in k je naravno število. Kotno hitrost robota vodimo, da se ta pelje v želeni smeri ϕref = arctan yg− y . xg− x Vloga vratar Vratar se se giblje v ravni liniji pred golom med točkama T in T kot je prikazano na sliki 7.12. Trenutno referenčno pozicijo xref , yref je potrebno določiti glede na pozicijo žoge xb, yb, njeno hitrostjo vb in smerjo kotaljenja ϕb. Robot se mora prediktivno premakniti na pozicijo kjer bo žoga prečkala črtkano črto pred golom. Iz znanih podatkov določimo čas, v katerem bo žoga prečkala linijo gola in referenčno pozicijo xg − xb t = vb cos ϕb xref = xg yref = yb + vbt sin ϕb π ϕref = 2 kjer je xg koordinata linije gola. 392 Agenti in večagentni sistemi Za vožnjo po liniji gola lahko uporabimo linearen algoritem vodenja za sledenje trajektoriji predstavljen v poglavju 3.3.5. Kjer nastavimo referenčne hitrosti ( vref , ωref ) za referenčno pozicijo na nič. Regulacijski zakon se tako poenostavi v v = kxex ω = kϕeϕ + sign( v) kyey kjer so ojačenja kx, ky in kϕ lahko konstantna in določena eksperimentalno. Lokalne pogreške ex, ey in eϕ izračunamo z izrazom (3.28), kjer poskrbimo, da je eϕ v območju − π < eϕ ≤ π. Delovanje vratarja lahko nadalje izboljšamo, če zagotovimo, da se vratar nikoli ne obrača za več kot | π |. V primeru, ko je | e , to dosežemo z vzvratno 2 ϕ| > π 2 vožnjo in popravljenim pogreškom orientacije ( eϕ − π ; eϕ > π e 2 ϕ = eϕ + π ; eϕ < − π 2 Izogibanje oviram Igrišče je obkroženo z ograjo in v igri sodeluje več igralcev, zato morajo opisane vloge (predvsem napadalec) vsebovati tudi algoritem izogibanja oviram. Tako obnašanje lahko učinkovito dosežemo z uporabo metode potencialnega polja, ki je predstavljena v poglavju 4.2.4. Arhitektura delovanja posameznega agenta Za namen lažje razlage predpostavimo, da moštvo sestoji iz treh robotov oziroma agentov. Vsak agent lahko v danem trenutku izbira med naborom različnih vlog (npr. vratar, napadalec, sredinski igralec, itd.). Izbira vlog in s tem delovanje agenta je izvedena z vsebovano shemo, kjer so vloge organizirane po prioritetah, kot je opisano v poglavju 7.4.5. Z izbiro prioritet lahko določamo strategijo igre. Bolj obrambno strategijo dosežemo z večjimi prioritetami obrambnih vlog in obratno, bolj napadalno strategijo dobimo, če imajo višjo prioriteto napadalne vloge. V danem trenutku glede na trenutno situacijo igre (lega agenta, pozicija žoge, pozicije soigralcev in nasprotnih igralcev) se agent odloči katero vlogo bo izvajal. Agent najprej preverja pogoje za izvedbo bolj prioritetnih vlog, če pogoji niso izpolnjeni, potem preverja manj prioritetne vloge. Primer pogoja za vlogo vratar je razdalja agenta do gola, v kolikor je najbližje golu (med soigralci) je pogoj za izvajanje vloge vratar izpolnjen. Podobno lahko definiramo pogoj za napadalne vloge upoštevajoč razdaljo do žoge. 7.5. Primeri uporabe večagentnih sistemov 393 Posamezne vloge lahko imajo več možnih načinov delovanja. Pri izvajanju vloge vratar se mora agent nahajati pred golom. Če temu ni tako, se mora agent najprej pripeljati pred gol. Če se nahaja pred golom, potem lahko brani gol, izračuna pot žoge in se prediktivno premakne na pozicijo, kjer bo žoga prečkala linijo gola (kot je opisano v podpoglavju 7.5.1). Podobno lahko agent izvaja napadalne vloge, če je najbližje žogi oziroma jo lahko najhitreje doseže (glede na ostale soigralce). Agent lahko izvaja osnovno vlogo napadalec brez predikcije gibanja žoge, če je hitrost kotaljenja žoge nizka, drugače pa izbere prediktivno delovanje. Če je izpolnjen pogoj za premočrtno pospešen strel, potem izvaja to vlogo, saj je njena uspešnost večja in ima zato nastavljeno višjo prioriteto. Dodatno napadalec preverja možnost trka z ovirami in se jim izogiba. Izogibanje pa ni vselej zaželeno, npr. če ima napadalec žogo v posesti, izogibanje soigralcem ali nasprotnikom ni vselej smiselno. Tretji agent je sredinski igralec, ki se poskuša pozicionirati na strateške lokacije (preddoločene) in se orientirati proti nasprotnikovem golu. Najprej se mora pripeljati v želeno pozicijo, nato pa še zavrteti v želeno smer proti nasprotnemu golu. Ta vloga je pomembna za potek igre, saj agenti lahko dinamično spreminjajo vloge med igro (vloge niso statično dodeljene). Igralec s to vlogo lahko v naslednjem trenutku, če je izpolnjen pogoj, prevzame vlogo prediktivnega napadalca in izvede pospešen strel na gol. Večagentna igra, koordinacija in strategija igre Izvajanje vseh predhodno definiranih vlog z razpoložljivimi agenti lahko rezultira v avtonomno igro robotskega nogometa. Za boljšo učinkovitost moštva pa je potrebno poskrbeti še za koordinacijo izvajanja vlog med agenti. V danem trenutku je namreč lahko vratar le en igralec in tudi ni smiselno, da več agentov hkrati želi prevzeti žogo, saj bi se pri tem ovirali. Opisane vloge imajo dodeljene prioritete, kar definira želeno strategijo igre (bolj obrambno ali napadalno). Dodatno ima vsaka vloga tudi funkcijo kriterij, s katero lahko posamezen agent preveri njeno učinkovitost v dani situaciji. Kriteriji so lahko enostavni kot na primer: razdalja do žoge, razdalja do gola in podobno. Vsi agenti tako najprej preverijo njihovo učinkovitost izvajanja za najbolj prioritetno vloge (npr. vratar) in se pogajajo za njeno izvajanje. Najbolj uspešen agent lahko prevzame vlogo v naslednjem trenutku igre, ostali pa se potegujejo za manj prioritetne preostale vloge (napadalec, prediktivni napadalec, sredinski igralec in podobno) iz seznama vlog. Za izvedbo pogajanja, kateri agent lahko v danem trenutku izvaja določeno vlogo, je potrebno zagotoviti medagentno komunikacijo. Agenti si lahko sporočijo kriterije učinkovitosti za posamezne vloge in se koordinirano odločijo za njihovo izvajanje. Vloge se med igro dinamično dodeljujejo agentom, kot je podrobneje opisano v 394 Agenti in večagentni sistemi [4] in prikazano tudi na dveh izsekih igre s slikama 7.13 and 7.14. V tem primeru igra pet robotov proti petim nasprotnikom in strategija domačega moštva sestoji iz enajstih vlog. Nekatere od njih so zelo podobne predhodno opisanim. Sliki 7.13 prikazuje daljši prodor agenta 1. Agent se začne približevati žogi z vlogo napadalec (indeks 0) in se nato približuje bodoči točki srečanja z žogo (vloga prediktivni napadalec z indeksom 8). Napad se zaključi tako, da agent preklopi v vlogo kreatorja (indeks 4), kjer robot v kontaktu z žogo pospešeno vozi proti golu. Omeniti je potrebno, da nasprotnikove pozicije na sliki 7.13 ustrezajo času posnetka 12.08 sekund. Zdi se, da bo nasprotnikov igralec blokiral agenta 1 in smer kotaljenja žoge, vendar se ta kasneje premakne proč in tako je imel agent 1 prosto pot v gol. Iz diagrama potekov indeksov vlog na sliki 7.13 lahko pri prvem agentu opazimo kratek skok iz vloge z indeksom 4 (kreator) na vlogo z indeksom 3 (prediktivni napadalec) in nato nazaj. To se je zgodilo zato, ker se je žoga nekoliko bolj oddaljila od agenta 1 in kriterijska funkcija za vlogo 4 ni bila več izpolnjena. V tem primeru je postala aktivna neka druga aktivna vloga (prediktivni napadalec). Nadalje lahko opazimo, da so ostali agenti zavzeli vloge vratarja (agent 2 ,vloga 1) in sredinskih igralcev (agenti 3 do 5, vloga 5). Primer dobre podaje agenta 3 z vlogo napadalec (indeks 0) preko odboja žoge od ograje prikazuje slika 7.14. Po odboju žogo prestreže agent 5 z vlogo napadalca s premočrtnim pospešenim strelom (indeks 6) in akcijo zaključi z vlogo kreatorja (vloga z indeksom 4, kjer robot v kontaktu z žogo pospešeno vozi proti golu). 7.5.2 Vožnja vozil v formaciji Bodoče inteligentne transportne sisteme si težko predstavljamo brez avtonomnih kolesnih vozil. Pomembna tovrstna aplikacija je avtomatiziran vod (formacija) vozil na avtocestah, ki lahko vozijo avtonomno en za drugim z minimalno varno- stno razdaljo kot virtualni vlak. Gre za primer večagentnega sistema. Vozila v formaciji morajo natančno in varno sledi svojemu predhodnemu vozilu z upošteva- njem minimalne varnostne razdalje. Tak pristop bi povečal gostoto transportnih vozil na avtocestah, izboljšal prometne zastoje, pretočnost in varnost. Prikazan je primer izvedbe algoritma vodenja za mobilna vozila v linearni formaciji. Za avtomatizirano vožnjo potrebujemo natančen senzorski sistem, ki meri globalne informacije vozil (npr. GPS senzorji) ali relativne informacije kot je razdalja in smer med vozili (npr. laserski pregledovalnih razdalj, LRF) oziroma oboje. V prikazanem primeru se bomo omejili le na relativne senzorje. Vodenje vozil bo izvedeno decentralizirano, kjer vozila (agenti) upoštevajo le lokalno informacije, ki jih lahko izmerijo s pomočjo laserskega pregledovalnika razdalj (LRF), kot je prikazano v [5]. Predpostavimo, da ima vsako vozilo LRF za merjenje razdalje in azimuta svojega predhodnega vozila. Pot vodilnega vozila se zabeleži v lokalnih koordinatah sledilnega vozila z uporabo odometrije in LRF meritev (razdalje D 7.5. Primeri uporabe večagentnih sistemov 395 Slika 7.13: Primer usklajenega izvajanja vlog v igri robotskega nogometa. Slika prikazuje trajektorije robotov in diagram dinamičnega dodeljevanja vlog. 396 Agenti in večagentni sistemi Slika 7.14: Primer usklajenega izvajanja vlog v igri robotskega nogometa. Slika prikazuje trajektorije robotov in diagram dinamičnega dodeljevanja vlog. 7.5. Primeri uporabe večagentnih sistemov 397 Slika 7.15: Vožnja vozil v linearni formaciji. Vsako sledilno vozilo ocenjuje pot svojega predhodnika in ji sledi s pomočjo algoritma sledenje po trajektoriji. in smeri α) kot je prikazano na sliki 7.15. Sledilna vozila sledijo ocenjeni poti svojih predhodnikov z uporabo algoritma sledenja trajektoriji, predstavljenega v poglavju 3.3. Znano je, da je lokalizacija vozila z uporabo odometrije podvržena akumulaciji različnih pogreškov skozi čas (zdrs koles, šum senzorjev in aktuatorjev ter po- dobno). Torej je uporabnost odometrije omejena le na krajša časovna obdobja, ko je napaka zaradi akumulacije še zanemarljiva. V nadaljevanju je pokazano, da absolutna napaka lege zaradi odometrije ni ključna pri vodenju v linearni formaciji saj je tu pomembna le relativna informacija med vozili ( D in α). Slednja je izmerjena z natančnim senzorjem LRF, medtem ko je odometrija uporabljena le za kratko obdobje, da ocenimo odsek poti predhodnega vozila, ki mu sledilno vozilo mora slediti v bližnji prihodnosti. V nadaljevanju najprej predstavimo tri podsisteme, ki jih kasneje integriramo v končno aplikacijo linearne formacije vozil. Prvi sklop opisuje izvedbo lokalizacije z odometrijo. Drugi sklop opiše oceno trajektorije predhodnega vozila. Tretji sklop pa predstavi algoritem vodenja za sledenje ocenjeni trajektoriji za formacijo. Vsako vozilo predstavlja neodvisnega agenta, ki izvaja omenjene algoritme. Vsi agenti (razen vodilnega) imajo enako obnašanje, s senzorji zbirajo informacije o predhodniku, ocenjujejo njegovo pot in sledijo ocenjeno pot predhodnika na predpisani varnostni razdalji. Lokalizacija z uporabo odometrije Odometrija je najpreprostejša metoda za lokalizacijo, kjer lego ocenjujemo z integracijo kinematičnega modela pri znanih hitrosti vozila. Hitrosti vozila z diferencialnim pogonom so znane v diskretnih časovnih trenutkih t = kTS, k = 0 , 1 , 2 , . . . kjer je Ts čas vzorčenja. Naslednjo lego vozila (pri ( k + 1)) ocenimo 398 Agenti in večagentni sistemi iz trenutne lege ( k) in trenutnih hitrosti (glejte poglavje 2.2.1) x( k + 1) = x( k) + v( k) Ts cos( ϕ( k)) y( k + 1) = y( k) + v( k) Ts sin( ϕ( k)) ϕ( k + 1) = ϕ( k) + ω( k) Ts Ker za namen sledenja v linearni formaciji začetna absolutna lega vozila ni pomembna jo lahko postavimo kar v začetno (pri k = 0) koordinatno izhodišče sledilnega vozila. Ocena referenčne trajektorije Vsako sledilno vozilo pozna svojo lego v globalnih koordinatah ocenjeno z odo- metrijo. Vozilo lahko tako tudi določi lokacijo svojega predhodnika iz poznanih relativnih pozicij med njima. Ta relativna pozicija je izmerjena s pomočjo meritev senzorja LRF in vsebuje razdaljo D( k) in kot do predhodnega vozila α( k). Osnovna ideja je sledenje pozicije predhodnega vozila, ocena njegove trajektorije vožnje in nato lahko sledilno vozilo uporabi to trajektorijo kot referenco in ji sledi. Lega predhodnega vozila ( xL( k), yL( k), ϕL( k)) je ocenjena kot xL( k) = x( k) + D( k) cos( ϕ( k) + α( k)) yL( k) = y( k) + D( k) sin( ϕ( k) + α( k)) Če potrebujemo pozicijo predhodnega vozila ob določenem času t 6= kTs jo lahko ocenimo s pomočjo interpolacije t − kTs xL( t) = xL( kTs) + ( xL ( k + 1) Ts − xL ( kTs)) T s t − kTs yL( t) = yL( kTs) + ( yL ( k + 1) Ts − yL ( kTs)) T s Vsako sledilno vozilo mora slediti opravljeni poti svojega predhodnika (vodilnega vozila) na razdalji DL merjeno po opravljeni trajektoriji vodilnega vozila. Torej moramo ob trenutnem času t 0 oceniti pozicijo vodilnega pri času t = T ( xL( T ), yL( T )) tako, da je razdalja med trenutno pozicijo vodilnega robota pri t 0 in prejšnjo pozicija vodilnega robota pri t = T enaka DL. Ta pozicija predstavlja referenco za sledilno vozilo (glejte sliko 7.16). Da sledimo opravljeni trajektoriji vodilnega vozila mora sledilno vozilo poznati referenčno trajektorijo in ne zgolj trenutno referenčno pozicijo. Trajektorija vodilnega vozila je ocenjena v parametrični polinomski obliki ˆ xL( t) = axt 2 + axt + ax 2 1 0 ˆ yL( t) = ayt 2 + ayt + ay 2 1 0 upoštevajoč šest pozicij vodilnega vozila, tri pred in tri za referenčno pozicijo, kot je prikazano na sliki 7.16. Koeficienti polinoma ax in ay ( i = 0 , 1 , 2) so ocenjeni i i z metodo najmanjših kvadratov. 7.5. Primeri uporabe večagentnih sistemov 399 Slika 7.16: Sledilno vozilo mora slediti predhodno vozilo na razdalji DL vzdolž trajektorije. Oblika referenčne trajektorije v okolici trenutne referenčne lege xref ( t 0), yref ( t 0) je ocenjeno s pomočjo šestih okoliških točk okrog referenčne točke. Referenčna pozicija za sledilno vozilo ob trenutnem času t 0 je ocenjena z       xref ( t 0) ˆ xL( T ) axT 2 + axT + ax 2 1 0  y      ref ( t 0) = ˆ yL( T ) = ayT 2 + ayT + ay (7.3)      2 1 0  ϕref ( t 0) ˆ ϕL( T ) atan2 (2 ayT + ay, 2 axT + ax) 2 1 2 1 kjer je atan2 ( y, x) štirikvadrantna razširitev funkcije arctan y . x Vodenje vozil v linearni formaciji Vsako sledilno vozilo mora oceniti in slediti referenčno trajektorijo (7.3) z uporabo nelinearnega regulatorja predstavljenega v poglavju 3.3.6 kot sledi vfb = vref cos eϕ + kxex sin eϕ ωfb = ωref + kyvref ey + kϕeϕ eϕ kjer so predkrmilni signali (glejte poglavje 3.3.2) določeni iz ocenjene referenčne trajektorije (7.3) kot q q vref ( t 0) = ˙ x 2 + ˙ y 2 = (2 axT + ax)2 + (2 ayT + ay)2 ref ref 2 1 2 1 in ˙ xref ( t)¨ yref ( t) − ˙ yref ( t)¨ xref ( t) (2 axT + ax)2 ay − (2 ayT + ay)2 ax ω 2 1 2 2 1 2 ref ( t 0) = = . ˙ x 2 ( t) + ˙ y 2 ( t) (2 axT + ax)2 + (2 ayT + ay)2 ref ref 2 1 2 1 400 Agenti in večagentni sistemi Sledilni pogrešek pa je izračunan upoštevajoč dejansko lego sledilnega vozila ( x( t 0) , y( t 0) , ϕ( t 0)) in referenčne lege ( xref ( t 0) , yref ( t 0) , ϕref ( t 0)) z enačbo (3.28). 7.5.3 Avtomatsko vodeni vozički Večagentni sistem so tudi avtomatsko vodeni vozički (AGV, angl. automated guided vehicle), ki jih dandanes srečamo v mnogih modernih industrijskih halah. Gre za floto mobilnih vozil, ki se avtonomno gibljejo in opravljajo naloge, ki so jim dodeljene. Pogosto se uporabljajo za razvoz materiala in/ali izdelkov. V ta namen imajo AGV-ji lahko vgrajen tovorni prostor, pogosteje pa imajo le posebne mehanizme (običajno preproste, lahko pa tudi dvižne vilice ali celo robotske roke), ki omogočajo prijemanje in odlaganje tovora. To lahko storijo tako, da se pripeljejo pred/pod pasivni mobilni voziček, ga zapnejo in nato vlečejo. Ali tako, da pridejo do standardiziranega zabojnika (npr. paleta), ki ga dvignejo, prepeljejo in nato odložijo. Lahko pa so mehanizmi ali robotske roke za natovarjanje/raztovarjanje kar na postajah, med katerimi tovor razvažajo AGV-ji. Gibanje AGV-jev med postajami običajno ni povsem prosto, temveč so postaje med seboj povezane z omrežjem označenih prog, po katerih se AGV- ji lahko gibljejo. Okolje je torej prirejeno za avtomatsko delovanje AGV-jev tako, da so v okolju na primeren način označene proge. Pogosto se v ta namen uporabljajo magnetni trakovi in RFID-značke, ki so vgrajeni v podlago. Lahko se uporabljajo tudi vidne oznake (npr. kontrastne/barvne črte, QR-kode). AGV lahko tako s primernimi senzorji sledi talnim oznakam in tudi določa svoj položaj v omrežju prog na podlagi unikatnih značk, ki se najajajo ob progah. Lahko pa so proge definirane tudi virtualno, če imamo na voljo globalni ali lokalni sistem, ki omogoča lokalizacijo AGV-ja v okolju. AGV-ji imajo vgrajene še dodatne senzorje bližine, ki jim omogočajo varno delovanje in zaustavitev v primeru ovir na poti. Če je v okolju prisoten tudi človek, potem se pogosto zahteva, da je AGV opremljen z varnostnim laserskih merilnikom razdalj. V nadaljevanju je predstavljen fizični model pomanjšane industrijske hale (slika 7.17) z omrežjem poti po katerih se vozijo miniaturni avtomatsko vodeni vo-zički. Dimenzije poligona so 2 , 2 m × 1 , 8 m, dimenzije miniaturnega AGV-ja pa 0 , 1 m × 0 , 2 m × 0 , 06 m. Sistem je bil izdelan za raziskave, razvoj in preizkušanje algoritmov, ki omogočajo avtonomno delovanje AGV-jev, ter za pedagoške namene. Pri izdelavi miniaturnih AGV-jev nismo zahtevali natančne preslikave dejanske situacije iz industrijskega okolja, saj vseh sistemov (npr. laserskega merilnika razdalj, pogonskega mehanizma) ni moč primerno pomanjšati. S fizičnim modelom tako posnemamo le tiste lastnosti avtomatsko vodenih vozičkov, ki so potrebne za učenje in razvoj algoritmov za avtonomno vožnjo. Celoten sistem smo zasnovali tako, da je možna izvedba in študija različnih algoritmov za avtonomno delovanje mobilnih vozičkov: odometrija, vodenje po poti, načrtovanje poti med poljubnimi cilji, iskanje alternativnih poti in obvozov v primeru zastojev, lokalizacija v znanem zemljevidu okolja, večagentno vodenje in podobno. Zaradi 7.5. Primeri uporabe večagentnih sistemov 401 Referenčna značka RFID-značka (pod podlago) Sledilna črta AGV Slika 7.17: Fizični model industrijske hale z AGV-ji boljše povezljivost med različnimi sistemi in modularnosti smo se odločili, da bomo uporabili okolje ROS (angl. Robot operating system). Globalni sistem za merjenje lege s strojnim vidom Nad poligonom z miniaturnimi AGV-ji se nahaja kamera, ki omogoča sledenje vseh objektov, ki se gibljejo v ravnini poligona. Lega kamere glede na poligon je poljubna, dokler so vsi objekti vidni v njenem vidnem polju. Predpostavimo, da je lega kamere glede na globalni koordinatni sistem (glede na poligon) podana z rotacijsko matriko R = [r1 , r2 , r3] in translacijskih vektorjem t. Če uporabimo model kamere z luknjico, je povezava med homogeno točko p T = [ x W W , yW , 1] v poljubni globalni ravnini in homogeno točko na sliki p T = [ x P P , yP , 1] podana z h i p P ∝ S r1 r2 t p W = Hp W (7.4) Matrika S v (7.4) vsebuje notranje parametre kamere (glejte poglavje 5.2.4). Matriko S lahko določimo s postopkom kalibracije kamere, npr. z uporabo dobro znanega pristopa s šahovnico [6]. Preslikava H v (7.4) je znana kot homografija — predstavlja preslikavo med ravninama. Homografijo H lahko ocenimo na podlagi vsaj štirih parov točk v slikovni in globalni ravnini. Zato vsebuje poligon štiri referenčne značke (glejte sliko 7.17) — lokacija teh značk glede na globalni koordinatni sistem je znana. Vsak AGV (ali drug objekt, katerega lego želimo meriti) mora tudi biti opremljen z unikatno značko, ki ni nujno, da se nahaja v ravnini tal. Med gibanjem AGV-ja po ravni podlagi se značka na njem giblje po virtualni ravnini, ki je vzporedna z ravnino tal. Situacija je prikazana na sliki 7.18. zatorej lege AGV-ja ne moremo oceniti neposredno iz znane homografije H. Ker predpostavljamo, da je višina 402 Agenti in večagentni sistemi Kamera H H h( h) Virtualna ravnina ID3 Značka ID2 ID11 Objekt h ID4 ID1 Referenčna ravnina Referenčna značka Slika 7.18: Sistem za merjenje lege s kamero h, na kateri je nameščena značka glede na podlago, znana, je možno določiti homografijo H h( h) za to vzporedno ravnino iz znane homografije H. Slika položaja p P značke, ki se nahaja v vzporedni ravnini glede na referenčno ravnino (tla) na višini h, lahko preslikamo (z ortogonalno projekcijo) v točko p W v ravnini tal: p W ∝ H−1( h)p h P Da lahko ocenimo homografijo H h( h), moramo poznati notranje parametre kamere S h i H h( h) = S q1 q2 (q1 × q2) h + q (7.5) n 3 kjer lahko koeficient normiranja n določimo kot n = (kq1k + kq2k) / 2 (7.6) Vektorji q1 do q3 v enačbah (7.5) in (7.6) so h i S−1H = q1 q2 q3 Če uporabljene značke niso invariantne na rotacijo, lahko določimo tudi usmer- jenost značke in torej celotno lego (položaj in orientacijo) označenega objekta v globalni ravnini. Predstavljen pristop merjenja lege objektov na ravnini je mogoč za poljubno postavitev kamere, dokler so vsi objekti vidni in v vidnem polju kamere, a točnost in natančnost meritev lahko variira. Sledenje objektov je mogoče izvesti tudi pri premikajoči se kameri, če pri tem ne pride do okluzij nobenih značk. Sicer pa lahko homografijo H ocenimo le kadar so vidne vse oz. vsaj štiri referenčne značke. V kolikor se AGV-ji gibljejo le po ravni podlagi poligona, omogoča predstavljen pristop ocenjevanja leg zelo točne meritve, saj je omejitev gibanja na ravnino upoštevana implicitno. Če je kamera kalibrirana, lahko globino značk ocenimo tudi na podlagi znane velikosti značk. V našem 7.5. Primeri uporabe večagentnih sistemov 403 primeru je globina ocenjena implicitno glede na štiri referenčne značke na ravnini tal. Te značke niso kolinearne in so relativno daleč narazen, kar omogoča, da je ocena homografije H zelo točna. V primeru močne popačitve slike, zaradi distorzij leč, lahko le-te opišemo z nelinearnim modelom p P = f (p∗ ) (7.7) P kjer p∗ predstavlja točko na popačeni sliki in p P P je točka na popravljeni sliki, kjer je popačitev odpravljena. Model distorzije (7.7) lahko ocenimo tekom postopka kalibracije kamere (glejte npr. [7]). Za detekcijo in sledenje unikatnih značk (s posebnimi črno-belimi vzorci) na sliki smo uporabili knjižnico ArUco [8, 9], ki je računsko učinkovita in robustna, torej je primerna za delovanje v realnem času, tudi ob spremenljivi osvetlitvi. Celoten sistem za sledenje več objektov je računsko precej učinkovit in ga je mogoče izvesti na nizkocenovni strojni opremi. V našem primeru smo izvedli celoten sistem na vgradnem računalniku Raspberry Pi 3 B+ s kamero Raspberry Pi Camera Board v2, ki se nahaja približno 2 , 5 m nad poligonom. V naši izvedbi smo dosegli merjenje lege več objektov v globalnem koordinatnem sistemu s frekvenco 15 Hz. Pri mirujočih objektih smo dosegli standardno deviacijo 0 , 0001 m pri merjenju položaja in 0 , 003 rad pri merjenju orientacije (širina značk je 0 , 1 m). Vse meritve se sproti objavljajo v omrežje ROS in so tako na voljo vsem AGV-jem in ostalim sistemom. Sistem je modularen in precej enostaven za uporabo. Ko imamo na voljo notranje parametre kamere (le-te lahko določimo s postopkom kalibracije, npr. s šahovnico), moramo vnesti le še lokacije štirih referenčnih značk na ravnini tal in izmeriti višine vseh značk na objektih nad ravnino tal. Miniaturni avtomatsko vodeni vozički Na sliki 7.19 je prikazana zgradba miniaturnega avtomatsko vodenega vozička z glavnimi sestavnimi deli. Prednji pogonski del predstavlja voziček z diferencialnim pogonom, ki je na šasijo pritrjen pasivno preko ležaja. Takšna oblika mehanskega mehanizma omogoča enostavno izvedbo, pri tem pa še vedno dosežemo želeni kinematični model trikolesnika s prednjim pogonom. Miniaturni AGV je opremiljen z več senzorji, ki omogočajo implementacijo algoritmov za avtonomno vožnjo. Motorja na prednjem vozičku, ki ženeta obe kolesi, sta opremljena z inkrementalnim enkoderjem. Poleg tega imamo še absolutni enkoder, ki omogoča merjenje kota prednjega vozička glede na šasijo. Na dnu prednjega vozička se nahaja namensko tiskano vezje z mikrokrmilnikom, ki skrbi za nizkonivojsko obdelavo signalov v realnem času in regulacijo hitrosti vrtenja motorjev. Na tiskanem vezju se nahaja tudi sedem segmentni linijski optični detektor črte. Zadnji kolesi sta pasivni in v trenutni izvedbi nista opremljeni z enkoderji. Na spodnjem delu šasije se nahaja še RFID-bralnik. Za obdelavo informacij s senzorjev, komunikacijo z zunanjimi sistemi in izvedbo regulacijskih algoritmov za avtonomno vožnjo se znotraj šasije nahaja računalnik 404 Agenti in večagentni sistemi Drsni obroč Ležaj Šasija Baterija Absolutni enkoder Zadnje levo kolo Računalnik Prednje desno kolo RFID-bralnik Prednje levo kolo Desni motor Prednji voziček z enkoderjem Optični detektor črte Slika 7.19: Zgradba miniaturnega AGV-ja v prerezu Raspberry Pi Zero W. V šasiji se nahaja še baterija. Na vrhu šasije se nahaja posebna unikatna značka s črnobelim vzorcem, ki omogoča, da določimo lego AGV-ja s sistemom za globalno asas merjenje lege s strojnim vidom (glejte poglavje 7.5.3). vs L γ Y vL ωα α ω v vR ϕ y D X x Slika 7.20: Štirikolesni robot s kinematičnim modelom bicikla Štirikolesni robot na sliki 7.19 ima enake kinematične omejitve kot bicikel (slika 7.20). Omejitve so posledica dejstva, da se kolesa (brez spodrsavanja) ne morejo gibati v smeri osi rotacije ˙ x sin ϕ − ˙ y cos ϕ = 0 (7.8) ˙ x sin α − ˙ y cos α − D ˙ ϕ cos γ = 0 7.5. Primeri uporabe večagentnih sistemov 405 V enačbi (7.8) je D razdalja med osjo zadnjih koles in centrom rotacije vozička, na katerega sta pritrjeni prednji kolesi. Vpeljimo posplošeni vektor stanj q T = [ x, y, ϕ, γ]. Kinematične omejitve lahko tako zapišemo v vrstice t. i. omejitvene matrike A(q): " # sin ϕ − cos ϕ 0 0 A(q) ˙ q = ˙ q = 0 (7.9) sin α − cos α − D cos γ 0 Kinematični model s hitrostnim vhodom u je ˙ q = Su (7.10) kjer je matrika S jedro (ničelni prostor) omejitvene matrike A(q), torej zadošča enačbi A(q)S = 0. Za hitrosti vhodnega vektorja u T = [ u 1 , u 2] lahko izberemo različne veličine: pri prednjem pogonu je u 1 = vs, pri zadnjem pogonu pa je u 1 = vs; kotna hitrost prednjega vozička je lahko podana glede na globalni ( u 2 = ωα = ˙ α) ali lokalni ( u 2 = ωγ = ˙ γ) koordinatni sistem. Različni zapisi kinematičnega modela, ki jih pri tem dobimo, so zbrani v tabeli 7.1. Vsi ti modeli zadostujejo omejitvam, ki so podane v (7.9). Ker so si kinematičnimi modeli med seboj precej podobni, moramo biti pri uporabi pozorni, da ne pride do zmede in napačne uporabe. Tabela 7.1: Kinematični modeli bicikla ˙ q = Su glede na hitrostne vhode (q T = [ x, y, ϕ, γ]) Globalna kotna hitrost ωα Lokalna kotna hitrost ωγ v s cos γ cos ϕ 0 cos γ cos ϕ 0 " # " # ogon  cos γ sin ϕ 0 v  cos γ sin ϕ 0 v p ˙ q s s =   ˙ q =    − sin γ 0 ω  − sin γ 0 ω  D  α  D  γ sin γ 1 0 1 D Prednji v  cos ϕ 0  cos ϕ 0 " # " # ogon  sin ϕ 0 v  sin ϕ 0 v p ˙ q =   ˙ q =   − tan γ 0 ω − tan γ 0 ω  D  α  D  γ tan γ 1 0 1 D Zadnji V našem primeru je vhodni vektor u T = [ vs, ωα] in je kinematični model (7.10) torej (model v prvi vrstici in prvem stolpcu v tabeli 7.1) cos γ cos ϕ 0 " #  cos γ sin ϕ 0 v ˙ q = Su = s    sin γ 0 ω  D  α − sin γ 1 D Linearna hitrost vs in kotna hitrost ωα = ˙ α sta neposredno povezani s hitrostjo 406 Agenti in večagentni sistemi levega kolesa vL in hitrostjo desnega kolesa vR Lωα Lωα vL = vs − vR = vs + (7.11) 2 2 kjer je L razdalja med prednjima kolesoma. Omrežje križišč in prog Oglejmo si primer poligona, ki je prikazan na sliki 7.21. Poligon je sestavljen iz omrežja usmerjenih prog, ki se v križiščih razdružijo v več prog ali pa se združijo v eno progo. AGV se lahko vozi le po progah in le v označenih smereh. Izjema je osrednji del poligona, kjer imamo področje brez prog. V tem področju je dovoljeno poljubno gibanje, za kar mora biti robot opremljen s primernimi sistemi za lokalizacijo. V nadaljevanju se bomo posvetili predvsem obravnavi navigacije v delu prostora s progami. 1 2 3 4 5 6 7 8 9 41 10 15 12 13 14 16 11 17 19 18 21 24 25 20 22 23 26 27 28 29 30 39 31 32 40 33 34 35 36 37 38 Slika 7.21: Omrežje oštevilčenih križišč in prog Na sliki 7.21 so vsa križišča oštevilčena. Celoten zemljevid lahko predstavimo z grafom, kjer vozlišča predstavljajo križišča (stik dveh ali več prog) in konce prog. Povezave med vozlišči pa predstavljajo proge, ki povezujejo vozlišča (križišča ali konci prog). Celoten zemljevid prog lahko tako predstavimo v obliki grafa, ki je prikazan na sliki 7.22. Iz grafa so razvidne le povezave med sosednjimi vozlišči, ni pa več vidna oblika poti, ki vodi med križišči. Predstavitev zemljevida z grafom 7.5. Primeri uporabe večagentnih sistemov 407 se izkaže za koristno, saj nam omogoča uporabo različnih algoritmov iz teorije grafov. Uporabimo lahko npr. Dijkstrov algoritem za iskanje optimalne poti — najkrajšo pot dobimo, če povezave med vozlišči utežimo z razdaljami prog. 39 40 32 31 29 37 36 41 28 38 27 35 22 16 25 23 5 20 4 17 21 34 6 11 30 3 19 18 7 33 24 2 12 15 13 9 1 8 14 26 10 Slika 7.22: Graf križišč in prog Za prostorsko predstavitev zemljevida moramo poznati položaje vseh križišč in koncev prog (vozlišč) ter obliko vseh prog, ki so v grafu predstavljene s povezavami med vozlišči. Eden izmed možnih načinov zapisa posamezne proge je s pomočjo parametričnih krivulj oz. zlepkov parametričnih krivulj. Na sliki 7.23 so prikazane tri bazične krivulje, ki jih lahko uporabimo za gradnjo zlepka: daljica (slika 7.23a), krožni lok (slika 7.23b) in Bézierjeva krivulja (slika 7.23c). Enačbe in parametri teh krivulj so podani v tabeli 7.2. Uporabimo lahko tudi kakšne druge parametrične krivulje, a že z Bézierjevimi krivuljami lahko aproksimiramo skoraj poljubno krivuljo. Ker pri Bézierjevih krivuljah visokega reda v praksi naletimo na numerične težave, se raje poslužujemo uporabe zlepkov Bézierjevih krivulj nižjega reda. Bézierjeva krivulja prvega reda ( n = 1) je enaka daljici, kar je razvidno tudi iz enačb v tabeli 7.2. Nikakor pa z Bézierjevo krivuljo (neglede na red) ne moremo natančno opisati krožnega loka — z Bézierjevo krivuljo ali zlepkom Bézierjevih krivulj lahko krožni lok le aproksimiramo s poljubno natančnostjo. Torej bi celoten zemljevid prog lahko opisali tudi le z zlepki Bézierjevih krivulj. Pomembna lastnost krivulj je tudi njihova dolžina. Za ravninsko krivuljo, ki je podana s p( λ) = [ x( λ) , y( λ)] in parametrom λ ∈ [0 , 1], izračunamo njeno dolžino z integralom 1 s Z d x( λ) 2 d y( λ) 2 D = + d λ d λ d λ 0 408 Agenti in večagentni sistemi p p 2 B p( λ) p3 p( λ) r p( λ) r β y α p A p p1 C x p0 (a) (b) (c) Slika 7.23: Množica osnovnih krivulj za opis poti: (a) daljica, (b) krožni lok in (c) Bézierjeva krivulja tretjega reda Tabela 7.2: Množica osnovnih krivulj podanih v parametrični obliki za opis poti Krivulja Enačba in parametri Omejitve Daljica p( λ) = (1 − λ)p A + λ p B λ ∈ [0 , 1] začetna točka p A = [ xA, yA] končna točka p B = [ xB, yB] Krožni lok p( λ) = p C + r[cos( α + λβ) , sin( α + λβ)] λ ∈ [0 , 1] center p C = [ xC , yC ] radij r r ≥ 0 začetni kot α ločni kot β | β| < 2 π Bézierjeva krivulja p( λ) = P n n(1 − λ) n− iλi p i=0 i i λ ∈ [0 , 1] kontrolna točka p i = [ xi, yi] i = 0 , 1 , . . . , n 7.5. Primeri uporabe večagentnih sistemov 409 Definiramo lahko tudi dolžino po krivulji od začetne točke krivulje do točke p( λ) λ s Z d x( ξ) 2 d y( ξ) 2 d( λ) = + d ξ (7.12) d ξ d ξ 0 Torej je celotna dolžina krivulje D = d(1). Dolžina daljice je enaka evklidski razdalji d( λ) = λ kp B − p A k2, dolžina krožnega loka pa je d( λ) = λβr. Medtem ko sta dolžini daljice in krožnega loka lahko določljivi, v splošnem ne obstaja analitična rešitev integrala (7.12) za izračun dolžine splošne Bézierjeve krivulje, razen za krivulje do tretjega reda. Dolžino krivulje v tem primeru zato poiščemo s pomočjo primerne numerične metode. Velja omeniti, da je parameter λ v primeru daljice in krožnega loka proporcionalen dolžini krivulje od začetne točke do točke p( λ). To pa v splošnem ne velja za Bézierjeve krivulje, kjer se še vedno z monotonim večanjem parametra λ monotono veča oddaljenost (po krivulji) točke p( λ) od začetne točke, a ne linearno. Z zlepkom, ki je sestavljen iz bazičnih krivulj, lahko aproksimiramo poljubno krivuljo. Glede na zahteve v stičnih točkah poznamo različne načine tvorjenja zlepka, ki je v stičnih točkah lahko le zvezen, lahko pa ima tudi zvezne prve in/ali višje odvode. Tako lahko dosežemo želeno gladkost zlepka v stičnih točkah. Začetna oblika vsake (razen prve) krivulje v zlepku je torej pogojena z obliko predhodne krivulje v zlepku. Zveznost zlepka, ki je sestavljen iz krivulj p i−1( λ) in p i( λ), v točki p i(0) dosežemo z p i(0) = p i−1(1) Zahtevamo lahko tudi zveznost prvega odvoda koordinat obeh krivulj v stični točki dp i(0) dp i−1(1) = d λ d λ in/ali tudi n-tega odvoda d n p i(0) d n p i−1(1) = d λn d λn Zahtevamo lahko tudi zveznost katere druge veličine, kot je usmerjenost ali ukrivljenost krivulje v stični točki. Na sliki 7.24 je narisana usmerjena pot, ki vodi od vozlišča 11 preko vozlišča 12 do vozlišča 8 (glejte sliko 7.21). Pot lahko opišemo z naslednjim zlepkom bazičnih krivulj (koordinate in razdalje so podane v milimetrih): 1. krožnim lokom s parametri p C = [1400 , 1200], r = 75, α = 180° in β = −90°; 2. daljico s parametroma p A = [1400 , 1275] in p B = [1550 , 1275] ter 3. Bézierjevo krivuljo s parametri n = 3, p0 = [1550 , 1275], p1 = [1775 , 1275], p2 = [1775 , 1575] in p3 = [2000 , 1575]. 410 Agenti in večagentni sistemi (2000 , 1575) (1775 , 1575) y 8 x (1400 , 1275) 12 (1775 , 1275) (1550 , 1275) 11 (1400 , 1200) (1325 , 1200) Slika 7.24: Pot od vozlišča 11 preko vozlišča 12 do vozlišča 8 (koordinate so v milimetrih) Vse krivulje so podane tako, da parameter λ vedno narašča v smeri usmerjenosti poti. V danem primeru gre za zvezen zlepek krivulj, ki ima tudi zvezno ukrivljenost v stičnih točkah, kar je za namen vodenja robota po črti smiselno in zaželeno, saj ne želimo, da bi kjerkoli prišlo do prehitre ali celo hipne spremembre usmerjenosti poti. Krivulje lahko združimo tudi kako drugače — npr. tako da so krivulje v stičnih točkah le zvezne, brez zveznih odvodov, kot je to med vozliščema 11 in 6 na sliki 7.21. Z omenjenimi bazičnimi krivuljami lahko opišemo celoten zemljevid na sliki 7.21. Proga med vozliščema 12 in 8 je podana z eno Bézierjevo krivuljo, medtem ko je proga med vozliščema 11 in 12 podana z zlepkom krožnega loka in daljice. Vsako progo lahko torej opišemo z urejeno množico krivulj proga = ( krivulja 1 , krivulja 2 , . . . , krivuljaK ) Posamezne proge, ki se stikajo, lahko združimo tako, da tvorimo pot, ki jo zopet lahko zapišemo v obliki urejenega seznama pot = ( proga 1 , proga 2 , . . . , progaL) Tako lahko opišemo pot oz. poti med poljubnima vozliščema (križiščema) v zemljevidu. Če želimo pot, ki je opisana z urejenim seznamom osnovnih parametričnih krivulj, razdeliti na dva ali več delov, moramo znati razdeliti vsako izmed osnovnih krivulj na poljubnem mestu. Razdelitev daljice na mestu λ na daljici p( λ) (med točkama p A in p( λ)) in p( λ) (med točkama p( λ) in p B) je enostavna (slika 7.25a): p( λ) = (1 − λ)p A + λ p( λ) ; λ ∈ [0 , 1] p( λ) = (1 − λ)p( λ) + λ p B ; λ ∈ [0 , 1] 7.5. Primeri uporabe večagentnih sistemov 411 p( λ) p B p( λ) p2 p( λ) p1 p p( λ) (1 3 − λ) β p( λ) p( λ) p( λ) r p( λ) r λβ p y α p( λ) 2 p A p p C x p 1 0 (a) (b) (c) Slika 7.25: Množica osnovnih krivulj za opis poti: (a) daljica, (b) krožni lok in (c) Bézierjeva krivulja tretjega reda Podobno lahko enostavno razdelimo tudi krožni lok (slika 7.25b): " # T cos( α + λλβ) p( λ) = p C + r ; λ ∈ [0 , 1] sin( α + λλβ) " # T cos( α + λβ + λ(1 − λ) β) p( λ) = p C + r ; λ ∈ [0 , 1] sin( α + λβ + λ(1 − λ) β) Tudi za Bézierjeve krivulje velja, da lahko krivuljo na poljubnem mestu razdelimo na dve krivulji, ki sta zopet Bézierjevi krivulji enakega reda kot osnovna krivulja. Kontrolne točke, ki definirajo novi Bézierjevi krivulji, lahko določimo z de Casteljaujevim algoritmom [10] (slika 7.25c). V vseh primerih velja, da je delitvena točka p( λ) na krivulji (daljica/krožni lok/Bézierjeva krivulja) enaka končni točki prve krivulje in začetni točki druge krivulje: p( λ) = p(1) = p(0). To nam omogoča, da zemljevid razširimo z novimi križišči in progami, pri čemer moramo spremeniti le zapis prog, ki se stikajo v novih križiščih. Omrežje postaj Pod progami se vgrajene RFID-značke z unikatnimi oznakami, ki jih AGV lahko zazna z RFID-bralnikom. Te RFID-značke omogočajo, da AGV ugotovi na kateri progi in kje na progi se nahaja. Če se RFID-značka nahaja pred križiščem, je to primeren trenutek, da se AGV odloči katero smer bo izbral v križišču. Ta izbira je lahko določena fiksno glede na ID značke — npr. v križišču, ki sledi znački z ID-jem 1, pojdi vedno levo. Lahko pa se izbira smeri v križišču določa tudi dinamično glede na pot, ki jo mora AGV opraviti — da gre AGV po želeni poti, mora v križiščih izbrati primerne smeri. Ker pred vsemi križišči ni RFID-značk, mora AGV svojo lego ocenjevati s postopkom lokalizacije (npr. z uporabo RFID-značk in na podlagi odometrije). Za namen planiranja poti in vodenja zato graf križišč in prog predstavimo v nekoliko spremenjeni obliki. Mesta na progah, kjer se nahajajo RFID-značke, obravnavamo kot postaje, kjer se AGV lahko ustavi oz. se odloči o svoji naslednji akciji — običajno izbiramo le med sledenjem levemu ali desnemu robu črte. Proge, ki ne vsebujejo RFID-značk, 412 Agenti in večagentni sistemi 1 3 4 101 102 5 103 104 2 105 106 107 110 111 6 142 143 108 109 112 113 114 115 116 7 117 118 119 8 9 10 11 12 13 120 121 15 122 14 123 126 127 125 124 16 128 129 130 131 17 132 18 133 135 136 134 137 138 139 140 19 141 20 Slika 7.26: Zemljevid z označenimi progami in položaji RFID-značk (sinje modre oznake) ter virtualnih značk (rumene oznake) 7.5. Primeri uporabe večagentnih sistemov 413 opremimo s t. i. virtualnimi značkami (njihov položaj lahko določimo le na podlagi odometrije/lokalizacije), ki jih tudi obravnavamo kot postaje. Vsaka proga tako vsebuje vsaj eno postajo, ki je označena bodisi z RFID-značko bodisi z virtualno značko. Na sliki 7.26 so na zemljevidu prog označene vse postaje. Postaje z ID-jem, ki je manjši ali enak 100, so označene z RFID-značkami, ostale postaje pa so označene z virtualnimi značkami. Vsaka RFID-značka je na sliki 7.26 označena z dvema točkama: krogec predstavlja dejanski položaj RFID-značke, križec pa lokacijo prednjega vozička AGV-ja, ko le-ta značko zazna. To je posledica dejstva, da je RFID-bralnik izmaknjen glede na točko na AGV-ju, ki potuje po črti. 10 459 126 600 288 131 259 111 107 519 201 162 486 9 614 11 409 116 880 985 129 108 104 1910 115 1 586 154 206 219 207 1998 288 2 114 139 341 109 110 225 216 352221 135 339 337 162 128 119 309 103 3 374 7 267 112 275 106 437 202 145 373 118 339 140 120 314 12 298 260 273 121 4 5 319 340 261 415 300 375 264 117 8 113 253 197 279 6 101 299 122 127 360 19 102 446 433 14 183 497 462 219 179 302 136 357 124 16 133 162 605 141 13 308 594 459 15 137 232 20 105 1225 247 294 153 539 17 142 138 157 289 549 123 487 284 134 18 190 472 143 245 125 419 1103 130 132 Slika 7.27: Graf postaj in prog Graf, ki predstavlja omrežje prog, lahko preslikamo v nov graf, ki predstavlja omrežje postaj. V novem grafu na sliki 7.27 vozlišča predstavljajo vse postaje, ki so povezane z usmerjenimi povezavami, ki predstavljajo poti med postajami. Uteži poleg povezav v grafu na sliki 7.27 predstavljajo razdalje (v milimetrih) med postajami vzdolž prog. Barva usmerjene povezave iz vozlišča določa, kako pridemo do sosednjega vozlišča: modra pomeni, da moramo v križišču izbrati levo smer; rdeča pomeni, da moramo izbrati desno smer; zelena predstavlja poseben način delovanja. Da v križišči izberemo levo ali desno smer, moramo sledili ali levemu ali desnemu robu črte, s katero je označena proga. V tej predstavitvi zemljevida se določeni deli prog med postajami torej prekrivajo. Z upoštevanjem 414 Agenti in večagentni sistemi pravil za delitev krivulj, ki sestavljajo proge, lahko originalni graf z omrežjem križišč avtomatsko pretvorimo v nov graf z omrežjem postaj. Na sliki 7.28 so predstavljene proge, ki povezujejo postaje. 1 3 4 101 102 5 103 104 2 105 106 107 110 111 6 142 143 108 109 112 113 114 115 116 7 117 118 119 8 9 10 11 12 13 120 121 15 122 14 123 126 127 125 124 16 128 129 130 131 17 132 18 133 135 136 134 137 138 139 140 19 141 20 Slika 7.28: Zemljevid z označenimi potmi in položaji RFID-značk (sinje modre oznake) ter virtualnih značk (rumene oznake) Vodenje po poti Na podlagi grafa postaj (slika 7.27) lahko torej s pomočjo algoritmov iskanja optimalne poti v grafu (poglavje 4.4) poiščemo pot med poljubnima postajama. Optimalno pot lahko opišemo kot urejen seznam postaj, ki jih moramo obiskati, če želimo priti od začetne do končne postaje. Ta urejeni seznam postaj pa lahko pretvorimo v urejeni seznam akcij, ki jih mora AGV izvesti, da se pelje po želeni poti. Vsako akcijo lahko opišemo s tremi parametri: 1. tip akcije: levo, če mora AGV slediti levemu robu črte, desno, če mora AGV slediti desnemu robu črte ali posebno za vse ostale primere; 2. ID naslednje postaje; 3. razdalja do naslednje postaje vzdolž proge. Na podlagi tako zapisanih akcij lahko izvedemo vodenje AGV-ja. AGV se torej avtomatsko pelje po poti tako, da sledi črtam, ki označujejo proge, in izbira 7.5. Primeri uporabe večagentnih sistemov 415 primerne smeri v križiščih. Tip akcije vpliva na izbiro regulatorja, ki ga uporabimo za vodenje AGV-ja. V večini primerov imamo v križiščih le dve možnosti. AGV lahko tako gre v želeno smer, če skozi križišče sledi levemu ali desnemu robu črte. Če je (pri trenutni akciji) ID naslednje postaje manjši ali enak 100, to pomeni, da je naslednja postaja označena z RFID-značko. V tem primeru izvajamo trenutno akcijo vse dokler z RFID-bralnikom ne zaznamo ID-ja naslednje postaje. Če je ID postaje večji od 100, potem izvajamo trenutno akcijo toliko časa, dokler AGV vzdolž proge ne prepotuje razdalje do naslednje postaja — ta podatek je vsebovan v akciji. Prepotovano razdaljo lahko ocenjujemo s postopkom odometrije, saj je AGV opremljen z enkoderji, ali globalne lokalizacije. AGV ima na prednjem vozičku nameščen linijski senzor za zaznavanje črte (slika 7.29). V našem primeru gre za sedem segmentni optični senzor, ki oddaja svetlobo v infrardečem spektru in zaznava količino odbite svetlobe od podlage. Vsak posamezni segment senzorja na beli podlagi vrne nizko vrednost in pri črni podlagi visoko vrednost. Če se senzor nahaja nad črto, lahko na podlagi zaporedja vrednosti senzorja določimo levi in desni rob črte — iščemo prehod iz nizkih k visokim vrednostim oz. obratno. Tako lahko v koordinatnem sistemu senzorja podamo položaj levega roba xL in položaj desnega roba xR. Koordinatni sistem smo v našem primeru definirali tako, da je koordinatno izhodišče na sredini senzorja, skrajna robova senzorja pa sta od izhodišča oddaljena za vrednost ena. xR xL −1 0 1 vs ωs vs + Lωα vL = vs − Lωα 2 = vR 2 Slika 7.29: Sledenje črti na podlagi linijskega senzorja črte (pogled od spodaj) Glede na želen položaj roba črte na senzorju x 0, lahko definiramo pogrešek: e( t) = x 0 − xL( t) za sledenje levemu robu in e( t) = x 0 − xR( t) za sledenje desnemu robu. Če bi želeli slediti sredini črte, bi lahko pogrešek definirali tudi kot e( t) = xL( t) + xR( t). Za sledenje levemu ali desnemu robu črte lahko nato zasnujemo preprost regulator za kotno hitrost prednjega vozička ωα( t) = Kωe( t) kjer je Kω ojačenje regulatorja. Želen položaj roba črte x 0 nastavimo različno za 416 Agenti in večagentni sistemi sledenje levemu in desnemu robu, tako da pri preklopu med načinom regulacije ne pride do udara regulirne veličine — razlika med želenima vrednostima levega in desnega roba bo tako ravno enaka širini črte. Maksimalna vrednost pogreška je v našem primeru enaka dva (| e( t)| ≤ 2). Linearno hitrost lahko nastavimo kar na konstantno vrednost ( vs( t) = v 0 = konst.), lahko pa jo tudi moduliramo glede na pogrešek (npr. vs( t) = v 0 cos πe( t) ), s čimer lahko dosežemo bolj robustno 4 sledenje črti v ovinkih. V kolikor s senzorjem ne moremo zaznati roba črte, ki mu sledimo, regulacijo prekinemo in robota ustavimo. Linearno hitrost vs( t) in kotno hitrost ωα( t) na podlagi enačbe (7.11) pretvorimo v hitrosti levega in desnega kolesa, vL( t) in vR( t). Pri izvedbi regulacije moramo upoštevati še, da sta hitrosti obeh koles omejeni vMIN ≤ | vL( t)| ≤ vMAX vMIN ≤ | vR( t)| ≤ vMAX (7.13) sicer se lahko zgodi, da se prednji voziček bodisi ne bo obračal bodisi se bo le obračal na mestu. To lahko rešimo tako, da primerno omejimo tudi hitrosti vs( t) in ωα( t). Lahko pa poskusimo ohraniti razmerje ωα( t) , ki predstavlja ukrivljenost, vs( t) pred in po upoštevanju omejitve (7.13). Kot že omenjeno, za regulacijo hitrosti vrtenja obeh motorjev oz. koles skrbi mikrokrmilnk na prednjem vozičku. Dodatni virtualni senzorji Zaradi majhnosti miniaturnega AGV-ja, smo omejeni z naborom senzorjev, ki jih lahko uporabimo, saj določenih senzorjev, ki se običajno uporabljajo na AGV-jih, ni na voljo v tako majhni izvedbi. Takšen primer je laserski merilnik razdalj (LMR), ki je pri sodelujočih AGV-jih običajno obvezen kot varnostni element. AGV-ji so lahko opremljeni celo z več LMR-ji, ki omogočajo pokrivanje čim večjega vidnega kota. LMR-ji se pogosto uporabljajo za namen lokalizacije in gradnje zemljevida okolja [11]. Čeprav LMR-ja ne moremo primerno pomanjšati, da bi ga vgradili na miniaturni AGV, pa lahko simuliramo meritve tega senzorja. Globalni sistem s strojnim vidom nam omogoča merjenje leg miniaturnih AGV-jev, poznamo pa tudi njihove oblike in obliko poligona. Oblike vseh objektov lahko opišemo z daljicami, nato pa uporabimo algoritem za detekcijo presečišč laserskih žarkov z daljicami ovir. Algoritem se sprehodi čez vse daljice vseh objektov, ki predstavljajo statične ali dinamične ovire, pri čemer za vsako daljico naredimo naslednje (slika 7.30): 1. Če sta a in b robni točki daljice zapisani v homogenih koordinatah, potem je premica skozi ti dve točki l ab = a × b in enotski vektor daljice e ab = b−a . kb−ak 2. Za vsak žarek LMR-ja h i l ψ = − sin ψ cos ψ x 0 sin ψ − y 0 cos ψ naredimo naslednje: 7.5. Primeri uporabe večagentnih sistemov 417 l ab l ψ a p ψ b ψ 0 Laser ( x 0 , y 0) Svet Slika 7.30: Modeliranje laserskega merilnika razdalj (a) Poiščemo presečišče h i T p = κx κy κ = l ψ × l ab žarka s premico daljice in izračunamo faktor q = e T ( p − a). ab κ (b) Če velja 0 ≤ q ≤ 1, potem presečišče p leži na daljici. V tem primeru izračunamo razdaljo r od objekta do izhodišča laserskega merilnika (v smeri vektorja žarka): p T h i T r = ( − p T ) cos ψ sin ψ 0 κ 0 Če velja 0 ≤ r ≤ rmax in če je r tudi manjši od trenutne najkrajše razdalje rψ, posodobimo najkrajšo razdaljo: rψ = r. Virtualni LMR lahko namestimo glede na katerikoli koordinatni sistem (npr. na prednji del AGV-ja). Nastavimo lahko različne parametre senzorja, kot so doseg, natančnost, vidni kot, kotna ločljivost itd. S stališča podatkov ne moremo ločiti med načinom uporabe realnega ali virtualnega senzorja. Virtualni senzor ne more zaznati objektov, katerih leg ne poznamo oz. jih ne merimo z globalnim sistemom za merjenje lege — poleg AGV-jev lahko z značkami označimo tudi druge objekte na poligonu in tako omogočimo zaznavanje tudi teh. Na sliki 7.31 je vizualizacija laserskih meritev (vijolične točke), kjer se laserski merilnik razdalj nahaja na vijoličnem AGV-ju. Vidimo odboje laserskih žarkov na zelenem, modrem in rumenem AGV-ju, ne pa tudi na sinje modrem AGV-ju, saj je zasenčen z zelenim AGV-jem; rdeči AGV pa je izven vidnega kota senzorja. Zaradi omejenega dosega senzor ne zaznava objektov, ki so preveč oddaljeni. 418 Agenti in večagentni sistemi Slika 7.31: Vizualizacija meritev LMR-ja, leg AGV-jev in načrtovanih poti Nekoordinirano večagentno vodenje Na podlagi algoritmov vodenja, lokalizacije in planiranja poti lahko zagotovimo, da AGV deluje avtonomno, če je edini agent v okolju. V primeru več agentov pa so za primerno delovanje potrebni dodatni sistemi vodenja, sicer lahko prihaja do blokiranj poti, zastojev ali celo trkov in drugih napak, ki lahko zahtevajo tudi ročno ukrepanje operaterja. Za usklajeno delovanje vseh AGV-jev lahko tako skrbi centralni nadzorni sistem, lahko pa imajo AGV-ji lastne sisteme, ki omogočajo razreševanje danih situacij v okolju. Večagentni sistemi so pogosto zasnovani tudi hierarhično, kjer imajo AGV-ji vgrajene sisteme za varno in predvidljivo delovanje, centralni nadzorni sistem pa bedi nad vsemi sistemi in koordinira celotno floto AGV-jev, tako da so vse naloge opravljene optimalno. AGV-ji lahko z LMR-senzorjem zaznavajo ovire v svoji okolici. Tako lahko AGV določi, če se v njegovi neposredni okolici na predvideni poti nahajajo ovire. V primeru zaznane ovire mora AGV prilagoditi svojo hitrost, da ne pride do trka. Hitrost lahko prilagodijo proporcionalno glede na oddaljenost do najbližje točke možnega trka. Ko se ovira sprosti, lahko nadaljujejo z vožnjo brez omejitev. V primeru, da je zaznana ovira drug AGV, mu AGV lahko sledi na primerni varnostni razdalji, vse dokler potujeta po isti progi. Takšen sistem preprečevanja trkov je enostaven za izvedbo in je v praksi običajno zahtevan kot varnostni mehanizem, ki mora biti tudi ustrezno certificiran. Na sliki 7.31 so prikazane poti po katerih potujejo AGV-ji. Poti so bile načrtane povsem neodvisno (za vsak AGV ločeno). Miniaturni AGV-ji imamo vgrajen preprost sistem za preprečevanje trkov na podlagi virtualnega LMR-senzorja, 7.5. Primeri uporabe večagentnih sistemov 419 ki je nameščen na prednjem delu AGV-ja. V situaciji na sliki 7.31 bodo rdeči, rumeni, zeleni in sinje modri AGV predvidoma lahko dosegli zadani cilj, brez da bi na poti naleteli na oviro. Modri AGV je že na cilju in miruje, zato vijolični AGV ne bo mogel povsem doseči ciljne postaje, dokler modri AGV te postaje ne zapusti. Vijolični AGV pa se cilju ne bo mogel predvsem približati, saj je na njegovi poti pred njim tudi zeleni AGV, ki bo na svoji končni postaji blokiral še pot vijoličnega AGV-ja. Pri uporabi le preprostega sistema za preprečevanje trkov hitro pride do situacij, kjer dva AGV-ja pripeljeta v križišče iz različnih smeri skoraj istočasno. Ker AGV-ja zaznavata eden drugega kot oviro, pride do zastoja, ki bi (brez poseganja operaterja ali višjenivojskega nadzornega sistema) trajal neskončno dolgo. Problem lahko rešimo z vpeljavo prednostnih pravil, podobno kot imamo v cestnem prometu desno pravilo in prednostne ceste. Zastoj lahko razrešimo tudi tako, da omogočimo komunikacijo med AGV-jema in vzpostavimo ustrezen sistem pogajanja. Lahko pa vpeljemo v križišča (vsa ali le določena) tudi semaforje. Sistem semaforjev, ki ureja prehode skozi določeno križišče, je tudi agent, ki lahko deluje povsem avtonomno ali pa sodeluje z ostalimi agenti v okolju. Ti agenti so lahko semaforji v drugih križiščih, AGV-ji ali pa tudi centralni nadzorni sistem. S koordiniranim vodenjem vseh agentov, lahko zagotovimo optimalno delovanje z minimalnim številom zastojev. V kolikor optimiziramo poti za vse AGV-je hkrati, lahko poskusimo doseči tudi, da do zastojev na poti ne prihaja in da lahko vsi AGV-ji dosežejo svoj cilj, če le je končna postaja prosta (glejte poglavje 7.5.4). V primeru na sliki 7.31 bi želeli doseči, da je vijolični AGV pred zelenim — vsaj na delu kjer bi sicer prišlo do zastoja. 7.5.4 Večagentno planiranje vožnje transportnih vozil Z vse bolj pogosto avtomatizacijo skladišč in proizvodnih obratov je postalo tudi raziskovanje na področju avtonomnih robotskih vozil zelo popularno. Eden bistvenih izziv avtonomnih robotskih vozil, je planiranje poti. To je lahko rešeno centralno, kjer centralna enota, ki povezuje vsa robotska vozila hkrati določi načrt poti za vsa vozila. Glavna prednost centralnih pristopov je večja optimalnost načrtanih poti [12, 13]. Pri bolj obsežnih zemljevidih in večjem številu vozil po navadi postanejo taki pristopi časovno preveč potratni, kar omejuje uporabo v realnih sistemih. Problem računske kompleksnost bolje naslavljajo decentralizirani pristopi, ki so v splošnem hitrejši od centraliziranih, bolj prilagodljivi na spremembe okolja in delovnih nalogov a večinoma zagotavljajo manj optimalne rešitve [14, 15]. V teh pristopih se naloga določanja poti iz višjega nivoja prestavi na sama vozila, tako da avtonomno določajo vsak svojo pot, hkrati pa sprotno rešujejo konflikte in zbirajo informacije o drugih vozilih. 420 Agenti in večagentni sistemi Prikazan je pristop planiranja poti za usklajeno delovanje skupine mobilnih robotskih vozil pri transportu materiala v proizvodnih obratih. Pristop je razvit v okviru magistrske naloge in je objavljen tudi v prispevku [16]. Predstavljen algoritem temelji na znanem algoritmu A*, ki je nadgrajen za planiranje poti več robotskih vozil, tako da najde kompromisno rešitev brez trkov in nepotrebnih zastojev. Pristop upošteva prioritete transportnih nalogov, zemljevid v obliki uteženega usmerjenega grafa ter predvidena časovna okna zasedenosti segmentov zemljevida. Algoritem najprej poišče pot za vozila z višjimi prioritetami. Ob vsakem planiranju poti vozila, algoritem na koncu zabeleži predvidena časovna okna zasedenosti cest in vozlišč na zemljevidu za najdeno pot. Te zasedenosti se nato upoštevajo pri iskanju poti za vozilo z nižjo prioriteto, tako da ne ovira vožnje vozil z višjo prioriteto in se izogne konfliktom. Dve pomembni možnosti, ki jih algoritem upošteva in predlaga, sta čakanje pred vozliščem, da se pot sprosti in pa možnost umika na stransko cesto v primeru onemogočenega čakanja. Pristop je ilustriran na simulacijskih primerih. Planiranje z upoštevanjem prioritet in oken zasedenosti Predlagan algoritem je nadgradnja algoritma A* in omogoča planiranje poti za več robotskih vozil, ki se hkrati vozijo na istem zemljevidu. Algoritem se izvaja ločeno za vsako vozilo posebej po prioritetnem sistemu. Vsakemu vozilu določimo stopnjo prioritete. Večja kot je stopnja prioritete, bolj pomembno je, da to vozilo doseže cilj v najkrajšem možnem času. Preden izvedemo algoritem na kateremkoli vozilu, je zemljevid popolnoma prost. Vsa vozlišča in povezave so brez predvidenih časovnih oken zasedenosti. Algoritem najprej izvedemo na problemu vozila z najvišjo prioriteto. Ker je zemljevid še prost, bo algoritem zanj našel optimalno pot. Hkrati bo določil časovna okna zasedenosti za ceste in vozlišča v časovnih trenutkih, ki jih narekuje rezultat planiranja poti. Ko zaženemo algoritem na robotu z nižjo prioriteto, bo z upoštevanjem zasedenosti časovnih oken cest in vozlišč zanj našel čim hitrejšo možno pot, ki ne bo ovirala vozil z višjo prioriteto. Osnovni potek iskanja poti za posamezno vozilo je enak kot pri algoritmu A*. Algoritem najprej doda začetno vozlišče na odprti seznam. Nato v zanki izvaja iterativni postopek, dokler ni odprti seznam prazen oz. dokler iz odprtega seznama na zaprti seznam ne prestavi ciljnega vozlišča, kar pomeni, da je pot najdena. V iterativnem postopku najprej iz odprtega na zaprti seznam prestavi vozlišče z najmanjšo skupno ceno. Nato za vsako sosednje vozlišče določi cene ter preveri zasedenost vozlišča in pripadajoče ceste. Če je pot prosta, algoritem nadaljuje z enakim potekom kot A*. Glavna razlika med algoritmoma nastopi, kadar algoritem pri odpiranju sosednjega vozlišča in preverjanju zasedenosti naleti na konflikt. Konflikt predstavlja zasedenost vozlišča ali ceste do sosednjega vozlišča. Prisotnost konflikta se ugotavlja s pomočjo časovnih oken zasedenosti vozlišča ali cest, ki jih za svojo pot definirajo vozila z višjo prioriteto. V primeru konflikta se s poizkušanjem išče prosta mesta za čakanje in izogib konfliktu z 7.5. Primeri uporabe večagentnih sistemov 421 vračanjem po poti, po kateri je prišel. Pseudo-algoritem z opisanim postopkom izogibanja konfliktov je podan v algoritmu 6. S tem se začne rekurziven postopek Algorithm 6 Planiranje poti za več vozil. Inicializacija: Dodelitev prioritet transportnim nalogom za N vozil. Predstavitev prostora z grafom prehajanja stanj in okni zasedenosti. for vozilo s prioriteto i = 1 , . . . , N do V zanki izvajaj iterativni postopek iskanja poti po algoritmu A*. V primeru konflikta izvajaj: while mestoKonf likta 6= zacetniP olozajV ozila do if prosto(cesta pred mestoKonf likta ) then Predlagaj čakanje na cesti pred mestoKonf likta. break end if if prosto(stranska cesta pred mestoKonf likta) then Predlagaj umik in čakanje na stranski cesti pred mestoKonf likta. break end if Prestavi mestoKonf likta na predhodno cesto po poti nazaj. end while Določi okna zasedenosti za vozlišča in ceste na poti. end for v katerem algoritem s poizkušanjem išče primerno mesto za čakanje na sprostitev poti in izogib konfliktu. Najprej poizkusi s čakanjem na predhodni cesti. Če tudi ta cesta ni prosta, poizkusi s čakanjem na kateri od stranskih cest. Stranske cesta so vse ceste povezane s predhodnim vozliščem, razen predhodne in trenutne ceste. Če nobena od stranskih cest ni prosta za čakanje, se algoritem rekurzivno pomakne po poti nazaj. Algoritem nadaljuje z novo iteracijo, kjer poizkusi s čakanjem na novi predhodni cesti, če ta ni prosta na novih stranskih cestah itd., dokler ne najde prostega mesta ali ne pride do začetka poti. Določitev cen povezav in oken zasedenosti povezav in voz- lišč Predlagan algoritem pri raziskovanju zemljevida prednostno izbira vozlišča z najmanjšo skupno ceno. Cena povezave predstavlja čas, ki ga vozilo porabi, da prevozi pot med dvema vozliščema. Pri tem predpostavimo konstantno hitrost vozila med premikanjem, ki jo podamo kot vhod algoritma. Predlagan algoritem vpeljuje možnost čakanja vozila na mestu, da se cesta ali vozlišče sprosti, vozilo pa lahko nadaljuje pot brez konfliktov. Čas, da vozilo prevozi neko pot se tako 422 Agenti in večagentni sistemi lahko podaljša s pribitkom cene zaradi čakanja na tej poti. Povečana cena torej predstavlja seštevek časa vožnje in časa čakanja med vozliščema. V splošnem izbira take cene pomeni, da bo algoritem iskal pot, po kateri bo najhitreje prispel do cilja. Ko algoritem najde pot za vozilo ji doda še časovna okna zasedenosti cest in vozlišč, ki jih bo vozilo prevozilo na najdeni poti ob predvidenih časovnih trenutkih. Teoretično se vozilo na vozlišču nahaja zgolj časovni trenutek. A je za določitev okna zasedenosti vozlišča potrebno upoštevati tudi dimenzije vozila in želeno varnostno razdaljo ter njegovo hitrost vožnje, da se prepreči trk z drugim vozilom, ki pripelje v vozlišče tik za prvim. Časovno okno zasedenosti vozlišča tako določa interval tV S ≤ t < tV E, kjer so tV S = tV − ∆ tvarn, tV E = tV + ∆ tvarn, tV čas prihoda vozila v vozlišče, ∆ tvarn = D varnostni čas, D seštevek polovične v dimenzija vozila in varnostne razdalje in v hitrost vožnje. Časovno okno zasedenosti ceste določajo naslednji podatki: čas prihoda na cesto ( tvstop), čas vožnje (∆ tcest), čas čakanja (∆ tcak) na cesti (pred končnim vozliščem) in smer vožnje. Vozilo lahko čaka le na cestah (povezavah), čakanje v vozliščih ni dovoljeno. Ko čaka na mestu, je to vedno na cesti pred vozliščem. Časovno okno zasedenosti ceste tako določa interval tCS ≤ t < tCE, kjer je upoštevan varnostni čas ∆ tvarn (dimenzije vozila in želena varnostna razdalja), tCS = tvstop − ∆ tvarn, in morebiten čas čakanja, ki je vključen v tCE = tvstop + ∆ tcest + ∆ tvarn + ∆ tcak. Preverjanje zasedenosti in določitev časa čakanja Preden algoritem doda vozlišče na odprti seznam, preveri če je vozlišče prosto. Pri tem preveri vstopno cesto in vozlišče posebej, saj je lahko zasedena samo cesta ali samo vozlišče. Algoritem najprej preveri zasedenost ceste in nato zasedenost vozlišča. V kolikor algoritem zazna zasedenost ceste ali vozlišča izlušči podatek o koncu zasedenosti ceste oz. vozlišča. Na podlagi tega podatka v nadaljevanju določi čas čakanja vozila. Vozilo lahko čaka na trenutni cesti (cesta pred vozliščem), ko je zasedeno vozlišče oziroma na predhodni cesti trenutne ceste, če je zasedena trenutna cesta. Preverjanje zasedenosti ceste in določitev časa čakanja na predhodni cesti Algoritem pri preverjanju zasedenosti ceste za trenutno vozilo primerja predvideno časovno okno s časovnimi okni zasedenosti, ki jih je za to cesto prej določil vozilom z višjo prioriteto. Pri tem ločimo časovno okno vozila, ki vozi v nasprotni smeri vožnje trenutnega vozila (z indeksom i) in časovno okno vozila, ki vozi v smeri vožnje trenutnega vozila. V primeru iste smeri vožnje po cesti je čas čakanja trenutnega vozila (∆ tcak ) pred i cesto določen upoštevajoč okna zasedenosti vozil z višjo prioriteto ( tCS ≤ t < tCE) 7.5. Primeri uporabe večagentnih sistemov 423 ter s parametri trenutnega vozila (vstopni čas tvstop , varnostni čas t ) i varni (0 ; | tvstop − tCS| ≥ ∆ tvarn ∆ t i i cak = i | tCS − tvstop | + ∆ t ; | t − t i varni vstopi CS | < ∆ tvarni Pri izračunu časa čakanja trenutnega vozila ∆ tcak na cesti je potrebno upoštevati i tudi morebiten čas čakanja predhodnega vozila z višjo prioriteto. Kjer trenutno vozilo lahko na cesto vstopi brez čakanja pred ali za predhodnikom (∆ tcak = 0), i če je vsaj za varnostni čas ∆ tvarn in za svoj predviden čas čakanja (brez i upoštevanja predhodnega čakanja na cesti) pred predhodnikom oziroma, če je vsaj za varnostni čas ∆ tvarn in čas čakanja predhodnika za njim. V nasprotnem i pa mora svoj čas čakanja ustrezno podaljšati. V primeru nasprotne vožnje po cesti, pa je čas čakanja trenutnega vozila z indeksom i določen kot  t ≥ t ali  vstopi CE + ∆ tvarni  0 ;   tizstop ≤ tCs − ∆ tvarn ∆ t i i cak = i t < t in  vstopi CE + ∆ tvarni   tCE − tvstop + ∆ tvarn ;  i i  tvstop > t i CS − ∆ tvarni Preverjanje zasedenosti vozlišča in določitev časa čakanja na trenutni cesti Potreben čas čakanja na cesti pred vozliščem (trenutni cesti) za vozilo i določimo glede na okno zasedenosti, ki so ga za to vozlišče določila vozila z višjo prioriteto. Čas čakanja lahko poenostavljeno (brez upoštevanja smeri vožnje skozi vozlišče) določimo kot (0 ; | tvstop − tV | ≥ ∆ tvarn + ∆ tV ∆ t i i cak = i ∆ tvarn + t ; | t − t + ∆ t i V E − tvstopi vstopi V | < ∆ tvarni V kjer je tV = tV S+ tV E trenutek prihoda (težišča) predhodnega vozila v vozlišče in 2 ∆ tV = tV E− tV S je polovični interval zasedenosti vozlišča (kar je enako varnostnem 2 času predhodnika). Izjema je preverjanje zasedenosti vozlišča, ki za trenutno vozilo predstavlja ciljno vozlišče. V tem primeru mora biti vozlišče prosto od trenutka prihoda nanj naprej. Čakanje zaradi zasedenosti Ko algoritem pri dodajanju vozlišč na odprti seznam naleti na vozlišče, ki je zasedeno, ali je zasedena cesta do njega, predlaga čakanje vozila pred zasedenim delom, da se ta sprosti. Ta čas čakanja algoritem prišteje k ceni-do-sem vozlišča, da se v nadaljevanju upošteva pri raziskovanju in izbiri poti. Vozlišče, ki ga algoritem dodaja na odprti seznam je sestavljeno iz dveh delov; iz vozlišča samega in iz ceste, ki vodi do njega. 424 Agenti in večagentni sistemi Kadar je zasedeno zgolj vozlišče, bo vozilo čakalo na trenutni cesti pred vozliščem. Kadar pa je zasedena trenutna cesta, vozilo ne čaka na predhodnem vozlišču, ampak na predhodni cesti. Vozlišča povezujejo več cest in predstavljajo križišča, čakanje na križišču pa bi zmanjšalo prehodnost zemljevida in zasedlo več poti hkrati. Vedno, ko vozilo čaka, je to na cesti pred vozliščem. Algoritem pri določanju cene čakanja vozila loči med čakanjem na trenutni cesti in čakanjem na predhodni cesti. Ceno-do-sem zato razdelimo na tri dele: cena-do-sem brez čakanja, cena čakanja na trenutni cesti in cena čakanja na predhodni cesti. Kadar algoritem posodobi tudi ceno čakanja na predhodni cesti, s tem zakasni prihod vozila na konec predhodne ceste, kar podaljša čas nahajanja vozila na predhodni cesti. Potrebno je preveriti, če je v tem dodatnem času predhodna cesta še prosta in če je ob novem času prihoda na predhodno vozlišče le to še prosto. Če sta predhodna cesta in predhodno vozlišče prosta, se ponovi postopek od preverjanja trenutnega vozlišča in trenutne ceste naprej. Če predhodna cesta ali vozlišče ob dodanem čakanju na predhodni cesti nista več prosta, trenutnega vozlišča ne moremo dodati na odprti seznam. V tem primeru algoritem predlaga umik na stransko cesto ali čakanje na cesti, ki je predhodna sedanji predhodni cesti. Kadar umik na stransko cesto ni možen, algoritem predlaga pomik čakanja po poti nazaj. Rezultat algoritma Ko algoritem razišče ciljno vozlišče je našel najhitrejšo pot. Eksplicitno ciljno vozlišče ne nosi informacije o celotni poti, ampak le o ceni te poti in predhodnem vozlišču. Kočno pot je potrebno sestaviti s sledenjem predhodnikov na zaprtem seznamu vozlišč. S sledenjem predhodnikov pridemo do začetnega vozlišča, če si za vsako najdeno vozlišče zapišemo še oznako pripadajoše ceste dobimo obrnjen seznam cest, ki vodi od začetka do cilja in sestavlja najdeno pot. Algoritem poleg seznama cest za vsako cesto vrne še podatek, če gre za umik na stransko ceste ter podatek o času čakanja na tej cesti. Primeri delovanja Algoritem bo predstavljen na primeru, ki je prikazan na sliki 7.32, kjer lahko vidimo začetne položaje treh robotskih vozil. Modro vozilo 1 z najvišjo prioriteto pot prične na vozlišču 7 in konča na vozlišču 1. Zeleno vozilo 2, ki je naslednje po prioriteti, prične pot na vozlišču 2 in konča na vozlišču 7. Zadnje, rdeče vozilo 3, katerega postopek iskanja poti bomo po korakih opisali, pot prične na vozlišču 1 in konča na vozlišču 6. Koordinate so predstavljene v metrih, hitrosti vseh vozil pa so 1m/s. Poti za vozila 1 in 2 sta že načrtani. Posledično so za te poti določena tudi časovna okna zasedenosti, ki za vsako cesto določajo čas prihoda, odhoda in čakanja vozila na njej. Prav tako za vsako vozlišče določajo trenutek 7.5. Primeri uporabe večagentnih sistemov 425 Slika 7.32: Zemljevid ter začetni položaji, cilji in končne poti treh robotskih vozil Slika 7.33: Časovna okna zasedenosti cest in vozlišč za modro in zeleno vozilo na sliki 7.32 prihoda vozila nanj. Časovna okna zasedenosti za načrtane poti vozil 1 in 2 lahko vidimo na sliki 7.33. Vsaka črta predstavlja časovni interval oz. časovno okno, ko vozilo zasede cesto ali vozlišče. Levo od črte je zapisana oznaka vozlišča ali ceste, ki je zasedena. Vse ceste so usmerjene in vodijo od nekega vozlišča k drugemu. Njihove oznake so sestavljene iz oznak vozlišč, ki jih povezujejo. Npr. cesta 502 vodi od vozlišča 5 proti vozlišču 2. Pri cestah polna črta predstavlja zasedenost z vožnjo v smeri ceste, črtkana črta pa zasedenost z vožnjo v nasprotni smeri ceste. Levo krajišče črte predstavlja čas prihoda na začetek ceste, desno krajišče pa čas prihoda na konec ceste. Čakanje na cesti je določeno z dodatno označeno točko na črti, kjer čas od srednje točke do desnega krajišča predstavlja čas čakanja. Pri vozliščih čas prihoda na vozlišče predstavlja sredina črte, ki je razširjena z namenom, da vozila na vozlišče prihajajo z razmakom varnostnega časa. Izjema je poltrak oblike črta-pika, ki označuje čas od prihoda vozila na ciljno vozlišče naprej. Ta časovna okna algoritem upošteva pri iskanju poti rdečega vozila 3. Pri iskanju poti rdečega vozila algoritem upošteva okna zasedenosti in išče možno pot v grafu. Začne z vozliščem 1, nadaljuje do naslednjega možnega vozlišča 4 in 426 Agenti in večagentni sistemi ker je v času vožnje po cesti 104 (0-2 s) in ob času prispetja v vozlišče 4 (2 s) prosto, ga doda na odprti seznam. V drugem koraku algoritem preišče naslednike vozlišča 4, torej vozlišči 3 in 5. Vozlišče imata enako ceno-do-sem (4 s) a vozlišče 5 ima manjšo ceno-do-cilja (3 s, vozlišče 3 pa ima ceno-do cilja 7 s) in posledično tudi manjšo ceno celotne poti, zato algoritem razišče njegove naslednike in ga doda na zaprti seznam. Nato v tretjem koraku algoritem izbere vozlišče 6, do katerega vodi cesta 506. Cena-do-sem za vozlišče 6 znaša 7 s in predviden interval vožnje po cesti 506 je 3-6 s. Cesta je znotraj tega intervala zasedena (glejte sliko 7.33) z vožnjo modrega vozila v nasprotni smeri do časa 6 s. To predstavlja konflikt, kateremu se želimo izogniti. S tem se začne rekurziven postopek v katerem algoritem s poizkušanjem išče primerno mesto za čakanje na sprostitev poti in izogib konfliktu. Najprej poizkusi s čakanjem na cesti, ki se na poti do zasedene ceste nahaja pred njo, torej je njena predhodna cesta (cesta 405). Ta je v časovnem intervalu (2-6 s) (prihod na začetek ceste in zakasnjen prihod na konec ceste) prosta, a zasedeno je vozlišče 5 do časa 8s, kar še podaljša čakanje na cesti 405 za 2s, v tem dodatnem času pa cesta ni več prosta, torej podaljšanje čakanja ni mogoče. Nadalje algoritem preveri, če bi lahko vozilo čakalo na sprostitev ceste 506 na stranski cesti 502 predhodnega vozlišča 5. Tudi cesta 502 v intervalu 4-6 s (vključujoč potreben čas čakanja 2s) ni prosta. Algoritem v nadaljevanju preizkuša ostala možna mesta čakanja po poti, ki vodi do trenutnega vozlišča, nazaj, z namenom, da najde mesto, kjer lahko brez oviranja drugih vozil čaka na sprostitev poti do željnega vozlišča. Po nekaj iteracijah algoritem najde možno mesto čakanja na stranski cesti 403 za časovni interval 2-9 s kot je prikazano na sliki 7.32. Po dodanem čakanje na stranski cesti, algoritem v naslednjem koraku razišče vozlišče 5, ki nasledi umik na stransko cesto. Vozlišče 5 je ponovno raziskano, saj je vmes dodano čakanje in zakasnitev prihoda. V naslednjem koraku razišče še vozlišče 6 in s tem pride do cilja. S tem je algoritem določil optimalne poti za vsa tri vozila brez konfliktov in upoštevajoč prioritetno listo. Na slikah 7.34 in 7.35 sta prikazana rezultata planiranja poti še na dveh drugih zemljevidih. Prikazani so začetni položaji vozil z njihovimi oznakami ter izris načrtanih poti. Vsa vozila vozijo s hitrostjo 1 m/s. Vozilo z oznako 1 ima najvišjo prioriteto in vozilo z najvišjo oznako ima najnižjo prioriteto. Čakanje je ponazorjeno z znakom ure s pripisom časa začetka in časa konca čakanja. Zemljevid na sliki 7.34 predstavlja skladišče z mesti dolaganja tovora na vozila (slepe ceste) ter z bolj obremenjenim osrednjim delom iz vozlišč 10 in 11. Zemljevid na sliki 7.35 predstavlja preprost a splošno uporaben mrežast zemljevid, ki ga lahko, s poljubno odstranitvijo posameznih cest ali vozlišč apliciramo na mnoga skladišča ali tovarne. Prikazan pristop planiranja več robotskih vozil nadgrajuje algoritem A*, ki je v osnovi namenjen planiranju poti enega vozila. Algoritem upošteva prioritete 7.5. Primeri uporabe večagentnih sistemov 427 Slika 7.34: Prikaz rezultata planiranja na primeru s 5 vozili. Izrisani so začetni položaji vozil in načrtane poti. Simbol ure s pripisom časa začetka in časa konca čakanja označuje mesto čakanja vozila. Slika 7.35: Prikaz rezultata planiranja na primeru mrežastega zemljevida cest. Izrisani so začetni položaji vozil in načrtane poti. Simbol ure s pripisom časa začetka in časa konca čakanja označuje mesto čakanja vozila. 428 Agenti in večagentni sistemi transportnih nalogov, kar omogoča bolj učinkovito izvajanje prioritetnih zadol- žitev, vozila z nižjo prioriteto pa se prilagodijo prvim. Strategija določanja časovnih oken zasedenosti omogoča zaznavo potencialnih konfliktov, ki se jim algoritem izogne z izbiro druge poti, s čakanjem pred konfliktom ali z umikom na stransko pot. Kompleksnost algoritma narašča s številom vozil, saj se vozila z nižjo prioriteto večkrat znajdejo v zasedenih delih zemljevida in morajo iskati alternativne poti brez konfliktov. Literatura [1] R. C. Arkin. Motor schema-based mobile robot navigation. The International Journal of Robotics Research, zv. 8, št. 4, str. 92–112, 1989. [2] R. A. Brooks. Intelligence Without Representation. Artificial Intelligence, zv. 47, str. 139–159, 1991. [3] G. Klančar, M. Kristan in sod. Robust and efficient vision system for group of cooperating mobile robots with application to soccer robots. ISA Transactions, zv. 43, str. 329–342, 2004. [4] G. Klančar in D. Matko. Strategy control of mobile agents in soccer game. V The 3rd International Conference on Computational Intelligence, Robotics and Autonomous Systems CIRAS 2005 & FIRA RoboWorld Congress Singapore 2005 FIRA 2005, str. 1–6. National University of Singapore: CIRAS & FIRA Organising Committee, 2005. [5] G. Klančar, D. Matko in S. Blažič. A control strategy for platoons of differential-drive wheeled mobile robot. Robotics and Autonomous Systems, zv. 59, št. 2, str. 57–64, 2011. [6] Z. Zhang. A flexible new technique for camera calibration. IEEE Transactions on pattern analysis and machine intelligence, zv. 22, št. 11, str. 1330–1334, 2000. [7] J. Perš in S. Kovačič. Nonparametric, model-based radial lens distortion correction using tilted camera assumption. V Kropatsch (Eds.), Proceedings of the Computer Vision Winter Workshop 2002, str. 286–295. 2002. [8] S. Garrido-Jurado, R. Muñoz-Salinas in sod. Generation of fiducial marker dictionaries using mixed integer linear programming. Pattern Recognition, zv. 51, str. 481–491, 2016. [9] F. J. Romero-Ramirez, R. Muñoz-Salinas in R. Medina-Carnicer. Speeded up detection of squared fiducial markers. Image and vision Computing, zv. 76, str. 38–47, 2018. [10] W. Boehm in A. Müller. On de casteljau’s algorithm. Computer Aided Geometric Design, zv. 16, št. 7, str. 587–605, 1999. [11] L. Teslić, I. Škrjanc in G. Klančar. Using a LRF sensor in the Kalman-filtering-based localization of a mobile robot. ISA transactions, zv. 49, št. 1, str. 145–153, 2010. [12] R. Olmi, C. Secchi in C. Fantuzzi. Coordination of multiple agvs in an industrial application. V 2008 IEEE International Conference on Robotics and Automation, str. 1916–1921. IEEE, 2008. [13] E. Gawrilow, E. Köhler in sod. Dynamic routing of automated guided vehicles in real-time. V Mathematics–Key Technology for the Future, str. 165–177. Springer, 2008. [14] V. Digani, L. Sabattini in sod. Ensemble coordination approach in multi-agv systems applied to industrial warehouses. IEEE Transactions on Automation Science and Engineering, zv. 12, št. 3, str. 922–934, 2015. Literatura 429 [15] Z. Zhang, Q. Guo in sod. Collision-free route planning for multiple agvs in an automated warehouse based on collision classification. IEEE Access, zv. 6, str. 26022–26035, 2018. [16] N. Presečnik in G. Klančar. Nadgradnja algoritma A ? za planiranje poti več robotskih vozil po prioritetnem sistemu. V Zbornik dvanajste konference Avtomatizacija v industriji in gospodarstvu (AIG’21), str. 1–9. Društvo avtomatikov Slovenije, 2021. Dodatni material http://msc.fe.uni-lj.si/ams-kv Document Outline Predgovor Kazalo Uvod v mobilne sisteme Roboti Mobilnost Kolesa Avtonomni mobilni sistemi Kratka zgodovina Modeliranje gibanja mobilnih sistemov Uvod Kinematika kolesnih mobilnih sistemov Diferencialni pogon Kolesni pogon Trikolesni pogon Tricikel s priklopnikom Avtomobilski (Ackermannov) pogon Sinhroni pogon Večsmerni pogon Gosenični pogon Omejitve gibanja Holonomične omejitve Neholonomične omejitve Integrabilnost omejitev Vektorska polja, porazdelitev, Liejevi oklepaji Vodljivost kolesnih mobilnih robotov Dinamični model mobilnega sistema z omejitvami Predstavitev dinamičnega modela mobilnega sistema z omejitvami v prostoru stanj Kinematični in dinamični model robota z diferencialnim pogonom Vodenje kolesnih mobilnih sistemov Uvod Vodenje v referenčno lego Vodenje orientacije Vodenje gibanja naprej Osnovni pristopi Vodenje po referenčni trajektoriji Osnovni pristopi k vodenju po referenčni trajektoriji Razčlenitev vodenja na predkrmiljenje in povratno zanko Povratnozančna linearizacija Izpeljava kinematičnega modela pogreška vodenja pri sledenju referenčne trajektorije Linearni regulator Načrtovanje vodenja na osnovi funkcij Ljapunova Načrtovanje mehkega vodenja Takagi-Sugeno v okviru linearnih matričnih neenačb Modelno prediktivno vodenje Vodenje na podlagi optimizacije z rojem delcev Vodenje mobilnega sistema s pristopom vodenja na osnovi slike Ocena optimalnega profila hitrosti za znano pot Načrtovanje poti Uvod Okolica robota Načrtovanje poti Konfiguracija in konfiguracijski prostor Matematični zapis oblike in lege ovire v okolici Predstavitev okolja za načrtovanje poti Predstavitev z grafi Razcep na celice Zemljevid cest Potencialno polje Načrtovanje poti z metodami vzorčenja prostora Preprosti algoritmi načrtovanja poti — algoritmi tipa hrošč Algoritem Hrošč0 Algoritem Hrošč1 Algoritem Hrošč2 Metode iskanja poti v grafu Iskanje v širino Iskanje v globino Iterativno iskanje v globino Dijkstrov algoritem Algoritem A-zvezdica Pohlepno iskanje najprej najboljši Senzorji v mobilnih sistemih Uvod Transformacije koordinatnih sistemov Orientacija in rotacija Translacija in rotacija Kinematika rotacijskih koordinatnih sistemov Projekcijska geometrija Metode merjenja lege Relativno določanje lege Merjenje smeri gibanja Aktivne značke in globalne meritve pozicije Navigacija z uporabo značilk okolja Ujemanje modelov okolja — zemljevidi Senzorji Karakteristike senzorjev Klasifikacija senzorjev Nedeterminističnost v mobilnih sistemih Uvod Osnove verjetnosti Diskretna slučajna spremenljivka Zvezna slučajna spremenljivka Bayesovo pravilo Ocenjevanje stanj Motnje in šum Ocena konvergence in pristranskosti Spoznavnost Bayesov filter Markovove verige Ocenjevanje stanj iz opazovanj Ocenjevanje stanj iz opazovanj in akcij Primer lokalizacije Zaznavanje okolja Gibanje v okolju Lokalizacija v okolju Kalmanov filter Kalmanov filter v matrični obliki Razširjeni Kalmanov filter Druge različice Kalmanovega filtra Filter delcev Agenti in večagentni sistemi Uvod Večagentni sistemi Agenti Arhitektura in delovanje agentov Kognitivni agenti Odzivni agenti Hibridni agenti Odzivno vedenjski agenti Osnovne vedenjske arhitekture Ostale delitve agentov in večagentnih sistemov Primeri uporabe večagentnih sistemov Robotski nogomet — avtonomna igra kolesnih robotov Vožnja vozil v formaciji Avtomatsko vodeni vozički Večagentno planiranje vožnje transportnih vozil