Att ta sig från punkt A till punkt B. En litteraturstudie om autonoma farkoster. Mateo Herrera Velasquez 2016-01-04
Innehållsförteckning 1. INLEDNING.... 3 2. HUR DET FUNDERAR.... 4 2.1. A*-HYBRID.... 4 2.2. HEURISTIKER.... 5 2.3. PROBLEM.... 7 2.3.1. Optimering.... 7 2.3.2. Termer och funktionalitet.... 7 3. EN STRUKTURERAD VÄRLD.... 9 4. DISKUSSION... 10 5. AVSLUTNING.... 11 LITTERATURFÖRTECKNING... 12
1. Inledning. Det är naturligt för allmänhet att på en daglig basis ta sig från punkt A till punkt B, det kan vara att ta sig från hemmet till sin arbetsplats eller studieplats. Detta kan göras på många olika sätt. Det finns cyklar, kollektivtrafik eller motordrivna fordon. Att äga en bil är i vissa länder inte så speciellt märkvärt och dessa används oftast till att ta sig fram punkt A till punkt B. Det är därför detta arbete skall handla om hur en autonom bil tar sig fram med hjälp av en A*- hybrid-sökmetod. Detta sker i okända semistrukturerade miljöer. Mestadels av litteraturstudien bygger på arbete från Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo och James Diebel (2010). Fordonet: Det autonoma fordonet som användes var en bil försedd med ett antal laseravståndsmätare, där den viktigaste är 3D LIDAR (Velodyne) och ett positionsuppskattning system med en hög noggrannhet även kallas Applanix. Bilen kommer mestadels köra inom det som är okända miljöer men även semistrukturerade miljöer. Figur 1. Bilen Junior och dess mätutrustning. 3
2. Hur det funderar. Hela processen är uppdelad i två större faser, där den första fasen använder sig av sökmetod och heuristiker för att få fram kinematiskt möjliga vägar till målet. Den andra fasen använder sig av en numerisk optimering för att förbättra den möjliga lösningen. Detta kommer mestadels förklaras utifrån en ostrukturerad kör miljö men även kort om vad som gäller vid mer strukturerade miljöer. Den metod som kommer att förklaras bygger på en vanlig sökmetod, nämligen A*. En viktig aspekt på sökning är att hitta den bäst lämpliga vägen till mål-stadie. Ibland hittas en och ibland inte. Detta skall också försöka vara så billigt som möjligt för att inte slösa resurser eller tid. För att en algoritm, skall vara antagbar skall den leda till det önskade mål-stadiet och vara optimal. För att algoritmen då ska vara så effektiv som möjligt skall den expandera endast de noder som leder till att målet uppnås och målnoden hittas. För att den då skall veta hur den ska välja de noder att expanderas finns en utvärderande funktion som tillför den informationen för att kunna välja sorts noder att expander. (Hart, Nilsson, & Raphael, 1968) Den metod som skall förklaras framöver är en variant på den traditionella A* med en viss modifiering. 2.1. A*-hybrid. För att ta sig fram till målet sker det flera olika steg, där det börjar med en input i form av en karta över miljön med alla dess hinder som i detta fall är ett rutnät, ett startläge och ett målläge. Outputen i sin tur är förhoppningsvis en sekvens av olika stadier fordonet befinner sig i som skall ta den fram till målläget men samtidigt uppfylla vissa kriterier. Vägen till målet skall vara säker, så kort som möjligt, smidig samt uppfylla de kinematiska begränsningarna såsom sväng manövrerar. För att undvika komplikationer med olika hastigheter så antags en konstant hastighet, som skall kunna användas i outputen, det vill säga lösningsvägen. I slutskedet, när den slutgiltiga lösningen finns blir hastigheten istället en funktion baserat på krökningen och avståndet till olika hinder. I första fasen av sökningen, som har sitt ursprung i den välkända A*-metod som appliceras på en 3D-tillsåndskarta (x, y,θ), där tillståndet består av bilens plats riktning. Den metod som används har istället en 4D-tillståndskarta (x, y, θ, r), där den fjärde faktorn är r, som bara kan anta värdet 0 eller 1 och är färdriktningen på fordonet, alltså om bilen kör framåt eller om bilen backar. Den fjärde faktorn, r, används inom kostnadsuppfattning för de olika strategierna att ta sig till målläget. Det läggs till en kostnad för varje segment bilen kör baklänges och ytterligare en kostnad för varje gång bilen byter riktning. Själva hybridmetoden påminner om traditionell A*, där olika värden eller andra ting representeras i form av 4
rutnät(noder), för att exempelvis finna den snabbaste vägen till punkt B så representerar en ruta en möjlig körbana. Alla möjliga noder som används läggs sedan till i en öppen lista (open list) för att kunna vid senare tillfälle inte expandera den på nytt så en onödig kostnad uppstår. Skillnaden på hybridmetoden och det traditionella är att för varje möjlig ruta(nod), det vill säga körbar väg så associeras det en kontinuerlig 3D-tillsånd för fordonet. Det utgår från att det kontinuerliga tillståndet är i samspel med initialtillståndet och när rutnätsceller med möjliga lösningar skall expanderas gör dessa, i detta fall med tre olika handlingar. Det går antigenen att köra helt och hållet och vänster (max left), att inte svänga (no turn) och att svänga helt och hållet till höger(max right). Det resulterar i att de nya barnnoder kommer att genereras med användningen av en kinematisk (kinematisk innebär att objektets massa eller krafter som påverkar den inte tas i hänsyn) modell av hur fordonet tas sig till målet. För varje sådan nod som genereras och som redan blivit representerad, det vill säga att den redan ligger i den öppna listan som har en kostnad lägre än nuvarande nod så uppdateras det kontinuerliga tillståndet och den nod som hade lägsta kostnaden prioriteras i den öppna listan. En fördel med denna metod, som exempelvis Field D*(är en annan typ av A*- sökmetod) inte har, är att istället för att begränsa sig till piecewise-linear paths, det vill säga styckvis linjära lösningsbanor så är inte hybrid-a-metoden det. Detta genom att använda sig av den kinematiska funktionen som resulterar i att alla möjliga körvägar som producerats av hybridmetoden, har en garanti att de är körbara. Något som inte kan garanteras med hybridmetoden är huruvida den funna vägen är den billigaste vägen eller inte. Detta på grund av att när diskretisering(diskretisering innebär att kontinuerliga värden omvandlas till diskreta värden) av tid och kontroller. Så fast den billigaste vägen inte säkert hittas så hittar den läsbara och körbara läsningar till punkt B. Men även om den lösning som genereras ligger nära det globala optimum så sker en försäkran i den andra fasen av algoritmen. 2.2. Heuristiker. I den andra delen av algoritmen används två stycken heuristiker som inte enbart är begränsade till denna hybridmetod utan dessa heuristiker går att använda för andra sökmetoder utan några vidare problem. Den första heuristiken har de valt att kalla, nonholonomic-without-obstacles. Denna heuristik, som namnet illustrerar, bortser alla objekt men förbiser inte de holonomiska egenskaper bilen innehar. Heuristiken fungerar på så sätt att den räknar ut den kortaste vägen till målläget, för varje möjligt tillstånd i den fyra dimensionella världen. Den huvudsakliga uppgiften som heuristiken har är att tilldela höga färdkostnader för 5
de noder som bemöter problemet och agerar på fel sätt. Denna metod visade sig vara mer effektiv jämfört med exempelvis 2D-Euclidean, som expanderade betydligt mer noder än vad den metod som de använder sig av gjorde. När det gällde att testa heuristiken i verkliga miljöer så märktes det att i mindre miljöer så var det bara en mindre skillnad i effektivitet medan i större miljöer var det en större skillnad i expanderade noder och förbättringen syntes tydligare. Se figur 2 för exempel. Den andra heuristiken, som de kallar holonomic-with-obstacles, som i sin tur jämfört med den första heuristiken, ignorerar de holonomiska karaktärer bilen innehar men denna heuristik väljer senare att använda sig av en karta med de olika hinder som presenteras. Den räknar ut den kortaste vägen till målet, med användningen av dynamisk 2D-programmering. Denna används för att, med hjälp av en kostnadsfunktion, räkna ut om en väg är säker men även för att undvika onödigt körande och undvika större kostnader. Dessa två heuristiker appliceras i den grad som det går, för att minska expansionen av onödiga noder och istället kommer rätt noder att expanderas och resultatet blir mindre antal expanderade noder men en korrekt väg till målet hittas. A. 37 181 expanderade noder. B. 11 302 expanderade noder. Figur 2. Bilden A och B visar skillnaden på expanderade noder med olika heuristiker. 6
2.3. Problem. Det uppstår problem med av diskretisering av handlingar. Problemet är att när en diskretisering av kontinuerliga tillstånd sker kommer inte det exakta målläget att uppnås. Detta problem har löst med det som är Reed-Sheep-modellen. Som tidigare beskrivits så expanderas en nod med hjälp av en kinematisk funktion. Det som görs för att problemet skall lösas är att för varje ny nod så skall ytterligare en barnnod genereras med användningen av Reed-Sheep-modellen. Den modellen tar fram en väg som inte kommer resultera i kollision men det är dyrare att expandera på sådant sätt. För att inte ta på sig en större kostnad så expanderas inte alla noder utan en regel appliceras för att begränsa kostnaden. Regeln lyder på följande sätt; för varje nod N, där N minskar som en funktion av kostnaden till målet. Detta resulterar i att flera noder expanderas desto närmre målnoden sökningen är och det blir mer precist. Då vägarna som hittades fortfarande inte var helt optimala och det fanns fortfarande rum för förbättring togs det fram ytterligare några funktioner som skulle förbättra hela processen. De vägar som hittades hade ofta onaturliga svängar, som innefattade extrema manövrar. För att förhindra detta valdes det att bearbeta de lösningar som funnits och optimera dessa i en tvåstegsoptimering. 2.3.1. Optimering. Eftersom det lösningar som ibland hittas har för extra svängningar gjordes det en optimering i två steg för att förhindra detta. I det första steget tas koordinaterna till de hörn som det skall svängas runt in och en icke-linjär optimering utgörs för att förbättra längden och smidigheten i svängen. Den metod som används kallas för conjugate-gradient(cg), och är en numerisk optimeringsmetod. I den andra delen så behandlas den output från det första steget. Med den data den får ut utför den en icke-parametrisk interpolation med CG. Den funktion som huvudsakligen används är följande: 2.3.2. Termer och funktionalitet., är i funktionen det objekt som närmast befinner sig ett givet hörn. 7
, är den försjuktningsvektorn till ett hörn. tangerande vinkeln i förhållande till hörnet. Dessa utgår då från en sekvens hörn som definieras på följande sätt:, är förändringen i den σ o och σ k är bägge straffunktioner. ω o, ω k och ω s är olika vikter.. Det går att se att huvudfunktionen egentligen är tre olika delar som utför tre olika funktioner. Den första delen är en straffunktion som straffar de körbanor som involverar en krock. Den andra delen av funktionen sätter en övre gräns för svängningar och krökningar som en väg till målet kan ha men även så upprätthåller de icke-holonomiska egenskaper hos fordonet. Den tredje och sista delen av huvudfunktionen är ett mått på hur jämn vägen som valts är fram till målet. Detta skall förhindra att krockar skall ske men när en jämn körväg skall hittas så A*-hybridmetoden optimal, det är därför dessa metoder används som komplement. Ett exempel (se figur 3) som visar sig vara problematiskt är då fordonet närmar sig en smal öppning mellan två hinder. Den banan som då väljs kommer den bakre delen av bilen vara nära en sida av öppningen för att säkert kunna svänga in i mynningen. Det problem som då uppstår är att fordonets form inte tas med i uträkningen så resulterar detta i att en osäker handling kommer att utföras. Figur 3. Ursprungsartikelns exempel på ett problematiskt hinder. Det som görs då för att förhindra detta är att det väljs en metod som fungerar. Körbanan processernas (via CG) så att det finns en smidighet när den körs som resulterar i att den är krockfri, skulle det vara så att en osäker väg ändå tas fram så förankras denna med A*- 8
hybridmetoden som gör om hela sökprocessen. Detta på grund av att den CG inte kan ändra på de koordinaterna som A*-hybridmetodens resultat innehar och en jämförelse mellan dessa sker som leder till att en krockfri väg hittas. 3. En strukturerad värld. Det är en stor skillnad på att ta sig från punkt A till en punkt B i en strukturerad värld och en ostrukturerad värld. Det är därför det används olika metoder att ta sig fram till målet. Förutsättningarna för att detta ska kunna göras är att en topografisk karta över området som det ska köras i. Hur denna karta har gjort eller fåtts tas inte upp. Det som då kan åstadkommas i en strukturerad värld med en topografisk karta är att fordonet kan, exempelvis hålla sig inom en körbana eller inte köra över massa parkeringsrutor. Det behövs inga större förändringar på algoritmen, men då det handlar om körbanor, som representeras i grafer måste några förändringar ske. Figur 4. En karta där körbanorna representeras. Den markanta skillnaden är att det har tillkommit en funktion som räknar ut ytterligare en kostnad. Heuristikerna som användes, non-holonomic-without-obstacles, funkar lika väl här och behöver inte ändras. Den andra heuristiken, holonomic-withobstacles, funkar i semi-strukturerade vägar men kan bidra med en dålig lösning som heuristik. Det som kan göras är att den modifieras så den kan behandla grafsystemet som används i mer strukturerade världar. En fördel med att ha en grafiskrepresentation av omgivningen är att macro-handlingar. Detta används när barnnoder skall expanderas, såsom vid okända strukturerade miljöer, så genereras barnnoder med ett par förutbestämda handlingar(exempelvis svänga vänster). Detta görs även på de macro-handlingar men på dessa applicerad Reed-Sheep-funktionen. Skulle dessa noder visa sig vara krockfria så expanderas 9
det och adderas i sökträdet. Vad gäller smidighetsfunktionen(cg) har den med blivit anpassad efter den grafiskarepresentationen. 4. Diskussion Finns det då sätt som hade varit bättre lämpade för att utföra samma sorts uppgift men med en annan algoritm. Det finns alternativa sätt såsom Anytime Dynamic A* (AD*) utvecklas av Maxim Likhachev och Dave Ferguson (2009). Denna metod använder sig av heuristik och en inflationsfaktor E som inte är större än 1, som kommer generera en snabb lösning till målnoden. Kostanden kommer även resultera i E gånger kostanden för den optimala lösningen. Denna metod börjar, i skillnad av A*-hybridmetoden, från målnoden och söker baklänges till det nuvarande läget fordonet. Fast denna algoritm börjar i slutet måste heuristiken fortfarande hitta slutläget från fordonets nuvarande läge, det vill säga att den går från början till slut. (Likhachev & Ferguson, 2009) Gällande alla algoritmer så är det väldigt viktigt att ha en fungerande heuristik, för med en dålig heuristik skulle det kunna göra mer skada än nytta. Ytterligare en skillnad på dessa två metoder är att den ena tar med bilens hastighet medan den andra inte gör det utan den bibehåller en konstant hastighet under hela processen. Hastighet är en sådan ting som kan ha stor påverkan. Med en hastighet måste ting som bromssträcka tänkas på men det går även att räkna ut eventuell kraft vid möjlig kollision. Att ha en hastighet blir påverkande när det gäller större områden såsom att köra inom en stad, däremot något mindre relevant när det gäller en parkering eller liknanden miljöer då hastigheten inte är så hög. Ett större problem som skulle kunna uppstå med användningen av AD* istället för A*- hybrid är att den ena är mycket mer anpassad för strukturerade miljöer och genom att börja med målnoden och gå baklänges så skulle en modifiering av bägge heuristikerna. Att utgå från målläget innebär att hela processen måste ske baklänges vilket resulterar i en modifiering av algoritmen med. Det är uppenbart att den algoritmen som gjordes för just denna typ av uppgift är mycket mer lämplig än vad någon annan som är mer lämpad för en annan typ av uppgifter. Dock ska det inte alltid tas för givet att detaljer eller idéer kan tas från andra liknande studier. Det gäller att på ett kritiskt sätt se över arbeten och se om det är lämpligt att använda andras funktioner eller liknande. 10
5. Avslutning. I denna litteraturstudie har en algoritm presenteras som kan användas för att ta sig från punkt A till punkt B med hjälp av en autonom farkost. Denna algoritm är uppdelad i två delar och har två heuristiker som ska minska antal expanderade noder. Även problem som diskretisering av kontinuerliga värden löses med hjälp av Reed-Sheep-funktionen. 11
Litteraturförteckning Dolgov, D., Sebastian, T., Montemerlo, M., & Diebel, J. (2010). Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments. The International Journal of Robotics Research, 485 501. Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A Formal Basis for the Heuristic Determination of Minimum Cost Paths. IEEE TRANSACTIONS OF SYSTEMS SCIENCE AND CYBERNETICS, 100-107. Likhachev, M., & Ferguson, D. (2009). Planning Long Dynamically Feasible Maneuvers for Autonomous Vehicles. The International Journal of Robotics Research, 933 945. 12