Designspecifikation Optimal Styrning av Autonom Racerbil

Transkript

1 No Oscillations Corporation Designspecifikation Optimal Styrning av Autonom Racerbil Version 1.0 Författare: Mikael Rosell Datum: 29 november 2013 Status Granskad Projektgruppen 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 Andra utkastet Alla Alla Mindre korrigeringar Alla Alla Mindre ändringar efter kommentarer Alla Alla från beställare

4 Innehåll 1 Inledning Syfte och mål Användning Definitioner Systemöversikt Hårdvara Informationsflöden mellan delsystem Koordinatsystem Reglersystem Reglering av styrservo Reglering av gas- och bromsprofil Algoritm Gränssnitt Simuleringssystem Implementation Gränssnitt Målföljningssystem Höjdgeometri Gränssnitt Visualiseringssystem Höjdgeometri Mellantider Textutskrift via projektorn Rita linjer via projektorn Gränssnitt

5 Optimal Styrning av Autonom Racerbil 1 1 Inledning Systemet består av en bilbana, radiostyrda bilar, kameror för positionering av bilarna, en dator med styrsystem och en projektor. Det nuvarande systemet har itererats fram i ett flertal tidigare projekt. Målet i detta projekt är att få en mindre oscillativ banföljning och en mer robust reglering. En simulator för de radiostyrda bilarna ska utvecklas för att kunna testa systemet utan att behöva köra det fysiska systemet. Dessutom har systemet utökats med en projektor som ska användas för att förbättra demo-effekten genom att exemeplvis projicera simuleringarna på bilbanan eller spela upp tidigare körningar. I detta dokument beskrivs funktionaliteten som behövs för att uppfylla kraven enligt kravspecifikationen. 1.1 Syfte och mål Syftet med projektet är att förbättra det system som redan finns implementerat för 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. Nuvarande systemet uppvisar ett oönskat oscillativt beteende och målet är att i största 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 men dessutom inom forskning, utbildningssyfte, laborationer och studentprojekt. 1.3 Definitioner Här definieras förkortningar och begrepp som används i dokumentet. Optimal trajektoria: Det av tidigare projektgrupp förberäknade spåret optimerat med avseende på varvtiden givet deras framtagna bilmodell. Otillåten oscillation: Avvikelser i bilens position relativt referenstrajektorian (referensspåret beräknat i tidigare projekt) klassas som otillåten oscillation om den sker med frekvens högre än 2 Hz eller med amplitud större än 20 mm. Med bilens position menas bilens mittpunkt. 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.

6 Optimal Styrning av Autonom Racerbil 2 Den önskade trajektorian som bilen skall följa är beräknad i tidigare projekt. Denna innehåller referenstillstånd i form av position, hastighet, vinkelhastighet och offline styrsignalerna. 2.1 Hårdvara Systemet består av följande hårdvara: Bilbana Radiostyrda bilar Radiostyrd lastbil Handkontroller Två IR-kameror Projektor Dator Beställaren tilldelar projektgruppen all hårdvara. 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 de radiostyrda bilarna som styrs via handkontrollen, antingen av datorn eller manuellt av användaren. IR-kamerornas bilder skickas till datorn där bildhanteringsalgoritmer körs för att skatta bilarnas position, hastighet och vinkelhastighet utifrån markörerna på bilarna. Detta sker med sampeltiden 100 Hz. Projektorn används för att visa information på bilbanan. All beräkning och funktionalitet finns i form av mjukvara på datorn. Figur 1: Informationsflöde mellan hårdvarukomponenter.

7 Optimal Styrning av Autonom Racerbil Informationsflöden mellan delsystem Systemet är uppdelat enligt 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 servo. 2.3 Koordinatsystem Figur 3 visar hur bilens koordinatsystem förhåller sig till det globala. För att räkna om rörelser från bilen koordinatsystem till det globala koordinatsystemet används (1) - (4). 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 Ω z sin θ + a y sin θ + v y Ω z cos θ (3) a Y = a x sin θ + v x Ω z cos θ a y cos θ + v y Ω z sin θ (4) Där Ω z ä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 och ett system som styr bilens gas- och bromsprofil.

8 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. 3.1 Reglering av styrservo Regulatorn som ska hantera styrningen av bilen i sidled ska inte vara modellberoende. I och med att regulatorn ska vara oberoende av modeller faller valet på en PID-regulator. Regulatorn ska använda sig av mätsignalerna r e (avståndet till spåret) och θ e (vinkel från spårets tangent till bilens riktning). Detta resulterar i två referenssignaler som ger upphov till var sin styrsignal som summeras ihop, se Figur 4. Styrsignalernas betydelse kommer att bestämmas med hjälp av storleken på parametrarna i PID-regulatorerna. Figur 4: Regulatorstruktur För vidare analys av systemet kan en treparametermodell tas fram med hjälp av stegsvarsexperiment. Det går bland annat att utföra stegsvarsexperiment på yaw-vinkeln genom att ta in ett steg i styrsignalen för att sedan utifrån stegsvarets utseende bestämma treparametermodellens parametrar. Utifrån denna modell av systemet kan en rad inställningsmetoder för PID-regulatorn avändas såsom Lambda- och IMC-trimning samt inställning enligt Åström-Hägglunds inställningsregler[3]. Nästa steg blir att utvärdera PIDregulatorn genom att jämföra referens- och mätsignaler, vilket till exempel kan göras genom att plotta och jämföra given referenssignal och mätsignal. En annan utvärderingsmetod är att summera ihop reglerfelet vid alla tidpunkter under en körning. I systemet finns det en tidsfördröjning, som måste tas hänsyn till under regleringen för att få en korrekt referensföljning. För att kompensera för tidsfördröjningen ska en tidigare

9 Optimal Styrning av Autonom Racerbil 5 framtagen modell från failsafe-systemet [4] användas för att prediktera framtida tillstånd. Tanken är att en enkel modell ska kunna korrigera för tidsfördröjningen. Till och börja med ska modellen endast prediktera från tidigare tillstånd. Om prediktering med de tidigare tillstånden inte visar sig tillräckligt kan predikteringen även ta hänsyn till de nuvarande styrsignalerna. För att göra en rimlighetsbedömning av det predikterade tillståndet kan skillnaden mellan predikterat och uppmätt tillstånd beräknas. Detta avstånd kan jämföras med det avstånd som bilen under högsta hastighet kan tillryggalägga under den predikterade tiden. Implementering av PID i pseudokod där T s är samplingstiden kan ses nedan. Det finns funktionalitet för att förhindra integratoruppvridning då styrsignalen blir mättad, där parametern i nämnaren sätts till T i enligt en tumregel. Parametrar och variabler i PID regulatorn: K - Proportionella förstärkningen Ti - Integrerande del Td - Deriverande del umin - Nedre styrsignal begränsning umax - Övre styrsignal begränsning Iprev - Det förgående värdet på I-delen i regulatorn errorprev - Det förgående värdet på felet i regulatorn Deklarera parametrar som behövs i PID-regulatorn: K, Ti, Td, Ts, umin, umax, Iprev och errorprev. Prediktera tillstånd med hjälp av rörelsemodell Vikta predikterade och uppmätta tillstånd för att få statevalue function PID(stateValue, refvalue){ Ta fram det nuvarande felet: error = refvalue - statevalue P-delen av regulatorn: P = K*error; I-delen av regulatorn: I = Iprev + K*(Ts/Ti)*error D-delen av regulatorn: D = K*(Td/Ts)*(error-errorPrev) Styrsignal från regulatorn: ureg = P+D+I Begränsning av styrsignal: if(ureg > umax) u = umax else if(ureg < umin)

10 Optimal Styrning av Autonom Racerbil 6 u = umin else u = ureg Funktionalitet för att förhindra integratoruppvridning: I = I + (Ts/Ti)*(u - ureg); Behövs vid nästa iteration: Iprev = I; errorprev = error; Returnera styrsignal: return u Om implementationen av PID-regulatorer inte klarar av att uppnå önskad prestanda är tanken att falla tillbaka på LQ-reglering. En idé är då att använda en enkel modell, som den som användes i projektet 2011, för att beskriva bilens dynamik och sedan linjärisera den i flera arbetspunkter. Se (5)-(8). ṙ e = v sin θ e (5) v = c v v + c v,g u g (6) θ e = c θ,s u s (7) ṙ int,e = r e Linjärisering kring olika arbetspunkter med θ e = 0 eller θ e 0, r e = 0 och olika hastigheter ska förbättra referensföljningen. Fördelen med att variera θ e mellan arbetspunkterna är att ṙ e påverkas mer av θ e då detta felet växer. En linjärisering kring noll medför att större fel i θ e inte påverkar ṙ e i den utsräckning som de bör göra jämfört med den olinjära modellen. Tillstånd som avviker från arbetspunkten kommer resultera i större modellfel. (8) 3.2 Reglering av gas- och bromsprofil Beräkning av gas- och bromsprofil ska göras adaptivt online. Profilen uppdateras under körning av en utvärderande algoritm. Målen med systemet som beräknar gas- och bromsprofil 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å 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 spåret Systemet ska bli oberoende av vilket fordon som används Bilens gas och broms styrs med styrsignalen u g [ 1, 1]. u g = 1 innebär fullt gaspådrag och u g = 1 innebär full broms. Bilbanan delas upp i n stycken segment som kan identifieras av världskoordinaterna (X, Y ). Till varje segment hör en styrsignal, så gas- och bromsprofil för ett visst segment k ges av u g [k].

11 Optimal Styrning av Autonom Racerbil Algoritm Vi antar att bilens avvikelse från referensspåret är starkt korrelerat med bilens hastighet. Därför används avståndet mellan bilens mittpunkt och referensspåret, r e, som grund för att utvärdera möjlig gas- och bromsprofil 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. Bilbanan delas upp i segment där gas- och bromsprofilen utvärderas. Styrsignalen för gas och broms i aktuellt och tidigare segment kan justeras inför nästa varv beroende på r e. Storleken på ändringen i de tidigare segmenten kan viktas med en avtagande funktion, då de närmaste segmenten är mer relevanta för hastigheten än segment på andra sidan banan. Den avtagande funktion som kommer att användas är en exponentialfunktion med basen mindre än ett. Storleken på basen bestäms experimentellt. För att bilen inte ska bli stillastående om avståndet mellan bilens mittpunkt och referensspåret blir för stort måste en minimi-nivå för bilens hastighet fastställas. Alternativt kan en ytterlägesregulator användas för att lösa detta problem genom att ta över styrningen och styra bilen tillbaka till referenstrajektorian. Då reglerfelet är stort riskerar huvudregulatorn att överstyra bilen mot referenstrajektorian, därför kan en ytterlägesregulator användas för att styra bilen på ett mindre aggressivt vis mot referenstrajektorian. För att få ett system som lär sig i en bra takt och konvergerar måste hänsyn tas till det antal segment som banan delas upp i, antal tidigare steg som ska uppdateras samt vilka avstånd som ska användas för utvärdering. Pseudokod: Definiera: r_e som avståndet mellan bilens mittpunkt och referenstrajektorian u_g som styrsignal för bilens gas Beräkna vilket segment som bilen befinner sig i utifrån bilens position i världskoordinaterna (X,Y) I segment k: if r_e > 0.01 // 1 cm används som mått (kan ändras) sänk u_g för segment k, k-1, k-2,... enligt avtagande viktfunktion elseif r_e > // 0.5 cm används som mått (kan ändras) öka u_g för segment k, k-1, k-2,... enligt avtagande viktfunktion else behåll tidigare u_g 3.3 Gränssnitt Nedan beskrivs flödet av in- och utsignaler inom reglersystemet samt till och från de andra delsystemen. Reglering av styrservo Insignaler: r e, θ e

12 Optimal Styrning av Autonom Racerbil 8 Utsignaler: u s Adaptiv gas- och bromsstyrning Insignaler: r e, (X, Y ) Utsignaler: u g 4 Simuleringssystem Simuleringssystemet ska kunna ersätta bilen, bilbanan och målföljningssystemet genom att position, hastighet och acceleration för bilen simuleras i datorn. Rörelsemodellen som kommer att användas för att simulera bilen är modellen över bilen som tagits fram i tidigare projekt. Från denna fordonsmodell fås acceleration i longitudinell-, lateral- och vinkelled. Fordonsmodellen behöver tidigare tillstånd och styrsignal (från regulator eller handkontroll) för att beräkna accelerationerna. Genom att integrera accelerationen i varje led fås hastigheterna v x och v y, positionen (X, Y ), vinkelhastigheten Ω z och vinkeln θ för bilen. Se (9)-(12). 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 ) (9) v x (t) = v x (t T ) + T v x (t T ) (10) θ(t) = θ(t T ) + T Ω z (t T ) + T 2 2 Ω z (t T ) (11) Ω z (t) = Ω z (t T ) + T Ω z (t T ) (12) Där accelerationerna v x och Ω z beräknas med fordonsmodellen. Notera att ekvationerna för Y -koordinaten 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. För regleringen av bilen behövs dessutom felen i position och vinkel relativt referenstrajektorian. Dessa tas fram enligt (13)-(14). r e = ± (X X ref ) 2 + (Y Y ref ) 2 (13) θ e = θ ref θ (14) Vilken punkt på referenstrajektorian som skall användas avgörs genom att minimera (13) 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. För att simulatorn skall innehålla slumpmässighet ska accelerationen från fordonsmodellen adderas med vitt brus som sedan propageras från acceleration till hastighet till position. Det ska vara möjligt för användaren att välja att köra Simuleringssystemet med eller utan störningsmodell. 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 kan göras genom att använda den algoritm som togs fram av 2011 års projekt. Denna algoritm avgör om en punkt befinner sig på banan eller ej. Gör vi detta för exempelvis bilens hörn vet vi att hela bilen befinner sig på banan.

13 Optimal Styrning av Autonom Racerbil 9 I Figur 5 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 gäller då simulatorn körs tillsammans med regulatorn och referenstrajektorian. Simuleringssystemet skall även kunna köras genom att styra bilen manuellt med handkontroll/tangentbord vilket innebär att regulatorn och referenstrajektorian inte längre används. Simulatorn skall alltså kunna 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 6 för en överblick av systemet då simulering körs manuellt. X ref, Y ref, θ ref v x, v y, X, Y, θ Referenstrajektoria Visualiseringssystem Simulator Banbegränsning OK? X, Y v x, v y, Ω z v x, v y, Ω z v x, v y, Ω z, r e, θ e Reglersystem Simuleringssystemet Fordonsmodell u g, u s Handkontroll/ Tangentbord Figur 5: Översikt över Simuleringssystemet med vilka signaler som skickas mellan moduler. Regulator och referenstrajektoria används. v x, v y, X, Y, θ Referenstrajektoria Visualiseringssystem Simulator Banbegränsning OK? X, Y v x, v y, Ω z v x, v y, Ω z Reglersystem Simuleringssystemet Fordonsmodell u g, u s Handkontroll/ Tangentbord Figur 6: Översikt över Simuleringssystemet vilka signaler som skickas mellan moduler. Manuell styrning används.

14 Optimal Styrning av Autonom Racerbil Implementation Figur 7 visar ett flödesschema över hur simuleringssystemet ska implementeras. Figur 7: Flödesdiagram över simuleringssystemet.

15 Optimal Styrning av Autonom Racerbil Gränssnitt Nedan beskrivs flödet av in- och utsignaler inom Simuleringssystemet samt till och från de andra delsystemen. Simulator Insignaler: v x, v y, Ω z, X ref, Y ref, θ ref Utsignaler: v x, v y, r e, θ e, X, Y, θ, Ω z Fordonsmodell Insignaler: v x, v y, Ω z, u g, u s Utsignaler: v x, v y, Ω z Referenstrajektoria Utsignaler: X ref, Y ref, θ ref Reglersystem Insignaler: v x, v y, Ω z, r e, θ e, X, Y Utsignaler: u g, u s Banbegränsning Insignaler: X, Y Utsignaler: ok? Visualiseringssystem Insignaler: X, Y, θ, v x, v y Handkontroll/Tangentbord Utsignaler: u g, u s 5 Målföljningssystem Målföljningssystemet ska ta hänsyn till markörers placering i rummet i tre dimensioner. Systemet måste hantera var på banan markörerna detekteras. Kamerans position är känd samt bilarnas mönster sett rakt ovanifrån. 5.1 Höjdgeometri Detekteringsalgoritmen från tidigare års projekt ska utvidgas med att alla identifierade markörer projiceras till en mängd fördefinierade plan, se Figur 8. De fördefinierade planens höjd definieras av markörernas höjder på fordonen. Dessa plan måste anges manuellt beroende på vilka fordon som används. Då alla markörer projiceras till de fördefinierade planen fås en mängd möjliga mönster. Den nuvarande algoritmen används för att söka efter mönster bland alla dessa kombinationer. Markörerna flyttas genom att beräkna linjen mellan markörerna och kameran som kan användas för att flytta upp markörerna till nya plan, se Figur 9. Höjden h till en markör definieras utifrån kalibreringsplanet. Definitionen baseras på att markörer i andra plan

16 Optimal Styrning av Autonom Racerbil 12 Figur 8: Beskrivning av hur markörerna transformeras till olika plan samt hur de kombineras i olika mönster. 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 p m = 1 ), p c = ( pc 1 ), p s = ( ps 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 (16) 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 = (17) h) Därefter kan punkten p s tas fram genom att hitta skärningspunkten mellan L och s: (15) p s = Ls (18) 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.

17 Optimal Styrning av Autonom Racerbil 13 IR-kamera p c Markörens plan L p s h p p p m Figur 9: Höjdgeometrin för IR-markörerna. Kalibreringsplan Kamerans position i rummet och även markörernas positioner på ett objekt kommer att tas fram för hand med hjälp av lämpligt mätredskap. Skulle det visa sig att dessa mätningar har dålig noggrannhet, kan alternativa metoder för att bestämma positionerna användas. Med denna lösning kommer det vara enkelt att detektera en markörgeometri på lastbilen. Dessutom kommer de mätfel som uppstår på grund av höjdskillnader för bilens markörer att hanteras med den nya algoritmen. En tanke är att placera ett specifikt mönster på lastbilen respektive släpet för att på så sätt kunna identifiera positionerna på båda delarna av lastbilen. 5.2 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, θ 6 Visualiseringssystem Visualiseringen av olika grafiska objekt via projektorn kommer implementeras med hjälp biblioteket HighGui i OpenCV. 6.1 Höjdgeometri Vid projicering ska en höjdparameter, h, användas för att specificera vilken höjd projiceringen ska ske på. Höjdparametern ska ändra skalningen som konverterar världskoordinater

18 Optimal Styrning av Autonom Racerbil 14 till pixelkoordinater då en markör befinner sig i ett annat plan relativt kalibreringsplanet i höjdled. Det första steget är att räkna ut bredden på det nya planet genom det proportionella sambandet mellan bredden på planen. w h = kw Notera att w h och w anger bredden i pixelkoordinater. När w h är beräknad används den istället för w för att räkna ut skalningsfaktorn, scale = worldscale/w h. Resultat är en skalningsfaktor som kommer konvertera världskoordinater till pixelkoordinater i det röda planet i Figur 10. Proportionalitetskonstanten k beräknas genom undersöka förhållandet mellan höjderna för de olika planen, se nedan. k = h p h h p Tanken med denna skalning är att plan på olika höjd skalas proportionerligt med höjden. Kalibreringen som utförs i kalibreringsplanet för höjden h p kan skalas till en annan höjd förutsatt att det nya planets höjd ovanför kalibreringsplanet och projektorns höjd är kända. Eftersom projektorn inte sitter rakt ovanför origo i världskoordinatsystemet måste allt som ska projiceras på andra plan än kalibreringsplanet translateras efter att det har skalats. Detta finns beskrivet i Figur 10. Hur stor translationen är för ett visst plan kan räknas ut med hjälp av homogena koordinater i tre dimensioner. Det görs genom att först, för hand, mäta positionerna i världskoordinater för projektorn och ett hörn på den projicerade bilden i kalibreringsplanet. En linje beskriven med Plücker-koordinater mellan projektorn och hörnet kan tas fram på samma sätt som i (16). För att ta fram den punkt, i ett plan parallellt med kalibreringsplanet, där detta hörn kommer att projiceras, kan man använda (17) och (18). Den bild man vill projicera i detta plan kan sedan, med hjälp av den beräknade punkten, translateras så att hörnet (och även resten av bilden) projiceras på rätt position. 6.2 Mellantider Då varvtider redan finns implementerat är det enkelt att utöka detta med stöd för mellantider. Tidtagningen sköts av målföljningssystemet och skickas sedan till visualiseringsystemet. När en ny tid fås markerar visualeringssystemet detta genom att skriva ut tiden på banan och låta linjen som markerar tidmätningsplatsen blinka. Följande datastruktur kan användas för att lagra tider: map<carnumber,map<lapnumber,map<timenumber,float>>> En map är en associativ datastruktur i C++ med par av nycklar och värden. Den första mappen har bilnummer som nyckel och ger en map med varv. Mappen med varv tar ett varvnummer och ger en map med tider. De olika tiderna är mellantider och varvtiden. 6.3 Textutskrift via projektorn Det är en rad utskrifter som ska göras på bilbanan via projektorn. I OpenCV finns det en funktion vid namn cvputtext som skriver ut text i bilden som ska ritas upp.

19 Optimal Styrning av Autonom Racerbil 15 Figur 10: Figuren visar två plan, det nya projiceringsplanet (rött) samt kalibreringensplanet (svart). Parametrar: w - bredd mellan kalibreringsmarkörerna, w h - bredden på planet vid höjd h, h - höjd för det röda planet och h p - projektorns höjd. cvputtext(mat& img, const string& text, Point org, intfontface, double fontscale, Scalar org, int thickness=1, int linetype = 8, bool bottomleftorgin=false) Ett exempel på en funktion som beskriver klockan: updateclock(){ tm *ltime = localtime(); string time; time.append(ltime->tm_hour); time.append(":"); time.append(ltime->tm_min); time.append(":"); time.append(ltime->tm_sec); cvputtext(&time,...) 6.4 Rita linjer via projektorn I mjukvaran måste det finnas funktionalitet med syftet att rita linjer för att implementera uppspelning av tidigare varv samt rita upp den körda trajektorian samtidigt som bilen körs. Det resulterar i två fall; uppritning offline samt online. I funktionaliteten för offlineläget krävs att en vektor för positionsdata samt iteratorer initieras. Tanken är att funktionen som ritar upp den körda banan använder sig av po-

20 Optimal Styrning av Autonom Racerbil 16 sitionerna i vektorn för att rita linjer mellan punkterna. Data från tidigare varv finns sparat på fil och måste därmed läsas in till vektorn. Det är bara var femte punkt som ska sparas i vektorn på grund av att tråden för visualiseringssytemet körs i 20 Hz jämfört med huvudtråden och reglersystemet som körs i 100 Hz. När detta är gjort måste iteratorerna till vektorn initieras. Sedan kommer funktion som ritar punkter kallas under varje sampel tills trajektorian är helt utritad. I online läget hämtas positionsdata direkt från tillstånden. Det är dock viktigt att spara undan varje tillstånd så detta kan användas i nästa iteration för att rita ut en linje mellan två punkter. Vid onlinefallet ska trajektorian färgkodas. Den aktuella motorsignalen kan hämtas från IOControl. IOControl är den del av programmet som hanterar styrsignaler till radiosändaren. Pseudokod för funktionerna som nämnts ovan: Definitioner: En vektor vid namn earlierlap som innehåller data från tidigare varv. Iteratorer till earlierlap definieras, preiter och Iter. Funktioner: Funktion som initierar iteratorer: function initearlierlap(){ preiter = pekare till början av vektorn earlierlap Iter = pekare till slutet av vektorn earlierlap Funktion som hämtar data som beskriver tidigare varv från fil: function getearlierlap(filename){ Öppna filen filename Töm vektorn earlierlap på eventuellt skräp if(filename öppnad){ Läs fil och spara innehåll i vektorn earlierlap (spara bara var femte punkt). Initiera iteratorer till vektorn med data: initearlierlap(); Variabeln mode anger om uppritningen ska ske från sparad eller aktuell data. Det ända skillnaden implementationsmässigt är varifrån data hämtas. I offline läge hämtas data från vektorn earlierlap och i online läge direkt från tillstånden. Funktion för att rita tidigare varv: drawearlierlap(bool mode){ Offline läge: if(mode){

21 Optimal Styrning av Autonom Racerbil 17 if(om Iter pekar på början av earlierlap)){ Spara iteratorn till nästa iteration: preiter = iter; else if(om Iter pekar på slutet av earlierlap) { do nothing else{ Rita en linje mellan två punkter, dvs punkterna som iter respektive preiter pekar på: cvline(...) Spara iteratorn till nästa iteration: preiter = iter Online läge: else{ if(första punkten){ spara punkten else if(sista punkten){ gör ingenting else{ cvline(...); spara positionsdata spara nuvarande punkten till nästa gång funktionen kallas Funktionen cvline() tar två punkter som argument och ritar en linje mellan dem. Det går även att ange vilken färg linjen ska ha. Detta blir mycket relevant när det utritade spåret ska färgas beroende på broms- och gasprofil. 6.5 Gränssnitt Insignaler: v x, v y, (X, Y ), θ Utsignaler: bild

22 Optimal Styrning av Autonom Racerbil 18 Referenser [1] Tomas Svensson, Christian Krysander, Projektmodellen Lips. Studentlitteratur. [2] Klas Nordberg, Introduction to Representations and Estimation in Geometry. [3] Martin Enqvist et al Industriell reglerteknik Kurskompendium. Institutionen för systemteknik, Linköpings Universitet, Linköping. [4] Projektgruppen Slipping limit Teknisk dokumentation Optimal Styrning av Radiostyrd Racerbil. Linköpings Universitet, Linköping.