Izvedba sistemov vodenja na osnovi modelov za avtonomno vožnjo miniaturnih robotskih vozil Marko Tavcar, Urban Skalar, Jernej Perko, Andrej Zdešar Fakulteta za elektrotehniko, Tržaška 25, 1000 Ljubljana E-pošta: mt6480@student.uni-lj.si, us1673@student.uni-lj.si, jp2352@student.uni-lj.si, andrej.zdesar@fe.uni-lj.si Implementation of model-based control systems for autonomous driving of miniature robotic vehicles In this work we focus on implementation of algorithms for localisation, path planning and path tracking on a cyber-physical system of miniature mobile robots. We implemented Kalman filter to combine the data from two different sensor sources. An A* algorithm was used to find the optimal path in the map. Robot was controlled in such a manner that it is capable of following the reference path, which leads to the desired goal. I. Uvod V svetu zanimanje za avtonomne mobilne sisteme narašča. V tem delu predstavljamo, kako smo implementirali algoritme, ki so ključni za avtonomno delovanje robotskih vozil. V ta namen smo uporabili poenostavljen fizični model mesta z miniaturnimi mobilnimi roboti, ki je prikazan na sliki 1. Na fizičnem modelu smo implementirali algoritme za lokalizacijo, načrtovanje poti in vodenje po poti [1] tako, da se mobilni robot lahko avtonomno vozi po čestah med poljubnimi čilji in pri tem vozi varno ter upošteva čestno-prometne predpise. Slika 1: Fizični model mesta s cestami in miniaturni mobilni roboti z značkami ter kamera Delo, predstavljeno v tem članku, je bilo opravljeno v okviru projekta InMotion, ki ga sofinancira program Evropske unije Erasmus+, št. 573751-EPP-1-2016-1-DE-EPPKA2-CBHE-JP. II. Predstavitev sistema A. Kolesni mobilni robot V našem sistemu smo uporabili 5 kolesnih mobilnih robotov z diferenčialnim pogonom. Na vsakem mobilnem robotu se nahaja mini računalnik Raspberry Pi Zero W, ki omogoča izvedbo algoritmov za avtonomno vožnjo, in mikrokrmilnk Arduino Mini, ki se uporablja za krmiljenje motorjev in branje podatkov iz enkoderjev. Maksimalna hitrost gibanja robota znaša 0,3 m/s. Na vsakem motorju kolesa ima robot nameščen en enkoder za merjenje zasuka kolesa s frekvenčo vzorčenja 100 Hz. Slika 2: Prikaz mobilnega robota od zgoraj z označenimi osnovnimi vektorji gibanja Model kolesnega mobilnega sistema z diferencialnim pogonom je prikazan na sliki 2. Vpliv vhodne tangencialne hitrosti v (t) in kotne hitrosti w(t) na spremembo lege mobilnega sistema podaja kinematicni model (1). ±(t) = v (t) cos y(t) y(t) = v(t) sin (k)]T mobilnega robota imamo na voljo dva nepovezana senzorska vira: enkoderje na kolesih in kamero. Na podlagi enkoderjev na kolesih lahko določimo lego mobilnega sistema s postopkom odometrije. Merilni sistem s kamero podaja lego mobilnega robota glede na globalni koordinatni sistem, in v tem smislu deluje kot globalni satelitski sistem za določanje položaja. A. Odometrija Kinematični model (1) lahko z Eulerjevo integracijsko metodo pretvorimo v diskretno obliko (2). Za kratke čase vzorčenja T je napaka, ki jo dobimo zaradi integracijske metode zanemarljiva glede na ostale vire napak (npr. zdrsovanje koles). x(k + 1) = x(k) + cos f(k) 0 sin f(k) 0 01 v(k)T w(k)T (2) Ad(k) = Ay(k) = 2 AdD(k) - AdL(k) L (3) kjer je L razdalja med kolesoma. Relativne premike koles lahko merimo z relativnimi spremembami vrednosti enkoderjev na kolesih (AaD (k) in AaL(k)): u(k) "AdL(k)" "KLA«L(k)" AdD (k) K D AaD (k) (4) x(k) + w(k) " cos (p(k) 2 sin ^(k) ■ L WL(k) wd (k) cos ^(k)~ 2 sin <^(k) (u(k) + w(k)) (5) Q L a L® D aLaD ®D . Predpostavljamo, da meritve s kamere z (k) vsebujejo normalno porazdeljen šum v (k) s srednjo vrednostjo nič in variančo R, kot to opisuje (6). z (k) = h(x(k), v (k)) = x (k) + v(k) vx(k) r®x 0 0 v (k) = vy(k) , R = 0 0 vv(k) 0 0 (6) C. Razširjeni Kalmanov filter Razširjeni Kalmanov filter smo izpeljali na podlagi nede-terminističnega modela mobilnega robota, ki je predstavljen v poglavju III-B. Algoritem vsebuje predikčijski in korekčijski korak, kiju izvajamo zaporedno s frekvenčo vzorčenja podatkov z odometrije. V predikčijskem koraku s (7) napovemo trenutno lego mobilnega sistema x(k) na podlagi meritev iz enkoderjev u(k — 1) in zadnje očene lege x(k — 1). Z (8) določimo še variančo napovedi. Vpeljemo relativni tangenčialni premik Ad(k) = v(k)T in relativni kotni zasuk Ay(k) = w(k)T mobilnega robota. Relačiji med relativnimi premiki mobilnega sistema in relativnimi premiki levega in desnega kolesa, AdL(k) in AdD (k), sta: AdD (k)+AdL(k) x(k) = f (x(k — 1), u(k — 1), 0) P(k) = Ak iP (k — 1)AT_ i + Fk_i QFT_! (7) (8) CCe imamo na voljo novo meritev s kamere, izračunamo ojačenje Kk: Kk = P (k)CT (Ck P(k)CT + R)- (9) Konstante KL = KD = 9,537 ^m in L = 0,075 m smo določili s postopkom kalibračije. Po vstavitvi (3) in (4) v (2), dobimo model, ki omogoča spremljanje lege mobilnega sistema na podlagi merjenja relativnih premikov koles z enkoderji. B. Nedeterministicni model kolesnega robota Za združevanja informačije o legi na podlagi odometrije in kamere smo uporabili razširjeni Kalmanov filter, ki je pogosta izbira pri očenjevanju lege v mobilni robotiki [2]. Za ta namen smo definirali nedeterministični model mobilnega robota, ki je predstavljen v (5) in (6). Različne vplive motenj, ki nastopajo pri merjenju položaja na podlagi odometrije, smo v modelu prehajanja stanj (5) modelirali z normalno porazdeljenim šumom w s srednjo vrednostjo nič in variančo Q. x(k + 1) = f (x(k), u(k), w(k)) = sičer pa postavimo Kk = 0. V korekčijskem koraku posodobimo očeno lege mobilnega robota x(k) na podlagi napovedi x(k) in ob upoštevanju meritve s kamere z (k), kot to opisuje (10). Z (11) posodobimo tudi variančo očene. x(k) = x(k) + Kk(z(k) — h(x(k), 0)) (10) P(k) = P(k) — Kk CkP(k) (11) Matrike Ak, Fk in Ck, ki se pojavljajo v enačbah (7) do (11), določimo na podlagi linearizačije nelinearnega modela mobilnega sistema v okoliči očene v trenutku k: df (x, u, w) A k= dx df (x, u, w) Ck d w dh(x, v) x=x(k) ,u=u(k) ,w=0 x=x(k) ,u=u(k) ,w=0 (12) dx x=x(k),v=0 IV. Načrtovanje poti Avtonomni mobilni roboti morajo biti sposobni načrtovanja optimalne poti od točke A do točke B. Pri imple-mentačiji načrtovanja poti so bila upoštevana določena čestno-prometna pravila. Mobilni robot se lahko vozi le po desnem pasu česte, vožnja po zeleniči ni dovoljena. Pri vožnji mora upoštevati tudi stop znake, na križiščih pa so mu dovoljeni U-zavoji. A. Modeliranje okolja Da lahko mobilni robot najde optimalno pot, se mora zavedati okolja v katerem se nahaja. Okolje smo z mrežo razdelili na 14 x 12 čelič (slika 3) in vsaki čeliči smo i k L 2 132 Slika 3: Razdelitev poligona na celice z označenimi koordinatnimi sistemi in dimenzijami priredili unikatno oznako v obliki dveh števil (i, j). Preslikavo med položajem v globalnem koordinatnem sistemu in oznako pripadajoče celice podaja (13), kjer operator [ J predstavlja celoštevilsko zaokroževanje navzdol. i = 0,15 j = y 0,15 (13) Obratna transformacija (14) priredi vsaki celici položaj v globalnem koordinatnem sistemu, ki se nahaja v središcu te celice. x = 0,15i + 0,075 , y = 0,15j + 0,075 (14) Slika 3 ilustrira primer transformacije za eno celico. B. Algoritem iskanja poti Cilj algoritma za iskanje poti je najti optimalno pot od starta do cilja. Pri tem vhod v algoritem predstavljata tocki start (zacetna lega robota) in cilj (želena lega robota). Izhod algoritma predstavlja urejen niz celic, ki sestavljajo pot od start do cilja. Optimalna pot ni nujno najkrajša pot. V naši implementaciji, algoritem posebej upošteva stop celice. Vsaka stop celica vzame mobilnemu robotu vec casa kot pa ostale celice, saj se mora mobilni robot pred stop znakom ustaviti. Posledicno imajo takšne celice manjši potencial, da se znajdejo na optimalni poti. 1) Algoritem A* V našem sistemu smo implementirali algoritem A* [3], ki je zaradi svoje optimalnosti in hitrosti izracuna, pogosta izbira za iskanje optimalne poti. Z našo implementacijo algoritma smo dosegli zadovoljivo hitro izvajanje algoritma, zato nismo iskali alternativnih algoritmov. A* je informiran algoritem iskanja poti, ki pri svojem delovanju uporablja hevristicno oceno cene do cilja. S tem se iskanje po vozlišcih omeji na celice, ki bolj verjetno sestavljajo optimalno pot, kar posledicno poveca hitrost izracuna. V algoritmu nastopa cena F, ki doloca vrstni red odkrivanja celic in je definirana kot vsota dveh cen: F = G + H. G predstavlja ceno poti, ki vodi od zacetne do trenutno izbrane celice. Dolocimo jo na podlagi vsote uteženih prehodov med celicami. V našem primeru smo uporabili razlicne uteži za prehode cez stop znake (utež 2) kot za obicajne prehode med celicami (utež 1). Tako postanejo celice s stop znakom slabši kandidati za nadaljevanje poti, saj se mora tam mobilni robot ustaviti. H podaja hevristicno oceno razdalje od trenutne celice do cilja. V našem primeru smo uporabili Manhattansko razdaljo. Inicializacija Odprtega ¡n Zaprtega seznama. Zaprti seznam je prazen, na odprtem je začetno vozlišče Izberi vozlišče z najmanjšo vrednostjo F Prestavi aktivno vozlišče na zaprti seznam Poišči vse možne sosede aktivnega vozlišča /^Izračunaj ceno novega^ vozlišča in mu dodeli starša Dodaj novo vozlišče na odprti seznam Ce je nova cena F manjša, posodobi seznam rt* ie nova cena F manjša. prestavi vozlišče ponovno na V._odprti seznam_> 3 Slika 4: Diagram izvajanja algoritma A* Koraki algoritma A* so razvidni na sliki 4. V inicia-lizaciji algoritma definiramo prazen odprti in zaprti seznam ter uvrstimo zacetno celico na odprti seznam. Nato vstopimo v neskoncno zanko, kjer v vsakem ciklu izvedemo naslednje korake. Iz odprtega seznama odstranimo celico, ki ima najmanjšo vrednost F. Nato poišcemo vse njene sosednje celice in jih uvrstimo na odprti seznam Za vsako novo odkrito celico izracunamo vrednosti G, H in F ter ji dodelimo starševsko (trenutno) celico. En cikel zanke koncamo tako, da uvrstimo trenutno celico na zaprti seznam. Ko po tem postopku pridemo do ciljne celice, lahko algoritem prekinemo, saj to pomeni, da obstaja pot do cilja. Optimalno pot poišcemo tako, da sledimo starševskim celicam, dokler ne prispemo do zacetne celice. Izvajanje algoritma zakljucimo tudi, ce je odprti seznam prazen. To pomeni, da pot do želenega cilja ne obstaja. x 133 V. Vodenje mobilnega robota po poti Lego mobilnega robota glede na podane vhodne hitrosti opisuje direktna kinematika (1). Pri načrtovanju vodenja po poti se ukvarjamo s problemom izračunavanja tangencialne in kotne hitrosti, ki sta potrebni, da se mobilni robot vozi po želeni poti. Sistem (1) ima neholonomično omejitev, zato je izračun invezne kinematike v splošnem zahtevna naloga. Preprosta rešitev inverzne kinematike obstaja za primere, ko se mobilni robot vozi le naravnost in obrača na mestu, a takšno gibanje robota običajno ni optimalno. V nadaljevanju je predstavljen prinčip vodenja mobilnega robota po referenčni poti, ki smo jo dobili s postopkom načrtovanja poti, predstavljenim v poglavju IV-A. A. Poenostavljen zapis poti Mobilni robot dobi navodila za pot v obliki urejenega seznama čelič, ki jih mora obiskati, da doseže čilj. Ali so vse čeliče potrebne za popoln opis poti, ki jo želimo opraviti? Sosednje čeliče v urejenem seznamu čelič nimajo dodane vrednosti, če le-te predstavljajo ravno pot. Takšne čeliče lahko s seznama odstranimo. Obdržati moramo torej le začetno in končno čeličo ter čeliče, kjer pride do spremembe smeri. Poleg tega moramo na seznamu obdržati še čeliče s stop znakom, saj se moramo na teh mestih ustaviti. Za zaznavanje čelič, kjer pride do spremembe smeri poti, smo uporabili naslednji algoritem. Sprehodimo se skozi urejen seznam čelič, ki predstavljajo pot. Za vsako čeličo izvedemo preprost test, tako da primerjamo oznako trenutne čeliče (oznaka čeliče je definirana v poglavju IV-A) z oznako čeliče s seznama, kije oddaljena za dve mesti. Ce se obe vrednosti oznake razlikujeta, pomeni, da je na tem mestu zavoj, zato vse čeliče, ki tvorijo zavoj, obdržimo na seznamu. Primer poenostavitve poti je prikazan na sliki 5. (a) (b) Slika 5: (a) Pot iz čeliče (0, 0) v čeličo (4, 5) in (b) poenostavljena pot B. Algoritem za vodenje po poti Oglejmo si algoritem za vodenje do točke. Definiramo pogrešek po razdalji (evklidsko mera) in kotu (slika 6): ed(k) = \J(xref - x(k))2 + (yref - y(k))2 , (15) e^(k) = arctan Vref - y(k) ^(k) + (n + 2m)n. Xref - x(k) Pri izračunu pogreška po kotu moramo s pravilno izbiro parametra n e {0,1} zagotoviti, da dobimo kot rezultat Slika 6: Prikaz vodenja do referenčne točke funkčije arctan kot, ki je v pravilnem kvadrantu. S parametrom m e Z pa moramo zagotoviti, da je zaloga vrednosti vedno znotraj območja e^(k) e [—n, n). Za izračun translatorne hitrosti v(k) uporabimo enostaven P-regulator z ojačenjem Kv: '(k) = Kved(k)cos2 e^(k); |ejk)| < f A ed(k) > £ 0 sicer. (16) Razdalja £ je radij, ki predstavlja krog okoli referenčne točke, ki ga smatramo za čilj. Za izračun kotne hitrosti w(k) uporabimo naslednji regulator z ojačenjem Kw: w(k) Ke^(k); ed(k) > £ 0 sicer. (17) Ojacanje P-regulatorja predstavljenega v (16) je 1. Oja-Canje regulatorja v (17) pa se spreminja, glede na oddaljenost od toCke, v katero se robot pelje. Tako zagotovimo veCjo stabilnost regulatorja. Vrednosti ojaCanja sta bili določeni eksperimentalno in znašata 1,65 , ko je robot blizu cilja, in 70 , ko je daleC od njega. Vodenje po poti lahko izvedemo s prilagoditvijo algoritma za vodenje mobilnega robota do toCke tako, da izvedemo primerno preklapljanje med toCkami, ki sestavljajo pot. Ko je vrednost pogreška ed(k) manjša od £, za referenCno lego vzamemo naslednjo toCko s seznama, ki predstavlja referenCno pot. Ce referenCna toCka pripada stop celici, se na njej ustavimo za doloCen Cas in nato nadaljujemo z izvajanjem algoritma. VI. Zaključek Razvoj algoritmov za avtonomno vožnjo je zahtevna naloga. FiziCni model mesta z avtonomnimi mobilnimi sistemi nam je omogoCil enostavno implementacijo in študijo osnovnih algoritmov, ki so potrebni za avtonomno vožnjo. Tekom implementacije smo se sreCali s številnimi izzivi, ki nastopajo pri prenosu teorije v prakso. V prihodnosti je priCakovati, da se bo podroCje avtonomnih vozil vedno bolj razvijalo. Literatura [1] G. KlanCar et al., Wheeled Mobile Robotics: From Fundamentals Towards Autonomous Systems, Elsevier, 2017. [2] G. KlanCar, L. Teslic 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. [3] Z. Zhang in Z. Zhao, "A Multiple Mobile Robots Path planning Algorithm Based on A-star and Dijkstra Algorithm", International Journal of Smart Home, zv. 8, št. 3, 75-86, 2014. 134