State Space

Zoals uit de vorige sectie is gebleken is de state space erg belangrijk voor het oplossen van path planning problemen. In dit stuk wordt uitgebreider ingegaan op de statespaces die gebruikt zijn bij het ontwikkelen van de pathplanning software.

Reeds Shepp Statespace

Voor een voertuig wat een bepaalde minimale stuur radius heeft en zowel vooruit en achteruit kan rijden (en sturen) wordt het kortste pad gegeven door een combinatie van cirkelbogen van deze bochtradius en rechte lijnen. Reeds en Shepp hebben in 1990 hier een analyse van gemaakt en geconstateerd dat er

(a) Een voorbeeld van een Reeds- Shepp curve, waar het voertuig begint in orientatie qIen eindigt in qGdoor

over drie cirkelbogen te rijden. (Figuur uit [48])

(b) Een voorbeeld van een pad be- paald in ROS met behulp van de Reeds Shepp Statespace uit OMPL.

(c) Een complexer voorbeeld van een pad bepaald in ROS met behulp van de Reeds Shepp Statespace uit OMPL.

Figuur 5.4: Voorbeelden van Reeds Shepp curves, elk van deze paden wordt volledig bepaald door de begin en eind state. Het steekgedrag wordt volledig bereikt door de Reeds Shepp curves, er zit geen extra state op de keerpunten om het steken te faciliteren.

48 primitieve zijn, deze primitieven beschrijven altijd het kortste pad van een begin state naar de eind state [68].

Zoals te zien is zijn de states in dit geval in SE(2): dus een positie in x, y en een bijbehorende hoek (yaw) ϑ. De INNOVADOvoldoet aan de eigenschappen van de Reeds Shepp curves, het voertuig kan vooruit en achteruit, en heeft een minimale stuur radius. Dit betekend dat de afstand tussen twee states geen rechte lijn is, maar de lengte is tussen de twee states volgens de lengte van de lijnen in de Reeds Shepp curve.

In OMPL is een state space te vinden die states heeft die in SE(2) zitten en waarbij de afstand tussen twee states gegeven wordt door de afstand van de Reeds Shepp curve tussen de twee states. Dit maakt het mogelijk om met OMPL paden te plannen door deze state space en zodoende enkel paden te maken die haalbaar zouden moeten zijn door de INNOVADO.

Control Space, first order car

Ook zou het mogelijk moeten zijn om het pad van de INNOVADOte plannen in de control space, de twee control parameters hierbij zouden snelheid en stuurhoek representeren. Op basis van de input snelheid en stuurhoek (usen uϕrespectievelijk) is vervolgens een state propagation functie te maken welke de vergelijkingen: ˙ x = uscos (ϑ), (5.1) ˙ y = ussin (ϑ), (5.2) ˙ ϑ =us L tan¡uϕ¢ , (5.3)

discretizeerd en uitrekend wat gegeven een bepaalde input state en stuur waarden de output state wordt.

Met deze manier van controllen is slechts kort ge-expirimenteerd omdat het vinden van een pad in de control space lastiger is dan is het vinden van een pad in de geometrische ruimte. Ook op de demo

(a) Een pad gezocht in de control space. (b) Een tweede voorbeeld van een pad gezocht in de control space.

Figuur 5.5: Enkele voorbeelden van gevonden paden door middel van zoeken in de control space.

pagina van OMPL[76] is te zien dat ondanks dat er met het zoeken in de control space valide paden te vinden zijn deze vaak grillig zijn en verre van optimaal.

Variable Reeds Shepp Statespace

Tijdens het ontwikkelen van het systeem is geuit dat het wenselijk is om een variabele bochtradius te hebben. De bij OMPL meegeleverdeReedsSheppStateSpaceis enkel in staat om een vaste bochtra- dius te hanteren voor alle bochten. Het is mogelijk om eenCompoundStateSpacete maken die bestaat uit de Reeds Shepp statespace en eenR1statespace voor de radius. Het is echter door de opzet van OMPL niet mogelijk om vanuit de statespace kennis te hebben van de andere statespace, waardoor het onmogelijk is om gebruik te maken van de waarde van de radius bij het berekenen van de padlengte. Om dit probleem te overkomen is een aangepaste versie van deReedsSheppStateSpacegemaakt. Door de SE(2) state die door deze state space gebruikt wordt te vervangen voor de nieuw gecreeërde

SE2Augmentedstate is er een extra veld aan deze state toegevoegd die de radius representeerd. Ver- volgens kan deVariableReedsSheppStateSpacede radius van de start-state gebruiken om met die radius de Reeds Shepp curve te bepalen naar de destination. Het variëren van de bochtradius gedu- rende één primitive is nog steeds niet mogelijk, maar wel is het mogelijk om elke individuele curve een andere bochtradius te geven. Een voorbeeld hiervan is te zien in figuur 5.6.

5.3.4

Planners

Voor het oplossen van het plannings probleem in het geometrische domein is gekeken naar ver- schillende planners. Hierbij is voornamelijk gekeken naar de planners RRT en PRM based planners. Uiteindelijk is er een planner gemaakt met de naamStructuredPlannerwelke in feitte een PRM planner is met een zeer duidelijke structuur in het opbouwen van de roadmap.

Figuur 5.6: Een pad bestaande uit drie states in de VariableReedsSheppStatespace. Dit maakt het mogelijk om zowel de hele scherpe steek te hebben en in een pad de bocht met een veel grotere bochtradius. (Dit pad zou niet gevolgd kunnen worden zonder botsingen, maar laat wel zien dat er twee boch-radiussen gebruikt worden.)

RRT

De Radidply-exploring Random Trees zijn een veelgebruikt principe om path planning uit te voeren. In OMPL zitten dan ook verscheidene planners die gebaseerd zijn op dit principe. Oorspronkelijk is dit algoritme beschreven voor de control space in [47]. Laat T de RRT zijn, K de vertices en xinitde

initial state. De tree T wordt geinitialiseerd met de xinitstate. Vervolgens worden er K iteraties gedaan,

waarbij de paper de volgende stappen beschrijft in de iteratie: 1. xrand← random_state()

2. xnear← nearest_neighbour(xrand, T )

3. u ← select_input(xrand, xnear)

4. T.add_vertex(xnew)

5. T.add_edge(xnear, xnew, u)

In woorden houdt dit in dat er voor elke iteratie een random state in de ruimte wordt gekozen (xrand),

dan wordt de dichtstbijzijnde neighbour (xnear) in de tree met deze random state gezocht.Als deze

gevonden is dan wordt de input u bepaald om van deze neighbour in de tree naar de random state te komen. Vervolgens wordt de random state als vertex toegevoegd aan de tree, en wordt er een edge geplaatst tussen xnearen xranddie informatie bevat hoe er xnearnaar xrandgekomen kan worden.

In kort komt het er dus op neer dat er steeds een nieuwe random state gekozen wordt, gekeken wordt hoe daar vanuit de reeds bekende states in de tree gekomen kan worden, en deze nieuwe state wordt dan aan de tree toegevoegd. Indien er oneindig gesampled wordt zal de gehele state space uiteindelijk in de tree zitten en zal dus ook het kortste pad tussen het begin en het einde in de tree zitten. Omdat er niet oneindig gesampled kan worden moet wordt er vaak een bias op de goal state gezet waardoor de goal state gesampled wordt in plaats van xrandom te kijken of er al een pad te vinden is.

Het kiezen van de random states kan uniform in de statespace gebeuren, maar dit kan ook rond reeds bekende states plaatsvinden, of in met een bepaalde vorm van heuristiek. Zo komen we op RRT∗2

(a) Het groeien van de tree. (b) Het uitbouwen van de tree, doel is nog steeds niet bereikbaar.

(c) Het gevonden pad.

Figuur 5.7: Een visualisatie van een Rapdily-exploring Random Tree process (RRT∗_{) om een pad te}

vinden. Figuren uit de primer van [76].

methoden, die bevoorbeeld richting het doel proberen een boom op te bouwen in plaats van volledig random.

PRM

Probabilistic RoadMap is een van de eerste sampling based pathplanning algoritmes [42]. Hierbij wordt een roadmap opgebouwd in de ‘wereld’, vervolgens worden de begin en goal states toegevoegd aan de roadmap en kan een pad van de begin state naar de goal state bepaald worden. De states in de roadmap worden milestones genoemd.

Het algoritme werkt als volgt. Laat een lege graaf R uit nodes N en edges E bestaan. Vervolgens wordt er voor K iteraties de volgende stappen gedaan:

1. c → random free state.

2. Nca set of candidate neighbours of c from N .

3. add c to N

4. for all n ∈ Ncin order of distance(c, n) do

• if c and n connected: add (c, n) to E . • Update R’s connected components.

In woorden houdt dit in dat er een valide random state gekozen wordt, deze milestone word toegevoegd aan de roadmap, vervolgens worden er een aantal dichtstbijzijnde states die al in de roadmap zitten bepaald (Nc). Op volgorde van dichtbijheid wordt er over deze lijst heengelopen, indien de verbinding

(a) De random states worden bepaald. (b) Op basis van de connection strategy wordt er be- paald welke milestones aan elkaar verbonden zijn.

(c) De start en goal state worden toegevoegd waarna er een pad bepaald kan worden.

Figuur 5.8: Een visualisatie van het path plannen met PRM. Figuren uit de primer van [76].

tussen de state die al in de roadmap zit en de nieuwe random state mogelijk is wordt deze edge toegevoegd aan de roadmap.

Eigenlijk is het algoritme op te splitsen in twee stappen:

• De sample stap, waarbij nieuw valide states toegevoegd worden.

• De connectie stap, waarbij de nieuwe states verbonden wordt met states reeds in de roadmap.

Door deze stappen te veranderen of variëren zijn verschillende eigenschappen te verkrijgen[30]. In OMPL zitten ook verschillenden implementaties van PRM, zo is het bijvoorbeeld mogelijk om de connecties tussen de milestones pas te evalueren als dit nodig is. Ook kan er op verschillende manieren door de uiteindelijke graaf gezocht worden. In OMPL is het ook mogelijk om bijvoorbeeld de sample stap of juist de connectie stap te beïnvloeden of volledig te vervangen voor een eigen implementatie.

Results

Met behulp van deReedsSheppStateSpacein OMPL zijn verscheidene planners geprobeerd. RRT Voorbeelden van routes bepaald met RRT∗zijn te zien in figuur 5.9. Dit zijn enkele voorbeelde waarbij er een route bepaald is binnen 20 s plantijd. Zo’n 10% van de routes kan niet binnen de tijdslimiet geplanned worden. Zoals te zien is komen wel routes uit die het doel bereiken, maar voor het meerder malen plannen naar hetzelfde doel worden telkens andere routes bepaald. Dit komt omdat het RRT algoritme een ‘single-query’ algoritme is en dus voor elke nieuwe route helemaal opnieuw begint met het planproces.

(a) Pad naar recht voor ‘hek’.

(b) Pad naar gang naar kuilen. (c) Pad naar naastgelegen kuil.

(d) Pad naar naastgelegen kuil. (e) Pad naar naastgelegen kuil.

PRM Het plannen met een Probabilistic Roadmap heeft eveneens problemen, zo worden er vaak routes geplanned welke via een omweg het doel bereiken. Dit komt doordat de milestones bij het plaatsen standaard mer de dichtstbijzijnde n reeds bestaande milestones worden verbonden. Omdat veel milestones niet in een logische orientatie staan zullen veel paden in de roadmap innefficient zijn of algeheel niet mogelijk zijn. De connectivity in de roadmap wordt laag door de combinatie van obstakels en slecthe orientatie. Dit heeft slechte paden tot gevolg. Het langer opbouwen van de roadmap helpt, indien er oneindig gesampled zou worden zou de gehele kaart bedekt zijn met milestones. Als we de aanname doen dat de costmap verandert moet de roadmap telkens opnieuw opgebouwd worden, dit zorgt ervoor dat er niet steeds betere routes bepaald worden maar ze allen van matige kwaliteit zijn bij een plantijd van 20 s. Enkele voorbeelden zijn te zien in figuur 5.10.

Als aangenomen wordt dat de costmap niet verandert kan de roadmap steeds verbeterd worden bij elke plan opdracht. Dit zorgt zeker wel voor steeds betere routes, een voorbeeld is te zien in figuur 5.11. Hierbij wordt bij elke plan actie de roadmap die al bekend is verbeterd. Het effect hiervan is dat de opeenvolgende route steeds optimaler wordt. Nog steeds blijft het een probleem dat veel van de routes veel bochten bevatten, dit omdat de gesamplede states nog steeds niet op één lijn liggen.

Analyse Voor alle methoden wordt gebruik gemaakt van sampling. Hierbij wordt een random positie en orientatie voor een state gekozen. Bij een zeer groot deel van deze random gekozen states is echter op voorhand al te zeggen dat deze states niet gaan leiden tot een goed pad. In figuur 5.12 zijn enkele states getekend in een kaart. In deze lopen de kuilen naar links, waar rechts in de kaart het pad naar de kuilen te zien is. In rood zijn de states getekend welke bij het random plaatsen in een obstakel zouden zitten. In oranje zijn de states aangegeven die door een mens afgekeurd zouden kunnen worden. In groen zijn de states getekend die wel logisch georienteerd zijn en onderdeel zouden kunnen worden van een logische route.

Naast de orientatie is het ook wenselijk dat de states op een rechte lijn van elkaar liggen, dit zorgt er immers voor dat de INNOVADOin een rechte lijn rijd in plaats van dat deze heen en weer slingert omdat de states niet op een rechte lijn liggen, wat een van de problemen uit voorgaande paragrafen was.

(a) Pad naar naastgelegen kuil.

(b) Pad naar naastgelegen kuil. (c) Pad naar naastgelegen kuil.

(d) Pad naar naastgelegen kuil. (e) Pad naar naastgelegen kuil.

(a) Pad naar naastgelegen kuil, eerste plan.

(b) Pad naar naastgelegen kuil, tweede plan. (c) Pad naar naastgelegen kuil, vierde plan.

kuilwand

Figuur 5.12: Voorbeeld van random sampling. In rood de samples die op basis van botsing afgekeurd zouden worden. In geel samples die gezien de obstakels valide zijn, maar zeer onwaarschijnlijk zijn of niet in een logische route zullen voorkomen. In groen samples die een logische orientatie hebben gezien het kuilgebied en de gang naar de kuilen.

5.3.5

StructuredPlanner

De pathplanners in OMPL zijn allen gemaakt om paden te plannen in ‘onbekende’ omgevingen. In het geval van de INNOVADOis de omgeving echter niet onbekend, er is immers bekend hoe de boerderij eruit ziet en waar de INNOVADOheen zal moeten en in welke richting hij ergens zal staan als er een pad geplanned moet worden. Over de omgeving is dus veel bekend en hier kan gebruik van gemaakt worden bij het plannen. Zo zal de INNOVADOin de stal achteruit inrijden in verband met het vegen, en elke kuil zal vooruit betreden worden om het uitkuilen mogelijk te maken. Doordat deze orientaties en posities bekend zijn is ook een logische uitleg te geven aan de positie en orientatie tussen deze locaties in.

De sampler zou bijgewerkt kunnen worden door te zorgen dat in een bepaald gebied de orientatie altijd klopt. Dit zou een groot deel van de problemen oplossen, maar dit garandeert niet dat er geen ‘slinger routes’ gevonden worden. De kans is immers klein dat de opeenvolgende states die gevonden worden keurig op een lijn liggen en de INNOVADOdus een recht pad kan volgen. Omdat bekend is waar de INNOVADOmoet rijden, en welke orientatie deze heeft kan deze kennis benut worden bij het opbouwen van de roadmap.

Structuur

De oplossing hiervoor is het garanderen dat de states en op logische posities liggen en een logische orientatie hebben. Derhalve is het wenselijk om structuur aan te brengen in de planner. Dit is gedaan door verschillende gebieden te definiëren. Elk gebied (patchin de code) is een rechthoek met de volgend eigenschappen:

• Een rechtoek met een positie en orientatie.

• De lengte van de rechthoek (local x), en breedte (local y). • Een begin (op x = 0) en end pose (op x = 1).

• Een stapgrootte in breedte en lengte∆x, ∆y. • Een lijst connectedpatches.

Deze structuur is verwerkt in de planner door een zeer kleine aanpassing te doen aan de PRM planner. Door deze te subclassen is het mogelijk om deaddMilestonefunctie beschikbaar te maken. Daarnaast is er een functie geschreven die deconnectionStrategyaanpast om controle te hebben over welke states de planner aan elkaar probeert te verbinden. Dit zorgt er voor dat er efficient de states uit de patches toegevoegd kunnen worden en de juiste verbindingen worden gemaakt met de aanliggende patches.

Aangezien bij de ingebruikname van de INNOVADOde kuilgebieden, het hekwerk en de stal toch aangegeven moeten worden is het niet problematisch geacht dat er handmatig patches aangegeven moeten worden voor de route. In figuur 5.13 zijn enkele voorbeelden te zien van de routes die gemaakt zijn met de Structured Planner, hier is te zien dat deze routes redelijk aan de eisen en verwachtingen voldoen.

Obstacle avoidance

Doordat er gebruikt gemaakt wordt van ROS’ navigation stack wordt de globale planner elke update loop uitgevoerd. Deze wordt ook aangeroepen als de doel positie niet verandert is. Door hier gebruik van te maken is het mogelijk om op elke update te controleren of de huidige route nog haalbaar is.

(a) Pad naar naastgelegen kuil. (b) Pad naar naastgelegen kuil.

(c) Pad naar naastgelegen kuil. (d) Pad naar ‘hek’.

Figuur 5.13: Enkele paden gecreeërd met de StructuredPlanner (PRM based) planner

Als de huidige route niet haalbaar is kan deze gediscard worden om vervolgens een nieuwe route te plannen. Omdat het updaten van de costmap vanzelf gaat is het ontwijken van obstakels niets anders dan het discarden van de route en het plannen van een nieuwe route. Doordat de costmap bijgewerkt is met de obstakels zal de nieuwe route niet door obstakels gaan.

Een voorbeeld hiervan is te zien in figuur 5.14, waar een obstakel geplaatst wordt op de hoek van de kuil. De route wordt bijgewerkt om voldoende afstand te blijven houden tot de randen.

5.4

Path tracker

Als een pad beschikbaar is dan is het nog niet vanzelfsprekend dat dit pad robuust te volgen is. Het voertuig moet immers aangestuurd worden op een manier waarop het pad betrouwbaar gevolgt wordt, ook als er verstoringen optreden in de positie of aansturing. Om een geplanned pad te kunnen volgen is een ‘path tracker‘ nodig, dit onderdeel moet de aansturingsinstructies sturen naar de INNOVADOom zo het geplande pad af te leggen.

5.4.1

Theorie

In de literatuur zijn verschillende manieren te vinden om een pad te volgen. De bekendste en meest gebruikte hiervan is Coulter’s pure pursuit [18], wat een geometrisch algoritme is specifiek ontworpen om voertuigen robuust en zonder oscillaties een pad te laten volgen. Omdat het pad niet enkel de posities bevat, maar ook de orientaties zijn er ook complexere path trakcer algoritmes die rekening houden met de fout in orientatie, bijvoorbeeld de methode die in Stanford’s Stanley werd toegepast tijdens de DARPA challenges[36].

(a) Het initiële geplande pad. (b) Het plaatsen van een obstakel op de kuilhoek.

(c) Het obstakel wordt gedecteerd, het pad bijgewerkt. (d) Het uiteindelijke pad.

Figuur 5.14: Obstacle avoidance in zijn werk.

algoritme toe te passen voor de INNOVADO. Dit is een geometrisch algoritme waarbij een punt verder op het pad gelegen als het ware gevolgt wordt. Vandaar de naam pure pursuit, doordat dit punt achtervolgt wordt door een cirkelboog wordt een smooth pad verkregen wat het voertuig kan volgen. Dit algoritme

In document Innovado: van rijdende machine naar autonome voerrobot (pagina 52-69)