Theory of Applied Robotics Kinematics, Dynamics, and Control
Theory of Applied Robotics Kinematics, Dynamics, and Control
Reza N. lazar DepartmentofMechanical Engineering Manhattan College Riverdale, NY
~ Springer
Reza N. lazar Department of Mechanical Engineering Manhattan College Riverdale, NY 1047 1
Theory of Applied Robotics: Kinemat ics, Dynamics, and Control Library of Congress Control Number: 2006939285 ISBN-IO: 0-387-32475-5 ISBN-13: 978-0-387-32475-3
e-ISBN-IO : 0-387-68964-8 e-ISBN-13: 978-0-387-68964-7
Printed on acid-free paper.
© 2007 Springer Science+Business Media , LLC All rights reserved. This work may not be translated or copied in whole or in part without the written permission of the publisher (Springer Science-Business Media, LLC, 233 Spring Street, New York, NY 10013, USA), except for brief excerpts in connection with reviews or scholarly analysis. Use in connection with any form of information storage and retrieval, electronic adaptation , computer software , or by similar or dissimilar methodology now known or hereafter developed is forbidden. The use in this publication of trade names, trademarks, service marks and similar terms, even if they are not identified as such, is not to be taken as an expression of opinio n as to whethe r or not they are subject to proprietary rights. 9 8 7 6 5 432 1 springer.com
Dedicated to my wife, Mojgan and our children, Vazan and Kavosh and to my supportive father.
Everything depends on "What you like to do, what you have to do, and what you do." Happiness means "what you do is what you like to do." Success means "what you do is what you have to do."
Preface
This book is designed to serve as a text for engineering students. It introduces th e fundamental knowledge used in robotics. This knowledge can be utili zed to develop computer programs for analyzing the kinematic s, dynamics , and control of robo tic syste ms. Th e subject of robo tics may appear overdosed by the number of available texts because the field has been growing rapidl y since 1970. However, the topic rem ains alive with modern developments , which are closely relat ed to t he classical material. It is evident that no single text can cover the vast scope of classical and modern mat erials in robotics. Thus the demand for new books arises because t he field continues to progress. Another fact or is the t rend toward analyt ical unification of kinem ati cs, dyn amics, and cont rol. Classical kinematics and dyn amics of robots has its root s in the work of great scientists of the past four centuries who est ablished the methodology and und erstanding of the behavior of dynamic syst ems. The development of dynamic science, since th e beginning of the twenti eth cent ury, has moved toward an alysis of cont rollable man-m ade systems. Therefore, merging t he kinematics and dynamics with control theory is t he expected development for robotic analysis. The other important developmen t is th e fast growing cap abilit y of accurate and rapid num erical calculations, along with intelligent computer programming.
Level of the Book This book has evolved from nearly a decade of resear ch in nonlinear dynamic syste ms, and teaching undergraduate-gradu ate level courses in robotics. It is addressed prim arily to t he last year of undergradu ate st udy and the first year graduate student in engineering. Hence, it is an intermediat e textbook. Thi s book can even be th e first exposure to topics in spatial kinemat ics and dynamics of mechanical syste ms. Therefore, it provides both fundamental and advanced topi cs on the kinemati cs and dynamics of robots. The whole book can be covered in two successive courses however, it is possible to jump over some sections and cover t he book in one course. The students ar e required to know the fund amentals of kinem atic s and dynamics, as well as a basic knowledge of numerical methods.
x
Preface
Th e contents of th e book have been kept at a fairly theoretical-pract ical level. Many concepts are deeply explained and their use emphasized, and most of the relat ed th eory and formal proofs have been explained. Throughout th e book , a strong emphasis is put on the physical meaning of the concepts introduced. Topics that have been selected are of high interest in th e field. An at tempt has been made to expose th e students to a broad range of top ics and approaches.
Organization of the Book Th e text is organized so it can be used for teaching or for self-study. Chapter 1 "Int roduct ion," contains general preliminaries with a brief review of t he historical development and classification of robots . Part I "Kinematics," presents the forward and inverse kinematics of robots . Kinemati cs analysis refers to position , velocity, and accelerat ion analysis of robots in both joint and base coordinate spaces. It establishes kinemati c relations among the end-effecter and th e joint variables. The method of Denavit-Hartenberg for representing body coordinate frames is introduced and utilized for forward kinemati cs analysis. The concept of modular tre atment of robots is well covered to show how we may combine simple links to make the forward kinemati cs of a complex robot . For inverse kinematics analysis, th e idea of decoupling, th e inverse mat rix method, and the iterative technique are introduced. It is shown th at the presence of a spherical wrist is what we need to apply analytic methods in inverse kinematics. Part II "Dynamics," presents a det ailed discussion of robot dynamics. An attempt is made to review t he basic approaches and demonstrate how th ese can be adapted for the act ive displacement framework utilized for robo t kinemati cs in th e earlier chapte rs. The concepts of th e recursive Newton-Eul er dynamics, Lagrangian function, manipul ator inerti a matrix, and genera lized forces are introduced and applied for derivation of dynamic equations of motion. Part III "Cont rol," presents the floating time technique for tim e-optimal control of robots. Th e out come of the technique is applied for an openloop control algorithm. Then , a compute d-t orque method is int roduced, in which a combinat ion of feedforward and feedback signals are utilized to render the syste m error dyn amics. Method of Presentation Th e structure of present ation is in a "fact-reason-application" fashion . Th e "fact" is th e main subject we introduce in each sect ion. Th en the reason is given as a "proof." Fin ally the application of the fact is examined in some" examples." The "examples" are a very impor tant part of the book because the y show how to implement th e knowledge introdu ced in "facts." Th ey also cover some other facts that are needed to expand the subject .
Preface
xi
Prerequisites Since the book is written for senior undergradu ate and first-year graduate level students of engineering, the assumption is th at users are familiar with matrix algebra as well as basic feedback control. Prerequisit es for readers of this book consist of the fundamentals of kinematic s, dynamics, vector analysis, and matrix t heory. These basics are usually taught in the first three und ergradu at e years. Unit System Th e system of unit s adopted in this book is, unless otherwise stated, th e international system of units (SI). The units of degree (deg) or radian (rad) are ut ilized for variables representing angular quantities. Symbols • Lowercase bold letters indicate a vector. Vectors may be expressed in an n dimensional Euclidian space. Example: s q
d
a
p
V
w
b y
w
a
€
(J
~
r
c z cjJ
• Uppercase bold let ters indicat e a dynami c vector or a dynamic matri x, such as and Jacobian. Example: F
M
J
• Lowercase letters with a hat indicat e a unit vector . Unit vectors are not bolded. Example: i j
J j
k k
• Lowercase letters with a tilde indicat e a 3 x 3 skew symmet ric matrix associated to a vector. Example:
• An arr ow above two upp ercase letters indicat es th e start and end point s of a position vector. Example: --+
ON = a position vector from point 0 to point N
xii
Preface • A double arrow above a lowercase letter indicat es a 4 x 4 matrix associated to a quat ernion. Exampl e: qO -ql ql qo q2 q3 [ q3 -q2
f--+
q
q
=
qo + ql i
-q2 -q3 qo ql
-q3] q2 -ql qo
+ q2j + q3k
• The length of a vector is indicat ed by a non-bold lowercase letter . Example:
= [r]
r
a= [a]
b = [b]
s
= lsi
• Capital letters A, Q , R , and T indicat e rot at ion or t ransformation matrices. Example: - sin 0: cos 0:
COS 0:
Qz ,o: =
[
Si~ O:
o
0] o
,
GT
B
=
[ CO: 0 0 1 so: 0 0 0
1
- so:
o
co:
o
-1]
0.5 0.2
1
• Capital let ter B is ut ilized to denote a bod y coordinate frame. Exampl e:
B(oxy z)
B (Oxy z)
• Capital letter G is utilized to denote a global, inertial, or fixed coordin ate frame . Example: G(XYZ)
G
G(OXYZ)
• Right sub script on a transformation matrix indicates t he departu re frames. Example:
T B = transformation mat rix from frame B(ox yz) • Left sup erscript on a transformation matrix indicat es t he dest ination fram e. Exampl e:
GTB
t ransformation matrix from frame B(oxy z) to frame G(OXYZ)
• Whenever th ere is no sub or superscript , the matrices are shown in a bracket. Exampl e:
[T] =
[ C~ 0~ so:
o
0
-so:
o CO:
o
-1 ] 0.5 0.2 1
Preface xiii • Left superscript on a vector denotes the frame in which the vector is expressed . That superscript indicates the frame that the vector belongs to ; so the vector is expressed using the unit vectors of that frame . Example:
c r = position vector expressed in frame G(OXYZ) • Right subscript on a vector denotes the tip point that the vector is referred to . Example: Crp
position vector of point P expressed in coordinate frame G(OXYZ)
• Right subscript on an angular velocity vector indicat es the frame that the angular vector is referred to. Example: WB
= angular velocity of the body coordinate fram e B (oxy z )
• Left subscript on an angular velocity vector indicates the frame that the angular vector is measured with resp ect to . Example: CWB
angular velocity of the body coordinate fram e B(oxyz) with respect to the global coordinate frame G(OXYZ)
• Left superscript on an angular velocity vector denotes the frame in which the angular velocity is expressed. Example: angular velocity of the body coordinate frame B 1 with respect to the global coordinate frame G, and expressed in body coordinate frame B 2 Wh enever the subscript and superscript of an angular velocity are the same , we usually drop the left superscript. Example: CWB
_C
= CWB
Also for position, velocity, and accelerat ion vectors , we drop the left subscripts if it is the same as the left superscript. Example:
• If the right subscript on a force vector is a number, it indicates t he number of coordinate frames in a serial robot. Coordinate frame B, is set up at joint i + 1. Example:
F,
= force vector at joint i + 1 measured at the origin of Bi(oxyz)
xiv
Pr eface At joint i there is always an action force F i , t ha t link (i) applies on link (i + 1), and a reaction force - F i , that link (i + 1) applies on link (i) . On link (i) there is always an act ion force F i - 1 coming from link (i - 1), and a reaction force - F i coming from link (i + 1). Action force is called driving force, and reaction force is called driven force.
• If t he right subscript on a moment vector is a number , it indic ates the number of coordina t e frames in a serial robo t . Coordinate frame B, is set up at joint i + 1. Ex ample: M,
= mom ent
vect or at joint i+1 measured at t he origin of Bi(oxy z)
At joint i t here is always an act ion moment M j , t hat link (i) applies on link (i + 1), and a reaction moment - M i , that link (i + 1) applies on link (i) . On link (i) there is always an act ion mom ent M i - 1 coming from link (i -1) , and a react ion moment - M , coming from link (i + 1). Action moment is called driving moment , and reaction moment is called driven mome nt. • Left supe rscript on derivative operators indi cat es the fram e in which the derivative of a vari able is t aken . Ex ample:
Cd
- x dt If t he variable is a vector function , and also t he fram e in which the vector is defined is the same as the frame in which a time derivative is t aken , we may use t he following short not at ion,
and write equa t ions simpler. Ex ample:
• If followed by angles, lower case c and s denote cos and sin functions in mathemat ical equations. Example: co: = coso:
sip
= sin
• Capital bold letter I indic ates a unit matrix, which , dep ending on t he dimension of the matrix equat ion, could be a 3 x 3 or a 4 x 4 unit matrix. 13 or 14 are also being used to clarify the dim ension of 1. Example:
Preface
xv
*
indicat es a more advanced subj ect or example th at is • An asterisk not designed for und ergraduate teaching and can be dropped in th e first reading. • Two parallel joint axes are indicated by a par allel sign, (II) . • Two orthogonal joint axes are indicat ed by an ort hogonal sign, (f- ). Two ortho gonal joint axes are intersecting at a right angle. • Two perpendicular joint axes are indicated by a perp endicular sign, (..L). Two perp endicular joint axes are at a right angle with respect to t heir common normal.
Contents 1 Introduction 1.1 Historic al Development 1.2 Components and Mechanisms of a Robotic System 1.2.1 Link . . . .. 1.2.2 Joint 1.2.3 Manipulator . 1.2.4 Wrist . . . . 1.2.5 End-effector . 1.2.6 Actuators 1.2.7 Sensors . .. 1.2.8 Controller .. 1.3 Robot Classifications . 1.3.1 Geometry . 1.3.2 Worksp ace 1.3.3 Actuation 1.3.4 Control 1.3.5 Appli cation 1.4 Introduction to Robot 's Kinematics, Dynamics, and Control 1.4.1 Triad 1.4.2 Unit Vectors 1.4.3 Reference Frame and Coordinate System 1.4.4 Vector Function . . . 1.5 Problems of Robot Dynamics . . . . . 1.6 Preview of Covered Topics . . . . . . . 1.7 Robots as Multi-disciplinary Machines 1.8 Summary Exercis es
1 1 2 3 3 5 5 6 7 7 7 7 8 11 12 13 13 14 15 16 16 19 19 21 22 22 25
I
29
Kinematics
2 Rotation Kinematics 2.1 Rotation About Global Cartesian Axes . 2.2 Successive Rotation About Global Cartesian Axes 2.3 Global Roll-Pitch-Yaw Angles . . 2.4 Rotation About Local Cartesian Axes 2.5 Successive Rotation About Local Cartesian Axes
33 33 38 41 43 47
xviii Contents 2.6 Euler Angles 2.7 Local Roll-P itch-Yaw Angles 2.8 Local Axes Rotation Versus Global Axes Rotation 2.9 General Transformation . . . . . . 2.10 Active and Passive Transformation 2.11 Summary Exercises 3
Orientation K inematics 3.1 Axis-angle Rotation Euler Parameters . . . . . . . . . . 3.2 Determination of Euler Parameters 3.3 Quaternions . . . . . . . . . . . . . 3.4 Spinors and Rot ators . . . . . . . . 3.5 Problems in Representing Rotations . 3.6 3.6.1 Rotation matrix 3.6.2 Angle-axis . . 3.6.3 Euler angles . . . 3.6.4 Quat ernion . . . 3.6.5 Euler parameters Composition and Decomposition of Rot ations 3.7 3.8 Summary Exercises
* * * * * *
48 59 61 63 71 73 75 81 81 88 96 99 102 105 105 106 107 109 111 113 118 119
4 Motion Kinematics 4.1 Rigid Body Motion . . . . . . . . . . . 4.2 Homogeneous Transform ation . . . . . 4.3 Inverse Homogeneous Transformation 4.4 Compound Homogeneous Transformation Screw Coordin at es 4.5 Inverse Screw . . . . . . . . . . . 4.6 Compound Screw Transformation 4.7 The Plucker Line Coordinate . . . 4.8 The Geometry of Plane and Line 4.9 Moment . . . . . . 4.9.1 Angle and Distance . . 4.9.2 4.9.3 Plane and Line . . . . Screw and Plucker Coordinate 4.10 4.11 Summary Exercises
127 127 131 139 145 154 169 170 173 180 180 181 181 186 188 191
5 Forward K inematics 5.1 Denavit-Hartenberg Notation . . . . . . . . . . . . . . . .. 5.2 Transformation Between Two Adjacent Coordinate Frames 5.3 Forward Position Kinemat ics of Robots
199 199 208 226
*
* * * * *
* * *
Contents
* *
5.4 Coordinate Transformation Using Screws Sheth Method 5.5 5.6 Summary Exercises
xix 242 247 253 255
6 Inverse Kinematics 6.1 Decoupling Technique . . . . . . . 6.2 Inverse Transformation Technique 6.3 It erat ive Techniqu e . . . . . . . . . 6.4 Comparison of the Inverse Kinematic s Techniques Existence and Uniqueness of Solution . 6.4.1 6.4.2 Inverse Kinemati cs Techniques. Singular Configuration 6.5 6.6 Summary Exercises
263 263 270 282 287 287 288 289 291 293
7 Angular Velocity 7.1 Angular Velocity Vector and Matrix 7.2 T ime Derivative and Coordinat e Fram es 7.3 Rigid Body Velocity . . . . . . . . . . . 7.4 Velocity Transformation Matrix. . . . . 7.5 Derivative of a Homogeneous Transform ation Matrix 7.6 Summary Exercises
297 297 310 320 325 330 336 339
*
*
8
* *
Velocity Kinematics 8.1 Rigid Link Velocity . . . . . . . . . . . . . . . . . . . . 8.2 Forward Velocity Kinematic s and the Jacobian Matrix 8.3 Jacobian Generating Vector s 8.4 Inverse Velocity Kinem atics 8.5 Summary Exercises
9 Numerical Methods in Kinematics 9.1 Linear Algebraic Equ ations . . 9.2 Matrix Inversion . . . . . . . . . . 9.3 Nonlinear Algebraic Equations . . Jacobian Matrix From Link Transformation Matrices 9.4 Kinematics Recursive Equ ations . . . . . . . . 9.5 9.5.1 Recursive Velocity in Base Frame . . . 9.5.2 Recursive Acceleration in Base Frame . 9.6 Summ ary Exercis es
* *
* *
343 343 346 351 363 368 371 375 375 388 395 402 410 410 412 412 413
xx
Contents
II
Dynamics
417
10 Acceleration Kinematics 10.1 Angu lar Acceleration Vector and Matrix 10.2 Rigid Body Acceleration . . . . . . . 10.3 Acceleration Transformation Matrix 10.4 Forward Acceleration Kinematics . 10.5 Inverse Acceleration Kinematics 10.6 Summary Exercises
421 421 429 432 435 437 440 443
11 Motion Dynamics 11.1 Force and Moment . . . . . . . . . 11.2 Rigid Body Translational Kinet ics 11.3 Rigid Body Rotational Kinetics . . 11.4 Mass Moment of Inertia Matrix . . 11.5 Lagrange's Form of Newton 's Equations of Motion 11.6 Lagrangian Mechanics 11.7 Summary Exercises
447 447 453 455 466 478 487 492 495
12 Robot Dynamics 12.1 Rigid Link Recursive Acceleration 12.2 Rigid Link Newton-Euler Dynamics 12.3 Recursive Newton-Euler Dynamics . 12.4 Robot Lagrange Dynamics . . . . . . Lagrange Equations and Link Transformation Matrices 12.5 12.6 Robot Statics 12.7 Summary Exercises .. . ..
505 505 509 520 528 534 544 551 557
III
565
*
*
Control
13 Path Planning 13.1 Joint Cubic Path . 13.2 Higher Polynomial Path . . . . 13.3 Non-Polynomial Path Planning 13.4 Manipulator Motion by Joint Path 13.5 Cartesian Pat h . Rotational Path . . . . . . . . . 13.6 13.7 Mani pulator Motion by En d-Effector Path . 13.8 Summary Exercises .
*
569 569 576 587 589 590 595 598 599 601
Contents xxi 14
* * *
Time Optimal Control 14.1 Minimum Time and Bang-Bang Contro l Floating Time Method . 14.2 Time-Opt imal Contro l for Robots 14.3 14.4 Summary Exercises .
*
607 607 616 627 633 635
15 Control Techniques 15.1 Open and Closed-Loop Control 15.2 Computed Torque Contro l . . 15.3 Linear Control Technique . . 15.3.1 Proportional Contro l . 15.3.2 Integral Control . 15.3.3 Derivative Contro l . 15.4 Sensing and Control . . .. 15.4.1 Position Sensors.. 15.4.2 Speed Sensors. . . 15.4.3 Acceleration Sensors. . 15.5 Summary Exercises .
641 641 647 652 652 653 653 655 656 657 657 658 661
References
664
A Global Frame Triple Rotation
675
B Local Frame Triple Rotation
677
C Principal C entral Screws Triple Combination
679
D Trigonometric Formula
681
Index
685
1 Introduction Law Zero: A robot may not injur e humanity, or, through inaction , allow humanity to come to harm . Law One: A robot may not injure a human being, or, through inaction , allow a human being to come to harm , unless this would violate a higher order law. Law Two: A robot must obey orders given it by human beings, except where such orders would conflict with a higher order law. Law Three: A robot must prot ect its own existence as long as such protection does not conflict with a higher order law. Isaac Asimov propos ed th ese four refined laws of "robotics" to protect us from intelligent generations of robots. Although we are not too far from that tim e when we really do need to appl y Asimov's rules, th ere is no immediate need however, it is good to have a plan. The term robotics refers to the study and use of robots. The term was first adopted by Asimov in 1941 th rough his short story, Run around. Based on the Robotics Institute of America (RIA) definition: "A robot is a reprogrammable multifunctional manipulator designed to move mat erial, parts, tools, or specialized devices th rough variable programmed motions for the performance of a variety of tasks." From th e engineering point of view, robots are complex, versatile devices th at contain a mechanical structure, a sensory system, and an automatic control system. Theoret ical fundament als of robotics rely on th e result s of research in mechanics, electric, electronics, automatic control, mathematics , and computer sciences.
1.1 Historical Development Th e first position controlling apparat us was invented around 1938 for spray painting. However , the first industrial modern robots were the Unimat es, made by J . Engelberger in the early 60s. Unimat ion was the first to market robots. Therefore, Engelberger has been called th e fath er of robotics. In th e 80s th e robot industry grew very fast prim arily because of th e huge investm ent s by th e automotive industry. In th e research community th e first automat a were probably Grey Walter's machina (1940s) and the John's Hopkins beast. Th e first programmable robot was designed by George Devol in 1954. Devol funded Unimation .
2
1. Introduction
The theory of Denavit and Hartenberg developed in 1955 and unified the forward kinematics of robotic manipulators. In 1959 th e first commercially available robo t appeared on the market . Robo tic manipulat ors were used in industries after 1960, and saw sky rocketing growt h in the 80s. Robots appeared as a result of combination two technologies: teleoperators, and compute r numerical cont rol (CNC) of milling machines. Teleoperat ors were developed during World War II to handl e radioacti ve mat erials , and CNC was developed to increase the precision required in machinin g of new technologic parts. Therefore, th e first robots were nothing but numerical cont rol of mechanical linkages that were basically designed to transfer mat erial from point A to B. Today, more complicated appli cations, such as welding, painting, and assembling, requ ire much more motion cap abilit y and sensing. Hence, a robo t is a multi-disciplinary engineering device. Mechani cal engineering deals with the design of mechanical components, arms, end-effectors, and also is responsible for kinem at ics, dyn amics and cont rol analyses of robots. Electri cal engineering works on robo t actuators, sensors, power , and control syste ms. System design engineering deals wit h perception, sensing, and cont rol methods of robo ts. P rogramming, or software engineering, is responsible for logic, intelligence, communication , and network ing. Today we have more than 1000 robotics-relat ed organi zations, associations, and clubs; more than 500 robotics-relat ed magazines, journals, and newslet ters; more t ha n 100 robotics-relat ed conferences, and compet itions each year; and more t ha n 50 robot ics-related courses in colleges. Robots find a vast amount indus tri al applicat ions and are used for various te chnological operations . Rob ots enha nce labo r producti vity in industry and deliver relief from tiresome, monotonous, or hazard ous works. Moreover, rob ots perform many oper ations bet ter than people do, and they provide higher accuracy and repeatability. In many fields, high technological standards are hardl y at tainable without rob ots. Apart from indust ry, robots are used in ext reme environments. They can work at low and high temp erat ures; they don 't even need lights, rest , fresh air, a salary, or promotions. Robots are prospective machines whose application area is widenin g. It is claimed t hat robots appeared to perform in 4A for 4D, or 3D3H environments. 4A performances are automat ion, augmentation, assistance, and autonomous; and 4D environments are dangerous, dirty, dull , and difficult . 3D3H means dull , dirty, dangerous , hot , heavy, and hazardous.
1.2
Components and Mechanisms of a Robotic System
Robotic manipulato rs are kinematic ally composed of links connected by joints to form a kinemati c chain. However, a robot as a system , consists
1. Introduction
3
J
FIGURE 1.1. A two-loop planar linkage with 7 links and 8 revolute joints. of a manipulator or rover, a wrist , an end-effector, actuators, sensors, controllers, processors, and software .
1.2.1 Link The individual rigid bodies that make up a robot are called links . In robotics we sometimes use arm to mean link. A robot arm or a robot link is a rigid member that may have relative motion with respect to all other links . From the kinematic point of view, two or more members connected together such that no relative motion can occur among them are considered a single link.
Example 1 Number of links . Figure 1.1 shows a mechanism with 7 links. There can not be any relative motion among bars 3, 10, and 11. Hence, they are counted as one link, say link 3. Bars 6, 12, and 13 have the same situation and are counted as one link, say link 6. Bars 2 and 8 are rigidly attached, making one link only, say link 2. Bars 3 and 9 have the same relationship as bars 2 and 8, and they are also one link, say link 3.
1.2.2
Joint
Two links are connected by contact at a joint where their relative motion can be expressed by a single coordinate. Joints are typically revolute (rotary) or prismatic (linear) . Figure 1.2 depicts the geometric form of a revolute and a prismatic joint. A revolute joint (R), is like a hinge and allows relative rotation between two links . A prismatic joint (P), allows a translation of relative motion between two links. Relative rotation of connected links by a revolute joint occurs about a line called axis of joint. Also, translation of two connected links by a prismatic joint occurs along a line also called axis of joint. The value of
4
1. Introduction
Revolute
Prismatic
FIGURE 1.2. Illustration of revolute and prismatic joints.
(a)
(b)
(c)
FIGURE 1.3. Symbolic illustration of revolute joints in robotic modeles. the single coordinate describing the relative position of two connected links at a joint is called joint coordinate or joint variable. It is an angle for a revolute joint, and a distance for a prismatic joint. A symbolic illustration of revolute and prismatic joints in robotics are shown in Figure 1.3 (a)-(c) , and 1.4 (a)-(c) respectively. The coordinate of an active joint is controlled by an actuator. A passive joint does not have any actuators and its coordinate is a function of the coordinates of active joints and the geometry of the robot arms . Passive joints are also called ina ctive or free joints. Active joints are usually prismatic or revolute , however, passive joints may be any of the lower pair joints that provide surface contact. There ar e six different lower pair joints: revolute, prismatic, cylindrical, screw, spherical, and planar. Revolute and prismatic joints are the most common joints that are utilized in serial robotic manipulators. The other joint types are merely implementations to achieve the same function or provide additional degrees of freedom . Prismatic and revolute joints provide one degree of freedom . Therefore, the number of joints of a manipulator is the degrees-of-freedom (DOF) of the manipulator. Typically the manipulator should possess at least six DOF: three for positioning and three for orientation. A manipula-
1. Introduction
(a)
(b)
5
(c)
FIGURE 1.4. Symbolic illustration of prismatic joints in robotic models.
Shoulder
Base FIGURE 1.5. Illust ation of a 3R manipulator.
tor having more than six DOF is referred to as a kinematically redundant manipulator.
1.2.3
Manipulator
The main body of a robot consist ing of the links, joints, and other st ruct ural elements, is called the ma nipulator. A manipulator becomes a robot when the wrist and gripper ar e attached, and the cont rol system is implement ed. However , in literature robo ts and manipulators are utilized equivalentl y and both refer to robots. Figure 1.5 schematically illustrat es a 3R manipulato r.
1.2.4
Wrist
The joints in the kinematic chain of a robot between the forebeam and endeffector ar e referr ed to as the wrist. It is common to design manipulators
6
1. Introduction
Attachedtoforearm FIGURE 1.6. Illustration of a spherical wrist kinematics.
with spherical wrists, by which it means three revolute joint axes intersect at a common point called the wrist point. Figure 1.6 shows a schematic illustration of a spherical wrist, which is a Rf--Rf--R mechanism. The spherical wrist greatly simplifies the kinematic analysis effectively, allowing us to decouple the positioning and orienting of the end effector. Therefore, the manipulator will possess three degrees-of-freedom for position, which are produced by three joints in the arm. The numb er of DOF for orientation will then depend on the wrist . We may design a wrist having one, two, or three DOF depending on the application.
1.2.5 End-effector The end-effector is the part mounted on the last link to do the required job of the robot. The simplest end-effector is a gripper, which is usually capable of only two actions: opening and closing. The arm and wrist assemblies of a robot are used primarily for positioning the end-effector and any tool it may carry. It is the end-effector or tool that actually performs the work. A great deal of research is devoted to the design of special purpose endeffectors and tools. There is also extensive research on the development of anthropomorphic hands. Such hands have been developed for prosthetic use in manufacturing. Hence, a robot is composed of a manipulator or mainframe and a wrist plus a tool. The wrist and end-effector assembly is also called a hand.
1. Introduction
7
1.2.6 Actuators Actuators are drivers acting as the muscles of robots to change their configuration. The actuators provide power to act on the mechanical structure against gravity, inertia, and other external forces to modify the geometric location of the robot 's hand. The actuators can be of electric, hydraulic, or pneumatic type and have to be controllable.
1.2.1 Sensors The elements used for detecting and collecting information about internal and environmental states are sensors. According to the scope of this book, joint position, velocity, acceleration, and force are the most important information to be sensed. Sensors, integrated into the robot , send information about each link and joint to the control unit, and the control unit determines the configuration of the robot.
1.2.8
Controller
The controller or control unit has three roles. I-Information role, which consists of collecting and processing the information provided by the robot's sensors . 2-Decision role, which consists of planning the geometric motion of the robot structure. 3- Communication role, which consists of organizing the information between the robot and its environment. The control unit includes the processor and software.
1.3 Robot Classifications The Robotics Institute of America (RIA) considers classes 3-6 of the following classification to be robots, and the Association Francaise de Robotique (AFR) combines classes 2, 3, and 4 as the same type and divides robots in 4 types. However, the Japanese Industrial Robot Association divides robots in 6 different classes: Class 1: Manual handling devices : A device with multi degrees offreedom that is actuated by an operator. Class 2: Fixed sequence robot: A device that performs the successive stages of a task according to a predetermined and fixed program. Class 3: Variable sequence robot: A device that performs the successive stages of a task according to a predetermined but programmable method. Class 4: Playback robot: A human operator performs the task manually by leading the robot, which records the motions for later playback. The robot repeats the same motions according to the recorded information.
8
1. Introduction
Class 5: Numerical control robot: The operator supplies the robot with a motion progr am rather than teaching it th e task manually. Class 6: Intelligent robot : A robot with th e ability to understand its environment and the ability to successfully complete a task despit e cha nges in the surrounding conditions under which it is to be performed. Oth er th an th ese official classifications , robots can be classified by oth er crit eria such as geometry, workspace, act uat ion, cont rol, and applicat ion.
1. 3.1
Geometry
A robot is called a serial or open-loop manipulator if its kinemati c struct ure does not make a loop chain. It is called a parallel or closed-loop manipulator if its struct ure makes a loop chain . A robot is a hybrid manipulator if its structure consist s of both open and closed-loop chains. As a mechanical system, we may think of a robot as a set of rigid bodies connected togeth er at some joints. Th e joints can be either revolute (R) or prismatic (P) , because any oth er kind of joint can be modeled as a combinat ion of th ese two simple joints . Most industrial manipul ators have six DOF . Th e open-loop manipulators can be classified based on th eir first three joint s starting from th e grounded joint . From the two types of joints there are mathemati cally 72 different manipulator configurat ions, simply because each joint can be P or R, and the axes of two adjacent joint s can be parallel (11) , orthogonal (f-), or perpendicular (1.). Two orthogonal joint axes intersect at a right angle, however two perpendicular joint axes are in right-angle with respect to their common normal. Two perpendicular joint axes become parallel if one axis turns 90 deg about th e common normal. Two perpendicular joint axes become ort hogonal if the length of their common normal tends to zero. Out of the 72 possible manipulators, the important ones are : R IIRIIP (SCARA) , Rf-R1.R (articulat ed) , Rf-R1.P (spherical) , RIIPf-P (cylindrical), and Pf-Pf-P (Cartesian). 1. RIIRIIP Th e SCARA arm (Selective Compliant Arti culat ed Robot for Assembly) shown in Figur e 1.7 is a popul ar manipul ator, which, as its name suggests, is made for assembly operations. 2. Rf-R.lR The Rf-R1.R configuration, illustrat ed in Figure 1.5, is called elbow, revolut e, articulat ed, or anthropomorphic. It is a suit able configurat ion for industrial robots. Almost 25% of industrial robots, PUMA for inst ance, are made of this kind. Because of its importance, a better illustration of an art iculated robot is shown in Figure 1.8 to indicate th e name of different components.
1. Introduction
9
d
Base FIGURE 1.7. Illustration of an RIIRIIP man ipulator .
FIGURE 1.8. Structure and terminology of a Rf-K1R elbow manipulator equipped with a spherical wrist .
10
1. Introduction
FIGURE 1.9. T he RI-Rl.P spherica l configuration of robot ic manipulat ors.
FIG URE 1.10. Illustration of Stanford arm; an RI-R.lP spherical manipulator.
3. RI-R..lP The spherical configuration is a suitable configuration for small robots. Almost 15% of industrial robots, St anford arm for instance, ar e made of this configuration. Th e Rf-R..lP configuration is illustrat ed in Figure 1.9. By replacing the third joint of an art iculate manipulator with a prismatic joint, we obtain the spherical manipulator. The term spherical manipulator derives from the fact that the spherical coordinates define the position of t he end-effect or with respect to its base frame . Figure 1.10 schematically illustrates the St anford arm, one of the most well-known spherical robots.
1. Introduction
11
FIGURE 1.11. The RIIP.iP configuration of robotic manipulators. 4. RIIPf--P
The cylindrical configurat ion is a suitable configuration for medium load capacity robots. Almost 45% of industrial robots are made of this kind . The RIIPf--P configurati on is illustrat ed in Figure 1.11. The first joint of a cylindrical manipulator is revolute and produces a rot ation about the base, while the second and third joints are prism atic. As t he name suggests, the joint variables are th e cylindrical coordinates of t he end-effector with respect to th e base. 5. Pf--Pf--P
The Cart esian configurati on is a suitable configurat ion for heavy load capacity and lar ge robots. Almost 15% of industrial robots are mad e of this configuration. The Pf--Pf--P configuration is illust rated in Figure 1.12. For a Cartesian manipulator, t he joint variables are the Cartesian coordinat es of t he end-effector with respect to th e base . As might be expected, the kinematic description of thi s manipulator is th e simplest of all manipulators. Cartesian manipulators are useful for table-top assembly applications and, as gantry robo ts , for t ransfer of cargo.
1.3.2
Workspace
The workspace of a manipulator is t he to tal volume of spac e the end-effector can reach. Th e workspace is const rained by the geometry of the manipulator as well as the mechani cal const raints on the joints. The workspace is broken into a reachable workspace and a dext erous workspace. The reachable workspace is the volume of space within which every point is reachable
12
1. Introduction
FIGURE 1.12. The PrPrP Cartesian configur ation of robotic manipulators.
by the end-effector in at least one orientation. The dexterous workspace is the volume of space within which every point can be reached by the endeffector in all possible orientations. The dexterous workspace is a subset of the reachable workspace . Most of the open-loop chain manipulators are designed with a wrist subassembly attached to the main three links assembly. Therefore, the first three links are long and are utiliz ed for positioning while the wrist is utilized for control and orientation of the end-effector. This is why the subassembly made by the first three links is called the arm, and the subassembly made by the other links is called the wrist.
1.3.3 Actuation Actuators translate power into motion. Robots are typically actuated electrically, hydraulically, or pneumatically. Other types of actuation might be considered as piezoelectric, magnetostriction, shape memory alloy, and polym eric. Electrically actuated robots are powered by AC or DC motors and are considered the most acceptable robo ts. They are cleaner, quieter, and more precise compared to the hydraulic and pneumatic actuated. Electric motors are efficient at high speeds so a high ratio gearbox is needed to reduce the high RPM . Non-backdriveability and self-braking is an advantage of high ratio gearboxes in case of power loss. However, when high speed or high load-carrying capabilities are needed , electric drivers are unable to compete with hydraulic drivers . Hydraulic actuators ar e satisfactory because of high speed and high torque/mass or power /mass ratios. Therefore, hydraulic driven robots are used primarily for lifting heavy loads . Negative asp ects of hydraulics, besides their noisiness and tendency to leak, include a necessary pump and other hardware.
1. Introduction
13
Pneumatic actuated robots are inexpensive and simple but cannot be controlled precisely. Besides the lower precise motion , they have almost the same advantages and disadvantages as hydraulic actuated robots.
1.3.4
Control
Robots can be classified by control method into servo (closed loop control) and non-servo (open loop control) robots. Servo robots use closed-loop computer control to determine their motion and are thus capable of being truly multifunctional reprogrammable devices. Servo controlled robots are further classified according to the method that the controller uses to guide the end-effector . The simplest type of a servo robot is the point-to-point robot. A pointto-point robot can be taught a discrete set of points, called control points, but there is no control on the path of the end-effector in between the points. On the other hand, in continuous path robots, the entire path of the end-effector can be controlled. For example , the robot end-effector can be taught to follow a straight line between two points or even to follow a contour such as a welding seam. In addition, the velocity and /or acceleration of the end-effector can often be controlled. These are the most advanced robots and require the most sophisticated computer controllers and software development. Non-servo robots are essentially open-loop devices whose movement is limited to predetermined mechanical stops , and they are primarily used for materials transfer.
1.3.5
Application
Regardless of size, robots can mainly be classified according to their application into assembly and non-assembly robots. However, in the industry they are classified by the category of application such as machine loading, pick and place, welding , painting, assembling, inspecting, sampling, manufacturing, biomedical, assisting, remote controlled mobile , and telerobot. According to design characteristics, most industrial robot arms are anthropomorphic, in the sense that they have a "shoulder," (first two joints) an "elbow," (third joint) and a "wrist" (last three joints) . Therefore , in total, they usually have six degrees of freedom needed to put an object in any position and orientation. Most commercial serial manipulators have only revolute joints. Compared to prismatic joints, revolute joints cost less and provide a larger dextrous workspace for the same robot volume. Serial robots are very heavy, compared to the maximum load they can move without loosing their accuracy. Their usefulload-to-weight ratio is less than 1/10. The robots are so heavy because the links must be stiff in order to work rigidly. Simplicity of the forward and inverse position and velocity kinematics has always been
14
1. Introduction
one of the major design criteria for industrial manipulators. Hence, almost all of them have a special kinematic structure.
1.4 Introduction to Robot's Kinematics, Dynamics, and Control The forward kinematics problem is when the kinematical data are known for the joint coordinates and are utilized to find the data in the base coordinate frame . The inverse kinematics problem is when the kinematics data are known for the end-effecter in Cartesian space. Inverse kinematics is highly nonlinear and usually a much more difficult problem than the forward kinematics problem . The inverse velocity and acceleration problems are linear, and much simpler, once the inverse position problem has been solved. An inverse position solution is said to have a closed form if it is not iterative. Kinematics, which is the English version of the French word cinemaiique from the Greek K,ivTJIUX (movement), is a branch of science that analyzes motion with no attention to what causes the motion . By motion we mean any type of displacement, which includes changes in position and orientation. Therefore, displacement , and the successive derivatives with respect to time , velocity, acceleration, and jerk , all combine into kinematics. Positioning is to bring the end-effector to an arbitrary point within dextrose , while orientation is to move the end-effector to the required orientation at the position. The positioning is the job of th e arm, and orientation is the job of the wrist . To simplify the kinematic analysis , we may decouple the positioning and orientation of the end-effector. In terms of the kinematic formation, a 6 DOF robot comprises six sequential moveable links and six joints with at least the last two links having zero length. Generally speaking, almost all problems of kinematics can be interpreted as a vector addition. However, every vector in a vectorial equation must be transformed and expressed in a common reference frame. Dynamics is the study of systems that undergo changes of state as time evolves. In mechanical systems such as robots, the change of states involves motion. Derivation of the equations of motion for the system is the main step in dynamic analysis of the system, since equations of motion are essential in the design, analysis, and control of the system. The dynamic equations of motion describe dynamic behavior. They can be used for computer simulation of the robot's motion, design of suitable control equations, and evaluation of the dynamic performance of the design. Similar to kinematics, the problem of robot dynamics may be considered as direct and inverse dynamics problems . In direct dynamics, we should predict the motion of the robot for a given set of initial conditions and
1. Introduction
15
torques at active joints. In the inverse dynamics problem, we should compute the forces and torques necessary to generate the prescribed trajectory for a given set of positions, velocities, and accelerations. The robot control problem may be characterized as the desired motion of the end-effector. Such a desired motion is specified as a trajectory in Cartesian coordinates while the control system requires input in joint coordinates. Sensors generate data to find the actual state of the robot at joint space. This implies a requirement for expressing the kinematic variables in Cartesian space to be transformed into their equivalent joint coordinate space. These transformations are highly dependent on the kinematic geometry of the manipulator. Hence, the robot control comprises three computational problems : 1- Determination of the trajectory in Cartesian coordinate space, 2- Transformation of the Cartesian trajectory into equivalent joint coordinate space, and 3- Generation of the motor torque commands to realize the trajectory.
1.4.1
Triad
Take any four non-coplanar points 0, A, B, C. The triad OABC is defined as consisting of the three lines OA, OB, OC forming a rigid body. The position of A on OA is immaterial provided it is maintained on the same side of 0, and similarly Band C. Rotate OB about 0 in the plane OAB so that the angle AOB becomes 90deg, the direction of rotation of OB being such that OB moves through an angle less than 90deg. Next , rotate OC about the line in AOB to which it is perpendicular, until it becomes perpendicular to the plane AO B, in such a way that OC moves through an angle less than 90deg. Calling now the new position of OABC a triad, we say it is an orthogonal triad derived by continuous deformation. Any orthogonal triad can be superposed on the OABC. Given an orthogonal triad OABC , another triad OA'BC may be derived by moving A to the other side of 0 to make the opposite triad OA' BC. All orthogonal triads can be superposed either on a given orthogonal triad OABC or on its opposite OA'BC. One of the two triads OABC and OA' BC is defined as being a positive triad and used as a standard. The other is then defined as negative triad. It is immaterial which one is chosen as positive, however, usually the right-handed convention is chosen as positive, the one for which the direction of rotation from OA to OB propels a right-handed screw in the direction OC . A right-handed (positive) orthogonal triad cannot be superposed to a left-handed (negative) triad. Thus there are just two essentially distinct types of triad. This is an essential property of three-dimensional space.
16
1. Introduction
1.4.2
Unit Vectors
An orthogonal triad made of unit vectors i , i. k is a set of three unit vectors whose directions form a positive orthogonal triad. From this definition,
i
=1
(1.1)
Moreover , since j x k is parallel to and in the same sense as i , by definition of the vector product we have
)xk=i
(1.2)
and similarly
kx i
)
(1.3)
i X)
i:
(1.4)
i·)
0
(1.5)
)·k
0
(1.6)
k· i
0
(1.7)
=
Further
and
(Jxk) .i=i .i=+1.
(1.8)
Any vector r may be put in the form
r = (r · i)i + (r ·))J + (r · k)k.
(1.9)
Vector addition is the key operation in kinematics. However, special attention must be taken since vectors can be added only when they are expressed in the same frame. Thus, a vector equation such as a=b+c
(1.10)
is meaningless without indicating the frame they are expressed in, such that
(1.11)
1.4.3 Reference Frame and Coordinate System In robotics, we assign one or more coordinate frames to each link of the robot and each object of the robot's environment. Thus, communication among the coordinate frames , which is called transformation of frames, is a fundamental concept in the modeling and programming of a robot. The angular motion of a rigid body can be described in one of several ways, the most popular being:
1. Introduction
17
1. A set of rotations about a right-h and ed globally fixed Cartes ian axis, 2. A set of rot at ions about a right-h and ed moving Cart esian axis, and 3. Angular rotation about a fixed axis in space. Reference frames are a par ticular perspective employed by the analyst to describ e the motion of links. A fixed f ram e is a reference frame that is motionless and attached to the ground. The motion of a robot takes place in a fixed frame called t he global reference f ram e. A movin g f ram e is a reference frame that moves with a link. Every moving link has an attached reference frame t hat sticks to the link and accepts every motion of the link. The moving reference frame is called th e local reference fram e. T he position and orientation of a link with respect to th e ground is explained by th e position and orient ation of its local reference frame in the global reference frame. In robotic analysis, we fix a global reference frame to the ground and attach a local reference frame to every single link. A coordinate system is slightly different from reference frames. The coordinate system det ermin es th e way we describe th e motion in each reference frame. A Cart esian system is th e most popular coordinate system used in robotics, but cylindri cal, spherical and other systems may be used as well. Hereafter, we use" reference frame," "coord inate frame," and "coordinate system" equivalently, because a Cartes ian syste m is th e only syste m we use. The position of a point P of a rigid body B is indicat ed by a vector r. As shown in Figure 1.13, t he position vector of P can be decomposed in global coordinate frame Gr
=
xi + y j + zk
(1.12)
or in body coordinate frame Br
= x i + yj + ei:
(1.13)
The coefficients (X , Y, Z) and (x , y , z) are called coordinates or com pon ents of th e point P in global and local coordinate frames respectively. It is efficient for mathemati cal calculations to show vectors Grand vertical array made by its components
Br
by a
(1.14)
(1.15) Kinemati cs can be called t he st udy of positi ons, velocities, and accelerations, with out regard s to the forces th at cause t hese motions. Vectors and
18
1. Introduction
z z
y
~ ------------------------------------------------------------_b/-/ FIGURE 1.13. Position vector of a point P may be decomposed in body frame B , or global frame G. reference frames are essential tools for analyzing motions of complex systems, especially when the motion is three dimensional and involves many parts. A coordinate frame is defined by a set of basis vectors, such as unit vectors along the three coordinate axes. So, a rotation matrix, as a coordinate transformation, can also be viewed as defining a change of basis from one frame to another. A rotation matrix can be interpreted in three distinct ways: 1. Mapping. It represents a coordinate transformation, mapping and relating the coordinates of a point P in two different frames.
2. Description of a frame . It gives the orientation of a transformed coordinate frame with respect to a fixed coordinate frame . 3. Operator. It is an operator taking a vector and rotating it to a new vector. Rotation of a rigid body can be described by rotation matrix R, Euler angles, angle-axis convention, and quaternion, each with advantages and disadvantages. The advantage of R is direct interpretation in change of basis while its disadvantage is that nine dependent parameters must be stored. The physical role of individual parameters is lost, and only the matrix as a whole has meaning. Euler angles are roughly defined by three successive rotations about three axes of local (and sometimes global) coordinate frames . The advantage of using Euler angles is that the rotation is described by three independent
1. Introduction
19
parameters with plain physical interpretations. Their disadvantage is that their representation is not unique and leads to a problem with singularities. There is also no simple way to compute multiple rotations except expansion into a matrix. Angle-axis convention is the most intuitive representation of rotation. However, it requires four parameters to store a single rotation, computation of combined rotations is not simple, and it is ill-conditioned for small rotations. Quaternions are good in preserving most of the intuition of the angleaxis representation while overcoming the ill conditioning for small rotations and admitting a group structure that allows computation of combined rotations . The disadvantage of quaternion is that four parameters are needed to express a rotation. The parameterization is more complicated than angleaxis and sometimes loses physical meaning. Quaternion multiplication is not as plain as matrix multiplication.
1.4.4 Vector Function Vectors serve as the basis of our study of kinematics and dynamics. Positions, velocities, accelerations, momenta, forces, and moments all are vectors. Vectors locate a point according to a known reference. As such, a vector consists of a magnitude, a direction, and an origin of a reference point. We must explicitly denote these elements of the vector. If either the magnitude of a vector r and/or the direction of r in a reference frame B depends on a scalar variable, say 0, then r is called a vector function of 0 in B. A vector r may be a function of a variable in one reference frame, but be independent of this variable in another reference frame . Example 2 Reference frame and parameter dependency. In Figure 1.14, P represents a point that is free to move on and in a circle, made by three revolute jointed links. 0, 'P, and 'ljJ are the angles shown, then r is a vector function of 0, 'P, and 'ljJ in the reference frame G(X, Y) . The length and direction ofr depend on 0, 'P, and 'ljJ. IfG(X , Y), and B(x , y) designate reference frames attached to the ground and link 2, and P is the tip point of link 3 as shown in Figure 1.15, then the position vector r of point P in reference fram e B is a function of'P and 'ljJ , but is independent of O.
1.5 Problems of Robot Dynamics There are three basic and systematic methods to represent the relative position and orientation of a manipulator link. The first and most popular
20
1. Introduction
y
• p
x FIGUR E 1.14. A plan ar 3R manipulator and posit ion vect or of the tip point P in glob al coordinate G (X , V) .
y
• y p
~-'------------ "
X
FIGU RE 1.15. A plan ar 3R manipulator and posit ion vect or of t he t ip point P in second link local coord inate B (x, y).
1. Introduction
21
method used in robot kinematics is based on the Denavit-Hartenberg notation for definition of spatial mechanisms and on the homogeneous transformation of points. The 4 x 4 matrix or the homogeneous transformation is utilized to represent spatial transformations of point vectors. In robotics, this matrix is used to describe one coordinate system with respect to another. The transformation matrix method is the most popular technique for describing robot motions. Researchers in robot kinematics tried alternative methods to represent rigid body transformations based on concepts introduced by mathematicians and physicists such as the screw theory, Li e algebra, and Epsilon algebra. The transformation of a rigid body or a coordinate frame with respect to a reference coordinate frame can be expressed by a screw displacement, which is a translation along an axis with a rotation by an angle about the same axis. Although screw theory and Lie algebra can successfully be utilized for robot analysis, their result should finally be expressed in matrices.
1.6 Preview of Covered Topics The book is arranged in three parts: I-Kinematics, Il-Dynarnics, and IIIControl. Part I is important because it defines and describes the fundamental tools for robot analysis. Rotational analysis of rigid bodies is a main subject in relative kinematic analysis of coordinate frames . It is about how we describe the orientation of a coordinat e frame with respect to the others. In Chapters 2 and 3, we define and describe the rotational kinematics for the coordinate frames having a common origin. So, Chapters 2 and 3 are about the motion of two directly connected links via a revolute joint. The origin of coordinate frames may move with respect to each other, so, Chapter 4 is about the motion of two indirectly connected links. In Chapter 5, the position and orientation kinematics of rigid links are utilized to systematically describe the configuration of the final link of a robot in a global Cartesian coordinate frame. Such an analysis is called forward kinematics, in which we are interested to find the end-effector configuration based on measured joint coordinates. The Denavit-Hartenberg convention is the main tool in forward kinematics. In this Chapter, we have shown how we may kinematically disassemble a robot to basic mechanisms with 1 or 2 DOF, and how we may kinematically assemble the basic mechanisms to make an arbitrary robot. Chapter 6 deals with kinematics of robots from a Cartesian to joint space viewpoint that is called inverse kinematics. We start with a known position and orientation of th e end-effector and search for a proper set of joint coordinates.
22
1. Introduction
Velocity relationships between rigid links of a robot is the subject of Chapters 7 and 8. The definitions of angular velocity vector and angular velocity matrix are introduced in Chapter 7. The velocity relationship between robot links, as well as differential motion in joint and Cartesian spaces, are covered in Chapter 8. The Jacobian matrix is the main concept of this Chapter. Part I is concluded by describing the applied numerical methods in robot kinematics. In Chapter 9 we introduce efficient and applied methods that can be used to ease computerized calculations in robotics . In part II, the techniques needed to develop the equations of robot motion are explained . This part starts with acceleration analysis of relative links in Chapter 10. The methods for deriving the robots' equations of motion are described in Chapter 11. The Lagrange method is the main subject of dynamics development . The Newton-Euler method is described alternatively as tool to find the equations of motion . The Euler-Lagrange method has a simpler concept , however it provides the unneeded internal joint forces. On the other hand, the Lagrange method is more systematic and provides a basis for computer calculation. In part III, we start with a brief description of path analysis . Then, the optimal control of robots is described using the floating time method. The floating time technique provides the required torques to make a robot follow a prescribed path of motion in an open loop control. To compensate a possible error between the desired and the actual kinematics, we explain the computed torque control method and the concept of the closed loop control algorithm.
1.7 Robots as Multi-disciplinary Machines Let us note that the mechanical structure of a robot is only the visible part of the robot. Robotics is an essentially multidisciplinary field in which engineers from various branches such as mechanical, systems , electrical, electronics, and computer sciences play equally important roles. Therefore, it is fundamental for a robotical engineer to attain a sufficient level of understanding of the main concepts of the involved disciplines and communicate with engineers in these disciplines.
1.8 Summary There are two kinds of robots: serial and parallel. A serial robot is made from a series of rigid links, where each pair of links is connected by a revolute (R) or prismatic (P) joint. An R or P joint provides only one degree of freedom, which is rotational or translational respectively. The
1. Introduction
23
final link of a robot , also called the end-effector, is the operating member of a robot that interacts with the environment. To reach any point in a desired orientation, within a robot 's workspace, a robot needs at least 6 DOF . Hence, it must have at least 6 links and 6 joints. Most robots use 3 DOF to position th e wrist point , and use the other 3 DOF to orient the end-effector about the wrist point. We attach a Cartesian coordinate frame to each link of a robot and determine the position and orientation of each frame with respect to th e others. Therefore, to determine the position and orientation of the endeffector, we need to find the end-effector frame in the base frame.
1. Introduction
25
Exercises 1. Meanin g of indexes for a position vector.
Explain t he meaning of G r p , ~ rp, and grp, if r is a position vector. 2. Meanin g of indexes for a velocity vector.
Expl ain t he meaning of G v p , ~ v p, and ~ vp, if v is a velocity vector. 3. Meanin g of indexes for an angular velocity vector. . f B 2w B l , BG 2w Bl , Gw B B , an d · th e meanmgo Exp Iam angular velocity vector.
B 2WB l , 1'f B3
' an Wls
4. Meaning of indexes for a transformation matrix. Expl ain the meanin g of matrix.
B2 T B l ,
and
GT B ,
if
T
is a t ra nsformation
5. Laws of rob oti cs. What is the difference between law zero and law one of robotics? 6. New law of robotics. Wh at do you t hink about adding a fourth law to rob oti cs, such as: A robot must protect the ot her robots as long as such protection does not conflict with a higher order law. 7. The word "robot." Find t he origin and meanin g of the word "robot." 8. Robot classification. Do you consider a crane as a robot? 9. Robot market . Most small robot manufacturers went out of t he market around 1990, and only a few large companies remained. Why do you t hink thi s happ ened? 10. Hum anoid robots. The mobile robot industry is trying to make robots as similar to humans as possible. What do you t hink is the reason for t his: hum ans have the best struct ural design, humans have the simplest design, or t hese kinds of rob ot s can be sold in t he market better? 11. Robotic person. Wh y do you t hink we call somebod y who works or beh aves mechanically, showing no emotion and often respondin g to orders wit hout question , a robo tic person?
26
1. Introduction
12. Robotic journals. Find the nam e of 10 technic al journals relat ed to robotics. 13. Number of robots in industrial poles. Search t he robotic literatures and find out , approximately, how many industrial robots are currently in operation in the Unit ed Stat es, Europ e, and J apan . 14. Robotic countries. Search th e robotic literatures and find out what countries are ranked first , second, and third according to th e number of indus trial robots in use? 15. Advantages and disadvantages of robots. Search th e robotic literatures and name 10 advantages and 2 disadvantages of robots . 16. Mechanisms and robots. Why do we not replace every mechanism, in an assembly line for example, with robots? 17. Higher pairs and lower pairs. Joints can be classified as lower pairs and higher pairs. Find th e meaning of "lower pairs" and "higher pairs" in mechanics of machinery. 18. Degrees of freedom (DOF) eliminat ion. Joints can be classified by th e numb er of degrees of freedom th ey provide, or by t he number of degrees of freedom th ey eliminate. T here are, th erefore, 5 classes for joints . Find the name and th e number of joints in each class. 19. Hum an wrist DOF . Examine and count th e numb er of DOF of your wrist. 20. Human hand is a redundant manipulator. An arm (including shoulder, elbow, and wrist) has 7 DOF. What is the advantage of having one extra DOF (with respect to 6 DOF) in our hand ? 21. Usefulness of redundant manipulators. Discuss possible applications in which th e redundant manipulators would be useful.
1. Introduction
22.
27
* Independent Cartesian coordinat e systems in Euclidean spaces. In 3D Euclidean space, we need a triad to locate a point. Th ere are two independent and non-sup erposable tri ads. How many different non-superposable Cartesian coordinate systems can be imagined in 4D Euclidean space? How many Cartesian coordinate systems do we have in an nD Euclidean space?
23.
* Disadvant ages of a non-orthogon al tri ad . Why do we use an orthogonal triad to define a Cartesian space? Can we define a 3D space with non-orthogonal tri ads?
24.
* Usefulness of an orthogonal tri ad.
Orthogonality is the common prop erty of all useful coordinate systems such as Cartesian , cylindrical, spherical, parab olical, and ellipsoidal coordin at e systems. Why do we only define and use orthogonal coordinate syst ems? Do you think ability to define a vector , based on inner product and unit vectors of the coordinate system, such as,
r = (r · i)i + (r · j)J + (r . k)k is t he main reason for defining the ortho gonal coordin at e systems?
* Three coplanar vectors. Show th at if a x b . c = 0, then a , b , c are coplanar. 26. * Vector function , vector variable.
25.
A vector function is defined as a dependent vectorial variable that relat es to a scalar independent variable.
r =r(t) Describe the meaning and define an example for a vector function of a vector variable, a=a(b) and a scalar function of a vector variable f = f(b) . 27.
* Frame-dependent and frame-independent . A vector function of scalar variables is a fra me-dependent quantity. Is a vector function of vector variables frame-depend ent ? What abou t a scalar function of vector variables?
28.
* Coordin ate frame and vector function .
Explain th e meaning of B v p (G r p ), if r is a position vector, v is a velocity vector, and v(r) means v is a function of r .
Part I
Kinematics
31
Kinematics is the science of geometry in motion. It is restricted to a pure geometrical description of motion by means of position, orientation, and their time derivatives. In robotics, the kinematic descriptions of manipulators and their assigned tasks are utilized to set up the fundamental equations for dynamics and control. Because the links and arms in a robotic system are modeled as rigid bodies, the properties of rigid body displacement takes a central place in robotics. Vector and matrix algebra are utilized to develop a systematic and generalized approach to describe and represent the location of the arms of a robot with respect to a global fixed reference frame G. Since the arms of a robot may rotate or translate with respect to each other, body-attached coordinate frames A , B, C,' " or B l , B 2 , B 3 , ' " will be established along with the joint axis for each link to find their relative configurations, and within the reference frame G. The position of one link B relative to another link A is defined kinematically by a coordinate transformation ATB between reference frames attached to the link. The direct kinematics problem is reduced to finding a transformation matrix GT B that relates the body attached local coordinate frame B to the global reference coordinate frame G. A 3 x 3 rotation matrix is utilized to describe the rotational operations of the local frame with respect to the global frame. The homogeneous coordinates are then introduced to represent position vectors and directional vectors in a three dimensional space. The rotation matrices are expanded to 4 x 4 homogeneous transformation matrices to include both the rotational and translational motions . Homogeneous matrices that express the relative rigid links of a robot are made by a special set of rules and are called Denavit-Hartenberg matrices after Denavit and Hartenberg (1955). The advantage of using the DenavitHartenberg matrix is its algorithmic universality in deriving the kinematic equation of a robot link. The analytical description of displacement of a rigid body is based on the notion that all points in a rigid body must retain their original relative positions regardless of the new position and orientation of the body. The total rigid body displacement can always be reduced to the sum of its two basic components: the translation displacement of an arbitrary reference point fixed in the rigid body plus the unique rotation of the body about a line through that point. Study of displacement motion of rigid bodies leads to the relation between the time rate of change of a vector in a global frame and the time rate of change of the same vector in a local frame. Transformation from a local coordinate frame B to a global coordinate frame G is expressed by Gr
=
GRBBr+Gd B
where B r is the position vector of a point in B, G r is the position vector of the same point expressed in G, and d is the position vector of the origin
32 o of the body coordinate frame B(oxyz) with respect to the origin 0 of the global coordinate frame G(OXYZ). Therefore, a transformation has two parts: a translation d that brings the origin 0 on the origin 0, plus a rotation, G RB that brings the axes of oxyz on the corresponding axes of OXYZ. The transformation formula G r = G RB Br+Gd B , can be expanded to connect more than two coordinate frames . The combination formula for a transformation from a local coordinate frame B 1 to another coordinate frame B 2 followed by a transformation from B 2 to the global coordinate G is
A robot consists of n rigid links with relative motions . The link attached to the ground is link (0) and the link attached to the final moving link, the end-effector, is link (n). There are two important problems in kinematic analysis of robots: the forward kinematics problem and the inverse kinematics problem. In forward kinematics, the problem is that the position vector of a point P is in the coordinate frame B n attached to the end-effector as n r l' , and we are looking for the position of P in the base frame Bi, shown by °rl' . The forward kinematics problem is equivalent to having the values of joints variable and asking for the position of the end-effector. In inverse kinematics, the problem is that we have the position vector of a point P in the base coordinate frame B o as °rl' , and we are looking for n r p , the position of P in the base frame B« . This problem is equivalent to having the position of th e end-effector and asking for a set of joint variables that make the robot reach the point P . In this part, we develop th e transformation formula to move th e kinematic information back and forth from a coordinate frame to another coordinate frame .
2
Rotation Kinematics 2.1 Rotation About Global Cartesian Axes Conside r a rigid bod y B with a local coordina te frame Oxyz t hat is originally coincident with a global coordinate frame 0 XY Z . Po int 0 of the body B is fixed to t he ground G and is t he origin of both coordinate fram es. If the rigid body B rotat es ex degrees about t he Z- axis of the glob al coordinate frame , then coordinates of any point P of t he rigid body in the local and global coordinate frames are related by the following equation G
r
= Q Z ,o B r
(2.1)
where,
(2.2)
(2.3) and
Q Za
~ [ :~: -;~nnfr ~] .
(24 )
Simil arl y, rot ation (3 degrees about the Y-axis, and y degrees about t he X -axis of t he globa l fram e relate the local and global coordina tes of point P by t he following equati ons r
= Q Y ,{3 B r
(2.5)
Gr
= QX,'Y B r
(2.6)
[
co~ (3 ~ Si~ (3
G
where, QY,{3 =
- sin (3
Q x ,'Y
=
]
(2.7)
0 cos (3
- s~n "'! ] . [~o co~"'! sin "'! cos-v
(2.8)
34
2. Rotation Kinematics
G
Y B
Pj
Yj=Yj
f\ X
X
X/=X/
FIGURE 2.1. Position vector of point P when local and global frames are coincident.
Proof. Let (z,),k) and (i,J,K) be th e unit vectors along th e coordinate axes of Oxy z and OXYZ respectively. The rigid body has a space fixed point at 0 , th at is the common origin of Oxyz and OXYZ. Figure 2.1 illustrates the top view of t he syst em. The initial position of a point P is indicat ed by Pl . Th e position vector rl of PI can be expressed in body and global coordinate fram es by XIZ+ YI.J +zlk x.i + ylJ + ZIK
= Xl = YI zl = Zl
(2.9) (2.10)
Xl
YI
(2.11)
where B r l refers to th e position vector rl expressed in the body coordinate frame B , and G r l refers to t he position vector rl expressed in the global coordinate frame G. If th e rigid bod y und ergoes a rot at ion c¥ about th e Z-axis, t hen th e local frame Oxyz , point P , and th e position vector r will be seen in a second position, as shown in Figur e 2.2. Now the position vector r 2 of P2 is expressed in both coordinate frames by
+ Y2) + Z2 k x 2i + Y2J + Z 2K.
X2Z
(2.12) (2.13)
Using th e definition of the inner product and Equ ation (2.12) we may
2. Rotation Kinematics
35
y
- --"'-- -L------'----------.. x
FIGURE 2.2. Position vector of point P when local frames are rotated about the Z-axis. write
= j . X2 2 + j . Y2) + j . Z2 k j . r2 = j . X2 2 + j . Y2) + j . Z2 k
j . r2
k . r2 =
(2.14) (2.15)
k . X22 + k . Y2) + k . Z2k
(2.16)
i. ~ ]
(2.17)
or equivalently
[
Z.
2 j .) J . 2 j. ) k '2 k .)
J·k
k
.t.
The elements of the Z -rotation matrix, Qz.«, are called the direction cosines of B r 2 with respect to OXYZ . Figure 2.3 shows the top view of the initial and final configurations of r in both coordinate systems Oxyz and 0 XY Z. Analyzing Figure 2.3 indicates that
j . 2 = cos a, j . 2 = sin o., k ' 2 = 0,
j . ) = - sin a , j .) = cos o, k.]A=O . ,
j ·k = 0
j.
k= 0
(2.18)
k ·k=1.
Combining Equations (2.17) and (2.18) shows that we can find the components of G r 2 by multiplying the Z-rotation matrix QZ ,a and the vector B r 2.
- sin a cos a
o
(2.19)
36
2. Rotation Kinemati cs
y B
a - --"'-- -L------'-------. x
FIG URE 2.3. Posit ion vectors of point P before and after t he rot ation of th e local frame about th e Z-axis of t he global frame.
It can also be shown in the following short not ation Gr 2
where QZ a
= Q z .« B r 2
(2.20)
~ [ :~: ~:F ~] .
(2.21)
Equation (2.20) says t hat the vector r at the second position in the globa l coordinate frame is equa l to QZ times th e positi on vector in the local coordinate frame. Hence, we are able to find t he globa l coordinates of a point of a rigid body after rot ation about t he Z- axis, if we have its local coordinates. Similarl y, rotat ion (3 about the Y-axis and rot ation 'Y about t he X-axis are describ ed by t he Y -rotation matrix Qy,{3 and the X -rotation matrix Q X ,,,! respectively.
Qy,{3 =
Qx,"!
=
COS(3 0 [ - sin (3
~ Si~(3 ] o
[~o C?~'Y - s~n sm-y
(2.22)
cos (3
'Y ] cos-y
(2.23)
The rot ation matrices Qz ,e> , QY,{3 , and Qx ,"! are called basic global rotation matrices. We usually refer to the first , second and t hird rotat ions about the axes of t he global coordina te frame by 0:, (3, and 'Y respectively.
•
2. Rotation Kinematics
37
z
x y FIGURE 2.4. Corner P of the slab at first position. Example 3 Su ccessiv e rotation about global axes. Th e final position of th e corne r P(5 , 30, 10) of the slab shown in Figure 2.4 aft er 30deg rotation about the Z-axis, follow ed by 30 deg about the X axis, and then 90 deg about th e Y -axis can be found by first multiplying Q Z ,30 by [5,3 0, lO]T to get the new global position after first rotation
X2] [ YZ22 =
[COS30 -sin30 sin 30 cos 30 0 0
5] =
0] [ 0 30 1 10
[-10.68 ] 28.48 , 10.0
and th en multiplying Q X ,30 and [-10.68,28 .48, 10.0r to get the position of P aft er the second rotation
X3 ]
Y3
[ Z3
[10 cos300 0
sin 30
o
- sin 30 cos 30
] [
-10.68 ] [-10.68 ] = 19.66 , 28.48 10.0 22.9
and finally multiplying QY,90 and [-10.68,19 .66, 22.9]T to get the final position of P after the third rotation. Th e slab and the point P in first, secon d, third , and fou rth positions are shown in Figure 2.5.
[ COS90 0 Sin 90 ] [ -10.68 ] X4 ] Y4 = 0 1 0 19.66 [ Z4 - sin 90 0 cos 90 22.9
22.90 ] 19.66 [ 10.68
Example 4 Global rotation, local position . If a point P is moved to G r 2 = [4,3, 2r aft er a 60 deg rotation about th e
38
2. Rotation Kinematics
x y
FIGURE 2.5. Corner P and the slab at first , second, third, and final positions. Z -axis, its position in the local coordinate is - l
QZ ,60
G r2
COS 60 [
sin60
-sin60 cos 60
o
o
0 0 1
]-1
[4] 3 2
[ 4.60] -1.95 . 2.0
Th e local coordinate fram e was coincident with the global coordin ate fram e before rotation, thus the global coordinates of P before rotation was also G r 1 = [4.60, -1.95, 2.0jT . Positions of P before and aft er rotation are shown in Figure 2.6.
2.2 Successive Rotation About Global Cartesian Axes The final global position of a point P in a rigid body B with position vector r , afte r a sequence of rot ations Ql , Q 2, Q3, ..., Q n about the global axes can be found by (2.24) where, (2.25)
and , G r and B r indicat e the position vector r in t he global and local coordinat e frame s. G Q B is called the global rotation matrix. It maps the local coordina tes to t heir corresponding global coordinates.
2. Rotation Kinematics
39
y
x
FIGURE 2.6. Positions of point P in Example 4 before and after rotat ion. Since matrix mult iplications do not commute, the sequence of perfor ming rotations is important . A rotation matrix is orthogonal; i.e., its transpose QT is equal to its inverse Q-l. (2.26)
Rotation about the global coordinate axes is conceptually simple because the axes of rotations are fixed in space . Assume we have the coordinates of every point of a rigid body in t he global frame that is equal to the local coordina tes initially. The rigid body rotates about a global axis, then the proper global rotation matrix gives us the new global coordinate of the points. When we get the coordinat es of points of the rigid body afte r the first rotation, our situation before the second rotation is similar to what we had before the first rot ation. Example 5 Successive global rotation matrix. The global rotation matrix after a rotation QZ,o: followed by QY,{3 and then Qx" is
Qx"QY,{3Q z ,o: co:cj3 cvso: + co:sj3s"'( [ so:s"'( - co:qsj3
-cj3so: sj3 ] co:q - so:sj3s"'( - cj3s"'( . (2.27) cosr; + q so:sj3 cj3q
Example 6 Successive global rotations, global position. The end point P of the arm shown in Figure 2.7 is located at
[
~llz,
]
= [
c~s ee ] = [ 11 c~s 75 ] = [ 0~206 ] . sin 75 0.97
l l sin
40
2. Rotation Kinematics
y
p()--Y--./
x
z FIGURE 2.7. The arm of Example 6.
The rotation matrix to find th e n ew position of the end point aft er - 29 deg rotation about th e X -axis , follow ed by 30 deg about the Z -axis, and again 132 deg about the X -axis is
GQB
= QX ,132QZ,30QX,-29 =
0.87 - 0.33 [ 0.37
-0.44 -0.24 ] - 0.15 -0.93 0.89 - 0.27
and its new position is at
[ 0.87 X2 ] Y2 = - 0.33 [ Z2 0.37
- 0.44 -0.15 0.89
- 0.24] [ 0.0 ] - 0.93 0.26 -0.27 0.97
=
[-0.35] - 0.94 . -0.031
Example 7 Tw elve independent tripl e global rotations. In gen eral, there are 12 different indep end ent combinations of triple rotations about the global axes. Th ey are:
Qx"QY,(3Q z ,a Qy"Q z ,(3Qx,a Qz"Q x ,(3Qy,a Qz"QY,(3Qx,a Qy"Qx ,(3Qz,a Qx "Q Z,(3Qy,a Qx"QY,(3Qx,a Qy"Q z ,(3Qy,a 9 - Qz"Qx ,(3Qz,a 10 - Qx"Qz,(3Qx,a 11 - Qy"Qx ,(3Qy,a 12 - Qz"QY,(3Q z ,a 12345678-
(2.28)
2. Rotation Kinematics
41
Th e expanded form of the 12 global axes tr iple rotations are presented in Appendix A .
Example 8 Order of rotati on, and order of matrix multiplication. Changing th e order of global rotation matrices is equivalent to changing th e ord er of rotations. Th e position of a point P of a rigid body B is located
f.
Its global position aft er rotation 30 deg about Xat B r p = [1 2 3 axis and th en 45 deg about Y -axis is at Qy,45 QX,30 B r p
0.53 -0.84 0.0 0.99 ] [ 1] 2 0.15 0.13 [ -0.85 -0.52 0.081 3
[-0.76 3.27 ] - 1.64
and if we change th e order of rotations th en its posit ion would be at QX ,30 Qy,45 B r p
0.53 0.0 0.85 ] [ 21] -0.84 0.15 0.52 [ -0.13 -0.99 0.081 3 Th ese two final positions of Pare d
[ 3.08] 1.02 -1.86
= I(Gr p) 1- (G r p) 21 = 4.456 apart .
2.3 Global Roll-Pitch-Yaw Angles The rotation about the X -axis of the global coordinate frame is called roll, the rotation about the Y-axis of the global coordinate frame is called pitch, and the rot ation about the Z-axis of the global coordinate frame is called yaw. The global roll-p it ch-yaw rotation matrix is
Q Z ,,,!QY,{3Qx ,o:
e(3q e(3s,,! [ -s(3
- co:S'y + q sa s(3 sas"! + caqs(3 ] co:e"! + sas(3s,,! - qsa + ea s(3s,,! . (2.29) e(3sa co:e(3
Given the roll, pitch , and yaw angles, we can compute the overall rotation matrix using Equ ation (2.29). Also we are able to compute the equivalent roll, pit ch, and yaw angles when a rot ation matrix is given. Suppose that r i j indicat es the element of row i and column j of the roll-pitch-yaw rot ation matrix (2.29), then th e roll angle is
a = tan - 1
(1'32) 1'33
(2.30)
42
2. Rotation Kinematics
and the pit ch angle is (2.31)
and the yaw angle is 'Y
= tan- 1
(r
21
(2.32)
)
rll
provided that cos (3
=f: O.
Example 9 Global roll, pitch, and yaw rotations. Figures 2.8, 2.9, and 2.10 illustrate 45 deg roll, pitch and yaw about the axes of global coordinate frame.
z
y
FIGU RE 2.8. Global roll.
z
y
FIGUR E 2.9. Global pit ch.
2. Rotation Kinematics
43
z
y FIGURE 2.10. Global yaw.
2.4 Rotation About Local Cartesian Axes Consider a rigid body B with a space fixed at point O. The local body coordinate fram e B(Oxyz) is coincident with a global coordinate frame G(OXYZ) , where the origin of both frame s are on th e fixed point O. If the body undergoes a rotation 'P about the z-a xis of its local coordinat e fram e, as can be seen in the top view shown in Figure 2.11 , t hen coordinates of any point of t he rigid body in local and global coordinat e frames are related by the following equat ion B r = A z,'f' Gr . (2.33) The vectors G r and B r are th e position vectors of the point in global and local fram es resp ectively [ X
[x
Y Y
Z
JT
zf
(2.34) (2.35)
and A z,'f' is the z -rotation matri x
A"
~ [ ~~r:~ :~~~ ~] .
(236)
e
Similarl y, rotation about the y-axis and rotation 1jJ about the x-axis are describ ed by t he y-rotation matrix A y,o and t he x -rotation matrix Ax,,p resp ectively.
cos e 0 - s~ne ] A y,o = 0 1 [ sine 0 cos e
Ax ,,p =
[
1 0
o
0 cos 1jJ - sin 1jJ
si~
1jJ ] cos 1jJ
(2.37)
(2.38)
44
2. Rotation Kinematics
y
----'l<::------'--~----'---___. x
FIGURE 2.11. Position vectors of point P before and after rotation of the local frame about t he z-axis of t he local frame.
Proof. Vector r l indicates the posit ion of a point P of t he rigid body B where it is initially at Pl. Using t he unit vectors (i,5, k) along t he axes of local coordinate frame B(Oxyz) , and (I ,), K) along t he axes of global coordinate frame B(OXYZ) , the initial and final position vectors r l and r 2 in both coordinate frames can be expresse d by B
B
(2.39) (2.40)
X2 i + Y25 + Z2 k x.i + Y2} + Z 2K.
(2.41) (2.42)
rl
r2
G
xli + Yl1 + zlk x .i + Yl } + ZlK
rl
G
r2
T he vectors B r l and B r2 are the initial and final positio ns of t he vector r expresse d in body coordinate frame Oxyz, and G r l and G r 2 are the initial and final positions of the vector r expressed in the global coordinate frame OXYZ . The components of B r2 can be found if we have the components of G r 2 . Using Eq uation (2.42) and t he definition of t he inner product , we may write
Y2
i · r 2 = i . x 21 + i . Y2} + i . Z2K 5· r 2 = 5· x 21 + 5· Y2} + 5· Z2K k. r 2 = k. x 2 i + k. Y2 } + k. Z2K
(2.43) (2.44) (2.45)
or equivalently
i .Z i ·} [
i·~ ]
5· J 5· } 5·K
k·l
k ·} k ·K
(2.46)
2. Rotation Kinemati cs
45
Th e elements of th e z-rot ation matrix Az,'P are th e direction cosines of with respect to Oxyz . So, the elements of t he matrix in Equat ion (2.46) are i· j = coscp, i · j = sin o, i · K = 0 j .j=- sin cp , j .j=cos cp, j .K=O (2.47) k . I = 0, k . j = 0, k.K = 1 Gr 2
Combining Equ ations (2.46) and (2.47), we can find the components of by multiplying z-rotation mat rix A z,'P and vector G r 2.
Br2
[
~~ ] = [~~~:cp ~~~: ~] [ ~: ] . 0 0 1 Z2
(2.48)
Z2
It can also be shown in th e following short form
(2.49) where COS
Az,'P =
cp
sin ip
0]
- s~n sp co~ cp ~
[
.
(2.50)
Equ ation (2.49) says that after rotation about the z-axis of the local coordinate frame, the position vector in the local frame is equal to A z,'P tim es the position vector in the global frame. Hence, after rotation about the z-axis, we are able to find the coordinates of any point of a rigid body in local coordinate frame, if we have its coordinates in the global frame. Similarly, rotation e about the y-axis and rotation 7/J about the x-axis are described by th e y-rotation matri x A y,o and th e x-rotation matrix A x,1/! respecti vely. COS
A y,o = [
A x ,1/! =
e
0 sin e
0 - sin o ] 1 0 0 cos e
1 cos07/J
[o 0
- sin 7/J
0]
sin 7/J cos 7/J
(2.51)
(2.52)
We indicate the first , second, and third rotations about the local axes by cp, e, and 7/J respectively. • Example 10 Local rotation, local position . If a local coordin ate fram e Oxyz has been rotated 60 deg about the z axis and a point P in the global coordinate fram e 0 XY Z is at (4, 3,2) , its coordinates in the local coordin ate fram e Oxyz are
cos 60 sin 60 - sin 60 cos 60
o
0
46
2. Rotation Kinematics
z
FIGURE 2.12. Arm of Exampl e 12.
Example 11 Local rotation, global position. If a local coordinate frame Oxyz has been rotated 60 deg about the z-axis and a point P in the local coordinate frame Oxyz is at (4,3 ,2), it s position in the global coordinate frame 0 XY Z is at
X] [~ =
[COS 60
sin 60
0]
-S~60 CO~60 ~
[ -0.60 ]
4 ]
T [
~
=
~~06
Example 12 Successive local rotation, global position. The arm shown in Figure 2.12 has two actuators. The first actuator rotates the arm -90 deg about y-axis and then the second actuator rotates the arm90degaboutx-axis. If the endpointP is at Br p = [9.5 -10.1 10.1 then its position in the global coordinate frame is at Gr 2
l
[Ax ,9o A y ,_ 90 r B r p I A-I B A y- ,-90 x ,90 r p T AT A y ,-90 x, 90 B rp [
10.1 ] -10.1 . 9.5
f,
2. Rotation Kinematics
47
2.5 Successive Rot ation About Local Cartesian Axes The final global position of a point P in a rigid body B with position vector r , after some rotations A l , A 2 , A 3, ..., An about the local axes, can be found by (2.53) where, B
A c = An '" A 3A2A l .
(2.54)
is called the local rotation matrix and it maps the global coordinates to their corresponding local coordinates. Rotation about the local coordinate axis is conceptually interesting because in a sequence of rotations, each rotation is about one of the axes of the local coordinate frame , which has been moved to its new global position during the last rotation. Assume that we have the coordinates of every point of a rigid body in a global coordinate frame . The rigid body and its local coordinate frame rotate about a local axis, then the proper local rotation matrix relat es the new global coordinates of the points to the corresponding local coordinates. If we introduce an intermediate space-fixed frame coincident with the new position of the body coordinate frame , we may give the rigid body a second rotation about a local coordinate axis. Now another proper local rotation matrix relates the coordinates in the intermediate fixed frame to the corresponding local coordinates. Hence, the final global coordinates of the points must first be transformed to the intermediate fixed frame and second transformed to the original global axes. B Ac
E x ample 13 Successive local rotation, local position. A local coordinate frame B (Oxyz) that initially is coincident with a global coordinate frame G ( 0 XY Z) undergoes a rotation 'P = 30 deg about the z -axis , then = 30 deg about the x -axis, and then 7/J = 30 deg about the y-axis. The local coordin ates of a point P located at X = 5, Y = 30, Z = 10 can be found by [ x y z] T = Ay,v,Ax.oAz,'P [5 30 10] T . Th e local rotation matrix is
e
B Ac
= Ay,30Ax,30Az,30 =
[ 063 -0.43 0.65
0.65 0.75 -0.125
-0.43 ] 0.50 0.75
[i~ ]
[ 1835 25.35 ] . 7.0
and coordinates of P in the local fram e is
[~ ] [
0.63 -0.43 0.65
0.65 0.75 - 0.125
-0.43 ] 0.50 0.75
48
2. Rotation Kinematics
Example 14 Successive local rotation. The rotation matrix for a body point P (x , y , z) after rotation Az,'f' followed by A x,e and Ay,,p is BA c
= Ay,,p Ax,eA z,'f' c
c'lj;s
- CBS'lj;] sB . (2.55) cBc'lj;
Example 15 Twelve independent triple local rotations. Euler proved a theorem that stated: Any two independent orthogonal coordinate frames can be related by a sequence of rotations (not more than three) about coordinate axes, where no two successive rotations may be about the sam e axis. In general, there are 12 different independent combinations of triple rotation about local axes. They are: 1 - Ax,,pAy,eAz,'f' 2 - A y,,pAz,eA x,'f' 3 - Az,,pAx,eAy,'f' 4 - Az,,pAy,eAx,'f' 5 - Ay,,pAx,eAz,'f' 6 - Ax,,pAz,eAy,'f' 7 - Ax,,pAy,eAx ,'f' 8 - Ay ,,pAz,eA y,'f' 9 - Az ,,pAx,eAz,'f' 10 - Ax,,pAz,eAx ,'f' 11 - Ay,,pAx,eAy,'f' 12 - A z,,pAy,eAz,'f'
(2.56)
The expanded form of the 12 local axes' triple rotation are presented in Appendix B.
2.6 Euler Angles The rotation about t he Z-axis of t he globa l coordi nate is called precession, t he rot ati on abo ut t he x-axis of t he local coord inate is called nutation, and t he rot ation abo ut th e z-axis of t he local coord inate is called spin. T he precession-nutation-spin rotation angles are also called Euler angles. Euler angles rotation matrix has a lot of application in rigid body kinematics. To find t he Euler angles rotation mat rix to go from t he global frame G(OXY Z ) to t he final body frame B (Ox yz ), we employ a body fra me B'(Ox'y ' z') as shown in Figure 2.13 t hat before t he first rot ation coincides with t he global frame. Let there be at first a rotation ip about the z'-axis. Because Z-axis
2. Rot ation Kinematics
49
z
z'
x FIGURE 2.13. First Euler angle.
and z'-axis are coincident , by our t heory B' B'
B' A
r
o or
~ [ - sin0 'P cos 'P
Ao
A"
sin 'P cos 'P 0
n
(2.57)
Next we consider t he B '(Ox'y' z') frame as a new fixed global frame and introduce a new body frame B " (Ox" y" z" ). Before t he second rot ation, the two frames coincide. Then, we execute a 0 rotation about x"-ax is as shown in Figur e 2.14. The transform ation between B'(Ox'y' z') and B "(Ox"y" z") is B" B"
r
A 13'
B" A , B' r B
Ax o
~[~
(2.58) 0
'i~O
cos O ] . - sin O cos O
(2.59)
Finally we consider the B" (Ox" y" z" ) frame as a new fixed global frame and consider the final body frame B(Oxyz) to coincide with B" before the third rotation . We now execute a 'l/J rot at ion about t he zIt-axis as shown in F igur e 2.15. The transformation between B" (Ox"y" z") and B(Oxyz) is Br
BA B"
B
A B"
A z ,1/J
B"
=
r
[ co, ~
- s~n 'l/J
sin 'l/J cos 'l/J 0
n
(2.60) (2.61)
By t he rule of composit ion of rot ations, t he transformation from G(OXY Z) to B(Oxyz) is (2.62) where
50
2. Rotation Kinematics
z'
FIGURE 2.14. Second Euler angle.
FIGURE 2.15. Third Euler angle.
2. Rotat ion Kinematics
51
Az ,,pAx,eAz,cp
opob [
c(}s
c'lj;s
+ c(}c
+
-c
-s
s(}s
s(}s'lj; ] s(}c'lj;
(2.63)
c(}
and therefore, B A~
=
CQB
= [Az ,,pAx ,eAz,cplT
c
- c
+ c(}c
- s
c'lj;s
s(}s'lj;
s(}s
c(}
Given the angles of precession ip , nutation (), and spin 'lj;, we can compute t he overall rotation matrix using Equation (2.63). Also we are able to compute the equivalent precession, nutation, and spin angles when a rotation matrix is given. If r ij indicat es t he element of row i and column j of the precessionnut ation-spin rotation matrix, then, (2.65)
(r
= - t an- 1 - 31 )
(2.66)
r 32
'lj;
provided that sin ()
= tan- 1 (
13
(2.67)
r ) r23
i= o.
Example 16 Euler angle rotation matrix. The Euler or precession-nutation-spin rotation matrix for
() = 41.41 deg,
'lj; in Equation
(2.63). B
Ac
= A z,- 40.7A x,41.41 A z, 79. 15 0.63 -0.43 [ 0.65
0.65 0.75 - 0.125
-0.43 ] 0.50 0.75
Example 17 Euler angles of a local rotation matrix. The local rotation matrix after rotation 30 deg about the z -axis, then rotation 30 deg about th e x - ax is , and th en 30 deg about th e y-axis is BAc
=
A y,30 Ax ,30 Az ,30
0.63 -0.43 [ 0.65
0.65 0.75 - 0.125
- 0.43 ] 0.50 0.75
52
2. Rotation Kinematics
and th erefore, the local coordinat es of a sample point at X Z = 10 are
X] [
y z
=
[0.63 -0.43 0.65
0.65 0.75 -0.125
= 5,
Y
= 30,
-0.43 ] [ 5] [18.35 ] 0.50 30 = 25.35 . 0.75 10 7.0
Th e Eul er angles of the corresponding precession-nutation- spin rotation matrix are cos- 1 (0.75)
()
- tan - 1
7/J
(
= 41.41deg
0.65 ) = 79.15 deg 0 - .125
= t an - 1 (-0.43) 0.50
= -40.7 deg .
Hence, Ay .30Ax ,30Az ,30 = A Z,'ljAx ,eAz ,rp when
Example 18 Relative rotation matrix of two bodies. Consider a rigid body B 1 with an orientation matrix B 1 A a made by Eul er angles
B Aa
=
A z ,60 A x ,-45 A z ,30
0.127 - 0.927 [ -0.354
0.78 -0.127 0.612
-0.612 ] -0.354 0.707
A z ,lOA x ,25 A z ,-1 5
0.992 0.103 [ 7.34 x 10- 2 Th e desired rotat ion matrix
B1
-6.33 x 10- 2 0.907 - 0.416
-0.109 ] 0.408 0.906
A B 2 may be found by
(2.68)
2. Rotation Kinematics
53
which is equal to B 1A
e B 2 A~
0.992 - 6.33 x 10- 2 [ - 0.109
0.103 0.907 0.408
7.34 X 10- 0.416 0.906
2
]
.
Example 19 Euler angles rotation matrix for small angles. The Euler rotation matri x B Ae = Az ,,pAx,oAz,
(2.69)
where, (2.70)
Therefore, in case of small angles of rotation, the angles rp and 'ljJ are indistingu ishable. Example 20 Small second Euler angle. If ----+ 0 then the Euler rotation matrix BA e to
e
BA e
= Az ,,pAx,oAz,
n n
[
crpc'ljJ - srps'ljJ - crps'ljJ - c'ljJsrp
c'ljJsrp + crps'ljJ -srps'ljJ + crpc'ljJ
0
0
[
coo (~ + ,p) - sin (rp + 'ljJ )
o
sin (~ + ,p) cos ( sp + 'ljJ ) 0
(2.71)
and therefore, the angles rp osul ib are indistinguishable even if the value of sp and 'ljJ are finit e. Hence, the Euler set of angles in rotation matrix (2.63) is not unique when e = o. Ex ample 21 Euler angles application in motion of rigid bodies. The zxz Euler angles are goodparameters to describe the configuration of a rigid body with a fixed point. The Euler angles to show the configuration of a top are shown in Figure 2.16 as an example.
*
Example 22 Angular velocity vector in terms of Euler frequencies. A Eulerian local frame E (0, e
54
2. Rotation Kinematics
FIGURE 2.16. Application of Euler angles in describing th e configurat ion of a top.
Th e angular velocity vect or C W B of th e body fram e B(Oxyz) with respect to the global f ram e G(0 XY Z) can be writte n in Eul er angles fram e E as the S1Lm of three Eul er angle rate vectors
(2.72) where, the rat e of Euler angles, 1jI, ii, and ;P are called Euler frequencies. To find C W B in body fram e we m ust express the un it vectors e
[0 0 1] T = K is in the global fram e and can be tran sformed to the body fram e afte r three rotations BAc K Az,,pAx,oAz,
sin (J sin 1/J ] sin (J cos 1/J
(2.73)
cos (J
Th e unit vector eo = [1 0 0] T = i' is in th e intermediate f ram e O x'y' z' and needs to get two rota tions Ax,o and Az,,p to be tran sformed
2. Rotation Kinematics
55
z
FIGURE 2.17. Euler angles frame
e
to the body frame
BA OXly'Z I
AI
Z
Az ,,p Ax ,1I i '
cos 'ljJ ]
-s~n 'ljJ
[
(2.74)
.
The unit vector e,p is already in the body frame, e,p = [0 Therefore, e w B is expressed in body coordinate fram e as B eWB
ep
sin 0 sin 'ljJ ]
[
Si~:~~S 'ljJ
+e
[COS 'ljJ ] s~n 'ljJ
-
+ ;p
0 1] T = i , [ 0 ]
~
(ep sin 0 sin 'ljJ + ecos 'ljJ) i
+ (ep sin 0 cos 'ljJ -
esin 'ljJ) j
+ (ep cos 0 + ;p) k
(2.75)
and therefore, components of eW B in body frame Oxyz are related to the Euler angle frame OrpO'ljJ by the following relationship: B
eWB
BA E
E eWB
sin Osin 'ljJ sin 0 cos 'ljJ [ cosO
(2.76) (2.77)
Then, ew B can be expressed in the global fram e using an inverse transfor-
56
2. Rot at ion Kinemat ics
mation of Eu ler rotation matrix (2.63) a
BA- I B w
c c
e WB
BA
B
e
(p sin ()sin 7/J + cos 7/J ] (p sin ()cos 7/J - ~ sin 7/J [ (p cos () + 7/J
c
1
(2.78)
(ecos cp + -0 sin ()sin sp) j + (esin cp - -0 cos cp sin ()) j + (p+ -0 cos ()) tc
(2.79)
and hence , components of ew B in global coordinate frame 0 XY Z are related to th e Euler angle coordinate fram e 0cp()7/J by the following relation ship :
ae WB Wx Wy [
eQ E
E
(2.80)
eWB
[0 c~s sp
]
wz
0
sin c
1
0
sin ()sin cp ] [ (p ] - cos cp sin () ~ cos () 7/J
.
(2.81)
*
E x ample 23 Eul er fr equen cies based on a Cartesian angular velocity vect or. Th e vector ~ w B , that indicates th e angular velocity of a rigid body B with respect to the global fram e G written in fram e B , is relat ed to the Euler fr equen cies by B e WB
BA
B e WB
W• • wy [ Wz
E
E e WB
=
[
sin ()sin 7/J sin ()cos 7/J cos()
!].
-,:r:~ ~ ] [
(282)
Th e matrix of coeffici ents is not an orthogonal matrix because, BAT
E
BAT
E
B A -I
E
i=
B A -I
E
[ sin e sin ~ cos 7/J
1 sin ()
sin ()cos 7/J - sin 7/J
0
0
[
sin 7/J sin ()cos 7/J - cos () sin 7/J
cr ]
cos 7/J - sin ()sin 7/J - cos ()cos 7/J
(2.83)
n
(2.84)
It is because the Eu ler angles coordinate fram e O cp()7/J is not an orthogonal fram e. For the same reason, the matrix of coeffic ients that relates the Eul er
2. Rot ation Kinematics
gwB
f requencies and the components of C C WB
cQ ECWB E
g WB
[
~~
]
=
wz
57
[ ~ ~~~~ ~~~:~i~i~O ] [ '~] ljJ 1
0
cos O
is not an orthogonal matrix. Therefore, the Eu ler frequencies based on local and global decomposition of the angular velocity vector C W B must solely be found by the inverse of coefficient matrices B A-I B w E C B
E C WB
[!]
,~e [
n[ ~: ]
(2.85)
cos 'ljJ - sin e sin 'ljJ - cos 0 cos 'ljJ
sin'ljJ sinO cos'ljJ - cos Osin 'ljJ
(286)
and E C WB
CQ - I C w E C B
[!]
(2.87)
e
1 [ - co, sin 'P cos 0 cos sp -sin 0 cos ep sin e sin ep sinO sin e - cos ep
~ [ :~ ]
]
(288)
Using (2.85) and (2.81), it can be verified that the transformation matrix B A c = B AE cQ"E} would be the same as Euler transformation matrix
(2.63). The angular velocity vector can thus be expressed as
[i 3 k ] [
C WB
j
[j
tc
[k eo
I
~:]
[ ~:J
-.[H
*
Example 24 Integrability of the angular velocity components. T he integra bility condition for an arbitrary total differential dz is
=
Pdx + Qdy
oP oy
eo ox '
(2.89)
(2.90)
58
2. Rotation Kinemat ics
The angular velocity components wx, w y , and W z along the body coordinate axes x, y , and z can not be integrated to obtain the associated angles because (2.91) wxdt = sin () sin 'l/J dcp + cos 'l/J d(} and
a(sin () sin 'l/J) -/.. acos 'l/J a(}
r
acp .
However, the integrability condition (2.90) is satisfied by the Euler frequencies because, for example, d(} = cos 'l/J (wx dt) - sin 'l/J (wy dt) and
a(cos 'l/J) a(- sin 'l/J) W at W x at y
sin 'l/J cos () cos 'l/J sin ()
(2.92)
(2.93)
It can be checked that dcp and d'l/J are also integrable.
*
Example 25 Cardan angles and frequencies. The syst em of Euler angles is singular at () = 0, and as a consequence, cp and 'l/J become coplanar and indistinguishable. From 12 angle syst ems of Appendix B, each with certain nam es, characteristics, advantages, and disadvantages, the rotations about three different axes such as B A c = A z,,pAy,eAx,ip are called Cardan or Bryant angles. The Cardan angle system is not singular at () = 0, and has som e application in mechatronics and attitude analysis of satellites in a central force field. B Ac
=
ccps'l/J + s(}c'l/Jscp ccpc'l/J - s(}scps'l/J - c(}scp
c(}c'l/J - c(}s'l/J [ s(}
scps'l/J - ccps(}c'l/J ] c'l/Jscp + ccps(}s'l/J c(}ccp
The angular velocity w of a rigid body can either be expressed in terms of the components along the axes of B(Oxyz) , or in terms of the Cardan frequencies along the axes of the nonorthogonal Cardan fram e. The angular velocity in terms of Cardan frequencies is c W B = epAz,,p Ay,e
[
~ ] + OA, . [
n~ n + [
therefore,
[~:] [ [!] [
cos () cos 'l/J - cos (} sin 'l/J sin o cos ,p
sin 'l/J cos'l/J
°
~ ][!] [ ~:]
cos e
_ sin,p cos e
sin 'l/J - t an() cos 'l/J
cos'l/J t an () sin 'l/J
n
(2.94)
2. Rotation Kinematics
59
FIGURE 2.18. Local roll-pitch-yaw angles. In case of small Cardan angles, we have
and
2.7
Local Roll-Pitch-Yaw Angles
Rotation about the x-axis of t he local frame is called roll or bank, rot ation about y-axis of the local frame is called pitch or attitude, and rotation about t he z-axis of the local frame is called yaw, spin, or heading. The local roll-pitch-yaw angl es are shown in Figure 2.18. The local roll-pitch-y aw rotation matrix is
Az,,pAy,oAx,cp cec"IjJ - ces"IjJ [ se
c<.ps"IjJ + sec"IjJs<.p c<.pc"IjJ - ses<.ps"IjJ -ces<.p
s<.ps"IjJ - c<.psec"IjJ ] c"IjJs <.p + c<.pses"IjJ . (2.95) cec<.p
Note the difference between roll-pitch-yaw and Eul er angles, alt hough we show both utili zing .p, e, and "IjJ .
60
2. Rotation Kinematic s
*
Example 26 Angular velocity and local roll-pit ch-yaw rate. Using the roll-pitch -yaw f requencies, the angular velocity of a body B with respect to the global reference fram e is
e WB
+ W y ) + w),,, rpe
Wx t
(2.96)
Relationships between the compon ents of ew B in body fram e and roll-pitchyaw compon ent s are found when the local roll uni t vector e
(2.97)
Th e pitch un it vector ee = rotatum sb
[0 1 0] T
transforms to the body fram e after
(2.98)
Th e yaw un it vector e,p = [0 0 1] T is already along the local z- axis. Hence, e W B can be ecpressed in body fram e Oxyz as
(2.99)
and theref ore, ew B in global fram e 0 XY Z in term s of local roll-pit ch-yaw
2. Rotation Kinematics
61
frequencies is
(2.100)
2.8 Local Axes Rotation Versus Global Axes Rotation The global rotation matrix G Q B is equal to the inverse of the local rotation matrix B A G and vice versa ,
GQB -_ BA G-1
BA
_ GQ-l
G -
(2.101)
B
where
- 1· · · A-l A 1- 1A 2- 1A 3 n Q 1- 1Q-2 1Q-3 l .. . Q-l n .
(2.102) (2.103)
Also, premultiplication of the global rotation matrix is equal to postmultiplication of the local rotation matrix. Proof. Consider a sequence of global rotations and their resultant global rotation matrix GQB to transform a position vector Br to Gr. (2.104) The global position vector rotation matrix B A G .
Gr
can also be transformed to
Br
using a local (2.105)
Combining Equations (2.104) and (2.105) leads to
CQB BAGcr B AcGQBBr
(2.106) (2.107)
62
2. Rot ation Kinematics
and hence, (2.108) Therefore, the global and local rotation matrices are the inverse of each other BA C1
GQ B GQB- 1
Assume that GQ B
= Qn ' "
BA G·
(2.109)
Q3Q2Q1 and BAG = An ' " A 3A 2A 1 then,
1A- 1A- 1 BA G - 1 -- A1 23 " GQ-1 _ Q-1Q-1Q -1 B- 1 2 3 "
' A n-I Q-1 ' n
(2.110) (2.111)
and Equ ation (2.108) becomes Qn ' . . Q2Q1 A n . . . A 2A 1 = An . . . A 2A 1Qn . .. Q2Q1 = I
(2.112)
and therefore,
a: " Q3Q2Q 1
A-I A 1A-I ... A-I 1 n 2- 3 Q 1- 1Q 2- 1Q 3- 1 " . Q-1 n
An ··· A 3A 2A 1
(2.113)
or QI 1Q;-lQ3"1
Q;;lQn '" Q3Q2Q1
All A ;-l A 3"l
A ;;1 A n ' " A 3A 2A 1
I I.
(2.114) (2.115)
Hence, t he effect of in order rotations about the global coordinate axes is equivalent to th e effect of the same rot at ions about th e local coordinate axes performed in th e reverse order. • Example 27 Global position and postmultiplication of rotat ion matrix. Th e local position of a point P aft er rotat ion is at B r = [1 2 3 If the local rotat ion matrix to transform G r to B r is given as
t.
B
A z,'P
=
[
COS
sin
o
0
0]
- sin
=
[COS30
sin 30 0]
- sin 30 cos 30 0
1
0
0
,
1
then we may find the global position vector G r by postmultiplication by the local position vector B r T ,
[1
2 3]
[
cos30 - s~ 30
[ - 0.13 2.23 3.0 ]
sin 30 cos 30
o
B
A z,'P
2. Rotation Kinematics
63
instead of premultiplication of B A;' ~ by B r . B A-I B r z ,
sin 30
- sin 30 cos 30
o
0
COS 30 [
[
-0.13 ] 2.23
3
2.9 General Transformation Consider a general situation in which two coordinate frames, G(OXYZ) and B(Oxyz) with a common origin 0 , are employed to express the components of a vector r . Th ere is always a transformation matrix G RB to map the component s of r from the reference frame B(O xy z) to th e oth er reference frame G(OXYZ) . (2.116)
In addition, th e inverse map, B r = G R'i/ G r , can be done by BRG
=
Br
BRG Gr
(2.117)
= IBRGI = 1
(2.118)
G R- I -
(2.119)
where,
IGRBI and
BR G --
B -
G RT
B'
Proof. Decomposition of th e unit vectors of G(OXYZ) along th e axes of B(Oxy z)
i j
K
+ (i . })j + (i . k)k (j . i )i + (j . })j + (j . k)k (K . i )i + (K .})j + (K . k)k
(i . i )i
introduces the transformation matrix global frame
[ 1] [1 'i
i
=
t.
i .) j .}
G RB
Z.~]
J ·k
K ·} K · k
(2.120) (2.121) (2.122)
to map the local frame to th e
(2.123)
64
2. Rotation Kinematics
where
[ ii J. i k .i
GRB
l .j J.j 1< . j
[ eos(i,i)
i k ]
J·k 1<.1.
cos(l,j) cos(J,j) cos(K ,j)
cos(J,i) cos(l< , i)
cos(i,k) ]
cos(~,~) cos(K, k)
.
(2.124)
Each column of G RB is decomposition of a unit vector of the local frame B(Oxyz) in the global frame G(OXYZ)
GR
n
~ ~i ~J ~k ] ~ [+++- ]
(2.125)
[
Similarly, each row of G RB is decomposition of a unit vector of the global frame G( OXYZ) in the local frame B( Oxyz)
GRB~ [ =:l • [= :;: =]
(2.126)
The elements of G RB are direction cosines of the axes of G( OXY Z ) in frame B(Oxyz) . This set of nine direction cosines then completely specifies the orientation of the frame B(Oxyz) in the frame G(OXYZ), and can be used to map the coordinates of any point (x, y, z) to its corresponding coordinates (X, Y, Z) . Alternatively, using the method of unit vector decomposition to develop t he matrix B RG leads to
BRGGr=GR1/Gr
i.~ i.~ i'~ ]
[
j.! j .J j ·K 1.·1 1. ·J 1..1< cos(i,~)
cos(i, ~) cos(j, J) cos(1.,J)
cos(j, I) [ cos(1.,1)
cos(i,~)
cos(j, K)
] (2.127)
cos(1. ,I<)
and shows that the inverse of a transformation matrix is equal to the transpose of the transformation matrix,
GR- 1 B
_ -
GRT
B'
(2.128)
A matrix with condition (2.128) is called orthogonal. Orthogonality of
R comes from this fact that it maps an orthogonal coordinate frame to another orthogonal coordinate frame .
2. Rotation Kinematics
65
The transformation matrix R has only three independent elements. The constraint equations among the elements of R will be found by applying the orthogonality condition (2.128). G RB" G R~ T13 ] T23
[
T33
Tn
T12 T13
I
~~~ ~:~]
T23
(2.129)
[~~ ~]. (2.130) 0 0 1
T33
Therefore, the dot product of any two different rows of G RB is zero, and the dot product of any row of G RB with the same row is one . 222 T 12 T 13 2 2 2 T2 2 T 23 T21 2 2 2 T32 T 33 T31
"ii
+ + +
+ + +
1 1 1
o o o
+ T12 T22 + T13 T23 ru T31 + T12 T32 + T13 T3 3 T21 T 31 + T22 T32 + T23 T33 Tn T21
(2.131)
These relations are also true for columns of G R B , and evidently for rows and columns of B RG. The orthogonality condition can be summarized in the following equation: 3
f IIi . f II j
=fhi t», =
(j, k = 1,2,3)
L:>ijTik =6jk
(2.132)
i=l
where Tij is the element of row i and column j of the transformation matrix R , and 6jk is the Kronecker 's delta 6jk
= 1 if j = k , and
6jk
= 0 if j -I- k.
(2.133)
Equation (2.132) gives six independent relations satisfied by nine direction cosines. It follows that there are only three independent direction cosines. The independent elements of the matrix R cannot obviously be in the same row or column, or any diagonal. The determinant of a transformation matrix is equal to one, (2.134) because of Equation (2.129), and noting that
IGRBI 'IGR~I
IGRBI ·IGRBI 2
G
I RB
I
= 1.
(2.135)
66
2. Rot ation Kinematics
x
y
FIGURE 2.19. Body and global coordina te frames of Exampl e 28.
Using linear algebra and row vectors that
r HI ' r H 2 , and r H 3 of G RB, we know (2.136)
and becau se the coordinate syste m is right handed , we have rH 2 < i n, so IGRBI = rk l . rH I = 1. •
= rH I
Example 28 Elem ents of transformation matrix. Th e position vector r of a point P may be expressed in terms of its components wit h respect to either G(OXYZ) or B(Oxyz) fram es. Body and a global coordinate fram es are shown in Figure 2.19. If G r = 100f 50} + 150K, and we are looking for components of r in the Oxyz fram e, then we hav e to find the proper rotation matrix B RG first . Th e row eleme nts of B fl G are the direction cosin es of the Oxyz axes in the 0 XY Z coordinate frame . Th e x -axis lies in the X Z plan e at 40 deg from the X -axis, and the angle between y and Y is 60 deg. Therefore,
cos ~O
)·I [ k ·j
0.76,6 )·I
[ k ·j
(2.137)
2. Rotation Kinematics
67
and by using BRG GR B = B R G BR'[; = I
u~ n
0.766 0 0.643] [0.766 r21 r 31] r21 0.5 r23 0 0.5 r32 [ r31 r32 r33 0.643 r23 r33
(2138)
we obtain a set of equations to find the missing elements. 0.766r21 + 0.643 r23 0.766r31 + 0.643 r33
0
r~l + r~3 + 0.25 r21 r31 + 0.5r32 + r23r33
1
222
r 31 + r32
+ r33
0
=
(2.139)
0 1
Solving these equations provides the following transformation matrix:
BR G
=
0.766 0 0.643 ] 0.557 0.5 -0.663 [ -0.322 0.866 0.383
(2.140)
and then we can find the components of B r . B RGG r
0.766 0 0.643 ] [ 100 ] 0.557 0.5 -0.663 - 50 [ -0.322 0.866 0.383 150
[~~~'.~~ ]
(2.141)
-18.05
Example 29 Global position, using B r and B R G. The position vector r of a point P may be described in either G (0 XY Z) or B (Oxy z) frames. If B r = 100£- 50} + 150k, and the following B R G is the transformation matrix to map G r to B r B RGG r
0.766 o 0.643 ] 0.557 0.5 -0.663 [ -0.322 0.866 0.383
Gr
(2.142)
68
2. Rot ation Kinematics
then the compon ents of G r in G (OXY Z) would be GRB Br BR'[;B r
0.766
0.557 - 0.322 ] [ 100 ] 0.5 0.866 -50 [ 0.643 -0.663 0.383 150
o
0.45 ] 104.9 . [ 154.9
(2.143)
Example 30 Two point s tran sformation matrix. Th e global position vector of two points, Hand P2 , of a rigid body B are
1.077 ] 1.365 [ 2.666
(2.144)
-0.473 ] 2.239 . [ -0.959
(2.145)
Th e origin of the body B (Oxyz ) is fixed on the origin of G ( 0 XY Z) , and the points PI and P2 are lying on the local x -axis and y -axis respectively. To find G R B' we use the local unit vectors G i and G j G
---.!.!l = IGrp11
to obtain
0.338 ] 0 429 0:838
(2.146)
-0.191 ] 0.902 [ -0.387
(2.147)
[
Gk G'
k
ixj = ij
0.~38
[ -0.429 -0.922 ] -0.029 [ 0.387
-0.838
o 0.338
0.429 ] [ -0.191 ] -0.338 0.902 o -0.387 (2.148)
where 1: is the skew-sym me tric m atrix correspon ding to i, and i j is an alternative for i x j .
2. Rot ation Kinematics
69
Hence, the transformation matrix using the coordinates of two points and G r p2 would be
G r p1
G
RB
=
[ Gi
Gj
Gk]
0.338 -0.191 0.429 0.902 [ 0.838 -0.387
-0.922 ] -0.029 . 0.387
(2.149)
Example 31 Length invariant of a position vector. Describing a vector in different fram es utilizing rotation matrices does not affect the length and direction properties of the vector. Th erefore, length of a vector is an inv ariant
(2.150) Th e length invariant property can be shown by
Irl 2
=
GrT G
r
[GRBBr( GR B B r B rTG R~G RBB r
B rT B r .
(2.151)
t:
Example 32 Sk ew symmetri c matrices for i, 3, and Th e definition of skew symmetric matrix a corresponding to a vector a is defin ed by
(2.152) Hence,
,~ [~ ~ ~!]
(2.153)
~ [J! ~ ~]
(2.154)
j
k~ [! ~!
n
Example 33 In verse of Euler angles rotation matrix. Precession-nutation-spin or Eul er angle rotation matrix (2.63)
(2.155)
70
2. Rotation Kinematics
A z ;lI,Ax,e A z ,'P
Ci.pc7/J - c() Si.ps7/J -Ci.ps7/J - c()c7/JSi.p [ s()Si.p
c7/JSi.p + c()Ci.ps7/J -Si.ps7/J + c()Ci.pc7/J -Ci.ps()
s()s7/J ] s()c'IjJ
(2.156)
c()
must be inverted to be a transformation matrix to map body coordinates to global coordinates. BR- l
AT AT AT
z ,'P x,e z ,,p Ci.pc7/J - c()Si.ps7/J -Ci.ps7/J - c()c7/JSi.p c7/Jsi.p + c()Ci.ps7/J -Si.ps7/J + c()Ci.pc7/J [ s()s7/J s()c7/J G
=
s()Si.p ] -Ci.ps() (2.157) c()
The transformation matrix (2.156) is called a local Euler rotation matrix, and (2.15'7) is called a global Euler rotation matrix.
*
Example 34 Group property of transformations. A set 5 together with a binary operation @ defined on elements of 5 is called a group (5, @) if it satisfies the following four axioms.
1. Closure: If SI, S2
E
5, then SI @ S2
E
5.
2. Identity: There exists an identity element So such that So s @ So = s for leis E 5.
@
s
3. Inverse: For each s E 5, there exists a unique inverse S-1 E 5 such that S-I @S=S @S-1 = so.
4·
Associativity: If SI, S2 , S3 E 5 , then (SI @ S2) @S3 = SI @ (S2
@
S3)'
Three dimensional coordinate transformations make a group if we define the set of rotation matrices by (2.158)
Therefore, the elements of the set 5 are transformation matrices R i , the binary operator @ is matrix multiplication , the identity matrix is I, and the inverse of element R is R:' = R T .
5 is also a continuous group because 5. The binary matrix multiplication is a continuous operation; and 6. The inverse of any element in 5 is a continuous function of that element .
2. Rotation Kinematics
71
Therefore, S is a differentiable manifold. A group that is a differentiable manifold is called a Lie group.
*
Example 35 Transformation with determinant -1 . An orthogonal matrix with determinant +1 corresponds to a rotation as described in Equation (2.134). In contrast, an orthogonal matrix with determinant -1 describes a reflection. Moreov er it transforms a right-handed coordinate syst em into a left-handed, and vice versa. This transformation does not correspond to any possible physical action on rigid bodies. Example 36 Alternative proof for transformation matrix. Starting with an identity
(2.159)
we may write (2.160)
Since matrix multiplication can be performed in any order we find
[i]
=
Z·i (3 (~][~] J . 3 J ·k J [ J. i
k .: k'3 k·k
k
(2.161)
where, GRB __ [
~Z" ] [i3
k] .
(2.162)
k].
(2.163)
Following the same method we can show that
BR,, =[i] [j
j
2.10 Active and Passive Transformation Rotation of a local frame when the pos ition vector G r of a point P is fixed in global frame and does not rotate with the local frame , is called passive
72
2. Rotation Kinematics
z ,....----
y
x x
FIGURE 2.20. A position vector r, in a local and a global frame.
transformation. Alternatively, rotation of a local frame when the position vector B r of a point P is fixed in the local frame and rotates with the local frame , is called active transformation. Surprisingly, the passive and active transformations are mathematically equivalent . In other words, the rotation matrix for a rotated frame and rotated vector (active transformation) is the same as the rotation matrix for a rotated frame and fixed vector (passive transformation) . Proof. Consider a rotated local frame B (Oxy z) with respect to a fixed global frame G(OXYZ) , as shown in Figure 2.20. P is a fixed point in the global frame , and so is its global position vector Gr. Position vector of P can be decomposed in either a local or global coordinate frame, denoted by B r and G r respectively. The transformation from G r to B r is equivalent to the required rotation of the body frame B(Oxyz) to be coincided with the global frame G(OXYZ). This is a passive transformation because the local frame cannot move the vector Gr. In a passive transformation, we usually have the coordinates of P in a global frame and we need its coordinates in a local frame ; hence , we use the following equation: (2.164) We may alternatively assume that B(Oxyz) was coincident with G(OXY Z) and the vector r = B r was fixed in B(Oxyz) , before B(Oxyz) and B r move to the new position in G(OXY Z). This is an active transformation and there is a rotation matrix to map the coordinates of B r in the local frame to the coordinates of G r in global frame. In an active transformation, we usually have the coordinates of P in the local frame and we need its
2. Rotation Kinematics
73
coord inates in t he global frame; hence, we use the following equation:
(2.165)
• 2.11
Summary
Two Cartesian coordinate frames with a common origin are convertible based on nine directional cosines of a frame in the other. The conversion of coord inates in t he two frames can be cast in matrix transformation Gr
GRBB r
[ ~n
i .j
[ JIi·1,
I k ]
J .j
J ·k k .1, K ·j k ·k
[~n
(2.166) (2.167)
where, GR B
=
[ cos(i , i) cos(i ,j) cos(i,k) cos(J,1,) cos(J,j) cos(J, k) cos(k, 1,) cos(k ,j) cos(k , k)
The transformation matrix equal to its transpose
G RB
]
(2.168)
is orthogonal and therefore its inverse is
GR- 1 B
_ -
GRT
B'
(2.169)
T he orthogonality condition generat es six equations between the elements of G R B t hat shows only three elements of G R B are independent. Any rotation can be decomposed into three simpler rotations about t he principal coordinate axes in eit her the B or G frame .
2. Rotation Kinematics
75
Exercises 1. Notation and symbols.
Describe th e meaning of a- G r g-
BR - 1
m- k
G
b- G r p
c-
h-
i- 2d 1
Gd B
n- j
0-
d-
B rp
GR B
j- Qx
A;,cp
p-
e,p
e- G R~
f-
k- QY,f3
1- Qy,~5
q- i
r- I .
B RG
2. Body point and global rotat ions. Th e point P is at r p = (1,2 ,1 ) in a body coordinate B(Oxyz). Find the final global posit ion of P after a rotation of 30 deg about the X -axis, followed by a 45 deg rot ati on ab out th e Z-axis. 3. Body point after global rot ation. Find the position of a point P in the local coordinate, if it is moved to G r p = [1 ,3 , 2jT after a 60 deg rotat ion about t he Z-axis. 4. Invariant of a vector. A point was at B r p = [1 ,2 , zjT. After a rot ation of 60deg about the X -axis, followed by a 30 deg rot ation about the Z-axis, it is at G rp =
X ]. Y [2.933
Find z, X , and Y . 5. Const ant length vector. Show th at t he length of a vector will not change by rot ation .
Show th at the dist ance between two body point s will not change by rotation.
6. Repeated global rotations. Rotat e B r p = [2,2 , 3jT, 60 deg about th e X -axis, followed by 30 deg about the Z-axis. Th en, repeat th e sequence of rot ations for 60 deg about th e X -axis, followed by 30 deg about t he Z-axis. After how many rot ations will point P be back to its initial global position?
76
2. Rotation Kinematics 7.
* Rep eated global rot ations. How many rotations of a = 1l" I m deg about t he X -axis, followed by /3 = 1l" Indeg about the Z-axis are needed to brin g a body point to its init ial global posit ion, if m , n E N?
8. Tripl e global rot at ions. Verify t he equations in App endix A. 9.
* Special t riple rotation . Assume t hat t he first t riple rotation in Appendix A brings a bod y point back to its initi al global position. Wh at are t he angles a i- 0, /3 i- 0, and I i- a?
10.
* Combination of triple rot ations.
Any t riple rot ation in App endix A can move a bod y point to its new global position . Assume a 1, /3 1' and 1 1 for t he case l-Q X.'Yj QY,{3j Q Z,Oj are given. Wh at can a2 , /32' and 1 2 be (in terms of a1 , /31' and 1 1) to get the same global posit ion if use t he case 2 - Qy,'YZQZ,{32Qx,oz? 11. Global roll-pitch-yaw rot ation matri x. Calculate t he global roll-pitch-yaw rotation matri x for a = 30, /3 = 30, and I = 30. 12. Global roll-pitch-yaw rotati on angles. Calcul at e t he role, pit ch, and yaw matrix: 0.53 BRe = 0.0 [ -0.85
angles for t he following rot ation -0.84 0.13 ] 0.15 0.99 -0.52 0.081
13. Body point, local rot at ion. Wh at is the global coordinates of a bod y point at after a rotation of 60 deg about t he x-axis?
Br p
=
[2 ,2,3jT ,
14. Two local rotat ions. Find t he global coordinat es of a body point at B r p = [2, 2, 3jT after a rotation of 60 deg about the x-axis followed by 60 deg about the z-ax is. 15. Tripl e local rotations. Verify t he equat ions in App endix B. 16. Combination of local and global rot ations. Find t he final global position of a body point at D r p = [10, 10, -lOjT afte r a rot ation of 45 deg about the x-axis followed by 60 deg about the Z- axis.
2. Rotation Kinematics
77
17. Combination of globa l and local rot ations. Find t he final global position of a body point at B r p = [10,10 , -lOjT afte r a rot ation of 45 deg about the X -axis followed by 60 deg about the z-axis.
18. Repeated local rotations.
3V,
Rot at e B r p = [2,2 , 60 deg about the z -axis, followed by 30 deg about the z-axis, Then repeat t he sequence of rot ations for 60 deg about t he x-axis, followed by 30 deg about the z-axis. After how many rot ations will point P be back to its initial global position? 19.
*
Rep eated local rot ations .
How many rotations of ex = 1r [m deg about the x-axis, followed by (3 = 1r Indeg about t he z-a xis are needed to bring a body point to its init ial globa l position if m , n E N? 20.
*
Remain ing rotation.
Find th e result of the following sequence of rotations:
21. Angles from rotation matrix. Find the angles ip, (), and e if the rotation of t he case 1- A x ,1jJ A y,oA z ,'I' in App endix B is given. 22. Euler angles from rot ation matri x. Fi nd th e Euler angles for the following rotati on matri x: BRG =
0.53 0.0 [ -0.85
-0.84 0.13 ] 0.15 0.99 -0.52 0.081
23. Equi valent Eul er angles to two rot ations. Find th e Eul er angles corres ponding to t he rotat ion matrix
B RG
=
B RG
=
Gr p
=
A y ,45 A x ,30 . 24. Equivalent Euler angles to t hree rotations.
Find th e Eul er angles corres ponding to t he rot ation matri x A z ,60 A y ,45 A x ,30 . 25.
*
Local and globa l positions, Euler angles.
F ind t he conditions between t he Euler angles to tran sform [1 ,1,0jT to B r p = [0,1 , IV.
78 26.
2. Rotation Kinematics
* Equ ivalent Euler angles to a triple rot ati ons. Find the Euler angles for the rot at ion matrix of the case
in Appendix B. 27.
*
Integrability of Euler frequencies.
Show th at dip and d'ljJ are integrable, if sp and 'ljJ are first and third Euler angles. 28.
*
Carda n angles for Euler angles.
Find the Cardan angles for a given set of Euler angles. 29.
* Card an frequencies for Euler frequencies. Find the Euler frequencies in terms of Card an frequencies.
30. Elements of rot ation matrix.
Th e elements of rot ation matrix
G RB
G RB
are
cos(l ,i) cos(l, j) cos(l ,k) ] = cos( ~,i) cos( ~, j) cos(J, k) . [ cos(K , i) cos(K, j ) cos(k ,k)
Find G R B if G r p 1 = (0.7071, -1.2247, 1.4142) is a point on the x-axis, and G r P2 = (2.7803,0.38049, - 1.0607) is a point on the y-axis. 31. Linearly independent vectors .
A set of vectors a ll a2 , . . ., an are considered linearly independent if th e equat ion k1al
in which k 1 , k 2 ,
. . .,
+ k2a2 + ...+ k nan = 0
k n are unknown coefficients, has only one solution k1
= k 2 = ... = k n = o.
Verify that th e unit vectors of a bod y frame B(Oxyz) , expressed in the global frame G(OXYZ) , are linearly indep endent . 32. Product of orthogonal mat rices.
A matrix R is called orthogonal if R- 1 = RT where (RT) ij = n.; Prove that th e produ ct of two orthogonal matri ces is also ort hogonal. 33. Vector identity.
Th e formula (a + b)2 = a2 + b2 + 2ab for scalars, is equivalent to (a + b) 2
= a . a + b . b + 2a . b
2. Rotation Kinematics
79
for vectors. Show that this formula is equal to
if a is a vector in local frame and b is a vector in global frame. 34. Rotation as a linear operation. Show that
R (a x b) = Ra x Rb where R is a rot ation matrix and a and b are two vectors defined in a coordinate frame. 35. Scalar tripl e produ ct. Show that for three arbit rary vectors a , b , and c we have
a· (b x c) = (a x b) . c.
3
Orientation Kinematics 3.1
Axis-angle Rotation
Two par ameters are necessary to define the direction of a line through 0 and one is necessary to define the amount of rot ation of a rigid body about thi s line. Let th e bod y frame B( Oxy z) rot at e ¢ about a line indicat ed by a unit vector u with direction cosines Ui , U 2 , U3,
u = u i i + u 2 J + U 3K ui + u~ + u~ = 1.
(3.1)
J
(3.2)
This is called axis-angle representation of a rotation.
z
z
G
y
x x
FIG UR E 3.1. Axis of ro tation il when it is coincident with t he local z-axis.
A transformat ion matrix G R B th at maps the coordin at es in th e local frame B (Oxy z) to t he corresponding coordinates in the global frame G(OXYZ) ,
(3.3) is G RB =
R u,¢ = I cos ¢
ui vers ¢ + c¢
G RB
=
[
vers ¢ + U 3S ¢ U i U 3 vers ¢ - U 2S¢ Ui U2
+ uuT vers ¢ + ii sin ¢
vers ¢ - U 3S ¢ u~ vers ¢ + c¢ U2U3 vers ¢ + Ui s¢ Ui U2
(3.4)
vers ¢ + U 2 S ¢ ] vers ¢ - Ui s¢ u~ vers ¢ + c¢ (3.5)
Ui U 3
U2U3
82
3. Orientation Kinematics
where vers ¢
versine ¢
1- cos ¢ 2sin and
2
(3.6)
~
u is t he skew-symmetric matrix corresponding to th e vector it (3.7)
A matrix
u is skew-symmetric
if
uT
=
-u.
(3.8)
The transformation matrix (3.5) is th e most general rotation matrix for a local frame rot ating with respect to a global frame. If th e axis of rot ation (3.1) coincides with a global coordinate axis, th en the Equ ations (2.21), (2.22), or (2.23) will be reproduced. Proof. Int erestingly, t he effect of rotation ¢ about an axis it is equivalent to a sequence of rot ations about the axes of a local frame in which th e local fram e is first rotated to bring one of its axes, say the z-axis, into coincidence with th e rotation axis ii, followed by a rot ation ¢ about t hat local axis, then t he reverse of the first sequence of rotation s. Figure 3.1 illustrat es an axis of rotation ii = u 1i +u 2J +U3K , th e global frame G (0 XY Z) , and t he rotated local frame B (Oxyz ) when th e local z-axis is coincident with ii. Based on Figure 3.1, th e local frame B (O xyz) undergoes a sequence of rotations ip about th e z-axis and e about t he y-axis to brin g the local z-axis into coincidence with th e rot ation axis ii, followed by rot ation ¢ about ii, and t hen perform the sequence backward . Therefore, using (2.110), the rotation matrix G R B to map coordinates in local frame to th eir coordinates in global frame afte r rotation ¢ about it is
(3.9) but sin tp = sine =
e
U2 ~::::;;;:==;;:
(3.10)
y'ui + u~
JUI + u~
sin sin tp =
U2
cos e = U3 sin F cos e
= Ul
(3.11) (3.12)
3. Orientation Kinematics
83
hence, GRB
= Ru,¢
(3.13)
uI vers ¢ + c¢
[
vers ¢ - U 3S ¢ u§ vers ¢ + c¢ U2U3 vers ¢ + Ul S¢ Ul U 2
vers ¢ + U3S¢ Ul U3 vers ¢ - U2S ¢ Ul U2
vers ¢ + U2S ¢ vers ¢ - U lS¢ u~ vers ¢ + c¢
Ul U 3
U 2U3
] .
The matrix (3.13) can be decomposed to
R u,¢
[1 00]
cos ¢
=
0 1 0 0 1
o
+ (l - cos f) [
~: ] [ U,
~3
+ sin f [
- U2
- U3
U2
0
- Ul
Ul
0
U2
U3 ]
]
(3.14)
to be equal to G RB
= Ru,¢ = Lcos e + uv? vers ¢ + usin ¢ .
(3.15)
Equat ion (3.4) is called t he R odriguez rotation f ormula (or the Euler-LexellRodriguez form ula) . It is sometimes reported in literature as the following equivalent forms:
R u,¢ = I
+ (sin ¢) u + (vers ¢) u2
Ru,¢ = [I-uuT ] cos ¢ + u sin ¢ + uuT R u,¢ = _u 2 cos ¢ + usin ¢ + u2 + I
(3.16) (3.17) (3. 18)
Th e inv erse of an angle-axis rotation is B
G R~
RG
=
= Ru,- ¢
L cos o
+ uuT vers ¢ -
usin ¢ .
(3.19)
It means orientation of B in G, when B is rotated ¢ abou t U, is the same as the orientation of Gin B , when B is rot ated - ¢ about U. The 3 x 3 real ort hogonal transformation mat rix R is also called a rotator and the skew symmetric mat rix u is called a spinor. We can verify that flu
(3.20)
I- uu
T
•
(3.21)
r Tur
o
(3.22)
ux r
ur = - f u = -r x U.
(3.23)
84
3. Orient ation Kinemat ics
Example 37 Axis-angle rotation when u = k . If th e local fram e B (O xyz) rotates about the Z -axis, then
(3.24) and th e transformation matrix (3.5) reduces to Overs
[
overs
COS
o
0
0 ver s
0]
over s
]
1 vers
(3.25)
0
1
which is equivalen t to the rotation matrix about the Z -axis of global fram e in (2.21).
Ex ample 38 Rotation about a rotat ed local axis. If th e body coordinat e fram e Oxyz rota tes 'P deg about the global Z -axis, th en th e x -axis would be along
Ux
GRZ,
A
[
=
c~~ sm c 0
- sin 'P cos 'P 0
~ ][ ~]
[ cos e ] Si~ 'P .
Rotation () about Ux formula (3.5)
G Rux,o
=
(3.26)
(cos o ) j
+ (sin e ) j
is defin ed by Rodriguez's
cos2 'P vers () + cos () cos 'P sin 'P vers () sin 'P sin () ] sin 2 'P vers () + cos () - cos 'P sin () . cos 'P sin 'P vcr s () [ cos () - sin 'P sin () cos 'P sin ()
Now , rotation 'P about th e global Z-axis follow ed by rotat ion () about the local x-axis is transfo rm ed by
GRUx ,O GR Z,
that m ust be equal to [ Ax,oAz,
- 1
T = A Tz,
sin ()sin 'P ] - cos 'P sin () cos ()
(3.27)
3. Orientation Kinematics
85
Example 39 A xis and angle of rotation. Given a transformation matrix C R B we may obtain the axis it and angle ¢ of the rotation by considering that
(3.28) (3.29)
cos¢ because
CR B
-
0 2 (sin ¢) U 3 [ -2 (sin ¢) U2
CRT B
2sin ¢ [
-2 (sin e) U 3
o 2 (sin ¢)u1
~3
-U2
(3.30)
2usin ¢ and r ll
+ r22 + r 33
3 cos ¢ + ui (1 - cos ¢ ) + u~ (1 - cos ¢) + u~ (1 - cos ¢ ) 3cos ¢ + ui + u~ + u~ - (ui + u~ + u~) cos¢ 2 cos ¢+1.
(3.31)
Example 40 A xis and angel of a rotat ion matrix. A body coordinate fram e, B , undergoes three Eul er rotations (tp,(},'l/J) = (30,45 ,60) deg with respect to a global fram e G . Th e rotat ion matrix to tran sform coordin ates of B to G is B
R~ = [Rz,,pRx,oRz,
R;'cpR; ,oR;',p 0.12683 -0.92678 0.35355 ] 0.78033 -0.12683 -0.61237 . [ 0.61237 0.35355 0.70711
(3.32)
Th e unique angle-axis of rotat ion fo r this rotation matrix can then be found by Equations (3.28) and (3.29).
cos- 1 (~ (tr
(CR B) - 1))
cos" (-0.14645) = 98deg
(3.33)
86
3. Orientation Kinemati cs
(G
1
G T)
-sm 2o'"' RB - RB 0.0 -0.86285 0.86285 0.0 [ 0.13082 0.48822
-0.13082 ] -0.48822 0.0
(3.34)
(3.35) As a double check, we may verify the angle-axis rotation fo rm ula and derive the same rotation matrix. G RB
=
Ru,
0.12682 -0.92677 0.78032 -0.12683 [ 0.61236 0.35355
0.35354] -0.61237 0.70709
(3.36)
Example 41 Non-uniqueness of angle-axis of rotation. Th e angle-axis representation of rotation is not unique. Rotation (8, it) is equal to rotation (-8, -u) , and (8 + 27f, u).
*
Example 42 Skew-symm etric characteris tic of rotation matrix. Tim e derivative of the orthogonality condition of rotation matrix (2.129)
:t
(G
R~ G R B)
leads to [G
showing that
[GR~ GRB ]
= G R~ G RB
R~ G RB
r
= - G
+ G R~ G RB =
R~ G RB
0
(3.37)
(3.38)
is a skew-symmetric matrix.
If we show the rotation matrix by its elem ents , R B = [rij] .
G RB
G '
*
Example 43 Angula r velocity vector of a rigid body We show the skew-symme tric matrix [GR~ GRB ] by W = G R~G RB
(3.39)
then, we find the following equation for the tim e derivative of rotation m atrix
(3.40) where w is the vector of angular velocity of the fram e B(Oxyz) with respect to fram e G(OXYZ) , and w is its skew-symmetric matrix.
3. Orient ati on Kinemati cs
87
*
Example 44 Differentiating a rotat ion matrix with respect to a param eter. Suppose that a rotat ion m atrix R is a fun ction of a variable T,. hence, R = R (T). To find the different ial of R with respect to T, we use the orthogonality characteris tic (3.41) and take derivative of both sides (3.42)
which can be rewritten in the follo wing form , (3.43)
showing that [~~ R T ] is a skew symmetri c matrix.
*
Ex ample 45 Eigenvalues and eigenvectors of G RB. Consider a rotation matrix G R B. Applying the rotat ion on the axis of rotation u cann ot change its direction (3.44)
so, the transform ation equation implies tha t (3.45)
Th e characteristic equation of this determ ina nt is (3.46)
Factoring the left-hand side, gives (3.47 )
and shows that >'1 = 1 is always an eigenvalue of G RB. Hen ce, there exist a real vector U, such that every point on the line in dicated by vector nl = U rema ins fix ed and in variant und er transformation G RB . Th e rotat ion angle cP is defin ed by
cos cP = '21 (tr (GR B) -l ) ='21 (r11+r22 + r33 -1 )
(3.48)
then the remaining eigenvalues are
A2 = ei = cos cP + i sin cP
(3.49)
A3 = e- i = cos cP - i sin cP
(3.50)
88
3. Orientation Kinematics
and their associat ed eigenvectors are v and v, where v is th e compl ex conjugate of v. Sinc e G R B is orthogonal, nj , v , and v are also orthogonal. Th e eigenve ctors v and v span a plan e perpendi cular to the axis of rotation nj , A real basis fo r this plan e can be found by using the following vectors: (3.51) Z
U3 = -Iv 2
vi
(3.52)
Th e basis U2 and U3 tronsjorni to
1
11 ' '
"2IA2v + A3V l = "2 et",v v cos ¢
+ et
+ v sin ¢
i
(3.53)
I I ''
-.
"2IA2V - A3Vl ="2 et",v - e'
+ v sin ¢.
(3.54)
Th erefore, the effe ct of the transformation G R B is to rotate vect ors in the plane spanned by U2 and U3 through angle ¢ about nj , while vecto rs along Ul are inv ariant.
3.2
* Euler Parameters
Assum e t ha t ¢ is the angle of rotation of a local frame B (Oxy z) about U = u l i + u 2J + U3K relative to a global frame G( OXY Z). The existence of such an axis of rot at ion is the an alyti cal repr esent ation of the Euler 's theorem about rigid body rotation: the most gen eral displacem ent of a rigid body with on e point fixed is a rotation about some axis. To find t he axis and angle of rotation we introduce the Eul er parameters eo, el , e2, e3 such that eo is a scalar and el , e2, e3 are components of a vector e, eo
(3.55)
e
(3.56)
and, (3.57)
3. Orientation Kinematics T hen, the transformation matrix G RB to satisfy t he equation G r = can be derived uti lizing the Eu ler parameters
where
89
G RB B r ,
e is t he skew-symm etric matrix corresponding to e defined below. (3.59)
Euler parameters provide a well-suited, red undant, and nonsingular rotation description for arbitrary and large rotations. Proof. Figure 3.2 depicts a point P of a rigid body with position vector r , and the unit vector u along t he axis of rotation ON fixed in t he global frame. The point moves to P' with position vector r' after an active rotation ¢ abo ut u. To obtain the relationship between r and r' , we express r ' by t he following vector equation: ----+
----+
-7
r ' = ON + N Q+ QP' .
(3.60)
By investigating Figure 3.2 we may describe Equation (3.60) utili zing r , r' , ii, an d ¢. (r . u) u + u x (r xu) cos ¢ - (r xu) sin ¢
r'
(r . u) u + [r - (r . u) u] cos ¢
+ (u x r ) sin ¢
(3.61)
Rearranging Equation (3.61) leads to a new form of t he Rodriguez rotation formula r ' = r cos ¢ + (1 - cos ¢) (u · r ) u + (u x r ) sin ¢ . (3.62) Using the Euler parameters in (3.55) and (3.56), along with t he following t rigonometric relations cos¢ sin¢ 1 - cos¢
2 cos2 -¢ - 1 2 2 sm. ¢ cos -¢ 2 2 2sin
2
~
(3.63) (3.64) (3.65)
converts the Rodriguez formula (3.62) to a more useful form, r'
= r (2e6 - 1) + 2e (e . r) + 2eo (e x r ) .
(3.66)
90
3. Orientation Kinematics
o
x
y
FIGURE 3.2. Axis and angle of rotation .
Using a more compact form and defining new not ations for r' r
=
= G r and
Br G
r = (e5 -e2) Br+2e(e T Br) + 2eo(e Br)
allows us to factor out the position vector meter transformation matrix G RB
Br
(3.67)
and extract t he Euler para-
(3.68) where (3.69)
• Example 46
* Axis-angl e rotation of
GRB •
Euler param eters for rotation ¢ = 30 deg about are
eo = e
30 cos 2"'
u=
(j + J + k)/J3
= 0.966
(3.70)
¢ usin'2= e1 I + e2J+ e3K
0.259(i +J +k)
(3.71)
therefore, the corresponding transformation matrix
GRB =
0.866 0.635 [ - 0.366
- 0.366 0.866 0.635
G RB
0.635 ] - 0.366 . 0.866
is
(3.72)
3. Orient at ion Kinematics
Henc e, the global position of a point at
Gr=GRBBr =
Br
= [ 1
0 0
r
91
is
0.866 ] 0.635 . [ -0.366
(3.73)
Example 47 A xis-angle rotation from G R B . Given a transformation matrix G RB we may obtain the angle of rotation ¢ and th e axis of rotation u by considering that
~ (tr (GR B ) - 1)
cos¢
1
2 (ru + r22 + r 33 - = -2 .1
U
d..
sm e
(G RB
-
GRT) B
1)
(3.74)
(3.75)
,
because tr
(G R B ) = 2 cos ¢ + 1
0
- 2 (sin ¢ ) U 3
(3.76)
and
2 (sin ¢ ) U3 [ -2 (sin ¢) U 2
o
2 (sin o ] Ul
2 (sin ¢ ) U 2 - 2 (sin ¢)ul 0
]
(3.77)
and therefore, (3.78)
*
Example 48 Eul er param eters from G R B . Employing a transformation matrix G RB we can find the Eul er param eters by (3.79) (3.80) because
and (3.82)
92
3. Orient ation Kinemati cs
Example 49
*
Eul er parameters and Eul er angles relation ship . Comparing the Eul er angles rotation matrix (2.63) and th e Euler param et er transformation matrix (3.58) we can determine th e following relationships between Euler angles and Eul er parameters =
eo
e
7jJ + ip
. e
7jJ -"P
(3.83)
cos "2 cos - 2 -
el
sm"2 cos - 2 -
e2
sm "2 sm--
(3.84)
. e.
7jJ -"P 2 e . 7jJ +"P cos "2 sm - 2 -
e3
(3.85) (3.86)
or "p
(3.87)
e
(3.88) (3.89)
*
Rotation matrix for angel of rotation ¢ = kn . Wh en the angle of rotation is ¢ = kn , k = ±1,±3, ..., then eo = Th erefore, the Eul er parameter transformation matrix (3.58) becom es
Example 50
o.
(3.90) which is a sym me tric matrix and indicates that rotation ¢ - kt: are equivalent.
Example 51
=
ktt , and ¢
=
*
Vector of infi nit esimal rotation. Cons ider th e Rodriguez rotation fo rm ula (3.62) fo r a differential rotation
d¢ r' = r
+ (it x r) d¢ .
(3.91)
In this case th e difference between r' and r is also ver y sm all, dr
= r' -
r
= d¢
it x r
(3.92)
an d hence, a differential rotation d¢ about an axis in dicated by the un it vector it is a vector along it with magnitud e d¢. Dividing both sides by dt leads to r= w xr (3.93) whi ch represents the global velocity vector of any point in a rigid body rotating about it.
3. Orientation Kinematics
93
*
Example 52 Exponential form of rotation. Consider a point P in the body fram e B with a position vector r . If the rigid body has an angular velocity w, then the velocity of P in the global coordinate frame is (3.94) r = w x r = wr. This is a first-order lin ear different ial equation that may be integrated to give
(3.95) where r(O) is the initial position vector of P , and ewt is a matrix exponen tial
wt _ (wt)2 (wt) 3 e = 1+ wt + - - + - - + . . . 2! 3!
(3.96)
Th e angular velocity w , has a magnitude wand direction indicated by a unit vector ii. Th erefore,
w
wu
W
wu
wt
wtu = ¢u
(3.97) (3.98) (3.99)
and hence
or equivalently e
= 1+ usin ¢ + u2 (1 - cos ¢) .
(3.101)
It is an alternative form of the Rodriguez formula showing that e
*
Example 53 Rotational charact eristic of e
5 = {R
E
IR3 X 3 : RRT = I, IRI = I}
(3.102)
we have to show that R = e cPu has the orthogonality property R T R = I and its det erminant is IRI = 1. Th e orthogonality can be verified by consid ering
(3.103) Thus R - 1 = RT and consequently RRT = I. From orthogonality, it follows that IRI = ±1, and from continuity of expone ntial fun ction , it follows that = 1. Th erefore, IRI = 1.
leol
94
3. Orientation Kinemati cs
o
x
y
FIG URE 3.3. Illustr ation of a rot ation of a rigid body to derive a new form of th e Rodriguez rotation form ula in Example 55.
Example 54 Expanding
* eit is equivalent to the rotation matrix
G R B.
eit = 1+ usin ¢ + u2 (1 - eos¢)
(3.104)
gzves
ui vers ¢ + c¢ eit =
Ul U2 vers ¢
+ U3S¢
[ Ul U3 vers ¢ -
U2S¢
Ul U2 vers ¢
- U3S¢
+ c¢ U2U3 vers ¢ + Ul s¢ u~ vers ¢
Ul U3 vers ¢ + U2S¢ ] U2U3 vers ¢ - Ul s¢ u5 vers ¢ + c¢
(3. 105) which is equal to the axis-angle Equation (3.5), and therefore, Ru, GR B l eos ¢
+ uuT vers ¢ + usin ¢ .
(3.106)
*
Example 55 New form of the Rodriguez rotation formula. Considering Figure 3.3 we may write
eos~ IMPI
sin ~ IN:MI
(3.107)
1N:MIMPi
IMPilu x N:M
(3.108)
to find
(3.109)
3. Orient ation Kinematics
95
Now using the follo wing equalities ---+
2MP'
--t --t
--t
2NM --t
--t
N P' -NP it x
--t
N P' - NP
=
(NP +FiP)
(3.110)
--t
N P' +NP
(3.111)
r' - r
(3.112)
it x (r'
+ r)
(3.113)
we can write an alterna tive f orm of the Rodriguez rotation formula
cos ~ (r' - r) = sin ~ it x (r'
+ r)
(3.114)
or
(r' - r) = tan ~it x (r' + r ) . Example 56 Th e vector
(3.115)
* Rodriguez vector. (3.116)
is called the Rodriguez vector. It can be seen that the Euler parameters are related to this vector according to
e q=eo
(3.117)
and eo e;
1
1
-}1 +qTq qi -}1 + qT q
-} 1 + q2 qi -} 1 +q2
(3.118) i = 1, 2, 3.
(3.119)
Th e R odriguez formu la (3.58) can be converted to a new form based on Rodriguez vector R u,¢ 1
= (e6 - e 2 ) I + 2e e T + 2eoe
IT +q q
((1- qTq ) I + 2qqT + 2q).
(3.120)
The combination of two rotations, q' and q" , is equivalent to a single rotation q , where q" + q' - q" x q' (3.121) q= 1 - q" ' q'
96
3. Orientation Kinematics
*
Example 57 Elements ofGR B . Introducing Lev i- Civita density or permutation sym bol Eijk
1 2
= - (i - j)(j - k)(k - i)
i,j,k = 1,2 ,3
(3.122)
and recalling Kronecker delta (2.133) Dij
= 1 if j = k , and s.; = 0 if j # k
(3.123)
we can redefine the elements of the Rodriguez rotation matrix by (3.124)
3.3
* Determination of Euler Parameters
Assume a transformation matrix is given
(3.125) It is t hen possible to find the Eu ler parameters eo, el , e2, e3 and indicate the axis and angle of rotation by utilizing one of the following four sets of equations:
eo
1 ±2v'1
el
-
e2 e3
+ rll + r22 + r33
1 r32 - r23 4 eo 1 r13 - r31 4 eo 1 r21 - r12 4 eo
el
1 ±2v'1 + rll
e2
-1 r21 + r12
e3 eo
4 el 1 r 31 + r13 4 el 1 r32 + r23 4 el
(3.126)
-
r22 - r33
(3.127)
3. Orientation Kinematics
1
±2V1-
eo
1 r32
+ r23
4
e2
+ r22 -
r33
1 r13 - r31
4
e2
1 r21
+ r12
4
e2
1
±2V1 eo
rll
97
(3.128)
rll - r22
1 r21
- r 12
4
e3
1 r31
+ r13
4
e3
1 r32
+ r23
4
e3
+ r33
(3.129)
Alt hough Equations (3.126)-(3 .129) present four different sets of solutions, their resu lting Euler parameters eo, el, e2, e3 are identical. Th erefore, numerical inaccuracies can be minimiz ed by using the set with maximum divisor. T he plus and minus sign indicates that rotation ¢ about u is equivalent to rotation - ¢ about - u. Pro of. Comparing (3.58) and (3.125) shows that for the first set of Equations (3.126), eo can be found by summing the diagonal element s rll , r22, and r33 to get tr (GRB) = 4eg - 1. To find el , e2, and e3 we need to simplify r32 - r23, r13 - r31 , and r21 - r12, respectively. The other sets of solutions (3.127)-(3 .129) can also be found by comparison. •
*
Euler parameters from E x a m ple 58 A transformation matrix is given. GRB
=
0.5449 0.3111 [ - 0.7785
G RB.
- 0.5549 0.6285 ] 0.8299 0.4629 - 0.0567 0.6249
(3.130)
To calculate the corresponding Euler parameters, we use Equation (3.126) and find rll + r22 + r33 0.5449 + 0.8299 + 0.6249 = 1.9997
(3.131)
98
3. Orientation Kinematics
therefore, eo = J(tr (GRB) + 1) / 4
= 0.866
(3.132)
and
- 0.15
0.406
(3.133)
0.25.
*
Example 59 Euler paramet ers when we have one of th em . Con sid er th e Eul er parameter rotation matrix (3.58) corresponding to rotati on ¢ about an axis indicat ed by a unit vector u . T he off diagonal eleme n ts of G R B =
eOel
1
'4 (r32 1
'4 (r 13 -
eOe2
1
r23)
r 3I)
eOe3
=
'4 (r21 -
ele2
=
'4 (r12 + r2I)
r12)
1
1
ele3
'4 (r 13 + r 31)
e2e3
'4 (rn + r32)
can be utilized to find e., i
1
= 0, 1,2,3
(3.134)
if we have one of them .
*
Example 60 Eu ler parame ters by the Stanley m ethod . Following an effe ctive m ethod developed by Stanley, we m ay fir st find the fo ur
e;
1
e02
2 (l +tr(GRB))
e21
~ (1 + 2rn -
t r (GR B))
e~
~ (1 + 2r22 -
tr (GR B))
2 e3
~ (1 + 2r33 -
tr (GRB) )
(3.135)
and tak e the positive square root of the largest e; . Th en the oth er e; are fo und by dividing th e appropriate three of the six equations (3.134) by the largest e.,
3.4
3. Orient ation Kinemati cs
* Quaternions
99
A quaternion q is defined as a quantity q
qo+q qo + ql 1, + q2) + q3k
(3.136)
where qo is a scalar and q is a vector . A quat ernion can also be shown in a flag form (3.137) q = qo + ql i + q2j + q3k where, i , j, k are flags and defined as follows
i
= k 2 = ij k = -1 - ji = k -kj = i - ik = j .
ij jk ki
(3.138) (3.139) (3.140) (3.141)
Addi tion of two quaternions is a quat ernion q+P
+ q) + (Po + p) qo + ql i + q2j + q3k + Po + Pl i + P2j + P3k (qO+ Po ) + (ql + Pl) i + (q2 + P2) j + (q3 + P3) k (qO
(3.142)
and multiplication of two quat ernion s is a quat ernion defined by qp
+ q) (Po + p) qoPo + qop + Poq + qp qoPo - q . p + qop + Poq + q (qO
xP
(qOPO - qlPl - q2P2 - q3P3)
+ PlqO - P2q3 + P3q2) i + (POq2 + qOP2 + Pl q3 - qlP3) j + (POq3 - Pl q2 + qOP3 + P2qd k + (POql
(3.143)
where qp is cross product minus dot product of q and p qp
=q
x p - q . p.
(3.144)
Quaternion addition is associative and commutative. Quat ernion mult iplication is not commutative, however it is associat ive, and distributes over addition, (pq) r
(p + q) r
P (qr)
(3.145)
pr + qr.
(3.146)
100
3. Orient ation Kinematics
A quat ernion q has a conjugate q* where
q*
qo - q qo - q1 z- q2) - q3k.
(3.147)
Th erefore, (qO+ q) (qO - q)
qq*
qoqo + qoq- Poq - qq qoqo + q . q - q x q q6 + + q~ + q5
qr
(3.148)
and t hen,
.j(j(j* =
Jq6
+
qr+ q§ + q§
q* q=~ ' 1
q-1
(3.149) (3.150)
If q is a unit quat ernion, Iql = 1, t hen «:' = q*. Let e (cP,u ) be a unit quat ernion , [e (cP,u) 1= 1,
e(cP,u)
eo + e
(3.151)
eo + e1Z+ e2) + e3k
cP
. cP ,
cos '2 + Slll'2 u and r = 0 + r be a quat ernion corresponding to a pure vector r . Th e vector r after a rot ation cP about u would be r' = e (cP,u) r eo (cP, u)
(3.152)
equivalent to (3.153) Th erefore, a rot ation R ft,q, can be defined by a corresponding quat ernion e (cP, u) = cos ~ + sin ~u , and consequently, two consecutive rotations R = R 2R1 are defined by e (cP,u) = e2 (cP2' U2 ) e1 (cP1' U1). Note th at e1 (cP1,Ut}, e2 (cP2,U2), . . . are quaternion while eo, e1, e2 , e3 are Euler paramet ers. Proof. Employing the quaternion multipli cation (3.143) we can write r e"
= eor + r x e * - r . e*
(3.154)
3. Orientation Kinematics
101
(3.155)
which G RB is equivalent to the Euler parameter transformation matrix (3.58) .
e5 + ei - e~ - e~ 2 (eOe3 + ele 2) [ 2 (el e3 - eOe2)
2 (eoe2 + ele3 ) ] 2 (e2e3 - eoed e5 - ei - e~ + e~ (3.156) Using a similar method we can also show that r = e* (¢, u) r' e (¢, u) is the inverse transformation of r' = e (¢, u) r e* (¢, u), which is equivalent to GRB =
Br
2 (el e2 - eoe3) + e~ - e~ 2 (eO el + e2e3)
e5 - ei
= e* (¢,u) Gre( ¢,u) .
Now assume el (¢l ' Ul) and e2 (¢2' U2) are the quaternions corresponding to the rotation matrix R Ui ,
(3.157)
e2 (¢ 2' U2)
(3.158)
B 2r
e; (¢2, U2)
which implies B3r = e2 (¢2,U2) el (¢l ,ud Bi r ei (¢l,Ul) e; (¢2,U2)
(3.159)
showing that
e (¢,u) = e2(¢2,U2) el (¢l ,ud is the quaternion corresponding to R
= R 2Rl .
(3.160)
•
*
Example 61 Rodrigu ez rotation formula , using quaternion. We may simplify the Equation (3.155) to have a vectorial form similar to the Rodriguez formula . r'
e (¢, u) r e*(¢,u) (e5 - e . e) r + 2eo (e x r)
+ 2e (e . r)
(3.161)
102
3. Orientation Kinematics
*
Example 62 Composit ion of rotations using quaternions. Using quaternions to represent rotations makes it easy to calculate the com position of rotat ions . If the quaternion el (¢l , Ul) represents the rotation Ru1,
e2 (¢2 ,U2) (el (¢ l ,Ul) r ei (¢l ,ut}) e; (¢2 ,U2) (e2 (¢2 ,U2) el (¢ l ,Ul)) r (ei (¢ l , Ut} e; (¢ 2, U2)) (e2 (¢2 ,U2) el (¢l,Ul)) r (el (¢l,Ul) e2 (¢ 2, U2))* .
*
Example 63 In ner automorphism propert y of e (¢, u). Since e (¢, u) is a unit quat ernion, e" (¢,u)
= e- l (¢,u)
(3.162)
we may write (3.163)
In abstract algebra, a mapp ing of the f orm r = q r q-l , computed by multiplying on the left by an eleme n t and on the right by its inve rse, is called an in n er automorphism. Thus, G r is th e inne r automorphism of B r based on the rotat ion quat ern ion e (¢ , u) .
3.5
* Spinors and Rot ators
Fin ite rot at ions can be expressed in two general ways: using 3 x 3 real orthogonal matri ces R called rotator, which is an abbreviation for rotation tensor ; and using 3 x 3 rea l skew-sym met ric mat rices u called spinor, which is an abbreviation for spin tensor. A rotator is a linear operator t hat maps B r to G r when a rotat ion axis U and a rotation angle ¢, are given. (3.164)
T he spinor u is corres ponding to t he vector U, which, along with ¢, can be utilized to describe a rotation. (3.165)
For t he moment let 's forget lui = Jui + u~ + u5 = 1 and develop the theory for non-unit vectors indicting the rotation axis. T he square of u,
3. Orientation Kinematics
103
computed through direct multiplication, is
-U22 - U32 UIU2 UIU3
[
UIU2 -UI - U5 U2 U3
- -T -UU = -U-T-U
ui)7 -
U
2
r.
(3.166)
This is a symmetric matrix with tr[u2] = -2!uI 2 = -2u 2 = -2(UI+U~+u5) whose eigenvalues are 0, _u 2 , -u2 . In other words, ii satisfies its own characteristic equation (3.167) The odd powers of u are skew symmetric with distinct purely imaginary eigenvalues , while even powers of u are symmetric with repeated real eigenvalues. Spinors and rotators are functions of each other so R must be expandable in a Taylor series of U. (3.168) However, because of (3.167), all powers of order three or higher may be eliminated. Therefore, R must be a linear function of I , U, and 2 .
u
R =
1 + a(Au) + b(AU)2
(3.169)
+ u5) + 1 -aAU3 + bA2uIU2 aAu2 + bA2uIU3 ] aAu3 + bA2 U 1U2 -bA 2(uI + u5) + 1 -aAul + bA2U2U3 [ aAul + b).2uZU3 -bA 2(uI + u~) + 1 -aAuz + bA2uIU3 -bA2(u~
where, A is the spinor normalization factor , while a = a(¢, u) and b = b(¢, u) are scalar functions of a rotation angle and an invariant of U. Recalling U = JUI + u~ + u5 = 1, Table 3.1 collects some representations of rotator R as a function of the coefficients a, b, and the spinor
AU. Table 3.1 - Rotator R as a function of spinors a
bAR 2 1 1 + sin ¢u + 2 sin 2 !f;:U,2
sin e
sin *
2 COS2 f2
2cos z f2
tan f2
2cos* ~ sin ¢
2
sin * ¢
z . ,2 ;psm
* 2
1 + 2 cos2 ~ [tan ~u + tan 2 ~u2] = [1+ tan ~u][l- tan ~U] -l 1 + 2 cos !f; sin!f;u + 2sin 2 !f;u 2 1 + sin ¢u + 2 sin 2 ~U2
Proof. Assuming A = 1, we may find tr R (3.169), is equal to , tr R
= 1 + 2 cos ¢ , which, because of
= 1 + 2 cos ¢ = 3 - 2bu2
(3.170)
104
3. Orientation Kinematics
and therefore, _ 1 - cos ¢ _ ~ . 21:.. b2 2 sm . U U 2 Now the orthogonality condition
RT R =
1
1 + (2b 1 + (2b -
(I -
au + bu 2 )
2
)u2 + b2u4
a a
2
-
b
2u2
(3.171)
(I + au + bu 2 )
)U2
(3.172)
leads to (3.173) and therefore,
R
1 . d. 2 . 2 ¢ -2 1 + -sm 'f'U + 2sm - U U U 2 2 1 + sin ¢fl + ver ¢u .
(3.174)
From a numerical viewpoint , the sine-squared form is preferred to avoid the cancellat ion in computing 1 - cos ¢ for small ¢. Replacing a and b in (3.169) provides t he explicit rotator in terms of u and ¢
R=
ui + (u~ + u §) c¢ 2UIU2S2~ - U 3S¢ 2UIU3S2 ~ + U2S ¢ 2Ulu2s2 1!. + U3S ¢ u~ + (u§ + ui) c¢ 2U2U3S2~ - UlS¢ [ 2UIU3S2~ - U2S ¢ 2U2U3S2~ + UlS ¢ u~ + (ui + uD c¢
]
(3.175)
which is equivalent to Equation (3.13). If A #- 1 but nonzero, the answers are a which do not affect R. •
= }u sin ¢ and b = (,\~)2 sin' ~
*
Example 64 Eigenvalues of a spinor. Con sider the axis of rotation indicated by = 7.
U
Th e associated spin matrix and its square are
u=
[ ~ ~3 -2
ii? =
6
[ ~~3 ~~5 18
6
!6 ] 0
168]
-40
where the eigenvalues of u are (0, 7i , -7i) while thos e of of u2 are (0, -49, - 49).
3.6
*
3. Orientation Kinematics
105
Problems in Representing Rotations
As is evident in this Chapter, there are a number of different methods for representing rotations, however only a few of them are fundamentally distinct. The parameters or coordinates required to completely describe the orientation of a rigid body relative to some reference frames are sometimes called attitude coordinates. There are two inherent problems in representing rotations, both related to incontrovertible properties of rotations. 1. Rotations do not commute. 2. Spatial rotations do not topologically allow a smooth mapping in three dimensional Euclidean space . The non-commutativity of rotations has been reviewed and showed in previous sections. Nonetheless, it is important to obey the order of rotations, although sometimes it seems that we may change the order of rotations and obtain the same result. The lack of a smooth mapping in three dimensional Euclidean space means we canno t smoothly repr esent every kind of rotation using one set of three numbers. Any set of three rot ational coordinates contains at least one geometrical orientation where the coordinates are singular, and it means at least two coordinates ar e undefined or not unique. The problem is similar to defining a coordinate syst em to locate a point on the Earth 's surface. Using longitude and latitude becomes problematic at the north and south poles, where a small displacement can produce a radical change in longitude. We cannot find a sup erior syst em because it is not possible to smoothly wrap a sphere with a plan e. Similarly, it is not possible to smoothly wrap the space of spati al rotations with three dimensional Euclidean space . This is the reason why we sometimes describ e rot ations by using four numbers. We may use only three-numb er systems and exp ect to see the resulting singularities, or use four numbers, and cope with the redundancy. The choice depends on the application and method of calculation. For computer applications, the redundancy is not a problem, so most algorithms use representations with extra numbers. However, engineers prefer to work with the minimum set of numbers. Therefore, there is no unique and superior method for representing rotations.
3.6.1
Rotation matrix
Rotation matrix repr esentation, derived by determination of directional cosines, is (for many purposes) the most useful representation method of spatial rotations. The two reference fram es G and B , having a common origin , are defined through orthogonal right-handed sets of unit vectors {G} = {i , J, K} and {B} = {i ,), k}. The rotation or transformation matrix
106
3. Orientation Kinematics
between the two frames can simply be found by describing the unit vectors of one of them in the other.
i
ii . i)i + it . j)j + ii . k)k (3.176)
J
cos(i, i)i + cos(i,j)j + cos(i, k)k (J . i)i + (J . j)j + (J . k)k cos(J, i)i + cos(J,j)j + cos(J, k)k
(3.177)
K
(K . i)i + (K . j)j + (K . k)k cos(K, i)i + cos(K,j)j + cos(K, k)k
Therefore, having the rotation matrix
G RB
Z·i J.j Z·j [ kr t.: K.j cos(Z,i) cos(J, i) [ cos(K, i)
(3.178)
Z·~]
i»
K·k
cos(Z,j) cos(J,j)
cos(i,k) ] cos(J, k)
(3.179)
cos(K,j) cos(K, k)
would be enough to find the coordinates of a point in the reference frame G, when its coordinates are given in the reference frame B. (3.180) The rotation matrices convert the composition of rotations to matrix multiplication. It is simple and convenient especially when rotations are about the global principal axes, or about the local principal axes. Orthogonality is the most important and useful property of rotation matrices, which shows that the inverse of a rotation matrix is equivalent to its transpose, G R"B 1 = G R~ . The null rotation is represented by the identity matrix, I . The primary disadvantage of rotation matrices is that there are so many numbers, which often make rotation matrices hard to interpret. Numerical errors may build up until a normalization is necessary.
3.6.2
Angle-axis
Angle-axis representation, described by the Rodriguez formula, is a direct result of the Euler rigid body rotation theorem. In this method, a rotation is described by the magnitude of rotation, ¢, with the positive right-hand direction about the axis directed by the unit vector , U. G RB
= Rft ,q, = I cos ¢ + uiJ,T vers ¢ + u sin ¢
(3.181)
3. Orientation Kinematics
107
Convert ing the angle-axis repr esentation to matrix form is simply done by expanding.
ui vers <jJ + c<jJ
G RE
=
[
vers <jJ - U3S<jJ U§ vers <jJ + c<jJ U2U3 vers <jJ + Ul s<jJ
Ul U2
<jJ + U3S<jJ Ul U3 vers <jJ - U2S<jJ Ul U2 vers
<jJ + U2S <jJ ] U2U3 vers <jJ - Ul s<jJ U~ vers <jJ + c<jJ
Ul U3 vers
Converting the matrix representation to angle-axis form is shown in Examp le 39 using matrix manipulation. However, it is sometimes easier if we convert t he matrix to a quaternion and th en convert the quaternion to ang le-axis form. Angle-axis representation has some prob lems. First , th e rotation axis is indeterminate when <jJ = O. Second, the angle-axis representation is a two-to -one mapping system because R-il,-1>
= R il,1>
(3.182)
and it is redundant because for any integer k,
R u,1>+2k1r = R u,1> '
(3.183)
However, both of thes e prob lems can be improved to some extent by restricting <jJ to some suitable range such as [0,1T] or [-I ' I ]' Finally, angleaxis representation is not efficient to find th e composition of two rotations and det ermine th e equivalent angle-axis of rotations.
3.6.3 Euler angles Euler angles are also employed to describ e th e rotation matrix of rigid bodies utilizing only t hree numb ers.
[Rz,,p Rx,eRz''P f c
-c
sBs
Euler angles and rot ation matrices are not generally one-to -one, and also, th ey are not convenient repr esentations of rot atio ns or of constructing composite rot ations. T he angles
108
3. Orientation Kinematics
cos- 1 (r33)
(J
- t an -
(r(r 1
(3.185) 31
)
(3.186)
r 32
't/J
= tan -1
13
(3.187)
)
r 23
It is possible to use a more efficient method that hand les all cases uniformly. The main idea is to work with the sum and difference of ip and
't/J (J
(3.188)
v
(3.189)
and then, (J - V
(3.190)
2 (J +V
't/J
(3.191)
2
Therefore,
ru + r22 rll - r 22 r 21 - r12 r21
+ r1 2
cos (J(1 + cos (J)
(3.192)
cosv(l- cos(})
(3.193)
sin (J(1 + cos B) sin v (l - cos (J ) ,
(3.194) (3.195)
which leads to (J
tan
- 1 r21 - r 12
+ r 22 - 1 r 21 + r 12 tan
(3.196)
r11
v
"ii -
.
(3.197)
r22
This approach resolves the problem at sin () = 0. At (J = 0, we can find v is und etermined, and at (J = it , we can find v, but (J is undetermined . The undetermined values are consequence of tan " ! §. Besides these singularities, both (J and v are uniquely determined. The middl e rotation angle , (J, can also be found using tan- 1 operator
(J, but
(J
=~n
- 1
(r 13sin
(3.198)
r33
The main advantage of Eul er angles is that th ey use only three numbers. They are int egrable, and the y provide a good visua lization of spatial rotation with no redundancy. Euler angles are used in dyn amic analysis of spinning bodies. T he other combinations of Euler ang les as well as roll-pit ch-yaw angles have the same kind of problems, and similar advantages.
3. Orientation Kinematics
3.6.4
109
Quaternion
Quat ernion uses four numb ers to represent a rotation according to a special rule for addition and multiplication. Rot ation quaternion is a unit quaternion t hat may be described by Euler parameters, or the axis and angle of rot ation. eo + e
e (¢, u)
eo + el i + e2) + e3 k
¢
. ¢
A
cos 2" +Slll2"U
(3.199)
We can also define a 4 x 4 matrix to represent a quaternion
(3.200)
which provides the important orthogonality prop erty +---+ - 1
q
T = +---+ q .
(3.201)
Th e matrix quat ernion (3.200) can also be represented by +---+
(3.202)
q =
where (3.203) Employing the m atrix quaternion, q , we can describe th e quat ernion multiplic ation by matrix multipli cation .
(3.204)
Th e matrix description of quaternions ties the quaternion manipulations and matrix manipul ations, because if p, q, and v are three quaternions and qp= v
(3.205)
th en +---+ +---+
q p
= +---+ v.
(3.206)
110
3. Orient ation Kinematics
Hence, qu aternion represent at ion of transform ation between coordinate fram es Gr =e (¢,u) Br e*(¢ ,u) (3.207) can also be defined by mat rix multiplication t-+
+----------+
t-+
+----------+
B
(
)
e(¢,u) Br e*( ¢,u)
Gr
+----------+ T
e (¢,u ) r e (¢,u ) .
(3.208)
(3.209)
(3.210)
el
e2
[ - eo el eo e3 e (¢,u f = -e2 - e3 eo - e3 e2 - el +----------+
-e3e2
]
.
(3.211)
el eo
Therefore, +----------+
t-+
+----------+
e(¢,u) B r e(¢,uf =
[
G~, Gr2
- Gr l 0
Gr3 Gr3 _ Gr2
_G r2 _ Gr3
r3
3
0 Gr l
Grz
_G
rl
]
(3.212)
0
where, Br l ( eo2 + e 2 - e22 - e32) l
+ Br2 (2e le2 - 2eOe3) + Br3 (2eoe2 + 2e l e3)
(3.213)
Br l (2eoe3 + 2ele2) + Br2 (2 eo - el2 + e22 - e32)
+ Br3 (2e2e3 -
2eoeI)
(3.214)
B r l (2e le3 - 2eoez)
+ Br2 (2eoe l + 2e2e3 ) + Br3 (e6- ei - e~ + e~ ) . which are compat ible with Equation (3.155). •
(3.215)
3. Orientation Kinematics
111
3.6.5 Euler parameters Euler parameters are the elements of rot ation quat ernions . Therefore, th ere is a direct conversion between rot ation quat ernion and Euler par ameters, which in turn are related to angle-axis parameters. We can obtain the axis and angle of rotation (¢, u), from Euler parameters or rot ation quaternion e(¢,u ), by 2 t an- 1 ~ eo e
¢ ii
(3.216) (3.217)
~.
Unit quat ernion provides a suit able base for describing spati al rotations , although it needs normaliz ation due to the error pile-up problem. In general, in some applicat ions quat ernions offer superior computational efficiency. It is interest ing to know th at Euler was th e first to derive Rodriguez's formula, while Rodri guez was the first to invent Euler parameters. In addition, Hamilton introduced quaternions, however, Gauss invented t hem but never published.
*
Example 65 Taylor expansion of rotation matrix. A ssum e the rotation matrix R = R(t) is a tim e-dependent transformation between coordinate fram es B and G . The body fram e B is coinci dent with G at t = O. Therefore, R(O) = I, and we may expand the elements of R in a Taylor series expansi on (3.218)
in which R i , (i = 1,2, 3" " ) is a constant matrix. Th e rotation matrix R(t) is orthogonal for all t , hence, (3.219)
(I +
R 1t + ;!R2t
2
+ .. .)
(I +
R ft
+ ;!Rf e + .. .) =
I.
(3.220)
Th e coefficie nt of t i , (i = 1,2,3, .. . ) must vanish on the left-hand side. This gives us R 1 +Rf R 2 + 2R 1R'[ R3
+ 3R2R'[ + 3R 1R f
+ Rf + Rf
o o o
(3.221) (3.222) (3.223)
or in general (3.224)
112
3. Orientation Kinematics
where
Ro = R'{;
= I.
(3.225)
Equation (3.221) shows that R I is a skew symmetric matrix, and therefore, RIR[ = - Rr = CI is symmetric. Now the Equation (3.222)
- 2R IR[ -[R IR[ + [RIRfl T]
Rz +Rf
2CI
(3.226)
leads to
Rz
CI CI CI
Rf that shows [CI
-
+ [C I + [CI + [CI
-
Rn R z] Rf]T
(3.227) (3.228)
RrJ is skew symmetric because we must have (3.229)
Therefore, the matrix product
(3.230) is symmetric. The next step shows that
- 3[R I Rf + RzRfl - 3[R I Rf + [RIRn T] 3[RI [Rr - RrJ + [Ri - RnR I] 2C z
(3.231 )
leads to
c; + [Cz -
Rn
(3.232)
C z + [Cz - R 3 ]
c« + [Cz - Rr( that shows [C z -
Rn
(3.233)
is skew symmetric because we must have
(3.234) Therefore, the matrix product
(3.235) is also symmetric.
3. Orientation Kinematics
113
Continu ing this procedure shows that the expansion of a rotat ion matrix R(t) around the un it matrix can be written in the form of R(t)
= I+C1 t
+ 2!1
[C 1 + [C1
-
2 RT] 2 ] t
+ ~!
[C2
+
-
RrJ] t
where C, are sym me tric and [Ci
-
[C 2
3
+ .. .
(3.236)
R TH] are skew symmetric matrices and
(3.237) Th erefore, the expansion of an inverse rotat ion matrix can be writte n as
1+ CIt
+;, [C1 + [C 1 - RfJ] t 2
+~! 3.7
[C 2 + [C2 - RrJ]t
3
+ ...
(3.238)
*
Composition and Decomposition of Rotations
Rotation ¢1 about iiI of a rigid body wit h a fixed point , followed by a rotat ion ¢2 about U2 can be composed to a unique rot ation ¢3 about U3. In other words, when a rigid body rotates from an initial position to a middle position B2 r = B2 R BI B I r , and t hen rot at es to a final position, B3r = B3 RB 2 B2r, t he middle position can be skipped to rot at e directly to the final position B3 r = B 3 R BI BI r . Proof. To show that two successive rot ations of a rigid body with a fixed point is equivalent to a single rotation, we st art with the Rodriguez rotation formul a (3.115) and rewrite it as
(r' - r) = W x (r'
+ r)
(3.239)
where
¢
W
A
= t an "2 u
(3.240)
is t he Rodriguez vector. Rotation WI followed by rot ation W2 are
WI X (rz + rd W2 x (r 3 + r 2)
(3.241) (3.242)
114
3. Orientation Kinematics
respectively. The right hand side of the first one is perpendicular to WI and t he second one is perpendicular to W2. Hence, dot product of the first one with WI and the second one with W2 show that (3.243) (3.244)
and cross product of the first one with W2 and th e second one with WI show that WI [W2 . (r2 + rl) ] - (WI ' W2) (r2 + r l ) -W2 [WI ' (r3 + r 2)] + (WI ' W2) (r3 + r2) . (3.245)
Rearranging while using Equations (3.243) and (3.244) gives us (W2 x
w j )
x (rl + r 3)
+ (WI ' W2) (r3 - r l ) (3.246)
which can be written as W2 x rl + WI
X
r3
+(W2 x WI) x (rl +r3) +(WI, w 2)(r3- rl).
(3.247)
Adding Equations (3.241) and (3.242) to obtain (WI + W2) x r 2 leads to (3.248)
Therefore, we obtain t he requir ed Rodriguez rotation formula to rotate r l to r 3 (3.249) where w 3=
WI + W2 + W2 X WI 1- WI ' W2
(3.250)
•Any rotation cPI of a rigid body with a fixed point about UI can be decomposed into three successive rotations about three arb itrary axes ii, b, and c through unique angles o, 13, and 'Y . Let G Ra,a, G Ri,,(3 ' and G R e,"! be any three in order rotation matrices about non-coaxis non-coplanar unit vectors ii, b, and C, through nonvanishing values o , 13, and 'Y . Then, any other rotation G Rii.,cP can be expressed in terms of G Ra,a, G Ri,,(3 ' and G R e,"! G Rii.,cP
= G R e,"! G Ri,,(3 G Ra,a
(3.251)
3. Orientation Kinematic s
115
if a, (3, and , are properly chosen numbers. Proof. Using the definition of rot ation based on quaternion, we may write
(3.252) Let us assume that rl indicates the position vector r before rotation, and r2 , r 3, and r 4 indicate the position vector r after rotation R a.a , Rh,(3 ' and R e" respectively. Hence, r2
aria *
r3
br2b*
r4
cr 3c*
r4
er le*
(3.253) (3.254) (3.255) (3.256)
where a (a , a)
ao +a =cos
a
. a, +sm
(3.257)
2 2a (3 (3, cos 2 + sin 2 b
b((3 ,b)
bo + b =
c (r , c)
Co + c = cos 2 + sm 2 c
e (¢, u)
eo + e = cos 2 + sm 2 u
,
. "t :
¢
. ¢,
(3.258) (3.259) (3.260)
are quat ernion s corres ponding to (a,a), ((3, b) , (r , c), and (¢,u) respectiv ely. We define the following scalars to simplify the equations. a cos -
C1
(3 cos 2 = C2
. -a sm 2
51
. (3 = 5 2 sm
2
b2 C3
-
b3 C2
,
cos 2 = C3
. ,
sm
2
b3 Cl
-
b1 C3
5 253
5 253
C2 a3 - c3a2
a3Cl - al c3
5 351
5 351
a2b3 - a3b2
5 152
2=
5
3
cos ~ = . ¢ = 5 sm
2
C (3.261)
(3.262)
f
-
2
= g2
a3bl - al b3 _ f
5 152
b ·c c ·a a ·b (axb) ·c
-
L2
n 15253 n2 5351 n351 52 n4 51 52 53
(3.266) (3.267) (3.268) (3.269)
116
3. Orientation Kinematics
Direct substitution shows t hat
(3.270) and t herefore ,
e
cba = c (boao - b · a + boa + aob + b x a ) coboao - aob · c - boc · a - coa ' b + (a x b }«; +aoboc + bocoa + coao b +ao (b x c) + bo (c x a ) + Co (b x a ) - (a ·b)c - (b·c)a + (c ·a)b.
(3.271)
Hence,
and
e
aoboc + bocoa + coaob +ao (b x c) + bo (c x a ) + Co (b x a ) -n1S2S3a + n2S3S1b -n3S1S2c
(3.273)
which generate four equat ions
C 1 C 2C3 - nl C 1S2S3 - n 2S1 C2S3 + n3S1S2C3 - n4S1S2S3 = C
(3.274)
a1S1C2C3 + b1C1S2C3 + C1C1C2S3 +!I C 1S 2S 3 + glSlC2S3 + h 1S 1S 2C3 -nl a1 Sl S2S3 + n2blS1S2S3 -n3clSlS2S3 = ulS
(3.275)
a2S 1C 2C3 + b2C 1S 2C3 + C2 CIC2S3 +!2C 1S 2S 3 + g2S1C2S3 + h 2S 1S 2C3 - n l a2S 1S2S3 + n2b2S1S2S3 - n3c2S 1S2S3 = u 2S
(3.276)
a3S1C2C3 + b13 C 1 S2C 3 + C3C l C2S3 +!3C1S2S 3 + g3S1C2S3 + h3S 1S 2C3 - nla3S 1S2S3 + n2b3S1S2S3- n3c3S1S2S3 = U3S
(3.277)
Since ea + ei + e~ ot hers along with
+ e~ =
1, only t he first equation and two out of t he
(3.278)
3. Orientation Kinematics
Example 66
117
*
Decompos ition of a vector in a non-orthogonal coordinate fram e. Let a , b ,and e be any three non-coplanar, non-van ishing vectors; th en any oth er vector r can be expressed in terms of a, b ,and e (3.279)
r = ua+ vb+ we
provided u , v , and w are properly chosen numbers . If (a , b , c) coordina te syste m is a Cart esian coordinate syst em it, J, K), then r
= (r .i)i + (r .J)J + (r.K )K.
(3.280)
To sho w this, we start with finding the dot product of Equation {3.279} by (b x c)
r - (b
xc)
=
tza- (b
xc) + vb · (b xc) + we · (b xc)
(3.281)
and noting that (b x c) is perpendicular to both b and e, consequently
r- (b x c) = ua - (b x c) .
(3.282)
Th erefore,
[r b e] [abc]
(3.283)
u=--
where QI
[abc] = a · b x e
= a · (b x c) =
Q2 Q3
bI b2 b3
CI C2
(3.284)
C3
Similarly v and w would be
[rca] [a b c] [rab] [a b c] '
v w
(3.285) (3.286)
Hen ce,
r = [rbe] a [abc]
+
[rca] b [abc]
+
[rab] e [abc]
(3.287)
that can also be written as
r=
(e xa) (a xb) c] a + r -[a b c] b + r - [ab c] e. (r -[abbxc)
(3.288)
Multiplying (3.288) by [abc] gives a sym metric equation
[abc] r - [ber] a
+ [era] b -
[rab] e = 0
(3.289)
118
3. Orientation Kinematics
If the (a, b , c) coordinate system is a Cart esian system a mutually orthogonal system of unit vectors, then
[1Jk] lxJ Jxl tc x 1
(1, J, k), that is
1
(3.290)
k k J
(3.291) (3.292) (3.293)
and Equation (3.288) becomes
r = (r.l) 1+ (r.J) J + (r.k) tc.
3.8
(3.294)
Summary
Two Cartesian coordinate frames Band G have a common origin. We may rotate B for ¢ rad about a specific axis u to transform B to G. Having the angle and axis of rotation, the transformation matrix can be calculated by the Rodriguez rotation formula.
oR B
= Ril , = Lcos e +
uuT vers ¢ + usin¢
(3.295)
On the other hand , we can find the angle and axis of rotation by having the transformation matrix c RB , (3.296)
cos ¢
(3.297) The angle and axis of rotation can also be defined by Euler parameters eO,el , eZ,e3 eo
¢
cosA
e
(3.298)
2
el l
A
¢
A
+ ezJ + e3 K = usin '2
R il ,
=
(e6 - e
Z
)
I
+ 2ee T + 2eo e
(3.299) (3.300)
or unit quaternions e (¢ ,u )
eo +e eo + eli + ez) + e3 k ¢ . ¢ cos '2 + sin '2 u A
that provide some advantages for orient ation analysis.
(3.301)
3. Orientation Kinematics
119
Exercises 1. Notation and symbols. Describ e the meaning of
u
b- vers ¢
c- u
d- Ru,
e- eo
f- e
1 g- BR-G
h-d¢
i-qo+q
j- ij
k-q*
l- e( ¢,u)
m- d¢
n-
q
p- ij
q- Iql
r- ~3 x 3 .
a-
j
0-
2. Invar iant axis of rot ati on. Find out if the axis ofrotation
u is fixed in B(Oxy z) or G(OXYZ).
3. z-axis-angle rot ation matrix. Exp and
GRB
=
BR-G 1
. = BRTG = DLit, .J.
= [Az,_
Ay,oAz,
A~_oA;,_
GRB = Ru, =
ui vers ¢ + c¢ Ul U 2 vers ¢ + U 3 S¢ [ U1U3 vers ¢ - U 2 S¢
Ul U2 vers ¢ -
U3S¢
u~ vers¢ + c¢ u 2u 3
vers ¢ +
U 1S¢
vers ¢ + U 2 S¢ U2U3vers ¢-U1 S¢ U5 vers¢ + c¢
U I U3
]
.
4. x-axis-angle rotat ion matrix. Find t he axis-angle rotat ion matrix by transforming the x-axis on the axis of rot at ion is: 5. y-axis-angle rot ation mat rix. Find the axis-angle rot ation mat rix by transforming the y-axis on the axis of rot ation u. 6. Axis-angle rotation and Euler angles. Find the Eul er angles corresponding to t he rotation 45 deg about u = [1 1 1
f.
7. Eul er angles between two local frames. The Euler angles between the coordinate frame B 1 and G are 20 deg, 35 deg, and - 40 deg. The Eul er angles between the coordinate fram e B 2 and G are 60 deg, -30 deg, and -10 deg. Find t he angle and axis of rotation that transforms B 2 to B 1 .
120 8.
3. Orientation Kinematics
* Angle and axis of rotation based on Euler angles. Compare t he Euler angles rot ation matrix with the angle-axis rotation matrix and find t he angle and axis of rotation based on Euler angles.
9.
* Euler angles based on angle and axis of rotation. Compare t he Euler angles rotation matrix with the angle-axis rot ation matrix and find t he Euler angles based on the angle and axis of rotation.
10.
*
Repeating global-local rotation s.
Rotate B r p = [6,2 , - 3jT , 60 deg about th e Z-axis, followed by 30 deg about the x-axis. Then repeat th e sequence of rotati ons for 60 deg about the Z-axi s, followed by 30 deg about th e x-axis. After how many rotations will point P be back to its initial global position? 11.
* Repeatin g global-local rotations. How many rot ati ons of ex = 1r/m deg abou t the X -axis, followed by fJ = 1r/ k deg about t he z-axis are needed to bring a body point to its initial global position, if m , k E N?
12.
* Small rotation angles. Show that for very small angles of rotation ip, B, and 'ljJ about the axes of th e local coordinate frame, th e first and third rotations are indistinguishable when t hey are about the same axis.
13.
* Inner auto morphism properly of a. If R is a rotati on matrix and a is a vector, show that
14.
* Angle-derivative of principal rot at ion matrices. Show that
KRz ,o.
iRx ,,. 15.
* Euler angles, Euler parameters.
3. Orientation Kinematics
121
Compare th e Euler angles rot ation matrix and Euler par ameter transform ati on matrix an d verify th e following relat ionship s between Eul er angles and Eul er par ameters
e
'ljJ + sp eo = cos 2 cos - 2 ei =
. ecos -'ljJ -- cp
sm
2
2 'ljJ - cp 2sm-2'ljJ + ip = cos 2 sm2-
. e. e2 = sm e3
e.
and
16.
*
Quat ernion definition.
F ind th e unit qu aterni on e (¢, u) associated to
1/J3 ] [ 1/J3 1/J3 ¢
= ~ 3
and find the result of e (¢, u) i e" (¢, u). 17.
*
Quat ernion product.
Find pq, qp, p'q , qp' , p'p , qq' , p'q' , and p' rq' if
18.
*
p
3 + i - 2j
q
2- i
r
-1+ i+ j-3k .
+ 2j
+ 2k + 4k
Quat ern ion inverse.
Find q-l , »:' , p -lq-l ,
«:« ,v»:' , q-lq' , and v: q. - l if
p
3 + i - 2j + 2k
q
2- i
+ 2j + 4k.
122 19.
3. Orientation Kinematics
* Quaternion and angle-axis rotation. Find the unit quaternion associated to p
3 + i - 2j
q
2- i
+ 2j
+ 2k + 4k
and find the angle and axis of rotation for each of the unit quaternion. 20.
* Unit quaternion and rotation. Use the unit quaternion p p=
l+i-j+k 2
and find the global position of
21.
* Quaternion matrix. Use the unit quaternion matrices associated to
+---+ +---+ t----t
p
3 + i - 2j
q
2- i
T
-1+i+j-3k.
+---+ +---+
+---+ * f--+
+ 2k
+ 2j + 4k f---t +---+
t----t t----+
+---+ +--+
+---+ +---+
and find p r q , q p, p q, q p* , p* p , q q* , p* q* , and +---> +---> +--->
p* r q*.
22.
* Euler angles and quaternion. Find quaternion components in terms of Euler angles, and Euler angles in terms of quaternion components.
23. Angular velocity vector. Use the definition G RB = [rij] and G RB = [iij], and find the angular velocity vector w , where w= G RB G R~. 24.
* bac-cab rule. Use the Levi-Civita density tijk to prove the bee-cob rule a x (b xc)
=
b (a . c) - c (a . b) .
3. Orientation Kinematics 25.
123
* bac-cab rule appli cation. Use the bac-cab rule to show that
a = n (a · ft) + n x (a x
n)
where n is any unit vector. Wh at is the geometric significance of this equation? 26.
* Two rotations are not enough. Show th at , in general, it is impossible to move a point P(X,Y, Z) from th e initial position P(X;, Yi , Z;) to th e final position P(Xj , Yj , Zj) only by two rot ations about th e global axes.
27.
* Three rot ation s are enough. Show that, in general, it is possible to move a point P(X, Y, Z) from th e initial position P(X; ,Yi ,Z;) to th e final position P(Xj ,Yj ,Zj) by three rot ations about different global axes.
28.
* Closure property.
Show the closure property of transformation matrices. 29. Sum of two orthogonal matrices. Show that th e sum of two orthogonal matrices is not , in general, an orthogonal matrix, but t heir product is. 30. Equivalent cross product . Show th at if a = [al arbitrary vectors, and
a2 a3]T and b = [ b1 b2 b3] T are two
is the skew symm etric mat rix corresponding to a , then iib = a x b . 31.
* Skew symm etric matri ces. Use a = [al
a2 a3 rand b = [ b1 b2 b3
a-
ab bc-
= -ba
r
to show th at :
124 32.
3. Orientation Kinematics
* Rotation matrices identity. Show that if A, B , and C are three rotation matrices then , a(AB) C
=
A (BC)
= ABC
b-
c-
d-
33.
* Skew symme tric matrix multiplication. Verify that a-
b-
ab = ba
34.
T
T
- a bI
* Skew symmetric matrix derivative. Show that
35.
* Time derivative of A =
[a, ii] .
Assume that a is a time dependent vector and A = [a, a] is a 3 x 4 matrix. What is the time derivative of C = AA T ? 36.
* Combined angle-axis rotations. The rotation cPl about Ul followed by rotation cP2 about U2 is equivalent to a rotation cP about U. Find the angle cP and axis U in terms of cPl' Ul, cP2' and U2 ·
37.
* Rodriguez vector. Using the Rodriguez rotation formula show that r' - r
= tan ~U x (r' + r) .
3. Orientation Kinematics
38.
125
* Equivalent Rodriguez rotation matrices. Show that the Rodriguez rotation matrix C
R B = I cos + ilfJ7 vers + il, sin
can also be written as C RB
39.
= I + (sin
ii
+ (vers
il,2.
* Rotation matrix and Rodriguez formula. Knowing the alternative definition of the Rodriguez formula
and il,2n-l il,2n
= (-It- 1 il,
= (-It- 1 il,2
examine the following equation: CRT CR B B -
40.
CR
B
CRT B
* Rodriguez formula application. Use the alternative definition of the Rodriguez formula
and find the global position of a body point at
after a rotation of 45 deg about the axis indicated by
41. Axis and angle of rotation. Find the axis and angle of rotation for the following transformation matrix: 3
4
~ 4
1
4
yQ 4 2
4
~ 4
tJ
126 42.
3. Orientation Kinematics
* Axis of rotation multiplication. Show th at and
43.
* Stanley method. Find th e Euler parameters of the following rotation matrix based on th e Stanley method. 0.5449
G RB
=
-0.5549 0.6285 ] 0.8299 0.4629 [ -0.7785 -0.0567 0.6249 0.3111
4
Motion Kinematics 4.1
Rigid Body Motion
Consider a rigid bod y with an attached local coordinate frame B (oxy z) moving freely in a fixed global coordinate frame G(OXYZ) . Th e rigid bod y can rotate in the global frame, while point 0 of the body frame B can translate relati ve to the origin 0 of G as shown in Figur e 4.1.
z
z
z
~ \jI y
y
x
x
x FIGURE 4.1. Rotation and tran slation of a local frame with respec t to a globa l fram e.
If th e vector G d indicat es the position of t he moving origin 0 , relative to th e fixed origin 0 , then the coordin at es of a body point P in local and global frames are relat ed by the following equation: Gr p
=
G RB B r p
+ Gd
(4.1)
where rp
[ x;
rp
[ Xp
yp
Gd
[ x,
Yo
G B
Yp
f f z; f · z;
Zp
(4.2) (4.3) (4.4)
128
4. Motion Kinematics
The vector G d is called the displacement or translation of B with respect to G, and G R B is the rotation matrix to map B r to G r when Gd = O. Such a combination of a rotation and a translation in Equation (4.1) is called rigid motion. In other words, the location of a rigid body can be described by the position of the origin 0 and the orientation of the body frame, with respect to the global frame . Decomposition of a rigid motion into a rotation and a translation is the simplest method for representing spatial displacement. We show the translation by a vector, and the rotation by any of the methods described in the previous Chapter. Proof. Figure 4.1 illustrates a translated and rotated body frame in the global frame. The most general rotation is represented by the Rodriguez rotation formula (3.62), which depends on B r p , the position vector of a point P measured in the body coordinate frame . In the translation G d, all points of the body have the same displacement, and therefore, translation of a rigid body is independent of the local position vector B r . Hence, the most general displacement of a rigid body is repr esented by the following equation, and has two independent parts: a rotation, and a translation. Gr
Br
cos ¢ + (1 - cos ¢) (7'1.
G RB B r
B
r) U + (u
+ Gd
X B
r) sin ¢ +
Gd
(4.5)
Equation (4.5) shows that the most general displacement of a rigid body is a rotation about an axis and a translation along an axis. The choice of the point of reference 0 is ent irely arbitrary, but when this point is chosen and the body coordinate frame is set up, the rotation and translation are uniquely determined. Based on translation and rotation, the position of a body can be uniquely determined by six independent parameters: three translation components X o, Yo, Zo; and three rotational components. If a body moves in such a way that its rotational components remain constant , the motion is a pure translation ; and if it moves in such a way that X o, Yo , and Zo remain const ant, the motion is a pure rotation. Therefore, a rigid body has three translational and three rotational degrees of freedom . • Example 67 Translation and rotation of a body coordinate fram e. A body coordinate frame B (oxy z) , that is originally coincident with global coordinate fram e G(OXY Z), rotates 45 deg about the X -axis and translates to [3 5 7 Then, the global position of a point at B r = [ x y z is
r.
Gr
r
GRBBr+Gd
[
~o .sin:4545 -coss~4545 ] [~z ] + [ ;7 ]
(x + 3) j
+ (O .707y -
O.707z + 5) j
+ (O .707y + O.707z + 7) k.
129
4. Motion Kinematics
z
•
Z
cf:
•
p
Gr
,
"' y
X
X
"' y
~
FIGURE 4.2. A translating and rot atin g bod y in a global coordinate frame.
Ex ample 68 Moving body coordinate fram e. Figure 4.2 shows a point P at B r p = O.li + 0.33 + 0.3k in a body fram e B , whi ch is rotat ed 50 deg about the Z-axis, and tran slat ed - 1 along X, 0.5 along Y , and 0.2 along the Z axes. Th e position of P in global coordinate fram e is G RBB r p + COS 50
sin 50 [
o
Gd
0]o [0.1 ]+ [-1 ]
- sin 50 cos 50
0
1
0.3 0.3
0.5 0.2
- 1.166 ] 0.769 . [ 0.5
Ex am pl e 69 Rotation of a translated rigid body. Point P of a rigid body B has an ini ti al position vector Brp
Br p.
= [ 1 2 3] T
If th e body rotates 45 deg about the x -axis, and then translates to [4 5 6] T, the final position of P would be BRT Br p x ,45
1o cos045
[o
sin 45
5.0 ] 4.23 . [ 9.53
+ Gd
0 ]
- sin 45 cos 45
Gd
=
130
4. Motion Kinematics
.z
Z4
(a)
FIG URE 4.3. A polar RP arm .
Note that rotation occurs with the assumption that Gd
= O.
Example 70 Arm rotation plus elongation. Position vector of point PI at the tip of an arm shown in Figure 4.3{a) is mm . Th e arm rotates 60 deg about at G r PI = B r PI = [ 1350 0 900 the global Z-axis , and elongates by d = 720.2i mm . Th e final configuration of the arm is shown in Figure 4.3(b) . Th e new position vector of P is
f
Gr p
where G R B = Gd= 0,
=
2
GR B B rr.
+ Gd
is th e rotation matrix to transform r P2 to r PI when
RZ,6o
60 sin 60
- sin 60 cos 60 0
COS
GR B
= [
o
0 ] 0 1
and G d is the translation vector of 0 with respect to 0 in the global fram e. Th e translation vector in th e body coordinate fram e is B d = [ 720.2 0 0] 7 so G d would be found by a transformation Gd
=
GRBB d
60 sin 60
- sin 60 cos 60
o
o
COS [
360.10 ] 623.71 . [ 0.0
o0 ] 1
[ 720.2 0.0 ] 0.0
4. Motion Kinemati cs
131
Th erefore, the final global position of the tip of the arm is at
GR B B r r. + [
c60 s60
- s60 c60
o
0
Gd
0] 0 1
1350] o [ 900
+
[360.1] 623.7 0.0
1035.1 ] 1792.8 . [ 900.0 Example 71 Compos ition of transformations. Assume (4.6) in dicates the rigid motion of body B 1 with respect to body B z, and
(4.7) indicate s the rigid motion of body B z with respect to fram e G. Th e com position defin es a third rigid motion , which can be described by substituting the expression for zr into the equation fo r Gr .
GR z eRl 1r + Zd1 ) + Gdz GRz ZRllr+GRz Zdl + Gd z G R 1 lr + G d 1
(4.8)
Th erefore, G R z zR 1
G R z zd 1
(4.9)
+ Gd z
(4.10)
which shows that th e transformation from frame B 1 to fram e G can be done by rotation G R 1 and tran slation G d ..
4.2
Homogeneous Transformation
As shown in Figure 4.4, an arbit ra ry point P of a rigid body attached to the local fram e B is denoted by B r P and G r p in different fram es. The vector G d indicates the position of origin 0 of the body fram e in t he global fram e. Therefore, a general motion of a rigid body B (oxyz) in t he global frame G (OXYZ) is a combination of rotation G RB and translation G d .
(4.11)
132
4. Motion Kinematics
y
x
~
FIGURE 4.4. Representation of a point P in coordinate frames Band G.
Using a rotation matrix plus a vector leads us to the use of homogeneous coordinates. Introducing a new 4 x 4 homogeneous transformation matrix GTB , helps us show a rigid motion by a single matrix transformation
(4.12) where
(4.13) and
rn Br~ pn
Gr~
cd
[
~ ~: j [
(4.14)
(4.15)
(4.16)
4. Motion Kinematics
133
The homogeneous transformation matrix GTB is a 4 x 4 matrix th at maps a homogeneous position vector from one frame to another. This extension of matrix representation of rigid motions is just for simplifying numerical calculations. Representation of an n-component position vector by an (n+ 1)-component vector is called homogen eous coordinate representation. The appended element is a scale fa ctor, w ; hence, in general, homogeneous representation of a vector r = [ x y z ]T is
(4.17)
Using homogeneous coordinates shows that the absolute values of th e four coordinates are not important . Instead , it is the three ratios, x/w, y/w, and zf u: that are important because,
(4.18)
provided w =1= 0, and w =1= 00 . Therefore, the homogeneous vector wr refers to the same point as r does. If w = 1, then th e transformed homogeneous coordin at es of a position vector are the same as physical coordinat es of th e vector and the space is th e standard Euclidean space. Hereafter, if no confusion exists and w = 1, we will use th e regular vectors , and their homogeneous represent ation equivalently. Proof. We append a 1 to the coordinates of a point and define homog en eous position vectors as follows:
134
4. Motion Kinematics
Using the definitions of homogeneous transformation we will find (4.19)
(4.20)
However, the standard method reduc es to (4.2 1)
(4.22)
which is compatible with th e definition of homogeneous vector and homogeneous transformation. • Ex ample 72 Rotation and translation of a body coordinate fram e. A body coordinate fram e B (ox y z ), that is originally coinc ident with global coordinate fram e G( OXY Z), rotates 45 deg about the X -axis and translates to [3 5 7 1] T . Th en , th e matrix representation of the global position of a point at B r
=
[ x
y
z
1] T
is
o
o
cos 45 sin 45
- sin45 cos 45
o
o
Ex ample 73 Decomposition of GTB into translation and rotation. Homog en eous transformation matrix GTB can be decompo sed to a matrix multiplication of a pure rotation matrix G R B , and a pure translation matrix
4. Motion Kinematics
135
(4.23) =
(4.24)
In other words, a transformation can be achieved by a pure rotation first, followed by a pure translation. Note that decomposition of a transformation to translation and rotation is not interchangeable GTB
=
GDBGR B
=f
GRBGDB .
However, according to the definition of G RB and
(4.25) G DB ,
GDBGR B GD B GRB
+ GRB-I + GDB -I.
(4.26)
Example 74 Pure translation. A rigid body with its coordinate frame B(oxyz), that is originally coincident with global coordinate frame G(OXYZ) , translates to
Then, the motion of the rigid body is a pure translation and the transformation matrix for a point of a rigid body in the global frame is
136
4. Motion Kinemat ics
Example 75 Homogeneous transformation fo r rotation about global axes. Using homogen eous representation of rot ation s about th e axes of global coordin ate fram e, th e rotation a about th e Z -axis is
GT
B
=R
Z ,Q
sin a 0
- sin a cos a 0
o
0
COS a
= [
0 0 1 0
0] 0 0 . 1
(4.27)
Example 76 Rotation about and translation along a global axis. A point P is located at (0,0 ,200) in a body coordinate fram e. If the rigid body rotates 30 deg about th e global X -axis and the origin of the body fram e trans lates to (X , Y, Z) = (500,0 ,600) , th en the coordin ates of th e point in th e global f rame are
o1 o
0] [500] 01 0 500 0 0 ] [ 01 cos030 -sin30 0 [ 0 0 ] -100 0 1 600 0 sin 30 cos 30 0 200 773.2 . [ 0001 00 01 1 1 Example 77 Rotation about and translation along a local axis . A point P is located at (0,0 ,200) in a body coordin ate fram e. If the origin of the body fram e translates to (X, Y, Z) = (500,0 ,600) and rotates 30deg about th e local x -axi s, th en th e coordin ates of the point in global fram e are
[
o1 o o
01 0 0
o
[10 cos030 sin 30 o 00 500] 0 o] 1 600 0 - sin 30 cos 30 ~ o 0 1 0 0
T [
0 - 100 0] [500] 2~0 77;.2 .
Example 78 Translation. If a body point at B
r
= [-1 0 2 1
t
= [0 10 -5 1
r
is translat ed to Gr
then th e corres pon ding transf ormation can be found by
Th erefore,
1 + d.x = 2
4 + dy
2 + dz =-5
= 10
and
dx = 1
dy = 6
dz
= - 7.
4. Motion Kinemati cs
137
Example 79 Pure rotation and pure translation. A set of basic hom ogeneous transformations for translation along and rotation about X , Y , and Z axes are given below.
GTB
GTB
GTB
GTB
GTB
GTB
l~ ~ l~ ll~
0 1 0 0
DX,a =
R
x,
0 cos!' sin !' 0 0 1 0 0
DY,b =
RY,{3
=
Dz,c =
RZ,a =
~j
0 0 1 0
0 0 1 0
coo~
ll!
0 1 0 0
C~Q smo: 0 0
0 - sin y cos !' 0
~j
0 1 0 0
- ,~n~ 0 0 1 0
(4.28)
(4.30)
sin ;3 0 cos ;3 0
~j
- sino: coso: 0 0
~1
(4.29)
~j
(4.31)
(4.32)
0 0 1 0
~]
(4.33)
Example 80 Homog eneous transformation as a consequence of vector addition. It is seen in Figure 4.4 that th e position of point P can be descri bed by a vector addition G r p = Gd + B r p . (4.34) Since a vector equation is meaningful when all the vectors are descri bed in the same coordinate frame , we need to transform either B r p to G and G d to B . Therefore, th e correct vector equation is (4.35)
or (4.36)
138
4. Motion Kinematics
The first one defines a hom ogen ous transformation from B to G, Cr p
cTB B r p
cTB
[
C~B
(4.37)
C1d ]
(4.38)
and the second one defines a transformation from G to B. Br p
BTc c r p
BTc
[
B~C
- BR c Cd 1
[
c~~
_C~~C d ]
(4.39)
] (4.40)
*
Example 81 Point at infinity. Points at infinity have a convenient representation with homogeneous coordinates. Consider the scale factor w as the fourth coordinate of a point, hence the homogeneous representation of the point is given by
(4.41)
As w tends to zero, the point tends toward infinity, and we may adapt the convention that the homogeneous coordinate
[~ ]
(4.42)
represents a point at infinity. More importantly, a point at infinity indicates a direction. In this case, it indicates all lines parallel to the vector r = [x y z] T , which intersect at a point at infinity. The hom ogeneous coordinate transformation of points at infinity introduces a neat decomposition of the homogeneous transformation matrices. r 12 r13 r22 r23 r32 r33
o
(4.43)
0
The first three columns have zero as the fourth coordinate . Therefore , they represent points at infinity, which are the directions corresponding to the three coordinate axes. The fourth column has one as the fourth coordinate , and represents the location of the coordinate frame origin .
4. Motion Kinematics
139
*
Example 82 The most general homogeneous transformation. The homogeneous transformation (4.13) is a special case of the gen eral homogeneous transformation. Th e most general homog eneous transformation, whi ch has been extens ively used in the fi eld of com pute r graphi cs, tak es the form AT B
[ ARB (3 x 3)
p(lx3) rotation [ perspective
Ad (3 x 1) ]
(4.44)
w(lx1) translation ] scale fa ctor .
For th e purpos e of roboti cs, we always tak e th e last row vector of [T] to be (0,0,0,1) . However, the more gen eral form of (4.44) could be us eful, for example, when a graphical simulator or a vision system is added to the overall robotic system. The upper left 3 x 3 submatrix ARB denotes the ori entation of a moving fram e B with respect to a reference fram e A . Th e upper right 3 x 1 submatrix Ad denotes the position of the origin of th e moving fram e B relati ve to th e reference fram e A . Th e lower left 1 x 3 submatrix p denotes a perspecti ve transformation , and the lower right eleme n t w is a scaling factor.
4.3 Inverse Homogeneous Transformation The advantage of simplicity to work with homogeneous transformation matrices come with the pen alty of losing the orthogonality property. If we show GTB by
(4.45)
then,
(4.46)
showing that ,
GT B- 1 GTB -- I 4 ·
(4.47)
However , a transformation matrix is not orthogonal and its inverse is not equal to its transpose (4.48)
140
4. Motion Kinematics
y
FIGURE 4.5. Illustration of a rotated and translated body frame B(oxyz) with respect to the global frame G(OXYZ) .
Proof. 1. Figure 4.5 depicts a rotated and translated body frame B(oxyz) with respect to the global frame G(OXYZ) . Transformation of the coordinates of a point P from the globa l frame to the body frame is BTc , t hat is the inverse of the transformation cTB . Let's start with the expression of c r and the definition of cTB for map ping B r to c r Cr [
~r
]
CRBBr + c d [
C~B
(4.49)
C1d ] [ B1r ]
(4.50)
and find B
r
CR E / (c r _ cd) cR~cr_
cR'fcd
to express the transformation matrix BTc for mapping c r to
(4.51 ) Br
(4.52)
•
P roof. 2. BTc can also be found by a geometric expression . Using the inverse of the rotation matrix C R B (4.53)
and describ ing the reverse of c d in the body coordinate frame to indicate the origin of the global frame with respect to the origin of the body frame (4 .54)
4. Motion Kinemat ics
141
allows us to define the homogeneous transformation BTc
(4.55) We use t he notation Br.
G --
cT-B 1
(4.56)
and remember t hat the inverse of a homogeneous transformation matrix must be calculated according to Equation (4.46), and not by regular inversion of a 4 x 4 matrix. This notation is consistent with the mult iplication of a T matrix by its inverse T- 1 , because 1 CTB "r B
-- I 4 ·
(4.57)
• Example 83 Inverse of a homogen eous tran sformation matrix. Assume that
-1]
- 0.766 0 0.643 0 0.5 o 1 0.2 o 0 1
= [
0
then cRB
=
[0.643 -0.766 0.766
o
cRB
0.643 0
Gd~ [ 0.2 ~q
n
and therefore,
_ C ~~ c d ]
o o
0,26 ] - 1.087 1 - 0.2 . o 1
Example 84 Transformation matrix and coordinate of points . It is possible and som etimes convenient to describe a rigid body motion in terms of known displacement of specified points fixed in the body.
142
4. Motio n Kinematics
Assum e A, B , C, and D are four points with the following coordinates at two different positions: A 1(2,4 ,1)
B 1(2,6,1)
C1(1,5 , 1)
D 1(3,5,2)
A 2(5, 1, 1)
B 2(7, 1, 1)
C2(6, 2,1)
D 2(6, 2, 3)
There must be a transformation matrix T to map the initial positions to the final, 1 [22 5 [T] 4 6
1 1 2 1 1 1
~]
6 1 2 1 1 1 1
7
[1
and hence
[I [I
[T]
[~
~][~L
6 1 2 1 1 1 1 1
7
1 0 0 0
l'
6][ 2 2 5
6 1 2 246 1 1 3 1 1 1 1 1 11
7
1/ 2
~]
5 21 2 1 1
-1 / 2 -1 /2 1/2 - 1/2 0 1/2 0 1/ 2
7/2
-3/2 1/2 -3/2
j
~2 j
0 1 1 0
Exam ple 85 Quick inverse transformation.
For numerical calculation, it is more practical to decompose a transformation matrix into translation times rotation, and take advantage of the inverse of matrix multiplication. Consider a transformation matrix
[T ]
[r"
r13 r" r23 r" r21 r22 r24 r31 r32 r33 r34 o 0 0 1 0 r" 1 0 r24 0 1 r34 0 0 1
0
U
j
= [D] [R]
][ r21 r" r31 0
r12 r13 r22 r23 r32 r33 0 0
(4.58)
~j
4. Motion Kinematics
143
therefore,
T- i
(4.59) =
Example 86
*
In verse of the general homogeneous transformation. Let [T] be defined as a general matrix combined by four submatrices [AJ, [BJ, [C], and [D] .
[T] =
[~ ~]
(4.60)
Then, the inverse is given by
(4.61 ) where
[E] = D - CA-iB.
(4.62)
In the case of the most general homogeneous transformation,
Ad (3 x 1) ] w (1 x 1) rotation perspective
(4.63)
translation ] scale factor
we have
[T] [A] [B] [0] [D]
ATB
(4.64)
ARB
(4.65)
Ad
(4.66)
[Pi
P2
[WiXi] = W
P3]
(4.67)
(4.68)
144
4. Motion Kinematics
and therefore, [E] = =
[E1 xd = E ww-
[PI [PI
P2
P3]
A R[/
P2
P3]
A R~ A d
Ad
+ d2r21 + d3r31) +P2 (d1r 12 + d2r22 + d3r 32) +P3 (d1r 13 + d2r 23 + d3r 33) 1 - PI (d 1rll
~
_E-1 CA- 1
(4.70)
+ P2r12 + P3r13 PI r21 + P2 r22 + P3r23 PI r 31 + P2 r 32 + P3r 33
91
PI rll
92 93
_A - 1BE - 1 =
[ 91 92 93]
(4.69)
1 [d 1rll + d2r 21 + d3r 31 ] d 1r12 + d2r22 + d3r32 d1r13 + d2r 23 + d3r 33
-E
(4.71)
where 1
III
rll
+E
!I2
r 21
+E
!I3
1 r 31 + E (PIr 31
+ P2r32 + P3r 33) (d1r u + d2r 21 + d3r 3d
121
1 r12 + E (Pl r ll
+ P2 r12 + P3r13) (d1r12 + d2r 22 + d3r 32)
122
1 r22 + E (Plr21
+ P2 r22 + P3r23) (d1r1 2 + d2r22 + d3r 32)
f 23
r32
1
1
+E
(Plrll
+ P2r12 + P3r13 ) (d1r ll + d2r 21 + d3r 3d
(Pl r21 + P2r 22 + P3r 23) (d1rll
(Plr31 + P2 r 32 + P3 r 33) (d1r 12
1
131
=
r 13 + E (Pl r ll
132
=
r23 + E (Pl r 21 + P2r 22 + P3r23) (d1r 13
133
1
1 r33 + E (Plr31
+ d1r 21 + d1r 3d
+ d2r22 + d3r 32)
+ P2r12 + P3r13) (d1r 13 + d2r 23 + d3r 33) + d2r23 + d3r 33)
+ P2 r 32 + P3 r33) (d1r13 + d2r23 + d3r 33) '
4. Motion Kinemati cs
145
Zj
Z,
c£ 0
Ad 2
....
~
Y
X
Y2
F IGURE 4.6. Three coordinate frames to analyze compound t ransformat ions.
In the case of a coordin ate homog en eous transformation
[Tj [Aj [B] [C] [D]
ATB
(4.73)
ARB
(4.74)
Ad
(4.75)
[0 0 0]
(4.76)
[1]
(4.77)
[E] = [1]
(4.78)
we have
and th erefore,
(4.79)
4.4
Compound Homogeneous Transformation
Figure 4.6 shows three reference frames: A , B , and C . The transformation matri ces to transform coordinates from fra me B to A , and from frame C to B are (4.80) (4.81)
146
4. Motion Kinematics
Hence, the transformation matrix from C to A is
ATC
ATBBTC [ AR B Ad l
a
I
[ AR BRc Ba
][ B~C B~z ] ARB Bd z + Ad l I
]
[ A ~C A~Z ]
(4.82)
and therefore, the inverse transformation is
CTA =
A
[ BR'Sa R~
- BR'S AR~ [AIRB Bd z + Ad l ]
[ B R'S aA R~
- BR'S Bd
AR'S - AR'S Ad z] [
a
I
'
zl
]
- AR~ Ad l ]
(4.83)
The value of homogeneous coordinates are better appreciated when several displacements occur in succession which, for instance, can be written as (4.84)
rather t han
GR4 4rp
+G d 4 G
=
u,
e e e Rz
R3
R4 4r p + 3d 4 ) + Zd 3) + l d z) + Gd l. (4.85)
Example 87 Homogeneous transformation for multiple frames. Figure 4.7 depicts a point P in a local frame B z (XZyzzz) . The coordinates of P in the global frame G(0 XY Z) can be found by using the homogeneous transformation matrices. The position of P in frame B z (xzyzzz) is indicated by zrp. Therefore, its position in frame B, (XlYlZl) is
(4.86)
4. Motion Kinematics
147
Zi
x
~
FIGURE 4.7. Point P in a local frame B 2 (X2Y2Z2) .
and therefore, its position in the global frame G( OXY Z) would be
[t]
[
C~l
c dl 1
[
C~2
c dl
[ CR
1
CR 2 20
][~j] ][
1R
cR 2 l
2
0
][
'~' ~n
d2 + c d l 1
][ ~n
(4.87)
Example 88 Rotation about an axis not going through origin. The homogeneous transformation matrix can represent rotations about an axis going through a point differ-ent from the origin. Figure 4.8 indicates an angle of rotation, ¢, around the axis ii , passing through a point P apart from the origin . We set a local frame B at point P parallel to the global frame G. Then, a rotation around it. can be expressed as a translation along -d, to bring the body fame B to the global frame G, fo llowed by a rota tion about it. and
148
4. Motion Kinematics
y
FIGU RE 4.8. Rotation about an axis not going through origin .
a reverse translation along d
(4.88)
where, ui vers q; + cq;
Ru,q, =
Ul Uz vers q; - u3sq; U l U3 ver s q; + uzsq; ] u~ vcrs q; + cq; UZU3 vcr s q; - ulsq; q; + u3s q; [ Ul U3 vers q; - uzsq; UZU3 vcrs q; + UlSq; u5 vcr s q; + cq; Ul Uz vers
(4.89)
and d - Ru,q, d dl (1 -
[
=
(4.90)
ui) vers q; - Ul vers q; (dzuz dz (1 - u~) vers q; - Uz vers q; (d3U3 d3( 1 - u5) vers q; - U3 vers q; (d 1Ul
+ d3U3) + sq; (dZU3 + d1 ud + sq; (d3Ul + dzuz) + sq; (d 1Uz -
d3Uz) ] d1 U3) . dzud
Example 89 End effector of an RPR robot in a global frame. Point P indicates the tip point of the last arm of the robot shown in Figure 4.9. Position vector of P in fram e B 2 (XZyz zz) is zrp. Frame B z (XZyz zz) can rotate about zz and slide along Yl . Frame B 1 (XIYl Zd can rotate about the Z -axis of the global fram e G(OXYZ) while its origin is at c d l . The position of P in G(OXYZ ) would then be at
CR 1 l Rzzr p + CR11 d z+ c d l
(4.91)
4. Motion Kinematics
149
FIGURE 4.9. An RPR manipulator robot .
where (4.92)
(4.93)
and (4.94)
Example 90 End-effector of a SCARA robot in a global frame. Figure 4.10 depicts an RIIRII? (SCARA) robot with a global coordinate frame G( OXY Z) attached to the base link along with th e coordinate frames B1(01XIYIZl) and B2(02X2Y2Z2) attached to link (1) and the tip of link (3) . The transformation matrix, which is utilized to map points in frame B 1 to the base frame G is
(4.95)
and the transformation matrix that is utilized to map points in frame B 2
150
4. Motion Kinematics
FIGURE 4.10. The SCARA robot of Exampl e 90.
to the frame B 1 is
(4.96)
Therefore, the transformation matrix to map points in the end -effector frame B 2 to the base frame G is GTB 2
l
GTB 1
B 1T
(4.97)
B2
c(8,+8,) S(0 1+ 02) 0 0
- s (fh
+ O2)
C(01+ 02) 0 0
0 0
1 0
1,<1I, +I,c(8, +8,) ]
h S01 + l2s (01 + O2) - h
.
1
The origin of the last frame is at B2 r 02 = [ 0 0 0 1] T, in its local frame . Hence , the position of 02 in the base coordinate frame is at
(4.98)
Example 9 1 Object manipulation. The geometry of a rigid body may be represented by an array containing the homogeneous coordinates of some specific points of the body described
4. Motion Kinematics
151
z
~:'6~--J
2
x
...y
(a)
x y
FIGURE 4.11. Describing the motion of a rigid body in terms of some body points.
in a local coordinate frame. The specific points are usually the corners, if there are any . Figure 4.11 (a) illustrates the configuration of a wedge. The coordinates of the corners of the wedge in its body frame B are collected in the matrix
P. 1 1 1 0 0 0 ]
Bp=
0
0
3
3
0
0
(4.99)
[ 4 0 0 0 0 4 1 1 1 1 1 1
The configuration of the wedge after a rotation of -90 deg about the Zaxis and a translation of three units along the X -axis is shown in Figure 4.11(b). The new coordinates of its corners in the global frame G are found by multiplying the corresponding transformation matrix by the P matrix.
D X ,3 R Z ,90
[
~ 0~ ~1 0~] ~~~=~~ 0
[
o o
[
0 0 1
0
~1o 0~ ~1 0~] . o
0
0
1
- sin -90 cos -90
o o (4.100)
152
4. Motion Kinem atics
FIGURE 4.12. Cylindical coordinates of a point P .
Therefore, the global coordinates of corners 1 to 6 after motion are
Gp
GTBBp
[0 103][1 -1
o o
[3 3 - 1 4 1
1 1 0 0 3 4 0 0 1 1 1
0 0 0 0 1 0 0 0 1 6 - 1 -1 0 0 1 1
3 0 0 0 0 1 1 6
0 3 0 1
n
0 0 0 1
~] (4.101)
Ex ample 92 Cylindrical coordinates. Th ere are situation in which we wish to specify the position of a robot end-effector in cylindrical coordinat es. A set of cylindrical coordinat es, as shown in Figure 4.12, can be achieved by a translation r along the X -axis, followed by a rotation ip about the Z -axis, and finally a translation z along the Z-axis. Th erefore, the homogeneous transformation matrix for going from cylin-
4. Motion Kinemat ics
153
FIGU RE 4.13. Spherical coordinates of a point P.
dri cal coordinates C(Onpz) to Cart esian coordinates G(OXYZ) is
»., Rz,'P Dx ,r
GTc
1 0 0 0
o 1 0 001 [ 000 [
=
0 z
1
['~~
- sin e cos cp 0 0
sin c
0 0
1
'OO ~ sm cp
- sin
0 0
0 0
cos o
oo rcos ~1 r sin cp 1
z
o
1
.
0 0 1 0
~ 1l ~ ~ 1 0 0 1 0 0 1 0 0
(4.102)
A s an example, consider a point Pat (1, i , 2) in a cylindrical coordinate fram e. T hen, the Cartesian coordin ates of P wou ld be
i sin i o o
COS
[
• 7l" -sm 3 cos i
o o
cos i sin i 1 2 o 1
o o
Example 93 Sph erical coordinates. A set of spherical coordin ates, as shown in Figure 4.13, can be achieved by a tran slation r along the Z -axis, follow ed by a rotation () about th e Y axis, and finally a rotation sp about th e Z -axis. Th erefore, the homogen eous transformation m atrix fo r going f rom spher-
154
4. Motion Kinematics
ical coordinates S (OrOrp) to Cartesian coordinates G(0 XY Z) is
GTs
Rz,'P Ry,1J Dz ,r [ cos 0 cos ~ cosO sin o - sin O
Rz,'P =
Ry"
~
sinO sin e cosO
0 0
0
where
cos o sin f
- sin rp cos ip
c?S~
- sin rp
0
0 0 1
0
0
0
0 sinO 1 0 0 cosO 0 0
cosO 0 [ - sin O 0
DZ,r =
~] ~]
[! n 0 1 0
0
0 0 1 0
rcosO 1
0
cos c 0
[ sm c
r cos c sin e ] r sin 0 sin rp (4.103)
(4.104)
(4.105)
(4.106)
As an example, consider a point P at (2, ~, ~) in a spherical coordinate frame . Then, the Cartesian coordinates of P would be
cos 1L 3 3 cos ~ sin ~ • 1r -sm 3
COS 1L
[
4.5
o
-sin~
cos ~
o o
cos ~ sin ~ sin ~ sin ~ cos ~
o
* Screw Coordinates
Any rigid body motion can be produced by a single translation along an axis combined with a unique rotation about t hat axis. T his is called Chasles theorem. Such a motion is called screw. Consider the screw motion illustrated in Figure 4.14. Point P rotates about the screw axis indicated by u and simultaneously translates along the same axis. Hence, any point on t he screw axis moves along t he axis, while any point off the axis moves along a helix. The ang ular rotation of the rigid body about the screw is called twist. Pitch of a screw, p, is t he ratio of translation, h, to rotation, tjJ.
h dJ
p= -
(4.107)
4. Motion Kinematics
155
FIGURE 4.14. A screw motion is tr anslation along a line combined with a rotation about th e line.
In ot her words, the rectilinear distan ce through which th e rigid body translates par allel to t he axis of screw for a unit rotation is called pitch. Note that if p > 0, t hen th e screw is right-han ded, whereas if p < 0, it is left-h and ed. A screw is shown by s(h, ¢, u, s) and is indicated by a uni t vector U, a location vector s , a twist angle ¢, and a t ra nslation h (or pitch p) . The locati on vector s indicates t he globa l positi on of a point on t he screw axis. The twist angle ¢, t he twist axis U, and t he pitch p (or translation h) are called screw param eters. Homogeneous matri x representation of a screw t ransforma tion is a combination of rotation and t ra nslat ion abo ut t he screw axis. If u passes t hro ugh t he origin of t he coordinate fra me, t hen s = 0 and t he screw motion is called central screw s(h, ¢, u). The screw is basically anot her transform ati on meth od to describ e the moti on of a rigid body. A linear displacement along an axis combined with an angular displacement about t he same axis arises repeatedly in robotic application. For a cent ral screw we have G sB(h, ¢ , u)
= D it,h R it ,
(4. 108)
where,
D it,h
=
[~
0 0 1 0 0 1 0 0
hu, hU2 hU3 1
]
(4.109)
156
4. Motion Kinematics
Ul U2
vers ¢ -
vers ¢ + U2S¢ vers ¢ - Ul s¢ u~ vers¢ + c¢
U3S¢
Ul U3
U~ vers e } ob U2U3
vers ¢ +
U2U3
Ul s¢
o
o
~j
(4.110) and hence ,
j
vers ¢ + U2S¢ bu, vers¢ - UlS¢ hU2 U2U3 vers ¢ + Ul s¢ u~ vers ¢ + c¢ hU3 . 0 0 1 (4.111) As a result , a central screw transformation matrix includes the pure or fundamental translations and rotations as special cases because a pure translation corresponds to ¢ = 0, and a pure rotation corresponds to h = 0 (or p = (0). A reverse central screw is defined as s(- h, - ¢, il) . When the screw is not central and il is not passing through the origin, a screw motion to move p to p" is denoted by Ul U2
vers ¢ -
U3S¢
u~ vers¢ + c¢
p"
Ul U3
U2U3
= (p-s)cos¢+(l-cos¢)(il·(p-s))il + (il x (p - s)) sin ¢ + s + hil
(4.112)
or
p"
=
+ s + hil G RB p + s - G RB S + hil
G RB
(p - s)
(4.113)
and therefore, p"
= s(h, ¢, il, s)p = [T] p
(4.114)
where
[T]
Gs
-
GR
Gd ] 1 .
f
Gs
+ hil
]
(4.115)
The vector G s , called location vector, is the global position of the body frame before screw motion. The vectors p" and p are global positions of a point P after and before screw, as shown in Figure 4.15. The screw axis is indicated by the unit vector ii. Now a body point P moves from its first position to its second position pi by a rotation about il . Then it moves to P" by a translation h parallel to ii. The initial position of P is pointed by p and its final position is pointed by p" .
4. Motion Kinematics
157
y
FIGURE 4.15. Screw motion of a rigid body. In some references, screw is defined as a line with a pit ch. To indicate a motion, they need to add the angle of twist . So, screw is the helical path of motion, and twist is the actual motion . However , in this text we define a screw as a four variable function s(h, ¢J, u, s) to indicate th e motion . A screw has a line of act ion u at os, a twist ¢J, and a translation h. The inst antaneous screw axis was first used by Mozzi (1763) although Chasles (1830) is credited with this discovery. Proof. The angle-axis rotation formula (3.4) relat es r' and r , which are position vectors of P after and before rot ation ¢J about u when s = 0, h = o.
r'
=
r cos ¢J + (1 - cos ¢J) (u . r) u +
(u x r) sin ¢J
(4.116)
However, when th e screw axis does not pass through the origin of G( 0 XY Z) , then r' and r must accordingly be substituted with the following equat ions: r
p-s
r'
p" _
S -
hii
(4.117) (4.118)
where r' is a vector after rotation and hence in G coordinate frame, and r is a vector before rot ation and hence in B coordinate frame. Therefore, th e relationship between the new and old positions of the bod y point P after a screw motion is pI!
= (p-s)cos ¢J+(I-cos ¢J)(u .(p-s))u
+ (u x
(p - s)) sin ¢J + (s + hu).
(4.119)
158
4. Motion Kinematics
Equ ation (4.119) is t he Rodriquez form ula for th e most general rigid bod y motion. Defining new not ations G p = p" and B p = P and also noting that s indicat es a point on the rot ation axis and t herefore rotation does not affect s , we may factor out B p and writ e t he Rodriguez formula in t he following form Gp
= (lcos ¢+ uil7(1-cos ¢ ) +u sin¢) B p - (I cos ¢ + uu T (1- cos¢) + u sin¢)
Gs
+
Gs
+ hu (4.120)
which can be rearranged to show that a screw can be repr esent ed by a homogeneous transformation GRBB p + G S _ GRBG S+ hu
(4.121)
GRBB p+ Gd GTBB p
where, L cos o
+ uu T
((I - uu
T
)
(1- cos¢) + u sin o (1- cos ¢) - usin¢) G s + hu.
(4.123) (4.124)
Direct subst it ut ion shows t hat
ui vers ¢ + c¢
vers ¢ -
vers ¢ + U2 S¢ ] vers ¢ - U 1s¢ [ u~ vers ¢ + c¢ U 2 S¢ U 2U 3 vers ¢ + U1 s ¢ (4.125) hU1 - U1 (S3 U3 + S2U2 + slu d vers ¢ + ( S2U 3 - S3U2) s¢ + Sl ] Gd = hU2 - U 2 (S3 U3 + S2U2 + slud vers ¢ + ( S3 U 1 - Sl U3 ) s ¢ + S2 . [ h U 3 - U3 ( S3113 + S2112 + S l U 1 ) vers ¢ + ( Sl U 2 - s211d s¢ + S3 (4.126) This repr esent ation of a rigid motion require s six independent par ameters , namely one for rotation angle ¢ , one for t ra nslat ion h, two for screw axis U, and two for location vector G s . It is because three components of u are related to each other according to G RB
=
vers ¢ + U1 U 3 vers ¢ U 1U 2
U3S¢
U 1U 2
U 3 S¢
u~ vers ¢ + c¢
uTu = 1
U1 U 3
U 2U3
(4.127)
and the location vector G s can locat e any arbitra ry point on th e screw axis. It is convenient to choose the point where it has t he minimum distance from o to make G s perp endicular to ii . Let 's indicat e the shortes t location vector
4. Motion Kinematics
159
by GSa , then there is a constraint among the components of the location vector (4.128)
If S = 0 then the screw axis passes through the origin of G and (4.122) reduces to (4.111). The screw parameters ¢ and h, together with the screw axis u and location vector G s, comp letely define a rigid motion of B(oxyz) in G(OXYZ) . It means, given the screw parameters and screw axis, we can find t he elements of t he transformation matrix by Equations (4.125) and (4.126). On t he other hand, given the transformation matrix GTB , we can find th e screw angle and axis by
~ (tr (G RB) - 1)
cos¢
~ (tr (GTB ) - 2) 1
"2 (Tn + T22 + T33 1 u= -2 -. +. sm e
(G R B
-
-
1)
GR T) B
(4.129)
(4.130)
hence, ,
U =
1 2sin ¢
(4.131)
-
To find all the required screw parameters, we must find h and coordinates of one point on the screw axis. Since the points on the screw axis are invari ant under the rotation , we must have
wher e (X, Y, Z) are coordinates of points on the screw axis . As a samp le point , we may find t he int ersection point of the screw line with Y Z- plane , by setting X = 0 and searching for s = [0 Y Z Therefore,
s
s sf .
T14 -
hu,
T24 -
hU2
T34 -
hU3
o
][ ~] [ ~]
(4.133)
160
4. Motion Kinematics
which generates t hr ee equations to be solved for Y s , Z s , and h .
(4.134)
Now we can find t he shortest locat ion vector Gsa by
Gso = s -(s ·U)U.
•
(4.135)
*
Example 94 Central screw transformation of a base unit vector. Consider two initially coincident frames G(OXYZ) and B(oxyz) . The body performs a screw motion along the Y-axis for h = 2 and ¢ = 90 deg. The position of a body point at [ 1 0 0 1 ] T can be found by applying the centra l screw transformation. 1f
8(h, ¢ ,u)
A
(4.136)
8(2'2 , J) D(2J) R(J,
~)
[1o 000][ 0 o
o
0 1 0 0
1 020 0 1 0 - 1 0 0 1 0
1 0 0 0
[ 0o 010] 1 - 1 0 o 0
0 0 0
~]
2 0 1
Therefore, •
A BA
1f
s(2, 2' J) z
(4.137)
[ ~1 ~ H][~ ] o
0 0 1
[ 0
2
h
2
4
¢
~
1f
-1
1
1
f.
The pitch of this screw is P=
- = - = - = 1.2732
unit /rad .
(4.138)
4. Motion Kinematics
161
*
Example 95 Screw transformation of a point. Consider two initially parallel frames G(OXYZ) and B(oxyz) . The body performs a screw motion along X = 2 and parallel to the Y-axis for h = 2 and ¢ = 90 deg. Therefore, the body coordinate frame is at location s = [2 0 0 ( . The position of a body point at B r = [3 0 0 1]T can be found by applying the screw transformation, which is [ G ~B
S-
~1 ~ o r
G R~
s
+ hu
]
(4.139)
H]
0 0
1
because,
Therefore, the position vector of G r would then be Gr
=
GTBBr
(4.140)
*
Example 96 Rotation of a vector. Transformation equation G r = G R B B r and Rodriguez rotation formula (3.4) describe the rotation of any vector fixed in a rigid body. However, the vector can conveniently be described in terms of two points fixed in the body to derive the screw equation. A reference point PI with position vector r l at the tail, and a point P2 with position vector r2 at the head, define a vector in the rigid body. Then the transformation equation between body and global frames can be written as G (r2 - rt}
=
GRB
B
(r2 - rl) '
(4.141)
Assume the original and final positions of the reference point PI are along the rotation axis . Equation (4.141) can then be rearranged in a form suitable
162
4. Motion Kinematics
fo r calculatin g coordin ates of the ne w position of point P2 in a tran sformation matrix form GRB B
(r2 - r1)
G RB B r 2
GT B
+ Gr 1
+ Gr 1 -
(4.142)
G RB B r 1
B r2
where G r1 -
G R B B r1 ]
1
.
(4.143)
It is compatible with screw motion (4.122) fo r h = O.
*
Example 97 Special cases for screw determination . Th ere are two special cases fo r screws. Th e fir st on e occurs when r ll = r22 = r33 = 1, then, ¢ = 0 an d the motion is a pure tran slation h parallel to il , where, , r 14 - Sl' r24 - S2' r34 - S3 ' (4.144) u= h 1+ h J+ h K. Sin ce there is no un ique screw axis in this case, we cannot locate any specific point on th e screw axis. Th e second special case occurs when ¢ = 180 deg . In this case
J~ (rll + 1) ]
it =
[ J~
(r22 + 1)
(4.145)
J~ (r33 + 1)
how ever, h and (X , Y, Z) can again be calculated from (4.134).
*
Example 98 Rotation and translation in a plan e. Assume a plan e is displa ced from position 1 to posit ion 2 according to Figure 4.16. N ew coordin ates of Q2 are
4. Motion Kinematics
y
163
I----+--~--+--+--+--+-- Po
FIG URE 4.16. Motion in a plane.
or equivalently (4. 147)
*
Example 99 Pol e of planar motion. In the plana r motion of a rigid body, going from position 1 to position 2, th ere is always one point in th e plan e of motion that does not change it s position. Hence, th e body can be cons idered as having rotated about this point, which is kno wn as the fi ni te rotation pole. Th e tran sformation matrix can be used to locate the pole. Figure 4.16 depicts a planar' mo tion of a triangl e. To locate the pole of motion Po(Xo , Yo) we need th e tran sformation of the motion. Using th e data given in Figure 4.16 we have
2T 1
[ 2
~l
[00sa0 0
r P2
-sa ca 0 0
-
~ R 1 r PI 0 0 1 0
]
(4.148)
~OO hH 4 1 + 3.5
- ca - sa
o 1
.
164
4. Motion Kinematics
Th e pole would be conserved under the transform ation . Th erefore,
which fo r a
= 58 deg
Example 100
provides
Xo
-1.5 sina + 1- 4 cos a
Yo
4 sin a
+1-
= 2.049
1.5 cos a = 3.956.
*
Determination of screw param eters. W e are able to determ in e screw param eters when we have the original and fin al posit ion of three no n- colin ear points of a rigid body. A ssume Po, qn, and ro denote the posit ion of points P , Q, and R before the screw motion , and PI , ql , and rl denote th eir posit ions aft er the screw motion . To determin e screw param eters, cP, fi , h , and 8 , we should solve th e following three simultaneous Rodriguez equations:
PI - Po
tan
~fi x (PI + Po -
28) + hfi
(4.150)
ql - qo
tan
~fi x (q l + qo -
28) + hfi
(4.151)
r l - ro
tan ~fi x (rl
28) + hfi
(4.152)
+ ro -
W e start with subtracting Equ ation (4. 152) from (4.150) and (4.151) .
~fi x [(PI + Po) -
(PI - Po) - (r l - ro)
tan
(ql - qo) - (r l - ro)
tan ~fi x [(ql
+ qo) -
(r l - r o)] (4.153) (rl - ro)] (4.154)
No w m ultiplying both sides of (4·153) by [(ql - q o) - (rl - ro)] which is perpendicular to fi
[(ql - qo) - (r l - ro)] x [(PI - Po) - (r . - ro)] = t an ~ [(q l - qo) - (rl - ro)] x { fi x [(P I + Po) - (r l - ro)]}
(4.155)
gives us
[(ql - qo) - (rl - ro)] x (PI
+ Po) -
(rl - ro)
= t an ~ [(ql - qo) - (rl - ro)] . (PI + Po) - (r , - ro) fi
(4.156)
and th erefore, the rotation angle can be found by equating t an ~ and the n orm of the right-hand side of the follow ing equatio n:
(4.157)
4. Motion Kinemati cs
To find s, we may start with the cross product of
u x PI -
Po
u x [tan ~u x (P I + Po -
=
t an
~ {[U' (PI + Po)] U-
u with Equation
2s) +hU] (PI
165
(4.150) .
(4.158)
+ Po) + 2 [s -
(u · s) ul}
Not e that s - (u . s) u is the component of s perpendicular to U, where s is a vect or from the origin of the global fram e G( OXY Z) to an arbitrary point on the screw axis . This perpendicular component indicates a vector with the shortest distance between 0 and U. Let 's assume So is the name of the shortest s . Th erefore, So
s-
= -21
(u· s) u
[u
X
PI 1?. - Po - [Au · ( PI t an 2
+ Po)] uA + PI + Po]
·
(
4.159 )
Th e last parameter of the screw is the pit ch h , which can be found from any one of the Equat ions (4.150), (4.151), or (4·152) .
h
u· (PI u· (qI
- Po)
- qo)
ii - (rl - ro)
(4.160)
*
Example 101 Alternative derivation of screw transform ation. Assume the screw axis does not pass through the origin of G. If G S is the position vector of some point on the axis U, then we can derive the matrix representation of screw s(h, ¢, U, s) by tran slating the screw axis back to the origin, performing the central screw motion , and translating the lin e back to its original position .
s(h, ¢ , ii , s)
D(Gs) s(h, ¢, u) D( - G s ) D(Gs) D(hu) R(u, ¢) D( -
[~ ~s] [G~B
[ ~B G
Gs
Gs)
h1U] [~
- GR:GS + hU ]
-;s] (4.161)
*
Example 102 Rotation about an off- cent er axis. Rotation of a rigid body about an axis ind icated by u and passing through a point at G s, where Gs x u -lOis a rotat ion about an off- cent er axis . Th e tran sformation matrix associat ed with an off-cent er rotat ion can be obtain ed from the screw tran sformation by setting h = O. Th erefore, an off-cent er rotat ion transformation is
GT = [ GRB B 0
Gs
- GRBG s ]
1
.
(4.162)
166
4. Motion Kinematics
*
Example 103 Principal central screw. Th ere are three principal central screws, namely th e x-screw, y-screw, and z -screw, which are _ ' s(h z , 0: , K)
_
[ cos o sin o:
~
=
[
A
=
s(hy , jJ,J)
s(hx ,"1,i) =
*
coo0 ~
0 1 . jJ 0 -sm 0 0
[~
0 cos "1 sin "1 0
0 0 1 0
p~o
sin jJ 0 cos jJ 0
P~~
- sino: coso: 0 0
0 - sin "1 cos "1 0
(4.163) ]
(4.164)
]
T] o
.
(4.165)
1
Example 10 4 Proof of Chasl es th eorem . Let [T ] be an arbitrary spatial displa cem ent, and decompose it into a rotation R about u and a translation D .
[T] = [D][R]
(4.166)
We may also decompose the tran slation [D] into two compone nts [D II ] and respectively .
[D.1], parallel and perpendicular to
u,
(4.167) Now [D.d [R ] is a planar motion, and is therefore equivalent to some rotation [R'] = [D.1][R ] about an axis parallel to the rotation axis u. This yields the decomposition [T ] = [D11][R']. This decomposition com pletes th e proof, since th e axis of [D ill can be tak en equal to ii.
*
Ex ample 105 Ev ery rigid motion is a screw. To show that any proper rigid motion can be consid ered as a screw mo tion, we must show that a homog eneous transformation matrix GTB
= [ G~B GIrl ]
(4.168)
can be written in the form GT
B
= [ G ~B
(1 -
GR
f ) + hu ] . S
(4.169)
This problem is then equivalent to the following equation to find hand U. (4.170)
4. Motion Kinematics
167
The matrix [I - G R B] is singular because G RB always has 1 as an eigenvalue . This eigenvalue corresponds to it as eigenvector. Therefore, (4.171)
and an inner product shows that
[I - G RB] S + it . hit [I - G R B ] it . s + it . hit
it ·
which leads to (4.172)
Now we may use h to find s
(4.173)
*
Example 106 Classification of motions of a rigid body. Imagine a body coordinate frame B(oxyz) is moving with respect to a global frame G (0 XY Z) . Point P with position vector P B = [PI P2 P3 ] T is an arbitrary point in B(oxyz) . The possible motions of B(oxyz) and the required transformation between frames can be classified as: 1. Rotation ¢ about an axis passing through the origin and indicating by the unit vector it = [Ul U2 U3] T (4.174)
where
G RB
comes from the Rodriguez 's rotation formula (3.4).
2. Translation by
Gd
=
[d 1
d2
d3
f
plus a rotation
= indicated by G S =
3. Rotation ¢ about an axis on the unit vector it passing through an arbitrary point
4.
G RB .
[ Ul
U2
U3] T
[SI
S2
S3 ] T .
Screw motion with angle ¢ and displacement h, about and along an passing through an arbitrary axis directed by it = [Ul U2 U3
f
point indicated by G s =
[SI
S2
S3
f.
(4.177)
168
4. Motion Kinematics
5. Reflection
(a) in the xy-plane; G
p= GR B Bp ( -
(4.178)
z)
where (4.179)
(b) in the yz -plane; G
P= GR B
B
(4.180)
p (- x )
where (4.181)
(c) in the x z -plane; G
P= GR B Bp ( -
(4.182)
y)
where (4.183)
(d) in a plane with equation U1X + U2Y + U3Z + h = 0; 2 \ 2 (GRBB p - 2hu) U1 + U2 + U3 GRBB p - 2hu
(4.184)
where - UI GRB
=
[
+ U§ + U§
2U2U1 - 2U1U3
-
- 2U1U2 UI - U§ + U§ - 2U3U2
(e) in a plane going through point u = [U1 U2 U3 ]T
[ Sl
S2
- 2U3U1 ] - 2U2U3 . UI + U§ - u§ S3
r
(4.185)
and normal to
(4.186)
where G RB is as in (4.185).
4.6
*
4. Motion Kinematics
169
Inverse Screw
Inverse of the screw s(h,
(4.187) where il is a unit vector indicating the axis of screw, s is the location vector of a point on the axis of screw,
(4.188)
Proof. The homogeneous matrix expression of a screw s(h,
(4.189) A homogeneous matrix (4.190) can be inverted according to
(4.191) To show the correctness of Equation (4.187), we need to calculate -
(C s _ CRBCs+hil) C R~ C s + C R~ C R B C S C R~ C s + C s - C R~ hil .
C R~
cd:
-CRI; -
C R~
hil
(4.192)
Since il is an invariant vector in both coordinate frames Band G, we have A
U=
CRBU= CRT BU A
A
(4.193)
and therefore, (4.194)
170
4. Motion Kinematics
This completes the inversion of a genera l screw:
Gv- l(h A. ' )= [ GR~ sB ,'I', u , S 0
GS-GR~GS-hU ]. 1
(4.195)
If the screw is central, the location vector is zero and the inverse of the screw simplifies to
G sB V -l(h ,'I', A. U,)=[ GR~ - 1hU ] . 0 Since the inversion of a rotation matrix rotation -
GR B
=
(4.196)
Rit , can be found by a
GR-B l = GRTB = BRG = Rit,-
(4.197)
the inversion of a screw can also be interpreted as a rot ation -
Gsi / (h,
(4.198)
• Example 107 Checking the in versi on formula . Employing the inv ersion screw formula , we mu st have
s(h,
(4.199)
It can be checked by [ G ~B
[I; [I;
Gs
*
G R~ G s
+ hu
] [
G ~~
Gs
-
GR
f
Gs
- hu ]
GRB(GS - GR~GS-hU{ +(GS - GRBGS +hU) ]
GRB GS- GS- h GRBt +
[~ ~] 4.7
-
=
Gs
- GRBGs + hu ]
14 .
(4.200)
Compound Screw Transformation
Assume 1 82(hI ,
Gs2(h,
4. Motion Kinematics
So - GR\so si -
1R
+ houo ]
2;1+ hI Ul ]
171
(4.202) (4.203)
shows that
G82(h,¢, u, s)
(4.204)
[ G~l
so- GR\ SO+ hOUO ] [1~2
[ G~2
GR l (Sl - l R2Sl + hlU{ ) + SO- GR l SO+ hOUO ]
[ G~2
GRl (l - lR2)Sl+(I- ~Rt}SO+hl GRlUl+hOUO]
Sl- lR2;1+hlUl]
where G R2
= G R l 1 R2 .
(4.205)
To find the screw parameters of the equivalent screw G 82(h,¢, U, s) , we st art obtainin g U and ¢ from G R 2 based on (4.131) and (4.129). Th en, utili zing (4.172) and (4.173) we can find h and S
h= u · Gd
(4.206) (4.207)
where Gd
•
GR l (l - lR2)Sl+(I- GR t} so + hlG RlUl + houo (GR l - GR 2) Sl + GR l (hlUl - so) + So + houo. (4.208)
*
Example 108 Exponential representation of a screw. To compute a rigid body motion associated with a screw, consider the motion of point P in Figure 4.15. The final position of the point can be given by
p"
+ e
(4.209)
S
[T]p where [T] is the exponential representation of screw motion _ [ e
[T] -
(I - e
.
(4.210)
172
4. Motion Kinemat ics
The exponential screw transformation matrix (4.210) is based on the exponential form of the Rodriguez formula (3.1 01) e
(4.211)
Therefore, the combination of two screws can also be found by
where Gd
+ e
= (e
st)
+ SI + hI iiI .
(4.213)
*
Example 109 Combination of two principal central screws. Combination of every two principal central screws can be found by matrix multiplication. As an example, a screw motion about Y followed by another screw motion about X is
s(h x ,"(, i ) s(h y,;3, J) 0 0 "( px c"( - s"( 0 0 S'f C"(
[~
0
0
[ sinc;3sin oofi "( - cos J sin;3
1 0 cos"( sin "( 0
(4.214)
][
~ffi
0 1 0 0
s;3
0 0 ;3 py c;3 0 1 0
sin;3 1Px cos "( - cos;3 sin "( ;3 py ;3 py sin "( cos ;3cos"( 0 1
] ] .
Screw combination is not commutative and therefore, s(h x ,"( ,i)S (h y,;3, J)
-I- s(hy ,;3,J )S (h x ,"(, i) .
(4.215)
*
Example 110 Decomposition of a screw. Every general screw can be decomposed to three principal central screws. [ G
~B
S -
G
Rf s + hu ]
s(h x, 'f ,i)S(h y,;3, J) s(h z, ex , K)
(4.216)
Therefore, G RB =
[
~:s;3s"(
cv sa sexs"( - cexC"(s;3
- c;3sex cexC"( - sexs;3s"( coe» + C"(sexs;3
s;3 ] -c;3s"( c;3C"(
(4.217)
4. Motion Kinemati cs
173
and
c~~ ~ ~~; ~~~~Sirq
~~
s - G R B S + hit = [ jJpy ] = [ ] . jJpy sin 'Y + o:pz cos jJ cos 'Y dz
Th e twis t angles 0: , jJ, 'Y can be found from Pz can be found as follo ws pz py = px
*
dx
=-
'Y
-
G RB ,
(4.218)
th en, th e pit ches ux, pv ,
dz cOS'Y - dy sin 'Y o:cosjJ dz sin 'Y + d y cos 'Y
e
dz cOS'Y - d y sin'Y . (.i sm f-!. 'Y cos jJ
(4.219) (4.220) (4.221)
Example 111 Decomposition of a screw to prin cipal central screws . In gen eral, there are six different independent com bina tions of triple principal cen tral screws and therefore, there are six different m ethods to decompose a general screw into a com bination of principal central screws . Th ey are: 1 - s (h x" , i) s (h y , jJ, J) s (hz , 0:, K) 2 - s(hy , jJ, J) s (h z, 0:, K) s (h x , 'Y, i) 3 - s (h z, 0: , K)S(h x , 'Y , i) s(h y , jJ, J) (4.222) 4 - s (h z, 0:, K) s (h y, jJ, J) s (h x, 'Y , i) 5 - s (h y, jJ, J) s (h x, 'Y , i) s (h z, 0: , K) 6 - s (hx, 'Y , i) s (hz , 0: , K) s (h y , jJ, J) Th e expanded form of the six com bin ations of prin cipal central screws are present ed in Appendi x C.
4.8
* The Plucker Line Coordinate
The most common coord inate set for showing a line is t he Plucker coordinates. An alytical representation of a lin e in sp ace ca n be found if we have t he position of t wo differ ent po ints of that line . Assume PI (X I, YI , ZI) and P2(X2 , Y2 , Z 2) at rl and r 2 ar e two different points on the line l as shown in Figure 4.17 . Using the position vect ors rl and r 2, the equation of lin e l can be defined by six elements of two vectors
l=[~]=
L M N p
Q R
(4.22 3)
174
4. Motion Kinematics
o
x
y
~
FIGURE 4.17. A line indicated by two points. referred to as P liicker coordin ates of t he directed line, where,
(4.224) is a uni t vector along the line referr ed to as directi on vector, and
(4.225) is the mo m ent vector of u about t he origin . The Plucker method is a canonical representation of line definiti on and t herefore is more efficient than t he ot her methods such as par ametric form l(t) = rj + til , point and dir ection form (rj ,u), or two-point representation (rj,r2) . Proof. The unit vecto r U, whose direction is along th e line connect ing P j and P2 , is
r2 - r j Ir2 - rj l
Li+MJ+ NK X2 - X j Y2 - Yj Z2 - Zj d 1+ d J+ d K A
A
A
(4.226)
where ,
(4.227) and the dist an ce between P, and P2 is (4.228)
If r repr esents a vector from t he origin 0 to a point on t he line l , t hen t he vector r - rj is parallel to u and therefore, the equation of the line can be written as (4.229) (r - rr) xu = 0
4. Motion Kinemati cs
175
or equivalently (4.230)
r xil=p
where p = rl x il
is the moment of the dir ection vector il about O . Furthermore, becau se vectors p and il are perpendicular , t here is a constraint among their components «. p= O. (4.231) Expanding (4.225) yields
p=
J K
j Xl
YI
z,
L
M
N
=pj+QJ+RK
(4.232)
where P
YIN - ZIM
Q
ZIL - XIN
R
X IM - Y1L
(4.233)
and t herefore the orthogonality condition (4.231) ca n be expressed as
LP+MQ+NR = O.
(4.234)
The Plucker coordinates of t he line (4.223) have four ind ependent coordinates becau se of two const ra ints: (4.227) and (4.234). Our arrangement of Plucker coordinates in the form of (4.223) is the lin e arrangem ent and is called ray coordinates , however somet imes the reverse order in axis arrang em ent l = [p it] T is also used by some other te xtbooks. In either case , a vertical line [ il
Ip
] T or a semi-colon
[ it p ] T may be utilized to separate the first three element s from the last three. Both arrangements can be used in kinem at ics efficient ly. The Plucker line coordinates [it p] T ar e homogeneous becaus e Equa-
f,
tion (4.225) shows t hat the coordina t es [wfj, w p where w E JR, det ermines the same line. Force - mom ent, angular velocity - transl ational velocity, and rigid motion act like a line vector and can be described in Plucker coordinates.
•
*
Example 112 Pliick er coordinates oj a lin e connecti ng two points. Plii cker lin e coordinates oJ th e line connecti ng points PI (1, 0, 0) and P2 (O, 1, 1) are
[~ ] [-1
1
1 0
-1
1
f
176
4. Motion Kinematics
z
y
FIGURE 4.18. A unit cube.
because
and
*
Example 113 Pliicker coordinates of diagonals of a cube. Figure 4.18 depicts a unit cube and two lines on diagonals of two adjacent faces . Line h connecting corners P 1(1, 0,1) and P2(0, 1, 1) is
h = .il 2 because
and
Pi = Pi
A
X Ul
=
-i-}+K
J2
Line l2 conn ecting corn ers Ql (1,0,0) and Q2(1, 1, 1) is
.il 2
T ]
T ]
4. Motion Kinematics
because U2
,
q2 - q l = Iq2 - q ll =
P2
= ql
and
, X U2
177
J+K V2
- J+ K
V2
=
*
Ex ample 114 Grassmanian matrix to show Pluck er coordinates. It can be verified that the Grassmanian matrix fo r coordinates of two points WI [ W2
Zl ]
Xl YI X 2 Y2
(4.235)
Z2
is a short notation for the Plu cker coordinate s if we defin e
£=1:
~
p = l~ ~ I
M
=I
YI Y2
Q
=I
N
= I W2 WI
R
= I Xl X2
WI W2
Zl
Z2
I
*
z, Z2
XI X2
I
I
I·
Y Y2
(4.236)
Example 115 Ray-axis arrang em ent transformation. It can be verified that the ray arrang em ent of Pliicker coordinates, (4.237) can be transformed to the axis arrangem ent, (4.238) and vice versa utilizing the following 6 x 6 transformation ma trix:
[ ~] = ~[ ~ ] ~=
0 0 0 1 0 0
Th e tran sformation matrix tions:
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
(4.239)
0 0 0 0 1 0
0 0 0 0 0 1
~
is sym me tric and satisfies the following equa-
=[I~ ] 13 0
(4.240)
(4.241) (4.242)
178
4. Motion Kinematics
p
0 p=o I
I
I
(a)
u
(b)
(c)
FIG URE 4.19. Three cases of P lucker coordinates; (a) general case, (b) line through origin, (c) line at infinity.
*
E xa m ple 116 Classification of Pliicker coordinates. Th ere ar-e three cases of Plucker coordinat es as shown in Figur e 4.19. Th ey are: general case, line through origin, and lin e at infinity . illustrated in Figur-e 4.19 (a) , is when Th e general case of 1 = [u P both u and p are non- zero. Th e dir-ection vector u is parallel to the lin e, P is normal to the plane including the origin and the line, and Ipi gives the distance from the origin to the line. Line through origin l = [ fj, 0 illustrated in Figur-e 4.19 (b), is when the line passes through the origin and p is zero. Line at infinity 1 = [0 p illustrated in Figure 4.19(c), is when the distance of the line from origin tends to infinity. In this case we may assume u is zero. When the line is at infinity, it is bett er to r-edefin e the Pliicker coordinat es by nor-malizing the moment vector
f,
rr,
f,
l=
[ I~I
(4.243)
Th erefore, the direction components of the lin e tends to zero with the distanc e, while the moment compon ents r-emain finit e. By noting that the line at infinity can be assumed as the inter-section of the set of all plan es perpendicular to the moment vector, it is evident that the moment vector completely specifies the line. No line is defin ed by the zero Pliicker coordinates [0 0 0 0 0 0] T .
*
Example 1 17 Transformaiion of a lin e vector . Consider the line B 1 in Figure 4.20 that is defined in a local fram e B(oxyz) by (4.244)
4. Motion Kinemati cs
x
179
.... y FIGURE 4.20. A line vector in B and G frames.
where iL is a unit vector parallel to th e lin e l , and P is any point on the line . Th e Plucker coordinates of th e lin e in the global fram e G(OXY Z) is expressed by (4.245)
where GiL
and
Gp
=
(4.246)
G R B BiL
is the moment of GiL about 0 Gp
Gr p
x
GiL
+ GRBBrp) x GRBBiL Gsa X G RB BiL + G RB(B r p x Gsa X G R B B i/, + G R B B P Gsa G R B Bi/, + G R B B p . (Gsa
B i/,)
(4.247)
Th erefore, the 6 x 1 Piiicker coordinates [ i/, p] T for a Zin e vector can be transformed from a fram e B to another fram e G
Gz= [
~:
Gr B
BZ
] = Gr B [
(4.248)
~:
]
(4.249)
0]
(4.250)
by a 6 x 6 transformation matrix defin ed as
Gr » = [ GSoG GR BR
B
GR B
where (4.251)
180
4. Motion Kinematics
o FIGURE 4.21. Illustration of two skew lines.
(4.252) S2 r32 - S3 r22 -Slr32
+ S3r12
Slr22 - S 2 r12
S2 r33 - S3 r2 3 ] -Slr33 S3 r13 .
+
Slr23 - S2 r13
(4.253)
4.9
* The Geometry of Plane and Line
Plucker coordinates introduces a suitable method to define the moment between two lines, shortest distance between two lines, and the angle between two lines.
4.9.1
~ A1o~ent
Consider two arbitrary lines h = [Ul PI] T and l2 = [U2 P2] T as shown in Figure 4.21. Points PI and P2 on II and l2 are indicated by vectors rl and r2 respectively. Direction vectors of the lines are Ul and U2' The moment of the line l2 about PI is (r2 - rd x
and we can define the moment of the line l2 X
which, because of Ul . rl x
U2 l2
about h by
h = Ul . (r2 - rd x
U2
= U2
.
U2
(4.254)
Ul x rl , simplifies to
(4.255)
4. Motion Kinematics
181
The reciprocal product or virtual product of two lines described by Plu cker coord inates is defined as
(4.256)
The recipro cal product is commutative and gives th e moment between two directed lines.
4.9.2
*
Angle and Distance
If s is the shortest dist ance between two lines II =
[U2
P2
[UI PI] T
f ,and a E [0, Jr] is the angle between h and l2, then
and l2 =
(4.257)
and
s
(4.258)
Therefore, two lines hand l2 intersect if and only if th eir reciprocal product is zero. Two parallel lines may be assumed to intersect at infinity. The dist ance expression does not work for parallel lines.
4.9.3
*
Plane and Line
The equat ion of a plane nJ( is
G 1r
having a norm al unit vector
n=
nIl + n 2 J + (4.259)
where s is th e minimum dist ance of the plan e to the origin O. We may indicat e a plane using a homogeneous representation, (4.260)
and writ e the condition be in the plane by
1r
T .
r
= 0 for a point r =
[X
Y
Z
w ]T
to
(4.261)
182
4. Motion Kinemati cs
Moreover, the w = 0 indicat es all points at infinity, and s = 0 indi cat es all planes cont aining the origin. The intersect ion of rr-plan e with the X -axis, or the X -intercept, is X = -slnl , the Y-intercept is Y = - sln2' and t he Z-in ter cept is Z = -sln3' The plane is perp endicular to XY-plan e if n3 = O. It is perpendicular to t he X -axis if n2 = n3 = O. There are similar conditions for the other planes and axes. If (Xo, Yo , Zo) is a point in the plane (4.260), t hen (4.262) The distan ce of a point [ X
Y
Z
w ]T
from t he origin is (4.263)
while the dist ance of a plane rr
s=
= [ m l m 2 m3 s ] T from the origin is (4.264)
mi +m ~ +m~ '
The equation of a line connect ing two points PI(X I, YI , Zd an d P2(X2, Y2, Z2) at rl and r 2 can also be expressed by (4.265) and t he distan ce of a point P(X,Y, Z) from any point on l is given by S2
= (Xl + m(X2 - Xd - X )2
+ (YI + m(Y2 - YI ) _ y) 2 + (Zl + m(Z2 - Zd - Z )2
(4.266)
which is minimum for (X 2 - Xd(X I - X) + (Y2 - Yd(YI - Y) + (Z2 - Zd(ZI - Z) (X 2 - Xd 2 + (Y2 - Yd 2 + (Z2 - ZI)2 (4.267) To find the minimum dist ance of the origin we set X = Y = Z = O.
m=
Example 118
*
Angle and distance between two diagonal s of a cube. Th e Plucker coordinates of the two diagonals of the un it cube shown in Figure 4.18 are
.il 2
T ]
4. Motion Kinematics
183
Th e angle between II and 12 is Ct
. - 1 I' = sm U2
' I = sin . -1 2J3 = 60 deg
x
UI
and the distance between them is
Example 119 Distance of a point from a line. Th e equation of the lin e connecting two points and r 2 = [ 1 - 2
r l
= [ - 1
2
1 1 ]T
- 1 1] T is r I + m (r 2-rI)
1 =
[ T=~;;3.' ]. No w the distan ce between point
S2
=
(X l
r3
= [ 1
1 0
1 ] T and I is given by
+ m (X 2 - X d - X 3)2
+ (YI + m (Y2 - YI )
-
y3 ) 2
+ (ZI + m( Z2 - Zd - Z3)2 24m 2 - 20m + 6 which is mini mum f or 5
m = -. 12 Th us, the point on the lin e at a minim um distan ce f rom _ r -
-1/1/6 ] 3 [ 1{6 .
r3
is
184
4. Motion Kinematics
Example 120 Distance between two lines . Th e distance between the line conn ecting r2
= [1
-2
-1
rl
[-1
2 1 1] T and
1] T is
and the lin e connecting r 3 = [1 is
1 0 1] T and r 4 = [0
-1
2 1] T
Th e distance between two arbitrary points on the lin es is S2 = (-1
+ 2m - 1 + n 2) + (2 -
4m - 1 + 2n)2
+ (1 -
2m - 2n)2 .
Th e minimum distance is found by minimizing s2 with respect to m and n.
m = 0.443
n
= 0.321
Thus, the two points on the lin es at a minimum distan ce apart are at
-0.114 ] 0.228 [ 0.114 1
0.679 ]
0.358 r., =
*
[
0 .~42
.
Example 121 Intersection condition for two lin es. If two lines h = [Ul Pi] T and 12 = [U2 P2] T intersect, and the posit ion of their common point is at r , then
4. Mot ion Kinemat ics
185
and therefore
(r x ih) . il 2 = r - (ill x il2) (r x il 2) · ill = r - (il 2 x ild which implies
or equivalently
*
Example 122 Pliicker coordinates of the axis of rotation. Consider a homogenous transforma tion matrix corresponding to a rotation 0: about Z , along with a trans lation in XY -plane
oo XO Yo ] 1
(4.268)
0
o
1
which must be equal to
- sm 0:
o o XO Yo ]
coso:
o o
1
o
0 1
.
(4.269)
The angle of rotation can be obtained by comparison
(4.270) The pole of rotation can be f ound by searching for a point that has the same coordinates in both frames
[ r" r2l 0 0
0 r 22 0 0 1 0 0 r12
x, Yo ][ x; Yp ] o 1
0 1
[?]
(4.271)
that can be written as
[ r"r2l0 0
1
0 r12 r22 - 1 0 1 0 0 0
x; Yo ] [ X, Yp ]
o 1
0 1
[n
(4.272)
186
4. Motion Kinematics
Th e soluti ons of these equati ons are
(4.273)
(4.274)
Th e Plucker lin e coordinat es l = to
l = [0
4.10
°
[ iL
p] T of the pole axis is then equal
1 Yp
- X p Or .
(4.2 75)
* Screw and Plucker Coordinate
Consider a screw s(h, ¢, iL, s) whose line of act ion is given by P lucker coordin at es [u p ]T , and whose pit ch is p = ~ . The screw can be defined by a set of P lucker coordinates
s(h, ¢ , ii, s)
(4.276)
If t he pitch is infinite p = 00 , t hen th e screw reduces to a pure trans lat ion, or equivalently, a line at infinity
s(h, O,iL ,r)
= [ hOiL ] .
(4.277)
°
A zero pitch screw p = corresponds to a pure rot ation , th en the screw coordina tes are identical to the Plucker coordinates of th e screw line. s(O,¢,iL,s) = [ : : ] = [ : ]
(4.278)
4. Motion Kinematics
187
A central screw is defined by a line through origin.
s(h, ¢, u)
s(h,¢,u ,O)
p~ ] [ ~~ ] [
D(hu)
tu«,¢)
(4.279)
Screw coordinates for differential screw motion is useful in velocity analysis of robots. Consider a screw axis l, an angular velocity w = wu = ¢u about l , and a velocity v along l . If the location vector s is th e position of a point on l , th en th e Plucker coordinates of th e line l are
l=[ U]=[ u ] p s xu' Th e pitch of screw is
(4.280)
Ivl
(4.281)
p=~
and the direction of screw is defined by
,
w
(4.282)
u=~
so the inst ant aneous screw coordinates v(p, w, u, s) are
v(p, w, U, r )
=
r x w+ Ivl w] T
wu [
Iwl
[ sx
~lu+ v
[ s
1~+ pw ]
X
]
(4.283)
[ p :pw ] .
*
Example 123 Pitch of an ins tantane ous screw. Th e pitch of an instantaneous screw, defin ed by Pliicker coordinates, can be found by (4.284) u·t,
p=
because two Pliicker vect ors are orthogonal, ii - p
¢u ·¢t,
= 0,
and therefore,
¢u · (p + ¢pu) (¢u . p + ¢2 p)
¢2 p .
(4.285)
188
4. Motion Kinematics
*
E xample 124 Nearest point on a screw axis to the origin. The point on the instantaneous screw axis, nearest to the origin, is indicated by the following position vector:
(4.286)
4.11 Summary Arb itrary mot ion of a body B with respect to another body G is called rigid motion and can be expressed by
Gr p = GRB "
r»
+ Gd
(4.287)
where,
Gr p
[ x;
Br p
[ Xp
yp
Gd
[ x,
Yo
Yp
r r z; r· z;
Zp
(4.288) (4.289) (4.290)
T he vector Gd is t rans lat ion of B with respect to G, and GR B is t he rotation matrix to map B r to Gr when Gd = O. By int roducing the homogen eous coordinate representation for a point wx wy ] -
r -
-
r
wz w
-
r x[u: y/w ] z/w
(4.291)
1
we may combine the rotation and translation of a rigid motion in a 4 x 4 homogeneous transformation matrix and show the coordinate transformation by where ,
GT = [ GR B Gd ] B 0 1 .
(4.292)
Since the homogeneous transformation matrix GTB is not orthogonal, its inverse obeys a specific ru le
GT-1 _ Br. _ [ GR~ B G 0 to be consistent with
GTB- 1 GTB -- I 4·
(4.293)
(4.294)
4. Motion Kinematics
189
Th e rigid motion can also be expressed with screw coordinate s(h,¢, il , s) and screw transformation Gr
' (h
S
=
G,SB (h, 'f' J..' ) B , U, S r
J..' ) = [ G RB G s - G RB G s ''f'' U , s O l
(4.295)
+ hil
]
.
(4.296)
Th e screw s(h, ¢, il , s) is indicat ed by screw parameters; a unit vector on th e axis of rotation il , a location vector s, a twist angle ¢, and a translat ion h (or pitch p). The location vector s indicat es the global position of a point on t he screw axis. Wh en s = 0, th en, il passes through th e origin of the coordinate frame and the screw motion is called centra l screw s(h,¢, il ). Every screw can be decomposed into three prin cipal cent ra l screws about the three axes of the coordinate frame G. A rigid motion can be expressed more effectively by screw and Plu cker coordin at es of direct ed lines. ' (h , 'f' J..' ) , U,S =
S
[it] ~
=
[ p +it pil ] = [ ¢p ¢il + hii p=r xit
]
(4.297) (4.298)
4. Motion Kinemat ics
191
Exercises 1. Notation and symbols. Describe the meaning of a- 2d 1
b- cTS1
c- BTc
d- cD B
f- D u,h
g- sv- l
h- cr B
i- [
~]
1- csT;
m- sv-l
n- [
~]
k- p =
g
0-
s(h, ¢, fL, s).
2. Global position in a rigid body motion.
We move the body coor dinate frame B to
Cd = [ 4 - 3 7
f.
Find c r p if the local position of a point is
an d t he orientation of B with respect to the global frame G can be found by a rotation 45 deg about t he X -axis, and then 45 deg about t he Y-axis. 3. Rotation matrix com patibility. It is not possible to find
from t he equation of rigid body motion
C RB
crp
=
cRB Brp
+ cd
if c r p, Br p , and c d are given . Explain why and find the required conditions to be ab le to find C R B . 4. Global position wit h local rotation in a rigid body motion. Assume a body coordinat e frame B is at
Cd = [4
-3 7
f.
Find c r p if the local position of a point is Brp
=
[ 7
3
2
f
and the orientation of B with resp ect to t he global frame G can be foun d by a rotation 45 deg about the x-axis, and then 45 deg about the y-axis.
192
4. Motion Kinematics
5. Global and local rotation in a rigid body motion . A body coordinate frame B is translated to Gd = [4
Find
Grp
-3
7
f.
if the local position of a point is Br p =
[7 3 2
f
and the orientation of B with respect to the global frame G can be found by a rotation 45 deg about the X -axis, then 45 deg about the y-axis, and finally 45 deg about the z-axis. 6. Combination of rigid motions . The frame B 1 is rotated 35 deg about the zl-axis and translated to B2 d
= [-40 30 20
f
with respect to another frame B 2 • The orientation of the frame B 2 in the global frame G can be found by a rotation of 55 deg about u =
[2 -3 4].
7. Rotation submatrix in a homogeneous transformation matrix. Find the missing elements in this homogeneous transformation matrix:
~? 0~ ~] 1
001
8. Angle and axis of rotation. Find the angle and axis of rotation for [T] and T- 1 .
[T]
0.866 -0.5 0 0.~66
=
[
05
o
0
0 4]
~ ~
0
1
9. Combination of homogeneous transformations. Assume that the origin of the frames B 1 , and B 2 are at 2d 1
-10
20
Gd 2
-15
-20
f 30 f .
-20
4. Motion Kinematics
193
The orientation of B 1 in B 2 can be found by a rotation of 60 deg about 2U
= [1 -2 4]
and the orientation of B 2 in the global frame G can be found by a rotation of 30 deg about Gu =
[4 3 -4] .
Calculate the transformation matrix GT2 and GT2-
1
10. Rotation about an axis not going through origin . Find th e global position of a body point at
[ 7 3 2
Br p =
r
after a rotation of 30 deg about an axis parallel to Gu
= [4 3 -4]
and passing through a point at (3,3,3) . 11. Inversion of a square matrix. Knowing that the inverse of a 2 x 2 matrix
[AJ
=
[~ ~]
(4.299)
is
_adb;t be ] ,
(4.300)
-ad+ be use the inverse method of splitting a matrix [T] into
[T] =
[~ ~]
and applying the inverse technique (4.61) , verify Equation (4.300), and calculate the inverse of
194
4. Motion Kinematics
FIGURE 4.22. A cube, before and after a rigid motion. 12. Combination of rotations about non-central axes. Consider a rotation 30 deg about an axis at the global point (3,0,0) followed by another rotation 30 deg about an axis at the global point (0,3,0) . Find the final global coordinates of B
rp = [ 1
0
f
to 13. Transformation matrix from body points.
Figure 4.22 (a) shows a cub e at initial configurations. Label the corners of the cube, at the final configuration shown in Figure 4.22 (b), and find the associated homogeneous transformation matrix. The length of each side of the cube is 2. 14.
*
Central screw.
Find the central screw that moves the point
to
°rp = [0 15.
1 4
f.
* Screw motion. Find the global position of B rp =
[1
0 0
1
f
4. Motion Kinematics
after a screw motion
16.
G SB (h ,
s)
195
= G SB (4,60 deg, ii , s) where
Gs
3 0 0
Gu
1
1 1
f
f.
* Pole of a planar motion. Find t he pole position of a plan ar morion if we have the coordinates of two body points, before and after the motion, as given below.
P2(5,2 ,1) Q2(7,2 + y'5, 1)
Pdl, 1, 1) QI (4,1 ,1)
17.
* Screw paramet ers Find the global coord inates of t he bod y points P I (5,0,0) QI (5,5 ,0)
R I (0,5 ,0) after a rot ation of 30 deg about the x-axis followed by a rotation of 40 deg about an axis parall el to the vector =
Gu
[ 4 3 - 4]
and passing through a global point at (0, 3,0) . Use t he coordinates of t he points and calculate the screw parameters t hat are equivalent to the two rot ations. 18.
*
Non-cent ral rotation. Find th e global coordinates of a point at Brp
= [ 10 20 -10
f
when the body frame rot ates about Gu
= [1 1 1
f
which passes through a point at (1- , - 4, 2). 19.
* Equivalent screw.
Calculate the transformation matrix GT B for a rotation of 30 deg about th e x-axis followed by a translation parallel to Gs
= [ 3 2 -1
I'
and then a rot ation of 40 deg about an axis parallel to th e vector GU
= [ 2 - 1 I f.
Find the screw par am eters of GT B .
196
20.
4. Motion Kinematics
* Central screw decomposition. Find a triple cent ral screws for th e case 1 in App endix C G sB(h,
iL, s)
= s(hx , 'Y, i)S(h y , (3, J)S(hz , o , K)
to get th e same screw as G sB (h,
ii , s)
= G sB (4, 60, ii, s)
where
21.
Gs
[3
0 0
Gu
[1
1
1
r r.
* Central screw composition. What is th e final position of a point at Br p
= [10 0 0
r
after the central screw s(4, 30 deg, J) followed by s(2, 30 deg,i) and s( -6,120 deg, K) ? 22.
* Screw composit ion. Find the final posit ion of a point at Br p
= [ 10 0 0
r
afte r a screw
followed by
23.
* Plucker line coordin at e. Find th e missing numb ers l = [1 /3
1/ 5 ? ? 2 -1
r.
4. Motion Kinematics
z
197
z
x
y
D
B A FIGURE 4.23. A pyramid. 24.
* Plucker lines. Find the Plucker lines for AE, BE, GE, DE in the local coordinate B , and calculate the angle between AE, and the Z-axis in the pyr amid shown in Figure 4.23. The local coordinate of t he edges are A(l , 1, 0) B(-l ,l ,O) G(-l ,-l ,O) D( l , -1 , 0) E(O , 0, 3). Tr ansforms the Plucker lines AE, B E , GE, DE to t he global coordin ate G. The global position of 0 is at 0(1,10 ,2 ).
25.
* Angle between two lines. Find t he angle between 0 E , 0 D , of t he pyr amid shown in Figure 4.23. Th e coordinates of the point s are D(l , -1 , 0) E(O, 0, 3).
26.
* Dist ance from the origin. The equat ion of a plane is given as
4X - 5Y - 12Z - 1 = O. Det ermine th e perp endicular distan ce of the plan e from t he origin .
5 Forward Kinematics 5.1 Denavit-Hartenberg Notation A series robot with n joints will have n + 1 links. Numbering of links starts from (0) for the immobile grounded base link and increases sequentially up to (n) for the end-effector link. Numbering of joints starts from 1, for the joint connecting the first movable link to the base link, and increases sequentially up to n . Therefore, the link (i) is connected to its lower link (i - 1) at its proximal end by joint i and is connected to its upper link (i + 1) at its distal end by joint i + 1, as shown in Figure 5.1.
... ~ Joint
ax~<";'<'0
FIGURE 5.1. Link (i) and its beginning joint i-I and its end joint i.
Figure 5.2 shows links (i - 1), (i) , and (i + 1) of a serial robot , along with joints i - I , i , and i + 1. Every joint is indicated by its axis, which may be translational or rotational. To relate the kinematic information of robot components, we rigidly attach a local coordinate frame B , to each link (i) at joint i + 1 based on the following standard method, known as Denavit-Hartenberg (DR) method. 1. The zi-axis is aligned with the i
+ 1 joint axis.
All joints, without exception, are represented by a z-axis . We always start with identifying the zi-axes. The positive direction of the zi-axis is arbitrary. Identifying the joint axes for revolute joints is obvious , however, a prismatic joint may pick any axis parallel to the direction of translation. By assigning the Zi-axes, the pairs of links on either
200
5. Forward Kinematics side of each joint , and also two joints on either side of each link are also ident ified.
2. The xi-axis is defined along th e common normal between the Zi- I and Zi axes, pointing from th e Zi- I to th e zi-axis. In general, th e z-axes may be skew lines, however th ere is always one line mutu ally perpendicular to any two skew lines, called th e common normal. Th e common normal has th e shortest dist ance between th e two skew lines. When th e two z-axes are parallel, th ere are an infinite numb er of common normals. In th at case, we pick th e common normal th at is collinear with th e common normal of the previous joints. Wh en the two z-axes are intersecting , th ere is no common normal between th em. In th at case, we assign th e xi-axis perpendicular to the plane formed by the two z-axes in the direction of Zi- I x z.. In case th e two z-axes are collinear, the only nontrivi al arrangement of joints is either PIIR or RIIP. Hence, we assign the xi-axis such th at we have th e joint variable equal to ()i = 0 in the rest position of the robot. 3. Th e Yi-axis is determined by th e right-hand rule, Yi = z;
X
Xi.
Generally speaking, we assign reference frames to each link so th at one of the three coordinate axes Xi, Yi , or Zi (usually Xi) is aligned along the axis of th e dist al joint. By applying th e DH method, the origin o, of the frame B, (Oi' Xi,Yi , Zi ), at tached to the link (i), is placed at the intersection of th e i + l-th joint axis with th e common normal between the Zi- I and z, axes. A DH coordinate frame is ident ified by four parameters: ai , ai , ()i, and
a;
1. Link length ai is the dist ance between Zi-I and Zi axes along th e
Xi-axis. ai is th e kinematic length of link (i) . 2. Link twist ai is the required rot ation of the Zi_I-axis about th e Xi-
axis to become parallel to th e zi-axis.
3. Joint distance d, is th e distance between Xi- I and Xi axes along the Zi _I-axis. Joint dist ance is also called link offset. 4. Joint angle ()i is the required rot ation of Xi _I-axis about the Zi_I-axis
to become par allel to the xi-axis. DH frame param eters of th e links in Figure 5.2 are illustrated in Figure 5.3. Th e parameters ()i and d, are called joint parameters, since they define
5. Forward Kin ematics
201
FIG URE 5.2. Links (i - 1), (i) , and (i + 1) along wit h coordinate frames B ; an d B i+l .
__------------------j
~,:
....+"
: : :
'-
~
.~
•
--4.o1l.-----I...__J:L_ ..
Zi
"-J
X i_I
FIGURE 5.3. DH parameters ai , Q i, d. , 8 i defined for joint i and link (i) .
202
5. Forward Kinematics
the relative position of two adj acent links connected at joint i. In a given design for a robot , each joint is revolut e or prismatic. Thus, for each joint , it will always be the case that either Oi or d i is fixed and th e other is variable. For a revolute joint (R) at joint i , the value of d; is fixed, while Oi is the unique joint variable. For a prismatic joint (P), the value of 0i is fixed and d i is the only joint variable . The variab le parameter either Oi or d; is called the joint variable. The joint parameters Oi and d; define a screw motion becaus e Oi is a rotation about th e Zi- I-axis, and d; is a translation along the Zi_ I-axis. The parameters (Xi and ai are called link parameters , because t hey define relative positions of joints i and i + 1 at two ends of link (i) . Th e link twist (Xi is the angle of rot ation Zi-I -axis about Xi to become parallel with the zi-axis. The other link parameter, tu , is the translation along the Xiaxis to bring th e Zi_I-axis on the zi-axis. The link parameters (Xi and a ; define a screw motion because (Xi is a rotation about the xi-axis, and a, is a translation along the xi-axis. In other words, we can move th e Zi_I-axis to the zi-axis by a central screw s(ai , (Xi , i) , and move th e xi- I-axis to th e xi-axis by a central screw s(di , Oi, ki-d ·
Example 12 5 Simplification com m ents for the DH method. There are some com ments to simplify the application of th e DH fram e m ethod. 1. Showing on ly z and x axes is suffi cient to id entify a coordinate fram e. Drawing is made clearer by not showing y axes. 2. Certain paramet ers of th e fram es attached to th e first and last links n eed not be defin ed. If the first and last jo ints are R , th en
o o
(5.1) (5.2)
In th ese cases, th e zero position for Ol, and On can be chosen arbitrarily, and link offsets can be set to zero.
(5.3) If th e first and last joints are P , th en
(5.4) and the zero position for d l, and d n can be chos en arbitrarily, but generally to make as many parameters as possible to zero. If the final joint n is R , we choos e Xn to align with Xn-l when On = 0 and the origin of B n is chosen such that d n = O. If th e final joint n is P , we choos e Xn such that On = 0 and the origin of B n is chosen at th e intersection of Xn- l and joint axis n th at d n = O.
5. Forward Kinemat ics
203
3. Eac h link, except the base and the last , is a binary link and is connect ed to two oth er links.
4.
Th e param eters a; and O:i are determined by the geom etric design of the robot and are always constant. T he distance di is the offs et of the fram e B , with respect to B i - 1 along th e Zi- l -cxis. Sin ce a; is a lengt h, ai 2': o.
5. The angles O:i and (}i are directional. Positi ve direction is determined by the right-hand rule and the direction of Xi and Zi- l respectively. 6. For industri al robots, the link twist angle, 7r /2 radian s.
O:i ,
is usually a multiple of
7. Th e DH coordin ate fram es are not unique because the direction of Zi-oxes are arbitrary. 8. Th e base fram e Bo(xo ,Yo, zo) = G(X , Y, Z) is the global fram e fo r an immobile robot. It is convenient to choose th e Z-axis along the axis of joint 1, and set the origin 0 where the axes of the G fram e are colin ear or parallel with the axes of the B 1 fram e at rest posit ion . 9. T he configuration of a robot at which all the joint variables are zero is called the home configuration or rest position, which is the refe rence for all motions of a robot. T he best rest posit ion is where it makes as many axes parallel to each oth er and coplana r as possible. 10. For conve nience , we can relax the strict DH definition for the direction of Xi, so that it points from Zi to Zi- l and still obtain s a valid DH param eterizati on . Th e direction flip of Xi is to set a more convenient reference fram e when mo st of th e jo int parameters are zero. 11. A DH param eter table helps to establish a systematic link f rame . As shown in Table 5.1, a DH table has five colum ns fo r fram e ind ex and four DH param eters . The rows of the four DH parameters for each fram e will be fill ed by constan t param eters and th e joint variable. The j oint variable can be f ound by consideri ng what fram es and links will m ove with each varying active jo int.
Table 5.1 - DH param eter table fo r establishme n t of link fram es. O:i d; (}i Fram e N o. a; 1 (}1 d1 al 0: 1 2 d2 (}2 a2 0:2 ..
. . .. j
.. . . . . n
..
.. ..
aj
.. .. ..
..
O:j
..
. . .. .. ..
an
O:n
..
dj
...
.....
dn
..
(}j (}n
..
204
5. Forward Kinematics
FIGURE 5.4. Illustration of a 3R planar manipulator robot and DR frames of each link.
Example 126 DH table and coordinate frames for 3R planar manipulator. R stands for revolute, hence , an RIIRIIR manipulator is a planar robot with three parallel revolute joints. Figure 5.4 illustrates a 3R planar ma nipulator robot. The DH table can be filled as shown in Table 5.2, and the link coordinate frames can be set up as shown in the Figure.
Table 5.2 - DH table for the 3R planar manipulator robot of Figure 5.4. Frame No . a; C¥i d; (}i 1 i. 0 0 (}l 2 0 (}2 l2 0 3 b 0 0 (}3
Example 127 Coordinate frames for a 3R PUMA robot. A P UM A manipulator shown in Figure 5.5 has Rf-RIIR main revolute joints, ignoring the structure of the end-effector of the robot. Coordinate frames attached to the links of the robot are indicated in the Figure and tabulated in Table 5.3.
5. Forward Kinematics
205
F IGURE 5.5. 3R PUMA manip ulator and links coordinate frame.
Table 5.3 - DH table for the 3R P UMA manipulator shown in Figure 5.5. D:i d; e, Frame No. a; 1 0 90deg 0 8 1 2 12 0 h 82 3 0 0 0 83
The joint axes of an K 1RI IR manipulator are called waist zo, shoulder and elbow Z2. Typica lly, the joint axes Zl and Z2 are parallel, and perpendicular to Zo .
Zl,
Example 128 Stanford arm . A schematic illustration of the Stanford arm, which is a spherical robot R'r R'r P attached to a spherical wrist R'r R'r R , is shown in Figure 5.6 and tabulated in Table 5.3. The DH parameters of the Stanford arm are tabulated in Table 5.4. This robot has 6 DOF: 8 1 , 82 , d3 , 84 , 85 , 86 .
206
5. Forward Kinematics
FIGURE 5.6. Stanford arm RI-RI-P:RI-RI-R.
Table
5.4 - DH ta ble for Stanford arm shown in Figure 5.6. Frame No. 1 2
3 4 5 6
a; 0 0 0 0 0 0
(Xi
- 90deg 90 deg 0 -90deg 90deg 0
d, II l2 d3 0 0
16
8i
81 82 0
84 85 86
Example 129 Special coordinate frames . In a robotic manipulator , some frames have special names. The bas e fram e Eo or G is the grounded link on which the robot is installed. It is in the base frame that every kinematic information must be calculated because the departure point, path of motion, and the arrival point of the end effector are defined in this frame. The base frame is illus trated in Figure 5.7. The station frame 5, also called world fra m e or u n iverse fram e, is the frame in which the kinematics of the robot are calculated . Station frame is important when severa l robots are installed in a workshop , or the robot
5. Forward Kinematics
207
•Yo
FIGURE 5.7. Base and tool fram es.
is mobile. Th e wrist frames Wi are installed on the wrist point where the hand is attached to the last arm of a robot. Th e hands of industrial robots are usually atta ched to a wrist m echan ism with three orthogonal revolut e axes. Th e tool frame T is atta ched to the end of any tool th e robot is holding . It may also be called the end-effector frame, or final frame . Wh en the hand is empty, the tool fram e is chosen with its origin betw een the fing ert ips of th e robot. Th e tool fram e is illustrated in Figure 5.7. Th e goal frame F is the location where th e robot is to move the tool. Th e goal fram e is specified relative to the station fram e.
Example 130 Transfo rming th e B i - I to Bi , T wo neighbor coordinate fram es can be brought into coinci dence by several sequences of tran slation s and rotations. Th e following prescribed set of two rotations and two translations is a straightforward m ethod to move th e fram e B i- I to coin cide with the frame Bi, 1. Translat e fram e B i - I along the
Zi - I
-axis by distance d i.
2. Rotate fram e B i - I through Bi around the Zi-I -axis . 3. Translat e fram e B i - I along the Xi-axis by distance ai .
4.
Rotate fram e B i- I through
Cl:i
about th e Xi-axis.
How ever, to make a tran sformation matrix we should start from two coinci den t coordinate fram es B , and B i- I at the current configurat ion of B i- I , then move B , to come to its present position. Th erefore, we must follow th e following sequence of motions: 1. Rotate fram e B , through
O!i
around the Xi-I -cxis.
2. Translat e fram e B, along the Xi- I -axis by distance ai .
208
5. Forward Kinematics
3. Rotate frame B, through Bi about the Zi-l-axis.
4.
Translate frame B, along the Zi- l-axis by distance d i .
Note that during these maneuvers, B i - 1 acts as the global coordinate frame for the local coordinate frame Bi, and thes e motions are about and along the global axes.
E x ample 131 Shortcomings of the Denavit-Hartenberg method. The Denavit -Harten berg method for describing link coordinate frames is neither unique nor the best method. Th e drawbacks of the DH method are 1. The successive coordinate axes are defined in such a way that the origin 0i and axis Xi are defined on the common perpendicular to adjacent link axes. This may be a difficult task depending on the geometry of the links, and may produce singularity.
2. The DH notation cannot be extended to ternary and compound links .
5.2 Transformation Between Two Adjacent Coordinat e Frames T he coordinate frame B, is fixed to t he link (i) and the coordi nate frame B i - 1 is fixed to the link (i - 1). Based on the Denav it-Hartenberg conventio n, t he transformation matrix i-1Ti to transform coordinate frames B, to B i - 1 is represented as a product of four basic transformations using t he parameters of link (i) and joint i . i-1Ti
(5.5)
D Zi _ 1,d i RZi _1,(J i Dx i _1 ,a i R Xi _1 ,O: i
[ =8, sin Bi 0 0
a, cosBi - sin Bi cos Cl:i sin Bi sin Cl:i cos Bi cos Cl:i - cos Bi sin Cl:i a; sinB i di sin Cl:i cos Cl:i 1 0 0
where R x i _1 ,a i
==
[~
D x i_1 ,a i
==
0 cos Cl:i sin Cl:i 0
0 - sin Cl:i cos Cl:i 0
[~
0 ai 0 0 1 0 0 1
0 1 0 0
]
n
] (5.6)
(5.7)
209
5. Forward Kinematics
r R Zi_1 ,Bi ==
D Zi _ 1,d i
coo 0; sin Bi 0 0
- sinB i cos Bi 0 0
0 0 1 0
H~ ~; ] .
r
=
o
0 0
~]
(5.8)
(5.9)
1
Therefore the transformation equat ion from coordina te fram e B, (Xi , Yi, z. ), t o its previous coordinate fram e B i - 1 (Xi- l , Yi-l , Zi- l ), is
(5.10)
where, sin Bi sin a i - cos Bi sin a i cos o ,
- sin Bi cos a i cos Bi cos o , sin ai
o
ai cos Bi ] ai sin Bi di
o
.
(5.11)
1
This 4 x 4 matrix may be par ti tioned int o two submatrices, which represent a unique rot ation combined wit h a unique t ranslation to produce t he same rigid mot ion required to move from B, t o B i - 1 ,
R-1 0
I. - I T, . _
[ i- I
1 -
i- l d 1. ]
(5.12)
1
wher e i- I
COS Bi .
u, =
[
sl~Bi
- sin Bi cos ai cos Bi cos o , sin a i
sin Bi sin ai ] - cos Bi sin a i cos o,
(5.13)
and i-
1
di =
ai cos Bi ] aisinBi . [ di
(5.14)
The inverse of the homogeneous tran sformation matrix (5.11) is i- 1
r.- 1
(5.15)
1
COs Bi - sin Bi cos ai [ sin Bi sinai
o
sin Bi cos Bi cos o , - cos Bi sin a i
o
o sin a i cos o ,
o
- ai ] -di sin a i - d i cos a i . 1
210
5. Forward Kinemat ics
p
FIGURE 5.8. Two coordinate frames based on Denevit-H art enberg rules.
Proof. 1 Assum e that the coordinate frames B 2(X2,Y2, Z2) and Bl( Xl ,Yl , Zt} in Figur e 5.8 are set up based on Denavit-Hartenberg rules. The position vector of point P can be found in frame B , (Xl, Yl , Zl) using 2 r p and 1 8 2 lr p = lR 2 2 r p + 1 8 2 (5.16) which, in homogeneous coordinate representation , is equal to
cos(k2 , it} cos(k2 , j t} cos(1" 2 , 1,, l )
. (5.17)
o
Using the parameters introduced in Figure 5.8, Equ ation (5.17) becomes
- Se2ca2 Ce 2 ca 2 sa2
o
Se 2sa2 -Ce 2sa2 ca2
(5.18)
o
If we subst it ut e the coordinat e frame B , by Bi-l(Xi-l ,Yi-l , Zi-t} , and B 2 by Bi( Xi , Yi, Zi) , then we may rewrite the above equation in the requir ed form i Xi_ ce Yi-lI ] _ [ sei 0 Zi-l [ 1 0
-Seica i Ceicai so;
o
Seisai - Ceisa i cai
o
aiCei aiSei di 1
(5.19)
5. Forward Kinematics
211
Following t he inversion rule of homogeneous transformation matrix i- 1T i
[
G~B ~S
i-ly- l
[
G~~
z
(5.20)
]
_ G~~ G s ]
(5.21)
we also find the required inverse transformation (5.15).
•
[~ ] [
COi -SOicai SOisai 0
SOi cOicai - cOisa i 0
0 sai cai 0
- ai -di sa i -dicai 1
][ x,_, ] Y , -1 Zi-l 1
(5.22)
Proof. 2 An alternat ive met hod to find iT i _ 1 is to follow t he sequence of t ranslations and rotations that brings the frame B i - 1 to t he present configuration starting from a coincident position wit h the frame Bi. Note t hat working with two frames can also be equivalent ly described by B == B, and G == B i - 1 , so all the following rotations and translations are abo ut and along t he local coordinate frame axes. Inspecting Figure 5.8 shows that: 1. Frame B i -
1
is translated along the local zi-axis by distance - di ,
2. The displaced frame B i - 1 is rotated through - Oi abo ut t he local zi-axis, 3. The displaced fram e B i distance - ai , and
1
is translat ed along t he local xi-axis by
4. The displaced frame B i - 1 is rotated through - a i abo ut the local xi-axis . Following t hese disp lacement sequences, t he transformation matrix iT i _ 1 would be iTi_ 1
(5.23)
R Xi ,-o. i D x i ,-a i RZi ,-O i DZi, -d i
[
cos Oi - sin Bi cos ai sin Bi sin a i 0
where D",,-d,
sin Oi cos Oi cos ai - cos Oi sin a i 0
~ ~ [
0 1 0 0
0 0 1 0
0 0
- s; 1
0 sinai cos a i 0
]
- ai - d i sinai - d i cos a i 1
] (5.24)
212
5. Forward Kinematics
FIGURE 5.9. Illustration of a 2R planar manipulato r robot and DR fram es of each link.
RZ i , _() i
=
~~~~;;
l
D x i ,-ai
=
- sin ei cose i
0 0
0
10
o 1
cos o, sinai
o
l
j
(5 .25)
0 1
[ ~ 1 o~ -;i] o 0 o
i - 1T i
0 0
(5.26)
0 1
o - sinai cos o,
(5 .27)
o
iT- 1
(5 .28)
i-1
cosO, sin e', 0 0
- sin ei cos ai cose i cos ai sinai
sine i sinai - cos e i sin a i cos o,
0
0
cose i ai sine i
a;
di
1
j
• Ex ample 132 DH transformation matrices for a 2R planar manipulator. Figure 5.9 illustrates an RIIR planar' manipulator and its DH link coordinate frames .
5. Forward Kinematics
213
Table 5.5 - DH table for 2R planar manipulator shown in Figure 5.9. Frame No . ai Cti d i Oi
RRfmf
1
l2
2
0
O2
0
B ased on the DH Table 5.5 we can find the transformation matrices from frame B, to frame B i - l by direct substitution of DH parameters in Equation 5.11. Therefore,
lT2
r =
2
008 O sin O
2
0 0
r »r, ~ Sirl 1
0080
- sin02 cos O2 0 0
0 0 1 0
12 cosO,
- sinOl COS OI 0 0
0 0 1 0
llCOSOl t, sinOI 0 1
h sin02
j (5.29)
0 1
j
(5.30)
and consequently
- S (01+ 02) c (0 1; O2)
0
hCOl +l2C(01 +02) + l2~ (0 1 + fh)
~ hSOI
j
. (5.31)
001
Example 133 Link with RIIR or RII P joints. When the proximal joint of link (i) is revolute and the distal joint is eit her revolute or prismatic, and the joint axes at two ends are paralle l as shown in Figure 5.10, then Cti = 0 deg (or Cti = 180 deg), a; is the distance between the joint axes , and Oi is the only variable parameter. The joint distance d; = cte is the distance between the origin of B, and B i - 1 along z; however we usually set XiYi and Xi-lYi-l coplanar to have di = O. The Xi and Xi- l are parallel for an RIIR link at rest position. Therefore, the transformation matrix i- 1Ti for a link with Cti = 0 and RIIR or RIIP joints, known as RIIR(O) or RIIP(O) , is
i-IT
=
,
while for a link with Cti
r
COSOi sinOi
- sinOi COS Oi
0 0
ai COS Oi a; sinOi
0
0 0
1 0
di 1
o
180 deg and
RIIR
or
RIIP
j
(5.32)
joints, known as
214
5. Forward Kinemati cs
~J
FIGURE 5.10. A RIIR(O) link . R IIR(180) or R IIP(180), is
;-' T;
~
'ir:-r;~1
COS
[
o
sin ei
0
(5.33)
Example 134 Link with K1R or KiP jo ints. Wh en the proximal j oin t of link (i) is revolute and th e dist al j oin t is either revolute or prismatic, and the joint axes at two ends are perpendicular as shown in Figure 5.11, then (}:i = 90 deg (or (}:i = -90 deg), a; is th e distance between th e j oin t axes on Xi, and ei is the only variable param eter. Th e j oint dista n ce d i = cte is the distance between the origin of B , and B i - I along Zi. However we usually set XiYi and Xi- IYi- I coplanar to have d; = O. T he R.lR link is made by twist ing th e RIIR link about its center line Xi-I -axis by 90 deg. Th e Xi and Xi- I are parallel for an R.lR link at rest position . Th erefore, the transformation matrix i- IT i f or a link with (}:i = 90 deg and R.lR or R.lP j oin ts, kno wn as R.lR(90) or R.lP(90) , is
e. sin~e:
COS
i- IT = i [
0 0
sin ei
e
- cos i
e e
cos i a; sin i
a;
1 0 di 001
]
(5.34)
5. Forward Kinematics
215
FIG URE 5.11. A R .1R(90) link.
while fo r a link with (Xi = -90 deg and R-lR or R-lP jo ints, known as R-lR( - 90) or R-lP( -90) , is
(5.35)
Ex ample 135 Link with R~ R or R~ P j oints. When the proximal joint of link (i) is revolute and the distal joint is either revolute or pri smatic, and the joint axes at two ends are intersecting orthogonal , as shown in Figure 5.12, then (Xi = 90deg (or (Xi = -90deg), a; = 0, d i = ete is the distance between the coordinat es origin on z. , and Oi is the only variable param eter. N ote that it is possible to have or assume d; = O. The Xi and Xi - I of an R~ R link at rest position are coincident (when d, = 0) or parallel (when d; =J 0). Th erefore, the transformation m atrix i-I T i for a link with (Xi = 90 deg and R~ R or R~ P joints, kno wn as R~ R( 90) or R~ P( 90) , is [ i- IT
i
=
COO0;
sinOi 0 0
0 0
sinOi - COS Oi
1
0 0
0
(5.36)
216
5. Forward Kinematics
FIGURE 5.12. An Rf-R(90) link.
while for a link with O:i = -90 deg and Rf- R or Rf- P joints, known as Rf- R( -90) or Rf- P(-90), is
e
- sin i cose i
(5.37)
o o
Example 136 Link with PII R or PII P joints. When the proximal joint of link (i) is prismatic and its distal joint is either revolute or prismatic, and the joint axes at two ends are parallel as shown in Figure 5.13, then O:i = Odeg (or O:i = 180deg), i = 0, a; is the distance between the joint axes on Xi, and d; is the only variable parameter. Note that it is possible to have a; = O. Therefore, the transformation matrix i-ITi for a link with O:i = 0 and PIIR or PIIP joints, known as PIIR(O) or PIIP(O), is
e
1 0 0 ai 0 1 0 0 to o 1 di [ o 0 0 1
i-IT-
while for a link with
O:i
=
]
(5.38)
180deg and PIIR or PIIP joints, known as
5. Forwar d Kinematics
217
FIGURE 5.13. A P IIR(O) link.
PII R(180) or PIIP(180) , is
i-lT,
~ l~ ~1 ~o ~ ] - 1 d;
.
(5.39)
1
The origin of the B i- 1 fram e can be chosen at any point on the Zi-l -axis or parallel to Zi-l -axis arbitrarily. One simple setup is to locate the origin o; of a prismatic joint at the previous origin Oi-l' This sets a, = 0 and furthermore, sets the initial value of the joint variable d; = 0, which will vary when o, slides up and down parallel to the Zi-l-axis.
Example 137 Link with P.1R or P.1P joints. When the proximal joint of link (i) is prismatic and its distal joint is either revolute or prismatic, with orthogona l axes as shown in Figure 5.14, then (Xi = 90 deg (or (Xi = - 90 deg), Bi = 0, ai is the distance between the joint axes on Xi, and di is the only variable parameter. Therefore, the transformation matrix -
'-'T,
l
~ ~ ~ ~1 ~ ]
(540)
218
5. Forward Kinematics
FIGURE 5.14. A P .lR(90) link.
while for a link with D:i = -90 deg and P.lR or Ps. P joints, kno wn as Pl.R( -90) or Pl.P( -90) , is
[
~ ~ ~ ~,]
o o
-1 0
0 0
d, 1
(5.41)
'
Example 138 Link with Pr R or Pr P jo ints. When the proxim al jo int of link (i ) is prismatic and the distal jo int is either revolut e or prismatic, and the joint axes at two ends are intersecting orthogonal as shown in Figure 5.15, then D:i = 90 deg (or D:i = - 90 deg), Bi = 0, a; = 0, and d; is the only variable param eter. Not e that Xi must be perpend icular to the plane of Zi - l and Zi, and it is possible to hav e a; # O. Th erefore, the tmnsformation ma trix i - 1T i for a link with and PrR or PrP joints, kno wn as PrR(90) or PrP(90) , is
i- l
. _
T, -
while for a link with
D:i
[
1 0 -10 0] 0
0 0 0 1 o 0
0 0
d, 1
D:i =
90 deg
(5.42)
-90 deg and Pr R or Pr P jo ints, known as
5. Forward Kinematics
219
FIGURE 5.15. A P f-R(90) link.
P1-R(-90) or P1-P( -90) , is
(5.43)
E x ample 13 9 Classification of industrial robot links . A robot link is id entified by its joints at both ends, which determin es the tran sformation matrix to go from th e distal jo int coordinate fram e B, to the proximal joint coordinate fram e B i - 1 . Th ere are 12 types of links to make an industrial robot. Th e transformation matrix fo r each type depends solely on the proximal joint, and angle between the z -axes . Th e 12 types of
220
5. Forward Kinematics
p
FIGURE 5.16. Alte rnative method to derive the Denavit-Hartenberg coordinate transformation.
transformation matrices are:
RII R(O) RIIR(180) K lR(90) Rl-R( -90) Rf- R(90) Rf-R(-90) PIIR(O) PIIR(180) Pl- R(90) Pl- R(- 90) Pf- R(90) Pf-R(- 90)
1 2
3 4
5 6 7
8 9 10 11 12
or or or or or or or or or or or or
RIIP(O) RIIP(180) K lP(90) Rl-P( -90) Rf-P(90) Rf-P(- 90) PIIP(O) PIIP(180) Pl- P(90) Pl- P(-90) Pf- P(90) Pf-P(- 90)
Example 140 DH coordinate transformation based on vector addition. The DH transformation from a coordinat e fram e to the other can also be described by a vector addition. The coordinates of a point P in frame B l , as shown in Figure 5.16, are given by the vector equation -----> OlP
----->
----+
= 02P + 0 102
where
BlM Bla;? B o;} 2
(5.44)
r r
[ Sl
S2
S3
[ Xl
Yl
Zl
[ X2
Y2
Z2 ] T .
(5.45) (5.46) (5.47)
5. Forward Kinematics
221
However, they must be expressed in the same coordinate frame, using cosines of the angles between axes of the two coordinate frames .
+ Y2 COS(Y2, Xl) + Z2 COS(Z2, xd + Sl X2 COS(X2 ' Yl) + Y2 COS(Y2, yr) + Z2 COS(Z2, Yl) + S2 X2 COS(X2 , Zl) + Y2 COS(Y2 ' Zd + Z2 COS(Z2 , Zl) + S3 X2(0) + Y2(0) + Z2(0) + 1
X2 COS(X2, Xl)
Xl Yl Zl
1
(5.48)
The transformation (5.48) can be rearranged to be described with the homogeneous matrix transformation COS(Y2 , xr) COS(Y2 ' yd COS(Y2 ' zd
o
In Figure 5.16 the axis X2 has been selected such that it lies along the shortest common perpendicular between axes Zl and Z2 . The axis Y2 completes a right-handed set of coordinate axes. Other parameters are defined as follows: 1. a is the distance between axes Zl and Z2 . 2. a is the twist angle that screws the
3. d is the distance from the
Xl -axis
Zl -axis
into the z2-axis along a .
to the X2 -axis.
4. () is the angle that screws the X l -axis into the X2-axis along d.
l l
Using these definitions, the homogeneous transformation matrix becomes,
X ' ]Zl Yl
1
or
_
'~' e smB
0 0
- sinB COS a cosB cos a - sin a
- sinBsin a cos Bsin a cos a
0
0
a't l~: ]
"'0,9 ]
l rp = lT 2 2r p
(5.49)
lT2 = (a,a,d ,B).
(5.50)
where The parameters a, a, B, d define the configuration of B 2 with respect to B, and belong to B 2. Hence, in general, the parameters ai, ai, Bi , di define the configuration of B, with respect to B i - l and belong to B i . (5.51)
222
5. Forward Kinemat ics
-
"'3
.....
G
2, 3
G2
d X
2,
4
FIG URE 5.17. A planar slider-crank linkage, making a closed loop or parallel mechanism .
Example 141 Th e same DH transformation matrix. Because in DH m ethod of setting coordinate fram es, a translation D and a rotat ion R are about and along one axis, it is immaterial if we apply the trans lation D first and then the rotation R or vice versa. Th erefore, we can change the order of D and R about and along the sam e axis and obtain the same DH transformation matri x 5.11. Th eref ore,
(5.52)
*
Ex ample 142 DH application for a slider-crank planar linkage. For a closed loop robot or mechanism there would also be a connection between the first and last links. So, the DH convention will not be satisfied by this connection. Figure 5.17 depicts a planar slider-crank linkage R.l.A- RIIRIIR and DH coordinat e fram es installed on each link .
5. Forward Kinematics
223
r---.-------
Zi
a. ! I
f
FIGURE 5.18. Non-standard definition of DR parameters joint i and link (i).
Table 5.6 - DH table for slider-crank planar Frame No . ai ai 1 -90deg a2 2 0 a3 3 0 a4 0 - 90 deg 4
a i , a i,
di , Bi defined for
linkage shown in Figur e 5.17. di Oi 180deg d 0 83 04 0 01 0
Applying a loop transformation leads to (5.53) where the transformation matrix [T ] contains elements that are functions of a2, d, a3, 03, a4, 04, and 0 1 . Th e parameters a2, a3, and a4 are constant while d, 03, 04, and 0 1 are variabl e. Assuming 01 is input and specified, we may solve for other unknown varia bles 03, 04, and d by equating corresponding elements of [T] and I.
*
Example 143 Non-standard Denavit-Hartenberg notation. Th e Denavit-Hartenberg notation presented in this section is the standard DH method. However, we may adopt a different set of DH parameters, simply by setting th e link coordinate fram e B, at proximal jo int i instead of th e distal jo int i + 1 as shown in Figure 5.18. Th e z.-axis is along th e
224
5. Forward Kinematics
axis of joint i and the xi-axis is along the common normal of the Zi and axes, directed from z; to Zi +l axes. The Yi-axis makes the B, frame a right-handed coordinate frame . Th e parameterization of this shift of coordinate frames are:
Zi+1
1. a; is the distance between the z, and Zi+1 axes along the Xi-axis. 2. ai is the angle from z, to Zi+l axes about the Xi-axis . 3. d; is the distance between the Xi-I and Xi axes along the Zi-axis.
4.
(}i
is the angle from the Xi-I and Xi axes about the zi-axis.
The transformation matrix from B i- I to B, utilizing the non-standard DH method, is made of two rotations and two translations about and along the local coordinate axes of B i - 1 . 1-Rotate ai-l about Xi-I' 2- Translate ai-l along Xi -I ' 3-Translate di along Zi-I . 4-Rotate (}i about Zi-I·
sin (}i cos ai -l cos (}i cos ai-I -sinai-I
sin (}i sin ai-I cos (}i sin ai -I
o
cos ai-I
o
Therefore, the transformation matrix from the B, to B i standard DH method is - si n (}i cos (}i cos ai-I cos (}i sin ai -I
o
o - sin ai-l cos ai -l
o
I
for the non-
ai -I
- d i sin a i - I di cos ai -I
]
.
1
(5.55) An advantage of the non-standard DH method is that the rotation (}i is around the z;-axis and the joint number is the same as the coordinate number. Actuation force, which is exerted at joint i , is also at the same place as the coordinate frame Bi , Addressing the link's geometrical characteristics, such as center of gravity , are more natural in this system. A disadvantage of the non -standard DH method is that the transformation matrix is a mix of i - 1 and i indices. Applying the standard or non -standard notation is a personal preference , since both can be applied effectively.
*
Example 144 Hasiati-Roberts notation and singularity of DH notation. In DH notation, the common normal is not well defined when the two joint axes are parallel. In this condition, the DH notation has a singularity, because a sma ll change in the spatial positions of the parallel joint axes can cause a large change in the DH coordinate representation of their relative position.
5. Forward Kinematics
225
B. I
FIGURE 5.19. Hayati-Roberts (HR) not ation to avoid the singu larity in the DH method .
Th e Hayat i-Roberts (HR) notation is another convention to represent subsequent links. HR avoids the coordinate singularity in the DH m ethod for the case of parallel lines. In the HR m ethod, the direction of the Ziaxis is defined in the B i - 1 fram e using roll and pitch angles (}:i and {3i as shown in Figure 5.19. Th e origin of the B , fram e is chosen to lie in the Xi- l Yi- l -plane where the distance d; is m easured between Oi-l and o. . Similar to DH convent ion , there is no uniqu e HR conve ntion concerning the freedom in choosing the angle of rotations. Furthermore, although the HR m ethod can elim inate the parallel joint axes' singularity, it has its own singularities when the Zi-axis is parallel to either the Xi_ l or Yi-l axes, or when z; intersects the origin of the B i- 1 fram e.
*
Ex ample 145 Parametri cally Continuous Convention. Th ere exists another alternative m ethod fo r coordinat e transformation called param etrically continuous convent ion (PC) . Th e PC method represent s the Zi-axis by two steps:
1. Direction of the z. -axie is defin ed by two direction cosin es, (}:i and {3i' with respect to the axes Xi- l and Yi-l . 2. Position of the z; -axis is defin ed by two distance param eters, li and m ; to indi cate the Xi- l and Yi-l coordinates of the intersection of z; in the Xi- l Yi-l -plane from the origin and perpendicular to the Zi-axis .
226
5. Forward Kinematics
Th e PC homogeneous matrix to transform B i 2
1-~
1+1
I-Al+1'i
-ai
- ;3i
where 'Yi
=
coordinates to B, is
_ Oti(3;
l +1'i _ Oti(3; l+1'i
o
1
i
(5.56)
o
V
1-
aT -
;3;'
(5.57)
Th e PC notation uses fou r param eters to indicate the position and orientation of the B i - 1 fram e in the B, fram e. Since we need six parameters in general, there mu st be two conditions: 1. Th e origin of the B, fram e must lie on the comm on normal and be along the j oint axis. 2. Th e
Xi -axis
must lie along the comm on normal.
5.3 Forward Position Kinematics of Robots The for ward or direct kin ematics is t he transformation of kinematic information from the robo t joint variable space to the Cartesian coordinate space of the final frame. So, the probl em of finding the end-effector position and orient ation for a given set of joint variables is the forward kinematics problem. Th is probl em can be solved by determining transformation matrices DT; to describ e t he kinemat ic information of link (i) in the base link coordinat e frame. The traditional way of producing forward kinemat ic equa tions for robo tic manipulators is to pro ceed link by link using t he Denavit-Hartenb erg not ations and frames. Hence, the forward kinematics is basically t ra nsformation matrix manipulation. For a six DOF robot , six DH transformation matrices, one for each link, are required to transform the final coordinates to the base coordinates. The last fram e at tached to the final frame is usually set at the cent er of the gripper as shown in Figure 5.20. For a given set of joint variables, the transformation mat rices i - i T; are uniquely det ermined. Therefore, the position and orientation of the end-effector is also a unique funct ion of th e joint vari ables. The kinematic information includ es: position, velocity, acceleration , and jerk. However, forward kinematics generally refers to the position analysis. So the forward position kinematics is equivalent to a det ermination of a combin ed transformation matrix (5.58)
5. Forward Kinematics
227
Xl
\ X
2
Z2
Or
x/I
P
Z/l
Zl "
Yo
0
xA
°
FIGURE 5.20. T he position of t he final frame in the base frame.
to find the coordi nates of a point P in t he base coord inate frame, when its coordi nates are given in t he final frame. (5.59)
E xample 146 3R planar manipulator forward kinematics. Figure 5.21 illustrates an RIIRIIR planar manipulator. Utilizing the DH parameters indicated in Example 126 and applying Equation (5.11), we can find the transformation matrices i-1Ti for i = 3,2,1. It is also possible to use the transformation matrix (5.32) of Example 133, since links (1) and (2) are both RIIR(O) .
2T3
=
lT2 =
°T, ~
l l
cos sinB03 0 0
- sinB 3 cosB3 0 0
0 13 cosO, ] 0 Is S~'03 1 0
cosO, sinB 2 0 0
- sinB 2 cosB2 0 0
0 0 1 0
l2 cos B2 h sin B2 0 1
- sinB l cos Bl 0 0
0 0 1 0
h cos Bl h sinB l
3
[ cosO,
Sir'
0 1
] ]
(5.60)
(5.61)
(5.62)
Therefore, the transformation matrix to relate the end-effector frame to the
228
5. Forward Kinematics
Xl j
y Yo
I-===
-.
v . . .... ,(_J._ ~ _
~ ~..
Y2 ~
<j>
x
FIGURE 5.21. A R IIRIIR planar manipulator.
base frame is
°T3
=
°T1 1T2 2T3 cos (81 + 8 2 + 83 ) sin (81 + 82 + 83 )
l
o o
-
sin (81 + 82 + 83 ) cos (8 1 + 82 + 83 ) 0 0
0 r 14 0 r24 1 0 0 1
]
(563) .
II cos 8 1 + h cos (8 1
+ 82 ) + l 3 cos (81 + 82 + 83 ) h sin 8 1 + l2 sin (8 1 + 82 ) + h sin (81 + 82 + 83 ) ,
r14
r24
The position of the origin of the fram e B 3 , which is the tip point of the robot, is at
(5.64)
It means we can find the coordinate of the tip point in the base Cart esian coordinate frame if we have the geom etry of the robot and all joint variab les.
x y
h cos 81 + h cos (81 + 82 ) + l 3 COS (8 1 + 82 + 83 ) h sin8 1 + l2 sin (8 1 + 82 ) + b sin (8 1 + 82 + 83 ) ,
(5.65) (5.66)
5. Forward Kinematics
229
Attached to forearm FIG URE 5.22. Spherical wrist kinematics.
Th e rest position of th e manipulator is lying on the x o-axis where fh = 0, 82 = 0, 83 = 0 because, °T3 becom es
1 0 0 0T = 0 1 0 3 0 0 1 [ 000
(5.67)
Example 147 Sph erical wrist forward kin ema tics. A spherical wrist is made by three Rf-R links with zero length s and zero offset where their joint axes are mutually orthogonal and intersecting at a point called the wrist point. Sphe rical wrist is a com m on wrist configuration in robotic application s. Figure 5.22 illustrates a schem atic of a spherical wrist configurat ion . It is made of an Rf-R(-90) link, attached to another Rf-R(90) link, that finally is att ached to a spinni ng gripper RIIR(O) . Th e gripper coordinate fram e B 7 is always parallel to B 6 and is attac hed at a distance d6 from the wrist point. Th e wrist will be atta ched to the final link of a manipulator, which is usually link (3). Th e coordin ate of the final link is usually set up at the wris t point or at the previous jo int and acts as the qrousul link for the wris t m echanism. So , the fir st fram e in a wrist m echanism is labeled 3, because we will be adding the wris t at th e end of a manipulato r. Th en the z3-axis will point along the for earm of the manipulator that is the first wrist joint axis. W e chose X5 = Z4 X Z5 and 0: = -90 deg because when 85 = 0 we wish for th e hand to point straight up from th e for earm . Utilizing the tran sformation matrix (5.37) fo r link (4) , (5.36) for link (5), R z ,IJ6 fo r link (6), and a D Z ,d 6 for f rame B 7 , we will find the follow ing
230
5. Forward Kinematics
individual transf orm ation matrices: [
0 0 - 1 0
COO04
sin 84 0 0
3T4 =
[
4T s =
COO05
sin8 s 0 0
[ cooO,
sin8 6 0 0
sT6 =
6T = 7
0 0 1 0
- sin 84 cas 84 0 0 sin8 s - cas 8s 0 0
- sin8 6 cas 86 0 0
0 0 1 0
[~ ~ ~
~] ~] ~]
(5.68)
(5.69)
(5.70)
(5.71)
0 0 1
000
The matrix 3T6 = 3T4 4Ts sT6 provides the wrist's orientation in the forebeam coordinate frame B 3 (5.72)
- c86 s84 - c84c8ss86 c84s8s c84c86 - c8ss8 4s86 s8 4s8 s c8s s8 ss8 6
o
o
~j
and the following transformation matrix provides the configuration of the tool frame B 7 in the fore beam coordinate frame B 3 .
3T7 = 3T4 4Ts sT6 6T7 C84C8sc86 - s8 4s86 - c86 s84 - c84c8ss86 c84s8s c84s86 + c8s c86s8 4 c84c86 - c8s s84s86 s8 4s8 s - c86s8s s8 ss8 6 c8s
r
o
o
0
Sometimes it is easier to define a compact sT6 to include rotation 86 and translation d6 by
oo
00
1 d6
o
1
j .
(5.74)
5. Forward Kinematics
231
Employing a compact 5T6 reduces the number of matrices, and therefore the number of numerical calculations. This is the reason we sometimes call B 6 the end-effector frame. Such an approach looks simpler, however, note that the position of the wrist point is invariant in a robot structure, but the position of the end-effector at d6 depends on application and the tool being used. The transformation matrix at rest position, where ()4 = 0, ()5 = 0, ()6 = 0, is
3T7 =
[H ~ 1j. o
0 0
(5.75)
1
Figures 5.23 and 5.24 depict a better illustration of a robot hand's links and joints. The common origin of frames B 4 , B 5, and B 6 is at the wrist point. The final frame, which is called the tool or end-effector frame, is denoted by three vectors, a , 5 , n , and is set at a symm etric point between the fingers of an empty hand or at the tip of the tools hold by the hand. The vector II is called tilt and is the normal vector perpendicular to the fingers or jaws. The vector s is called twist and is the slide vector showing the direction of fingers opening. The vector a is called turn and is the approach vector perpendicular to the palm of the hand. The placement of internal links' coordinate frames is predetermined by the DH method, however, for the end link the placement of the tool's frame B n is somehow arbitrary and not clear. This arbitrariness may be resolved through simplifying choices or by placement at a distinguished location in the gripper. It is easier to work with the coordinate system B n if Zn is made coincident with Zn - l . This choice sets a; = 0 and CKi = O. Example 148 R'rRII R articulated arm forward kinematics.
Consider an R'rRII R arm as shown schematically in Figure 5.25. To develop the forward kinematics of the robot, the DH parameter table of the robot at rest position is set up as indicated in Table 5.7. The rest position of the robot could be set at any configuration where the joint axes Zl , Z2 , Z3 are coplanar, however two positions are mostly being used. The first position is where X l is coplanar with Zl , Z2 , Z3 , and the second position is where X o is coplanar with Zl , Z2 , Z3. Table 5.7 - DH parameter table for setting up the link frames. CKi di ()i Frame No. ai 1 0 -90deg d l ()l 2 12 0 d2 ()2 3 0 90deg l3 ()3
We recommend applying the link-joints classification in Examples 133 to 138. Therefore, we must be able to determine the type of link-joints
232
5. Forward Kinematics
FIG URE 5.23. Hand of a robot at rest position.
FIGURE 5.24. Hand of a robot in motion.
5. Forward Kinematics
233
Yo - -- ...
__... -.... . . ..
FIGURE 5.25. Rf-RIIR articulat ed arm.
combination as shown in Table 5.8. Table 5.8 - Link classification fo r set-up of th e link fram es. Link No . Type 1 Rf-R(-90) 2 RIIR(O)
3
Rf-R(90)
Th erefore, the successive transformation matrices have th e follow ing expressions:
(5.76)
(5.77)
(5.78)
(5.79)
234
5. Forward Kinematics
we only need to find the result of a matrix multiplication. Therefore, °T3
= °TI IT2 2T3
~~~ ~~~ ~~:r33 ~~:r34 ]
[ r 31 r32 00
0
(5.80)
1
where
+ ( 3) sin Ol cos( O2 + ( 3) - sin(02 + ( 3)
(5.81)
rl2
- sin Ol
(5.84)
r22 r32
cos Ol
(5.85)
0
(5.86)
cos(h COS(02
rll r21 r 31
(5.82) (5.83)
rl3
COS OI sin(02 + ( 3)
(5.87)
r23 r33
sin Olsin( O2 + ( 3)
(5.88)
cos( O2 + ( 3)
(5.89)
=
12 cos Olcos O2 - d2 sin Ol 12 cos 82 sin Ol + d2 cos Ol d l - l2 sin02 '
rl4 r24
r34
(5.90) (5.91) (5.92)
The tip point P of the third arm is at [0 0 13] T in B 3. So, its position in the base frame would be at
The transforma tion matrix at rest position, where Ol = 0, O2 = 0, 03 = 0, is
0T = 3
Therefore, at rest position, xo,
1 0 0 l2] 0 1 0 d2 0 0 1 dl [ o 0 0 1 Xl
.
(5.94)
are parallel, and Zl, Z2 , Z3 are coplanar.
5. Forward Kinematics
X7
X
5
235
7
-5
'It
°6 ,
FIGURE 5.26. A spherical robot made by a spherical manipulator equipped with a sp herical wrist .
Example 149 Spherical robot forward kinematics. Figure 5.26 illustrates a spherical manipulator attached with a spherical wrist to make an R'r-R'r- P robot. The associated DH table is shown in Table 5.9.
Table 5.9 - DH parameter table for (Xi Frame No. ai 1 0 -90deg 90deg 2 0 0 3 0 4 0 - 90 deg 90deg 5 0 0 0 6
Stanford arm. di 8i 0 81 l2 82 d3 0 0 84 0 85 0 86
However, it is recommended to apply the link-j oints classification in Ex amples 133 to 138. Therefore, we must be able to determine the type of link-joint combinations as shown in Table 5.10.
236
5. Forward Kinematics
Table 5.10 - DH parameter table for setting up the link frames . Link No . Type 1 Rf-R(-90) 2 Rf- P(90)
3 4 5
6
PIIR(O) Rf-R(-90) Rf-R(90) RIIR(O)
Employing the associated transformation matrices for moving from B, to B i - l shows that
- sinOl eos Ol
o o
~]
l~
(5.95)
(5.96)
]
(5.97)
(5.98)
(5.99)
(5.100)
(5.10 1)
5. Forward Kinematics
237
where, sB6 (-cB 4sB1 - cB1cB2sB4) +cB6 (-cB 1sB2sB 5 + cB5 (-sB 1sB4 + cB 1cB2cB 4)) (5.102) sB6(cB 1cB4 - cB 2sB1sB4) (5.103) +cB6 (- sB1sB2sB 5 + cB 5 (cB 1sB4 + cB 2cB 4sBd) (5.104) sB2sB4sB 6 + cB6 (-cB 2sB5 - cB 4cB 5 sB2)
rn
r21 r31
r12
cB 6 (-cB 4sB1 - cB1cB2sB4) -sB6 (-cB 1sB2sB 5 + cB5 (-sB 1sB4 + cB1cB2cB4)) (5.105)
r22
cB6 (cB 1cB4 - cB2sB1sB4) -sB 6 (-sB 1sB2sB 5 + cB 5 (cB 1sB4 + cB 2cB 4sBd) cB6 sB2sB4 - sB6 (- cB 2sB 5 - cB 4cB 5sB 2)
r32
r13 r23 r33
cB 1cB 5sB 2 + sB5 (-sB 1sB4 + cB1cB2cB4) cB 5 sB1sB2 + sB5 (cB 1sB4 + cB2cB4sB1) cB 2cB 5 - cB 4sB2sB 5 -12SB1
r14 r24
+ d3cB 1sB2
12CB1 + d3sB1sB2 d3cB2.
r34
(5.106) (5.107)
(5.108) (5.109) (5.110)
(5.111) (5.112) (5.113)
The end-effector kinemati cs can be solved by multiplying the position of the tool fram e B 7 with respect to the wrist point, by °T6 (5.114)
where 1 0 0 0 1 0
6
T7
=
0 ] 0
0 0 1 d6 [ o 0 0 1
.
(5.115)
Example 150 Checking the robot transformati on matri x. To check the correctness of the final transformation matri x to map the coordinates in tool fram e into the base fram e, we may set the joint variables at a specific rest position. Let us substitut e the joint rotational angles of the spherical robot analyzed in Example 149 equal to zero. (5.116)
238
5. Forward Kinematics
Therefore, the transformation matrix (5.101) would be °T6
=
»r, lT2 2T3 3T4 4T5 5T6
[
~ ~ ~ l~]
o o
(5.117)
0 1 d3 0 0 1
that correctly indicates the origin of the tool frame in robot 's stretched-up configuration, at
(5.118) Example 151 Assembling a wrist mechanism to a manipulator. Consider a robot made by mounting the hand shown in Figure 5.23, to the tip point of the articulated arm shown in Figure 5.25. The resulting robot would have six DOF to reach any point within the working space in a desired orientation. The robot's forward kinematics can be found by combining the wrist transformation matrix (5.73) and the manipulator transformation matrix (5.80). °T7
=
(5.119)
