Upcoming SlideShare
×

# Robotics: Modelling, Planning and Control

2,528 views
2,356 views

Published on

1 Like
Statistics
Notes
• Full Name
Comment goes here.

Are you sure you want to Yes No
• Be the first to comment

Views
Total views
2,528
On SlideShare
0
From Embeds
0
Number of Embeds
5
Actions
Shares
0
88
0
Likes
1
Embeds 0
No embeds

No notes for slide

### Robotics: Modelling, Planning and Control

1. 1. Introduction Planning Techniques Application to Robot ManipulatorsRobotics: Modelling, Planning and Control Chapter 12 Summary Cody A. Ray February 6, 2011 1 / 66
2. 2. Introduction Planning Techniques Application to Robot ManipulatorsIntroduction Motion Planning Canonical Problem Conﬁguration Space Examples of ObstaclesPlanning Techniques Retraction Cell Decomposition Probabilistic Planning Artiﬁcial PotentialApplication to Robot Manipulators 2 / 66
3. 3. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesBuilding on Previous Work Chaps. 4 and 7 present trajectory planning methods for manipulators and mobile robots, respectively Chap. 12 removes assumption of empty workspace Goal: avoid collision with obstacles – both static structures and other moving objects 3 / 66
4. 4. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesPlanning Examples Manipulator in robotized cell must avoid collision with static objects and other manipulators Mobile robot carrying baggage in airport navigates among obstacles that may be ﬁxed (ﬁttings, conveyor belts) or mobile (passengers, workers) Motion planning amounts to deciding which path to follow to execute transfer task from initial to ﬁnal posture without colliding with obstacles 4 / 66
5. 5. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesAutonomous Planning High-level task description provided by the user and a geometric characterization of the workspace Workspace characterization made available entirely in advance, oﬀ-line planning gathered by the robot itself during the motion by means of on-board sensors, on-line planning Developing automatic methods for motion planning is very diﬃcult, and is still an active topic of research 5 / 66
6. 6. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of Obstacles12.1 Canonical Problem Consider a robot B, which may consist of a single rigid body (mobile robot) or a kinematic chain whose base is either ﬁxed (standard manipulator) or mobile (mobile robot with trailers or mobile manipulator). Robot moves in Euclidean space W = RN , with N = 2 or 3, called workspace. Let O1 , . . . , Op be the obstacles, i.e., ﬁxed rigid objects in W. Assume that both the geometry of B, O1 , . . . , Op and the pose of O1 , . . . , Op in W are known. Suppose that B is free-ﬂying, that is, the robot is not subject to any kinematic constraint. 6 / 66
7. 7. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesCanonical Problem Statement The motion planning problem is the following given an initial and a ﬁnal posture of B in W, ﬁnd if exists a path, i.e., a continuous sequence of postures, that drives the robot between the two postures while avoiding collisions (including contacts) between B and the obstacles O1 , . . . , Op ; report a failure if such a path does not exist. 7 / 66
8. 8. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesSpecial Cases Robot is a single body moving in R2 is also known as the piano movers’ problem The generalized movers’ problem is canonical for single-body robot moving in R3 8 / 66
9. 9. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of Obstacles12.2 Conﬁguration Space Representing the robot as a mobile point in an appropriate space, where the images of the workspace obstacles are also reported. To this end, it is natural to refer to the generalized coordinates of the mechanical system, whose value identiﬁes the conﬁguration of the robot. This associates to each posture of the latter a point in the conﬁguration space, i.e., the set of all the conﬁgurations that the robot can assume. 9 / 66
10. 10. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesConﬁguration Consider a system Br of r rigid bodies and assume that all the elements of Br can reach any position in space. In order to ﬁnd uniquely the position of all the points of the system, it is necessary to assign a vector x = [x1 . . . xp ]T of 6r = p parameters, termed conﬁguration. These parameters are termed Lagrange or generalized coordinates of the unconstrained system Br , and p determines the number of degrees of freedom (DOFs). 10 / 66
11. 11. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesConﬁguration Space Examples The conﬁguration of a polygonal mobile robot in W = R2 is described by the position of a representative point on the body (e.g., a vertex) and by the orientation of the polygon, both expressed with respect to a ﬁxed reference frame. The conﬁguration space C is R2 × SO(2), with dimension n = 3. For a polyhedral mobile robot in W = R3 , the conﬁguration space C is R3 × SO(3), whose dimension is n = 6. For ﬁxed-base planar manipulator with n revolute joints, the conﬁguration space is subset of (R2 × SO(2))n . Dimension of C equals the dimension of (R2 × SO(2))n minus the number of constraints due to the presence of joints, i.e., 3n − 2n = n. In fact, in a planar kinematic chain, each joint imposes two holonomic constraints on the following body. 11 / 66
12. 12. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesConﬁguration Space Examples (cont’d) For a ﬁxed-base spatial manipulator with n revolute joints, the conﬁguration space is a subset of (R3 × SO(3))n . Since in this case each joint imposes ﬁve constraints on the following body, the dimension of C is 6n − 5n = n. For a unicycle-like vehicle with a trailer in R2 , conﬁguration space is subset of (R2 × SO(2)) × (R2 × SO(2)). If the trailer is connected to the unicycle by revolute joint, conﬁguration of the robot can be described by the position and orientation of the unicycle and the orientation of the trailer. The dimension of C is therefore n = 4. 12 / 66
13. 13. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.1 Consider a 2R manipulator (planar with two revolute joints) Figure: (2.14.) Two-link planar arm 13 / 66
14. 14. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.1 The conﬁguration space has dimension 2, and may be locally represented by R2 , or more precisely by its subset Q = {q = (q1 , q2 ) : q1 ∈ [0, 2π), q2 ∈ [0, 2π)}. This guarantees that the representation is injective, i.e., that a single value of q exists for each manipulator posture. However, this representation is not topologically correct: for example, the conﬁgurations denoted as qA and qB in Fig. 12.1, left, which correspond to manipulator postures that are ‘close’ in the workspace W, appear to be ‘far’ in Q. 14 / 66
15. 15. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.1 Figure: (12.1.) The conﬁguration space of a 2R manipulator; left: a locally valid representation as a subset of R2 , right: a topologically correct representation as a two-dimensional torus 15 / 66
16. 16. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.1 To take this into account, one should ‘fold’ the square Q onto itself (so as to make opposite sides meet) in sequence along its two axes. This procedure generates a ring, properly called torus, which can be visualized as a two-dimensional surface immersed in R3 as in Fig. 12.1, right. The correct expression of this space is SO(2) × SO(2). As we can see, even in simple examples, the geometric structure of the conﬁguration space is in general more complex than that of a Euclidean space. 16 / 66
17. 17. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of Obstacles12.2.1 Distance in conﬁguration space C Given a conﬁguration q , let B(q ) be the subset of the q workspace W occupied by the robot B, and p(q ) be the q position in W of a point p on B. Intuition suggests that the distance between two conﬁgurations q 1 and q 2 should go to zero when the two regions B(q 1 ) and B(q 2 ) tend to coincide. q q d1 (q 1 , q 2 ) = max ||p(q 1 ) − p(q 2 )|| q q q (12.1) p∈B In other words, the distance between two conﬁgurations in C is the maximum displacement in they induce on a point, as the point moves all over the robot. 17 / 66
18. 18. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesDistance in conﬁguration space C (cont’d) q However, the use of d1 (q ) is cumbersome. We often choose the simple Euclidean norm as the conﬁguration space distance. d2 (q 1 , q 2 ) = ||p(q 1 ) − p(q 2 )|| q q q (12.2) This is only appropriate when C is a Euclidean space. From Example 12.1, it’s easy to realize that, unlike d1 (q A , q B ), the q q Euclidean norm d2 (q A , q B ) does not represent correctly the distance on the torus. 18 / 66
19. 19. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of Obstacles12.2.2 Build the ‘images’ of obstacles in C Given an obstacle Oi (i = 1, . . . , p) in W, its image in conﬁguration space C is called C-obstacle and is deﬁned as COi = {q ∈ C : B(q ) ∩ Oi = ∅}. q q (12.3) In other words, COi is the subset of conﬁgurations that cause a collision (including simple contacts) between the robot B and the obstacle Oi in the workspace. 19 / 66
20. 20. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesBuild the ‘images’ of obstacles in C (cont’d) The union of all C-obstacles p CO = COi (12.4) i=1 deﬁnes the C-obstacle region, while its complement p Cfree = C − CO = {q ∈ C : B(q ) ∩ q q Oi = ∅} (12.5) i=1 is the free conﬁguration space, that is, the subset of robot conﬁgurations that do not cause collision with the obstacles. A path in conﬁguration space is called free if it is entirely contained in Cfree . 20 / 66
21. 21. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesBuild the ‘images’ of obstacles in C (cont’d) Although C is a connected space – given two arbitrary conﬁguration there exists a path that joins them – the free conﬁguration space Cfree may not be connected because of occlusions due to C-obstacles. Note also that the assumption of free-ﬂying robot in the canonical problem means that the robot can follow any path in the free conﬁguration space Cfree . 21 / 66
22. 22. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesCompact Canonical Motion Planning Problem Assume the initial and ﬁnal posture of the robot B in W are mapped to corresponding conﬁgurations in C, respectively called start conﬁguration q s and goal conﬁguration q g . Planning a collision-free motion for the robot means then generating a safe path between q s and q g if they belong to the same connected component of Cfree , and reporting a failure otherwise. 22 / 66
23. 23. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.2 Consider the case of a point robot B. In this case, the conﬁguration of the robot is described by the coordinates of point B in the workspace W = RN and the conﬁguration space C is a copy of W. Similarly, the C-obstacles are copies of the obstacles in W. 23 / 66
24. 24. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.3 Conﬁguration of spherical robot in W = RN can be described by its center point; orientation is irrelevant. C is copy of W, but C-obstacles aren’t copies of obstacles in W; growing procedure: Figure: (12.2.) C-obstacles for a circular robot in R2 ; left: the robot B, an obstacle Oi and the growing procedure for building C-obstacles, right: the conﬁguration space C and the C-obstacle COi 24 / 66
25. 25. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.4 Conﬁguration of polyhedral robot that’s free to translate with ﬁxed orientation in RN described by representative point (e.g., a vertex). C is copy of W, C-obstacles obtained through growing procedure. Figure: (12.3.) C-obstacles for a polygonal robot translating in R2 ; left: the robot B, an obstacle Oi and the growing procedure for building C-obstacles, right: the conﬁguration space C and the C-obstacle COi 25 / 66
26. 26. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.5 Polyhedral robot that can translate and rotate in RN has higher dimensional C. Ex. if W = R2 , C is represented as point (e.g., a vertex) and angular coordinate θ w.r.t ﬁxed frame, or R2 × SO(2). Repeat growing procedure for each possible θ. The C-obstacle COi is the volume generated by ‘stacking’ (in the direction of the θ axis) all the constant-orientation slices thus obtained. 26 / 66
27. 27. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.6 Robot manipulator B with rigid links B1 , . . . , Bn connected by joints, there are two kinds of C-obstacles: collision between body Bi and obstacle Oj self-collisions, i.e., interference between two links Bi and Bj of the kinematic chain. To obtain the boundary of the C-obstacle COi , we must identify through appropriate inverse kinematics computations all conﬁgurations that bring one or more links of the manipulator B in contact with Oi . Fig 12.4 shows C-obstacles for 2R manipulator in two cases (no self-collisions). Simple-shaped obstacles in W, complex C-obstacles proﬁle. (C is a torus shown as R2 for simplicity.) 27 / 66
28. 28. Motion Planning Introduction Canonical Problem Planning Techniques Conﬁguration Space Application to Robot Manipulators Examples of ObstaclesExample 12.6 (cont’d) Figure: (12.4.) C-obstacles for a wire-frame 2R manipulator in two diﬀerent cases; left: the robot and the obstacles in W = R2 , right: the conﬁguration space C and the C -obstacle region CO. 28 / 66
29. 29. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.3 Planning via Retraction Represent free conﬁguration space as a roadmap R ⊂ Cfree , i.e., network of paths describing the connectivity of Cfree . The solution of a particular instance of a motion planning problem is then obtained by connecting (retracting) to the roadmap the start conﬁguration q s and the goal conﬁguration q g , and ﬁnding a path on R between the two connection points. 29 / 66
30. 30. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialPlanning via Retraction (cont’d) Assume Cfree is subset of C = R2 and is polygonal, i.e., its boundary is entirely made of line segments, implying that the C-obstacle region is itself a polygonal subset of C. The clearance of each conﬁguration q in Cfree is γ(q ) = min ||q − s ||, q q (12.6) s ∈∂Cfree where ∂Cfree is the boundary of Cfree . The clearance γ(q ) q represents the minimum Euclidean distance between the conﬁguration q and the C-obstacle region. 30 / 66
31. 31. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialPlanning via Retraction (cont’d) Set of points on boundary of Cfree that are neighbors of q : N(q ) = {s ∈ ∂Cfree : ||q − s || = γ(q )}, q s q q (12.7) i.e., points on ∂Cfree determine the value of clearance for q . With these deﬁnitions, the Voronoi diagram of Cfree is the locus of its conﬁgurations having more than one neighbour: V(Cfree ) = {q ∈ Cfree : card(N(q )) > 1}. q q (12.8) 31 / 66
32. 32. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialVoronoi Diagrams Figure: (12.5.) An example of Voronoi diagram and the solution of a particular instance of the planning problem, obtained by retracting q s and q g on the diagram. The solution path from q s to q g consists of the two dashed segments and the thick portion of the diagram joining them 32 / 66
33. 33. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialVoronoi Diagrams (cont’d) V(Cfree ) can be considered a graph having elementary arcs of the diagram as arcs and endpoints of arcs as nodes. Voronoi diagrams locally maximize clearance, and are therefore a natural choice as a roadmap of Cfree for planning motions characterized by a healthy safety margin with respect to the possibility of collisions. 33 / 66
34. 34. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialFigure 12.6 A retraction procedure must be deﬁned to connect a conﬁguration in Cfree to the diagram. (Using V(Cfree ) as a roadmap.) Figure: (12.6.) The retraction procedure for connecting a generic conﬁguration q in Cfree to V(Cfree ) 34 / 66
35. 35. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialSteps for Motion Planning via Retraction 1. Build the generalized Voronoi diagram V(Cfree ). 2. Compute the retractions r (q s ) and r (q g ) on V(Cfree ). q q 3. Search V(Cfree ) for a sequence of consecutive arcs such that q q r (q s ) belongs to the ﬁrst and r (q g ) to the last. 4. If the search is successful, replace the ﬁrst arc of the sequence q with its subarc originating in r (q s ) and the last arc of the q sequence with its subarc terminating in r (q g ), and provide as output the path consisting of the line segment joining q s to q r (q s ), the modiﬁed arc sequence, and the line segment joining q q g to r (q g ); otherwise, report a failure. 35 / 66
36. 36. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.4 Cell Decomposition Assume Cfree can be decomposed in simply-shaped regions, called cells, with the following characteristics: ‘easy’ to compute collision-free path that joins two conﬁgurations belonging to the same cell. ‘easy’ to generate collision-free path between adjacent cells. Generate connectivity graph from the cell decomposition, where nodes represent cells and arcs represent adjacency. Diﬀerent motion planning methods based on type of cell; we explore exact and approximate cell decomposition. 36 / 66
37. 37. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.4.1 Exact Cell Decomposition Cfree partitioned as set of cells whose union gives exactly Cfree convex polygonal cells guarantee the line segments joining two conﬁgurations in the same cell lies entirely in the cell itself, and therefore in Cfree . travel safely between adjacent cells by passing through the midpoint of the segment constituting their common boundary 37 / 66
38. 38. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialExact Cell Decomposition (cont’d) Figure: (12.7.) An example of trapezoidal decomposition via the sweep line algorithm (above) and the associated connectivity graph (below). Also shown is the solution of a particular planning problem (cs = c3 and cg = c20 ), both as a channel in Cfree and as a path on the graph 38 / 66
39. 39. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialExact Cell Decomposition (cont’d) 1. Compute a convex polygonal (e.g., trapezoidal) decomposition of Cfree . 2. Build the associated connectivity graph C . 3. Search C for a channel, i.e., a sequence of adjacent cells from cs to cg . 4. If a channel has been found, extract and provide as output a collision-free path from q s to q g ; otherwise, report a failure. 39 / 66
40. 40. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.4.2 Approximate Cell Decomposition Disjoint cells of predeﬁned shape (e.g., rectangle for C = R2 ). The union of all cells approximates Cfree . Trade-oﬀ between approximation accuracy and decomposition eﬃciency: recursive algorithm starts with coarse grid whose resolution is locally increased to adapt to Cfree ’s geometry. The associated connectivity graph is searched for a channel, from which a solution path can be extracted. 40 / 66
41. 41. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialApproximate Cell Decomposition (cont’d) Figure: (12.8.) An example of motion planning via approximate cell decomposition; left: the assigned problem, right: the solution as a free channel (thick line) 41 / 66
42. 42. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.5 Probabilistic Planning Determine a ﬁnite set of collision-free conﬁgurations that adequately represent the connectivity of Cfree , and use these conﬁgurations to build a roadmap. Choose at each iteration a sample conﬁguration and check if it entails a collision between the robot and workspace obstacles. Discard samples with collisions. Else, add to current roadmap and connect if possible to other stored conﬁgurations. Sample conﬁgurations chosen according to some probability distribution. Two planners of this type are discussed. 42 / 66
43. 43. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.5.1 Probabilistic Roadmap (PRM) First, generate a random sample q rand of the conﬁguration space using a uniform probability distribution in C. Test q rand for collision; if safe, try to connect through local paths to suﬃciently ‘near’ conﬁgurations already in roadmap. ‘Nearness’ usually deﬁned on basis of Euclidean distance in C Local path between q rand and q near usually selected as rectilinear in C, chosen by local planner procedure Sample path at suﬃcient resolution; test samples for collision Stop after a maximum number of iterations or the number of connected components becomes smaller than threshold. Connect q s and q g to the same connected component of the PRM by free local paths to ﬁnd solution. 43 / 66
44. 44. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialProbabilistic Roadmap (cont’d) Figure: (12.9.) A PRM in a two-dimensional conﬁguration space (left) and its use for solving a particular planning problem (right) 44 / 66
45. 45. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.5.2 Bidirectional Rapidly-exploring Random Tree (RRT) Single-query methods quickly solve speciﬁc problem instance An exhaustive Cfree connectivity roadmap is not generated. Incremental expansion of RRT (T ) using simple randomized procedure at each iteration Generate a random conﬁguration q rand according to uniform probability distribution in C Conﬁguration q near in T closest to q rand , and new candidate q new produced on segment joining q near to q rand at a predeﬁned distance δ from q near . Collision check on q new and segment from q near to q new ; if both in Cfree , T is expanded by incorporating q new and the segment connecting it to q near . q rand is not added, only indicates direction of expansion 45 / 66
46. 46. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialBidirectional RRT (cont’d) Speedup: use two trees Ts , Tg , respectively rooted at q s , q g At each iteration, both trees expanded as previously described After set number iterations, try to connect the two trees Generate q new as expansion of Ts , try to connect Tg to q new q new acts as q rand for Tg with variable step-size–not constant δ If collision-free segment, done! else, add free portion to Tg , Tg and Ts exchange roles and connection attempt is repeated If not successful after set number iterations, conclude trees are still far apart and resume the expansion phase 46 / 66
47. 47. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialBidirectional RRT (cont’d) Figure: (12.10.) The bidirectional RRT method in a two-dimensional conﬁguration space, left: the randomized mechanism for expanding a tree, right: the extension procedure for connecting the two trees 47 / 66
48. 48. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialExtension to nonholonomic robots Adapt RRT to nonholonomic robots – not free-ﬂying Ex. robot with unicycle kinematics, rectilinear paths in conﬁguration space are not admissible in general. General approach: use motion primitives, admissible local paths in C, each produced by speciﬁc choice of velocity inputs in kinematic model. Admissible paths are concatenation of motion primitives. Velocity inputs follow for unicycle robot: υ=υ ¯ ω = {−¯ , 0, ω } ω ¯ t ∈ [0, ∆], (12.9) resulting in three admissible local paths: ﬁrst and third are left/right turn while second is rectilinear 48 / 66
49. 49. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialBidirectional RRT for a unicycle robot Figure: (12.11.) RRT-based motion planning for a unicycle; left: a set of motion primitives, right: an example of RRTs 49 / 66
50. 50. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.6 Artiﬁcial Potential Previous methods are oﬀ-line, requiring a priori knowledge of geometry and pose of obstacles in workspace On-line planning uses partial information on the workspace gathered during the motion Robot (point in C) moves under inﬂuence of potential ﬁeld U U is superposition of attractive potential to the goal and repulsive potential from C-obstacle region. Artiﬁcial force generated by U at each conﬁguration q is the negative gradient − U(q ) of the potential, indicating the q most promising direction of local motion 50 / 66
51. 51. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.6.1 Attractive Potential Guide robot to goal q g using a paraboloid with vertex in q g 1 1 Ua1 (q ) = kae T (q )e (q ) = ka ||e (q )||2 , q q e q e q (12.10) 2 2 with ka > 0 and e = q g − q is the ‘error’ vector w.r.t the goal. (Always positive and global minimum at goal, where it is zero) Resulting attractive force is f a1 (q ) = − Ua1 (q ) = kae (q ) q q q (12.11) Alternatively, conical attractive potential and resulting force q e (q ) Ua2 (q ) = ka ||e (q )|| q e q q f a2 (q ) = ka (12.12; 12.13) ||e (q )|| e q 51 / 66
52. 52. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialAttractive Potential (cont’d) Figure: (12.12.) The shape of the paraboloidic attractive potential Ua1 (left) and of the conical attractive potential Ua2 (right) in the case C = R2 , for ka = 1 52 / 66
53. 53. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.6.2 Repulsive Potential Prevents collisions with obstacles as robot moves under inﬂuence of attractive force f a Assume C-obstacle region partitioned as convex components COi , i = 1, . . . , p. kr ,i γ 1 1 γ q ηi (q ) − η0,i if ηi (q ) ≤ η0,i q q Ur ,i (q ) = (12.14) 0 q ) > η0,i if ηi (q where kr ,i > 0, ηi (q ) = minq ∈COi ||q − q || is the distance of q q q from COi , η0,i is range of inﬂuence of COi and γ = 2, 3, . . . 53 / 66
54. 54. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialRepulsive Potential (cont’d) The repulsive force resulting from Ur ,i is f r ,i (q ) − Ur ,i (q ) q = q kr ,i γ−1 1 1  ηi2 (q ) q ηi (q ) − η0,i q ηi (q ) if ηi (q ) ≤ η0,i q = q  0 q if ηi (q ) > η0,i (12.15) The aggregate repulsive potential is p q Ur (q ) = q Ur ,i (q ). (12.16) i=1 54 / 66
55. 55. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialRepulsive Potential (cont’d) Figure: (12.13.) The equipotential contours of the repulsive potential Ur in the range of inﬂuence of a polygonal C-obstacle in C = R2 , for kr = 1 and γ = 2 55 / 66
56. 56. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.6.3 Total Potential Total potential Ut is superposition of attractive and aggregate repulsive potentials: q q q Ut (q ) = Ua (q ) + Ur (q ). (12.17) This results in the force ﬁeld p f t (q ) = − Ut (q ) = f a (q ) + q q q q f r ,i (q ) (12.18) i=1 56 / 66
57. 57. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialTotal Potential (cont’d) Figure: (12.14.) The total potential in C = R2 obtained by superposition of a hyperboloidic attractive potential and a repulsive potential for a rectangular C-obstacle: left: the equipotential contours, right: the resulting force ﬁeld 57 / 66
58. 58. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.6.4 Planning Techniques Three collision-free motion planning approaches using Ut and f t : q 1. Consider f t (q ) as a vector of generalized forces that induce a motion of the robot in accordance with its dynamic model. q τ = f t (q ) (12.19) 2. The second method regards the robot as a unit point mass q moving under the inﬂuence of f t (q ), as in ¨ q q = f t (q ) (12.20) q 3. The third possibility is to interpret the force ﬁeld f t (q ) as a desired velocity for the robot, by letting ˙ q q = f t (q ) (12.21) 58 / 66
59. 59. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialPlanning Techniques (cont’d) (12.19) directly represents control inputs for the robot. It generates smoother paths because reactions to presence of obstacles are naturally ‘ﬁltered’ through the robot dynamics. (12.20) requires the solution of the inverse dynamics problem (to compute the generalized forces τ ). (12.21) can be used on-line in a kinematic control scheme. It executes the motion corrections faster, and may be considered safer. Further, it guarantees the asymptotic stability of q g (the robot reaches the goal with zero velocity). 59 / 66
60. 60. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialPlanning Techniques (cont’d) Commonly, numerical integration of (12.21) via Euler method: f q q k+1 = q k + Tf t (q k ) (12.22) where q k and q k+1 represent the current and next robot conﬁguration, and T is integration step. (Gradient method for q minimizing Ut (q ), aka the method of steepest descent). Local minima can ‘trap’ generated paths from reaching their goal (methods are not complete in general). Grid-discretized steepest descent using best-ﬁrst search is resolution complete but exponential in dimension of C Improve: bound ‘basin ﬁlling’ iterations before random walk 60 / 66
61. 61. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial Potential12.6.5 Navigation Functions Best-ﬁrst search (basic or randomized) solve the local minima problem, but may generate ineﬃcient paths Navigation functions: artiﬁcial potentials with no local minima This property already holds if the C-obstacles are spheres Could approximate-by-excess all C-obstacles with spheres Could use diﬀerentiable homeomorphism to map C-obstacles to collection of spheres, generate classic total potential in transformed space, and map back to original space. Could use harmonic functions, solutions of particular diﬀerential equation describing physical process of heat transmission or ﬂuid dynamics 61 / 66
62. 62. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artiﬁcial PotentialNavigation Functions (cont’d) Figure: (12.15.) An example of numerical navigation function in a simple two-dimensional gridmap using 1-adjacency. Cells in gray denote a particular solution path obtained by following from the start (cell 12) the steepest descent of the potential on the gridmap 62 / 66
63. 63. Introduction Planning Techniques Application to Robot Manipulators12.7 The Robot Manipulator Case High computational complexity due to high dimension of C (typically n ≥ 4) and rotational DOFs (revolute joints). Reduce the dimension of C approximating by excess robot size (e.g, replace wrist/end-eﬀector in six-DOF anthropomorphic manipulator with the volume their joint ‘sweeps’.) Shape of C-obstacles is complex due to rotational DOFs and nonlinearity of inverse kinematics (recall Fig. 12.4). Non-polyhedral shape doesn’t allow application of methods presented in Secs. 12.3 and 12.4. 63 / 66
64. 64. Introduction Planning Techniques Application to Robot ManipulatorsThe Robot Manipulator Case (cont’d) The most convenient choice for oﬀ-line planning is probabilistic methods, due to high-dimensional conﬁguration spaces. (Computation of C-obstacles is not required). For on-line planning, use method from artiﬁcial potentials. To avoid computation of C-obstacles and plan in reduced dimensional space, the potential is directly built in workspace W rather than C, and acts on manipulator control points set. A point representing end-eﬀector (assigned goal of motion planning problem) and at least one point for each link body. Use attractive-repulsive ﬁeld for end-eﬀector, and repulsive ﬁeld for other control points distributed on manipulator links. 64 / 66
65. 65. Introduction Planning Techniques Application to Robot ManipulatorsThe Robot Manipulator Case (cont’d) Let p1 , . . . , pP−1 represent body control points and pP the end-eﬀector control point 1. impose the generalized forces from combined action of various force ﬁelds on control points in W, according to P−1 τ =− J T (q ) Ur (p i ) − J T (q ) Ut (p p ) i q p P q p (12.23) i=1 q where J i (q ), i = 1, . . . , P, denotes the Jacobian of the direct kinematics function associated with the control point p (q ). q ˙ 2. Alternatively, we could feed these as joint velocities q rather than forces τ (12.24). These options correspond to (12.19) and (12.21), inheriting the same characteristics. 65 / 66
66. 66. Introduction Planning Techniques Application to Robot ManipulatorsExample of Artiﬁcial Potentials Figure: (12.16.) Examples of motion planning via artiﬁcial potentials acting on control points for a planar 3R manipulator; left: planning is successful and leads to a collision-free motion between the start S and the goal G, right: a failure is reported because the manipulator is stuck at a force equilibrium 66 / 66