quality motion planning in a dynamic environment

QUALITY MOTION PLANNING IN A DYNAMIC ENVIRONMENTAugust 2011

PROJECT GOAL  Generate a path for a Robot in a 2D

Environment containing both Static & Dynamic obstacles.

Few assumptions: The Robot has a maximum velocity and a Time

limitation to reach destination. The dynamic obstacles movement is known and

cyclic. The static Obstacles are simple

polygons .

IMPLEMENTATION & DEVELOPMENT ENVIRONMENT The project was written in C++ using Visual

Studio 2008, with support of the following software packages: MFC - we used this package to create our GUI. CGAL - for basic implementations & functions we

used in our algorithms.

ALGORITHM OVERVIEW Based on

PRM Zucker-Kant Algorithm

Amending the Zucker-Kant Algorithms to achieve higher probability of solving problems

ALGORITHM – THE ASSEMBLY LINE The solution at each stage is represented by

a directed graph The final path is a list of edges on graph The graph is passed to each of the stages of

the algorithm Each stage modifies the graph and brings it

closer to solution

ALGORITHM CHART

MaBakerView

User Output

User Input

RunAlgorithm

Stage 1

Stage 2

Stage 5

A s s e m b l y L i n e

Algorithm Support

BEGINNING

STAGE 1 Sample points on completely free

configurations (vertices on the graph) Use input to determine sample strategy Use CGAL to determine free configuration

STAGE 2 Connect close points (edges on the graph) Make sure each edge is in almost free

configuration Use CGAL to determine free configuration

STAGE 3 Transform each edge found in stage 2 into a

distance – time graph, considering moving obstacles

Solve using the VGraph method If unsolvable – give edge an infinite weight If solvable – give weight indicating the time it

takes to pass the edge If no moving obstacles on edge, just time

when moving in max velocity

Edges with heavy weights

Edges with light weights

THE ZUCKER KANT PLANE Let’s look at one segment of movement

intersecting with one dynamic obstacle:

R

RO

O

O

BlockStart

BlockEnd

THE ZUCKER KANT PLANE In Time to distance on path plane, we get:

Time

DistanceBlock

StartBlockEnd

R

RO

OO

THE ZUCKER KANT PLANE After setting a goal time, we can solve with

vGraph:Time

DistanceBlock

StartBlockEnd

DestTime R

RO

OO

STAGE 4 Run Dijkstra on the graph and find the final

path This is the path the robot is going to use, we

still don’t know the velocity function However – it is guaranteed that this path is

passable

STAGE 5 Transform the path found in stage 4 into a

distance – time graph, considering moving obstacles

Calculate velocity function, which determine the progress velocities on the path we found in stage 4.

This promises the shortest arrival time to destination.

EXAMPLES

WHAT DID WE LEARN? New algorithms for path planning Working with complicated libraries – MFC,

CGAL Analytical geometry when applied in a

program Working on a big project

POSSIBLE IMPROVEMENTS Improved sampling strategies Incremental algorithm Fix the case we fail (allow sampling almost

free places) 3D?

ADDITIONAL READING K. Kant and S. Zucker. Toward efficient

planning: the path-velocity decomposition. International Journal of Robotics Research, 5(3):72–89, 1986

Path Planning in Dynamic Environments, Jur Pieter van den Berg, May 1981