Robot Motion Planning

1. Robot Motion Planning Computational Geometry Lecture by Stephen A. Ehmann

2. Motivation • Want to design autonomous robots • performs actions without being told how • Robot must be able to plan its motion • has a map of the environment • We will focus on collision-free motion • There is a tradeoff between having an easy/fast solution and being conservative

3. Defining the General Problem • Environment may vary over time • Other robots may exist in the environment • coordinated motion problems • There may be constraints on the motion • Ex. Minimum turning radius • Ex. Maximum displacement of a prismatic joint

4. Defining the General Problem • There may be a cost function to minimize or maximize • time, distance, energy, safety • The robot can have many degrees of freedom (DOF) • translation, rotation, joints (revolute, prismatic)

5. Lots of DOF Possible!

6. Defining a Simpler Problem • Environment does not vary over time • Single robot in the environment • No constraints on the allowed motion • No cost function • Polygonal obstacles and robot • Allowed to touch obstacles • can always enlarge the robot for safety margin

7. Example Problem • Input • polygonal floor plan in 2D containing obstacles • polygonal robot • start and goal positions • Output • a path from start to goal which does not cause the robot to collide with the obstacles if one exists otherwise report that fact.

8. Example Problem

9. Motivation Defining the Problem Work Space and Configuration Space Minkowski Sums Point Robot Solution Rotations Three and Higher Dimensions Conclusions References Outline

10. Work SpaceandConfiguration Space • Work Space (WS) is the space of the original problem (the real world) • Configuration Space (CS) is the parameter space of the problem (more abstract)

11. Work SpaceandConfiguration Space

12. Robot Reference Point • A robot has a reference point which uniquely determines placement according to the allowed degrees of freedom

13. Reference Point in CS • A point in the CS corresponds to a placement of a robot in WS • This point is exactly the reference point of the robot in WS for our example

14. Free/Forbidden Space • Free configuration space (free space) is the set of all points in CS such that the corresponding robot placements in WS do not intersect any of the obstacles • Forbidden configuration space (forbidden space) is the set of all point in CS such that the corresponding robot placements in WS intersect with at least one of the obstacles

15. Paths • A path is a curve through CS • moves continuously from one set of parameter values to another • is the curve traveled by the point representing the placement of the robot in WS • A collision-free path is a curve wholly contained within the free space

16. Mapping from WS to CSfor our Example • Robot has been mapped by using its reference point in WS as its point in CS • Paths have been mapped as the path the robot reference point moves in CS and WS • Obstacles still need to be mapped

17. Mapping Obstacles • Intuitively it can be seen that we need to enlarge the obstacles in some manner when mapping them from WS to CS • The union of the maps of the obstacles will become the forbidden space -- the rest of CS will be the free space

18. Mapping Obstacles • This will lead us to a simpler problem in CS which is to solve for the motion of a point robot through a modified obstacle field

19. Mapping Obstacles • Wish to determine what the enlarged obstacles will look like based upon “sliding” the robot polygon around the perimeter

20. Minkowski Sums • We can describe the enlarged obstacles using Minkowski sums

21. Minkowski Sums • S1  S2 = {p+q : p  S1, q  S2} where • p = (px, py) • q = (qx, qy) • p+q = (px+qx, py+qy) • The mapping of a polygonal obstacle P to CS is P  (-R(0,0)) where R(0,0) is the robot with its reference point at (0,0)

22. Proof of the Mapping • Suppose robot R(x,y) intersects obstacle P at q = (qx, qy) • q  R(x,y) ==> (qx - x, qy - y)  R(0,0) | ==> (-qx + x, -qy + y)  (-R(0,0)) • q  P ==> (qx, qy )  P | ==> (qx+ (-qx + x), qy + (-qy + y)) |  P  (-R(0,0)) | ==> (x,y)  P  (-R(0,0))

23. Minkowski Sums • Let P and R be two convex polygons with n and m edges respectively. Then P  R is a convex polygon with at most m+n edges. • Intuitively, each edge on the original polygons generates an edge on P  R

24. Minkowski Sums

25. Computing Minkowski Sums • Want to compute the Minkowski sum of two convex polygons • Can add each pair of vertices (one from each polygon) and then compute the convex hull of the resulting point set O(mn log mn) • We can however make the algorithm run in linear time O(m+n)!

26. Computing Minkowski Sums • Observe that an extreme point on P R in a direction d is the sum of the extreme points in direction d on P and R • This means that we can use a gift wrapping method to compute the desired convex hull without considering points not on the convex hull boundary. It is like the merge step from merge sort.

27. Computing Minkowski Sums • Each polygon is has its vertices sorted in counter-clockwise order and the first vertex has the smallest y coordinate • Add edges with the smallest angle first

28. Computing Minkowski Sums MINKOWSKI-SUM(P,R) 1. i := 1; j := 1 2. vn+1 := v1; wm+1 := w1 3. repeat 4. Add v i + w j as a vertex to PR 5. if angle( vi vi+1 ) < angle( wj wj+1 ) then 6. i := i + 1 7. else if angle( vi vi+1 ) > angle( wj wj+1 ) then 8. j := j + 1 9. else 10. i := i + 1; j := j + 1 11. until i = n+1 and j = m+1

29. Unions of Minkowski Sums • The following property holds: • S1  (S2  S3) = (S1S2)  (S1S3) • To deal with non-convex polygons, we can first triangulate them and then take the union of all Minkowski sums generated by all the pairs of triangles P  R = (i=1..n-2, j=1..m-2)  ti  uj where the ti are the triangles from P and the uj are from R

30. Complexity of Minkowski Sums • O(m+n) if both polygons are convex • O(mn) if one of the polygons is convex and the other is not • O(m2n2) if both polygons are non-convex Minkowski Sum Demo

31. Motivation Defining the Problem Work Space and Configuration Space Minkowski Sums Point Robot Solution Rotations Three and Higher Dimensions Conclusions References Outline

32. Motion Planning for aPoint Robot • Enclose the obstacles in a bounding box B

33. Computing the Free Space • Want to compute a representation of the free space within which we can find a free path COMPUTE-FREE-SPACE 1. Let E be the set of edges of the obstacles 2. Compute the trapezoidal map T(E) (from Ch. 6) 3. Remove the trapezoids that lie inside the obstacles

34. Computing the Free Space • Recall that the trapezoidal map is constructed in O(n log n) expected time where n is the number of edges in the set of the obstacles • The step of removing the unwanted trapezoids takes O(n) time since we must simply check for each trapezoid whether the top of it is an edge that bounds an obstacle from above or from below

35. Computing Paths • Need a road map through the trapezoids • Create a graph with a vertex • in the middle of each vertical extension • in the middle of each trapezoid • Connect vertices in the center of a trapezoid with the vertices on the edges of the same trapezoid • Can create the road map in O(n) time

36. Computing Paths

37. Computing Paths • Use the point location structure that is built along with the trapezoidal map to lookup the start and goal trapezoids • If the start and the goal points are in the same trapezoid, the robot can simply move in a straight line from start to goal

38. Computing Paths • Connect the start and goal points to the vertices in the center of their respective trapezoids • Use breadth-first search to compute the path connecting the start and goal trapezoid centers if one exists

39. Point Robot Solution • Free space can be computed in O(n log n) time • A collision free path can then be computed in O(n) time • The complexity of the free space representation is O(n) • The road map also has a complexity of O(n)

40. Polygonal Robot Solution • For a convex robot R, the obstacles can be preprocessed in O(n log2 n) expected time • A path can then be computed in O(n) time if it exists

41. Motivation Defining the Problem Work Space and Configuration Space Minkowski Sums Point Robot Solution Rotations Three and Higher Dimensions Conclusions References Outline

42. Rotations • If a robot is allowed rotation in addition to translation in 2D then it has 3 DOF • The configuration space is 3D: (x,y,φ) where φ is in the range [0:360)

43. Mapping to CS • The obstacles map to “twisted pillars” in CS • They are no longer polygonal but are composed of curved faces and edges

44. Computing Free Space • Exact cell decomposition is really hard • Compute z: a finite number of slices for discrete angular values • Each slice will be the representation of the free space for a purely translational problem • Robot will either move within a slice (translating) or between slices (rotating)

45. Computing the Road Map • Each slice has a road map like before • But how do we move between slices?

46. Moving Between Slices • To find graph edges between two slices: • 1. compute the overlay of the trapezoidal maps of the two slices to get all pairs of trapezoids that intersect (one trapezoid from each slice) • 2. for each pair • 3. find a point (x,y) in their intersection and make one new vertex in each slice at this (x,y) • 4. connect the two new vertices • 5. connect the each of the two new vertices to the vertex at the center of their respective trapezoids

47. Slice Problems (Aliasing) • Start and/or goal position may be in the free space whereas the start/goal position in the nearest slice may not • May have an undetected collision when moving between slices • Increasing the number of slices reduces problems but does not solve them

48. Dealing with the Problems • Enlarge the robot by sweeping out some additional area (180o/z) in each direction • Introduces yet another way to incorrectly determine that there is no path

49. Three Dimensional Translation • Have k obstacles, each bounded by n edges • Have 3 DOF • Compute 3D Minkowski sums and then compute the 3D equivalent of a trap. map • Compute the graph and then search it to find the solution • The space has complexity O(nk log2k) • Expected construction time is O(nk log3k )

50. Three Dimensions with the Addition of Rotations • Have k obstacles, each bounded by n edges • Have 6 DOF • Do something similar to the slice method • Slices now become cells in the 3D space representing rotations • each cell has the purely translational solution for the set of orientations that it represents • move to 6 adjacent cells (3 different rotations)