VIEWS: 23 PAGES: 46 POSTED ON: 9/29/2012
Introduction to ROBOTICS Robot Kinematics II Dr. Jizhong Xiao Department of Electrical Engineering City College of New York jxiao@ccny.cuny.edu The City College of New York 1 Outline • Review – Manipulator Specifications • Precision, Repeatability – Homogeneous Matrix • Denavit-Hartenberg (D-H) Representation • Kinematics Equations • Inverse Kinematics The City College of New York 2 Review • Manipulator, Robot arms, Industrial robot – A chain of rigid bodies (links) connected by joints (revolute or prismatic) • Manipulator Specification – DOF, Redundant Robot – Workspace, Payload – Precision How accurately a specified point can be reached – Repeatability How accuratelyrepeated many times be reached if the motion is the same position can The City College of New York 3 Review • Manipulators: Cartesian: PPP Cylindrical: RPP Spherical: RRP Hand coordinate: SCARA: RRP n: normal vector; s: sliding vector; Articulated: RRR (Selective Compliance a: approach vector, normal to the Assembly Robot Arm) tool mounting plate The City College of New York 4 Review • Basic Rotation Matrix px i x i u i x jv i x k w pu Pxyz p y jy i u j y jv jy k w pv RP uvw p z k z i u k z jv k z k w pw z Pxyz RPuvw w P v Puvw QPxyz Q R 1 RT y u x The City College of New York 5 Basic Rotation Matrices – Rotation about x-axis with 1 0 0 Rot( x, ) 0 C S 0 S C – Rotation about y-axis with C 0 S Rot( y, ) 0 1 0 S 0 C – Rotation about z-axis with C S 0 Pxyz RPuvw Rot( z, ) S C 0 0 0 1 The City College of New York 6 Review • Coordinate transformation from {B} to {A} r ARB B r P Ar o ' A P A r P A RB A o' r Br P 1 013 1 1 • Homogeneous transformation matrix RB r R33 P31 A A o' Rotation TB A matrix 013 1 0 1 Position vector Scaling The City College of New York 7 Review • Homogeneous Transformation – Special cases 1. Translation I 33 r A o' A TB 013 1 2. Rotation A RB 031 A TB 013 1 The City College of New York 8 Review • Composite Homogeneous Transformation Matrix • Rules: – Transformation (rotation/translation) w.r.t. (X,Y,Z) (OLD FRAME), using pre-multiplication – Transformation (rotation/translation) w.r.t. (U,V,W) (NEW FRAME), using post- multiplication The City College of New York 9 Review • Homogeneous Representation – A point in R 3 space px p P y Homogeneous coordinate of P w.r.t. OXYZ pz z P( px , p y , pz ) 1 a s – A frame in R space 3 n nx sx ax px y n s a P n y sy ay py F n 0 0 0 1 z sz az pz 0 0 0 1 x The City College of New York 10 Review • Orientation Representation (Euler Angles) – Description of Yaw, Pitch, Roll • A rotation of about the OX Z axis ( R x , ) -- yaw roll • A rotation of about the OY axis ( R y , ) -- pitch yaw • A rotation of about the OZ Y axis ( R z , ) -- roll pitch X The City College of New York 11 Quiz 1 • How to get the resultant rotation matrix for YPR? T Rz , R y , Rx , C S 0 0 C 0 S 0 1 0 0 0 S C 0 0 0 1 0 0 0 C S 0 0 0 1 0 S 0 C 0 0 S C 0 0 0 0 1 0 0 0 1 0 0 0 1 Z Y X The City College of New York 12 Quiz 2 • Geometric Interpretation? R33 P31 Orientation of OUVW coordinate T frame w.r.t. OXYZ frame Position of the origin of OUVW 0 1 coordinate frame w.r.t. OXYZ frame • Inverse Homogeneous Matrix? 1 RT RT P Inverse of thetorotation submatrix T Position of the origin of OXYZ is equivalent its transpose 0 1 reference frame w.r.t. OUVW frame 1 RT R T P R P R T R 0 T T I 44 0 1 0 1 0 1 The City College of New York 13 Kinematics Model • Forward (direct) Kinematics q (q1 , q2 , qn ) z Joint Position and Orientation variables of the end-effector Direct Kinematics y Inverse Kinematics x Y ( x, y, z, , , ) • Inverse Kinematics The City College of New York 14 Robot Links and Joints The City College of New York 15 Denavit-Hartenberg Convention • Number the joints from 1 to n starting with the base and ending with the end-effector. • Establish the base coordinate system. Establish a right-handed orthonormal coordinate system ( X 0 , Y0 , Z 0 ) at the supporting base with Z 0 axis lying along the axis of motion of joint 1. • Establish joint axis. Align the Zi with the axis of motion (rotary or sliding) of joint i+1. • Establish the origin of the ith coordinate system. Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis. • Establish Xi axis. Establish X i (Zi1 Zi ) / Zi1 Zi or along the common normal between the Zi-1 & Zi axes when they are parallel. • Establish Yi axis. Assign Yi (Zi X i ) / Zi X i to complete the right-handed coordinate system. • Find the link and joint parameters The City College of New York 16 Example I • 3 Revolute Joints Z3 end-effector frame Z0 Z1 Joint 3 Y0 O3 X3 Y1 Link 1 Link 2 d2 Joint 1 O0 X0 O1 X1 O2 X2 Joint 2 Y2 a0 a1 The City College of New York 17 Link Coordinate Frames • Assign Link Coordinate Frames: – To describe the geometry of robot motion, we assign a Cartesian coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows: • establish a right-handed orthonormal coordinate frame O0 at the supporting base with Z0 lying along joint 1 motion axis. • the Zi axis is directed along the axis of motion of joint (i + 1), that is, link (i + 1) rotates about or translates along Zi; Z3 Z0 Z1 Joint 3 Y0 O3 X3 Y1 Link 1 Link 2 d2 Joint 1 O0 X0 O1 X1 O2 X2 Joint 2 Y2 a0 a1 The City College of New York 18 Link Coordinate Frames – Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis. – the Xi axis lies along the common normal from the Zi-1 axis to the Zi axis X i (Zi1 Zi ) / Zi1 Zi , (if Zi-1 is parallel to Zi, then Xi is specified arbitrarily, subject only to Xi being perpendicular to Zi); Z3 Z0 Z1 Joint 3 Y0 O3 X3 Y1 d2 Joint 1 O0 X0 O1 X1 O2 X2 Joint 2 Y2 a0 a1 The City College of New York 19 Link Coordinate Frames – Assign Yi (Zi X i ) / Zi X i to complete the right- handed coordinate system. • The hand coordinate frame is specified by the geometry On of the end-effector. Normally, establish Zn along the direction of Zn-1 axis and pointing away from the robot; establish Xn such that it is normal to both Zn-1 and Zn axes. Assign Yn to complete the right-handed coordinate system. Z3 Z0 Z1 Joint 3 Y0 O3 X3 Y1 d2 Joint 1 O0 X0 O1 X1 O2 X2 Joint 2 Y2 a0 a1 The City College of New York 20 Link and Joint Parameters • Joint angle i : the angle of rotation from the Xi-1 axis to the Xi axis about the Zi-1 axis. It is the joint variable if joint i is rotary. • Joint distance d i : the distance from the origin of the (i-1) coordinate system to the intersection of the Zi-1 axis and the Xi axis along the Zi-1 axis. It is the joint variable if joint i is prismatic. • Link length a i : the distance from the intersection of the Zi-1 axis and the Xi axis to the origin of the ith coordinate system along the Xi axis. • Link twist angle i : the angle of rotation from the Zi-1 axis to the Zi axis about the Xi axis. The City College of New York 21 Example I Z3 Z0 Z1 Joint 3 Y0 O3 X3 Y1 d2 Joint 1 O0 X0 O1 X1 O2 X2 D-H Link Parameter Table Joint 2 Y2 Joint i i ai di i a0 a1 1 0 a0 0 1 i : rotation angle from Zi-1 to Zi about Xi 2 -90 a1 0 2 a i : distance from intersection of Zi-1 & Xi 3 0 0 d2 3 to origin of i coordinate along Xi di : distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1 i : rotation angle from Xi-1 to Xi about Zi-1 The City College of New York 22 Example II: PUMA 260 1. Number the joints 2. Establish base frame 1 2 3. Establish joint axis Zi Z1 4. Locate origin, (intersect. O1 3 of Zi & Zi-1) OR (intersect of common normal & Zi ) X1 Z2 Z6 Y1 5. Establish Xi,Yi O2 Y3 Z Z4 O3 X 2 5 6 Y6 X i (Zi 1 Zi ) / Zi 1 Zi Y2 O6 5 Yi (Zi X i ) / Zi X i Z0 X 3 Y4 Y5 t O5 X5 X6 O4 Z 3 X4 4 PUMA 260 The City College of New York 23 Link Parameters J i i ai d i 1 2 1 1 -90 0 13 Z1 2 2 0 8 0 O1 3 3 3 90 0 -l X1 Z2 Z6 4 4 -90 0 8 Y1 O 5 5 90 0 0 Y3 2 Z Z4 O3 X 2 5 6 Y6 6 6 0 0 t Y2 O6 5 i : angle from Xi-1 to Xi Z0 X 3 Y4 Y5 about Zi-1 O5 X5 X6 O4 Z 3 i : angle from Zi-1 to Zi about Xi X4 4 a i : distance from intersection of Zi-1 & Xi to Oi along Xi Joint distance d i : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1 The City College of New York 24 Transformation between i-1 and i • Four successive elementary transformations are required to relate the i-th coordinate frame to the (i-1)-th coordinate frame: – Rotate about the Z i-1 axis an angle of i to align the X i-1 axis with the X i axis. – Translate along the Z i-1 axis a distance of di, to bring Xi-1 and Xi axes into coincidence. – Translate along the Xi axis a distance of ai to bring the two origins Oi-1 and Oi as well as the X axis into coincidence. – Rotate about the Xi axis an angle of αi ( in the right- handed sense), to bring the two coordinates into coincidence. The City College of New York 25 Transformation between i-1 and i • D-H transformation matrix for adjacent coordinate frames, i and i-1. – The position and orientation of the i-th frame coordinate can be expressed in the (i-1)th frame by the following homogeneous transformation matrix: Source coordinate Ti 1 T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R( xi , i ) i C i C i S i S i S i ai C i Reference S C i C i S i C i ai S i Coordinate i 0 S i C i di 0 0 0 1 The City College of New York 26 Kinematic Equations • Forward Kinematics q (q1 , q2 , qn ) – Given joint variables – End-effector position & orientation Y ( x, y, z, , , ) n • Homogeneous matrix T 0 – specifies the location of the ith coordinate frame w.r.t. the base coordinate system – chain product of successive coordinate transformation i matrices of Ti 1 Position T T T T 0 n 1 2 0 1 n n 1 vector Orientation R0n P0n n s a P0n matrix 0 1 0 0 0 1 The City College of New York 27 Kinematics Equations • Other representations – reference from, tool frame Tref Bref T0n H n tool 0 tool – Yaw-Pitch-Roll representation for orientation T Rz , R y , Rx , C S 0 0 C 0 S 0 1 0 0 0 S C 0 0 0 1 0 0 0 C S 0 0 0 1 0 S 0 C 0 0 S C 0 0 0 0 1 0 0 0 1 0 0 0 1 The City College of New York 28 Representing forward kinematics • Forward kinematics • Transformation Matrix 1 px p nx sx ax px 2 y n 3 pz sy ay py T y nz sz az pz 4 5 0 0 0 1 6 The City College of New York 29 Representing forward kinematics • Yaw-Pitch-Roll representation for orientation CC CSS SC CSC SS px SC SSS CC SSC CS py T0n S CS CC pz 0 0 0 1 nx sx ax px sin 1 (nz ) n py az T0n y sy ay cos ( 1 ) nz sz az pz cos nx 0 0 0 1 cos ( 1 ) cos Problem? Solution is inconsistent and ill-conditioned!! The City College of New York 30 atan2(y,x) y x 0 90 for x and y 90 180 for x and y a tan 2( y, x) 180 90 for x and y 90 0 for x and y The City College of New York 31 Yaw-Pitch-Roll Representation T Rz , R y , Rx , C S 0 0 C 0 S 0 1 0 0 0 S C 0 0 0 1 0 0 0 C S 0 0 0 1 0 S 0 C 0 0 S C 0 0 0 0 1 0 0 0 1 0 0 0 1 nx sx ax 0 n sy ay 0 y nz sz az 0 0 0 0 1 The City College of New York 32 Yaw-Pitch-Roll Representation 1 R T Ry , Rx, z , C S 0 0 n x sx ax 0 S 0 0 n y C sy ay 0 0 0 1 0 n z sz az 0 0 0 0 1 0 0 0 1 (Equation A) C 0 S 0 1 0 0 0 0 0 C 0 1 0 0 S S 0 C 0 0 S C 0 0 0 0 1 0 0 0 1 The City College of New York 33 Yaw-Pitch-Roll Representation • Compare LHS and RHS of Equation A, we have: sin nx cos n y 0 a tan 2(n y , nx ) cos nx sin n y cos nz sin a tan 2(nz , cos nz sin n y ) sin s x cos s y cos sin ax cos a y sin a tan 2(sin a x cos a y , sin s x cos s y ) The City College of New York 34 Kinematic Model • Steps to derive kinematics model: – Assign D-H coordinates frames – Find link parameters – Transformation matrices of adjacent joints – Calculate Kinematics Matrix – When necessary, Euler angle representation The City College of New York 35 Example Z3 Z0 Z1 Joint 3 Y0 O3 X3 Y1 d2 Joint 1 O0 X0 O1 X1 O2 X2 Joint 2 Y2 a0 a1 Joint i i ai di i 1 0 a0 0 1 2 -90 a1 0 2 3 0 0 d2 3 The City College of New York 36 Example Joint i i ai di i cosθ1 sinθ1 0 a 0 cos1 0 a 0 sin 1 1 0 a0 0 1 1 sinθ1 cosθ1 T 0 0 0 1 0 2 -90 a1 0 2 0 0 0 1 3 0 0 d2 3 cosθ 2 0 sin 2 a1 cos 2 cos 2 a1 sin 2 2 sinθ 2 0 T 1 0 1 0 0 C i C i S i S i S i ai C i S C i C i S i C i ai S i 0 0 0 1 T i i 1 i 0 S i C i di 0 0 0 1 cosθ 3 sinθ 3 0 0 0 0 3 sin 3 cos 3 T 2 T03 (T 0)(T 2)(T 2) 1 3 0 0 1 d2 1 0 0 0 1 The City College of New York 37 Example: Puma 560 The City College of New York 38 Example: Puma 560 The City College of New York 39 Link Coordinate Parameters PUMA 560 robot arm link coordinate parameters Joint i i i ai(mm) di(mm) 1 1 -90 0 0 2 2 0 431.8 149.09 3 3 90 -20.32 0 4 4 -90 0 433.07 5 5 90 0 0 6 6 0 0 56.25 The City College of New York 40 Example: Puma 560 The City College of New York 41 Example: Puma 560 The City College of New York 42 Inverse Kinematics • Given a desired position (P) & orientation (R) of the end- z effector q (q1 , q2 , qn ) y • Find the joint variables which can bring the robot x the desired configuration The City College of New York 43 Inverse Kinematics • More difficult – Systematic closed-form solution in general is not (x , y) available – Solution not unique • Redundant robot • Elbow-up/elbow-down configuration – Robot dependent The City College of New York 44 Inverse Kinematics • Transformation Matrix 1 nx sx ax px 2 n sy ay py 3 T y T01T12T23T34T45T56 nz sz az pz 4 0 0 0 1 5 6 Special cases make the closed-form arm solution possible: 1. Three adjacent joint axes intersecting (PUMA, Stanford) 2. Three adjacent joint axes parallel to one another (MINIMOVER) The City College of New York 45 Thank you! Homework 2 posted on the web. Due: Sept. 24, 2012 Next class: Inverse Kinematics, Jocobian Matrix, Trajectory planning z y z y x z z x y y x x The City College of New York 46