Survey
* Your assessment is very important for improving the workof artificial intelligence, which forms the content of this project
* Your assessment is very important for improving the workof artificial intelligence, which forms the content of this project
Introduction to ROBOTICS Velocity Analysis Jacobian University of Bridgeport 1 Kinematic relations 1 2 3 4 5 6 X=FK(θ) Joint Space θ =IK(X) x y z X Task Space Location of the tool can be specified using a joint space or a cartesian space description Velocity relations • Relation between joint velocity and cartesian velocity. • JACOBIAN matrix J(θ) 1 X J ( ) x y 2 z 3 x 4 y 5 J 1 ( ) X z 6 Joint Space Task Space Jacobian • Suppose a position and orientation vector of a manipulator is a function of 6 joint variables: (from forward kinematics) X = h(q) h1 (q1 , q2 , , q6 ) q1 x h (q , q , , q ) q y 6 2 2 1 2 h3 (q1 , q2 , , q6 ) q3 z X h( ) 61 h (q , q , , q ) 6 4 1 2 q4 h5 (q1 , q2 , , q6 ) q5 h6 (q1 , q2 , , q6 ) 61 q6 Jacobian Matrix Forward kinematics X 61 h(qn1 ) d dh(q) dq dh(q) X 61 h(qn1 ) q dt dq dt dq x y z x y z q1 q dh(q ) 2 dq 6n q n n1 X 61 J 6n q n1 dh(q) J dq Jacobian Matrix x y q1 z dh(q ) q 2 x dq 6n y q n n1 z dh(q ) J dq 6n Jacobian is a function of q, it is not a constant! h1 q 1 h2 q1 h 6 q1 h1 q2 h2 q2 h6 q2 h1 qn h2 qn h6 qn 6n Jacobian Matrix x y z V X x y z Linear velocity x V y z The Jacbian Equation X J 6n q n1 Angular velocity x y z q1 q q n1 2 q n n1 Example • 2-DOF planar robot arm – Given l1, l2 , Find: Jacobian x l1 cos1 l2 cos(1 2 ) h1 (1 , 2 ) y l sin l sin( ) h ( , ) 1 1 2 1 2 2 1 2 1 x Y J y 2 h1 J 1 h2 1 (x , y) 2 l2 1 l1 h1 2 l1 sin 1 l2 sin( 1 2 ) l2 sin( 1 2 ) h2 l1 cos 1 l2 cos(1 2 ) l2 cos(1 2 ) 2 Singularities • The inverse of the jacobian matrix cannot be calculated when det [J(θ)] = 0 • Singular points are such values of θ that cause the determinant of the Jacobian to be zero • Find the singularity configuration of the 2-DOF planar robot arm determinant(J)=0 Not full rank x X J 1 y 2 V l1 sin 1 l2 sin( 1 2 ) l2 sin( 1 2 ) J l cos l cos( ) l cos( ) 1 2 1 2 2 1 2 1 l2 Y 2 =0 Det(J)=0 2 0 1 l1 x (x , y) Jacobian Matrix • Pseudoinverse – Let A be an mxn matrix, and let A be the pseudoinverse of A. If A is of full rank, then A can be computed as: AT [ AAT ]1 1 A A [ AT A]1 AT mn mn mn Jacobian Matrix – Example: Find X s.t. 1 0 2 3 1 1 0 x 2 1 1 1 4 1 5 1 1 T T 1 A A [ AA ] 0 1 1 5 1 2 9 2 0 4 2 Matlab Command: pinv(A) to calculate A+ 5 1 x A b 13 9 16 Jacobian Matrix • Inverse Jacobian J11 J X Jq 21 J 61 q J X 1 J12 J16 q q J 22 J 26 q q q J 62 J 66 q 1 q5 2 3 4 5 6 q1 • Singularity – rank(J)<n : Jacobian Matrix is less than full rank – Jacobian is non-invertable – Boundary Singularities: occur when the tool tip is on the surface of the work envelop. – Interior Singularities: occur inside the work envelope when two or more of the axes of the robot form a straight line, i.e., collinear Singularity • At Singularities: – the manipulator end effector cant move in certain directions. – Bounded End-Effector velocities may correspond to unbounded joint velocities. – Bounded joint torques may correspond to unbounded End-Effector forces and torques. Jacobian Matrix • If ax bx A a y , B by az bz • Then the cross product i j A B ax ay bx by a y bz az by az (axbz az bx ) bz axby a y bx k Remember DH parmeter ci s Ai i 0 0 -c isi s isi ci c i -s i ci s i c i 0 0 a i ci a isi di 1 • The transformation matrix T T0i A1 A2 ..... Ai Jacobian Matrix J J1 J 2 .... J n Jacobian Matrix 2-DOF planar robot arm Given l1, l2 , Find: Jacobian • Here, n=2, (x , y) 2 l2 1 l1 ci s Ai i 0 0 -c isi s isi ci c i -s i ci s i c i 0 0 a i ci a isi di 1 Where (θ1 + θ2 ) denoted by θ12 and 0 Z 0 Z1 0 1 0 a1 cos 1 a1 cos 1 a2 cos(1 2 ) O0 0 , O1 a1 sin 1 , O2 a1 sin 1 a2 sin(1 2 ) 0 0 0 19 Jacobian Matrix 2-DOF planar robot arm Given l1, l2 , Find: Jacobian • Here, n=2 z0 (o2 o0 ) z1 (o2 o1 ) J1 , J2 z z 0 1 (x , y) 2 l2 1 l1 Jacobian Matrix z0 (o2 o0 ) J1 z 0 0 a1 cos 1 a2 cos(1 2 ) Z 0 (o2 o0 ) 0 a1 sin 1 a2 sin(1 2 ) 1 0 i j k 0 0 1 a1 cos 1 a2 cos(1 2 ) a1 sin 1 a2 sin(1 2 ) 0 a1 sin 1 a2 sin(1 2 ) a1 cos 1 a2 cos(1 2 ) 0 Jacobian Matrix z1 (o2 o1 ) J2 z 1 0 a2 cos(1 2 ) Z1 (o2 o1 ) 0 a2 sin(1 2 ) 1 0 i j k 0 0 1 a2 cos(1 2 ) a2 sin(1 2 ) 0 a2 sin(1 2 ) a2 cos(1 2 ) 0 Jacobian Matrix a1 sin 1 a2 sin(1 2 ) a cos a cos( ) 1 2 1 2 1 0 J1 0 0 1 a2 sin(1 2 ) a cos( ) 1 2 2 0 J2 0 0 1 The required Jacobian matrix J J J1 J2 Stanford Manipulator The DH parameters are: ci s Ai i 0 0 -c isi s isi ci c i -s i ci s i c i 0 0 a i ci a isi di 1 Stanford Manipulator T01 A1 c1c2 s c T02 A1 A2 1 2 s2 0 c1c2 s c T03 A1 A2 A3 1 2 s2 0 s1 c1s2 c1 s1s2 0 c2 0 0 s1 c1s2 c1 s1s2 0 c2 0 0 d 2 s1 d 2c1 0 1 0 c1s2 s1 c1s2 z0 0 z c z s s z3 s1s2 1 1 2 1 2 1 c2 0 c2 d 3c1s2 d 2 s1 d 2 s1 0 d c O d s s d c O d3c1s2 d 2 s1 O0 O1 0 2 2 1 3 3 1 2 2 1 d 3c2 0 0 d3 s1s2 d 2c1 d3c2 1 Stanford Manipulator T04 A1 A2 A3 A4 T05 A1 A2 A3 A4 A5 T06 A1 A2 A3 A4 A5 A6 T4 = [ c1c2c4-s1s4, -c1s2, -c1c2s4-s1*c4, c1s2d3-sin1d2] [ s1c2c4+c1s4, -s1s2, -s1c2s4+c1c4, s1s2d3+c1*d2] [-s2c4, -c2, s2s4, c2*d3] [ 0, 0, 0, 1] c1c2 s4 s1c4 z4 s1c2 s4 c1c4 s2 s4 d3c1s2 d 2 s1 O4 d3 s1s2 d 2 c1 d3c2 Stanford Manipulator T5 = [ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5, c1s2d3-s1d2] [ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5, s1s2d3+c1d2] [ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3] [ 0, 0, 0, 1] c1c2c4 s5 s1s4 s5 c1s2c5 z5 s1c2c4 s5 c1s4 s5 s1s2c5 s2c4 s5 c2c5 c1c2c4 s5 s1s4 s5 c1s2c5 z5 s1c2c4 s5 c1s4 s5 s1s2c5 s2c4 s5 c2c5 Stanford Manipulator T5 = [ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5, c1s2d3-s1d2] [ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5, s1s2d3+c1d2] [ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3] [ 0, 0, 0, 1] c1c2c4 s5 s1s4 s5 c1s2c5 z5 s1c2c4 s5 c1s4 s5 s1s2c5 s2c4 s5 c2c5 d 3c1s2 d 2 s1 O5 d 3 s1s2 d 2 c1 d 3c2 Stanford Manipulator T6 = [ c6c5c1c2c4-c6c5s1s4-c6c1s2s5+s6c1c2s4+s6s1c4, c5c1c2c4+s6c5s1s4+s6c1s2s5+c6c1c2s4+c6s1c4, s5c1c2c4-s5s1s4+c1s2c5, d6s5c1c2c4-d6s5s1s4+d6c1s2c5+c1s2d3-s1d2] [ c6c5s1c2c4+c6c5c1s4-c6s1s2s5+s6s1c2s4-s6c1c4, -s6c5s1c2c4s6c5c1s4+s6s1s2s5+c6s1c2s4-c6c1c4, s5s1c2c4+s5c1s4+s1s2c5, d6s5s1c2c4+d6s5c1s4+d6s1s2c5+s1s2d3+c1d2] [ -c6s2c4c5-c6c2s5-s2s4s6, s6s2c4c5+s6c2s5-s2s4c6, -s2c4s5+c2c5, d6s2c4s5+d6c2c5+c2d3] [ 0, 0, 0, 1] d6s5c1c2c4 d6s5s1s4 d6c1s2c5 c1s2d3 s1d2 O6 d6s5s1c2c4 d6s5c1s4 d6s1s2c5 s1s2d3 c1d2 d6s2c4s5 d6c2c5 c2d3 Stanford Manipulator z0 (o6 o0 ) z1 (o6 o1 ) J1 , J2 z z 0 1 z2 J3 0 Joints 1,2 are revolute Joint 3 is prismatic z3 (o6 o3 ) z5 (o6 o5 ) z4 (o6 o4 ) J4 , J5 , J6 z z z 3 5 4 The required Jacobian matrix J J J1 J 2 J3 J 4 J5 J 6 Inverse Velocity – The relation between the joint and end-effector velocities: X J (q )q where j (m×n). If J is a square matrix (m=n), the joint velocities: q J 1 (q) X – If m<n, let pseudoinverse J+ where J J T [ JJ T ]1 q J (q ) X Acceleration – The relation between the joint and end-effector velocities: X J (q )q – Differentiating this equation yields an expression for the acceleration: d X J (q )q [ J (q )]q dt – Given X of the end-effector acceleration, the joint acceleration q d J (q)q X [ J (q )]q dt d 1 q J (q)[ X J (q )]q] dt