41828119-robotics.pdf

1-r

ROBOTICS:Control, Sensing, Vision, and Intelligence

CAD/CAM, Robotics, and Computer Vision

Consulting EditorHerbert Freeman, Rutgers University

Fu, Gonzalez, and Lee: Robotics: Control, Sensing, Vision, and IntelligenceGroover, Weiss, Nagel, and Odrey: Industrial Robotics: Technology, Programming,

and ApplicationsLevine: Vision in Man and MachineParsons: Voice and Speech Processing

3F

ROBOTICS:Control, Sensing, Vision, and Intelligence

K. S. FuSchool of Electrical Engineering

Purdue University

R. C. GonzalezDepartment of Electrical Engineering

University of Tennessee

andPerceptics Corporation

Knoxville, Tennessee

C. S. G. LeeSchool of Electrical Engineering

Purdue University

McGraw-Hill Book CompanyNew York St. Louis San Francisco Auckland Bogota

Hamburg London Madrid Mexico Milan Montreal New DelhiPanama Paris Sao Paulo Singapore Sydney Tokyo Toronto

it Unzv

0.

ToViola,Connie, andPei-Ling

14137

Library of Congress Cataloging-in-Publication Data

Fu, K. S. (King Sun),Robotics : control, sensing, vision, and intelligence.

(McGraw-Hill series in CAD/CAM robotics and computervision)

Bibliography: p.Includes index.1. Robotics. I. Gonzalez, Rafael C. II. Lee,

C. S. G. (C. S. George) III. Title.TJ21I.F82 1987 629.8' 92 86-7156

ISBN 0-07-022625-3

ISBN 0-07-022626-1 (solutions manual)

This book was set in Times Roman by House of Equations Inc.The editor was Sanjeev Rao;the production supervisor was Diane Renda;the cover was designed by Laura Stover.Project supervision was done by Lynn Contrucci.R. R. Donnelley & Sons Company was printer and binder.

ROBOTICS: CONTROL, SENSING, VISION, AND INTELLIGENCE

Copyright 1987 by McGraw-Hill, Inc. All rights reserved. Printed in the United Statesof America. Except as permitted under the United States Copyright Act of 1976, no partof this publication may be reproduced or distributed in any form or by any means, orstored in a data base or retrieval system, without the prior written permission of thepublisher.

2 3 4 5 6 7 8 9 0 DOCDOC 8 9 8 7

ISBN 0-07-022625-3

'L7

.,

one

1j?

0j)

..,

`'"

ABOUT THE AUTHORS

K. S. Fu was the W. M. Goss Distinguished Professor of Electrical Engineering atPurdue University. He received his bachelor, master, and Ph.D. degrees from theNational Taiwan University, the University of Toronto, and the University of Illi-nois, respectively. Professor Fu was internationally recognized in the engineeringdisciplines of pattern recognition, image processing, and artificial intelligence. Hemade milestone contributions in both basic and applied research. Often termed the"father of automatic pattern recognition," Dr. Fu authored four books and morethan 400 scholarly papers. He taught and inspired 75 Ph.D.s. Among his manyhonors, he was elected a member of the National Academy of Engineering in 1976,received the Senior Research Award of the American Society for Engineering Edu-cation in 1981, and was awarded the IEEE's Education Medal in 1982. He was aFellow of the IEEE, a 1971 Guggenheim Fellow, and a member of Sigma Xi, EtaKappa Nu, and Tau Beta Pi honorary societies. He was the founding president ofthe International Association for Pattern Recognition, the founding editor in chief ofthe IEEE Transactions of Pattern Analysis and Machine Intelligence, and the editorin chief or editor for seven leading scholarly journals. Professor Fu died of a heartattack on April 29, 1985 in Washington, D.C.

R. C. Gonzalez is IBM Professor of Electrical Engineering at the University ofTennessee, Knoxville, and founder and president of Perceptics Corporation, a high-technology firm that specializes in image processing, pattern recognition, computervision, and machine intelligence. He received his B.S. degree from the Universityof Miami, and his M.E. and Ph.D. degrees from the University of Florida, Gaines-ville, all in electrical engineering. Dr. Gonzalez is internationally known in hisfield, having authored or coauthored over 100 articles and 4 books dealing withimage processing, pattern recognition, and computer vision. He received the 1978UTK Chancellor's Research Scholar Award, the 1980 Magnavox Engineering Pro-fessor Award, and the 1980 M.E. Brooks Distinguished Professor Award for hiswork in these fields. In 1984 he was named Alumni Distinguished Service Professor

v

gin'

C17

p.,

._D

C].

.A

i..s"'

`--

cad

VI ABOUT THE AUTHORS

at the University of Tennessee. In 1985 he was named a distinguished alumnus bythe University of Miami. Dr. Gonzalez is a frequent consultant to industry andgovernment and is a member of numerous engineering professional and honorarysocieties, including Tau Beta Pi, Phi Kappa Phi, Eta Kappa Nu, and Sigma Xi. Heis a Fellow of the IEEE.

C. S. G. Lee is an associate professor of Electrical Engineering at Purdue Univer-sity. He received his B.S.E.E. and M.S.E.E. degrees from Washington StateUniversity, and a Ph.D. degree from Purdue in 1978. From 1978 to 1985, he wasa faculty member at Purdue and the University of Michigan, Ann Arbor. Dr. Leehas authored or coauthored more than 40 technical papers and taught robotics shortcourses at various conferences. His current interests include robotics and automa-tion, and computer-integrated manufacturing systems. Dr. Lee has been doingextensive consulting work for automotive and aerospace industries in robotics. He isa Distinguished Visitor of the IEEE Computer Society's Distinguished Visitor Pro-gram since 1983, a technical area editor of the IEEE Journal of Robotics and Auto-mation, and a member of technical committees for various robotics conferences. Heis a coeditor of Tutorial on Robotics, 2nd edition, published by the IEEE ComputerSociety Press and a member of Sigma Xi, Tau Beta Pi, the IEEE, and the SME/RI.

.--

88,0

000

Preface

1. Introduction- 1.1. Background

1.2. Historical Development1.3. Robot Arm Kinematics and Dynamics1.4. Manipulator Trajectory Planning

and Motion Control1.5. Robot Sensing1.6. Robot Programming Languages1.7. Machine Intelligence1.8. References

2. Robot Arm Kinematics2.1. Introduction2.2. The Direct Kinematics Problem2.3. The Inverse Kinematics Solution2.4. Concluding Remarks

ReferencesProblems

3. Robot Arm Dynamics3.1. Introduction3.2. Lagrange-Euler Formulation3.3. Newton-Euler Formation3.4. Generalized D'Alembert

Equations of Motion3.5. Concluding Remarks

ReferencesProblems

CONTENTS

xi

7

8

9

10

10

12

12

13

52

75

76

76

82

82

84

103

124

142

142

144

Vii

.AA

Viii CONTENTS

4. Planning of Manipulator Trajectories4.1. Introduction4.2. General Considerations on

Trajectory Planning4.3. Joint-interpolated Trajectories

4.4. Planning of Manipulator CartesianPath Trajectories

4.5. Concluding RemarksReferencesProblems

5. Control of Robot Manipulators5.1. Introduction5.2. Control of the Puma

Robot Arm5.3. Computed Torque Technique5.4. Near-Minimum-Time Control5.5. Variable Structure Control5.6. Nonlinear Decoupled Feedback

Control5.7. Resolved Motion Control5.8. Adaptive Control5.9. Concluding Remarks

ReferencesProblems

6. Sensing6.1. Introduction6.2. Range Sensing6.3. Proximity Sensing6.4. Touch Sensors6.5. Force and Torque Sensing

6.6. Concluding RemarksReferences

Problems

7. Low-Level Vision7.1. Introduction7.2. Image Acquisition7.3. Illumination Techniques7.4. Imaging Geometry7.5. Some Basic Relationships Between Pixels7.6. Preprocessing7.7. Concluding Remarks

ReferencesProblems

0000

0000

00!-

'

'r1C

/1

a'

boo

(IQ

'.D

C1'

(N]

.L'

.-a

rig

ricer0000000000

CONTENTS iX

8. Higher-Level Vision 3628.1. Introduction 3628.2. Segmentation 3638.3. Description 3958.4. Segmentation and Description of

Three-Dimensional Structures 4168.5. Recognition 4248.6. Interpretation 439

8.7. Concluding Remarks 445References 445

Problems 447

9. Robot Programming Languages 4509.1. Introduction 4509.2. Characteristics of Robot-

Level Languages 4519.3. Characteristics of Task-

Level Languages 4629.4. Concluding Remarks 470

References 472

Problems 473

10. Robot Intelligence and Task Planning 47410.1. Introduction 47410.2. State Space Search 474

10.3. Problem Reduction 48410.4. Use of Predicate Logic' 489

10.5. Means-Ends Analysis 49310.6. Problem-Solving 49710.7. Robot Learning 504

10.8. Robot Task Planning 506

10.9. Basic Problems in Task Planning 50910.10. Expert Systems and

Knowledge Engineering 51610.11. Concluding Remarks 519

References 520

Appendix 522A Vectors and Matrices 522B Manipulator Jacobian 544

Bibliography 556

Index 571

c+,

PREFACE

This textbook was written to provide engineers, scientists, and students involvedin robotics and automation with a comprehensive, well-organized, and up-to-date account of the basic principles underlying the design, analysis, and syn-thesis of robotic systems.

The study and development of robot mechanisms can be traced to themid-1940s when master-slave manipulators were designed and fabricated at theOak Ridge and Argonne National Laboratories for handling radioactive materi-als. The first commercial computer-controlled robot was introduced in the late1950s by Unimation, Inc., and a number of industrial and experimental devicesfollowed suit during the next 15 years. In spite of the availability of this tech-nology, however, widespread interest in robotics as a formal discipline of studyand research is rather recent, being motivated by a significant lag in produc-tivity in most nations of the industrial world.

Robotics is an interdisciplinary field that ranges in scope from the design ofmechanical and electrical components to sensor technology, computer systems,and artificial intelligence. The bulk of material dealing with robot theory,design, and applications has been widely scattered in numerous technical jour-nals, conference proceedings, research monographs, and some textbooks thateither focus attention on some specialized area of robotics or give a "broad-brush" look of this field. Consequently, it is a rather difficult task, particularlyfor a newcomer, to learn the range of principles underlying this subject matter.This text attempts to put between the covers of one book the basic analyticaltechniques and fundamental principles of robotics, and to organize them in aunified and coherent manner. Thus, the present volume is intended to be of useboth as a textbook and as a reference work. To the student, it presents in a logi-cal manner a discussion of basic theoretical concepts and important techniques.For the practicing engineer or scientist, it provides a ready source of referencein systematic form.

xi

CA

D

"i7

."3

O..

crop

may

"

CD

'ac

';T

;;-

r

gas

-on

Xii PREFACE

The mathematical level in all chapters is well within the grasp of seniors andfirst-year graduate students in a technical discipline such as engineering and com-puter science, which require introductory preparation in matrix theory, probability,computer programming, and mathematical analysis. In presenting the material,emphasis is placed on the development of fundamental results from basic concepts.Numerous examples are worked out in the text to illustrate the discussion, and exer-cises of various types and complexity are included at the end of each chapter. Someof these problems allow the reader to gain further insight into the points discussedin the text through practice in problem solution. Others serve as supplements andextensions of the material in the book. For the instructor, a complete solutionsmanual is available from the publisher.

This book is the outgrowth of lecture notes for courses taught by the authors atPurdue University, the University of Tennessee, and the University of Michigan.The material has been tested extensively in the classroom as well as throughnumerous short courses presented by all three authors over a 5-year period. Thesuggestions and criticisms of students in these courses had a significant influence inthe way the material is presented in this book.

We are indebted to a number of individuals who, directly or indirectly, assistedin the preparation of the text. In particular, we wish to extend our appreciation toProfessors W. L. Green, G. N. Saridis, R. B. Kelley, J. Y. S. Luh, N. K. Loh, W.T. Snyder, D. Brzakovic, E. G. Burdette, M. J. Chung, B. H. Lee, and to Dr. R. E.Woods, Dr. Spivey Douglass, Dr. A. K. Bejczy, Dr. C. Day, Dr. F. King, and Dr.L-W. Tsai. As is true with most projects carried out in a university environment,our students over the past few years have influenced not only our thinking, but alsothe topics covered in this book. The following individuals have worked with us inthe course of their advanced undergraduate or graduate programs: J. A. Herrera,M. A. Abidi, R. O. Eason, R. Safabakhsh, A. P. Perez, C. H. Hayden, D. R.Cate, K. A. Rinehart, N. Alvertos, E. R. Meyer, P. R. Chang, C. L. Chen, S. H.Hou, G. H. Lee, R. Jungclas, Huarg, and D. Huang. Thanks are also due to Ms.Susan Merrell, Ms. Denise Smiddy, Ms. Mary Bearden, Ms. Frances Bourdas, andMs. Mary Ann Pruder for typing numerous versions of the manuscript. In addition,we express our appreciation to the National Science Foundation, the Air ForceOffice of Scientific Research, the Office of Naval Research, the Army ResearchOffice, Westinghouse, Martin Marietta Aerospace, Martin Marietta Energy Systems,Union Carbide, Lockheed Missiles and Space Co., The Oak Ridge National Labora-tory, and the University of Tennessee Measurement and Control Center for theirsponsorship of our research activities in robotics, computer vision, machine intelli-gence, and related areas.

K. S. FuR. C. Gonzalez

C. S. G. Lee

PREFACE Xiii

Professor King-Sun Fu died of a heart attack on April 29, 1985, inWashington, D.C., shortly after completing his contributions to this book. Hewill be missed by all those who were fortunate to know him and to work withhim during a productive and distinguished career.

R. C. G.C. S. G. L.

`i7

0C7

..t

Z".

`",

[n'

UQ

"'

Os.

CIO

.U+

CHAPTER

ONE

INTRODUCTION

One machine can do the work of ahundred ordinary men, but no machine

can do the work of one extraordinary man.Elbert Hubbard

1.1 BACKGROUND

With a pressing need for increased productivity and the delivery of end products ofuniform quality, industry is turning more and more toward computer-based auto-mation. At the present time, most automated manufacturing tasks are carried outby special-purpose machines designed to perform predetermined functions in amanufacturing process. The inflexibility and generally high cost of thesemachines, often called hard automation systems, have led to a broad-based interestin the use of robots capable of performing a variety of manufacturing functions ina more flexible working environment and at lower production costs.

The word robot originated from the Czech word robota, meaning work.Webster's dictionary defines robot as "an automatic device that performs functionsordinarily ascribed to human beings." With this definition, washing machines maybe considered robots. A definition used by the Robot Institute of America gives amore precise description of industrial robots: "A robot is a reprogrammablemultifunctional manipulator designed to move materials, parts, tools, or special-ized devices, through variable programmed motions for the performance of avariety of tasks." In short, a robot is a reprogrammable general-purpose manipu-lator with external sensors that can perform various assembly tasks. With thisdefinition, a robot must possess intelligence, which is normally due to computeralgorithms associated with its control and sensing systems.

An industrial robot is a general-purpose, computer-controlled manipulator con-sisting of several rigid links connected in series by revolute or prismatic joints.One end of the chain is attached to a supporting base, while the other end is freeand equipped with a tool to manipulate objects or perform assembly tasks. Themotion of the joints results in relative motion of the links. Mechanically, a robotis composed of an arm (or mainframe) and a wrist subassembly plus a tool. It isdesigned to reach a workpiece located within its work volume. The work volumeis the sphere of influence of a robot whose arm can deliver the wrist subassemblyunit to any point within the sphere. The arm subassembly generally can movewith three degrees of freedom. The combination of the movements positions the

1

''d

'CS 0_

o

2 ROBOTICS: CONTROL, SENSING, VISION, AND INTELLIGENCE

wrist unit at the workpiece. The wrist subassembly unit usually consists of threerotary motions. The combination of these motions orients the tool according to theconfiguration of the object for ease in pickup. These last three motions are oftencalled pitch, yaw, and roll. Hence, for a six jointed robot, the arm subassembly isthe positioning mechanism, while the wrist subassembly is the orientation mechan-ism. These concepts are illustrated by the Cincinnati Milacron T3 robot and theUnimation PUMA robot arm shown in Fig. 1.1.

Waist rotation 320

Figure 1.1 (a) Cincinnati Milacron T3 robot arm. (b) PUMA 560 series robot arm.

CD

'

v,

C1,

i.,

INTRODUCTION 3

Many commercially available industrial robots are widely used in manufactur-ing and assembly tasks, such as material handling, spot/arc welding, parts assem-bly, paint spraying, loading and unloading numerically controlled machines, spaceand undersea exploration, prosthetic arm research, and in handling hazardousmaterials. These robots fall into one of the four basic motion-defining categories(Fig. 1.2):

Cartesian coordinates (three linear axes) (e.g., IBM's RS-1 robot and the Sigmarobot from Olivetti)

Cylindrical coordinates (two linear and one rotary axes) (e.g., Versatran 600 robotfrom Prab)

Spherical coordinates (one linear and two rotary axes) (e.g., Unimate 2000B fromUnimation Inc.)

Revolute or articulated coordinates (three rotary axes) (e.g., T3 from CincinnatiMilacron and PUMA from Unimation Inc.)

Most of today's industrial robots, though controlled by mini- and micro-computers, are'basically simple positional machines. They execute a given task by

Cartesian or xyz

Spherical

Cylindrical

Revolute

Figure 1.2 Various robot arm categories.

Cro

p.1

1'T

1

(3.

"-h

"C7

C1.

"CD

CD

"

"y,

7."

0

H+


playing back prerecorded or preprogrammed sequences of motions that have beenpreviously guided or taught by a user with a hand-held control-teach box. More-over, these robots are equipped with little or no external sensors for obtaining theinformation vital to its working environment. As a result, robots are used mainlyin relatively simple, repetitive tasks. More research effort is being directed towardimproving the overall performance of the manipulator systems, and one way isthrough the study of the various important areas covered in this book.

1.2 HISTORICAL DEVELOPMENT

The word robot was introduced into the English language in 1921 by the play-wright Karel Capek in his satirical drama, R.U.R. (Rossum's Universal Robots).In this work, robots are machines that resemble people, but work tirelessly. Ini-tially, the robots were manufactured for profit to replace human workers but,toward the end, the robots turned against their creators, annihilating the entirehuman race. Capek's play is largely responsible for some of the views popularlyheld about robots to this day, including the perception of robots as humanlikemachines endowed with intelligence and individual personalities. This image wasreinforced by the 1926 German robot film Metropolis, by the walking robot Elec-tro and his dog Sparko, displayed in 1939 at the New York World's Fair, and morerecently by the robot C3PO featured in the 1977 film Star Wars. Modern indus-trial robots certainly appear primitive when compared with the expectations createdby the communications media during the past six decades.

Early work leading to today's industrial robots can be traced to the periodimmediately following World War II. During the late 1940s research programswere started at the Oak Ridge and Argonne National Laboratories to developremotely controlled mechanical manipulators for handling radioactive materials.These systems were of the "master-slave" type, designed to reproduce faithfullyhand and arm motions made by a human operator. The master manipulator wasguided by the user through a sequence of motions, while the slave manipulatorduplicated the master unit as closely as possible. Later, force feedback was addedby mechanically coupling the motion of the master and slave units so that theoperator could feel the forces as they developed between the slave manipulator andits environment. In the mid-1950s the mechanical coupling was replaced by elec-tric and hydraulic power in manipulators such as General Electric's Handyman andthe Minotaur I built by General Mills.

The work on master-slave manipulators was quickly followed by more sophis-ticated systems capable of autonomous, repetitive operations. In the mid-1950sGeorge C. Devol developed a device he called a "programmed articulated transferdevice," a manipulator whose operation could be programmed (and thus changed)and which could follow a sequence of motion steps determined by the instructionsin the program. Further development of this concept by Devol and Joseph F.Engelberger led to the first industrial robot, introduced by Unimation Inc. in 1959.The key to this device was the use of a computer in conjunction with a manipula-

,I'

''.

`"T

r-.

2V"

'_'

5

1

't3

,YO

B,

INTRODUCTION 9

It is convenient to group these various areas of vision according to the sophis-tication involved in their implementation. We consider three levels of processing:low-, medium-, and high-level vision. While there are no clearcut boundariesbetween these subdivisions, they do provide a useful framework for categorizingthe various processes that are inherent components of a machine vision system. Inour discussion, we shall treat sensing and preprocessing as low-level vision func-tions. This will take us from the image formation process itself to compensationssuch as noise reduction, and finally to the extraction of primitive image featuressuch as intensity discontinuities. We will associate with medium-level vision thoseprocesses that extract, characterize, and label components in an image resultingfrom low-level vision. In terms of our six subdivisions, we will treat segmenta-tion, description, and recognition of individual objects as medium-level visionfunctions. High-level vision refers to processes that attempt to emulate cognition.The material in Chap. 7 deals with sensing, preprocessing, and with concepts andtechniques required to implement low-level vision functions. Topics in higher-levelvision are discussed in Chap. 8.

1.6 ROBOT PROGRAMMING LANGUAGES

One major obstacle in using manipulators as general-purpose assembly machines isthe lack of suitable and efficient communication between the user and the roboticsystem so that the user can direct the manipulator to accomplish a given task.There are several ways to communicate with a robot, and the three majorapproaches to achieve it are discrete word recognition, teach and playback, andhigh-level programming languages.

Current state-of-the-art speech recognition is quite primitive and generallyspeaker-dependent. It can recognize a set of discrete words from a limited voca-bulary and usually requires the user to pause between words. Although it is nowpossible to recognize words in real time due to faster computer components andefficient processing algorithms, the usefulness of discrete word recognition todescribe a task is limited. Moreover, it requires a large memory space to storespeech data, and it usually requires a training period to build up speech templatesfor recognition.

The method of teach and playback involves teaching the robot by leading itthrough the motions to be performed. This is usually accomplished in the follow-ing steps: (1) leading the robot in slow motion using manual control through theentire assembly task, with the joint angles of the robot at appropriate locationsbeing recorded in order to replay the motion; (2) editing and playing back thetaught motion; and (3) if the taught motion is correct, then the robot is run at anappropriate speed in a repetitive motion. This method is also known as guidingand is the most commonly used approach in present-day industrial robots.

A more general approach to solve the human-robot communication problem isthe use of high-level programming. Robots are commonly used in areas such asarc welding, spot welding, and paint spraying. These tasks require no interaction

Clo

g

'o .''

.-.

CG. Although Euler angles describe the orien-tation of a rigid body with respect to a fixed reference frame, there are manydifferent types of Euler angle representations. The three most widely used Eulerangles representations are tabulated in Table 2.1.

The first Euler angle representation in Table 2.1 is usually associated withgyroscopic motion. This representation is usually called the eulerian angles, andcorresponds to the following sequence of rotations (see Fig. 2.5):

Table 2.1 Three types of Euler angle representations

Eulerian angles Euler angles Roll, pitch, and yawsystem I system II system III

Sequence 0 about OZ axis 0 about OZ axis >

-e-

a.+

ww

ROBOT ARM KINEMATICS 23

Z, W

U,"

Figure 2.5 Eulerian angles system I.

1. A rotation of 0 angle about the OZ axis (R=,, )2. A rotation of 8 angle about the rotated OU axis (R,,, 0)3. Finally a rotation of 1 angle about the rotated OW axis (R,,,,

The resultant eulerian rotation matrix is

Rm, 0, 0 = RZ, , R11, 0 RIV, >G

Co - So 0 1 0 0 Ci1i - Si 0So Co 0 0 CO - SO S>1' Ci 00 0 1 0 Se CO 0 0 1

Corn - SOCOSO - CCS,i - socec,t SoseS4CO + WOS1i -Soso + CcCOCJ -CcSO

ses SOCV1 Co(2.2-17)

The above eulerian angle rotation matrix R, 0, >G can also be specified in termsof the rotations about the principal axes of the reference coordinate system: a rota-tion of 0 angle about the OZ axis followed by a rotation of 0 angle about the OXaxis and finally a rotation of 0 angle about the OZ axis.

With reference to Fig. 2.6, another set of Euler angles 0, 0, and ' representa-tion corresponds to the following sequence of rotations:

1. A rotation of 0 angle about the OZ axis (Rz. 4,)2. A rotation of 0 angle about the rotated OV axis (R,,,0)3. Finally a rotation of 0 angle about the rotated OW axis (R,,.,0)

mil

4-.

000

ate,


Z, W

X, U

Figure 2.6 Eulerian angles system II.

The resultant rotation matrix is

R0, e, V, = RZ,0R, aR,v,

Co -Ski 0so Co 0

0 0 1

ce 0 se

0 1 0

-so 0 co

co -so 0so co 00 0 1

cocec - soso - coCeW - soco COOsgCeco + COW -so COW + Coco Sose

- seal/ seso co(2.2-18)

The above Euler angle rotation matrix R0,B,0 can also be specified in terms ofthe rotations about the principal axes of the reference coordinate system: a rotationof 0 angle about the OZ axis followed by a rotation of 0 angle about the OY axisand finally a rotation of 0 angle about the OZ axis.

Another set of Euler angles representation for rotation is called roll, pitch, andyaw (RPY). This is mainly used in aeronautical engineering in the analysis ofspace vehicles. They correspond to the following rotations in sequence:

1. A rotation of 0 about the OX axis (R,,,0)-yaw2. A rotation of 0 about the OY axis (Ry, g)-pitch3. A rotation of 0 about the OZ axis (R_, 0)-roll

The resultant rotation matrix is

..t

mil

0'm O

..

L."i..

i..

Lo.


Z

Y

Figure 2.7 Roll, pitch and yaw.

R0,e,0 = Rz,0Ry,0RX,v =

co - So 0S0 co 00 0 1

Co 0 So

0 1 0

-so o co

1 0 0

0 C,, - S0 S>G co

Coco COW - scbc>G cgsoc> + SOW

Soco ScSOS,,& + cock sosoc - CcbS,c

-So cos, CBC,I(2.2-19)

The above rotation matrix R0 , 0 for roll, pitch, and yaw can be specified interms of the rotations about the principal axes of the reference coordinate systemand the rotating coordinate system: a rotation of 0 angle about the OZ axis fol-lowed by a rotation of o angle about the rotated OV axis and finally a rotation of 0angle about the rotated OU axis (see Fig. 2.7).

2.2.5 Geometric Interpretation of Rotation Matrices

It is worthwhile to interpret the basic rotation matrices geometrically. Let uschoose a point p fixed in the OUVW coordinate system to be (1, 0, 0)T, that is,

L. Then the first column of the rotation matrix represents the coordinatesof this point with respect to the OXYZ coordinate system. Similarly, choosing p tobe (0, 1, 0) T and (0, 0, 1)T, one can identify that the second- and third-columnelements of a rotation matrix represent the OV and OW axes, respectively, of theOUVW coordinate system with respect to the OXYZ coordinate system. Thus,given a reference frame OXYZ and a rotation matrix, the column vectors of therotation matrix represent the principal axes of the OUVW coordinate system withrespect to the reference frame and one can draw the location of all the principalaxes of the OUVW coordinate frame with respect to the reference frame. In otherwords, a rotation matrix geometrically represents the principal axes of the rotatedcoordinate system with respect to the reference coordinate system.

a-,s'"

:bin.-,

^T'


Since the inverse of a rotation matrix is equivalent to its transpose, the rowvectors of the rotation matrix represent the principal axes of the reference systemOXYZ with respect to the rotated coordinate system OUVW. This geometricinterpretation of the rotation matrices is an important concept that provides insightinto many robot arm kinematics problems. Several useful properties of rotationmatrices are listed as follows:

1. Each column vector of the rotation matrix is a representation of the rotated axisunit vector expressed in terms of the axis unit vectors of the reference frame,and each row vector is a representation of the axis unit vector of the referenceframe expressed in terms of the rotated axis unit vectors of the OUVW frame.

2. Since each row and column is a unit vector representation, the magnitude ofeach row and column should be equal to 1. This is a direct property of ortho-normal coordinate systems. Furthermore, the determinant of a rotation matrixis + 1 for a right-hand coordinate system and - 1 for a left-hand coordinatesystem.

3. Since each row is a vector representation of orthonormal vectors, the inner pro-duct (dot product) of each row with each other row equals zero. Similarly, theinner product of each column with each other column equals zero.

4. The inverse of a rotation matrix is the transpose of the rotation matrix.

R-1 = RT and RR T = 13

where 13 is a 3 x 3 identity matrix.

Properties 3 and 4 are especially useful in checking the results of rotation matrixmultiplications, and in determining an erroneous row or column vector.

Example: If the OU, OV, and OW coordinate axes were rotated with a angleabout the OX axis, what would the representation of the coordinate axes of thereference frame be in terms of the rotated coordinate system OUVW?

SOLUTION: The new coordinate axis unit vectors become i = (1, 0, 0)T,jv = (0, 1, 0)T, and k,v = (0, 0, 1)T since they are expressed in terms ofthemselves. The original unit vectors are then

ix = 1i1f + 0j + Ok,,, = (1, 0, 0)T

jy = Oi,, + cosaj,, - sinak,v = (0, cosa, - sina)T

kZ = Oi + sin aj + cos ak,,, = (0, since, cos a)T

Applying property 1 and considering these as rows of the rotation matrix, theRx,a matrix can be reconstructed as

cut

Sao

O.:

60

ova

0,o

CA

D


0 0

0 cos a sin a0 - sin a COS a

which is the same as the transpose of Eq. (2.2-12).

2.2.6 Homogeneous Coordinates and Transformation Matrix

Since a 3 x 3 rotation matrix does not give us any provision for translation andscaling, a fourth coordinate or component is introduced to a position vectorP = (px, py, pz)T in a three-dimensional space which makes it p = (wpx, wpy,wpz, W) T. We say that the position vector p is expressed in homogeneous coordi-nates. In this section, we use a "hat" (i.e., p) to indicate the representation of acartesian vector in homogeneous coordinates. Later, if no confusion exists, these"hats" will be lifted. The concept of a homogeneous-coordinate representation ofpoints in a three-dimensional euclidean space is useful in developing matrixtransformations that include rotation, translation, scaling, and perspective transfor-mation. In general, the representation of an N-component position vector by an(N+ 1)-component vector is called homogeneous coordinate representation. In ahomogeneous coordinate representation, the transformation of an N-dimensionalvector is performed in the (N + 1)-dimensional space, and the physical N-dimensional vector is obtained by dividing the homogeneous coordinates by the(N + 1)th coordinate, w. Thus, in a three-dimensional space, a position vectorp = (pX, Py, Pz)T is represented by an augmented vector (wpx, wpy, wpz, w)Tin the homogeneous coordinate representation. The physical coordinates arerelated to the homogeneous coordinates as follows:

x'Px WPy WPzPx =

w Py = w Pz = w

There is no unique homogeneous coordinates representation for a position vec-tor in the three-dimensional space. For example, p1 = (w, px, WI py, wt pz, wt )Tand 02 = (w, px, w2 py, W2 pz, w2 )T are all homogeneous coordinates representingthe same position vector p = (px, py, pz)T. Thus, one can view the the fourthcomponent of the homogeneous coordinates, w, as a scale factor. If this coordi-nate is unity (w = 1), then the transformed homogeneous coordinates of a positionvector are the same as the physical coordinates of the vector. In robotics applica-tions, this scale factor will always be equal to 1, although it is commonly used incomputer graphics as a universal scale factor taking on any positive values.

The homogeneous transformation matrix is a 4 x 4 matrix which maps a posi-tion vector expressed in homogeneous coordinates from one coordinate system toanother coordinate system. A homogeneous transformation matrix can be con-sidered to consist of four submatrices:

yam

.

28 ROBOTICS: CONTROL. SENSING, VISION, AND INTELLIGENCE

T=P3x1 '

lxi

rotation

matrix

perspective

transformation

positionvector

scaling

(2.2-20)

The upper left 3 x 3 submatrix represents the rotation matrix; the upper right3 x 1 submatrix represents the position vector of the origin of the rotated coordi-nate system with respect to the reference system; the lower left 1 x 3 submatrixrepresents perspective transformation; and the fourth diagonal element is the globalscaling factor. The homogeneous transformation matrix can be used to explain thegeometric relationship between the body attached frame OUVW and the referencecoordinate system OXYZ.

If a position vector p in a three-dimensional space is expressed in homogene-ous coordinates [i.e., p = (pX, pti pz, 1)T], then using the transformation matrixconcept, a 3 x 3 rotation matrix can be extended to a 4 x 4 homogeneoustransformation matrix Trot for pure rotation operations. Thus, Eqs. (2.2-12) and(2.2-13), expressed as homogeneous rotation matrices, become

1 0 0 0 cos cp 0 sin 0 0

0 cos a -sina 0 0 1 0 0TX,c, 0 sina cosa 0 TY'a = -sin o 0 cos 0 0

0 0 0 1 0 0 0 1

T,,0 =

cos 0 -sinB 0 0sinB cos 0 0 0

0 0 1 0

0 0 0 1

(2.2-21)

These 4 x 4 rotation matrices are called the basic homogeneous rotation matrices.The upper right 3 x 1 submatrix of the homogeneous transformation matrix

has the effect of translating the OUVW coordinate system which has axes parallelto the reference coordinate system OXYZ but whose origin is at (dx, dy, dz) of thereference coordinate system:

1 0 0 dx

0 1 0 dyTtran = 0 0 1 dz

(2.2-22)

0 0 0 1

This 4 x 4 transformation matrix is called the basic homogeneous translationmatrix.

f-+

CA

D"

,.1'

1-r

CC

'

(''

(DD

`--1

,

'LS


The lower left 1 x 3 submatrix of the homogeneous transformation matrixrepresents perspective transformation, which is useful for computer vision and thecalibration of camera models, as discussed in Chap. 7. In the present discussion,the elements of this submatrix are set to zero to indicate null perspective transfor-mation.

The principal diagonal elements of a homogeneous transformation matrix pro-duce local and global scaling. The first three diagonal elements produce localstretching or scaling, as in

a 0 0 0 x ax

0 b 0 0 y by(2 2-23)

0 0 c 0 z cz.

0 0 0 1 1 1

Thus, the coordinate values are stretched by the scalars a, b, and c, respectively.Note that the basic rotation matrices, Two do not produce any local scaling effect.

The fourth diagonal element produces global scaling as in

1 0 0 0

0 1 0 0

0 0 1 0

0 0 0 s

x

y

z

1

x

y

z

S

where s > 0. The physical cartesian coordinates of the vector are

x -px py 11Z w = = 1S s s s

(2.2-24)

(2.2-25)

Therefore, the fourth diagonal element in the homogeneous transformation matrixhas the effect of globally reducing the coordinates if s > 1 and of enlarging thecoordinates if 0 < s < 1.

In summary, a 4 X 4 homogeneous transformation matrix maps a vectorexpressed in homogeneous coordinates with respect to the OUVW coordinate sys-tem to the reference coordinate system OXYZ. That is, with w = 1,

pxyz = Tpuv1>>

and

T =

nx sx ax px

nY sy. ay Pynz sz az pz

0 0 0 1

n s a p

0 0 0 1

(2.2-26a)

(2.2-26b)

0CO

C

BC

D

'"'

'-h

Coo

III

`3'

coo

CD

O

'-'

s-.

-.ps..

.'

,-+

ten.'a.

30 ROBOTICS: CONTROL, SENSING. VISION, AND INTELLIGENCE

2.2.7 Geometric Interpretation of HomogeneousTransformation Matrices

In general, a homogeneous transformation matrix for a three-dimensional spacecan be represented as in Eq. (2.2-26b). Let us choose a point p fixed in the OUVWcoordinate system and expressed in homogeneous coordinates as (0, 0, 0, 1) T; thatis, p,,ti.,,, is the origin of the OUVW coordinate system. Then the upper right 3 x 1submatrix indicates the position of the origin of the OUVW frame with respect tothe OXYZ reference coordinate frame. Next, let us choose the point p to be(1, 0, 0 1)T; that is i,,. Furthermore, we assume that the origins of bothcoordinate systems coincide at a point 0. This has the effect of making the ele-ments in the upper right 3 x 1 submatrix a null vector. Then the first column (orn vector) of the homogeneous transformation matrix represents the coordinates ofthe OU axis of OUVW with respect to the OXYZ coordinate system. Similarly,choosing p to be (0, 1, 0, 1)T and (0, 0, 1, 1)T, one can identify that thesecond-column (or s vector) and third-column (or a vector) elements of the homo-geneous transformation matrix represent the OV and OW axes, respectively, of theOUVW coordinate system with respect to the reference coordinate system. Thus,given a reference frame OXYZ and a homogeneous transformation matrix T, thecolumn vectors of the rotation submatrix represent the principal axes of the OUVWcoordinate system with respect to the reference coordinate frame, and one candraw the orientation of all the principal axes of the OUVW coordinate frame withrespect to the reference coordinate frame. The fourth-column vector of the homo-geneous transformation matrix represents the position of the origin of the OUVWcoordinate system with respect to the reference system. In other words, a homo-geneous transformation matrix geometrically represents the location of a rotatedcoordinate system (position and orientation) with respect to a reference coordinatesystem.

Since the inverse of a rotation submatrix is equivalent to its transpose, the rowvectors of a rotation submatrix represent the principal axes of the reference coordi-nate system with respect to the rotated coordinate system OUVW. However, theinverse of a homogeneous transformation matrix is not equivalent to its transpose.The position of the origin of the reference coordinate system with respect to theOUVW coordinate system can only be found after the inverse of the homogeneoustransformation matrix is determined. In general, the inverse of a homogeneoustransformation matrix can be found to be

T-' =

nx ny. nz -nTPSx Sy. Sz -STpax ay az -aTP0 0 0 1

_ TP83x3 -STp

-aTP0 0 0 1

(2.2-27)

Thus, from Eq. (2.2-27), the column vectors of the inverse of a homogeneoustransformation matrix represent the principal axes of the reference system withrespect to the rotated coordinate system OUVW, and the upper right 3 x 1 subma-

.--'

t3.

'^w

.-.

may

00-

.-+


trix represents the position of the origin of the reference frame with respect to theOUVW system. This geometric interpretation of the homogeneous transformationmatrices is an important concept used frequently throughout this book.

2.2.8 Composite Homogeneous Transformation Matrix

The homogeneous rotation and translation matrices can be multiplied together toobtain a composite homogeneous transformation matrix (we shall call it the Tmatrix). However, since matrix multiplication is not commutative, careful atten-tion must be paid to the order in which these matrices are multiplied. The follow-ing rules are useful for finding a composite homogeneous transformation matrix:

1. Initially both coordinate systems are coincident, hence the homogeneoustransformation matrix is a 4 x 4 identity matrix, 14-

2. If the rotating coordinate system OUVW is rotating/translating about the princi-pal axes of the OXYZ frame, then premultiply the previous (resultant) homo-geneous transformation matrix with an appropriate basic homogeneous rotation/translation matrix.

3. If the rotating coordinate system OUVW is rotating/translating about its ownprincipal axes, then postmultiply the previous (resultant) homogeneous transfor-mation matrix with an appropriate basic homogeneous rotation/translationmatrix.

Example: Two points a,,,,,v = (4, 3, 2)T and (6, 2, 4)T are to betranslated a distance + 5 units along the OX axis and - 3 units along the OZaxis. Using the appropriate homogeneous transformation matrix, determinethe new points a,,,, and b,,y,.

SOLUTION:

key, =

4

,3

2

1

1 0 0

0 1 0

0 110 1

0 0 0

5

0

-3

1

6

2

4

The translated points are axyZ = (9, 3, -1)T and bXy. = (11, 2, 1)T.

1 0 0 5

0 1 0 0

0 0 1 -30 0 0 1

Example: A T matrix is to be determined that represents a rotation of aangle about the OX axis, followed by a translation of b units along the rotatedOV axis.

R..

''Y

>e'

...

L=

.

32 ROBOTICS- CONTROL, SENSING, VISION, AND INTELLIGENCE

SOLUTION: This problem can be tricky but illustrates some of the fundamentalcomponents of the T matrix. Two approaches will be utilized, an unorthodoxapproach which is illustrative, and the orthodox approach, which is simpler.After the rotation T.,,,,, the rotated OV axis is (in terms of the unit vectorsi.,, jy, k, of the reference system) j, = cos ajy + sin akz; i.e., column 2 ofEq. (2.2-21). Thus, a translation along the rotated OV axis of b units is b j _bcosajy + bsin cakZ. So the T matrix is

1 0 0 0 1 0 0 0

0 1 0 bc osa 0 cos a - since 0T = Tv, b T x, a = 0 0 1 b s in a 0 sina cosa 0

0 0 0 1 0 0 0 1

1 0 0 0

0 cos a - sina b cos a0 sina cosa b sin a0 0 0 1

In the orthodox approach, following the rules as stated earlier, one shouldrealize that since the T.,,,, matrix will rotate the OY axis to the OV axis, thentranslation along the OV axis will accomplish the same goal, that is,

1 0 0 0 1 0 0 0

0 cos a - since 0 0 1 0 bT = Tx, a Tv, b =

0 sina cosa 0 0 0 1 00 0 0 1 0 0 0 1

1 0 0 0

0 cos a -sina b cos a0 sina cosa b sin a0 0 0 1

Example: Find a homogeneous transformation matrix T that represents a rota-tion of a angle about the OX axis, followed by a translation of a units alongthe OX axis, followed by a translation of d units along the OZ axis, followedby a rotation of 0 angle about the OZ axis.

SOLUTION:

T = T

"C3

s.,

>~'

.-,

R OBOT ARM KI NEMATICS 33

cos 0 -sin O 0 0 1 0 0 0 1 0 0 a 1 0 0 0sin 0 cos 0 0 0 0 1 0 0 0 1 0 0 0 cosa - since 0

0 0 1 0 0 0 1 d 0 0 1 0 0 sina cosa 00 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1

cos 0 -cosa sin 0 sina sin 0 a cos 0sin 0 cos a cos 0. - sin a cos 0 a sin 0

0 sina cosa d0 0 0 1

We have identified two coordinate systems, the fixed reference coordinateframe OXYZ and the moving (translating and rotating) coordinate frame OUVW.To describe the spatial displacement relationship between these two coordinate sys-tems, a 4 x 4 homogeneous transformation matrix is used. Homogeneous transfor-mation matrices have the combined effect of rotation, translation, perspective, andglobal scaling when operating on position vectors expressed in homogeneous coor-dinates.

If these two coordinate systems are assigned to each link of a robot arm, saylink i - 1 and link i, respectively, then the link i - 1 coordinate system is thereference coordinate system and the link i coordinate system is the moving coordi-nate system, when joint i is activated. Using the T matrix, we can specify a pointpi at rest in link i and expressed in the link i (or OUVW) coordinate system interms of the link i - 1 (or OXYZ) coordinate system as

Pi-I = TPi

where

(2.2-28)

T = 4 x 4 homogeneous transformation matrix relating the two coordinatesystems

pi = 4 x 1 augmented position vector (xi, yi, zi, 1)T representing a pointin the link i coordinate system expressed in homogeneous coordinates

pi- I = is the 4 x 1 augmented position vector (xi_ I, yi_ I, z_1, 1)Trepresenting the same point pi in terms of the link i - 1 coordinatesystem

2.2.9 Links, Joints, and Their Parameters

A mechanical manipulator consists of a sequence of rigid bodies, called links, con-nected by either revolute or prismatic joints (see Fig. 2.8). Each joint-link pairconstitutes 1 degree of freedom. Hence, for an N degree of freedom manipulator,there are N joint-link pairs with link 0 (not considered part of the robot) attachedto a supporting base where an inertial coordinate frame is usually established forthis dynamic system, and the last link is attached with a tool. The joints and links

.._.

CA

D

5;.

CA

D

cam

',

7c'

1r-


Figure 2.8 A PUMA robot arm illustrating joints and links.

are numbered outwardly from the base; thus, joint 1 is the point of connectionbetween link 1 and the supporting base. Each link is connected to, at most, twoothers so that no closed loops are formed.

In general, two links are connected by a lower pair joint which has two sur-faces sliding over one another while remaining in contact. Only six differentlower-pair joints are possible: revolute (rotary), prismatic (sliding), cylindrical,spherical, screw, and planar (see Fig. 2.9). Of these, only rotary and prismaticjoints are common in manipulators.

A joint axis (for joint i) is established at the connection of two links (see Fig.2.10). This joint axis will have two normals connected to it, one for each of thelinks. The relative position of two such connected links (link i - 1 and link i) isgiven by di which is the distance measured along the joint axis between the nor-mals. The joint angle Oi between the normals is measured in a plane normal to thejoint axis. Hence, di and Oi may be called the distance and the angle between theadjacent links, respectively. They determine the relative position of neighboringlinks.

A link i (i = 1, ... , 6 ) is connected to, at most, two other links (e.g., linki - 1 and link i + 1); thus, two joint axes are established at both ends of the con-nection. The significance of links, from a kinematic perspective, is that they main-tain a fixed configuration between their joints which can be characterized by two


Revolute

Cylindrical

Spherical

Figure 2.9 The lower pair.

Planar

Prismatic

Screw

Figure 2.10 Link coordinate system and its parameters.

parameters: ai and a;. The parameter ai is the shortest distance measured alongthe common normal between the joint axes (i.e., the z, _ 1 and zi axes for joint iand joint i + 1, respectively), and a; is the angle between the joint axes measuredin a plane perpendicular to ai. Thus, ai and ai may be called the length and thetwist angle of the link i, respectively. They determine the structure of link i.

'"r

p..

`C1

Coo

o-'

C'fi,

S>G

C = tan-IJ L J

(2.3-1 la)

(2.3-11b)

= atan2 ( - Cps, - SOsy, COn,, + Son,,) (2.3-12)

Equating the (2, 3) and (3, 3) elements of the both matrices, we have:

SO = Sg ax - Ccba,

CO = az (2.3-13)

which gives us the solution for 0,

Scbax - Ccba y.6=tan-1 8 = tan - a J

= atan2 (SOa,, - CCa y, az)

(2.3-14)

Since the concept of inverse transform technique is to move one unknown tothe LHS of the matrix equation at a time and solve for the unknown, we can try tosolve the above matrix equation for cp, 0, 1 by postmultiplying the above matrixequation by its inverse transform R,v, 1

Ci so 0- So co 0

0 0 1

Multiplying the matrices out, we have,

nxCi1i - s,S%1i n,S% + s,C0 a,,

nyc> - syS'1G n,,S1 + syC ayn,Ci/i - s,S0 nzS' + s,C0 az

r - Cq s_,. - Scbsy 1

Co - So 0 1 0 0

so Co 0 0 co - S6

0 0 1 0 S6 co

Co -SoC6 SoS6

So CoC6 - CoS6

0 S6co(2.3-15)

Again equating the (3, 1) elements of both matrices in the above matrix equation,we have

nzCVi - s,S1i = 0 (2.3-16)

Con, + Sony

00


which gives

Equating the (3, 2) and (3, 3) elements of both matrices, we have:

SO = nZSI/i + szCt/i

CO = aZ

which leads us to the solution for 0,

0 = tan-'sz

= atan2 (nZ, sz) (2.3-17)

(2.3-18a)

0=tan ' nZS>/i + sZCt/i 1I. az

(2.3-18b)

= atan2 (nzSt' + szCtk, az) (2.3-19)

Equating the (1, 1) and (2, 1) elements of both matrices, we have

CO = nxC 1, - sxS>/i

SO = n yC>/i - s yS>G

which gives

nyCo-sySnxCi/i - sxSi/i J

(2.3-20a)

(2.3-20b)

atan2 (n_,C1 - nxC/i - (2.3-21)

Whether one should premuitiply or postmultiply a given matrix equation is up tothe user's discretion and it depends on the intuition of the user.

Let us apply this inverse transform technique to solve the Euler angles for aPUMA robot arm (OAT solution of a PUMA robot). PUMA robots use the sym-bols 0, A, T to indicate the Euler angles and their definitions are given as follows(with reference to Fig. 2.17):

O (orientation) is the angle formed from the yo axis to the projection of the tool aaxis on the XY plane about the zo axis.

A (altitude) is the angle formed from the XY plane to the tool a axis about the saxis of the tool.

T (tool) is the angle formed from the XY plane to the tool s axis about the a axisof the tool.

Initially the tool coordinate system (or the hand coordinate system) is alignedwith the base coordinate system of the robot as shown in Fig. 2.18. That is, whenO = A = T = 0 , the hand points in the negative yo axis with the fingers in ahorizontal plane, and the s axis is pointing to the positive x0 axis. The necessary

0 = tan - I


0, a measurement of theangle formed betweenthe WORLD Y axis anda projection of theTOOL Z on the WORLDXY plane

A, a measurement of the angleTOOL Z formed between the TOOL Z

and a plane parallel to theWORLD XY plane

T, a measurement of the angleformed between the TOOL Yand a plane parallel to theWORLD XY plane

Figure 2.17 Definition of Euler angles 0, A, and T. (Taken from PUMA robot manual398H.)

transform that describes the orientation of the hand coordinate system (n, s, a)with respect to the base coordinate system (xo, yo, zo) is given by

0 1 0

0 0 - 1 (2.3-22)

-1 0 0

t`'

r-,


ai

xo

Yo

Figure 2.18 Initial alignment of tool coordinate system.

From the definition of the OAT angles and the initial alignment matrix [Eq. (2.3-22)], the relationship between the hand transform and the OAT angle is given by

7nx sX ax

RZ. o

0 1 0

0 0 -1

-1 0 0

RS, A Ra, Tny sy ay

nZ sZ aZj

CO - SO 0

SO CO 0

0 0 1

0 1 0

0 0 -1

-1 0 0

CA 0 SA

0 1 0

- SA 0 CA

Postmultiplying the above matrix equation by the inverse transform of Ra, T,

CT ST 0

- ST CT 00 0 1

x

and multiplying the matrices out, we have:

nXCT - sxST nXST + sXCT ax

nyCT - syST nyST + syCT ay

nZCT - sZST nZST + s.CT aZ

CO - SO 0

SO CO 0

0 0 1

CT - ST 0ST CT 0

0 0 1

CA 0 SA

0 1 0

- SA 0 CA

0 1 0

0 0 -1

-1 0 0

- SOSA CO SOCA

COSA SO - COCA

-CA 0 -SA

(2.3-23)

Equating the (3, 2) elements of the above matrix equation, we have:

zo

nZST + sZCT = 0 (2.3-24)

-Q.

C].

N=

-


which gives the solution of T,

S

T = tan-' = atan2 (s, -nt) (2.3-25)- nZ


and

CA = -n,CT + sZST (2.3-26b)

SA = -as (2.3-26a)

then the above equations give

A = tan - I - az = atan2 ( -as, -nZCT + sZST) (2.3-27)-nzCT + sZST J


CO = nXST + sCT (2.3-28a)

SO = nyST + syCT (2.3-28b)

which give the solution of 0,

n>,ST + syCT

n,ST + sECT

= atan2 (nyST + syCT, nXST + ssCT) (2.3-29)

The above premultiplying or postmultiplying of the unknown inverse transformscan also be applied to find the joint solution of a PUMA robot. Details about thePUMA robot arm joint solution can be found in Paul et al. [1981].

Although the inverse transform technique provides a general approach indetermining the joint solution of a manipulator, it does not give a clear indicationon how to select an appropriate solution from the several possible solutions for aparticular arm configuration. This has to rely on the user's geometric intuition.Thus, a geometric approach is more useful in deriving a consistent joint-anglesolution given the arm matrix as in Eq. (2.2-39), and it provides a means for theuser to select a unique solution for a particular arm configuration. This approachis presented in Sec. 2.3.2.

2.3.2 A Geometric Approach

This section presents a geometric approach to solving the inverse kinematics prob-lem of six-link manipulators with rotary joints. The discussion focuses on aPUMA-like manipulator. Based on the link coordinate systems and human arm

ms`

s-'

0..

row

,_h

0O

1 0 + 1 + 1

DOWN < 0 + 1 -1UP >1 0 -1 -1UP < 0 -1 +1

Hip

a)-0

r-+

E-+


Again looking at the projection of the coordinate frame (x4, Y4, z4) on theX3 y3 plane and from Table 2.5 and Fig. 2.23, it can be shown that the followingare true (see Fig. 2.23):

sin 84 = -M(Z4 X3) COS04 = M(Z4 y3) (2.3-69)

where x3 and y3 are the x and y column vectors of T3, respectively, M =WRIST sign (9), and the sign function is defined as:

sign (x) _f+1 if x '> 0L-1 if x < 0

Thus, the solution for 84 with the orientation and WRIST indicators is:

84 = tan - I

= tan - I

F sin 84 1

cos 04 J

M(Clay - Slax)

M(CI C23ax + SI C23ay - S,3a,) J

(2.3-70)

- 7r < 04 < 7r (2.3-71)

If the degenerate case occurs, any convenient value may be chosen for 84 as longas the orientation of the wrist (UP/DOWN) is satisfied. This can always beensured by setting 84 equals to the current value of 04. In addition to this, the usercan turn on the FLIP toggle to obtain the other solution of 04, that is,04 = 84 + 1800.

Joint 5 solution. To find 85, we use the criterion that aligns the axis of rotation ofjoint 6 with the approach vector (or a = z5). Looking at the projection of the

sin B4 = -(z4 . x3)

cos 04= Z4 - Y3

X3

Figure 2.23 Solution for joint 4.

"'h

`N:

CD

'

yon

N/1

ran

..i


coordinate frame (x5, y5, z5) on the X4 Y4 plane, it can be shown that the followingare true (see Fig. 2.24):

sin B5 = a x4 cos 05 = - (a Y4) (2.3-72)

where x4 and y4 are the x and y column vectors of T4, respectively, and a is theapproach vector. Thus, the solution for 05 is:

05 = tan-1

= tan-I

C sin 05

LCOS 05 J- 7r < 05 < 7r

(CiC23C4-SiS4)ax+(SiC23C4+C1S4)ay-C4S23az

C1 S23ax+S1 S23ay+C23az

If 05 = 0, then the degenerate case occurs.

(2.3-73)

Joint 6 solution. Up to now, we have aligned the axis of joint 6 with the approachvector. Next, we need to align the orientation of the gripper to ease picking upthe object. The criterion for doing this is to set s = Y6. Looking at the projectionof the hand coordinate frame (n, s, a) on the x5 Y5 plane, it can be shown that thefollowing are true (see Fig. 2.25):

sin 06 = n y5 cos 06 = S y5 (2.3-74)

where y5 is the y column vector of T5 and n and s are the normal and slidingvectors of T6, respectively. Thus, the solution for 06 is:

06 = tan-]

= tan'

sin BS = a x4

cos B5 = - (a Y4)

r sin 06

COS 06- 7r ' < 06 < 7r

(-S1C4-C1C23S4)nx+(CIC4-SiC23S4)ny+(S4S23)nz(-SIC4-C1C23S4)Sx+(CIC4-S1C23S4)SY+(S4S23)sz J

(2.3-75)

Y4


''d

w-+

'J'

r/,

L..

U4'

p,;

'C3

A..

,--.

,O`1

.

a)"

b0.0

own

CJ"

C).


sin 06 =n - ys

cos 06 = S ys

x5


The above derivation of the inverse kinematics solution of a PUMA robot arm isbased on the geometric interpretation of the position of the endpoint of link 3 andthe hand (or tool) orientation requirement. There is one pitfall in the abovederivation for 04, 05, and 86. The criterion for setting the axis of motion of joint 5equal to the cross product of z3 and a may not be valid when sin 85 = 0, whichmeans that 05 = 0. In this case, the manipulator becomes degenerate with boththe axes of motion of joints 4 and 6 aligned. In this state, only the sum of 04 and06 is significant. If the degenerate case occurs, then we are free to choose anyvalue for 04, and usually its current value is used and then we would like to have84 + 86 equal to the total angle required to align the sliding vector s and the nor-mal vector n. If the FLIP toggle is on (i.e., FLIP = 1), then 04 = 04 + ir, 85 =

85, and 06 = 86 + 7r .In summary, there are eight solutions to the inverse kinematics problem of a

six joint PUMA-like robot arm. The first three joint solution (81, 82, 03) positionsthe arm while the last three-joint solution, (04, 05, 06), provides appropriate orien-tation for the hand. There are four solutions for the first three joint solutions-twofor the right shoulder arm configuration and two for the left shoulder armconfiguration. For each arm configuration, Eqs. (2.3-47), (2.3-56), (2.3-63), (2.3-71), (2.3-73), and (2.3-75) give one set of solutions (01, 82, 83, 04, 05, 86) and(81, 02, 03, 04 + ir, - 85, 86 + ir) (with the FLIP toggle on) gives another set ofsolutions.

Decision Equations for the Arm Configuration Indicators. The solution for thePUMA-like robot arm derived in the previous section is not unique and dependson the arm configuration indicators specified by the user. These arm configurationindicators (ARM, ELBOW, and WRIST) can also be determined from the jointangles. In this section, we derive the respective decision equation for each armconfiguration indicator. The signed value of the decision equation (positive, zero,or negative) provides an indication of the arm configuration as defined in Eqs.(2.3-31) to (2.3-33).

'77

'C1

'"b

000


For the ARM indicator, following the definition of the RIGHT/LEFT arm, adecision equation for the ARM indicator can be found to be:

i .1 kZI x p,

1

g(0,p) = z0.11zl X P'll =

z0 - sin 01 cos 01 0Ikzl X p'I!

PX Py 0

- py sin 01 - pX cos 01

Hzl X p'll(2.3-76)

where p' _ (P_" py., 0)T is the projection of the position vector p [Eq. (2.3-36)]onto the x0 y0 plane, zI = (- sin 01, cos 01, 0)T from the third column vector ofT1, and z0 = (0, 0, 1)T. We have the following possibilities:

1. If g(0, p) > 0, then the arm is in the RIGHT arm configuration.2. If g(0, p) < 0, then the arm is in the LEFT arm configuration.3. If g(0, p) = 0, then the criterion for finding the LEFT/RIGHT arm

configuration cannot be uniquely determined. The arm is within the innercylinder of radius d2 in the workspace (see Fig. 2.19). In this case, it isdefault to the RIGHT arm (ARM = + 1).

Since the denominator of the above decision equation is always positive, thedetermination of the LEFT/RIGHT arm configuration is reduced to checking thesign of the numerator of g(0, p):

ARM = sign [g(0, P)] = sign (- pX cos 01 - py sin 01) (2.3-77)

where the sign function is defined in Eq. (2.3-70). Substituting the x and y com-ponents of p from Eq. (2.3-36), Eq. (2.3-77) becomes:

ARM = sign [g(0, p)] = sign [g(0)] = sign ( -d4S23 -a3C23 - a2 C2) (2.3-78)

Hence, from the decision equation in Eq. (2.3-78), one can relate its signed valueto the ARM indicator for the RIGHT/LEFT arm configuration as:

-ARM = sign( -d4S23 -a3 C23 - a2 C2) + 1 RIGHT arm (2.3-79)- 1 LEFT armFor the ELBOW arm indicator, we follow the definition of ABOVE/BELOW

arm to formulate the corresponding decision equation. Using (2p4)y and the ARMindicator in Table 2.4, the decision equation for the ELBOW indicator is based onthe sign of the y component of the position vector of 2A3 3A4 and the ARM indica-tor:

'G1.


Joint anglesDirect kinematics

Position and orientationof the end-elector

n s a p

() I) () I

Error

Decision equations

ARM, ELBOW, WRIST

Inverse kinematics

Figure 2.26 Computer simulation of joint solution.

ELBOW = ARM sign (d4 C3 - a3 S3) =I-

+ 1 ELBOW above wrist (2 3-80)-1 ' ELBOW below wrist

For the WRIST indicator, we follow the definition of DOWN/UP wrist toobtain a positive dot product of the s and y5 (or z4 ) unit vectors:

WRIST =+1 if s z4 > 0-1 if s z4 < 0 = sign (s z4) (2.3-81)

If s z4 = 0, then the WRIST indicator can be found from:

WRIST =+1 if n - z4 > 0-1 if n - Z4 < 0 = sign (n z4) (2.3-82)

Combining Eqs. (2.3-81) and (2.3-82), we have

sign (s z4) if S Z4 0 + 1 WRIST DOWNWRIST = sign (n z4) if S z4 = 0 -1 =>WRIST UP

(2.3-83)

These decision equations provide a verification of the arm solution. We usethem to preset the arm configuration in the direct kinematics and then use the armconfiguration indicators to find the inverse kinematics solution (see Fig. 2.26).

Computer Simulation. A computer program can be written to verify the validityof the inverse solution of the PUMA robot arm shown in Fig. 2.11. The softwareinitially generates all the locations in the workspace of the robot within the jointangles limits. They are inputed into the direct kinematics routine to obtain the armmatrix T. These joint angles are also used to compute the decision equations toobtain the three arm configuration indicators. These indicators together with thearm matrix T are fed into the inverse solution routine to obtain the joint angle

(IQ

ono

-7,

7r0

-CD

CDN

tin

((D

D

:-t

CA

D

Q'5

.ti

,.C

O-,

'C)


solution which should agree to the joint angles fed into the direct kinematics rou-tine previously. A computer simulation block diagram is shown in Fig. 2.26.

2.4 CONCLUDING REMARKS

We have discussed both direct and inverse kinematics in this chapter. The param-eters of robot arm links and joints are defined and a 4 x 4 homogeneous transfor-mation matrix is introduced to describe the location of a link with respect to afixed coordinate frame. The forward kinematic equations for a six-axis PUMA-like robot arm are derived.

The inverse kinematics problem is introduced and the inverse transform tech-nique is used to determine the Euler angle solution. This technique can also beused to find the inverse solution of simple robots. However, it does not providegeometric insight to the problem. Thus, a geometric approach is introduced to findthe inverse solution of a six joint robot arm with rotary joints. The inverse solu-tion is determined with the assistance of three arm configuration indicators (ARM,ELBOW, and WRIST). There are eight solutions to a six joint PUMA-like robotarm-four solutions for the first three joints and for each arm configuration, twomore solutions for the last three joints. The validity of the forward and inversekinematics solution can be verified by computer simulation. The geometricapproach, with appropriate modification and adjustment, can be generalized toother simple industrial robots with rotary joints. The kinematics concepts coveredin this chapter will be used extensively in Chap. 3 for deriving the equations ofmotion that describe the dynamic behavior of a robot arm.

REFERENCES

Further reading on matrices can be found in Bellman [1970], Frazer et al. [1960],and Gantmacher [1959]. Utilization of matrices to describe the location of a rigidmechanical link can be found in the paper by Denavit and Hartenberg [1955] andin their book (Hartenberg and Denavit [1964]). Further reading about homogene-ous coordinates can be found in Duda and Hart [1973] and Newman and Sproull[1979]. The discussion on kinematics is an extension of a paper by Lee [1982].More discussion in kinematics can be found in Hartenberg and Denavit [1964] andSuh and Radcliffe [1978]. Although matrix representation of linkages presents asystematic approach to solving the forward kinematics problem, the vectorapproach to the kinematics problem presents a more concise representation of link-ages. This is discussed in a paper by Chase [1963]. Other robotics books thatdiscuss the kinematics problem are Paul [1981], Lee, Gonzalez, and Fu [1986],and Snyder [1985].

Pieper [1968] in his doctoral dissertation utilized an algebraic approach tosolve the inverse kinematics problem. The discussion of the inverse transformtechnique in finding the arm solution was based on the paper by Paul et al. [1981].The geometric approach to solving the inverse kinematics for a six-link manipula-

!'o

ivy m

-1

'0-

a0-+


for with rotary joints was based on the paper by Lee and Ziegler [1984]. The armsolution of a Stanford robot arm can be found in a report by Lewis [1974]. Othertechniques in solving the inverse kinematics can be found in articles by Denavit[1956], Kohli and Soni [1975], Yang and Freudenstein [1964], Yang [1969], Yuanand Freudenstein [1971], Duffy and Rooney [1975], Uicker et al. [1964]. Finally,the tutorial book edited by Lee, Gonzalez, and Fu [1986] contains numerous recentpapers on robotics.

PROBLEMS

2.1 What is the rotation matrix for a rotation of 30 about the OZ axis, followed by a rota-tion of 60 about the OX axis, followed by a rotation of 90 about the OY axis?2.2 What is the rotation matrix for a rotation of 0 angle about the OX axis, followed by arotation of >li angle about the OW axis, followed by a rotation of B angle about the OY axis?

2.3 Find another sequence of rotations that is different from Prob. 2.2, but which results inthe same rotation matrix.

2.4 Derive the formula for sin (0 + 0) and cos (0 + 0) by expanding symbolically tworotations of 0 and B using the rotation matrix concepts discussed in this chapter.

2.5 Determine a T matrix that represents a rotation of a angle about the OX axis, followedby a translation of b unit of distance along the OZ axis, followed by a rotation of angleabout the OV axis.

2.6 For the figure shown below, find the 4 x 4 homogeneous transformation matrices '-'Aiand A; for i = 1, 2, 3, 4, 5.

'-'

.J.

.^y

>,3

'pp


2,7 For the figure shown below, find the 4 x 4 homogeneous transformation matrices A;and A; for i = 1, 2, 3, 4.

4 in

2.8 A robot workstation has been set up with a TV camera, as shown in the example inSec. 2.2.11. The camera can see the origin of the base coordinate system where a six-linkrobot arm is attached, and also the center of a cube to be manipulated by the robot. If alocal coordinate system has been established at the center of the cube, then this object, asseen by the camera, can be represented by a homogeneous transformation matrix T1. Also,the origin of the base coordinate system as seen by the camera can be expressed by a homo-geneous transformation matrix T2, where

0 1 0 1 1 0 0 -101 0 0 10 0 -1 0 20

T = and =T, 0 0 -1 9 2 0 0 -1 100 0 0 0 0 0 1

(a) Unfortunately, after the equipment has been set up and these coordinate systems havebeen taken, someone rotates the camera 90 about the z axis o the camera. What is theposition/orientation of the camera with respect to the robots base coordinate system?(b) After you have calculated the answer for question (a), the same person rotated theobject 90 about the x axis of the object and translated it 4 units of distance along the

?D+

QC

-.

L1.


rotated y axis. What is the position/orientation of the object with respect to the robot's basecoordinate system? To the rotated camera coordinate system?

2.9 We have discussed a geometric approach for finding the inverse kinematic solution of aPUMA robot arm. Find the computational requirements of the joint solution in terms ofmultiplication and addition operations and the number of transcendental calls (if the sameterm appears twice, the computation should be counted once only).2.10 Establish orthonormal link coordinate systems (xi, y;, z,) for i = 1, 2, ... , 6 forthe PUMA 260 robot arm shown in the figure below and complete the table.

Waist rotation 330

Shoulder rotation 310

Flange

rotation360

Wristrotation

PUMA robot arm linkcoordinate parameters

Joint i 0, a; a, di

4

5

6

AB

C


2.11 Establish orthonormal link coordinate systems (xi, yi, zi) for i = 1, 2, ... , 5 forthe MINIMOVER robot arm shown in the figure below and complete the table.

MINIMOVER robot arm linkcoordinate parameters

Joint i Bi cxi ai di

5

yo

'2.12 A Stanford robot arm has moved to the position shown in the figure below. The jointvariables at this position are: q = (90, -120, 22 cni, 0, 70, 90)T. Establish theorthonormal link coordinate systems (xi, yi, zi) for i = 1, 2, ... ,6, for this arm andcomplete the table.

Stanford arm link coordinate

parameters

Joint i Oi ai ai di

2.13 Using the six "Ai matrices ( i = 1 , 2, ... , 6) of the PUMA robot arm in Fig.2.13, find its position error at the end of link 3 due to the measurement error of the firstthree joint angles (MB1, OB2, MB3). A first-order approximation solution is adequate.

2.14 Repeat Prob. 2.13 for the Stanford arm shown in Fig. 2.12.

.z.

C7.

.fl

bow

;-.


2.15 A two degree-of-freedom manipulator is shown in the figure below. Given that thelength of each link is 1 m, establish its link coordinate frames and find A, and 'A2. Findthe inverse kinematics solution for this manipulator.

'2.16 For the PUMA robot arm shown in Fig. 2.11, assume that we have found the firstthree joint solution (0,, 02, 03) correctly and that we are given '-'A;, i = 1, 2, ... , 6and T6. Use the inverse transformation technique to find the solution for the last threejoint angles (04, 05, 06). Compare your solution with the one given in Eqs. (2.3-71), (2.3-73), and (2.3-75).

2.17 For the Stanford robot arm shown in Fig. 2.12, derive the solution of the first threejoint angles. You may use any method that you feel comfortable with.

2.18 Repeat Prob. 2.16 for the Stanford arm shown in Fig. 2.12.

E3.

.:t7

`CD

C)'

.7"

Y',

t=

At'

',.'L

7

.'3

`/d

acs

pt-

p.,

..

CA

D.3

''

',.o

f7'

Obi

.,,r3

,..y

CHAPTER

THREE

ROBOT ARM DYNAMICS

The inevitable comes to pass by effort.Oliver Wendell Holmes

3.1 INTRODUCTION

Robot arm dynamics deals with the mathematical formulations of the equations ofrobot arm motion. The dynamic equations of motion of a manipulator are a set ofmathematical equations describing the dynamic behavior of the manipulator. Suchequations of motion are useful for computer simulation of the robot arm motion,the design of suitable control equations for a robot arm, and the evaluation of thekinematic design and structure of a robot arm. In this chapter, we shall concen-trate on the formulation, characteristics, and properties of the dynamic equations ofmotion that are suitable for control purposes. The purpose of manipulator controlis to maintain the dynamic response of a computer-based manipulator in accord-ance with some prespecified system performance and desired goals. In general,the dynamic performance of a manipulator directly depends on the efficiency of thecontrol algorithms and the dynamic model of the manipulator. The control prob-lem consists of obtaining dynamic models of the physical robot arm system andthen specifying corresponding control laws or strategies to achieve the desired sys-tem response and performance. This chapter deals mainly with the former part ofthe manipulator control problem; that is, modeling and evaluating the dynamicalproperties and behavior of computer-controlled robots.

The actual dynamic model of a robot arm can be obtained from known physi-cal laws such as the laws of newtonian mechanics and lagrangian mechanics. Thisleads to the development of the dynamic equations of motion for the various arti-culated joints of the manipulator in terms of specified geometric and inertialparameters of the links. Conventional approaches like the Lagrange-Euler (L-E)and Newton-Euler (N-E) formulations could then be applied systematically todevelop the actual robot arm motion equations. Various forms of robot armmotion equations describing the rigid-body robot arm dynamics are obtained fromthese two formulations, such as Uicker's Lagrange-Euler equations (Uicker [1965],Bejczy [1974]), Hollerbach's Recursive-Lagrange (R-L) equations (Hollerbach[1980]), Luh's Newton-Euler equations (Luh et al. [1980a]), and Lee's generalizedd'Alembert (G-D) equations (Lee et al. [1983]). These motion equations are"equivalent" to each other in the sense that they describe the dynamic behavior ofthe same physical robot manipulator. However, the structure of these equations82

Q,>

.fl

'=r

CA

D..Q

;D,

'.3"3

'`J

'C

3'Q

..

.fl

.-r

r+'

.fl

_00

CD

.

pr'

.^,

.-t

'-'..t

[17

(74

b1)

4)o">

c"4"

C's

S].,d,

-..a'-

.s6

(1)

s'O

("

(CS

ue.

',-','G

.+.N

.,ice..

ice.s0.

ROBOT ARM DYNAMICS 83

may differ as they are obtained for various reasons and purposes. Some areobtained to achieve fast computation time in evaluating the nominal joint torques inservoing a manipulator, others are obtained to facilitate control analysis and syn-thesis, and still others are obtained to improve computer simulation of robotmotion.

The derivation of the dynamic model of a manipulator based on the L-E for-mulation is simple and systematic. Assuming rigid body motion, the resultingequations of motion, excluding the dynamics of electronic control devices, back-lash, and gear friction, are a set of second-order coupled nonlinear differentialequations. Bejczy [1974], using the 4 x 4 homogeneous transformation matrixrepresentation of the kinematic chain and the lagrangian formulation, has shownthat the dynamic motion equations for a six joint Stanford robot arm are highlynonlinear and consist of inertia loading, coupling reaction forces between joints(Coriolis and centrifugal), and gravity loading effects. Furthermore, thesetorques/forces depend on the manipulator's physical parameters, instantaneous jointconfiguration, joint velocity and acceleration, and the load it is carrying. The L-Eequations of motion provide explicit state equations for robot dynamics and can beutilized to analyze and design advanced joint-variable space control strategies. Toa lesser extent, they are being used to solve for the forward dynamics problem,that is, given the desired torques/forces, the dynamic equations are used to solvefor the joint accelerations which are then integrated to solve for the generalizedcoordinates and their velocities; or for the inverse dynamics problem, that is, giventhe desired generalized coordinates and their first two time derivatives, the general-ized forces/torques are computed. In both cases, it may be required to computethe dynamic coefficients Dik, h,k,,,, and c, defined in Eqs. (3.2-31), (3.2-33), and(3.2-34), respectively. Unfortunately, the computation of these coefficientsrequires a fair amount of arithmetic operations. Thus, the L-E equations are verydifficult to utilize for real-time control purposes unless they are simplified.

As an alternative to deriving more efficient equations of motion, attention wasturned to develop efficient algorithms for computing the generalized forces/torquesbased on the N-E equations of motion (Armstrong [1979], Orin et al. [1979], Luhet al. [1980a]). The derivation is simple, but messy, and involves vector cross-product terms. The resulting dynamic equations, excluding the dynamics of thecontrol device, backlash, and gear friction, are a set of forward and backwardrecursive equations. This set of recursive equations can be applied to the robotlinks sequentially. The forward. recursion propagates kinematics information-suchas linear velocities, angular velocities, angular accelerations, and linear accelera-tions at the center of mass of each link-from the inertial coordinate frame to thehand coordinate frame. The backward recursion propagates the forces andmoments exerted on each link from the end-effector of the manipulator to the basereference frame. The most significant result of this formulation is that the compu-tation time of the generalized forces/torques is found linearly proportional to thenumber of joints of the robot arm and independent of the robot arm configuration.With this algorithm, one can implement simple real-time control of a robot arm inthe joint-variable space.

--]

CA

D

Cam

,

CA

D

BC

D

..S

^'.

-h

TV

''-s

D~

,

CA

D

Cam

'

CN

D

'-'

s-.C

13

s..

o'.

one

84 ROBOTICS. CONTROL, SENSING, VISION, AND INTELLIGENCE

The inefficiency of the L-E equations of motion arises partly from the 4 x 4homogeneous matrices describing the kinematic chain, while the efficiency of theN-E formulation is based on the vector formulation and its recursive nature. Tofurther improve the computation time of the lagrangian formulation, Hollerbach[1980] has exploited the recursive nature of the lagrangian formulation. However,the recursive equations destroy the "structure" of the dynamic model which isquite useful in providing insight for designing the controller in state space. Forstate-space control analysis, one would like to obtain an explicit set of closed-formdifferential equations (state equations) that describe the dynamic behavior of amanipulator. In addition, the interaction and coupling reaction forces in the equa-tions should be easily identified so that an appropriate controller can be designedto compensate for their effects (Huston and Kelly [1982]). Another approach forobtaining an efficient set of explicit equations of motion is based on the generalizedd'Alembert principle to derive the equations of motion which are expressed expli-citly in vector-matrix form suitable for control analysis. In addition to allowingfaster computation of the dynamic coefficients than the L-E equations of motion,the G-D equations of motion explicitly identify the contributions of the transla-tional and rotational effects of the links. Such information is useful for designinga controller in state space. The computational efficiency is achieved from a com-pact formulation using Euler transformation matrices (or rotation matrices) andrelative position vectors between joints.

In this chapter, the L-E, N-E, and G-D equations of robot arm motion arederived and discussed, and the motion equations of a two-link manipulator areworked out to illustrate the use of these equations. Since the computation of thedynamic coefficients of the equations of motion is important both in controlanalysis and computer simulation, the mathematical operations and their computa-tional issues for these motion equations are tabulated. The computation of theapplied forces/torques from the generalized d'Alembert equations of motion is oforder 0(n3), while the L-E equations are of order 0(n4) [or of order 0(n3) ifoptimized] and the N-E equations are of order 0(n), where n is the number ofdegrees of freedom of the robot arm.

3.2 LAGRANGE-EULER FORMULATION

The general motion equations of a manipulator can conveniently be expressedthrough the direct application of the Lagrange-Euler formulation to nonconserva-tive systems. Many investigators utilize the Denavit-Hartenberg matrix representa-tion to describe the spatial displacement between the neighboring link coordinateframes to obtain the link kinematic information, and they employ the lagrangiandynamics technique to derive the dynamic equations d a manipulator. The directapplication of the lagrangian dynamics formulation, together with the Denavit-Hartenberg link coordinate representation, results in a convenient and compactalgorithmic description of the manipulator equations of motion. The algorithm isexpressed by matrix operations and facilitates both analysis and computer imple-mentation. The evaluation of the dynamic and control equations in functionally

p-;

,_.

I0.

a0.

ROBOT ARM DYNAMICS 85

explicit terms will be based on the compact matrix algorithm derived in this sec-tion.

The derivation of the dynamic equations of an n degrees of freedom manipula-tor is based on the understanding of:

1. The 4 x 4 homogeneous coordinate transformation matrix, '-'A;, whichdescribes the spatial relationship between the ith and the (i -1)th link coordi-nate frames. It relates a point fixed in link i expressed in homogeneous coordi-nates with respect to the ith coordinate system to the (i -1)th coordinate sys-tem.

2. The Lagrange-Euler equation

d r aL 1 aL = T, i = 1 2 ... n (3.2-1), ,,dt L aqr J aq,

where

L =K =P =

qi =

q, _Ti =

lagrangian function = kinetic energy K - potential energy Ptotal kinetic energy of the robot armtotal potential energy of the robot armgeneralized coordinates of the robot armfirst time derivative of the generalized coordinate, q;generalized force (or torque) applied to the system at joint i to drivelink i

From the above Lagrange-Euler equation, one is required to properly choose a setof generalized coordinates to describe the system. Generalized coordinates areused as a convenient set of coordinates which completely describe the location(position and orientation) of a system with respect to a reference coordinate frame.For a simple manipulator with rotary-prismatic joints, various sets of generalizedcoordinates are available to describe the manipulator. However, since the angularpositions of the joints are readily available because they can be measured by poten-tiometers or encoders or other sensing devices, they provide a natural correspon-dence with the generalized coordinates. This, in effect, corresponds to the general-ized coordinates with the joint variable defined in each of the 4 x 4 link coordinatetransformation matrices. Thus, in the case of a rotary joint, qi = Bi, the jointangle span of the joint; whereas for a prismatic joint, q, = d;, the distance trav-eled by the joint.

The following derivation of the equations of motion of an n degrees of free-dom manipulator is based on the homogeneous coordinate transformation matricesdeveloped in Chap. 2.

3.2.1 Joint Velocities of a Robot Manipulator

The Lagrange-Euler formulation requires knowledge of the kinetic energy of thephysical system, which in turn requires knowledge of the velocity of each joint. In

'C3


this section, the velocity of a point fixed in link i will be derived and the effects ofthe motion of other joints on all the points in this link will be explored.

With reference to Fig. 3.1, let 'r; be a point fixed and at rest in a link i andexpressed in homogeneous coordinates with respect to the ith link coordinate

frame,

xi

Yi

zi

1

= (xi, Yi, zi, 1)

41828119-robotics.pdf

Documents