Teknisk Dokumentation

Transkript

1 No Oscillations Corporation Teknisk Dokumentation Optimal Styrning av Autonom Racerbil Version 1.0 Författare: Mikael Rosell Datum: 29 december 2013 Status Granskad Alla Godkänd

2 Projektidentitet E-post: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Kristoffer Lundahl, Linköpings Universitet Telefon: , E-post: Daniel Axehill, Linköpings Universitet Telefon: , E-post: Daniel Axehill, Linköpings Universitet Telefon: , E-post: Mikael Rosell Isak Nielsen, Linköpings Universitet Telefon: , E-post: Gruppmedlemmar Namn Ansvarsroller Telefon E-post Mikael Rosell (MR) Projektledare (PL) mikro418 Victor Carlsson (VC) Informationsansvarig (IA) vicca226 Henrik Edlund (HE) Hårdvaruansvarig (HA) hened061 Mathias Hallmén (MH) Testansvaring (TA) matha848 Sofia Johnsen (SJ) Designansvarig (DA) sofjo100 Lukas Lorentzon (LL) Figuransvarig (FA) luklo656 Dennis Lundström (DL) Dokumentansvarig (DOC) denlu994 Oskar Sunesson (OS) Leveransansvarig (LA) osksu783

3 Dokumenthistorik Version Datum Förändringar Signatur Granskad Första utkastet Alla Alla Ändringar gjorda efter kommentarer Alla Alla från handledare Ändringar gjorda efter kommentarer Alla Alla från beställare Ändringar gjorda efter kommentarer Alla Alla från handledare Ändringar gjorda efter kommentarer från beställare. Alla Alla

4 Innehåll 1 Inledning Syfte och mål Användning Definitioner Systemöversikt Hårdvara Mjukvara Koordinatsystem Reglersystem Gränssnitt Referensdata Implementation av PID-regulator Reglering av styrservo Uppdelning av banan i kurvor och raksträckor Avståndsregulator Vinkelregulator Framkopplingslänk Utvärdering Övrigt Reglering av hastighetsprofil Hastighetsregulator Utvärderingsalgoritm Q-learning Algoritm Implementering Utvecklingsmöjligheter Simuleringssystem Gränssnitt Fordonsmodell från projekt Rörelseekvationer Krafter Slipvinklar Styrvinkel Fordonsmodell från projekt Rörelseekvationer Krafter Styrvinkel Simulator Modellbegränsningar Banbegränsning Moduluppbyggnad Rörelsemodell

5 4.5.2 Bilbana Regulator Referenstrajektoria Implementation Utvecklingsmöjligheter Ytterligare modellbegränsningar Knappläsningar Målföljningssystem Gränssnitt Höjdgeometri Positioner och geometrier Sökalgoritm Projicering av markörpositioner Mönstersökning Special Orthogonal Procrustes Problem Bortkastning av orealistiska mätningar Filter Visualiseringssystem Gränssnitt Höjdgeometri Mellantider Textutskrift via projektorn Rita trajektorier och bilar

6 Optimal Styrning av Autonom Racerbil 1 1 Inledning Detta system är en vidareutveckling av det system som itererats fram i flera tidigare projekt, bl.a. projektet OSARR 2011 och VOSARR Systemet består av en bilbana, radiostyrda bilar, kameror för positionering av bilarna, en dator med styrsystem och en projektor. Målet i detta projekt har varit att få en mindre oscillativ banföljning och en mer robust reglering. En simulator för de radiostyrda bilarna har utvecklats för att kunna testa systemet utan att behöva köra det fysiska systemet. Dessutom har systemet utökats med en projektor som har förbättrat demoeffekten genom att exempelvis projicera simuleringarna på bilbanan eller spela upp tidigare körningar. I detta dokument beskrivs hur systemet fungerar, vilken funktionalitet som finns implementerad samt diskussioner kring hur systemet kan vidareutvecklas. 1.1 Syfte och mål Syftet med projektet har varit att förbättra det system som redan fanns implementerat sedan tidigare i syfte att göra demonstration av systemet mer attraktivt. Detta genom att få en snyggare banföljning och höja demo-effekten med hjälp av en projektor. Tidigare system uppvisade ett oönskat oscillativt beteende och målet har varit att i största möjliga mån bli av med detta. 1.2 Användning Systemet ska efter slutleverans användas i demonstrationssyfte av avdelningen för reglerteknik på ISY. Det ska även användas inom forskning, utbildningssyfte, laborationer och studentprojekt. 1.3 Definitioner Här definieras begrepp som används i dokumentet. Optimal trajektoria: Förgåenden projektgrupp tog fram ett referensspår genom en optimering med avseende på varvtid, som utfördes med hjälp av deras framtagna bilmodell. Kalibreringsplan: Det plan som är parallellt med bilbanans plan och skär kalibreringsmarkörernas positioner. Koordinater: Variabler i det globala koordinatsystemet betecknas med stora bokstäver och variabler i bilens koordinatsystem betecknas med små. 2 Systemöversikt Systemet består av hårdvara och mjukvara med flera olika delsystem. Önskad trajektoria som bilen skall följa är beräknad i tidigare projekt. Denna innehåller referenstillstånd i form av position, hastighet, vinkelhastighet och offlineberäknade styrsignaler. I detta projekt används endast position och vinkelhastighet.

7 Optimal Styrning av Autonom Racerbil Hårdvara Systemet består av följande hårdvara: Bilbana Radiostyrda bilar Radiostyrd lastbil Handkontroller Två infraröda kameror (IR-kameror) Projektor Dator Hur hårdvarukomponenterna interagerar med varandra beskrivs i Figur 1. Bilbanan består av en bana uppbyggd av golvplattor och väggar i skumgummi. På bilbanan körs en radiostyrd bil som styr via handkontroll, antingen av datorn eller manuellt av användaren. IR-kamerornas bilder skickas till datorn där bildhanteringsalgoritmer används för att skatta bilarnas position, hastighet och vinkelhastighet utifrån markörerna på bilarna. Detta sker med samplingsfrekvensen 100 Hz. Projektorn används för att visa information på bilbanan. All beräkning och funktionalitet finns implementerad i mjukvaran på datorn. Figur 1: Informationsflöde mellan hårdvarukomponenter. Bilder av bilbanan från IRkamerorna behandlas i datorn för att bland annat skatta bilarnas postion. Med hjälp av skattningarna skapar datorns mjukvara en styrsignal till fjärrkontrollen och visuella effekter via projektorn.

8 Optimal Styrning av Autonom Racerbil Mjukvara Systemet är uppdelat i följande delsystem: Reglersystem Simuleringssystem Målföljningssystem Visualiseringssystem Informationsflödet mellan delsystemen sker enligt Figur 2. Figur 2: Informationsflöde mellan delsystemen. Grönt motsvarar hårdvara och blått mjukvara. Tillstånden är position, hastighet och vinkelhastighet. Styrsignalerna är gaspådrag samt signal till styrservot. Mjukvaran är implementerad i C++. Programmet har behov av att köra flera funktioner simultant. I C++ kan ett program delas upp i trådar vilket möjligör att flera funktioner kan exekveras samtidigt på var sin tråd. Mjukvaran är uppbygg av fyra trådar: Gui thread: Hanterar gränsnittet. Run thread: Hanterar målföljningsystemet och vid simulering körs simuleringssystemet här. Regulator thread: Hanterar reglersystemet. Draw thread: Hanterar visualiseringssystemet. Mellan trådarna delas data via tre dataobjekt, RegulatorThreadData,DrawThreadData och sentfrommainthreadstruct. sentfrommainthreadstruc hanterar data mellan Gui thread och Run thread. DrawThreadData hanterar data som delas mellan Run thread och Draw thread. RegulatorThreadData hanterar data mellan Run thread och Regulator thead. 2.3 Koordinatsystem Figur 3 visar hur bilens koordinatsystem förhåller sig till det globala koordinatsystemet. För att räkna om rörelser från bilen koordinatsystem till det globala koordinatsystemet används (1) (4).

9 Optimal Styrning av Autonom Racerbil 4 Y x Y y X X Figur 3: Bilens koordinatsystem {ˆx, ŷ} i det globala koordinatsystemet { ˆX, Ŷ }. Framåt på bilen definieras i positiv ˆx-riktning. v X = v x cos θ + v y sin θ (1) v Y = v x sin θ v y cos θ (2) a X = a x cos θ v x θ sin θ + ay sin θ + v y θ cos θ (3) a Y = a x sin θ + v x θ cos θ ay cos θ + v y θ sin θ (4) Där θ är vinkelhastigheten för bilen, vilket är densamma i båda koordinatsystemen. θ definieras enligt Figur 3. 3 Reglersystem Reglersystemet delas upp i två delsystem. Ett system som styr bilens styrservo, dvs styrvinkeln på framhjulen, och ett system som styr bilens hastighetsprofil. Samtliga regulatorer är av typen PID. För att styra hastighetsprofilen finns även ett Q-learningsystem implementerat, dock enbart i simulatorn. På sikt kan Q-learningsystemet implementeras i det riktiga systemet. 3.1 Gränssnitt Nedan beskrivs de in- och utsignaler som används inom reglersystemet. Reglering av styrservo Insignaler: θ avg e Utsignaler: u s, re avg, θ Adaptiv hastighetsstyrning Insignaler: r e, θ, (X, Y )

10 Optimal Styrning av Autonom Racerbil 5 Utsignaler: ug θeavg och reavg a r medelva rdesbildade signaler fra n tva pa varandra fo ljande ma tningar av bilens avvikelse fra n referensspa ret i vinkel och avsta nd, θe respektive re, se (6) och (7). Detta go rs fo r att minska brus. us och ug a r utsignalerna fra n regleringen av styrservot respektive den adaptiva hastighetsstyrningen, hur dessa ra knas fram beskrivs mer i detalj i Avsnitt 3.4 och Avsnitt 3.5. Signalerna θ, θref, θe och re finns fo rtydligade i Figur 4. θref θ θe re Figur 4: Variabler som anva nds i reglersystemet. re a r avsta ndet mellan bilens mittpunkt och referensspa ret. θe a r skillnaden mellan bilens vinkel och referensspa rets vinkel. 3.2 Referensdata I projektet 2012 [5] togs en optimala trajektoria fram som inneha ller information om: X- och Y- position fo r referensspa ret pa bilbanan. Referensspa ret a r uppdelat i 1001 punkter. hastighet i longitudinell led, vx, och transversell led, vy girvinkel θ girhastighet θ styrsignaler us och ug I detta projekt har de fo rbera knade hastigheterna och styrsignalerna uteslutits och reglering sker endast mot X- och Y -position, girvinkel och girhastighet. Bilbanans begra nsingar och det optimala spa ret info rda i det globala koordinatsystemet ses i Figur 5. Kursnamn: Projektgrupp: Kurskod: Projekt: Reglerteknisk projektkurs No Oscillations TSRT10 OSAAR E-post: Dokumentansvarig: Fo rfattarens e-post: Dokument: noosc@googlegroups.com Mikael Rosell mikro418@student.liu.se Teknisk dokumentation.pdf

11 Optimal Styrning av Autonom Racerbil 6 Figur 5: Det globala koordinatsystemet med begränsningar för bilbanan i svart och det förberäknade optimala spåret i blått. 3.3 Implementation av PID-regulator Använda PID-regulatorer är implementerade genom funktionen PIDcontroller tillhörande klassen PID enligt ekvation (5), där e(t) är reglerfelet, u(t) styrsignal och K, T i, T d regulatorparametrar. Kod för integratoruppvridning och stötfri övergång vid parameterbyten finns även implementerad. t ) u(t) = K (e(t) + 1Ti de(t) e(t)dt + T d (5) 0 dt 3.4 Reglering av styrservo Två sammankopplade PID-regulatorer med framkoppling har implementerats som styrregulator. De tre delarna är följande: Avståndsregulator: Använder mätsignalen r avg e och ger styrsignalen u 1. Vinkelregulator: Använder mätsignalen θ avg e och ger styrsignalen u 2. Framkopplingslänk: Använder sig av θ och ger styrsignalen u ff. Dessa tre delar ger alltså upphov till var sin styrsignal som sedan summeras ihop enligt u s = u 1 + u 2 + u ff, se Figur 6. Styrsignalernas betydelse bestäms med hjälp av storleken på parametrarna i PID-regulatorerna samt en viktfunktion för framkopplingslänken. För att undvika att brus från tillståndsskattningarna i målföljningssystemet uppkommer i styrsignalen använder avståndsregulatorn och vinkelregulatorn medelvärdesbildade reglerfel re avg respektive θe avg. Dessa skapas genom medelvärdesbildning på nuvarande och föregående reglerfel enligt ekvation (6) och (7), där re prev och θe prev är föregående reglerfel och r e och θ e nuvarande.

12 Optimal Styrning av Autonom Racerbil 7 Figur 6: Struktur för regulatorn som styr bilens styrservo. Regulatorn består av tre delar: En framkoppling av referensspårets girhastighet θ samt två PID-regulatorer som styr på avståndsfelet r e respektive vinkelfelet θ e. r avg e = r e + re prev 2 (6) θ avg e = θ e + θe prev 2 (7) Uppdelning av banan i kurvor och raksträckor För att uppnå bra referensföljning i kurvor krävs en snabb regulator. Då en snabb regulator gav upphov till oscillationer på banans raksträckor har bilbanan delats upp i kurviga partier och raka partier, se Figur 7. Om bilen befinner sig i en kurva eller på en raksträcka avgörs genom att titta på absolutbeloppet av den förberäknade girhastigheten θ tillhörande bilens närmsta position på referensspåret. Ett stort värde på girhastigheten innebär att referensspåret svänger mycket. På detta sätt kan kurvor upptäckas. I PID-regulatorerna används olika parametrar beroende på om bilen befinner sig på en raksträcka eller i en kurva, se Tabell 1 och 2. Då θ > 1.0 rad/s används parametersetet för kurva, annars befinner sig bilen på en raksträcka och tillhörande parameterset används. Definitionen av raksträcka samt höger- och vänstersväng på referensspåret finns utmarkerat i Figur 7. Gränsen θ > 1.0 rad/s testades fram under körning på bilbanan Avståndsregulator Avståndsregulatorn reglerar på reglerfelet r e och beroende på om bilen befinner sig i en kurva eller på en raksträcka väljs olika regulatorparametrar. I kurvor används en snabb regulator för att bilen ska ha bra följning. På raksträckor används en långsammare regulator för att undvika oscillationer. De valda parameterseten finns beskrivna i Tabell 1.

13 Optimal Styrning av Autonom Racerbil 8 Figur 7: Referensspåret med de delar som räknas som vänster- och högersväng utmarkerade med cyanblått respektive magenta och raksträckan utmarkerad med mörkblått. Denna uppdelning används då parameterset för vinkel- och avståndsregulator väljs samt för bestämning av skalfaktor i framkopplingslänken. Tabell 1: Här visas avståndsregulatorns valda parameterset för raksträcka och kurva. K T i T d Raksträcka Kurva Vinkelregulator Vinkelregulatorn reglerar på reglerfelet θ e och har, likt avståndsregulatorn, två olika moder, med olika regulatorparametrar, beroende på om bilen befinner sig i en kurva eller på raksträcka. De valda parameterseten finns beskrivna i Tabell 2. Tester visade att vinkelregulatorn ej behövdes i kurvor och därför sattes K = 0. Parameterbytet är kvar i implementationen eftersom det kan undersökas ytterligare i kommande projekt. Tabell 2: Här visas vinkelegulatorns valda parameterset för raksträcka och kurva. Att K = 0 för regulatorn i kurvor innebär att vinkelregulatorn ej används i kurvor. K T i T d Raksträcka Kurva Framkopplingslänk Då girhastigheten θ är ett mått på hur mycket referensspåret svänger kan den användas i en framkoppling i regulatorn som styr bilens styrservo. En framkoppling har implemen-

14 Optimal Styrning av Autonom Racerbil 9 terats enligt u ff = k ff θ. (8) Framkopplingslänken ger bättre följning genom att ge ett större bidrag till styrsignalen då θ är stor. Styrsignalen räknas ut genom (8), där k ff är en skalning och θ girhastigheten tillhörande referensspåret för punkten närmast bilens position. Ett problem som observerats är att bilen vid högersväng tenderar att får en mindre svängradie än den hos referensspåret, medan den vid vänstersväng istället tenderar att få en större svängradie. Därför skalas framkopplingskoefficienten k ff beroende på bilens avstånd från referensspåret enligt Tabell 3. Då bilens beteende visat sig vara olika vid höger- respektive vänstersvängar hanteras dessa skalningar på olika sätt. Om bilen ligger innanför en högerkurva minskas bidraget från framkopplingen för att bilen ska svänga ut till referensspåret. Om bilen ligger utanför referensspåret i en vänstersväng ökas bidraget från framkopplingen så att bilen styr in mot referensen. θ indikerar om bilen är i en höger- eller vänstersväng. r e kan användas för för att kolla om bilen är innanför eller utanför referensspåret i en kurva. Tabell 3: Skalning av styrsignal u ff tillhörande framkopplingslänken beroende på om det är höger- eller vänstersväng och svängradiens storlek relativ referensspårets radie. Vänstersväng ( θ > 1 rad/s) Högersväng ( θ < 1 rad/s) re avg < m k ff = re k ff = re annars k ff = k ff = Utvärdering Då traditionella parameteransatser så som stegsvarsexperiment inte varit möjliga att utföra på systemet har PID-parametrar och viktfunktioner istället tagits fram ad-hoc genom att studera systemets beteende då olika ändringar av parametrar och viktfunktioner gjorts Övrigt Systemet måste kompensera för tidsfördröjningar så korrekt referensföljning erhålls. Tidsfördröjningen motverkas genom att en tidigare framtagen rörelsemodell från failsafesystemet [4] använts för att prediktera framtida tillstånd. Predikteringen görs fyra steg framåt och används sedan då styrsignalen räknas fram. 3.5 Reglering av hastighetsprofil Beräkning av hastighetsprofil görs adaptivt online. Profilen uppdateras under körning av en utvärderande algoritm. Målen med systemet som beräknar hastighetssprofil adaptivt online är att: Systemet ska bli oberoende av att batteriets spänningsnivå sjunker under körning. Systemet ska bli oberoende av däckens slitagenivå.

15 Optimal Styrning av Autonom Racerbil 10 Systemet ska bli oberoende av en förberäknad gas- och bromsprofil och ska istället lära sig att köra så fort det går längs det förberäknade referensspåret. Systemet ska bli oberoende av vilket fordon som används. Hastighetsprofilen består av referensspårets 1001 punkter och kan identifieras av världskoordinaterna (X, Y ). Till varje punkt hör en referenshastighet, så hastighetsprofilen för ett viss punkt k på referensspåret ges av v[k]. Referenshastigheten v[k] uppdateras online utifrån en utvärderingsalgoritm som tar hänsyn till hur bra referensspåret följs och bilen styrs sedan mot referenshastigheten för närmaste punkt på referensspåret med hjälp av en hastighetsregulator Hastighetsregulator Hastighetsregulatorn är en PID-regulator som reglerar på skillnaden i hastighet mellan referenshastigheten, som beräknas enligt Avsnitt 3.5.2, och faktisk hastighet. Utsignalen från hastighetsregulatorn är styrsignalen u g. Valda parametrar för regulatorn finns i Tabell 4. Tabell 4: Här visas hastighetsregulatorn valda parameterset. K T i T d Utvärderingsalgoritm Avståndet mellan bilens mittpunkt och referensspåret, r e, används som grund för att utvärdera möjlig hastighetsprofil i bilens position på bilbanan. För att minimera varvtiden ska bilens hastighet maximeras. Genom att öka hastigheten då bilen följer referensspåret bra och sänka bilens hastighet då avståndet mellan bilens mittpunkt och referensspåret är stort kan detta uppnås. En översiktlig beskrivning av uppdateringen av hastighetsprofilen visas i Figur 8. Utvärderingen av hastighetsprofilen sker i funktionen updatevvec() och profilen består av referensspårets 1001 punkter. Justeringen av hastigheter görs bakåt enligt en skalad viktfunktion i form av täthetsfunktionen för en normalfördelning enligt (9). Standardavvikelsen σ ändras utifrån reglerfelet r e enligt (10) och väntevärdet µ beror av hur många punkter, it, bakåt man uppdaterar samt en skalningsfaktor k var enligt (11). Ett tröskelvärde på r e avgör om viktfunktionen ska användas för att minska eller för att öka hastigheten v[k]. De valda inställningarna för hur långt utvärderingsalgoritmen uppdaterar bakåt och storlek på skalfaktorer beskrivs i Tabell 5. f(i) = 1 σ (i µ) 2 2π e 2σ 2 (9) σ 2 = k var r e µ = it 2 (10) (11) När hastigheten skall öka adderar man viktfunktionen på nuvarande hastighetsprofil och när inbromsning istället skall ske subtraheras viktfunktionen bakåt. För att inbromsning

16 Optimal Styrning av Autonom Racerbil 11 Figur 8: Normalfördelad viktfunktion i uppdateringsfunktionen. Notera att σ 2 1/r e. Av detta följer att ett litet reglerfel ger en kruva som är bred och låg medan ett stort reglerfel ger motsatsen. ska ske i tid har uppdateringen av hastighetsprofilen förskjutits tio steg bakåt vilket innebär viktfunktionen uppdaterar längre bak på hastighetsreferensen v[k]. Ett större reglerfel r e kommer ge en brantare kurva för uppdatering av hastighetsreferensen, enligt Figur 9. Detta ger ett önskat beteendet med aggressiva inbromsningar i nästkommande varv på de positionerna på spåret som kommer före positionen där reglerfelet r e var stort. När bilen följer referensspåret bra, det vill säga då r e är litet, kommer hastigheten istället att öka bakåt. En dödzon är inlagd då reglerfelet ligger mellan 2 och 3cm, detta för att hastighetsprofilen ska konvergera. Tabell 5: Valda inställningar för hur långt utvärderingsalgoritmen uppdaterar bakåt och storlek på skalningsfaktorer använda i utvärderingsalgoritmen för den adaptiva hastighetsprofilen. Skalningsfaktor (k var ) Antal it. (it) Minska hastighet ( r e > 0.03) Öka hastighet ( r e < 0.02)

17 Optimal Styrning av Autonom Racerbil 12 Figur 9: Viktfunktion för uppdatering av hastighetsreferensen v[k i], där k är index för nuvarande position och i = 1, 2, 3,..., 35 är uppdateringar bakåt av hastigheten, plottad för olika reglerfel r e. Här har 35 iterationer bakåt använts. 3.6 Q-learning En alternativ utvärderingsalgoritm för beräkning av gas- och bromsprofilen med stöd i litteratur är Q-learning [7]. Implementering av Q-learning har endast gjorts i simulatorn och mycket inställningsarbete kvarstår för att uppnå bra resultat. I denna sektion presenteras Q-learningalgoritmen kort samt hur algoritmen implementerats. För en utförligare beskrivning av Q-learning hänvisas läsaren till [8] och [9] Algoritm Q-learning är en reinforcement learning-algoritm och har därför problemformuleringen: Givet tillstånd (states), handlingar (actions) och belöningar (rewards) hitta en policy (en handling för varje tillstånd) som maximerar den totala belöningen. Lösningen till att maximera total belöning är att maximera belöningen i varje tillstånd. För detta behövs en utvärderingsfunktion Q(s, a), vilket är den förväntade belöningen givet tillstånd s och handling a. Uppdatering av Q(s, a) sker enligt ekvation (12). [ ] Q t+1 (s t, a t ) = Q t (s t, a t ) + α r t+1 + γ max Q t (s t+1, a) Q(s t, a t ) (12) a där α [0, 1] är learning rate, γ [0, 1] är discount factor och r t+1 är den observerade belöningen för att ha utfört handling a t i tillstånd s t. Learning rate bestämmer hur stor hänsyn det ska tas till ny information. Om α = 1 används bara senaste informationen och om α = 0 används ingen ny information alls. Discount factor bestämmer hur stor hänsyn det ska tas till framtida belöningar. Om γ = 0 bidrar inga framtida belöningar till den förväntade belöningen och med ökande γ tas mer hänsyn till framtida belöningar. I Algoritm 1 beskrivs Q-learningalgoritmen med ɛ-greedy-strategi för att välja handling. ɛ-greedy-strategin innebär att det med någon sannolikhet ɛ väljs en slumpmässig handling

18 Optimal Styrning av Autonom Racerbil 13 istället för den med högst Q-värde. Detta gör att nya policies kan upptäckas. Algoritm 1: Q-learningalgoritm som väljer handling med ɛ-greedy-strategi från [8]. Initiera Q(s, a); for varje episod do Initiera starttillstånd s; while s inte sluttillstånd do if slumptal ɛ then Välj handling a slumpmässigt; else Välj handling a så att Q(s, a) är maximerad; end Gör handling a, observera belöningen r och nya tillståndet s ; /* Uppdatera Q(s, a) */ Q(s, a) Q(s, a) + α [r + γ max a Q(s, a ) Q(s, a)]; /* Uppdatera tillståndet */ s s ; end end Implementering Tillstånden väljs till segment på banan och handlingarna till att vara diskretiserade gaspådrag. Detta medför att Q(s, a) blir en matris med lika många rader som antalet tillstånd och lika många kolumner som antalet handlingar. Q-matrisen skapas i klassen QTable. Handlingarna är definierade som nio stycken gaspådrag i QTable-klassen. Segmenten bestäms i QLearningAgent-klassen med funktionen initqstates som delar upp referenstrajektorian i segment med 20 punkter vardera. Om 2012 års referenstrajektoria används blir antalet segment 50. Belöningsfunktionen definieras i ekvation (13) (15) och beräknas för varje segment. Belöningsfunktionen är implementerad i calcqreward i klassen QLearningAgent. r T = T (13) { 5000, om banans slut nås r slut = (14) 0, annars { 400, om krasch, backning eller avvikelse större än 10 cm r = (15) r T + r slut, annars T är tiden det tar att köra ett segment. Belöningsfunktionen baseras på att det är mycket bra att klara banan, därav den stora r slut. Dessutom är det bra att köra snabbt därför finns r T. Den är negativ eftersom att maximering av r kommer resultera i att T blir så liten som möjligt. För att få bort dåliga beteenden som till exempel krascher ges stor negativ belöning vid sådana situationer. Q-learningalgoritmens implementering beskrivs i flödesdiagrammet i Figur 10. Implementeringen är byggd på klassen QLearningAgent som innehåller learning rate, discount factor och Q-matris. Flödesdiagrammet beskriver funktionen runqlearning i QLearningAgentklassen. Implementeringen är väldigt lik standard implementeringen Algoritm 1, men den skiljer sig i två avseenden. Den första skillnaden är att det finns ett omstartskriterium, om ett dåligt resultat (krasch, backning eller avvikelse större än 10 cm från trajektorian) registreras börjar algoritmen om. Den andra skillnaden är att en handling inte alltid leder

19 Optimal Styrning av Autonom Racerbil 14 till ett nytt tillstånd. I sådana fall upprepas handlingen tills ett nytt tillstånd observeras och därefter följer implementeringen Algoritm 1. Figur 10: Flödesdiagram över implementeringen av Q-learning algoritmen, motsvarar funktionen runqlearning i klassen QLearningAgent Utvecklingsmöjligheter Den nuvarande implementeringen har inte finjusterade designparametrar så som learning rate, discount factor, tillstånd, handlingar och belöningsfunktion. Det första som bör göras vid utveckling är att granska dessa parametrar. Belöningsfunktionen är i mest behov av finjustering och följande aspekter kan vara intressanta att undersöka. Storleken på belöningarna.

20 Optimal Styrning av Autonom Racerbil 15 Strukturen. Ta hänsyn till reglerfelen mer än endast max avvikelse från trajektorian. Angående tillstånden är nedanstående punkter något som bör utvärderas. Längden på segmenten. Uppdelningen av segmenten. Nuvarande uppdelning är uniform. Det kan tänkas att tätare segment i kurvorna är lämpligt. Utökning av tillstånden med till exempel hastighet och reglerfel. Rörande handlingarna bör diskretiseringens lämplighet granskas, till exempel med avseende på antal nivåer samt fördelning. I framtiden kan det även vara intressant att lägga till servosignalen till handlingarna för att undersöka om Q-learning klarar att reglera hela systemet. Om finjusteringarna ökar dimensionerna för tillstånden och handlingarna kan en Q-matris implementation bli orimlig, i så fall kan mer avancerade implementationer vara intressanta [10]. 4 Simuleringssystem Simuleringssystemet ersätter bilen, bilbanan och målföljningssystemet genom att position, hastighet och acceleration för bilen simuleras i mjukvara. Två rörelsemodeller har implementerats och kan användas för att simulera bilen. Rörelsemodellerna är framtagna och parametriserade av tidigare års projektgrupper [4][5], samt under ett sommarprojekt från 2012 [6]. 4.1 Gränssnitt Nedan beskrivs flödet av in- och utsignaler inom Simuleringssystemet samt till och från de andra delsystemen. Signalen ok? är true om bilen befinner sig på banan, annars false. Simulator Insignaler: v x, v y, θ, X ref, Y ref, θ ref Utsignaler: v x, v y, r e, θ e, X, Y, θ, θ Fordonsmodell Insignaler: v x, v y, θ, u g, u s Utsignaler: v x, v y, θ Referenstrajektoria Utsignaler: X ref, Y ref, θ ref Reglersystem Insignaler: v x, v y, θ, r e, θ e, X, Y Utsignaler: u g, u s

21 Optimal Styrning av Autonom Racerbil 16 Banbegränsning Insignaler: X, Y Utsignaler: ok? Visualiseringssystem Insignaler: X, Y, θ, v x, v y Handkontroll/Tangentbord Utsignaler: u g, u s 4.2 Fordonsmodell från projekt 2012 Denna fordonsmodell bygger på en så kallad single track-modell och dynamiken för bilen ges i form av acceleration i longitudinell- och lateralled samt vinkelled. Fordonsmodellen använder tidigare tillstånd och styrsignal (från regulator eller handkontroll) för att beräkna accelerationerna. Denna modell är framtagen under ett sommarprojekt 2012 och modellen validerades senare i denna projektkurs av projektgruppen från Ekvationerna som beskriver modellen ges nedan och modellparametrarna samt beskrivning av parametriseringen hänvisas till Magnus Almroth tekniska rapport [6] samt den tekniska dokumentationen för projektet 2012 [5] Rörelseekvationer Rörelseekvationerna som beskriver rörelse i longitudinell-, lateral- samt vinkelled. v x = F xr F yf sin δ f m v y = F yf cos δ f + F yr m + v y θ (16) v x θ (17) θ = l 1F yf cos δ f l 2 F yr I z (18) Hastigheterna och accelerationerna i globala koordinater beskrivs enligt ekvationerna (1) (4). Indexen f och r betyder front respektive rear och motsvarar alltså krafter på den främre respektive bakre hjulaxeln Krafter Krafter som verkar på bilen. u g är styrsignalen för gaspådrag. ( Fxi F yi = F y0i (α i ) 1 F xmax ) 2, i = f, r F xr = ma x = m(a 1 v 2 x + B 1 v x + C 1 u g v x + D 1 u 2 g + E 1 u g ) (u g 0) F xr = ma x = m(a 2 v 2 x + B 2 v x + C 2 u g v x + D 2 u 2 g + E 2 u g ) (u g 0)

22 Optimal Styrning av Autonom Racerbil 17 F xf = 0 F xmax = µw W f = l 2W L W r = l 1W L h L ma x + h L ma x F y0f (α) = µw f sin(c f arctan (B f (α + A f ) E f (B f (α + A f ) arctan(b f (α + A f ))))) F y0r (α) = µw r sin(c r arctan (B r (α + A r ) E r (B r (α + A r ) arctan(b r (α + A r ))))) Parameterarna A f och A r finns med i modellen för att kompensera för osäkerheter i målföljningen, vilket vi inte tar hänsyn till i den implementerade modellen i simulatorn Slipvinklar Slipvinkel för fram- och bakaxel. ( ) l 1 θ + Vy α f = δ f arctan α r = arctan ( l 2 θ Vy V x V x ) Styrvinkel Styrvinkeln från styrservosignalen u s. δ f = C 1 u s + C 0 [ ] δ f,max = ±22 [ ] Parametern C 0 representerar ett konstant fel i bilens styrvinkel vilken härhör från att hjulen har ett litet glapp. För simuleringen antas C 0 = Fordonsmodell från projekt 2011 För fordonsmodellen framtagen 2011 antas bilen sakna slip vilket betyder att rörelsemodellen antar att bilen endast har en hastighet i longitudinelled ˆx och alltså saknar hastighet i lateralled ŷ. Dessutom antas styrvinkeln påverka bilens vinkelhastighet linjärt och modellen saknar alltså dynamik i form av vinkelacceleration. Ekvation (19) (26) beskriver bilens rörelser. För härledning av dessa ekvationer samt modellparametrar se [4].

23 Optimal Styrning av Autonom Racerbil Rörelseekvationer Rörelseekvaktioner som beskriver bilens rörelse i longitudinell- lateral- samt vinkelled. ẋ = v (19) v = 1 m (F driv + F res ) + C 5 vδ 2 (20) 1 θ = C 3 δ(1 C 4 v) v + C 6 (21) Ẋ = v cos θ (22) Ẏ = v sin θ (23) Krafter Krafter som verkar på bilen. F driv = K d u g (24) F res = C 1 v + C 2 sign(v) (25) Styrvinkel Styrvinkel från styrservosignalen u s. δ = K s u s (26) 4.4 Simulator Genom att integrera accelerationen i varje led fås hastigheterna v x och v y, positionen (X, Y ), girhastigheten θ och girvinkeln θ för bilen, se (27) (28). Denna fiktiva mätuppdatering kan tänkas som en ersättning till målföljningen i det fysiska systemet. X(t) = X(t T ) + T v X (t T ) + T 2 2 v X(t T ) (27) v x (t) = v x (t T ) + T v x (t T ), (28) där accelerationerna v x, v y och θ beräknas med fordonsmodellen. Notera att ekvationerna för Y -koordinaten samt vinkeln θ inte presenteras här men kommer att motsvara dem för X-koordinaten. Steglängden T måste i dessa ekvationer vara tillräckligt liten för att få en stabil simulering. Vid val av för stort T tas för stora steg mellan de fiktiva mätuppdateringarna och simuleringen riskerar att driva iväg. Detta har dock inte varit ett problem då simuleringen beter sig önskvärt med den samplingstid som används för det fysiska systemet, vilken är T = 0.01s Notera att de två fordonsmodellerna skiljer sig åt men att integrationen i simulatorn är generaliserad för en modell som liknar den modell som togs fram 2012, alltså då man räknar med slip samt vinkelacceleration. Det antas alltså att fordonsmodellen beräknar acceleration i longitudinell- och lateralled samt vinkelaccelerantion. Detta gör dock inte den fordonsmodell som tagits fram i projektet Denna modell beräknar endast acceleration i longitudinelled och eftersom den saknar slip beräknas ingen hastighet i lateralled. Då integration görs för modellen i projekt 2011 antas acceleration och hastighet i lateralled samt vinkelacceleration vara noll. Man kan alltså använda samma ekvationer för

24 Optimal Styrning av Autonom Racerbil 19 integeringen både för modell 2012 och modell Detta gör det enkelt att byta mellan fordonsmodellerna. För regleringen av bilen behövs dessutom felen i position och vinkel relativt referenstrajektorian. Dessa tas fram enligt (29) (30). r e = ± (X X ref ) 2 + (Y Y ref ) 2 (29) θ e = θ ref θ (30) Vilken punkt på referenstrajektorian som skall användas avgörs genom att minimera (29) kring några punkter framåt och bakåt från den tidigare använda punkten på referenstrajektorian. Första gången simulering körs initieras bilen till en punkt på referenstrajektorian och alltså kommer r e och θ e att vara noll Modellbegränsningar Om modellerna implementeras i sin grundform utan begränsningar kan bilen hamna i ofysikalisa tillstånd. Till exempel finns det inget i modellerna som hindrar bilen från att rotera kring sin mittpunkt utan att ha någon hastighet. På grund av sådana situationer finns det i simulatorn en del begränsningar och villkor som måste uppfyllas. Dessutom har en körning med den fysiska bilen utvärderats i Matlab för att sätta begränsningar på maximala/minimala hastigheter och accelerationer. För ytterligare information se avsnitt Banbegränsning Vid simulering av körning på bilbanan måste en kontroll göras för att säkerställa att bilen befinner sig på banan. Detta görs genom att använda den algoritm som togs fram av 2011 års projekt. Algoritmen läser in banans utseende från filen track.txt och kan sedan avgöra om en punkt befinner sig på banan eller ej. Detta görs för bilens fyra hörn för att hela bilen skall befinner sig på banan. 4.5 Moduluppbyggnad Den framtagna simulatorn är moduluppbyggd vilket underlättar modifieringar och utbyte av enstaka delar. I Figur 11 presenteras en schematisk bild över simulatorns moduler och vilka signaler som skickas mellan dessa samt flödet av signaler mellan andra delsystem utanför simulatorn. Denna figur är för fallet då simulatorn körs tillsammans med regulatorn och referenstrajektorian. Simuleringssystemet kan även köras genom att styra bilen manuellt med handkontroll/tangentbord vilket innebär att regulatorn och referenstrajektorian inte används. Simulatorn kan alltså köras i två olika uppsättningar där styrsignalen i ena fallet kommer från regulatorn och i andra fallet från handkontrollen/tangentbordet. Se Figur 12 för en överblick av systemet då simulering körs manuellt. I styckena beskrivs kortfattat hur de olika modulerna kan bytas ut.

25 Optimal Styrning av Autonom Racerbil 20 Figur 11: O versikt o ver Simuleringssystemet med vilka signaler som skickas mellan moduler. Regulator och referenstrajektoria anva nds. Figur 12: O versikt o ver Simuleringssystemet vilka signaler som skickas mellan moduler. Manuell styrning anva nds Ro relsemodell Ro relsemodellen byts ut genom att skapa en ny CarModell -klass som man sedan la ter simulator -klassen a rva. Dessa klasser inneha ller bilens tillsta nd i form av acceleration, hastighet samt position. De klasser som har implementerats i detta projekt a r simcarmodel2012 vilken utga r fra n 2012 a rs modell och simcarmodel2011 som utga r fra n den modell som togs fram Notera att enligt Figur 11 och 12 antas det att Fordonsmodulens utsignaler a r v x, v y och θ men att i fordonsmodellen fra n 2011 bera knas vinkelhastigheten θ direkt vilket alltsa inskra nker mot grundide n i modula riteten hos Simuleringssystemet. Detta va llar dock inga problem annat a n det bryter mot konventionen i systemo versikten i figurerna. I implementationen av Simuleringssystemet inneha ller fordonsmodellerna alla tillsta nden fo r bilen, ba de acceleration och hastighet, och fordonsmodellen kan alltsa a ndra hastigheten direkt. Simulatorn a rver publikt fra n fordonsmodellen och kommer a t tillsta nden via arvet na r position integreras fram Bilbana Bilbanan byts ut genom att a ndra i eller byta ut filen track.txt da r banans utseende definieras. Denna fil la ses in av programmet fo r utritning samt fo r att avgo ra om en bil Kursnamn: Projektgrupp: Kurskod: Projekt: Reglerteknisk projektkurs No Oscillations TSRT10 OSAAR E-post: Dokumentansvarig: Fo rfattarens e-post: Dokument: noosc@googlegroups.com Mikael Rosell mikro418@student.liu.se Teknisk dokumentation.pdf

26 Optimal Styrning av Autonom Racerbil 21 befinner sig inom banans gränser Regulator En ny regulator skapas genom att definiera en ny TotController-klass. Denna ska innehålla en funktion CalculateControl som använder reglerfel, hastighet samt referensindex som insignal och ger styrsignaler som utsignal Referenstrajektoria Liksom bilbanan byts referenstrajektorian ut genom att ändra i eller byta ut en fil. I detta fall ref.txt som innehåller referenser i form av position och hastighet. 4.6 Implementation Simulatorn har liksom övriga projektet implementerats i C++ och kan köras fristående på en dator eller i billabbet på projektdatorn tillsammans med projektorn och handkontrollen för bilen. Koden för simulatorn är uppbyggd enligt den kod som tagits fram i tidigare års projekt, detta program benämns Racetrack. Simulatorns huvud-loop följer samma mönster som huvud-loopen i den funktion som kallas Run som definieras i filen racetrack.cpp som tillhör Racetrack. Motsvarande funktion i simulatorn heter Simulate och finns definierad i filen runsimulator.cpp. En hel del kod har återanvänds från Racetrack, så som uppritning av bilbana/bil, implementering av regulatormodul samt hantering av referenstrajekoria och banbegränsningar. För att kunna välja om man vill köra simulatorn i billabbet tillsammans med handkontroll och projektor eller fristående på en vanlig dator används ett kompileringsvillkor som sätts i felen settingsdefines.h. Genom att i denna filen sätta variabeln projcomputer till 1 kompileras programmet för att köras i billabbet och för fristående sätts variabeln till 0. Figur 13 visar ett flödesdiagram över hur simuleringssystemet är implementerat. 4.7 Utvecklingsmöjligheter I detta avsnitt presenteras exempel på områden som kan eller bör vidareutvecklas i simulatorsystemet Ytterligare modellbegränsningar De modellbegränsningar som är implementerade i den nuvarande simulatorn är inte fullständiga, det finns fortfarande ofysikaliska fenomen som kan uppstå. De rådande begränsningarna är inte heller grundligt inställda eller utvärderade. En utvidgning av modellbegränsningar och en genomgång av de befintliga skulle med största sannolikhet göra att simulatorn stämmer bättre överens med verkligheten och förbättra körkänslan Knappläsningar För att kunna köra simulatorn med ett tangentbord bör rutinerna för att läsa in knappar ses över. Den nuvarande implementationen fungerar men uppvisar ett oönskat beteende då tangenter hålls ner. Det som händer är att det grafiska interfacet låser sig och startar igång igen då tangenten/tangenterna släpps upp.

27 Optimal Styrning av Autonom Racerbil Figur 13: Flo desdiagram o ver simuleringssystemet. Kursnamn: Projektgrupp: Kurskod: Projekt: Reglerteknisk projektkurs No Oscillations TSRT10 OSAAR E-post: Dokumentansvarig: Fo rfattarens e-post: Dokument: noosc@googlegroups.com Mikael Rosell mikro418@student.liu.se Teknisk dokumentation.pdf 22

28 Optimal Styrning av Autonom Racerbil 23 5 Målföljningssystem Målföljningssystemet är den del av systemet som detekterar fordon på bilbanan och skattar tillstånd för dessa. Målföljningssystemet består hårdvarumässigt av två IR-kameror samt ett antal reflexmarkörer som är fastsatta på fordonen. Mjukvarumässigt består delsystemet av en algoritm som omvandlar data från kamerorna till markörpositioner, en algoritm som hittar fordon och skattar dess positioner och vinklar, samt ett filter som skattar samtliga tillstånd för fordonen utifrån tidigare tillstånd och mätdata. Målföljningssystemet tar hänsyn till markörers placering i rummet i tre dimensioner. Kamerornas positioner samt fordonens reflexmarkörgeometrier är kända. 5.1 Gränssnitt Nedan beskrivs flödet av in- och utsignaler inom målföljningssystemet samt till och från de andra delsystemen. Insignaler: Bilder från IR-kamerorna Utsignaler: (X, Y ), v x, v y, θ 5.2 Höjdgeometri Detekteringsalgoritmen från tidigare års projekt har utvidgats med att alla identifierade markörer projiceras till en mängd fördefinierade plan, se Figur 14. De fördefinierade planens höjd definieras av markörernas höjder på fordonen. Dessa plan anges manuellt där de olika fordonens markörgeometrier defineras. Då alla markörer projiceras till de fördefinierade planen fås en mängd möjliga mönster. Dessa söks igenom i en algoritm för att detektera fordon på bilbanan. De olika möjliga positionerna för en markör tas fram genom att först beräkna linjen mellan markören och kameran som detekterat markören, och sedan ta fram skärningspunkterna mellan denna linje och de fördefinerade planen, se Figur 15. Höjden till en markör definieras utifrån kalibreringsplanet. Definitionen baseras på att markörer i andra plan ger upphov till en offset i position, anledningen till detta är att kameran är kalibrerad för att ta bilder i kalibreringsplanet. En tydligare redogörelse för detta följer nedan: Antag att en markörs position p m samt kamerans position p c i tre dimensioner i koordinatsystemet. p m är markörens position som kameran ser den, alltså i kalibreringsplanet. Det som söks är markörens nya position p s i tre dimensioner. Homogena 3D-koordinater för dessa punkter kan tas fram enligt: ( ) ( ) ( ) pm pc ps p m =, p 1 c =, p 1 s = (31) 1 Plücker-koordinater L för den linje som skär dessa tre punkter kan tas fram enligt: L = p m p T c p c p T m (32) Om höjden h över kalibreringsplanet antas vara känd för den sökta markörens position p s, och en normal n till kalibreringsplanet, kan ett plan s beskrivas i homogena 3D-koordinater som innehåller punkten p s : ( n s = (33) h)

29 Optimal Styrning av Autonom Racerbil 24 Figur 14: Beskrivning av hur markörerna transformeras till olika plan samt hur de kombineras i olika mönster. Därefter kan punkten p s tas fram genom att hitta skärningspunkten mellan L och s: p s = Ls (34) Genom att först p-normera p s kan markörens position i tre dimensioner tas fram. Denna kan sedan användas i detekteringsalgoritmen för att identifiera mönster. 5.3 Positioner och geometrier Kamerans position i rummet och markörernas positioner på fordonen har mätts ut för hand med hjälp av en tumstock. Varje fordon har en speciell markörgeometri, där varje markörs position är definierad med en tredimensionell vektor, som är den position som markören befinner sig på när fordonets position är i origo och fordonets vinkel är noll. Fordonens markörgeometrier och kamerornas positioner är hårdkodade i filen carmarkercameraidentparam.cpp. I Figur visualiseras de olika fordonens markörgeometrier. I Tabell 6 listas markörpositioner för de olika fordonen samt kamerornas positioner. 5.4 Sökalgoritm En sökalgoritm används i målföljningssystemet för att söka mönster utifrån de markörpositioner som detekterats av IR-kamerorna. Algoritmen är implementerad i systemet med en funktion som heter DetectCars3D. De funna markörerna från en kamera samt ett antal kända fordons markörgeometrier är inargument till funktionen. Funktionen sparar undan förslag till fordon, med position (X- och Y -koordinat) samt vinkel angivet. Funktionen är uppdelad i fyra delar, som var och en beskrivs nedan.

30 Optimal Styrning av Autonom Racerbil 25 IR-kamera p c Markörens plan L p s h p p p m Figur 15: Höjdgeometrin för IR-markörerna. Kalibreringsplan Figur 16: Höjdgeometri för IR-markörer för chassi 1. Bilens position antas vara i origo och det utritade koordinatsystemet är världskoordinatsystemet. Den streckade linjen motsvarar kalibreringsplanet, som ligger 18 mm ovanför bilbanans plan. Figur 17: Höjdgeometri för IR-markörer för chassi 3. Bilens position antas vara i origo och det utritade koordinatsystemet är världskoordinatsystemet. Den streckade linjen motsvarar kalibreringsplanet, som ligger 18 mm ovanför bilbanans plan Projicering av markörpositioner Denna del av algoritmen är implementerad med en funktion som heter markerpos3d. Funktionen får som inargument positioner för alla markörer projicerade till kalibreringsplanet. Genom att kamerans position samt alla möjliga markörhöjder är kända kan alla möjliga 3D-positioner för varje markör tas fram. Dessa sparas i en matris, där varje rad motsvarar en markör, och varje par av kolumner motsvarar x- och y-positioner för alla

31 Optimal Styrning av Autonom Racerbil 26 Tabell 6: Tabell över nuvarande positioner för kamerorna samt de olika fordonens markörer. Positionerna är markörernas positioner när respektive fordons position är i origo och fordonets vinkel är 0. Alla värden i tabellen har enheten meter. Position x y z Position x y z Kamera Kamera Chassi 3 (ID 1) x y z Chassi 1 (ID 2) x y z Markör Markör Markör Markör Markör Markör Markör Markör Lastbil (ID 3) x y z Släp (ID 4) x y z Markör Markör Markör Markör Markör Markör Markör Markör Figur 18: Höjdgeometri för IR-markörer för lastbilen. Lastbilens position antas vara i origo och det utritade koordinatsystemet är världskoordinatsystemet. Den streckade linjen motsvarar kalibreringsplanet, som ligger 18 mm ovanför bilbanans plan. Figur 19: Höjdgeometri för IR-markörer för släpet. Släpets position antas vara i origo och det utritade koordinatsystemet är världskoordinatsystemet. Den streckade linjen motsvarar kalibreringsplanet, som ligger 18 mm ovanför bilbanans plan. markörer projicerade till en speciell höjd Mönstersökning Givet alla markörers möjliga positioner i tre dimensioner söks dessa igenom för att hitta mönster som liknar de markörgeometrier som finns beskrivna för varje fordon. Denna del av algoritmen är implementerad med två funktioner, possiblecars samt possiblecarshelp. possiblecarshelp är en rekursiv funktion som bland de funna markörerna letar efter mönster som skulle kunna vara fordon. Antag att ett fordon har N

32 Optimal Styrning av Autonom Racerbil 27 markörer. I varje rekursion testas alla funna markörer som den markör med index n, 1 n N. Avståndet beräknas mellan den testade markören och alla markörer med index från 1 till n 1, som hittats under tidigare rekursioner. Om alla avstånd ligger inom en viss felmarginal, som ställs in manuellt i programmet, påbörjas en ny rekursion där den markör med index n + 1 eftersöks. Om N markörer slutligen hittats, sparas markörkombinationen undan som ett möjligt funnet fordon Special Orthogonal Procrustes Problem Orthogonal Procrustes Problem är ett matrisapproximeringsproblem, där, givet två matriser A och B, den ortogonala matris som bäst avbildar A på B, med avseende på frobeniusnormen, söks. I ett specialfall som kallas Special Orthogonal Procrustes Problem tillåts endast rotationer och translationer. I sökalgoritmen appliceras problemet på de funna uppsättningarna av markörer i funktionen SOPPLocation. Den position och vinkel som bäst avbildar ett fordons kända markörgeometri på en funnen markörgeometri tas fram för varje funnet förslag på fordon som tagits fram i föregående del av algoritmen Bortkastning av orealistiska mätningar I SOPPLocation tas det genomsnittliga geometriska felet fram för varje funnen markörgeometri jämfört med den roterade och translaterade korrekta markörgeometrin. Om felet är större än en viss felmarginal, som ställs in manuellt i programmet, kastas förslaget. I en funktion som heter removeimplausiblecars kan ytterligare förslag kastas. Varje funnen markör antas få förekomma ett visst antal gånger, som ställs in manuellt i programmet. Antag att en funnen markör får förekomma n antal gånger. Om markören förekommer i fler förslag till funna fordon, sparas enbart de n förslag med de lägsta geometriska felen, medan resten kastas. 5.5 Filter Den tidigare mätekvationen för filtret tog tre markörerpositioner som mätning. Eftersom målföljningssystemet är uppgraderat för att kunna hantera godtyckliga markörpositioner har mätekvationen ändrats till en mer generell mätning bestående av x- och y-position samt vinkel. Vinkelmätningen ligger mellan π och π och för att få en korrekt innovation vid mätuppdatering har justeringar lagts till så att filtrets tillstånd alltid ligger inom samma gränser. 6 Visualiseringssystem Visualiseringen av olika grafiska objekt via projektorn har implementeras med hjälp biblioteket HighGui i OpenCV. Visualiseringssytemet körs i tråden Draw thread, se Figur 20 för översiktligt flöde.

33 Optimal Styrning av Autonom Racerbil 28 Figur 20: Figuren visar flödet i Draw thread. 6.1 Gränssnitt Nedan beskrivs flödet av in- och utsignaler till och från visualiseringssystemet. Insignaler: v x, v y, (X, Y ), θ Utsignaler: Bild på datorskärm och projektor 6.2 Höjdgeometri Vid projicering används en höjdparameter, h, för att specificera vilken höjd projiceringen ska ske på. Notera att h mäts från kalibreringsplanet, se Figur 21. Om man t.ex. vill rita något ovanpå lastbilens flak så sätts höjdparametern till flakets höjd. Projektorn projicerar allt i kalibreringplanet så världskoordinater med andra höjder än detta plan behöver räknas om för att hamna på rätt plats, se Figur 21. Steg ett är att ta fram en linje i pluckerkoordinater mellan två punkter, det vill säga projektorns och projiceringspunktens