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