Designspecifikation LiU Racetrack

Transkript

1 Designspecifikation LiU Racetrack Version 1.0 Författare: Salko Bjelevac Datum: 8 oktober 2014 Status Granskad Projektgruppen Godkänd Isak Nielsen

2 Projektidentitet E-post: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Isak Nielsen, Linköpings universitet Telefon: E-post: Daniel Axehill, Linköpings universitet Telefon: E-post: Daniel Axehill, Linköpings universitet Telefon: E-post: Kristin Bergstrand Telefon: E-post: Kristoffer Lundahl, Linköpings universitet Telefon: E-post: Gruppmedlemmar Namn Ansvarsroller Telefon E-post Kristin Bergstrand (KB) Projektledare (PL) kribe281 Måns Klingspor (MK) Informationsansvarig (IA) mankl061 Joachim Larsson (JL) Mjukvaruansvarig (MA) joala815 John Wilhelms (JW) Testansvaring (TA) johwi572 Erik Hedberg (EH) Designansvarig (DA) erihe251 Salko Bjelevac (SB) Dokumentansvarig (DOC) salbj522 Marcus Trulsson (MT) Hårdvaruansvarig (HA) martr316 Oskar Ljungqvist (OL) Leveransansvarig (LA) osklj052

3 Dokumenthistorik Version Datum Förändringar Sign Granskad Första utkastet KB Projektgrupp Andra utkastet EH Projektgrupp Tredje utkastet EH Projektgrupp Första godkända versionen SB Projektgrupp

4 Innehåll 1 Inledning 1 2 Systemöversikt 1 3 Hårdvara 2 4 Mjukvara Mjukvarumiljö Struktur Mål Delsystem: Målföljning Gränssnitt Delsystem: Hinderhanteringssystem Implementering Gränssnitt Delsystem: Beslutssystem Racingläget Säkerhetsläget Implementation Implementation i racingläge Implementation i säkerhetsläge Gränssnitt Delsystem: Referensgenerering Lokala trajektorier Teori Samplingstrategier Olika samplingar Sampling på raksträcka Sampling i kurva Körlägen Säkerhetsläge Racingläge Hantering av dynamiska hinder Gränssnitt Adaptiv anpassning av hastighetsprofilen Optimeringsproblemet Val av algoritm Parametrisering Algoritmen i korthet Implementering Gränssnitt Delsystem: Reglering 20

5 10.1 Översikt Gränssnitt Reglering av styrservo Avstånds- och vinkelregulator Framkopplingslänk Reglersystemet efter projektavslut Delsystem: Lastbil Manuell körning från dator Estimering av vinklar Delsystem: Visualisering och GUI Utseende och funktionalitet för GUI Projektion på bilbanan Delsystem: Simulator Gränssnitt Befintligt simuleringssystem Fordonsmodeller Simulator Simuleringssystemet efter projektavslut

6 LiU Racetrack 1 1 Inledning Detta dokument beskriver den design efter vilken systemet Fast Adaptive Steering Technology, FAST, är tänkt att implementeras. Systemet i fråga 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 som det kan delas upp i beskrivs utförligt i detta dokument. Då systemet är en utveckling från tidigare års projekt beskrivs framförallt de delar av systemet som tillkommer i och med projektet FAST. Det befintliga systemet beskrivs i den mån det är nödvändigt för att sätta utökningarna i sitt sammanhang och kunna ge en fullständig beskrivning utav dessa. För mer ingående beskrivning av det befintliga systemet hänvisas till den tekniska dokumentationen från No Oscillation Corporation, projektet ifrån år 2013 [3]. I dokumentet används flödesscheman 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 allra 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 för att sedan mäta och visualisera resultatet. Det som styrs är radiostyrda bilar, i dagsläget en åt gången, på en bana tillverkad i materialet 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. 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, något som finns illustrerat i Figur 2. Tre utav 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.

7 LiU Racetrack 2 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 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 finns att tillgå i projektet i dagsläget är följande: Bilbana Fyra radiostyrda bilar, Kyosho dnano [9] Radiostyrd lastbil innefattande dragbil, dolly och släp Handkontroller Två IR-kameror Projektor PC: rt-pc-11

8 LiU Racetrack 3 PC: rt-pc-03 Styrkort NI PCI-6024E Kopplingsplatta, BNC-2110, med tillhörande kablar Under projektets gång kommer ny hårdvara att köpas in för att kunna ställa ut styrsignaler till lastbilen. Möjlighet att ställa ut minst 2 analoga utgångar i intervallet mellan 1.6 V från datorn krävs för att kunna tillgodose det behovet, och för framtida kompabilitet efterfrågas fler analoga utgångar. Det kort som i dagslägget ligger som förslag är NI PCI [6], detta kan komma att ändras framöver. I Figur 3 visas en schematisk bild av hur informationenflödet kommer att se ut med den tillagda hårdvaran. Den nya hårdvaran som kommer läggas till systemet är enligt nedanstående lista. Styrkort NI PCI-6723 Kopplingsplatta, BNC-2110, med tillhörande kablar Hinder, 10 x 10 cm block med IR-markörer Figur 3: Schematisk bild av hur systemet med tillagd hårdvara kommer att kommunicera. Det nya styrkortet kommer att möjliggöra styrning av lastbilen via datorn, samt tillgodose ett ökande behov av styrsignaler i framtiden. 4 Mjukvara Detta avsnitt beskriver hur mjukvaran kommer att struktureras. Eftersom implementeringen av systemet till största del kommer att handla om dess mjukvara blir detta också ett perspektiv att se hela systemet ur. 4.1 Mjukvarumiljö Programmet för det övergripande systemet kommer att skrivas i programmeringsspråket C++. Utvecklingsmiljön är en PC med Windows 7.0 och koden kompileras genom Microsoft Visual C

9 LiU Racetrack 4 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 Programmeringen sker genom en objektorienterad struktur. Då delsystemen behöver kunna arbeta parallellt med varandra delas mjukvaran upp i så kallade trådar. Mjukvaran är i dagsläget uppbyggd i fyra 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. En ny tråd kommer att införas för hinderhanteringen, Obstacle thread. 4.3 Mål Det är svårt att förutsäga exakt hur den slutgiltiga mjukvaran kommer att vara strukturerad, men utvecklingen sker utefter dessa punkter: Mjukvaran ska vara skriven i C++ med objekorienterad struktur. Delsystemen ska i källkoden vara modulära och logiskt uppdelade genom trådning. Koden ska vara enkel för någon utomstående, med tillräcklig kunskap inom programmeringsspråket, att sätta sig in i. Detta innebär logiska och förklarande variabelnamn samt välkommenterad och strukturerad kod. Programmet ska vara användarvänligt med ett grafiskt och tydligt användargränssnitt. Visualisering och inmatning ska vara lättillgängligt och ska inte kräva att användaren har någon direkt programmeringsvana. 5 Delsystem: Målföljning Målföljningssystemet har som uppgift att detektera fordon 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 4 ges en översikt av Målföljningssystemet. Systemet kommer att utökas med 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 kommer systemet använda en generell

10 LiU Racetrack 5 Figur 4: Denna figur illustrerar målföljningssystemets interaktion med sin omgivning. Som framgår är skillnaden mellan hur hinder och bilar hanteras liten. modell som lätt kan ändras och anpassa 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 dessa hinder finns sedan hårdkodade i filen CarMarkerCameraIdentParam.cpp. I denna fil kommer sedan de fysiska hindrena att finnas tillagda. Se Figur 5 hur geometrin för chassi 1 ser ut. Vi har ännu inte specificerat den exakta geometrin för det fysiska statiska hindret. Däremot är det bestämt att vi kommer att placera alla reflexmarkörer på samma höjd. Figur 5: Reflexmarkörernas geometri på chassi 1.. 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]. 5.1 Gränssnitt

11 LiU Racetrack 6 Insignaler Bilder Utsignaler Position px, Y q, hastighet v x, v y, vinkel θ Beskrivning Bilder från IR-kamerorna. Information om objekt skickas till de andra delsystemen Tabell 1: I denna tabell presenteras alla in- och utsignaler som målföljningsystemet använder sig av och genererar. 6 Delsystem: Hinderhanteringssystem Hinderhanteringssystemet 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. Notera att hinderhanteringssystemet inte sitter i bilen utan är en implementationsdetalj. 6.1 Implementering Ett hinder är antingen fysiskt eller virtuellt. De fysiska hindrena måste ha ett fördefinierat mönster på sina reflexmarkörer så att målföljningssystemet kan skicka information om att det finns fysiska hinder på bilbanan. De virtuella hindrena är bestämda av användaren och finns således direkt att tillgå i hinderhanteringssystemet. Hinderhanteringssystemet är tänkt att implementeras genom att definiera en basklass Obstacle som innehåller exempelvis position och utbredning. Ett hinder kan vara statiskt eller dynamiskt och kommer implementeras som subklasserna VirtualObstacle och PhysicalObstacle. Genom arv så ärver subklasserna alla egenskaper som definierats i basklassen. Varje instans av subklasserna kommer innehålla information om ett specifikt hinder och beslutssystemet tar del av denna information genom att sätta en pekare till aktuell instans. De virtuella hindrena kommer först att vara hårdkodade, exempelvis i en textfil som vi läser in. Därefter kommer vi att undersöka möjligheterna att lägga in hinder via GUI:t. Figur 7 beskriver hur beslutssystemet hanterar hinder. 6.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 2: I denna tabell presenteras alla in- och utsignaler som hinderhanteringssystemet använder sig av och genererar.

12 LiU Racetrack 7 7 Delsystem: Beslutssystem Detta avsnitt beskriver den beslutsfattande delen i bilen. Målet med beslutssystemet är att kunna reagera på olika typer av hinder; andra fordon som rör sig på banan samt övriga statiska och dynamiska hinder. För att implementera detta på ett enkelt sätt kommer systemet att ha två olika körsätt; racingläge och säkerhetsläge. Systemet kommer att reagera olika på hinder beroende på vilket av dessa lägen som är aktivt. Respektive läge förklaras i 7.1 och 7.2. Figur 6: Illustration av detektion av hinder. Bilen kör längst den fördefinierade optimala trajektorian. Ett hinder kommer in i bilens avståndssensorers cirkeltrajektoria och information om hindret läggs in i beslutssystemets lista över hinder i bilens synfält. 7.1 Racingläget I racingläge förutsätts alla hinder på banan vara andra fordon som tävlar med den autonoma bilen. Med detta antas att de rör sig i samma riktning som den autonoma bilen, och strävar efter att röra sig på den optimala referenstrajektorian. 7.2 Säkerhetsläget I säkerhetsläget beräknas hindret kunna röra sig längs en godtycklig trajektoria på banan som den autonoma bilen inte känner till på förhand. Här ses ingen poäng i att utföra en snabb omkörning utan appliceras för att säkert kunna ta sig förbi hinder av en mer oförutsägbar dynamik. Här ska bilen reagera genom att bromsa in vid upptäckt av ett hinder och sedan låta referensgenereringssystemet arbeta för att hitta en säker väg förbi det berörda hindret. 7.3 Implementation Utöver tidigare nämnda mål ska omkörningen kunna ske automatiskt eller genom att en omkörning triggas av användaren. En automatisk omkörning ska initieras så fort ett hinder dyker upp i bilens synfält. I manuellt läge ska den autonoma bilen lägga sig bakom hindret med samma hastighet som detta tills dess att den får klartecken för omkörning. Vilket läge som är inställt och triggning av omkörning ska vara val i GUI:t. När bilens hindersensorer ser att ett hinders position är inom sitt valbara område kommer

13 LiU Racetrack 8 information om detta hinder fås. Detta sker genom att vi sätter en pekare till listan över alla hinder på bilbanan i hinderhanteringssystemet. Vi ser att ett hinder är inom bilens hindersensorers valbara område genom att undersöka absoluta positioner. I kravspecifikationen använder vi termen bilens avståndssensorer. För att göra det mer intuitivt vad vi menar kommer vi fortsättningsvis använda termen bilens hindersensorer i designspecifikationen. För att göra scenariot mer likt verkligheten är tanken att vi inte ska ta del av om ett hinder är statiskt eller dynamiskt utan skatta informationen. Hur detta ska implementeras är inte bestämt i dagsläget. En illustrativ beskrivning för hur bilen planeras att hantera hinder kan ses i Figur 7. Figur 7: Förklaring av beslutshantering av hinder. Hinderhanteringssystemet är gulmarkerat och innehåller information om alla fysiska och virtuella hinder på bilbanan. I vit markering är den logik som utförs i beslutssystemet gällande hur den ska hantera information om hinder i bilens hindersensorers valbara område. Blå markering är den information som bilen skickar till andra delsystem. Som kan ses i Figur 7 finns information om alla hinder på bilbanan, fysiska som virtuella, i hinderhanteringssystemet. Beslutssystemet undersöker hela tiden om det finns något hinder inom bilens hindersensorers valbara område. Detta görs genom att jämföra positioner i hinderhanteringssystemet med bilens skattade position med hänsyn till dess valbara område. När ett hinder dyker upp så sätter vi, i listan över hinder i bilens synfält, en pekare till hindret. Olika processer startas sedan beroende på om bilen är i racing- eller i säkerhetsläge.

14 LiU Racetrack Implementation i racingläge I racingläget vet vi att det rör sig om ett annat fordon som rör sig med lägre hastighet än den autonoma bilen, men i samma riktning. Den enda ytterligare informationen som den autonoma bilen behöver är hindrets hastighet vilken kommer att skattas. När denna information erhållits undersökts huruvida bilen befinner sig i manuell eller i automatisk omkörning. Befinner sig bilen i automatiskt omkörningsläge skickas information om hindrets position och dynamik till referensgenereringssystemet som börjar räkna på möjliga omkörningsspår. Är omkörningsläget manuellt ska bilen anpassa sin hastighet efter hindrets och lägga sig i ett så kallat följeläge. Detta för att invänta signal för omkörning ifrån användaren. När denna signal ges kommer information om omkörningsobjeketet att skickas till referensgenereringssystemet Implementation i säkerhetsläge I säkerhetsläge skickas ett beslut att bromsa bilen så fort ett hinder upptäckts. Detta är för att inte försvåra en omkörning ifall hindrets hastighetsvektor är riktad mot bilens färdriktning. I nästa iteration undersöks om hindret är statiskt eller dynamiskt genom skattning. Denna information skickas till referensgenereringssystemet, som tar hänsyn till hastighet och riktning på hindret när ett omkörningsspår räknas ut. Detta läge är givetvis inte lika tidseffektivt som racingläget men kommer innebära ett säkrare sätt att passera hinder med en mer komplicerad dynamik. Befinner sig bilen i manuellt omkörningsläge inväntar bilen klarsignal från användaren innan ett omkörningsspår räknas ut. 7.4 Gränssnitt Insignaler Hinder Bilens tillstånd Val från GUI 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 Information från GUI:t vilket läge som är valt och om triggad omkörning är valt. 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 3: I denna tabell presenteras alla in- och utsignaler som beslutssystemet använder sig av och genererar.

15 LiU Racetrack 10 8 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 9 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 8 illusterar bilbanan och den förberäknade optimala trajektorian. Figur 8: Bilbanan avbildad med den förberäknade optimala trajektorian utritad i blått. Datan 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 Lokala trajektorier Beräknandet av de lokala trajektorierna är tänkt att implementeras med hjälp av en avsökningsmetodik 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 [7], [11] och [8]. I de två senare har RRT använts just för att beräkna trajektorier för bilar, och i framför allt [8] har det visats att det är möjligt att generera en lämplig trajektoria till bilen online 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

16 LiU Racetrack 11 Figur 9: Ö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. den tillbaka till den optimala trajektorian. Problematik som finns med konceptet RRT ä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 den bilmodell som simuleras i RRT:n kallas Pure Pursuit och beskrivs i [5] och bygger enbart på ren geometri. 8.2 Teori Den grundläggande teorin bakom konceptet RRT är omfattande och beskrivs utförligt i exempelvis [7]. I detta dokument kommer bara den konceptuella strukturen för algoritmen att tas upp. Med utgångspunkt i en kombination av [7], [11] och [8] 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, flera delmål och en karta över alla för bilen kända hinder och väggar på banan. Efter att vår RRT har initierats genereras så många samplingar som hinns med under en begränsad tidsrymd t. Hittas inte en trajektoria upprepas proceduren igen med uppdaterade tillstånd på bilen och hindret. Funktionen Sampel() generar en ny samplad position givet en samplingstrategi och en referensposition. När ett sampel har genererats kontrolleras det så att samplet hamnade inuti tillåtet området 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 undersöks vilka nära grannar som redan finns i trädstrukturen. Detta görs med funktionsanropet Near(). Om det saknas grannar kommer istället roten till trädet att väljas som förälder om det ä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, detta görs med funktionen ObstacleFree(). När den

17 LiU Racetrack 12 Figur 10: Figurerna visar en ska rmdump fra n Niclas Everstets 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. billigaste grannen har hittats va ljs den till fo ra lder, ihopkopplingen go rs med funktionen Connect(). I slutet av varje iteration kontrolleras sedan om vi har hittat en trajektoria som tar oss till va rt ma l eller delma l. Slutvillkoret pa funktionen Goalreached() avgo r ifall en nod ska bedo mas som framme eller ej. Detta avgo rs beroende pa om nodens tillsta nd a r tillra ckligt lika ma lets o nskade tillsta nd i ba de aspekten position och vinkel. 8.3 Samplingstrategier Det finns flera olika samplingsstrategier beroende pa var pa banan fordonet befinner sig, samt vad fo r slags mano ver bilen ska utfo ra. Det finns olika fo r- och nackdelar fo r varje strategi som man beho ver vara medveten om na r man va ljer hur samplingen ska utfo ras. Fordonet kommer att va lja samplingsstrategi beroende pa vad fo r behaviour som a r insignal vid generering av trajektoria. Behaviour a r en form av beskrivning o ver hur banan ser ut; om det a r en fra ga om rakstra cka, ho ger- eller va nsterkurva. Detta mo jliggo r att man kan va lja ba ttre samplingstrategier med avseende pa banutseende eller situation. Val av sampling och samplingsstrategier a r inte na gon exakt vetenskap utan detta a r ett omra de da r vi kommer beho va testa oss fram Olika samplingar De olika typerna av samplingar som kommer anva ndas a r listande nedan. 1. Gaussisk sampling med avseende pa vinkel mellan bilens och ma lets position och en standardavvikelse i ba de la ngdled och vinkel. Pa detta sa tt genereras en samplingsyta fra n bilen mot ma let som a r formad som en kon. Detta omra de a r realistisk med tankte pa bilens ro relsefo rma ga. 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 Designspecifikation.pdf

18 LiU Racetrack 13 Algorithm 1 Body to RRT 1: initiate RRT 2: while ActiveRRT do Uppdate vehicle states and X free. 3: while t ă t do 4: x rand Ð Sampelpposition,Sampel strategyq; 5: x rand,goal Ð SampelpGoal position,sampel strategyq; 6: if x rand P X free then 7: X near Ð NearpTree structure, x rand q; 8: for x near P X near do 9: if ObstacleFreepx near, x rand q then 10: c 1 Ð Costpx near q ` CostFromTopx near, x rand q; 11: if c 1 ă Costpx rand q then 12: x min Ð x near ; 13: end if 14: end if 15: end for 16: if Exist(x min ) then 17: Connect(Tree structure,x rand, x min ); 18: end if 19: end if 20: if x rand,goal P X free then 21: X near Ð NearpTree structure, x rand,goal q; 22: for x near P X near do 23: if ObstacleFreepx near, x rand,goal q then 24: c 1 Ð Costpx near q ` CostFromTopx near, x rand,goal q; 25: if c 1 ă Costpx rand,goal q then 26: x min Ð x near ; 27: end if 28: end if 29: end for 30: if Exist(x min ) then 31: Connect(Tree structure,x rand,goal, x min ); 32: end if 33: end if 34: if GoalReached(Tree structure,goal) then 35: BestNode Ð GetBestNode(Tree structure); 36: HigherBoundCost Ð GetCost(BestNode); 37: end if 38: end while 39: end while 2. Gaussisk sampling av en slumpad punkt i en rektangel med ett visst medelvärde och standardavvikelse i både x-led och y-led. 3. Gaussisk sampling bakom bilen som i punkt 1 och bilen ska med andra ord backa. Detta möjlig omkörning även om bilen har hamnat för nära ett hinder. Dessa samplingsstrategier kommer att kombineras och möjliggöra för bilen att genomföra relativt avancerade manövrar.

19 LiU Racetrack Sampling på raksträcka Vid framtagande av trajektoria på raksträckor är det fördelaktigt att sampla mycket längre framåt samt att begränsa samplingen åt sidorna. Detta eftersom bilen med stor sannolikhet kommer att köra framåt och inte röra sig avsevärt i sidled. Därför väljs samplingsområdet för en raksträcka i form av en liksidig trapetsoid, se Figur 11. Figur 11: Figuren visar hur sampligsområdet ser ut för RRT:n vid en raksträcka. Den blå rektangeln representerar ett fordon tillsammans med dess tillhörande samplingsområde i gult. Den röda rektangeln kan exempelvis vara ett omkörningsobjekt Sampling i kurva För sampling i kurva är det, till skillnad från sampling på raksträcka, ett bättre val att välja ett kortare samplingsområdet med utbredning åt det hållet kurvan svänger. Detta resulterar i ett mer kompakt och mindre samplingsområde eftersom även hastigheten är lägre vid kurvkörning. I detta fall tar samplingsområdet form av en rätvinklig triangel, se Figur 12. Samplingsområdet väljs på motsvarande sätt för en högersväng. Figur 12: Figuren visar hur sampligsområdet ser ut för RRT:n vid en vänstersväng. Den blå rektangeln representerar ett fordon tillsammans med dess tillhörande samplingsområde i gult. Den röda rektangeln kan exempelvis vara ett omkörningsobjekt.

20 LiU Racetrack 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. Exakt vad dessa körlägen innebär finns noggrannare definierat under punkterna 7.1 samt 7.2, som finns under huvudrubriken Delsystem: Beslutssystem 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 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. 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. För ett framförvarande omkörningsobjekt kommer fordonet ligga bakom det på ett säkert avstånd tills dess att en fullständig omkörningstrajektoria är beräknad. Ett flödesschema för omkörningen presenteras nedan i Figur 13. Figur 13: Flödesschema för en omkörning i säkerhetsläget 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 omkörningen delas upp i delmål för att på så sätt snabba upp omkörningen. När en lämplig trajektoria till första delmålet har hittats påbörjas omkörningen. Omkörningen påbörjas således innan hela sträckan runt hindret är beräknat. Det finns en triggad omkörningsfunktion som gör att användaren manuellt aktiverar omkörning av omkörningsobjektet. Det här innebär att fordonet på förhand inte vet om och när en omkörning initieras. Detta medför att en fullständig omkörningstrajektoria troligtvis ej hinner genereras och fordonet kommer då att behöva köra mot ett närmare mål, exempelvis ett delmål bredvid omkörningsobjektet. Därefter skapas en ny nod som befinner sig längre fram och på så sätt jobbar sig bilen fram framför omkörningsobjektet tills dess att bilen kan återvända till den optimala trajektorian. Ett flödesschema för omkörningen presenteras nedan i Figur 14.

21 LiU Racetrack 16 Figur 14: Flödesschema för en omkörning i racingläget. 8.5 Hantering av dynamiska hinder Det finns flera sätt att hantera dynamiska hinder. Ett sätt är att förläga hindret i dess färdriktning. Detta kommer skapa en konservativt förbjudet område. På detta sätt kan dock hindret hanteras som ett statiskt hinder, vilket förenklar implementeringen av dynamiska hinder i systemet. Ett annat alternativ är att skatta hindrets position framåt i tiden och istället göra kollisionskontroll med tiden. Den senare metoden är att föredra då en konservativ skattning av hindrets storlekt kommer leda till att det kommer bli betydligt svårare att köra om. 8.6 Gränssnitt Insignaler Information Hinderklassen Hindrets position, storlek och dynamik. Bilens tillstånd Position och hastighet. Mode Säkerhets- eller racingläge. Behaviours Raksträcka, vänster- eller högersväng. Aktiv/inaktiv Om RRT:n ska vara aktiv eller inte. Triggad omkörning Flagga för omkörning triggad av användaren. Utsignaler Genererad trajektoria Flagga för färdig genererad trajektoria. Trajektoria Den beräknade trajektorian som består av positioner med tillhörande girvinkel och girvinkelhastighet på bilen. Tabell 4: I denna tabell presenteras alla in- och utsignaler som RRT:n använder sig av och genererar.

22 LiU Racetrack 17 9 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 kring banan. 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. Under föregående års projekt löstes detta genom att först sätta hastigheterna till lågt värde och sedan successivt öka dem så länge det inte resulterade i för stor avvikelse från den optimala trajektorian. I förra årets projekt implementerades en adaptiv hastighetsprofil med mål att minimera varvtiden. Principiellt fungerar den på så sätt att då trajektorian följs bra ökar hastigheten och om avvikelsen till trajektorian är stor sänks den. 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 utav årets uppgifter är därför att utveckla en ny adaptiv algoritm med hjälp av väldokumenterade metoder från lärande system. 9.1 Optimeringsproblemet Den uppgift som systemet ska lösa genom användning av adaptiva metoder kan beskrivas som ett optimeringsproblem. Det finns ett antal sätt man kan tänka sig att definiera den funktion som ska optimeras. Det enklaste är att optimera tiden det tar att köra runt banan, men det kan också vara intressant studera en funktion som väger samman bantiden med avvikelsen från den optimala trajektorian. Vi planerar att använda den senare typen av funktion, eftersom det annars inte finns några garantier för att bilen kommer att följa den förberäknade optimala trajektorian. Eftersom det är hastighetsprofilen som ska väljas optimalt är det den som är argument, eller insignal om man så vill, till målfunktionen. Man skulle kunna motsätta sig vårt val av en målfunktion som väger samman varvtid och hur väl bilen följer den förberäknade trajektorian. Att bilen inte följer den förberäknade optimala trajektorian är i sig inget problem så länge varvtiden i snitt blir lägre, den trajektorian är ju bara optimal under förutsättningar som inte riktigt är uppfyllda. Men anledningen till att vi ändå vill försöka följa trajektorian nära är att vi misstänker att för stor avvikelse kan leda till problem då reglersystemet inte anpassats för körning med för stor avvikelse från den optimala trajektorian. 9.2 Val av algoritm 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 ämnar vi använda metoden Gaussian Processes for Global Optimization [10], som författarna till artikeln hävdar är väldigt bra 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 [12]. Vi kommer att implementera Gaussian Processes for Global Optimization (GPGO) med One-step Lookahead [10], då det tycks fungera tillräckligt bra och vi vill undvika den ökade komplexiteten med Multi-step Lookahead. Begreppet Lookahead handlar om hur många funktionsutvärderingar in i framtiden som metoden ska försöka optimera. Algoritmen beskrivs ytterligare i 9.4

23 LiU Racetrack Parametrisering 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 kommer därför att bli nödvändigt att på något sätt parametrisera hastighetsprofilen, som i dagsläget har 1001 punkter, så att dess utseende helt bestäms av 5 till 20 parametrar. Figur 15 visar hur hastighetsprofilen för den förberäknade optimala trajektorian ser ut. Figur 15: Hastighetsprofil över hastigheten i trajektorians riktning för den förberäknade optimala trajektorian. Hastighetsprofiler av den här typen behöver parametriseras så att de kan representeras med 5-20 parametrar. Det finns många tänkbara parametriseringar, vi avser dock att börja med en enkel splinekurva där kontrollpunkterna placeras på banans raksträckor. Att välja en bra parametrisering kommer kräva ytterligare resonerande kombinerat med ingenjörsmässigt experimenterande, men Figur 15 tyder på att någon form av spline-kurva skulle passa. 9.4 Algoritmen i korthet GPGO 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: där: ppy xq fi N py; µpxq, Kpx, xqq fi 2πKpxq 1 2 e 0.5py µpxqqj Kpxq 1 py µpxqq K Kpx, x 1 q är den kovariansfunktion som ska beskriva kovariansen hos varvtiden mellan två olika hastighetsprofiler. µpxq är väntevärdet av varvtiden vid en viss hastighetsprofil. Eftersom vi inte känner µ eller K används våra ingenjörskunskaper 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 θ. En av utmaningarna här är hur kovariansfunktionen Kpx, x 1 q ska väljas. Det finns en uppsjö av olika kovariansfunktioner föreslagna i [12] och vi ämnar att analysera problemet ytterligare innan vi väljer en eller ett par att testa. Vi ämnar dock välja en relativt enkel kovariansfunktion, som exempelvis squared exponential :

24 LiU Racetrack 19 Kpx, x 1 q expp 1 2 x x1 q I ovanstående exempel är faktorn 1 2 något som typiskt skulle ingå i våra hyperparametrar θ. Genom att använda notation från [10] 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 Där 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, så kommer vi med hjälp av kovariansfunktionen K att kunna använde 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 inför vi en förlustfunktion, λpx q, på följande sätt: " y : y λpx q ă minpy 0 q minpy 0 q : y ą minpy 0 q One-step Lookahead handlar om att välja nästa funktionsutvärdering y px q så att den förväntade förlusten, Erλs minimeras. Eftersom vi vet vad y 0 är och har uttryck för fördelningen hos y så kan vi ta fram uttryck för fördelningen hos λ. Att ta fram det uttrycket ä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 [10]. 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. Vi har i dagsläget ännu inte närmre undersökt metoder för numerisk marginalisering, men vårt intryck från [10] är att det inte ska utgöra några större svårigheter. 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. Eftersom vi inte i varje iteration försöker uppnå en så låg varvtid som möjligt så 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).

25 LiU Racetrack Implementering Inledningsvis kommer en version av GPGO att implementeras i Matlab som ett första steg för att få en bättre uppfattning om metoden. Eventuellt måste vi använda ett externt bibliotek för att kunna utföra BMC. När detta program fungerar tillfredställande kommer vi bygga in denna funktionalitet i det riktiga systemet. Eventuellt kommer vi att undersöka om det är möjligt att anropa Matlab versionen från C++ koden istället för att integrera funktionaliteten. 9.6 Gränssnitt I Tabell 5 sammanfattas de in- och utdata som GPGO-systemet använder sig av. Insignaler Senast använd hastighetsprofil Senaste varvtid Avvikelse under senaste varvet Utsignaler Nästa hastighetsprofil Beskrivning Parametrarna som användes för att generera hastighetsprofilen som användes under det senaste varvet. Den tid det tog att köra senaste varvet Något skalärt mått på avvikelsen från den optimala trajektorian under senaste varvet. Beskrivning Parametrarna för den hastighetsprofil som ska användas nästa varv. Tabell 5: Systemet för att adaptivt anpassa hastighetsprofilen har inga stora behov av data från övriga delsystem, det räcker med att utbyta några få skalära värden en gång per varv. 10 Delsystem: Reglering Detta avsnitt beskriver hur systemet får bilen att följa den referenstrajektoria som levereras av referensgenereringssystemet. Reglersystemets uppgift är att få bilen att så väl som möjligt följa den givna referenstrajektorian. I dagsläget finns ett reglersystem bestående av ett antal PID-regulatorer samt en så kallad hastighetsprofil som förbättras adaptivt under körning. Tanken är att behålla det system som finns, men att använda väldokumenterade metoder från maskininlärning till att förbättra systemets förmåga att anpassa sina parametrar. I första hand önskas hastighetsprofilen förbättras, beroende på hur väl bilen klarar av att följa banan. I Figur 16 ges en översiktlig bild av reglersystemet Översikt Reglersystemet består idag av två delsystem som styr styrvinkeln på framhjulen respektive bilens gaspådrag. Reglersystemet består endast av PID-regulatorer och är implementerad i funktionen PIDcontroller. Hantering av integratoruppvridning och stötfri övergång vid parameterbyten är implementerad. Bruskänslighet hanteras genom att medelvärdesbilda styrsignalerna. För mer information om regleringen se [3].

26 LiU Racetrack 21 Figur 16: Reglersystemet levererar styrsignaler i form av styrvinkel och gaspådrag. Detta sker genom två separata delsystem, både baserade på PID-regulatorer, men också referensen för hastigheten vilken anpassas adaptivt Gränssnitt I Tabell 6 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 6: Sammanfattning av in- och utsignaler inom reglersystemet för styrservot. I Tabell 7 ges en översikt över in- och utsignaler inom reglersystemet för hastighetsregleringen.

27 LiU Racetrack 22 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 7: Sammanfattning av in- och utsignaler inom reglersystemet för hastighetsregleringen Reglering av styrservo Styrservot reglerar bilens styrvinkel och ser till att bilen håller sig på den givna trajektorian. Styrservot består av tre delar som bidrar med varsin del till den slutgiltiga styrsignalen u s enligt u s u 1 ` u 2 ` u ff Avstånds- och vinkelregulator Avståndsregulatorn och vinkelregulatorn har tillsammans med framkopplingslänken (se nedan) som uppgift att se till att bilen följer den givna trajektorian. Avståndsregulatorn och vinkelregulatorn består av en PID-regulator vardera och båda reglerar på medelvärdesbildade signaler för att undvika för stor inverkan av brus. Ekvation (1) nedan beskriver typen av PID-regulatorer som används. uptq K eptq ` 1 T i ż t 0 deptq eptqdt ` T d dt (1) Avståndsregulatorn reglerar på bilens medelvärdesbildade avvikelse i avstånd från trajektorian re avg, se (2). Utsignal från avståndsregulatorn är u 1. Två olika moder används beroende på om bilen är i en kurva eller på en raksträcka; i kurvor används snabb reglering för bra följning och på raksträckor används långsam reglering för att undvika oscillationer. r avg e r e ` re prev 2 (2) Vinkelregulatorn reglerar på bilens medelvärdesbildade vinkelavvikelse från trajektorian θe avg, se (3). Utsignal från regulatorn är u 2. Även här används två olika moder beroende på om bilen är i en kurva eller på en raksträcka; Då bilen är i en kurva används inte vinkelregulatorn. θ avg e θ e ` θe prev 2 (3) Framkopplingslänk Till reglersystemet hör en framkopplingslänk som kopplar fram girvinkelhastigheten 9 θ, som mäter hur snabbt referenstrajektorian svänger. Framkopplingslänken bidrar till att vinkelregulatorns prestanda förbättras.

28 LiU Racetrack Reglersystemet efter projektavslut Inga stora ändringar ska göras i reglersystemet men med största sannolikhet kommer regulatorparametrarna behöva trimmas. 11 Delsystem: Lastbil Detta avsnitt beskriver hur systemet ska utökas för att integrera lastbilen. I Figur 17 syns en schematisk bild av detta delsystem. Delsystemets uppgifter har delats upp i manuell körning från dator och estimering av vinklar, se punkterna 11.1 och 11.2 nedan för detaljer. Figur 17: Översikt av delsystemet lastbil, dess insignaler, uppgifter och utsignaler. Uppgifterna är indelade i två undergrupper vilka beskrivs under kommande rubriker Manuell körning från dator För att kunna köra lastbilen manuellt från datorn krävs att detta delsystem beräknar två utspänningar utifrån knapptryck utav användaren och ställer ut dessa genom ett styrkort från datorn till lastbilens styrdon. Utspänningen ska vara två analoga signaler mellan 1.6V som ställs ut av ett styrkort från datorn. Denna utspänning är kopplade till gas, broms respektive höger och vänster styrning på den handkontroll som kontrollerar lastbi-

29 LiU Racetrack 24 len. För att skapa denna funktionalitet kommer en ny klass att skapas. Denna klass kallad manual truck, ska anropas genom knapptryckningar i GUI:t och utifrån dessa bedömma vilken utsignalspänning som ska ställas ut. För att manual truck ska ha möjlighet att ställa ut önskade utsignaler måste den nya hårdvaran implementeras för utställande av analoga signaler. Utöver det måste filen analogout.cpp utökas för att hantera den nya hårdvaran och utställandet av önskade spänningsnivåer. Detta kommer att göras med hjälp av API:t för den tillagda hårdvaran, NI DAQmx Estimering av vinklar I Figur 18 är en shematisk bild av de vinklar som kommer att estimeras för att lastbilen ska bli helt integrerat i det befintliga systemet. Framförallt krävs att vinklarna mellan lastbilen och dollyn, β 2, och dollyn och släpet, β 3, kan beräkans (dessa är refererade till som α respektive β i [4]). I Figur 18 är L 1 lastbilen, L 2 är dollyn och L 3 är släpet. Punkterna px 1, Y 1 q och px 2, Y 2 q är infästningspunkterna mellan dolly, lastbil och släp. Dessa punkter kommer att markeras med en markör för IR-kamerorna. Även dollyn kommer att markeras med markörer för IR-kamerorna med hjälp av en tvärslående pinne för att placerar markörerna i kamerornas synfält. Estimering av vinklarna β 2 och β 3 kommer att kunna göras med och utan information från dollymarkörerna. Detta val kommer att utföras i GUI:t. I båda fallen kommer beräkning av vinklarna att göras utifrån geometriska samband. Med information från dollymarkörerna om axlarnas postioner är detta möjligt. Genom trigonometri skattas vinklarna θ 1,β 2 och β 3, definerade enligt Figur 18, genom att beskriva lämpliga punkter på två sätt och ekvation (4) - (9) sammanfattar skattningsmetodiken. Då tan() är definierat i den första kvadranten kommer atan2() att användas i implementering som är en generalisering i alla kvadranter och har alltså en värdemängd mellan 0 och 2π. X3 X2 X1 Y 1 j G j X0 ` L 1 cos θ 1 Y 0 ` L 1 cos θ 1 ˆ Y1 Y 0 θ 1 arctan X 1 X 0 j j X1 ` L 2 cos pβ 2 ` θ 1 q Y 2 Y G 1 ` L 2 cos pβ 2 ` θ 1 q G ˆ Y2 Y 1 β 2 θ 1 ` arctan X 2 X 1 j j X2 ` L 3 cos pβ 2 ` θ 1 ` β 3 q Y 3 Y G 2 ` L 3 cos pβ 2 ` θ 1 ` β 3 q G ˆ Y3 Y 2 β 3 pθ 1 ` β 2 q ` arctan X 3 X 2 G (4) (5) (6) (7) (8) (9)

30 LiU Racetrack 25 Figur 18: Schematisk bild av lastbilsekipaget, innefattande lastbil (L 1 ), dolly (L 2 ) och släp (L 3 ). Vinklarna som ska estimeras β 2, β 3 (refererade till som α respektive β i [4]) och det globala referenssystemet X och Y. 12 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. Samtliga funktioner finns tillgängliga i det befintliga visualiseringssystemet. Därmed kommer systemet att utökas snarare än att byggas om från grunden för att införa ny funktionalitet. Denna funktionalitet kommer att kräva en del nya in- och utsignaler för systemet. I praktiken innebär detta att Draw-tråden (den tråd som i dagsläget sköter visualiseringssystemet) kommer att behöva kunna kommunicera med ytterligare trådar. En översikt av visualiseringssystemet ges av Figur 19. Det är en schematisk bild över delsystemets uppgifter och vad för information systemet kräver samt ger ifrån sig Utseende och funktionalitet för GUI I dagsläget sker all kommunikation mellan användare och hela systemet genom ett användargränssnitt på datorn, ett GUI. Detta GUI består av två olika fönster som syns Figur 20 och Figur 21. Huvudfönstret för GUI:t ses i Figur 20. 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 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 8 är en sammanfattning över alla parametrar som ska kunna matas in genom kontrollpanelen (exklusive de befintliga PID-

31 LiU Racetrack 26 Figur 19: Översikt av delsystemet för visualisering och GUI. I bilden syns insignaler, utsignaler och funktionalitet i systemet. parametrarna). Förutom kontrollpanelen består GUI:t även av fönstret som syns i Figur 21. 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.h. Detta är en nedärvd klass från basklassen display.h 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. 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

32 LiU Racetrack 27 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 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.h. Liksom för den tidigare GUI-delen i Figur 21 vill vi även här rita ut hinder, virtuella sensorer och RRT-trajektoria. Eftersom ProjectorDisplay.h delar basklass med MainDisplay.h 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 8: 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.

33 LiU Racetrack 28 Figur 20: Skärmdump av kontrollpanelen som den ser ut i dagsläget. Här kommer nya inmatningsmöjligheter att införas. Figur 21: 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.

34 LiU Racetrack 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 Gränssnitt I Tabell 9 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 9: Sammanfattning av in- och utsignaler i simulatorn Befintligt 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 22. Ett mer detaljerat flödesschema kan ses i Figur 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. För ytterligare information angående simulatorn, se förra årets tekniska dokumentation [3].

35 LiU Racetrack 30 Figur 22: 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 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 (10) - (13). Utan denna så kallade fiktiva mätuppdatering skulle varken bilens position eller hastighet vara känd. Xptq Xpt T q ` T v x pt T q ` T 2 Y ptq Y pt T q ` T v y pt T q ` T 2 2 9v xpt T q (10) 2 9v ypt T q (11) v x ptq v x pt T q ` T 9v x pt T q (12) v y ptq v y pt T q ` T 9v y pt T q (13) Notera att då fordonsmodellen från 2012 används behöver en integration av 9v x och 9v y göras innan (10) - (13) kan användas Simuleringssystemet efter projektavslut Fokus på simuleringssystemet kommer under detta projekt vara att integrera simuleringssystemet med det fysiska systemet enligt Figur 2. Utöver detta kommer regulatorparametrarana att anpassas för att effektivare kunna reglera fordonsmodellen. Simulatorn ska uppfylla alla regleringskrav som det fysiska systemet uppfyller.