o EXAMPLE 3.7 Equations with MatricesThe form of an equation relating physical variables yields information about

the mathematical model being used. Many models lead to scalar equations.For example, Ohm's law, E = I R (introduced in Example 3.6) relates onlythe magnitude of current and voltage. There is no indication of the spatialproperties of the resistor, which in reality may be a 3D cylinder of carbon orother material.

Many of the electrical and mechanical properties of materials are definedby relations between vector forces and flows or displacements. Newton's secondlaw written in the form a = F [in. indicates that the acceleration of an objectwith constant mass is in a direction parallel to the applied force. In effect,the resistor in Ohm's law and the object in Newton's law are treated as pointobjects. Further, a = F [rn. can be treated as a shorthand notation for th\equations

r; Fy r,ax = -, ay = -, az = -.m m m

Thus, the acceleration in the x direction is a function only of Fx and the constantm, and so is independent of the forces in the other directions.

Rewriting Ohm's law in a slightly more sophisticated form results in theequation

J =O"E,

where J is the current density in amperes per square meter in two dimensions oramperes per cubic meter in three dimensions. The quantity 0" is conductivity.This is the reciprocal of resistance measured in mhos. E is the electric fieldapplied to the body with conductivity 0". In one dimension, J becomes thecurrent I, 0" = 1IR, and the field E is replaced by the voltage E.

The equation J = O"E is a vector equation and implies that the conductivityof the material is isotropic if it is a constant, no matter what the direction ofthe applied field. This is true for materials constructed from cubic crystals inwhich the x, y, and z directions are equivalent. In matrix form, this relationshipcould be written

In many materials, the resultant current is not in the same direction asthe applied field. This fact leads to many interesting electrical and opticalproperties of materials. In fact, crystals of noncubic structure may be expectedto have anisotropic properties. The more general relationship between currentdensity and electric field can be written as

i. O"llEx + 0"12Ey + 0"13Ez,

Jy O"21Ex + 0"22Ey + O"23Ez,

J, 0"31Ex + O"32Ey + 0"33Ez.

These equations show that an electric field in the x direction, for example,creates a current in all three directions, assuming theO"il (i = 1,2,3) are notzero. Thus, a matrix can be used to define the relationship between an appliedelectric field and the directions and magnitude of the resulting current in a

3.3 Square and Symmetric Matrices 113

»At=A'At =

148

1oo

1-3-7

»det (At)ans=

4»quit

o

ORTHOGONAL AND TRIANGULAR MATRICES

Orthogonal and triangular matrices have special properties that are par-ticularly useful in applications. These types of matrices are introducedin this section and used in other sections of this chapter and later in thebook.

ORTHOGONALMATRICES

An orthogonal matrix Q is an ti x n matrix that is invertible with theproperty that its transpose is also its inverse, so that

(3.9)

This relationship is equivalent to QT Q = QQT = I, where I is the identitymatrix. An important example in two dimensions is the matrix

Qr = [ c~sesin f

- sin e ]cose .

This matrix is associated with rotations of vectors in a plane as describedlater in this chapter. Clearly, QrQrT = I for this matrix since

Q Q T = [ coser r . esm- sin e ] [ cos e

cos e - sin e sin e] [1 0]cos e 0 1 .

The following theorem states the relationship between orthogonal ma-trices and sets of orthonormal vectors. The theorem also shows how toconstruct orthogonal matrices.

~----------------------------------------------------------ogonal and T·nangular Matrices 119

The relationship between vector spaces and linearity for the pointsin R 2 is that if a set of points represented by vectors xj , X2, ... ,Xn alllie on one line through the origin, any linear combination of them lies inthe same line since they are all multiples of the same vector. Also, thezero vector is in the set. We say that a subset S of a vector space V is alinear subspace if every linear combination of elements ofS is also in S.Usually, the linear subspace is simply called a subspace, as was the casein our discussions in Chapter 2.

TRANSFORMATIONS IN THE PLANE AND THREE-DIMENSIONAL SPA~EIn computer graphics, a typical problem is to display a view of a 2D or 3Dobject on a video screen. Using matrix algebra, new views of the objectcan be generated by rotation, translation, and scaling. Describing themotion (kinematics) of a robot manipulator is an important problem inrobotics. Matrices can be used to define the position and orientation ofthe manipulator at any time with respect to the coordinates of the robot'sreference frame.

In general, matrices can be used to allow points and vectors to berotated about coordinate axes, translated in space, and referenced relativeto other reference frames. As shown in a later chapter, we may alsouse matrices to map the coordinates of spherical or cylindrical referenceframes into xyz-coordinates (Cartesian space), or vice versa.

Rotations in the Plane In Figure 3.2, a vector X = [x,yJ is rotatedthrough the angle e to become the vector x' = [x', y'J.

y

(x', y') = [r cos (8 + ex), r sin(8 + ex)],\ ,,

• (x, y) = [r cos ex, r sin ex]

o~--~----L---------------------------------~~x

FIGURE 3.2 Rotation of a two-dimensional vector

The length r of the vector is not changed by rotation. From thegeometry, the coordinates of x in terms of the angles involved are

x = r cos o and y = r sin o.

142 Chapter 3 • MATRICES

It is'desired to obtain the coordinates of the rotated vector in terms of x, y,and the angle 8., Thus, the rotated vector x' in Figure 3.2 has coordinates

Xl Tcos(8 + ex) = r cos e cos o - r sin e sin o,yl T sin (8 + ex) = T sin 8 cos ex + r cos 8sin ex.

Thus, the relationship between the endpoint coordinates is

x' = X cos 8 - y sin 8,yl = x sin 8 + y cos 8.

Defining the 2D rotation matrix as

s, = [cos8sin8

- sin 8 ]cos8 ' (3.28)

we see that

by multiplying xT by Re. If the angle 8 is changed to -8, the sign ofthe off-diagonal terms are changed. Of course, the rotation matrix shouldbecome the identity matrix if 8 = 0°.

Notice that in these operations, the vectors are considered 2 x 1 col-umn vectors. Also, the series of operations ReI Re2xT results in a rotationby angle 82 + 81. If the order of matrix multiplications is reversed, therotation angle remains the same. This is always the case when a series ofrotations of a vector from the origin are made in the plane.

Three-dimensional Rotations Figure 3.3 illustrates the 3D referenceframe we will use for rotations.

z

~e

r------ ~(J:-.------~.~y<jl

x

FIGURE 3.3 Cartesian 'reference frame in three dimensions

:-----a.8linear I -----------~----------------------

ransformations 143

In three dimensions, the axis of rotation must be specified. The sub-scripts for a rotation matrix will indicate the axis of rotation and theangle. The rotation matrices for the three axes are as follows:

l. Rotation by an angle a about the z-axis:

Rx,Q = [ ~

0 0 1cos o -sinasm o cos o

2. Rotation by an angle ¢ about the y-axis:

[ cos¢ 0 sin¢ 1Ry,¢ = 0 1 0-sin¢ 0 cos¢

3. Rotation by an angle e about the z-axis:

[ cose - sine 0 1Rz,() = Si~e cose 00 1

(3.29)

(3.30)

(3.31)

When performing a series of 3D rotations about several axes by matrixmultiplications, the order in which the rotations are performed is impor-tant, since the rotation matrices do not commute in general. Except forspecial cases, applying the rotation matrices to a vector in different orderwill generate a different result. As a check on the results, the rotationmatrices should become identity matrices when the angles involved areset to zero. Also, the rotation matrices are orthogonal matrices, so thecolumns (or rows) must be orthonormal.

Homogeneous Transformations (Optional) The 3x 3 rotation ma-trix does not provide for translation or scaling of a vector. The conceptof homogeneous coordinate representation is introduced to develop matrixtransformations that include rotation, translation, scaling, and perspec-tive transformation. Using homogeneous transformations, the transfor-mation of an n-dimensional vector is performed in an (n + 1)-dimensionalspace.

In homogeneous transformations, the true vectors in R 3 are writtenas vectors in R4 with a scaling factor as the last component. Let the 3Dcolumn vector be defined as

144 Chapter 3 • MATRICES

80 ROBOT ATTITUDE

~t!(Lft'41CtrDt·Robot Frame

YvEarth Frame

x,Figure 3.1 Mobile Robot with Earth and Robot Coordinate Frames

As the robot moves about, it experiences translation or change inposition. In addition to this, it also may experience rotation or changein attitude. The various rotations of the robot are now defined. Yaw isrotation about the Z axis in the counter-clockwise direction as viewedlooking into the Z axis. Pitch is rotation about the new (after the yawmotion) X axis, in the counter-clockwise direction as viewed lookinginto the X axis, i.e., front end up is positive pitch. Roll is rotation aboutthe new (after both yaw and pitch) Y axis in the counter-clockwisedirection as viewed looking into the Y axis, i.e., left side of vehicle upis positive. In the system used by those in the aerospace field pitch iscounter-clockwise rotation about the Y axis while roll is counter-clockwise rotation about the X axis, i.e., the roles of the X and Y axesare reversed with respect to these two rotations.

3.2 ROTATION MATRIX FOR YAW

The rotation matrices for basic rotations are now derived. For yaw wehave the diagram shown in Figure 3.2. Axes 1 represent the robot coor-dinate frame before rotation and axes 2 represent the robot coordinate

~frame after positive yaw rotation by the amount tfI. The z axes comeout of the paper. It bears repeating that counter-clockwise rotationabout the z axis is taken as positive yaw.

We wish to express in the original coordinate frame 1 the locationof a point whose coordinates are given in the new frame 2. For x andy we have

Xl = X2 cos tfI- Y2 sin tfI

Yl :::;X2 sin VI + Yz cas VI

-

and

or

Agn

mge III

changeYaw isviewedhe yawooking1aboutickwisetide uppitch isounter-Yaxes

yaw weot coor-irdinate.s comerotation

locationir x and

ROTATIO MATRIX FOR YAW 81

R.o~o..~t~a.M-efo 1Y\1- .s t=1)(' eJ \.

Figure 3.2 Frame 2 Yawed with Respect to Frame 1

and for z

or

rXl ' rc~sIf/y = SIll If/

Z 1 °-sin If/

cos If/

°Thus the rotation matrix for yaw is

-SIll If/ oOllcos If/

°(3.1)

EXAMPLE 1

A vector expressed in the rotated coordinate system with If/ of n 12 isgiven by

Express this vector in the original coordinate system.

ames

. change inlor change[led. Yaw is1 as viewedter the yaw'ed lookingation aboutr-clockwise: vehicle upeld pitch isis counter-and Y axes

For yaw werobot coor-: coordinate: axes comeise rotation

the locationz. For x and

ROTATION MATRIX FOR YAW 81

Figure 3.2 Frame 2 Yawed with Respect to Frame 1

and for z

or

-sm ljf

cos ljf

°Thus the rotation matrix for yaw is

-sm ljf~ 01cos ljf

°EXAMPLE 1

(3.1)

A vector expressed in the rotated coordinate system with ljf of 1C12 isgiven by _

.>GJ

82 ROBOT ATTITUDE

. y (

.1..L'/l<-lSOLUTION 1

.The expression of this vector in the original coordinate system becomes

fX1 fcosnl2 -sinnl2 01f11 fOl~J= Sin;!2 COS;!2 ~J~r~J

Note that the Euclidean norm of each column of the rotation matrixis one and that each column is orthogonal to each of the others. This isthe definition of an orthonormal matrix. A convenient property of suchmatrices is that the inverse is simply the transpose, i.e.,

(3.2)

or

This property can be proved by pre-multiplying an orthonormalmatrix by its transpose and then using the properties which it possesses,i.e.,

<coli,colj) = 1, l = ]=0 i r ]

3.3 ROTATION MATRIX FOR PITCH

For pitch we have the situation depicted in Figure 3.3. The x axes comeout of the paper. Note again that front end up corresponds to positivepitch

Again we wish to express in the original coordinate frame the loca-l-

tion of a point whose coordinates have been given in the new frame.For x and z we have

Yl = Yz cos 0 - Zz sin 0

Zl = Yz sinO + Zz cosO

and for x

-

or

T

EX

Avpitc

1

SO

Th

92 ROBOT ATTITUDE

where the entries in the upper part of the last column are defined by

(3.13)

In all of these, use has been made of the fact that for a rotationmatrix

since the rotation matrix is orthonormal.This homogeneous transformation provides a concise means of

expressing a vector in an original frame when the second frame is bothrotated and translated with respect to the original frame. Its conve-nience becomes even more pronounced when there is a series of trans-formations, e.g., sensor frame to vehicle frame and then vehicle frameto earth frame. .

3.7 ROTATING A VECTOR

Another important application of the rotation matrix is to express thenew coordinates of a vector after the vector itself has been yawed,pitched and rolled. Here the same coordinate frame is used before andafter the rotation. To illustrate, consider an initial vector expressed inframe 1.This vector is now rotated about the z axis. The expression forthis rotated vector, again in frame 1, is given by the following

[X] [COS lJIY = SIll lJI

Z after rotation 0

or

[X] [X]Y = Ryaw(lJI) Y

Z after rotation Z before rotation

" (3.14)

This s:rotated fthe z axi

or

Torion theandthvector

Wegeneo

EXEl

1. Evljf=

2. Evljf=

3. E\seeljf:

fn

fined by

(3.13)

. rotation

neans ofle is bothts conve-; of trans-de frame

.press the

.n yawed,efore andiressed inession forg

(3.14)

EXERCISES 93

This same process holds for each of the rotations. Thus if a vector isrotated first about the y axis, then about the x axis and finally aboutthe z axis, the new vector in the original frame is given by

lcos ljI cos ¢ - sin ljI sin 8 sin ¢sin ljI cos ¢ + cos ljI sin 8 sin ¢

-cos8sin¢

- sin ljI cos 8cos ljIcos8

sin8

cos ljI sin ¢ + sin ljI sin 8 cos ¢]sin ljI sin ¢ - cos ljI sin 8 cos ¢

cos8cos¢

or

lX] lX]y =R(ljI,8,¢) y

z after rotation Z before rotation

(3.15)

To reiterate, in this second application of rotation matrices, the vectoron the right-hand side of the equation is the vector before its rotationand the result on the left-hand side is the vector after its rotation. Bothvectors are expressed in the same frame.

We shall find important uses for these rotation matrices and homo-geneous transformation matrices in the chapters that follow.

EXERCISES

1. Evaluate the rotation matrix for the case whereljI=nI2,8=-nI2,and ¢=O.

2. Evaluate the rotation matrix for the case whereljI = 0, 8 = n 12, and ¢ = n 12.

3. Evaluate the homogeneous transformation for the case where thesecond frame has orientation with respect to the first frame ofljI = tt 12,8 = =tt 12, and ¢ = 0 and location with respect to the firstframe of x = 3, y = 2, Z = 1.

100 ROBOT NAVIGATION

X IV = - R cos long sin long., cos lat + R sin long cos long., cos lat

YIV = -R coslongcoslongc cos latsin lat.,- Rsuitongssnlongs coslatsinlato + Rsinlatcoslato

ZIV = R cos(longo -long)coslatcoslato + Rsuilat., sinlat - R

which reduce to

XIV = Rcoslatsin(long-longo) (4.6a)

YIV = -R coslatsinlato cos(long -Longo) + R sin lat coslat., (4.6b)

and

ZIV ~ R cos lat cos lau, + R sin lau, sin lat - R = R cos(lat -lato) - R (4.6c)

For points on the surface of the earth in the vicinity of the origin ofthe final frame these equations for x and y may be approximated quiteaccurately as

XIV ""Rcos(lat)[long-longo]

YIV ""R[lat -lato]

(4.7a)

(4.7b)

and

(4.7c)

EXAMPLE 3

A local coordinate system is set up at Long = 70 deg W = -70 deg andLat = 38deg N A mobile robot is at Long = 69.998deg W = -69.998degand Lat = 38.001 deg N Find the X, Y coordinates for the robot. TakeX-East and Y-North.

SOLUTION 3

Xloeal= R( Long - Longo) cos( Lato)( tt 1180)= 6,378, 137(.002)(.788)(n 1180) = 175.4m

Ylocal= R(Lat - Lato)(n 1180) = 6,378,137(.001)(n 1180) = 111.3m

Figun

If we 1

X axis ofIV i.e., E

The a

Note tFor all apose) istheir exjrather thces were

Applyexpressi:

and

EXAMI

Re-do thclockwis

oslat

a

--R

(4.6a)

to (4.6b)

-R (4.6c)

e origin ofated quite

(4.7a)

(4.7b)

(4.7c)

70deg and69.998degobot. Take

ll1.3m

ASSOCIATED COORDINATE SYSTEMS 101

XlV~------~---------+Figure 4.2 Local Coordinate System with X Axis Rotated Relative to East

If we now go to a final local coordinate system rotated such that thex axis of frame V is at an angle a with respect to the x axis of frameIV i.e., East, then we have the frame shown in Figure 4.2.

The appropriate rotation matrix is given by

[X] [ cosa sin a O][X]Y = -sina cosa 0 YZ v 0 0 1 Z IV

(4.8)

Note that this transformation matrix is given by Ryaw (atI or Ryaw (af.For all of these transformations in the preceding, the inverse (or trans-pose) is required because the coordinates are being converted fromtheir expression in the old frame to their expression in the new fi~rather than vice versa as was the case considered as the rotation matri-ces were derivttd. . of ~ (s t "*' q'l

Applying the above transformation to equation (4.7) yields as theexpression for the coordinates in frame V

Xv = R cos a cos lata[long -longo] +R sin a[lat -lata] (4.9a)

and

Yv = - R sin a cos lata[long -longo] +R cos a[lat -lata] (4.9b)

EXAMPLE 4

Re-do the previous example but with X- Y axes now rotated by 30 degreesclockwise.