robot motion planning
DESCRIPTION
Robot Motion Planning. Computational Geometry Lecture by Stephen A. Ehmann. 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 - PowerPoint PPT PresentationTRANSCRIPT
![Page 1: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/1.jpg)
Robot Motion Planning
Computational Geometry Lecture
by
Stephen A. Ehmann
![Page 2: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/2.jpg)
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
![Page 3: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/3.jpg)
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
![Page 4: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/4.jpg)
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)
![Page 5: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/5.jpg)
Lots of DOF Possible!
![Page 6: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/6.jpg)
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
![Page 7: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/7.jpg)
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.
![Page 8: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/8.jpg)
Example Problem
![Page 9: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/9.jpg)
Outline
• Motivation• Defining the Problem• Work Space and
Configuration Space• Minkowski Sums• Point Robot Solution
• Rotations• Three and Higher
Dimensions• Conclusions• References
![Page 10: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/10.jpg)
Work Spaceand
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)
![Page 11: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/11.jpg)
Work Spaceand
Configuration Space
![Page 12: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/12.jpg)
Robot Reference Point• A robot has a reference point which
uniquely determines placement according to the allowed degrees of freedom
![Page 13: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/13.jpg)
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
![Page 14: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/14.jpg)
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
![Page 15: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/15.jpg)
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
![Page 16: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/16.jpg)
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
![Page 17: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/17.jpg)
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
![Page 18: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/18.jpg)
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
![Page 19: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/19.jpg)
Mapping Obstacles
• Wish to determine what the enlarged obstacles will look like based upon “sliding” the robot polygon around the perimeter
![Page 20: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/20.jpg)
Minkowski Sums
• We can describe the enlarged obstacles using Minkowski sums
![Page 21: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/21.jpg)
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)
![Page 22: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/22.jpg)
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))
![Page 23: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/23.jpg)
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
![Page 24: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/24.jpg)
Minkowski Sums
![Page 25: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/25.jpg)
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)!
![Page 26: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/26.jpg)
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.
![Page 27: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/27.jpg)
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
![Page 28: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/28.jpg)
Computing Minkowski SumsMINKOWSKI-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 PR
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
![Page 29: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/29.jpg)
Unions of Minkowski Sums
• The following property holds:– S1 (S2 S3) = (S1S2) (S1S3)
• 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 trianglesP 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
![Page 30: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/30.jpg)
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
![Page 31: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/31.jpg)
Outline
• Motivation• Defining the Problem• Work Space and
Configuration Space• Minkowski Sums• Point Robot Solution
• Rotations• Three and Higher
Dimensions• Conclusions• References
![Page 32: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/32.jpg)
Motion Planning for aPoint Robot
• Enclose the obstacles in a bounding box B
![Page 33: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/33.jpg)
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
![Page 34: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/34.jpg)
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
![Page 35: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/35.jpg)
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
![Page 36: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/36.jpg)
Computing Paths
![Page 37: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/37.jpg)
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
![Page 38: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/38.jpg)
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
![Page 39: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/39.jpg)
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)
![Page 40: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/40.jpg)
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
![Page 41: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/41.jpg)
Outline
• Motivation• Defining the Problem• Work Space and
Configuration Space• Minkowski Sums• Point Robot Solution
• Rotations• Three and Higher
Dimensions• Conclusions• References
![Page 42: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/42.jpg)
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)
![Page 43: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/43.jpg)
Mapping to CS
• The obstacles map to “twisted pillars” in CS• They are no longer polygonal but are
composed of curved faces and edges
![Page 44: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/44.jpg)
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)
![Page 45: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/45.jpg)
Computing the Road Map
• Each slice has a road map like before• But how do we move between slices?
![Page 46: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/46.jpg)
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 pair3. 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 vertices5. connect the each of the two new vertices to the
vertex at the center of their respective trapezoids
![Page 47: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/47.jpg)
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
![Page 48: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/48.jpg)
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
![Page 49: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/49.jpg)
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 )
![Page 50: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/50.jpg)
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)
![Page 51: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/51.jpg)
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
![Page 52: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/52.jpg)
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
![Page 53: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/53.jpg)
Higher Dimensions
• J. Canny has proposed a very general and “complicated” road map method to solve any motion planning problem exactly– O(nd 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
![Page 54: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/54.jpg)
Approximate Cell Decomposition
![Page 55: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/55.jpg)
Approximate Cell Decomposition
• Use a 2m-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
![Page 56: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/56.jpg)
Potential Field Methods
![Page 57: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/57.jpg)
Potential Field Methods
• 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
![Page 58: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/58.jpg)
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
![Page 59: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/59.jpg)
Conclusions• Have concentrated on exact motion
planning: there are other heuristics– approximate cell decompositions (2m-tree)– potential field methods (minimize energy)– probabilistic road map (throw darts)
• Have not worried about:– lowest cost path, motion constraints, changing
environment, multiple robots
![Page 60: Robot Motion Planning](https://reader033.vdocuments.us/reader033/viewer/2022061519/56815616550346895dc3d552/html5/thumbnails/60.jpg)
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