Chapter 2 Robot Arm Kinematics

2.1 IntroductionRobot Arm Kinematics : deals with analytic study of the geometry of motion of robot arm w.r.t. fixed reference coordinate system as a function of time without regard to the forces or moment that cause the motion.

* Direct (or forward) Kinematics 1 2~ n n nX Y Zq q * Inverse Kinematics n n n iX Y Z q* Links of robot arm : rotate and/or translate w.r.t reference coordinate frame

* Spatial displacement of the end-effector is due to the

angular rotation and linear translation.* cartesian coordinate frame right hand rule

* best way to describe the orientation of the object (motivation of multi coordinate systems)

: World coordinate frame Base coordinate frame Fixed coordinate frame

2.2 The Direct Kinematics Problem

2.2.1 Rotation matrices

( )

x y z

x

y

z

x x y y z z

x

x y z y

z

P P P P

PP P

PP P i P j P k

Pi i i P

P

=

=

= + +

=

OXYZ : reference coordinate system unit vector OUVW : moving coordinate system (body attached frame) unit vector

and

We want to find the relationship. (transformation) Let be 33 transformation matrix that will transform the coordinates of to the coordinates expressed w.r.t. OXYZ coordinates system, after OUVW coordinate system has been rotated. That is but

: component of in x direction

Similarly , but then - in vector-matrix form

Or,

where,

(3rd column of R : representation of w.r.t. )

Similarly, (orthogonal transformation)

* Rotation matrices represent the rotations of the OUVW coordinate system about each of the 3 principal axes of the OXYZ coordinate system

Fundamental rotations

Assume OUVW coordinate system is rotated an angle about the OX axis, then

cos sin sin cos

OUVW rotates an angle about OY axis, then

cos sin sin cos

OUVW rotates an angle about OZ axis, then

cos sin sin cos

-2-2 Composite Rotation matrix

(*) (**) Substitute into (*), then Compare with (**), then

*Basic rotation can be multiplied together to represent a sequence of limited rotations about the principal axes of the OXYZ coordinate system. (Since matrix multiplications do not commute, the order of performing rotations is important.)

Ex. rotation of angle about OX axis , thenrotation of angle about OZ axis , then rotation of angle about OY axis.

The resultant rotation matrix : pre-multiply one after another.

cos sin sin cos

cos sin sin cos

cos sin sin cos

* OX OZ OY OY OZ OX

*Rotating coordinate system OUVW can also rotate about it's own principal axes.

* rules Initially both coordinate systems are coincident If rotation is about one of the principal axes of the OXYZ frame, then the premultiply. If rotation is about one of the principal axes of the OUVW frame , the postmultiply.

Ex. rotate about OX, about OY, about OU, about OW. (Rotation OX OY OU OW)

Premultiplyy

Postmultiply

(OZ OX OX OY)

2-2-3 Rotation Matrix about an Arbitrary Axis

Rotating coordinate system OUVW, rotates angle about an arbitrary axis r which is an unit vector. Then,

but it is very hard to find base vectors ( ) of the rotated coordinate system, after the angle rotation.Therefore, first, align the r with OZ axis, then rotate , then let the r go back to original orientation.

To find

make rotation about the principal axes of OXYZ frame to align the axis r with OZ axis.

then make rotation about the r axis (=OZ axis) with angle.

Lastly, rotate the moving coordinate system about the principal axes of OXYZ frame again to return the r axis back to it's original location.

Ex. Let : unit vector

cos sin sin cos

cos sin sin cos

cos sin sin cos

cos sin sin cos

cos sin sin cos

From Figure, we can easily find that

sin cos

sin cos

Substituting them into the above equation, we have

where cos

* Other representations of orientation

Orientation of rigid body w.r.t. reference coordinate 1. Matrix representation : need 9 elements (only 3 elements are linearly independent) 2. Axis / Angle representation : 4 component but r is unit vector 3. Euler angle representation : 3 Euler angles 4. Roll-Pitch-Yaw representation : 3 RPY angles

* Axis/Angle representation v.s. Rotation matrix

Arbitrary rotations (or Orientation) can be represented by a single rotation about a suitable axis in space by a suitable angle as . and can be found from R as

cos cos sin

* note that the rotation is not unique. (i.e.. ) If = 0, then is identity matrix and the axis of rotation is undefined.

Ex. Consider a sequence of rotations as

then, cos

2.2.4 Rotation Matrix with Euler Angle Representation

Euler angle system (gyroscopic motion) 1. rotation of angle about the OZ axis 2. rotation of angle about the rotated OU axis 3. Finally a rotation of angle about the rotated OW axis

The resultant Eulerian rotation matrix is(also can be represented by )

Euler angle system II

1. about the OZ 2. about the rotated OV 3. about the rotated OW (also )

*YPR Transformation

1. about OX yaw2. about OY pitch3. about OZ roll

(also )

cos sin sin cos

cos sin sin cos

cos sin sin cos

coscos cossinsinsincos cossincossinsinsincos sinsinsincoscos sinsincoscossinsin cossin coscos

2.2.5 Geometric Interpretation of Rotation Matrix

cos sin

sin cos ____ __ ___ u v w (3rd column of R : representation of w w.r.t xyz )

magnitude of representation is 1 =1 u, v, w are Orthonormal vectors inner product of each row = 02.2.6 Homogeneous coordinates and Transformation matrix

*linear translation ( angular rotation)

: linear translation

*with rotation : rigid motion*with another rotation, *after several consecutive rotations and translations, the transformation matrix becomes mess!

*Is there any solution? - The Homogeneous transformation in homogeneous coordinate frame.

Definition:

* Homogeneous coordinate : makes it possible to represent both translation and rotation as a matrix multiplication.

* Let = be the cartesian coordinates of a point in R. Then any set of (n+1) numbers ,

0 , for which

,

, ,

is called a set of homogeneous coordinates for point X.

Note that any algebraic equation in , when transformed to a set of homogeneous coordinates, becomes a homogeneous equation in homogeneous coordinate.

Homogeneous coordinate a class of objects involves no explicit constants.

Example : Equation of a 2-d line y=ax+b homogeneous equation ax-y+b=0 which is a representation of the line in3-D.

* position vector

in 3 dimensional Euclidean space.

* position vector

in 4 dimensional Homogeneous

coordinate system. ( component w : scale factor. In robotic applications usually, w=1)

*Homogeneous transformation matrix

where = rotation matrix = position vector of the origin of the rotated coordinate

system w.r.t the reference coordinate system. = perspective transformation (for graphics)11 = scale factor (=1 in robotics)

*Basic homogeneous rotation matrix

cos sin sin cos

where,

cos sin

sin cos : rotation matrix

: origin of rotating coordinate system which is same as

reference coordinate system. : no prospection (used only for graphics, computer

vision and the calibration of camera model) : scale factor = 1

*Basic homogeneous translation matrix

where,

: no rotation.

: translation the OUVW coordinate system which has axes

parallel to the OXYZ but whose origin is at in OXYZ.

*Local scaling

: Coordinate values are stretched by the scalar a, b and c.

*note that basic rotation matrix T dose not produce any local scaling effect

*global scaling

, ,

,

if s>1, reduction, if 0

2.2.7 Geometric interpretation of Homogeneous transformation matrix

From equation(2.2-26a, b)

or

if

and

then,

n representation of unit vector in OXYZ. Similarly .

if

: origin in OUVW

: coordinate of OUVW origin w.r.t. OXYZ

Homogeneous transformation matrix geometrically represents the location of a rotated coordinate system (position and orientation) w.r.t. reference coordinate system.

*Inverse of rotation matrix R

( columns of R are representation of in OXYZand rows of R are representation of in OUVW.)

* the inverse of homogeneous transformation matrix is not equivalent to it's transpose.

where

: Transpose of (inverse of )

: represents the position of the origin of the

reference frame w.r.t OUVW system.

2-2-8 Composite Homogeneous Transformation Matrix

The homogeneous rotation and translation matrices can be multiplied together to obtain a composite homogeneous translation matrix(T)

*process

1. Initially both coordinate system coincide, hence 2. If OUVW system is rotation / translation about the principal

axes of the OXYZ frame then premultiply previous H.T.M with an appropriate basic H.T.M. .

3. If it rotates / translates about the principal axes of the OUVW system, postmultiply. .

* Screw Transformation

Screw( , , y) Rot ( , y ) Tran ( , )

translate along the Y by

Rotate about the Y by

go to 1) and repeat

2.2.9 Link, Joint and their parameters

A mechanical manipulator consists of a sequence of rigid bodies, called links, connected by joints. Or one can say each joint is connected by rigid link.

*6 types of lower pair joint. (revolute, prismatic, cylindrical spherical, screw, planar joints)

*Joint-link pairs

L 0

J 1

J 2L 1 L 2

*Each joint-link pair constitutes 1 degree of freedom. *N degree of freedom N joint-link pairs with link 0.*Joints and links are numbered outwardly from the base.*Joint-link pair is characterized by 4 geometric parameters:

(effective) link length ( ) joint Angle ( ) twist angle ( ) offset ( )

2.2.10 Denavit-Hartenberg representation

Denavit-Hartenberg proposed a matrix method of systematically establishing a coordinate system to each link of an articulated chain.

L 0

J 1J 2

L 1L 2

Y 0 Z 0

X 0Y 1

Z 1

X 1

*Algorithm 2.1 : Link coordinate assignment

- i th coordinate system is fixed in link i, and it moves together with link i.

- World coordinate frame OXYZ is fixed on Link 0 and does not move.

- connects and .

- : axis of motion of , direction pointing away from the base

- : if and intersects, then, = ( ) / if and are parallel, then, is a normal vector pointing away (not unique, so choose a proper one).

- : use right handed coordinate system - One side of is fixed on the base. Therefore, can be arbitrarily assigned as long as XZ.

- One side of ( end-effector ) is free (No Joint 7). So

choose to align with approach vector a, and with normal vector n, and with sliding vector s. ( is usually a rotating joint with end-effector parallel to )

*Note that D-H representation is not unique.

It is a convention for affixing coordinates to links, and D-H representation results in a 4x4 HTM representing each link's coordinate system at the joint with respect to the previous link's coordinate system.

*Homogeneous transformation matrix : 44 matrix and has 16 elements. Among them only 6 elements has meaning. (position information : 3, orientation information :3 )

But. Denavit-Hartenberg representation uses only 4 geometric parameters against 16 elements in HTM. ( by using freedom of the choice of the origin and coordinates on the rigid frame)

The DH parameters are : effective link length ( ), joint angle ( ), twist angle ( ), offset ( ).

The definitions distance(offset) and joint angle ( , )

length (Link) and twist angle ( , )

* Link coordinate system for a PUMA robot (Fig 2.11)

* Link coordinate assignment for Puma robot

- Assign 7 coordinate systems for 6 joints. - note that axis Z0 and axis Z1, intersect each other. normal line to both axis is a point.

1. Since there is no joint 0, origin of X0Y0Z0 coordinate system and direction of X0 can be chosen arbitrarily. * choose the origin of X0Y0Z0 on the intersection point with the axis Z0 and axis Z1 for convenience. * choose the direction of X0 arbitrarily, fixed on link 0.

2. Since direction of X1 is the direction of normal line, the origin of X1Y1Z1 coincide with that of X0Y0Z0. Then, * a1 = O ( normal line is a point ) * d1 = O ( origins of X0Y0Z0 and X1Y1Z1 coincide ) * 1 = 90 ( angle between X0 and normal line ) * 1 = -90 ( angle between Z0 and Z1 )

3. Since Z1//Z2, number of normal lines are infinity. choose the origin of X2Y2Z2 on the center of Joint 3.

* a2 = 431.8mm * d2 = 149.09mm

* 2 = 0( X1 // X2 ) * 2 = 0( Z1 // Z2 )

4. * a3 = + 20.32 * d3 = 0 ( normal line intersect the origin of 02X2Y2Z2) * 3 = -90 * 3 = -90

5. * a4 = 0 ( normal line is a point ) * 4 = +180 * 4 = -90 * d4 = 433.07

6. * a= d=0 * = 0 * = 90

7. * Z6 : choose as same as Z5 ( joint 6 is a revolute joint ) * = = 0 * a = 0 (normal line- point) * d = 56.25

Home work : Establish link coordinate systems for a Stanford robot and find the DH parameters for any given configuration of Stanford Arm.

* Homogeneous Transformation Matrix for relating i th cordinate frame to (i-1)st coordiante frame.

At the beginning, 2 coordinate systems coincides :

Rotate about axis an angle Translate along axis a distance of Translate along axis a distance : Rotate about the axis an angle of

Zi-1, Zi

Xi-1,Xi0i-1,0iI4X4

Zi-1, Zi

Xi-1,Xi0i-1,0iI4X4

Then, Homogeneous transform matrix is;

*For sliding joint, axis and axis intersect each other, so, =0 (normal line is a point. ). And is a constant, and is a variable.

Therefore,

(There is no consecutive sliding joints. : redundancy)

2.2.11 Kinematic Equations for manipulator

T

i

direct Kinematic solution for given 1

i

X

i

, Yi

, Zi

: orientation of i th coordinate systemP

i

: origin of i th coordinate system

* physical meaning of

*computer program for forward Kinematic solution

=

C

1

C

23

-S

1

C

1

S

23

a

2

C

1

C

2

+a

3

C

1

C

23

-d

2

S

1

S

1

C

23

C

1

S

1

S

23

a

2

S

1

C

2

+a

3

S

1

C

23

+d

2

C

1

-S

23

0 C

23

-a

2

S

2

-a

3

S

23

0 0 0 1

=

C

4

C

5

C

6

-S

4

S

6

-C

4

C

5

C

6

-S

4

S

6

C

4

S

5

d

6

C

4

S

5

S

4

C

5

C

6

+C

4

S

6

-S

4

C

5

S

6

+C

4

C

6

S

4

S

5

d

6

S

4

S

5

-S

5

C

6

S

5

S

6

C

5

d

6

C

5

+d

4

0 0 0 1

where

So, T=T1

T

2

Ex. 1. Find the forward Kinematic solution for PUMA 560 for

1

=90

0

,2

=0

0

,3

=90

0

,4

=0

0

,5

=0

0

,6

=0

0

Ans

T=

0 -1 0 -149.09

0 0 1 921.12

-1 0 0 20.32

0 0 0 1

Ex. 2. A robot work station has been set up with a TV camera. The camera can see the origin of the base coordinate system where a 6-joint robot is attached. It can also see the center of the object(cube) to be manipulated by the robot.If the local coordinate system has been established at the center of the cube, this object as seen by the camera can be represented by homogeneous transformation matrix T1

.If the origin of the base coordinate system as seen by the camera can also be expressed by a homogeneous transformation matrix T2

.

T1

=

0 1 0 1

1 0 0 10

0 0 -1 9

0 0 0 1

T2

=

1 0 0 -10

0 -1 0 20

0 0 -1 10

0 0 0 1

a) what is the position of the center of the cube w.r.t the base coordinate system?b) what is the orientation matrix [ n , s, a ] if you want the gripper of the hand to be aligned with the y axis of the object and at the same time pick up the object from the top?

base baseTobj

=

base

T

camera

camera

T

obj

= (T

2

)

-1

T

1

base

T

obj

=

1 0 0 10

0 -1 0 20

0 0 -1 10

0 0 0 1

0 1 0 1

1 0 0 10

0 0 -1 9

0 0 0 1

T=

0 1 0 11

-1 0 0 10

0 0 1 1

0 0 0 1

where,

X

obj

:

0

-1

0

, Yobj

:

1

0

0

, Zobj

:

0

0

1

P :

11

10

1

object has a thickness 2 ( PZ

: center of

object)

*note the tool center point

xobj

yobj

zobj

z6(n)

y6(n)

x6(n)

xobj

yobj

zobj

z6(n)

y6(n)

x6(n)

n = s a

a =

0

0

-1

:

s=

1

0

0

: finger aligned with yobj

(x

b

)

n= s a=

0

1

0

= (y

b

)

0

T

6

=

0 1 0 11

1 0 0 10

0 0 -1 1

0 0 0 1

* 5 DOF robot : Microbot Alpha

Home Work1. Assign a coordinate systems on the manipulator.2. Find the DH parameters for the given configuration above.

*A pitch-roll spherical wrist of the Alpha II robot

(universal joint has 3 bevel gears {A, B, C}) Gears A and B are driven by separate stepper motors, and Gear C is attached to the tool roll axis.

1. A, B are driven in opposite directions at same speed - tool exhibits a pure roll motion. 2. A, B are driven in same direction at same speed - tool exhibits a pure pitch motion.* Five axis robot : (Rhino XR3)

Home Work.

1. Assign a coordinate systems on the manipulator.2. Find the DH parameters for any given configuration. * Joint coupling of Rhino Robot

The movements of shoulder, elbow, and tool pitch joints are coupled together.

When shoulder motor rotates , then elbow motor counteracts , so that . Similarly, when elbow motor rotates , then tool pitch motor counteracts

, so that . Thus the global pitch angle 234 is fixed either the shoulder motor or the elbow motor is activated.

* motivation : If a liquid-filled container is carried by the tool, it will not spill on account of tipping when either the shoulder motor or the elbow motor is activated.* It can be decoupled by

h=

1

0 0 0 0

0 2

0 0 0

0 0 -3

0 0

0 0 0 -4

0

0 0 0 0 5

-1

1 0 0 0 0

0 1 0 0 0

0 1 1 0 0

0 1 1 1 0

0 0 0 0 1

=P -1C

h encoder hole number countP

-1 inverse of diagonal precision matrix C coupling matrix desired joint angle

=[0.2269 0.1135 0.1135 0.1135 0.3125]T degree/count

note that positive 3

and 4

correspond to negative encoder count off diagonal term of C : to remove the coupling terms