Teknisk dokumentation Racetrack 2015

Transkript

1 Teknisk dokumentation Racetrack 2015 Version 1.0 Författare: Jonathan Stenström Datum: November 2015 Status Granskad IK, PK, LK Godkänd

2 Projektidentitet Grupp Hemsida: Beställare: Kund: Examinator: Projektledare: Handledare: Oskar Ljungqvist, Reglerteknik, Linköpings universitet Daniel Axehill, Reglerteknik, Linköpings universitet Telefon: , Daniel Axehill, Reglerteknik, Linköpings universitet Telefon: , Ingrid Kugelberg Kristoffer Lundahl, Fordonssystem, Linköpings universitet Telefon: , Niclas Evestedt, Reglerteknik, Linköpings universitet Projektmedlemmar Ingrid Kugelberg Projektledare Jonathan Stenström Dokumentansvarig Oskar Nordmark Mjukvaruansvarig Henrik Bäckman Testansvarig Lina Karlsson Designansvarig Patrik Nyberg Komponentansvarig Planerare Olle Holmer Komponentansvarig Regulatorsystem

3 Dokumenthistorik Version Datum Gjorda ändringar Signatur Granskare Första utkastet Projektgruppen LK Andra utkastet Projektgruppen Projektgruppen Tredje utkastet Projektgruppen Projektgruppen Fjärde utkastet Projektgruppen IK, ON Femte utkastet Projektgruppen IK, ON, LK Första versionen Projektgruppen IK, PK, LK

4 Innehåll 1 Inledning Definitioner Bakgrund Syfte och Mål Systemöversikt Hårdvara Mjukvara Delsystem: Regulatorsystem för bil Gränssnitt Modell Parameterskattning Val av regulatorparametrar Delsystem: Regulatorsystem för lastbil Generering av referens Pure-pursuit regulator Delsystem: Planerare Gränssnitt Terminal manifold Generering av trajektorier Undvika hinder och kollision Approximation med cirklar Kollisionsdetektering med rutnät Optimeringsproblemet Transformation till globalt koordinatsystem Delsystem: Simuleringsmiljö Miljön för bilen Simuleringsmotorn Kommunikation med V-REP Rörligt hinder Delsystem: Visualisering 23 8 Resultat Regulatorsystem för bil Regulatorsystem för lastbil Planerare Simuleringsmiljö Vidareutveckling och framtida arbete Regulator för bil Regulator för lastbil Planerare

5 9.4 Simuleringsmiljö Referenser 30

6 Racetrack 1 1 Inledning Detta dokument beskriver det system som har implementerats på det, under flera år, utvecklade systemet LiU Racetrack. I stora drag är LiU Racetrack ett system som autonomt kan kontrollera radiostydra bilar och lastbilar. 1.1 Definitioner För att visualisera vissa delsystem kommer flödesscheman användas. I figur 1 förklaras vad de olika symbolerna som används i flödesscheman betyder. Figur 1: Förklaring av symboler som används i dokumentet. Här definierar vi begrepp och förkortningar som används i dokumentet. API: application programming interface. CDIO-projekt: Projektkurs omfattande 12 högskolepoäng som ges av ISY. GUI: graphical user interface. Hinder: Objekt i körbanan som markerar ej tillåten körväg för bilen. Hinder kan vara virtuella eller fysiska. IMM: Interacting Multiple Model. ISY: Institutionen för Systemteknik. LiU: Linköpings Universitet. Omkörning: En avvikelse från förberäknad referesspår för att undvika kollision med hinder eller fordon. Planerare: Algoritm för att generera referensspår för bilen. Trajektoria: Beräknad körväg med tidsstämplar som genereras av planerare eller referensgenereringssystemet. V-REP: Virtual Robot Experimentation Platform

7 Racetrack Bakgrund Utveckling av algoritmer för styrning av autonoma fordon är ett mycket aktuellt ämne inom både forskning och fordonsindustrin. För att kunna bedriva forskning inom detta område byggde Avdelningen för Reglerteknik, år 2011, upp en anläggning bestående av en bilbana med radiostyrda bilar, IR-kameror och målföljningsalgoritmer sammankopplade via en dator. Anläggningen har sedan utvecklats under fyra CDIO-projekt och två sommarprojekt. Vid starten av detta projekt fanns ett system som klarade av att köra de radiostyrda bilarna autonomt runt bilbanan. Styrningen av bilarna sköttes av en banföljande regulator som bestod av fem stycken PID-regulatorer med framkoppling. Denna banföljande regulator krävde information om bilarnas tillstånd, såsom position och hastighet, vilket uppskattas av ett målföljningssystem som använder sig av två IR-kameror monterade i taket ovanför bilbanan. Det fanns även en radiostyrd lastbil som kunde manövreras runt banan via piltangenterna på datorn. Till systemet fanns också en projektor kopplat till datorn som kunde visualisera information genom att projicera den direkt på bilbanan. Bland annat kunde bilarnas önskade referensbana och varvtid visas upp via detta. För att förenkla systemet så behandlas de olika delarna som nämnts ovan som delsystem. Datorn som kopplar ihop alla delsystem har även ett grafiskt användargränssnitt, där användaren dels har möjlighet att göra vissa inställningar och dels kan se information om till exempel fordonens tillstånd. Mer information om tidigare års system finns att läsa i föregående års tekniska dokumentation [1]. 1.3 Syfte och Mål Syftet med årets projekt är att vidareutveckla det befintliga systemet. Målen med projektet kan delas upp dels i generella mål och dels i mål för varje delsystem. Målen för årets projekt beskrivs i listan nedan. Efter avslutat projekt ska systemet vara robust och stabilt. Vidareutveckla hård- och mjukvara för att kvalitetssäkra det befintliga systemet. Utveckla och implementera en ny regulator för banföljning av radiostyrda bilar. Utveckla och implementera algoritmer för omkörning av andra fordon och hinder. Utveckla och implementera regulator för banföljning av lastbil med släpvagn. Utvekckla ett nytt simuleringssystem för testning av algoritmer och regulatorer.

8 Racetrack 3 2 Systemöversikt Här nedan följer en översiktlig beskrivning av systemet. Först är en överblick, därefter beskrivs hård- och mjukvara. Figur 2: Kommunikationsflöden för hårdvaran Bilen kör numera runt banan med en regulator baserad på Lyapunov-teori och som används för att kunna följa en given trajektoria. Den trajektorian följs så länge som inget hinder är detekterat, men om det händer ger planeraren ut en ny trajektoria som, med hjälp av en kollisionskontroll, undviker kollision. Den trajektoria som väljs är den som beräknas vara minst ryckig, alltså vara minimerad med avseende på förändring i acceleration. Ett lämpligt hinder för omkörning är lastbilar, gärna körandes. Därför har en Pure-pursuit regulator implementerats för att efterlikna ett verkligare och mer repetitivt scenario än med manuell styrning. För att underlätta utvecklingen har ett simuleringssystem tagits fram, som i grunden är byggt utifrån programmet V-REP. Med hjälp utav simuleringssystemet kan kod som ska användas på hårdvaran testas i en teoretisk miljö utan hårdvarudefekter. I figur 2 kan man se banan tillsammans med bil och lastbil. 2.1 Hårdvara Vid projektets start fanns följande hårdvara: Bilbana Tre radiostyrda bilar av modell Kyosho dnano Radiostyrd lastbil, innefattande dragbil, dolly och släp Handkontroller till bilar och lastbil Två IR-kameror Projektor PC: rt-pc-03 Styrkort NI PCI-6024E Kopplingsplatta, BNC-2110, med tillhörande kablar Raspberry Pi B+ med tillbehöor SparkFun I2C D/A-omvandlare - MCP4725

9 Racetrack 4 Microchip I2C D/A-omvandlare - TC1321EOA Extra nätverkskort i rt-pc-03 Nätverksswitch med ett antal ethernetsladdar Den hårdvara som lagts till under projetet är Radiostyrd bil av modell Kyosho Mini-Z Beslut att köpa in en ny bil togs på grund av att bilarna av modell Kyosho dnano enkelt slirar då hastigheten ökar. Då det var av intresse att köra i en högre hastighet än vad dnano-bilarna verkade klara av beslutades det att köpa in en ny bil av modell Kyosho Mini-Z. Denna modell var större och tyngre och bedömdes vara mindre benägen att slira. Efter tester visade det sig att den nya bilen hade problem att köra i låga hastigheter vilket gjorde att beslut togs att återgå till bilarna av modell Kyosho dnano. I figur 3 finns en översikt av kommunikationsflöden för hårdvaran. Figur 3: Kommunikationsflöden för hårdvaran 2.2 Mjukvara All mjukvara är utvecklad i Visual Studio i en Windowsmiljö. Under 2015 har versionen av Visual Studio uppgraderats och projektet använder nu 2015 års version. Programmet körs i flera parallella trådar, se följande lista för en kort beskrivning av hierarkin och vad varje tråd ansvarar för. OSAAR Huvudtråden för programmet. Skapar och sköter det grundläggande fönster som dyker upp när programmet startas.

10 Racetrack 5 Racetrack Barn till OSAAR. Ansvarar för målföljningssystem och kör IMM-filteret för att följa bilar och lastbilar. Tråden körs i 100Hz vilket är uppdateringfrekvensen för kamerorna. Regulator Barn till Racetrack. Kör regulator för bil och lastbil varje gång ny data finns tillgängligt i Racetrack-tråden. DrawThread Barn till Racetrack, tråd som hanterar utritning på skärm och via projektor. PlannerThread Barn till Racetrack, tråd för planering. Kör planeringaren med 5 Hz och skickar sedan ny trajektoria till regulatorn. ColissionThread Barn till PlannerThread, tråd för kollisionskontroll. Hjälper planeraren för att göra den mer effektiv. Observera att varje gång programmet startas görs en kalibrering av rotation och skalning för att få skarven mellan kamerorna att stämma överens. Detta görs med två punkter på ramen till banan. Det är viktigt att båda syns och att inga andra bitar av reflextejp finns nära dessa punkter. Om något av detta sker kommer programmet inte veta vilka punkter det ska välja och kalibreringen misslyckas, varpå programmet inte kommer gå att köra.

11 Racetrack 6 3 Delsystem: Regulatorsystem för bil Delsystemet Regulator för bil syftar till det system som gör att bilen följer den trajektoria som planeringsystemet genererat. Regulatorn som är implementerad till bilen finns beskriven i [2] och bygger på en kinematisk cykelmodell av en bil. 3.1 Gränssnitt Från planeraren kommer bilens regulator att få framräknade trajektorier att följa och från målföljningssystemet kommer regulatorn få bilens nuvarande position. Dessa signaler används sedan för att beräkna styrsignaler för fordonet. Signalerna till och från regulatorn finns beskrivna i tabell 1. Tabell 1: Tabell över de signaler som rör bilens regulator Signaler från planerare y d1, y d2 v d ψ d κ d Signal från målföljningssystemet x 1, x 2 Signaler för reglering av bil δ F Beskrivning Den önskade positionen för bilen. Bilens önskade hastighet. Bilens önskade girvinkel. Den önskade kurvaturen för bilen. Beskrivning Bilens faktiska position. Beskrivning. Styrsignal för bilens styrvinkel. Styrsignal för kraften som läggs på bakaxeln. 3.2 Modell Tillstånden för bilen finns presenterade i figur 4. Styrsignalerna är styrvinkeln, δ, och kraften på de bakre hjulen, F. Styrsignalerna har begränsningarna δ δ max π 6 rad samt F 1. Med dessa tillstånd och under förutsättningarna att beskrivna begränsningar är uppfyllda kan en kinematisk modell, där inget slip antas, beskrivas enligt ẋ 1 v cos(ψ) v sin(ψ) ẋ 2 ψ = v v tan(δ) l Av + BF (1) Jämfört med modellen i [2] är modellen här modifierad för att passa det aktuella systemet. De tre första ekvationerna är identiska med modellen i [2] och finns även härledda i [3]. Den sista ekvationen är ny och härstammar från Newtons andra lag. A och B är parametrar specifika för det aktuella systemet och hur dessa har bestämts beskrivs senare i kapitlet. För att möjliggöra reglering av bilen måste styrlagar tas fram. Modell (1) förenklas genom att införa de virtuella insignalerna w 1 och w 2. w 1 := κ δ (2) w 2 := v = Av + BF = F = 1 B (w 2 + Av) (3) Då den önskade positionen och bilens position inte alltid kommer att sammanfalla kommer det alltid finnas vissa reglerfel. Dessa är e t, som beskriver felet i longitudinellt led, e n,

12 Racetrack 7 Figur 4: Cykelmodell av en bil. De olika parametrarna i figuren är: [x 1, x 2 ], som avser positionen för bilen i globala koordinater, v avser bilens hastighet, F avser kraften på de bakre hjulen, ψ är bilens girvinkel och δ är bilens styrvinkel. Förutom dessa finns avstånden l som är axelavståndet samt λ som är avståndet från bilens centrum till bakre axeln. Figuren är hämtad från [2] som beskriver felet i lateralt led samt e ψ, som beskriver skillnaden mellan den önskade girvinkeln och den girvinkel bilen står i. Den diffeomorphiska transformationen: [ ] [ ] e t cos(ψd ) sin(ψ d ) y1 y 1d e n e ψ v = sin(ψ d ) cos(ψ d ) y 2 y 2d ψ ψ d v tan(δ) κ δ l transformerar dessa reglerfel till en så kallad Frenet frame, se figur 5. Transformationen ger, enligt [2], det slutna systemet invarianta egenskaper. Om de fyra första sambanden i transformationen deriveras erhålls systemet (4) ė t = v cos(e ψ ) v d [1 κ d e n ] ė n = v sin(e ψ ) v d κ d e n ė ψ = vκ δ v d κ d v = w 2 (5a) (5b) (5c) (5d) Med hjälp av Lyapunovteori, se [2], tas sedan två styrlagar fram [ ] cos(e ψ 1) sin(e ψ ) w 1 = κ d k 1 e t + e n ζk 2 e ψ (6) e ψ e ψ w 2 = v d k 1 e t k 3 e v + ζk 2 e 2 ψ e ψ κ d (7)

13 Racetrack 8 Figur 5: En illustration av en Frenet frame. Figuren är hämtad från [2]. där ζ är 1 om bilen ska köra framåt och 1 om bilen ska köra bakåt och e v := v v d och designparametrarna k 1, k 2, k 3 > 0. Med styrlagarna (6) och (7) har [2] visat att det slutna systemet är asymptotiskt stabilt om trajektorian är tillräckligt jämn och tar hänsyn till begränsningen av styrsignalen. Slutligen måste de virtuella signalerna w 1 och w 2 transformeras tillbaka till styrsignalerna δ och F. Med κ δ = w 1 och sista sambandet i (4) erhålls styrsignalen δ. Styrsignalen F fås enligt (3) där w 2 ges av (7). δ = arctan(w 1 l) (8) F = 1 B (w 2 + Av) (9) De två styrsignalerna δ och F måste anpassas till det verkliga systemet innan de kan användas. För det verkliga systemet kommer därför styrsignalerna och u s, för bilens styrvinkel, och u g, för bilens gaspådrag, att användas. Förhållandena mellan δ och u s samt mellan F och u g var okänt men har bestäms genom experiment som beskrivs i avsnitt Parameterskattning Tester har visat att u s är proportionell mot δ men med olika koefficienter beroende på tecknet på δ. Genom att låta bilen köra med konstant u s och notera radien r på den cirkel som bilen kör i, samt använda att kan följande samband erhållas κ = 1 r = tan δ l u s = = δ = arctan l r { δ, δ δ, δ > 0 (10) (11) Laplacetransformering av (1) ger v = B s + A F (12)

14 Racetrack 9 2 Hastighet Tid [s] 0.3 Gaspådrag, u g Tid [s] Figur 6: Hastighet då u g ökas långsamt. vilket är ett första ordningens system med tidskonstant 1 A samt förstärkning B A. A och B kan alltså bestämmas utifrån tidskonstanten och förstärkningen hos systemet. I figur 6 visas hastigheten då u g ökats långsamt. För u g < är v = 0 och efter det ökar hastigheten linjärt, detta ger { 0 u g < α F = (13) u g α u g α där α = i det här fallet. Eftersom u g ökas långsamt befinner sig systemet approximativt i stationärt tillstånd och v = 4F verkar gälla. Detta ger att B A = 4. I figur 7 visas hastigheten då u g var ett steg. Som man kan se ser förhållandet mellan u g och hastighet ut som ett första ordningens system med tidskonstant s vilket ger A = Vi har alltså A = { A = B A = 4 = (14) B = Val av regulatorparametrar För att göra lateralt samt longitudinellt fel så litet som möjlig har k 1 gjorts så stor som möjligt och k 2 och k 3 har valts för att tillåta detta. I figur 8 visas fel från körningar med olika värden på k 2, som man kan se ger både små och stora värden ett oscillativt beteende. I figur 9 visas fel från körningar med olika värden på k 3. Som man kan se ger både små och stora värden ett oscillativt beteende samt stora värden gör att felen konvergerar långsamt eftersom hastigheten tvingas följa sin referens hårdare. I figur 10 visas fel från körningar med olika värden på alla parametrar, som man kan se ger små värden ett oscillativt beteende och stora värden ett mer nervöst och brusliknande beteende.

15 Racetrack v [m/s 2 ] Tid [s] Figur 7: Hastighet då u g var ett steg vid tiden 0 s. Stationära nivåer före och efter steget, nivån då 63% av slutvärdet uppnåtts samt tiden när detta inträffar är markerade med streckade linjer. Även de skattade parametrarna har justerats lite för att ge bättre prestanda, de skattade värdena på A och B gav ett ryckigt betende, genom att minska A och öka B blev prestandan bättre. Även koefficienterna i (11) ökades lite då bilen svängde för lite med de ursprungliga i högre hastigheter.

16 Racetrack k 2 = k 2 = k 2 = Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel Tid [s] Tid [s] Tid [s] Figur 8: Fel vid körning runt banan för några olika värden på k 2 med k 1 = 35 och k 3 = 13. Bilen startar alltid i ungefär samma position. För k 3 = 1 och 30 kolliderar bilen efter ungefär 10 s. 0.2 k 3 = k 3 = k 3 = Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel Tid [s] Tid [s] Tid [s] Figur 9: Fel vid körning runt banan för några olika värden på k 3 med k 1 = 35 och k 2 = 8. Bilen startar alltid i ungefär samma position. 0.2 a = a = a = a = Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel 0.15 Longitudinellt fel Lateralt fel Tid [s] Tid [s] Tid [s] Tid [s] Figur 10: Fel vid körning runt banan när k 1 = 35a, k 2 = 8a samt k 3 = 13a för några olika värden på a. Bilen startar alltid i ungefär samma position.

17 Racetrack 12 4 Delsystem: Regulatorsystem för lastbil Lastbilens regulatorsystem har som uppgift att autonomt köra lastbilen runt banan efter en given referens. Regulatorsystemet består av en Pure-pursuit regulator som beräknar ut vilken styrvinkel lastbilen bör hålla för att komma till en önskad punkt, så kallad waypoint. I systemet som helhet är lastbilens främsta uppgift att agera som statiskt och rörligt hinder hinder för bilarna. 4.1 Generering av referens Generering av referens görs via en simulering i Simulink och Matlab. Först ritas en ortogonal referens gjord på endast punkter i samband med varje sväng. Detta gör att referensen enbart består av 90- och 180-gradiga svängar, se figur 11. Därefter körs en simulering med den referensen, och punkterna för den simulerade körda sträckan sparas undan och används som den slutgiltiga referensen, se figur 11. Figur 11: Figuren till höger visar lastbilens ortogonala referens och till vänster visas lastbilens simulerade, slutgiltiga, referens. De blå linjerna illustrerar referensen och de gröna linjerna illustrerar banan. När referensen sedan testas kan det visa sig att den genererade referensen inte klarar av att användas för att köra igenom banan utan att lastbilen slår i väggar. Detta innebär att referensen måste justeras, och detta görs lämpligen genom att korrigera den ortogonala referensen. Referensen får sedan testas tills den anses bra nog av användaren. För ytterligare information, se [4]. 4.2 Pure-pursuit regulator Lastbilen regleras genom banan med hjälp av en Pure-pursuit regulator. Denna regulator bygger på att regulatorn plockar ut den punkten ur en given referensbana som ligger på ett bestämt avstånd R, så kallad look-ahead-radie, från fordonet som ska regleras. Regulatorn kommer sedan att beräkna vilken styrvinkel som ska ställas ut för att fordonet ska nå denna punkt. Se geometrin som används i figur 12. I listan nedan beskrivs algoritmen mer utförligt.

18 Racetrack Bestäm fordonets position. 2. Hitta den punkt, way-point, i referensen som ligger närmast utanför fordonets lookahead-radie och sätt den punkten till nuvarande way-point. 3. Hitta skärningen mellan den tänkta linjen mellan den nuvarande way-point:en och den föregående way-point:en och fordonets look-ahead-radie. Detta blir den punkt som lastbilen ska reglera till. 4. Beräkna styrvinkeln. 5. Uppdatera fordonets position och gå tillbaka till 1. Figur 12: Geometrin som används för att ta fram styrlagen till Pure-pursuit regulatorn. Figuren är hämtad från [3]. I [3] finns en utförlig beskrivning av hur styrlagen för Pure-pursuit regulatorn tas fram. Resultatet är att styrvinkeln α beräknas enligt ( ) L α = arctan R där L är fordonets längd och r radien på den cirkelbåge som fordonet kommer följa till punkten. Radien r beräknas enligt r = R 2 sin (θ e ) där θ e är skillnaden mellan fordonets girvinkel och den girvinkel fordonet bör ha för att nå punkten som ska regleras till. Sammantaget leder detta alltså till styrlagen: ( ) 2L sin (θe ) α = arctan R Look-ahead-radien R, är Pure-pursuit regulatorns enda justerbara parameter. Ett högt värde på denna parameter ger ett mjukt körsätt, men en stor avvikelse från referensbanan. Ett lägre värde följer bättre, men kan ge ett oscillativt beteende, eller till och med leda till instabilitet. Vilken look-ahead-radie som är mest lämplig beror också på vilken hastighet fordonet har. Generellt kräver en högre hastighet en längre look-ahead-radie.

19 Racetrack 14 Look-ahead-radien bestämdes i praktiken genom att testa olika radier på det fysiska systemet, med stycket ovan i åtanke. Resultatet av detta var att en look-ahead-radie på 0.2 meter var rimligt för den valda hastigheten 0.4 (En hastighet på -1 representerar maxhastighet bakåt och en hastighet på 1 representerar maxhastighet framåt).

20 Racetrack 15 5 Delsystem: Planerare Planerarens uppgift är att beräkna en trajektoria för bilen att följa. Detta ska göras kontinuerligt och ska möjliggöra för att olika hinder, som kan finnas på bilens referenstrajektoria, ska kunna undvikas. Hindren undviks antingen genom att bilen gör en omkörning eller att den stannar. För att kunna göra detta kommer en algoritm enligt [5] att implementeras. Denna bygger på att imitera en människas körsätt så likt som möjligt. Detta görs genom att minimera förändring i acceleration, så kallad jerk. Minimeringen sker i både lateral och longitudinell led. Algoritmen kommer att arbeta enligt följande steg. Trajektorier kommer att genereras i enighet med bilens momentana position, hastighet och acceleration och de önskade slutvärden för dessa, kallat terminal manifold, se avsnitt 5.2. Därefter kommer samtliga trajektorier att kontrolleras för att se om de är körbara och inte ligger i kollision med bilbanans väggar eller andra fordon. Slutligen väljs den optimala trajektorian ut enligt en målfunktion och ställs ut till regulatorn för bilen. Ett flödesschema för hur algoritmen arbetar finns specificerat i figur Gränssnitt Planeraren kommer som referens behöva punkter i det globala koordinatsystemet enligt [x 1, x 2, ψ, v] på punkter som den vill att bilen ska följa. Efter att planeraren nu bestämt en väg bilen ska följa kommer den att skicka vägen som referens till regulatorn. Detta kommer ske på formen [x 1, x 2, ψ, κ d, v, a](t), där ψ är vinkel för bilen, κ d är kurvatur, v hastighet och a acceleration. 5.2 Terminal manifold Terminal manifold kallas det område där slutpunkter för vår planerare finns. Detta område delas in i diskreta punkter och till varje punkt beräknas en trajektoria, se avsnitt 5.3. Detta område bestämmer hur bilen kommer att agera på vägen. I ett första steg kommer området att ligga så att punkterna sprids i lateralt led en bit framför bilen. Om ingen av dessa visar sig vara körbar så kommer ett nytt område genereras där punkterna sprids både i lateralt och longitudinellt led för att möjliggöra en inbromsning eller fullt stopp för bilen, se figur 14.

21 Racetrack 16 Bestäm terminal manifold Skapa trajektorier Välj första trajektorian Kan bilen följa trajektorian? Ja Välj nästa trajektoria Nej Krockar trajektorian med något objekt? Nej Markera trajektorian som 'valid' Ja Markera trajektoria som 'invalid' Ja Finns det fler trajektorier att kontrollera? Nej Beräkna kostnadsfunktion för alla trajektorier markerade 'valid' Välj trajektoria med minst kostnad Figur 13: Flödesschema för hur planeraren kommer att arbeta

22 Racetrack Generering av trajektorier Vägen från en startpunkt till en slutpunkt ska ske med minimal jerk. Detta problem kan lösas analytiskt och ger då ett polynom som beskriver trajektorian. För detta införs beteckningar för lateralt led, d(t), och longitunell led, s(t), längs med trajektorian samt två integratorsystem ξ(t), ett för lateralt och ett för longitunell led, båda enligt ξ(t) = ξ(t) + 0 u(t) (15) Detta ger nu ett optimeringsproblem som: minimize u( ) subject to τ 1 h(ξ(t), t) u2 (t)dt ξ1 (t) = ξ 2 (t) ξ 2 (t) = ξ 3 (t) ξ 3 (t) = u(t) ξ(0) = [ξ 1 (0), ξ 2 (0), ξ 3 (0)] T ξ(τ) = [0, ξref (τ), ξref (τ)] T med u(t) =... ξ 1 (t) (16) h(ξ(t), t) := k t t k ξ 1 (ξ 1 (t) ξ ref (t)) 2 (17) där k t samt k ξ1 är designparametrar för att straffa långsam konvergens samt avvikelse från referenstrajektorian. Detta kan göras för lateral samt longitudinell led oberoende av varandra. Observera att termen k t t i vårt fall inte är nödvändig eftersom endast en tidshorisont används, den är dock nödvändig om algoritmen utökas. Lösningen till detta ges av: ξ(t) = M 1 (t)c M 3 (t)c 345 (18) där M 1 (t) := 1 t t t (19) t 3 t 4 t 5 M 2 (t) := 3t 4t 3 5t 4 (20) 6t 12t 2 20t k ξ1 M 3 (t) := M 2 (t) (21) c 012 = M 1 (0) 1 ξ(0) (22)

23 Racetrack 18 Kvintettillståndstrajektorian ges av c 345 = M 1 3 (τ)(ξ ref (τ) M 1 (τ)c 012 ) (23) ( c T 012 c T 345) = ( c0 c 1 c 2 c 3 c 4 c 5 ) (24) där c 012 och c 345 är koefficienter givna av ekvation (22) och (23). Först genereras trajektorier med konstant hastighet som referens. Detta gör att det nu endast finns värden på ṡ ref. Detta gör att den optimala trajektoria som ska följas istället beräknas enligt ξ(t) = M 1 (t)c M 3 (t)c 34 (25) Observera att i detta fall är c 5 = 0, samt med M 1 (t) och c 012 som beskrivet ovan. Detta gör nu att inget krav finns på ξ 1 (τ), vilket gör att en referens på slutgiltig hastighet oavsett position kan generas. Figur 14: Trajektorier genererade av planeraren spridda i lateralt led. Trajektorier som ej är följbara eller ligger i kollision har markerats med rött och den optimala trajektorian har markerats i grönt. Det kan hända att alla dessa trajektorier blir ogiltiga, detta om till exempel hela körbanan är blockerad. För att kunna hantera dessa situationer generars i detta fall också trajektorier med konstant position enligt ekvation 18. Detta kan nu göras med bilens nuvarande position som startposition till varje diskret punkt i terminal manifold för lateral rörelse, som sprids homogent framför bilen. 5.4 Undvika hinder och kollision För att undvika att bilen kolliderar med objekt eller att dessa trajektorier går att följa måste de undersökas för att se om de är giltiga eller inte. Detta görs först genom att

24 Racetrack 19 kontrollera kurvatur samt acceleration för hela trajektorian, så att bilen fysiskt kan följa trajektorian. För detta transformeras alla valda trajektorier till det globala koordinatsystemet. När dessa finns i det globala koordinatsystemet kan maximala värdet på acceleration samt kurvradie jämföras med bilens egenskaper för att avgöra om trajektorian går att följa eller inte. För att avgöra om en trajektoria går att följa utan att en kollision inträffar approximeras först alla objekt med cirklar som sedan markeras i ett rutnät. För att avgöra om kollison uppstår räcker det sedan att kontrollera om trajektorian går igenom markerat område i rutnätet. Detta beskrivs mer utförligt nedan Approximation med cirklar Ett objekt med bredd B och höjd H delas först upp i mindre rektanglar med bredd samt höjd H N H. Dessa mindre rektanglar kan approximeras med cirklar C i = (x i 1, x i 2, r), i = 1, 2,...N B N H där (x i 1, x i 2) är centrum för den rektangel som approximeras av cirkel i samt ( ) 2 ( ) 2 B H r = + (26) 2N B 2N H är cirklarnas, gemensamma, radie. Det största felet i approximationen inträffar på mitten av någon av rektanglarnas sidor alltså ( e = max r B, r H ) (27) 2N B 2N H N B och N H väljes därför så att e får önskad storlek. Ett exempel visas i figur 15. B N B Kollisionsdetektering med rutnät Först approximeras bilen med cirklar enligt avsnitt Området där kollisionskontroll ska utföras delas sedan upp i ett rutnät och de rutor som ett visst hinder ockuperar. För att underlätta markeringen av hinder approximeras de också med hjälp av cirklar som sedan är lättare att markera. När hindren markeras läggs även bilens radie till eftersom det då räcker att kontrollera att ingen av bilens cirklars centrum ligger i en ruta som är markerad för att avgöra kollision. Detta görs i olika rutnät för varje tidpunkt som kollisionskontroll ska göras och hindrens position skattas genom att anta konstant hastighet. Till höger i figur 15 har ett hinder markeras i ett rutnät, om en av bilens cirklars centrum skulle ligger i en röd ruta är bilen i kollision med hindret. Trajektorior testas genom att för varje tidpunkt i trajektorian kontrollera om någon av bilens cirklars centrum ligger i en markerad ruta i aktuell tidpunkts rutnät. 5.5 Optimeringsproblemet Efter att alla trajektorier har kontrollerats kan det finnas flera trajektorier som är möjliga att följa. Trajektorian med lägst kostnad väljs då ut som den vi vill följa, se figur 14. Kostnaden evalueras för både den laterala och longitunella rörelse som är associerad med den aktuella trajektorian. Dessa kostnader läggs sedan ihop för att få den totala kostanden.

25 Racetrack 20 Figur 15: Till vänster visas ett objekt som approximerats med cirklar och till höger visas samma objekt markerat i ett rutnät. 5.6 Transformation till globalt koordinatsystem För att transformera tillbaka från det definierade koordinatsystemet [s, ṡ, s, d, d, d](t) till det globala koordinatsystemet [x 1, x 2, ψ, κ d, v, a](t) har följande transformation gjorts x(s(t), d(t)) = r(s(t)) + d(t)n c (s(t)) (28) Hastighet och acceleration har beräknats med derivatan, respektive andragrads derivatan, av x(s(t), d(t)). Dessa har sedan slätats ut genom medelvärdesbilding över fyra närliggande värden. Kurvatur har beräknats genom att låta P 1, P 2, P 3 vara tre följande två-dimensionella punkter på trajektorian. Kurvaturen, κ, kan då beräknas enligt κ = 2 P 2 P 1 P 3 P 2 /( P 2 P 1 P 3 P 2 P 3 P 1 ) (29) Trajektorian, uttryckt i de globala parametrarna [x 1, x 2, ψ, κ d, v, a](t), ställs slutligen ut till regulatorn.

26 Racetrack 6 21 Delsystem: Simuleringsmiljo Simuleringsmiljo n som har utvecklats har gjorts i syfte att kunna testa o vrig mjukvaruutveckling. Fo r att skapa simulatorn har simuleringsprogrammet V-REP anva nds. V-REP a r gjort av fo retaget Coppelia Robotics och a r till fo r att kunna simulera olika dynamiska robotar i olika miljo er. 6.1 Miljo n fo r bilen I projektet har det byggts upp en simuleringsmiljo som a terspeglar den uppsa ttningen som finns i labbet, skalenligt och efterpassat att a terspegla samma koordinatssystem som det verkliga systemet anva nder sig av. I figur 16 syns miljo n som byggts upp. Bilen a r en fa rdig modell fra n V-REP och bygger pa att demonstrera Ackermannstyrning. Det stora gra a skiktet som syns runt banan a r golvet, det golvet beho vs fo r att bilen ska kunna ko ra. Anledningen till detta a r fo r att det som ser ut som banan gjort sa att ingen interaktion mellan banan och bilen sker. Bilen ko r alltsa endast pa det gra a golvet. De ho ga cylindrarna i varje ho rn a r utplacerade fo r att kunna matcha in banan till det koordinatsystem som finns i labbet och den runda skivan i nederkant markerar origo. Figur 16: Simuleringsmiljo n med bilen 6.2 Simuleringsmotorn Fo r att kunna kontrollera bilen har ett API byggts upp. Genom att ko ra den simuleringsmotorn som byggts upp i C++ skapas en exekverbar fil som V-REP kan hitta och ko ra. I den filen finns ocksa regulatorkoden implementerad vilket go r att samma kod som ko rs pa det verkliga systemet nu kan testas i simuleringsmiljo n Kommunikation med V-REP Data skickas till och fra n V-REP med hja lp av det fja rrstydra API som finns fo r C++ och a r dokumenterat pa websidan fo r V-REP [6]. Fo r att erha lla snabb uppdatering och Kurs: Projektgrupp: TSRT10: Projekt: Reglerteknisk projektkurs OTC TSRT10 Racetrack Dokumentansvarig: Fo rfattarens Dokumentnamn: racetrack15@googlegroups.com Jonathan Stenstro m jonst361@student.liu.se Teknisk dokumentation.pdf

27 Racetrack 22 o verfo ring av information fra n V-REP till va r simuleringsmotor i C++ sa anva nds en kommunikationslo sning da r V-REP kontinuerligt stro mmar data till en buffer som C++ programmet kan la sa fra n. Hur V-REP:s fja rrstyda API kan anva ndas fo r denna och andra kommunikationsmetoder finns ocksa beskrivet pa websidan [7]. 6.3 Ro rligt hinder Lastbilen a r representerad som en gra vmaskin i V-REP. Gra vmaskinen styrs endast genom V-REP och pa verkas inte av na got yttre program. Referensen laddas in genom en textfil i V-REP. Ett tra dat script i V-REP flyttar sedan runt gra vmaskinen kring den laddade referensen i given hastighet. Detta illustreras i figur 17. Figur 17: Simuleringsmiljo n med ro rligt hinder Kurs: Projektgrupp: TSRT10: Projekt: Reglerteknisk projektkurs OTC TSRT10 Racetrack Dokumentansvarig: Fo rfattarens Dokumentnamn: racetrack15@googlegroups.com Jonathan Stenstro m jonst361@student.liu.se Teknisk dokumentation.pdf

28 Racetrack 23 7 Delsystem: Visualisering Visualiserinsgssystemet har två huvuduppgifter. Dels hanterar den projektorn och den information som ska visas på banan samt det användargränssnittet som syns på datorn. Exempel på information som kan visas är fordonens trajektorier, fordonens körda bana och hinder. Visualiseringssystemet är uppbyggt på samma sätt som tidigare år men med några förändringar. I detta avsnitt kommer endast dessa förändringar att beskrivas och för utförligare beskrivning av hur visualiseringsystemet är uppbyggt hänvisas till tidigare års dokumentation [1]. GUI:t är uppbyggt av två huvudfönster, det är en kontrollpanel och ett main-fönster där fordonens referens och tillstånd med mera kan ritas ut. I detta års projekt har bilens regulator förändrats och bygger inte längre på PID-regulatorer och därför är det ej längre aktuellt att sätta dessa parametrar. Anledningen till att dessa parametrar ändå finns kvar är på grund av att den motsvarande programkoden finns kvar i bakgrunden. Kontrollpanelens utseende ses i figur 18. Figur 18: Kontrollpanelen från vilken systemet startas. Main fönstret ser ut på samma sätt och har samma funktioner som tidigare år, se figur 19. Ett tillägg har gjorts för att möjliggöra utritning av lastbilens referens. Implementation av detta har gjorts på samma sätt som utritningen av bilens referens. Lastbilens referens läses in tråden som sköter utritning för GUI:t och projektorn och som går att finna i draw.cpp. Det är möjligt att rita ut lastbilens referens för samma inställningar, så kallade displaymode s och projektormode s, som det går att rita ut bilens referens på. Se användarhandledningen [8] för vilka inställningar detta är. Funktionsanrop för utritning görs i MainDisplay.cpp för GUI:t och i ProjektorDisplay.cpp för projektorn. När det finns en trajektoria beräknad från planeraren kommer även denna att ritas ut. För utritning av trajektorian finns en ny funktion DrawOptTrajFromPlanner som går att finna i Display.cpp. Referensen kommer läsas in i draw.cpp och funktionsanrop görs sedan i MainDisplay.cpp för GUI:t och i ProjektorDisplay.cpp för projektorn.

29 Racetrack 24 Figur 19: Mainfönstret.

30 Racetrack 25 8 Resultat Här nedan presenteras de resultaten och den prestandan som har uppnåtts under projektet. Samtliga delsystem uppfyller de grundläggande kraven enligt kravspecifikationen [9], vilket finns dokumenterat i testprotokollet [10]. Regulatorerna reglerar enligt kravställd prestanda och klarar av detta under flera på varandra följande varv. Planeraren klarar med stor säkerhet av att undvika statiska hinder genom omkörning eller inbromsning. Då rörliga hinder förekommer klarar planeraren att köra om i många fall, men inte i alla. Samspelet mellan regulatorn för bilen och planeraren behöver förbättras, se kapitel 9.1 och 9.3. Vidare finns det fysikaliska systemet simulerat i V-REP. Bilen kan i simulatorn köras med samma regulator och den uppnår där mycket god noggrannhet för följning av referensbanan i lateralt led men ej i longitudinell led. Statiska och rörliga hinder finns implementerade i simuleringsmiljön men stöd för autonoma omkörningar är ej implementerat på grund av problem men hastighetsföljning, se kapitel 9.4. Slutligen kan visualiseringssystemet även rita ut lastbilens referens. 8.1 Regulatorsystem för bil I figur 20 visas longitudinellt och lateralt fel när bilen har kört fem varv med konstant hastighet 1 m/s. Parametrarna i regulatorn var k 1 = 35, k 2 = 8 samt k 3 = 13. Som man kan se ligger felen, efter ett kort insvängningsförlopp, alltid inom 4 cm. Det laterala felet är mindre än 2 cm under 98% av sträckan och det longitudinella felet är mindre än 2 cm under 89% av sträckan. Kraven i kravspecifikationen uppfylls därmed. Det brusiga utseendet på det longitudinella felet beror på att referensen består av diskreta punkter. Om hastigheten ökas blir systemet snabbt instabilt. För k 1 = 35, k 2 = 8 samt k 3 = 13 sker detta redan vid ungefär 1.2 m/s. Däremot kan värdena på k 1, k 2 och k 3 sänkas för att möjligöra högre hastigheter men detta görs på bekostnad av storleken på felen Lateralt fel Tid [s] Longitudinellt fel Tid [s] Figur 20: Longitudinellt och lateralt fel när bilen har kört fem varv med konstant hastighet 1 m/s. Röda linjer markerar fel inom 4 cm och gröna linjer markerar fel inom 2 cm.

31 Racetrack Regulatorsystem för lastbil Lastbilen kan väl följa sin referensbana under flera varv, se figur 21. Avvikelsen från referensbanan är med god marginal innanför det avståndet på 4 cm som kravspecifikatinen kräver, se figur 22. Körningen visad i figurerna är genomförda med en konstant hastighet på 40% av den maximala hastigheten som lastbilens interna styrning tillåter. Designparametern lookahead distance hade värdet 0.2 m vid körningen. Figur 21: Fem körda varv med lastbilen. Den blå linjen visar lastbildens körda väg och den röda linjen visar dess referens. Figur 22: Fem körda varv med lastbilen inzoomat på bansegmentet i det övre högra hörnet. Den blå linjen visar lastbildens körda väg och den röda linjen visar dess referens. Både designparametern och hastigheten valdes genom testning på det fysiska systemet för att uppnå önskat beteende på den här specifika banan. En annorlunda referens kan kräva andra konfigurationer för att uppnå tillräcklig prestanda.

32 Racetrack Planerare Planeraren klarar av att sprida trajektorier för konstant hastighet väl. Den klarar av att göra kollisionskontroll och kan sedan välja den bästa trajektorian att följa, enligt given kostnadsfunktion. Vidare klarar den sedan att skicka denna trajektoria till regulatorn. Detta klarar den av att göra i 5 Hz. Beräkningstiden för planeraren är kortatre än så men för att få ett repeterbart beteende görs det alltid i med samma frekvens. Detta gör att planereren som helhelt klarar av att undvika statiska hinder utan problem. Observera att en kortare planeringstid, på ungefär s, är fördelaktigt när statiska hinder ska undvikas. Den klarar även av att i vissa fall genomföra omkörning av hinder i rörelse. Här är det fördelaktigt med en längre planeringstid, på ungefär 2 s. Se avsnitt 9.3, om vidareutveckling av planeraren, för att få mer information om vad som behöver göras för att detta ska fungera bra. 8.4 Simuleringsmiljö Det verkliga systemet finns återskapat i en simuleringsmijö för att köras i programmet V-REP. Modellen i simuleringsmiljön har samma avstånd och storlekar på bil och bilbana som det verkliga fysikaliska systemet. Till detta har det skapats en simuleringsmotor som fungerar som ett fjärrstyrt API till V-REP. Simuleringsmotorn ligger som ett eget projekt i koden men använder sig av koden som är skriven för det verkliga systemet. Idag kan systemet styra bilen så att den kan följa referensbanan i lateral led med mycket god noggrannhet men inte i longitunell led. Problemet har legat i fysikmotorn som är integrerad i V-REP, läs mer om detta i stycke 9.4. Statiska och rörliga hinder finns implementerade i simuleringsmiljön, där det rörliga hindret består av en grävmaskin som körs helt oberoende av vår egenutvecklade simuleringsmotor. Denna grävskopa är styrs istället direkt från V-REP och är programmerad att följa den referensbana som läggs in i simuleringsmiljön. Då inte regulatorn har kunnat bli fullt implementerat i simuleringsmiljön har funktionalitet för autonoma omkörningar inte blivit implementerat. I figur 16 och figur 17 syns de visuella resultaten.

33 Racetrack 28 9 Vidareutveckling och framtida arbete Här nedan presenteras områden inom respektive delsystem där vidareutveckling kan ske. Förslag på förbättringar ges nedan under respektive delsystem. 9.1 Regulator för bil Det största problemet med regulatorn är förmodligen att longitudinell och lateral följning är kopplade och att den longitudinella följningen har varit svårare att få till bra. Genom att separera dem och fixa en bättre regulator för longitudinellt fel skulle prestandan förmodligen bli bättre och robustare. Bilen svänger lite olika vid olika hastigheter. Att ta hänsyn till detta skulle göra att regulatorn fungerar bättre, framförallt när hastigheten varierar. 9.2 Regulator för lastbil Parallellt med detta projektet pågår ett projekt där lastbilen ska få en ny sändare. Det kommer förhoppningsvis minska fördröjningen mellan PC och lastbil. Minskas fördröjningen, kommer lastbilen kunna köra med högre hastighet utan att bli instabil. Det går även att förbättra och förenkla hur referenser för lastbilen genereras. Dagens sätt är omständigt och inte helt självklart första gången. Att som användare bara öppna ett GUI i matlab, dra ut linjer för att få sin referens inskriven på rätt ställe, vore smidigt. En vidareutveckling skulle inte ge någon prestandaförbättring, men det skulle kunna spara tid om referensen ska bytas ut mycket. 9.3 Planerare Planeraren behöver utvidgas med mer funktionalitet för att fungera på ett bra sätt i alla situationer. Just nu klarar den bra att köra om statiska hinder, men rörliga kan ge problem. Det som skulle behövas för att detta ska fungera bra är att hastighet och acceleration beräknas från fernet frame och inte deriveras fram. Deriveringen har någon form av numeriskt problem och gör att hastigheten ibland sticker iväg väldigt. Planeraren skulle även behöva sprida punkter i longitunell led på ett smart sätt, till exempel att bara lägga punkter bakom de hinder som finns. Slutligen skulle det vara intressant att låta planeraren beräkna trajektorier med olika sluttid vid varje planeringscykel. Detta på grund av att långa trajektorier fungerar bättre om hindret rör på sig, men kan ge problem för fall med statiska hinder. Allt detta finns beskrivet i forskningsrapporten [5], men har inte hunnits med under 2015 års projekt. Hantering av hinder (kallade Obstacles i koden) bör förbättras. Den funktionalitet som finns nu har flera buggar och använder bland annat ett helt eget filter, som är helt onödigt och som inte fungerar. Detta har gjort att årets projekt inte använder dessa klasser utan istället har en snabblösning får att få hinder till planeraren. 9.4 Simuleringsmiljö Ett problem gällande användningen av V-REP för simuleringar har varit att fysikmotorn har haft svårigheter att hantera körning av bilen utan sladd. Det bör nämnas att V-

34 Racetrack 29 REP i aktuell version har fyra stycken olika fysikmotorer implementerade och att vardera av dessa har en mängd olika inställningar och parametrar som kan ändras. Därmed är det inte helt enkelt att inse hur simuleringen bäst bör konfigureras. Med de aktuella inställningarna, som har funnits mest lämpliga för att kunna simulera att bilen kör utan att tappa fästet, blir hastighetsstyrningen väldigt svårhanterlig. Bilens egenskaper för hur hastigheten påverkas av styrsignalen blir då fundamentalt olika vid olika nivåer. Därmed har det inte gått att uppnå en tillräckligt god följning av trajektorian i tidshänsyn för att kunna använda planeraren. En vidareutveckling för att kunna hantera detta är att avstå från att använda sig av en fysikmotor i V-REP och istället implementera en modell för att simulera bilens dynamik i C++ programmet och sedan uppdatera bilens position och orientering i V-REP enligt resultatet. Andra utvecklingsmöjligheter är att bygga egna modeller i V-REP och göra så att de får de egenskaper som är önskvärda och så lika verkligheten som möjligt. Lastbilen, eller grävmaskinen i V-REP styrs nu helt oberoende av API:t. Detta är kanske inte önskvärt om men implementeringar kring lastbil vill göras, till exempel platooning eller liknande. Det kan då vara bra om man försöker inkludera styrningen av det rörliga hindret till API:t.

35 Racetrack 30 Referenser [1] Projektgruppen FAST. Teknisk dokumentation, [2] Moritz Werling, Lutz Gröll, and Gorg Bretthauer. Invariant trajectory tracking with a full-size autonomous road vehicle. IEEE Transactions on Robotics, [3] Oskar Ljungqvist. Motion planning and stabilization for a reversing truck and trailer system. Master s thesis, Tekniska högskolan vid Linköpings universitet, [4] Henrik Bäckman & Lina Karlsson. Att ändra lastbilens referens, [5] Mortiz Werling, Sören Kammel, Julius Ziegler, and Lutz Gröll. Optimal trajectories for time-critical street scenarios using discretized terminal manifolds. The International Journal of Robotics Research, [6] V-rep remote api. remoteapifunctionlistcategory.htm. Accessed: [7] V-rep remote api modus operandi. helpfiles/en/remoteapimodusoperandi.htm. Accessed: [8] Projektgruppen OTC. Användarhandledning, [9] Projektgruppen OTC. Kravspecifikation, [10] Projektgruppen OTC. Testprotokoll, 2015.