Robotics1 SS09 PDF

Institute of Automation
Prof. Dr.-Ing. A. Grser
Robotics I
Lecture Notes
Dr.-Ing. Danijela Risti-Durrant
Summer Semester 2009
University of Bremen Institute of Automation
ROBOTICS I
PREFACE
This handbook follows the one-term course in Robotics I held at the Institute of Automation, University of Bremen. Besides the theoretical units presented in the course Robotics I, which are subjects of handbook chapters, a short overview of the research topics in the filed of robotics of the Institute of Automation is presented in the introductory section. The aim is to introduce the students to the research projects, having the theoretical background encompassed by course Robotics I, in which they might work on the diploma thesis on different levels (Bachelor degree, Master and PhD thesis).
ROBOTICS I
TABLE OF CONTENTS
1.
Introduction ........................................................................................................................ 5 1.1. Robotics applications ................................................................................................. 5 Industrial robots.................................................................................................. 5 Service robotics .................................................................................................. 6 1.1.1. 1.1.2.
2.
Mathematical preliminaries................................................................................................ 8 2.1. 2.2. 2.3. 2.4. Matrices...................................................................................................................... 8 Vectors ....................................................................................................................... 8 Vector operations ............................................................................................... 9 Cartesian coordinate systems ..................................................................................... 9 Coordinate transformations ...................................................................................... 10 Relation between two coordinate systems ....................................................... 10 Point transformations ....................................................................................... 12 Homogenous transformations matrices............................................................ 13 Inverse transformations .................................................................................... 14 Compound transformations .............................................................................. 16 Standard transformations.................................................................................. 17 Relative transformations .................................................................................. 20 Transformation of a body................................................................................. 23 Euler angles ...................................................................................................... 24 Roll, Pitch, Yaw angles .................................................................................... 26
2.2.1.
2.4.1. 2.4.2. 2.4.3. 2.4.4. 2.4.5. 2.4.6. 2.4.7. 2.4.8. 2.5. 2.5.1. 2.5.2. 3. 3.1.
Further properties of rotation matrix........................................................................ 24
Manipulator kinematics.................................................................................................... 27 Direct kinematics...................................................................................................... 27 Two-link planar manipulator............................................................................ 27 Denavit-Hartenberg convention ....................................................................... 29 The orientation of manipulator's end-effector.................................................. 35 Analytical solution ........................................................................................... 38 3.1.1. 3.1.2. 3.1.3. 3.2. 3.2.1.
Inverse kinematics.................................................................................................... 37
ROBOTICS I
3.2.2. 4. 4.1.
Numerical solution ........................................................................................... 41
Trajectory planning .......................................................................................................... 44 Joint space trajectory planning................................................................................. 44 Third order polynomials................................................................................... 45 Third order polynomials for a path with via points .......................................... 46 4.1.1. 4.1.2. 4.2.
Higher order polynomials......................................................................................... 49 The control problem ................................................................................................. 52 Independent joint control ......................................................................................... 54 Equations of motion ......................................................................................... 54 Actuator model................................................................................................. 55 Feedback control .............................................................................................. 58 Decentralized feedforward control................................................................... 62
5.
Robot control.................................................................................................................... 52 5.1. 5.2.
5.2.1. 5.2.2. 5.2.3. 5.2.4.
REFERENCES................................................................................................................. 64
ROBOTICS I
1. Introduction
What is a robot? In 1980, the Robot Institute of America (RIA) came up with the following definition: A robot is a reprogrammable multifunctional manipulator designed to move material, parts, tools, or specialized devices through variable programmed motions for the performance of a variety of tasks. Nowadays, this definition would be considered too restrictive, as it reflects the concentration of the RIA on robot manipulators on an assembly line. Robotics has broadened over the years in many ways: to include mobility platforms, to address the service sector as well as the manufacturing sector, and to incorporate man-machine interactions, not just autonomy, in telerobotic and virtual reality systems. For this reason more convenient is a broad definition: A robot is device that operates with some degree of autonomy, usually under computer control. A basic distinction is between mobility and manipulation. A mobile robot can be any form of vehicle, such as a motorized cart, a car, a plane, or a submersible, and in the case of land navigation it can have wheels, tracks, or legs. The main goal of a mobile robot is transport, under guidance of on-board sensors and an intelligent controller. A manipulator is a mechanical linkage, which may or may not be arm-like, with a gripper or tool to perform some action on the environment. In this handbook, the focus will be on robot manipulators, but the basic considerations from manipulators apply to mobile robots and multi-fingered hands.
1.1. Robotics applications

Historically, there have been two main streams for the development of robotics. One application area is the traditional industrial robotics, and the other application area is teleoperation for hazardous environments, such as handling nuclear materials. More recently, the domain of application for robotics has expanded greatly. One of the relatively recent research and development area, which attracted attention not only in the professional world but also in the broad media, is the service robotics. 1.1.1. Industrial robots According to the ISO 8373: 1994 standard Manipulating Industrial Robots Vocabulary defines an industrial robot as an automatically controlled, reprogrammable, multipurpose manipulator programmable in three or more axes. In a simple phrase, industrial robotics refers to the study, design and use of robots for manufacturing. Typical applications of industrial robots include welding, painting, ironing, assembly, palletizing, product inspection, and testing. The Institute of Automation possesses an industrial robot, Stubli RX-90 (Figure I1), used for practical research in the context of diploma- and Ph.D. theses. Further, this robot is used for laboratory courses in the framework of the lectures Process Automation I / II and Robotics I / II.
ROBOTICS I
Fig. I1. Stubli RX-90 The main research work at the Institute of Automation focuses on the area of the service robotics shortly presented in the following section. 1.1.2. Service robotics Basically, service robotics comprises everything that is not industrial robotics, and reflects the distinction between the manufacturing and service sectors. The work of the Institute of Automation in the field of service robotics is concentrated on applications in the area of care and support of disabled and old persons in daily life activities. In the context of this work, the rehabilitation robotic system FRIEND has been developing since 1997.
Fig. I2. The rehabilitation robot FRIEND I The first generation, the rehabilitation robotic system FRIEND I (Figure I2) belongs to the category intelligent wheelchair mounted manipulators. The system consists of an electric wheelchair and a 6 DOF robot arm (MANUS, Exact Dynamics, Netherlands).
ROBOTICS I
The rehabilitation robot FRIEND III (Figure I3) is currently under development and is a successor of FRIEND I and FRIEND II (Figure I3(a)). It is equipped with a dexterous lightweight robot arm with 7 degrees of freedom (DoF). The characteristic is that this electrically driven robot arm has a humanlike kinematics.
(a)
(b)
Fig. I3. Two generations of the rehabilitation robotic system FRIEND, FRIEND II (a) and FRIEND III (b) The FRIEND III represents both a research and demonstration platform offering a great variety of possibilities for student projects and diploma works in the fields of motion planning, software architecture, robot vision etc. Robotics I lectures, overviewed in this handbook, focuses on the mechanics and control of the most important form of the industrial robots, the mechanical manipulator. Hence, Robotics I lectures provide the knowledge background for one interested in the research work within the above presented research projects performed at Institute of Automation.
ROBOTICS I
2. Mathematical preliminaries
The study of robot manipulation is concerned with the relationship between objects as well as between objects and manipulators. Since the description of these relationships is based on the use of vectors, transformation matrices and coordinate systems, the goal of this section is to establish notation and to review mathematics used throughout the handbook.
2.1. Matrices
Matrices are denoted by uppercase bold letters like: R, T, A matrix R of dimensions (m x n), with m and n positive integers, is a two-dimensional array of elements rij arranged into m rows and n columns:
r11 r12 K r r22 K R = rij i =1,K,m = 21 M M j=1,K, n rm1 rm 2 K If m = n, the matrix is said to be square. An (n x n) square rij = 0 for i j , i.e.
[ ]
r1n r2 n . (2.1) M rmn matrix R is said to be diagonal if
r11 0 K 0 0 r K 0 22 = diag{r , r , K r }. (2.2) R= 11 22 nn M M M 0 0 K rnn If an (n x n) diagonal matrix has all unit elements on the diagonal (rii = 1), the matrix is said to be identity matrix and is denoted by In.
2.2. Vectors
Vectors are denoted by lowercase bold letters like: v, u, In contrast to scalar quantity characterised by magnitude only, a vector is characterised by its direction as well as by its magnitude. Usually, a vector is represented graphically by a directed line segment whose length and direction correspond respectively to the magnitude and direction of the vector (Figure 1).
B A v = AB
Fig. 1. Vector representation In three-dimensional space R , a vector v is considered as column matrix of the following form:
v1 v= v 2 , v3
3
(2.3)
ROBOTICS I
where v i , i = 1,2,3 are real numbers.

2 2 The magnitude of the vector v is a positive scalar: v = v1 + v2 2 + v 3 . A vector with a mag-
nitude of unity (with unit length in the assigned direction) is called a unit vector.
2.2.1. Vector operations
The scalar product (dot product or inner product) of two given vectors v and u is defined as:
v u = v1 u 1 + v 2 u 2 + v 3 u 3 ,
(2.4)
or
v u = v u cos ,
(2.5)
where is the angle between the two vectors as it is shown in Figure 2.
u
Fig.2. The angle between two vectors Fig. 3. Vector cross product
If the scalar product of v and u is zero, then either one (or both) of the vectors is zero (with magnitude equal zero) or they are perpendicular to each other because cos(90 o ) = 0 . Thus, two nonzero vectors v and u are orthogonal if and only if their scalar product is zero. Using the definition of scalar product the magnitude of the vector v is expressed as:
v = ( v v )1 / 2 .
(2.6)
The vector product (cross product) of two vectors v and u is defined as the vector u v (Figure 3) which is orthogonal to both vectors (i.e. orthogonal to the plane formed by the vectors v and u) and has the magnitude u v sin . (2.7)
2.3. Cartesian coordinate systems

If there is a subset of linearly independent vectors {x 0 , y 0 , z 0 } in three-dimensional vector space V and a set of scalars {v1 , v 2 , v 3 } such that every vector v in V can be expressed as:
v = v1 x 0 + v 2 y 0 + v 3 z 0
(2.8)
then it is said that v is linear combination of the vectors {x 0 , y 0 , z 0 } which represent the basis vectors for the vector space V. If a set of basis vectors {x 0 , y 0 , z 0 } are all drawn from a common origin 0, then these vectors form an oblique coordinate system with axes 0x, 0y and 0z drawn along the basis vectors as it
ROBOTICS I
10
is shown in Figure 4. By properly choosing the direction of the basis vectors, one can form various coordinate frames commonly used in engineering work.
z z0 x0 x y0 0 xA {A} A0 zA y yA
Fig. 4. Cartesian coordinate system
Fig. 5. Coordinate system {A}
If the basis vectors {x 0 , y 0 , z 0 } are orthogonal to each other, that is, if they intersect at right angles at the origin 0, then they form a rectangular or Cartesian coordinate system. Furthermore, if each of the basis vectors is of unit length, the coordinate system is called orthonormal. Basis vectors of an orthonormal coordinate system (orthonormal vectors) satisfy the following equations: x 0 y 0 = 0; x 0 z 0 = 0; y 0 z 0 = 0 , (2.9) and
x0 = y 0 = z 0 = 1.
(2.10)
If the basis vector {x 0 , y 0 , z 0 } of an orthonormal coordinate system are chosen in the directions along the principal axes and a right-handed rotation of 90 about 0z carries 0x into 0y, then the coordinate system is called a right-handed coordinate system. Analogly, if the basis vector are chosen in the directions along the principal axes and a left-handed rotation of 90 about 0z carries 0x into 0y, then the coordinate system is called a left-handed coordinate system. Throughout this text, we use only right-handed coordinate system. By adopted notation, throughout this handbook coordinate systems are denoted with uppercase not bold letters like: {A}, {B},. The origin of the coordinate system {A} is denoted by A0 as in Figure 5 where
1 0 0 x A = 0; y A = 1; z A = 0 0 0 1
(2.11)
represent three orthonormal vectors of the coordinate system {A}.
2.4. Coordinate transformations

The goal of this section is to define the relationship between two coordinate systems {A} and {B} in three-dimensional space.
2.4.1. Relation between two coordinate systems
Consider two coordinate systems shown in Figure 6. Let {A} be the orthonormal reference frame and {x A , y A , z A } be the unit vectors of the frame axis. The coordinate frame {B} is
ROBOTICS I
11
completely described with respect to {A} (its position and orientation is completely defined related to {A}) by the following vectors:
A
v A0 B0 - vector from the origin of {A} to the origin of {B} expressed related to {A}
xB A y B - orthonormal vectors of {B} expressed with respect to {A}, A zB

A
where for the sake of notation simplicity the following notation is adopted:
leading superscript denoting the reference systemA
v Bsubscript denoting the related system
Fig.6. Relationship between two coordinate systems The orthonormal vectors of related coordinate system {B} with respect to {B} are expressed by:
B
1 0 0 B B x B = 0; y B = 1; z B = 0 , 0 0 1
(2.12)
while related to {A} these vectors can be written in the form:

A
x a1 y a1 z a1 A A x B = x a 2 ; y B = y a 2 ; z B = z a 2 . x a ya z a 3 3 3
(2.13)
The vectors {A x B , A y B , A z B } are mutually orthogonal since they represent the unit vectors of an orthonormal coordinate system, i.e.
A
x B A y B = 0; A x B A z B = 0; A y B A z B = 0 .
(2.14)
Also, they are normalized to the unit magnitude:

A
xB =
yB =
z B = 1.
(2.15)
Taking into account constraint equations (2.14) and (2.15) it follows that the nine scalar quantities (2.13) being the coordinates of vectors A x B , A y B , A z B are not independent. The unit
ROBOTICS I
12
vectors A x B , A y B , A z B thus represent 9 6 = 3 independent scalar quantities that specify the orientation of the coordinate system {B} with respect to {A}. 2.4.2. Point transformations In many problems concerning the coordinate transformations, the position of a point is known in terms of one coordinate system, and it is necessary to determine the position of the same point with respect to another coordinate system. Consider the problem of point transformation depicted in Figure 7.
Fig. 7. Point transformation problem Starting from the given vectors:

B
v B0 1 - point vector defining the coordinates of the point 1 related to the {B}, v A 0 B0 - vector determining the position of the origin of {B} with respect to {A},
x B , A y B , A z B - vectors specifying the orientation of the coordinate system {B} with respect to the coordinate system {A}, the task is to find the vector
A
v A 0 1 - from the origin of the coordinate system {A} to the considered point 1, i.e. vec-
tor defining the coordinates of the point 1 with respect to {A}. The following vector equation can be written from the triangle A0B01 in Figure 7:
v A 0 1 = v A 0 B0 + v B0 1 .
(2.16)
If all vectors in above equation are expressed with respect to the coordinate system {A}, then the equation becomes:
A
v A 0 1 = A v A 0 B0 + A v B0 1 .
A
(2.17)
In contrast to the vector
v A0 B0 which is known, vector
v B0 1 from the origin of the coor-
dinate system {B} to the considered point 1 related to {A} should be determined. Vector v B0 1 can be expressed with respect to the coordinate system {B} in the form:
B
v B0 1 = b1 B x B + b 2 B y B + b 3 B x B ,
(2.18)
ROBOTICS I
13
where {b1, b2, b3} are scalars denoting the components of the vector B v B0 1 along the coordinate system {B} axes, and B x B , B y B , B z B are orthonormal vectors of the form (2.12). Since the vector B v B0 1 is given, the values of scalars {b1, b2, b3} are known. Hence the vector
v B0 1 can be expressed with respect to the coordinate system {A} as follows:

A
v B0 1 = b1 A x B + b 2 A y B + b 3 A z B .
(2.19)
Substituting equation (2.19) into the (2.17) yields the following equation:
A
v A 0 1 = A v A 0 B0 + b1 A x B + b 2 A y B + b 3 A z B ,
(2.20)
which can be written in compact matrix form as:

b1 v A 0 1 = v A 0 B0 + x B yB zB b 2 . b 3 For the sake of notation simplicity, from here on the following notation is adopted:
A A
(2.21)
p j - point vector defining the coordinates of a point j with respect to the coordinate sys-
tem {I}. Hence, I v I 0 j = I p j ,
R = [ A x B A y B A z B ] - (3 x 3) matrix defining the rotation of the coordinate system {B} with respect to {A}.
A B
With adopted notation, equation (2.21) can be written in the form:

A B p 1 = A p B0 + A B R p 1 .
(2.22)
Equation (2.22), which expresses the transformation of any point in one coordinate system to a reference coordinate system when the relative position and orientation of the pair of considered coordinate systems are known, can be written in a simpler form by introducing the homogenous transformation matrices of dimensions (4 x 4).
2.4.3. Homogenous transformations matrices

Homogenous coordinates have been introduced by the mathematician August Ferdinand Mbius and allow us to represent affine transformations1 by a matrix operation. Homogenous coordinates play also an important role in projective geometry. Homogenous coordinates embed three-dimensional space R3 into P3, the three-dimensional projective space, which is R4. As a result, inversions or combinations of linear transformations are simplified to inversion or multiplication of the corresponding matrices. Instead of representing each point (x*, y*, z*) in three-dimensional space with a single threedimensional point vector x * * (2.23) y , * z
1
Affine transformation is a combination of single transformations such as translation or rotation
ROBOTICS I
14
homogenous coordinates allow each point (x*, y*, z*) to be represented by any of an infinite number of four dimensional vectors: x y (2.24) z w where
x* =
x * y * z ,y = ,z = w w w
(2.25)
and w 0 is an arbitrary scalar value. Hence, the three-dimensional vector corresponding to any four-dimensional vector can be computed by dividing the first three elements by the fourth, and a four-dimensional vector corresponding to any three-dimensional vector can be created by simply adding a fourth element and setting it equal to one. Using the homogenous coordinates, equation (2.22) can be represented in the form:
A p1 A BR = 1 01 x 3
A
p B0 B p 1 1 1
(2.26)
where all (3 x 1) point vectors are expanded to (4 x 1) homogenous vectors with fourth component w =1. Introducing the notation
A B
AR T=B 01 x 3
A
p B0 , 1
(2.27)
equation (2.26) can be written in simpler form:

B p1 = A BT p 1 .
(2.28)
Matrix A BT is so-called transformation matrix of the coordinate system {B} to coordinate system {A}. It follows from the (2.26) that this matrix encompasses information about the rotation between two coordinate systems as well as information about the distance between their origins. Hence, in the case of pure rotation of the coordinate system {B} with respect to {A} matrix A BT is of the form:
A B
AR T=B 01 x 3
03 x1 , 1
(2.29)
and in the case of pure translation of the coordinate system {B} with respect to {A} matrix has the following form: A I3 p B0 A (2.30) = T . B 1 01 x 3
2.4.4. Inverse transformations

The task is: starting from given transformation matrix of the coordinate system {B} to reference coordinate system {A}, A BT , to determine the inverse
ROBOTICS I
15
( T)
A B
=B AT = ?
(2.31)
Transformation matrix B A T is simply the description of the reference coordinate frame with respect to the transformed frame. In other words, the inverse transformation is the transform which carries the transformed coordinate frame back to the original frame. Hence, the inverse of A BT can be obtained from equation (2.27) by interchanging the letters A and B as follows:
B A B A R T= 01 x 3 B A B
p A0 . 1
(2.32)
So, the inverse will be defined once the matrix determined.
R and the coordinates of the point B p A 0 are
The column vectors of matrix A B R defining the rotation of the coordinate system {B} with respect to {A} are mutually orthogonal since they represent the unit vectors of an orthonormal frame. Hence, the conditions (2.14) and (2.15) hold. As a consequence, A B R is an orthogonal matrix satisfying
A B
R T A BR = I
(2.33)
where I denotes the (3x3) identity matrix. Postmultiplying the both sides of (2.33) by the inverse matrix
( R)
A B
, the following useful result is obtained:

A B
RT =
( R)
A B
B =A R,
(2.34)
that is, the transpose of the rotation matrix is equal to its inverse.
B R , the remaining term B p A 0 in equation (2.32) can be determined using the With the known A
(2.22)
B B p B0 = B p A 0 + A R A p B 0 .
(2.35)
The point vector
p B0 defines the coordinates of the origin of the coordinate system {B}

B
with respect to the coordinate system {B}. Hence, it is zero vector:
p B0 = [0 0 0] .
T
(2.36)
Substituting (2.36) into (2.35) yields:

B B T A p A 0 = A R A p B0 = A B R p B0 .
(2.37)
Finally, transformation matrix B A T is of the form:

B A T A BR T= 0 1x 3 T A A B R p B0 . 1
(2.38)
In general, given a transformation matrix
ROBOTICS I
n x n T= y n z 0 ox oy oz 0 ny oy ay 0 ax ay az 0 nz oz az 0 px py , pz 1 p n p o , p a 1
16
(2.39)
the inverse is
T 1
n x o = x a x 0
(2.40)
where p, n, o and a are column vectors of the transformation matrix T and "." represents the vector dot (scalar) product.
2.4.5. Compound transformations

The coordinates of point 1 in Figure 8 are known in terms of the coordinate system {C}. The position and orientation of the coordinate system {C} are known relative to the coordinate system {B} while the position and orientation of the {B} coordinate system is known relative to the {A} coordinate system. The task is to determine the coordinates of point 1 with respect to coordinate system {A}.
Fig. 8. Compound transformation

A From the problem description it is obvious that the transformation matrices B C T and BT are known. Thus, the problem can be solved in two steps. First, the coordinates of point 1 with respect to the coordinate system {B} according to (2.28) are: B C p1 = B CT p 1 .
(2.41)
Finally, the coordinates of point 1 with respect to the coordinate system {A} are:
A B p1 = A BT p1 .
(2.42)
Combining the equations (2.41) and (2.42) yields:

A A B B C B C p1 = A BT C T p 1 .
(2.43)
The matrix T T transfers a point directly from the coordinate system {C} to the coordinate system {A}. Hence, it can be concluded that
A C B T= A BT C T .
(2.44)
ROBOTICS I
17
The ability to perform matrix multiplication to yield compound transformations is the primary reason for the introduction of the (4x4) homogenous transformation notation. The described ability is particularly useful in robotics since it enables the combination of mathematical and graphical description of a manipulator as open-chain constituted by n+1 links connected by n joints where a coordinate frame is attached to each link. The direction of arrow, pointing from one origin to another origin, indicates which way the frames are defined as it is shown in Figure 9 in the case of an open-chain constituting of 5 links.
Fig. 9. Coordinate transformations in an open-chain manipulator Then, the coordinate transformation describing the position and orientation of the coordinate frame {F} attached to the end-effector2 (end-effector frame) with respect to base frame {A} is given by the following transfer equation:
A F B C D E T= A BT C T D T E T F T .
(2.45)
2.4.6. Standard transformations

In many problems, the relationship between coordinate systems will be defined in terms of rotations about the x, y or z axes.
YB s in 0 {A } {B } cos cos YA
XB s in XA
Fig. 10. Rotation about the z axis A typical problem statement would be as follows: coordinate system {B} is initially aligned with coordinate system {A} and then rotated by degrees about the z axis. The task is to find A A y B A z B ] defining the rotation of the coordinate system {B} with the matrix A B R = [ xB respect to {A}. From Figure 10, that shows the x and y axes of the coordinate system {A} (solid lines) and of the coordinate system {B} (dashed lines) after the rotation by degrees about the z axis (that is perpendicular to the given x0y system), the following vectors are defined:
In robotics, an end-effector is a device or tool connected to the end of the robot arm.
ROBOTICS I
sin yB = cos ; 0 sin cos 0 0 zB = 0 . 1
18
cos xB = sin ; 0
(2.46)
Thus, the rotation matrix A B R is of the form: cos A B R = sin 0
0 0 . 1
(2.47)
Using the (4 x 4) homogenous coordinate transformation matrix notation, the transformation matrix A BT representing the rotation of the coordinate system {B} by degrees about the z axis with respect to {A} is of the form: cos sin 0 0 A B R 03 x1 sin cos 0 0 A (2.48) = BT = Rot ( z, ) = 0 0 1 0 0 1 1x 3 0 0 1 0 It can be easily shown that the following conditions hold: cos sin sin cos B A T = Rot ( z, ) = 0 0 0 0 0 0 1 0 0 0 , 0 1
(2.49)
Rot (z, ) Rot (z, ) = I; Rot (z, ) = Rot 1 (z, ) = Rot T (z, ) .
(2.50)
According to the same procedure as above, the transformation matrices representing the rotation of the coordinate system {B} by degrees about the x and by degrees about the y axes with respect to {A} are of the form: 0 1 0 cos Rot( x, ) = 0 sin 0 0 0 sin cos 0 0 cos 0 0 ; Rot( y, ) = sin 0 1 0 0 sin 1 0 0 cos 0 0 0 0 . 0 1
(2.51)
The following example shows the transformations of the point vector in one coordinate system by using the previously considered transformation matrices.
EXAMPLE 2.1: [3] The given point u = [7 3 2 1] is rotated by 90 about the z axis to point v. The obtained point v is then rotated by 90 about the y axis to point w. Lastly, the T point w is translated by a vector [4 3 7 1] . Calculate the transformations and represent them graphically.
T
The matrix representing rotation about the z axis by 90 is:
ROBOTICS I
0 1 1 0 Rot( z,90) = 0 0 0 0 0 0 1 0 0 0 . 0 1 0 0 1 0 0 7 0 3 . 0 2 1 1
19
(2.52)
Hence, the coordinates of the point v are obtained from the following equation: 3 0 1 7 1 0 v = Rot(z,90)u = 2 0 0 1 0 0
(2.53)
The graphical presentation of the above rotation of the point u by 90 about the z axis is shown in Figure11 (a)
(a)
(b) Fig. 11. (a) Rot(z,90) (b)Rot(y,90) Rot(z,90)
Now, obtained point v is rotated by 90 about the y axis as shown in Figure 11(b) which can be represented by the following transformation equation: 2 0 7 0 w = Rot( y,90) v = 3 1 1 0 0 1 0 0 1 0 0 0 0 3 0 7 . 0 2 1 1
(2.54)
Combining (2.53) and (2.54), the transformation from point u to point w can be represented as follows: 2 0 7 1 w = Rot( y,90) Rot(z,90)u = 3 0 1 0 0 0 1 0 1 0 0 0 0 7 0 3 . 0 2 1 1
(2.55)
Here it is important to note that the reverse order of rotations yields the different position of the final point. Namely, if the given point u is first rotated by 90 about the y axis and then by 90 about the z axis the coordinates of final point are:
ROBOTICS I
20
3 0 1 0 0 7 2 0 0 1 0 3 , w = Rot(z,90) Rot( y,90)u = (2.56) 7 1 0 0 0 2 1 0 0 0 1 1 which is graphically presented in Figure 12(a). The obtained result is expected having in mind that the matrix multiplication is noncommutative AB BA .
(a)
(b)
Fig. 12. (a) Rot(z,90)Rot(y,90) (b) Trans(4,-3,7)Rot(y,90)Rot(z,90) In order to solve the given task the point w determined by (2.55) is translated for [4 3 7 1]T and the overall transformation is: 1 0 Trans(4,3,7) Rot( y,90) Rot(z,90) = 0 0 Hence, the resulted point x is: 6 0 4 1 x = Trans(4,3,7) Rot( y,90) Rot(z,90)u = 10 0 1 0 as it is shown in Figure 12(b).

0 1 0 0
0 4 0 0 3 1 1 7 0 0 1 0
0 0 1 0
1 0 0 0
0 0 0 = 1 0 0 1 0 0 0 1 0
0 0 1 0
1 4 0 3 . (2.57) 0 7 0 1
1 4 7 0 3 3 0 7 2 0 1 1
(2.58)
2.4.7. Relative transformations Generally, transformations of coordinate systems (rotations and translation) can be made with respect to the reference coordinate system or with respect to the transformed coordinate system. Figure 13(a) shows the first case where the coordinate system (frame), initially aligned with the reference coordinate system, is first rotated about the reference z axis by 90, then roT tated by 90 about the reference y axis and finally translated by [4 3 7 1] . This transformation can be interpreted in reverse order with respect to transformed coordinate system (frame). In that case the frame, initially aligned with reference coordinate system, is first T translated by [4 3 7 1] , then rotated by 90 about the y axis of the translated coordinate system and lastly rotated about the current z axis as shown in Figure 13(b) [3].
ROBOTICS I
21
(a)
(b)
Fig. 13.Transformations with respect to reference coordinate system (a) to frame (b) In general, if we postmultiply a transform representing a frame by a second transformation describing a rotation and/or translation, we make that translation and/or rotation with respect to the frame axes described by the first transformation. If we premultiply the frame transformation by a transformation representing a translation and/or rotation, then that translation and/or rotation is made with respect to the base reference coordinate frame. Thus, given a frame C and a transformation T, corresponding to a rotation by 90 about the z axis, and a translation of 10 units in the x direction, we obtain a new position X when the change is made in base coordinates 0 0 1 0 0 1 0 10 1 0 0 20 1 0 0 20 1 0 0 0 0 0 1 10 = , X = TC 0 1 0 0 0 0 1 0 0 1 0 0 0 0 0 1 0 0 0 1 0 0 0 1 and a new position Y when the change is made relative to the frame axes as 0 1 0 30 1 0 0 0 1 10 0 0 = Y = CT 1 0 0 0 0 1 0 0 0 1 0 0 The results (2.59) i (2.60) are shown in Figure 14 [3]. 0 20 0 1 1 10 1 0 0 0 0 0 0 1 0 0 0 10 0 0 . 1 0 0 1
(2.59)
(2.60)
Fig. 14.Transformations with respect to base and frame coordinates
ROBOTICS I
22
EXAMPLE 2.2: [1] The coordinate system {B} is initially aligned with coordinate system T {A}. As first it is translated to the point [5 4 1 1] and then rotated by 30 about its x axis. Lastly, the coordinate system is rotated by 60 about an axis that passes through the T point [2 0 2 1] , measured in the current coordinate system, which is parallel to the y axis. Find A BT . Figure 15(a) shows the coordinate system {A} and the coordinate system {B} after all the transformations have been performed while Figure 15(b) shows all intermediate coordinate systems denoted by {C}, {D}, {E} and {F} respectively.
(a)
(b)
Fig. 15. (a) Initial and final coordinate systems. (b) Intermediate coordinate systems The coordinate system {C} was initially aligned with coordinate system {A} and then was T translated to the point [5 4 1 1] . Thus, the transformation matrix A C T is: 1 0 0 5 0 1 0 4 A . (2.61) C T = Trans(5,4,1) = 0 0 1 1 0 0 0 1 The coordinate system {D} was initially aligned with coordinate system {C} and then rotated by 30 about the x axis. Hence the matrix C D T is of the form: 0 0 0 1 0 cos 30 sin 30 0 C . (2.62) D T = Rot( x ,30) = 0 sin 30 cos 30 0 0 0 1 0 The last transformation of the coordinate system was a rotation by 60 about an axis parallel T to the y axis, which passes through the point [2 0 2 1] , measured in terms of the coordinate system {D}. The point that the axis of rotation passes through is denoted by M. From the Figure 15(b) it is apparent that D p M = B p M = [2 0 2 1]T . Because of this fact, the transformation that relates coordinate system {D} and coordinate system {B} will be formed by T translating to the point [2 0 2 1] (see coordinate system {E}), rotating by 60 about the current y axis (see coordinate system {F}), and then translating [ 2 0 2 1]T to obtain coordinate system {B}. The transformations that accomplish this are:
ROBOTICS I
cos 60 0 E T = F sin 60 0
A BT
23
1 0 D T = E 0 0
0 1 0 0
0 0 1 0
2 0 ; 2 1
0 sin 60 0 1 0 0 ; 0 cos 60 0 0 0 1
1 0 F T = B 0 0
0 1 0 0
0 2 0 0 . (2.63) 1 2 0 1
Hence, the overall transformation
is:
0 0.866 4.268 0.5 0.433 0.866 0.25 2.634 A A C D E F . BT= CT D T E T F T B T = 0.75 0.5 0.433 3.366 0 0 1 0 2.4.8. Transformation of a body
(2.64)
Figure 16(a) [3] shows an object described by six points with respect to a coordinate frame fixed in the object.
(a) Fig. 16. Transformation of a body
(b)
If the object is rotated by 90 about the z axis and then by 90 about the y axis, followed by a translation of 4 units in the x direction the transformation matrix representing the rotation and translation of a coordinate frame originally aligned with the reference coordinate frame is: 0 1 Trans(4,0,0) Rot( y,90) Rot(z,90) = 0 0 0 0 1 0 1 0 0 0 4 0 . 0 1
(2.65)
With determined transformation matrix (2.65) the transformation of six points of the object can be calculated from the following equation:
ROBOTICS I
4 4 0 1 1 = 1 4 4 0 1 1 0 0 0 1 0 1 0 0 0 4 1 1 1 0 0 0 0 0 0 0 2 1 1 1 1 1 0 2 1 1 1 4 4 . 0 0 1 1
24
4 4 6 1 1 1 0 0 0 1 1 1
6 1 0 1
(2.66)
The transformation result is shown in Figure 16(b).
2.5. Further properties of rotation matrix

In this chapter further possibility to represent the orientation of one coordinate system relative to another is considered. The nine elements in a general (3x3) rotation matrix, defining the rotation of one coordinate system with respect to another, are not independent quantities but related by six constraints due to the orthogonality conditions given by (2.14) and (2.15). This implies that three parameters (three different independent rotations around frame axes) are sufficient to describe orientation of a rigid body in space. Frequently used way to specify an arbitrary rotation matrix in terms of only three independent quantities is to use the so-called Euler angles. In the following three sets of Euler angles are analysed: the so-called ZYZ, ZYX and Roll, Pitch, Yaw angles. 2.5.1. Euler angles The rotation described by Euler angles, known as ZYZ angles, is obtained as composition of the following elementary rotations (Figure 17): rotation by degrees about the z axis, then rotation by degrees about the new y axis (y'), and, finally, rotation by degrees about the new z axis (z'').
Fig. 17. Euler angles The Euler transformation, Euler(, , ), can be computed by multiplying of the matrices of above mentioned elementary rotations, made with respect to the current frame (transformed coordinate system), as follows:
ROBOTICS I
25
Euler( , , ) = Rot( z, ) Rot( y' , ) Rot( z' ' , ) = cos sin = 0 0 sin cos 0 0 0 0 cos 0 0 0 1 0 sin 0 1 0 0 sin 1 0 0 0 c s s s c 0 0 cos 0 cos 0 sin 0 0 1 0 0 0 , 0 1 sin cos 0 0 0 0 0 0 = 1 0 0 1
(2.67)
c c c s s s c c + c s = s c 0
c c s s c s c s + c c s s 0
where the following notation has been used: (2.68) c = cos , s = sin , c = cos , s = sin , c = cos , s = sin . In robotics, the Euler angles are used to specify the orientation of the manipulators endeffector with respect to the reference (fixed) coordinate system usually attached to the robot base as shown in Figure 18.
Fig. 18. Position and orientation of the end-effector The illustrated Euler angles in Figure 18 are known as ZYX angles since they correspond to rotation by degrees about the z axis, then rotation by degrees about the new y axis (y'), and, finally, rotation by degrees about the new x axis (x''). In this case, the Euler transformation can be computed as follows:
Euler( , , ) = Rot( z, ) Rot( y' , ) Rot( x ' ' , ) = cos sin = 0 0 c c s c = s 0 0 0 cos cos 0 0 0 0 1 0 sin 0 0 1 0 c s s s c c s c s s s + c c s s c c s 0 sin 0 sin 1 0 0 cos 0 0 + s s c s 0 0 . 0 1 0 0 1 0 0 cos 0 0 sin 0 1 0 0 sin cos 0 0 0 = 0 1
(2.69)
c c 0
ROBOTICS I
26
2.5.2. Roll, Pitch, Yaw angles Another set of Euler angles originates from a representation of orientation in the (aero)nautical field. These are so-called Roll, Pitch and Yaw angles (RPY). These angles denote the typical motions of an (air)craft or a ship. Roll corresponds to a rotation by about the z axis, pitch corresponds to a rotation by about the y axis while yaw corresponds to a rotation by about the x axis. In the case of the manipulator end-effector the angles , and represent rotations defined with respect to a fixed frame attached to the centre of the endeffector as shown in Figure 19.
Fig. 19. Roll, pitch and yaw angles for a manipulator The resulting frame orientation is obtained by composition of rotations with respect to the fixed (reference) frame where the rotation about x axis is followed by a rotation about reference y axis and, finally, followed by rotation about the reference z axis. The corresponding rotation matrix can be computed via multiplication of the matrices of elementary rotation as follows:
RPY ( , , ) = Rot( z, ) Rot( y, ) Rot( x , ) = 0 cos sin 0 0 cos 0 sin 0 1 sin cos 0 0 0 1 0 0 0 cos = 0 0 1 0 sin 0 cos 0 0 sin 0 0 0 1 0 0 0 1 0 0 c c c s s s c c s c + s s 0 s c s s s + c c s s c c s 0 . = s c s c c 0 0 0 1 0 0 sin cos 0 0 0 = 0 1
(2.70)
ROBOTICS I
27
3. Manipulator kinematics
Kinematics is the science of motion regardless of the forces which cause it. Within the kinematics one studies the position, velocity, acceleration and all higher order derivatives of the position variables (with respect to time or any other variable(s)). Hence, the study of the kinematics of manipulators refers to all the geometrical and time based properties of the motion. The relationships between these motions and the forces and torques which cause them is the problem of dynamics.
A manipulator can be schematically represented from a mechanical point of view as a kinematic chain of rigid bodies (links) connected by means of revolute or prismatic joints. One end of the chain is constrained to a base, while an end-effector (gripper, tool) is mounted to the other end, as it was shown in principle in Figures 9 and 18. The resulting motion of the structure is obtained by composition of the elementary motions of each link with respect to the previous one. Therefore, in order to manipulate an object in space, it is necessary to describe the end-effector position and orientation with respect to the base coordinate system. There are two problems regarding the manipulator kinematics. The direct or forward kinematics problem is concerned with the relationship between the individual joints of the robot manipulator and the position and orientation of the tool or endeffector. Stated more formally, the forward kinematics problem is to determine the position and orientation of the end-effector frame, given the values for the joint variables of the robot3, relative to the robot base frame. Sometimes, the direct kinematics problem is stated as changing the representation of manipulator position from a joint space description into a Cartesian space description4. In contrast to forward kinematics problem, the inverse kinematics problem can be stated as follows: given a desired position and orientation for the end-effector of the robot, determine a set of joint variables that achieve the desired position and orientation.
3.1. Direct kinematics

3.1.1. Two-link planar manipulator The ability to perform matrix multiplication to yield compound transformations, providing combination of mathematical and graphical representation of the manipulator, will be used in the following to solve the direct kinematics problem for the two-link planar manipulator. Consider the schematic representation of a simple robot lying in the xoy plane shown in Figure 20. The robot has two links of lengths l1 and l2. The coordinate system {B1} is attached to joint connecting the links of the robot, while the coordinate frames {B0} and {B2} are attached to the robot base (reference frame {R}) and centre of the gripper (end-effector frame), respectively. 1 and 2 are the rotational angles of links 1 and 2.
The joint variables are the angles between the links in the case of revolute or rotational joints, and the link extension in the case of prismatic or sliding joints. The Cartesian space is the space in which the position of a point is given with 3 numbers, and in which the orientation of a body is given with 3 numbers. It is sometimes called task or operational space.
ROBOTICS I
28
Fig. 20. Two-link planar robot Using the homogenous transformations, the relative position and orientation between the embedded coordinate frames can be described as shown in the following manipulator transform graph
Fig. 21. The transform graph of the two-link planar manipulator According to discussion in 2.4.5 the position and orientation of the manipulator end with respect to the base (reference) coordinate system is given by: (3.1) T= 01T1 2T . j Historically, the homogenous matrix i T describing the position and orientation of the coordinate system {i} with respect to {j} has been called an A matrix. In the considered example A1 describes the position and orientation of the first link with respect to base and A2 describes the position and orientation of the second link with respect to the first. Hence, the equation (3.1) can be found in some literature [3] in the form: 0 (3.2) 2T = A 1 A 2 . From Figure 20 it is obvious that the position and orientation of the coordinate system {B1} with respect to the reference coordinate system can be described with the following transformation: c 1 s 1 0 l1 c 1 s c 1 0 l 1s 1 1 0 , = = T (3.3) Trans ( l c , l s , 0 ) Rot ( z , ) 1 1 1 1 1 1 0 0 1 0 0 0 1 0 where the following notation has been used: (3.4) c1 = cos 1 , s1 = sin 1 .
0 2
ROBOTICS I
29
The position and orientation of the coordinate system {B2} with respect to the {B1} is given by the following transformation: c 2 s 2 0 l 2 c 2 s c 2 0 l 2s 2 2 1 , = = T (3.5) Trans ( l c , l s , 0 ) Rot ( z , ) 2 2 2 2 2 2 0 0 1 0 0 0 1 0 where (3.6) c 2 = cos 2 , s 2 = sin 2 . Finally, the position and orientation of the end-effector frame with respect to the reference coordinate system is determined by: c 1 s 0 1 T = 2 0 0 where s1 c1 0 0 0 l1 c 1 c 2 0 l 1s 1 s 2 1 0 0 0 1 0 s2 c2 0 0 0 l 2 c 2 c12 0 l 2s 2 = s12 1 0 0 0 1 0 s12 c12 0 0 0 l1c1 + l 2 c12 0 l1s1 + l 2 s12 , 1 0 0 1
(3.7)
c12 = cos(1 + 2 ) = c1c 2 s1s 2 ; s12 = sin(1 + 2 ) = s1c 2 + c1s 2 .
(3.8)
The transformation matrix (3.7) represents the solution of direct kinematics problem for the considered two-link planar manipulator since it gives the answer to question given the angles at each of the robots joints, where is the robot's tool in the reference coordinate system (xtool, ytool, tool)? And according to (3.7) the answer is: x tool = l1c1 + l 2 c12 , y tool = l1s1 + l 2 s12 , (3.9)
tool = 1 + 2 .
For the general spatial case, the solution is not so trivial as in the case of simple planar robot considered above. This is because the joint angles do not simply add as they do in the planar case. Using Denavit-Hartenberg kinematics parameters provide a systematic, general method for the solution of direct kinematics problem. 3.1.2. Denavit-Hartenberg convention As it was said in the introduction of the current chapter, a manipulator can be thought of as a set of rigid bodies, called links, connected in a chain by joints which allow relative motion of neighbouring links. The joints can either be simple, such as a revolute joint or a prismatic joint, or else they can be more complex, such as a ball and socket joint (recall that a revolute joint is like a hinge and allows a relative rotation about a single axis, and a prismatic joint permits a linear motion along a single axis, namely an extension or retraction). The difference between these two situations is that, in the first instance, the joint has only one degree-offreedom of motion: the angle of rotation in the case of revolute joint and the amount of linear displacement in the case of prismatic joint. In contrast, a ball and socket joint has two degrees-of-freedom. In the rare case that a mechanism is built with a joint having n degrees of freedom, it can be modelled as n joints of one degree-of-freedom connected with n - 1 links of zero length. Therefore, without loss of generality, we will consider only manipulators that have joints with a single degree-of-freedom. Generally, the number of degrees-of-freedom
ROBOTICS I
30
(DOF) that a manipulator possesses is the number of independent position variables which have to be specified in order to locate all parts of the mechanism5. In order to compute the direct kinematics equation for an open-chain manipulator according to the expression (2.45), a systematic, general method is to be derived to define the relative position and orientation of two consecutive links. The links are numbered starting from the fixed base of the robot arm, which might be called link 0. The first moving body is link 1, and so on, out to the free end of the robot arm, which is link n6. A single link of a typical robot has many attributes which a mechanical designer has to consider during its design (the type of used material, the strength and stiffness of the link, the location and type of the joint bearings etc.). However, for the purpose of obtaining the kinematic equations of the robotic mechanism, a link is considered only as a rigid body that defines the relationship between the manipulator joints at each end of the link. Any link can be characterized by two dimensions. First one, usually denoted with ai, is the link length i.e. the length of the common normal7 between the axis i of the joint i, connecting link i - 1 to link i, and the axis i + 1 of the joint i + 1. The second parameter needed to define the relative location of the two joints (i.e. joint axes) is called the link twist. The twist of the link is the angle i between the axes in a plane perpendicular to ai as shown in Figure 22.
Fig. 22. The length a and twist of a link As shown in Figure 23, each axis has two normals to it, one for each link. The distance between these normals measured along the axis of joint i is usually denoted by di. The angle qi between the normals measured in a plane normal to the axis is known as the angle between the links or joint angle. Once the relative position and orientation of two consecutive links is determined by di and qi the problem is that to determine two frames attached to the two links and compute the coordinate transformations between them.
In the case of typical industrial robots, because a manipulator is usually an open kinematic chain, and because each joint position is usually defined with a single variable, the number of joints is equal to the numbers of DOF. In order to position an end-effector generally in 3D space, a minimum of 6 joints are required. This makes good intuitive sense as the description of an object in space requires 6 parameters-3 for position and 3 for orientation. The common normal between two lines is the line containing the minimum distance segment between two lines.
7 6
ROBOTICS I
31
Fig. 23. Denavit-Hartenberg kinematics parameters for revolute joints According to the so-called Denavit-Hartenberg convention the coordinate frame of link i is defined as follows: Choose axis zi along the axis of joint i + 1. Locate the origin Oi at the intersection of axis zi with the common normal to axes zi-1 and zi. Also, locate Oi ' at the intersection of the common normal with axis zi-1. Choose axis xi along the common normal to axis zi-1 and zi with direction from joint i to joint i + 1. The Denavit-Hartenberg convention gives a nonunique definition of the link frame in the following cases: For frame {0} only the direction of axis z0 is specified. The origin O0 and the axis x0 can be arbitrarily chosen. For frame {n}, since there is no joint n + 1, zn is not uniquely defined while xn has to be normal to axis zn-1. Typically, joint n is revolute, and thus zn is to be aligned with the direction of zn-1. When two consecutive axes are parallel, the common normal between them is not uniquely defined. When two consecutive axes intersect, the direction of xi is arbitrary. When joint i is prismatic, the direction of zi-1 is arbitrary. In all such cases, the indeterminacy can be exploited to simplify the procedure. For instance, the axes of consecutive frames can be made parallel. Once the link frames have been established, the Denavit-Hartenberg parameters (DH parameters) that completely specified the position and orientation of frame i with respect to frame i-1 can be defined as: ai distance between Oi and Oi ', di coordinate of Oi ' along zi-1. i angle between zi-1 and zi axes about axis xi. qi angle between axes xi-1 and xi about axis zi-1. The parameters ai and i are always constant and depend only on the geometry of connection between consecutive joints established by link i. From the remaining two parameters of the above four only one is variable depending on the type of joint that connects link i-1 to link i. In particular: If joint i is revolute (as in Figure 22) the variable is qi.
ROBOTICS I
32
If joint i is prismatic the variable is di. Having assigned coordinate frames to all links according to the preceding scheme, the relationship between successive frames {i-1} and {i} can be established by the following rotations and translations: rotation about the zi-1 by qi, translation along zi-1 for di, translation along the rotated axis xi-1 = xi for ai, and rotation about xi by i. This sequence of the rotations and translations can be represented as the product of the following homogenous transformations:
i 1 i
T(q i ) = A i (q i ) = Rot( z, q i )Trans(0,0, d i )Trans(a i ,0,0) Rot( x, i ) = sin q i cos q i 0 0 0 0 1 0 0 1 0 0 0 0 1 0 0 1 0 0
0 a i 1 0 0 0 0 0 0 cos i sin i 0 = (3.10) 1 d i 0 sin i cos i 0 0 1 0 0 0 1 sin q i sin i a i cos q i cos q i sin q i cos i sin q cos q i cos i cos q i sin i a i sin q i i . = 0 sin i cos i di 0 0 1 0 Having the homogenous transformation matrices i 1iT , i = 0, , n between successive frames i-1 and i, the transformation matrix between the fixed coordinate frame (attached to the base of manipulator) and the last coordinate frame attached to end-effector (end-effector frame) is: cos q i sin q i = 0 0
0 n i 1 n 2 n 1 T(q)= 01T(q 1 )1 2T(q 2 )K i T(q i )K n 1 T(q n 1 ) n T(q n ) ,
(3.11)
where the vector of joint variables is:

q = [q1
q 2 K q i K q n 1
q n ]T .
(3.12)
EXAMPLE 3.1: Figure 24 shows the three-link manipulator with coordinate frames assigned to the links.
Fig. 24. Three-link manipulator
ROBOTICS I
33
The DH link parameters for the considered three-link manipulator are specified in the following Table. Link Variable ai di i 1 2 3 q1 q2 q3 90 0 0 0 a2 a3 0 0 0
According to the (3.10) the homogenous transformation matrices between the successive coordinate frames are: c1 0 s 1 0 s 0 c 0 1 0 , 1 (3.13) 1T(q 1 ) = A 1 (q 1 ) = 0 1 0 0 0 0 0 1 c 2 s 1 2 2 T( q 2 ) = A 2 ( q 2 ) = 0 0 c 3 s 2 3 ( q ) ( q ) T A = = 3 3 3 3 0 0 s2 c2 0 0 s3 c3 0 0 0 a 2 c2 0 a 2 s2 , 1 0 0 1 0 a 3 c3 0 a 3 s3 , 1 0 0 1
(3.14)
(3.15)
where the following notation has been used: (3.16) c1 = cos q 1 , s1 = sin q 1 , c 2 = cos q 2 , s 2 = sin q 2 , c 3 = cos q 3 , s 3 = sin q 3 . 8 Finally, the position and orientation of the last coordinate frame with respect to the base coordinate system is determined by: c1c 23 c1s 23 s1 c1 (a 3 c 23 + a 2 c 2 ) s c 1 23 s1s 23 c1 s1 (a 3 c 23 + a 2 c 2 ) 0 0 1 2 , (3.17) 3T(q) = 1T 2 T 3T = A1 A 2 A 3 = s 23 c 23 0 a 3 s 23 + a 2 s 2 0 0 1 0 where:
c 23 = cos(q 2 + q 3 ), s 23 = sin(q 2 + q 3 ) .
(3.18)

EXAMPLE 3.2: Figure 25 shows the robot PUMA 560 with the given axis coordinate systems where the vector of joint variables is q = [1 2 3 4 5 6 ]T . (3.19)
The assigned parameters are: l1 = 432 mm, l2 = 149.5 mm, l3 = 432 mm and l4= 56.5 mm.
Note that here is no end-effector connected to the end of the robot arm.
ROBOTICS I
34
Fig. 25. The robot PUMA 560 The DH link parameters for the considered manipulator are specified in the following Table. Link Variable di ai i 1 2 3 4 5 6
1 2 3 4 5 6
-90 0 90 -90 90 0
0 432 0 0 0 0
0 149.5 0 432 0 56.5
According to the (3.10) the homogenous transformation ordinate frames are: 1 0 0 0 0 T A ( ) ( ) = = 1 1 1 1 0 1 0 0 1 0 1 2 T( 2 ) = A 2 ( 2 ) = 0 0 1 0 2 3T( 3 ) = A 3 ( 3 ) = 0 0 0 1 0 0
matrices between the successive co0 1 0 0 0 0 , 0 1
(3.20)
0 432 0 0 , 1 149.5 0 1 0 0 0 1 1 0 0 0 0 0 , 0 1
(3.21)
(3.22)
ROBOTICS I
0 0 1 0 , 0 432 0 1 0 0 , 0 1
35
1 0 0 0 3 ( ) = ( ) = T A 4 4 4 4 0 1 0 0 1 0 4 T A ( ) = ( ) = 5 5 5 5 0 0 1 0 5 6 T( 6 ) = A 6 ( 6 ) = 0 0
(3.23)
0 0 0 1 1 0 0 0 0 1 0 0
(3.24)
0 0 0 0 . 1 56.5 0 1
(3.25)
Finally, the position and orientation of the end-effector frame with respect to the base coordinate system is determined by the following transformation matrix: 1 0 0 432 0 1 0 149.5 0 . (3.26) 6 T(q ) = A 1 A 2 A 3 A 4 A 5 A 6 = 0 0 1 488.5 1 0 0 0

3.1.3. The orientation of manipulator's end-effector The homogenous transformation matrix between the coordinate system {n} attached to the end-effector and the fixed reference coordinate system (robot base) {0}
0R 0 n ( ) = T q n 0 0 0 x y , z 1
(3.27)
respect to the base coordinate system {0}, while the column [x y z ] represents the coordinates of the end-effector in the base coordinate system. With the specified numerical values 0 of the elements of the matrix n R it is possible to obtain the three external coordinates (e.g. Euler angles , and ) that define the orientation of the end-effector with respect to the base coordinate system. With the definition of these coordinates the direct kinematics problem is completely solved, i.e. the vector of external coordinates (Cartesian coordinates) T x e = [x y z ] , (3.28)
T
resulting from the solution of the direct kinematics problem, gives the analytical relationship between the matrix elements and the joint angles (joint variables) qi, i = 1,.., n. For the given joint variables it is possible to determine the numerical values of the elements of the 0n T ma0 trix. As it was said, the sub-matrix n R defines the rotation of the coordinate system {n} with
ROBOTICS I
36
describing the position and orientation of end-effector with respect to the reference coordinate system, is defined. The procedure for the determination of the Euler angles is as follows. Consider the given transformation matrix describing the position and orientation of the end-effector with respect to the robot base in the following form:
a 11 a 0 21 T = n a 31 a 41 a 12 a 22 a 32 a 42 a 13 a 23 a 33 a 43 a 14 a 24 . a 34 a 44
(3.29)
Comparing this expression with the Euler transformation matrix for the case of ZYZ angles c c c s s s c c + c s Euler( , , ) = s c 0 c c s s c s c s + c c s s 0 c s s s c 0 0 0 , 0 1
(3.30)
gives the following system of nine not-independent equations of three unknown variables: a 11 = c c c s s , a 21 = s c c + c s , a 31 = s c , a 12 = c c s s c , a 22 = s c s + c c , a 32 = s s , a 13 = c s , a 23 = s s , a 33 = c . (3.31) (3.32) (3.33) (3.34) (3.35) (3.36) (3.37) (3.38) (3.39)
There are different ways for the determination of the angles , and from the equations (3.31)-(3.39). One possible way (and maybe the most obvious) is as follows: from(3.39)
= arccos(a 33 ) ,
(3.40) (3.41) (3.42)
from (3.37) and (3.40) = arccos(a 13 / sin ) , from (3.33) and (3.40) = arccos(a 31 / sin ) . Although obvious, the above solution is useless for the following reasons:
a) In attempting to obtain an angle using the arccos function not only is the sign of the angle undefined but accuracy in determining the angle is itself dependent on the angle, i.e. cos( ) = cos( ) and sin(0) = sin(180) = 0 .
ROBOTICS I
37
b) Division by sin( ) in expressions (3.41) and (3.42) for and leads to inaccuracy whenever sin( ) is close to 0. c) Equations (3.41) and (3.42) are undefined when = 0 o or 180 . Because of above reasons it is recommended to use arctan function. An admissible solution can be obtained as follows. Multiplying the both sides of the equation (3.37) by s and (3.38) by c and subtracting the obtained equations yields: a 13s a 23 c = 0 , or
a 23 . a 13
(3.43)
= arctan
(3.44)
With the determined angle , the angle can be calculated by multiplying (3.37) by c and (3.38) by s and adding the obtained equations: a 13 c + a 23s = s , Equation (3.45) together with the equation (3.39) yields: a 13 c + a 23s a 33 (3.45)
= arctan
(3.46)
Finally the angle can be determined by multiplying the equations (3.33) by s and (3.36) by c and adding the obtained equations: a 31s + a 32 c = 0 . From (3.47) it follows:
a 32 a 31 .
(3.47)
= arctan
(3.48)
Notice that the determination of , and depends on the sign of nominators and denominators in corresponding expressions (3.44), (3.46) and (3.48). Therefore it is necessary to use the numerical function arctan29 instead of arctan. The ZYX and roll, pitch and yaw angles can be determined according to the procedure analogue to the one above described.
3.2. Inverse kinematics

The inverse kinematics problem is problem of finding the answer to question: "Given the desired position of the robot's tool, what must be the angles at all of the robots joints?". So, it can be said that the solution of inverse kinematics problem is much more useful then one of the direct kinematics problem. E.g. in the task of object gripping by robot arm if the software knows where the object is in relation to the shoulder, it simply needs to calculate the angles of
9
The function arctan2(x,y) computes the arctangent of the ratio x/y but utilizes the sign of each argument to determine which quadrant the resulting angle belongs to. This allows the correct determination of angle in a range of 2.
ROBOTICS I
38
the joints to reach it. Humans solve this problem all the time without even thinking about it. When one is eating his cereal in the morning he just reach out and grab his spoon. He doesn't think "my shoulder needs to do this, my elbow needs to do that, etc." But in the case of robot this problem must be exactly solved. According to discussion in 3.1 the direct kinematics problem can be written in a form other than (3.11): x e = f (q) , (3.49) where f : R n R m is nonlinear continuously differentiable vector function that map the joint variables (3.12) into Cartesian coordinates (3.28)10. Hence, the inverse kinematics problem is the problem of determination of the inverse function:
q = f 1 (x e ) .
(3.50)
If the vector of external coordinates xe is of the same dimension as vector of joint variables q, m = n, the manipulator is said to be non-redundant with respect to the given task described in the operational space. If n > m the manipulator is said to be redundant, i.e. there are infinite number of vectors of joint variables that correspond to given position of the manipulators end-effector. The extra DOF of a redundant manipulator may be profitably used to improve the ability of the manipulator itself (e.g. to interact with the environment, to avoid singular configurations, etc.). On the other hand, the control problem, and in particular the inverse kinematics, is more complex for redundant manipulators than for non-redundant ones. Since a typical industrial manipulators are not-redundant, the subject of interest in this handbook are only the methods of solution of inverse kinematic problem for not-redundant robots. The inverse kinematic problem is much more complex than the direct kinematic problem since the equations to solve are in general nonlinear, and thus it is not always easy or even possible to find the closed-form solution. Also, the questions of existence of a solution, and of multiple solution, arise. The existence or non-existence of a kinematic solution defines workspace of a given manipulator. The lack of a solution means that the manipulator can not attain the desired position and orientation because it lies outside of the manipulator's workspace11. There are no general algorithms which may be employed to solve a set of nonlinear equations forming the inverse kinematic problem. But, in principle, all proposed solution strategies can be split into two broad classes: analytical solutions and numerical solutions. Both solutions have some advantages and disadvantages which are discussed in the following.
3.2.1. Analytical solution

Since the function f in (3.50) is the complex nonlinear function of n variables it is not possible to find the general analytical solution of inverse kinematics problem for an arbitrary manipulator configuration. The basic principles of the analytical inverse kinematic solution is discussed in the following through the two simple examples:
Generally, the vector xe has m coordinates, where m is the number of coordinates needed for description of the certain class of manipulation tasks and n is the number of DOF of manipulator. Usually, it is adopted that m = 6 as in (3.28). Depending on task, it is possible to have smaller number of coordinates of xe. For example, if it is needed only to position the end-effector then the vector xe is of the form: x e = x
11
10
y z] .
T
Workspace of a robot is the space which the end-effector of the manipulator can reach.
ROBOTICS I
39
EXAMPLE 3.3: Consider the manipulator shown in Figure 20 whose direct kinematics was given in (3.7). It is desired to find the vector of joint variables q = [1 2 ]T corresponding to a given end-effector position and orientation.
Comparing the given transformation matrix describing the position and orientation of the endeffector with respect to the robot base with the (4x4) matrix in general form:
a 11 a 0 21 2T = a 31 a 41 a 12 a 22 a 32 a 42 a 13 a 23 a 33 a 43 a 14 c12 a 24 = s12 a 34 0 a 44 0 a 11 = c12 ,
a 21 = s12 , a 12 = s12 , a 22 = c12 , x = a 14 = l1c1 + l 2 c12 , y = a 24 = l1s1 + l 2 s12 .
s12 c12 0 0
0 l1c1 + l 2 c12 0 l1s1 + l 2 s12 , 1 0 0 1
(3.51)
yields the set of 6 nonlinear equations which must be solved for 1 and 2: (3.52) (3.53) (3.54) (3.55) (3.56) (3.57)
One possible solution of system (3.52)-(3.57), i.e. of inverse kinematics problem, can be obtained as follows: a 24 l 2 a 21 s a l a (3.58) tan 1 = 1 = 24 2 21 1 = arctan 2 . a l a c1 a 14 l 2 a 22 14 2 22 From (3.52) and (3.53) one can solve for the sum of 1 and 2: a 21 s a tan (1 + 2 ) = 12 = 21 1 + 2 = arctan 2 a . c12 a 11 11 The angle 2 can be calculated from (3.59), since the 1 is known by (3.58), as: (3.59)
2 = arctan 2
a 21 a 24 l 2 a 21 arctan 2 a l a . a 11 14 2 22
(3.60)
It is typical with manipulators that have two or more links moving in a plane that in the course of solution, expressions for sums of joint angles arise.

EXAMPLE 3.4: Consider the three-link manipulator shown in Figure 24 whose direct kinematics, i.e. analytical relationship between the elements of transformation matrix 03T and joint variables, has been determined by (3.17). The position of the last coordinate frame with respect to the base coordinate system is determined by:
x = c1 (a 3 c 23 + a 2 c 2 ) , y = s1 (a 3 c 23 + a 2 c 2 ) , (3.61) (3.62)
ROBOTICS I
z = a 3 s 23 + a 2 s 2 .
40
(3.63)
Multiplying (3.61) by s1 and (3.62) by c1 and subtracting the obtained equations yields:
s 1 x c1 y = 0 ,
(3.64)
or
y q 1 = arctan 2 . x
(3.65)
Having the q1, the joint variable q3 can be calculated by multiplying (3.61) by c1 and (3.62) by s1 and adding the obtained equations: c1 x + s1 y = a 3 c 23 + a 2 c 2 . Squaring the (3.66) and (3.63) and summing the squared equations one obtains: c3 =
2 (c 1 x + s 1 y ) 2 + z 2 a 3 a2 2 . 2a 2 a 3
(3.66)
(3.67)
The existence of a solution obviously imposes 1 c 3 1 , otherwise the given point would be outside the arm reachable workspace. Then, set
2 s3 = 1 c3
(3.68)
where the positive sign is relative to the robot elbow-down posture and the negative sign to the robot elbow-up posture. Hence, the angle q3 can be computed as:
s3 q 3 = arctan 2 c . 3
(3.69)
Starting from (3.66) and (3.63) the angle q2 can be obtained in form:
s2 = c2 = ( c 3 a 3 + a 2 ) z s 3 a 3 (c 1 x + s 1 y ) , 2 2 (c 3 a 3 + a 2 ) 2 + s 3 a3 (c 3 a 3 + a 2 )(c1 x + s1 y) + s 3 a 3 z , 2 2 (c 3 a 3 + a 2 ) 2 + s 3 a3
(3.70) (3.71)
i.e.
(c 3 a 3 + a 2 ) z s 3 a 3 (c1 x + s 1 y ) q 2 = arctan 2 (c a + a )(c x + s y) + s a z . 2 1 1 3 3 3 3
(3.72)

Some advantages of inverse kinematic analytical solution are:
The singularities, representing the configurations at which mobility of the structure is reduced (i.e. it is not possible to impose an arbitrary motion to the end-effector), are obvious and so it is possible to treat them. In the above considered example, from (3.65), it is obvious that the angle q1 is undefined if x = y = 0. This occurs when the end-effector is on the z axis. The second singularity is when the q3 = 0. The analytical solution results in all possible solutions to the manipulator kinematics.
ROBOTICS I
41
The analytical solution can be found in a fixed number of operations. Hence, as computationally fast this solution is desirable for real-time control.
All above statements indicate that the analytical (closed-form) solution of the inverse kinematic problem is desirable overall. However, the problem is that this solution is only possible on relatively simple manipulator configurations.
3.2.2. Numerical solution
In contrast to analytical solution of the inverse kinematics, the numerical solution can be applied to all robots. Hence, despite the fact that the numerical solutions to inverse kinematics are much slower than analytical ones and so more expensive, in most of the applications the numerical solution of inverse kinematic problem is necessary. Therefore it is important to find numerical method that is as fast as possible but still reasonably accurate and robust. Numerical solution results in a numerical, iterative solving of the system of equations (3.50). In contrast to analytical solution, numerical method returns only a single solution for vector of joint variables q corresponding the given vector of external (Cartesian) coordinates xe. This solution is the closest to some initial configuration q0
q = f 1 (x e , q 0 ) .
(3.73)
The most commonly used method for the solution of (3.73) is the Newton method that is in short described in the following. Consider the function F(q) = f (q) x e , (3.74) where, in the case of not-redundant robots, f : R n R n is nonlinear continuously differentiable vector function that map the joint variables (3.12) into Cartesian coordinates (3.28). It is needed to find the value of q for which F(q) = 0 nearby an approximation qk. The first order Taylor approximation of F at qk is
F(q k ) + J (q k )(q q k ) = 0 ,
(3.75)
where
f 1 q 1 f f (q) 2 = q J (q) = 1 q K f n q 1 f 1 q 2 f 2 q 2 K f n q 2 f 1 q n f 2 K q n , K K f n K q n K
(3.76)
is the so-called Jacobian matrix12 of partial derivatives of the function f (q) . Relation (3.75) yields:
k , q = q k J 1 (q k )(f (q k ) x e ) = q k + J 1 (q k )x e
(3.77)
where
J 1 (q k ) is the inverse of Jacobian,
12
Mathematicians call it the Jacobian matrix while roboticists usually shorten it to simply Jacobian
ROBOTICS I
42
k x e = x e f (q k ) is the increment of the vector of external (Cartesian) coordinates with respect to the joint variables corresponding to the approximation qk.
Substituting the q = q k +1 in (3.77) yields the iterative method that converges on solution when
k x e < . Hence, based on (3.77) it can be said that Jacobian shows how each component of
xe varies with respect to each joint angle. In another words, Jacobian gives the answer to question "What would happen to xe if the q is increased by a small amount?"
By dividing both sides of (3.77) by the differential time element, the Jacobian can be thought & and the vector of end-effector velocity as mapping between the vector of joint velocities q &e: x
&. & e = J (q)q x
(3.78)
& can be calculated & e , the vector of joint velocities q Given the vector of end-effector velocity x 13 by using the inverse of Jacobian : & = J 1 (q)x &e. q
(3.79)
Beside the usefulness in determining the inverse kinematics algorithms, Jacobian is useful for finding singular configuration, analysing redundancy and for deriving dynamic equations of motion and designing operational space control schemes [2]. To find the singularities of a manipulator is of great interest for the following reasons: Singularities represent configurations at which mobility of the structure is reduced; When the structure is at a singularity, infinite solutions to inverse kinematics problem may exist; In the neighbourhood of a singularity, small velocities in the operational space may cause large velocities in the joint space. Singularities can be classified into: Boundary singularities that occur when the manipulator is either outstretched or retracted. These singularities can be avoided on condition that the manipulator is not driven to the boundaries of its reachable workspace. Internal singularities occur inside the reachable workspace and are generally caused by the alignment of two or more axes of motion, or else by the attainment of particular endeffector configurations. Differently from above, these singularities constitute a serious problem, as they can be encountered anywhere in the reachable workspace for a planned path in the operational space. The illustration of the behaviour of a manipulator at a singularity, by using the Jacobian is given in the following example.
EXAMPLE 3.5: Consider the two-link planar manipulator shown in Figure 20 whose direct kinematics was given in (3.7). If we are interested only in position of the end-effector the coordinates of the vector xe are: x = l 2 c12 + l1c1 , (3.80) y = l 2 s12 + l1s1 .
13
In the case of redundant robots (n > m) the Jacobian is (mxn) matrix. Hence, the pseudo-inverse of J,
+
J = (J T J ) 1 J T has to be calculated
ROBOTICS I
43
Hence, the Jacobian matrix for the considered manipulator is: x 2 l 2 s12 l1s1 l 2 s12 = . (3.81) y l 2 c12 l 2 c12 + l1c1 2 To find the singular points of a mechanism the determinant of its Jacobian has to be examined. Where the determinant is equal to zero, the Jacobian has lost full rank, and is singular.
l 2 s12 l1s1 det J = det l 2 c12 + l1c1 l 2 s12 = l1 l 2 s 2 = 0 . l 2 c12
x J= 1 y 1
(3.82)
For l1 , l 2 0 , it is easy to find that the determinant in (3.83) vanishes whenever
2 = 0, 2 = ,
(3.83)
and 1 is irrelevant for the determination of singular configurations. Physically, when 2 = 0 the robot arm is stretched straight out (the arm tip is located on the outer boundary of the reachable workspace) as shown in Figure 26.
Fig. 26. Two-link planar arm at a boundary singularity Obviously, in the shown configuration, motion of the end-effector is possible only along one Cartesian direction (the one perpendicular to the arm). Therefore, the manipulator has lost one degree of freedom. Likewise when 2 = the arm is folded completely back on itself, and motion of the hand is again only possible in one Cartesian direction instead of two. The danger in applying (3.79) in a robot control system is that at a singular point, the inverse Jacobian blows up! This results in joint rates approaching infinity as the singularity is approached. For example, let the considered two-link robot moving its end-effector along the x axis at 1m/sec. By using the inverse of Jacobian matrix (3.81)
l 2 c12 l 2 s12 l c l c l s l s , 2 12 1 1 2 12 1 1 based on (3.80), the joint rates are obtained in the following form: & = c12 , & = c12 c1 . 1 2 l1s 2 l 2 s 2 l1s 2 J 1 = 1 l1 l 2 s 2
(3.84)
(3.85)
Obviously, as the arm stretches out toward 2 = 0 both joint rates go to infinity.

ROBOTICS I
44
4. Trajectory planning
Having the direct and the inverse kinematics problems solved, the next task is to compute a trajectory that describes the desired motion of a manipulator. At this point it is worth to note that the term path is used often as a synonym for trajectory. However, there is a difference between these two terms. A path denotes the locus of points in joint space the manipulator has to follow in the execution of the assigned motion. A path is then a pure geometric description of motion. On the other hand, a trajectory is a path on which a time law is specified, for instance in terms of velocities and/or accelerations at each point. Therefore, trajectory refers to a time history of position, velocity, and acceleration for each degree of freedom. Mostly, the motion of a manipulator is considered as motion of the end-effector (tool) frame relative to the robot base frame. Minimal requirement for a manipulator is the capability to move from an initial position to some desired final position as shown in Figure 27 [4].
(a) (b) Fig. 27. In executing a trajectory a manipulator should move from its initial position (a) to a desired goal position (b) in a smooth manner Sometimes it is necessary to specify the motion much more detailed than simply stating the desired final configuration. One way to include more details in a path description is to give a sequence of desired via points or intermediate points between the initial and final positions. Thus, in completing the motion, the end-effector frame must pass through a set of intermediate positions and orientations as described by the via points. Each of these via points is actually a frame which specifies both the position and orientation of the end-effector relative to the robot base. Along with these spatial constraints on the motion, the user may also wish to specify temporal attributes of the motion. For example, the time passed between via points might be specified in the description of the trajectory. It is desirable for the motion of the manipulator to be smooth. For the purpose of trajectory planning, a smooth function is defined as one which is continuous and has a continuous first derivative. Sometimes, a continuous second derivative is also desirable. There are a great variety of the ways that trajectory might be specified and planned. Any smooth function of time which passes through the via points could be used to specify the exact trajectory shape. In this chapter some of frequently used choices for these functions are discussed.
4.1. Joint space trajectory planning

In this section, the methods of trajectory generation in which the trajectory shapes are described in terms of functions of joint angles are considered.
ROBOTICS I
45
4.1.1. Third order polynomials
Consider the problem of moving the end-effector frame from its initial position to a goal position in a certain amount of time. Using the inverse kinematics the set of joint angles that correspond to the goal position and orientation can be calculated. The initial position of the manipulator is also known in the form of a set of joint angles. What is required is a function for each joint whose value at t0 is the initial position of the joint, and whose value at tf is the desired goal position of that joint. As shown in Figure 28 there are many smooth functions q(t) which might be used to interpolate the joint value.
Fig. 28. Several possible trajectory shapes for a single joint In defining a single smooth motion, at least four constraints on q(t) are obvious. Two constraints on the value of function, representing this motion, come from the selection of initial and final values: t 0 = 0, q(0) = q 0 , q ( t f ) = q f . (4.1) Additional two constraints are that the velocities at the beginning and the end of the motion segment are known values. In case they are zero, the additional constraints are: & (0) = 0, q & (t f ) = 0 . q (4.2) These four constraints (4.1) and (4.2) allow one to solve for four unknowns, or a third order polynomial, in the following form: q( t ) = a 3 t 3 + a 2 t 2 + a 1 t + a 0 . (4.3)
Taking the first derivative of the polynomial (4.3) one gets the following parabolic velocity profile:
& ( t ) = 3a 3 t 2 + 2a 2 t + a 1 , q
(4.4)
Combining (4.3) and (4.4) with the four desired constraints yields four equations in four unknowns:
2 2 q 0 = a 0, q f = a 3 t 3 f + a 2 t f + a 1 t f + a 0 , 0 = a 1, 0 = 3a 3 t f + 2a 2 t f + a 1.
(4.5)
Solving these equations for the ai, i =0,..,3 one obtains: a 0 = q 0 , a 1 = 0, a 2 = 3t f2 (q f q 0 ), a 3 = 2t f3 (q f q 0 ). (4.6)
ROBOTICS I
46
Using (4.6) one can calculate the third order polynomial that connects any initial joint angle position with any desired final position for the case of the zero velocities in initial and final manipulator positions. The same procedure is used for each joint individually.
EXAMPLE 4.1: A one-link robot with a rotary joint is motionless at q0 = 15. It is desired to move the joint in a smooth manner to qf = 75 in 3 seconds. The task is to find the coefficients of a third order polynomial that accomplishes this motion and brings the manipulator to rest at the goal.
Substituting q0 = 15 and qf = 75 into (4.6) one finds: a 0 = 15, a 1 = 0, a 2 = 20, a 3 = 4.44 . Using (4.3)-(4.4) yields:
& ( t ) = 13.32t 2 + 40 t. q( t ) = 4.44t 3 + 20t 2 + 15, q
(4.7) (4.8) (4.9)
According to (4.8) acceleration along the trajectory q(t) is:
& &( t ) = 26.64t + 40. q
Figure 29 shows the position, velocity and acceleration for the motion described by the above equations.
Fig. 29. Position, velocity and acceleration profiles for Example 4.1

4.1.2. Third order polynomials for a path with via points
So far the motions, described by a desired duration and a final goal point, have been considered. In general, the paths which include intermediate via points are to be specified. If the manipulator is to come to rest at each via point, then the third order polynomial solution of section 4.1.1 can be used. Usually, it is desired to pass through a via point without stopping, and so it is needed to generalize the way in which polynomials are fitted to the path constraints. As in the case of a single goal point, each via point is usually specified in terms of a desired position and orientation of the end-effector frame relative to the base frame. Each of these via points is "converted" into a set of desired joint angles by application of the inverse kinemat-
ROBOTICS I
47
ics. Then, the problem of computing third order polynomials which connect the via point values for each joint is considered. If desired velocities of the joints at the via points are known, then the third order polynomials can be determined as before, but now, the velocity constraints at each end are not zero, but rather, some known velocity. Thence, the constraints (4.2) become
& (0) = q & 0, q & (t f ) = q & f. q
(4.10)
Four equations describing this general third order polynomial are:

q 0 = a 0,
2 qf = a 3t 3 f + a 2 t f + a1t f + a 0, & 0 = a 1, q
(4.11)
& f = 3a 3 t f2 + 2a 2 t f + a 1. q Solving these equations for the ai, i =0,..,3 one obtains:
a 0 = q 0, & 0, a1 = q a2 = 1 2 3 &0 q &f, (q f q 0 ) q 2 tf tf tf 1 2 &0 +q & f ). (q f q 0 ) + 2 (q 3 tf tf (4.12)
a3 =
Using (4.12) one can calculate the third order polynomial that connects any initial and final positions with any initial and final velocities. If the desired joint velocities at each via point are known, then the (4.12) is to be applied to each segment to find the required polynomial. There are several ways in which desired velocity at the via points might be specified [4]: The user specifies the desired velocity at each via point in terms of a Cartesian linear and angular velocity of the end-effector frame at that instant. The system automatically chooses the velocities at the via points by applying a suitable heuristic in either Cartesian space or joint space. The system automatically chooses the velocities at the via points in such a way as to cause the acceleration at the via points to be continuous.
In the first option, Cartesian desired velocities at the via points are "mapped" to desired joint rates using the inverse Jacobian of the manipulator evaluated at the via point. If the manipulator is at a singular point at a particular via point, then the user is not free to assign an arbitrary velocity at this point. While it is useful capability of a trajectory generation scheme to be able to meet a desired velocity which the user specifies, it would be a burden to require that the user always make these specifications. Therefore, a convenient system should include either second or third option (or both). In the second option the system automatically chooses reasonable intermediate velocities using some kind of heuristic. Consider the path specified by the via points shown for some joint, q, in Figure 30.
ROBOTICS I
48
Fig. 30. Via points with desired velocities at the points indicates by tangents The reasonable choice of joint velocities at the via points have been made as indicated with small line segments representing tangents to the curve at each via point. This choice is the result of applying a conceptually and computationally simple heuristic. Imagine the via points connected with straight line segments. If the slope of these lines changes the sign, at the via point choose zero velocity. If the slope of these lines does not change the sign, choose the average of the two slopes as the via velocity. In this way, from specification of the desired via points alone, the system can choose the velocities at each point. In the third option, the system chooses velocities such that acceleration is continuous at the via point. To do this, a new splining solution is needed. In this kind of spline, we replace the (two) velocity constraints at the connection of two third order polynomials with the (two) constraints that: a) velocity be continuous and b) acceleration be continuous.
EXAMPLE 4.2: Find the coefficients of two third order polynomials which are connected in a two-segment spline with continuous velocity and acceleration at the intermediate via point. The initial angle is q0, the via point is qv, and the goal point is qg.
The first polynomial is q( t ) = a 13 t 3 + a 12 t 2 + a 11 t + a 10 , and the second is q( t ) = a 23 t 3 + a 22 t 2 + a 21 t + a 20 . (4.14) (4.13)
Each polynomial is evaluated over an interval starting at t = 0 and ending at t = tfi, where i = 1 or i = 2. The constraints we wish to enforce are
q 0 = a 10 ,
2 q v = a 13 t 3 f 1 + a 12 t f 1 + a 11 t f 1 + a 10 ,
q v = a 20 ,
2 q g = a 23 t 3 f 2 + a 22 t f 2 + a 21 t f 2 + a 20 ,
0 = a 11, 0 = 3a 23 t f22 + 2a 22 t f 2 + a 21, 3a 13 t f21 + 2a 12 t f 1 + a 11 = a 21, 6a 13 t f 1 + 2a 12 = 2a 22.
(4.15)
These constraints specify a linear equation problem of eight equations and eight unknowns. Solving for the case t f = t f 1 = t f 2 one obtains:
ROBOTICS I
a 10 = q 0 , a 11 = 0, a 12 = a 13 = 12q v 30q g 9q 0 4t f2 8q v + 3q g + 5q 0 4t 3 f 3q g 3q 0 4t f , ,
49
a 20 = q v , a 21 = a 22 = a 23 = ,
(4.16)
12q v + 6q g + 6q 0 4 t f2 8q v 3q g 5q 0 4t 3 f .

For the general case of n segments the equations which arise from insisting on continuous acceleration at the via points may be cast in matrix form which is solved to compute the velocities at the via points.
4.2. Higher order polynomials

If it is required to specify the position, velocity and acceleration at the beginning and the end of a trajectory segment, the total number of boundary condition is six enabling us to use a fifth order polynomial to plan the joint trajectory: q( t ) = a 5 t 5 + a 4 t 4 + a 3 t 3 + a 2 t 2 + a 1 t + a 0 . According to (4.17), the joint velocity and acceleration are:
& ( t ) = 5a 5 t 4 + 4a 4 t 3 + 3a 3 t 2 + 2a 2 t + a 1 , q & &( t ) = 20a 5 t 3 + 12a 4 t 2 + 6a 3 t + 2a 2 . q
(4.17)
(4.18)
The constraints are given as: q 0 = a 0,

4 3 2 qf = a5t5 f + a 4 t f + a 3 t f + a 2 t f + a1t f + a 0, & 0 = a 1, q 2 & f = 5a 5 t f4 + 4a 4 t 3 q f + 3a 3 t f + 2a 2 t f + a 1, & & 0 = 2a 2 , q 2 & & f = 20a 5 t 3 q f + 12a 4 t f + 6a 3 t f + 2a 2 .
(4.19)
These constraints specify a set of six linear equations with six unknowns whose solution is:
ROBOTICS I
50
a 0 = q 0, & 0, a1 = q & &0 q 2 & f + 12q & 0 )t f (3& &0 & & f )t f2 20q f 20q 0 (8q q q , a3 = 2t 3 f a2 = & f + 16q & 0 )t f + (3& & 0 2& & f )t f2 30q f + 30q 0 + (14q q q , a4 = 2t f4 a5 = & f + 6q & 0 )t f (& &0 & & f )t f2 12q f 12q 0 (6q q q . 2t 5 f
(4.20)
Various algorithms for computing smooth functions (e.g. polynomial) that pass through a given set of data points can be found in literature. Complete coverage is beyond the scope of this handbook.
EXAMPLE 4.3: A single-link rotary robot (Figure 31) is required to move from (0) = s = 30 to (2) = e = 100 in 2 s. The joint velocity and acceleration are both zero at the initial and final positions.
s
Fig. 31. A single-link rotary robot What is the lowest degree polynomial that can be used to accomplish the described joint motion? Find the coefficients of that polynomial. The constraints on the described joint motion are:
(0) = 30 o ; (0) = 0; (0) = 0; (2) = 100 ; (2) = 0; (2) = 0.

o . ..
..
(4.21)
Since six constraints have to be satisfied, the polynomial of at least fifth order is needed to interpolate the function representing the joint motion: (t) = a 5 t 5 + a 4 t 4 + a 3 t 3 + a 2 t 2 + a 1t + a 0 . (4.22) Hence, the joint velocity and acceleration along the path (4.22) are:
( t ) = 5a 5 t 4 + 4a 4 t 3 + 3a 3 t 2 + 2a 2 t + a 1 , ( t ) = 20a 5 t + 12a 4 t + 6a 3 t + 2a 2 .
3 2 ..
(4.23)
ROBOTICS I
51
Combining equations for the position, velocity and acceleration with the six given constraints yields six equations in six unknowns: a 0 = 30, 32a 5 + 16a 4 + 8a 3 + 4a 2 + 2a 1 + a 0 = 100, a 1 = 0, 80a 5 + 32a 4 + 12a 3 + 4a 2 + a 1 = 0, a 2 = 0, 160a 5 + 48a 4 + 12a 3 + 2a 2 = 0. Solving these equations for the a i , i = 0,...,5 we obtain: a 0 = 30; a 1 = 0; a 2 = 0; a 3 = 87.5; a 4 = 65.625; a 5 = 13.125 . Hence, the polynomial that can be used to accomplish the described joint motion is:
( t ) = 13.125t 5 65.625t 4 + 87.5t 3 + 30 .

ROBOTICS I
52
5. Robot motion control

In the previous chapter, trajectory planning technique has been presented which allows generating the reference inputs to the robot motion control system. The control problem for robot manipulators is the problem of determining the time history of joint inputs required to cause the end-effector to execute a commanded motion. The joint inputs may be joint forces and torques, or they may be inputs to the actuators, for example, voltage inputs to the motors, depending on the model used for controller design. The commanded motion is typically specified either as a sequence of end-effector positions and orientations, or as a continuous path. The command task may regard either the execution of specified motions for a manipulator operating in free space, or the execution of specified motions and contact forces for a manipulator whose end-effector is constrained by the environment. In this chapter the motion control in free space is discussed while the interaction control in constrained space is beyond the scope of this handbook [5].
5.1. The control problem

There are many control techniques that can be applied to the control of manipulators. The particular control method, as well as the way of its implementation, may have a significant influence on the manipulator performance and consequently on the range of its possible applications. For example, the need for trajectory tracking control may lead to hardware/software implementations which differ from those allowing point-to-point control where only reaching of the final position is concerned. On the other hand, the mechanical design of the manipulator has an influence on the type of needed control scheme. The driving system of the joints has also an effect on the used control strategy. Compare a robot actuated by electric motor with reduction gears of high ratios to a direct-drive robot using high-torque motors with no gear reduction. In the first case, the motor dynamics are linear and well understood and the effect of the gear reduction is largely to decouple the system by reducing the nonlinear coupling among the joints. However, the presence of the gears introduces friction, elasticity and backlash that may limit system performance more than it is due to configuration-dependent inertia, Coriolis and centrifugals forces, etc. In the case of a direct-drive robot, the problems of backlash, friction and elasticity due to the gears are eliminated. However, the nonlinear coupling among the links is now significant, and the dynamics of the motors themselves may be much more complex. As a consequence, different control strategies have to be thought of to obtain high performance. Without any concern to the specific type of mechanical manipulator, we define the joint variables, q1,..., qn, as it was said in previous chapters (e.g. Chapter 3) as the relative angles between the links. For example, i is the angle between the link i and link i - 1. A vector q = (q1,...,qn)T = (1,...,n)T with each i [0,2 ) , is called a configuration. The set of all possible configurations is called configuration space or joint space. The control actions (joint actuator generalized forces) are performed in the joint space, whereas the task specification (end-effector motion and forces) is usually carried out in the operational (Cartesian) space also named task space. Hence, it can be said that the task space is the space of all positions and orientations (called poses) of the endeffector. As it was said in previous chapters, a coordinate frame, called the base frame, or world frame, is attached at the base of the robot and a task frame is attached at the end-effector as shown in Figure 32 where the endeffector position is represented by a vector x (specifying the coordinates of the origin of the task frame in the base frame) and R is the matrix describing the endeffector orientation.
ROBOTICS I
53
Fig. 32. A link manipulator with the attached base frame, task frame, and configuration variables The fact that task specification is usually carried out in the operational space, whereas control actions are performed in the joint space leads to considering two types of general control schemes: configuration space control (Figure 33) and Cartesian (operational) space control.
Fig. 33. Feedback robot control in configuration space As shown, the reference input (set point) in configuration space control is the desired vector of joint variables qd. Since the manipulator task (control goal) is usually defined through the given vector xed, the configuration space control problem can be considered as consisting of two subproblems. As shown in Figure 34, first manipulator inverse kinematics is solved to transform motion requirements, given by vector xed, from the operational space to joint space. Then, a configuration space control scheme is performed to track the qd. Bearing in mind that the reference input (set point) is defined in Cartesian space, the control structure shown in Figure 34 in literature is also treated as robot control in Cartesian space.
Fig. 34. Standard robot control structure in Cartesian space The disadvantage of solution shown above is that the operational space variables are controlled in an open-loop through the manipulator mechanical structure (open-loop control with respect to the TCP-Tool Center Point). The conceptual advantage of the closed-loop Carte-
ROBOTICS I
54
14
sian space control (closed-loop control with respect to the TCP), shown in Figure 35 gards the possibility of acting directly on operational space variables.
, re-
Fig. 35. Closed-loop control with respect to tool center point (TCP) However, above mention advantage is only a potential advantage since measurement of operational space variable is often performed not directly, but through a direct kinematics starting from measured joint space variables. Since the operational space control is at basis of interaction control in constrained manipulator motion, in the following only some joint space control schemes for manipulator motion in the free space are presented.
5.2. Independent joint control

The simplest type of control strategy is the independent joint control. In this type of control each axis of the manipulator is controlled as a single-input / single-output system. Any coupling effects due to the motion of the other links is either ignored or treated as a disturbance. Since each joint is considered independently of the others, the design of the control algorithm leads to a decentralized control structure. The joint i is driven by an actuator, which may be electric, hydraulic or pneumatic. Hence, the plant is the actuator and the joint to be controlled, i.e. joint i drive system. In the following first the model of the mechanical part of the system, the equations of motion of a manipulator, and than the mathematical model of the actuator are considered.
5.2.1. Equations of motion

The dynamic equations (equations of motion) of a manipulator can be written in the following form:
&& + C(q, q & )q & + Fv q & + Fs sgn(q & ) + g (q ) = J T (q )h B (q )q
(5.1)
where:
q - the vector of joint variables;

B (q ) - (n x n) symmetric, positive definite inertia matrix of the manipulator; & )q & - (n x 1) vector of centrifugal and Coriolis terms15; C(q, q
Fv - (n x n) diagonal matrix of viscous friction coefficients;

14 15
Notice that inverse kinematics is here embedded into the feedback control loop.
& )q & caused by centrifugal force depend on the square of joint velocity, while the terms The terms in C(q, q caused by Coriolis force contain the product of two different joint velocities.
ROBOTICS I
55
& - viscous friction torques; Fv q
Fs - (n x n) diagonal matrix of static friction coefficients;

& ) - simplified model of Coulomb friction torques; Fs sgn(q
& ) - (n x 1) vector whose components are given by the sgn functions of the single sgn( q joint velocities; g (q) - (n x 1) vector of gravity terms;
- input generalized forces;

J T (q)h - joint torques; J (q) - the Jacobian of the manipulator;
h - the force acting on the robot due to contact with the environment.
In the absence of external end-effector forces and of static friction the equations of motion can be written in the form: && + C(q, q & )q & + Fv q & + g(q) = . B(q)q (5.2) It is important to understand exactly what the above equation represents. The equation (5.2) represents the dynamics of an interconnected chain of ideal rigid bodies, supposing that there is generalized force acting at the joints. It can be assumed that the i-th component i of the generalized force vector is a torque about the axis zi-1 if joint i is revolute and is a force along zi-1 if joint i is prismatic. Even though (5.2) is an idealization since a number of dynamic effects are not included in it (e.g. no physical body is completely rigid), it is extremely complicated for all but the simplest manipulators. A more detailed analysis of robot dynamics would include various sources of flexibility, such as elastic deformation of bearings and gears, deflection of the links under load, and vibrations. Here we are interested mainly in the dynamics of the actuators producing the generalized force. To solve the problem of motion control in free space means to determine the n components of generalized forces that allow execution of a motion q(t) so that
q( t ) = q d ( t ) ,
where qd(t) denotes the vector of desired joint trajectory variables.
(5.3)
5.2.2. Actuator model

In the following, the permanent magnet DC-motor is treated since it is common for use in present-day robots. Consider the schematic diagram of the armature circuit of a DC-motor in Figure 36 where: V(t) - the armature voltage; L the armature inductance; R the armature resistance; Vb the back emf; ia the armature current;
m rotor position (the angular position of the motor shaft);
ROBOTICS I
56
m the torque generated by the motor; l the load torque.
Fig. 36. A DC-motor The armature circuit is described by the following equation: di L a + Ri a = V Vb . (5.4) dt The back emf, arising in the armature circuit when the motor is driving the load, is linearly & of the motor shaft, thus: proportional to the angular velocity m
& , Vb = K b m
(5.5)
where Kb is the back emf constant. The torque m generated by the motor is given by: m = K mi a , (5.6)
where Km is the motor constant. The torque must drive the load (Figure 37). This gives the differential equation: & +B & J m& (5.7) m m m = m r l , where: Jm = Ja + Jg - the sum of the actuator and gear inertias; Bm the viscous friction; r the gear ratio.
Fig. 37. Model of a single link16 with actuator and reduction gears In the Laplace domain the equations (5.4)-(5.7) can be written as:
16
Subscript i, denoting the i-th link, has been omitted for notation simplicity.
ROBOTICS I
(Ls + R )I a (s) = V(s) K b s m (s) ,
57
(5.8) (5.9)
( J m s 2 + B m s) m (s) = K m I a (s) r l (s) .
The block-diagram of the above system is shown in Figure 3817.
Fig. 38. Block-diagram of a DC motor driving the single link The transfer function from V(s) to m(s) is:
m (s) . = V (s) L K bK m J m s s + 1 B s + 1 + RB R m m Km RB m
(5.10)
Frequently it is assumed that the electrical time constant

chanical time constant
L is much smaller than the meR
Jm . This is a reasonable assumption for many electro-mechanical Bm
systems and leads to a reduced order model of the actuator dynamics. The block-diagram of reduced order model of voltage-controlled actuator (with neglected viscous friction Bm) is given in Figure 39.
Fig. 39. Block-diagram of a reduced order model of system (5.10) If the output side of the gear train is directly coupled to the robot link, then the joint variables and the motor variable are related by
17
For the sake of notational simplicity the Laplace transforms of time-dependent functions are indicated by capital letters without specifying dependence on complex variable s.
ROBOTICS I
q i = si = 1 m , i = 1, K , n , ri i
58
(5.11)
where ri is the i-th gear ratio.
5.2.3. Feedback control

The goal is to design the control providing the each joint to achieve the reference position, i.e. end-effector of a robot manipulator to reach the desired position in spite of disturbances. The simplest approach to the control of the system shown in Figure 39 is to consider the nonlinear term l i = i , i = 1, K , n entering (5.9) and defined by (5.2), which represents the nonlinear inertial, centripetal, coriolis and gravitational coupling effects due to the motion of the manipulator, as an input disturbance to the motor and design an independent controller for each joint according to the model (5.10). There are a number of control problem solutions. Figure 40 shows a common position feedback control of a single joint
Fig. 40. Block-diagram of position feedback control where disturbance D represents interaction with the other joints. The implemented control is proportional-integral (PI) control action of transfer function C P (s) = K P 1 + sTp s , (5.12)
that yields zero steady-state error for a constant disturbance and offers a stabilizing action due to presence of the real zero at s = 1 / TP. According to Figure 40, the open-loop transfer function is: 1 s+ Tp G o (s) = K , (5.13) 1 s2 s + T m where , Tm with the velocity-to-voltage gain and time constant of the motor respectively RJ m 1 , Tm = Kt = . Kb KbKm K= K p K t Tp (5.14)
(5.15)
ROBOTICS I
59
A root locus analysis can be performed as a function of the gain K of the position control loop. Three situations are illustrated in Figure 41 with reference to the relation between Tp and Tm.
(a)
(b)
ROBOTICS I
(c)
60
Fig. 41. Root locus for the position feedback control scheme Obviously, stability of the closed-loop system imposes some constraints on the choice of the parameters of the PI controller. In Figure 41(a) two poles of the closed-loop system are in the right-half- s plane for any K. Hence, if Tp < Tm the system is inherently unstable. Therefore, the controller parameter must be chosen so that Tp > Tm (Figure 41b). As Tp increases, the absolute value of the real part of the two roots of the locus (dominant poles) tending towards the asymptotes increases too, and the system has faster time response. Hence, it is convenient to provide Tp >> Tm. The effective rejection of the disturbance D on the output can be ensured by a large value of the gain KP. However, it is not advisable to increase KP too much, because small damping ratios would result leading to unacceptable oscillations of the output. To improve dynamic performance and for the purpose of achieving good disturbance rejection, it is worth choosing the controller as a cascade of elementary actions with local feedback loops closed around the disturbance. Besides closure of a position feedback loop, the more general solution is obtained by closing inner loop on velocity too (Figure 42). One of the reasons for the implementation of cascade control is to provide the achievement of the desired joint position without overshoot. Namely, when the joint i reaches the desired position ir in position control-loop the control error becomes zero. Hence, the input voltage signal of the actuator is zero and the actuator stops. However, due to inertia of both rotor of the motor and manipulator itself, the stop of the motor can not be immediately. Hence, the overshoot is possible, i.e. the actual angular position i can exceed the desired position ir before the actuator stops. The inclusion of the velocity feedback can damp sudden changes in the position of the motor shaft caused by the position loop.
Fig. 42. Block diagram of position control with angular speed control As shown in Figure 42 the implemented controllers in the outer (position) loop and in the inner (velocity) loop are respectively:
P controller : C P (s) = K P ; PI controller : C V (s) = K V

The closed-loop transfer function is:
1 + sTV , s
(5.16)
K t K P K V (1 + sTV ) m (s) = 3 , 2 mr (s) s Tm + s (1 + K t K V TV ) + s(K t K V + K t K V K P TV ) + K t K P K V

defining the characteristic equation of the closed-loop system:
(5.17)
ROBOTICS I
61
s 3 Tm + s 2 (1 + K t K V TV ) + s( K t K V + K t K V K P TV ) + K t K P K V = 0 .
(5.18)
Applying the Routh stability criterion to the equation (5.18), according to the following table s3 s2 s1
Tm
1 + K t K V TV
K t K V (1 + TV K P ) KtKPKV
Tm K P K t K V 1 + TV K P (1 + K t K V K P )
KtKVKP
s0
It follows that the closed-loop system is stable if the controllers parameters are positive, and in addition,
1 + K P TV >
Tm K P . (1 + K t K V K P )
(5.19)
To carry out a root locus analysis as a function of the velocity closed-loop gain, the characteristic equation (5.18) can be transformed to the following form:
1 K t K V TV s + T (s + K p ) V 1 + W (s) = 1 + , 1 2 Tm s s + T m
(5.20)
where the W(s) is the open-loop transfer function. The zero of the controller s = 1 / TV can be chosen so as to cancel the effects of the real pole of the motor s = 1 / Tm. Then, by setting TV = Tm the poles of the closed-loop system move on the root locus as a function of the loop gain KtKV as shown in Figure 43.
Fig. 43. Root locus for the position and velocity feedback control scheme By increasing the position feedback gain KP, it is possible to confine the closed-loop poles into a region of the complex plane with large absolute values of the real part. Then, the actual location can be established by a suitable choice of the parameter KV.
ROBOTICS I
62
The most general solution of cascade control is obtained by closing inner loops on velocity and acceleration besides closure of a position control loop as shown in Figure 44
Fig. 44. Block diagram of position control with velocity and acceleration control where CP(s), CV (s) and CA(s) represent position, velocity and acceleration controllers respectively. Controllers are usually of P, PI or PID type. The analysis on the choice of the parameters of controllers can be performed as shown above for the cases of position control and position control with velocity control
5.2.4. Decentralized feedforward control
Cascade control, described in the previous section, is often used in industry for motion control. However, , the tracking capabilities of this control scheme are unavoidably degrade when the joint control servos are required to track reference trajectories with high values of speed and acceleration. This can usually be solved by decentralized feedforward control. In the following the idea of feedforward control is presented on the example of the system shown in Figure 45
Fig. 45. Feedforward control scheme where r is an arbitrary reference trajectory, G(s) is the transfer function of a plant and C(s) is feedback controller transfer function. A feedforward control scheme consists of adding a feedforward path with transfer function F(s). Let each of the three transfer functions be represented as ratios of polynomials: q(s) c(s) a (s) G (s) = ; C(s) = ; F(s) = . (5.21) p(s) d(s) b(s) Assumption is that the G(s) is strictly proper and C(s) is proper function. According to the block-diagram shown in Figure 44 the closed-loop transfer function is: Y(s) q(s)[c(s)b(s) + a (s)d (s)] = . (5.22) R (s) b(s)[p(s)d(s) + q(s)c(s)]
ROBOTICS I
63
Hence, the closed-loop characteristic polynomial is:
b(s)[p(s)d(s) + q(s)c(s)] .
(5.23)
Therefore, for stability of the closed-loop system the controller and the feedforward transfer function must be chosen so that the polynomial (5.23) is Hurwitz. That means that feedforward transfer function F(s) must itself be stable. If the feedforward transfer function F(s) is chosen equal to G1(s), i.e. a(s)=p(s) and b(s)=q(s), then from the closed-loop transfer function (5.22) it follows:
q(s)[p(s)d(s) + q(s)c(s)]Y(s) = q(s)[p(s)d(s) + q(s)c(s)]R (s) ,

or, in terms of the tracking error E(s) = R(s) Y(s),
(5.24)
q(s)[p(s)d(s) + q(s)c(s)]E(s) = 0 .
(5.25)
Thus, assuming stability of the closed-loop system, the output y(t) will track any reference trajectory r(t). Note that we can choose F(s) as the inverse of the plant transfer function only if the plant is system of minimum phase (with all zeroes in the left-half-s plane). Applying of the idea of feedforward control to the system shown in Figure 39 result into the control scheme shown in Figure 46.
Fig. 46. Block-diagram of position feedback control with feedforward compensation Notice that computing time derivatives of the desired trajectory is not a problem, once mr(t) is known analytically from the trajectory planner. The equivalent control schemes can be obtained for the position control systems with cascade control shown in Figures 42 and 44.
ROBOTICS I
64
REFERENCES
[1] Crane, C., D., Duffy, J., Kinematic Analysis of Robot Manipulators, Cambridge University Press, 1998. [2] Sciavicco, L., Siciliano, B., Modelling and Control of Robot Manipulators, SpringerVerlag London Limited, 2000. [3] Paul, R., Robot Manipulators: Mathematics, Programming and Control, The MIT Press, Cambridge, 1981. [4] Craig, J. J., Introduction to Robotics, Mechanics and Control, Addison-Wesley Publishing Company, Massachusetts, 1986. [5] Spong, W. M., Robot Dynamics and Control, John Wiley & Sons, 1989.

Robotics1 SS09 PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotics1 SS09 PDF

Uploaded by

Copyright:

Available Formats

Institute of Automation

Prof. Dr.-Ing. A. Grser

Summer Semester 2009

University of Bremen Institute of Automation

University of Bremen Institute of Automation

Further properties of rotation matrix........................................................................ 24

University of Bremen Institute of Automation

Numerical solution ........................................................................................... 41

Robot control.................................................................................................................... 52 5.1. 5.2.

5.2.1. 5.2.2. 5.2.3. 5.2.4.

University of Bremen Institute of Automation

1.1. Robotics applications

University of Bremen Institute of Automation

University of Bremen Institute of Automation

University of Bremen Institute of Automation

r1n r2 n . (2.1) M rmn matrix R is said to be diagonal if

University of Bremen Institute of Automation

where v i , i = 1,2,3 are real numbers.

where is the angle between the two vectors as it is shown in Figure 2.

2.3. Cartesian coordinate systems

University of Bremen Institute of Automation

Fig. 4. Cartesian coordinate system

Fig. 5. Coordinate system {A}

represent three orthonormal vectors of the coordinate system {A}.

2.4. Coordinate transformations

University of Bremen Institute of Automation

xB A y B - orthonormal vectors of {B} expressed with respect to {A}, A zB

v Bsubscript denoting the related system

while related to {A} these vectors can be written in the form:

Also, they are normalized to the unit magnitude:

University of Bremen Institute of Automation

Fig. 7. Point transformation problem Starting from the given vectors:

In contrast to the vector

v A0 B0 which is known, vector

v B0 1 from the origin of the coor-

University of Bremen Institute of Automation

v B0 1 can be expressed with respect to the coordinate system {A} as follows:

which can be written in compact matrix form as:

tem {I}. Hence, I v I 0 j = I p j ,

With adopted notation, equation (2.21) can be written in the form:

2.4.3. Homogenous transformations matrices

Affine transformation is a combination of single transformations such as translation or rotation

University of Bremen Institute of Automation

equation (2.26) can be written in simpler form:

2.4.4. Inverse transformations

University of Bremen Institute of Automation

So, the inverse will be defined once the matrix determined.

R and the coordinates of the point B p A 0 are

, the following useful result is obtained:

The point vector

p B0 defines the coordinates of the origin of the coordinate system {B}

with respect to the coordinate system {B}. Hence, it is zero vector:

Substituting (2.36) into (2.35) yields:

Finally, transformation matrix B A T is of the form:

In general, given a transformation matrix

University of Bremen Institute of Automation

2.4.5. Compound transformations

Fig. 8. Compound transformation

Combining the equations (2.41) and (2.42) yields:

University of Bremen Institute of Automation

2.4.6. Standard transformations

University of Bremen Institute of Automation

Thus, the rotation matrix A B R is of the form: cos A B R = sin 0