; Introduction - CUNY
Documents
Resources
Learning Center
Upload
Plans & pricing Sign in
Sign Out

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
  – 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   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
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  (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

                      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  (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
     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
          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
  – 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 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
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

								
To top