Teknisk Dokumentation

Transkript

1 Teknisk Dokumentation Optimal Styrning av Radiostyrd Racerbil Version 1.0 Per Svennerbrandt 7 december 2011 Status Granskad Isak Nielsen 7 december 2011 Godkänd

2 Projektidentitet Grupp-e-post: Hemsida: Beställare: Kund: Kursansvariga: Handledare: Kristoffer Lundahl, Avdelningen för Fordonssystem vid LiTH Telefon: +46 (0) , E-post: Daniel Axehill, Avdelningen för Reglerteknik vid LiTH Telefon: +46(0) , E-post: David Törnqvist, Avdelningen för reglerteknik vid LiTH Telefon: +46 (0) , E-post: Daniel Axehill, Avdelningen för Reglerteknik vid LiTH Telefon: +46(0) , E-post: Niklas Wahlström, Avdelningen för Reglerteknik vid LiTH Phone: +46 (0) , E-post: Gruppmedlemmar Namn Ansvar Telefon E-post Mikael Gustafsson Projekledare Per Svennerbrandt Designansvarig Thom Magnusson Dokumentansvarig Jesper Sandberg Testansvarig Isak Nielsen Informationsansvarig Martin Lilja

3 Dokumenthistorik Version Datum Gjorda förändringar Signatur Granskad av december 2011 Första utkast TM, ML, TM, ML, MG, IN, PS MG, IN, PS december 2011 Första utkast PS IN

4 Innehåll 1 Inledning 1 2 Syfte och mål 1 3 Användning 1 4 Systemöversikt 1 5 Bilmodellering Fordonsmodell Longitudinell dynamik Experimentuppställning för longitudinell dynamik Rotationsdynamik Experimentuppställning för rotationsdynamik Parameterestimering Retardationstest - C 1,C Accelerationstest - K d Bromstest - K d Svängtest - C 3, C 4, C 5, C Resultat och diskussion Beräkning av optimal trajektoria Ingående mjukvaror Implementation av optimeringsproblem i Optimica Lösning av ett optimeringsproblem Banparametrisering Kontinuerliga begränsningar Beräkning av optimal trajektoria Resultat och diskussion Gränssnitt Failsafesystem Bilmodell för prediktering av undanmanövrar Algoritm Gränssnitt Insignaler Utsignaler Regulator Huvudregulator Styrsignalväljare Bilmodell för regulatorsyntes Gränssnitt Insignaler Utsignaler Baninterface 30

5 9.1 Algoritm för att avgöra ifall en punk ligger innanför banan Pseudokod Gränssnitt Insignaler från offlinesystemet Insignaler från onlinesystemet Utsignaler till onlinesystemet Referensinterface Gränssnitt Insignaler från offlinesystemet Insignaler från onlinesystemet Utsignaler till onlinesystemet Filer Inläsningsfiler serial.txt ref.txt track.txt settings.txt Skapade filer controlsignals.txt failsafeactions.txt fromcam0.txt fromcam1.txt refmeas.txt state.txt Bibliotek Programkod C Failsafe Failsafemodel Controller ControllerType LQ ManualController TrackInterface RefInterface Matlab createref.m estimateregmodel.m mergeandextract.m playrun.m readcarpos.m readcontrolsignals.m readmarkers.m readref.m

6 readrefmeas.m readstates.m readtrack.m readfailsafeaction.m A Parameterestimeringskod 41 B Parameterestimeringsskript 43 C Optimeringskod 44 D Optimeringsskript 46 E Draw Ellipse 49 F Plot signals 50

7 Optimal Styrning av Radiostyrd Racerbil 1 1 Inledning I detta projekt har ett styrsystem för en radiostyrd bil (RC-bil) tagits fram. Detta kan användas för att styra bilen längs en trajektoria beräknad offline med metoder från optimal styrning. För att köra den givna trajektorian så fort som möjligt krävdes att en modell för RC-bilen togs fram. Ambitionen var att med hjälp av modellen och en lämplig regulator kunna styra bilen autonomt så nära stabilitetsgränsen det bara går längs den framräknade trajektorian. Utöver detta har även ett failsafesystem designats för att förhindra materiella skador på RC-bilarna. Detta system griper in och tar över det primära styrsystemet när en olycka är på väg att ske. Failsafesystemet är även aktivt när bilen styrs manuellt. 2 Syfte och mål Syftet med projektet var att konstruera ett styrsystem för en autonom radiostyrd bil. Systemet ska klara av att manövrera bilen längs ett offline-beräknat spår runt en given bana. Målet var att räkna fram ett optimalt spår, givet en modell över RC-bilen, med hjälp av optimal styrnings-metoder, och att sedan kunna följa detta spår med hjälp en regulator. 3 Användning Detta system ska eventuellt användas som laborationsmoment och studentprojekt vid ISY. 4 Systemöversikt Systemet består av flera olika delar som samverkar. Hårdvarumässigt är det fyra stora delkomponenter som är aktuella: Bilbanan/bilarna Kamerorna Radiokontrollen Dator Det huvudsakliga informationsflödet mellan hårdvarukomponenterna ses i figur 1 på följande sida. Banan som bilarna kör på är uppbyggd av flera små plattor som är gjorda av mjukt gummi. Dessa bitar är sedan monterade på en träplatta med en sarg som gör att banan inte ändrar form. Över banan är två kameror monterade i taket så att deras gemensamma synfält täcker banans alla delar. Kamerorna skickar sedan över bilder till datorn som kör algoritmer för att detektera och skatta bilarnas tillstånd. Sampeltiden för systemet är 100Hz, men kan ändras om så önskas (dock klarar det ej över cirka 110Hz). Styrningen av bilarna kan ske på flera olika sätt, antingen helt autonomt, helt manuellt eller en blandning av dessa. I det helt autonoma läget så beräknar datorn en styrsignal (via någon form av regulator) som sedan ställs ut till handkontrollen. I det manuella läget så körs en enkel loop-back funktionalitet som gör att bilen körs helt manuellt (dock kan failsafesystemet vara inkopplat). En blandning mellan dessa två lägen kan också användas,

8 Optimal Styrning av Radiostyrd Racerbil 2 Figur 1: Informationsflöde mellan hårdvarukomponenter. då utsignalen från dator till handkontroll är en mix av manuella styrsignaler och styrsignaler beräknade från en regulator. De styrsignaler som datorn skickar till handkontrollen överförs sedan till bilarna av den i handkontrollen inbyggda radiomodulen. Mjukvarumässigt består systemet av sju huvudkomponenter: Målföljningssystemet Failsafesystemet Regulatorn System för att beräkna en trajektoria med metoder från optimal styrning. Bilmodellen Baninterface Referensinterface Målföljningssystemet beräknar bilens/bilarnas skattade positioner (detta systsem är implementerat sedan tidigare). Failsafesystemet finns till för att förhindra materiella skador och sköter kommunikation mellan regulator (eller manuella styrsignaler) och bilarna. Regulatorns uppgift är att manövrera bilen så att den givna referenstrajektorian följs tillräckligt

9 Optimal Styrning av Radiostyrd Racerbil 3 väl. Bilmodellen används för att beräkna referenstrajektorian, vilket görs offline, men den kan även användas i regulatorn om så önskas. För att hantera information om banan på ett smidigt sätt så används ett ban- och referensinterface av systemet. Ett blockschema över hela mjukvarustrukturen ses i figur 2. Bilmodell Trajektoriaoptimering Baninformation offline bilmodell reducerad trajektoria karta online Referensinterface x ẑ r u offline Regulator u h Handkontroll Tracking ˆx u Failsafe ũ x på/av banan Baninterface Radiomodul Figur 2: Blockschema beskrivande signalflödet mellan de olika delsystemen. Mer detaljerade beskrivningar av dessa system återfinns nedan i detta dokument.

10 Optimal Styrning av Radiostyrd Racerbil 4 5 Bilmodellering RC-bilarna som används i projektet är tillverkade av företaget Kyosho och går under modellklassen dnano. Dessa bilar i skala 1:43. Bilarna består av ett chassi och en kaross, som sätts samman med hjälp av små fästen längst fram på bilen samt vid bakdäcken. Karosserna, som består av plast, har samma form som olika typer av välkända sportbilar. Bilen som kommer att modelleras i detta projekt är byggd för att efterlikna en Lamborghini Murcielago. Denna har måtten 107x50x27 mm och vikten 50 gram. På chassit sitter fyra gummitäckta däck, ett styrservo som är kopplat till framdäcken samt en liten elmotor som driver bakdäcken. Elmotorn sköter såväl acceleration som inbromsning och back. Bilen får sin energi från ett batteri som sitter monterat i mitten av chassit. Kommunikationen mellan bilarna sker genom en fjärrkontroll som skickar information över 2.4 GHz-bandet. Informationen som skickas är gas-/bromssignal (u g ) samt styrsignal (u s ). Bilen har modellerats och dess parametrar bestämts utifrån diverse olika tester, såsom accelerations-, broms- och svängtest. Den fullständiga bilmodellen används för att beräkna en optimal trajektoria längs banan som bilen sedan skall följa. 5.1 Fordonsmodell Y x Y y X X Figur 3: Illustration över bilens koordinatsystem i banans koordinatsystem. Dessa ekvationer beskriver bilens rörelse:

11 Optimal Styrning av Radiostyrd Racerbil 5 δ = K s u s (1) Ẋ = v cos(θ) (2) Ẏ = v sin(θ) (3) ẋ = v (4) v = 1 m (F driv + F res ) + C 5 vδ 2 (5) 1 θ = C 3 δ (1 C 4 v) v + C 6 (6) F driv = K d u g (7) F res = C 1 v + C 2 sign(v) (8) Ekvation 1 beskriver förhållandet mellan styrsignal, u s, och hjulens styrvinkel, δ. Ekvation 2 och 3 beskriver hur bilens hastighetsvektor projiceras på det banfixa koordinatsystemet. Bilens koordinatsystem är definerat med ˆx-axel i bilens longitudinella riktning och ŷ-axel i laterala enligt Figur 3 på föregående sida. X och Y anger bilens position i banans koordinatsystem och θ är vinkeln mellan bilens och banans respektive koordinatsystem. Ekvation 5 anger bilens acceleration i longitudinell riktning. De krafter som antas påverka bilens acceleration beskrivs av ekvation 7 och 8. Ekvation 6 beskriver hur bilen roterar runt sin tyngdpunkt i XY -planet. Trackingsystemet kan mäta X, Y och θ. Utifrån information om dessa tillstånd har experiment ställts upp för att skatta samtliga parametrar till modellen. 5.2 Longitudinell dynamik Vid framtagning av den longitudinella dynamiken antas att bilen inte har något slip vilket leder till att bilen endast har en hastighet enligt v = vˆx. Vidare antas även att insignalen, u g, kommer in linjärt som en kraft. Alla resistiva krafter sammanfogas i ekvation 8 där luftmotstånd har uteslutits på grund av bilens storlek och låga hastigheter. Termen C 5 vδ 2 beskriver effekterna av ökad friktion då styrvinkel appliceras på framhjulen Experimentuppställning för longitudinell dynamik För att kunna identifiera parametrarna K d, C 1, C 2 och C 5 i den longitudinella dynamiken har tre typer av test utförts. Vid ett retardationstest utan svängning ges den longitudinella dynamiken utav: v = C 1 v + C 2 (9) där C 1 och C 2 är observerbara. Därefter utförs ett accelerationstest respektive ett bromstest utan svängning vilket ger longitudinell dynamik enligt: v = C 1 v + C 2 + K d u g (10) där C 1 och C 2 är kända sedan tidigare och K d kan skattas för broms och gas. Konstanten C 5 har sedan parametersatts ad-hoc för att öppnå önskat beteende på hastigetsprofilen vid körningar med kurvtagningar.

12 Optimal Styrning av Radiostyrd Racerbil Rotationsdynamik Då bilen väger lite och har bra fäste har antagandet att styrvinkeln påverkar vinkelhastigheten linjärt gjorts, C 3 δ, i ekvation 6 på föregående sida. Termen (1 C 4 v) har införts för att kompensera för lastförskjutningen som uppstår vid acceleration och retardation. Till skillnad från traditionella fordon, där vinkelhastigheten är proportionerlig mot hastigheten påvisar experiment att så ej är fallet med RC-bilen. Utförda experiment antyder istället att vinkelhastigheten är omvänt proportionell mot hastigheten. Parametern K s som translaterar styrsignal till styrvinkel har mätts på fordonet och antas vara konstant Experimentuppställning för rotationsdynamik Då modellen ska användas för att ta fram en optimal bana har experiment valts för att passa de tänkta körfallen, det vill säga maximal gas respektive broms samt maximalt svängutslag. Därefter optimeras C 3 och C 4 för att minimera vinkelavvikelsen från uppmätt data. Parametern C 6 har sedan valts ad-hoc för att få önskad svängprestanda vid flertal olika körningar. 5.4 Parameterestimering I detta kapitel redogörs för resultaten av parameterestimeringen som beskrivits ovan. Samtliga parametrar har optimerats fram med hjälp av optimica genom att minimera kvadratfelen i position och vinkel. Vid verifieringarna görs bland annat jämförelser mellan hastigheter då dessa är mer intuitiva än att jämföra till exempel körd sträcka. Hastigheterna har skattats med Euler framåt från uppmätta positioner. Implementation av parameteroptimeringarna kan ses i appendix A på sidan 41 och appendix B på sidan 43. I tabellen nedan visas en sammanställning över samtliga parametrar och de skattade värdena. Parameter Värde K d K s C C C C C C 6 2 K d (u g < 0)/K d (u g > 0) Retardationstest - C 1,C 2 Retardationsdynamiken beskrivs av: v = C 1 v + C 2. För att bestämma C 1 och C 2 accelererades bilen till en hög hastighet och därefter släpptes gasen så att bilen började retardera. C 1 och C 2 har sedan optimerats för att minimera kvadratfelet för positionsavvikelse vid givna tidpunkter. Det utförda experimentet samt modellen med de optimala parametrarna illustreras i Figur 4 på följande sida. I samma figur visas även ett verifikationsexperiment för att validera modelen. Storleken på datasekvensen som parametrarna skattats ifrån är begränsade av banans storlek och trackingsystemets räckvidd.

13 Optimal Styrning av Radiostyrd Racerbil v_modell v_skattad fart [m/s] tid [s] fart [m/s] v_modell v_skattad tid [s] Figur 4: Retardationstest - Övre figuren visar skattad hastighet från uppmätt data samt hastigheten för modellen som skattats utifrån denna. Nedre bilden är ett verifikationsexperiment för att validera modellen.

14 Optimal Styrning av Radiostyrd Racerbil Accelerationstest - K d Accelerationsdynamiken beskrivs av: v = C 1 v + C 2 + K d u g. För att skatta K d under normala körfall utfördes ett experiment där bilen höll konstant hastighet på cirka 1.5 m/s varpå max acceleration applicerades. K d har sedan optimerats för att minimera kvadratfelet för positionsavvikelse vid givna tidpunkter. Det utförda experimentet samt modellen med de optimala parametrarna illustreras i Figur 5 på nästa sida. I samma figur visas även ett verifikationsexperiment för att validera modelen. Storleken på datasekvensen som parametrarna skattats ifrån är begränsade av banans storlek och trackingsystemets räckvidd Bromstest - K d Då bilen kör rakt fram och bromsar antas det att dynamiken är identisk med accelerationsdynamiken i ekvation 5.4.2, med undantaget att K d antar ett annat värde. Då u g kommer in linjärt i modellen i båda dessa fall kan en skalning av negativa u g :n göras off-line för att undgå diskontinuitet. Skalningen på u g bestäms utav kvoten mellan K d (u g > 0) och K d (u g < 0). Kvoten optimeras för att minimera kvadratfelet för positionsavvikelse vid givna tidpunkter. Det utförda experimentet samt modellen med de optimala parametrarna illustreras i Figur 6 på sidan 10. I samma figur visas även ett verifikationsexperiment för att validera modelen Svängtest - C 3, C 4, C 5, C 6 Då parametervärdena för rotationsdynamiken varierar kraftigt mellan olika körfall skattades parametrarna för typiska körfall samt att vissa parametrar nollställdes. Vid olika experiment varierade dessutom vilka parametrar som nollställdes vilket ledde till att två parametrar, C 5 och C 6, bestämdes ad-hoc. C 5 anpassades i huvudsak för bra passning i hastighetsprofil. Detta innebär att bilen måste bromsa in tillräckligt mycket på grund av styrutslaget. C 6 har anpassats för att erhålla en försämrad styrförmåga vid höga hastigheter. För att skatta C 3 och C 4 utfördes ett experiment där bilen startar med full gas och full sväng som övergår i full broms och full sväng. Dessa parametrar har optimerats för att minimera kvadratfelet i vinkelavvikelse vid givna tidpunkter. Bilens vinkel från experimentet samt modellens under samma körfall med de skattade parametrarna kan ses i Figur 7 på sidan 11. I samma figur visas även ett verifikationsexperiment för att validera modelen. I Figur 8 på sidan 12 visas hastighetsprofilen från svängexperimentet samt från ett verifikationsexperiment och i Figur 9 på sidan 13 visas position i XY-planet från samma experiment. Efter en optimering har dessa två figurer studerats varpå C 5 och C 6 har justerats. Därefter har C 3 och C 4 optimerats om och samma procedur upprepats tills acceptabelt resultat uppnåtts. 5.5 Resultat och diskussion Den framtagna modellen i ekvation 1-8 på sidan 5 är skräddarsydd för en tänkt optimal körning på den givna banan, det vill säga full gas respektive full broms och full sväng. Samtliga parametrar i modellen har dessutom optimerats under liknande körfall. Utförda verifikationsexperiment påvisar att modellen stämmer bra under nämnda förutsättningar, se kapitel 5.4 på sidan 6.

15 Optimal Styrning av Radiostyrd Racerbil v_modell v_skattad 3.5 fart [m/s] tid [s] fart [m/s] v_modell v_skattad tid [s] Figur 5: Accelerationstest - övre figuren visar skattad hastighet från uppmätt data samt hastigheten för modellen som skattats utifrån denna. Nedre bilden är ett verifikationsexperiment för att validera modellen.

16 Optimal Styrning av Radiostyrd Racerbil v_modell v_skattad fart [m/s] tid [s] v_modell v_skattad 2.5 fart [m/s] tid [s] Figur 6: Bromstest - övre figuren visar skattad hastighet från uppmätt data samt hastigheten för modellen som skattats utifrån denna. Nedre bilden är ett verifikationsexperiment för att validera modellen.

17 Optimal Styrning av Radiostyrd Racerbil theta modell theta uppmatt 4.0 vinkel [rad] tid [s] theta modell theta uppmatt 0.5 vinkel [rad] tid [s] Figur 7: Svängtest - övre figuren visar skattad vinkel hos bilen i det globala koordinatsystemet från uppmätt data samt vinkeln för modellen som skattats utifrån denna. Nedre bilden är ett verifikationsexperiment för att validera modellen mot annan data.

18 Optimal Styrning av Radiostyrd Racerbil v_modell v_skattad fart [m/s] tid [s] v_modell v_skattad 2.0 fart [m/s] tid [s] Figur 8: Svängtest - övre figuren visar skattad hastighet från uppmätt data samt hastigheten för modellen som skattats utifrån denna. Nedre bilden är ett verifikationsexperiment för att validera modellen.

19 Optimal Styrning av Radiostyrd Racerbil X_modell,Y_modell X_uppmatt,Y_uppmatt 1.6 Y [m] X [m] Y-pos Modell Uppmatt data X-pos Figur 9: Svängtest - övre figuren visar skattad position från uppmätt data samt position för modellen som skattats utifrån denna. Nedre bilden är ett verifikationsexperiment för att validera modellen.

20 Optimal Styrning av Radiostyrd Racerbil 14 Då beräkning av optimal trajektoria gjorts, se Kapitel 6, erhölls en körprofil som anses vara förenlig med den framtagna modellen. Då modellen tagits fram endast för detta ändamål så behövs ingen hänsyn tas till felaktigheter i modellen för låga hastigheter och styrsignaler. Om bilen hade kört på en bana där bilen erhåller sämre fäste hade man inte kunnat göra antagandet om en slipfri modell. I detta fall måste en modell enligt beskrivning i projektets Designspecifikation tas fram och parametersättas. För att förbättra modellen beskriven i 1-8 på sidan 5 borde hänsyn till tidsfördröjningar mellan pålagd styrsignal till verkning på bilen tas. Dessutom kan dynamik från styrutslag till rotationshastighet implementeras. Anledningen till att dessa fenomen har uteslutits är, förutom att de anses vara små, för att få en modell som är enklare att optimera. 6 Beräkning av optimal trajektoria Detta avsnitt beskriver beräkning av en trajektoria runt banan, vilken regulatorn använder som referens, med metoder från optimal styrning. Trajektorian beräknas med hjälp av en bilmodell och en parametrisering av banan som bivillkor. Bilmodellen, som beskrivs av ett Modelica-objekt, körs tillsammans med bivillkoren, som bland annat beskriver banan, i Optimica. Optimica kommer då att generera AMPL-kod som tillsammans med lösaren IPOPT genererar den optimala trajektorian om en sådan existerar. Flödet för detta kan ses i Figur 10. Fordonsmodell JModelica Optimica AMPL Bivillkor IPOPT Optimal Trajektoria Figur 10: Informationsflöde för att beräkna optimal trajektoria med JModelica. 6.1 Ingående mjukvaror För att lösa optimeringsproblemet används följande mjukvara: JModelica.org 1.5. Java Runtime Environment (JRE) version 6 Python 2.6 med paketen NumPy SciPy matplotlib IPython 0.1

21 Optimal Styrning av Radiostyrd Racerbil 15 PyReadline Implementation av optimeringsproblem i Optimica Ett optimeringsproblem av typen min u( ) t f s.a. ẋ = f(x(t), u(t)) g(u(t), x(t)) 0 x(t 0 = 0) = x 0 x(t f ) = x f u min u u max t [t 0, t f ] implementeras i modelica som ett Optimica-objekt enligt optimization My_Opt ( objective = finaltime, starttime = 0, finaltime ( free =true, min =0.1, initialguess =2)) // The states Real x1( start =x1_0, fixed = true ); Real x2( start =x2_0, fixed = true );... // The control signal input Real u1( free =true, min = u1_min, max = u1_max ); input Real u2( free =true, min = u2_min, max = u2_max );... equation der (x1) = f1(u1,u2,...,x1,x2,... ); der (x2) = f2(u1,u2,...,x1,x2,... );... constraint 0 <= g1(u1,u2,...,x1,x2,... ); 0 <= g2(u1,u2,...,x1,x2,... );... x1( finaltime ) = x1_f ; x2( finaltime ) = x2_f ;... end My_Opt ; 6.3 Lösning av ett optimeringsproblem För att styra informationsflödet i optimeringen enligt Figur 10 på sidan 14 används Python-script. Optimica-objektet kompileras till JModelicaspecifikt objekt, JMUModel,

22 Optimal Styrning av Radiostyrd Racerbil 16 som sedan kan kommunicera med IPOPT för att lösa optimeringsproblemet. För att skapa en JMUModel från ett Optimica-objekt skrivs: my_jmu_code = compile_jmu("my_opt","path\my_opt.mop") my_jmu_object = JMUModel(my_jmu_code) Därefter körs optimeringslösaren med: Opt_Res = my_jmu_object.optimize() För att ändra inställningarna för optimeringen, så som lösare, tolerans och så vidare, måste ett JMUModel.Options-objekt skapas. Inställningar görs sedan på det nyskapade objektet och därefter körs optimeringen med JMUModel.Options-objektet som inargument: options = my_jmu_object.optimize_options() options["anoption"] = my_option Opt_Res = my_jmu_object.optimize(options) Resultatet av optimeringen, styrsignaler och tillstånd för hela trajektorian med tillhörande tidpunkter, finns i Opt Res på vektorform. De enskilda variablernas värden nås genom x1 = Opt_Res[ x1 ] u1 = Opt_Res[ u1 ] t = Opt_Res[ time ] 6.4 Banparametrisering För att optimeringslösaren ska kunna beräkna en optimal trajektoria runt banan krävs att bilens positionering är begränsad av banans sarger. I detta avsnitt beskrivs hur dessa bivillkor implementeras i Optimica Kontinuerliga begränsningar När väggarna ska formuleras som bivillkor beskrivs sargerna, förutom de yttersta, som förbjudna områden med hjälp av ellipser av ordning 6, se Figur 11 på nästa sida. De förbjudna områdena i optimeringsproblemet beskrivs på formen ( ) n ( ) n x r n xi y yi + a i Där (x i, y i ) betecknar ellipsernas centrum och a i och b i dess radier. Dessa radier har sedan ökats med 8cm för att kompensera för bilens bredd och för att ge regulatorn marginal för avvikelser från den optimal trajektorian. Dessa krav skrivs in i Optimica på följande form r^n <= ((x - xi)/ai)^n + ((y - yi)/bi)^n; x >= xmin ; y >= ymin ; x <= xmax ; y <= ymax ; b i

23 Optimal Styrning av Radiostyrd Racerbil Y [m ] X [m ] Figur 11: En skiss över hur banrestriktioner ser ut då ellipser av ordning n = 6 används som förbjudna områden. 6.5 Beräkning av optimal trajektoria Vid beräkning av den optimala trajektorian med avseende på bilmodellen från avsnitt 5 på sidan 4 har banans sarger implementerats som bivillkor enligt avsnitt 6.4 på föregående sida. Då bilmodellen är framtagen för positiva hastigheter och för att bilen inte ska börja backa sätts ett krav på att hastigheten ska vara större än 0.2 m/s. De yttre sargerna i kurvorna är egentligen rundade, men detta tas ej hänsyn till i optimeringen då det antas att en optimal trajektoria ej ligger i dessa områden vilket bekräftas av Figur 12 på sidan 19. För att få en kontinuerlig trajektoria sätts även krav på att hastighet, styrsignaler, vinkel och position i Y-led är densamma för starpunkten och slutpunkten. Dessutom har en ellips placerats tvärs över den längsta raksträckan för att undvika att bilen vänder om och kör åt fel håll på banan. Då detta optimeringsproblem är väldigt ickekonvext krävs en god initialgissning för att optimeringslösaren ska komma fram till en lösning. Denna initialgissning kan genereras på flera sätt, varav några nämns nedan: I) Initialgissning i mitten av banan - En enkel initialgissning är en genererad trajektoria längs mitten av banan med konstant hastighet. Genom att endast sprida ut positionerna längs banan med korrekt vinkel och hastighet förenklas optimeringsproblemet avsevärt. II) Inspelad trajektoria - Om den framtagna modellen som optimeringen ska ske med avseende på är tillräckligt bra kan en, från trackingsystemet, inspelad trajektoria runt

24 Optimal Styrning av Radiostyrd Racerbil 18 hela banan användas som initialgissning. Fördelen med detta tillvägagångssätt är att initialgissningen är mer förenlig med modellen än ovanstående tillvägagångssätt, vilket borde förenkla optimeringen ytterligare. III) Stegvis ökning av optimeringshorisont - Ett annat tillvägagångssätt är att lösa ett optimeringsproblem för en kort sträcka med startposition som utgångspunkt. Därefter kan denna lösning användas som initialgissning till en optimering där sträckan utökas till en slutposition längre fram på banan. Detta upprepas tills hela banan är genomlöpt. Under projektets gång har metod I) och III) använts, dock togs den slutgiltiga lösningen fram med hjälp av metod III). Implementationen av detta optimeringsproblem kan studeras närmare i appendix C på sidan 44 och appendix D på sidan 46. Med en god initialgissning och välformulerade krav tar en optimering ungefär en timme. Är initialgissningen dålig kan lösaren divergera och om kraven är dåligt formulerade kan optimeringen ta flera dagar. 6.6 Resultat och diskussion I Figur 12 på nästa sida visas den framtagna optimala trajektorian för den givna banan tillsammans med signalerna för gas/broms respektive styrning. För att regulatorn ska få möjlighet att korrigera modellfelaktigheter i svängar har en övre begränsning satts på styrsignalen för svängning i denna optimering. I Figur 13 på sidan 20 visas dessa styrsignaler som funktion av tiden. Som synes erhålls nästan en Bang-Bang-styrning av bilen vilket ofta är resultatet av liknande optimala styrningsproblem. Körprofilen för den optimala trajektorian ser även den rimlig ut, då bilen ligger i de spår som naturligt känns snabba. Tiden det tar för modellen att köra ett varv på banan optimerades till 7.68 sekunder. 6.7 Gränssnitt Till optimeringen används bilmodellen framtagen i avsnitt 5 på sidan 4 om bilmodellering samt en parametrisering av banan enligt avsnittet 6.4 på sidan 16. Resultatet från optimeringen sparas till en fil som sedan laddas in i baninterfacet för att sedan kunna användas av regulatorn.

25 Optimal Styrning av Radiostyrd Racerbil Y [m] X [m] Y [m] X [m] Figur 12: Optimal trajektoria runt banan tillsammans med insignaler till modellen när bilen färdas medurs. I den övre figuren återfinns gas- och bromssignal, där gas indikeras av markeringar till höger om trajektorian och broms till vänster. I nedre figuren återfinns styrsignalen där högersväng indikeras med markering till höger om trajektorian och vänstersväng med markeringar till vänster.

26 Optimal Styrning av Radiostyrd Racerbil ug tid [s] us tid [s] Figur 13: Styrsignaler till modellen som genererar den optimala trajektorian.

27 Optimal Styrning av Radiostyrd Racerbil 21 7 Failsafesystem Failsafesystemet har till uppgift att avgöra huruvida det föreligger en överhängande risk att bilen är på väg att manövreras in i någon av sargerna. För att göra detta ligger failsafesystemet kontinuerligt och predikterar trajektorierna för femton förutbestämda maximala undanmanövrar. Se schematiskt exempel i figur 14 där bara fem trajektorior visas. Om någon av dessa manövrar anses möjlig att utföra utan kollision med sargen utför systemet ingen åtgärd. Om däremot samtliga undanmanövrar predikteras ge kollision med sargen, väljer systemet att utföra den undanmanöver som predikteras ge lägst hastighet vid kontakt med sargen. Figur 14: Schematiskt exempel på predikterade trajektorior för maximala undanmanövrar (prickade). I den avbildade situationen predikteras de tre längst åt vänster (svartprickade) som möjliga att utföra utan krash, medans de två längst åt höger (rödprickade) anses leda till kollision med den högra sargen. Systemet griper i detta läge alltså inte in. Om bilen däremot skulle forstätta i samma riktning och hastighet skulle systemet gripa in i det läge då trajektorian längst åt vänster precis predikteras nudda sargen. 7.1 Bilmodell för prediktering av undanmanövrar Bilmodellen som används då failsafesystemet predikterar de maximala undanmanövrarna får information om de skattade tillstånden position (X, Y ), longitudinell hastighet v, riktning θ samt vinkelhastighet ω. Modellen har medvetet gjorts så enkel som möjligt och kan därför inte beskriva bilens dynamik i hela tillståndsrummet speciellt väl. För att ändå få bra prediktioner av samtliga undanmanövrar har modellen därför parametersatts separat i olika hastighetsintervall. Empiriska studier visade att bilens vinkelhastighet kan beskrivas tillfredställande som proportionell mot styrutslaget u s samt med en term som reducerar styrförmågan proportionellt mot hastigheten. För att få bra passning skrevs därför uttrycket som ω = θ = (c s,v v + c s )u s. (11)

28 Optimal Styrning av Radiostyrd Racerbil 22 Bilens acceleration antas ges av summan av motorns bromsande kraft samt en friktionsterm som är proportionell mot bilens hastighet. Sammantaget ger detta modellen Ẋ = v cos(θ) (12) Ẏ = v sin(θ) (13) v = c g,v v + c g u g (14) θ = (c s,v v + c s )u s (15) Experiment visade att det gick att få bra överensstämmelse mellan bilens modellerade och faktiska beteende om skattningarna gjordes separat för hastigheter över/under 2.1 m/s. Vid hastigheter över 2.1 m/s är bilens svängförmåga relativt begränsad på de sträckor som är aktuella och därför var det tillräckligt att modellera beteendet vid de tre undanmanövrarna Maximal broms, maximalt styrutslag vänster Maximal broms, neutralt styrutslag Maximal broms, maximalt styrutslag höger Vid hastigheter under 2.1 m/s visade det sig fungera väl med de fem undanmanövrarna Maximal broms, maximalt styrutslag vänster Maximal broms, halvt styrutslag vänster Maximal broms, neutralt styrutslag Maximal broms, halvt styrutslag höger Maximal broms, maximalt styrutslag höger För att ta hänsyn till den tidsfördröjning på 4 sampel som föreligger mellan det att en styrsignal ställs ut och faktiskt ger observerbar påverkan på bilen predikteras de fyra första stegen med de fyra senaste styrsignalerna. Modellkonstanterna skattades med hjälp av mätdata från identifieringsexperiment. Ett optimeringsproblem formulerades i modelleringsspråket Optimica och löstes med hjälp av programpaketet JModelica.org. Som kostnadsfunktion i optimeringsproblemet användes kvadratsumman av termerna avstånd- samt vinkelfel. De parametervärden som erhölls vid skattningar ses i tabell 1. Tabell 1: Tabell över värden på modellparametrar. Giltlighetsområde c g,v c g c s,v c s Högfart Lågfart Algoritm För att beräkna de maximala undanmanövrarna ligger alltså failsafesystemet och predikterar bilens trajektorior under en viss sträcka framåt i tiden. Trajektorian för varje undanmanöver predikteras tills antingen bilens hastighet är noll (eller lägre), eller tills

29 Optimal Styrning av Radiostyrd Racerbil 23 dess då trajektorian leder till en krasch. För att i ett senare läge kunna avgöra vilken undanmanöver som ger lägst hastighet vid krasch så sparas denna undan. Så länge det finns någon trajektoria där hastigheten är noll innan krasch så kommer failsafesystemet ej att ingripa. Ett exempel på hur de predikterade trajektorierna kan se ut under körning ses i figur T: 254 A: 0 H:2 L:4 ug: us: vel: Figur 15: Exempel på hur failsafesystemets beräknade färdvägar för de femton förbestämda undanmanövrarna kan se ut. I detta läge predikteras de röda trajektorierna leda till krasch medan de blå kan stanna bilen utan kontakt med sargen. Om hastigheten initialt är över 2.1 m/s så inleds predikteringen med en av högfartstrajektorierna tills dess att hastigheten predikteras understiga gränsen på 2.1 m/s. Därefter predikteras de fem lågfartstrajektorierna till krasch eller stillastående. Detta uprepas sedan med nästa högfartstrajektoria. Om hastigheten initialt understiger 2.1 m/s så predikterar systemet direkt lågfartstrajektorierna. Predikteringen och detekteringen av krasch utförs av följande C++-funktion /* Determine if some of the trajectories leads to crash */ crashinfo Failsafe :: detectcrash () { int nroftrajlow = motionveclow. size (); int nroftrajhigh = motionvechigh. size (); matrix xp; matrix xplow ; crashinfo cinfo ; bool ontrack ; #if debug int inx = 0; # endif matrix Q (5,5);

30 Optimal Styrning av Radiostyrd Racerbil 24 ctpv mm (0.04f, Q); ModelConstants mc; mc. cv = 1.88 f; mc. cvg = 1.12 f; mc. cs = f; mc. cvs = f; Failsafemodel fm (0.01f, mc ); cinfo. usehightraj = false ; cinfo. crash = true ; /* Initialize crash speed to something big */ cinfo. v_crash = ; cinfo. trajnrhigh = 0; cinfo. trajnrlow = 0; float v_pred ; char tmpwrite [256]; /* Loop over all trajectories */ for ( int tnr = 0; tnr < nroftrajhigh ; tnr ++) { list < float >:: iterator ugit = ugprev. begin (); list < float >:: iterator usit = usprev. begin (); /* Predict the state 4 samples ahead */ // xp = mm. f( currentstate ); xp = currentstate ; for ( int k = 0; k <4; k ++) { fm. mconstants.ug = * ugit ; fm. mconstants.us = * usit ; ugit ++; usit ++; fm. predictstate (xp ); # if savepred if( tnr == 0) { /* Write states */ sprintf_s ( tmpwrite,"%d %d %d %f %f %f %f %f %d\n", * framecount,0,0, xp.get ( x_index_ ), xp.get ( y_index_ ),xp.get ( v_index_ ), xp.get ( th_index_ ),xp.get ( thd_index_ ),0); predictfile. write (( const char *) tmpwrite, strlen ( tmpwrite )); } # endif } /* Check if the speed is above splitspeed */ while (xp.get ( v_index_ ) > splitspeed ) { cinfo. usehightraj = true ; /* Predict the high velocity motion model */ motionvechigh.at(tnr )-> predictstate (xp ); v_pred = xp. Get ( v_index_ ); /* Check if the car is on the track */ ontrack = trackint - > carontrack ( xp ); # if savepred sprintf_s ( tmpwrite,"%d %d %d %f %f %f %f %f %d\n", * framecount, tnr +1,0, xp.get ( x_index_ ), xp.get ( y_index_ ),xp.get ( v_index_ ), xp.get ( th_index_ ),xp.get ( thd_index_ ), ( int )(! ontrack )); predictfile. write (( const char *) tmpwrite, strlen ( tmpwrite )); # endif /* If the car is not on the track, then a crash is detected */

31 Optimal Styrning av Radiostyrd Racerbil 25 } if (! ontrack ) { /* Check if the new crash speed is less than the previous one */ if( v_pred < cinfo. v_crash ) { cinfo. v_crash = v_pred ; cinfo. trajnrhigh = tnr +1; break ; } } /* For every low speed trajectory, predict until either crash or stand still */ for ( int tnrlow = 0; tnrlow < nroftrajlow ; tnrlow ++) { xplow = xp; while (1) { /* Predict next state */ motionveclow.at( tnrlow )-> predictstate ( xplow ); v_pred = xplow. Get ( v_index_ ); # if savepred # endif } } } /* Check if this position is on the track */ ontrack = trackint - > carontrack ( xplow ); sprintf_s ( tmpwrite,"%d %d %d %f %f %f %f %f %d\n", * framecount, tnr +1, tnrlow +1, xplow. Get ( x_index_ ), xplow. Get ( y_index_ ), xplow. Get ( v_index_ ), xplow. Get ( th_index_ ), xplow. Get ( thd_index_ ), ( int )(! ontrack )); predictfile. write (( const char *) tmpwrite, strlen ( tmpwrite )); /* If the car is not on the track, then a crash is detected */ if (! ontrack ) { /* If this crash speed is lower than the previous one */ if( v_pred < cinfo. v_crash ) { cinfo. v_crash = v_pred ; cinfo. trajnrlow = tnrlow +1; cinfo. trajnrhigh = tnr +1; } break ; } else if( v_pred <= 0.05) { cinfo. crash = false ; break ; } } return cinfo ; 7.3 Gränssnitt I detta avsnitt beskrivs den information som failsafesystemet använder Insignaler ˆx Bilens skattade tillstånd. Ges av målföljningsalgoritmen.

32 Optimal Styrning av Radiostyrd Racerbil 26 u Styrsignaler som kommer från regulatorn. Styrsignalerna är på formen u = [u g u s ] T där u g är gaspådraget och u s styrvinkelsignalen. på/av Boolesk variabel från baninterfacet som avgör om bilen är på eller av banan för ett givet tillstånd Utsignaler x ũ Tillståndsvektor som skickas till baninterfacet för att avgöra om en bil i detta tillstånd är på banan eller ej. Styrsignal från failsafesystemet till handkontrollens radiomodul. 8 Regulator Regulatorns uppgift är att få bilen att följa en referensbana i en viss referenshastighet(som kan variera med positionen). Det som då är intressant är hur långt från referensbanan som bilen befinner sig (r e ), hur snabbt den kör (v) samt dess avvikelse från referensvinkeln (θ e = θ ref θ där θ är bilens nuvarande riktning). Definitionen av dessa storheter ses i figur 16. Avvikelsen från referensbanan r e är positiv då bilens masscentrum befinner sig till höger om referensbanan och negativ till vänster. Då bilen oftast styr mer åt endera hållet (det är svårt att trimma in kontrollen exakt) så införs även ett tillstånd som integrerar reglerfelet i sida. Detta möjliggör integralverkan i LQ-återkopplingen. θ ref θ, v x θ e r e ṙ e Figur 16: Variabler som är intressanta för regulatorn.

33 Optimal Styrning av Radiostyrd Racerbil Huvudregulator Huvudregulatorn implementeras som en linjär tillståndsåterkoppling u = L( ẑ + r) där ẑ = M ˆx är de storheter som regleras. Matrisen L har bestämts med hjälp av minimering av linjärkvadratiska kriterier (LQ-teknik). De styrsignaler som är framräknade offline ger huvudbidraget till styrsignalerna och återkopplingen ska enbart kompensera för modellfel och störningar. Se figur 17. Referensinterface u offline r L Lr u Failsafe ũ Bil Lẑ L ẑ Referensinterface Figur 17: Blockschema beskrivande det tillståndsåterkopplade systemet. Två olika regulatorförstärkningar har tagits fram för systemet. Den ena används för att köra den långsamma testbanan där kraven verifieras, och den andra används då en optimal trajektoria ska följas och är den som normalt används. Då en linjär modell används kommer regulatorn fungera bäst i ett visst arbetsområde och av den anledningen används olika förstärkningar för de olika fallen. För att ändra förstärkningen så ändrar man motsvarande parametrar i filen för inställningar. Vid framtagning av förstärkningen för låg hastighet (L l ) användes straffmatrisen Q l = för tillstånden. Vid framtagning av förstärkningen för hög hastighet (L h ) användes istället straffmatrisen Q h = För båda förstärkningarna användes straffmatrisen för styrsignalen. R = [ ]

34 Optimal Styrning av Radiostyrd Racerbil 28 Med dessa tre straffmatriser erhölls de två förstärkningarna [ ] L l = [ ] L h = Styrsignalväljare I nuvarande implementering av systemet så är styrsignalväljarens uppgift att välja om regulatorn för manuellt läge eller den för autonomt läge ska användas. Detta val baseras på vilket läge som användaren valt att köra programmet i. Programkoden för styrsignalväljaren är förberedd så att mer avancerade valmöjligheter kan läggas till om så önskas. u h Inläsning handkontroll Huvudregulator Ytterlägesregulator Styrsignalväljare u Regulator Figur 18: Blockschema beskrivande styrsignalväljarens funktion. 8.3 Bilmodell för regulatorsyntes Regulatorn använder de storheter som är definierade i figur 16 på sidan 26 för att reglera bilen. För att kunna utföra regulatorsyntes med hjälp av LQ-teknik önskas en enkel linjär modell som beskriver hur regulatorn kan påverka bilens tillstånd via styrsignalerna. Den bilmodell som användes i optimeringen är uttryckt i ett annat koordinatsystem än det som regulatorn arbetar med. Därför togs en separat modell fram för regulatorsyntesen som sedan parametersattes för att beskriva bilens dynamik tillfredställande i regulatorns arbetsområde. En olinjär modell med tillstånden r e, v, θ e och r int,e = r e dt kan bestämmas genom att studera problemets geometri samt utgå från generella fysikaliska samband. För att modellera förändringshastigheten för r e (ṙ e ) så antas det att denna är projektionen av hastighetsvektorn på referensbanans normal. En förenklad modell av accelerationen fås genom att använda Newtons andra lag (F = ma). Kraften som verkar i bilens longitudinella riktning antas vara summan av en hastighetsberoende friktion samt motorns drivande kraft. Dynamiken i motorn negligeras och då är kraften direkt proportionell mot gaspådraget. Vinkelhastigheten modelleras som en linjär funktion av styrutslaget. Den olinjära modell

35 Optimal Styrning av Radiostyrd Racerbil 29 för dynamiken som kondenseras ur resonemanget ovan blir då ṙ e = v sin(θ e ) (16) v = c v v + c v,g u g (17) θ e = c θ,s u s (18) ṙ int,e = r e (19) Mätningarna för de fyra tillstånden skapas i referensinterfacet och mätekvationen h(x) blir r e y = h(r e, v, θ e, r int,e ) = v θ e (20) r int,e där y är de (transformerade) mätningar som regulatorn får. För att få en linjär modell så linjäriseras modellen ovan kring en viss arbetspunkt. Regulatorn kommer att försöka styra systemet mot r e = 0 och θ e = 0, varför dessa värden väljs som arbetspunkt för linjäriseringen. Hastigheten och intregralen har, till skillnad från de andra tillstånden, ingen konstanta arbetspunkter v = v och r int,e = rint,e att linjärisera kring. Linjäriseringshastigheten v får istället bestämmas så att en bra avvägning mellan modellens validitet i kurvor och på raksträckor erhålls. Värdet på rint,e sattes till noll. För linjärisera på ett smidigt sätt så definieras tillståndvektorn som x = [ r e v θ e r int,e ] T och insignalvektorn till u = [ u g u s ] T. Modellen kan då att skriva som ẋ = f(x, u) (21) y = h(x) (22) där och x 2 sin(x 3 ) f(x, u) = c v x 2 + c v,g u 1 c θ,s u 2 (23) x 1 h(x) = x 1 x 2 x 3 x 4 (24) är vektorvärda funktioner. Linjäriseringen sker kring arbetspunkten x = [ 0 v 0 0 ] T samt ū = [0 0] T och beräknas enligt ẋ f( x) + f(x) x (x x) + f(x) x= x u (u ū) (25) x= x y y( x) + h(x) x (x x) (26) x= x

36 Optimal Styrning av Radiostyrd Racerbil 30 Denna approximation ger slutligen att ṙ e = v θ e (27) v = c v v + c v,g u g (28) θ e = c θ,g u s (29) ṙ int,e = r e (30) y 1 = r e (31) y 2 = v (32) y 3 = θ e (33) y 4 = r int,e (34) och denna modell kan skrivas på matrisform enligt 0 0 v ẋ = 0 c v x + c v,g 0 0 c θ,s u (35) y = x (36) Gränssnitt I detta avsnitt beskrivs all information som regulatorn anänder och skapar Insignaler ˆx y v ref u offline u h Skattade tillståndet hos bilen. Kommer från målföljningssystemet. Mätsignaler som används av regualtorn. Referenshastigheten för bilen. Offlineberäknade styrsignaler. Används i framkoppling. Styrsignalen från handkontrollen. Om denna används bestäms av styrsignalväljaren Utsignaler u Önskade styrsignaler. Skickas till failsafesystemet. 9 Baninterface Baninterfacet agerar länk mellan banrepresentationen, som specificeras offline, och failsafesystemet som körs online. Utifrån informationen om banans utseende kan baninterfacet bestämma om en punkt ligger på banan eller ej. Detta används av failsafesystemet för att avgöra om en predikterad undanmanöver leder utanför banan eller ej.

37 Optimal Styrning av Radiostyrd Racerbil 31 Banan representeras av polygoner där banans ytter- respektive innerkant representeras av en sluten positivt orienterad kurva uppbyggd av punkter samanbundna med raka linjer. Detta gör det möjligt för baninterfacet att avgöra om en punkt ligger på banan med hjälp av en variant av argumentvariationsprincipen. 9.1 Algoritm för att avgöra ifall en punk ligger innanför banan 5 s p1 9 p3 2 8 p2 1 p4 Figur 19: Skiss med exempelkurva och exempelpolygon. Låt θ beteckna den vinkel som bildas mellan horisontallinjen och den vektor som går från punkten p 1 till godtycklig punkt på kurvan S i figur 19. Med denna definition av θ gäller då θ = 2π S om kurvan S genomlöpes i positiv riktning och det samma kommer även att gälla för samtliga andra punkter som ligger innanför det av kurvan begränsade området. För samtliga punkter utanför det av kurvan begränsade området, exempelvis p 2, gäller att θ = 0. Ovanstående gäller alltså vid genomlöpning av den kontinuerliga kurvan S. S Låt θ i beteckna den vinkel som bildas mellan vektorn från punkten p 3 till polygonpunkten i och vektorn från p 3 och punkten i + 1. Analogt med ovan så gäller då θ i = 2π (37) i och det samma kommer även att gälla för alla andra punkter som ligger innanför det av polygonen begränsade området. För samtliga punkter utanför det av polygonen begränsade området, exempelvis p 4, gäller på samma sätt att θ i = 0. i Ett enkelt kriterium för om en punkt p ligger innanför polygonen blir därför θ i π. (38) i Ovanstående gäller även om en yttre polygon skärs av en en inre, vilket kommer vara fallet för vår bana. Vinkelsumman blir då 2π för punkter som ligger i den yttre men inte den inre och 0 för samtliga andra.