Motion Planning for Point Robots

1. Motion Planning for Point Robots CS 659 Kris Hauser

2. Agenda • Skimming through Principles Ch. 2, 5.1, 6.1

3. Fundamental question of motion planning • Are the two given points connected by a path? Feasible space collision with obstaclelack of stabilitylost sight of target… Forbidden region

4. Basic Problem • Statement:Compute a collision-free path for a rigid or articulated object among static obstacles • Inputs: • Geometry of moving object and obstacles • Kinematics of moving object (degrees of freedom) • Initial and goal configurations (placements) • Output: Continuous sequence of collision-free robot configurations connecting the initial and goal configurations

5. Why is this hard?

6. Tool: Configuration Space • Problems: • Geometric complexity • Space dimensionality

7. Tool: Configuration Space

8. s Problem free space obstacle free path obstacle g obstacle

9. s Problem Semi-free path obstacle obstacle g obstacle

10. Local constraints: e.g., avoid collision with obstacles Differential constraints: e.g., bounded curvature Global constraints: e.g., minimal length Types of Path Constraints

11. Motion-Planning Framework Continuous representation Discretization Graph searching (blind, best-first, A*)

12. Path-Planning Approaches • RoadmapRepresent the connectivity of the free space by a network of 1-D curves • Cell decompositionDecompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential fieldDefine a function over the free space that has a global minimum at the goal configuration and follow its steepest descent

13. Roadmap Methods • Visibility graphIntroduced in the Shakey project at SRI in the late 60s. Can produce shortest paths in 2-D configuration spaces

14. Simple (Naïve) Algorithm • Install all obstacles vertices in VG, plus the start and goal positions • For every pair of nodes u, v in VG • If segment(u,v) is an obstacle edge then • insert (u,v) into VG • else • for every obstacle edge e • if segment(u,v) intersects e • then goto 2 • insert (u,v) into VG • Search VG using A*

15. Complexity • Simple algorithm: O(n3) time • Rotational sweep: O(n2 log n) • Optimal algorithm: O(n2) • Space: O(n2)

16. Rotational Sweep

17. Rotational Sweep

18. Rotational Sweep

19. Rotational Sweep

20. Rotational Sweep

21. can’t be shortest path Reduced Visibility Graph tangent segments  Eliminate concave obstacle vertices

22. Generalized (Reduced) Visibility Graph tangency point

23. Shortest path passes through none of the vertices locally shortest path homotopic to globally shortest path Three-Dimensional Space Computing the shortest collision-free path in a polyhedral space is NP-hard

24. Voronoi diagramIntroduced by Computational Geometry researchers. Generate paths that maximizes clearance. O(n log n) timeO(n) space Roadmap Methods

25. Roadmap Methods • Visibility graph • Voronoi diagram • SilhouetteFirst complete general method that applies to spaces of any dimension and is singly exponential in # of dimensions [Canny, 87] • Probabilistic roadmaps

26. Path-Planning Approaches • RoadmapRepresent the connectivity of the free space by a network of 1-D curves • Cell decompositionDecompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential fieldDefine a function over the free space that has a global minimum at the goal configuration and follow its steepest descent

27. Cell-Decomposition Methods Two classes of methods: • Exact cell decompositionThe free space F is represented by a collection of non-overlapping cells whose union is exactly FExample: trapezoidal decomposition

28. Trapezoidal decomposition

29. Trapezoidal decomposition

30. Trapezoidal decomposition

31. Trapezoidal decomposition

32. … critical events  criticality-based decomposition Trapezoidal decomposition

33. Trapezoidal decomposition Planar sweep  O(n log n) time, O(n) space

34. Cell-Decomposition Methods Two classes of methods: • Exact cell decomposition • Approximate cell decompositionF is represented by a collection of non-overlapping cells whose union is contained in FExamples: quadtree, octree, 2n-tree

35. Octree Decomposition

36. Sketch of Algorithm • Compute cell decomposition down to some resolution • Identify start and goal cells • Search for sequence of empty/mixed cells between start and goal cells • If no sequence, then exit with no path • If sequence of empty cells, then exit with solution • If resolution threshold achieved, then exit with failure • Decompose further the mixed cells • Return to 2

37. Path-Planning Approaches • RoadmapRepresent the connectivity of the free space by a network of 1-D curves • Cell decompositionDecompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential fieldDefine a function over the free space that has a global minimum at the goal configuration and follow its steepest descent

38. Potential Field Methods • Approach initially proposed for real-time collision avoidance [Khatib, 86]. Hundreds of papers published on it. Goal Robot

39. Attractive and Repulsive fields

40. Local-Minimum Issue • Perform best-first search (possibility of combining with approximate cell decomposition) • Alternate descents and random walks • Use local-minimum-free potential (navigation function)

41. Sketch of Algorithm (with best-first search) • Place regular grid G over space • Search G using best-first search algorithm with potential as heuristic function

42. 2 1 2 3 1 1 2 2 3 3 4 4 Simple Navigation Function 0 5

43. Simple Navigation Function 2 1 2 3 1 0 1 2 2 3 3 4 5 4

44. Simple Navigation Function 2 1 2 3 1 0 1 2 2 3 3 4 5 4

45. Completeness of Planner • A motion planner is completeif it finds a collision-free path whenever one exists and return failure otherwise. • Visibility graph, Voronoi diagram, exact cell decomposition, navigation function provide complete planners • Weaker notions of completeness, e.g.:- resolution completeness (PF with best-first search)- probabilistic completeness (PF with random walks)

46. A probabilistically complete planner returns a path with high probability if a path exists. It may not terminate if no path exists. • A resolution complete planner discretizes the space and returns a path whenever one exists in this representation.

47. Preprocessing / Query Processing • Preprocessing: Compute visibility graph, Voronoi diagram, cell decomposition, navigation function • Query processing:- Connect start/goal configurations to visibility graph, Voronoi diagram- Identify start/goal cell- Search graph

48. Some Variants • Moving obstacles • Multiple robots • Movable objects • Assembly planning • Goal is to acquire information by sensing • Model building • Object finding/tracking • Inspection • Nonholonomic constraints • Dynamic constraints • Stability constraints • Optimal planning • Uncertainty in model, control and sensing • Exploiting task mechanics (sensorless motions, under-actuated systems) • Physical models and deformable objects • Integration of planning and control • Integration with higher-level planning

49. Issues for Future Lectures on Planning • Space dimensionality • Geometric complexity of the free space • Constraints other than avoiding collision • The goal is not just a position to reach • Etc …

50. Readings • Principles Ch. 3.1-3.4 • *Lozano-Perez (1986) • *Lozano-Perez (1983)