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. Outline Motivation Defining the Problem Work Space and Configuration Space Minkowski Sums Point Robot Solution Rotations Three and Higher Dimensions Conclusions References

10. Work Space and Configuration 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 Space and Configuration 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 CS for 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 = (p x, p y ) –q = (q x, q y ) –p+q = (p x +q x, p y +q y ) 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 = (q x, q y ) q  R(x,y) ==> (q x - x, q y - y)  R(0,0) | ==> (-q x + x, -q y + y)  (-R(0,0)) q  P ==> (q x, q y )  P | ==> (q x + (-q x + x), q y + (-q y + 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.v n+1 := v 1 ; w m+1 := w 1 3.repeat 4.Add v i + w j as a vertex to P  R 5.if angle( v i v i+1 ) < angle( w j w j+1 ) then 6.i := i + 1 7.else if angle( v i v i+1 ) > angle( w j w j+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)  t i  u j where the t i are the triangles from P and the u j 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(m 2 n 2 ) if both polygons are non-convex Minkowski Sum Demo

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

32. Motion Planning for a Point 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. 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 log 2 n) expected time A path can then be computed in O(n) time if it exists

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

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 (180 o /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 log 2 k) Expected construction time is O(nk log 3 k )

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)

51. Three Dimensions with the Addition of Rotations Moving from cell to cell represents rotating about one of the three axes Have the same problems we had with slices –Start and/or goal position may be in the free space whereas the start/goal position in the nearest cell may not –May have an undetected collision when moving between cells –Increasing the number of cells reduces problems but does not solve them

52. A Word about Higher Dimensions When we have d DOF we have a d- dimensional CS Adding constraints to the motion restricts the range of some of the dimensions of CS –May need additional geometric primitives to represent these constraints

53. Higher Dimensions J. Canny has proposed a very general and “complicated” road map method to solve any motion planning problem exactly –O(n d log n) time where d is the dimension of the configuration space (DOF) –disadvantage is that the robot moves in contact with an obstacle most of the time which is not usually desirable

54. Approximate Cell Decomposition

55. Use a 2 m -tree to represent free CS having empty, full, and mixed types of cells +Easier, faster than exact methods +Minimum size of cells controls the amount of free space surrounding a generated path Measure of safety cost –Conservative May miss solutions

56. Potential Field Methods

57. At each step move an increment in the direction that minimizes the energy +Good heuristic for high DOF –Can get trapped in local minima use some probabilistic motion to escape –Oscillations can also occur

58. Probabilistic Methods Throw darts into free space to generate graph vertices Search the graph to find a path +Easy –May generate “bad” paths –May miss paths

59. Conclusions Have concentrated on exact motion planning: there are other heuristics –approximate cell decompositions (2 m -tree) –potential field methods (minimize energy) –probabilistic road map (throw darts) Have not worried about: – lowest cost path, motion constraints, changing environment, multiple robots

60. References “Computational Geometry -- Algorithms and Applications” by de Berg, van Kreveld, Overmars, and Schwarzkopf - 1997 “Robot Motion Planning” by Latombe 1991 “On translational motion planning in 3- space” by Aronov and Sharir - Proc. 10th Annu. ACM Sympos. Comput. Geom. 1994