; Introduction - CUNY
Documents
User Generated
Resources
Learning Center

# Introduction - CUNY

VIEWS: 23 PAGES: 46

• pg 1
```									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
– 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   013            1  1 
• Homogeneous transformation matrix
 RB           r   R33            P31 
A        A o'                                Rotation

TB                  
A                                                   matrix

 013         1   0                1       Position
vector
Scaling
The City College of New York                   7
Review
• Homogeneous Transformation
– Special cases
1. Translation
 I 33         r 
A o'
A
TB                   
013           1 

2. Rotation
 A RB         031 
A
TB                     
 013          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?
 R33 P31             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 44
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

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  (Zi1  Zi ) / Zi1  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

Joint 1
O0 X0                    O1 X1 O2 X2
Joint 2
Y2

a0                a1

The City College of New York                                  17
– 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

Joint 1
O0 X0                    O1 X1 O2 X2
Joint 2
Y2

a0                a1
The City College of New York                         18
– 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  (Zi1  Zi ) / Zi1  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
– 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
• 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
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
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
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
CC CSS  SC               CSC  SS    px 
 SC SSS  CC              SSC  CS    py 
T0n                                                       
  S     CS                         CC       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
– 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 cos1 
                           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
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

```
To top