Teknisk Rapport Autonom truck

Transkript

1 Teknisk Rapport Autonom truck Version 1.0 Redaktör: Joar Manhed Datum: 19 december 2018 Status Granskad Kim Byström Godkänd Andreas Bergström

2 Projektidentitet Grupp E-post: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Andreas Bergström, ISY, Linköping University Telefon: , E-post: Magnus Persson, Toyota Material Handling, Mjölby Telefon: , E-post: Daniel Axehill, ISY, Linköping University Telefon: , E-post: Kim Byström Erik Hedberg, ISY, Linköping University Telefon: , E-post: Gruppmedlemmar Namn Ansvarsområde Telefon E-post Kim Byström Projektledare kimby803 Lovisa Jansson Designansvarig lovja529 Anton Johansson Komponentansvarig rörelseplanering antjo244 Joar Manhed Dokumentansvarig joama350 David Sandmark Komponentansvarig modellering & simulering Niklas Stenberg Komponentansvarig reglering davsa nikst888 Pär Sörliden Mjukvaruansvarig parso619 Gustaf Westerholm Testansvarig guswe541

3 Dokumenthistorik Version Datum Utförda förändringar Utförda av Granskad Första utkast. Alla Kim Byström Ändringar efter kommentarer från handledare och beställare Alla Godkänd av beställare - - Kim Byström

4 Innehåll 1 Introduktion 1 2 Översikt Kommunikation Simuleringsmodulen Planeringsmodulen Regleringsmodulen Kommunikation C# - Simulering C++ - Reglering & Planering Smartness Planeringsmodulen Regleringsmodulen Kvar att implementera och tolkningar vid implementering Förslag på förändringar i Smartness Delsystem - Simuleringsmodul Inledande beskrivning Truckmodell WheelCollider-modell Idealiserad kinematisk modell LIDAR Karta Delsystem - Planeringsmodul Inledande beskrivning A*-algoritmen Hastighetsangivelser Karta Hinderdetektion Planeringens kommunikation med Smartness Vidareutveckling Hastighetsangivelser Algoritm Sekvensplanering Flotthantering Delsystem - Regleringsmodul Inledande beskrivning Tidigare regleralgoritm Alternativa reglerstrukturer Filtrering av bana från planeringen Dynamisk återkopplingslinjäriserad regulator B-spline Implementation

5 6.4 PID-regulator Resultat Vidareutveckling Problem A Klassdiagram 30

6 Autonom truck 1 1 Introduktion Toyota Material Handling i Mjölby producerar cirka truckar per år, varav andelen autonoma truckar i nuläget utgör ungefär 1%. Andelen autonoma truckar förväntas öka, liksom behovet av lagerhanteringssystem som utnyttjar en truckflotta optimalt. I utvecklingsarbetet av autonoma truckar ser Toyota ett behov av kunna modellera och simulera produkter och koncept i en 3D-miljö. I detta projekt har en sådan miljö tagits fram tillsammans med truckmodell och funktionalitet rörande reglering och planering. I denna tekniska rapport beskrivs övergripande hur en autonom truck har modellerats och simulerats i spelmotorn Unity. Dessutom beskrivs hur funktionalitet som planering och reglering av trucken har designats i projektet. För att skydda Toyotas immateriella rättigheter kan viss information ha utelämnats från detta dokument.

7 Autonom truck 2 2 Översikt Det framtagna systemet består av tre stycken program; simuleringsmodulen, regleringsmodulen samt planeringsmodulen. Programmen kommunicerar med varandra genom att använda Toyotas egna protokoll benämnt Smartness. Smartness är tillsammans med simuleringsmodulen implementerat som ett Unity-program, vilket är skrivet i C#. De resterande två modulerna är skrivna som separata program i C++. Denna uppdelning har gjorts för att efterlikna det system som finns på Toyotas nuvarande truck där olika moduler (för reglering, planering, positionering och säkerhet) implementeras som separata, utbytbara program. Dessa moduler implementeras på varsin Raspberry Pi för att enklare hålla dem som fristående program samt begränsa presentandan. Alla dessa program kommunicerar sedan med varandra via Smartness. I appendix A finns ett klassdiagram för hela det system som utvecklats i detta projekt. I projektet har den positioneringsmodul och säkerhetssystem som finns i Toyotas system inte implementerats utan fokus har lagts på att ta fram en simulering samt att ta fram en reglering och planering som fungerar tillsammans med simuleringen. Kommunikationen via Smartness har valts att göras i Unity-programmet så att de respektive modulerna (reglering och planering) har kunnat utvecklas på samma sätt som Toyota använder dem. 2.1 Kommunikation De olika delsystemen kommunicerar med varandra via Toyotas egenutvecklade gränssnitt Smartness. En översikt av kommunikationen kan ses i figur 1. Smartness använder sig av bestämda meddelandeformat för att skicka meddelanden mellan moduler och använder sig i botten av protokollet TCP/IP för att skicka meddelanden mellan servern och klienterna. På en fysisk truck kommunicerar även Smartness med motorn, representerad i figur 1 som MCU (motor control unit). I det här projektet är MCU utbytt mot en simuleringsmiljö i Unity. Själva hanteringen av Smartness-protokollet är implementerat i Unity-programmet. Vid kommunikationen agerar Unity-programmet server och de övriga modulerna agerar klienter. Figur 1: Översikt av hur de olika delsystemen på trucken kommunicerar.

8 Autonom truck Simuleringsmodulen Simuleringen av själva trucken, LIDAR-sensorn, samt kommunikationen via Smartness görs i spelmotorn Unity. Eftersom positioneringsmodulen inte ingår i projektet har truckens position samt kartan över omgivningen istället genererats direkt från simuleringen. I Unity visualiseras trucken samt truckens miljö i 3D. Skripten som används vid simuleringen är skrivna i C#. 2.3 Planeringsmodulen Planeringsmodulen beräknar den trajektoria som trucken ska följa. Den får ett mål att köra till och beräknar sedan en trajektoria för att nå målet. Om hinder uppstår i trajektorian, planeras en ny rutt baserat på nya miljön. Trajektorian består av en bana samt hastighetsangivelser längs banan. Den genererade trajektorian skickas till regleringsmodulen. Planeringsmodulen är skriven i programmeringsspråket C++ och utvecklat för att kunna köras på en Raspberry Pi, det vill säga den är utvecklad för att fungera i en Linux-miljö. 2.4 Regleringsmodulen Regleringsmodulen är den modul som styr trucken i simuleringen. Den tar emot en planerad trajektoria från planeringsmodulen och tar även emot statusmeddelanden från simuleringen om truckens nuvarande position, hastighet, vinkel m.m. Denna information används sedan för att beräkna styrsignaler som skickas tillbaka till simuleringen. Regleringsmodulen utvecklades först i Unity eftersom kommunikationen inte var färdigutvecklad. Då reglering körs från Unity fungerar den som den ska. Regleringsmodulen är även skriven i språket C++ och utvecklad för att kunna köras på en Raspberry Pi, det vill säga den är utvecklad för att fungera i en Linux-miljö. Dock fungerar den inte fullt ut i C++-koden.

9 Autonom truck 4 3 Kommunikation Kommunikationen mellan modulerna sker via protokollet TCP/IP. Vid denna kommunikation används Toyotas egenutvecklade Smartness-protokoll för att skicka meddelanden till reglerings- samt planeringsmodulen. Vid kommunikationen agerar Unity-programmet server och de övriga modulerna agerar klienter. Eftersom positioneringsmodulen inte utvecklas i projektet behöver en genererad karta vara tillgänglig för planeringsmodulen. Denna karta genereras av simuleringen och för att kunna skicka den används en separat kommunikationskanal. 3.1 C# - Simulering Kommunikationen sköts av ett skript (TCPServer) i Unity (kopplat till ett tomt Game- Object). Detta skript skapar tre trådar som lyssnar på inkommande meddelanden från respektive kommunikationskanal. Regleringsmodulen är kopplad till port 4012 på datorn som kör simuleringen, planeringsmodulen till port 4010 och kommunikationen för kartan (också till planeringsmodulen) till port För varje av de tre typerna av kommunikation skapas i TCPServer en instans av en subklass till klassen Connection. Dessa klasser har metoder för att skicka samt ta emot de olika typer av meddelanden som respektive modul ska kunna hantera. 3.2 C++ - Reglering & Planering De två modulerna som är utvecklade i C++ använder sig av samma kod för att sköta kommunikationen. Grunden till kommunikationen finns i klassen Client som tar hand om själva kommunikationen. När klassen instansieras anges vilken port kommunikationen ska ske på, och för varje instans av klassen skapas en tråd som lyssnar efter meddelanden på porten. I klassen finns även metoder för att skicka olika typer av meddelanden. För att kommunicera med den övriga koden tar även konstruktorn till Client ett objekt av typen CommunicationInterface. Detta interface, som är en abstrakt klass, innehåller vissa gemensamma funktioner för att läsa in olika meddelanden samt deklarerar en metod för avkodningen av ett inkommet meddelande. De två modulerna skapar instanser av varsin subklass till CommunicationInterface och instansierar sedan ett Client-objekt. 3.3 Smartness Smartness agerar som kommunikationslager mellan de olika modulerna samt truckens MCU. Det är utvecklat av Toyota och finns beskrivet i Toyotas interna dokument. Smartness-protokollet går ut på att skicka meddelanden med en header, ett fält med datainformation samt fält med parametervärden beroende på vilken typ av meddelande det är. Headern innehåller information om vilken typ och storlek meddelandet har samt ett ID för varje meddelande. Sedan följer ett fält med meddelandeinformation som innehåller parametrar kopplade till den aktuella meddelandetypen. Slutligen har vissa meddelanden ett eller flera ytterligare fält med fler parametrar. Vad som skickas till och från de olika modulerna finns sammanfattat i figur 2 och beskrivs i mer detalj nedan Planeringsmodulen Planeringsmodulen kommunicerar med Unity på port 4010.

10 Autonom truck 5 Figur 2: Översikt på kommunikationen med Smartness Från planeringsmodulen får Smartness kommandomeddelanden för en planerad rutt. Dessa innehåller en rad koordinater som beskriver rutten med hastighet för varje koordinat. Dessa meddelanden ska skickas en gång i sekunden och Smartness skickar dem sedan vidare till regleringsmodulen. Planeringsmodulen får meddelanden om truckens status från Smartness (VehicleStatus). Dessa meddelanden innehåller information om truckens position, riktning, hastighet, vikt på gafflarna samt batteristatus. Dessa meddelanden ska skickas inom 100ms efter att Smartness mottagit ett kommando. Nästkommande 3 sekunder av trajektorian skickas till regleringsmodulen (Navigation- Command). Dessa meddelanden skickas en gång i sekunden och de beskriver hur trucken ska köra de närmaste tre (3) sekunderna. Detta gör att sträckan som skickas inte är en fast längd, utan behöver integreras fram från den önskade medelhastigheten med avseende på tiden. Planeringsmodulen kan även skicka ett meddelande med en initial position (Initial- Position) till Smartness. Detta ska sedan skickas vidare till positioneringsmodulen (vilket inte är implementerat). Planeringsmodulen kan även skicka kommandon till Smartness (ActionType), för att till exempel avbryta körningen. Kartan som skickas från Unity till planeringsmodulen skickas via en separat kommunikaiton på port Unity skickar kartan en gång i sekunden Regleringsmodulen Regleringsmodulen kommunicerar med Unity på port Från regleringsmodulen får Smartness meddelanden om hur hjulen ska styras (Drive- Command). Dessa innehåller hastighet (mm/s) och hjulvinkel (rad) för varje hjul samt en eventuell felkod, enligt kommunikationsprotokollet Smartness. Denna information ska sedan skickas till truckens aktuatorer eller, i simuleringsmiljön, till skriptet som styr trucken.

11 Autonom truck 6 Dessa meddelanden skickas med 100 ms intervall. Varje gång Smartness tar emot ett DriveCommand skickas ett statusmeddelande (Vehicle- Status) tillbaka till regleringsmodulen. Dessa meddelanden innehåller information om truckens position, riktning, hastighet, vikt på gafflarna samt batteristatus. Dessa meddelanden ska skickas inom 50ms efter att Smartness mottagit ett DriveCommand. Smartness skickar även meddelanden till regleringsmodulen med odometri-information (DriveStatus), det vill säga nuvarande hjulvinkeln och hjulhastighet för varje hjul samt eventuell felkod. I simuleringsmiljön tas denna information från hjulkomponenterna och skickas till regleringsmodulen. Regleringsmodulen får även meddelanden om den planerade rutten (NavigationCommand) från planeringsmodulen, via Smartness. Dessa innehåller en rad koordinater som beskriver rutten samt hastighet och riktning för varje koordinat. Dessa meddelanden skickas en gång i sekunden från planeringsmodulen. Regleringsmodulen kan även skicka kommandon till Smartness (ActionType), för att till exempel avbryta körningen. 3.4 Kvar att implementera och tolkningar vid implementering Till vissa meddelandetyper kan en felkod inkluderas, dock händer inget då denna tas emot. Detta beror på att det under projektet inte fanns något riktigt syfte att använda denna. Meddelandet DriveStatus skickas aldrig till regleringsmodulen. I dagsläget behöver inte regleringen denna information och det var inte helt klart vid vilka tillfällen det är bra att skicka detta meddelande. Då en ActionType skickas antingen från reglering eller planering ändras olika bitar i statusflags, som skickas med i VehicleStatus, beroende på vilken ActionType som skickas. För vissa ActionType uppdateras inget. Vilka bitar som ändras finns sammanfattat i tabell 1 där aktiverar innebär att de sätts till 1 och avaktiverar att de sätts till 0. För de typerna där inga bitar sätts händer inget då en ActionType skickas. Tabell 1: Vilka bitar i statusflags som ändras vid olika ActionType ActionType Aktiverar Avaktiverar Abort E-stop active, Error active Auto mode enable, Driving enable Sleep - Auto mode enable, Driving enable Wake up Auto mode enable, Driving enable - Load - - Unload - - Start charge Warning active - Stop charge - - Eftersom regleringen inte är fullt fungerade som en egen modul i C++ så körs denna i Unity. När denna istället ska köras via C++ måste några rader ändras i Connection.cs. Fem rader kan tas bort och en rad som nu är en kommentar ska köras. Dessa rader är kommenterade så de är enkla att hitta. När en klient har kopplat upp sig mot servern skickas ett VehicleStatus meddelande till respektive modul. Detta lades till då planeringen måste veta truckens startposition för att kunna planera en inledande trajektoria och regleringen behöver veta nuvarande position för att kunna beräkna styrsignalerna. Senare skickas VehicleStatus som ett svar

12 Autonom truck 7 på andra meddelanden enligt Smartness standard, se och Förslag på förändringar i Smartness Under utvecklingen har det uppkommit några tankar på hur Smartness skulle kunna utvecklas. Från planeringen skickas trajektorian som koordinater. Vid beräkningen av hastigheten för olika delar av trajektorian hade det varit fördelaktigt att använda sig av B-spline, något som också används i regleringen. Därför hade det varit bra att även kunna skicka trajektorian som B-spline. Enligt Smartness protokollet ska en initial position skickas från planeringsmodulen till positioneringsmodulen. Det har inte använts under projektet dels för att ingen positioneringsmodul har implementerat men också för att planeringen inte känner till den initiala positionen. Som det är implementerat nu hämtar planeringen den nuvarande positionen och utifrån den samt en given slutpunkt beräknas den optimala vägen. Om planeringsmodulen ska sätta den initiala positionen måste trucken på något sätt kunna ta sig till denna först innan den ska köra den planerade trajektorian. Det känns mer logiskt att positioneringen börjar med att skicka en initial position till planeringen så denna kan beräkna bästa trajektorian från nuvarande position till den önskade destinationen.

13 Autonom truck 8 4 Delsystem - Simuleringsmodul Simuleringsmodulen simulerar en truck i spelmotorn Unity. Syftet med modulen är att kunna ersätta den verkliga trucken och kommunicera med de andra modulerna på samma sätt som den verkliga trucken gör. 4.1 Inledande beskrivning Trucken har modellerats och simulerats i en 3D-miljö i spelmotorn Unity. Simuleringsmodulen har skrivits i språket C# och simuleras och redigeras i Unitys grafiska miljö. Det finns två modeller av trucken i Unity, samt en visuell representation av den (se figur 3). Den ena är en modell som använder sig av WheelCollider-objekt vilket är objekt som redan finns implementerat i Unity och den andra är en kinematisk modell som bygger på rörelseekvationer för ett allmänt trehjuligt fordon. I simuleringen styrs båda modellerna genom att ge det bakre hjulet en styrvinkel och hastighet, i stället för att som under föregående projekt bara ge hela trucken en riktning och hastighet. Under detta tidigare projekt har trucken även modellerats i Gazebo, där den bestod av olika sammankopplade volymer med givna massor och tröghetsmoment. Där har modellen varit mycket mer fullständig än den som idag finns i Unity. Målet var ursprungligen att helt eller delvis återanvända tidigare modeller och migrera dessa till Unity. Tidigare CDIO-projekt från 2017 tog även fram en mer fullständig matematisk modell för trucken, vilken har implementerats och utvärderats i Unity [3]. Figur 3: Nuvarande modell av trucken i Unity. 4.2 Truckmodell En illustration av simuleringens koordinatsystem kan ses i figur 4. Det globala koordinatsystemet är markerat med x, y och z och truckens lokala koordinatsystem med x, y och z. Truckens styrvinkel är markerad med α i figuren. Observera att Unity använder ett vänsterhänt koordinatsystem. Vinkeln som Smartness använder för hela trucken benämns frameheading och visas i figur 5. Den är mellan π och π, samt positiv i motursriktning.

14 Autonom truck 9 Figur 4: Lokalt och globalt koordinatsystem samt styrvinkeln α. Figur 5: Vinkeln frameheading. Notera även att i Unity åker trucken i xz-planet medan Smartness och därmed planeringen räknar med att trucken åker i xy-planet. Vid kommunikation via Smartness konverteras därmed dessa koordinater för att överensstämma med Smartness-protokollet. De båda truckmodellerna i Unity är underklasser till en superklass kallad TruckModel. Denna superklass innehåller de egenskaper hos trucken som är gemensamt för de båda underklasserna, här kallade KinematicModel och WheelColliderModel. De huvudsakliga medlemsvariablerna som TruckModel har är tillståndsvariablerna x, z, frameheading och theta samt styrsignalerna alpha och vs. Vid varje FixedUpdate uppdateras tillståndsvariablerna i enlighet med den underklass till TruckModel som används. Truckens styrsignaler sätts av en utomstående regulator med hjälp av de medlemsfunktioner som finns i TruckModel.

15 Autonom truck WheelCollider-modell Denna modell består av tre WheelCollider-objekt som sitter ihop med ett BoxColliderobjekt. I denna modell är WheelCollider-objekten kopplade till de delar av CAD-modellen som utgör truckens hjul. WheelCollider är en inbyggd standardkomponent som finns i Unity som beskriver kollisionsegenskaper hos ett hjul. Komponenten innehåller även inställningar för flera andra hjulegenskaper, såsom friktion, massa och fjädring som kan justeras. Dessa inställningar har under projektets gång finjusterats för att få ett så verklighetstroget beteende som möjligt hos trucken, baserat på visuell bedömning. Det har alltså inte gjorts någon validering mot en verklig truck. Det bakre hjulet - som är truckens styr- och drivhjul - förses via en utomstående regulator med en viss hastighet och styrvinkel. Därefter tar den bakomliggande funktionaliteten hand om själva fysiken hos WheelCollider-objekten. En inställning som fick justeras för att WheelCollider-modellen skulle ha ett tillfredsställande beteende var Unitys Fixed Timestep. Detta tidssteg fick göras mindre för att bland annat eliminera oönskade vibrationer. Denna inställning återfinns under Edit Project Settings Time och sedan inställningen Fixed Timestep. Den är inställd till Ingen detaljerad inställning gjordes av den utan den sänktes tills problemen med vibrationer försvann. Fixed Timestep bestämmer hur ofta Unitys FixedUpdate() funktion körs. På grund av att Unity inte tillåter att hastigheten på ett WheelCollider-objekt sätts direkt har en regulator som styr hjulets vridmoment implementerats i funktionen Torque- Controller. Detta är en enkel PID-regulator som försöker styra den aktuella hastigheten på drivhjulet till en önskad hastighet. Detta är gjort för att WheelCollider-modellen ska kunna använda styrhjulets hastighet som styrsignal och därmed ha samma styrsignal som underklassen Kinematicmodel, se nedan Idealiserad kinematisk modell Om gaffeltrucken ses som ett omvänt trehjuligt fordon kan den modelleras enligt ẋ cos α cos θ ż = cos α sin θ v s θ sin α/l där x och z är truckens position i x- respektive z-led, α är drivhjulets vinkel och v s är drivhjulets hastighet. Vinkeln θ definieras enligt figur 6. Anledning till att θ används istället för frameheading är att trucken ses som en omvänd trehjuling och då är θ relevant, men den vinkeln är endast använd internt och även för regulatorn. Utåt via Smartness är det frameheading som används. Relationen mellan frameheading och θ är frameheading = { θ π 0 < θ π θ + π π θ 0 respektive θ = { frameheading π 0 < θ π frameheading + π π θ 0

16 Autonom truck 11 Figur 6: Vinkeln θ. För att implementera den kinematiska modellen i Unity behöver den skrivas på diskret form. Det kan göras enligt x[k + 1] x[k] cos α cos θ z[k + 1] = z[k] + T s cos α sin θ v s θ[k + 1] θ[k] sin α/l där T s är samplingstiden. Eftersom koden som simulerar truckens kinematik körs i Unitys FixedUpdate() funktion som körs varje fixed framerate frame bestäms T s här av Unitys projektinställningar. Inställningen återfinns som tidigare nämnt under Edit Project Settings Time och sedan inställningen Fixed Timestep. Den är inställd till LIDAR Den simulerade trucken är utrustad med en LIDAR-sensor med en vidd på 270 och en upplösning på 2.5 mätningar per grad. LIDAR-sensorn är modellerad med hjälp av Physics.Linecast, vilket är en funktion som skickar en stråle i en given riktning och returnerar om den träffade ett Collider-objekt och i så fall i vilken punkt. Alla positioner som LIDAR-sensorn träffar vid en mätning lagras i en lista. På grund av problem vid bearbetning av denna lista används LIDAR:n ej i slutprodukten. 4.4 Karta Planeringsmodulen behöver en karta för att kunna planera en rutt. På den verkliga trucken genereras denna karta av positioneringsmodulen genom att använda LIDAR-data och använda en SLAM-algoritm. Eftersom positioneringsmodulen inte ingår i detta projekt genererar istället simuleringsmiljön en karta (som en array av bytes) som skickas en gång i sekunden till planeringsmodulen. Kartan genereras genom att undersöka om det finns ett Collider-objekt i Unity-miljön på varje position. Om det gör det läggs en etta in och annars en nolla. Själva truckmodellen är också ett Collider-objekt men den har fått en speciell tagg så att den inte syns i kartan.

17 Autonom truck 12 5 Delsystem - Planeringsmodul I planeringsmodulen sker beräkning av rutt med tillhörande hastighetsangivelser. I första delen av detta kapitel beskrivs hur detta görs i nuläget. I andra delen av detta kapitel finns förslag på förbättringar och vidareutvecklingar angående planering. 5.1 Inledande beskrivning Planeringsmodulen genererar en trajektoria för trucken att följa givet en start- och slutposition samt en karta över omgivningen. Rörelsebanan skapas med hjälp av algoritmen A*, som med hjälp av kartan hittar kortaste körbara väg mellan start- och slutpositionen. Planeringsmodulen tar även fram hastighetsangivelser för trucken i varje punkt. Hastigheterna i punkterna baseras på huruvida det finns kurvor i rörelsebanan eller ej. För kurvor skall hastigheten sänkas för att erhålla bättre kurvtagningsförmåga. Rörelsebanan tillsammans med hastighetsangiverlserna är trajektorian. För att planeringsmodulen skall kunna ta hänsyn till lagermiljön importeras en karta av lagret. Denna karta är en binär matris med 1:or och 0:or. Varje tal i matrisen representerar en 10x10cm ruta i lagermiljön (storleken på rutnätet är variabel). 0 signalerar att rutan är körbar och 1 signalerar att rutan har något typ av hinder och därför ej är en körbar ruta. Planeringsmodulen är dynamisk, dvs om ett hinder uppstår i rörelsebanan under körning så körs A*-algoritmen igen och en ny röreslebana tas fram som tar hänsyn till det nya hindret. Startpositionen för varje A* ansätts automatiskt till att vara truckens nuvarande position. Slutpositionerna hämtas in från en jobblista med koordinater. 5.2 A*-algoritmen Nedan följer en översiktlig beskrivning av A*-algoritmen. A*-algoritmen beräknar vägen med lägst kostnad i en viktad graf mellan en start- och en slutnod. Huvudformeln som används i A*-algoritmen ges i (1). Den beräknar ett värde för en nod n, där g(n) är kostnaden för vägen från startnoden fram till nod n, och h(n) är ett heuristiskt estimat av kostnaden för vägen mellan n och slutnoden. Exempelvis kan h(n) vara det euklidiska avståndet mellan nod n och slutnoden. f(n) = g(n) + h(n) (1) Algoritmen använder sig av två listor: O (open) och C (closed). Listan O innehåller de noder som undersöks för att hitta vägen med lägst kostnad. Den andra listan C innehåller noder som inte ska undersökas igen. A*-algoritmen fungerar i stora drag enligt följande. 1. Initiera O och C till tomma listor. Lägg startnoden till O och sätt dess kostnad till Välj från O den nod med lägst kostnad (om flera välj till exempel den senast använda). Beteckna denna nod med N. 3. Ta bort N från O och lägg den till C. 4. Om N är slutnoden, gå till steg 7.

18 Autonom truck Gör något av följande för varje nod M som är närliggande till N samt passerbar: (a) Om M ligger i C, gör inget. (b) Om M inte ligger i O, lägg den till O samt beräkna dess kostnad enligt (1) och sätt dess föregångare till N. (c) Om M ligger i O, beräkna en ny kostnad för M givet noden N. Om den nya kostnaden är lägre än den gamla kostanden, uppdatera M:s kostnad och sätt dess föregångare till att vara N. Låt M vara kvar i O. 6. Om O inte är tom gå till steg 2, annars om O är tom och målnoden inte nåtts finns ingen passerbar väg mellan start- och slutnod. 7. Vägen med lägst kostande mellan slut- och startnod har hittats. Den fås genom att hitta föregångaren till slutnoden, och sedan dess föregångare och så vidare tills startnoden nås. 5.3 Hastighetsangivelser För varje punkt i rörelsebanan finns en hastighetsangivelse. Hastighetsangivelsen är implementerad för att förenkla kurvtagningen när trajektorian sedan skickas till regleringsmodulen. Ekvation (2) visar att hastigheten v väljs som en funktion av maximala hastigheten v max och bestraffningskoefficienten K. v = K v max (2) K är en bestraffningskoefficient som straffar hastigheten v beroende på hur skarp kurvan är. Genom att jämföra förändringen i x-led och y-led mellan noden ett steg fram respektive noden två steg fram kan hastighetsriktning respektive kurvans karaktär bestämmas, se figur 7. Baserat på kurvans vinkel straffas hastigheterna enligt nedan. Värdena på K är en designparameter och har i detta projekt valts för tydlig visuell effekt vid simulering, enligt (3). 0 K = 1 K( 45 K = 0.5 ) = 90 K = K = 0.1 När den önskade kurvhastigheten är beräknad med hjälp av ekvation (2) och (3) beräknas hur mycket hastigheten måste minskas i ett antal punkter innan kurvan för önskad nedbromsning. Den totala bromssträckan beräknas enligt ekvation ekvation (5) och tiden fås från (4) där a är den önskade retardationen, som är en designparameter. t = v max v a (3) (4) s = v max + v t (5) 2 Då avståndet mellan punkterna är konstant och känt fås antalet punkter där hastigheten behöver korrigeras (n) genom att dividera bromssträckan på avståndet mellan punkterna. I punkten där kurvan börjar sätts hastigheten till v och därefter backtrackas hastighetsvektorn n punkter, där föregående element skalas med en retardationsfaktor för att uppnå önskad nedbromsning, se figur 8.

19 Autonom truck 14 Figur 7: Illustration av hur kurvorna ser ut för de olika fallen. Figur 8: Illustration av hur backtracking av hastighetsvektorn fungerar. Grafen till vänster är innan backtracking och grafen till höger är efter backtracking. 5.4 Karta Planeringsmodulen tar emot en karta från simuleringen en gång per sekund som uppdateras kontinuerligt. Då A*-algoritmen behandlar trucken som en punkt vid ruttplaneringen och därmed inte tar hänsyn till truckens storlek, behöver den mottagna kartan efterbehandlas innan ruttplaneringen genomförs för att säkerställa att den planerade rutten blir körbar. Denna efterbehandling av kartan går ut på att bredda alla hinder på kartan lika mycket som halvan fordonets bredd med lite marginal. Hela kartan loopas igenom och extra ettor läggs till runt ettorna i den ursprungliga kartan. 5.5 Hinderdetektion Planeringsmodulen jämför punkterna i rörelsebanan med punkterna i den efterbehandlade kartan så alla punkter i rörelsebanan är körbara. Om ett hinder skulle uppstå i körbanan så triggas planeringsmodulen att generera en ny körbana som tar hänsyn till den nya miljön. Figur 9 visar hur ett hinder upptäcks och rutten planeras om. Figur 9a visar planerad rutt utan hinder och figur 9b visar planerad rutt efter att hinder upptäckts på tidigare rutt. 5.6 Planeringens kommunikation med Smartness Planeringsmodulen skickar en trajektoria via Smartness i enlighet med Smartness-standard. Trajektorian innehåller rörelsebanans punkter samt hastighetsangivelser i de punkterna

20 Autonom truck 15 (a) Rutt utan hinder. (b) Rutt med hinder. Figur 9: Illustration av hur rutten uppdateras när hinder upptäcks på redan planerad rutt. för de kommande tre sekunderna i truckens körbana. Hur många punkter som ska skickas bestäms genom att summera kvoten mellan kartans storlek på rutnätet och hastigheterna i nästa punkt tills dess att summan blir 3 sekunder. Planeringsmodulen tar emot truckens position samt kartan via Smartness som används vid ruttplaneringen. 5.7 Vidareutveckling I detta kapitel finns förslag på utvecklingsmöjligheter inom projektet. Arbetet är utfört på sådant sätt att det enkelt ska kunna vidareutvecklas och utvidgas Hastighetsangivelser En troligtvis lämpligare metod för att sätta hastighetsangivelser längs rörelsebanan är att utgå från maximal vinkelhastighet som trucken kan uppnå utan att tappa kontroll. Den maximala vinkelhastigheten kommer bero på vilken typ av truck som används och hur den är lastad. Varje truck och varje lastfall kommer kunna tilldelas en specifik konstant maximal vinkelhastighet. Vidare krävs att rörelsebanan är kontinuerlig/består av splines. Detta gör att kurvaturen för rutten alltid är känd, vilket resulterar i att man kan ansätta hastigheten enligt ekvation (6). ω max är maximala vinkelhastigheten och r är radien, dvs inversen av kurvaturen. Dock leder detta till komplikationer, då Smartness inte godkänner kommunikation av splines, utan endast vill ha koordinater med visst intervall. Att beräkna splines både i planeringsmodulen och regleringsmodulen bedöms som onödigt arbete. Därför bör det ses över om det går att utveckla Smartness för att möjliggöra detta. v max = ω max r (6) Algoritm Nuvarande algoritm hanterar trucken som en punkt i sin ruttplanering, dvs planerad bana skulle kunna gå i gångar smalare än truckens bredd. Rent praktiskt innebär detta att planeringen kan planera rutter som är fysiskt omöjligt för trucken att köra. Problemet hanteras genom att modifiera kartan, se 5.4. Detta innebär att man expanderar alla hinder på kartan så pass mycket att trucken kan köra enligt planerad rutt utan att hindras. Dock

21 Autonom truck 16 är detta onödigt beräkningstungt, då hela kartan måste uppdateras på detta sätt varje gång en ny rutt ska planeras. Ett mindre beräkningstungt sätt jämfört med att uppdatera kartan är att i själva A*- algoritmen ta hänsyn till att noden man undersöker måste omges av en matris med körbara element (matrisens storlek beror på truckens storlek). Detta alternativ är mer tidskrävande att implementera, vilken är orsaken till att det ännu inte har implementerats. Vidare kan det vara värt att undersöka andra ruttplaneringsalgoritmer, så som rapidly exploring random tree (RRT). RRT är en friare ruttplanerare i det avseendet att den inte behöver ta hänsyn till rutnät och noder på samma sätt som A*. D* (Lite) kan ses som en vidareutveckling av A* och kan därför också vara bra att undersöka för vidareutveckling Sekvensplanering I nuvarande projekt finns en uppdragslista i form av en textfil där det översta uppdrager väljs ut och tas bort. Ett uppdrag kan vara att åka och hämta eller lämna en pall. När det är dags för nästa uppdrag plockas nästa från listan. En utvecklingsmöjlighet är hur denna textfil skapas och sorteras. Det kan tänkas att den sorteras för att optimera för en truck baserat på avstånd, körtid, energiåtgång, etc. för aktuell truck. Om det finns flera truckar kan det tänkas att listan även delas upp mellan truckarna. Denna utveckling och implementering kan ske smidigt, då det redan är inprogrammerat att trucken följer en uppdragslista. Det som behöver göras är alltså att strukturera listan efter eget tycke. Detta kan göras antingen i planeringen, eller utanför så länge textfilen finns tillgänglig att hämtas av planeringsmodulen Flotthantering En annan möjlighet till vidareutveckling är möjlighet till flotthantering. Flotthantering innebär ett system med flera truckar där systemet vet de olika truckarnas förutsättningar så det på bästa möjliga sätt kan planeras deras samspel. Detta är något som är önskvärt från kundens perspektiv och bör därför tas hänsyn till under vidareutveckling. Genom planeringsmodulen kan man få vetskap om var truckar kommer befinna sig vid olika tidpunkter, och därmed kunna planera rutter för andra truckar baserat på hinder som ännu ej finns, men som kommer uppstå. Här finns stora utvecklingsmöjligheter.

22 Autonom truck 17 6 Delsystem - Regleringsmodul 6.1 Inledande beskrivning Regleringsmodulen har till uppgift att styra trucken så att den följer den av planeringsmodulen genererade rörelsebanan så bra som möjligt. Rörelsebanan beskrivs av ett antal punkter med koordinater med tillhörande hastigheter. Målet med den reglering som togs fram under projektet var att trucken ska kunna följa en angiven bana med hastighetsangivelserna. Dessutom ska den klara av skarpa kurvor bättre än den gör vid projektstarten, då den vid projektstart gör mjuka kurvor, det vill säga genar i hörnen. Det finns ett flertal olika sätt att lösa detta. Simuleringen av trucken görs i Unity och ursprungligen var tanken att regleringen skulle vara en separat modul på en Raspberry Pi. För att detta ska fungera krävs kommunktionsgränsnittet Smartness. Vid projektstart existerade detta inte vilket innebar att för att kunna komma igång med att utveckla regleringen så kördes den direkt i Unity med C#-kod, men utvecklas fristående så att det skulle gå att flytta över till C++ och därefter köra på en Raspberry Pi. Eftersom utvecklandet av Smartness var omfattande dröjde det innan regleringen kunde börja flyttas över till en Raspberry Pi och testas därifrån. När överföringen till Raspberry Pi inleddes uppstod ett antal problem bland annat med kod och med att utnyttja kommunikationsgränssnittet. Överföringen till C++ är nu gjord men fungerar inte helt och problemen som uppstod fanns det inte tid till att lösa. Därför körs just nu regleringen i Unity men planeringen körs via en Raspberry Pi. Orsaken till problemet är oklar, men det kan ha att göra med fördröjningar som uppstår vid kommunikationen mellan de olika modulerna. En eventuell lösning på detta diskuteras längre ner Tidigare regleralgoritm Den reglering som fanns implementerad för trucken vid projektstart var en regleralgoritm med namnet Pure Pursuit, som finns beskriven i Designspecifikationen [1]. Den fanns dock inte i Unity och var endast skriven i C++. Eftersom utvecklingen av reglering gjordes i Unity bedömdes det vara för tidskrävande att skriva om denna kod till C#. Denna regleralgoritm var planerad att implementeras och undersökas vidare endast i mån av tid, och på grund av tidsbrist i slutet av projektet utfördes detta inte Alternativa reglerstrukturer Ett alternativ för reglering var att implementera algoritmen Vector Pursuit. Denna har samma grundtanke som Pure Pursuit men tar även hänsyn till den önskade vinkeln i varje punkt. Vid närmare efterforskning konstaterades att denna algoritm bygger på många ekvationer och är inte helt lätt att förstå. Därför gjordes inget mer på denna algoritm. Ett andra alternativ är MPC-reglering, se Designspecifikationen [1]. Reglering med MPC är dock väldigt beräkningskrävande. Tillståndsmodellen för trucken är även olinjär, vilket innebär att det antingen krävs en linjäriserad modell eller användning av olinjär MPC, som i sin tur är ännu mer beräkningskrävande. Tillståndmodellen är inte heller av den karaktär att den kan linjäriseras med exempelvis en småvinkel-approximation, då de ingående vinklarna kan vara - och ofta är - stora. Av dessa anledningar gjordes valet att inte implementera en MPC-regulator. Vid efterforskning upptäckes möjligheten att ta fram en regulator genom insignal-utsignallinjärisering. Det fanns en del skrivet om detta vilket gjorde att detta alternativ un-

23 Autonom truck 18 dersöktes närmare. Se 6.3 nedan. Under projektets gång implementeras även en PID-regulator. Vid projektstart fanns ingen sådan implementeras på trucken, men med tanke på hur välanvänd denna typ av reglering är på andra typer av autonoma fordon gjordes valet att implementera en sådan även i detta fall. Därför undersöktes en PID-regulator för att ha ett alternativ och att ha något att den andra regulatorn jämföra med. Se 6.4 nedan. 6.2 Filtrering av bana från planeringen Den bana som planeringsmodulen genererar innehåller i regel skarpa svängar. Detta kan innebära problem för regleringen, då detta kan leda till överkompensering och ge trucken ett väldigt svängigt beteende. För att hantera detta implementerades funktionalitet för att filtrera den mottagna trajektorian och därmed mjuka upp de skarpa svängarna. Detta gjordes genom att först fylla ut den mottagna banan med ett antal punkter mellan varje par av punkter från den ursprungliga trajektorian. Därefter bildas för varje punkt ett medelvärde av kringliggande punkter, som då blir punktens nya position. På så vis erhålls en trajektoria med kurvor som är tillräckligt mjuka för att inte orsaka problem i regleringen. Denna filtrering sker innan B-spline-funktionaliteten körs och är gemensam kod för regulatorerna nedan. Huvudsyftet med denna funktionalitet är att även regulatorer som inte använder B-spline ska kunna reglera tillfredsställande på banan, till exempel PIDregulatorn som beskrivs nedan. 6.3 Dynamisk återkopplingslinjäriserad regulator Framtagningen av regulatorn som tas upp i den här sektionen gjordes i stort med utgångpunkt från [6] men även delvis [4]. Gaffeltrucken kan modelleras enligt ẋ cos α cos θ 0 ż θ = cos α sin θ sin α/l v 0 s + 0 u 2 = α 0 1 cos θ sin θ tan α/l 0 0 u u 2 1 där x och z är truckens position, v s är drivhjulets hastighet och u 2 är drivhjulets vinkelhastighet. Det är i princip samma modell som togs upp i Samma beteckningar vinklar och längder gäller här. Vi har att u 1 = v s cos α (7) u 2 = α. (8) Styrvinkeln α begränsas av α M (9) där M väljs som 0 < M < π/2. Det är för α inte ska vara exakt ±π/2 vilket leder till π division med noll i formlerna nedan. M valdes till ett värde ganska nära gränsen: 2.1. för att styrvinkeln inte ska vara nära extremvärden. Det är för att undvika att dela med noll och även undvika att dela med allt för små värden (relevant vid implementering i kod). Styrvinkeln kan därmed skrivas som α = M tanh w, (10)

24 Autonom truck 19 där w är en hjälpvariabel. Om variablen µ 2 introduceras enligt fås µ 2 = ẇ. (11) α = Mµ 2 sech 2 w (12) Modellerar man sedan gaffeltrucken med w istället för α fås nu ẋ ż θ = ẇ cos θ sin θ tan(m tanh w)/l 0 0 u µ 2. 1 Väljer vi sedan truckens position η = (x, z) T (i Unity koordinater rör trucken sig i xzplanet) som utsignalen vi vill styra, får vi att dess tidsderivata blir ( ) ( ) ( ) ẋ cos θ 0 u 1 η = =, ż sin θ 0 vilket inte är att styrbart system eftersom bara u 1 påverkar η. Vi går därför vidare med att derivera η tills vi får styrbarhet. För att göra detta definierar vi integratorerna µ 2 u 1 = ν 1, ν 1 = µ 1. (13) Den utökade modellen blir nu ẋ u 1 cos θ 0 0 ż u 1 sin θ 0 0 θ ẇ = u 1 tan(m tanh w)/l µ µ 2. u 1 ν ν Fortsätter vi derivera η får vi nu ( ) ν 1 cos(θ) u 2 1 tan(m tanh(w)) sin(θ)/l η = ν 1 sin(θ) + u 2 1 tan(m tanh(w)) cos(θ)/l vilket fortfarande inte är linjäriserbart eftersom µ 1 och µ 2 inte ingår. Ytterligare en derivering ger ( ) 1... u η 1 = l (3ν 1 sin(θ) tan(m tanh(w)) + u l cos(θ) tan2 (M tanh(w))) 1 u 1 l (3ν 1 cos(θ) tan(m tanh(w)) u l sin(θ) + tan2 (M tanh(w))) ( ) ( ) cos(θ) u 2 1 M l sin(θ)sec 2 (M tanh(w))sech 2 (w) µ 1 sin(θ) u 2 1 M l cos(θ)sec 2 (M tanh(w))sech 2, (14) (w) vilket är linjäriserbart eftersom µ kan sättas till µ 2

25 Autonom truck 20 ( µ 1 µ 2 ) ( ) 1 ( cos(θ) u 2 1 M = l sin(θ)sec 2 (M tanh(w))sech 2 (w) sin(θ) u 2 1 M l cos(θ)sec 2 (M tanh(w))sech 2 (w) ( ) 1 cos(θ) u 2 1 M l sin(θ)sec 2 (M tanh(w))sech 2 (w) sin(θ) u 2 1 M l cos(θ)sec 2 (M tanh(w))sech 2 (w) ( u 1 1 l (3ν 1 sin(θ) tan(m tanh(w)) + u l cos(θ) tan2 (M tanh(w))) u 1 1 l (3ν 1 cos(θ) tan(m tanh(w)) u l sin(θ) tan2 (M tanh(w))) r 1 r 2 ) ) (15) där r 1 och r 2 är referenser. Notera även att u 1 0 måste gälla. Detta får hanteras separat, det vill säga då trucken står stilla (styrvinkeln är strikt begränsad mellan π/2 och π/2). Insättning av (15) i (14) ger därmed att... η =... = ( r 1 r 2 ). (16) Det går alltså att styra trucken med avseende på tredje-derivatan av dess position genom att sätta r 1 och r 2. För att använda denna regulator krävs en tre gånger kontinuerligt deriverbar trajektoria. Om den önskade trajektorian betecknas med x d (t), z d (t) ska referenserna alltså sättas till r 1 =... x d och r 2 =... z d. Därefter fås u 1 från (13) och u 2 från (12). Slutligen fås styrvinkeln α från (10) och styrhjulets hastighet v s från (7). Angående θ kan inte denna storhet styras direkt. Regulatorn styr truckens två koordinater x och z med två styrsignaler α (styrhjulets vinkel) och v s (styrhjulets hastighet). När systemet insignal-utsignal-linjäriseras får θ en viss intern dynamik. Även w får intern dynamik. Den interna dynamiken blir vad den behöver vara för att systemet ska bli insignal-utsignal-linjäriserat och den har inte undersökts närmare. För att hantera modellfel, att trucken avviker från trajektorian samt att trucken initieras fel (till exempel fel position, eller hastighet) har följande återkoppling införts r 1 =... x d + k a (ẍ d ẍ) + k v (ẋ d ẋ) + k p (x d x) (17) r 2 =... z d + k a ( z d z) + k v (ż d ż) + k p (z d z) (18) där k a, k v och k p väljs så att polynomet λ 3 +k a λ 2 +k v λ+k p har rötter i vänstra halvplanet [4]. Dessa förstärkningar valdes så att polynomet kan skrivas som (λ 2 + 2ζωλ + ω 2 )(λ + a) (19) det vill säga så att det blir ett reellt nollställe i a och två komplexkonjugerande nollställen med dämpning ζ och vinkelhastighet ω. Olika värden på a, ζ och ω testades och ω = 2.75, ζ = och a = 4 gav tillfredsställande beteende. Det skulle kunna göras ytterligare tester här för att få bättre prestanda. Derivatorna ẍ, ẋ, z och ż uppskattas med hjälp av truckens modellekvationer och derivatorna av modellekvationerna B-spline Eftersom denna regulator kräver en trajektoria som är tre gånger deriverbar medan planeringsmodulen ger diskreta punkter och hastigheter behövs ett verktyg för att lösa detta. Verktyget som valdes heter B-spline.

26 Autonom truck 21 En B-spline defineras enligt [2] n P (t) = C i B i,d (t) (20) i=0 med basfunktioner B i,0 (t) = { 1, t i t < t i+1 0, annars (21) B i,d (t) = t t i B i,d 1 (t) + t i+d+1 t B i+1,d 1 (22) t i+d t i t i+d+1 t i+1 där d är graden på B-splinen, C i är kontrollpunkter och t 0, t 1,... är knutpunkter. Detta kan användas genom att planeringens punkter får utgöra kontrollpunkter och tiderna då trucken ska nå punkterna får vara knutpunkter (tiderna räknas fram med hjälp av hastigheterna). Kurvan som fås av B-splinen garanteras inte gå genom kontrollpunkterna, men den ligger i konvexa höljet till kontrollpunkterna [5]. Om graden på splinen sätts tillräckligt låg kommer alltså endast ett fåtal kontrollpunkter att vara relevanta i ett visst knutintervall när splinen beräknas. Splinen kommer därmed inte att avvika så mycket från den trajektoria som fås genom att linjärinterpolera planeringspunkter, såvida det inte är en skarp kurva. Om det är en skarp kurva gör B-splinen att banan blir något mjukare, vilket är önskvärt. Notera att det skulle vara möjligt att få B-splinen att gå genom planeringens punkter, säg q 0, q 1,... vid vissa tidpunkter t 0, t 1,... genom att ställa upp ett ekvationssystem och lösa ut kontrollpunkterna. Det vill säga att ur q k = P (t k ) = n C i B i,d (t k ), k = 0, 1,... i=0 lösa ut C i. Att lösa detta ekvationssystem skulle utgöra extra svårigheter och beräkningsarbete. Det skulle underlättats av att knutpunkterna var likformigt fördelade, vilket de inte är i detta fall eftersom hastigheten kan variera. För en B-spline av grad d gäller det att om följden av knutpunkter inte innehåller någon multiplictet så är kurvans d 1 stycken första derivator kontinuerliga över knutpunkterna [2]. Eftersom den aktuella regulatorn kräver en trajektoria med kontinuerliga derivatorer upp till grad tre så ska d i det fallet vara minst fyra. Derivatorna för B-splinen beräknas enligt [7] med C (k) i = n k P (k) (t) = C (k) i B i,d k (t) (23) i=0 C i k = 0 (d k+1)(c (k 1) i+1 C (k 1) i ) t i+d+1 t i+k k > 0 (24) På intervallet [t i, t i+1 ) är det som mest basfunktionerna B i d,d, B i d+1,d,..., B i,d som är nollskilda, vilket ses om rekursion följs med (21) och (22). Vidare går det att konstatera att basfunktionen B i,d beror på knutpunkterna t i, t i+1,..., t i+1+d. Givet dessa två fakta valdes det att B-spline får ha d + 1 stycken kontrollpunkter P 0, P 1,..., P d och 2d + 2 stycken knutpunkter t 0, t 1,..., t 2d+1. Detta fönster av kontrollpunkter och knutpunkter flyttas varefter trucken följer trajektorian.

27 Autonom truck 22 Det är inte helt entydigt hur planeringens punkter och de tidpunkter som fås (beräknas från hastigheterna) ska matchas mot B-splinens kontrollpunkter och knutpunkter. Om planeringen ger att en punkt q i ska passeras vid tidpunkten t i valdes för enkelhetens skull att q i får vara kontrollpunkt P i och t i får vara knutpunkt. Eftersom B-splinen kräver kontroll- och knutpunkter i en omgivning av det intervall som evalueras gäller det att i början och slutet av trajektorian se till att tillräckligt med kontrollpunkter och knutpunkter finns. I början av trajektorian valdes det att sätta kontrollpunkterna bakåt i tiden innan start samt nuvarande kontrollpunkt till trajektorians startpunkt. Låt trajektorians tid starta vid tiden noll, då sattes knutpunkter bakåt i tiden till några negativa tal. Valet av knutpunkter och kontrollpunkter i slutet av trajektorian har inte helt löst ännu. Där bör punkter läggas till på ett smart sätt så att trucken åker exakt till sista punkten på trajektorian Implementation Det huvudsakliga steget för att implementera regulatorn i kod var att tidsdiskretisera alla tidskontinuerliga derivator. Detta gjordes genom att för en funktion f(t) ersätta dess derivata f(t) med (f(t) f(t T s ))/T s där T s är tiden till föregående beräkning och f(t T s ) är värdet av funktionen vid denna tidpunkt. När regulatorkoden körs i Unity är T s lika med Fixed Timestep som har beskrivits tidigare. Om reglering körs utanför Unity får denna tid väljas till ett fixerat värde eller ställas in igen. Denna funktionalitet har dock inte implementerats då det inte har gått att använda regleringen utanför Unity. 6.4 PID-regulator Styrsignalen för PID-regulatorn är vinkeln på styrhjulet. För PID-regulatorn beror styrsignalen av tre delar; en proportionerlig del, en deriverande del och en integrerande del. Reglerfelet består i detta fall av skillnaden mellan truckens vinkel och vinkeln mellan trucken och en punkt på trajektorian framför trucken. Avståndet till denna punkt anges i antal punkter framåt på trajektorian och kan enkelt ändras. Den proportionerliga delen kommer då att vara reglerfelet multiplicerat med en reglerparameter K. För integraldelen används parametern T I och för derivatadelen T D. Styrvinkeln α sätts därför enligt α(t) = K(e(t) + 1 t d e(τ) dτ + T D e(t)) (25) T I t o dt Implementeringen av PID-strukturen görs i diskret form enligt där T s är sampeltiden. I = I n + K T s e/t I (26) α = K e + I + K T D (e e n )/T s (27) I n = I (28) e n = e (29) Valet av styrhjulets hastighet sattes direkt med hjälp av den hastighet som kommer från planeringen. I simuleringen och för tester stängdes styrvinkeln α in i intervallet π/4 < α < π/4.

28 Autonom truck 23 Figur 10: Vinkelfelet som PID-regulatorn använder. 6.5 Resultat I figur 11 och figur 12 har den dynamiskt återkopplingslinjäriserade regulatorn jämförts med PID-regulatorn. Notera dock att med PID-regulatorn åker trucken något långsammare än med den andra regulatorn. Trots detta är den dynamiskt återkopplings-linjäriserade regulatorn överlägsen PID-regulatorn vad gäller att följa trajektorian. Det maximala ortogonala felet med PID-regulatorn ligger på ungefär 3.5 mm. Motsvarande siffra för den dynamiskt återkopplingslinjäriserade regulatorn är 5 cm. Den dynamiskt återkopplingslinjäriserade regulatorn verkar alltså fungera mycket bra. Anledningen till att PID-regleringen gör att trucken får en lägre fart är för att PID-regulatorn inte är perfekt inställd. Den gör att styrvinkeln ändras lite mer fram och tillbaka kring 0 vilket gör att trucken svänger lite mer kring banan. Fördelen med att använda Unitys wheel collider -modell som innefattar dynamik som fram- och sidofriktion, dämpning och fjädring i hjulen är att den skiljer sig mycket från den kinematiska modell som regulatorn använder sig av. Att regulatorn fungerar så pass bra som den gör är alltså inte på grund av perfekt framkoppling, utan på grund av att framkopplingen och återkopplingen samverkar bra. Dock har inga störningar eller brus använts för att få just dessa resultat. I figur 14 visas hur bra den dynamiskt återkopplingslinjäriserade regulatorn följer angiven hastighet. Det visade sig att regulatorn presterar bäst om farten är någorlunda konstant. För att åstadkomma detta implementerades att trucken bromsar om det behövs när den ska minska sin fart. Detta förbättrade regleringen och tog bort tidigare oscillationer som uppstod kring trajektorian. I figur 15 och 16 visas även truckens position, hastighet och acceleration i x- och z-led över tiden jämfört med motsvarande B-spline-data.

29 Autonom truck 24 Figur 11: Punkterna trucken ska följa ges av Planned trajectory, Angle PID Controller är PID-regulatorn och slutligen så är Feed Forward Controller dynamiskt återkopplingslinjäriserade regulatorn. Figur 12: Inzoomade områden från figur 11.

30 Autonom truck 25 Figur 13: Felet, det vill säga, avståndet från trucken till närmaste planerade punkten på trajektorian för de två regulatorerna. Den dynamiskt återkopplingslinjäriserade regulatorn når ett maxfel på endast 3.5 mm medans PID-regulatorns maximala fel är cirka 5 cm. Figur 14: Truckens fart är den estimerade fart som trucken förmodas ha utifrån modellekvationerna, Fart från B-spline är den fart som fås då B-splinen skapas av trajektorian som består av en önskad väg och Önskad fart.