Вы находитесь на странице: 1из 19

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

2.1 Representing Positions

The main approach to study robots and how to manipulate them is to adopt an analytical
approach, where we represent the points or lines using coordinates and equations, and
manipulation is being done via algebraic manipulations. The first step in the analysis requires the
choice of a reference coordinate frame. A coordinate frame consists of an origin (a single point
in space) and three orthogonal coordinate axes (for three dimensional spaces).

Figure 2.1: Vector relative to frame.

The second step is to locate any point in the universe with a 3 1 position vector. Since the
location of any point in universe depends on the coordinate frame, vectors are written with a
leading superscript indicating the coordinate frame to which the vector is referring. A point A P
therefore is represented as a vector in coordinate frame  A and can equivalently be thought of
as a position in space, or simply as an ordered set of three numbers that indicate distances along
the axes of  A .

19

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

 A Px 
 
A
P   A Py  (2.1)
A 
 Pz 

Note that while a point corresponds to a specific location in space, a vector specifies a direction
and a magnitude. Vectors can be used, for example, to represent displacement or forces.
Therefore, while the point A P is not equivalent to vector v1 , the displacement from the origin
o0 to point A P is given by the vector v1 . In a different coordinate frame, for example  B , the
same point is represented by vector v2 with three numbers

 B Px 
 
B
P   B Py  (2.2)
B 
 Pz 

Another issue with vectors to remember is that in this course, when we refer to a vector, what we
mean is free vector, that is, vectors that are not constrained to be located at a particular point in
space. Under this convention, two vectors are equal if they have the same direction and the same
magnitude. In cases where there is only a single coordinate frame, or in which the reference
frame is obvious, we will often omit the A and B in the superscripts.

We said earlier that one of our main tools in study robots is algebraic manipulations using
coordinates. Therefore it is essential that all coordinate vectors be defined with respect to the
same reference coordinate frame. In case of free vectors, it is enough that they be defined with
respect to “parallel” coordinate frames, that is, frames whose respective coordinate axes are
parallel, since only their magnitude and direction are specified and not their absolute locations in
space.

Using this convention, an expression of the form 0 v1  1v2 as shown in Figure 2.2 is not defined
since the frames 0 and 1 are not parallel. Thus, we see a clear need not only for a
representation system that allows points to be expressed with respect to various coordinate
frames, but also for a mechanism that allows us to transform the coordinates of points from one
coordinate frame to another. Such coordinate transformations are the topic of the remainder of
this chapter.

20

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Figure 2.2: Two coordinate frames, a point p, and two vectors v1 and v2

2.2 Representing Orientation

As we mentioned in the first chapter, we not only need to know the location of the robot in
space, we need to know the orientation of the body as well. In order to represent the relative
position and orientation of a body, we will attach coordinate frames to the body, and then specify
the geometric relation between the body and the reference coordinates. In Figure 2.3, coordinate
frame B has been attached to the body in a known way. By describing B relative to  A , we
will describe the orientation of the body. So, in summary, the position of points are described
with vectors, and orientations of bodies are described with an attached coordinate system.

Figure 2.3: Locating an object in position and orientation.

21

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

There are two ways to describe the body-attached coordinates relative to the reference
coordinate. First method is by rotating the reference coordinate till it is aligned with the body
coordinate and define the angles. But this method is not very popular for three dimensional
spaces. The more popular method is to write the unit vectors of B three principal axes in terms
of the coordinate  A .

Let’s start with a simple case of two dimensional plane, and two coordinate frames of Figure 2.4.
The coordinate vectors for the axes of frame 1 with respect to coordinate frame 0

0
1 R   10 x 0
1 y  (2.3)

This is called a rotation matrix. Let’s explore why.

Figure 2.4: Coordinate frame B is oriented at an angle with respect to A

As illustrated in Figure 2.4, and considering that x and y are unit vectors, it is easy to compute
the entries of the rotation matrix

22

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

 cos     sin  
0
x  , 1y  
0

sin   cos  
1

(2.4)
cos   sin  
1R  
0

sin  cos  

Hence the name rotation matrix. But it is not necessary to the drive the entries for matrix 10 R in
terms of angle  . An alternative approach, and one that scales nicely to the three-dimensional
case, is to build the rotation matrix by projecting the axes of frame 1 onto the coordinate axes
of frame 0 . Remember that dot product of two unit vectors gives the projection of one onto the
other, we obtain

 x1.x0   y1.x0 
0
1 x  , 1y  
0

 x1. y0   y1. y0 
(2.5)
 x1.x0 y1.x0 
1R  
0

 x1. y0 y1. y0 

Note that this method of defining the rotation matrix by projection gives the same results as
Equation (2.4). The projection technique described above scales nicely to the three-dimensional

case. In three dimensions, each axis of the frame B (in Figure 2.3) is projected onto


coordinate frame  A . Let’s say the unit vectors of coordinate frame B are X̂ B , YˆB and Ẑ B .
Each vector has three elements with respect to the reference coordinate  A . If we stack the
three unit vectors together, they form a 3  3 rotation matrix
     
 X B .X A  YB . X A   Z B .X A 
          
A
X B   X B .YˆA  , A
YB  YB .YA  , A
ZB   Z B .YA 
        
 X B .Z A  YB .Z A   Z B .Z A 
      (2.6)
 X B .X A YB . X A  Z B .X A 
         
A
B R   A X B A
YB A
Z B    X B .YˆA YB .YA  Z B .YA 
       
 X B .Z A YB .Z A  Z B .Z A 

Note that we have omitted the leading superscripts in the rightmost matrix of Equation (2.6).
Since the dot product of two unit vectors is the cosine of the angle between them, the
components of rotation matrix is often referred to as direction cosines.

23

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Now let’s consider the case, where we want to describe the orientation of the frame  A with

 
respect to the frame B (that is, if we desired to use the frame B as the reference frame). In
this case, and following the same procedure as before, we will end up with
     
B
X A   X A . X B X A .YˆB X A .Z B  ,
      
B
YA  YA . X B YA .YB YA .Z B  ,
      
B
Z A   Z A . X B Z A .YB Z A .Z B 
  (2.7)
    
 B X A   X A . X B YA . X B  Z A.X B 
B     ˆ      
A R   YA    X A .YB
B
YA .YB  Z A .YB 
B           
 Z A   X A .Z B YA .Z B  Z A .Z B 

Since dot products is commutative, (that is, xi . y j  y j .xi ), we see that

B
A R  BA RT (2.8)


Or the description of frame  A relative to frame B , is given by the transpose of Equation
(2.6). Note that coordinate axes are mutually orthogonal, and therefore it can readily be seen and
easily be verified that
B
A RT  AB R 1  BA R (2.9)

And that
A
B RT AB R  I 3 (2.10)

Let’s do some examples to clear things a bit.

Example 2.1: Suppose the frame 1 is rotated through an angle  about the z0 axis. Find the
resulting transformation matrix 10 R .

24

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

2.3 Description of a Frame

Now that we have seen how to represent both position and orientation, we combine these two
concepts to specify the whereabouts of the manipulator hand, similar to Figure 2.3. Note that a
rigid motion is a pure translation together with a pure rotation. The first step to define the
location of the hand is to specify a reference coordinate, which we call frame  A . Then we
choose a point on the body, its position and orientation we want to describe, which we call frame
B. In order to find the location and orientation of frame B with respect to  A , we need a
distance between origin of B with respect to origin of  A , and a rotation of B with respect
to  A . So as you can see, we need four vectors, one for position, and three for orientation. This


entity of four vectors is called a frame. So in figure 2.3, frame B is described as A PBORG

which is the vector that locates the origin of the frame  B ,

B
and rotation of frame B with
respect to frame  A which is described by rotation matrix BA R .

2.4 Mappings: changing Descriptions from One Frame to Another

Consider a point P in space, similar to the point P shown in Figure 2.1 If you remember from
previous notes at the beginning of the Chapter 2, depending on what coordinate we use, the
vector describing point P is different, though its location remains constant. This is called
mapping. We now consider the mathematics of mapping by considering three cases.

Case 1: Mapping Involving Translation Only

In Figure 2.5, we see a position defined by vector B P . Now we wish to express this point by a

vector of frame  A . In this case, note that frames  A and B only differ by translation. Since
both frames have the same orientation (i.e., their coordinates are parallel), we can calculate the
description of point P relative to frame  A , vector A P , by vector addition

A
P  B P  A PBORG (2.12)

Note that since the coordinates are in the same orientation, the rotation matrix to describe frame
B in  A , is the identity matrix I.

25

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Figure 2.5: Translational mapping.

Case 2: Mapping Involving Rotation Only

Let A P and B P be position vectors of a point P in frames  A and  B respectively. (See Figure
2.6). In this case, the components of vector A P and B P are

 APˆ   B Pˆ 
 XA   XB 
A
P   A PYˆ  , B
P   B PYˆ  (2.13)
 A   B 
 A Pˆ   B Pˆ 
 ZA   ZB 

Figure 2.6: Coordinate frame attached to a rigid body.
26

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Let’s say we know the position of point P in frame  B , B P , and now want to find the position
in frame  A , A P . We also assume that the rotation matrix of  B relative to  A is known as
A
B R . In order to calculate the components of A P , note that any vector’s components are simply
the dot product of the vector and the unit axes of the coordinate (i.e., the projection of vector into
coordinates). That is
A
PXˆ  A Xˆ A . A P  B Xˆ AT . B P
A

A
PYˆ  A YˆA . A P  BYˆAT . B P (2.14)
A

A
PZˆ  A Zˆ A . A P  B Zˆ AT . B P
A

Or in terms of rotation matrix, we note that Equation (2.14) can be written as

 B Xˆ AT . B P   B Xˆ AT 
   
A
P   BYˆAT . B P    BYˆAT  B P
B ˆ T B  B ˆ T 
 Z A . P   Z A 
T
(2.15)
A
P   Xˆ A BYˆA
B B
Zˆ A  B
P
A
P  AB RT B P
A
P  BA R B P

Note that with the notation adopted in this course, it is easy to keep track of mapping, by
imagining that the leading subscript cancels the leading superscript of the following entity.

Example 2.2: A frame  B is located initially coincident with a frame  A . We rotate  B about
Zˆ B by  degrees, and then we rotate the resulting frame about Xˆ B by  degrees. Give the
rotation matrix that will change the descriptions of vectors from B P to A P .

Case 3: General Mapping

We now consider the general case, where the origin of the frame  B is not coincident with the
origin of frame  A and also rotated with respect to it. Given B P , we wish to compute A P . As
you can guess, this can be done in two separate steps, first describe the position vector B P in a
27

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

frame that has its origin with frame  B , but has the same orientation as  A . This can be
expressed as

A
P  BA R B P (2.16)

Then, we take the translation between the origins of frame  B and  A :

A
P  BA R B P  A PBORG (2.17)

As you noticed, this is not just a matrix operator and involves a summation as well. In order to
turn the two terms in Equation (2.17) into one matrix operator, we change the 3  3 matrix into a
4  4 matrix operator and change 3 1 position vectors into a 4  1 vector. Then Equation (2.17)
can be written as

 AP A  B P
   BT  
1  1 
(2.18)
 BA R A
PBORG 
A
B
T   
 000 1 

The 4  4 matrix ABT is called a homogeneous transform.

Example 2.3: Find the transformation matrix H that represents a rotation by angle  about the
current x-axis followed by a translation of b units along the current x-axis, followed by a
translation of d units along the current z-axis, followed by a rotation by angle  about the
current z-axis.

2.5 Operators
So far, we learned about homogeneous transform, in order to map the same point in different
frames. Now we want to change one point in one frame, into another point in the same frame, be
it translation, rotation, or both. Even though the math seems similar to the homogeneous
transform, the interpretation as the operator is different.

28

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Translational operators

Suppose we want to move a point P or its position vector A P1 along a certain direction by certain
distance. This move is characterized by vector AQ (as shown in Figure 2.7). The mathematics
involved is exactly the same as mapping a point to a second frame. Instead of moving the point
forward by the vector AQ , imagine that we keep the point constant, and now use a mapping to
move the coordinate (or the frame) backward. The mathematics in two cases is identical.

Figure 2.7: Translation operator

A
The resulting vector P2 can be calculated by

A
P2  A P1  AQ (2.19)

or as homogeneous (matrix operator)

 A P2   I 3 X 3 A
Q   A P1 
    (2.20)
 1  000 1  1 

Note that this time, since the frame is only one, we don’t need to use superscript or subscripts for
the matrix operators.
29

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Rotational Operators

To rotate a point, P1 or its position vector, A P1 in the same frame  A , about the axis k by an
angle  , the math is the same as if keep the point constant and rotate the frame  A in the
opposite direction by the angle  .
A
P2  Rk ( ) A P1 (2.21)

If the rotation axis is one of the principal axis of  A , then

1 0 0 
Rx ( )  0 cos -sin 
0 sin cos 
 cos  0 sin 
Ry ( )   0 1 0  (2.22)
  sin  0 cos 
cos  -sin 0
Rz ( )  sin  cos 0 
0 0 1 

 k x k x  c k y k x  k z s k z k x  k y s 

 
RK ( )   k x k y  k z s k y k y  c k z k y  k x s  (2.23)
 k k   k s k y k z  k x s k z k z  c 
 x z y

where   1  c .

Example 2.5: Rotate vector v  2 j by an angle 30 degrees about the vector axis
T
 2 2 
k , , 0 .
 2 2 
30

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Summary of Interpretations

A homogeneous transformation is characterized by a 4X4 matrix of the form

 R (3 X 3) Q(3 X 1) 
T  
000 1 

Homogeneous transformation has three interpretations:

1. It is a description of a frame {B} in relation to a frame {A}. In this case,  R  BA R  and  Q  A PBORG .

2. It is a transformation mapping, which transforms a vector in frame {B} into the same vector in
frame {A}.

3. It is an operator which transforms a vector in the same frame to another vector.

Transformation Arithmetic

1. Compound Transformations (Multiplications)

Consider the situation where point P in frame {C} is specified as C P and we want to
create A P but we have no immediate knowledge about how frames {A} and {C} are
related to each other. However, we know how frame {C} is related to frame {B} and how
frame {B} is related to frame {B} (i.e., we know the homogeneous transformation CBT and
A
BT ). The

 B P  B  C P  AP A  B P
   CT   ,    BT  
1  1  1  1 
(2.24)
 AP  A B  C P 
   BT CT    CT  BT CT
A A B

1  1 

Note that structure of CAT is of a homogeneous transform.

2. Inverting Transformation

We should that since coordinates are mutually orthogonal, a rotation matrix inverse can
be calculated from its transpose (Equation (2.9))
31

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

B
A R 1  AB RT

Note that only rotation matrix is unitary and can follow the above relation. If a matrix is
not a simple rotation matrix, it may not be unitary. Also note that the preceding result is
true only for a simple 3X3 rotation matrix. But how do we calculate the inverse of a
homogeneous transformation? This can be done by dividing the matrix into two portions:
the rotation portion of the matrix can simply be transposed, as it is still unitary. The
position portion of the homogeneous matrix is the negative of the rotation matrix by the
position vector

1  BA RT  BA RT A PBORG 
A
BT   (2.25)
 000 1 

And since BAT  ABT 1 , we can write

 A RT  BA RT A PBORG 
B
A T  B  (2.26)
 000 1 

Example 2.6: Calculate the matrix representing Rot ( x, 40 ) 1 .

As you have noticed, we need a rotation matrix of 3  3 to show the orientation of a point or a
frame in another frame. The next step would be to see if we can simplify the rotation matrix and
reduce the number of elements of the rotation matrix (which currently are 9). First let’s review
the characteristics of the rotation matrix to investigate if these characteristics may be useful in
reducing the number of elements.

Rotational matrices are special since all columns are mutually orthogonal and have the unit
magnitude. Furthermore, the determinant of rotation matrices is always equal to +1. And last,
rotation matrices are called proper orthonormal matrices, where proper refer to the fact that the
determinant is +1 instead of -1. Based on the above characteristics, there are several ways to
describe an orientation with fewer than nine elements. They are Cayley’s Parameters, Fixed
Angles, and Euler Angles. We will discuss them here.

32

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Cayley’s Parameters

Cayley’s formula for orthonormal matrices, is a linear algebraic process that shows that for any
proper orthonormal matrix R, there exists a skew-symmetric matrix S such that

R  ( I 3 X 3  S ) 1 ( I 3 X 3  S ) (2.27)

Equation (2.27) is called Cayley’s formula and matrix S is in the form of

0 sx  sy 
 
S    sx 0 sz  (2.28)
sy  sz 0 

And sx , s y , sz are known as Cayley’s parameters.

Fixed Angles

Even though through Cayley’s parameters, we reduced the number of elements of a rotation
matrix, it is not still an easy job for the operator of the robot to input the data to orient the robot
hand in the desired orientation. One way of reducing the number of elements required to define
the orientation, is by describing the rotations about the principal axes of a frame  A which is the
principal coordinate frame. These rotations are called roll, pitch and yaw, which we shall denote
as  ,  ,  , respectively as seen in Figure 2.8.

Figure 2.8: Roll, Pitch and Yaw Angles.

33

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

c -s 0  c 0 s   1 0 0 

 0  0 0  0  s 
B RXYZ ( ,  ,  )   s c
A
ca 1 (2.29)
0 0 1    s  0 c  0 s c 

Where c represents cos and s represents sin .Note in order to build the rotation matrix, only
three angles should be defined. However, the order of the multiplication is very important.

The inverse problem, that is extracting fixed angles (pitch, roll and yaw) from a rotation matrix is
often is of interest as well. That is

 r11 r12 r13 

 r23    ,  , 
B RXYZ ( ,  ,  )   r21
A
r22 (2.30)
 r31 r32 r33 

The solution depends on solving nine equations to get three unknowns. But upon further
investigation, it is obvious that six of the equations are dependents on the other three, so we have
a three-equations three-unknown problems and

  Atan2(r31 , r112  r212 ),

  Atan2(r21 / c , r11 / c ), (2.31)
  Atan2(r32 / c , r33 / c ),

Where Atan2( y, x) is a two-argument arc tangent function.

34

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Example 2.7: Suppose R is generated by a roll of 90 degrees, followed by a pitch of 30 degrees,

and lastly by a yaw of 60 degrees.

1. Calculate R.

2. Find the equivalent vector K and rotation angle along K which gives the same rotation
matrix.

Euler Angles

A common method of specifying a rotation matrix in terms of three independent quantities is to

use the so-called Euler Angles. Consider the fixed rotating frame  A and the rotated frame
B shown in Figure 2.9. We can specify the orientation of the frame B relative to the frame
 A by three angles  , , , known as the Euler angles, and obtained by three successive
rotations as follows. First rotate about the z-axis by the angle  . Next rotate about the current y-
axis by the angle  . Finally rotate about the current z-axis by the angle  .

Figure 2.9: Euler Angles Representation.

In terms of the basic rotation matrices the resulting rotational transformation can be generated as
the product

35

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

 c -s 0  c 0 s   c -s 0

   
B RZYZ ( ,  , )   s
A
c 0  0 1 0   s c 0
0 1   s 0 c   0 1
 0 0
(2.32)
 c c c  s s -c c s -s c c s 
 
B RZYZ ( ,  , )   s c c  c s
A
-s c s +c c s s 
 -s c s s c 
  

The matrix RZYZ is called ZYZ-Euler Angle Transformation.

The more important and more difficult problem is to determine for a particular rotation matrix,
set of Euler angles  , ,  .

 r11 r12 r13 

A
B RZYZ   r21 r22 r23    ,  , (2.33)
 r31 r32 r33 

To find a solution for this problem, we break it down into two cases. First, suppose that not both
r13 and r23 are zero. Then from Equation (2.32) we deduce that s  0 , and hence not both r31 ,
r32 are zero. In this case, r33  1 and we have

  Atan2(r33 , 1  r332 ),
or (2.34)
  Atan2(r33 ,  1  r332 ),

Based on what value of  is chosen we will have

  Atan2(r13 , r23 ),
  Atan2(r31 , r32 ),
or (2.35)
  Atan2(r13 , r23 ),
  Atan2(r31 , r32 ),

   Atan2(r11 , r21 )  Atan2(r11 , r12 ) (2.36)

36

ENGR 4280U: Robotics and Automation    Payam Rahimi, PhD, PEng

Which means that there are many solutions to this case. We take   0 as convention.

   Atan2(r11 , r21 ) (2.37)

References:
 Introduction to Robotics, Mechanics and Control, 3rd Edition, By John J. Craig, Pearson
Prentice Hall.

 Industrial Robotics, Technologies, Programming, and Application, By M. P. Groover, M.

Weiss, R. Nagel, N. Odrey, McGraw-Hill.

37