Path Planning for Point Robots. NUS CS 5247 David Hsu

Transcription

1 Path Planning for Point Robots NUS CS 5247 David Hsu

2 Problem Input Robot represented as a point in the plane Obstacles represented as polygons Initial and goal positions Output A collision-free path between the initial and goal positions NUS CS 5247 David Hsu 2

3 Framework continuous representation (configuration space formulation) discretization (random sampling, processing critical geometric events) graph searching (breadth-first, best-first, A*) NUS CS 5247 David Hsu 3

4 Visibility graph method Observation: If there is a a collision-free path between two points, then there is a polygonal path that bends only at the obstacles vertices. Why? Any collision-free path can be transformed into a polygonal path that bends only at the obstacle vertices. A polygonal path is a piecewise linear curve. NUS CS 5247 David Hsu 4

5 What is a visibility graph? A visibility graph is a graph such that Nodes: q init, q goal, or an obstacle vertex. Edges: An edge exists between nodes u and v if the line segment between u and v is an obstacle edge or it does not intersect the obstacles. NUS CS 5247 David Hsu 5

6 A simple algorithm for building visibility graphs Input: q init,q goal, polygonal obstacles Output: visibility graph G 1: for every pair of nodes u,v 2: if segment(u,v) is an obstacle edge then 3: insert edge(u,v) into G; 4: else 5: for every obstacle edge e 6: if segment(u,v) intersects e 7: go to (1); 8: insert edge(u,v) into G. NUS CS 5247 David Hsu 6

7 Computational efficiency 1: for every pair of nodes u,v O(n 2 ) 2: if segment(u,v) is an obstacle edge then O(n) 3: insert edge(u,v) into G; 4: else 5: for every obstacle edge e O(n) 6: if segment(u,v) intersects e 7: go to (1); 8: insert edge(u,v) into G. Simple algorithm O(n 3 ) time More efficient algorithms Rotational sweep O(n 2 log n) time Optimal algorithm O(n 2 ) time Output sensitive algorithms O(n 2 ) space NUS CS 5247 David Hsu 7

8 Framework continuous representation (configuration space formulation) discretization (random sampling, processing critical geometric events) graph searching (breadth-first, best-first, A*) NUS CS 5247 David Hsu 8

9 Breadth-first search A B C F E D NUS CS 5247 David Hsu 9

10 Breadth-first search A B C F E D NUS CS 5247 David Hsu 10

11 Breadth-first search A B C F E D NUS CS 5247 David Hsu 11

12 Breadth-first search A B C F E D NUS CS 5247 David Hsu 12

13 Breadth-first search A B C F E D NUS CS 5247 David Hsu 13

14 Breadth-first search A B C F E D NUS CS 5247 David Hsu 14

15 Breadth-first search A B C F E D NUS CS 5247 David Hsu 15

16 Breadth-first search A B C F E D NUS CS 5247 David Hsu 16

17 Breadth-first search A B C F E D NUS CS 5247 David Hsu 17

18 Breadth-first search Input: q init, q goal, visibility graph G Output: a path between q init and q goal 1: Q = new queue; 2: Q.enqueue(q init ); 3: mark q init as visited; 4: while Q is not empty 5: curr = Q.dequeue(); 6: if curr == q goal then 7: return curr; 8: for each w adjacent to curr 10: if w is not visited 11: w.parent = curr; 12: Q.enqueue(w) 13: mark w as visited F B A E C D NUS CS 5247 David Hsu 18

19 Other graph search algorithms Depth-first Best-first, A* NUS CS 5247 David Hsu 19

20 Framework continuous representation (configuration space formulation) discretization (random sampling, processing critical geometric events) graph searching (breadth-first, best-first, A*) NUS CS 5247 David Hsu 20

21 A brief summary Discretize the space by constructing visibility graph Search the visibility graph with breadth-first search How to perform the intersection test? NUS CS 5247 David Hsu 21

22 Geometric Preliminaries NUS CS 5247 David Hsu

23 Primitive objects in the plane 0-dim class Point { double x; double y; } 1-dim class Segment { Point p; Point q; } NUS CS 5247 David Hsu 23

24 Primitive objects in the plane 2-dim class Triangle { Point p; Point q; Point r; } class Polygon { list<point> vertices; int numvertices; } NUS CS 5247 David Hsu 24

25 Intersecting primitive objects in the plane Case 1: line segment & line segment? = NUS CS 5247 David Hsu 25

26 Intersect line segments Counter-clockwise test c a b ab bc x y b b i x a y a x y c c j x b y b k 0 0 isccw(point a, Point b, Point c ) { u = b a; v = c b; return (u.x*v.y v.x*u.y) > 0; } NUS CS 5247 David Hsu 26

27 Intersect line segments with ccw Pseudo-code iscoincident(segment s, Segment t) { return xor(isccw(s.p, s.q, t.p), isccw(s.p, s.q, t.q)) && xor(isccw(t.p, t.q, s.p), isccw(t.p, t.q, s.q)) } Special cases NUS CS 5247 David Hsu 27

28 Intersecting primitive objects in the plane Case 2 line segment l & polygon P Intersect l with every edge of P Case 3 polygon P & polygon Q Intersect every edge of P with every edge of Q More efficient algorithms exist if P or Q are convex. NUS CS 5247 David Hsu 28

29 Additional information CS 4235 computational geometry Efficient geometry libraries CGAL, LEDA, etc. NUS CS 5247 David Hsu 29

30 Visibility graph method Represent the connectivity of the configuration space in the visibility graph Running time O(n 3 ) Compute the visibility graph Search the graph An optimal O(n 2 ) time algorithm exists. Space O(n 2 ) Can we do better? NUS CS 5247 David Hsu 30

31 Classic path planning approaches Roadmap Represent the connectivity of the free space by a network of 1-D curves Cell decomposition Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function NUS CS 5247 David Hsu 31

32 Classic path planning approaches Roadmap Represent the connectivity of the free space by a network of 1-D curves Cell decomposition Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function NUS CS 5247 David Hsu 32

33 Roadmap Visibility graph Shakey Project, SRI [Nilsson, 1969] Voronoi diagram Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2-D configuration spaces. NUS CS 5247 David Hsu 33

34 Voronoi diagram method Space O(n) Running time O(n log n) NUS CS 5247 David Hsu 34

35 Other roadmap methods Silhouette First complete general method that applies to spaces of any dimensions and is singly exponential in the number of dimensions [Canny, 87] Probabilistic roadmaps NUS CS 5247 David Hsu 35

36 Classic path planning approaches Roadmap Represent the connectivity of the free space by a network of 1-D curves Cell decomposition Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function NUS CS 5247 David Hsu 36

37 Cell-decomposition methods Exact cell decomposition The free space F is represented by a collection of nonoverlapping simple cells whose union is exactly F Examples of cells: trapezoids, triangles NUS CS 5247 David Hsu 37

38 Trapezoidal decomposition NUS CS 5247 David Hsu 38

39 Computational efficiency Running time O(n log n) by planar sweep Space O(n) Mostly for 2-D configuration spaces NUS CS 5247 David Hsu 39

40 Adjacency graph Nodes: cells Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent. NUS CS 5247 David Hsu 40

41 Framework continuous representation (configuration space formulation) discretization (random sampling, processing critical geometric events) graph searching (breadth-first, best-first, A*) NUS CS 5247 David Hsu 41

42 A brief summary Discretize the space by constructing an adjacency graph of the cells Search the adjacency graph NUS CS 5247 David Hsu 42

43 Cell-decomposition methods Exact cell decomposition Approximate cell decomposition F is represented by a collection of non-overlapping cells whose union is contained in F. Cells usually have simple, regular shapes, e.g., rectangles, squares. Facilitate hierarchical space decomposition NUS CS 5247 David Hsu 43

44 Quadtree decomposition empty mixed full NUS CS 5247 David Hsu 44

45 Octree decomposition NUS CS 5247 David Hsu 45

46 Sketch of the algorithm 1. Decompose the free space F into cells. 2. Search for a sequence of mixed or free cells that connect the initial and goal positions. 3. Further decompose the mixed. 4. Repeat (2) and (3) until a sequence of free cells is found. NUS CS 5247 David Hsu 46

47 Classic path planning approaches Roadmap Represent the connectivity of the free space by a network of 1-D curves Cell decomposition Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function NUS CS 5247 David Hsu 47

48 Potential field Initially proposed for real-time collision avoidance [Khatib, 1986]. Hundreds of papers published on this topic. A potential field is a scalar function over the free space. To navigate, the robot applies a force proportional to the negated gradient of the potential field. A navigation function is an ideal potential field that has global minimum at the goal has no local minima grows to infinity near obstacles is smooth NUS CS 5247 David Hsu 48

49 Attractive & repulsive fields F att = k att ( x x goal ) F rep k = rep ρ 2 ρ ρ0 ρ x 0 if ρ ρ, 0 ifρ> ρ 0 goal k att, k rep : positive scaling factors goal force x : position of the robot ρ : distance to the obstacle motion robot ρ 0 : distance of influence [Khatib, 1986] NUS CS 5247 David Hsu 49

50 Algorithm in pictures + NUS CS 5247 David Hsu 50

51 Sketch of the algorithm Place a regular grid G over the configuration space Compute the potential field over G Search G using a best-first algorithm with potential field as the heuristic function NUS CS 5247 David Hsu 51

52 Local minima What can we do? Escape from local minima by taking random walks Build an ideal potential field navigation function that does not have local minima NUS CS 5247 David Hsu 52

53 Question Can such an ideal potential field be constructed efficiently in general? NUS CS 5247 David Hsu 53

54 Completeness A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. Is the visibility graph algorithm complete? Yes. How about the exact cell decomposition algorithm and the potential field algorithm? NUS CS 5247 David Hsu 54