ROBOT DYNAMICSAND CONTROL

MARK W. SPONG----

University of IlliJwis al lJrhalla-CllOnlpaign

M. VIDYASAGARUniversity nf W'llerloo

JOHN WILEY & SONS

c.'fHh;ht ~; \YKY, by John Wile)' . Sons, Inc:\1: fl':'''\f~ rc~~n'c\1. ruhli~hed ~ilnulullcously In Canada.

RcrmdllC'tl0n or ,r,Ulslil1;on 01 ,lOy Jl.,n ot:h:~ \\"urk hc)'ond ,hat permitted hy s.cctlons

:O~ ~'lU [Olluf the 1976 Unltcd !'Utcs Copynght~~I ".chout 11'\0: permissiun (lIth ... copyriRht(",~.:r I~ unlllwfu1. Rt."\jl1eSIS fur ptrmi~sion('I I~"h.::r inlQrmal,on should be addresH'J wth. J'cn~lls~mn... tk[>.Illmt'llt.John Wil ...y &. Suns,

LibnJT.l" of CunJ(TC!i.\ ellllllfiging in Publicdtlofl DOla:

~J'on,; :-1..!1.. W.ROl-...l ,h-'l,lmtcs

PREFACE

The present age has been given many l

viii

En~lish dictionary} as the "automatic connol of manufacturing productthrough successive stages; usc of automatic equipment to san' menuland manual lahor:?." To the lay person, however, automation meansthe process of replacing human workt:rs by machines. Thus it is oftentrue that the word automation evokes fear and mistnlst. Being lireplaced by a machine" is no longer an idle threat to a significant portionof modem society. Yet historically, every advancement in technologythat has It:d to an increase in productivity has been good in the longrun. in the nineteenth eentury most people in the United States madetheir livings un farms. Farm automation took away most of those jobsso that at the presem time less than two percent of the population ofthe u.s. is engaged in farming. Yet there are more johs now than everbefore due ro advances in technulogy and changing lifestyles. The samewill Hkely be true of automation in the long nln, \Vhilc automation el-iminates some iubs in weldi.ng, machining, assembly line work, etc., italso creates otht:r ions, such a~ rohm installation and maimenam:I:,softw:lre development and so forth. At the same time, the increasedprudllCtivity hrought anoUl by autumation enables a society to competesuccessfully in a global economy with the re~;ult tbat the general standanl of livin,l; is raised.

Increasmgly, automation has come to mean the introduction of wbots into the manufacturing environment. At the present time therobot rcpre!'ents the highest form of automation. Although technicallyjust machines, robots arc viewed by most people in a much differentlight than other machines. This special, almost romantic, position oc-cupied by robots is due partly to tbe Hollywood vkw of a rohot as a"mechanical being," often with evil intent, or as a lovable companionsuch as R2D2 of tht: motion pictute Star Wars. The word robot itsdfwas introduced into our vocabulary by the Czech pbywright KarelCapek in his 1910 play RO$$um's Universal Rohots; the word robotobeing thc Czech word for work.

Even Without the Hollywood imagery, however, humans would probably h.lVe a special attraction to robots compared to other machinesfor the

ox

biJlion-dollar-a-ycar industry by 1990. MOSt of these predictions havefailed to materialize for a variety of reasons. MallY applications, suchas assembly, have proved to he extremely difficult to automate. Theprest:nt level of roboti.cs technology, in such areas as machine vision,tactile sensing, artificial intelligence, elC" while spectacular, is stillprimitive compared to the marvcJous adaptahility and dexterity of hu-mans. One gains a sense of awe and respect for human eyes, hands, andbmins when attempting tu program a rohot to pcrfmm even simpleu.sks. There arc also economic reasons why robots have not lived up totheir expectations to date. A successful application of robotics requiresmuch morc than simply installing a robot on the factory floor. Mostoften it requires a rethinking and redesign uf the entire process that isto be automated. The new Acid of applications en~incer is beginning [Qaddress tbe issues of how hest to automate manufacturing proceSSlSwith robots.

Another very important reason for the slow growth of the robotic.:sindustry deals with the interdisciplinary nature of robotics itself. Tbefield of robotics combines aspects of electrical, mechanical, and indus-trial engineering with compulcI science, mathematics, and economics.There is at present a critical sbonage of trained people with the cross-disciplinary knowledge m:i.:essary to integrate sueccsshdly the varioustechnologies involved in robotic applications. It is the task of theuniversities to provide such cross-disciplinary education.

The present text grew out of a set of lecture notes developed by theArst author during 1983-84 in the School of Electrical Engineering atCornell University. These notes have heen extcnsively revised throughsubsequent courses taught in the Department Of General EngilleeIin~and tht: Deparm1t~nt of Electrical and Comruter Engi.neering at theUnivcrsity uf illinois at Urbana-Champaign, and in the Department ofElectrical Engineering:lt the Universlty of Waterloo, Ontario, Canada.

The book h.1S been written primarily for electrical engineering stu-dents who may have little or no exposure to the subjects uf kinematicsand dyn

x PREFACE

This textbook can be used for courses at two levels. Chapters Onethrough Nine, can be used in a first course in robotics at the seniorlevel with only a first course in feedback control systems as aprerequisite. It is very helpful in such a course to have access to a la-boratory where the students can learn a specific programming languageand can test some of the material on real systems. The instructor mayalso wish to supplement the material with outside reading on computerinterfacing, sensors, and vision. At the graduate level, the entire bookcan be covered in one semester. The latter chapters will be most usefulto students who have studied at least the state space theory of dynam-ical systems and who have access to computational facilities to designand simulate the advanced control algorithms presented. After cove-ring the chapters on advanced control a graduate student should beprepared to tackle the research literature on robot dynamics and con-trol.

The text is organized as follows. After an introductory chapterdefining the basic terminology of robotics and outlining the text,Chapter Two gives some background on rotations and homogeneoustransformations sufficient to follow the subsequent development.Chapter Three discusses the forward kinematics problem using theDenavit-Hartenberg convention for assigning coordinate frames to thelinks of a manipulator. Chapter Four discusses the problem of inversekinematics from a geometric viewpoint that is sufficient to handle themost common robot configurations the student is likely to encounter.Chapter Five discusses velocity relationships and derives the manipu-lator Jacobian in the so-called cross product form and includes a discus-sion of singularities. Chapter Six discusses dynamics.

We have found that many students at the senior level have little ex-posure to Lagrangian dynamics, so we have made Chapter Six self-contained by including a derivation of the Euler-Lagrange equationsfrom the principle of virtual work. We also discuss the recursiveNe\vton-Euler formulation of manipulator dynamics.

Chapter Seven begins our discussion of robot control by treating thesimplest approach, namely, independent joint control. We also discuss,in this chapter, the most basic approaches for trajectory interpolation.A robot is much more than just a series of rigid mechanical linkages.Thus \'ie include material on actuator dynamics and drive-traindynamics and show how both significantly impact the manipulatorcontrol problem. We also introduce the idea of feedforward control andcomputed tOrque, which paves the way to the more advanced nonlinearcontrol theory to follow.

Chapter Eight discusses robot control in the context of multivariablesystems. Here we discuss the method of inverse dynamics and intro-duce the reader to the idea of robust control. Chapter Nine discussesforce control, chiefly hybrid position/force control and impedance con-trol, which are the tWO most common approaches to force control todate. Chapter Ten introduces the idea of feedback linearization of

PRF.F,\CF: xi

nonlinear sySt~ms. This is 3. recent and quite advanced concept in con-trol theory. We have attempted to give a self-contained introduction tothe subject while minimizing tnt: mathematical background required ofthe reader. Thus, although wr:. introduce, for example, the notion of Licbrackets of vector fields, we do so primarily as a nutational con-venience. All calculations are performed in local coot

XII PI-l~:F,\(:~:

The initial discussions which cvcnw'llly led to this book began du-ring the summer of 19R4 while both authors were in the Comrul Tech-nolog)' Branl;h at General Elt:ctrk's Corporate Research am.I Develop-ment Headquarters in Schent'(;wdy, New York. We would like tothank John Cassidy a.nd Larry Sweet for thdr support during thatperiod, which made this collaboration possible. We would also like tothank Rruce Kro~b and Kcn Loparo for their critical revlcws of themanuscript, and Lila Acosta for proofreading the manuscript. Wewould also like to thank Riccardo Marino, Romel) Onega, HeberttSira-Ramirez, and Jean-Jacques Slmim;. for numerous helpful discus-sions on the robot control problem.

finally, wc would like to thank Christina Mediate, Joe Ford, and Mi-chael Tung at John Wiley jor their patience and gllldance in the prepara-tion of this text.

MARK W. SPO~GM. VIDYASAGAR

CONTENTS

CHAPTER OI\E, II\THOUl.CTIO.~1.1 Jlllroduction1.2 Ro/lOtiCS1.3 Cotlll'oncllt$ and Structure of !~olJor::.1.4 l,OmmOlI Kinematic Arrangements1.5 Outline of ,he Text

References and Su~r:.esledUCddingProblems

CHAPTER Two, RI!:1D MOTlOI\SAN 0 HOMO!: E:,/EOUSTRANSFORMATIOI\S2.1 RouJtiwl~'2.2 Composition of Rotatious1.:$ Furrher Propetlies of R{)fdUom1.1, Homogeneous TU1J)s!ormat;omi1.5 Skew Symmetric Marrke..'i1.6 Angular Velocity and Acce1erarion

14H

21272CJ

333843465053

1

32

XIII

xiv

2. '; /lddition oj Angular VelucitiesReference'! and Sllxxe~redReadingProblems

CIiU'TF:R TIiHEE, FOIl.WMWKII'(EMi\T1CS, TIiE OENAVIT-H ARTF.I'(RF.H(; REPIl.ESE'

CONT~:NTS xv

CHAPTER SEVEN: INDEPENDENTJOINT CONTROL 1677.1 Introduction 1677.2 Actuator Dynamics 1697.3 Set-Point Tracking 1757.4 Drive Train Dynamics 1837.5 Trajectory Interpolation 1957.6 Feedforward Control and Computed Torque ~O6

References and Suggested Reading 211Problems 212

CHAPTER EIGHT: MULTTVARIABLECONTROL 2168.1 Introduction 2168.2 PD Control Revisited 2168.3 Inverse Dynamics 2218.4 Implementation and Robustness Issues 2248.5 Robust Outer Loop Design 227

References and Suggested Reading 2:36Problems 239

CHAPTER NINE: FORCE CONTROL 2419.1 Introduction ~Il9.2 Natural and Artificial Constraints 2-1-;)9.3 Stiffness and Compliance 2469.4 Inverse Dynamics in Tosl, Space 2519.5 Impedance Control 2529.6 Hybrid Position/Force Control 254

References and Suggested Reading 256Problems 257

CHAPTER TEN: FEEDBACKLINEARIZATION 259]0.1 Introduction 259] 0.2 Background: The Frobenius Theorem 26110.3 Single-Input Systems 26610.4 Feedback Linearization for N-Linl< Robots 27410.5 Introduction to Outer Loop Design 27710.6 Outer Loop Design by Lyapunov's

Second Method 279

.x \0'1

Reference... lind SUj.lgesred ReadingProblems

CHAPTER ELJ:;VEN, VARfAllLESTRCCTUIlE AND AflAPTlVF:CONTtlOL11.1 lnlmductionlJ.2 The Merhod of Sliding Modes11.3 Adaptive Control

References (Iud SllgXesred ReadingProblems

Apr'E'iDIX -I.: LL\EAR ALGF:llRA

ApPENDIX R: ST.\TE SPAO: THEOtlYOF D' 'I HflCAI. SYSTEMSApPE'lTHX C: LYAPU OV STAHILITY

INDEX

:28128)

2"'4-2X,,"3013133].1

CU'< ,-.-'VI'S

2U-1

:H6

321

CHAPTER ONE

INTRODUCTION

1.1 INTRODUCTIONRobotics is a relatively new field of modern technology that crossestraditional engineering boundaries. Understanding the complexity ofrobots and their applications requires knowledge of electrical enginee-ring, mechanical engineering, industrial engineering, computer science,economics, and mathematics. New disciplines of engineering, such asmanufacturing engineering, applications engineering, and knowledgeengineering, are beginning to emerge to deal with the complexity of thefield of robotics and the larger area of factory automation. Within afew years it is possible that robotics engineering will stand on its ownas a distinct engineering discipline.

In this text we explore the kinematics, dynamics, and control of ro-botic manipulators. In doing so we omit many other areas such as lo-comotion, machine vision, artificial intelligence, computer architec-tures, programming languages, computer-aided design, sensing, gras-ping, and manipulation, which collectively make up the disciplineknown as robotics. While the subject areas that we omit are importantto the science of robotics, a firm understanding of the kinematics,dynamics, and control of manipulators is basic to the understandingand application of these other areas, so that a first course in roboticsshould begin with these three subjects.

1

21.2 ROBOTICS

1:\TIt'jUl'!..'l' III r>o

The term robot was first introduced into our vocabulary by the Czechplaywright Kard Capek in his 1920 play Rossum's Universal Rohots,the word robotll being the Czech word for work. Since then the termhas been applied to a gre.u variety of mechanical devices, such as teleo-pelators, underwater vehides, ;11,Ltonomous land rovers, etc. Virtuallyanything that operates with some degree of autonomy, usually undert:omputcT control, has at some point been called a robot. In thiS textthe leon robot will mean.:l computer controlled industrial manipulatorof the type shown in Figure 11. This type of robot is esst::ntially amechanical arm operating under computer control. Such devices,thous;h far from the rohots of science fiction, alc nevertheless ex-

FIGUHE 1-1The Cincinnati MiJacron 7" indu~trial manipulatOr. Photo courtesy ofCincinnati Milacron.

tremely compl~x electro-mcdumical systems whose analytical descrip-tion requires advanced methods, and which present many challengingand interesting research problems.

An official definition of such a robot comes from the Robot Instituteof America (RIA): A robot i~ a reprogramnwhle multifunctional manipulator designed to move material, parrs, Joo]s, or ~pecialized devicesthrough variable programmed morion.~ for the performance of a varielyof tasks.

The key element in the above definition is the rt:programmability ofrobot.S_ It is the wmputer brain that gives tht' robot its utility andadaptahility. The so-called robotics revolution is, in fact, pan of thelarger computer revolution.

Even tbis restricted vCtsion of a robot has several fcatures that makeit attractive in an industrial environment. Among thc advantages oftencited in favor of the introduction of robots are decreased labor costS, in-creased precision and productivity, increased flexibility comparcd withspecialized machines, and more humane working wnditions as dull,n:petitive, or hazardous jobs arc performed by robuts.

The robot, as WI:: have defined it, was horn Out of the marria~e of twoI::arlier technologies: that of teleopcrators and numerically contmlledmilling machines. Telcoperators, or master-slave devic~s, weredeveloped during the second world war to handle radioactive materials.Computer numerical comrollCNCl was devdoped because of the highprecision required in the machining of certain items, such as com-ponents of high performance aircraft. The 6rst robot::. essentially combincd the mech:miC3.1 linkages of thc releoperator with the autonomyand programmability of CNC machines. Se\'craJ mHesrones on tberoad to present day rubor technolog}' are listed bdow[17],[21]:

1947-thc first servocd electric powered teleoperator lSdeveloped

1948- a tdcoperator is developed incorporating force feedback1~}49-research on Ilunlt:rically contrulled milling machines is

initiatedI954--Gcurge Devol designs the first programmable robot19S6-Joseph E.ngelbcrgcr, a Columbia University physics stu-

dent, huys the rights to Devol's rooot and founds theUnimtltion Compau}'

1961-thc first Unimate robot is installed in a Trenton, Nt:wJersey plant of General Motors (10 tend tl die castingmachinel

1961-the 6rst rohot inco:rpoftlting force feedhack informationis developed

19(...~-the first wbot vi~ion system is developed1971-the Stanford Arm is developed at Stanford University

4 1.YII\Olll.CI"I( )~

1973-the first robot programming language iWAVE) isdevelopc:d at Stanforu

1974--Cincinnati Milacron introduct:s the: T-1 robot with com-puteT control

1975-Unimation Inc. registcrs its first fin

COMt'O~J::""N ANO STRl-'(:Tl'ltJ::QI' RVUOl'8 5

between two links. We use the convention (R I for representing revo-lute joints and {PI for prismatic joints ~s shown in Figure 1-2. Eachjoint represents the interconnection between two links, say, lj and lj+l'We denote the axis of rotation of a revolute joint, or the axis alongwhich a prismatic joint slides, by z, if the jomt is the interconnectionof links i and i +1. The joint variables, denoted by 9," for a revolutejoint and eli for a prismatic joint, represem the relative displacementbetween adjacent links. We will make thiS precise in Chapter Three.

The joints of a manipulator may be electrically, hydraulically, orpneumatically actuated. The number of iOintS determines the degrees-of-freedom (OOF) of the manipulator. Typically, a mampuLator shouldpossess at least six independent DOF: threc for positioning and threefor orientation. With fewer than six DOF the arm Cannot reach everypoint in its work envirnnment with arbitrary orientation. Certain ap-plications such as reaching around OT behind obstacles requiTe morethan six DOF. The difficulty of contrulling a manipulator inCTeases ra-pidly with the number of links. A manipulator having more than sixlinks is teferred to as a kinematically redundant manipulator.. The workspace of a manipulator is the total volume swept out by thecndeffectoT as the manipul3tor executes all possible motions. Theworkspace is constrained by the geometry of the manipul.nor as well asmechanical constraints on the joints. For example, a revolute jointmay be limited to less than a full .160 of mution. The workspace is

" ", ,

I I, I I,Revolute I, I, Vioinl (R) --'---

~ '-----'",,- ,

Prismaticjoin! (P)

I,

I, '

I,

I.

FIGLRE 1-2Symbolic representation of robot joints

6 1"lrll').II.C-"ffl~

often broken down into a reachable workspace and a dextrouswork!:lpace. The reach:lbl~ workspace is the entire set of pOLmsreachable b)' the m.ampulalor, whereas the dextrous wurkspacc con5istsof those points that the manipul

COMI'O."IJ::NT,,""ln STIIUCTURF.\W HOllo",,,, 7

typically no direct measurement of the end-effector position and orien-tation. One must rely on the as!>umed geometry of the manipulatorand its rigidity to infer (i.e_, to calculate) the end-effector position fromthe nu:asured joint angles. Accuracy i!> affected therefore by computa-tional errors, machining accuracy in the construction uf the manipu-lator, flexibility effects such as the bending of the links under gravita-tional and other loads, gear backlash, and a host of other static anddynamic effects. It is primarily for this reason that robots an~ designedwith extremely high rigidity. Without high rigidity, accuracy can onlybe improved by some sort of direct sensing oj the end-effector position,such as with visioll.

Once a point is taught to the manipulator, however, say with a teachpendant, the above. effects are taken into account and the proper en-coder values necessary to return to the given point are stored hy thecontrolling computer. Repeatability therefore is affected primarily bythe controller resolution. Controller resolution means the smal1est in-crement of motion that the controllcr can sense. The resolution is com-puted as the total distance traveled by the tip divided by 2'l, where n isthl.: numher of bits of encou.cr :lccura,,:y. In this context linear axes, (hatis, prismatic joint!>, typically have higher resolution th:Hl revolLltejoints, since the straight Line distance traversed by the tip of a linearaxis between twO points is less than the corresponding arclength tracedby the tip of a rut.1(ion~\llink.

In addition, as we wll! see in later chapters, rotational axes llsu:llIyresult in a large amount of kinematic and dynamic coupling among the!.inks with a resultant accumulation of errors and a morl' difficult control problem. One may wonder then what the adVant,lge~ of revolutejoints are in m.anipulator design. The answer lies primarily in the in-creased dexterity and compactn~ss of revolute joint design~. For example, Figure 1-4 shows that for the same range uf mOl inn, a rotationallink can he made much smaller th3n a link with linear motiun. Thusmanipulators made fmm revolute joints occupy a smaller working vo-lunle than manipulators with linear axes. This increases th~ ability ofthe manipulator to work in the same space with other robots,machines, and p~op1c. At the same time revolute joillt manipulatorsare better a!'lle to maneuver around ohstaclcs and have il wider range ofpossible applications.

n-,------,-----iL}---j-------'~I , ,/ ,, FIGURE 1-4Linear vs. rotational link motion

Il\,TIIOlIllCTIO!"

1.4 COMMON KINEMATICARRANGEMENTSAlthough in principle a manipul

F1GURE 1-5The Unimation PUMA lProgrammabl!: Universal Manipulator furAssembly) Phow courtesy of WestinKhouse Automation Oivision/Unimation Incorporated.

spherical coordinates defining the position of the cnd-effector withrespect to a frame whose origin lies at the intersection of the axes Zland Zz are the same as tbe first thrce iuint variablc:s. A common mani-pulatOr with this configuratiun is the Stanfurd manipulator IFigurc \-IU). The workspace of :l spherical manipulator is !lhown in Figun: 1-11.1.'1.:\ SCARA CO:'lFlGlIRATION (KKP)The so-called SCARA (for Se1t;:ctive Compham Articulated Rooot forAssembly) shown in Figure 1-]2 is a recent and increa$l.ingly popularconfiguration, which, as its name suggestsl is tailored for assemblyoperations. Although the SCARA has an RRP structur~, it is quit'c different from the spherical configuration in both appearance and in itsrange 01 applic:nions. Unlike the Isphericall Stanford design, which hasZn,Z I,Z2 mutually perpendicular, the SCARA has ZO,Z I,Z2 pamllel. Fi-

gun~ 1-13 shuws the AdeptOne, a manipulator of this type. The SCARAmanipulator workspace is shown in Figure 1-14.

1.4.4 Cv LI NDRTCAL CONFIGliRATION (RPP)The cylindrical configuration is shown in Figur~ 1-15. The first joint i&rtvo!ute and produces a rotation about the base, while the second andthird ioints are prismatic. As the name suggests, the joint variables are

10 II\TltlJlHtl'TIOI\"

FIGURE 1-6The' Cincinnati Milacrcm T 3 735 robot. COUrtt:sy of Cincinn

I 1

Zo+I 82 z, 83 z2I (Y../" ~"

Shoulder~Z {2 &z 13/~upper ./~Forearm

,.. 81 arm

Body I,

Base

FIGURE 1-7StructL1r~of the e1how manipulator

g;l.ntry robots, fnr transf~r of Iuatcrial or cargo. An example of a car-tesian robot, the Cincinnati Milacron TJ gantry robot, is shown in Fi

gun~ 1-19. The workspace of a carttsian manipulawr is shown in Figure:1-20.

Top

(

FIGlJRF: 1-8Work.~paccof th~ elhow manipulator

12 TI\;TRfll'lIJCTlul'o

FIGURE 1-9The sph~rical manipulatorconfiguration

"i,

i.... .'~

FIGURE 1-10The Stanford manipulator {Courtesy of the Coordinated Science Labora-tory, University of Illinois)

FIGUHE 1-11Workspace;:. of dIe spherical manipuLator

,,"

" IIIII

'3

j

14 r....TIlUIIl "f:TIU:-'

FIGURE 1-13The AdeptOne robot. Photocourtesy of Adept Tcchnolo-gies.

FIGuRE ]-14Tbe workspace of the SCARA manipulator

15

II1'

FIGURE 1-15Tb~ l:yliudrical manipulatorconnKura tion

1.4.6 OTlIEH METt-tODS OF CLASSIFYINGROBOTSOther common ways to classify robots are by their power source, appli-cation are;], amI method of control.(i) Power SourceTypically, robotl; art: eirner electrically, hydraulically, or pneumaticallypowered. Hydraulic "ICtuatOrS are unrivaled in theH speed of responseand torque producing capability. Therdore hydraulic rohors are usedprimarily for lifting heavy loads. The drawbacks of hydraulic !Ohms arethat they tend to leak hydc

)(,

... ~1lll

.,..

FIGl:RE 1-16The GMF M-lOO robot. photo c.:ouncsy of CMf Rohotics.

I :>ITflOPUl.:TI'J'\

(iii) Method of ColJtro}Robots are classified by coO(col metbud into servo and non-servo mhots. The earliest robots w~rc non-servu robots. These rohots arcesst:ntially openloup devices whose movement is limih::d to predeter-mined mechanical SlOpS, and arc useful primarily for materi;lls transfer.In fact, a(;cordin~ to thl.: definition Kiven previously, fixed stOp robotshardly qualify as robots. Servo robots usc closeJ-loop computer controlto determine their moriun and arc thus capable of being truly multi-functional, reprogramm3ble devices.

Servo contrulled robots arc further classified according 10 themethod that the controller uses to guide thl.: cnd-effector. The simplesttype of moot in this c1as" is the point-til-point mho.. A pointto-pointrobot can be l

17

FIGURE 1-17Workspace of tht: cylindrical manipulator

usually taught a series of points with a teach pendant. The points arethen stored and played bade Pujot-to-point rohots are severely limitedin their range of applications. In continuous path robots, on the otherhand, tht:: entire path of the eDl]cIfcc-tor can be controlled. For ex-ample, the robot end-effector C:lIl be taught to follow a straigbt linebetween two points or even to follow a contour such as a weldingscam. In addition, thl' velocity and(or acceleration of the end-effectorcan often be cODlrolled. These are the most

18 1."l'1'1I0UL.CJ'lO.\j

FIGURE 1-19A Gantry robot, the Cincinnati Milacron T 3 886. Photo courtesy ofCincinnati Milacron.

1.4.7 WBISTSAND END-EFFECTORSThe wrist of a manipulator refers to the joints in the kinematic chainbetween the ann and hand. The wrist jointS are nearly always all revo-lute. It is increasingly common to design mani.pulators with sphericalwrisL"'t by which we mean wri,;ts whose joint axes intersect at acommon point. The Cincinnati Milacron r of Figure 1-1 and the Stan-ford manipulator of Figure l-lO both helve three degree-oi-freedomspherical wrists. The spherical wrist is represented symbolically in fj-,lime 1-21. The spherical wrist greatly simplifies the kinematicanalysis, effenively alJowing one to decouplt:: the positioning and orientation of an objeL:t to as great an extent as possible. T}'pically therdore,the manipulator will possess three positional degrees-af-freedom,which are produced by three or more ioints in tht:: am1. The Humber oforientational de~rees-offreedomwill then depend on the degrees-of-freedom of the wrist. It is common to find wrists having one, two, orthree degrees-of-freedom depending of the application. For example,the AdcptOnc IFigurc 1-13! ha~ fmlr degrees-af-freedom. The wrist hasonly a roll about the final zaxi~. The Cincinnati Milacml1 T" ns has

19

o

FIGURE 1-20Workspace of the cartesian manipulator.

five dcgrccs~o(.freed()m. The wrist has pitch and roll hut no yaw mo-tion. The PUMA has a full three degreesof-frccdom spherical wristand hence the manipulator possesses six degrees-nf-freedom.

It has been said tbat a robot is (Jnly as good as liS h~nd or end.effector. The arm and wriSt assemblies of a robot ;lTC used primarily forpm,itiuning the end.effector and any tool it may carry. It is the end-effectur or tnnl that actually performs the work. Thi.: simplest typc ofcml-effectnrs aTC grippers, such as shown m Figures 122 and 1-23,which usually arc capabk uf only tWO actIOns, opening :md closing.

----QP~ GRoll

FIGURE 1-21Structure of a spherical wrist.

20 J 'IT'll '1.'l'Cr!I,,,

FICVHF. 1-22A parallel jaw gripper. (From: A Robot En.~ineeringTextbook, by Mohsen Shahinpnor. Copyright 19R7,Harper & Row Publish~rs, Inc.)

While tJlis is adequate for materials tmosfer, some parts h.:mdling, urgripping simple tools, it is not tldequ

OL'I'Ll.'n: OF TilE TJ::XT

1.5 OUTLIN E OF TH E TEXT

21

A typical application involvlllg an industrial manipul3tor is shown inFi);ure 1-24. The manipulator, which pm;scss\:~ six dcgrecs~of-reedom,is shown with a );Timling tOol! which It muSt use to remove a cen:1inamount of ml'tal from a s'llrfar.:t:. 1n the rn:~~1lt text we are concernedwith the following question: Wh

22 INTR.HHiCT10:-;

Hom,

" f .

FIGlJUE 1-25Two-link planar rohot eX3.mpl~.

c

Typically, the manipulator will be able to I'ense its own position illsome manner using internal sensors (po!>ition encooers) located

OIJTT.TI\F OF THE TEXT 2:1

y,

FIGLRE 1-26Coordinate tLlmes tor two-linkplanar robot.

fi1ioh.io] fcoslll+l2! -Sin ill+l1Ijli1-jo jx.io -l5iniOI+fl21 c05(8 1 +l2 ) (1.5.4)

where io,.io are the standard orthonormal unit vectors in the base frame,and i2, h are the standard orthonormal unit vectors in the tool frame.

These equations (1.5.1-1.5.4) are called the forward kinematic equa-tions. For a six degree-nf-freedom robot these equations are quite com-plex and cannot be written down as easily as for the two-link manipu-lator. The general procedure that we discuss in Chapter Three esta-blishes coordinate frames at each joint and allows one to transform sys-tematically among these trames using matrix transformations. Theprocedure that we liSC is referrcd to as the Denavit-Hartenber,g conven-tion. We thcn usc homo,geneous coordinates and homogeneoustransformations to simplify the transformation among coordinateframes.

1.5.2 PHOHLK\1 2, TNVEHSI: KI.... EMATICSNow, given the joint angles Ih,82 we can determine the end-effectorcoordinates x and y. In order to command the rohot to move to loca-tion B we need the inverse; that is, we need the joint variables 81,82 interms of the x and y coordinates of B. This is the problem of InverseKinematics. In other words, given x and .r in the forward kinematicequatlOns 1.5.1-1.5.2, we wish to solve for the joint angles. Since theforward kinematic equations are nonlinear, a solution may not be easyto find nor is there a unique solution in general. We can sec, for ex~ample, in the case of a two-link planar mechanism that there may beno solution, if the given Ix ,y 1coordinates arc out of reach of the mani-pulator. If the given Ix,Y) coordinates arc within the manipulatorsreach there may be two solutions as shown in Figure 1-27, the so-calledelbow up and elbow down configurations, or there may be exactly onesolution if the manipulator must he fully extended to reach the point.There may even hc an infinite number of solutions in some cases iPro-hi em 1251.

24 I .... nil 'Ul"'TU)~

FTGUllE 1-27Multiple inverse kinematic solu-tions.

Consider the diagram of Figure 128. Using the Law of Cosines wesee that the angle 92 is given by

1 1 J 1.X+Y-(JI-Q,co,a,= 2 ,D 11.5.51

ala2We cnuld now dc,tcrmine 8! as

8) = cosI(D J 11.5.61However, a belteT way to find 92 is tu notice that if cos(~J il) given by

ILS.51 then sinl9!1 is given as

'inia,) = "I=iJi 11.5.71and, hence, 82 can be found by

, -'iI_O'-

lh = tan- l D

Tbe advantage of this latter approach is that both the elbow-up andelbowdown solutions are recovered by choosing the positive ami nega-tive Si,itllS in j 1.5.8), r~spectivcly.

It is left as an exercise (prohlcm 1-19) to show that 91 is now ~ven asa2sin 928j = tan-l(y Ix)- laD I( J i 1.5.9)

a1+I/lcos8lNotice that the angle 6, depends on 92 Tbis makes sel1st: physically

since we would cxpt't:t to require a different value for at dt'pt:mlin~ 011which solution is chu~t'n for 9z.1.5.3 PrrOHI.EM 3, VEI.OCITY KTI"EMATICSIn order to follow a contOur at constant velocity, or at any prescrihedvelocity, we must know the relationsbip h~tw~en the velocity of thetool and the joint velocities. in this case we t:an differentiate Equations1.5.1 and J.5.2 to obtain

ILi.lOI

OUTLINE OF TilE TEXT

y -------------

IIIII

21 _-A--III1

x

25

FIGCRE 1-28Solving for the joint angles of atwo-link planar arm.

. . .y = a ICOS ele l + a2COS (e l + e2)(e l + e2l

Using the vector notation x = [~J and 9 = [~~] we may write theseequations as

x_ [-a lsine l - a2sin (e l +e2) -a2sin (e l +e2)] 6- a lCOSe] + a2cos(9t +e2) a2COS(e\ +e2) (1.5.11)

=]8The matrix] defined by (1.5.11) is called the Jacobian of the manipu-

lator and is a fundamental object to determine for any manipulator. InChapter Five we present a systematic procedure for deriving the Jaco-bian for any manipulator in the so-called cross-product form.

The determination of the joint velocities from the end-effector velo-cities is conceptually simple since the velocity relationship is linear.Thus the joint velocities are found from the end-effector velocities viathe inverse Jacobian

(1.5.12)or

I

(1.5.13)

The determinant det] of the Jacobian in (1.5.11) is ata2sin82' TheJacobian does not have an inverse, therefore, when e2 = 0 or 7'[, in whichcase the manipulator is said to be in a singular configuration, such asshown in Figure 1-29 for e2 = O. The determination of such singularconfigurations is important for several reasons. At singular

2fi

Yo

FIGURE 1-29;Co A singular configuration.

configurations there are infinitesimal motions that ar~ unacruevilhlcithat is, the manipulator end-effector cannot move in certain directions.In the above cases the cnd effector cannot move in the directionparallel to () I from a singular configuration. Singular coongurations arcalso related to the nOll-uniqueness of solutions of the inversekinematics. For example, for a Kiven end-effector position, there are ingeneral two possible solutions to the inverse kinematics, Note that thesingular configuration separates these t\VO solutiol1S in the sense thatthe manipuhnor cannot go from one configuration to the other withoutpassing through the singularity. For many applications it is importantto plan manipulatur motions in such a way that singular configurationsare avoided,

1.5.-i PHOHI.;M4, DYNAMICSA robot manipulator is basically a positioning device. To control tbeposition we must know the dynamic properties of the manipulator inorder ta know how much force to exert on it to cause it to moye. Toolittle farce and the manipulator is slow to reaCL Too much force andthe arm may crash into objects or oscillate ahout its desired position.

Deriving the dynamic equations of motion for robots is not a simpletask due to the large number of degrees of freedom and nonlinearitiespresent in the system. In Chapter Six we develop techniques based onLagrangian dynamics for systematically deriving the equations of mo-tion of such a system. In addition to the rigid links, the completedescription of robot dynamic,.:; includes the dynamics of the actuatorsthat produce the forces and torques to drive tbe cohoe, and thedynamics of the dnve trains that transmit the power from the actuatorsto the links. Thus, in Chapter Seven we also discuss actuator and drivetrain dynamics and tht:ir effects on the control problem.

27

1.5.5 PROHLEM 5, POSITION CONTROLControl theory is used in Chapters Seven and Eight w dt:sign control al-gorithms for the execution of programmed ta5ks. The motion controlproblem consists of the Tracking and Disturbance Rejection Problem,which is the problem of determining the control inputs necessary tofollow, or track, a desired trajectory that has been planned for the mani-pulator, while simultaneously [ejcctin~ disturbances due to unmodeleddynamic effects such as friction and. noise. We detail the standard ap-

pro~cbes to mhO[ control based on frequency domam techniques. Wealso introduce the nutiun of feedforward control and the techniques ofcomputed torque and inverse dynamics as a means tor compensatingthe complex nonlinear interaction forces among the links of the mani-pulator. Robust control is introduced in Chapter Eight using the Se-cond Method of Lyapunov. Chapters Ten and Eleven provide some ad-ditional advanced techniques from nonlinear control tbeory tbat arcuseful for controlling high performance robots.

1.5.6 PROBLEM IS, FOHC~; CONTROLOnce the manipulator has reached location B, it must follow the con-tour 8 maintaining a constant force normal to thc surface. Conceivahly, knowing th~ location of thc object and the shape of the con-tour, we could carry out this task using position control alon~. Thiswould be: quite difficult to accomplish in practice, howevl."'r. 5ine,(: themanipulator itself possess high rigidity, any errors in position d~le touncertainty in the exact location of the surface or tool would give riseto extremely large forces at the end-effector that could damage the rool,tbc surface, or the rohot. A better jpproach lS to measure the forces ofinteraction directly and ll~e a force control scheme to accomplish thetask. in Chapter Nine we discuss force control and compliance anddiscuss [he two mos[ common approaches to force control, hybrid control and impedance COI1IOI.

ARKlH, M., Computers and the CybernetIC Sociely, AC3dcmicPn:ss, New York, 1977.ASADA, H., and SLOTINE. J-J. E., nabat Analysis and Clmtwl,Wiley, New York, 19R6.BEN!, G., and HACKWOOD, S., eus., Recent Advance.. ill RoboLics,Wiley, New York, 1985.

REFERENCES AND SUGGESTEDREADING[11

[31121

2H ''''THOUI 1~r1"'l

[4] B((AIJY, M., Ct. aI., cds., Robot MotiorJ: Planning and Concrol,MIT Press, Camhridge, MA, 1983.

[5] CRAtG. J" Introduction to Robutks: Meehl/nics and Control,Addison-Wc3!cy, Reading.. MA, 1986.

16J CRITCHLOW, A.I., InlToduction to Robolics, Macmillan,New York, 19R5.

[7] DOIlF, R., Robotics. and Automated Manufaal1ClIlg, Reston, VA,1983.

[81 ENGLEBERGER, J., Robotics in Practice, KO~:lIl Pa~e, London,1980.

[91 FU, K.S., CO;..lZALEZ, R.C., and LEE, C.S.G., Robotics: ControlSen.sing, Vision, alld Intelligellce, McCraw-Hill, St Louis, 1987.

[10] GROOVER, M., ~t. aI., Industr1ll1 Nobotics: Technology, Pro-gramming. and Applications, McCraw-Hill, St. Louis, 1986.

[111 KOR.EN, Y., No/mUcs for 1I~il1cers, McGraw-Hili, St. Louis,1985.

[12] LEE, C.S.G., et. 31., eds., nWITialoIl Robotics, IEEE ComputerSodety Press, Silver Sprin].\, MU, 19ft~.[1~1 MCCu)y, lJ., and HAIlIlI~. M., nObOtiCS: An Introduction, Hal-

stead Press, New York, 1986.[14] MCCORnUCK, P., M(lchill~ Whu Think, W.H. Freeman,

San Frand!lw, 1979.[lSI MINSKY, M., ed., J

PROHlYM,~

PROBLEMS1-1 What are the key fcatures that distinguish robots from other

forms of "automation," such as CNC milling machines?12 Bridly JeAne eaeh of the following t~nns: forward kinematics,

inverse killcmatlcs, trajectory planning, worhpal.:c, accuracy,repeatability, resolution, ioint variable, sph..:rical wrist, end-effector.

1-3 What are the main wa}'~ to classify rohots~1-4 Make a list of robotics related magazines and iournals carried by

the university library.1-5 From the list of rderem;;,;s at the end of this chapter make .1 list

of 20 rooot application:.. For each application di.ocial consequences of sucha development:

1-12 Suppose a law were passeJ banning all future use of industrialrobots in the United States. What would be some of thec(,;onomic and social conseqUcllCC:' of such an act?

I-J3 Discuss possible applic:ltions where r~dundant manipulatorswould he useful.

~o I....TIlOllt (:TIO""

1-14 Referring to Figure \-30 suppuse thilt the tip of a single linktravels a distance d between two points. A linear axis wouldtravel the distance d while a rotational link would travelthrough an arclength f a as shown. Using the law of cosinesshow that the distimcc d is ~ivt:n byd =1 '2(1-co'(611which is of course Ie.~s than t. e. With LO-bit accuracy ,lndt =1In, a=90u what is the resolution of tht.: lint:ar linkl of therotatiullallink?

I-tS A single-link n:volute ann is shown in Figure 1-30. If the lengthof the link is 50 ern and the arm travels ISUn what is the: controlresolution obtained with an 8-hit encoded

-= 10

d

I

Fl

POULt.:....'S 31

1-2.3 Write a computer program to plot the joint an,ll;l~s as a functionof time given the tool locations and velocities as a function oftime in cartesian coordinateI'.

l-24 Suppose we Jesirt= tbat the tool follow a straight line betweenthe points (0,21 and 12,0) at COllstant speed s. Plot the time historyof joint angles.

125 For the two.hnk planar manipulatOr of Figure 125 is it possiblefor there to be an infinite numhcr of solutiuns to the inversekinematic equations! If so, explain how this clin occur.

CHAPTER TW"O

RIGID MOTIONS ANDHOMOGENEOUSTRANSFORMATIONS

A large part of robot kinematics is concerned with the establishment ofvarious coordinate systems to represent the positions and orientationsof rigid objects and with transformations among these coordinate sys-tems. Indeed, the geometry of three-dimensional space and of rigid mo-tfons plays a central role in all aspects of robotic manipulation. In thischapter we study the operations of rotation and translation and intro-duce the notion of homogeneous transformations. l Homogeneoustransformations combine the operations of rotation and translation intoa single matrix multiplication, and are used in Chapter Three to derivethe so-called forward kinematic equations of rigid manipulators. Wealso investigate the transformation of velocities and accelerationsamong coordinate systems. These latter quantities are used in subse-quent chapters to study the velocity kinematics in Chapter Five, inclu-ding the derivation of the manipulator Jacobian, and also to derive thedynamic equations of motion of rigid manipulators in Chapter Six.

I Since we make extensive lISC of clementary matrix theory, the reader may wish to rc-view Appendix A before beginning this chapter.

32

2.1 ROTATIONSFigure 2-1 shows a rigid object S to which :l coordinate frame ox If IZ Iis :ltt3ched. We wi!'h to n:!atc the coordinates of a point p on S i.n theOXI}'1Z] frame to the coordinates oj p in a fixed lur nonrotatedl re-ference frame OXoYoZn. Let! io! in, ko I denoll. the ~tandard orthonormalbasis in OX-oYoZOi tllLls io,jo,ko arc unit vectors along the Xo.}'n,Zu axes,respectively. Similarly, let l idll k] I be the standard orrhonormalhasis in OX1Y1ZI' Then the veClUf from the common oril(in to the pointp on the object can be represented either with n,:spcCt to ()XO.\'(lZo as

Po = Pox io + PU'fio + PO:.. ko 121.11

12.1.2)Since Po and PI arc representations of the same vector P, the nda-

tionship between the components of p in the tWO coordinate framescan bl' obtained as follows.

"

Po,< =Po"io =PI"io= {I h iri.o + Pl,.hia + P I' k l ,jl1

o

12.1.31

FJGUU; 2-1Cuurdin:.ltcs frallle .:lttacbcll 10.\ ri~id bully

34 RrGJD MOTIONS ,-\NO HOMOCENEOUS TI{A:-;SFORMATIONS

We have similar formulas for POy and POz, namely

POy = P lxido + P lyjdo + P lzkdoP Oz = P 'x i} .ko + P Izj I.ko + P Izk r ko

We may write the above three equations together as

Po = R6PIwhere

(2.104)(2.1.5)

(2.1.6)

(2.1.7)

The 3 x3 matrix represents the transformation matrix from the coordi-nates of P with respect to the frame ox IY 12 I to the coordinates withrespect to the frame OXoYoZo. Thus, if a given point is expressed inOXIYlzl-coordinates as PI then R6PI represents the same vector ex-pressed relative to the oXoYoZo-coordinate frame.

Similarly we can write

etc., or in matrix form

where

Pix =pr i}=Poi l= Poxio i , + Poyjo-i1 + Pozko'i l

(2.1.8)

(2.1.9)

(2.1.10)l~o'~1 ~O'~I kO'~I]R ~ = 10'JI JO'JI ko'JIio'k l jo' kl ko'k lThus the matrix R ~ represents the inverse of the transformation R6.Since the inner product is commutative, i.e., io'jo =jO'iol etc., we seethat

R~=(R6)-'=(R6)T (2.1.11)Such a matrix R 6 whose inverse is its transpose is said to be ortho-

gonal. The column vectors of R 6 are of unit length and mutually ortho-gonal (Problem 2-11. It can also be shown (Problem 2-2) thatdet R6= 1. If we restrict ourselves to right-handed coordinate sys-tems, as defined in Appendix A, then det R6 =+1 (Problem 2-3). Forsimplicity we refer to orthogonal matrices with determinant +1 as rota-tion matrices. It is customary to refer to the set of all 3 x 3 rotation ma-trices by the symbol SO (3).2

2The notation SO (3) stands for Special Orthogonal group of order 3,

iii Example 2.1.1Suppose the frame oXLhzJ is rota teo through

36 RICID MOTIONS AND HOMOGENEOUS THA:'IISFOR:\IATlOi'S

The transformation (2.1.13) is called a basic rotation matrix (aboutthe z -axis). In this case we find it useful to use the more descriptive no-tation Rz,e instead of R 6 to denote the matrix (2.1.13). It is easy to ve-rify that the basic rotation matrix Rz,e has the properties

which together imply

Rz,o =I

Rz,eRz,tn = Rz,e+'l>

(2.I.l4)(2.I.lS)

(2.1.16)

Similarly the basic rotation matrices representing rotations aboutthe x and Y axes are given as (Problem 2-5)

Rx,B= ~l aa cosSa sinS -s~ns]cosS (2.1.17)[

COSS a SinS]Ry,B = a 1 a

-sinS a cosEl(2.1.18)

which also satisfy properties analogous to (2.1.14)-(2.1.16).We may also interpret a given rotation matrix as specifying the

orientation of the coordinate frame ox lY lZ I relative to the frameoXoYoZo. In fact, the columns of R6 are the direction cosines of thecoordinate axes in OXIYIZ1 relative to the coordinate axes of OXoYoZo.For example, the first column (il'io,ido,il'ko)T of R6 specifies thedirection of the x I-axis relative to the OXoYoZo frame.

(ii) Example 2.1.2Consider the frames OXoYoZo and OXIYIZI shown in Figure 2-3. Projec-ting the unit vectors il,jl, k 1 onto io,jo, ko gives the coordinates ofidl' k l in the OXoYoZo frame. We see that the coordinates of i 1 are( ~ ,O,~)T, the coordinates of jl are ( .~ ,a, -~ )T and the coordinates of

~2 ~2 V2 V2k 1 are (a, I, O)T. The rotation matrix R6 specifying the orientation ofOXIYIZI relative to OXoYoZo has these as its column vectors, that is,

1 0Ii -,=-'2R6= a 0 1 (2.1.19)

1 -1 aIi ..)2

ROTATIONS

FIGURE 2-3Defining the relative orientation of two frames.

37

A third interpretation of a rotation matrix R E SO (3) is as anoperator acting on vectors in a fixed frame OXoYoZo. In other words, in-stead of relating the coordinates of a fixed vector with respect to twodifferent coordinate frames, the expression (2.1.10) can represent thecoordinates in OXoYoZo of a point PI which is obtained from a point Poby a given rotation.

(iii) Example 2.1.3The vector Po=(I,I,O)T is rotated about the Yo-axis by ~ as shown inFigure 2-4. The resulting vector P I is gi ven by

PI = R n PoY'l

[001][1] [0]o 1 1 = 1-1 0 0 0 -1

(2.1.20)

2.1.1 SUMMARYWe have seen that a rotation matrix R E 50(3) can be interpreted inthree distinct ways:

1. It represents a coordinate transformation relating the coordi-nates of a point p in two different frames.

38

//

//

//

//

/

RICII) MOTIONS AND HOMOGENEOUS THA'IISFOH'\1ATIONS

FIGURE 2-4Rotating a vector aboutan axis.

2. It gives the orientation of a transformed coordinate frame withrespect to a fixed coordinate frame.

3. It is an operator taking a vector P and rotating it to a new vector R P. in the same coordinate system.

The particular interpretation of a given rotation matrix R that is beingused must then be made clear by the context.

2.2 COMPOSITION OF ROTATIONSIn this section we discuss the composition of rotations. It is importantfor subsequent chapters that the reader understand the material in thissection thoroughly before moving on. Recall that the matrix R~ inequation (2.1.6) represents a rotational transformation between theframes OXoYoZo and OXIYlZJ' Suppose we now add a third coordinateframe OX2Y2Z2 related to the frames OXoYoZo and OXlYIZI by rotationaltransformations. A given point P can then be represented in threeways: Po, PI, and P2 in the three frames. The relationship betweenthese representations of P is

IPo =ROPl2Po = ROP2

PI = R ip2

(2.2.1)(2.2.2)(2.2.3)

where each R: is a rotation matrix. Note that R ~ and R ~ represent ro-tations relative to the OXoYoZo axes, while R ~ represents a rotation rela-tive to the ox IY lZ I frame. Substituting (2.2.3) into (2.2.1) yields

I 2Po = R oR tP2 (2.2.4)

COMPOSITION OF ROTATIONS 39

Comparing /2.2.2) and (2.2.4) we have the identity2 I R 2 (2 )R o = R O 1 .2.5

Equation 2.2.5 is the composition law for rotational transformations.It states that, in order to transform the coordinates of a point P from itsrepresentation P2 in the OX2Y2z2-frame to its representation Po in theoXoYoZo-frame, we may first transform to its coordinates PI in theox IY IZ I-frame using R i and then transform PI to Po using R ~ .

We may interpret Equation 2.2.4 as follows. Suppose initially thatall three of the coordinate frames coincide. We first rotate the frameOXIY\Zl relative to oXoYoZo according to the transformation R6. Then,with the frames OXIYIZt and OX2Y2Z2 coincident, we rotate OX1J'lZl re-lative to ox iY IZ I according to the transformation R i. In each case wecall the frame relative to which the rotation occurs the current frame.

(i) Example 2.2.1Henceforth, whenever convenient we use the shorthand notationCa = cosS , 5a = sinS for trigonometric functions. Suppose a rotation ma-trix R represents a rotation of degrees about the current Y-axis fol-lowed by a rotation of S degrees about the current z axis. Then the ma-trix R is given by

(2.2.6)

[

cq>ca -Cill5a 50]= Sa Ca 0

-Sq>Ce Sq>Se Cq>

It is important to remember that the order in which a sequence of ro-tations are carried out, and consequently the order in which the rota-tion matrices are multiplied together, is crucial. The reason is that ro-tation, unlike position, is not a vector quantity and is therefore notsubject to the laws of vector addition, and so rotational transformationsdo not commute in general.

Iii) Example 2.2.2Suppose that the above rotations are performed in the reverse order,that is, first a rotation about the current z-axis followed by a rotationabout the current y-axis. ~~NEMERIT UHN'EIIlllO'""'A'.'_W-"A7 ~J.IU.O.9Cl'."""

40 RIGID MOTIONS AND HOMOGENEOUS TRANSFORMATIONS

Then the resulting rotation matrix is given by

(2.2.7)

lce -Se 0] [Cll> 0S]Se Ce 0 0 1 0o 0 1 -s 0 clj)Comparing (2.2.6) and (2.2.7) we see that R ::F- R '.

Many times it is desired to perform a sequence of rotations, eachabout a given fixed coordinate frame, rather than about successivecurrent frames. For example we may wish to perform a rotation aboutthe Xo axis followed by a rotation about the Yo (and not YI!) axis. Wewill refer to oXoYOZo as the fixed frame. In this case the above composi-tion law is not valid. It turns out that the correct composition law inthis case is simply to multiply the successive rotation matrices in thereverse order from that given by (2.2.5). Note that the rotations them-selves are not performed in reverse order. Rather they are performedabout the fixed frame rather than about the current frame.

(iii) Example 2.2.3Suppose that a rotation matrix R represents a rotation of degreesabout the Yo-axis followed by a rotation of 8 about the fixed zo-axis.Refer to Figure 2-5. Let Po, PI, and P2 be representations of a vector p.

FIGURE 2-5Composition of rotations.

COMPOSITION OF ROTATIONS 41

Initially the fixed and current axes are the same, namely oXoYoZo, andtherefore we can write as before

Po =RY,4>PI (2.2.8)where Ry,lj> is the basic rotation matrix about the Y-axis. Now, sincethe second rotation is about the fixed frame oXoYoZo and not thecurrent frame ox IY \Z I, we cannot conclude that

PI = Rz ,eP2 (2.2.9)since this would require that we interpret Rz,a as being a rotation aboutZI. In order to use our previous composition law we need somehow tohave the fixed and current frames, in this case Zo and Z I, coincident.Therefore we need first to undo the previous rotation, then rotate aboutZo and finally reinstate the original transformation, that is,

PI = RY,-i!Rz ,eRy,lj>P2 (2.2.10)This is the correct expression, and not (2.2.9). Now, substituting(2,2.10) into (2.2.8) we obtain

Po = RY ,4>Pl (2.2.11)= Ry ,lj>Ry ,-lj>Rz ,aRy,lj>P2= Rz ,eRy,cbP2

It is not necessary to remember the above derivation, only to note bycomparing (2.2.11) with (2.2.6) that we obtain the same basic rotationmatrices in the reverse order.

We can summarize the rule of composition of rotational transforma-tions by the following recipe.

Given a fixed frame oXoYoZo, a current frame oX IY IZ I, together withrotation matrix R6 relating them, if a third frame OXIYIZ2 is obtainedby a rotation R ~ performed relative to the current frame then postmul-tiply R6by R ~ to obtain

2 I 2 ( )R o= R o R I 2.2.12

If the second rotation is to be performed relative to the fixed frame thenpremultiply R6by R ~ to obtain

2 2 1 ( )R o=R I Ro 2.2.13

In each case R ~ represents the transformation between the framesoXoYoZo and OX2Y2Z2. The frame OX2Y2Z2 that results in (2.2.12) will bedifferent from that resulting from (2.2.13).2.2.1 ROTATION ABOUT AN ARBITRARYAxISRotations are not always performed about the principal coordinateaxes. We are often interested in a rotation about an arbitrary axis inspace. Therefore let k = (kx , ky , kz )T, expressed in the frame OXoYoZo, be

42 RIGID MOTIONS ANU HOMOCENEOlJS TRANSFORMATIONS

a unit vector defining an axis. We wish to derive the rotation matrixR k,e representing a rotation of edegrees about this axis.

There are several ways in which the matrix Rk,e can be derived.Perhaps the simplest way is to rotate the vector k into one of the coor-dinate axes, say Zo, then rotate about Zo by e and finally rotate k backto its original position. Referring to Figure 2-6 we see that we can ro-tate k into Zo by first rotating about Zo by -a, then rotating about Yo by- p. Since all rotations are performed relative to the fixed frameoXoYoZo the matrix Rk,e is obtained as

From Figure 2-6, since k is a unit vector, we see that

kysin a = --==---,-

-Vk 2+1{2x Y

k xcos a = --====-

-Vk;+k;

sinp=-Vk;+k;

cosp = k z

FIGURE 2-6Rotation about an arbitrary axis.

(2.2.14)

(2.2.151

FI IlTllt;1\ .PltO".Id(Tlt~,'1OF RnT.... THJi'\S 4:1

12.2.16)k'k'V'+k,s01kr1

-14 RIGID MOTIONS AND HOMOGENEOUS TRANSFORMATIONS

Ii 8 = 0 then R is the identity matrix and the axis of rotation isundefined.(i) Example 2.3.1Suppose R is generated by a rotation of 90 about 20 followed by a rota-tion of 30 about Yo followed by a rotation of 60 about xo. Then

R = Rx,60Ry,30Rz,90 (2.3.5)

0 13 1-2 2I 13 3- --

2 4 413 I 13- - -

2 4 4

We see that Tr(R) = 0 and hence the equivalent angle is given by (2.3.2)as

(2.3.6)

The equivalent axis is given from (2.3.3) asIII 1 IT

k =(13 ' 213 -2:' 213 +2:) (2.3.7)The above axis/angle representation characterizes a given rotation by

four quantities, namely the three components of the equivalent axis kand the equivalent angle 8. However, since the equivalent axis k isgiven as a unit vector only two of its components are independent. Thethird is constrained by the condition that k is of unit length. Therefore,only three independent quantities are required in this representation ofa rotation R. We can represent the equivalent angle/axis by a singlevector r as

r = (rx , ry , rz f = (8Z

FURTHER PROPERTIES OF ROTATIONS 45

\\\\\ IJ

\\

\\\\\

---------8----IJ

FIGURE 2-7Euler angle representation.

Finally rotate about the current z by the angle \jI. In terms of the basicrotation matrices the resulting rotational transformation Rb can be gener-ated as the product

(2.3.9)

= r~: ~:. ~] [~a ~ ~a] [:: ~s~~ ~]lo a 1 - S a a ca a a 1

[

Cq>cac'I'-S,p'l' - cq>ces'I'-s",c'I' Clbsa]= scac'I'+cs'I' -sceS'I'+cq>c~ sq>sa

-SOcl/I SoS" Ce

In Chapter Four we study the inverse problem of finding the Euler An-gles (8,$,'1'1 given an arbitrary rotation matrix R.2.3.3 ROLL, PITCH, YAW ANGLESA rotation matrix R can also be described as a product of successive ro-tations about the principal coordinate axes Xo,Yo, and Zo taken in aspecific order. These rotations define the roll, pitch, and yaw angles,which we shall also denote $,8,'1', and which are shown in Figure 2-8.

46

Yaw

Roll

RH;IO MOTIONS AND HOMOGENEOUS TRANSFORMATIONS

YoPitch

FIGURE 2-8Roll, pitch and yaw angles.

We specify the order of rotation as x-y-z, in other words, first ayaw about the xo-axis through an angle 'V, then pitch about the Yo-axisan angle 8, and finally roll about the zo-axis an angle . Since the suc-cessive rotations are relative to the fixed frame, the resulting transfor-mation matrix is given by

(2.3.10)

[

C'ilce -s~cljI+c$seS IjI S$SIjI+C$SeCIjI 1= S$Ce C~CIjI+S~SeSljI - C$SIjI+S $SeCIjI

-Se CeSIjI CeCIjI

Of course, instead of yaw-pitch-roll relative to the fixed frames wecould also interpret the above transformation as roll-pitch-yaw, in thatorder, each taken with respect to the current frame. The end result isthe same matrix (2.3.10).

2.4 HOMOGENEOUSTRANSFORMATIONSConsider now a coordinate system IX IY IZ 1 obtained from 0oXoYoZo bya parallel translation of distance Id I as shown in Figure 2-9. Thusio,jo,ko are parallel to idl,k l, respectively. The vector dci is the vectorfrom the origin 00 to the origin I expressed in the coordinate systemoXoYoZo

HOMOGENEOUS TRANSFORMATiONS

Yo

FIGURE 2-9Translated frame.

47

Then any point P has representation Po and PI as before. Since therespective coordinate axes in the two frames are parallel the vectors Poand PI are related by

or

POx = P Ix+ d 6xPOy = Ply + d6yPOz =Plz + d6z

(2.4.1 )

(2.4.2)

The most general relationship between the coordinate systems0oXoYoZo and 0tX1YIZI that we consider can be expressed as the com-bination of a pure rotation and a pure translation, which is referred toas a rigid motion.

(i) Definition 2.4.1A transformation

Po = RPI + d (2.4.3)is said to define a rigid motion if R is orthogonal. Note that thedefinition of a rigid motion includes reflections when detR =-1. Inour case we will never have need for the most general rigid motion, sowe assume always that R E SO (3).

If we have the two rigid motions

(2.4.4)and

(2.4.5\then their composition defines a third rigid motion, which we can

-!-8 RIGID MOTIONS AND HOMOGENEOUS THAI'SFORMATIO:,/S

describe by substituting the expression for PI from (2.4.5) into (2.4.4).Po = R6 R ~P2 + R6d~ + dJ (2.4.6)

Since the relationship between Po and P2 is also a rigid motion, we canequally describe it as

2 2Po = R OP2 + doComparing equations (2.4.6) and (2.4.7) we have the identities

2 I 2Ro=ROR td 2 d I R I d 20= 0+ 0 I

(2.4.7)

(2.4.8)(2.4.9)

Equation 2.4.8 shows that the orientation transformations can simplybe multiplied together and Equation 2.4.9 shows that the vector fromthe origin 00 to the origin 01: is the vector sum of the vectors dJ from00 to a I and the vector R6d I from a I to 02 expressed in the orientationof the coordinate system 0oXoYozo.

A comparison of this with the matrix identity

[R I d I] [R 2 d2] [R IR 2o 0 1 I 0 Io 1 0 1 = 0 (2.4.10)

(2.4.11 )

where 0 denotes [00 OL shows that the rigid motions can be representedby the set of matrices of the form

H = [~ ~} R E 50(3)Using the fact that R is orthogonal it is an easy exercise to show thatthe inverse transformation H- I is given by

(2.4.12)

Transformation matrices of the form (2.4.11) are called homogeneoustransformations. In order to represent the transformation (2.4.3) by amatrix multiplication, one needs to augment the vectors Po and PI bythe addition of a fourth component of 1 as follows. Set

Po= [~o]PI = [~l]

(2.4.13)

(2.4.14)

The vectors Po and PI are known as homogeneous representations ofthe vectors Po and PI, respectively. It can now be seen directly that the

49

12.4.151

transformation i2.4.3) is equivalent to the (homogeneou!'l matrix equa-tion

The set of all 4x4 matrices H of tlu; form i2.4.11J is denoted by(31.3 A set of basic homogeneous tr,msformatiom ,l!;enerati[]~ (3) isgiven by

TraI1s~,'J =

1 0 0 1o 1 0 0o 0 1 0o 0 0 1

01hi~JT'""5, ... -1000]a I a a001 c

000 I

IVLl61

,2.4.18)

for translation, amI

Ro!,. = I~ :0: ~~: ~]; IMy ,. = [_~'. ~ ~ ~]; Ro!.,o = I~ -~O ~ ~]0001 0001 0001

{2.4.171for rotation abuut tbe x,}' ,z axes respectively.

The mn,

50 RIGID MOTIONS AND HOMOGENEOUS TRA'ISFORMATIONS

along the current z -axis, followed by a rotation of e degrees about thecurrent z -axis, is given by

H = Rotx,aTransx,b Transz,dRotz,e (2.4.19)1 0 0 0 100 b 100 0 ce -Se o 00 Co. -so. 0 o 1 0 0 o 1 0 0 se ce o 00 So. Co. 0 001 0 o Old 0 0 1 0o 0 0 1 000 1 000 1 0 0 o 1

Ce -Se 0 bcase CaCe -so. -sadSaSe SaCe Co. Cad

0 0 0The homogeneous representation (2.4.11) is a special case of homo-

geneous coordinates, which have been extensively used in the field ofcomputer graphics. There one is, in addition, interested in scalingand/or perspective transformations. The most general homogeneoustransformation takes the form

[

R3X3 I d3X1 jH- - I - -

f lx3 I S Ixl lRotation I TranSlation]- I -erspective I scale factor (2.4.20)

(2.5.1)

For our purposes we always take the last row vector of H to be (0,0,0,1),although the more general form given by (2.4.20) could be useful for in-terfacing a vision system into the overall robotic system or for graphicsimulation.

2.5 SI(EW SYMMETRIC MATRICESIn this section we derive some further properties of rotation matricesthat are useful for computing relative velocity and accelerationtransformations between coordinate frames. Such transformations in-volve computing derivatives of rotation matrices. By introducing thenotion of a skew symmetric matrix it is possible to simplify many ofthe computations involved.(iii) Definition 2.5.1A matrix S is said to be skew symmetric if and only if

ST + S = 0

We denote the set of all 3x3 skew symmetric matrices by S5(3). IfS E SS (3) has components Sij , i, j = 1,2,3 then (2.5.1) is equivalent tothe nine equations

S,,+SjJ=O i,j= 1,2,351

12.5.2)From /2.5.2) we see that s" = 0; tbat is, the diagonal terms of S are zeroand the off diagonal terms 5,] i~j satisfy 5,] = -SJ" Thus S contains onlythree i.nd~pcndcnt entries and every 3x3 skew symmetric matrix hasthe form

is:1 3-vector, we define the skew symmetric matrix

r0 -., "'1SIal = a~ 0 -ax

-Dr ax 0

12.5.31

12.5.41

liv) Example 2.5.2The skew symmetric matrices SliJ, Sm, and S(k) tbat result when aequals the unit normaJ vectOrs it j, and k, respectively, arc given by

~OOO] [001] ~0-10]51il= OO-I;S[jl= 0 OO;5Ikl= 1 00o 1 0 -I 0 0 0 0 0 12.5.;';)An importam property possessed by the m:nrix Sial is linearity.

Thus for any vectors a and b belonging to m,l and scalars (l and 13 wehave

Another importantp = (P.. ,p},PzV

Slna+ ~bl = aSia). ~51blproperty of Sial is that

S(alp = axp

for any

12.5.6)vecror

12.5. 71where axp denotes the vectur cross product defined in Appendix A.Equation 25. 7 can be verifi~d by direct calculation.

If R SO/3) and a,b are wCtors in lRJ it can also be shown hy directc31culation that

R(axbj = RaxRb 12.5.81Equatiun 2.$.8 is not true in general unless R is orthogonal. Equation2..;.8 says that if we first rotate the vectors a and b using the rotation:r,msformation R and then form the cross product of the rotated ycc:0r!'> Ra and Ubi tht= result is the same as that obtained by flna iormlnf

::,~ cross product axb and lht=n rotating to R\axbJ.

-.);:)- RIGID MOTIONS AND HOMOCENEOUS TR,\'iSFOR:\'IATIO!XS

For any R E 50(3) and any b E IR3, it follows from (2.5.7) and (2.5.8)that

R5 (a)R Tb = R (a x RTb) (2.5.9)= (R a)x(RRTb)= (R a) x b since R is orthogonal=5(R alb

Thus we have shown the useful fact that

R5(a)R T = 5(Ra) (2.5.10)for R E SO (3), a E IR3 . As we will see, (2.5.10) is one of the most usefulexpressions that we will derive. The left hand side of Equation 2.5.10represents a similarity transformation of the matrix 5 (a). The equationsays therefore that the matrix representation of 5 (a) in a coordinateframe rotated by R is the same as the skew symmetric matrix 5 (R a)corresponding to the vector a rotated by R.

Suppose now that a rotation matrix R is a function of the single vari-able e. Hence R = R (e) E so (3) for every e. Since R is orthogonal forall e it follows that

R (e)R (e)T = I (2.5.ll)Differentiating both sides of (2.5.11) with respect to eusing the productrule gives

dR R(e)T R (e) dR T =0de + de

Let us define the matrix

Then the transpose of 5 is

ST =(dR R(e)T)T =R(el dR Tde de

Equation 2.5.12 says therefore that

5 + ST =0

(2.5.12)

(2.5.13)

(2.5.14)

(2.5.15)In other words, the matrix 5 defined by (2.5.13) is skew symmetric.Multiplying both sides of (2.5.13) on the right by R and using the factthat R T R = I yields

dR =SR(e)de (2.5.16)

Equation 2.5.16 is very important. It says that computing the deriva-tive of the rotation matrix R is equivalent to a matrix multiplicationby a skew symmetric matrix S. The most commonly encountered si-tuation is the case where R is a basic rotation matrix or a product ofbasic rotation matrices.

53

Iv) Example 2.5.3If R = R",e, the basic rotation matrix given by (2.1.171, then direct compu-tation shows that

o 0] :1-sin8 -cos8 iocos8 -SlOe :...0

s ~ ~~ R' ~ [~~ [~ o 010-1 =S'il, ,

1 0 J

00]cos8 sinB-~ine cose

(2.5.17)

Thus we have shown that

dR." ~ S{.JRde 1 -",6Similar computations show that

(2.5.18)

dRz,o '= 51-kIR.de --,0 (2.5.19)

Ivi) Example 2.5.4Let Rk,. be a rotation about the axis defined by k as in 12.2.161. It is easyto check that S (kJJ =- S (kl. Using thiS fact together with Problem 2-25it fullows that

;2.5.20)

Other examples are given in the next section and also in Chapter Five.

2.6 ANGULAR VELOCITY ANDACCELERATIONIn the previous sections we derived exprtssions rdating position andorientation of various cooruinate frClmes via the imrodul.:tion of homo-geneous transformations. In this section WI.: disl:uss relative vdOCiticsa.nd accelerations in the same contt:xt.

Suppose that a rotation matrix R hi time varying, so thatR = R It J E SO (3) for every t E R. An argument identir.:al to the one inthe previous scction ~hows that the time derivative R(rl of R (1.1 isgiven by

Ri'!~SI'!Ri'! 12,6,11where the matrix Sft! is skew ~ymmetric. Nnw, since S(l) is skew

~\-mmctric, it can be represented as 5100(t II fnT a uniquc vector will.This vector ro{t) is the angular velocity of the rotating frame with

~~spect to the fixed frame at time t.

54 RIGID MOTIONS AND HOMOGENEOUS THA~S"'ORMATlO"lS

(vii) Example 2.6.1S h R (I R Then R' (t) -_ dR , d' huppose t at t = X,e(l}' dt IS compute USIng t echain rule as

, dR de .R = de dt =eS(i)R(t)=S(ro(t))R(t) (2.6.2)

where ro = ie is the angular velocity about the x -axis.Suppose PI represents a vector fixed in a coordinate frame IX IY IZ I,

and the frame 0IXIYIZl is rotating relative to the frame 0oXoYoZo. Thenthe coordinates of PI in 0oxoYoZo are given by

Po = R(t lpIThe velocity Po is then given as

Po = S (ro)R (t )Pl=S(ro)Po =IDXpo

(2.6.3)

(2.6.4)

which is the familiar expression for the velocity in terms of the vectorcross product. Now suppose that the motion of the frame IX lY lZ I re-lative to 0oXoYozo is more general. Suppose that the homogeneoustransformation relating the two frames is time-dependent, so that

(2.6.5)

For simplicity we omit the argument t and the subscripts and super-scripts on R 6and dci, and write

Po = RPI + dDifferentiating the above expression using the product rule gives

Po = RPI + d since PI is constant=S(ro)Rpl + d=IDxr+v

(2.6.6)

(2.6.7)

where r = R PI is the vector from I to P expressed in the orientation ofthe frame 0oXoYoZo, and v is the rate at which the origin 01 is moving.

If the vector PI is also changing relative to the frame IX lY lZ I thenwe must add to the term v the term R (t )PI' which is the rate of changeof PI expressed in the frame 0oXoYoZo.

We may also derive the expression for the relative acceleration in thetwo coordinate frames as follows. First, recall that the cross productsatisfies the product rule for differentiation (Appendix A)

d da db-(axb) = -xb + ax-dt dt dt (2.6.8)

AODITION OF ANGUI.AR VELO\.lTIES

If we now rewrite Equation 2.6.7 asPo - it = RPI

=roXRPI

and differentiate both sides with respect to t we obtain

Po - d= c:i> x R PI + ro x (R PII= c:i>xr + roxlroxrl

Thus (2.6.10) may be written asPo = c:i>xr + rox(roxr) + a

55

(2.6.9)

(2.6.10)

12.6.11)

(2.7.1)(2.7.2)(2.7.3)

where a is the linear acceleration. The term rox(roxr) is called the centripetal acceleration of the particle. It is always directed toward theaxis of rotation and is perpendicular to that axis. The term c:i>x r iscalled the transverse acceleration.

Again, if the vector PI is changing with respect to IX IY IZ I, theabove expression must be modified. In this case it is left as an exerciseto show that Equation 2.6.11 is replaced by

Po = c:i>x r + rox (roxr) + 2rox RPI + a (2.6.12)where a = RPI + d. The term 2roxRpi is known as the Coriolis ac-celeration.

2.7 ADDITION OF ANGULARVELOCITIESWe are often interested in finding the resultant angular velocity due tothe relative rotation of several coordinate frames. We now derive theexpressions for the composition of angular velocities of two movingframes O\XIY1ZI and 02X2Y2Z2 relative to a fixed frame 0oXoYoZo.

Given a point P with representations Po, PJ, P2 in the respectiveframes we have the relationships

I d IPo =RoPI + 02 d2PI = R IP2 + I2 2Po = R OP2 + do

where as before

and

d 2 d I 1 d 20= o+R o IAs before, all of the above quantities arederivatives of both sides of (2.7.4) yields

(2.7.4)

(2.7.5)functions of time. Taking

56 RIGID MOTIONS ANI) HOMOGENEOUS TRANSFORMATIONS

. 2 . 1 2 I' 2RO=ROR J +RoR IThe term R~ on the left-hand side of (2.7.6) can be written

. 2 2 2Ro= 5( roo)R oThe first term on the right-hand side of (2.7.6) is simply

R~ R ~ = 5 (ro6)R 6R ~ = 5 (ro6) R~Let us examine the second term on the right hand side of (2.7.6).the expression (2.5.10) we have

R~ R~ = R6Slror)R~=R6 S(ror)R~TR6R~=S(R6ror)R6R~

=5(R6ror)R~Combining the above expressions we have shown that

S(ro6)R~=15(ro6) + 5(R6rorJlR6 R~Since SIal + SIb) = S(a+b), we see that

2 J R 1 2roo = roo + 0 ro 1

(2.7.6)

(2.7.7)

(2.7.8)Using

(2.7.9)

(2.7.10)

(2.7.11 )In other words, the angular velocities can be added once they are ex-pressed relative to the same coordinate frame, in this case 0oXoYoZo.

The above expression can be extended to any number of coordinatesystems. For example, if

Rg = R6 R~' .. Rnn_1 (2.7.12)then

(2.7.13)where

n 1 R 1 2 R 2 3 R 3 4 R n-l nroo =roo+ oro, + 0 002 + 0 003+'" + 0 ron-l (2.7.14)

BARNETT, S., Matrix Methods for Engineers and Scientists,McGraw-Hill, London, 1979.

CHEN, Y.c., and VIDYASAGAR, M., "On the Axis-AngleParametrization of the Rotation Group and its Applications toRobotics," preprint 1987.

[2]

REFERENCES AND SUGGESTEDREADING[11

PROBI.F."'S 57

131 CURTISS, M.L., Malrix Groups, Second Edition, Springer-Verlag,New York, 1984.

l4J FR1EDHERC, S.H., INSEL, A.J., and SPENCE, l.E., Linear Algebra,Prentice-Hall, Englewood Cliffs, NT, 1979.

[5] REDDY, ].N., and RASMUSSEN, M.L., Advdnced EngineerinxAnalysis, Wiley, New York, 1982.

[6] SOKOLNIKOFF, LS., and REDHEFFER. R.M., Muthematical A1elhod"of Physics and Modern Engineering, McCr.T''N-Hill, r\ew York,1958.

171 WHITTAKER, LT., Dynamic., of jJmtic1e.'i and Rigid J)odies,Cambridge University Press, London, 1904.

PROBLEMS2-1 If R is an orthogonal matrix show that the column vectors of R

are of unit length and mutually perpendicular.

2- 2 If R is an orthogonal rna trix show that det R = 1.

2-3 Show that detR =+1 if we restrict ourselves to right-handedcoordinate systems.

2-4 V"ify Equatio", 2.1 14-2.1.10.

2-5 Derive Equations 2.1.17 and 2.1.18.

2-6 Suppose A is a 2x2 rotation matrix. In other words A T A = 1and det A = 1. Show that there exists a unique El such that A isof the form

_ Icos8 -SinEl]A - lsin8 cos8

.2-7 Find the rotation matrix representing a roll of : followed hy a

yaw of ~ followed by a pitch uf ; .2-R If the coordinate frame () jX 1Y 1Z I is obtained from the coordinate

frame 0oXoY(}2o by a rotation of ~ about the x-axis followed bya rotation of ; about the fixed y-axis, find the rotation matrixR representing the composite transformation. Sketch the initialand final frames.

2-12

2-9 Suppost: that three coordinate frames 0IXLJ'IZ1, 0lX::'Y2Z2, anuO,lXJY.lZ3 arc given, and suppose

1 a 0fa 0 -1], 0 1 ';.3 U;= o 1 al{ I = -2 2

..f;l loa10-' -2 2

Find the matrix Ri.2-10 Verify Equation 1.2.16.

2-11 If!{ is a rotation marrix show that +1 is an eigenvalue of R. Letk be a Lmit eigenvector corrcsponding to the cigenvalue +1.Give a physical interpretation of k.

Letk=-!:..O,l,I)T,8=.90". Find Rkfl ."';l .

2-13

2-14

2-1 :S

2-16

2-17

Show by direct calculation that Rk,A given by 12.2.16) is cqual toN. given by 12.3.5) if e and kart: givcn hy (2..1.6) and 12.3.7),respectively.

Suppose R represents a rotation of 90(.1 about Yo followt:d by arotation of 45

"RIlRI.F:\IS

"

FTGURE 2-10Diagram for Problem 2-17.

59

2-18 Consider the diagram of Figure 2-11. A robot is St.'t up I mt:terfrom a tahle, two of whose legs arc on the Yo axis

60

" 1m

camera

" ~""':::'~-----,------r/Table

FIGUHF. 2-11Diagram [or Prohlem 2-18.

6=-2'oR;

alo~EvaluateGiven R~ := R.1 ,&N)",{J, computt:~= ~.

2-25 Usc E4U3tiun 2.2.16 to shuw thatRk,8 "" I + S(klsin(ij) + S2(k)vt=rs(A)

224

2-26 Vt:rify (2.5.19J by llirecl C

PH()UL~'M;; 61

2-29 Show that R k,lJ = eS1k!9.[Hint: Use the serics exp,msion for the matrix exponential to-gether with Problems 2-25 and 2-27. Alternatively use the factthat R),. satisfies tbe differential equation

dR = SlklRdB

2-30 US~ Problem 2-29 to show the converse of 2-28, that is, ifR E sot,,) tben there exists S E SSj31 sllch that R = e\'.

2-31 Given the Euler an,l;le transformation

R = N.~,,,,R.I'.I+'\~,C!d

show th.:1t Cit R = S (wlR wherero = lc ..~~ - s~eli + Is"s6~+c ~elj + lo/+co4>lk.

The components of i,j,k, respectively, arc called thc nutation,spin, and precession.

2-32 Repeat Problem 2.;.H for the Roll-PilchYaw transfonnation. Inother words, find an explicit expression for 00 slich that:, R = SIw)R, where R is given by (2.3.101.

2-33 Two frames Of)XoYoZo and O,XIYIZl are related by the homo-geneous uansformation

H=I!n~Jj;000 I

A particle has velocity vdt) = (,'l,I,OlT relative to framc0lX1YJZI. What is the velocity of the partidc III frameOnxuYOZo?

234 Three frames 0I)XoYOZo, O.XIY1Zl, and O!X1J'2Z2 are givcn below.If the angular velocities (J)~ aud wi are given as

what is the angular velocity 005 at th~ instant when

R,; J~ ~ ~l]l~ I 0

CHAPTER THREE

FORWARD I

of-ut!edoOl of motion: the angle of rotation in the ease of a revolutejoint, and the amount of linear displacement in the case of a prismaticjoint. In COntrast, a h311 and socket joint has two dcgn:cs-of-frcedom.III this book it is assumed througbout that all joints bave only a single

l1e~ee-offreedom. Note that the assumption does not involve any realloss of generality, since joints such as a ball and socket joint (twOdef.;reesofheedom) or a spherical wrist ithree degrees-of-freedom) canalways be thought of as a slicceSSlOn of single degree-of-freedom jointswith links of length zero in between.

With the assumption that each joint has a single dt!gree-of-freedom,the action of each joint can be descrihed by a single real number; theangle of rot.atiun in the case of :l revolute joint or the displacement inthe case of a prismatic joint. The nhjcctive of forward kmematicanalysis is to detennine the cumulative effect of tbe entiJ"e set of juintvariables. To do this in a systematic m:mner, one should really intro-duce some conventions. It is of course possible to carry out forwardkinematics analysis even without re~pccting these conventions as wedid for the two link planar examplc in Chaptcr One. However, thekinematic analysis of lID n link manipulatOr is extremely complex andthe conventions introJuc.:ed below simplify the equations considerably.Moreover, they give rise to a universal language with which robot en-gineer!> can communicate.

Suppo!>e a rohot has n+1 links numbered from 0 to n slarting fromthe base of the rohot, which is taken as link O. The ;aims are num-bered from 1 to n, and The j -th joint is the point in sp:at.:e where linksi-I and i arc connected. The jth joint variable. is dcnon.:d by q,. Inthe case of a revolute jOlOt, q, is the angle of rotOltion, and in lhe t.:as\,. ofa prismatic joint, qJ is the joint displacement. Next, a coonliJulte frameis attached rigidly to each link. To he specific, we attach an int:rtialframe to the hase and call it frame O. Then we choose frames 1 throughn such that frame i is rigidly 3uached to link i. This means that,whatever motion the roOOt executes, the coordinates of each point onlink i arc constant when expre!\!\ed in the j th coordinate frame. Figure3-1 illustrates the idea of attaching frames rigidly to links in the case ofan elbow manipulator.

uw suppose AI is the homogeneous matrix that transforms thecoordinates of a point from fTamc i to frame i-l. The matrix Al is notconstant, but varies as the configuration of the robot is changed.However, the assumption that all joints are either revolute or prismaticmeans that A j is a function of only a single joint variable, namely q,.in other words,

A j = A,{qj 1 13.1.11:\:ow the homogeneous matrix that transforms the coorJinat~ of apoint hom frame j to frame i is called, by convention, a transformationmatrix, and is usually denoted by T~. From Chapter 2 we see that

TI "" A'!~i'2 ...Ai_.Aiifj

Link 1

"

'1

base(lInItO)

Joint I

FIGURE 3-1Coordinate frames attached to elbow manipulator,

T~-ljfj-jT~ _ lTi) 1 if j :> i

13.1.2)

Ry the manner in which we have rigidly attached the various framesto the corresponding links, it follows that the position of any point onthe cnd-effectOr, when expressed in frame n, is a constant independentof the configuration of the robot. lJenote tbe position and orientationof lh~ rmd-dfector with respect to the inertial or hase frame by a three--Vector d; and a 3x3 rotiltion matrix R~, respectively, and define thehomogeneous matrix

(3.1.3)

Tben the position and orientation of the cnd-effector in the im:rtialframe are given hy

13.1.4)

DEl'iA\'IT_HAII'1 FJ';HEHI; RF.PI(I;"F."'T,\TI~I""

EllCh homogeneous transformation A, is of the (onn65

[R,:,

Ai = 0

Hence

13,1.51

T; A,., "A, = [RO; dij (-'.1.61The matrix R~ expresses the orientation of frame j relative to frame i

and is given by the rot

Co, -so, Co, So, Sa, Of Ce,

Se, Ceo Cn, -Ce,Su, (I, Sa,

0 S" Ca. d;0 0 0

where the four quantities 8" a" d j , O:j .He parameters of link i anu jointi. The various parameters in (3.2.1 J are generally given th~ fuUuwingnames: OJ is called the length, Ctj is called the twist, dj is caIted theoffset, and 8; is called the angle. Since the matrix A, is a function of asingle variable it turns out that three of the above four quantities areconstant for a given link, while the fourth parameter, 81 for a revolutejoint and d j for a prismatic joint, is the joint variable.

From Chapter Two one can see that an arbitrary homogeneous rnatrix can be characterized by six numbers, such as, for example, thethree components of the displacement vector d and three Euler anglescorresponding to the rotation matrix R. In the D-H representation, incontrast, there are only four parameters. How is this possible? Theanswer is that, while frame j is required to be rigidly attached to link i,we have considerable freedom in choosing the origin and the coordinateaxes of the frame. For example, it is not necessary that the oriKin 0i offrame i should correspond to ioint i or to joint i -+ I, that is, to eitherend of link i. Thus, by a clever choice of the origin and the coordinateaxes, it is possible to cut down the number of parameters needed fromsix to four jor even fewer in some cases). Let us see how this can bedone.

We begin by determining just which homogeneous transformationscan be expressed in the form 13.2.1). Suppose we are ,l!;iven twO frames,denoted by frames 0 and I, respectively. Then there exists a unique ho-mogeneous transformation matrix A that takes the coordinates fromframe 1 into those of frame O. Now suppose the tWO frames have twoadditional features, namdy:

iDH1J The axis x I is perpendicular to the axis z0tDH2J The axis Xl imerst:ccs the axis Zo

as shown in Figure 3-2. Under these conditions, we claim that thereexist unique numbers a, d, 8, a such that

13.22)

13.2.31

Of course, since 8 and a are angles, we really mean that they arc uniqueto within a multiple of 2lt.

Tn show that the matrix A can he written in this form, write A as

A = [~~]

DENAVIT-HAIITE:\IlERG REPHESENTATJO:\ 67

and let rj denote the i -th column of the rotation matrix R. Referring toFigure 3-2 we see that assumption (DHl) above means that the vectorr1 (which is the representation of the unit vector it in frame 0) is ortho-gonal to ko ~ [00 1f, that is, 131 ~ O. From this we claim that thereexist unique angles 9 and a such that

(3.2.4)

The only information we have is that r 31 ~ O. But this is enough. First,since each row and column of R must have unit length, 131 ~ 0 impliesthat

(3.2.5)

Hence there exist unique 9, a such that

(3.2.6)

Once 9 and a are found, it is routine to show that the remaining ele-ments of R must have the form shown in (3.2.4), using the fact that Ris a rotation matrix.

Yo

\ (1'\ I\ I\ I

Zl III

Xl I-------~--E_=~-------

d

a

IIIII Zo

\,,\\\

\\

\\

FIGURE 3-2Coordinate frames satisfying assumptions DH-l andDH-2.

Next, assumption (DH2J means that the vector d - d 6(which is thecoordinate vector of the origin of frame I in terms of frame 01 is a linearcombination of ko and Ri,. Therefore, since (;31 _ 0, we can express d~uniquely as

(3.271

Substituting R from (3.2.4) and d hom (3.2.7) into (3.2.4) we obtain(3.2.1) as claimed.

Now that we have established that each homogeneous matrix sa-tisfying conditions IDH1J and (DH2) above can be represented in theform 13.2.1 J, we can in fact give a physical interpretation to each of thefour quantities in (3.2.1.1- The parameter a is the distance between theaxes Zo and ZI, aud is measured along the axis XI. The an~le a is theangle between the axes Zo 3ud Zt, measured in a plane norma! to Xl'The positive sense for a is determined from Zo to Z I by the right-handrule as shown in Figure 3...3. The parameter d is tbe distance betweenthe origin 00 and the intersection of the Xl axis with Zo measured alongthe Zo axis. Finally, e is the angle between the Xo and x 1axes measuredin a plane normal to the Zo axis.

It only remains now to show that, for a robot manipulator, one canalways choose the frames 0, ... ,n in such a way that the above twO con-ditions

DENAVIT-HARTE:\"BERG REI'HI';SENTATIO:,\ 69

choices of the various coordinate frames are not unique, even whenconstrained by the requirements above. Once we have illustrated thegeneral procedure, we will discuss various common special cases whereit is possible to simplify the homogeneous matrix further.

To start, it is helpful to identify all of the joint axes and label themZO," . ,ZI1-1- Zi is the axis of revolution of joint i +1 if joint i +1 is revo-lute, and is the axis of translation of joint i+l if ioint i+l is prismatic.Next choose the origin 00 of the base frame. This point can be chosenanywhere along the Zo axis. Finally, choose xo, Yo in any convenientmanner so long as the resulting frame is right-handed. This sets upframe O.

Now suppose frames 0, ... ,i -1 have been set up. To understand thefollowing it will be helpful to consider Figure 3-4. In order to set upframe i it is necessary to consider two cases: (a) the axes Zi-l, Zi are notcoplanar, and (b) they are coplanar. If the axes Zi-l, Zi are not coplanar,then there exists a unique line segment perpendicular to both such thatit connects both lines and it has minimum length. The line containingthis common normal to Zi-l and Zi is then defined to be Xi, and thepoint where it intersects Zi is the origin 0i' Then by construction, bothconditions (DH1) and (DH2) are satisfied and the vector from 0i-l to 0iis a linear combination of Zi-I and Xi' The specification of frame i iscompleted by choosing the axis Yi to form a right-hand frame. Since as-sumptions (DH1) and (DH2) are satisfied the homogeneous matrix Ai isof the form (3.2.1).

FIGURE 3-4Denavit-Hartenberg frame assignment.

70 F()JI\\ .\I\1J !\.J.'E.\1ATlC~,TilE DEl'iAYIT-l1.\I\TEf\llEIH; HEI'I\E~E.~TYnON

Now consider case (b), that is, the axes Zj_l and Zi arc coplanar. Thismeans either that they arc parallel, or that they intersect. This situa-tion is in fact quite common, and deserves some detailed analysis. Ifthe axes Zi_l and Zj arc parallel, there arc infinitely many common nor-mals between them and condition (DH1) docs not specify Xj com-pletely. In this case we choose the origin 0; to be at joint i so that Xi isthat normal from Z,-_j which passes through 0i' Note that the choice of01 is arbitrary in this case. We could just as well choose the normalthat passes through 0'--1 as the Xi axis in which case eli would be equalto zero. In fact, the latter choice is common in much of the robotics li-terature. Since the axes Zi 1 and Zj are parallel, u,- will be zero in thiscase. Once Xj is fixed, y, is determined, as usual hy the right-hand rule.

Finally, consider the case where Z; intersects the axis Z;_I. In thiscase Xj is chosen normal to the plane formed by z, and Z;_l' The posi-tive direction of Xi is arbitrary. The most natural choice for the origin0, in this case is at the point of intersectio