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)
T arm Twrist 6T7 °T3 3T6 6T7
The wrist transformation matrix 3T7 has been analyzed in Example 147, and the arm transformation matrix °T3 has been found in Example 148. However, since we are attaching the wrist at point P of the frame B 3, the transformation matrix 3T4 in (5.68) must include this joint distance. So, we substitute matrix (5.68) with
3T,
l
~ ~1:: ~1
~]
(5.120)
to find Twrist
3T6 -SB4SB6 + cB4cB5cB6 cB4sB6 +cB 5cB6sB4 -cB 6sB5 [
o
l
-cB 6sB4 - cB4cB5sB6 cB4cB6 -cB5sB4sB6 sB5sB6
0
The rest position of the robot can be checked to be at
0T7 =
~o ~ ~ d +l~ db ] o
0 1 0 0
d2 l
6
1
(5.122)
5. Forward Kinematics
239
FIGURE 5.27. An RIIRIIRIIP SCARA manipulator robot.
because 6
T7 =
[
01 01 00 00 ] 0 0 1 d6
o
0 0
(5.123)
.
1
Example 152 SCARA robot forward kinematics. Consider the R IIR IIRIIP robot shown in Figure 5.27. The forward kin ematics of the robot can be solved by obtaining individual transformation matrices i - 1 T i. The first link is an R IIR(O) link, which has the following transformation matrix:
°T, ~
[ 00,0,
'1°'
- sin ()1 cos ()1 0 0
0 II cos ()1 0 t, sin ()1 1 0 1 0
]
(5.124)
Th e second link is also an RIIR(O) link
'T, ~
'ir'
[ =0
2
- sin ()2 cos ()2 0 0
0 l, COSO, 0 l2 sin ()2 o 1 0 1
1 .
(5.125)
240
5. Forward Kinematics
Th e third link is an R IIR(O) with zero length
=
2r. 3
cos (h - sin 03 0 sin 03 cos 03 0 0 0 1 [ 000
and fina lly the f ourth link is an R IIP(O )
'T,
~ [~ ~ ~
(5.126)
n
(5.127)
Th erefore, the configuration of the end-effector in the base coordinate fram e is aT4
=
aT1 IT2 2T 3 3T 4
[ e(O, S(OI
+ 0, +8, ) + 02 + 03) 0 0
(5.128)
- S(OI + 02 + 03) C(OI + 02 + 03 ) 0 0
ce, + 12 c(0, + O2 ) 1
0 I, 0 h SOl 1 0
that shows the rest position of the robot 0 1 = 02 = 03 = d
1 0 0 II
a
T4 =
r
010 0 0 1 o 0 0
+ 12 0 0 1
+ 12S(01 + 02) d 1
=0
is at
]
.
(5.129)
Example 15 3 Space station remote manipulator system (SS R MS). Shuttle remote m anipulator system (S R MS) , also kn own as (SS R MS), is an arm an d a hand attached to the Shu ttle or space station . It is ut ilized f or several purpo ses such as: satellite deploy m ent, constructi on of a space station, transporting a crew m ember at the end of the arm, surv eying and in specting the outside of the station using a cam era. A sim plifie d mod el of the SRMS, schem atically shown in Figure 5.28, has the charact eristics indicated in Table 5.11 . Table 5.11- Spa ce station's robot arm characteris tics. Length 14.22m Diam et er 38.1cm W eight 1336kg Num ber of j oints S even Handl ing capacity 116000kg (in space) M ax velocity of end of arm Carrying nothing : 37crnj s Full capacity : 1.2crnj s Max rotational speed Approx. 4 deg j s
5. Forward Kinematics
Elbow joint - - -_
241
.-.10..
Yo FIGURE 5.28. Illustrat ion of the space station remote manipulator system.
Table 5.12 - DH parameters for SRMS. 1 2
3 4 5 6 7
0 0 7110m m 7110m m 0 0 0
- 90 deg -90deg 0 0 90 deg - 90 deg 0
380mm 1360m m 570mm 475mm 570mm 635mm d7
01 82 03 04 05 06 0
Consider the numerical values of joints offset and links ' length as tabulated in Table 5.12. Utilizing these values and indicating the type of each link ena bles us to determine the required transformation matrices to solve the forward kinematics problem. Links (1) and (2) are Rf-R(- 90) , and therefore,
o - sin01 o cos 01 -1
o
0 0
(5.130)
242
5. Forward Kinematics
'T,
- sin 82 cos 82 0 0
0 0 - 1 0
[ cooO,
= Si~O'
~2
(5.131) ]
Lin ks (3) and (4) are RIIR(O), hence
[ cosO, - sin 83 0 .,0050, ] 'T,
Sir'
cos 83 0 0
0 1 0
a3 sin 83 d3 1
Sbt
- sin 84 cos 84 0 0
0 0 1 0
0,a4 coo 04 ] sin 8
=
4
'T,
[ oos8
~
d4
4
(5.132)
.
(5.133)
1
Link (5) is Rf-R(90), and link (6) is Rf-R(-90), therefore
4T5 ~
Sirs
[ cos 05
6 [ 0088
Sir'
5T, =
0 0 1 0
sin8 5 - cos 85 0 0
~5
0 0 - 1 0
- sin 86 cos 86 0 0
~]
(5.135)
0 0 ] 0 0 1 d7 0 1
(5.136)
(5.134) ]
Finally link (7) is Rf-R(O) and the coordinate frame attached to the endeffector has a translation d7 with respect to the coordinate frame B 6 . COS 8 7
6T = 7
[
sin8 7 0
o
-
sin 8 7 cos 8 7 0 0
The forward kinematics of SSRMS can be found by direct multiplication of 1 Ti, (i = 1, 2" " ,7).
i-
(5.137)
5.4
* Coordinate Transformation Using Screws
It is possib le and easier to use screws to describe a transformation matrix between two adjacent coord inate frames B; and B i - 1 . We can move B i - 1
5. Forward Kinematics
243
to B, by a central screw s(ai, ai,ii- I ) followed by anot her central screw s(di, Oi, ki-r) .
s(di, Oi, ki - I) s(ai,ai, ii- I) - sin Oi cos ai sin Oi sinai COS Oi - cos Oi sin ai sin Oi cos Oi cos ai cos o, o sin ai [ o o 0
Proof. The cent ra l screw s(ai, ai, ii- r) is
s(ai,ai, ii- r)
tx«; ii-r) R(ii-I' ai )
(5.139)
D X i _ ll a i R X i _ 1 , Q i
[~
[~
0 0 1 0 0 1 0 0 0 cos o ,
.l~
sinai 0
0 - sinai cos o , 0
0 cos o,
sin a i 0
0 - sin ai cos o, 0
n
~1
and t he cent ral screw s(di, Oi, ki-r) is
s(di, Oi, ki-r)
(5.140)
D(d i , ki-r)R(ki - I , Bi ) D Zi _l ,d iRz i _ l ,Oi
U ~] 0 0 1 0 0 1 0 0
[ cos O; sin Oi 0 0
- sin Oi COS Oi 0 0
[ c009; sinOi 0 0
0 0 1 0
- sin Oi COS Oi 0 0
~1
0 0 1 0
n
Therefore, t he transformation matrix i-I Ti made by two screw motions
244
5. Forward Kinematics
would be i- 1T i
(5.141)
s( d i, Bi, ki -d s(ai, ai , zi-d
sin Bi
- sin Bi cos Bi
0 0
0 0
0 0
0
[ coo O;
[ =0; sin Bi 0 0
1
~, ][~
0 cos o,
0
sinai
- sinai cos o,
0
0
- cos ai sin Bi cos Bi cos o, sinai
sinBi sinai - cos Bi sin ai cos o,
0
0
ai cos Bi a; sin Bi di 1
ai 0 0 1
]
]
T he resultant t ransformation matrix i-1Ti is equivalent to a general screw whose parameters can be found based on Equations (4.125) and (4.126). The twist of screw, ¢, can be comp ute d based on Equation (4.129)
~(tr(CRB)-l)
cos¢
1 2"(cosB i + cosBi cos o,
+ coso, -
(5.142)
1)
and the axis of screw, U, can be found by using Equation (4.131) ii
_ 1_ (CR B _ CRT) 2sin¢ B -1 2s¢
1 2s¢
-
ca isBi ,0,.,"" ] [C O'i -cBicai - cBlsa l sB 0
so;
[ ,0, + ~",,'O, - sBisa i
cal -sB i - caisBi 0 sai + cBisai
[
cBi - ca i sBi sBisai
sBi cBicai - cBisa i
sBisai -sai - cBisai 0
]
,~,
cai
]
(5.143)
and t herefore , A
sin a i. + co~ Bi sin ai ] 1 smBismai . 2s¢ [ sin Bi + cos ai sin Bi
U= -
(5.144)
The translation parameter, h, and t he position vector of a point on t he screw axis, for instance [ 0 Yi-l Zi-l ], can be found based on Equation (4.134).
5. Forward Kinematics
Yi~1 ] [ Yi-l
Ul - r12 U2 1 - r22 [ U3 - r32 1
2
~
- r I3] - r23 1 - r33
[ SCl:i + COiSCl:i SOiSCl:i
~ +~~
- 1 [
r14 ] r24 r34
(5.145)
- SOi CCl:i SOi SCl:i ] 1 - COi CCl:i -COi so;
~
245
~
- 1 [
ai COi ] a; SOi
~
•
*
Example 154 Classification of industrial robot link by screws. There are 12 different configurations that are mostly used for industria l robots. Each type has its own class of geometrical configuration and transformation. Each class is identified by its joints at both ends, and has its own transformation matrix to go from the distal j oint coordinate frame B , to the proximal joint coordinate fram e B i- 1. The transformat ion matri x of each class depends solely on the proximal joint, and the angle between z- axes. The screw expression for two arbitrary coordinate frames is
(5.146) where s(di, Oi , ki- d s(ai, Cl:i , ii- I)
o ie; ki-dR(ki - 1, Oi) o i« , ii- d R(ii-l , Cl:i).
(5.147) (5.148)
The screw expression of the frame transformation can be simplified for each class according to Table 5.13. Table 5.13 - Classification of industrial robot link by screws. No. Type of Link ' - lTi = 1 or s(O, Oi , ki-d s(ai , 0, ii- d R IIP(O) RIIR(O) 2 or R IIP(180) R IIR(180) s(O, Oi, k i - 1) s(ai , 27f, ii- d or K lP(90) 3 K lR(90) s(O, Oi , ki-d s(ai , 7f, ii- d 4 R.lR(- 90) or R.lP( - 90) s(O, Oi , ki - 1 ) s(ai , - 7f, ii- d Rf- R (90) or Rf- P(90) 5 S(O, Oi, ki-d s(O, n ,ii-I) R f-R(- 90) or R f-P(- 90) S(O, Oi, ki-d s(O, - 7f, ii- d 6 or PIIP(O) 7 s(di , 0, ki-d s(ai , 0, ii- d PIIR(O) or PI IP(180) 8 PIIR(180) s(di , 0, ki-d s(ai, 27f, ii-d P.lR(90) or P.lP(90) 9 s(di, 0, ki-d s(ai , n , ii-d 10 P.lR(- 90) or P.lP(- 90) s(di, 0, ki-d s(ai, - 7f, ii- d 11 I+R(90) or I+P(90) s(di, 0, ki-d s(O, n , ii- d 12 I+R(-90) or I+P( - 90) s(d i, 0, ki-d s(O, - 7f , ii-d
246
5. Forward Kinemat ics
A s an example, we ma y exam ine the first class
8(0, ()i, k i- I) 8(ai , 0, ii- I)
i- IT i
II
D (O, k i - I )R(ki - I , ()i )D (ai , ii-I) R( ii -l , 0)
~~~:; ~~~n()~i ~ ~
~ ~ °° ai°1 °° °1 °1 °° °° °1 °1
l l
°° ~~~:; ~~~~~i ~ ~: ~~~:; °° °° ° °
]
1
and find the same result as Equation (5.32).
1
*
Example 155 Sph eri cal robot forward kinematics based on screws. Application of screws in for ward kin ema tics can be done by determining the class of each link and applying the associat ed screws. Th e class of links f or th e spherical robot shown in Figure 5.26, are in dicated in Table 5. 14. Table 5.14 - S crew transformation for the spherica l robot shown in Figure 5.26. Link No . Class Screw transf ormation R f-- R (- 90) °T I = 8(0, ()i, k i - I ) 8(0, - 7r , ii-d 1 R f--P (90) IT 2 = 8(0, ()i , ki- d 8(0, tt , ii- I) 2 2T 3 = 8(d i, 0, ki- I)s (ai , 0, ii -d 3 P IIR (O ) R f--R (-90) 3T 4 = 8(0, ()i , k i - I ) 8(0, - 7r , ii- I) 4 R f-- R (90) 4Ts = 8(0, ()i, k i- I) 8(0, n , ii - I) 5 6 sT 6 = 8(0, ()i , k i-d8( ai, 0, ii -d RIIR(O) Th erefore, the configuration of the en d-effec tor fram e of the spherical robot in the base fram e is o~
0nl~ 2 n 34 4~S~
8(0, ()i , ki-d 8(0, -7r , ii - d 8(0, ()i , k i - I ) 8(0, n , ii- d x 8(d i, 0, ki - I ) 8(ai , 0, ii- d 8(0, ()i , k i - I) 8(0, -
7r ,
ii- d
X8(0, ()i, ki-d 8(0, rr, ii- d 8(0, ()i, ki-d 8(ai, 0, ii- I). (5.149)
*
Example 156 Pliicket coordinate of a central screw. Utilizin g Pliicker coordinates we can defin e a central screw 8(h, ¢,u) =
[ ~~ ]
(5.150)
which is equal to [
~~
] = D (hu) R(u, ¢).
(5.151)
5. Forward Kinematics
247
*
Example 157 Pliicker coordinate for the central screw s(ai , ai, ii-d . The central screw s(ai, ai ,ii- d can also be describe by a proper Pliicker coordinate.
[ ~; ;;=: ]
(5.152)
oi« ,ii-dR(ii-1 ' ai) Similarly, the central screw s(di , Oi, ki - 1) can also be described by a proper Pliicker coordinate. Oi ~i-1 [ di k i - 1
]
(5.153)
tx«; ki-dR(ki- 1, Oi)
*
Example 158 Intersecting two central screws. Two lines (and therefore two screws) are intersecting if their reciprocal product is zero. We can check that the reciprocal product of the screws s(ai ' ai , ii-d and s(d i , Oi, ki-d is zero.
[~: ~:=: ]
® [
Oi ki - 1 . a; ii-1
o.
5.5
*
~; :;=: ]
+ a i ii-I ' Oi k i - 1 (5.154)
Sheth Met hod
The Sheth method can overcome the limit at ion of the DH method for higher order links , by introducing a number of frames equal to the number of joints on the link. It also provides more flexibility to spe cify the link geometry compar ed to the DH method. In Sheth method, we define a coordinat e frame at each joint of a link, so an n joint robot would have 2n frames . Figure 5.29 shows the case of a binary link (i) where a first frame ( Xi , Yi , Zi) is attached at the origin of the link and a second frame (Ui , Vi, Wi) to t he end of the link. The assignment of t he orig in joint and the end joint are arbitrary, however it is easier if they are in the dir ecti on of bas e-to-tool fram es. To describe the geometry, first we locate the joint axes by Zi and Wi , and t hen determine the common perpendicular to both joint axes Zi and Wi. The common normal is indicated by a unit vector 'hi' Specifying the link geometry requires six paramet ers, and are determined as follows: 1. a: is the distance from Zi to W i, measured along 'hi. It is the kin ematic distance between Zi and W i .
248
5. Forward Kinematics z. I
a I
1 !. I '..
)..1
"< ,
--
.
:
..
-----_ l V. --_
I
, lt .
I' FIGURE 5.29. Sheth method for defining the origin and end coordinate frames on a binary link. 2. b, is the distance from of the wi-axis .
ni
to
Ui,
measured along
Wi.
It is the elevation
3. c; is the distance from of the Zi-axis.
Xi
to
ni,
measured along
Zi.
It is the elevation
and
Wi ,
measured positively from z;
is the angle made by axes ii, and about Wi.
iu,
measured positively from
ni
and iu, measured positively from
Xi
4. (Xi is the angle made by axes to ui; about ni.
5.
f3i
to
Zi
Ui
6. Ii is the angle made by axes to ni about z. ,
Xi
The Sheth parameters generate a homogeneous transformation matrix (5.155)
5. Forward Kinematics
249
to map the end coordinate frame Be to the origin coordinate frame B;
(5.156)
where °Te denotes the Sheth transformation matrix T12
T13
T22
T2 3
T32
T33
o
(5.157)
0
where, cos (3i cos I' i - cos O:i sin (3i sin I' i
(5.158)
+ cos O:i cos I' i sin (3i
(5.159)
cos (3i sin I'i
(5.160)
sin O:i sin (3i - cos I' i sin (3i - cos O:i cos (3i sin I'i
(5.161)
+ cos O:i cos (3i cos I'i
(5.162)
- sin (3i sin I'i
(5.163)
cos (3i sin O:i T1 3
sin O:i sin I'i
T23
-
T33
cos O:i
(5.164) (5.165)
cos I'i sin O:i
(5.166)
T 14
a;
cos I'i
+ b, sin O:i sin I' i
(5.167)
T24
a;
sin I'i - b, cos I' i sin O:i
(5.168)
T34
c; + b, cos O:i.
(5.169)
The Sh eth transformation matrix for two coordinat e frames at a joint is simplified to a translation for a prismatic joint, and a rotation about the Z-axis for a revolute joint. Proof. The homogenous transformation matrix to provide the coordinates in Bi, wh en the coordinates in B j are given , is
(5.170) Employing the asso ciated tran sformation matrices COS (3i
R .
z,, (3 i
=
sin (3i
0
[
o
- sin (3i cos (3i
o o
o
0]
0 1 0 o 1
(5.171)
250
5. Forward Kinematics
lf n 0
0
=
D Zi ,bi
1 0 0 1
(5.172)
0 0
R X i 1Q:i
==
[f ==
D x i ,a i
[f
C~ 1; sm -y,
[ R Zi ,'Yi
0 cos D:i sin D:i 0
==
0 1 0 0
0 0 1 0
0 0
- sin /'i cos /'i 0 0
[f
0 1 0 0
==
D Zi ,Ci
0 - sin D:i cos D:i 0
0 0 1 0
~]
n 0 0 1 0
(5.173)
(5.174)
~]
n
(5.175)
(5.176)
we can verify t hat t he Shet h t ransformat ion matrix is
°Te =
•
c(3iC'Yi
- C'Yi s (3i
- CD:i s (3i S /'i
- CD:ic(3 is/, i
a i C/' i S D:iS/'i
+bi SD:i S/' i
c(3i S/'i
- s (3i S /'i
+ CD:i C'Yi s (3i
+ CO:i c (3i C'Yi
- c /'i SD:i
a i s /' i -bi C'Yi SD:i '
SD:i s (3i
C(3i SD:i
CD:i
Ci +
0
0
0
1
(5.177)
b.ca,
*
Example 159 Sheth transformation matrix at revolute and prismatic joints. Two links connected by a revolute joint are shown in Figure 5.30 (a) . The coordinate frame s of the two links at the comm on joint are set such that the axes z; and W j coincide with the rotation axis, and both fram es have the same origin . The She th parameters are a = 0, b = 0, C = 0, D: = 0, (3 = 0, /' = 0 , and therefore,
cos O - sinO sinO cosO
l
o o
o o
o
0 0
1 0 o 1
1.
(5.178)
5. Forward Kinematics
z.I
251
W. }
(b)
(a)
F IGURE 5.30. Illust ration of (a) a revolute joint and (b) a prosmatic joint to define Shet h coord inate t ransformation.
FIG URE 5.31. Illustration of a cylindrical joint to define Shet h coordinate tra nsformation.
Two links connected by a prismatic joint are illus trated in Figure 5.30 (b). Th e Sheth variable at this j oint is d along the joint axis. Th e coordinate fram es of the two links at the common joint are set such that the axes z, and Wj coinci de with the translation al axis, and axes Xi an d U j are chosen parallel in the same direction. Th e Sh eth parameters are a = 0, b = 0, c = d, a = 0, /3 = 0, 'Y = and therefore,
°,
(5.179)
*
E x a mple 160 Sheth transformation m atrix at a cylindrical join t. A cylindrical jo int provides two DOF, a rotational and a trans lational about the same axis . Two links connecte d by a cylindrical joint are shown in Figure 5.31. The transform ation ma trix for a cylindrica l joint can be described by com bining a revolut e and a prism atic joint. Therefore, the
252
5. Forward Kinematics
FIGURE 5.32. Illust rat ion of a screw joint to define Shet h coordinate transformation .
Sheth parameters are a
= 0, b = 0, c = d, 0: = 0, (3 = 0, "( = e, and - sine 0 0 cose 0 0 Old o 0 1
j
.
(5.180)
*
Example 161 Sheth transform ation matrix at a screw joint. A screw joint, as shown in Figure 5.32, provides a proportional rotation and trans lation motion, which has one DOF. The relationship between translation h and rotation () is called pitch of screw and is defined by p
=
h
o·
(5.181)
The transformation for a screw joint may be expressed in terms of the relative rotation
e
iTj
=
[ cosO sine 0 0
- sin e cos e 0 0
0 0 1 0
~j
(5.182)
or displacement h
i
Tj
.
r
coo' _sin K
-
P
0 0
h
- SlIlp
cos !l
P
0 0
0 0 1 0
~]
(5.183)
The coordinate frames are installed on the two connected links at the screw joint such that the axes Wj and z; are aligned along the screw axis, and the axes Uj and Xi coincide at rest position.
5. Forward Kinematics
253
FIGURE 5.33. Illustr ation of a gear joint to define Sheth coordinate tr ansformation.
*
Example 162 Sh eth transformation matrix at a gear joint. Th e Sheth method can also be utilized to describe the relativ e motion of two links connected by a gear jo int. A gear jo int, as shown in Figure 5.33, provid es a proportional rotation, which has 1 DOF. Th e axes of rotat ion s in dicate the axes W j and Zi , and their comm on perpendi cular shows the ii vector. Th en, the Sh eth parameters are defined as a = R ; + R j , b = 0, c = 0, a = 0, (3 = (}j , 'Y = (}i , to have cos (1 + c)(}i - sin (1 + c) () i cos (1 + c) (}i sine! ~£)Bi
-r, ~ [
o o
o o
Rj (1 + c) COS (}i R j (1 + c) sin () i
1
0
o
1
]
(5.184)
where (5.185)
5.6
Summary
Forward kinematics describ es the end-effecto r coord inate fram e in t he base coordinate frame of a robot when the joint variables are given.
(5.186) For an n link serial robot , it is equivalent to finding the transformation matrix »i; as a function of joint vari ables
(5.187) There is a special rule for installing the coordina te frames attached to each robot 's link called the standard Denavi t-Hartenb erg convention. Based on the DH rule, each t ra nsformat ion matrix i - 1T i from the coordinate fram e
254
5. Forward Kinematics
B, to B i - 1 can be expressed by four par ameters; link length ai , link twist ai, joint distan ce di , and joint angle fh COS (}i
i- 1T = i
-
sin (}i [
o o
sin (}i cos a i cos ~i cos ai sm o, 0
sin (}i sin a i - cos (}i sin ai cos o, 0
ai cos (}i a; sin (}i
d; 1
j
(5.188)
However , for most indus trial robots, the link transformation matrix i- 1Ti can be classified into 12 simple typ es. 1 2 3 4
5 6 7 8 9 10 11
12
RIIR(O) RIIR (180) K1R(90) K1R( -90) Rf-R(90) Rf-R( - 90) PIIR (O) PIIR(180) P-lR(90) P-lR( -90) Pf-R(90) Pf-R(-90)
or or or or or or or or or or or or
RIIP(O) RIIP (180) K1P(90) K1P(-90) Rf-P(90) Rf-P (- 90) PIIP (O) PIIP (180) P-lP (90) P-lP (- 90) Pf-P(90) Pf-P (-90)
Most indu stri al robots are mad e of a 3 DOF man ipulator equipped with a 3 DOF spherical wrist . The transformation matrix °T 7 can be decompo sed into three subma t rices °T3, 3T6 and 6T7 . (5.189)
T he matri x °T 3 position s the wrist point and depends on the man ipul ato r links and joints. The matrix 3T6 is t he wrist t ra nsformat ion matri x and 6T7 is t he tool s t ra nsformat ion matrix. Decomp osing °T7 into submatrices ena bles us to make the forward kinemati cs modular .
5. Forward Kinematics
255
Exercises 1. Notation and symbols.
Describe th e meaning of a- Bi
b-
a i
c- [ ¢u hu ]
f- i- 1T i
g-
@
h- [
k- d;
1- a;
Bi~i-l
d- s(di ,Bi ,ki- 1)
e- RIIR(180)
]
i- s(h , ¢, u)
j- P l-R( -90)
]
n- s (ai, ai,ii- l )
0-
d; k i - 1
m-
ii - l ai 2i - l
[ a i
A
m-R( -90) .
2. A 4R planar manipulator. For the 4R planar manipulator, shown in Figure 5.34, find the (a) DR t able (b) link-typ e t able (c) individual frame transfo rmation matrices i- 1T i , i = 1,2 ,3 ,4 (d) global coordinates of the end-effector (e) orientation of the end-effector 'P.
FIGURE 5.34. A 4R planar manipul ator.
3. A one-link RI-R( -90) arm.
256
5. Forward Kinematics
(a) FIGURE 5.35. A one-link m-R( -90) manipulator. For the one-link Rf-R( -90) manipulator shown in Figure 5.35 (a) and (b) , find the transformation matrices °T1 , ITz, and °Tz. Compare the transformation matrix ITz for both frame installations. 4. A 2R planar manipulator. Determine the link 's transformation matrices °T1 , ITz, and °Tz for the 2R planar manipulator shown in Figure 5.36. 5. A polar manipulator. Determine the link 's transformation matrices ITz, zT3 , and IT3 for the polar manipulator shown in Figure 5.37. 6. A planar Cartesian manipulator. Determine the link 's transformation matrices ITz, zT3 , and IT3 for the planar Cartesian manipulator shown in Figure 5.38. 7. Modular articulated manipulators. Most of the industrial robots are modular. Some of them are manufactured by attaching a 2 DOF manipulator to a one-link Rf-R( -90) arm. Articulated manipulators are made by attaching a 2R planar manipulator, such as t he one shown in Figure 5.36, to a one-link Rf-R( -90) manipulator shown in Figure 5.35 (a) . Attach the 2R manipulator to the one-link Rf-R( -90) arm and make an articulated manipulator. Make the required changes into the coordinate frames of Exercises 3 and 4 to find the link 's transformation matrices of the articulated manipulator. Examine the rest position of the manipulator.
5. Forward Kinematics
\ 1------
__ 3
-f---'-------- ~
Xl
F IGURE 5.36. A 2R planar manipulator.
FIGURE 5.37. A 2 DOF polar manipul ato r.
257
258
5. Forward Kinematics
d
Z,
2
.
Z3
d~ _ _ _I--L_ _
X
2
~
Z,
FIGURE 5.38. A 2 DOF Cartesian manipulator. 8. Modular spherical manipulators. Spherical manipulators ar e made by attaching a polar manipulator shown in Figure 5.37, to a one-link Rf--R( -90) manipulator shown in Figure 5.35 (b) . Attach th e polar manipulator to the one-link Rf--R( -90) arm and make a spherical man ipulator. Make t he required cha nges to the coordinate frames Exercises 3 and 5 to find the link 's transformation matrices of t he spherical manipulator. Examine the rest position of t he manipulator. 9. Modular cylindrical manipulators. Cylind rical manipulators are made by at taching a 2 DOF Car tesian manipulato r shown in Figure 5.38, to a one-link RI-R(-90) manip ulator shown in Figure 5.35 (a) . Attach th e 2 DOF Cartesian manipulator to t he one-link Rf--R( -90) arm and make a cylindrical manipul ator. Make t he required chan ges into the coordinate frames of Exercises 3 and 6 to find t he link 's t ransformation matrices of the cylindrical man ipulator. Examin e the rest position of th e manipulator. 10. Disassembled spherical wrist. A spherical wrist has three revolut e joints in such a way that th eir joint axes inte rsect at a common point, called the wrist point. Each revolute joint of the wrist at tac hes two links. Disassembled links of a spherical wrist are shown in Figure 5.39. Define the requir ed DR coordinat e fra mes to t he links in (a) , (b) , and (c) consistentl y. Find the transformation matrices 3T4 for (a) , 4Ts for (b) , and sT6 for (c). 11. Assembled spherical wrist .
5. Forward Kinematics
259
(b)
(c)
FIGURE 5.39. Disassembled links of a spherical wrist. Lab el the coordinat e frames attached to the spherical wrist in Figure 5.40 according to the frames that you inst alled in Exercise 10. Det ermine the transformation matrices 3T 6 and 3T 7 for t he wrist. 12. Articulat ed robots. Attach the sph erical wrist of Exercise 11 to the articulated manipulator of Exercis e 7 and make a 6 DOF articulated robot. Change your DR coordinate frames in the exercises accordingly and solve th e forward kinematics probl em of the robot . 13. Spherical robots.
Attach th e sph erical wrist of Exercise 11 to t he spherical manipulator of Exercise 8 and make a 6 DOF spherical rob ot . Change your DR coordinate frames in th e exercises accordin gly and solve the forward kinematic s problem of the robot.
260
5. Forward Kinematics
FIG URE 5.40. Assembled spherical wrist .
FIGURE 5.41. A 2 DOF RIIP manipulator
14. Cylindrical robots. Attach the spherical wrist of Exercise 11 to the cylindrical manipulator of Exercise 9 and make a 6 DOF cylindrical robot. Change your DR coordinate frames in the exercises accordingly and solve the forward kinematics problem of the robot. 15. An
RIIP
manipulator.
Figure 5.41 shows a 2 DOF RIIP manipulator. The end-effector of the manipulator can slide on a line and rotate about the same line. Lab el the coordinate frames installed on the links of the manipulator and determine th e transformation matrix of the end-effector to the base link.
5. Forward Kinematics
261
FIGURE 5.42. A 2R manipulator, acting in a horizontal plane. 16. Horizontal 2R manipulator Figure 5.41 illustrates a 2R planar manipulator that acts in a horizontal plane . Label the coordinate frames and determine the transformation matrix of the end-effector in the base frame. 17. SCARA manipulator. A SCARA robot can be made by attaching a 2 DOF RIIP manipulator to a 2R planar manipulator. Attach the 2 DOF RIIP manipulator of Exercise 15 to the 2R horizontal manipulator of Exercise 16 and make a SCARA manipulator. Solve the forward kinematics problem for the manipulator. 18.
* SCARA robot with a spherical wrist.
Attach the spherical wrist of Exercise 11 to the SCARA manipulator of Exercise 17 and make a 7 DOF robot. Change your DH coordinate frames in the exercises accordingly and solve the forward kinematics problem of the robot .
* Modular articulated manipulators by screws. Solve Exercise 7 by screws. 20. * Modular spherical manipulators by screws. Solve Exercise 8 by screws. 21. * Modular cylindrical manipulators by screws. Solve Exercise 9 by screws. 22. * Spherical wrist kinematics by screws.
19.
Solve Exercise 11 by screws.
262
23.
5. Forward Kinematics
* Modular SCARA manipulat or by screws. Solve Exercise 15, 16, and 17 by screws.
24.
* Space station remote man ipulato r system . Attach a spherical wrist to the SSRMS and make a 10 DOF robot . Solve the forward kinematics of the robot by matrix and screw methods.
6
Inverse Kinematics 6.1 Decoupling Technique Det ermin ati on of joint variables in terms of t he end-effector position and orientation is called inverse kinematics . Mathematically, inverse kinematics is searching for th e elements of vector q when a tr ansformation is given as a funct ion of the joint variables Ql , Q2 , Q3 , . .. .
Computer contro lled robots are usually actuat ed in the joint variable space, however objects to be manipulated are usually expressed in th e global coordinate frame. Th erefore, carrying kinematic information , back and forth, between joint space and Cartesian space, is a need in robot app licat ions. To cont rol the configuration of the end-effector to reach an object , th e inverse kinematics solut ion must be solved. Hence, we need to know what the required values of joint variables are, to reach a desired point in a desired orientation. Th e result of forward kinematics of a 6 DOF robot is a 4 x 4 tr ansformation matrix
°T6
»r , lT2 2T 3 3T 4 4n 5T 6
r
r 12
r 13
r 21 u
r 22
r 23
rr24 l'
r31
r 32
r3 3
r 34
0
0
0
1
1
(6.2)
where 12 elements are t rigonometric functions of six unknown joint variables. However, since the upper left 3 x 3 submatrix of (6.2) is a rotation matrix, only three elements of them are independent. This is because of the ort hogonality condition (2.129). Hence, only six equat ions out of the 12 equations of (6.2) are independent . Trigonometr ic functions inherently provide multip le solut ions. Therefore, mult iple configurat ions of the robot are expected when the six equat ions are solved for the unkn own joint variables. It is possible to decouple the inverse kinematics problem into two subproblems, known as inverse position and inverse orientation kinematics. Th e practical consequence of such a decoupling is the allowance to break the problem into two independent problems, each with only three unknown
264
6. Inverse Kinematics
parameters. Following the decoupling principle, the overall transformation matrix of a robot can be decomposed to a translation and a rotation
~] (6.3)
°
The translation matrix D 6 can be solved for wrist position variables, and the rotation matrix R 6 can be solved for wrist orientation variables. Proof. Most robots have a wrist made of three revolute joints with intersecting and orthogonal axes at the wrist point. Taking advantage of having a spherical wrist, we can decouple the kinematics of the wrist and manipulator by decomposing the overall forward kinematics transformation matrix °T6 into the wrist orientation and wrist position
°
(6.4) where the wrist orientation matrix is 3R 6-- 0RT3 oR6-- 0RT3
(6.5)
and the wrist position vector is
(6.6) The wrist position vector includes the manipulator joint variables only. Hence, to solve the inverse kinematics of such a robot , we must solve °T3 for position of the wrist point , and then solve 3T6 for orientation of the wrist . The components of the wrist position vector °d 6 = d., provides three equations for the three unknown manipulator joint variables . Solving d w , for manipulator joint variables, leads to calculating 3 R6 from (6.5). Then, the wrist orientation matrix 3 R6 can be solved for wrist joint variables. In case we include the tool coordinate frame in forward kinematics, the decomposition must be done according to the following equation to exclude
6. Inverse Kinematics
265
the effect of too l distance d6 from the ro bot's kinematics.
°T7
°T3 3T7 °T3 3T6 6T7
[0~3 - .[3~ ~] [: ~6 ]
(6 7)
In this case, inverse kinemat ics starts from determination of °T6 , which can be found by
°T6
°T7 6T7-
°T7
or7
1
(6.8)
[!
0 1 0 0
[~
0 1 0 0 0 0
~r
0 0 1 d6 0 1 0 0 1 0
-~ ]
• Example 163 Inv erse kin ematics of an articulated robot. Th e forward kinematics of the arti culat ed robot, illustrated in Figure 6.1, was found in Example 151, where the overall transformation matrix of the en d-effector was found, based on the wrist and arm transformation matrices.
TarmTwrist °T3 3T7
(6.9)
The wrist transformation matrix Twrist is described in (5.73) and the ma nipulator transformation matrix, T arm is found in (5.80) . However, according to a new setup coordin ate fram e, as shown in Figure 6.1, we have a 6R robot with a six links configuration
1 2 3 4 5 6
Rf-R(90) RIIR(O) Rf-R(90) Rf- R(-90) Rf-R(90) RIIR(O)
266
6. Inverse Kinematics
Wrist point
Elbow
Base FIG URE 6.1. A 6 DOF art iculated manipul ator.
and a displacement TZ,d6. Therefore, the individual links' transformation matrices are 0 sin B1 I [ CO, O 0T _ sinB 1 0 - cos B1 (6.10) 1 -
0
1
0
0 - sin 02 cos B2 0
0 0
a
a
0 0
- cos B3
1
0
a
0
a
[ cooO,
0 0
- sin 04
-1
0
a
a
[ co, O, 1T2 =
2T _ 3 -
3T 4 =
sin B2 0 0
[ 00, 0, sinB 3 0
sinB 4
a a
[ 00, 0, 4 1', _ 5 -
0 0
a
~]
I, co, 0, ] l2 sin O2 1 d2
sin B3
cos B4
sin B5 - cos 05
sin B5 0
0
1
a
0
0
0
(6.11)
1
~] ~] ~]
(6.12)
(6.13)
(6.14)
6. Inverse Kinematics
'T, ~
- sin(h
[ cooO,
Sir'
=
6T
cose 6
1
0 0
0
~' ]
0 [10 U I 0 0
7
o
0 0
0 0
1 0
~]
267
(6.15)
(6.16)
and the tool transformation matrix in the base coordinate frame is
4Ts 5T6 6T7
°Tl lT2 2T3 3T4
°T7
(6.17)
°T3 3T6 6T7
[ t2l tll
tr2
tr3
t22
t23
t" t24 ]
t3l
t32
t33
t34
0
0
0
1
where
- Ce 6 Se 4 - Ce4Ce5Se6 Ce4CCe6 - Ce5Se 4S e 6 S(hS(}6
o and tll
Cel
(c (e 2 + ( 3 ) (C(}4Ce5Ce6
- Se 4S(6) - C(}6S(}5S (e 2
+ (3) )
+ Ce5Ce6S(4) (6.20) + ( 3 ) ( -Se 4Se 6 + Ce4Ce5C(6) - Ce6Se 5S (e 2 + ( 3 )) - Ce l (Ce 4Se 6 + Ce5Ce6S(4) (6.21) S (e 2 + ( 3 ) (Ce 4Ce5Ce6 - Se 4S(6) + Ce6Se 5C (e 2 + ( 3 ) (6.22)
+ s e l (Ce 4Se 6
t2l
t3l
t 12
Sel (C (e 2
Cel (Se 5Se 6S (e 2
+ (3 ) -
C (e 2
+ ( 3 ) (Ce 6Se 4 + Ce4Ce5S(6)) (6.23)
+se l (Ce 4Ce6 - Ce5Se 4S(6) t22
Sel (Se 5Se 6S (e 2
+cel t 32
(-C(}4C(}6
- Se 5 Se 6 C ((}2
+ (3) -
C (e 2
+ ( 3 ) (C(}6Se4 + Ce4C(}5Se6))
+ C(}5S(}4S(}6)
+ (3) -
S (e 2
+ ( 3 ) (ce 6se 4 + ce 4Ce5S(6)
(6.24)
(6.25)
268
6. Inverse Kinematics
t23
+ COl (C05S (02 + 03) + C04 S05C(02 + 03)) (6.26) - COl S04S05 + SOl (C05S (02 + 03) + C04S05C (02 + 03)) (6.27)
t33
C04S05S(02 + 03) -C05C (02 + 03)
sfh s04Sfls
tl3
(6.28)
d6 (SOI S04 S05 + COl (C04 S05C(02 + 03) + C05S(02 + 03)))
h4
+l3C01 S (02 + 03) + d2s0 1 + l2C01 C02
(6.29)
d6 (- C01S04 S05 + SOl (C04 S05C(02 + 03) + C05S (02 + 03)))
t 24
+ SOI S (02 + 03) b
- d2COl + l2C02S01 d6 (C04S05S (02 + 03) - C05C (02 + 03)) +l2S02 + b c (02 + 03),
t34
(6.30) (6.31)
Solution of the inverse kinematics problem starts with the wrist position of °T7 for d6 = 0 vector d , which is [t14 t 24 t 34
f
d
=
COl (l3S (02 + 03) + l2c02) + d2S01] sOd bs (02 + 03) + l2c02) - d2COl [ b c (02 + 03) + l2s02
=
[ dx dy d,
] .
(6.32)
Theoretically, we must be able to solve Equation (6.32) for the three j oint variables 01 , O2, and 03. It can be seen that (6.33)
which provides 0 1 = 2 atan2(d x
d~
± J d~ + d~ - d~ , d2 - dy ) .
(6.34)
Equation (6.34) has two solutions for d~ + d~ > d~ , one solution for + d~ = d~ , and no real solution for d~ + d~ < d~ . Combining the first two elements of d gives l3 sin (02 + 03) = ±Jd~
+ d~ - d~ -l2 cos O2
(6.35)
then, the third element of d may be utilized to find l5 =
(±Jd~ + d~ - d~ -l2 cos O2) 2 + (dz -l2 sin 02)2
(6.36)
which can be rearranged to the following form a cos O2
+ bsin O2 =
(6.37)
c
a
2l2Jd~ + d~ - d~
b
2l2d z
(6.39)
c
d; + d; + d; - d~ + l~ -
(6.40)
(6.38)
l5.
6. Inverse Kinemat ics
with two solutions
Cg 2
atan2(- , ± r
1 - 2') - atan2(a, b) r
atan2 (-~ , ±Jr2 r
a
2
-
c2 )
-
269
(6.41)
atan2(a, b)
(6.42)
+ b2 .
(6.43)
Summing the squares of the eleme nts of d gives
that provides (6.45) Having 01 , O2 , and 03 m eans we can find the wrist point in space. However, because th e j oin t variables in °T3 and in 3T 6 are independent, we should fin d the orientation of the end-effector by solving 3T 6 or 3 R 6 for 04 , Os, and 06 . C04COSC0 6 - S04S0 6
-C06 S0 4 - C0 4COSS06
C0480S ]
+ C0 4 S0 6
C04CC06 - COSS 04S06
S04 8 0S
SOS S06
cOs
COSC0 6S 04 [
[
- C06 S0S S l1 S 21
S12 S 22
S 13] S23
s3 1
832
8 33
(6.46)
Th e angles 04 , Os, and 86 can be f ound by examining eleme n ts of 3 R 6 (6.47)
8s = atan2 ( 06
JSi3 + S~3 '
= atan2 ( S32 ' -
Example 164 Solution of equation a cos8 Th e fir st t ype of trigonometric equati on
(6.48)
S33 )
(6.49)
s 3d .
+ bsin8 =
c.
a cos 0 + b sin 0 = c
(6.50)
can be solved by in troducin g two n ew varia bles rand ¢ such that a
r sin o
(6.51)
b
r cos ¢
(6.52)
270
6. Inverse Kinematics
and
Va
r
2
+ b2
(6.53) (6.54)
atan2(a, b) . Substituting the new variables show that
sin(¢ + B)
(6.55)
cos(¢ + B)
(6.56)
Hence , the solutions of the problem are
Cg2
B = atan2( -, ±
1 - 2') - atan2(a, b)
r
and
(6.57)
r
2 - c2 ) - atan2(a, b). B = atan2C~, (6.58) r Therefore, the equation a cos B + b sin B = c has two solutions if r 2 a 2 + b2 > c2, one solution if r 2 = c2, and no solution if r 2 < c2.
±Vr
Example 165 Meaning of the function tan 21 ;; = atan2(y, x). In robotic calculation, specially in solving inverse kinematics problems, we need to find an angle based on the sin and cos functions of the angle. However, tan " ! cannot show the effect of the individual sign for the numerator and denominator. It always represents an angle in the first or fourth quadrant. To overcome this problem and determine the joint angles in the correct quadrant, the atan2 function is introduced. atan2(y,x)
t an
-1
-Y x
tan -1 JL x IT
•
2' sign x
+ IT sign y
if
y>O
if
y
if
y=O
(6.59)
In this text, whether it has been mentioned or not, wherever tan- 1 ;; is used, it must be calculated based on atan2(y, x).
6.2
Inverse Transformation Technique
Assume we have the transformation matrix °T6 indicating the global position and the orientation of the end-effector of a 6 DOF robot in the base frame B«. Furthermore, assume the geometry and individual transformation matrices °T1(Ql), lT2(Q2), 2T3(q3), 3T4(q4), 4Ts(Qs), and ST6(q6) are given as functions of joint variables.
6. Inverse Kinematics
271
According to forward kinematics, °T6
=
(6.60)
°TI IT2 2T3 3T4 4T5 5T 6
~:: ~:: ~:: ~:: ] .
l
000
1
We can solve the inverse kinematics problem by solving the following equations for the unknown joint variables: IT6 2T 6 3T 6 4T6 5T 6
I
°TI- I °T6 IT I °TI- I 22T - I IT I 23 3T - 1 2T I 4 34T - I 3T I 45 5T I 4T I 56-
(6.61) (6.62)
°T6 °T I IIT I 22T I 33T - I 4
°T6
(6.63)
°T - I °T6 I IT I °T I °T6 2I2T - I IT I °T I °T6 3 2I-
(6.64)
P roof. We multiply both sides of the transformation matrix to obtain °T - I OT6 I
=
°T6
(6.65) (6.66)
by °TI- I
°T - I (OT I IT2 2T3 3T 4 4T5 5T 6 ) I
(6.67)
I T6 •
Not e that °TI- I is the mathematical inverse of the 4 x 4 matrix °TI , and not an inverse transform ation. So, °TI- I must be calculated by a math ematical matrix inversion. The left-hand side of th e Equ ation (6.67) is a funct ion of ql . However, th e element s of th e matrix In on th e right-hand side are eith er zero, constant , or functions of q2, q3, q4, q5, and q6. The zero or constant elements of the right-hand side provid es the requir ed algebraic equat ion to be solved for ql ·
Then, we multiply both sides of (6.67) by IT2-10TI-IOT6
IT2- I
to obtain
I T ; I 0TI- I (OTIIT2 2T3 3T44T5 5T6) 2T6 .
(6.68)
The left-hand side of the this equation is a function of q2, while th e elements of th e matrix 2T 6 , on th e right hand side, are eit her zero, constant , or functions of q3, qs , Q5 , and q6. Equ ating the associated element , with constant or zero elements on th e right-hand side, provides th e required algebraic equat ion to be solved for qzFollowing this pro cedure, we can find th e joint variables q3, q4, Q5, and q6 by using the following equalit ies respectively. 2T - I IT I °T - I °T6 23 1 = 2T 3- I IT2- I ° T I- I (OTI IT2 2T 3 3T 4 4T 5 5T 6 ) = 3T 6 .
(6.69)
272
6. Inverse Kinemat ics
X6
-
"'6
, ...... '
t
F IGURE 6.2. A spherical robot, made of a spherical manipulato r attached to a spherical wrist .
On
3T4-I2T3-I IT I 0T - I 2I 3T - I 2T 1 I T I °T - I (OTI IT2 2T3 3 T 4 4Ts ST6 ) 4 32I 4T6 .
= =
(6.70)
4T I 3T - I 2T I IT I °T - I °T 6 4 32I s4T - I 3T - I 2T - I IT I °T 1 (OTI I T 2 2T3 3T 4 4Ts ST 6 ) 4 2Is 3
= = sT6 .
sT - I 4T - I 3T I 2T - I IT;I 0T - I 0 T 6 46 s 3 I = s T 6- I 4Ts- I 3T4- I 2T3- I IT2- I °TI- I (OTI IT2 2T3 3 T 4 4Ts ST6)
(6.71)
(6.72)
= I.
T he inverse transformation techni que may sometimes be called Pieper technique. • Example 166 Inverse kinematics for' a spherical robot. Transformation ma tri ces of the spherica l robot shown in Figur e 6.2 are
o o -1
o
S02 - C0 2
o o
273
6. Inverse Kinematics
2T 3 =
[1 00 010 00 1 000
{3]
[,8 0 58 5
4T5 =
~]
5
sB5 0 - cB 5 o 1 0 o 0 0
3T,
~
5T,
~
[~,
0 0 -1 0
S~4
[
~6 S~6
-.,8, 0]
- sB6 cB6 0 0
o o
0 0 1
o
0]
o
1
cB4
0 1 0 (6.73)
Therefore, the position and orientation of the end-effector for a set of joint variables, which solves the forward kinematics problem, can be found by matrix multiplication °T6
= °T1 1T2 2T3 3T4 4T5 5T6
[
~~~ ~~~ ~~: r34 ~~: ]
r31 r32 r33 000
(6.74)
1
where the elements of °T 6 are the same as the elements of the matrix in Equation (5.101). Multiplying both sides of the (6.74) by °T 1- 1 provides
n
°T1- 1O
[
COO 8
1
- 5~81
[ In 121 131 0
h2 122 132 0
sin B1
0 cos B1
0
0 -1 0 0
h3 123 s; 124 133 134 0 1
oo ][ Til r2j o 1
r31 0
r12 r22 r32 0
]
r13 r23 r33 0
T14 r24 ]
r34 1
(6.75)
where r1i cos B1 + r-u sin Bj -r3i r2i cos B1 - rli sin Bj 1,2 , 3, 4.
hi
hi hi
(6.76) (6.77) (6.78)
Based on the given tmnsformation matrices , we find that
In
=
1T2 2T:1 3T4 4T5 5T6 h2 h3 [ 1n 121 122 f23 114 124 131 132 133 134 1 0 0 0
]
(6.79)
274
6. Inverse Kinemati cs
-dhsB4sfh + cB6 (-sB 2sB s + cB2 cB4cBs ) -sB2sB4sB6 + cB6 (cB 2sB s + cB4 cBs sB2 ) cB 4 sB6 +cB scB6sB4
(6.80)
-cB2cB6sB4 - sB6 (-sB 2sB s + cB 2cB4cB s) -cB 6sB 2sB4 - sB6 (cB 2sB s + cB 4 cB ssB2 ) cB4 cB6 - cB ssB4sB6
(6.83)
cB ssB2 +cB 2cB4 sBs -cB 2cB s + cB 4sB2sB s sB4sB s
(6.86)
(6.81) (6.82)
(6.84) (6.85)
(6.87) (6.88) (6.89) (6.90) (6.91)
The only constant element of the matrix (6.79) is f34
=
12, therefore, (6.92)
This kind of trigonometric equation frequently appears in robotic inverse kinematics, which has a systematic method of solution. We assume r cos ¢
(6.93)
r sin e
(6.94)
where (6.95) (6.96)
and therefore, Equation (6.92) becomes 12
sin ¢ cos B1 sin( ¢ -
-
cos ¢ sin B1
Bd
(6.97) (6.98)
showing that
±J1 - (l2 / r)2
= cos(¢ -
Bd·
(6.99)
Hence, the solution of Equation (6.92) for B1 is (6.100)
6. Inverse Kinematic s
275
FIGURE 6.3. Left shoulder configuration of a spherica l robot .
The (-) sign corresponds to a left shoulder configuration of the robot, as shown in Figure 6.3, and th e (+) sign corresponds to the right shoulder configuration. Th e eleme nts f14 and 124 of matrix (6.79) are functions of 82 and 82 only .
d 3 sin 8 2
= r14
- d3 cos 82
=
cos 8 1 + r 24 sin 8 1
(6.101)
(6.102)
- r 34
Hence, it is possible to use them and find 82
(6.103) where 81 must be substituted from (6.100). In the next step, we find the third joint variable d 3 from 1r.-1 0T-1 0'7' _ 2 1 16 -
2'7' 16
(6.104)
where (6.105)
276
6. Inverse Kinematics
and
2T
o
=
- S84Sfh + c84c8 sc 80 c84s8 0 + c8 sc8 0s8 4 - cOOsOS
- c80 s84 - c84c8ss8 0 c8 4c80 - c8 Ss 04s0 0 sOS sOO
c8 4s8 s S04S0S cOs
o
0 0 d3
0
0
1
l
j .
(6.106) Employing the elements of the matrices on both sides of Equation (6.104) shows that the element (3,4) can be utilized to find d 3. (6.107) Since there is no other element in Equation (6.104) to be a function of another single variab le, we move to the next step and evaluate 84 from 3T- 1 2r. - 1 1r. - 1 0T-1 4
3
2
1
0'7' _
4'7'
.io -
(6.108)
.io
because 2T3- 1 1T2- 1 °T1- 1 °To = 3To provides no new equation. Evaluating 4To
4r.
s
=
l
cos 8s cos 80 cos 80 sin Os sin 8 0
~]
cos 8s sin 8 0 - sin Os sin 8 0 cos 8 0
-
o
0
(6.109)
and the left -han d side of (6.108) utilizing
1 0
2r. - 1 _ 3
-
l
o o o
1
(6.110)
0 0
and
[ cosO,
- s~O'
3T- 1 _ 4
-
shows that
3T- 1 2r. - 1 1r. - 1 0T- 1 or.6-432
1
sin 84 0 cos 84 0
l
g21 gn g31 0
g12 g22 g32 0
~]
(6.111)
g13 g23 gn g24 ] g33 g34 I 0
(6.112)
0 -1 0 0
6. Inverse Kinematics
277
where -r3iC(}4s(}2
+ ru (C(}lS(}4 + C(}2 C(}4 S(}d
+rli (-S(}lS(}4
(6.113) (6.114)
+ C(}lC(}2 C(}4)
d 364i - r 3l C(}2 r 3lS(}2S(}4 +r2l
rU C(} l S(}2 - r2l S(}l S(}2 (C(}lC(}4 -C(}2 S(}lS(}4)
(6.115)
+rU (- C(}4S(}1 - C(}lC(}2S(}4)
1,2 ,3 ,4 . The symbol 64i indicates th e Kronecker delta and is equal to
=1 =0
64i
if if
i i
=4
(6.116)
# 4.
Th erefore, we can find (}4 by equating the elem ent (3,3), (}5 by equating the eleme nts (1,3) or (2, 3), and (}6 by equating th e elements (3,1) or (3, 2). Starting from elem ent (3,3) r1 3
(-C(}4S(}1 - C(}lC(}2 S(}4) + r2 3 (C(}l C(}4 - C(}2S(}l S(}4) + r 33s(}2s(}4
=0 (6.117)
we find (}4 ()4 = tan
-1
-r13S(}1 + r23 C(}1 C(}2 (r13c(}1 + r23s(}d - r 33s(}2
(6.118)
which , based on th e second value of (}l, can also be equal to () 4
Jr
= - + tan
-1
2
C(}2
-r13S(}1 + r2 3C(}1 (r13c(}1 + r2 3s(}j) - r3 3s(}2
(6.119)
Now we use elem ents (1,3) and (2, 3), rn
(cos (}l sin (}4 + cos (}2 cos (}4 sin (}d
+r1 3
-r33 cos (}2
to find
-
r 33 cos (}4
(COS(}lCOS (}2 COS(}4 - sin(}l sin(}4) - r 13 cos (}l sin (}2 - r n sin (} l sin (}2
sin (}2
(6.120) (6.121)
(}5
()5 -_ t an -1 sin (}5 () . cos 5 Finally, (}6 can be found from th e eleme nts (3,1) and (3,2) sin (}6
=
r 3l
sin (}2 sin (}4 + r2l (cos (} l cos (}4 - cos (}2 sin (}j sin (}4)
+ru (- cos (}4 sin (}l - cos (}l cos (}2 sin (}<1) cos (}6
(6.122)
r 32 sin (}2 sin +r12 (-
(6.123)
()4 + r2 2 (cos (}l cos ()4 - cos (}2 sin (}l sin ()4)
cos (}4 sin (}l - cos (}l cos (}2 sin (}4) ()6
_ -
t an
-1 sin (}6 () . cos 6
(6.124) (6.125)
278
6. Inverse Kinematics
Ex ample 167 Inverse of Euler angles transformation matrix. The global rotation matri x based on Euler angles has been found in Equation (2.64).
[Az,,p Ax,o AZ,'Pf Rz,'P Rx,o RZ,1/J crpC'lj; - c() srps'ljJ c'ljJsrp + c()crps'ljJ [ s()s'ljJ
- crps'ljJ - C()c'ljJSrp - Srps'ljJ + C()CrpC'lj; S()C'lj;
s()srp ] -CrpS() C()
rI2 r13 ] r2I r22 r23 [ r3I r32 r33 ru
Premultiplying
G RB
[
[
(6.126)
by R Z,I'P ' gives
COSrp -sinrp
sin rp cos e
o
0
O~]
G RB
rn cip + r2Isip rI2crp + r22srp r2Iop - rn sip r22Crp - rI2Srp
r3I
co~~ss~ 'ljJ
[ sin ()sin 'ljJ
.;
z:
r I3crp + r23srp ] r 23Crp - rI 3srp
r32 'ljJ
sin ()cos 'ljJ
r33 -
s~n
() ] . cos ()
(6.127)
Equating the elements (1,3) of both sides rI3 cos rp + r23 sin rp = 0
(6.128)
gives (6.129)
Having rp helps us to find 'ljJ by using elements (1,1) and (1,2) cos 'ljJ
rn cos rp + r21 sin rp
(6.130)
-sin 'ljJ
rI2 cos sp + r22 sin rp
(6.131)
-r12 cos rp - r22 sin sp . . r n cos sp + r21 sm sp
(6.132)
therefore, 'ljJ = atan2
6. Inverse Kinematics
In the next step , we may postmultiply
G RB
279
by Rz,~, to provide
(6.133)
The elements (1,1) on both sid es make an equation to find 'ljJ . r3l cos 'ljJ - r3l sin 'ljJ
=0
(6.134)
Th erefore, it is possible to find 'ljJ from the following equation:
(6.135) Finally, () can be found using elem ents (3,2) and (3,3)
which give () = atan 2
sin ()
(6.136)
cos()
(6.137)
r32 cos 'IjJ + r3l sin 'IjJ . r33
(6.138)
Example 168 Invers e kin ematics for a 2R planar manipulator. Figure 5.9 illustrates a 2R planar manipulator with two RIIR links according to the coordinate frames setup shown in the figure . T he forward kinematics of the manipulator was found to be °T2 =
»r,
lT2
[ ,(0.+0,) S((}1 +(}2)
0 0
(6.139)
- s ((}l + (}2) 0 c ((}l + (}2) 0 0
0 1 0
l'eB' + 12 C(0' +O'l ]
h S(}l + l2s ((}l +
o
(}2)
.
1
T he inverse kin ematics of planar robots are generally easier to find analytically. In this case, we can see that the position of the tip point of th e manipulator is at
(6.140)
280
6. Inverse Kinematics
(a)
(b)
FIG URE 6.4. Illustrati on of a 2R planar manipulat or in two possible configuration s: (a) elbow up and (b) elbow down.
therefore (6.141) and
X 2 + y2 - li - l~ cos(/z = l l -2 1 2
(6.142)
How ever, we should avoid using arcsin and arccos because of the inaccuracy. So , we em ploy the half angle formula
20
t an 2
I-cosO
= -----,-
(6.143)
(h + l2)2 - (X + y) 2 (X2 + y 2) - (li + l~) .
(6.144)
1 + cos O
to find O2 using an atan2 fun ction
O2 = ±2 at an2
Th e ± is because of the square root, which generates two soluti ons. Th ese two soluti ons are called elbow up and elbow down , as shown in Figure 6.4. Th e first joint variable 0 1 can be found from (6.145) Th e two different solution s fo r 0 1 correspond to the elbow up and elbow down configurations.
6. Inverse Kinematics
281
*
Example 169 Inverse kinematics and nonstandard DH frames. Consider a 3 DOF planar manipulator shown in Figure 5.4. The nonstandard DH transformation matrices of the manipulator are COS B1
0T _
sinB 1
(6.146)
0
1 [
o - sinB z cosB z 0 0
0 0 1 0
l~
]
(6.147)
0 0
- sinB 3 cosB 3 0 0
0 0 1 0
[~
~]
(6.148)
0 0 1 0 0 1 0 0
2
IT z =
[ 0089 sinB
z
0 0 3
zT3 =
[ c009 sinB
3
3TF
lo~ ] .
(6.149)
1
Solution of the inverse kinematics problem is a mathematical problem and none of the standard or nonstandard DH methods for defining link frames provide any simplicity. To calculate the inverse kinematics, we start with calculating the forward kinematics transformation matrix °T4 (6.150)
i. cos B1 + lz cos BIZ + b cos B1Z3 ] h sin B1 + lz sin BIZ + 13 sin BI23
o 1
r13 rZ3 r33
o
r14 ] rZ4 r34 1
where we used the following short notation to simplify the equation. (6.151)
Examining the matrix °T4 indicates that (6.152)
282
6. Inverse Kinematics
The next equation
shows that
O2
-
arccos
if + J? - lr - l§ 2h l2
(6.154)
01 = atan2 (hh - Id4 , hh + 12/4) where l3r ll COl (l2C02 + h) - SOl (l2S02) COd3 - SOd 4 r14 -
r24 - b r 2l SOl (l2C02 + ll)
12
SOd3
+ COl (l2S02)
(6.155)
(6.156)
+ COd 4.
Finally, the angle fh is (6.157)
6.3
Iterative Technique
The inverse kinematics probl em can be interpreted as searching for the solution qk of a set of nonlinear algebraic equations »t;
T(q) »r, (qd lT2(q2) 2T3(q3 ) 3T4(q4 ) . . . n-lTn (qn)
(6.158)
~~~r si r32 ~~~ ~~: ~~: ] r33 r34
r 000
1
or k
= 1,2"
, ·n.
(6.159)
where n is the numb er of DOF . However, maximum m 6 out of 12 equations of (6.158) are independent and can be utilized to solve for joint
6. Inverse Kinematics
283
variables qk. The function s T (q) are transcendent al, which are given explicitly based on forward kinemati c analysis. Numerous methods are available to find th e zeros of Equ ation (6.158). However , the methods are , in general, iterative. T he most common method is known as th e N ewton -Raph son m ethod. In th e iterative techniqu e, to solve t he kinemati c equations
'I' (q) = 0
(6.160)
for variables q , we st ar t with an initial guess q*
= q +oq
(6.161)
for the joint variables. Using the forward kinematics, we can determine th e configur at ion of the end-effecto r frame for t he guessed joint variables.
T* = T (q*)
(6.162)
The difference between t he configuration calculated with th e forward kinemati cs and the desired configura tion represents an error, called residue, which must be minimized.
oT= T - T *
(6.163)
A first order Taylor expansion of th e set of equat ions is T
T(q*
+ oq)
T(q*)
+
8T
8q oq + O( oq2).
(6.164) (6.165)
Assuming oq << I allows us to work with a set of linear equations
oT
=
J oq
(6.166)
where J is the Jacobian matri x of t he set of equat ions
8T J(q) = [ i]
(6.167)
oq = J- 1 oT.
(6.168)
8 qj
t hat implies Therefore, th e unknown variables q are
q =q*+J-1oT.
(6.169)
We may use the values obt ained by (6.169) as a new approximation to rep eat the calculat ions and find newer values. Rep eating the methods can
284
6. Inverse Kinematics
be summarized in th e following iterative equat ion to converge to the exact value of the variables. (6.170)
This iter ation technique can be set in an algorithm for easier numerical calculat ions. Algorithm 6.1. Inverse kinematics iteration technique. 1. Set the initial count er i = O.
2. Find or guess an initial estimate 3. Calculate the residue 8T(q(i))
q(O) .
= J(q(i))8q(i).
If ever y elem ent of T(q(i)) or its norm IIT(q(i))11 is less than a tolerance, IIT(q(i)) II < f then term inate the it eration . Th e q(i) is the desired solution.
5. Set i
= i + 1 and return to step 3.
The toleran ce f can equivalently be set up on variabl es (6.171)
or on Jacobian J - 1<
f.
(6.172)
Example 170 Inv erse kin ematics for a 2R planar manipulator. In Example 168 we have seen that the tip point of a 2R planar manipulator can be described by (6.173)
To solve the inverse kin ematics of the manipulator and find the joint coordinates for a kno wn position of the tip point, we define
:~
]
(6.174)
T=[~]
(6.175)
q
= [
6. Inverse Kinematics
285
therefore, the Jacobian of the equations is
=
J(q)
Th e inverse of the Jacobian is
and therefore, the iterative formula (6.170) is set up as
[88 ] 1 2
(HI )
=
[8] 1
(i)
82
+ J-1
(6.178)
Let 's assum e
and start from a guess value q(O)
= [ 81
] (0)
= [ 7r/ 3 ]
82
-7r/3
for which
8T
1 ] _ [ cos7r/3 [ 1 sin 7r/3
+ cos (7r/3 + -7r/3) + sin (7r/ 3 + -7r/ 3)
[ ~ ] [ ~~ ] [-!A+1] ' -
Th e Jacobian and its inverse for these values are
]
286
6. Inverse Kinematics
and theref ore, [
(0)
:~
+ J - 1 8T
]
7f/3 ] [_1J3 0 ] [ - .! ] [ - 7f/3 + J3 1 - ! A+ 1 1.6245 ] [ - 1.779 2 . Ba sed on the iterative techniqu e, we can find the following values and find the solution in a f ew iterations. Iteration 1.
J --
q
2 2
= [
8T
(1)
0] 1
_.!2 J3
[
_!
A
+1
]
= [ 1.6245 ] - 1.779 2
Iteration 2.
J = [ -0.844 0.154 ] 0.934 0.988 8T
=
q
[
6.516 X 100.15553
(2) = [
2 ]
1.583 ] - 1.582
It eration 3.
J = [ - 1.00 - .433 x 10.988 .999 8T
= [ .119 X 10- .362
1
10- 3
]
-1.570867014
4. J 8T (4)
q
-
[ - 1.000 0.0 ] 0.998 50 1.0
= [ - .438 X 10-4
= [7f/2 - 7f/2
f.
6 ]
.711 X 10-
= [ 1.570796329 ] - 1.570796329
The result of the fourth iteration q
]
= [ 1.570795886 ]
(3)
q Iteration
X
3
q (4)
is close enough to the exact value
6. Inverse Kinematics
6.4 6.4.1
*Techniques Comparison of the Inverse Kinematics
287
* Existence and Uniqueness of Solution
It is clear th at when the desired tool fram e position °d 7 is out side the workin g space of t he robot , there can not be any real soluti on for the joint vari abl es of th e robot . In this condition, t he overall resultant of the term s under squ are root signs would be negative. Furthermore, even when the tool frame position °d 7 is within t he working space, t here may be some tool orientat ions °R7 t hat are not achievable with out breaking joint constraints and violating one or more joint variable limit s. Therefore, existing solut ions for inverse kinemat ics probl em genera lly depends on the geometric configuration of t he robot. The norm al case is when t he numb er of joints is six. Then, provided that no DOF is redundant and t he configuration assigned to the end-effectors of t he robo t lies within t he workspace, t he inverse kinemati cs solution exists in finite numbers. The different solut ions corr espond to possible configurations to reach the same end-effector configur ation . Generally spea king, when t he solution of t he inverse kinematics of a robot exists, t hey are not unique. Multipl e solut ions appea r because a robot can reach to a point within th e workin g space in different configurat ions. Every set of solut ions is associated to a par ti cula r configuration. Th e elbow-up and elbow-down configuration of t he 2R manipulat or in Exampl e 168 is a simple exam ple. The multipli city of t he solut ion depends on th e numb er of joints of t he manipulator and t heir type. The fact th at a manipulat or has multipl e soluti ons may cause problems since the system has to be able to select one of them. T he crite ria on which to base a decision may vary, but a very reason abl e choice consists of choosing t he closest solut ion to t he current configuration. When t he numb er of joints is less t han six, no soluti on exists unless freedom is reduced in the same t ime in t he task space, for example, by constraining th e to ol orientation to certain directions. When th e numb er of joints exceeds six, the st ruct ure becomes redunda nt and an infinite number of solut ions exists to reach t he same end-effector configur ation within t he robo t workspace. Redundan cy of t he robot architecture is an interesti ng feature for syste ms installed in a highly const rai ned environment . From t he kinema ti c point of view, t he difficulty lies in formulating t he environment const raints in mathematical form, to ensure the uniqueness of t he solut ion to t he inverse kinem at ic probl em.
288
6. Inverse Kinematics
6.4.2
* Invers e Kinematics Techniques
Th e inverse kinematics problem of robots can be solved by several methods, such as decoupling, inv erse transformation, iterative , screw algebra, dual matrices, dual quaternions, and geometric techniques. The decoupling and inverse transform technique using 4 x 4 homogeneous tr ansformation matrices suffers from th e fact that th e solut ion does not clearly indicat e how to select the appropriate solution from multiple possible solutions for a particular configuration. Thus, th ese techniques rely on the skills and intu ition of the engineer. Th e iterative solution method often requires a vast amount of computation and moreover, it does not guarant ee convergence to th e correct solution . It is especially weak when the robot is close to the singular and degenerate configurations. The iterative solut ion method also lacks a met hod for selecting th e appropriate solution from mult iple possible solutions. Although the set of nonlinear tr igonomet ric equat ions is typically not possible to be solved ana lytically, there are some robot structures t hat are solvable analytically. Th e sufficient condition of solvability is when th e 6 DOF robot has three consecutive revolute joints with axes intersecting in one point. The oth er property of inverse kinemati cs is ambiguity of a solution in singular point s. However, when closed-form solutions to th e arm equat ion can be found, they are seldom unique.
*
Example 171 It eration technique and n-m relationship. 1- Iteration m ethod when n = m . When the number of joint vari ables n is equal to the nu m ber of independent equations generated in fo rward kin ematics m , then provided that the Ja cobian matrix rema ins non singular, the lin earized equation
JT = J Jq
(6.179)
has a unique set of soluti ons and therefore, the Newton-Raphson technique may be util ized to solve the inverse kinematics problem . Th e cost of the procedure depends on the number of iterations to be perform ed, which depends upon different parameters such as the distance between the estim ated and effec tive solutions , and the conditi on number of the Jacobian matrix at the solution. Since the solution to the in verse kinematics problem is not uniqu e, it may generate different configuration s according to the choice of the estim ated solution. No convergence may be observed if the ini tial estim ate of the soluti on falls outside the convergence domain of the algorithm. 2- It eration m ethod when n > m . When the number of jo int variables n is m ore than the numb er of independ ent equations m , then the problem is an overdetermin ed case for which no soluti on exists in gen eral because the number of j oints is not enough to generate an arbitrary configurati on for the en d-effect or. A soluti on can be generated, which minimizes the position error.
6. Inverse Kinematics
289
3- It eration m ethod when n < m. When the numb er of jo int variables n is less than the number of independ ent equations m, then the problem is a redundant case for which an infini te number of soluti ons are generally available.
6.5
* Singular Configuration
Generally speaking, for any robot , redundant or not , it is possible to discover some configurations, called singular configurati ons, in which the number of DOF of t he end-effector is inferior to the dimension in which it generally operat es. Singular configurat ions happ en when: 1. Two axes of pr ismat ic joints become par allel
2. Two axes of revolute joints become identical. At singular positions, t he end-effector loses one or more degrees of freedom , since t he kinematic equat ions become linearly dependent or certain solutions become und efined. Singular positions must be avoided as the velociti es required to move the end-effector become t heoretically infinite. The singular configurations can be det ermined from the J acobian matrix. T he J acobian matrix J relat es t he infinitesimal displacements of t he endeffecto r (6.180) to the infinitesimal joint var iables (6.181)
and has t hus dimension m x n , where n is t he number of joints, and m is t he number of end-effector DOF. Wh en n is larger than m and J has full ra nk, then t here are m - n redundan cies in the syste m to which m - n arbit rary vari ables correspond. The J acobian matrix J also det ermines the relationship between endeffector velocities X and joint velocities q
X=Jq.
(6.182)
T his equation can be int erpreted as a linear mapping from an m-dim ensional vecto r space X to an n-dimensional vector space q . The subspace IR(J) is the range space of the linear mapping, and represents all t he possible endeffector velocities that can be genera ted by the n joints in t he current configuration. J has full row-rank, which means t hat t he system does not present any singularity in that configurat ion, then t he range space IR (J) covers t he entire vector space X . Otherwise, there exist s at least one dire ction in which the end-effecto r cannot be moved.
290
6. Inverse Kinematics
The null space N(J) represents the solutions of J it = O. Therefore, any vector it EN(J) does not generate any motion for the end-effector. If the manipulator has full rank, the dimension of the null space is then equal to the number m - n of redundant DOF . When J is degenerate, the dimension of lR(J) decreases and the dimension of the null space increases by the same amount. Therefore, dimlR(J)
+ dimN(J) = n.
(6.183)
Configurations in which the Jacobian no longer has full rank, corresponds to singularities of the robot, which are generally of two types: 1. Workspace boundary singularities are those occurring when the manipulator is fully stretched out or folded back on itself. In this case, the end effector is near or at the workspace boundary. 2. Workspace interior singularities are those occurring away from the boundary. In this case, generally two or more axes line up .
Mathematically, singularity configurations can be found by calculating the conditions that make
131 = 0
(6.184)
or (6.185)
Identification and avoidance of singularity configurations are very important in robotics. Some of the main reasons are: 1. Certain directions of motion may be unattainable. 2. Some of the joint velocities are infinite. 3. Some of the joint torques are infinite. 4. There will not exist a unique solution to the inverse kinematics problem. Detecting the singular configurations using the Jacobian determinant may be a tedious task for complex robots. However, for robots having a spherical wrist, it is possible to split the singularity detection problem into two separate problems : 1. Arm singularities resulting from the motion of the manipulator arms.
2. Wrist singularities resulting from the motion of the wrist.
6. Inverse Kinematics
6.6
291
Summary
Inverse kinematics refers to det ermining the joint vari ables of a robot for a given position and orient ation of the end-effector frame. The forward kinem at ics of a 6 DOF robo t generates a 4 x 4 t rans format ion matrix
°T6
°T1 IT2 2T33T4 4T5 5T6 r1 2 [
O~6
°d 6 1
]
=
[ "11 r21 r 31
0
r13
r22 r23 r32 r33 0 0
"r24 14 ]
r34
(6.186)
1
where only six element s out of the 12 elements of °T6 are independent. Therefore, the inverse kinematics reduces to finding the six independent elements for a given °T6 matrix. Decoupling, inverse t ransforma tion, and iterative te chniqu es are three applied methods for solving the inverse kinematics problem. In decoupling technique, th e inverse kinematics of a robo t with a spherical wrist can be decoupled into two subproblems: inverse position and inverse orient ation kinem atics . Practically, the tools transformation matrix °T7 is decomposed into three submatrices °T3, 3T6, and 6T7 . (6.187) The matrix °T3 positions t he wrist point and depends on the three manip ulator joints ' variables. The matrix 3T6 is the wrist transformation matrix and the 6T7 is the tools transformation matrix. In inverse transformation technique , we extract equat ions with only one unknown from the following matrix equations, step by ste p. (6.188) (6.189) (6.190) (6.191) (6.192) (6.193) The iterat ive technique is a numerical method seeking to find th e joint variable vector q for a set of equations T (q ) = O.
6. Inverse Kinematics
293
Exercises 1. Notation and symbols.
Describe t he meanin g of a- atan2 (a, b) 2.
b- »t;
c- T (q)
d- w
e- q
f- J .
3R planar manipulator inverse kinemat ics. Figure 5.21 illustrates an RIIRIIR planar manipulator.
Th e forward kinemati cs of t he manipulator generates th e following matri ces. Solve the inverse kinematics and find (h , (ho B3 . 0 0 1 0
l 3 cos B3
2T 3
3 - sinB3 [ 0058 sin B3 cos B3 = 0 0 0 0 - sin B2 cos B2 0 0
0 0 1 0
b cos B2
IT 2
[ c058, = sin B2 0 0
1 - sinBI [ c008 0T _ sin BI cos BI I0 0 0 0
b sin B3 0 1 l 2 sin B2
0 1
0 h cos BI 0 ll sinB I 1 0 1 0
j j j
3. Spherical wrist inverse kinematics. Figure 5.22 illustrat es a schematic of a spherical wrist with following tra nsformation matrices. Assume that th e frame B 3 is th e base frame. Solve the inverse kinemat ics and find 84 , B5 , 86 .
3T 4
4T 5
5T,
[ 0058, sin B4 = 0 0
- sin B4 cosB4 0 0
0 0 -1 0
sin B5 0 [ cos 85 cos B5 sin B 0 5 = 0 1 0 0 0 0
~
6 - sin B6 [ 0058 86 cos B6
81
0 0
0
0 1 0
~j ~j
n
294
6. Inverse Kinematics
4. SCARA robot inverse kinematics. Consider the R IIRIIRIIP robot shown in Figure 5.27 with the following forward kinematics solution. Solve the inverse kinematics and find 8 1 ,
82 , 83 , d.
cooO, sin 8 1 0 0
»r, =
lT2
l l l
COO02
sin 8 2 0 0
=
2T3
- sin8 l cos 8 1 0 0
0 II COS 0 1 0 h sin8 l 1 0 1 0
- sin 8 2 cos 82 0 0
0 0 1 0
l2
- sin 83 cos 83 0 0
0 0 1 0
0 0 1 0 0 1 0 0
~]
COO03
sin 83 0 0
=
3T4
=
l~
l2
cos 82 sin 82 0 1
]
]
~]
»r, lT2 2T3 3T 4 -S8 l 23 C8 123 0 0
[ cOn, S8l 23 0 0 8 1 + 82
0 0 1 0
i.», + I,di u». + hs8
n ] 12
d 1
+ 83
8 1 + 82 5. m-R IIR articulated arm inverse kinematics. Figure 5.25 illust ra t es 3 DOF m-R IIR manipulator. Use the following transformation matrices and solve the inverse kinematics for 8 1 , 8 2 ,
83 . 0TI
IT,
~
~
[ 0000
1
SiYI
Sir'
[ cooO,
0 0 - 1 0
- sin8 l cos8 l 0 0
- sin8 2 cos 8 2 0 0
~I ]
0 I, cos 0, ] 0 l 2 sin 82 1 d2 0 1
6. Inverse Kinematics
295
FIGURE 6.5. A PRRR manipulator.
o o
~]
1
o 6. Kinematics of a PRRR manipulator.
A PRRR manipulator is shown in Figure 6.5. Set up the links's coordinate frame according to standard DR rules . Determine the class of each link. Find the links' transformation matrices. Calculate the forward kinematics of the manipulator. Solve the inverse kinematics problem for the manipulator. 7.
* Space station remote manipulator system inverse kinematics . Shuttle remote manipulator system (SRMS) is shown in Figure 5.28 schematically. The forward kinematics of the robot provides the following transformation matrices. Solve the inverse kinematics for the SRMS.
o o -1
o o o -1
o
- sinOz cos Oz
o o
296
6. Inverse Kinematics
' T,
~
s;r'
[ co, 0,
[ coo O,
,;~o,
'T, =
0 0 - sin() 4 cos ()4
0 0
[ 00,0, 0
'n = 5T,
~
0 a, co, 0, ] 0 a 3 sin ()3 1 d3 1 0
- sin fh cos ()3
,ir'
[ COO06
S;~06
0 1 0
sin ()5 - COS ()5
0 0
0 0
- sin ()6 cos ()6
-1
0 0
0
' T,
~
'i,t
[ coo O,
0 a, coo 0, ] 0 a 4 sin ()4 1 d4 1 0
- sin ()7 cos ()7
0 0
0 0 1 0
~5
]
;] ~7
]
7 Angular Velocity 7.1 Angular Velocity Vector and Matrix Consid er a rot atin g rigid bod y B(Oxy z) with a fixed point 0 in a reference frame G(0 XY Z) as shown in Figure 7.1. The motion of th e body can be describ ed by a tim e varying rot ation transformation matrix between the global and body frames to map th e inst ant aneous coordin at es of any fixed point in body frame B into th eir coordinates in th e global frame G
(7.1)
z
•
.J p rp
x .... X~
y
Y
FIGUR E 7.1. A rot ating rigid body B(O xy z) with a fixed poin t 0 in a global fra me G(OXYZ).
The velocity of a body point in th e global frame is
Gv (t ) G RB(t) Br GWB Gr(t ) GWB x Gr(t)
(7.2) (7.3) (7.4) (7.5)
where GWB is th e angular velocity vector of B with respect to G . It is equa l to a rotation with angular rate ;p about an instantaneous axis of rotation U.
298
7. Angular Velocity
z
....
x~
y
FIG URE 7.2. A bod y fixed point Pat
B
... y
r in th e rotating bod y fram e B .
(7.6) The angular velocity vector is associated with a skew symmetric matrix CWB called the angular velocity matrix
w~ [
:, - W2
- W3 0
W2 - WI
WI
0
]
(7 .7)
wher e
CWB
C RB C R~
(7.8)
cP u.
(7.9)
P roof. Consider a rigid body with a fixed point 0 and an at tached frame B(O xy z) as shown in Figure 7.2. The bod y frame B is initi ally coincident with the global frame G. Therefore, the position vector of a body point P is
Cr(to) =
Br .
(7.10)
The global time derivative of c r is
c·r cd
dt cr(t) cd
dt
[CR B(t) B r ]
c d [CR B(t) Cr(to) ] dt C R B(t)
Hr .
(7.11)
7. Angula r Velocity
299
Eliminating B r between (7.1) and (7.11) determin es the velocity of the point in global frame (7.12) We denot e th e coefficient of Cr(t ) by W (7.13) and writ e th e Equation (7.12) as Cv
= CW B c r(t )
(7.14)
or as (7.15) The time derivative of the orthogonality condition, troduces an important identity
C R B C R~
= I , in-
(7.16) which can be util ized to show th at CW B = matrix, because c RB
cR~ =
[C
RB C R~] is a skew symmetric
[CR B CR'b f
.
(7.17)
gw
The vector B is called the instantaneous angular velocity of the bod y B relative to th e global frame G as seen from t he G frame. Since a vectori al equat ion can be expr essed in any coordinate frame, we may use any of th e following expressions for the velocity of a body point in bod y or global frames c Vp
C
C C CWB x r p
B
B
c Vp
CWB x
B
rp
(7.18) (7.19)
gyP
where is the global velocity of point P expressed in global frame and is the global velocity of point P expressed in bod y frame.
gyP
C
c Vp
(7.20)
gyPand gyPcan be convert ed to each other using a rot ation matrix B
c Vp
CRT C v B C P CRT - cC r p B CWB CRT c R' CRT c r B B B C P CRT CR ' B B B c r p.
(7.21)
(7.22)
300
7. Angular Velocity
showing th at BCWB
C R T C R' B B
=
(7.23)
which is called th e instantaneous angu lar velocity of B relative to the global frame G as seen from the B frame. From th e definitions of cw Band gwB we are able to tr ansform the two angular velocity matri ces by CR BC RT CWB = B CW B B
(7.24)
BCWB
(7.25)
=
C RT CCR B CWB B
or equivalently C·
RB
C' RB
=
C
=
CR CWB B
C
RB
(7.26)
BR B C WB
(7.27)
CW B
CR
=
B B CW B ·
(7.28)
Th e angular velocity of B in G is negative of th e angular velocity of G in B if both are expressed in t he same coordinate frame. C-
(7.29)
CWB BCWB
CWB
(7.30)
and can always be expressed in the form C WB =
where U is a unit vector parallel to
WU
CWB
(7.31)
and indicates the instantaneous
axis of rota tion.
Using t he Rodriguez rotation formula (3.5) we can show that
Ru, =
1> UR u,¢
(7.32)
and therefore (7.33) or equivalently Cd lim - R u ¢
¢->O
dt
lim Cdd t
¢ ->O
(7.34)
'
(_u2 cos ¢ + usin ¢ + ii? + I)
1>u and therefore W
•
=
1>u.
(7.35)
7. Angular Velocity
301
Example 172 Rotation of a body point about a global axis . The slab shown in Figure 2.4 is turning about the Z-axis with eX = lOdeg js. The global velocity of the corner point P(5 ,30, 10), when the slab is at a = 30deg, is G
v p
G RB(t)
Br p
(7.36)
-sina cos a 0
«« ([ c?, a sma
dt
0
~ ][ id [-497 ] ~ ][ i~ ]
. [ -,;na - cos a
a
cos a 0
-sina 0
10. [-,;n}
180
~ ]) [i~ ]
-cosi • 1r - sm '6 0
cos6 0
- 1.86 0
at this moment , th e point P is at Gr p
GRB Brp
[ cos, - ,;n, sini 0
cos i
o
0][ 5] [-1067] 0 1
30 10
28.48 10
=
(7.37) .
Example 173 Rotation of a global point about a global axis. The corner P of the slab shown in Figure 2.4, is at Br p = [5 30 10 Wh en it is turned a = 30 deg about the Z-axis , the global position of P is
f.
G RB
Br p rr
(7.38) •
1r
cos '6 - sm '6 0 ] [ sin i cos i [
o
0
~
5] = [ -2~.~8 10.67 ] .
~~
If the slab is turning with eX = 10 deg j s, th e global velocity of the point P would be
(7.39)
oo ] T 1
[
- 10.67 ] 28.48 10
302
7. Angular Velocity
Example 174 Principal angular velocities. Th e principal rotational matrices about the axes X , Y, and Z are
Rx,'Y =
RY,{3
=
R z ,ex =
10 C?s"'! 0 - sin0] "'!
[o
sm y
COS(3 0 [ - sin (3
[
(7.40)
cos y 0
1 0
sin (3 ] 0 cos (3
COSa: - sin a: cos a: Si~ a:
(7.41 )
(7.42)
o
and hence, their time derivatives are
Rx,'Y = 1
0 0 [ 0o
- sm "'!
cos "'!
c~s"'!
] - sm ",! cos (3 ] -
R z ,ex
=
a:
[
s~n (3
- sin a: - cos a: - sin a: co~ a:
(7 .43)
(7.44)
(7.45)
o
Th erefore, the principal angular velocity matrices about axes X , Y, and Z are
cwx
GWY
GWZ
[~ ~ ~1]
(7.46)
~ Ry pRE8 ~ fi [~1 ~ ~]
(7.4 7)
=
RX' 'YR~. 'Y
=
1
~ Rz oRi .o ~ &
010
[! ~1 ~]
(7.48)
which are equivalent to (7 .49) (7.50) (7 .51 )
7. Angular Velocity
303
and therefore, the principal angular velocity vectors are
e Wx e Wy e Wz
wx
i = -t
(7.52) (7.53) (7.54)
Wy J = $J wzK=&.K.
Utilizing the same technique, we can find the following principal angular velocity matrices about the local axes. 0
BT ' . eWx = Rx,>/J Rx,>/J = 1/J [ 0 ~ 0
1
0 0 - 1 0
BT ' . eWy = Ry,eRy,e = e [ 0 0
BT ' . eWz = Rz,
- 1 0 0
~1
]
= 1/J i
~ ] OJ ~ ] epk
(7.55)
=
(7.56)
=
(7.57)
Example 175 Decomposition of an angular velocity vector. Every angular velocity vector can be decomposed to three principal angular velocity vectors .
e WB
(ewB ' i) i + (ewE ' J) J + ( eWB ' K ) K Wx 1 + Wy
(7.58)
1 + Wz k
11 + $1 + &.K w x +W y +Wz
Example 176 Combination of angular velocities. Starting from a combination of rotations
(7.59) and taking derivative, we find
(7.60) Now, substituting the derivative of rotation matrices with OW2 0 R 2 OWl 0 R 1 1W2 1 R 2
(7.61) (7.62) (7.63)
304
7. Angular Velocity
results in OWl 0 R 1 1 R 2
+ 0 R 1 1W2 1 R 2
+ »n1 1W- 2 0RTOR 1 1 -n2 - OR 0- »n OWl 2 + 1W 2 2
- OR 2 OWl
(7.64)
where
(7.65) Th erefore, we find OW2
=
OWl
+ 0lW2
(7.66)
which indicates that the angula r velocities may be added relati vely
(7.67) This result also holds for any number of angula r velocities
(7.68) n
L i~l Wi ' i= l
Example 177 Angular velocity in terms of Eul er fr equencies. Th e angular velocity vector can be expressed by Eul er fr equencies as described in Chapter 2. Th erefore,
+ W y ] + wJ;; (pe
WxZ
(p
[
sin 0 sin 7/J ] sin 0 cos 7/J [ cos O sin 0 sin 7/J sin 0 cos 7/J
+B
cos 7/J - sin 7/J
°
cos O
[ COS 7/J] - s~n 7/J
+ 7/J
[0]~
~][ ! ]
(7.69)
and
a
OWB
BR - 1 Bw 0 B
a
(P sin 0 sin 7/J
BR
+ Bcos 7/J
c/ [ (P sin 0 cos 7/J - ~ sin 7/J
]
(P cos 0 + 7/J
[~
cos
°
sin 0 sin
(7.70)
7. Angular Velocity
305
where the inverse of the Euler transformation matrix is Ci.pc'IjJ - dJsi.ps'IjJ
BR
- Ci.ps'IjJ - cBc'IjJsi.p - si.ps'IjJ + cBci.pc'IjJ sBc'IjJ
+ cBci.ps'IjJ c/ = [ c'IjJsi.p sBs' IjJ
SBSi.p ] -ci.psB . cB
(7.71)
Example 178 Angular velocity in terms of rotation frequencies. Appendices A and B show the 12 global and 12 local axes' triple rotations. Utilizing those equations, we are able to find the angular velocity matrix and vector of a rigid body in terms of rotation frequencies . As an example, consider the Euler angles transformation matrix in case 9, Appendix B.
BR e = RZ,'ljJRx,eRz,'P
(7.72)
The angular velocity matrix is then equal to BWe
B ReBR~
;/, dR z,'ljJ R R ) . R R dRz,'P 0R dRx,eR z ,'ljJ~ z,'P + 'f/ --;u- x,e z,'P ( i.p z,'ljJ x,e--;u- + x (Rz ,'ljJRx,eRz,'Pf
dRz ,'P R Tz,'P R Tx,eR Tz,'ljJ i.p. R z,'ljJ R x,e --;u. dRx ,e T T +B RZ , 'ljJ ~Rx ,eRz , 'ljJ
.i, dR z,'ljJ R T
+'f/ --;u- z,'ljJ
(7.73)
which, in matrix form, is BWe =
c~s B sin Bcos 'IjJ
+0 [
cos B
~
- sin 'IjJ
- sin Bcos 'IjJ ] sinBsin 'IjJ
o
-
- sin Bsin 'IjJ
~
~~~~ ] +;p [ ~1 ~ ~ ] 0 0 0 0
- cos 'IjJ
or BWe =
[
-;p -0
0
;p +
- Oc'IjJ -
r: .
(7.74)
ih ,p - (psBC¢ ] Bc'IjJ +
(7.75)
~ ][n
(7.76)
The corresponding angular velocity vector is BWe
-
[ OC¢ + (psBs,p ] - Osy; +
-
[ sin Bsin" sinB cos'IjJ cos B
cos'IjJ - sin 'IjJ 0
306
7. Angular Velocity
However,
(7.77) (7.78) and therefore,
gWB
sin Bsin 'lj; = sinBcos'lj; [ cosB
(7.79)
*
Example 179 Coordinate transformation of angular velocity. Angular velocity ~ W2 of coordinate frame B 2 with respect to B 1 and expressed in B 1 can be expressed in base coordinate frame B o according to
(7.80) To show this equation, it is enoug h to apply both sides on an arbitrary vector or. Therefore, the left- hand side would be
°R °R °R
W2 1 Ro Or 1 l W2 l r
11
1 (I W2
x l r)
°RII W2xoRll r ?W2
x Or
(7.81)
which is equal to the right-hand side after applying on the vector or
(7.82)
*
Example 180 Time derivative of unit vectors. Using Equation (7.19) we can define the time derivative of unit vectors of a body coordinate frame B(i, j , k), rotating in the global coordinate frame
cii.r.tc,
Cdi dt Cdj dt cdk dt
B CWB x z
(7.83)
B CWB x J
(7.84)
B CWB x k.
(7.85)
A
A
A
7. Angular Velocity
Example 181
307
*
Angular velocity in terms of quatern ion and Eul er param eters. Th e angular velocit y vector can also be expressed by Eul er paramet ers. Starting from the unit quaternion representation of a fin it e rot ation e (t) B r e* (t)
e (t)
B
r e- 1 (t)
(7.86)
where eo + e
e e*
e
- 1
(7.87) (7.88)
=eo - e
we can find
o r·
e Bre* +cB re*
e e* o r
+ or e e*
2e e* o r
(7.89)
and therefore, th e angular velocity quat ernion is
(7.90)
OWB = 2e e*. W e have used the orthogonality property of unit quatern ion ee- 1 =ee*=1
(7.91)
+ e e" = O.
(7.92)
whic h provides e e"
Th e angula r velocity quat ernion can be expan ded using quaternion produ cts to fi nd the angular velocity com pone nts based on E111er paramet ers. OWB
2e e* = 2 (eo + e) (eo - e) 2 (eoeo + eoe - eoe + e. e 2[
e x e)
eO~l e1~0 1e2~3 e3~2
] eOe2 - e2eO - e1e3 + e3e1 eoe3 + e1e2- e2e1 - e3eO
- e2e3 ] [ - eo e1 ] -: e1 - e2 Co - e3
(7.93)
Th e scalar com ponent of th e angular velocity quatern ion is zero because
(7.94)
308
7. Angular Velocity
Th e angular velocity vector can also be defin ed as a quaternion +-----+ +---+ +---+ GWB=2 e e*
(7.95)
to be utilized for definition of the deri vat ive of a rotation quatern ion +-----+ +---+ 2"1 GWB e .
'"7
e =
(7.96)
A coordinate tran sformation can tran sform the angular velocity into a body coordinate fram e B GW B
e*
G GWB
(7.97)
e
2e* e
(7.98)
:~ ~~2 1 [ ~~ 1
eo el and therefore, +-----+
BW B G
'"7
e =
ei
e2
eo
e3
+---+ +---+
= 2 e*
e.
1 +---+ ~
2"
e G W B·
(7.99)
(7.100) (7.101)
*
Example 182 Differential of Eul er parameters . Th e rotation matrix G R B based on Euler param eters is given in Equ ation
(3.58) 2 (el e2 - eoe3)
e5- ei + e~ - e5 2 (eoel
+ e2e3)
(7.102)
and the individual parameters can be found from any set of Equations (3.126) to (3.129) . Th e first set indicates that eo
1
±2"V 1 + r ll + r22 + r33 1 r32 - r23 eo 1 r1 3 - r 3 1 4 eo 1 r 21 - r12 4 eo
4
(7.103) (7.104) (7.105) (7.106)
7. Angular Velocity
309
and therefore, Tn
eo
+ T22 + r33
(7.107)
8eo
el
-;. ((r32 - r23) eo - (r32 - r23) eo) 4eo
(7.108)
e2
-;. ((r13 - r3t) eo - (r13 - r3t) eo) 4eo
(7.109)
e3
-;. ((r21 - r12) eo - (r21 - r12) eo) . 4eo
(7.110)
We may use the differential of the transformat ion matrix G '
G
R B = GWB RB
to show that eo
1 - (- elwl - e2W2 - e3w3)
(7.111)
el
2 (eOWl + e2w3 -
e3w2)
(7.112)
e2
2 (eOW2 -
el w3 - e3w t)
(7.113)
e3
2 (eOW3 + el W2 -
2 1
1 1
e2w l ) .
(7.114)
Similarly we can find el, ez, and e3, however the final result can be set in a matrix form - Wi - w2 w, 0 W3 -- W2 Wi 0 W2 - W3 Wi o W3 W2 - Wi
['0] [0 e2 2 ~1
=~
e3 or
["] [ ~ ~1 e2 e3
=
~ 2
- e3 - e2 e, eo - e3 -e2 el e2 el eo - e3 - e2 el eo e3
] [ e, el ] e2 e3
][ Wi 0 W2 W3
]
'
(7.115)
(7.116)
*
Example 183 Elements of the angular velocity matrix. Utilizing the permutation symbol introduced in (3.122) Eij k
1 2
= -(i - j)(j - k)(k - i)
i, j , k
= 1, 2, 3
(7.117)
allows us to find the elements of the angular velocit y matrix, W, when the W = [Wi W2 W3 ]T, is given.
tinqular velocity vector,
(7.118)
310
7. Angular Velocity
7.2 Time Derivative and Coordinate Frames The time derivative of a vector depends on the coordinate frame in which we are taking the derivative. The time derivative of a vector r in the global frame is called G-d erivative and is denoted by cd
dt r while th e tim e derivative of the vector in th e body frame is called the B -derivative and is denoted by
Bd
- r.
dt
The left sup erscript on th e derivative symbol indicat es the frame in which the derivative is taken, and hence, its unit vectors are considered constant . Therefore, th e derivative of B r p in B and the derivative of c r p in G are
Bd
_B r p
B r p = Bvp= xi+y}+ ik
dt cd c rp dt
crp
=
cvp
= X j + y j + Z k.
(7.119) (7.120)
It is also possible to find th e G-derivative of B r p and the B-derivative of
c r p . We use Equation (7.19) for the global velocity of a body fixed point P , expressed in body frame B
cVp
=
B B CWB x rp
to define th e G-derivative of a body vector B
c Vp =
cd
dt
B
BVp
Bd C
= dt
by
Brp
(7.122)
rp
and similarly, a B-d erivative of a global vector C
(7.121)
Crp
by (7.123)
rp .
When point P is moving in frame B while B is rotatin g in G, the Gderivative of B r p (t) is defined by cd
dt
Brp
(t) B· c rp
(7.124)
and the B-derivative of c r p is defined by C r p - CWB
c· B rp .
x
crp
(7.125)
7. Angular Velocity
x~
y
311
y
FIG URE 7.3. A moving bod y point P at Br(t) in th e rot ating body fram e B .
tc
Proof. Let G(OXYZ) with unit vectors i, J, and be t he global coordinate frame, and let B(O xy z) with unit vectors i, i, and k be a body coordinate frame. The position vector of a moving point P, as shown in Figure 7.3, can be expressed in the body and global frames
x (t )i + y (t ) j +z(t ) k X (t) i + Y (t) J + Z (t) tc. The time derivative of B r p in B and
Gr
(7.126) (7.127)
p in G are
(7.128) G rp
=
G
.A
Vp
= X
'A
1+ Y J
+ZK '
A
(7.129)
because the unit vect ors of B in Equ ation (7.126) and the unit vectors of G in Equ ation (7.127) are considered constant . Using the definition (7.122), we can find t he G-derivative of the position vector B r p as
(7.130) We achieved this result because the x, y, and z component s of
Brp
are
312
7. Angular Velocity
scalar. Scalars are invariant with respect to frame tr ansformations. Therefore, if x is a scalar then, Cd
Bd
di x = di x = ±.
(7.131)
The B-derivative of c r p is Bd ( A A A) di X I+ Y J+ ZK . A
.
A
B
. A
B
A dI
A dJ
B
A dK
XI +YJ +ZK+Xdi+Ydt+Z dt
(7.132) and therefore, Bd
-
dt
crp -
-
c r p - C WB
x crp.
(7.133)
Th e angular velocity of B relative to G is a vector quantity and can be expressed in either frames. g WB
=
g WB
wx i+wy J +wzk wxi +wyJ +wzk.
(7.134) (7.135)
• Example 184 Tim e derivative of a mo ving point in B. Consider a local fram e B , rotating in G by 0: about the Z- axis, and a moving point at B r p (t ) = ti . Th erefore, crp
C R B " r»
=
R Z ,a (t ) B r p
[ :~: -;~F ~ ] [~ ] tcosai
=
+ t sin aJ.
(7.136)
Th e angular velocity m atrix is C·C T ·CWB = R B R B = aK
(7.137)
that gives
(7.138) It can also be verified that BC RT CCR CW B = B eWB B =
and therefore,
B
e WB
.
A
= ok.
a' k-
(7.139) (7.140)
7. Angular Velocity
313
Now we can find the following derivatives
Bd -:«; dt
B r· p
i
(7.141)
G·r p
+ (sin a + to: cos a) 1.
(cos a - to:sin a) j
(7.142)
For the mixed derivatives we start with
(7.143)
which is the global velocity of P expressed in B . We may, however, transform ~ rp to the global frame and find the global velocity expressed in G.
GRB
G rp
B·
Grp
c~s a sm o
[
- sin a
cos a
° t~°
C?S 0: -
[
(cos 0:
sin 0:
]
+ to: cos 0:
sin 0:
°
to: sin 0:) j
-
+ (sin 0: + to: cos 0:) j
(7.144)
The next derivative is
C?S 0: [ [
sm
0:
c~s a sm o
°
t~ sin 0:
+ to: cos 0: ]
]
- 0:
° =
(coso:) j
~ x [ 0]
[ t cos o:] t s~ 0:
+ (sino:) j (7. 145)
314
7. Angular Velocity
whic h is the velocity of P relative to B and expresse d in G . To express this velocity in B we apply a f ram e tran sformation C R T C r'
B rp '
B B P
[
c~s 0: sin o
-
sin 0: coso:
o
0
O] O:] o T [ C~S sm o 1
0 (7.146)
S ometimes it is more applied if we transform the vector to th e same fram e in which we are taking the derivati ve and th en apply the different ial operator. Th erefore,
t c~s 0: [
t~ sin 0:
C?S 0: -
[
sm 0:
+ t o:cos 0:
]
r sin o
o
]
(7.147)
o
and
(7.148)
Example 185 Orthogonality of position and velocity vectors. If the posit ion vector of a body poin t in global f ram e is denoted by r then dr dt . r
= O.
(7.149)
To show this property we may tak e a derivat ive from (7.150)
and fin d
d dt
- (r· r)
dr dr -·r+r · dt dt dr 2- ·r dt
O.
(7.151)
7. Angular Velocity
315
Th e Equation (7.149) is correct in every coordinate fram e and for every constant length vector, as long as the vector and the derivative are expresse d in the same coordin ate fram e.
*
Example 186 Derivat ive tran sformation formula . Th e global velocity of a fixed poin t in the body coordinate f ram e B (Oxyz ) can be fou nd by Equation (7.5). Now consider a point P that can mo ve in B (O xy z) . In this case, the body position vector B r p is not constant, and therefore, th e global velocity of such a point expressed in B is Bd B
di
rp
B
+ CW B
x
B· crp.
B
rp
(7.152) (7.153)
Sometimes the result of Equation (7.152) is ut ilized to define tran sform ation of th e differential operator from a body to a global coordinate fram e
(7.154) (7.155) however, special att ention must be paid to the coordin ate fram e in which th e vector 0 and the final result are expressed. Th e final result is ~ D showing the global (G) tim e derivative expressed in body fram e (B) . The vector 0 might be any vector su ch as position, velocity, angular velocity, momentum, angular velocity, or even a tim e-varying fo rce vector. Th e Equation (7.154) is called the derivative transformation formula and relates th e tim e derivati ve of a vector as it would be seen from fram e G to its derivative as seen in frame B. Th e derivative transformation formula (7.154) is more general and can be applied to every vector fo r derivative transform ati on betw een every two relatively moving coordinate frames.
*
Example 187 Differential equati on for rotat ion ma trix. Equa tion (7.8) fo r defin ing the angular velocity matrix may be writte n as a first-order different ial equation
d
dt
-
(7.156)
Th e solut ion of the equation confirms the expone ntial definition of the rotat ion matrix as
(7.157) or
wt = ;p it = In (CR B) .
(7.158)
316
7. Angular Velocity
y
FIGURE 7.4. A body coordinate frame moving with a fixed point in th e global coordinate frame.
*
Example 188 Acceleration of a body point in the global fram e. The angular acceleration vector of a rigid body B(Oxyz) in the global fram e G(OXYZ) is denoted by C a B and is defin ed as th e global time derivative of C W B · (7.159)
Using this definition , the acceleration of a fixed body point in th e global fram e is
Cd
dt
( CW B
C aB
x
x Crp
C r p)
+ CWB
x (CW B x C r p ) .
(7.160)
*
Example 189 Alternative definition of angular velocity vector. The angular velocity vector of a rigid body B( i ,j , k) in global fram e cit, J, K) can also be defin ed by cd '
c dk
dt
dt
~W B = i (_ J . k) + j ( - . i)
c d'
+ k(_2 . j) . dt
(7.161)
Proof. Consider a body coordinate fram e B moving with a fixed point in the global coordinate frame G. Th e fixed point of the body is tak en as the origin of both coordinate fram es, as shown in Figure 7.4. In order to describe th e motion of the body, it is sufficie nt to describe th e motion of the local unit vectors i, i, k . Let r p be the position vector of a body point P . Then, B r p is a vector with constant componen ts. Brp
= xi + yj + zk
(7.162)
7. Angular Velocity
317
When the body moves, it is only the un it vectors i , t, and k that vary relative to the global coordinate frame . Therefore, the vector of differential displacement is (7.163) drp =xdi +ydj +zdk which can also be expressed by
dr p
= (dr p . i) i + (dr p . j) j + (dr p . k ) k .
(7.164)
Substituting (7.163) in the right-hand side of (7.164) results in
dr p
=
( x i . di
+ yi . dj + zi . dk) i
+ (x j . di + yj. dj + zj . dk ) j + ( x k . di + yk . dj + zk . dk )
k.
(7.165)
Utilizing the unit vectors' relationships
i -di k · dj
-i · dj
(7.166)
-j .dk
(7.167)
i· dk i · di
- k · di
(7.168)
j . dj = k . dk = 0
(7.169)
i·j
j .k =k.i =O
(7.170)
i ·i
j·j =k ·k = l
(7.171)
the dr p reduces to ( zi . dk - yj . di) i
drp
+ (x j . di -
z k . dj) j
+ (yk. dj -
x i : dk) k.
(7.172)
This equation can be rearranged to be expressed as a vector product drp= ((k .dj)i + (i .dk)j+ (j .di)k) x ( x i +yj +zk) or
Comparing this result with rp
=
eWB
x rp
(7.173)
318
7. Angular Velocity
shows that A A
BA CW B = Z (
•
A
CdJ A dt' k
A
( Cdk
A
A
Z
A
Cd + J & ' Z + k ( dt 'J
)
)
)
(7.174)
.
*
Example 190 Alternative proo] for' angular velocity definition (7.161) . Th e angular velocit y definition presented in Equat ion (7.161) can also be shown by direct su bstituti on for CR B in the angular velocity matrix ~w B B- = CR T B CR'B· CWB
(7.175)
Th erefore,
B-
CWB
[.: ; :~
j .) j .)
k· j i . j
z·~ ]
J·k
1< .) 1< .1.
(7.176)
whic h shows that
B C WB
=
(
Cd~~ . k)
c dk ·i dt
(7.177)
Cdi A ) ( - dt ' J
*
Example 191 Second derivati ve. In general, c d r/dt is a variable vector in G(OXYZ) and in any other coordinate fram e such as B (oxyz) . Th erefore, it can be differentiated in either coordinate frame s G or B. However, the order of different iat ing is important. In general, Bd c dr ««Bdr
dtdt -I- dt& '
(7.178)
As an exam ple, consider a roiaiuiq body coordinate fram e about the Z-axis, and a variable vector as
(7.179)
7. Angular Velocity
319
Therefore, (7.180)
and hence, B.r
G
= R Tz ,'P [A] I COS
- sin
o
[
sin ip cos
0 (7.181)
cos
which provides Bd Gdr dt dt
-- =
..
A '
A
-
(7.182)
and G
( Bd Gdr ) = - epJ. dt dt
Now Br
= RI ,'P
[ti] = t cosspi-
(7.183)
t sin
(7.184)
that provides Bdr
dt
= (- tep sin
+ cos
(sin
(7.185)
and
[
R z ,'P (( - tep sin
[ :~; ~~r: ~] ::~fn':~~~:: ] i-
tepJ
(7.186)
which shows GdB dr
---
dt dt
- (ep + t(jJ)
J
(7.187)
320
7. Angular Velocity
x .... ' y
x
~
FIGURE 7.5. A rigid body with an at tached coordinate frame B (oxyz) moving freely in a global coordinate frame G(OXYZ) .
7.3 Rigid Body Velocity Consider a rigid body with an attached local coordinate frame B (oxyz) moving freely in a fixed global coordinate frame G(0 XY Z), as shown in Figure 7.5. The rigid body can rotate in the global frame , while the origin of the body frame B can translate relative to the origin of G. The coordinates of a body point P in local and global frames are related by th e following equation: G fp
= GR B B fp
+ Gd B
where Gd B indicates the position of the moving origin fixed origin O. The velocity of the point P in G is
(7.188)
0
relative to the
G·fp
GR B
Bf p
G G W B Bfp
+ Gd B + G'dB
G GWB ( fp G
+ G'dB G G' dB) + d B.
G)
GWB X ( f p -
dB
(7.189)
7. Angular Velocity
321
Proof. Direct differentiating shows
(7.190) The local posit ion vector
Brp
can be substituted from (7.188) to obtain
(7.191) It may also be written using relative position vector C C Vp = CWB x Brp
" + CdB .
(7.192)
• Example 192 Geometric interpretation of rigid body velocity. Figure 7.6 illustrates a body point P of a moving rigid body. The global velocity of th e point P CVp
=
CWB x C Brp
+ C"d B
is a vector addition of rotational and translational velocities, both expressed in the global frame. At th e moment, th e body fram e is assumed to be coincid ent with the global fram e, and the body frame has a velocit y C dB with respect to the global fram e. The translational velocity cd B is a common property for every point of th e body, but th e rotational velocity CWB x ~ r p differs for different points of th e body.
Example 193 Velocity of a moving point in a moving body fram e. Assume that point P in Figure 7.5 is moving in the fram e B , indicating by a time varying position vector Brp(t) . Th e global velocity of P is a composition of th e velocity of P in B , rotation of B relati ve to G, and velocity of B relati ve to G. Cd C rp dt
(7.193)
322
7. Angular Velocity
x FIGU RE 7.6. Geometric interpret at ion of rigid bod y velocity.
Example 194 Velocity of a body point in multiple coordinate fram es. Cons ider three fram es, B o, B 1 and B 2 , as shown in Figure 7.7. The velocity of point P should be m easured and expressed in a coordinate fram e. If the point is stationary in a fram e, say B 2 , then the tim e derivative of 2 r p in B 2 is zero. If the fram e B 2 is mo ving relative to the fram e B 1 , then, the time derivative of 1 r p is a com binati on of the rotational com ponent due to rotation of B 2 relative to B 1 and the velocit y of B 2 relative to B 1 . In fo rward velocity kinematics of robots, the velocities m ust be m easured in the base fram e B o . Th erefore, the velocit y of point P in the base fram e is a com bination of the velocity of B 2 relative to B 1 and the velocit y of B 1 relative to B o . Th e global coordinate of the body point P is
(7.194) (7.195) Therefore, the velocity of point P can be found by com bining th e relative velociti es
o·r p (7.196)
Most of the time, it is bett er to us e a relative velocity m ethod and wri te
(7.197)
7. Angular Velocity
323
z
• ' y
x y FIGUR E 7.7. A rigid body coordinate frame B 2 is moving in a frame B 1 that is movin g in t he base coordinate fram e Bi;
because (7.198) (7.199) (7.200) and th erefore, Dv
p
= Ddt +
gWl
x ~d2
+ DR 1 Id 2 + gW 2 x
~ rp .
(7.201)
Example 195 Velocity vectors are free vectors. Velocity vectors are free, so to express them in different coordinate f ram es we n eed only to premultiply th em by a rotat ion matrix. Hence, considering jVi as th e velocity of th e origin of the B , coordinate f ram e with respect to the origin of the fram e B j expressed in fram e Be , we can write (7.202) and (7.203) and th erefore, (7.204)
324
7. Angular Velocity
*
Example 196 Zero velocity points. To answer whether there is a poin t with zero velocity at each tim e, we may uti lize Equation (7.189) and wri te GW B (
G
G
ro -
dB
)
+ G'd B = 0
(7.205)
to search for Gr o which refers to a poin t with zero velocity G
ro =
Gd
B -
- -1 G d'
GW B
(7.206)
B
however, the skew symmetric matrix GW B is sin gular and has no inverse. In other words, th ere is no general solution for E quation (7.2 05) . If we restrict ourse lves to planar motions, say X Y -plane, then GW B = wK and = l/w. Hence, in 2D space there is a poin t at any time with zero velocity at position G r o given by
Gwi/
(7.207)
Th e zero velocity poin t is called the pole or in stant aneous center of rotation. The position of the pole is generally a function of time and the path of its mo tion is called a centroid .
*
Example 197 Eulerian and Lagrangian view poin ts . When a varia ble quantity is measured within the stationary global coordinate frame, it is called absolute or the Lagrangian viewpoint. On the other hand, when the varia ble is measured within a moving body coordinate frame, it is called relative or the Eulerian viewpoint. In 2D planar motion of a rigid body, there is always a pole of zero velocity at G GIG' ro = d B - d B. (7.208) w
The position of the pole in the body coordinate frame can be found by substitutin g for G r from (7.188) GR B Bro
+ Gd B =
Gd B -
GW 1 G d B
E
(7.209)
and solving for the position of th e zero velocity point in the body coordinate frame Bro. -
GR T - -1 G d' B GW B B
_ GR~ [GRB GR~r 1 - G R~ - G
[G R B G R
RE1 G d B
E/ ]
Gd B
Gd B
(7.210)
Theref ore, G r o indicates the path of motion of the pole in th e global frame, while B r o indicates the same path in the body frame. The G r o refers to Lagrangian centroid and B r o refers to Eulerian centro id.
7. Angular Velocity
Example 198
325
*
S crew axis and screw motion. Th e screw axis may be defin ed as a lin e for a mo ving rigid body B whos e points P have velocity parallel to th e angular velocit y vect or CWB = wU. Su ch points satisf y c Vp
CWB x (Cr p - C d B)
+ Cd' B (7.211)
P CwB · where, p is a scalar. Si nce CWB is perpendicula r to CWB x (Cr dot product of Equation (7.211) by CWB yields p=
:2
Cd),
a
(7.212)
( CWB ' Cd B ).
Introducing a paramet er m to in dicate different points of the lin e, th e equati on of the screw axis is defin ed by C
rp
=
C
dB
+ w12
( CWB
X C d' B )
+m Cw B
(7.213)
becaus e if we have a x x = b , and a · b = 0, then x = -a- 2(a x h) + rna . In our case, CWB x (C r p - Cd B) = P CWB - c d B , and (C r p - CdB ) is perpendicula r to CWB x (Cr p - Cd B) , hen ce to (p CWB - Cd B) ' Th erefore, there exis ts at any time a lin e s in space, parall el to CWB , which is the locus of points whose velocity is parallel to CWB . If 5 is the position vect or of a point on s , then CWBX
(c 5 - C dB)=p cw B- C d' B
(7.214)
an d th e velocity of any point out of 5 is Cv
=
CWB x (C r - C) s
+ p CWB
(7.215)
whic h expresses that at an y time th e velocit y of a body point can be decomposed into perpendicular and parallel compone nts to the angul ar velocity vector CWB . Th erefore, th e motion of any point of a rigid body is a screw. Th e paramet er p is th e rat io of translation velocit y to rotation velocity, and is called pitch. In gen eral, 5 , CWB , and p may be functions of time.
7.4 Velocity Transformation Matrix Consid er t he motion of a rigid body B in the globa l coordina te frame G, as shown in Figure 7.5. Assum e the body fra me B(oxyz) is coincident at some initial tim e to with t he globa l frame G(OXYZ) . At any time t i= t o, B is not necessarily coincident with G and therefore, the homogeneous transformation matrix CTB (t ) is t ime varying.
326
7. Angular Velocity
The global position vector Grp (t) of a point P of the rigid body is a function of tim e, but its local position vector Br p is a constant , which is equal to Gr p(to). B r p == Grp (t o) (7.216) Th e velocity of point P on the rigid body B as seen in th e reference fram e G is obt ained by differentiating the position vector Gr(t ) in the reference fram e G G d G () G. Vp = dt r p t = rp (7.217) where Grp denot es the differentiation of the quantity Grp (t) in t he reference fram e G. The velocity of a body point in globa l coordinate frame can be found by applying a homogeneous transformat ion matrix (7.218)
where GVB is the velocity transformation matrix Gt B GTi3l
(7.219)
[GRBOGR~ GclB - G R~G R~ GdB ]
(7.220)
[
GWB GclB - GWB Gd B ] 00 ·
(7.221)
Proof. Based on homogeneous coordinat e transformation, we have (7.222)
and t herefore, (7.223)
(7.224)
7. Angular Velocity
327
Thus, the velocity of any point P of the rigid bod y B in the reference fram e G can be obt ained by premultiplying t he position vector of the point P in G with t he velocity transformation matrix, G V B, (7.225)
where , GT B GT i3 l
[ G~B
G dB -
~WB G dB
]
[ G~B G~B ]
(7.226)
and (7.227) G d·
B -
G·
GR' B GR TB G d B G
dB - GWB d B G· G d B - GwB x d B.
(7.228)
The velocity transformation matrix G V B may be assum ed as a matrix operator that provides t he global velocity of any point attached to B(oxyz). It consists of th e angular velocity matrix GW B and t he frame velocity G dB both describ ed in the global frame G(OXYZ) . The matrix G VB depends on six parameters: t he t hree components of t he angular velocity vector GW B and the three components of the frame velocity G d B . Someti mes it is convenient to introduce a 6 x 1 vector called velocit y transformation vector to simplify numerical calculat ions. G tB
= [
GVB ] GWB
= [
Gd B -
GW B G d B ] GWB
(7.229)
In analogy to the two representation s of the angular velocity, th e velocity of body B in reference frame G can be repr esent ed eit her as t he velocity transformation matrix G V B in (7.226) or as the velocity transformation vector G tB in (7.229) . The velocity tr ansformation vector repr esents a noncomm ensurate vector since thc dimension of GWB and G VB differ. • Example 199 Velocity tran sformation m atri x based on coordinate transformation matrix. Th e velocity transformation m atrix can be found based on a coordinate tran sformation matrix. St arting from (7.230)
328
7. Angular Velocity
and taking the derivati ve, shows that (7.231)
howev er, (7.232)
and therefore, [ G
~B
GOd]
[ G
~B
GOd]
[
(7.233)
GRBoGR~
GVB Gr .
Example 200 In verse of a velocity transformation matrix. Transformation from a body fram e to the global fram e is given by Equation (4.46)
GTB l = [G ~~ _ G~~ Gd] .
(7.234)
Following the same prin ciple, we may introduce the in verse velocity transformation matrix by
[
GRB ~JiB'
[ GRB~REI
- G
RBG JiB'
(G
- GRB GREal Gd
~-
G
+ Gd
JiB G R~ G d) ]
] (7.235)
to have GVB GVil
= I.
(7.236)
Th erefore, having the velocity vector of a body point G y P and the velocity transformation matrix GVB we can find the global position of the point by Gr
P -
GVB- l G y p .
(7.237)
7. Angular Velocity
329
Exam pl e 201 Velocity transformation matrix in body frame. The velocity transformation matrix GVB defined in the global frame G is described by
(7.238) How ever, the velocity transformation matrix can be expressed in the body coordinate frame B as well
(7.239)
GT/3 1 G t B
[ G~~ _ G~~ Gd ] [ G~B God ] [ GR~ oG RB GR~ Gd ]
[ g~B
Bod]
gw
where B is the angular velocity vector of B with respect to G expressed in B , and B d is the velocity of the origin of B in G expressed in B . It is also possible to use a matrix multiplication to find th e velocity transformation matrix in the body coordinat e frame. gyP
GT/31 G v p GT/31 GtB B r p BV B G B rp
(7.240)
Using the definition of (7.219) and (7.239) we are able to transform the velocity transformation matrices between the Band G fram es.
GT BV GT- 1 BGB B'
(7.241)
It can also be useful if we define the time derivative of the transformation matrix by
(7.242) or
G '
TB
=
G
TB
B
(7.243)
G VB·
Similarly, we may define a velocity transformation matrix from link (i) to (i - 1) by . ,-1 V;
_ [ i-1
,-
k " i -1 RT
i- 1
a
d _ i-1 k z i - 1R T i - 1d
a
]
(7.244)
and i i -1
Vi =
[
i -1RT i -1Ri
'0
(7.245)
330
7. Angular Velocity
Example 202 Motion with a point fixed . When a point of a rigid body is fixed to the global frame, it is convenient to set the origins of the moving coordinate frame B (Oxyz) and the global coordina te frame G(OXYZ) on the fixed point. Under these conditions,
and Equation (7.224) reduces to GyP
=
GWB Grp(t)
=
GWB x Grp(t).
(7.246)
Example 203 Velocity in spherical coordinates The hom ogen eous transformation matrix from the spherical coordinates S (OrOcp) to Cartesian coordinates G(0 XY Z) is found as Rz,'P H'r',e D z» = [ G~B COS 0 cos
cp cos 0 sin cp - sinO 0
[
G1d ]
- sin cp cos sp sin 0 r cos cp sin 0 cos cp sin 0 sin cp r sin 0 sin cp 0 cos O rcosO 00 1
1.
(7.247)
Time derivative of GTs shows that G'
Ts
GVsGTs [
G~S
Gy s 0
]
GTs - cp'
cp' [ 0 - 0' ~os cp
0 - 0' sin cp 0
(7.248)
0' cos cp 0' sincp 0 0
r' eo, ~ ,in r' sin 0 sin cp r' cosO
e
1
GT B·
0
7.5 Derivative of a Homogeneous Transformation Matrix T he velocity transformation matrix can be found directly from t he homogeneous link transformation matrix. According to forward kinematics, there is a 4 x 4 hom ogeneous transformation matrix to move between every two coordinate frames . rI2
rI3
r22 rn r32 r33
o
0
rI4 ]
r24 r34 1
(7.249)
7. Angular Velocity
When the elements of the transformation matrix ative is drll dr12 dr13 dt dt dt dr21 dr22 dr23 GdT G ' -- = TB = dt dt dt dt dr31 dr32 dr33
331
are time varying, its deriv-
dt
dt
dt
dr14 dt dr24 dt dr34 -dt
0
0
0
0
- -
(7.250)
T he time derivative of the transformation matrix can be arranged to be proportional to the transformation matrix G '
TB
G
=
G
(7.251 )
VB T B
where GVB is a 4 x 4 homogeneous matrix called velocity transformation matrix or velocity operator matrix and is equal to
GTB GTi;I [ GRBOGR~
Gd -
G
RB G R~ G d
]
1
.
(7.252)
The homogeneous matrix and its derivative based on the velocity transformation matrix are useful in forward velocity kinem atics. The i- Ii'; for two links connected by a revolute joint is
'< i;
~ 8,
-
sinBi
lB' r
- cos Bi cos CYi - sin Bi cos CYi
cos Bi sin CYi sin Bi sin CYi
o o
o o
-ai sin Bi ai cosB i
o o
1
(7.253)
and for two links connected by a prismatic joint is
'< i: =
r ~o
H11· 0
0
(7.254)
0
The associated velocity transformation matrix for a revolute joint is i -I
Vi
=
.
.
Bi 6.R = Bi
and for a prism atic joint is i- I
Vi
.
.
= d; 6.p = d,
~
-1 0 0 0
0 0 0 0
n rl !1
r
0 0 0 0
0 0 0 0
(7.255)
(7.256)
332
7. Angular Velocity
Proof. Since any transformation matrix can be decomposed into a rotation and translation
[R~,¢ ~]
[T]
=
[~ ~] [R~,¢ ~]
[D] [R] we ca n find
t
(7.257)
as
[R~,¢ ~]
t
=
[~ ~] [~,¢ ~]
_
1
[I+D] [I+R]-I
(7.258)
[V][T] where [V] is the velocity transformation matrix described as
[V]
(7.259) The transformation matrix between two neighbor coordinate frames of a robot is described in Equation (5.11) based on the DR param et ers , COS Bi sin e',
i- IT = i
[
o o
- sin e. cos O'i COS~i COSO'i
sm O'i 0
sin Bi sin O'i - cosBi sinO'i cos O'i 0
a; cos Bi ai sinBi d; 1
j
.
(7.260)
Direct differentiating shows that in case two links are connected via a revolute joint, Bi is the only vari abl e of the DR matrix, and therefore, - cos Bi cos O'i - sin Bi cos O'i
cos Bi sin O'i -ai sin Bi sin Bi sin O'i a; cos Bi o 0 0 000
j
(7.261)
7. Angular Velocity
333
which shows th at the revolute velocity transform ati on matrix is
(7.262)
However , if the two links are connecte d via a prismatic joint, di is the only variable of th e DH matrix, and therefore,
,- d",
i - I T' · -
(7.263)
which shows that the prismatic velocity transformation matrix is
(7.264)
The 6.R and 6.p are revolute and prismatic velocity coeffi cient matrices with some applicat ion in velocity analysis of robots. • Example 204 Velocity of fram e B , in B o . Th e velocity of the fram e B , atta ched to the link (i) with respect to the base coordin ate fram e B o can be found by differentiating "d, in the base fram e.
However, the part ial derivatives a i - I T, / aqi can be found by utilizing the velocity coefficient matrices 6.i , which is either 6. R or 6. p . ai-IT 6. , aqi ,
_ _ _i _
Hence,
i- I T
s-
(7.266)
334
7. Angular Velocity
Example 205 Differential rotation and trans lation. A ssume the angle of rotation about the axis i.i. is too small and indicated by d¢ , then the differential rotation matrix is
(7.268)
because when ¢
<< 1,
then, sin ¢
d¢
cos ¢
1
vers ¢
O.
Differential translation dd = d(dxi+ dyJ +dzK) is shown by a differential translation matrix
1 0
1 o o dd dd y 00 1 dd z X
o
I + dD =
[
0
o
+ dD] [I + dR]
- 1
o
]
(7.269)
0
and therefore, dT
[I
0 -d¢U3 d¢U2 0 - d¢Ul d¢U3 -d¢U2 d¢Ul 0 [ 000
dd y .u, dd z
1 .
(7.270)
o
Example 206 Combination of principal differential rotations. Th e differential rotation about x, Y , Z are R
_ X ,d, -
R Y ,d/3 =
R Z ,da
=
[10 01 0
d'Y
o
0
0 - d'Y 1 0
[-~~
0 1 0 0
[ 1~
- do: 1 0 0
do:
df3 0 1 0 0 0 1 0
~]
~1 ~1
(7.271)
(7.272)
(7.273)
7. Angular Velocity
335
therefore, the combination of the principal differential rotation matrices about axes X, Y, and Z is
[I + RX,d,l [I + R Y ,dl3 ] [I + RZ,da] -do: 1
o1 o
0 0 0 ] [ 1 0 d{3 1 -d"( 0 0 1 0 d"( 1 0 - d{3 0 1 [ 0001 000 I [
do: - d{3
o
- do: 1 d"( 0
o o
0]
d{3 - d"( 0 1 0 0 1
[I + RZ,da ] [I + RY ,dl3 ] [I + RX,d,].
(7.274)
The combination of differential rotations is commutative.
Example 207 Differential of a transformation matrix. Assume a transformation matrix is given as
(7.275)
subj ect to a differential rotation and differential translation given by d¢il
[0.1
0.2 0.3]
(7.276)
dd
[0.6
0.4
(7.277)
0.2 ] .
Then, the differential transformation matrix dT is dT
= [I + dD ][1 + dR] -
1
0 - 0.3 0.2 0.3 0 -0.1 - 0.2 0.1 0 [ 00 0
0.6 0.4 ] 0.2 .
(7.278)
o
Example 208 Derivative of rotation matrix. Bas ed on the Rodrigu ez formula , the angle-axis rotation matrix is
Ru,q, = 1 cos ¢ + ilil T vers ¢ + u sin ¢
(7.279)
therefore, the tim e rate of the Rodrigu ez formula is kU, 'I-'~
- ¢ sin ¢I + ilil T ¢ sin¢ + u¢cos ¢
¢uRu,q, .
(7.280)
336
7.6
7. Angular Velocity
Summary
When a body coordinate frame B and a global frame G have a common origin and frame B rotates continuously with respect to frame G, th e rotation matrix C R B is time depend ent. (7.281) Then , th e global velocity of a point in B is (7.282) where cw B is th e skew symmetri c angular velocity matrix (7.283) The matrix c w B is associated with th e angular velocity vector CW B = ¢ U, which is equal to an angular rate ¢ about the instantaneous axis of rotation u. Angular velocities of connected links of a robot may be added relatively to find the angular velocity of the end link in the base frame (7.284) To work with angular velocities of relatively moving links, we need to follow the rules of relative derivat ives in bod y and global coordin at e frames.
Bd B rp dt _Cd c r p dt
_
Cd
ill "r» (t) Bd
ill c r p (t)
B·
rp=
B
.,
.,
' k'
vp= xz+YJ+ z
(7.285) (7.286)
B·
rp
B + CWB
c·rp -
CWB
x B rp = cB·rp
(7.287)
, x o rp = a Brp .
(7.288)
The global velocity of a point P in a moving frame B at c r p = c R B Br p
+ c dB
is C·r p C C CWB ( r p dB) CWB
X (
Ct e -
· + C dB C d B ) + C"dB ·
(7.289)
7. Angular Velocity
337
The velocity relationship for a body B having a continues rigid motion in G may also be expressed by a homogeneous velocity transformation matrix eVB
(7.290) where,
c:dB -
e] .
eWB dB
o
(7.291)
339
7. Angular Velocity
Exercises 1. Notation and symbols. Describe the meaning of a-
GWB
b-
g-
gWl
h- ~Wl
m- G rp(t)
s-
Gd _ Gr p dt
B WG
c-
GWB
G
d- g WB
e-
B B WG
f- ~ WG
.
3
j-
G '
k-
gWl
1-
1- 2Wl
n-
G 2 Vp
0-
t-
Gd _ Brp dt
u- -
b. R Bd
RB
Gd
p- b. p
q- -
v-
w-
dt
G "
JWi ' Bd
r- -
dt
x-
GVB
A body is turning abo ut the Z-axis at a constant ang ular rate 2 rad/ sec. Find the global velocity of a point at
a=
Br p
dt
Gr p
dp
2. Local position, globa l velocity.
3. Global position, constant angular velocity. A bo dy is turning abo ut the Z-axis at a constant angular rate 2 rad/ s. Find the global position of a point at
a=
after t = 3 sec if the body and globa l coordinate frames were coincident at t = 0 sec. 4. Turning about x-axis . Find the ang ular velocity matrix when the body coordinate frame is t urning 35 deg / sec at 45 deg about the x-axis. 5. Combined rotation and angular velocity. Find the rotation matrix for a body frame after 30 deg rotation about the Z-axis, followed by 30 deg about the X -ax is, and then 90 deg about t he Y-axis . Then calculate the angular velocity of t he bo dy if it is t urn ing with a = 20 deg / sec, /3 = - 40 deg / sec, and /y = 55 deg / sec about the Z, Y, and X axes resp ect ively.
340
7. Angular Velocity
6. Angular velocity, expressed in body fram e. The point P is at rp = (1,2,1) in a body coordinate B(Oxyz) . Find gw B when the body frame is turned 30 deg about the X -axis at a rate '1 = 75 deg / sec, followed by 45 deg about the Z-axis at a rate a = 25 deg / sec. 7. Global roll-pitch-yaw angular velocity. Calculate the angular velocity for a global roll-pitch-yaw rotation of 0: = 30 deg, f3 = 30 deg , and 'Y = 30 deg with a = 20 deg / sec, ~ = -20 deg / sec, and '1 = 20 deg / sec. 8. Roll-pitch-yaw angul ar velocity. Find gw B and GW B for the role, pit ch, and yaw rates equal to a = 20 deg / sec, ~ = -20 deg / sec, and '1 = 20 deg / sec respectively, and having the following rotation matrix:
BRG =
0.53 0.0 [ -0.85
-0.84 0.13] 0.15 0.99 -0.52 0.081
9. Angular velocity from the Rodriguez formula . We may find the time derivative of G R B d
G'
= R u,¢ by
. d
G
G
R B = dt R B = ¢ d¢ R B .
Use the Rodriguez rot ation formula and find GWB and gWB . 10. Skew symmetric matrix Show that any squar e matrix can be express ed as the sum of a symmetric and skew symmetric matrix. A
B+C
B
2 (A + A)
C
- (A - A)
1
1 2
11. A rotating slider. Figure 7.8 illustrates a slider link on a rotating arm . Calculate
7. Angular Velocity
341
l:a!!!!7"~-_.L-_---" X
FIGURE 7.8. A slider on a rotating bar. and find B v and mixed derivative
12.
Ba
of m at
eM of the
slider by using the rule of
* Differentiating in local and global frames. Consider a local point at B r p = ti + j. The local frame B is rotating . G by o. a b out t he Z -axis. . CaIcuIate dt "« B rp, dt Cd G rp, dt Bd G rp, an d III Cd B
dt
13.
rp.
* Skew symmetric identity for angular velocity. Show that
14.
* Transformation of angular velocity exponents. Show that
15.
* An angular velocity matrix identity. Show that and C}k
16.
= (_I)k
w2(k - l ) (w 2 1 _ ww T ) .
* Angular velocity and global triple rotation frequencies. Find the angular velocity in terms of the 12 cases of triple global rotations in Appendix A.
17.
* Angular velocity and local triple rotation frequencies. Find the angular velocity in terms of the 12 cases of triple local rotations in Appendix B.
8
Velocity Kinematics 8.1 Rigid Link Velocity The ang ular velocity of link (i) in the glob al coordinate frame Bi, summation of global angular velocities of the links (j) , for j ::; i
IS
a
(8.1) where if joint i is R if joint i is P.
(8.2)
The velocity of the origin of B, attached to link (i) in the base coordinate frame is 0O.Wi X i - 0I d i
o d. _
i- I
2 -
{
0
A
di k i -
I
+
0 0 OWi X i- I d,
if ioi
.. R
I joint Z IS . . . . . If joint Z IS
P
(8.3)
where () and d are DR parameters, and d is a frame's origin position vector. Therefore , if a robot has n links, the global angular velocity of the final coordinate frame is
(8.4) and the global velocity vector of the last link 's coordinate fram e is
(8.5) Proof. According to the DR definition, the position vector of the coordinate frame B, with respect to B i - I is
(8.6) which dep ends on joint variables qj , for j ::; i, and therefore, i-~\ di is a function of joint velocities qj, for j ::; i . Assume that every joint of a robot except joint i is locked . Then , the angula r velocity of link (i) connecting via a revolute joint to link (i - 1) is if the only movable joint i is R.
(8.7)
344
8. Velocity Kinematics
However, if the link (i) and (i - 1) are connecting via a prismatic joint then, i_~\Wi = 0 if the only movable joint i is P. (8.8) The relative position vector (8.6) shows that the velocity of link (i) connecting via a revolute joint to the link (i - 1) is
o . i-I d, if the only movable joint i is R. (8.9) We could substitute a; °Zi with i~l d, because the xi-axis is turning about the Zi_I-axis with angular velocity iii and therefore , iii °k i_ l X d; °k i _ l = o. However, if the link (i) and (i -1) are connecting via a prismatic joint then, if the only movable joint i is P.
(8.10)
Now assume that all lower joints j :S i are moving. Then, the angular velocity of link (i) in the base coordinate frame is
or o . -_ OW,
if joint j is R
{
(8.11)
if joint j is P which can be written in a recursive form
(8.12) or o
OWi
0
=
OWi-1 {
0
OWi-1
+ B. i
0'
ki -
l
if joint i is R if joint i is P.
(8.13)
The velocity of the origin of the link (i) in the base coordinate frame is if joint i is R if joint i is P.
(8.14)
The translation and angular velocities of the last link of a n link robot is then a direct application of these results. • Example 209 Serial rigid links angular velocity. Consider a serial manipulator with n links and n revolute joints. The global angular velocity of link (i) in terms of the angular velocity of its previous links is (8.15)
8. Velocity Kinematics
345
or in gen eral
(8.16) becaus e
o
(8.17)
i _I Wi
o
(8.18)
i - 2 Wi - 1
(8.19) and th erefore,
""' • OA 6 OJ kj -
(8.20)
I.
j=1
Example 210 S erial rigid link s tran slational velocity. Th e global angular velocity of link (i) in a serial manipulator in term s of th e angular velocity of its previou s links is
oV i = where
0
V i-I
o
+ i-0 I Wi
Vi
0
X i- I
di
= O·d,
(8.21)
(8.22)
or in gen eral
(8.23) because OWi X
i _~\ di
OWi -1 X
i ~2 di- 1
(8.24) (8.25)
o
i - 2 Vi
(8.26)
346
8. Velocity Kinematics
and therefore, i -I
L
° Vi
j~I Vj
+ i_~\ Vi
j=1
~ L (0'kj - 1 X
0 ) OJ. .
(8.27)
i -l d i
j=1
8.2 Forward Velocity Kinematics and the Jacobian Matrix T he forward velocity kinematics solves the prob lem of relat ing joint velocities, q, to t he end-effector velocities X
(8.28) where q is the joint variab le vector, and q
q is the joint velocity vector
= [ qn qn qn
q=
[ qn
qn
f
(8.29)
. ]T.
(8.30)
qn
qn qn
In addition, X is t he end-effector configuration vector and effector configuration velocity vector.
X
is the end-
X (8.31)
T WYn
wZ n ]
(8.32) T his prob lem is solved by finding the 6 x n transformation matrix J (q) where n is the number of joint variables. The matrix J (q) is called Jacobian. It is possible to break the forward velocity prob lem into two sub pro blems to find the t ra nslation and angular velocities of the end frame independently. (8.33) (8.34)
X = [ ~Vn W
n
]
= [
JD ] JR
q= J q
(8.35)
8. Velocity Kinematics
347
FIGURE 8.1. A planar polar manipulator . From t he forwar d kinematics we know t hat
n
0T (q) =
[ O~n O~n ]
(8.36)
and t herefo re,
J (q) = 8T (q)
(8.37)
8q
Example 211 Jacobian matrix f or a planar polar manipulator. Figure 8.1 illustrates a planar polar manipulator with the following for ward kin em atics. °T2
=
»r,
lT2
[ cosO sin () 0 0
[ NS O sin e 0 0
~ ][ ~ ~ ]
- sin () cos () 0 0
0 0 1 0
- sin () cos () 0 0
0 rcoo O ] 0 r sin () 1 0 1 0
0 0 1 0 0 1 0 0
(8.38)
The tip point of the manipulator is at
[ X ]
Y
=[ rrsm C?S ()e ]
(8.39)
and therefore, its velocity is
[ ~] =[ C?S () Y sm()
- r sin () ] [ ~ ] r cos () ()
(8.40 )
348
8. Velocity Kinematics
FIGURE 8.2. A 2R planar manipulator.
which shows that
J
= [ cos 8 D
- r sin 8 ] r cos 8 .
sin 8
(8.41)
Example 212 Jacobian matrix for the 2R planar manipulator. A 2R planar manipulator with two RII R links was illustrated in Figure 5.9 and is shown in Figure 8.2 again. The manipulator has been analyzed in Example 132 for forward kinematics, and in Examp le 168 for inverse kinematics. The angular velocity of links (1) and (2) are
OWl
0 l W2
OW2
.
0
A
.
0
A
81 k o
(8.42)
82 k l
(8.43) 0 l W2
+ (01+ 02) oko
(8.44)
+ 0l W2 (01+02) °ko
(8.45)
OWl
and
OW2
OWl
8. Velocity Kinematics
349
and the global velocity of the tip position of the manipulator is O·
O·
OWl X
°d 1 + OW2 X 7d2
d 1 + 1d 2
.) 0 ' 0 0. 1 0 k' o X h 0 21 + ( 0. 1 + O 2 ko X 12 22
h ih 0)1 Th e unit vectors 0)1 and mation m ethod,
0)2
X 12
(ih + (h) 0h
(8.46)
can be found by using th e coordinate tran sfor-
- sin 01 COS 0 1
o (8.47)
-sin(Ol +0 2 ) cos (01 + O2 )
o
(8.48)
Substituting back shows that
(8.49)
which can be rearranged to have
- h S01 - 12s (01 + O2) [ h C01 + 12C(01 + O2) JD
[
t~ ].
(8.50)
Taking advantage of the structural simplicit y of th e 2R manipulator, we may find its Jacobian simpler. Th e fo rward kin ematics of the manipulator
350
8. Velocity Kinematics
was found as
°T2
=
-s (81 + 82 ) c (81 + 82)
o o
which shows the tip position 8d2 of the manipulator is at
X ] = [ hcos8 1 +l2 cos(8 1 +8 2 ) [ Y llsin81 + l2 sin (81 +8 2 )
(8.52)
] •
Direct differentiating gives
which can be rearranged in a matrix form
or
(8.55) Example 213 Columns of the Jacobian for the 2R manipulator. Th e Jacobian of the 2R planar manipulator can be found system atically using the column -by-column m ethod. Th e global position vector of the coordinate frames are
l2 0Z2 h °Zl
(8.56)
+ l2 °Z2
(8.57)
and therefore,
°<12
°d 2 + ~W2 X ~d2 ° ° . 081 k o x (h Zl + l.z h) + 82 k: OWl X
.
0-
[ °ko x (h °h
X
l2 °Z2
+ h °Z2) °k 1 x l2°1,2 ] [
~~]
(8.58)
which can be set in the form
J
8.
(8.59)
8. Velocity Kinematics
351
8.3 Jacobian Generating Vectors The Jacobian matrix is a linear transformation, mapping joint velocities to Cartesian velocities (8.60) and is equal to J=
0' [
0
ko0><:' od n ko
(8.61)
J can be calculated column by column. The ith column of J is called Jacobian generating vector and is denoted by c, (q). .( ) _ q -
C,
[ 0'k
i- 1
x
ok'
0] .
i - 1d n
(8.62)
i- 1
To calculate the ith column of the Jacobian matrix, we need to find two vectors i.5!1 d., and 0 ki - 1 . These vectors are position of origin and the joint axis unit vector of the frame attached to link (i -1), both expressed in the base frame . Calculating J , based on the Jacobian generating vectors , shows that forward velocity kinematics is a consequence of the forward kinematics of robots. Proof. Let °d i and °di _ 1 be the global position vector of the frames B, and B i - 1 , while i - 1 d i is the position vector of the frame B, in B i - 1 as shown in Figure 8.3. These three position vectors are related according to a vector addition
(8.63) in which we have used the Equation (8.6). Taking a time derivative,
shows that the global velocity of the origin of B, is a function of the translational and angular velocities of link B i - 1 • However, (8.65)
oR',-1 . i - 1 d ,·
OW i -1 X
°R
i - 1 i-
1
di
OWi -1 X i.5!l d,
°
. 0' (}i ki - 1 X i-1 d,
(8.66)
352
8. Velocity Kinemati cs
"' x I.
FIGURE 8.3. Link (i) and associated coordinate frames.
and
o
.
i - I'
Ri - I di
ki -
. 0
di
I
i- I '
Ri -
ki -
I
I
. 0'
d i ki -
(8.67)
I
therefore,
(8.68) Since at each joint , eit her
e or d is vari abl e, we conclude that if joint i is R
(8.69)
if joint i is P.
(8.70)
or
The end-effect or velocity is then expressed by n O'
odn
=
s:
'"
n
o .' ""0 ' i - I di = ~ei ki -
i= 1
and
I
x
0 i - I dn
(8.71)
i= 1
n
ooW n = '" ~ i =1
n i -0 I Wi
=
" ' . 0k ' i- I . ~ei i= 1
(8.72)
8. Velocity Kinematics
353
They can be rearranged in a matri x form
(8.73)
We usually show t his equation by
where t he vector q = [Ill q2 and J is the Jacobian matrix. Ok' 0d J -- [ OXon 0' ko
] =Jq
(8.74)
qn ]T
is the joint velocities vector
0k'
ok' lX 0d 1 n
0]
n - 1 x n -l dn 0'
0'
k1
kn -
(8.75)
1
Practically, we find t he Jaco bian matrix column by column. Each column is a J acobian generating vector, c, (q), and is associated to joint i . If joint i is revolute , then
.( ) _ q -
Cz
[ 0k'
i- 1
0 ]
x i -l dn ki - 1
0'
(8.76)
and if joint i is prismatic t hen, c, (q) simplifies to (8.77) Equation (8.35) provides a set of six equations, the first one the t ranslational velocity of t he end-effect or along t he xo-axis . T he rest of the equations are for translational and rotational velocities of the end-effector frame along and about t he base frame axes. _ Example 214 Jacobian matrix for a spherical manipulator. Figure 8.4 depicts a spherical manipulator. To find its Jaco bian, we start with determining the °ki _ 1 axes for i = 1,2,3 . It would be easier if we use the homogeneous definitions and write,
(8.78)
354
8. Velocity Kinematics
FIGURE 8.4. A spherical manipulator.
0'
kl
°T, 'k, ~
[ cos 0, Si r'
- sin (h
0 0
- 1
easel 0
0
0
l~ ~ ][
]
[ - eas sinel0, ]
(8.79)
0 0
[ c8, cO, -Sel Ce l Se 2 0'
k2
oT 2'k = 2 2
Ce 2Sel - se 2 0
Cel 0 0
Se l Se 2 ce2 0
l~ ][!]
[ cos 0, sin 0, ] sin e l sin e 2 cas e 2 .
(8.80)
0
Then, the vectors i~l d ., must be evaluated.
0d =l o 2
0
Ok
+d 0
3
Ok = 2
[ d3 cos B, sin B, ] d3 sinel sine2 lo + d3 cas e 2 0
(8.81)
8. Velocity Kinematics
od2
= d3
I
0'
[
k2 =
~:~~~:~:~~:: d3cos 02
]
355
(8.82)
o T heref ore, the Jacobian of the manipulator is
J
=
0' [
0 k o ~ od 2 °k o
0'
k 1 ~ 01 d 2 °k 1
0'
]
k2
0
- d3 sin 01 sin O2
d3 cos 01 cos O2
d3 cos 01 sin O2
d3 cos O2 sin 01 - d3 sinB 2 - sin Ol cos 01 0
o o o 1
cos 01 sin O2 sinOl sin O2 cos O2
o o o
(8.83)
Example 215 Jacobian matrix for an articulated robot . The Jacobian matrix of a 6 DOF articulated robot is a 6 x 6 matrix. The robot was shown in Figure 6.1 and its transformation matrices are calcu lated in Example 163. Th e ith column of the Jacobian, Ci (q ), is
.( ) _
C,
q -
[ 0'k
0 ].
i - 1 X i-l d6
Ok'
(8.84)
i -I
For the first column of the Jacobian matrix, we need to find °ko and °d 6 . T he direction of the zo-axis in the base coordinate frame is
(8.85)
and the position vector of the end-effector fram e B 6 is °d 6 , which can be directly determined from th e fourth column of the transformation matrix °T6 °T6
=
°T1 IT2 2T3 3T4 4T5 5T6
[
o~
h2 h3
°d 6 1
]
=
[ tl1 t21 t22 t23 t 31 t32 t33
0
0
0
tl' t 24 ] t34
(8.86)
1
which is
(8.87)
356
8. Velocity Kinematics
where, tl4
t24 t34
d6 (S(hS(J4S(Js + C(Jl (C(J4S(J5C ((J2 + (J3) + C(J5S((J2 +bc(Jl s ((J2 + (J3) + d2s(J l + l2c(Jlc(J2
+ (J3)))
d6 (- C(JlS(J4S(J5 + S(Jl (C(J4S(J5C((J2 + (J3) + C(J5S ((J2 +S(Jl S ((J2 + (J3) l3 - d2c(J l + l2c(J2 s(Jl
+ (J3) - C(J5C((J2 + (J3)) +l2s(J2 + bc ((J2 + (J3) .
(8.88)
+ (J3))) (8.89)
d6 (C(J4 S(J5S((J2
(8.90)
Therefore, (8.91)
and the first Jacobian generating vector is
Cl
=
For the 2nd column we need to find fram e can be found by
(8.92)
°k l
and ~d6 ' The zs-oaie in the base
(8.93)
=
N6.
N6
The first half of C2 is 0 kl X The vector is the position of the end-effector in the coordinat e fram e B l , however it must be expressed in the base fram e to be able to perform the cross product. An easier method is to find 1 kl x ld 6 and transform the resultant into the base frame. The vector ld6 is the fourth column of lT6 = lT2 2T3 3T4 4T 5 5T 6 , which, from Example 163, is equal to
(8.94)
8. Velocity Kinematics
357
Therefore, the first half of C2 is
oR 1
e.h x
1d 6 )
0 ]
oR 1
( [
~
X
[l2 cos O2 + h sin (02 + 03) ]) l2 sin02 -l~~os (02 + 03)
COS01 (-h sin O2 + l3 cos (02 + 03)) ] sin01 (-l2 sin O2 + h cos (02 + 03)) [ l2 cos O2 + h sin (02 + 03)
(8.95)
and C2 is cos 01 (-l2 sin02 + h cos (02 + 03)) sin01 (-l2 sin O2 + h cos (02 + 03)) l2 cos O2 + h sin (02 + 03) sin 01 - COS01
(8.96)
o The 3rd column is made by 0 k2 and gd 6 . The vector gd 6 is the position of the end-effector in the coordinate frame B 2 and is the [ourtli column of 2T6 = 2T3 3T4 4T 5 5T6 . The Z2-axis in the base fram e can be [ousid by
(8.97) and the cross product 0 k2 x gd6 can be [ourul by transform ing the resultant of 2k2 x 2d 6 into the base coordinate frame.
(8.98)
(8.99) Therefore,
C3
is l3 COS01 sin (02 + 03) h sin01 sin (02 + 03) - h cos (02 + 03) sin01 - cos 01
o
(8.100)
358
8. Velocity Kinematics
°
The 4th column needs k3 and gd 6 . The vector forming 3 k3 to the base frame
3
can be found by trans-
0] = [ cos 0 (cos O sin 0 + cos 0 sin ( sin01(cos02sin03+sin02cos03) 1
[
°k
°k3=OR11R22R30 1
2
3
3
2) ]
-cos (02 +(3 )
(8.101) and the first half of J 4 can be found by calculating 3k 3 x 3d 6 and transforming the resultant into the base coordinate frame .
(8.102) Therefore,
C4
is
o o o
(8.103)
cos 01 (cos O2 sin 03 + cos 03 sin ( 2 ) sin01 (cos O2 sin03 + sin O2 cos ( 3 ) - cos (02 + ( 3 )
Th e 5th column needs °k4 and ~d6 ' We can find the vector °k4 by transforming 4 i. to the base frame
(8.104)
Th e first half of C5 is 4k 4
X
4d 6 , expressed in the base coordinate fram e.
(8.105) Th erefore,
C5
is
o o o
(8.106)
cos 04 sin 01 - cos 01 sin 04 cos (02 + ( 3 ) - cos 01 cos 04 - sin 01 sin 04 cos (02 + ( 3 ) - sin () 4 sin ((}2 + (}3) Th e 6th column is found by calculating °k 5 and °k 5 x
gd6 .
The vector
8. Velocity Kinematics
359
(8.107) - C81C84S(82+83)- s84(s81S84+ c81C84C(82+83)) ] - s8 1c84s (82 + 83) - s8 4 (-c8 1s8 4 + s8 1c84c (82 + 83)) [ c84c (82 +8 3 ) - ~S(82 +83)s284 an d the first half of C6 is sks X sd 6 , expressed in the base coordinate fram e.
(8.108)
The refore,
C6
is
o o o -c8 1c84s (82 + 83) - s84 (s8 1s8 4 + c81c84c (82 + 83)) -s8 1c84s (82 + 83) - s84 (-c81s8 4 + s8 1c84c (82 + 83)) c84c (82 + 83) - ~ s (82 + 83) s284
(8.109) and the Jacobian m atrix for the art iculat ed robot is calculated.
(8.110) Example 216 Th e effec t of a spherical wrist on Jacobian matrix. Th e Ja cobian m atrix for a robot having a spherical wrist is always of the form
(8.111) which shows the upper 3 x 3 subm atrix is zero. This is because of the spherical wris t structure and having a wrist point as the origin of the wrist coordinate fram es B 4 , B s , and B 6 .
*
Example 217 Ja cobian m atrix for an articulated robot using the direct different iat ing m ethod. Figure 6.1 illustrates an articulat ed robot with tran sformation m atrices given in Exampl e 163. Using the result of the fo rward kin ematics
or.6 = [ °R0 6
Od6 1
]
(8.112)
360
8. Velocity Kinemati cs
we know that the position of the end-effector is at
(8.113) where, t14 t 24 t34 =
d6 (SOl S04S05 + COl (C04S05C (02 + 03) + C05S (02 + 03))) (8.114) +l3CO l S (02 + 03) + d2sO l + l2COlC02 d6 (-COlS04S05 + SOl (C04S05C (02 + 03) + C05S(02 + 03))) +SOIS (02 + 03) b - d2COl + l2C02S0l (8.115) d6 (C04S05S (02 + 03) - C05c(02 +l2S02 + l3C(02 + 03) .
+ 03)) (8.116)
Taking the derivative of X 6 yields aX . aX . aX . - 6 01 + - 602 + .. . + - 606 oe, a02 a06 Ju ih + J 1202 + ... + J 1606 -t240l + dh (- l2 S02 + l3C(02 + 83 ) ) O2
+bCOlS(02 + 03 ) 03
(8.117)
that shows -t24 cos 01 (- l2 sin O2 + b cos (02 + 03))
l3 COS OI sin (02 + 03)
o o o.
(8.118)
Similarly, the derivative of Y6 and Z6
Y6
==
aY6 . aY . aY6 . - 6 01 + - 0 2 + .. . + - 06 ee, a0 2 a06 J2 1 81 + J22 82 + ... + J26 86
=
t140l + SOl (- l2S02 + l3C(02 + 03)) O2
=
+l3S0l S (02 + 03) 03 aZ 6 . aZ . aZ . - 6 01 + - 6 02 + ... + - 0 6 a0 2 a06 hlOl + J3202 + . .. + h606 (l2 cos 82 + ls sin (82 + 03)) 82 - l3 cos (82 + 83 ) 83
(8.119)
».
(8.120)
8. Velocity Kinemat ics
361
show that
J21
lt 4
h 2
sinOl (-l 2 sin02 + l3 cos (02 + ( 3))
J23
b sinOl sin (02 + ( 3)
J24
0
J25
0
J26
0
h I hz
(8.121)
h 3
0 lz cos Oz + b sin (O z +( 3) - b cos (Oz +( 3)
h4 h5 J36
0 0 O.
(8.122)
Since there is no explicit equation for describing the rotat ions of the end-effect or 's fram e about the axes, there is no equation to find differential rotat ion s about the three axes. This is a reason fo r searching indi rect or m ore systematic m ethods fo r evaluating the Jacobian matrix. However, the n ext three rows of the Ja cobian m atrix can be found by calculating the angular velocity vector based on the angular velocity matrix 0 OW6
=
R6 0 R 6T
O '
=
OW,
- Wz
WZ [ - Wy
0
(8.123)
Wx
~ ~:J [
(8124)
an d then rearranging the components to show the Jacobian eleme nts.
Wx Wy Wz
awx ' awx ' awx ' --0 1 + --Oz + .. .+ --(h aOl a02 a06 awy . awy . awy . --0 1 + --Oz + ... + --0 6 oe, aOz a06 awz · awz· awz · - 0 1 + - Oz + .. .+ - 06 aO l aOz a06
(8.125) (8.126) (8.127)
Th e angular velocity vector of the end-effec tor fram e is wx
=
sin 018z
+ sin 0183 + cos 01 sin 02384
+ (cos 04 sin 01 - cos 01 sin 04 cos ( 23) 85 - (C01 C04S0Z3 + S04 (SOl S04 + C0 1C04C(23)) 86
(8.128)
362
8. Velocity Kinemati cs Wy
=
-COS8182- Cos8183+sin81sin82384 + (- cos 8 1 cos 84 - sin 81 sin 84 cos (2 3) 8 5 + (-S81 C84S823 - s84 (- C81S84 + S81C84 C(23)) 86 (8.129)
Wz
8 1 - cos (82 + ( 3) 84 - sin 84 sin (8 2 + ( 3) 8 5 + (cos 84 cos 823 -
~ sin 823sin 2(4) 86
(8.130)
and therefore,
J41 J 42
o
J43 J44
sin81
J45
cos 84 sin 81 - cos 8 1 sin 84 cos (82 + ( 3)
J46
sin8 1 cos 8 1 (cos 82 sin 83 + cos 8 3 sin ( 2)
- C81 C84 S (82 + ( 3) -s84 (S81 S84 + C81C84c(82 + ( 3))
=
(8.131)
o - COS81
- cos e 1 sin 81 (cos 82 sin 83 + sin 82 cos ( 3) - cos 81 cos 8 4 - sin 81 sin 84 cos (82 + ( 3)
- S81 C84S (82 + (3) - s84 (-c8 1s84 + s81c84c (82 + ( 3))
(8.132)
1
o o - cos (82 + ( 3) - sin 8 4 sin (82 + ( 3) cos 84 cos (82 + ( 3) -
1
"2 sin (82 +
( 3) sin 284.
(8.133)
*
Example 218 Analytical Jacobian and geometrical Jacobian . A ssume th e global position and orientation of the end-effec tor fram es are specified by a set of six parameters (8.134)
8. Velocity Kinematics
363
where
(8.135) are three independent rotational param eters, and
(8.136) is th e Cartesian position of the end-effe ctor fram e, both functions of the jo int vari able vector, q . Th e translational velocity of the end-effe ctor fram e can be expressed by
o
rn
orq. = J D (q ).q = oq
(8.137)
and the rota tional velocity of the end-effector fram e can be expressed by 0 ).
-r «
. J ( ). = o¢ oq q = 4> q q .
(8.138)
Th e rotational velocity vector ;p in genera l differs from the angular velocity vector w . Th e combination of the Jacobian matrices J D and J in the form of
(8.139) is called analytical Jacobian to indicate its difference with geometrical Jacobian J . Having a set of orientation angles, ¢ , it is possible to find the relationship between the angular velocity w and the rotational velocity ;P . A s an exam ple, consider the Eul er angles rpB'lj; about zxz axes defin ed in S ection 6.3. Th e global angular velocity, in terms of Eul er frequ enc ies, is found in 2.81
[~~wz ] w
[ ~1 ~
