Robot Arm Transformations, Path Planning, and Trajectories!

Transcription

1 Robot Arm Transformations, Path Planning, and Trajectories Robert Stengel Robotics and Intelligent Systems MAE 345, Princeton University, 217 Forward and inverse kinematics Path planning Voronoi diagrams and Delaunay triangulation Probabilistic Road Map Rapidly Exploring Random Tree Closed-form trajectories; connecting the dots Polynomials and splines Acceleration profiles Copyright 217 by Robert Stengel. All rights reserved. For educational use only. 1 Manipulator Maneuvering Spaces Joint space: Vector of joint variables, e.g., r J = waist shoulder elbow wrist bend flange wrist twist End-effecter space: Vector of end-effecter positions, e.g., r E = x tool y tool z tool tool tool tool Task space: Vector of task-dependent positions, e.g., locating a symmetric grinding tool above a horizontal surface: r T = x tool y tool z tool tool tool T T T 2

2 Forward and Inverse Transformations of a Robotic Assembly Forward Transformation Transforms homogeneous coordinates from tool frame to reference frame coordinates s base = A base tool s tool = A waist A shoulder A elbow A wristbend A flange A wristtwist s tool Inverse Transformation Transform homogeneous coordinate from reference frame to tool frame coordinates s tool = A tool base s base = A 1 wristtwista 1 flangea 1 wristbenda 1 elbowa 1 shouldera 1 waists base 3 Forward and Inverse Kinematics Between Joints, Tool Position, and Tool Orientation Forward Kinematic Problem: Compute the position of the tool in the reference frame that corresponds to a given joint vector (i.e., vector of link variables) s base = A waist A shoulder A elbow A wristbend A flange A wristtwist s tool = A base tool s tool To Be Determined Given Inverse Kinematic Problem: Find the vector of link variables that corresponds to a desired task-dependent position A waist A shoulder A elbow A wristbend A flange A wristtwist s tool = A base tool s = s base To Be Determined Given 4

3 Forward and Inverse Kinematics Single-Link Example Forward Kinematic Problem: Specify task-dependent position that corresponds to a given joint variable ( = n ) s n1 = A( z n1, n )A( z n1,d n )A( x n1,l n )A( x n1, n ) s n = cos n sin n cos n sin n sin n l n cos n sin n cos n cos n cos n sin n l n sin n sin n cos n d n 1 ) ) ) ) () s n Red: Known Blue: Unknown 5 Forward and Inverse Kinematics Single-Link Example Inverse Problem: Find the joint variable,, that corresponds to a desired task-dependent position s n1 = A( z n1, n )A( z n1,)a( x n1,l n )A( x n1,9 ) s n = cos n sin n l n cos n sin n cos n l n sin n 1 1 ( ( ( ( s n Red: Known Blue: Unknown x n1 = x n cos n + z n sin n + l n cos n y n1 = x n sin n z n cos n + l n sin n In this simple case, check by elimination and inverse trig functions 6

4 Manipulator Redundancy and Degeneracy More than one link configuration may provide a given end point Redundancy: Finite number of joint vectors provide the same task-dependent vector Degeneracy: Infinite number of joint vectors provide the same task-dependent vector Co-linear joint axes are degenerate 7 Why? Space Robot Arms are Highly Redundant 8

5 Transformations for a Two-Link Manipulator cos 1 sin 1 H 1 = sin 1 cos 1 1 ( ( ( ; r = l 1 ( ( ( Example: Type 1 Two-Link Manipulator, neglecting offset (e.g., Puma geometry without waist and wrist) A 1 = 1 H r ( ) 1 = cos 1 sin 1 (l 1 (sin 1 cos A 2 1 = 2 H 1 r 1 ( ) 1 = cos 2 sin 2 (l 2 (sin 2 cos Position of Distal Joint Relative to the Base (2-link manipulator) B = s base = = x y z 1 base cos B (sin B l 1 cos 1 + l 2 cos B = A 1 A 1 sin 2 s distal = B cos B l 1 sin 1 + l 2 sin B 1 1 l 1 cos 1 + l 2 cos B l 1 sin 1 + l 2 sin B 1 1 distal 1

6 Path Planning Baxter Path Planning (UNC, 214) 11 Path Planning Environment idealization and nominal path Trajectory decomposition and segmentation Well-defined Start and Goal Waypoints Path primitives (line, circle, etc.) Timing and coordination Obstacle detection and avoidance Feasibility and regulation Optimization and constraint 12

7 Path Planning with Waypoints Define Start, Goal, and Waypoints by position and time Connect the dots Various interpolation methods Straight lines Polynomials Splines Generate associated velocity and acceleration Satisfy trajectory constraints 13 Path Planning with Obstacles and Destinations Given set of points, e.g., obstacles, destinations, or centroids of multiple points Chart best path from start to goal Tessellation (tiling) of decision space 2-D Voronoi diagram Polygons with sides equidistant to two nearest points (black dots) Voronoi diagram 14

8 Delaunay Triangulation Constructs the Voronoi Diagram Threats/obstacles are black points Edges (black) connect all triplets of black points lying on circumferences of empty circles, i.e., circles containing no other black points Circumcircle centers are red points Voronoi segment boundaries (red) connect centers and are perpendicular to each edge Delaunay_triangulation 15 Voronoi Diagrams in Path Planning Threat/obstacle avoidance Choose path with farthest distance from obstacles 16

9 Path Planning with Potential Fields Map features attract or repel path from Start to Goal, e.g., +/ gravity fields 17 Path Planning on Occupancy Grid Admissible and Inadmissible Blocks Identify feasible paths from Start to Goal Chose path that best satisfies criteria, e.g., Simplicity of calculation Lowest cost Highest performance 18

10 Bug Path Planning 1) Identify shortest unconstrained path from Start to Goal, i.e., green path 2) Chose path that navigates the boundary 1) Stays as close as to possible to unconstrained path (dashed line) 2) Satisfies constraint 3) Follows simple rule, e.g., stay to the left 19 D* or A* Path Planning (TBD) Determine occupancy cost of each block Chose path from Start to Goal that Reduce occupancy cost with each step 2

11 Probabilistic Road Map (PRM) Construct random configuration of admissible points Connect admissible points to nearest neighbors Assess incremental cost of traveling along each edge between points Query to find all feasible paths from Start to Goal Select lowest cost path 21 Rapidly Exploring Random Tree (RRT*) Space-filling tree evolves from Start Open-loop trajectories with state constraints Initially feasible solution converges to optimal solution through searching Committed trajectories Branch-and-bound tree adaptation 22

12 Trajectories 23 One-Dimensional Trajectory Constant Velocity, v Velocity, v(t) vs. t, is constant v( t) = x ( t) = v( ) Position, x(t) vs. t, is a straight line x( t) = x( ) + v( )t 24

13 One-Dimensional Trajectory Constant Velocity, v Position specified at and t x t = 1 1 t Velocity at to be determined = 1 1 t 1 x t = 1 1 t 1 t x t 25 One-Dimensional Trajectory Constant Acceleration, a Velocity, v(t) vs. t, is a straight line v( t) = x ( t) = v( ) + at Position, x(t) vs. t, is a parabola x( t) = x( ) + v( t) + at

14 One-Dimensional Trajectory Constant Acceleration, a Position specified at and t; velocity specified at x t = 1 1 t t a Acceleration at to be determined a 1 = 1 t t x t = t 2 2 t 2 2 t x t 27 One-Dimensional Trajectory Constant Jerk, j, = Derivative of Acceleration, a Acceleration, a(t) vs. t, is a straight line a( t) = v ( t) = x ( t) = a( ) + jt Velocity, v(t) vs. t, is a parabola v( t) = x ( t) = v( ) + a( )t + jt 2 2 Position, x(t) vs. t, is cubic x( t) = x( ) + v( )t + a( )t jt

15 One-Dimensional Trajectory Constant Jerk, j Position and velocity specified at and t; acceleration and jerk at to be determined x t v t = 1 1 t t 2 2 t t t 2 2 a j 29 One-Dimensional Trajectory Constant Jerk, j Find a() and j to produce desired position and velocity a j = Start Finish Start Finish x t v t 1 1 t t 2 2 t t t 2 2 = t t 2 2 t t t 2 2 x t v t = a j Start Start TBD TBD Inverse of (4 x 4) relationship defines required a() and j t 2 6 t 2 4 t 2 t 12 t 3 12 t 3 6 t 2 6 t 2 x t v t 3

16 Further Derivatives Snap, s, = Derivative of Jerk, j Crackle, c, = Derivative of Snap, s What is the derivative of Crackle? Pop 31 One-Dimensional Trajectory Constant Crackle, c Snap, s(t) vs. t, is linear in time = d j( t) s t dt = +s + ct Jerk, j(t) vs. t, is quadratic j( t) = a ( t) = j( ) + s( )t + ct 2 2 Acceleration, a(t) vs. t, is cubic a( t) = v ( t) = x ( t) = a( ) + j( )t + s( )t ct

17 One-Dimensional Trajectory with Constant Crackle, c Velocity, v(t) vs. t, is quartic v( t) = x ( t) = v( ) + a( )t + jt s( )t ct 4 24 Position, x(t) vs. t, is quintic x( t) = x( ) + v( )t + a( )t j( )t s( )t ct One-Dimensional Trajectory with Constant Crackle, c Position, velocity, and acceleration specified at and t x t v t a a t = 1 1 t t 2 2 t 3 6 t 4 24 t t t 2 2 t 3 6 t t t 2 2 t 3 6 a j s c 34

18 One-Dimensional Trajectory Inverse of (6 x 6) relationship defines controls a j s c = 1 1 t t 2 2 t 3 6 t 4 24 t t t 2 2 t 3 6 t t t 2 2 t x t v t a a t a j s c = t 3 6 t 3 36 t 2 24 t 2 9 t 3 t 36 t 4 36 t t t 3 36 t 2 24 t 2 72 t 5 72 t 5 36 t 4 36 t 4 6 t 3 6 t 3 x t v t a a t 35 One-Dimensional Trajectory Eliminate unnecessary equations and define acceleration constants j s c 6 t 3 6 t 3 36 t 2 24 t 2 9 t 3 t = 36 t 4 36 t t t 3 36 t 2 24 t 2 72 t 5 72 t 5 36 t 4 36 t 4 6 t 3 6 t 3 x t v t a a t Corresponding acceleration and force are specified by a( t) = a( ) + j( )t + s( )t ct 3 6 = a control ( t) + a gravity ( t) + a disturbance ( t) = f control t + f gravity ( t) + f disturbance ( t) m t 36

19 One-Dimensional Trajectory Calculate trajectory components, given acceleration constants x t v t a t = 1 t t 2 2 t 3 6 t 4 24 t t t 2 2 t 3 6 t t t 2 2 t 3 6 a j s c Example Calculate constants for x() =, x(1) = = Calculate trajectory, given constants for t f = 1 1 x t v t a t = 1 t t 2 2 t 3 6 t 4 24 t t t 2 2 t 3 6 t t t 2 2 t

20 1-D Example a net ( t) = ( ) +.6t.36t t Examples with Different End Conditions v(1) = 1 v(1) = 1, a(1) = 2 4

21 Sensitivity to Errors 1 error in Crackle 41 Constrained 1-D Trajectories v( t) v max a net ( t) a max What are the alternatives for achieving desired end conditions? 42

22 Alternatives for Reaching End Position Increase end time Lower max/min values of velocity and acceleration Fatten velocity and acceleration profiles Multi-segment trajectory Unconstrained arcs Constrained arcs (velocity and/or acceleration held constant 43 Connect the Dots Interpolation Piecewise polynomials (linear -> quintic) End-point discontinuities End-point constraints Parabolic blend Single polynomial through all points Polynomial degree = of points Sensitivity to high-degree terms (e.g., ct 6 ) Possibility of large excursions between points Polynomials through adjacent points e.g., cubic B splines Kriging 44

23 Next Time: Time Response of Dynamic Systems 45 Supplemental Material 46

24 Joint Variables for Different Link Types Joint Variable = n Joint Variable = n Joint Variable = n Joint Variable = n Joint Variable = d n Joint Variable = n Joint Variable = n Joint Variable = d n 47 Position of Distal Joint Relative to the Base (2-link manipulator) Tool Plate Suppose a tool plate is fixed to the distal joint at (x y z) distalt ; then s base = x y z 1 base cos B (sin B l 1 cos 1 + l 2 cos B = A 1 A 1 sin 2 s distal = B cos B l 1 sin 1 + l 2 sin B 1 1 x cos B ( ysin B + l 1 cos 1 + l 2 cos B xsin = B + ycos B + l 1 sin 1 + l 2 sin B z 1 Alternatively, straightforward trigonometry could be used in this example x y z 1 distal 48

25 Tool Plates and Jaws Tool Changer v=g8zqooledhyfeature=related Four-Bar Linkage and 2 nd Set of Jaws Another Tool Changer v=lkpnt_nudlcfeature=related 49 Robot Arms for Space Dextre Manipulator, ISS Canadarm2, ISS Curiosity Robot Arm Kibo Arm, ISS 5

26 Snake-Like Manipulator Multi-Jointed Arms Octopus Arms OctArm Tentacle Arm 51 DNA Microarray- Spotting Robot DNA strands representing different genes are spotted on a microscope slide Finished slide is used to analyze DNA from tissue samples 52

27 American Android Multi-Arm UGV (David Handelman, *89) Voronoi Diagrams in Data Processing Computer graphics textures (2-D and 3-D meshes) Density characterization (3-D mesh) Vector quantization in data compression 54

28 One-Dimensional Trajectory with Constant Kix, k Position, velocity, acceleration, and jerk specified at and t 1 v( ) 1 1 a( ) 1 j( ) = 18 /( 19t 4 ) 18 / ( 19t 4 ) 168 / ( 19t 3 ) 12 / ( 19t 3 ) 36 / ( t 2 ) 96 / ( 19t 2 ) 156 / 19t s( ) 144 / ( 19t 5 ) 144 / ( 19t 5 ) 1116 / ( 19t 4 ) 324 / ( 19t 4 ) 192 / ( t 3 ) 312 / ( 19t 3 ) 564 / 19t 2 c( ) 54 / 19t p( ) ( 6 ) 54 / ( 19t 6 ) 288 / ( 19t 5 ) 216 / ( 19t 5 ) 36 / ( t 4 ) 324 / ( 19t 4 ) 72 / 19t 3 k( ) 72 / ( 19t 7 ) 72 / ( 19t 7 ) 216 / ( 19t 6 ) 54 / ( 19t 6 ) 144 / ( 19t 5 ) 6 / 19t 4 12 / ( 19t) 96 / ( 19t 2 ) 12 / ( 19t 3 ) 18 / ( 19t 4 ) x( t) v( ) v( t) a( ) a( t) j( ) j( t ) Snap, crackle, pop, and kix computed One-Dimensional Trajectory Inverse of (8 x 8) relationship defines controls x t v t a a t j j t = 1 1 t t 2 2 t 3 6 t 4 24 t 5 12 t 6 6 t t t 2 2 t 3 6 t 4 24 t 5 12 t t t 2 2 t 3 6 t 6 6 t t t 2 2 t 3 6 t 4 24 a j s c p k