Motion planning for terrain vehicles: Path generation with radial-constrained A* and trajectory optimization

Abstract

Å planlegge en optimal rute for et bil-lignende terrengkjøretøy byr på forskjellige utfordringer. Den optimale bane kan ikke alltid anses som den korteste, men kan være den ruten med minst stigning, mest klaring eller det tryggeste veivalget. Disse terrengegenskapene kan lagres i et 2d-kart som representerer farbarheten i et visst område, og kartet kan videre brukes i et banesøk. Ved å behandle kartet som en graf, kan populære graf-søk algoritmer som A* brukes til å finne den optimale ruten. Et terrengkjøretøy vil få problemer ved å direkte følge A* sin rute, da ruten vil være diskret og bestå av svinger som kjøretøyet ikke kan følge. Baneplanleggeren må da respektere kjøretøyets begrensninger i planleggingsfasen.

Denne rapporten presenterer relevant litteratur som har påvirket utviklingen av ruteplanleggeren. Materialer innen navigering i terreng, ruteplanlegging for bil-lignende kjøretøy og rute-optimalisering vil bli introdusert. Ruteplanleggeren finner en bane i et kostkart som er mottatt fra FFI, der hver rute i kartet representer farbarheten i det respektive området. Videre, hvor et strukket (dilated) kostkart og start- og sluttpunkt er gitt, brukes det en radius-begrenset A* algoritme til å finne en diskret og trygg rute gjennom terrenget, hvor svingradiusen til kjøretøyet er tatt hensyn til. Den diskrete ruten blir deretter glattet med gradient descent optimering. Optimeringsalgoritmen bruker gradienter fra en kostfunksjon som glatter en diskret bane, samtidig som den tar vare på krumningen og det billige banevalget gjennom optimeringen. Avslutningsvis, når stoppkriteriene for gradient descent optimeringen er tilfredsstilt, vil den diskrete ruten ha blitt forandret til en glatt, billig og kjørbar bane for et terrengkjøretøy.

To perform non-holonomic motion planning in terrain is a challenging task. The optimal path in terrain may not be shortest in length, but the one with the smallest climb, least vegetation, or the route considered the safest. The terrain is often represented as a grid with traversability values. Finding the shortest path in a grid can be done with algorithms like A*, but the paths are not feasible for a car-like vehicle to follow due to too abrupt turns. The path planner needs to create a path that is driveable for a non-holonomic vehicle, by modifying an existing path-search algorithm or/and post-process the path by optimization.

This paper presents earlier work on terrain traversability, non-holonomic navigation, and trajectory optimization which are inspirations of this thesis' path planner. The path planner finds the path in a costmap provided by FFI, where each cell has a value that represents the traversability degree of this area. By dilating the costmap and given a start- and a goal-location, the radial-constrained A* algorithm is used to find a discrete- and safe-path that respects the turning radius of a non-holonomic vehicle. Further on, the discrete path is optimized by gradient descent optimization. The optimization uses the gradient of a cost function that smooths the path while maintaining the curvature and cost-efficiency of the path. If the stopping criterion of the gradient descent optimization is met, the initial discrete path will have been transformed into a smooth, cheap, and driveable path.