Quality Motion Planning In a Dynamic Environment

1. Quality Motion Planning In a Dynamic Environment August 2011

2. 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.

3. 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.

4. Algorithm Overview • Based on • PRM • Zucker-Kant Algorithm • Amending the Zucker-Kant Algorithms to achieve higher probability of solving problems

5. 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

6. User Output Stage 1 A s s e m b l y L i n e Algorithm chart RunAlgorithm MaBakerView Stage 2 User Input Stage 5 Algorithm Support

7. Beginning

8. Stage 1 • Sample points on completely free configurations (vertices on the graph) • Use input to determine sample strategy • Use CGAL to determine free configuration

10. Stage 2 • Connect close points (edges on the graph) • Make sure each edge is in almost free configuration • Use CGAL to determine free configuration

12. 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

13. Edges with light weights Edges with heavy weights

14. The Zucker Kant Plane • Let’s look at one segment of movement intersecting with one dynamic obstacle: R O O O R Block Start Block End

15. The Zucker Kant Plane • In Time to distance on path plane, we get: Time R O O O R Distance Block Start Block End

16. The Zucker Kant Plane • After setting a goal time, we can solve with vGraph: Time R O Dest Time O O R Distance Block Start Block End

17. 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

19. 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.

20. Examples

21. 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

22. Possible improvements • Improved sampling strategies • Incremental algorithm • Fix the case we fail (allow sampling almost free places) • 3D?

23. 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