q uality m otion p lanning i n a d ynamic e nvironment august 2011
TRANSCRIPT
![Page 1: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/1.jpg)
QUALITY MOTION PLANNING IN A DYNAMIC ENVIRONMENTAugust 2011
![Page 2: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/2.jpg)
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.
![Page 3: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/3.jpg)
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.
![Page 4: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/4.jpg)
ALGORITHM OVERVIEW
Based on PRM Zucker-Kant Algorithm
Amending the Zucker-Kant Algorithms to achieve higher probability of solving problems
![Page 5: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/5.jpg)
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
![Page 6: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/6.jpg)
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
![Page 7: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/7.jpg)
BEGINNING
![Page 8: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/8.jpg)
STAGE 1
Sample points on completely free configurations (vertices on the graph)
Use input to determine sample strategy Use CGAL to determine free configuration
![Page 9: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/9.jpg)
![Page 10: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/10.jpg)
STAGE 2
Connect close points (edges on the graph) Make sure each edge is in almost free
configuration Use CGAL to determine free configuration
![Page 11: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/11.jpg)
![Page 12: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/12.jpg)
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
![Page 13: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/13.jpg)
Edges with heavy weights
Edges with light weights
![Page 14: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/14.jpg)
THE ZUCKER KANT PLANE
Let’s look at one segment of movement intersecting with one dynamic obstacle:
R
RO
O
O
BlockStart
BlockEnd
![Page 15: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/15.jpg)
THE ZUCKER KANT PLANE
In Time to distance on path plane, we get:
Time
DistanceBlock
StartBlockEnd
R
RO
OO
![Page 16: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/16.jpg)
THE ZUCKER KANT PLANE
After setting a goal time, we can solve with vGraph:
Time
DistanceBlock
StartBlockEnd
DestTime
R
RO
OO
![Page 17: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/17.jpg)
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
![Page 18: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/18.jpg)
![Page 19: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/19.jpg)
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.
![Page 20: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/20.jpg)
EXAMPLES
![Page 21: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/21.jpg)
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
![Page 22: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/22.jpg)
POSSIBLE IMPROVEMENTS
Improved sampling strategies Incremental algorithm Fix the case we fail (allow sampling almost
free places) 3D?
![Page 23: Q UALITY M OTION P LANNING I N A D YNAMIC E NVIRONMENT August 2011](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649d145503460f949e9814/html5/thumbnails/23.jpg)
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