Robotics: Modelling, Planning and Control

2,528 views
2,356 views

Published on

0 Comments
1 Like
Statistics
Notes
  • Be the first to comment

No Downloads
Views
Total views
2,528
On SlideShare
0
From Embeds
0
Number of Embeds
5
Actions
Shares
0
Downloads
88
Comments
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 Configuration Space Examples of ObstaclesPlanning Techniques Retraction Cell Decomposition Probabilistic Planning Artificial PotentialApplication to Robot Manipulators 2 / 66
  3. 3. Motion Planning Introduction Canonical Problem Planning Techniques Configuration 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 Configuration 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 fixed (fittings, conveyor belts) or mobile (passengers, workers) Motion planning amounts to deciding which path to follow to execute transfer task from initial to final posture without colliding with obstacles 4 / 66
  5. 5. Motion Planning Introduction Canonical Problem Planning Techniques Configuration 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, off-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 difficult, and is still an active topic of research 5 / 66
  6. 6. Motion Planning Introduction Canonical Problem Planning Techniques Configuration 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 fixed (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., fixed 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-flying, that is, the robot is not subject to any kinematic constraint. 6 / 66
  7. 7. Motion Planning Introduction Canonical Problem Planning Techniques Configuration Space Application to Robot Manipulators Examples of ObstaclesCanonical Problem Statement The motion planning problem is the following given an initial and a final posture of B in W, find 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 Configuration 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 Configuration Space Application to Robot Manipulators Examples of Obstacles12.2 Configuration 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 identifies the configuration of the robot. This associates to each posture of the latter a point in the configuration space, i.e., the set of all the configurations that the robot can assume. 9 / 66
  10. 10. Motion Planning Introduction Canonical Problem Planning Techniques Configuration Space Application to Robot Manipulators Examples of ObstaclesConfiguration 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 find 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 configuration. 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesConfiguration Space Examples The configuration 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 fixed reference frame. The configuration space C is R2 × SO(2), with dimension n = 3. For a polyhedral mobile robot in W = R3 , the configuration space C is R3 × SO(3), whose dimension is n = 6. For fixed-base planar manipulator with n revolute joints, the configuration 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesConfiguration Space Examples (cont’d) For a fixed-base spatial manipulator with n revolute joints, the configuration space is a subset of (R3 × SO(3))n . Since in this case each joint imposes five constraints on the following body, the dimension of C is 6n − 5n = n. For a unicycle-like vehicle with a trailer in R2 , configuration space is subset of (R2 × SO(2)) × (R2 × SO(2)). If the trailer is connected to the unicycle by revolute joint, configuration 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 Configuration 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesExample 12.1 The configuration 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 configurations 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesExample 12.1 Figure: (12.1.) The configuration 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 Configuration 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 configuration space is in general more complex than that of a Euclidean space. 16 / 66
  17. 17. Motion Planning Introduction Canonical Problem Planning Techniques Configuration Space Application to Robot Manipulators Examples of Obstacles12.2.1 Distance in configuration space C Given a configuration 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 configurations 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 configurations 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesDistance in configuration space C (cont’d) q However, the use of d1 (q ) is cumbersome. We often choose the simple Euclidean norm as the configuration 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 Configuration 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 configuration space C is called C-obstacle and is defined as COi = {q ∈ C : B(q ) ∩ Oi = ∅}. q q (12.3) In other words, COi is the subset of configurations 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 Configuration 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 defines 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 configuration space, that is, the subset of robot configurations that do not cause collision with the obstacles. A path in configuration space is called free if it is entirely contained in Cfree . 20 / 66
  21. 21. Motion Planning Introduction Canonical Problem Planning Techniques Configuration 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 configuration there exists a path that joins them – the free configuration space Cfree may not be connected because of occlusions due to C-obstacles. Note also that the assumption of free-flying robot in the canonical problem means that the robot can follow any path in the free configuration space Cfree . 21 / 66
  22. 22. Motion Planning Introduction Canonical Problem Planning Techniques Configuration Space Application to Robot Manipulators Examples of ObstaclesCompact Canonical Motion Planning Problem Assume the initial and final posture of the robot B in W are mapped to corresponding configurations in C, respectively called start configuration q s and goal configuration 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesExample 12.2 Consider the case of a point robot B. In this case, the configuration of the robot is described by the coordinates of point B in the workspace W = RN and the configuration 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 Configuration Space Application to Robot Manipulators Examples of ObstaclesExample 12.3 Configuration 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 configuration space C and the C-obstacle COi 24 / 66
  25. 25. Motion Planning Introduction Canonical Problem Planning Techniques Configuration Space Application to Robot Manipulators Examples of ObstaclesExample 12.4 Configuration of polyhedral robot that’s free to translate with fixed 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 configuration space C and the C-obstacle COi 25 / 66
  26. 26. Motion Planning Introduction Canonical Problem Planning Techniques Configuration 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 fixed 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 Configuration 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 configurations 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 profile. (C is a torus shown as R2 for simplicity.) 27 / 66
  28. 28. Motion Planning Introduction Canonical Problem Planning Techniques Configuration 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 different cases; left: the robot and the obstacles in W = R2 , right: the configuration space C and the C -obstacle region CO. 28 / 66
  29. 29. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial Potential12.3 Planning via Retraction Represent free configuration 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 configuration q s and the goal configuration q g , and finding 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 Artificial 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 configuration 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 configuration q and the C-obstacle region. 30 / 66
  31. 31. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial 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 definitions, the Voronoi diagram of Cfree is the locus of its configurations 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 Artificial 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 Artificial 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 Artificial PotentialFigure 12.6 A retraction procedure must be defined to connect a configuration in Cfree to the diagram. (Using V(Cfree ) as a roadmap.) Figure: (12.6.) The retraction procedure for connecting a generic configuration q in Cfree to V(Cfree ) 34 / 66
  35. 35. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial 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 first and r (q g ) to the last. 4. If the search is successful, replace the first 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 modified 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 Artificial 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 configurations 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. Different 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 Artificial 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 configurations 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 Artificial 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 Artificial 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 Artificial Potential12.4.2 Approximate Cell Decomposition Disjoint cells of predefined shape (e.g., rectangle for C = R2 ). The union of all cells approximates Cfree . Trade-off between approximation accuracy and decomposition efficiency: 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 Artificial 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 Artificial Potential12.5 Probabilistic Planning Determine a finite set of collision-free configurations that adequately represent the connectivity of Cfree , and use these configurations to build a roadmap. Choose at each iteration a sample configuration 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 configurations. Sample configurations 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 Artificial Potential12.5.1 Probabilistic Roadmap (PRM) First, generate a random sample q rand of the configuration space using a uniform probability distribution in C. Test q rand for collision; if safe, try to connect through local paths to sufficiently ‘near’ configurations already in roadmap. ‘Nearness’ usually defined 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 sufficient 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 find solution. 43 / 66
  44. 44. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial PotentialProbabilistic Roadmap (cont’d) Figure: (12.9.) A PRM in a two-dimensional configuration 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 Artificial Potential12.5.2 Bidirectional Rapidly-exploring Random Tree (RRT) Single-query methods quickly solve specific 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 configuration q rand according to uniform probability distribution in C Configuration q near in T closest to q rand , and new candidate q new produced on segment joining q near to q rand at a predefined 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 Artificial 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 Artificial PotentialBidirectional RRT (cont’d) Figure: (12.10.) The bidirectional RRT method in a two-dimensional configuration 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 Artificial PotentialExtension to nonholonomic robots Adapt RRT to nonholonomic robots – not free-flying Ex. robot with unicycle kinematics, rectilinear paths in configuration space are not admissible in general. General approach: use motion primitives, admissible local paths in C, each produced by specific 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: first 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 Artificial 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 Artificial Potential12.6 Artificial Potential Previous methods are off-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 influence of potential field U U is superposition of attractive potential to the goal and repulsive potential from C-obstacle region. Artificial force generated by U at each configuration 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 Artificial 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 Artificial 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 Artificial Potential12.6.2 Repulsive Potential Prevents collisions with obstacles as robot moves under influence 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 influence of COi and γ = 2, 3, . . . 53 / 66
  54. 54. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial 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 Artificial PotentialRepulsive Potential (cont’d) Figure: (12.13.) The equipotential contours of the repulsive potential Ur in the range of influence 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 Artificial 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 field 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 Artificial 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 field 57 / 66
  58. 58. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial 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 influence of f t (q ), as in ¨ q q = f t (q ) (12.20) q 3. The third possibility is to interpret the force field 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 Artificial 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 ‘filtered’ 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 Artificial 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 configuration, 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-first search is resolution complete but exponential in dimension of C Improve: bound ‘basin filling’ iterations before random walk 60 / 66
  61. 61. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial Potential12.6.5 Navigation Functions Best-first search (basic or randomized) solve the local minima problem, but may generate inefficient paths Navigation functions: artificial 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 differentiable 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 differential equation describing physical process of heat transmission or fluid dynamics 61 / 66
  62. 62. Retraction Introduction Cell Decomposition Planning Techniques Probabilistic Planning Application to Robot Manipulators Artificial 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-effector 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 off-line planning is probabilistic methods, due to high-dimensional configuration spaces. (Computation of C-obstacles is not required). For on-line planning, use method from artificial 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-effector (assigned goal of motion planning problem) and at least one point for each link body. Use attractive-repulsive field for end-effector, and repulsive field 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-effector control point 1. impose the generalized forces from combined action of various force fields 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 Artificial Potentials Figure: (12.16.) Examples of motion planning via artificial 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

×