Teknisk dokumentation

Teknisk dokumentation LiU Racetrack Version 1.0 Författare: Salko Bjelevac Datum: 15 december 2014 Status Granskad Projektgruppen 2014-12-12 Godkänd Isak Nielsen 2014-12-15

Projektidentitet E-post: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: liurace@googlegroups.com http://www.isy.liu.se/edu/projekt/tsrt10/2014/racetrack/ Isak Nielsen, Linköpings universitet Telefon: 013-28 28 04 E-post: isak.nielsen@liu.se Daniel Axehill, Linköpings universitet Telefon: 013-28 40 42 E-post: daniel@isy.liu.se Daniel Axehill, Linköpings universitet Telefon: 013-28 40 42 E-post: daniel@isy.liu.se Kristin Bergstrand Telefon: 070-63 66 388 E-post: kribe281@student.liu.se Kristoffer Lundahl, Linköpings universitet Telefon: 013-28 66 23 E-post: kristoffer.lundahl@liu.se Gruppmedlemmar Namn Ansvarsroller Telefon E-post (@student.liu.se) Kristin Bergstrand (KB) Projektledare (PL) 070-63 66 388 kribe281 Måns Klingspor (MK) Informationsansvarig (IA) 070-55 25 053 mankl061 Joachim Larsson (JL) Mjukvaruansvarig (MA) 073-06 71 511 joala815 John Wilhelms (JW) Testansvaring (TA) 070-40 84 358 johwi572 Erik Hedberg (EH) Designansvarig (DA) 070-51 22 275 erihe251 Salko Bjelevac (SB) Dokumentansvarig (DOC) 076-22 66 599 salbj522 Marcus Trulsson (MT) Hårdvaruansvarig (HA) 073-35 45 432 martr316 Oskar Ljungqvist (OL) Leveransansvarig (LA) 070-57 71 868 osklj052

Dokumenthistorik Version Datum Förändringar Sign Granskad 1.0 2014-12-15 Godkänd OL Projektgrupp

Innehåll 1 Inledning 1 1.1 Syfte och mål........................................... 1 1.2 Användning............................................ 1 1.3 Definitioner............................................ 1 2 Systemöversikt 2 3 Hårdvara 3 4 Mjukvara 4 4.1 Mjukvarumiljö.......................................... 5 4.2 Struktur.............................................. 5 4.3 Utveckling under 2014 års projekt............................... 5 5 Kommunikation mellan datorerna 5 5.1 Kommunikationsprotokoll för RRT............................... 6 5.2 Kommunikationsprotokoll för GPGO.............................. 7 5.3 Kommunikationsprotokoll för styrningsinformation av lastbilen............... 7 6 Delsystem: Målföljning 8 6.1 Utveckling............................................. 8 6.2 Markörpositioner......................................... 9 6.3 Skattning av dollyvinklar.................................... 9 6.4 Gränssnitt............................................. 11 7 Delsystem: Hinderhanteringssystem 11 7.1 Implementering.......................................... 11 7.1.1 Klass för geometriska figurer.............................. 11 7.1.2 Hinderklass........................................ 12 7.1.3 Sensorklass........................................ 12 7.1.4 Hinderdetektering.................................... 12 7.2 Gränssnitt............................................. 12 8 Delsystem: Beslutssystem 13 8.1 Implementation.......................................... 13 8.2 Gränssnitt............................................. 14 9 Delsystem: Referensgenerering 15 9.1 Lokala trajektorier........................................ 15 9.2 Teori................................................ 17 9.2.1 Optimeringsproblemet.................................. 18 9.2.2 Bilmodell......................................... 18 9.2.3 Regulator......................................... 18 9.2.4 Samplingsstrategier................................... 20 9.2.5 Rapidly-exploring Random Tree (RRT)........................ 20 9.3 Körlägen.............................................. 21 9.3.1 Säkerhetsläge....................................... 21

9.3.2 Racingläge........................................ 22 9.4 Det implementerade RRT-programmet............................. 22 9.4.1 Uppstart av programmet................................ 22 9.4.2 DEV MODE....................................... 22 9.4.3 RUNNING WITH SERVER.............................. 22 9.4.4 Under simulering..................................... 22 9.4.5 Skillnader mellan simuleringsmiljö och racetracksystem............... 23 9.4.6 Hantering av dynamiska hinder............................. 23 9.5 Gränssnitt............................................. 23 9.6 Utvecklingsmöjligheter...................................... 23 10 Adaptiv anpassning av hastighetsprofilen 26 10.1 Optimeringsproblemet...................................... 26 10.2 Parametrisering av optimal trajektoria............................. 26 10.3 Gaussian Processes for Global Optimization.......................... 31 10.4 Implementation.......................................... 37 10.5 Resultat.............................................. 42 10.6 Utvecklingsmöjligheter...................................... 42 10.7 Avslutande ord.......................................... 43 10.8 Gränssnitt............................................. 44 11 Delsystem: Reglering 45 11.1 Gränssnitt............................................. 45 12 Delsystem: Manuell styrning av lastbil 45 12.1 Raspberry Pi och D/A konverterare.............................. 46 12.2 Skickande av styrsignaler från datorn.............................. 46 13 Delsystem: Visualisering och GUI 46 13.1 Utseende och funktionalitet för GUI.............................. 46 13.2 Projektion på bilbanan...................................... 48 14 Delsystem: Simulator 50 14.1 Gränssnitt............................................. 50 14.2 Simuleringssystem........................................ 50 14.2.1 Fordonsmodeller..................................... 50 14.2.2 Simulator......................................... 51

LiU Racetrack 1 1 Inledning Systemet som beskrivs i detta dokument har utvecklats under flera tidigare års projekt, senast OSAAR2013. Systemet består i korta drag av en bilbana med radiostyrda bilar samt utrustning för att följa och styra bilarna ifrån en dator. Systemet ska i framtiden användas i demonstrationssyfte, men även i utbildnings- och forskningssyfte. Systemets hårdvara, mjukvara och de funktionella delsystemen beskrivs utförligt i detta dokument. Då systemet är en utveckling från tidigare år, läggs fokus framförallt på de delar av systemet som tillkommer i och med detta projekt, Fast and Adaptive Steering Technology, (FAST). En översikt av de tidigare projekten ges nedan. Sommarprojekt 2011 - Systemet byggdes. TSRT10 2011 - Fokus på reglering och optimal trajektoria. Sommarprojektet 2012 - Fokus låg på att förbättra bilmodellerna. TSRT10 2012 - Regleringen och den optimala banan förfinades. Sommarprojekt 2013 - Målföljningen förbättrades. Projektor installerades. TSRT10 2013 - Visualiseringen utvecklades bland annat. Projektorn implementerades och GUI:t utvecklades. För en mer ingående beskrivning av det befintliga systemet hänvisas till de tidigare tekniska dokumentationenerna, framförallt till den tekniska dokumentationen från No Oscillation Corporation, projektet ifrån år 2013 [3]. 1.1 Syfte och mål Syftet med projektet var att utveckla det befintliga systemet genom att implementera ny funktionalitet samt utveckla redan befintlig sådan. Projektet skulle bidra till att i framtiden göra systemet användbart inom forskning men också attraktivare för demonstrering, till exempel vid studiebesök. 1.2 Användning Systemet ska efter detta projekt fortsätta att användas inom forskning, i undervisningssyfte och vid demonstrationer av avdelningen för reglerteknik på ISY. 1.3 Definitioner Här defineras flödesscheman och begrepp som används i dokumentet. Trajektoria: Den beräknade körvägen som ges av referensgenereringssystemet. Hinder: Hinder är objekt på banan som markerar ej tillåten körväg för bilen. Hinder kan vara statiska eller dynamiska. De kan vara virtuella och visualiserade på banan med projektorn eller fysiska genom lastbilen eller blockformade objekt. Hindren har en bredd på maximalt 10 cm placerade ortogonalt mot banriktningen. De dynamiska hindrena kan röra sig i alla riktningar.

LiU Racetrack 2 Aktiva hinder: Aktiva hinder är hinder som befinner sig i bilens planerade körväg. Omkörning: Omkörning innebär att avvika från den förberäknade referenstrajektorian kring banan för att följa en beräknad omkörningstrajektoria. När hindret är passerat ska bilen sedan manövrera tillbaka till den ursprungliga trajektorian och sedan fortsätta att följa denna. Flödesscheman: Flödesscheman används för att tydliggöra olika system och processer. Figur 1 förklarar vad symbolerna i sådana scheman betyder om inte annat anges. Figur 1: Förklaring av de symboler som huvudsakligen kommer att användas i flödesdiagrammen i det här dokumentet. 2 Systemöversikt Den enklaste uppdelning av systemet som kan göras är att dela in systemet i en del som ska styras, samt en del som utför styrningen. Målföljningssystemet samlar den nödvändiga informationen och resultatet visualiseras av visualiseringssystemet. Systemet kan också delas upp i delsystem som alla finns i denna rapport. Det som styrs är radiostyrda bilar och en radiostyrd lastbil, på en bana tillverkad i skumgummi. Styrningen av bilarna sker från en dator som med hjälp av IR-kameror kan följa bilarna. Med hjälp av en inkoppling på bilarnas fjärrkontroller kan datorn skicka styrsignaler till fordonen men lastbilen kan även styras från dator via tangentbordet. En projektor gör att datorn kan visualisera mottagen information direkt på banan. Datorn kan även simulera hur bilarna skulle reagera på olika styrsignaler. Huvuddelen av systemets komplexitet finns alltså i koden som datorn exekverar. Systemet kan också delas upp i delsystem efter funktionalitet, se Figur 2. Tre av delsystemen representerar den funktionalitet som konceptuellt finns i själva bilarna; beslutssystemet som avgör vilka val bilen ska göra, referensgenereringen, som genererar den väg bilen ska försöka följa i varje givet ögonblick, samt regleringen vars syfte är att få bilen att följa den valda vägen. I detta års projekt har referensgenereringssystemet utökats med Rapidly exploring Random Tree (RRT) och Gaussian Processes for Global Optimization (GPGO). Ytterligare två delssystem behövs för att hantera den information bilen behöver för att göra rätt val. Målföljningen identifierar objekt på banan och kan därmed även kan ge bilen information om vilket tillstånd den har. Målföljningen identiferar även andra objekt, som

LiU Racetrack 3 då utgör hinder för den aktuella bilen och därmed hanteras av hinderhanteringen. Hinderhanteringen tar delvis emot information från målföljningen samtidigt som den hanterar användardefinierade virtuella hinder. Hinder på banan kan sedan detekteras av bilens virtuella hindersensorer, vilket sker genom kommunikation mellan beslutssystemet och hinderhanteringssystemet. Visualiseringen visar information om systemets tillstånd via projektorn och på datorskärmen samt tillåter användaren att interagera med systemet via ett GUI under körning. Visualiseringssystemet utgör ytterligare ett delsystem i systemet FAST. Eftersom en del av projektet är att integrera en radiostyrd lastbil i systemet beskriver vi även den funktionaliteten som ett delsystem. Figur 2: Funktionell översikt av systemet, där de delsystem systemet delats upp i markerats med grönt. Referensgenereringen väljer vilken bana regleringen skall försöka följa. Regleringen ställer ut styrsignaler antingen till de fysiska bilarna eller till simulatorn. Målföljningssystemet skattar bilarnas tillstånd samt följer fysiska hinder. 3 Hårdvara Den hårdvara som fanns att tillgå i projektet vid start: Bilbana Tre radiostyrda bilar, Kyosho dnano [7] Radiostyrd lastbil innefattande dragbil, dolly och släp Handkontroller Två IR-kameror Projektor PC: rt-pc-11 PC: rt-pc-03 Styrkort NI PCI-6024E Kopplingsplatta, BNC-2110, med tillhörande kablar

LiU Racetrack 4 Den hårdvara som har lagts till under projektets gång är; Raspberry Pi B+ med tillbehör SparkFun I2C D/A-omvandlare - MCP4725 Microchip I2C D/A-omvandlare - TC1321EOA Extra nätverkskort i rt-pc-11 och rt-pc-03 Nätverksswitch med ett antal ethernetsladdar Hinder, pappersbit 10 x 10 cm med IR-markörer Hinder, pappersbit 5 x 5 cm med IR-markörer För att uppfylla kraven för styrning av lastbilen var ny hårdvara tvungen att inhandlas. Initialt fördes diskussioner kring att använda styrkort NI PCI-6723. Då National Instruments har ett högt pris för denna produkt och poängterade att deras support inte är till för studenter eller projekt som inte använder deras egen mjukvara LabVIEW undersöktes alternativ. Det alternativ som ansågs mest attraktivt var att använda I2C bussen på en Raspberry Pi för att ställa ut signaler till D/A omvandlare. Efter att ha undersökt tidsåtgången att ställa ut en analog utsignal från ett kommando över nätverket till raspberry Pi datorn och skicka tillbaka ett meddelande om utförd åtgärd på samma nätverk kunde slutsatser dras kring systemets tidsåtgång. Tidsåtgången för detta var oftast strax under 4 ms. Systems ändamål är att möjliggöra reglering för autonomt körning av lastbilen, systemets uppmätta tidsåtgången anses acceptabel då den i relation till resten av styrsystemets tidsåtgång är liten. I Figur 3 kan det nuvarande systemets hårdvaruppbyggnad ses. Figur 3: Schematisk bild av hur systemet med tillagd hårdvara kommunicerar. 4 Mjukvara Detta avsnitt beskriver hur mjukvaran är strukturerad och hur den har utvecklats under detta projekt. Fokus har legat på att strukturera upp koden.

LiU Racetrack 5 4.1 Mjukvarumiljö Programmet för det övergripande systemet är skrivet i programmeringsspråket C++. Utvecklingsmiljön är en PC med Windows 7.0 och koden kompileras genom Microsoft Visual C++ 2010. Mjukvaran är beroende av följande externa bibliotek: NI-DAQmx från National Instruments, för att hantera styrkortet. OpenCV, ett fritt mjukvaruprojekt för bildbehandling. FlyCapture2 från Grey Research för att hantera IR-kamerorna. 4.2 Struktur Då delsystemen arbetar parallellt med varandra är mjukvaran uppdelad i fyra så kallade trådar. Gui thread: Tråd för användargränssnittet. Run thread: Tråd för målföljningssystemet. Regulator thread: Tråd för reglersystemet. Draw thread: Tråd för visualiseringssystemet. Utöver det övergripande systemet som beskrivits ovan har två nya program utvecklats. RRT: (Rapidly exploring Random Tree) används för att beräkna ny referenstrajektoria vid omkörning och är skrivet i C++. GPGO: (Gaussian Process for Global Optimization) används för att adaptivt förbättra varvtiderna för bilen och är skrivet i Matlab Dessa två program körs på separata datorer och kommunikation mellan programmen och huvudprogrammet sker med en nätverksswitch. 4.3 Utveckling under 2014 års projekt För att i framtiden kunna sätta sig in i koden snabbt, underlätta felsökning samt implementering av ny funktionalitet har ett stort arbete lagts på att städa och strukturera om koden. Detta tillsammans med att filer har delats in i delprojekt har lett till att koden i någon mån kan betraktas som modulär. Dock återstår en hel del arbete för att koden verkligen ska vara modulär och en hel del arbete återstår också om man ska kunna kalla koden objektorienterad. 5 Kommunikation mellan datorerna För ökad moduluppbyggnad har systemet utvecklats till att vara uppdelat på flera datorer, en kommunikationslänk har satts upp för att dessa ska kunna kommunicera. Denna kommunikationslänk är ett lokalt nätverk i laborationssalen. För att möjliggöra detta används nätverkskort tilldelade statiskt IP-adresser som är kopplade med ethernet sladd

LiU Racetrack 6 till en nätverksswitch. I Tabell 1 ses de tilldelade IP-adresserna till respektive dator. En extern dator som kopplas till systemet kan välja ett statiskt ip enligt 192.168.0.xx exkluderat de redan använda, nätverksmasken ska sättas till 255.255.0.0. Dator Statisk IP-adress Rt-pc-03 192.168.0.9 rt-pc-11 192.168.0.99 Raspberry pi 192.168.0.1 Extern dator 192.168.0.xx Tabell 1: I denna tabell presenteras de statiska IP-adresser datorerna i laborationssalen tilldelats för det lokala nätverket. För en extern datorn innebär xx ett tal mellan 0-99 uteslutet de redan använda numren för de andra datorerna. För att kommunikationen inte ska fastna i brandväggar har porten 27015 exkluderats från brandväggen på datorn rt-pc-11. Denna dator har därför kunnat göras till en TCP-server dit TCP-klienter kan ansluta på denna port när Racetrack-systemet körs. Den kommunikation som idag förs över det lokala nätverket kan ses i Tabell 2. Kommunikationsprotokollen för denna information behandlas nedan. Kommunikation mellan datorer Rt-pc-11, rt-pc-03 Rt-pc-11, extern dator Raspberry pi, rt-pc-03 Typ av information RRT-information GPGO-information Styrnings-information för lastbilen Tabell 2: I denna tabell presenteras vilken information som skickas mellan vilka datorer. Den dator som skrivs först agerar server, den andra klient. Den externa datorn måste ha möjlighet att köra Matlab 2014b för att kunna köra GPGO kommunikationen. Figur 4: Schematisk bild av hur RRT- och GPGO-informationsutbyte initieras. För att ansluta en TCP-klient som har möjlighet att kommunicera skickas INIT CONNECTION. Därefter skickas packet i båda riktningarna med först packet type och sedan client type för att specificera vilken information som nu kommer skickas. Därefter skickas den avsedda informationen. 5.1 Kommunikationsprotokoll för RRT För att skicka RRT-information måste det schema som beskrivs i Figur 4 användas för att initiera sändningen. I Tabell 3 presenteras de konstanter som initierar RRT överföring av

LiU Racetrack 7 information. Efter RRT kommunikation är initierad kommer specifik RRT-information att skickas en gång. Denna information är specifik för vilken riktning informationen går. Från rt-pc-11 till rt-pc-03 skickas bilen och hindrets position vid upptäckandet av ett hinder. Från rt-pc-03 till rt-pc-11 skickas trajektorian för omkörning då en sådan har beräknats. Först skickas då längden av antalet trajektoria punkter därefter X,Y och θ värden för trajektorian. För att se hur detta definieras i koden studera NetworkData.h, struct Packet SERVER to RRT och struct Packet RRT to SERVER. Det är viktigt att NetworkData.h är definierad likadant på server och klient sidan av kommunikationen och att alla packetvariabler är initierade. Kommunikationkonstanter för RRT Värde INIT CONNECTION 0 RRT EVENT 1 RRT CLIENT 0 Tabell 3: I denna tabell presenteras de konstanter som skickas för att initiera en RRT informationsöverföring. RRT EVENT och RRT CLIENT behöver skickas varje gång ny RRT information ska överföras. 5.2 Kommunikationsprotokoll för GPGO För att skicka GPGO-information måste det schema som beskrivs i Figur 4 användas för att initiera sändningen. I Tabell 4 presenteras de konstanter som initierar GPGO överföring av information. Efter GPGO kommunikation är initierad kommer specifik GPGO-information att skickas en gång. Denna information är specifik för vilken riktning informationen går. Från en extern dator som kör GPGO till rt-pc-11 skickas först en 1001 lång vektor innehållande en hastighetsprofil. Dessa kommer i rt-pc-11 att skrivas till textfilen GPGO-vVec.txt och läsas in av programmet precis innan bilen kör förbi mållinjen. Från rt-pc-11 till en extern datorn som kör GPGO skickas en varvtid när ett varv med en uppdaterad hastighetsprofil har avklarats. För att se hur detta definieras i koden studera NetworkData.h, struct Packet SERVER to GPGO och struct Packet GPGO to SERVER. Kommunikationkonstanter för GPGO Värde INIT CONNECTION 0 GPGO EVENT 3 GPGO CLIENT 1 Tabell 4: I denna tabell presenteras de konstanter som skickas för att initiera en GPGO informationsöverföring. GPGO EVENT och GPGO CLIENT behöver skickas varje gång ny GPGO information ska överföras. 5.3 Kommunikationsprotokoll för styrningsinformation av lastbilen Kommunikationen för styrning av lastbilen sker mellan rt-pc-03 och Raspberry Pi datorn. Raspberry Pi datorn är en TCP-server och rt-pc-03 är TCP-klient, kommunikationen sker

LiU Racetrack 8 på port 27016. Den information som behöver kommuniceras för att kunna styra lastbilen är kör framåt/bakåt, styr höger/vänster och storlek för dessa styrsignaler. Detta har implementerats genom att skicka en character buffer med 5 värden från rt-pc-03, där första biten beskriver om denna styrsignal är för körning framåt/bakåt eller för styrning höger/vänster. Om denna bit är en nolla adresseras körning framåt/bakåt, därefter läses de resterande 4 bitarna som en styrsignal med ett värde i intervallet 0-3000, detta motsvarar en utsignal på 0-3.2 V, där 1480 motsvarar 1,6 V vilket innebär stillastående. Om den första biten är en etta adresseras styrning höger/vänster, därefter läses de resterande 4 bitarna som en styrsignal med ett värde i intervallet 0-4000, detta motsvarar en utsignal på 0-3.2 V, där 1850 är 1,6 V vilket innebär styrning rakt fram. Varför de två styrsignalsintervallena skiljer sig åt beror på att digital- till analog-korten är av två olika fabrikat och samma funktion används för att adressera båda två och då skiljer de sig åt lite i adresseringen. 6 Delsystem: Målföljning Målföljningssystemet har som uppgift att detektera fordon och hinder på banan och skatta dess tillstånd. Det består av två IR-kameror och en observatör. IR-kamerorna är monterade i taket ovanför bilbanan och på fordonens tak sitter reflexmarkörer. Informationen från IR-kamerorna används som mätsignal i observatören som skattar bilens tillstånd. Målföljningssystemet tar hänsyn till markörernas placering i rummet i tre dimensioner. I Figur 5 ges en översikt av Målföljningssystemet. För mer detaljerad information av målföljningssystemet hänvisas läsaren till den tekniska dokumentationen ifrån föregående års projekt [3]. Figur 5: Denna figur illustrerar målföljningssystemets interaktion med sin omgivning. Som framgår är skillnaden mellan hur hinder och bilar hanteras liten. 6.1 Utveckling Systemet har utökats och har nu förmågan att följa hinder med godtycklig dynamik, vilket principiellt är detsamma som att följa en bil utan känd dynamisk modell. För att kunna följa dynamiska hinder på ett bra sätt använder sig systemet av en generell modell som lätt kan ändras och anpassas till olika situationer, till exempel en konstant hastighetsmodell. För att målföljningssystemet ska kunna se skillnad på olika hindertyper har varje objekt en speciell geometri på reflexmarkörerna, och information om reflexmarkörernas position

LiU Racetrack 9 för varje hinder finns hårdkodade i filen CarMarkerCameraIdentParam.cpp. Under rubriken Markörpositioner nedan har de nya och uppdaterade reflexmarkörgeometrierna för fordon och fysiska hinder beskrivits. En annan funktionalitet som har implementerats är vinkelskattning för lastbilekpiaget, vilket det står mer om i Avsnitt 6.3. 6.2 Markörpositioner Nedan beskrivs de nya markörpositionerna för lastbilsekipaget och för de fysiska hindren. Markörpositionerna för de små bilarna har inte ändrats och tas inte upp här, men finns beskrivna i föregående års tekniska dokumentation [3]. Lastbil (ID 3) x [m] y [m] z [m] Släp (ID 4) x [m] y [m] z [m] Markör 1 0.073-0.023 0.101 Markör 1 0.118 0.0 0.095 Markör 2 0.04 0.024 0.103 Markör 2 0.088-0.034 0.095 Markör 3 0.04-0.024 0.013 Markör 3-0.1 0.031 0.095 Markör 4-0.072 0.0 0.018 Markör 4-0.1-0.031 0.095 Dolly (ID 5) x [m] y [m] z [m] Hinder 1 (ID 6) x [m] y [m] z [m] Markör 1 0.0 0.04 0.089 Markör 1 0.04 0.0475-0.018 Markör 2 0.017-0.020 0.089 Markör 2-0.04 0.0475-0.018 Markör 3-0.017-0.04 0.089 Markör 3-0.02-0.0275-0.018 Markör 4 0.04-0.0475-0.018 Hinder 2 (ID 7) x [m] y [m] z [m] Markör 1 0.018 0.02-0.018 Markör 2-0.018 0.02-0.018 Markör 3-0.018-0.02-0.018 Markör 4 0.02-0.02-0.018 Tabell 5: Tabell över de uppdaterade positionerna för fordon och fysiska hinder. För övriga fordons positioner, se [3]. Positionerna är markörernas positioner i ett kroppsfast koordinatsystem med origo i fordonets eller hindrets centrum. 6.3 Skattning av dollyvinklar För att skatta dollyvinklarna α och β för lastbilsekipage i Figur 6 har två olika formler använts. Den ena tar hänsyn till dollyns position och den andra gör det inte och dessa formler beskrivs nedan. Beskrivningen av vinkelestimeringen är i detta dokument lite förenklad för att inte komplicera läsandet men i filen angles.cpp finns koden för vinkelestimering tillgänglig för närmare inspektion. För att skatta α och β utan dollyns position har (1) - (2) använts α arctan p1y p 2y θ 1 (1) p 1x p 2x β arctan p1y p 2y p 1x p 2x θ 2 (2) där θ 1 och θ 2 är lastbilens respektive släpets vinkel i globala koordinater och p 1x, p 1y och p 2x, p 2y är bakre punkten på lastbilen samt främre punkten på släpet, se Figur 6.

LiU Racetrack 10 Figur 6: Illustration av lastbilsekipaget, innefattande lastbil, dolly och släp. Vinklarna som ska estimeras är α och β. I figuren syns också det globala koordinatsystemet, det vill säga bilbanans koordinatsystem. För att göra skattningen säkrare kan även dollyns position tas med i beräkningarna, enligt (3) - (4), där medelvärdet av vinkeln mellan lastbil släp samt mellan lastbil och dolly eller släp och dolly beräknas. Dollyns vinkel har inte tagits hänsyn till och det är något som i framtiden skulle kunna implementeras och förmodligen öka prestandan något. p arctan 1y p 2y p 1x p α 2x p arctan 1y p 3y p 1x p 3x θ 1 (3) β p arctan 1y p 2y p 1x p 2x p arctan 1y p 3y p 1x p 3x 2 där p 3x och p 3y är dollyns mittpunkts position i X- och Y-led, se Figur 6. 2 θ 2 (4) För att utvärdera prestandan för vinkelestimeringen utfördes tester då lastbilsekipaget stod still på bilbanan. Resultaten av testerna kan ses i Tabell 6 för vinkelskattning utan dollyposition och i Tabell 7 för vinkelskattning med dollyposition. Fall nr: α r s β r s Skattad α r s Skattad β r s 1-30 -30-30 1.2-30 1.2 2-30 0-30 1.2 0 1.2 3-30 20-30 1.2 20 1.2 4 0-30 0 1.2-30 1.2 5 0 0 0 1 0 1 6 0 20 0 1.2 20 1.2 7 20-30 20 1.2-30 1.2 8 20 0 20 1.2 0 1.2 9 20 20 20 1.2 20 1.2 Tabell 6: De vinklar som ska skattas för lastbil och släp utan hjälp av dollyns position. De skattade värdena skrivs ut som ett genomsnittligt värde och en maximal avvikelse. Slutsatsen som kan dras av Tabell 6 är att vinkelskattningen ger lika bra resultat för olika vinkelkombinationer och att avvikelsen endast är 1.2. Tabell 7 visar att prestandan kan förbättras ytterligare då dollyns position tas med i beräkningarna. Då blir avvikelsen endast 1. Att precisionen inte blir högre än så tror vi har mycket att göra med att kamerornas precision bara är 2mm, vilket bidrar till skattningsfel.

LiU Racetrack 11 Fall nr: α r s β r s Skattad α r s Skattad β r s 1 30-30 30 1-30 1 2-20 0-20 1 0 1 3-10 20-10 1 20 1 Tabell 7: De vinklar som ska skattas för lastbil och släp med hjälp av dollyns position. De skattade värdena skrivs ut som ett genomsnittligt värde och en maximal avvikelse. 6.4 Gränssnitt Målföljningssystemets gränssnitt presenteras i Tabell 6.4. Insignaler Bilder Utsignaler Position px, Y q, hastighet v x, v y, vinklar θ, α, β Beskrivning Bilder från IR-kamerorna. Information om objekt skickas till de andra delsystemen Tabell 8: I denna tabell presenteras alla in- och utsignaler som målföljningsystemet använder sig av och genererar. 7 Delsystem: Hinderhanteringssystem Hinderhanteringssystemets uppgift är att hantera information om alla fysiska och virtuella hinder på bilbanan. Beslutssystemet som sitter i bilen skickar en förfrågan till hinderhanteringssystemet om det finns några hinder inom bilens valbara område. Om så är fallet får beslutssystemet ta del av information om de aktuella hindrena, exempelvis position och utbredning. 7.1 Implementering Hindersystemet har implementerats enligt följande. 7.1.1 Klass för geometriska figurer Hinder och bilens virtuella sensorer representeras som geometriska objekt. För detta ändamål är klassen Shape skapad som en virtuell klass. Följande konkreta, geometriska figurer ärver från denna klass: Circle: Enkel cirkelskiva. Trapezoid: Fyrhörning med två parallella sidor (ej lika långa) och två likbenta ben. Mer specifikt: Denna klass representerar en likbent parallelltrapets. Square: Enkel rektangel. Att klassen inte heter Rectangle som bättre skulle beskriva klassen beror på en namnkonflikt med en inbyggd Windows-funktion. Dessa geometriska figurer har utbredning men är inte bundna till någon fix position. De två sistnämnda har dock varsin medlem i form av en vinkel som hjälper till att orientera figuren i det globala koordinatsystemet.

LiU Racetrack 12 7.1.2 Hinderklass Hinder implementeras genom klassen Obstacle. Hinder har alltid formen av en rektangel varför klassen har en medlem som är av klassen Square. Detta ger hindret sin geometriska utbredning i det faktiska systemet. Dessutom har hinder en position i det globala koordinatsystemet som anges i form av koordinater i X- och Y -led. Hinder kan både vara statiska och dynamiska. För ett dynamiskt hinder (som kan röra på sig) har klassen två medlemmar i form av vektorer, trajectory_x och trajectory_y som anger den trajektoria hindret rör sig utefter i X- respektive Y -led. För att skilja olika typer av hinder åt har klassen Obstacle en medlem obstacle_type i form av ett heltal. Detta hjälper användaren att dela upp hindren i olika kategorier såsom virtuella, fysiska, statiska och dynamiska eller skräddarsydda typer av hinder. 7.1.3 Sensorklass Varje reglerad bil har numera en aktiv medlem från klassen Sensor. Denna klass har en medlem som anger vilken sensortyp som är aktiv och de olika sensortyperna som ges i form av klassen Shape. Dessutom har klassen en lista över vilka typer av hinder som kan detekteras och dessa bestäms av användaren vid initiering. Användandaren anger själv vilka bilar som är reglerade och därmed i behov av virtuella sensorer. 7.1.4 Hinderdetektering För att avgöra om ett hinder är innanför en bils sensorområde används en teknik som bygger på Seperating Axis Theorem. Satsen lyder i sin enkelhet att givet två konvexa, geometriska figurer (alla figurer i Shape-klassen är konvexa) överlappar varandra om och endast om det inte går att dra en rät linje mellan dem. Denna metod används mycket inom spelprogrammering och implementationen av densamma är enkel. Algoritmen är dessutom snabb och helt säker (hinder detekteras alltid om sensorområde överlappar hindrets utbredning). 7.2 Gränssnitt Insignaler Position, hastighet, utbredning Utsignaler Position och utbredning av ett specifikt hinder Beskrivning Får information om fysiska hinder på bilbanan från målföljningssystemet. Skickar information om hinder i bilens hindersensorers valbara område till beslutssystemet. Tabell 9: I denna tabell presenteras alla in- och utsignaler som hinderhanteringssystemet använder sig av och genererar.

LiU Racetrack 13 8 Delsystem: Beslutssystem Detta avsnitt beskriver den beslutsfattande delen i bilen. Målet med beslutssystemet är att kunna reagera på hinder som bilen får information om. Figur 7: Illustration av detektion av hinder. Ett hinder kommer in i bilens avståndssensorer och information om hindret läggs in i beslutssystemets lista över hinder i bilens synfält. 8.1 Implementation Implementationen av detta system har gjorts i filen regulator.cpp. Där har beslutsfattandet för vad bilen ska göra då ett hinder detekteras av dess sensorer implementerats, se Figur 8. De olika besluten och händelserna kommer här förklaras mera detaljerat. Detekterat hinder?: Hinderlistan för bilarna på banan itereras igenom. Är hinderlistan tom ser bilen inget hinder. Är den inte tom ser bilens sensorer ett hinder. Är hinderlistan inte tom för den kontrollerade bilen har ett hinder detekterats och detta block returnerar ja. Stanna bilen: Den kontrollerade bilen stannas genom att sätta en flagan som heter STOP CAR till true för dennes kontrollobjekt totcontroller. Flaggan sätter utsignalen för gaspådraget till -1. Systemet kommer även börja skicka information om bilen och hindrets position till en eventuell RRT-klient. Omkörningstrajektoria mottagen?: Systemet kommer i varje loop att fråga om kommunikationen har tagit emot någon uppdaterad trajektoria. Om inte detta har skett kommer systemet fortsätta skicka bilen och hindrets position tills detta uppfylls. Körs RRT-klienten på rt-pc-03 kommer denna efter att ha tagit emot information om bilen och hindret att svara med en omkörningstrajektoria om en sådan är möjlig att räkna ut och detta block kommer då returnerar ja. Följ ny trajektoria långsamt: När en ny trajektoria är mottagen kommer denna sparas temporärt. Systemet frågar vilket index på optimala referenstrajektorian som är närmast bilens position. Detta index (refererars vidare som startindex) används som utgångspunkt för att skriva över den optimala referenstrajektorian med den mottagna från RRT n. När detta är utfört sätts en flagga som heter SLOW MOVE till true i den kontrollerade bilens kontrollobjekt. Denna tillåter långsam framfart (hårdkodad hastighet i totcontroller.cpp). Nu kommer den kontrollerade bilen att följa den mottagna RRT trajektorian långsamt. Har hindret passerats?: Så länge hindret inte har passerats kommer bilens position att

LiU Racetrack 14 jämföras med startindex adderat med mottagna trajektorians längd. Överskrider bilens position startindex adderat med trajektorians längd anses hindret passerat. Läs in optimala trajektorian: När hindret har passerats läses den optimala trajektorian över de positioner där RRT-trajektorian har skrivits. Normal hastighet kommer sättas till bilen genom att sätta SLOW MOVE till false och kommunikation med RRT-klient stängs av. Fortsätt köra i normalläge: Här kör bilen i normalläge och kan ta emot GPGO - kommunikation eller starta denna besluts process om sensorerna detekterar ett hinder. Figur 8: Flödesschema över hur beslutsystemet reagerar på hinder. 8.2 Gränssnitt I Tabell 10 visas den information som detta delsystem behöver från andra system och vilka utsignaler detta genererar.

LiU Racetrack 15 Insignaler Hinder Bilens tillstånd RRT-trajektoria Utsignaler Beslut Visualisering Beskrivning Får information om hinder inom bilens hindersensorers valbara område från hinderhanteringssystemet. Information om bilens tillstånd för att kunna jämföra absolut position av hinder och bil X,Y och θ värden som beskriver en omkörningstrajektoria kring ett hinder. Skickar beslut till referensgenereringssystemet vilken typ av trajektoria som ska genereras Skickar information till visualiseringssystemet att t.ex. rita ut att ett hinder är upptäckt av bilen. Tabell 10: I denna tabell presenteras alla in- och utsignaler som beslutssystemet använder sig av och genererar. 9 Delsystem: Referensgenerering Referensgenereringssystemets uppgift är att generera de referenstrajektorier som reglersystemet ska reglera bilen efter. Referensgenereringssystemet styrs av beslutssystemet, som fattar de principiella besluten om hur bilen ska bete sig. Figur 10 illustrerar strukturen för hur flödet planeras att utformas. I normalfallet matas en förberäknad optimal trajektoria in till reglersystemet, men när ett hinder upptäcks i bilens planerade körväg tar ett lokalt system i bilen över referensgenereringsarbetet för att generera en ny, tillfällig trajektoria runt hindret. Tills dess att hindret är passerat följer bilen denna lokala trajektoria istället för det optimala banvarvet. Figur 9 illusterar bilbanan och den förberäknade optimala trajektorian. Datn för den optimala trajektorian innehåller även vilka vinklar och hastigheter som bilen teoretiskt skulle ha då den kör längs den optimala trajektorian. Då bilen har svårt att följa trajektorian på det sätt som är optimalt har därför istället hastigheterna ersatts med en låg hastighet som systemet sedan successivt försöker optimera genom metoder från maskininlärning, vilket beskrivs i Avsnitt 10. 9.1 Lokala trajektorier Beräknandet av de lokala trajektorierna är tänkt att implementeras med hjälp av en vägplaneringsalgoritm kallad Rapidly exploring Random Tree (RRT). I korthet kan det beskrivas som att man på ett smart sätt simulerar vad olika styrsignaler vid olika framtida tidpunkter skulle ge för resultat, med andra ord en slags heurestik. Denna typ av avsökningsmetodik har tidigare prövats och visat sig mycket lyckad i exempelvis [5], [9] och [6]. I de två senare har RRT använts just för att beräkna trajektorier för bilar, och i framför allt [6] har det visats att det är möjligt att generera en lämplig trajektoria till bilen i realtid med denna metodik. Om ett hinder som är placerat ivägen för den optimala trajektorian har hittats ska RRT:n beräkna en ny trajektoria, med avsikten att föra bilen runt hindret och sedan hjälpa den tillbaka till den optimala trajektorian. Problematik som finns med konceptet RRT

LiU Racetrack 16 Figur 9: Bilbanan avbildad med den förberäknade optimala trajektorian utritad i blått. Figur 10: Översikt av referensgenereringssystemet. Referensgenereringssystemet väljer mellan att fortsätta leverera den förberäknade optimala trajektorian till reglersystemet och att räkna ut en ny lokal trajektoria för att kunna passera hinder. Den blåa romben symboliserar beslut som tas av beslutssystemet. är att det är beräkningskrävande och i svåra fall tar det realtivt lång tid att finna en lämplig trajektoria. Genom att dela upp problemet i delmål går det dock att påskynda planeringen. RRT:n slumpar positioner på banan på en smart sätt och sedan, med hjälp av bilens kinematik, beräknas en styrlag för att följa en möjlig cirkelbåge till positionen. Denna slumpgenerering kallas hädanefter för en sampling. Styrlagen som används för att beräkna trajektorian för den bilmodell som simuleras i RRT:n kallas Pure Pursuit och beskrivs i [4] och bygger enbart på ren geometri. Denna algoritm har implementerats i C++ av Niclas Evestedt och har sedan använts av denna grupp för att nicha koden för vårt endamål. En skärmdump av programmet kan ses i Figur 11

LiU Racetrack 17 Figur 11: Figurerna visar en ska rmdump fra n Niclas Evestedts RRT-simulator. I detta fll simuleras en omko rning av ett hinder. Bilen sta r i detta fall still och bera knar en trajektoria fo rbi hindret till ma let pa andra sidan (gul cirkel). Na r en trajektoria (helro d linje) har hittats pa bo rjas omko rningen och under omko rningen fo rba ttras den ta nkta trajektoria successivt fra n ny information och nya samplingar. De gula linjerna a r de samplade referenspunkterna som Pure pursuit regulatorn har reglerat mot. 9.2 Teori Den grundla ggande teorin bakom konceptet RRT a r omfattande och beskrivs utfo rligt i exempelvis [5]. I detta dokument kommer bara den konceptuella strukturen fo r algoritmen att tas upp. I [6] fo rsla s det att det a r enklare att sampla ett slutet stabilt system och sa ledes sampla referensva rden till en regulator ista llet fo r att sampla faktiska styrsignaler till systemet. Fo r att frama t-simulera system anva nds en kinematisk bilmodell som sedan a terkopplas med en va gplanerande regulator sa givna referenspositioner kan fo ljas, en illustration av det slutna systemet som da rmed erha lls kan ses i Figur 12. Figur 12: Figuren visar det sluta systemet som anva nds fo r en frama t-simuleringar med RRT. Givet en referens punkt ra knar regulatorn ut en la mplig styrlag till bilen. Kursnamn: Projektgrupp: Kurskod: Projekt: Reglerteknisk projektkurs FAST TSRT10 LiU Racetrack E-post: Dokumentansvarig: Fo rfattarens e-post: Dokument: liurace@googlegroups.com Salko Bjelevac salbj522@student.liu.se Teknisk dokumentation.pdf

LiU Racetrack 18 9.2.1 Optimeringsproblemet Denna sektion definerar vägplanerings-problemet. Givet ett system som beskrivs av en olinjär modell 9pptq fppptq, uptqq, pp0q p 0 (5) där p och u är modellens tillstånd respektive styrsignaler och p 0 är initialtillstånden vid tiden t 0. Givet att det finns bivillkor för både tillstånden och styrsignalerna som fångas av bivillkoren U respektive X free ptq. Bivillkoret U beskriver eventuella stysignalsbegränsningar och X free ptq beskriver omgivningen där det är tillåtet att befinna sig. Vidare har vi kravet att ppt f q P X goal som anger att vi har ett krav på sluttillståndet vid tiden t t f. Optimeringsproblemet kan slutligen formuleras som; beräkna styrsignalsekvensen uptq, t P r0, t f s, t f P r0, 8q som minimerar minimize t f Γpppt f qq u # 9pptq fppptq, uptqq, pp0q p subject to 0, ppt f q P X goal (6) pptq P X free ptq, uptq P U där Γpppt f qq är ett straff på systemets sluttillstånd. Givet konstant hastighet översätts minimeringsproblemet till ett kortasteväg-problem med en tilläggskostnad för systemets slutkonfiguration ppt f q. 9.2.2 Bilmodell I RRT-programmet används en enkel kinematisk beskrivning av bilen. Den kinematiska modellen (7) som används består av tre tillstånd p rx, Y, θs T ; bilens position px, Y q som är position av mittpunkten av dess bakre axeln i globala koordinater och bilen orientering θ med avseende på det globala koordinatsystemet. L är längden mellan bilens fram- och bakaxel. Insignalerna till bilen är hastigheten v och styrvinkel γ, u rv, γs T. Alla dessa tillstånd och styrsignaler är illuserarade i Figur 13. $'& '% 9X v cos θ 9Y v sin θ (7) 9θ G ss pvq v L tan γ där G ss 1 1 v V CH (8) G ss ska beskriva inverkan av slip vid en given hastighet och V CH har valts till 20 [cm/s] utan några tester mer än vid simulering och ska väljas så att underlagets friktion så bra som möjligt modelleras. Inverkan av G ss är att bilen kommer understyra vid höga hastigheter. Den kinematiska modellen (7) är sedan diskretiserad med euler-framåt approximation av derivata vid implementering. I och med att (8) är ett statiskt samband behöver den inte diskretiseras. 9.2.3 Regulator För att kunna sampla referenspunkter till vårt slutna system vid simulering av trajektorier används en väg följande regulator kallad Pure-pursuit. Den arbetar med ett look-ahead

LiU Racetrack 19 Figur 13: Figuren illusterar alla de tillstånd, styrsignaler och designparametrar som används av bilmodellen och regulatorn Pure-pursuit vid simulering av en trajektoria med RRT:n. avstånd R och givet en referenspunkt den vill nå (I Figur 13 benämnt som px, yq) beräknas en styrlag för att nå denna punkten. Eftersom att bilen hela tiden kommer förflytta sig kommer även referenspunkten att göra det samma och man erhåller en mjuk rörelse. Ju större look-ahead avståd desto mjukare bana. Om look-ahead avståndet väljs för kort kan stabilitet inte garanteras och det är således en avvägning mellan följsamhet och robusthet. Styrlagen erhålls genom rena geometriska samband och en härledning har gjorts nedan (Se Figur 13 för definition av variablerna som används): x d r (9a) x 2 y 2 R 2 (9b) y 2 d 2 y 2 pr xq 2 r 2 ô (9c) loomoon R 2 r 2 2rx x 2 y 2 r 2 ñ r R 2 2x R 2 2R sin θ e R 2 sin θ e (9d) Där r är önskad kurvradie. Vidare med hjälp av sambandet v r 9 θ (Negativt tecken på grund av att θ är definerad positiv moturs.) och (7) får vi till slut önskad styrlag (10) där

LiU Racetrack 20 θ e är riktingsavvikelsen mot målet. γ arctan L r arctan 2L sin θe RG ss (10) Denna styrlag används för att sluta loopen vid framåt-simuleringar. 9.2.4 Samplingsstrategier Som ovan nämnt samplas referenspositioner till vår regulator vid framåt-simulering. Denna sampling kommer bestå av en lista med tre olika kommandon per element px, y, v cmd q i där i P t1,..., n ref u är mål placerade med jämna mellanrum för att pure-pursuit regulatorn ska få ett glidande mål att följa. Den sista referenspunkten är den som samplas och sedan skapas stödpunkter bakåt iterativt. v cmd bestäms deterministiskt så att sista referens punkten får hastighet noll. Sampling av px, yq nref görs med olika samplingsstrategier och de två som används är gaussisk-sampling och arc-sampling. Gaussisk-sampling som med hjälp av någon referens position px 0, y 0 q är given av x y n ref x0 σ x n x y 0 σ y n y där σ x och σ y är standard avvikelse i respektive led och är design parametrar. n i är en standard gaussisk slumpvariabel. Arc-sampling är lite annurlunda och lyder: Givet en refrensposition px 0, y 0, θ 0 q ges den samplade punkten av x y n ref x0 y 0 r cos θ sin θ där (11) #r σ r n r r 0 θ σ θ n θ θ 0 (12) där σ r och σ θ är standard avvikelse i radial och i omkrets rikting och n i är även här en standard gaussisk slumpvariabel. 9.2.5 Rapidly-exploring Random Tree (RRT) Med utgångspunkt i en kombination av [5], [9] och [6] har strukturen för en förenklad algoritm av vår RRT tagis fram och visas i Algoritm 1. I algoritmen initieras först vår RRT och det skapas en RRT-planerare som innehåller en tom trädstruktur, en bilmodell, (del)mål och en karta över alla för bilen kända hinder och väggar på banan. Algorithm 1 Body to RRT 1: initiate RRT 2: while RRT is activated do 3: Uppdate vehicle states and obstacles 4: while t T do 5: ExpandTree(); 6: end while 7: if Trajectory is found then 8: Send trajectory to server 9: end if 10: end while

LiU Racetrack 21 Efter att vår RRT har initierats byggs lösningsträdet genom att sampla referensvärden till vår vägföljande regulator. Detta görs under en begränsad tid T och efter detta så utvärderas trädet och om en trajektoria har hittats till målet så skickas denna via TCP till huvudprogrammet. Efter detta så upprepas proceduren igen med uppdaterade tillstånd på bilen och hindret erhålls via TCP från huvudprogrammet. Funktionen ExpandTree() som beskriver hela RRT funktionaliteten är beskriver i psedukod i Algoritm 2. Algorithm 2 ExpandTree(): 1: x rand Ð SampelpGoal position, position of car, Sampel strategyq; 2: if x rand P X free then 3: Generate a random variable between 0 and 1 called Optimize 4: if Optimize 0.7 then 5: X near Ð NearpTree structure, x rand q; 6: for x near P X near do 7: Try to connect with nearest neighbour 8: end for 9: else 10: Try to connect x rand with root of the tree 11: end if 12: end if 13: if fesibletrajectoryfound() then 14: BestNode Ð GetBestNode(Tree structure); 15: end if Denna funktion börjar med att genera en ny samplad referensposition givet en samplingstrategi som gör med funktionsanropet Sampel(). När ett sampel har genererats kontrolleras det så att samplet hamnade inuti tillåtet område X free, vilket definieras som alla punkter som är inuti banan som inte är inuti ett hinder. När ett lämpligt sampel har genererats kommer 70% av gångerna samplet försöka paras ihop med rooten av trädet och resterande 30% av gångerna undersöks vilka nära grannar som redan finns i trädstrukturen som görs med funktionsanropet Near(). Om det saknas grannar kommer istället roten till trädet att väljas som förälder om detta är möjligt. Alla nära grannar genomsöks för att hitta den billigaste föräldern att koppla ihop sig med. Samtidigt kontrolleras att inget hinder mellan sampels omöjliggör ihopkoppling. I slutet av varje iteration kontrolleras sedan om vi har hittat en trajektoria som tar oss till vårt mål eller delmål. Slutvillkoret på funktionen feasibletrajectoryfound() avgör ifall en nod ska bedömas som framme eller ej. Detta avgörs beroende på om nodens tillstånd är tillräckligt nära målets önskade tillstånd i både aspekten position och vinkel. 9.3 Körlägen Det finns två olika körlägen som ger olika typer av beteenden för fordonet vid omkörning eller vid väjning av hinder, säkerhetsläge och racingläge. Vilket läge som är aktivt bestäms av användaren i användargränssnittet. Körlägena har olika samplingsstrategier i RRT programmet, och just nu används bara säkerhetsläget när programmet kommunicerar med racetracksystemet. Att kunna använda både ett racingläge och ett säkerhetsläge i racetracksystemet är något som vi ser som en potentiell vidareutveckling. 9.3.1 Säkerhetsläge I säkerhetslägen påbörjas en omkörningen endast när RRT:n har lyckats att generera en full trajektoria förbi hela objektet. Detta gör att det inte existerar en tidspress under själva

LiU Racetrack 22 beräkningen av trajektorian eller att påbörja en omkörning eftersom hela trajektorian redan är färdigräknad vid start av omkörningen. Samplingsstrategin i säkerhetsläget är att enbart hitta en säker väg fram till nästa checkpoint. Detta resulterar i en säkrare omkörningsprocedur eftersom fordonet inte behöver chansa på att RRT:n ska hitta en fullständig omkörningstrajektoria. 9.3.2 Racingläge I racingläget där RRT:n ska användas för att beräkna en omkörning ställer hårdare krav på prestanda med avseende på snabbhet. För att påskynda referensgenereringen ska samplingstrategin dela upp sin sökning mellan en körväg fram till nästa checkpoint, och till checkpointen efter det. På det viset kommer omkörningen att kunna ske snabbare än i säkerhetslägen, på bekostnad av att kanske inte den allra snabbaste vägen hittas. 9.4 Det implementerade RRT-programmet Algoritmerna för RRT har implementerats i racetracksystemet. I denna del kommer systemets RRT applikation att presenteras. RRTs uppgift är att givet fordonets tillstånd och information om hinder beräkna en omkörningstrajektoria. 9.4.1 Uppstart av programmet Programmet som kör RRT hittas på rt-pc-03 (den nya datorn) i mappen C:/Temp/OSAAR/RRT/RRT 2014 Final. För att starta programmet öppnar man filen 1project solution.sln. Programmet realiserar Algoritm 1 och 2 som beskrivs i teoriavsnittet. I MAIN.cpp väljer användaren ifall RRT ska köras mot racetracksystemet eller i utvecklingsmiljön. Detta görs genom att definiera antingen DEV MODE eller RUNNING WITH SERVER. Vill man göra ändringar i programvaran för DEV MODE vara aktiverat för att enkelt kunna testa sina ändringar i simuleringsmiljön. För att starta programmet är det enklast att trycka ctrl + F5 från projektet. Det medför att programmet kör exekveringsfilen i release-mappen. 9.4.2 DEV MODE I utvecklingsläget körs programmet i en simuleringsmiljö istället för att det kommunicera med racetracksystemet. Själva miljön är konstruerad att likna racetrackbanan så mycket som möjligt, vilket har gjorts genom att mäta bilbanan och utforma banan i simuleringsmiljön i samma form och storlek. Simuleringsmiljön illustreras i Figur 14. 9.4.3 RUNNING WITH SERVER Detta läget startar kommunikationslänken med racetracksystemet. Nu kommer bilen på banan, när dess sensorer detekterar ett hinder, att invänta en trajektoria från RRT. För att skicka en omkörningstrajektoria till racetracksystemet kör man RRT applikationen efter det att ett hinder detekterats. 9.4.4 Under simulering Under den simulerade körningen har de samplingsstrategier som beskrevs i teoriavsnittet implementerats. Hur de samplade referenspunkterna och deras stödpunkter ser ut och

LiU Racetrack 23 Figur 14: Simuleringsmiljo n fo r RRT applikationen. Banan ska i sto rsta mo jliga ma n efterlikna ko rva gen fo r det fysikaliska systemet fo r att kunna bera kna en omko rningstrajektoria med samma begra nsningar som finns i racetracksystemet. hja lper regulatorn att styra mot ma let syns i Figur 15. Genom att ho gerklicka pa ska rmen kan man va lja olika visningsla gen. 9.4.5 Skillnader mellan simuleringsmiljo och racetracksystem Det finns vissa skillnader mellan det fysikaliska systemet och simuleringen. Viktigt att komma iha g na r man genomfo r a ndringar i RRT programmet a r att stra ckor uttrycks i centimeter [cm] och hastighet i centimeter per sekund [cm/s], men i o vrigt a r koordinatsystemen identiska. 9.4.6 Hantering av dynamiska hinder Fo r att hantera dynamiska hinder skattas hindrets position frama t i tiden med hja lp av konstant hastighets antaganden och kollisionskontroll go r sedan med tiden pa ba de hindret och bilen. En omko rningstrajektoria kan endast fullfo ljas om det inte finns na gon kollision i na got samplingso gonblick. 9.5 Gra nssnitt Vilka in- och utsignaler som delsystemet beho ver beskrivs i Tabell 11. 9.6 Utvecklingsmo jligheter Det finns flera utvecklingsomra den fo r referensgenereringssystemet. Uto ver att arbeta pa kommunikationsla nken mellan RRT och racetracksystemet fo r att fa systemet mer robust, fo resla r vi fo ljande: Testa la mpligare samplingsstrategier fo r att hitta ba ttre va gar runt hinder. Kursnamn: Projektgrupp: Kurskod: Projekt: Reglerteknisk projektkurs FAST TSRT10 LiU Racetrack E-post: Dokumentansvarig: Fo rfattarens e-post: Dokument: liurace@googlegroups.com Salko Bjelevac salbj522@student.liu.se Teknisk dokumentation.pdf

LiU Racetrack 24 Figur 15: Referenspunkter och stödpunkter i RRT programmet. De blå linjerna illustrerar referens- och stödpunkter medan den gula linjen är det träd som ger den billigaste vägen mellan fordonets position och målet. Den röda linjen är den väg som fordonet kommer att följa. Sampelstrategin tillåter grenarna att kopplas ihop vilket hjälper fordonet att hitta en väg till målet. Insignaler Array med med hinder Bilens tillstånd Utsignaler Genererad trajektoria Trajektoria Information Hindrets position, orientering, storlek och dynamik. Position och hastighet. Flagga för färdig genererad trajektoria. Den beräknade trajektorian som består av positioner med tillhörande girvinkel på bilen i varje punkt. Tabell 11: I denna tabell presenteras alla in- och utsignaler som RRT:n använder sig av och genererar. Implementera racingläge för att kunna genomföra snabbare omkörningar genom att placera ut lämpliga delmål. Det finns funktionalitet för att söka mot både den kommande och nästkommande

LiU Racetrack 25 (del)målet (RacingMode). Detta bör kunna utnyttjas genom att delmål placeras ut förhoppningsvis ska då även hinder i svåra kurvor ska kunna undvikas snabbt och snyggt. Få till integreringen i racetracksystemet bättre. Exempelvis för att trajektorian oftare ska kunna uppdateras. I nuläget läggs bara en ny trajektoria in en gång och sedan uppdateras den inte. Byt fordonsmodell i RRT programmet mot en modell som i större utsträckning liknar den racerbil som används i racetracksystemet. PID-regulatorn som används av Racetrack skiljer sig tyvärr för mycket från purepursuit reglatorn som gör att bilen i verkligheten inte klarar följa föreslagen trajektoria. Antingen måste man göra trajektorian från RRT:n mer konservativ eller bör det bytas regulator i racetracksystemet mot en regulator som faktiskt reglerar med hjälp av framtida information från trajektorian.

LiU Racetrack 26 10 Adaptiv anpassning av hastighetsprofilen Som nämnts i tidigare avsnitt finns en trajektoria beräknad som under vissa förutsättningar motsvarar ett optimalt sätt att köra runt banan, även kallad den optiamala trajektorian. Trajektorian innehåller positioner samt vilka vinklar, vinkelhastigheter och hastigheter bilen optimalt skall ha för att kunna ta sig runt banan på kortast möjliga tid. Det har dock visat sig att systemet har svårt att följa trajektorian med de optimala hastigheterna. Detta antas bero på att den modell av bilen som användes vid framtagandet av den optimala trajektorian inte representerar verkligheten tillräckligt bra. I förra årets projekt implementerades en adaptiv hastighetsprofil med mål att minimera varvtiden. Principiellt fungerade den på så sätt att då trajektorian följs bra ökas hastigheten successivt och om avvikelsen till trajektorian är stor sänks hastigheten. För vidare detaljer hänvisas läsaren till förra årets tekniska dokumentation [3]. Denna implementering skedde ad hoc och fungerade inte helt tillfredställande. En av årets uppgifter var därför att utveckla en ny adaptiv algoritm med hjälp av väldokumenterade metoder från lärande system. 10.1 Optimeringsproblemet Den uppgift som systemet ska lösa genom användning av adaptiva metoder kan beskrivas som ett optimeringsproblem. Vi vill minimera tiden det tar för bilen att köra ett varv runt bilbanan. Detta genom att välja hastighetsprofilen optimalt, så den är argument till målfunktionen. Vårt optimeringsproblem karakteriseras av att det tar lång tid att evaluera målfunktionen, det vill säga att köra ett varv kring banan. Vi behöver därför en algoritm som väljer varje hastighetsprofil till utvärdering med omsorg. Efter tips från ämnesexperter har vi använt oss av metoden Gaussian Processes for Global Optimization (GPGO) [8], som författarna till artikeln hävdar är väl lämpad till denna typ av problem. Metoden tillämpar ett ramverk med gaussiska processer för att beskriva den okända målfunktionen. Mer bakgrund kring gaussiska processer i ett bredare maskininlärningssammanhang finns i [11]. Vi har fått stor hjälp av Andreas Svensson från Uppsala Universitet som fungerat som teknisk expert för GPGO. 10.2 Parametrisering av optimal trajektoria För att metoden GPGO ska vara användbar krävs att rummet av möjliga insignaler inte har för hög dimension. Det har därför varit nödvändigt att parametrisera hastighetsprofilen, som i dagsläget har 1001 punkter, så att dess utseende helt bestäms av färre parametrar. En styckvis affin approximation används för att utöka antalet parametrar till 1001. Detta genom en linjär interpolation mellan två parametrar i taget. Figur 16 och Figur 17 visar hur hastighetsprofilen för den förberäknade optimala trajektorian ser ut respektive var dessa paramatrar fysiskt befinner sig på banan.

LiU Racetrack 27 Figur 16: Optimal hastighet beräknad från tidigare års projektkurs. En styckvis affin approximation används för att utöka antalet parametrar till 1001. Figur 17: Grafisk illustration var de tretton parametrar som beskriver den optimala trajektorian fysiskt befinner sig på bilbanan.

LiU Racetrack 28 Under lång tid parametriserade vi hastighetsprofilen efter den optimala trajektorian vilket fungerade, men inte helt tillfredställande. Genom testning av förra årets adaptiva implementation analyserade vi hastighetsprofilen. Vi kom fram till att den optimala trajektorian ej speglar verkligheten väl med avseende på vilka parametrar som beskriver utseendet på hastighetsprofilen, se Figur 18. Med avseende på detta parametriserade vid hastighetsprofilen med femton parametrar, se Figur 19, vilket fungerade markant bättre. Se Figur 20 för en illustration var de femton parametrarna befinner sig fysiskt på bilbanan. Figur 18: Jämförelse av ett den optimala trajektorian och ett varv kört på bilbanan med förra årets adaptiva hastighetsprofil.

LiU Racetrack 29 Figur 19: Hastighetsprofil för ett varv på bilbanan evaluerat med förra årets adaptiva hastighetsprofil. En styckvis affin approximation används för att beskriva kurvan. Detta ökar antalet parametrar från 15 till 1001.

LiU Racetrack 30 Figur 20: Grafisk illustration var de femton parametrar som beskriver hastighetsprofilen för ett varv, uträknat från förra årets adaptiva ad hoc implementation, fysiskt befinner sig på bilbanan.

LiU Racetrack 31 10.3 Gaussian Processes for Global Optimization Gaussian Processes for Global Optimization baseras på att rummet av kandidater till den okända målfunktionen beskrivs i termer av gaussiska processer. Om vi låter x vara en hastighetsprofil och ypxq varvtiden som fås då hastighetsprofilen används så kan vi skriva: ppy xq N py; µpxq, Kpx, x 1 qq 2πKpx, xq 1 2 e 0.5py µpxqqt Kpxq 1 py µpxqq där K Kpx, x 1 q är den kovariansfunktion beskriver kovariansen hos varvtiden mellan två olika hastighetsprofiler och µpxq är väntevärdet av varvtiden för en viss hastighetsprofil. Eftersom vi inte känner µ eller K måste ingenjörskunskaper användas för att skapa funktioner som hjälpligt beskriver dessa förhållanden. Då det finns ett visst osäkerhetsmått så väljer vi funktioner som är parametriserade och inför fördelningar för dessa parametrar, kallade hyperparametrar och betecknade θ. Hyperparametrar är alltså parametrar som påverkar systemet, men som vi inte är intresserade av, men ändå måste ta med i beräkningarna. En av utmaningarna är hur kovariansfunktionen Kpx, x 1 q ska väljas. Det finns en uppsjö av olika kovariansfunktioner föreslagna i [11] och vi har valt squared exponential : Kpx, x 1 q expp 1 2 x x1 2 q I ovanstående exempel är faktorn 1 2 något som i teorin typiskt ingår i hyperparametrar θ. Genom att använda notation från [8] kan vi nu skriva sannolikheten för varvtid y vid en ej ännu testad hastighetsprofil x som: där: ppy x, θ, I 0 q N py ; m θ py I 0 q, C θ py I 0 qq m θ py I 0 q µ θ px q K θ px, x 0 qk θ px 0, x 0 q 1 py 0 µ θ px 0 qq C θ py I 0 q K θ px, x q K θ px, x 0 qk θ px 0, x 0 q 1 K θ px 0, x q I 0 är en informationsmatris som inkluderar all tidigare erfarenhet, inklusive de funktionsutvärderingar som gjorts. De hastighetsprofiler som tidigare utvärderats betecknas x 0 och deras resulaterande varvtider y 0. Innebörden av det ovanstående är att vi fått ett uttryck för väntevärdet och kovariansen av varvtiden som en oprövad hastighetsprofil kommer att resultera i. Vi ser även att om vi tidigare har utvärderat andra hastighetsprofiler, kommer vi med hjälp av kovariansfunktionen K att kunna använda de mätningarna för att justera vårt väntevärde och göra säkrare förutsägelser. Idén med GPGO är att optimera över dessa förutsägelser, så att vi alltid utvärderar den hastighetsprofil som kommer att ge oss den bästa blandningen av ny information och chans till låg vartid. För att kunna göra detta kan vi definiera förlusten, λpx q, på följande sätt: λpx q " y : y minpy 0 q minpy 0 q : y minpy 0 q

LiU Racetrack 32 Vi vill välja x s så att nästa funktionsutvärdering y px q minimerar den förväntade förlusten, Erλpx s. Eftersom vi vet vad y 0 är och har uttryck för fördelningen hos y kan vi ta fram uttryck för fördelningen hos λpx. Att ta fram detta uttryck är lite omständigt, men kräver inget mer än vanlig sannolikhetslära och kännedom om normalfördelningar, för detaljerna hänvisar vi till [8]. Kontentan är att eftersom fördelningen hos y beskrivs av normalfördelningar så gör fördelningen av λ också det, och är då väl lämpad för numerisk optimering. Men för att minimera Erλpx qs med avseende enbart på x behöver hyperparametrarna θ marginaliseras bort ur sannolikhetsfördelningarna. Denna marginalisering är svår att göra analytiskt, och måste därför approximeras numeriskt, till exempel med Bayesian Monte Carlo (BMC) metodik. När vi alltså har ett uttryck för Erλpx qs så kan vi minimera det numeriskt och välja att utvärdera det x (det vill säga den hastighetsprofil) som minimerar den förväntade förlusten. Vi kallar denna förlustfunktion för V: där: V px I 0 q η pm θ ηqφpη; m θ, Cq CN pη; m θ, Cq η minpy 0 q Eftersom vi inte i varje iteration försöker uppnå en så låg varvtid som möjligt blir denna metod friare att undersöka hastighetsprofiler som skulle kunna vara riktigt bra men också riktigt dåliga, och kommer därför att på ett bättre sätt göra en avvägning av vad man i maskininlärningssammanhang kallar exploit (att optimera varvtiden) och explore (att lära sig mer om funktionen). Se Figur 21-24 nedan hur GPGO principiellt fungerar med en parameter. För detaljer hänvisas läsaren till GPGO dokumentationen [8].

LiU Racetrack 33 Figur 21: Principiell beskrivning hur GPGO i en parameter fungerar. Prickad svart linje beskriver den okända målfunktionen som vi vill optimera. Röda linje beskriver väntevärden för evaluerade värden, det vill säga hur vi tror att målfunktionen ser ut givet gammal information. Det rosa området beskriver kovarians för evaluerade värden. Blå linje beskriver förlustfunktionen som vi vill minimera. I x 1.6 har vi samplat förut och är därför säkra på hur fuktionen ser ut i denna punkt (liten kovarians). Minimum på förlustfunktionen är här x 0.

LiU Racetrack 34 Figur 22: Då x 0 var minimum på förlustfunktionen samplar vi här. Detta leder till att kovariansen minskar i x 0. Vi blir alltså mer säkra hur den okända målfunktionen ser ut för varje sampel. Vi får nu en ny förlustfunktion som minimeras. Denna gång ligger minimum på x 3.7.

LiU Racetrack 35 Figur 23: Då x 3.7 var minimum på förlustfunktionen samplar vi här. Detta leder till att kovariansen minskar i x 3.7.Vi får nu en ny förlustfunktion som minimeras. Denna gång ligger minimum på x 6.

LiU Racetrack 36 Figur 24: Då x 6 var minimum på förlustfunktionen samplar vi här. Detta leder till att kovariansen minskar i x 6. Som synes får vi mer och mer information hur målfunktionen ser ut för varje sampel. Även om varvtiden inte blir bättre direkt så är all information bra information med avseende på att vi får mer information om målfunktionens utseende.

LiU Racetrack 37 10.4 Implementation Vi har implementerat GPGO i Matlab på en extern dator. Vi använder oss av Matlabbiblioteket Gaussian Processes for Machine Learning (GPML) [11]. Genom att använda GPML-biblioteket sköts en hel del av teorin av biblioteket [10]. Svårheten har varit att skapa en brygga mellan teorin i GPGO och funktionaliteten i GPML. Vi skickar data mellan den externa datorn och rt-pc-11 över TCP (Transmission Control Protocol). Programmet är modulärt uppbyggt och det enda som krävs är en Ethernet-kabel samt att ändra inställning till statiskt IP på den externa datorn. Huvudprogrammet i C++ på rt-pc-11 startas först och någon gång under körning startar användaren GPGO genom att exekvera det första blocket i filen main.m. GPGO börjar då att räkna ut en hastighetsprofil. När hastighetsprofilen är uträknad och vi använt en splinefunktion för att öka antalet parametrar till 1001 skickas den till rt-pc-11. GPGO-implementationen väntar då på att få tillbaka en varvtid. Se Figur 25 för en grov skiss över GPGO-implementationen. Filstrukturen på GPGO-implementationen ser ut enligt Figur 26. Figur 25: Gränsnitt för GPGO-implementationen på extern PC och rt-pc-11 över TCP.

LiU Racetrack 38 Figur 26: Filstruktur över implementationen av GPGO i Matlab med tillhörande beskrivning av varje fil.

LiU Racetrack 39 Som synes finns det två textfiler som läses in vid uppstart. Dessa ser ut enligt Figur 27 och Figur 28. Vid nerstängning av programmet måste användaren spara ner nya värden manuellt genom att köra det andra blocket i GPGO-implementationen i filen main.m. Detta för att användaren ska göra ett aktivt val att spara ner nya värden för att förhindra att dåliga värden sparas ner vid exempelvis oavsiktligt avbrott av körning. Figur 27: I filen Velocity paramaters.txt specificeras evaluerade hastighetsprofiler och laddas in vid start av programmet. Filen har en ny hastighetsprofil per rad och värden på alla 15 parametrar finns på varje rad, skiljt av ett kommatecken. Till exempel hastighetsprofil 1 har hastigheten 4.91 i parameter 1 och hastigheten 2.079 i parameter 2. Figur 28: I filen Laptime.txt specificeras varvtider av evaluerade hastighetsprofiler specificerade i filen Velocity parameters.txt. Filen har en ny varvtid per rad och längden på de båda textfilerna måste överenstämma. Till exempel hastighetsprofil 5 fick en varvtid på 8.1 sekunder. Huvudprogrammet finns i main.m och består av två block. Det första blocket innehåller initiering, huvudloopen, kommunikation och spline. Det andra blocket innehåller en funktion för att spara värden om resultatet är tillfredställande. Se Figur 29 för en schematisk bild av huvudprogrammet.

LiU Racetrack 40 Figur 29: Schematisk bild över implementationen av GPGO i Matlab. I bilden syns initiering, huvudloopen med modifiering av upplösningsvektorn och optimering i form avgridsökning och numerisk optimering Koden är välkommenterad men specifika detaljer som nya användare behöver veta tas upp här. För övriga implementationsspecifika frågor hänvisas läsaren till källkoden respektive dokumentation för GPGO [8] och GPML [10][11]. num of param = 15 Antalet parametrar vi optimerar över. hb vec = [... ] Kolumnvektor med en övre gräns för hastigheten i varje parameter. lb vec = [... ] Kolumnvektor med en undre gräns för hastigheten i varje parameter. Notera att längden av hb vec och lb vec måste vara lika med antalet parametrar. res vec = [3 4 3 2 3 2 2 3 3 2 2 2 2 2 2 ] Kolumnvektor med upplösningen på varje parameter som gridsökningen ska undersöka. Denna uppdateras automatiskt genom hjälpfunktionen modify res vec.m.

LiU Racetrack 41 hyp2 Vi definierar hyperparametrar genom att bland annat lägga in väntevärde- och kovariansfunktion i en strukt med namnet hyp2. num of iterations = 4 Antalet gånger som huvudloopen ska itereras innan en uträkning är klar. En gridsökning och en numerisk optimering per iteration. Idag tar fyra iterationer ungefär 2-3 sekunder och en önskvärd övre gräns minimum av den bästa varvtiden. xpos = [1 70 134 172 247 290 361 414 461 531 627 671 749 800 886 1001 ] Index på parametrarna på den optimala trajektorian. Längden på vektorn måste vara lika med antalet parametrar. testset Kör Matlab-funktionen linspace beorende på lb vec, hb vec och res vec. Skapar en matris över värden som gridsökningen ska testa. get all combinations(num of param, testset, res vec) Funktionen get all combinations.m skapar en kombinationsmatris över alla kombinationer av värden i matrisen testset. gp(... ) Inbyggd funktion i GPML-biblioteket som räknar ut väntevärdena och kovarianser för kombinationsmatrisen viktat med alla gamla hastighetsprofiler och varvtider. Det är alltså m θ py I 0 q och C θ py I 0 q vi får ut här. Detta är vår gridsökning. fmincon(... ) Inbyggd funktion i Matlab som minimerar vår förlustfunktion V. Begynnelsevärdet till fmincon är resultatet från vår gridsökning. modify res vec.m Hjälpfunktion som analyserar resultatet mellan gridsökningen och den numeriska optimeringen för att modifiera res vec. Genom att ändra på res vec varje iteration görs gridsökning på nya områden som kan tänkas vara intressanta att testa. Tanken är att om differansen mellan resultatet av vår gridsökning och den numeriska optimeringen är liten är vi säkrare på dessa parametrar än de parametrar som har hög differans. Vi minskar då upplösningen på dessa parametrar och kan öka på andra. Detta för att kombinationsmatrisen inte ska växa okontrollerat. Styckvis affin approximation Den optimala trajektorian består av 1001 punkter. Därmed måste de parametrar som optimeringen tagit fram använda en linjär interpolation för att få upp antalet parametrar till just 1001. För att kunna använda den linjära interpolationen lägger vi till en parameter på slutet. Den utökade parametern har samma värde som parameter 1 då den första och sista punkten på trajektorian har samma koordinater. Resultatet blir en styckvis affin approximation. Längd på x 0 och y 0 - gammal information Hur många gamla värden kan vi vikta in innan det kostar för mycket beräkningstid? Vid runt 80 gamla varvtider märks stor skillnad jämfört med färre än 50 värden, men uppfyller fortfarande kravet att räkna ut en ny hastighetsprofil på mindre än ett varv. Dock är det inget problem i sig att beräkningarna tar längre tid. Det kan vara värt att ta bort dubblerad information från x 0 och y 0 för att göra rum för ny data. Dock har det visat sig att programmet har lärt sig systemet väl innan vi når upp i högt antal data. Av denna anledning är det upp till användaren att spara den data som är viktig efter körning.

LiU Racetrack 42 10.5 Resultat I Figur 30 ser vi varvtider med dagens implementation av GPGO. Den första toppen beskriver att GPGO ställer ut maximala hastigheter i alla parametrar eftersom det rimligtvis bör ge en bra varvtid. Resultatet blir dock en väldigt dålig varvtid då bilen inte håller sig på den optimala trajektorian. Hastigheterna sänks då och resultatet blir bättre. GPGO fortsätter då att undersöka hastigheter i närheten av den som gick bra. Det syns tydligt att bilen utnyttjar exploit and explore, även om varvtiden blir bra så kommer GPGO fortsätta att utforska andra hastighetsprofiler. Bästa varvtid med denna implementation är ungefär 7.5 sekunder. Figur 30: Resultat över varvtider körda med hastighetsprofiler uträknade med GPGO 10.6 Utvecklingsmöjligheter Det finns stora utvecklingsmöjligheter och många parametrar att trimma för framtida projektgrupper. Nedan listas saker som vi tror skulle förbättra prestanda på GPGO. Straffa avvikelse från referenstrajektoria Detta är nästa naturliga steg och vi tror att mycket tid finns att tjäna här. Genom att skicka med avvikelsen från systemet via TCP till GPGO-implementationen kan en funktion y tot varvtid konstant avvikelse bildas och sparas ner som y 0. Avvikelsen skulle kunna tänkas att vägas in som som den totala arean (integralen) av avvikelsen, den största avvikelsen eller avvikelsen i kvadrat på respektive punkt.

LiU Racetrack 43 lb vec, hb vec, res vec Dessa värden kan trimmas beroende på observationer och ingenjörskunskap. modify res vec(... ) Hur upplösningen ska ändras beroende på resultat är helt en ingenjörslösning och funktionen kan göras intelligentare. Parametrisering Antalet parametrar vi optimerar över och var dessa befinner sig på den optimala trajektorian kan ändras. Det är en avvägning mellan beräkningshastighet och noggranhet. Hyperparametrar Vi har inte lagt tid på att testa olika kovariansfunktioner eller väntevärdesfunktioner. Alla variabler som tillhör hyperparametrarna är värden som valdes tidigt i projektet och det vore intressant att undersöka hur mycket valet av dessa värden påverkar systemet. Styckvis affin approximation Vi använder idag en linjär interpolation för att få upp antalet parametrar från 15 till 1001. Det kan tänkas vara intressant att undersöka hur andra typer av interpolation påverkar systemet, exempelvis splinefunktioner. Denna förändring görs enkelt i main.m-filen däremot är det svårare att utvärdera hur mycket bättre/sämre resultatet blir. Tanken att använda en linjär interpolation bygger på att vi tror att det är fördelaktigt att snabbt variera mellan gas och broms jämfört med att göra mjuka övergångar i gaspådrag. Genom att använda den linjära interpolationen fås snabbare variationer i hastighetsprofilen. Antal iterationer av huvudloopen och kombinationsmatrisen Vi kör idag huvudloopen (gp() till fmincon) fyra gånger och kombinationsmatrisen är satt på 500000 kombinationer per varv. Detta kan trimmas och även detta en avvägning mellan beräkningshastighet och noggranhet. GPGPU-implementation Det som är tekniskt mest avancerat men som troligen hade gett bäst resultat hade varit att göra en GPGPU (General Purpose Graphics Processing Unit) implementation av gridsökningen i GPGO. Istället för att köra en kärna på CPU:n skulle kombinationsmatrisen kunna förstoras men styckas upp och köra på olika kärnor på grafikkortet. Uträkningarna beror idag inte på varandra och är högst parallelliserbar. Noggrannheten i hastighetsprofilen som skickas till systemet skulle troligen då bli markant mycket bättre. Dock kan det innebära problem att översätta GPML-biblioteket tillsammans med övriga matematiska delar till C. Vill man inte gå hela vägen kan man utveckla den nuvarande Matlab-implementationen genom att köra parfor-loopar istället för for-loopar genom lite omstrukturering i koden för att använda datorns alla CPU-kärnor. 10.7 Avslutande ord Vi har fått GPGO att fungera i praktiken och byggt en grund som är lätt att bygga vidare på. Det är av stort intresse att lägga in straff på avvikelsen från referenstrajektorian för framtida projektgrupper. Det finns stora utvecklingmöjligheter, specificerade ovan, och vi tror att GPGO lämpar sig väldigt väl för en adaptiv hastighetsprofil i projektet Racetrack.

LiU Racetrack 44 10.8 Gränssnitt I Tabell 12 sammanfattas de in- och utdata som GPGO-implementationen använder sig av. Insignaler Varvtid Utsignaler Nästa hastighetsprofil Beskrivning Tiden det tar för bilen att köra ett varv runt bilbanan med senast använd hastighetsprofil Beskrivning Parametrarna för den hastighetsprofil som ska användas nästa varv. Tabell 12: Tabell över gränsnittet för GPGO-implementationen

LiU Racetrack 45 11 Delsystem: Reglering Reglersystemets uppgift är att få bilen att så väl som möjligt följa den givna referenstrajektorian. Det består av två delsystem som styr bilens gaspådrag samt styrvinkel. Systemen består i huvudsak av ett antal PID-regulatorer och en framkopplingslänk. I detta projekt har inte regleringen utvecklats och beskrivs därför inte utförligt. För närmare beskrivning av regleringen, se [3]. 11.1 Gränssnitt I Tabell 13 ges en översikt över in- och utsignaler inom reglersystemet för styrservot. Insignaler θ avg e Beskrivning, re avg Medelvärdesbildad avvikelse i avstånd från trajektorian samt medelvärdesbildad girvinkelavvikelse från trajektorian. 9θ Utsignaler u s Girvinkelhastighet. Beskrivning Styrsignal (till styrservot). Tabell 13: Sammanfattning av in- och utsignaler inom reglersystemet för styrservot. I Tabell 14 ges en översikt över in- och utsignaler inom reglersystemet för hastighetsregleringen. Insignaler Beskrivning r e Avvikelse i avstånd från trajektorian 9θ Girvinkelhastighet. X, Y Position i x- och y-led. Utsignaler Beskrivning Styrsignal (gaspådrag). u s Tabell 14: Sammanfattning av in- och utsignaler inom reglersystemet för hastighetsregleringen. 12 Delsystem: Manuell styrning av lastbil Detta avsnitt beskriver hur systemet har utökats för att möjliggöra manuell styrning av lastbilen från någon av laborationssalens datorer. Målet med detta var att ge goda förutsättningar för att framöver kunna utveckla ett reglersystem som styr lastbilen från en dator. Detta har byggt mycket på den nya hårdvaran i systemet beskriven i Avsnitt 3. Där Raspberry Pi och de digital till analog konverterarna är ena delen och kommunikationen över det lokala nätverket till en dator den andra.

LiU Racetrack 46 12.1 Raspberry Pi och D/A konverterare En Raspberry Pi tillsammans med digital till analog konverterare har köpts in och prestanda testats som beskrivet i Avsnitt 3. Raspberry Pi n kör operativsystemet Raspbian - Debian Wheezy med default användaren och lösenordet, pi respektive raspberry. Raspberry Pi n och digital till analog korten kommunicerar över I2C bussen med 400 khz baud rate. I filen cmdline.txt har Raspberry Pi n tilldelats en statisk IP-adress. Raspberry Pi n har sedan satts upp som en TCP-server med ett protokoll för att ta emot styrsignaler från en TCP-klient enligt Avsnitt 5. Detta program startas i bakgrunden vid start av raspberry n definierat i etc/rc.local. På raspberry n i Home/truck manual steering finns de filer och program som har byggts för kommunikation och utställande av korrekt analog signal. Genom att stå i den mappen och skriva sudo python truck manual control.py kommer den server starta som körs vid upstart. 12.2 Skickande av styrsignaler från datorn På rt-pc-03 i C://local Homes/Racetrack Fast 2014/ finns mappen truck manual controll denna innehåller ett visual studio-projekt som kan kommunicera med raspberryn med rätt protokoll. Handkontrollen som styr lastbilen har ett spänningsområde för styrsignaler mellan 0-3.2 V. Det är inom detta intervall programmet på rt-pc-03 ställer ut signaler. Detta innebär olika värdes intervall för de olika digital till analog-korten beskrivet i Avsnitt 5.3. Så som detta program har skrivits finns en tråd för styrning höger/vänster och en tråd för gaspådrag. När piltangenterna trycks ned så räknas utsignalen upp eller ned inom sitt givna intervall och när piltangenten släpps skickas neutral läge vilket innebär styrning rakt fram och inget gaspådrag. 13 Delsystem: Visualisering och GUI Visualiseringssystemet har två huvuduppgifter. Dels sköter systemet projektionen av information genom projektorn, dels hanterar systemet användargränsnittet (GUI:t) som syns på datorn. Exempel på information som projektorn visualiserar är varvtider och trajektoria för bilarna. En översikt av visualiseringssystemet ges av Figur 31. Det är en schematisk bild över delsystemets uppgifter och vad för information systemet kräver samt ger ifrån sig. 13.1 Utseende och funktionalitet för GUI I dagsläget sker all kommunikation mellan användare och hela systemet genom ett grafiskt användargränssnitt på datorn, ett GUI. Detta GUI består av två olika fönster som syns Figur 32 och Figur 33. Huvudfönstret för GUI:t ses i Figur 32. Detta är en kontrollpanel varifrån användaren kan styra systemet. Den nya funktionaliteten i visualiseringssystemet kommer huvudsakligen att synas i detta fönster med nya inmatningsparametrar och en

LiU Racetrack 47 Figur 31: Översikt av delsystemet för visualisering och GUI. I bilden syns insignaler, utsignaler och funktionalitet i systemet. helt ny del som kommer att tillåta användaren att hantera hinder. Denna kontrollpanel definieras och skapas i filen ControlPanel.ccp. I den filen finns färdiga funktioner för att utöka kontrollpanelen med nya knappar, inmatningsfält och text. Tabell 15 är en sammanfattning över alla parametrar som ska kunna matas in genom kontrollpanelen (exklusive de befintliga PID-parametrarna). Förutom kontrollpanelen består GUI:t även av fönstret som syns i Figur 33. Där ritas en karta över bilbanan och bilarnas skattade positioner i realtid ut. Användaren kan också välja att visa optimal trajektoria, konfidensintervall för bilarnas positioner och bilarnas trajektoria (både tidigare och aktuellt varv). Detta fönster och dess funktionalitet skapas i MainDisplay.ccp och med sin klassdefinition i MainDisplay. Detta är en nedärvd klass från basklassen Display som i sin tur bygger på det grafiska biblioteket OpenCV. Ny funktionalitet som ska införas i detta fönster är att hinder, både statiska och dynamiska, ska synas på skärmen.

LiU Racetrack 48 Utöver detta ska de virtuella sensorerna med skalenlig form och räckvidd ritas ut. För detta finns det funktioner i det befintliga biblioteket, circle och rectangle är exempel på sådana, vilket låter oss rita ut geometriska figurer. För att rita ut en bild från en fil (om vi exempelvis vill låta ett hinder representeras av en älg) erbjuds OpenCV imread och imshow vilka laddar in en bildfil respektive visar den. Till sist ska även GUI:t rita ut ny, beräknad trajektoria när RRT:n försöker få bilen att undvika hinder. Då det redan finns funktionalitet för att rita ut trajektoria i form av funktionen Display::DrawLapOnline är detta inget nytt som behöver programmeras från grunden utan det enda som behövs är att Draw-tråden får tillgång till RRT-trajektorian. 13.2 Projektion på bilbanan Förutom att rita upp ett GUI på datorn sköter visualiseringssystemet också projektorn som projicerar ut diverse efterfrågad information på den fysiska bilbanan. Den matematiska bakgrunden till hur projektionen fungerar finns beskriven i den tekniska dokumentationen från föregående år [3]. I enlighet med denna teori finns allt implementerat i ProjectorDisplay.ccp med sin klassdefinition ProjectorDisplay.h. Även denna klass är nedärvd från basklassen Display. Liksom för den tidigare GUI-delen i Figur 33 vill vi även här rita ut hinder, virtuella sensorer och RRT-trajektoria. Eftersom ProjectorDisplay delar basklass med MainDisplay implementeras dessa funktioner på likvärdiga sätt. Ny inparameter sensor_range sensor_type trigger_overtaking race_mode truck_angles Beskrivning Räckvidd för virtuella sensorerna. Grafisk form för de virtuella sensorerna (cirkelskiva, kvadrat och så vidare). Flagga för att trigga omkörning. Byter mellan race-mode och vanlig körning. Inställning för hur vinklarna till lastbilen ska skattas. Tabell 15: Tabell över de nya inparametrarna som kommer att föras vidare från kontrollpanelen till systemet. Dessutom en beskrivning vilken funktionalitet dessa parametrar styr över.

LiU Racetrack 49 Figur 32: Skärmdump av kontrollpanelen som den ser ut i dagsläget. Här kommer nya inmatningsmöjligheter att införas. Figur 33: Skärmdump av kartan över bilbanan som ritas upp i realtid på datorskärmen. Förutom ett möjligt ansiktslyft ska även eventuella hinder och RRT-trajektoria synas här.

LiU Racetrack 50 14 Delsystem: Simulator Simulatorns uppgift är att simulera det fysiska systemet och därmed underlätta för testning av uppställda krav och vidareutveckling av systemet. Till simulatorn hör en fordonsmodell och en mätuppdatering som ersätter det fysiska systemets målföljning. Fordonsmodellen ersätter den fysiska bilen och simuleringen ersätter målföljningen i det fysiska systemet. I 2014 års projekt har koden som tidigare nämnt delats upp i moduler, detta innefattar även simulatorkoden. I övrigt har simulatorn inte utvecklats vidare och därför beskrivs bara det allra mest grundläggande kring simulatorn i detta dokument. För utförligare information om simlatorn hänvisas läsaren till tidigare års tekniska dokumentationer. 14.1 Gränssnitt I Tabell 16 ges en översikt över in- och utsignaler inom simuleringssystemet. Insignaler Beskrivning 9v x, 9v y Acceleration i x- och y-led. :θ Girvinkelaccelerationen X ref, Y ref, θ ref Referensposition i x- och y-led samt girvinkelreferens. Utsignaler Beskrivning v x, v y Hastighet i x- och y-led. r e, θ e Avvikelse i avstånd från trajektorian samt girvinkelavvikelse från trajektorian. X, Y Position i x- och y-led. θ, θ 9 Girvinkel och girvinkelhastighet. Tabell 16: Sammanfattning av in- och utsignaler i simulatorn. 14.2 Simuleringssystem Simulatorn har i dagsläget en del brister; den klarar exempelvis inte av att simulera ett helt varv runt banan. Simulatorn har två lägen, autonom eller manuell simulering och dessa två lägen fungerar på samma sätt som för det fysiska systemet, vilket finns beskrivet tidigare i detta dokument. En systemöversikt av det nuvarande simuleringssystemet kan ses i Figur 34. Ett mer detaljerat flödesschema kan ses i Figur 35. 14.2.1 Fordonsmodeller I dagsläget finns två olika fordonsmodeller (implementerade i tidigare års projekt). Den ena modellen är från 2011 [1] och den andra från 2012 [2]. En skillnad mellan de två modellerna är att den senare tar hänsyn till fler fysikaliska egenskaper, som exempelvis slip. En annan stor skillnad är att i modellen från 2011 beräknas hastigheterna i longitudinelloch lateralled, v x och v y samt vinkelhastigheten 9 θ direkt. I modellen från 2012 beräknas istället accelerationerna 9v x, 9v y samt vinkelaccelerationen : θ och för att beräkna v x,v y och 9θ behöver en integration utföras.

LiU Racetrack 51 Figur 34: Illustration av hur simuleringsfunktionen fungerar idag. Tillstånden uppdateras efter hur regulatorn avgör hur den simulerade bilen lämpligast följer den optimala trajektorian. 14.2.2 Simulator Simulatorn kan ses som en fiktiv mätuppdatering som beräknar v x, v y, X, Y, θ och 9 θ från en fordonsmodell. θ och 9 θ beräknas genom integration och dubbelintegration av : θ och v x, v y, X och Y beräknas enligt (13) - (16). Utan denna så kallade fiktiva mätuppdatering skulle varken bilens position eller hastighet vara känd. Xptq Xpt T q Y ptq Y pt T q T v x pt T q T v y pt T q T 2 2 9v xpt T q (13) T 2 2 9v ypt T q (14) v x ptq v x pt T q T 9v x pt T q (15) v y ptq v y pt T q T 9v y pt T q (16) Notera att då fordonsmodellen från 2012 används behöver en integration av 9v x och 9v y göras innan (13) - (16) kan användas.