Szakdolgozat. Miskolci Egyetem. Vezető nélküli autó irányítási algoritmusának fejlesztése

Átírás

1 Szakdolgozat Miskolci Egyetem Vezető nélküli autó irányítási algoritmusának fejlesztése Készítette: Pető Albert Programtervező informatikus alapszak Témavezetõ: Dr. Házy Attila Miskolc, 2014

2 Miskolci Egyetem Gépészmérnöki és Informatikai Kar Alkalmazott Matematikai Tanszék Szám: Szakdolgozat Feladat Pető Albert (GXG195) programtervező informatikus jelölt részére. A szakdolgozat tárgyköre: dinamikus rendszerek, visszacsatolt rendszerek, szenzorfúzió, beágyazott programozás A szakdolgozat címe: Vezető nélküli autó irányítási algoritmusának fejlesztése A feladat részletezése: A hallgató feladata olyan algoritmust fejleszteni, amely egy modellautó navigációs feladatait emberi beavatkozás nélkül képes elvégezni. Az algoritmus az autóra szerelt inerciális és egyéb szenzorok jeleit felhasználva az előzetesen definiált útvonalat automatikusan képes követni. A feladat magába foglalja a témához szükséges irodalomkutatást, modellfejlesztést, szimulálást, optimalizálást, és végül a hardverre történő implementációt és az azzal járó finomhangolást és tesztelést. Témavezető: Dr. Házy Attila, egyetemi docens Konzulens: Árvai László, tudományos munkatárs, BAY-IKTI A feladat kiadásának ideje: szakfelelős

3 Eredetiségi Nyilatkozat Alulírott ; Neptun-kód: a Miskolci Egyetem Gépészmérnöki és Informatikai Karának végzős szakos hallgatója ezennel büntetőjogi és fegyelmi felelősségem tudatában nyilatkozom és aláírásommal igazolom, hogy című szakdolgozatom/diplomatervem saját, önálló munkám; az abban hivatkozott szakirodalom felhasználása a forráskezelés szabályai szerint történt. Tudomásul veszem, hogy szakdolgozat esetén plágiumnak számít: szószerinti idézet közlése idézőjel és hivatkozás megjelölése nélkül; tartalmi idézet hivatkozás megjelölése nélkül; más publikált gondolatainak saját gondolatként való feltüntetése. Alulírott kijelentem, hogy a plágium fogalmát megismertem, és tudomásul veszem, hogy plágium esetén szakdolgozatom visszautasításra kerül. Miskolc, év hó nap Hallgató

4 1. A szakdolgozat feladat módosítása szükséges (módosítás külön lapon) nem szükséges dátum témavezető(k) 2. A feladat kidolgozását ellenőriztem: témavezető (dátum, aláírás): konzulens (dátum, aláírás): A szakdolgozat beadható: dátum témavezető(k) 4. A szakdolgozat szövegoldalt program protokollt (listát, felhasználói leírást) elektronikus adathordozót (részletezve) egyéb mellékletet (részletezve) tartalmaz dátum témavezető(k) 5. bocsátható A szakdolgozat bírálatra nem bocsátható A bíráló neve: dátum szakfelelős 6. A szakdolgozat osztályzata a témavezető javaslata: a bíráló javaslata: a szakdolgozat végleges eredménye: Miskolc, a Záróvizsga Bizottság Elnöke

5 Tartalomjegyzék 1. Bevezetés 6 2. Állapotbecslés A motor matematikai modellje A Kálmán-szűrő Szenzor modell A vezérlés Az útvonaltartás rétegei Vezérlési irányelvek Útvonaltartás egyenes szakaszok esetén Útvonaltartás körív alakú pályaszakaszok esetén A vezérlő paraméterek használhatósági spektruma Motor vezérlés Szimuláció A szimulációs eszközökről Szimuláció Scilab-ban A mozgás modellje A motor modellje A vezérlés Szenzorok Zajosítás A Kálmán-szűrő Eredmények Összefoglalás 36 Irodalomjegyzék 37 Adathordozó használati útmutató 40 5

6 1. fejezet Bevezetés Az utóbbi évtizedekben a technológia lehetővé tette számunkra, hogy hétköznapi feladatokból teljes mértékben kiiktassuk az emberi munkát, azt számítógép által végrehajtott algoritmusokkal helyettesítve. Ezzel számtalan esetben sikerült szélesebb körű felhasználást és jobb teljesítményt, pontosabb működést elérni. A civilek számára is elérhető és megfizethető távirányítású járműmodellek, mikrokontrollerek és alkatrészek megjelenése újabb szakértőket vonzott be és felgyorsította azt a kutatási területet, melynek általános célja mindenféle járművet autonóm, minimális emberi beavatkozást igénylő vezérléssel felszerelni. Különösen népszerű és a szakirodalomban kedvelt téma a pilóta nélküli repülők (UAV 1 ) és vezető nélküli földi járművek (UGV 2 ) navigációs feladatainak automatikus ellátása. [4] [7] [9] Bár létezik egy általános séma, amelyre a járművek vezérlő algoritmusai épülnek, általában a jármű használati céljai és a rendelkezésre álló eszközök komoly korlátokat és speciális követelményeket szabnak ki ezekre. A navigációs algoritmusok két meghatározó komponense minden esetben az állapotbecslés és az irányítás. A legelterjedtebb módszer az állapotnyilvántartásra a GPS használata, amellyel az algoritmus a pozíció és sebesség viszonylag pontos tudatában lehet. A GPS használat hátrányait (pontatlanság, alacsony működési frekvencia, vétel nélküli időszakok) újabb, az autóra szerelt szenzorok bevezetésével próbálják minimalizálni. Típikus szenzorok például az inerciális szenzorok (giroszkóp és gyorsulásmérő), magnetométer, vagy a kamera, melyek rövid időn belül kisebb hibákat eredményeznek, mint a GPS, az ezek alapján dead reckoning 3 módon számolt pozició hibája azonban felülről nem korlátozott. Az autók vezérlési algoritmusai általában a pályatartáson kívül magukban foglalják az útvonal megtalálását és kiválasztását is, illetve olyan speciális eseteket, mint az egyik sávból a másikba való váltás, vagy az akadályok kikerülése. Mint látni fogjuk, a feladat kritériumainak megfelelő vezérlésnek ennél kevesebb feladatot kell majd ellátnia, mivel az útvonal előre adott lesz, és a feltételezések szerint nem lesznek rajta akadályok. A vezérlés fő feladata a pályatartás lesz. Ebben a szakdolgozatban egy átalakított távirányítós autó navigációs algoritmusának tervezését és tesztelését tárgyalom. Az 1.1a. ábrán ez az autó látható. Az autó 1 UAV jelentése: Unmanned Aerial Vehicle 2 UGV jelentése: Unmanned Ground Vehicle 3 A dead reckoning olyan módszereket jelöl, amelyek a pozíció (vagy általában bármilyen más állapot) meghatározását egy időpontban a korábbi időpontban meghatározott adatok alapján végzik. Általában a dead reckoning használata hosszú időn belül nagy hibákat eredményez, mivel az apró hibák összeadódnak. 6

7 (a) A modellautó (b) A modellautó és repülő 1.1. ábra. Az autó egy nagyobb projekt keretében készül, amelyben mozgó leszállóplatformként fog szolgálni egy hasonló méretű UAV számára. egy nagyobb projekt részeként készül, melynek célja egy olyan UAV készítése, amely többek között képes lesz mozgó járműveken landolni. Az autó tehát a mozgó platform szerepét játsza majd, és az algoritmussal szembeni követelmény, hogy az autó az adott útvonalon pontosan, a kitérítő hatások kompenzálásával végig tudjon haladni. Ehhez a feladathoz a GPS önmagában nem elég, mivel az általa becsült adatokban akár méteres eltérések is lehetnek, illetve az adatok frissítési frekvenciája is túl alacsony a precíz útvonaltartáshoz. Az algoritmusnak inerciális szernzorok használatával kell ezt a feladatot megvalósítania. A működés során feltételezzük, hogy az autó útjában nem állnak akadályok. Az akadálymentes pálya és a síkbeli mozgás feltételezése nyilvánvalóan korlátozza az eltervezett algoritmus használhatóságát, de már ez a probléma önmagában érdekes kihívásokat rejt, és betekintést nyerhetünk vele a teljes feladat jellegzetességeibe. Az itt elkészített navigációs algoritmus nem használ GPS-t, ám pusztán dead reckoning típusú számításokat alkalmazva a pozicionális hibát nem lehet felülről korlátozni, tehát a valódi alkalmazásban hosszútávon szükséges lesz GPS adatokat is használni. A dolgozatot három lényeges fejezetre osztottam: a 2. fejezet foglalkozik az állapotbecslés kérdéseivel, a 3. fejezet az autó pontos útvonaltartásához szükséges irányítás elméleti levezetését tárgyalja, végül a 4. fejezet az előző két fejezetben levezetett elméleti eredmények használhatóságát vizsgálja egy alkalmasan kialakított szimulációs környezetben. A dolgozatkiírás tartalmazza az algoritmus hardverre történő implementációját és vizsgálatát is említi. Ezt nem végeztem el idő hiánya miatt, viszont a szimulációk során az algoritmus használhatónak bizonyult, és elkészítettem az algoritmus beágyazott eszközökön is alkalmazható változatát C programozási nyelven, ezzel előkészítve a későbbi éles tesztelést. 7

8 2. fejezet Állapotbecslés A vezérlő algoritmus az autó pozícióját az egyenáramú motor feszültsége és a kormányszög befolyásolásával változtathatja, amik ismeretében az autó pozíciójára vonatkozólag is következtetni tudunk. Ideális helyzetben, ha minden környezeti hatást és az autó paramétereit beleértve a motor és a kormány paramétereit tökéletesen ismernénk, a vezérlő kimenete alapján pontosan meg tudnánk határozni az autó helyzetét. A környezeti hatások azonban túl bonyolultak a modellbe való beépítéshez, és az autó paramétereit is csak bizonyos mértékű pontossággal ismerjük, így az autó állapotára vonatkozva csak becsléseket tudnánk adni. Ez a becslés pedig nem csak hogy hibás, de eltérése a valódi állapottól nagy valószínűséggel csak növekedni fog, mivel az újabb becsléseket a korábbiakból származtatjuk. Ez a dead reckoning egy típikus hibája. A dead reckoning hibáinak kiküszöbölésére a helymeghatározásban általánban GPSt használnak, amellyel a hibára felső korlátot lehet adni, viszont bármelyik pillanatban akár méteres nagyságrendű hibákat is tartalmazhatnak a becsült állapotok. [7] [19] [23] Az ebben a dolgozatban meghatározott navigációs algoritmus nem fog GPS-t használni, hanem az autóra helyezett inerciális szenzorokat használja majd a számításokhoz. Mivel ezek adatait származtatni lehet az autó állapotából (síkbeli helyzet, sebsség, gyorsulás, beállítottság, kormányszög), valamilyen módon fel is használhatók az autó mozgásegyenleteiből levezetett becslések ellenőrzésére, korrigálására. Ezt a módszert amelyben több szenzort használunk ugyanannak az állapotnak a meghatározására szenzorfúziónak nevezik. A szenzorfúziós eljárás menetét szemlélteti a 3.1. ábra. Ezzel a technikával tehát javítani lehet a becsléseket, de még mindig dead reckoning módon számolunk, mivel a szenzorok adatai is kis mértékben eltérhetnek a valóságtól. A célom tehát az volt, hogy szenzorfúzió használatával az autó a lehető leghosszabb ideig viszonylag pontosan tudja becsülni az állapotát, és csak egy elfogadható időn belül váljanak a becslések használhatatlanná. Az algoritmus levezetéséhez szükséges az autó egy leegyszerűsített matematikai modelljének ismerete, amely az alapul szolgáló becslések meghatározására fog szolgálni. Természetesen e modell nélkül is elvégezhetjük a szenzor-fúziót, pusztán a szenzorokból következtetve az autó állapotára, ezzel azonban a számításokból kiesik egy viszonylag jó becslés. Az autó matematikai modelljét írják le a egyenletek, melyet a 2.2. ábra szemléltet. Az autó első kerekei közti P (p x, p y ) pont síkbeli koordinátáit, pályaívre érintőleges sebességét és gyorsulását, és az autó Θ beállítottságát és ω szögsebességét szeretnénk nyilvántartani. Ezek ismeretében már becslés készíthető az autó állapotára a következő időpillanatban, feltételezve, hogy az autó minden kereke a talajon gördül. 8

9 2.1. A motor matematikai modellje w u P x K z h v x 2.1. ábra. A szenzorfúzió szemléltetése. Az u bemenet és w zaj által vezérelt P folyamat x állapotát szeretnénk megbecsülni, viszont mi csak u-t ismerjük. Az u ismeretéből származtatott becslésből és x-ből származó, v fehér zajjal szennyezett z mérésekből azonban a K szenzor-fúziós algoritmus elkészítheti az ˆx becslést. Mindezeket a δ kanyarodási szögből és az a gyorsulásból származtathatjuk. dp x dt dp y dt dθ dt dv dt = v cos(δ + Θ) (2.1) = v sin(δ + Θ) (2.2) = ω = v sin(δ) b (2.3) = a (2.4) A kanyarodási szöget közvetlenül befolyásoljuk, így ez ismert, a gyorsulás viszont közvetetten származtatható az egyenáramú motor feszültségéből, ezért szükséges a motor matematikai modelljének bevezetése is A motor matematikai modellje A motor állapotát a 2.5 egyenletrendszerrel jellemezhetjük [6], ahol I az áramerősség, V a feszültség, R a motor belső ellenállása, E az un. back electromotive force, k a motorállandó, ω m a forgórész szögsebessége, τ m a forgórész nyomatéka, τ f pedig a belső súrlódásból származó, ki-vel ellenirányú súrlódási nyomaték. I = V E R E = kω m (2.5) τ m = ki τ f A nyomaték tehát nem csak a feszültségtől, hanem a sebességtől is függ. A motor a hátsó kerekeket hajtja meg. Ezek segítségével kell a B pont a m érintőirányú gyorsulását meghatározni a feszültség és sebesség függvényében. 9

10 2.2. A Kálmán-szűrő y δ d b B P δ v Θ 2.2. ábra. Az autó modellje a síkon. A leegyszerűsített autó a B és P pontok közötti szakasznak felel meg. x Tegyük fel, hogy az autó tömegközéppontja a 2.2. ábrának megfelelő B és P pontokat összekötő szakaszon helyezkedik el, d c távolságra a B ponttól, érintőirányú sebessége és gyorsulása pedig v c és a c : d c v c = v m (1 + b cos(δ) d ) c = γv m (2.6) b d c a c = a m (1 + b cos(δ) d ) c = γa m (2.7) b Az autó kinetikus energiája E = 1 2 mv2 c, amelynek változása, azaz a teljesítmény kétféle módon is felírható : P = v m τ m ω = τ m r (2.8) P = mv c a c. = mγ 2 v m a m (2.9) A két kifejezést egyenlővé téve a következő differenciálegyenletet kapjuk az autó motorjának a m érintőirányú gyorsulására: dv m dt = a m = k2 mγ 2 r 2 R v m + τ f mγ 2 r kv R = αv m + β (2.10) A v = 1 v cos(δ) m és a = 1 a cos(δ) m egyenlőségek alapján kapjuk, hogy a P pont sebessége és érintőirányú gyorsulása dv dt = a = αv + β (2.11) cos(δ) A dolgozat hátralevő részében néhány egyszerűsítéssel éltem a motort illetően: azt feltételeztem, hogy az autó tömegközéppontja a P pont, illetve hogy a motor az első kerekeket hajtja meg. Továbbá a motor maximális teljesítmánye által szabott korlátokat sem vettem figyelmbe. Ezeket a feltételezéseket később, a valódi alkalmazásban el kell majd hagyni, a szimuláció eredményeire viszont nincsenek nagy hatással. 10

11 2.2. A Kálmán-szűrő 2.2. A Kálmán-szűrő A Kálmán-szűrő az egyik legelterjedtebb szenzor-fúziós algoritmus, melynek működése azon alapul, hogy az egyes szenzorok és állapotok bizonytalansági mértékének ismeretében a legkiss ebb négyzetek értelmében optimális állapotbecslést ad[10][11]. Az eredeti algoritmus a 2.12 alakú, lineáris egyenletrendszerrel megadható x k állapotok és z k szenzorok esetén szolgáltat optimális megoldást. x k = Ax k 1 + Bu k 1 + w k 1 z k = Hx k + v k (2.12) Mivel az autót leíró rendszer nemlineáris, ezért a kibővített Kálmán-szűrőt, az EKFet 1 használom az állapotbecslésre, amely nemlineáris rendszerek állapotbecslésére lett kialakítva és az eredeti lineáris szűrőből származtatva. A rendszert a egyenletekkel írjuk le, ahol x k az állapotvektor, z k R n a szenzorok adatait tartalmazó vektor, u k R m a rendszer bemenetét képező vektor (a mi esetünkben a kormányszög és feszültség), w k N(0, Q) és v k N(0, R) pedig normális eloszlású valószínűségi vektorok a k-adik időpillanatban, melyeket a Q és R kovarianciamátrixok jellemeznek. w k tartalmazza mindazt a bizonytalanságot, amely a modell hiányosságaiból fakad, v k pedig a szenzorok adatait zavaró zajt jellemzi. x k = f(x k 1, u k 1, w k 1 ) (2.13) z k = h(x k, v k ) (2.14) Jelöljük az algoritmus által a k-adik időpontban becsült állapotot ˆx k -val, a 2.13 egyenletbe w k 1 = 0,x k 1 = ˆx k 1 behelyettesítéssel kapott becslést pedig x k -val. Legyenek továbbá A és W az f függvény x és w szerinti Jacobi-mátrixai, H és V pedig a h függvény x és v szerinti Jacobi-mátrixai: A [i,j] = f [i] x [j] (ˆx k 1, u k 1, 0) (2.15) W [i,j] = f [i] w [j] (ˆx k 1, u k 1, 0) (2.16) H [i,j] = h [i] x [j] ( x k, 0) (2.17) V [i,j] = h [i] v [j] ( x k, 0) (2.18) Az EKF segítségével az x k állapot ˆx k becslését a egyenletek szerint állítjuk elő. A szűrő az állapotbecslés mellett annak pontosságát is megadja a P k = E(e k e T k ) kovarianciamátrix formájában, ahol E a várható értéket jelöli és e k = x k ˆx k. 1 EKF: Extended Kalman Filter ˆx - k = f(ˆx k 1, u k 1, 0) (2.19) P - k = A k P k 1 A T k + W k Q k 1 W T k (2.20) K k = P - k Hk T (H k P - k Hk T + V k R k Vk T ) 1 (2.21) ˆx k = ˆx - k + K k (z k h(ˆx - k, 0)) (2.22) P k = (I K k H k )P - k (2.23) 11

12 2.2. A Kálmán-szűrő Az előzőek ismeretében már csak a következő kérdésekkel kell foglalkoznunk, hogy a szenzor-fúziós algoritmust meghatározhassuk : a) Mik legyenek az x állapotvektor elemei? b) Mik legyenek a z mérési vektor elemei? c) Hogyan érdemes az f függvényt meghatározni? d) Hogyan érdemes az R és Q kovariancia-mátrixokat meghatározni? Általában a Kálmán-szűrőt és inerciális szenzorokat a szenzorok térbeli beállítottságának meghatározására használják [21] [24] [26]. Mivel én egyelőre az autó mozgását a vízszintes síkra korlátoztam, ezért csak a függőleges tengely menti elfordulás ismeretére lesz szükség a beállítottság szempontjából. Az a) kérdésre választ adva tehát, az állapotvektornak szükségszerűen elemei lesznek az p x és p y koordináták, a nyilvántartott pont v érintőirányú sebessége és a érintőirányú gyorsulása, valamint ω és Θ, a jármű szögsebessége és iránya, azaz x értéke egy minimális szinten ( ) x = p x p y Θ ω v a (2.24) Az állapotvektor ezen felül további elemekkel bővíthető, ezeket a 2.3. alfejezetben részletezem. A b) kérdésre egyszerű választ adni, hiszen akkor érjük el a legjobb teljesítményt, ha minden lehetséges szenzort használunk. A h függvény meghatározásához z mellett x ismeretére lesz szükség. Mivel egyelőre az autó mozgását a vízszintes síkra korlátoztuk, ezért z tartalmazni fogja a giroszkóp autó síkjára merőleges tengelyének ω z méréseit,és a gyorsulásmérő autó síkjával párhuzamos tengelyeinek a x és a y méréseit: z = ( ω z a x a y ) T (2.25) A 2.13 egyenletben szereplő f függvényt a folytonos egyenletek diszkretizálásával nyerjük. A c) kérdésnek a számítási komplexitás szempontjából van jelentőssége. Ha az állapotvektorban a gyorsulást és a szögsebességet a mintavételezési intervallum alatt is változónak vesszük és így diszkretizáljuk a folytonos rendszert, akkor túlságosan nagy számítási igényű algoritmust fogunk kapni. Schubert és mások kimutatták, hogy a rendszer teljesítményét csak elhanyagolhatóan kis mértékben rontja, ha állandó gyorsulás és változó szögsebesség helyett a gyorsulást és szögsebességet is állandónak feltételezzük a mintavételezési periódus során. [4] Feltételezzük azt is, hogy elegendően nagy mintavételezési frekvencia mellett a gyorsulást is állandónak tekinthetjük a mintavételezési intervallumban a teljesítmény jelentős romlása nélkül. Legyenek most ω pk és a pk értékei ω pk = v k 1 b sin(δ k 1) (2.26) a pk = a k 1 + αv k 1 + β (2.27) A 2.24 egyenlettel jellemzett x állapotvektorhoz az imént leírt, egyszerűsített feltételek 12

13 2.3. Szenzor modell felhasználásával konstruált diszkrét f függvény p xk 1 + p x p yk 1 + p y x k = f(x k 1, δ k 1, w k 1 ) = Θ + ω pk T ω, (2.28) pk v k 1 + a pk T a pk és p x = 1 ω pk [(v k 1 + a pk T )sin(θ k 1 + δ k 1 + ω pk T ) v k 1 sin(θ k 1 + δ k 1 ) ], (2.29) + ap k ω pk (cos(θ k 1 + δ k 1 + ω pk T ) cos(θ k 1 + δ k 1 )) p y = 1 ω pk [( v k 1 a pk T )cos(θ k 1 + δ k 1 + ω pk T ) +v k 1 cos(θ k 1 + δ k 1 ) ]. (2.30) + ap k ω nk (sin(θ k 1 + δ k 1 + ω pk T ) sin(θ k 1 + δ k 1 )) Az R kovarianciamátrix meghatározását a szenzorok offline elemzése után elvégezhetjük, úgy, hogy v k -ban minden elem megegyezik valamelyik szenzor kimenetéhez hozzáadódó fehér zaj komponenssel. Ezzel foglalkozik a 2.3 fejezet. A Q kovarianciamátrixnak nehezebb analitikus módon meghatározni az optimális értékét. Viszonylag magas értékek esetén a szenzorok adatai jobban befolyásolják a becsléseket, mint alacsony értékek esetén. Tulajdonképpen a w k vektor mérete sem egyértelmű. Én w k -val azt a kételemű vektort jelöltem, melynek első eleme a kormányszög bizonytalanságát, a második pedig a P pont érintőirányú gyorsulásának bizonytalanságát jellemző fehér zaj komponens. Általában w k -nál és v k -nál is feltételezzük, hogy a vektor elemei korrelálatlanok, azaz a Q és R mátrixok a következőképpen fognak kinézni: σ 2 (w k1 ) σ 2 (w k2 )... 0 Q =. (2.31) σ 2 (w kn ) σ 2 (v k1 ) σ 2 (v k2 )... 0 R =. (2.32) σ 2 (v km ) 2.3. Szenzor modell Az autóban használt giroszkóp és gyorsulásmérő MEMS 2 technológiával készültek. Ezeknek a szenzoroknak a kimenetei a valódi értékektől kis mértékben eltérnek, különböző forrásokból származó hibák miatt. Ezek a hibák lehetnek determinisztikusak, azaz 2 Microelectromechanical Systems 13

14 2.3. Szenzor modell előre meghatározhatók és sztochasztikusak is. Típikus determinisztikus hibák például misalignment error, bias, vagy scale factor error, sztochasztikus hibák pedig a fehér zaj és bias drift. A fehér zaj és bias drift abban különbözik, hogy míg az előbbi minden frekvenciatartományban ugyanakkora mértékben van jelen, addig az utóbbi főleg alacsony frekvenciájú változásokat eredményez. A MEMS technológiával készült inerciális szenzorok kimenetét általában a 2.33 diszkrét egyenlettel jellemezhetünk, ahol x k a valódi fizikai érték, b a determinisztikus bias, n k a fehér zaj komponens, d k pedig a bias drift. [9] [12] [13] [15] [16] [26] o k = m e s e (x k + b + d k + n k ) (2.33) s e a scale factor error, m e pedig a misalignment error, ami azt tükrözi, hogy a szenzor tengelye nem tökéletesen egyezik meg az elméleti tengellyel. Ezek a hibák általában elég kicsik, továbbá számomra nem nyílt lehetőség az efféle kalibrációra, ezért a továbbiakat m e = s e = 1 feltétel mellett folytatom. A maradék három tag, azaz b,d k és n k paramétereinek meghatározásához egy kellőképpen hosszú, a szenzor statikus állapotában készült méréssorozat szükséges. A determinisztikus bias meghatározható offline módon, a méréssorozat átlagolásával, vagy az eszköz bekapcsolása után azonnal, egy kalibrációs időszakban. A Kálmán-szűrőben ez a h függvényben fog megjelenni, a szenzor értékéhez adódó konstans tagként. A fehér zajt modellező n k normális eloszlású valószínűségi változó, melynek szórását az Allan-variance módszerrel határozhatjuk meg. [12] [16] [18] [20] Ennek szórásnégyzete majd az R mátrixban kerül a megfelelő helyre. A szakirodalomban általában számításba veszik a bias driftet is, mivel ennek elhanyagolása, bár kis mértékű, de állandó hibát vinne a rendszerbe, ami dead reckoning számítások esetén jelentős lenne. A bias driftet több fajta módon is szokták modellezni. A szakiradolmban gyakran modellezik a bias driftet elsőfokú Gauss-Markov folyamattal [8] [9] [16] [26], ám többen kimutatták, hogy ez nem képes a folyamat valósághű jellemzésére, és helyette célszerűbb magasabb rendű autoregresszív folyamatokat használni. [2] [12] [15] Egy p-edrendű autoregresszív folyamat általános alakja ɛ k fehér zaj mellett [3] y k = a 1 y k a p y k p + ɛ k. (2.34) A Kálmán-szűrő állapotában nyilvántarthatjuk a folyamat y k,y k 1,...,y k p+1 értékeit, amiket a 2.35 egyenletrendszer kapcsol össze: y k p y k p = + (2.35) y k y k 2 0 y k a p a p 1 a p 2... a 1 Ebben az esetben a szűrő modelljében a w k vektort ki kell egészíteni ɛ k -val, és a Q kovarianciamátrixot is ennek megfelelően bővíteni. A szenzorok esetén a bias drift autoregresszív modelljének paraméterei offline elemzés után meghatározhatók. A paraméterek azonosítására különböző módok léteznek, például az autókorreláció függvény, a Burg-módszer, vagy a legkisebb négyzetek módszere. Az ebben a dolgozatban bemutatott algoritmusba nem építettem be a szenzorok bias drift-jének becslését, később azonban erre is szükség lesz. y k 1 ɛ k 14

15 3. fejezet A vezérlés w aktuális szakasz adatai u C m C a P x x K z h v 3.1. ábra. A navigációs algoritmus felépítése. C m, a magas szintű vezérlő komponens felelős a tervezett pálya ismeretében az aktuális szakasz meghatározásáért és a szakasz adatainak átadásáért az alacsony szintű C a vezérlő komponensnek. C a feladata a kormányszög (δ = u) generálása úgy, hogy az autó ne térjen le az útvonalról. P a jármű dinamikáját leíró rendszer, amelynek a w zajtól torzított kimenetét a K kálmán szűrő becsüli meg a z zaj által torzított szenzoradatok használatával. Ebben a fejezetben az autó mozgását közvetlenül befolyásoló algoritmusokról fogok írni. Ezek feladata a kormányszög és az egyenáramú motor feszültségének meghatározása az autó becsült állapotának ismeretében olyan módon, hogy az autó a feladatkiírásnak megfelelően mozogjon Az útvonaltartás rétegei A kormány vezérlésének feladatát egy alacsony és egy magas szintű vezérlő komponensre osztottam. A magas szintű vezérlés az egész tervezett útvonalat ismeri, egymásba kapcsolódó egyenes vagy körív alakú pályaszakaszok formájában. Ez a komponens fogja 15

16 3.2. Vezérlési irányelvek eldönteni a becsült pozíció ismeretében, hogy az autó melyik pályaszakaszt teljesítse. A kiválasztott pályaszakaszra vonatkozó információkat átadja az alacsony szintű visszacsatolt vezérlő komponensnek, melynek feladata a szakasz minél pontosabb betartása az autó környezetéből származó, az autót a pályáról kitérítő hatásokkal szemben. Tulajdonképpen a magasrendű vezérlés különböző pályaszakaszokhoz és sebsségekhez különböző paraméterezésű alacsonyrendű vezérlőket fog rendelni, ahogy azt később olvashatjuk Vezérlési irányelvek A fejezet hátrelevő részében az előbb említett alacsony szintű visszacsatolt vezérlésről fogok írni. A dinamikus rendszerekhez és visszacsatolt vezérléshez szükséges alapfogalmak, illetve az ebben az alfejezetben leírt kritériumok Murray és Åström munkájából származnak. [1] A feladathoz minden esetben proporcionális vezérlőket határoztam meg. Az iparban gyakran használt, integrált és derivált tagot is tartalmazó PI, PD, vagy PID vezérlők az autó irányításánál ugyanis nem használhatóak sikeresen. Láthatjuk majd a (3.7) és (3.14) egyenletekből, hogy az autó modelljének P (s) transzfer függvénye s második hatványát tartalmazza a nevezőben, azaz alacsony frekvenciáknál a zavaró hatásokkal szemben ellenálló az autó, tehát nem szükséges az integrált tag használata. Másrészt az autó elfordulási szöge egyébként is folyamatosan csökken vagy növekszik egy konstans, nem nulla kormányszögnél, így ezért sincs szükség integrált tagra. A derivált tag, melynek transzfer függvénye C d (s) = k d s, a Kálmán-szűrőből származó magas frekvenciájú zajjal szennyezné a vezérlő kimenetét, így azt ezért nem fogom használni. A következő jelölésmódot használtam: P (s) az autó mozgásegyenleteiből levezetett modell transzferfüggvénye, melynek bemenete δ, a kormányszög, kimenete pedig y, az autó állapotából származtatott valamilyen skaláris érték. A vezérlő transzfer függvénye C(s), melynek bemenete e = r (y + n), azaz P kimenete és a hozzáadódó mérési zaj, n, és az r refereinciaérték különbsége (mi r = 0-val fogunk dolgozni). C kimenete pedig u, a vezérelt kormányszög, ami az út egyenetlenségeit leíró d zavaró hatásokkal együtt alkotja P bemenetét, azaz δ = u + d. A vezérlő feladata az lesz, hogy a closed loop rendszer y kimenetét a referencia érték körül tartsa. A vezérlők paramétereinek meghatározásánál a következő kritériumokra kell figyelni: a) Stabilitás a nyquist-kritérium alapján és jó stabilitási határok, azaz jó gain margin 1 és phase margin 2. b) A járművet az út szabálytalan felülete kitérítheti a tervezett útvonalából. Ezeket a zavaró hatásokat (load disturbance) a vezérlő u kimenetéhez hozzáadódó változó d értékkel jellemeztem. A closed loop rendszer y kimenetét d-ből származtató transzfer függvény P G yd = 1 + P C. (3.1) 1 gain margin: egy stabil rendszer esetén a legnagyobb érték, amellyel az amplitúdókarakteriksztikát megszorozva a rendszer még stabil marad. 2 phase margin: egy stabil rendszer esetén az az eltolás a fáziskarakterisztikában (phase lag), amely mellett a rendszer már instabillá válik. 16

17 3.3. Útvonaltartás egyenes szakaszok esetén Mivel ezek a zavaró hatások viszonylag alacsony frekvenciájúak, ezért a vezérlő hatásfoka akkor lesz jó, ha G yd értéke alacsony frekvenciáknál alacsony, azaz ha az open loop transzfer függvény, L(s) = P (s)c(s) értéke alacsony frekvenciáknál magas lesz. c) A vezérlő a jármű pontos állapotát nem ismerheti, csak a Kálmán-szűrő által nyújtott becslésekre támaszkodhat. Ezeknek a becsléseknek a tökéletlenségét a zárt rendszer y kimenetéhez hozzáadódó n értékkel jellemezhetjük (measurement noise). A vezérlőtől elvárjuk, hogy amennyire csak lehet, érzéketlen legyen ezekre a becslési pontatlanságokra, azaz G un = C P C 1 + P C = 1+P C P, (3.2) a vezérlő kimenete és a zaj közti kapcsolatot leíró transzfer függvény értéke kicsi legyen. Mivel a zaj általában magas frekvenciákban jelenik meg, ezért G un -nek magas frekvenciák esetén kell alacsony értékkel rendelkeznie. Ezt úgy is megfogalmazhatjuk,hogy az open loop transzfer függvénynek, L(s) = P (s)c(s)-nek magas frekvenciák esetén alacsony értékekkel kell rendelkeznie. d) A vezérlő nem fog 100 Hz-nél magasabb frekvenciával működni, továbbá a kormányszög változásának mértéke is korlátozott, így lehetséges, hogy a következő, folytonos vezérlést feltételező számítások alapján stabil rendszer a valóságban nem lesz stabil. A következő két alfejezet az autó alacsony szintű útvonaltartási algoritmusait tárgyalja. Ezeknél állandó sebességet feltételezünk. Mivel az autó mozgását leíró egyenletek nemlineárisak, először ezeket linearizáljuk és a (3.3) formára hozzuk őket azzal a szándékkal, hogy a vezérlés analitikus elemzését is elvégezhessük a frekvenciatartományban. Nyilvánvalóan az így kapott rendszer viselkedése eltér az eredeti rendszertől, viszont egyensúlyi pontok körül linearizálva a két rendszer tulajdonságai azonosak lesznek az egyensúlyi pontok közelében. dx dt = Ax + Bu y = Cx + Du (3.3) Mivel a visszacsatolt vezérlésnek a rendszer kimenetét egy állandó referenciaérték körül kell tartania, ezért az egyenes és görbe útszakaszokon más-más modellt kell használnunk, hogy a görbület vagy az egyenes mentén való haladást egy állandó értékkel tudjuk jellemezni Útvonaltartás egyenes szakaszok esetén Az autó mozgását a vízszintes síkon a (3.4) egyenletrendszerrel írhatjuk le. Az egyenletrendszer az autó első kerekeit összekötő egyenes közzéppontjának (p x, p y ) koordinátáit és az autó hosszanti tengelyének a Θ-val jelzett irányát írja le δ kormányzási szög mellett, b = 0.18 m értéke a leegyszerűsített autómodell első és hátsó keréktengelyei közti távolság, v értéke pedig pedig a nyilvántartott pont sebességének nagysága. A rendszert szemlélteti a 3.2. ábra. 17

18 3.3. Útvonaltartás egyenes szakaszok esetén y δ d b B P δ v Θ 3.2. ábra. Az autó modellje a síkon. A leegyszerűsített autó a B és P pontok közötti szakasznak felel meg. Az (3.4) egyenletrendszer állapotvektorának elemei a P pont koordinátái és Θ. x f(p x, p y, Θ) = dp x dt = v cos(δ + Θ) dp y dt = v sin(δ + Θ) dθ = v = v sin(θ) dt d b (3.4) A linearizáláshoz, azaz a (3.4) rendszer (3.3) rendszerrel való közelítéshez a Jacobimódszert használjuk. A linarizálását csak az egyensúlyi pontok körül végezhetjük, azaz nem képezhetünk olyan linearizációt, amelyben a p x és p y állapotokat is nyilvántarthatnánk. Képezhetünk viszont olyan változatot, amelyben csak az egyik koordinátát és az autó Θ elfordulását tartjuk nyilván, ugyanis az algoritmus feladata csupán a pálya megtartása, és ha az eltervezett útvonal egy-egy egyenes szakaszainál a megfelelő koordinátatranszformációt elvégezzük, akkor a pályatartás szempontjából elég csak azt a koordinátát nyilvántartani, amelyik a pályára merőleges bázisvektorhoz tartozik. [1] Tegyük fel tehát, hogy a jármű a koordinátarendszer x tengelye mentén mozog és Θ = 0. (ha nem az x tengely mentén mozog vagy Θ = 180, akkor pedig forgassuk el a rendszert úgy, hogy az állítás igaz legyen) Ebben az esetben p ye = 0, Θ e = 0, és u e = δ e = 0 megfelelnek az egyensúlyi feltételeknek, így a következőt kapjuk : ahol v b dz dt [ ] [ ] [ ] 0 v v py p ye A =, B =, z = = 0 0 Θ Θ e A rendszer kimenete legyen y = = Az + Bu, (3.5) [ py Θ ], u = δ δ e = δ. [k y k Θ ] x, (3.6) azaz az állapotvektor két elemének súlyozott összege. Tulajdonképpen, mivel proporcionális vezérlőt tervezek, a k y és k Θ paraméterek megválasztásával már a vezérlő paramétereit is megválaszthatjuk, ezért tervezhetjük úgy a rendszert, hogy C(s) = 1. 18

19 3.3. Útvonaltartás egyenes szakaszok esetén (a) k y = 2, k Θ = 0 (b) k y = 2, k Θ = ábra. Az autó útjának szimulálása az egyenestől 0.2 méterre lévő kezdőállapotból kiindulva, két különböző vezérlő paraméterezéssel. Az első oszlop ahhoz a vezérlőhöz tartozik, amely nem veszi figyelembe az egyenessel bezárt szöget. Az alsó sor a kormányzási szöget mutatja az idő függvényében, a felső sor pedig az autó távolságát az egyenestől. 1 (a) Bode-diagram (b) Nyquist-diagram 3.4. ábra. Open loop bode-diagram (v = 2 m/s, k y = 2, k Θ = 2) és nyquistdiagram egyenes szakaszhoz készült vezérlő esetén. 19

20 3.4. Útvonaltartás körív alakú pályaszakaszok esetén A k y és k Θ paraméterek ellentétes módon befolyásolják az autó mozgását. k y hatására ugyanis az autó, amíg nem érte el az egyensúlyi pontot, egyre nagyobb szöget zár be az útvonalhoz tartozó egyenessel, ennek hatására pedig az egyensúlyi pont elérése után azonnal átlép az egyenes útvonal másik oldalára. k Θ hatása a PID rendszerkben a derivált taghoz hasonlít. Ez a tag nem az autó távolságát fogja az egyensúlyi pont körül tartani, hanem az autó egyenessel bezárt szögét, azaz Θ-t. k y és k Θ optimális megválasztása esetén az autó pályája az útvonaltól való eltérés után szépen fog ráilleszkedni az előre megadott egyenes pályaszakaszra. Különböző paraméterek esetén látható összehasonlítás a 3.3 ábrán. Az ábrán látható szimulációkat a 4. fejezetben bemutatott eszközökkel végeztem. A rendszer transzfer függvénye P (s) = C(sI A) 1 B = v k y(bs + v) + k Θ s bs 2. (3.7) A C(s) = 1 feltétel mellett az open loop transzfer függvény, illetve az open loop amplitúdókarakterisztika és fáziskarakterisztika pedig a következők L(s) = P (s)c(s) = v k y(bs + v) + k Θ s. (3.8) bs 2 L(iω) = v (k bω 2 y b + k Θ ) 2 ω 2 + kyv 2 2 (3.9) ( ) L(iω) = tan 1 ky b + k Θ ω π. (3.10) k y v L(s) Nyquist-diagramját és az open loop Bode-diagramot mutatja a 3.4 ábra. L(s) tulajdonságait vizsgálva megállapíthatjuk, hogy a closed loop rendszer akármilyen k y > 0 és k Θ > 0 érték mellett stabil, és mindig jó gain margin-nal rendelkezik. Ezt szemlélteti a 3.4b ábra. Láthatjuk azt is, hogy az amplitúdókarakterisztikára alacsony frekvenciáknál k y nagyobb hatással van, mint k Θ, a fáziskarakterisztika pedig k Θ növelése esetén jobbra, k y növelése esetén pedig balra tolódik, azaz megválaszthatunk olyan k y és k Θ értékekeket, hogy a rendszernek jó phase margin-ja legyen. A hangolás során ezeket a tulajdonságokat kell majd figyelembe venni, illetve a kapott vezérlő teljesítményére ez alapján következtethetünk Útvonaltartás körív alakú pályaszakaszok esetén Láthattuk az előző alfejezetben, hogy a jármű (3.4) modelljét linearizálva csak egyenes vonalú pályaszakaszokra tudtunk útvonaltartó vezérlést készíteni. Bár az útvonalat össze lehet állítani egyenes szakaszokból, ez a vezérlés önmagában mégsem elég, mivel irányváltoztatásnál mindenképpen kanyarodni kéne, és ezt a helyzetet nem jellemzi megfelelően az előbbi linearizált változat. Olyan rendszert kell tehát találni, amelynek egyensúlyi pontja lehet az az állapot, amelynél a jármű egy adott körív mentén mozog. Ilyen rendszer a (3.11) egyenletrendszer, amelyet a 3.5. ábra szemléltet. Tehát egy meghatározott körív esetén nyilvántartjuk a leegyszerűsített autó elejének d távolságát és hátuljának r távolságát a körívhez tartozó kör O középpontjától. δ a kormányzási szög, b = 0.18 m a leegyszerűsített autó hossza, v pedig a leegyszerűsített autó elejének sebességének nagysága. 20

21 3.4. Útvonaltartás körív alakú pályaszakaszok esetén y O r d ψ b β δ B P v 3.5. ábra. A kanyarodó autó modellje a síkon. A leegyszerűsített autó a B és P pontok közötti szakasznak felel meg. Az (3.11) egyenletrendszer állapotvektorának elemei a B és P pontok távolsága az O ponttól, r és d. x dd dt dr dt = v cos(π β δ) = v cos(δ) cos(ψ) ( ) d β = cos b 2 r 2 2 d b ( ) r ψ = cos b 2 d 2 2 r b (3.11) A rendszeren az előző, 3.3. alfejezetben leírtakhoz hasonlóan most is a Jacobilinearizálást alkalmazzuk a d e,r e és δ e egyensúlyi pontok körül, amelyek az adott köríven hibátlanul végighaladó autó állapotát és kormányszögét határozzák. A linearizálás eredménye ahol A = [ v r d b v b v b v r d b dz dt = Az + Bu, (3.12) ] [ ] [ ] v d de, B =, z =, u = δ δ e. 0 r r e Mivel proporcionális vezérlést használok, ebben az esetben is a kimenet megválasztása fogja eldönteni az autó mozgásának főbb tulajdonságait. Ha csupán a két állapot, d és r valamilyen összegét tartalmazná az y kimenet, akkor az egyensúlyi állapot eléréséig a kormányszög végig pozitív vagy végig negatív lenne, ami az autó irányának tartós 21

22 3.4. Útvonaltartás körív alakú pályaszakaszok esetén eltérését eredményezni a kanyar teljesítéséhez szükséges beállítottságtól. Érdemes tehát olyan kimenetet (azaz a (3.3) egyenletrendszer jelölésével élve olyan C mátrixot) választani a rendszernek, amely nem csak az útvonalszegmenstől való távolságot, hanem az autó útvonalszegmenshez képesti beállítottságát is tükrözi, hogy az egyensúlyi ponthoz közeledve az elfordulás egyre kevésbé térjen el az útvonal teljesítéséhez szükséges elfordulástól. A ψ szög megfelel a kanyarhoz tartozó érintővel bezért szög nyilvántartására, ψ-t azonban lineárisan nem lehet kinyerni az állapotokból. Használhatjuk a d r kifejezést, ami az egyensúlyi pont közelében jól írja le az autó elfordulását a kanyarhoz képest: Vizsgáljuk a d r < d e r e egyenlőtlenséget, ahol d e és r e a kanyar tökéletes teljesítése esetén fennálló egyensúlyi értékek. Ha az egyenlőtlenség teljesül, akkor negatív kormányszöget kell használni, hogy az autó visszatérjen az egyensúlyi állapotba. Ha az egyenlőtlenség nem teljesül és egyenlőség sem áll fenn, akkor pozitív kormányszöget kell használni. Tehát hasonló vezérlést valósítotunk meg, mint az előző alfejezetben, azaz az autó távolságát és elfordulását is felhasználjuk a kormányszög generálásához. A vezlérlő transzfer függvényét most is tekinthetjük C(s) = 1-nek. Legyen a linearizált rendszer kimenete ] y = [k y + k Θ k Θ z, (3.13) tehát a vezérléshez használjuk d-nek és (d r)-nek a súlyozott összegét. Szándékosan használtam az egyenes szakaszokhoz készült vezérlők paramétereivel megegyező jelölést, hogy ez a paraméterek szerepe közti hasonlóságot tükrözze. (d r) használatának a tulajdonságait mutatja a 3.6. ábra. A linearizált rendszer transzfer függvénye P (s) = vd2 b 2 s vbd(r + d) d 2 b 2 s 2 + v 2 (d 2 r 2 ). (3.14) Előre megjegyzem, hogy most mind a k d, minde a k Θ együtthatók negatívak lesznek. Ennek szüksége egyszerűen belátható a 3.11 egyenletrendszer vagy a 3.5. ábra vizsgálatával. A C(s)P (s) open loop rendszer transzfer függvénye szerepel a (3.15) egyenletben. A rendszer bode-diagramját és nyquist-diagramját mutatja a 3.7. ábra. L(s) = v k y(sd 2 b 2 + vrdb) + k Θ vbd 2 (3.15) d 2 b 2 s 2 + v 2 (d 2 r 2 ) L(iω) = vdb d 2 b 2 ω 2 + v 2 (d 2 r 2 ) kyd 2 2 b 2 ω 2 + v 2 (k y r + k Θ d) 2 (3.16) ( ) tan 1 k yd 2 b 2 vdb(k L(iω) = ω yr+k Θ, ha d 2 b 2 ω 2 + v 2 (d 2 r 2 ) > 0 d) ( ) tan 1 k yd 2 b 2 ω vdb(k yr+k Θ + π, ha d 2 b 2 ω 2 + v 2 (d 2 r 2 ) < 0 (3.17) d) Az egyenes útszakaszokhoz tartozó vezérléshez hasonlóan most is mindig stabil rendszert sikerül elérnünk jó gain margin-nal. Láthatjuk, hogy magas frekvenciáknál L(iω) -re főleg k y van hatással. 22

23 3.4. Útvonaltartás körív alakú pályaszakaszok esetén (a) k y = 1, k Θ = 0 (b) k y = 1, k Θ = ábra. Az autó útjának szimulálása a körívtől 0.2 méterre lévő kezdőállapotból kiindulva, két különböző vezérlő paraméterezéssel. Az alsó sor a kormányzási szöget mutatja az idő függvényében, a felső sor pedig az autó pozícióját, ahol piros vonal jelzi a feladat által meghatározott pályát, kék pedig az autó szimulált útvonalát. (a) Bode-diagram (b) Nyquist-diagram 3.7. ábra. Open loop bode-diagram és nyquist-diagram körív alakú pálya esetén esetén. (v = 2 m/s, k y = 1, k Θ = 20, δ e = 5 ) 23

24 3.6. Motor vezérlés 3.5. A vezérlő paraméterek használhatósági spektruma A 3.3. és 3.4. alfejezetekből tudjuk, hogy adott k d és k Θ paramétereknél a closed loop rendszer tulajdonságai különböző sebességeknél különbözőek lesznek. Az autó által bejárt útvonal azonban nem fog változni a sebesség függvényében, ezért különböző sebességekhez használhatóak ugyanazok a paraméterek. Hasonlóan érvelhetünk a körív alakú pályák esetén is, a körívhez tartozó sugár (vagy a körívhez tartozó kanyarodási szög) viszont már komolyabban korlátozhatja a paraméterek használhatóságát. A körív alakú pályaszakaszokhoz tartozó vezérlő esetén tehát a különböző méretű íveket csoportosítani kell, majd csoportonként kell meghatározni a vezérlő paramétereket Motor vezérlés A vezérléstől azt is megköveteljük, hogy olyan feszültséget állítson be az egyenáramú motor számára, hogy az autó egy általunk megadott sebességgel mozogjon. Ehhez elég ismernünk az egyenáramú motor és az autó paramétereit. Ezek alapján meghatározhatjuk majd az egyenáramú motor számára azt a feszültséget, amivel majd állandósult helyzetében olyan nyomatékot generál, hogy az autó sebessége eléri a kívánt értéket. Az útvonalkövetéssel szemben most nincs szükség visszacsatolt vezérlés kialakítására, ugyanis az út egyenletlenségeiből adódó sebességváltozást az egyenáramú motor kompenzálni fogja, ha a feszültséget állandó szinten tartjuk. [6] 24

25 4. fejezet Szimuláció 4.1. A szimulációs eszközökről Az előző két fejezetben levezetett algoritmusok elemzését az éles tesztelés előtt célszerű számítógéppel szimulációs szoftverekkel is elvégezni. A szimuláció gyorsabb az eszköz valódi kipróbálásánál, a paramétereket tetszőlegesen manipulálhatjuk és megvizsgálhatjuk őket a modellezett folyamat bármely állapotában, ezzel pontos betekintést nyerve a rendszer működésébe. A vezető nélküli járművek esetében a szimulációs szoftverek a legfontosabb feladata a jármű és a mi esetünkben kifejezetten a modellautó mozgásának pontos modellezése. Erre a célra egy maroknyi specializált szoftver létezik, köztük néhány ingyenes is, amelyek az általuk használt fizikai modell minőségében, használati célban és testreszabhatóságban különböznek. A jövőben érdemes lesz ezekben a szimulációs rendszerekben is kipróbálni a navigációs algoritmust. Az autóiparban széleskörűen használt modellező szoftver a Carsim, amelynek célja kifejezetten az autó fizikai szempontból valósághű modellezése, illetve a vezérlő algoritmusok tesztelése. A Morse egy ingyenes általános robot szimulátor, amelynek nagy előnye, hogy egyszerűen bővíthető Python programozási nyelven és modellautókat is tud szimulálni. A ClearView kifejezetten modellautók szimulálására lett kifejlesztve. A Torcs egy versenyautó szimulátor, melynek elsősorban inkább szórakoztató célja van, mindenesetre ez a szoftver is biztosítja külső szoftveres vezérlőkomponens csatolását a játékhoz, illetve játék közben az autó adatainak lekérdezését. A beágyazott rendszerek szimulációjára általában két mód van. Hardware-in-theloop (HIL) szimulációnak nevezik, ha az igazi eszköz kezeli szimulációs környezet adatait és irányítja a folyamatot. Software-in-the-loop (SIL) szimulációnak nevezik, ha nem az igazi eszköz, hanem az eszközre írt szoftver számítógépen futó változata kapcsolódik a szimulációs környezethez. Én SIL szimulációt végeztem, azaz elkészítettem az autóra szánt programot C programozási nyelven, és számítógépen futtatva ellenőriztem a helyességét. Erről szólnak a következő alfejezetek Szimuláció Scilab-ban A vezérlés folyamatát a Scilab matematikai környezetben végeztem az Xcos grafikus felülettel. A Scilab egy francia cég által fejlesztett általános célú, nyílt forrású matematikai szoftver. Külső megjelenésében, könyvtárakban és az interpreter szintaxisában 25

26 4.2. Szimuláció Scilab-ban is hasonlít a Matlabhoz. Az Xcos egy grafikus felület a Scilabhoz, amellyel egyszerűen lehet hibrid dinamikus rendszereket 1 modellezni, kimeneti és bemeneti portjaikkal egymáshoz kapcsolódó blokkok formájában. A Scilab jó dokumentációval rendelkezik, továbbá a forrásaihoz is hozzáférhetünk. További bár kissé elavult információ olvasható Press és mások munkájában.[28] Az Xcos minden szükséges általános blokkal rendelkezik, amelyeket összekapcsolva bonyolult rendszereket képezhetünk, de olyan specializált rendszerek esetén, mint például a Kálmán-szűrő, rendkívül kényelmetlen feladat lenne minden műveletet blokkok összekapcsolásával megvalósítani. Az Xcos megengedi saját blokkok készítését is C nyelven vagy a Scilab interpreter nyelvén, ezért a navigációs rendszer bonyolultabb egységeit C nyelven implementáltam. A saját blokkkok további előnye, hogy az általuk modellezett alrendszer paramétereit a blokkra kattintva közvetlenül, saját definiálású ablak segítségével állíthatjuk be, nem pedig az egész munkaterület számára fenntartott rendszer kontextusban. Az utóbbi érvelés alapján készítettem az autó kinematikai modelljének is külön blokkot. A blokkok lehetnek folytonos vagy diszkrét állapotúak. A szimuláció során két esemény fog periodikusan bekövetkezni a blokkok számára: az állapotok frissítése és a kimenetek lekérdezése. A frissítés periódusa folytonos állapotú blokkok esetén a szimuláció lépésközétől függ, diszkrét blokkok esetén pedig külső időzítőkkel lehet a blokk adatait frissíteni. Folytonos állapotú blokkok esetén a blokknak az állapotok deriváltjait kell kiszámolni, és a szimulációs motor frissíti ez alapján az állapotokat. A következő alfejezekben először lépésről lépésre bemutatom a vezérléssel ellátott autó modelljének megalkotását Xcos-ban A mozgás modellje Még mindig a síkbeli mozgásnál maradva, a rendszer alapját most is az (2.1)-(2.4) egyenletek adják, amit a 2.2. ábra szemléltet. Ennek a kinematikai modellnek külön blokkot hoztam létre, melynek állapotai, és egyben kimenetei a síkbeli koordináták és az autó elfordulása, és további kimenete az autó szögsebessége, bemenetei pedig a leegyszerűsített autó elejének sebessége és a vezérelt kormányszög. Ebben az esetben a blokk létrehozása nem is a C implementáció miatt, hanem a testreszabott paraméterezés miatt volt kedvező. A blokk paraméterei a leegyszerűsített autó hossza, a kezdeti pozíció és az elfordulás A motor modellje A motor Xcos modellje látható a 4.1. ábrán. A 2.1 alfejeztben említett paramétereket most két, a folyamat során állandónak feltételezett α és β értékbe tömörítettem. Ezeket a változókat a szimulácós kontextusban kell megadni. A szimuláció során a motor a motor paramétereiből származtatott adatok alapján (ezek az ellenállás, motorállandó, belső súrlódási nyomaték) 4 V bemeneti feszültséget feltételezve α = 0.5 és β = 1 értékeket használtam, amig megközelítőleg 2m/s állandó sebességet eredményeztek. A modellezett motor paramétereit nem lehet változtatni, azaz azt feltételezzük, hogy a 1 Hibrid dinamikus rendszerek az olyan rendszerek, amelyek egyszerre rendelkeznek folytonos és diszkrét tulajdonságokkal. Az autót irányító algoritmus, amely periodikusan frissíti állapotait egy mikrokontrolleren, diszkrét alrendszerként modellezhető, viszont összefüggésben áll az autó mozgási folyamatával, ami pedig folytonos. 26

27 4.2. Szimuláció Scilab-ban 4.1. ábra. A motor modellje. A későbbi alfejezetben ezt még zajjal fogom bővíteni. bemeneti feszültség végig ugyanakkora.a motor modellezése során komoly egyszerűsítésekkel éltem, amiket a 2.1 fejezetben részleteztem, de a szenzorfúziós algoritmus és a vezérlés elemzése így is elvégezhető és használható eredményeket szolgáltat A vezérlés Egyedi blokkot készítettem a vezérlésnek is, amely az útszakaszok kiválasztásáért felelős magas szintű komponenst és az aktuális szakasz betartásáért felelős alacsony szintű komponenst együtt tartalmazza. Az útvonal adatait is a blokk paramétereiben kell megadni, egymásba végpontjaikkal illeszkedő egyenes és körív alakú szegmensek formájában. Az egyenes szegmenseket végpontjukkal kell megadni, a körív alakú szegmenseket pedig végpontjukkal és a köríven a végpont és a kezdőpont között elhelyezkedő tetszőleges ponttal. Mindkét esetben a kezdőpont az előző szakasz végpontja, vagy az útvonal kezdetén az autó kezdeti helyzete. Inicializációkor a vezérlő blokk ezekből az adatokból a vezérléshez szükséges paramétereket kiszámolja és egymás után elhelyezett struct segment típusú elemekben tárolja. A vezérlő feltételezi, hogy a pálya zárt, azaz a végpont megegyezik a kezdőponttal, és a végpont elérése esetén újrakezdi a pályát. A blokk pályája alapbeállítás szerint egy oldalra fordított nyolcashoz hasonlítható alakú pálya, amely a 4.2. ábrán látható. A magas szintű vezérlő a működés során nyilvántartja, hogy az autó melyik pályaszegmens mentén halad. Amint a következő pályaszegmens közelebb van az autóhoz, mint amin eddig haladt, a vezérlő frissíti az aktuális pályaszegmens adatait. A blokk az állapotfrissítések között a munkaterületen tart egy struct hlc típusú struktúrát, amely az útvonal komponenseinek adatait, az aktuális útvonal azonosítóját és a vezérlőhöz szükséges adatokat tárolja: /* Low Level Control structure */ struct llc { /* A mintavételezési intervallum. Ezt most nem használjuk */ double dt; /* Az egyenes szakaszokhoz készült vezérlő paraméterek */ double ky_straight; double ktheta_straight; /* A körív alakú szakaszokhoz készült vezérlő paraméterek. */ double ky_circular; double ktheta_circular; 27

28 4.2. Szimuláció Scilab-ban 4.2. ábra. A szimuláció során használt pálya. }; /* A generált kormányszög */ double delta; /* High Level Control structure */ struct hlc { /* Az aktuális szegmens indexe */ int index; struct segment *segments; int segment_count; /* A leegyszerűsített autó hosza */ double b; struct llc llc_s; }; /* Egyenes útvonal */ struct s_segment { double angle; }; /* Körív alakú útvonal */ struct c_segment { /* Az autó elejének távolsága a körívhez tartozó középponttól */ double d; /* Az autó hátuljának távolsága a körívhez tartozó középponttól */ double r; /* * A körív kezdőpontja és végpontja által a középpontból 28

29 4.2. Szimuláció Scilab-ban }; * kifeszített félegyenesek által bezárt szög. */ double angle; /* A középpont */ double center[2]; /* Az útvonalhoz tartozó ideális kanyarodási szög */ double delta; struct segment { /* 1=egyenes, 2=körív */ int type; union { struct s_segment s; struct c_segment c; } params; double start[2]; double end[2]; }; A vezérlő a következő módon frissíti állapotát és állítja elő a kimenetet, azaz a vezérelt kormányszöget:... /* * A flag tartalmazza a szimulációs fázist: * 1=kimenetek frissítése * 2=állapotok frissítése * 4=inicializáció * 5=bejezetés */ switch(flag) { case 1 : {... /* * getdistance az autó távolságát adja meg a megadott * útvonalszegmenstől. * hlc_s típusa struct hlc* * p az autó pozícióját tartalmazó tömb */ /* 29

30 4.2. Szimuláció Scilab-ban * Ha az autó közelebb van a következő * útvonalszegmensnek, akkor jelezzük az alacsony * szintű vezérlőnek, hogy a következő szegmenshez * igazítsa az autót. */ double d1 = getdistance(p,&(hlc_s->segments[hlc_s->index])); int nextindex = (hlc_s->index+1) % hlc_s->segment_count; double d2 = getdistance(p,&(hlc_s->segments[nextindex])); if (d2 <= d1) { hlc_s->index = nextindex; } ll_control(p,*yaw,hlc_s); } /* delta a vezérelt kormányszög */ *delta = hlc_s->llc_s.delta; break; } case 2 : /* * Az állapotokat is az 1. fázisban frissítettük, ezért itt * nem kell csinálni semmit. */... Tekintettel arra, hogy a vezérlő 0.01 másodperc periódusidővel fut le, további intézkedések szükségesek ahhoz, hogy az autó kormányzási sebességét limitáljuk, hiszen nyilvánvaló, hogy a kormánynak nem 0.01 másodpercbe telik, amíg például a maximálisan jobbra fordult állapotából átér a maximálisan balra fordult állapotába. A vezérlőblokk kimenetét az Xcos-ban még kiegészítettem egy ratelimiter blokkal, amely a bemenet változásának mértékét meghatározott határok között tartja. A 4.4. ábrában látható ratelimiter blokkban a kormányszög változásának értékeit -180 és 180 /s határok közé szorítottam. Próbálgatás után az egyenes szakaszokon a vezérlő paramétereire a szimulált 2 m/s sebesség esetén k y = 2,k Θ = 2 értékeket határoztam meg. Ugyanakkora sebességnél a 4.2. ábrán látható pálya körív alakú szakaszaihoz k y = 1 és k Θ = 20 értékeket választottam. További munka lesz a vezérlő paramétereinek meghatározása változó kanyarodási szögekre Szenzorok A szenzorok kimenetét a kinematikus folyamat és a motor állapotaiból származtathatjuk, viszonylag egyszerű módon: 30

31 4.2. Szimuláció Scilab-ban A függőleges tengely körüli elfordulást mérő ω z giroszkóp adatai megegyeznek a kinematikai modell blokkjának szögsebesség kimenetével, ω-val. A gyorsulásmérő azon a x tengelyének értéke, amely a Θ elfordulással megegyező irányba mutat, közvetlenül származtatható a motor modell által nyilvántartott gyorsulásból. Emlékeztetem az olvasót ugyanis, hogy itt azzal az egyszerűsítéssel éltem, amely azt feltételezi, hogy az autó motorja az első kerekeket hajtja meg, és a tömegközéppont is az első kerekek között van. A gyorsulásmérő előző tengellyel merőleges szöget bezáró, oldalirányú a y tengelye a centripetális gyorsulást méri, amely az a y = v2 = ωv egyenlet szerint r számolható az ω szögsebességből és a Θ irányú v sebességből Zajosítás A valóságban az autót irányító algoritmus nyilvánvalóan nem fogja tökéletesen ismerni a folyamat aktuális paramétereit, két okból: egyrészt a folyamat modelljének hiányosságai miatt, másrészt pedig a szenzorokhoz hozzáadódó zaj miatt. A feladat során azt feltételeztem, hogy a modell hiányosságai az elfordulásban és a gyorsulásban jelennek meg, és elsősorban a talaj egyenletlen felületéből származnak. Az elfordulás bizonytalanságát a vezérlő kimenetéhez hozzáadódó sávkorlátolt fehér zajjal jellemeztem. Intuitív módon azt feltételeztem, hogy a zaj frekvenciatartománya 2.5 Hz, azaz az autó kerekét kitérítő hatások nem tartalmazhatnak 2.5 Hz-nél nagyobb frekvenciájú komponenseket. Az Xcos-ban ezt úgy valósítottam meg, hogy a vezérlő kimenetéhez 0.2 másodpercenként változó normális eloszlású, független valószínűségi változókat adtam hozzá. A Shannon-tételből következik, hogy ezek a minták egy 2.5 Hz sávszélességű fehér zaj mintavételei.[5] A gyorsulás bizonytalanságát a motort tartalmazó alrendszerbe építettem be, 2.5 Hz sávszélességű fehér zaj formájában, az előző esethez hasonló megvalósításban. Ha a zajt csupán így szimulálnánk, a jármű sebessége már túl hosszú ideig térne el az ideális sebességtől, mivel a gyorsulást és a hozzáadódó zajt integrálva véletlen sétát hoznánk létre. Az autót érő nem modellezett hatások pedig általában negatív gyorsulást eredményeznek, a véletlen generátor várható értéke azonban 0, azaz ugyanannyiszor fog a hatások következtében növekedni az autó gyorsulása, mint csökkenni, ami szintén eltér a valóságtól. Hogy az autó sebessége hamarabb visszatérjen az ideális értékre, a véletlen generátor kimenete csak bizonyos periódusonként fog hozzáadódni a gyorsuláshoz, egyébként pedig a motornak lesz ideje a sebességet visszahozni az eredeti értékre. Ezt egy pulzusgenerátor kimenetével való szorzással valósítottam meg. Továbbá, hogy az autó gyorsulása csak a negatív irányba változzon, a véletlengenerátor pozitív kimeneteinek a -1-szerese fog hozzáadódni a gyorsuláshoz. Az így kibővített motor modelljét mutatja a 4.3. ábra. A 2.3 alfejezetben írtam, hogy a szenzorok adatai általában fehér zajtól szennyezettek. Ezt az előző fejezetben bemutatott modell kibővítésével könnyen elérhetjük, ha minden szenzorhoz a rá jellemző fehér zaj szórásával megegyező szórású normális eloszlású valószínűségi változót adunk hozzá, amelynek frissítési frekvenciája megegyezik a szenzorok mintavételezési frekvenciájával. A szenzorok sztochasztikus paramétereinek a meghatározására 24 órás statikus méréssorozatot készítettem az autóba kerülő szenzorokkal megegyező típusú szenzorokkal, majd a fehér zajra jellemző szórásnégyzetet 31

32 4.2. Szimuláció Scilab-ban 4.3. ábra. A gyorsulás nem modellezett változásainak szimulálása sávkorlátolt fehér zajként. az Allan-variance módszer segítségével határoztam meg. A szimulációban tehát valódi, később majd az éles rendszerben is megfigyelhető adatokkal dolgoztam A Kálmán-szűrő A 2.2. alfejezetben említett szenzorfúziós algoritmust valósítja meg a Kálmán-szűrő blokk, amelyet szintén C nyelven implementáltam. A programozás során figyelembe vettem a mikrokontrollerek által meghatározott egyedi követelményeket is, mint például a dinamikus memória minimális használata. A Kálmán-szűrő korábbi alkalmazásainál is megfigyeltem, hogy az általánosan elterjedt dinamikus memóriakezelő rutinok használata esetén a teljesítmény nagyságrendekkel romlott. A lineáris algebrai műveletekre saját, egyszerű memóriaallokációs igényekkel rendelkező eljárásokat készítettem. A mátrix invertálást Crout algoritmusa szerint végeztem. [29] A blokk paraméterei a kezdeti pozíció, kezdeti elfordulás, a leegyszerűsített autó hossza, a szenzorok sztochasztikus paraméterei, a folyamathoz hozzáadódó zaj sztochasztikus paraméterei, a motor paraméterek és a mintavételi frekvencia. Be lehet állítani továbbá a tesztelések és összehasonlítások kedvéért, hogy a szűrő ne használja a szenzorokat a becslések készítésekor. A blokk két bemenete a vezérelt még nem zajosított kormányszög, illetve a szenzoradatokat tartalmazó vektor. Kimenete a becsült pozíció, elfordulás, szögsebesség, sebesség és gyorsulás. A egyenletben, azaz az x k = f(x k 1, u k 1, w k 1 ) egyenletben szereplő w változó tehát a korábban említett nem modellezett kormányszög és gyorsulás változásokat [ ] T jellemzi: w k = w δ w acc. A munkám során azt feltételeztem, hogy ezek függetlenek egymástól, tehát a Q mátrixnak csak diagonális elemei lesznek, amelyek w két komponensének a szórásnégyzetét tartalmazzák. A folyamat modellje tehát a 2.2 fejezetben levezetett, a periódusok során állandó szögsebességet és gyorsulást feltételező CTCA 32

33 4.2. Szimuláció Scilab-ban 4.4. ábra. A vezérlővel és kálmán-szűrővel ellátott autó modellje. A piros blokk a rendszer kinematikai modellje. A barna blokk a Kálmán-szűrő, a kék blokk pedig a vezérlés. A motor és a szenzor adatok a megfelelően jelölt szuperblokkokba vannak csoportosítva. A Kálmán-szűrő és a vezérlő együtt egy hurkot alkotnak, ezért a Kálmán-szűrő definiáló rutinja jelzi az Xcos-nak, hogy a Kálmán-szűrő kimenete nem függ közvetlenül a bemenettől, azaz a t-edig pillanatban nincs szüksége a t-edig pillanatban beállított kormányszögre. modell lesz. Vezessük be a következő jelölést: ω nk = v k 1 b sin(δ k 1 + w δk 1 ) (4.1) a nk = k 1 + αv k 1 + β + w acc. (4.2) Most újraközlöm a korábbi egyenleteket a folyamathoz hozzáadódó zajjal kiegészítve: p xk 1 + p x p yk 1 + p y x k = f(x k 1, δ k 1, w k 1 ) = Θ + ω nk T ω, (4.3) nk v k 1 + a nk T a nk és p x = 1 ω nk [(v k 1 + a nk T )sin(θ k 1 + δ k 1 + ω nk T ) v k 1 sin(θ k 1 + δ k 1 ) ] + an k ω nk (cos(θ k 1 + δ k 1 + ω nk T ) cos(θ k 1 + δ k 1 )) p y = 1 ω nk [( v k 1 a nk T )cos(θ k 1 + δ k 1 + ω nk T ) +v k 1 cos(θ k 1 + δ k 1 ) ] + an k ω nk (sin(θ k 1 + δ k 1 + ω nk T ) sin(θ k 1 + δ k 1 )),. (4.4) (4.5) A parciális deriváltakat tartalmazó A,W,H és V mátrixok a deriválás szabályai alapján egyszerűen származtathatók, és nagy méretük miatt most nem közlöm őket, az olvasó megtalálhatja őket az algoritmus C-ben implementált változatában. Már most is 33