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 ~~:; 0
sin Bsin sp ] [
T( ¢) ;P.
(8.140) (8.141)
8.4 Inverse Velocity Kinematics Th e inve rse velocity kin ematics problem , also known as t he resolved rates problem, is searching for the joint velocity vector corresponding to t he endeffect or velocity vector. Six DOF are needed to be able to move t he endeffector in an arb itrary direction with an arbitrary angular velocity. The velocity vector of the end-effecto r X is related to the joint velocity vector q by th e J acobian matrix
364
8. Velocity Kinematics
(aJ FIGURE 8.5. Singular configurations of a 2R planar manipulator.
(8.142) Consequently, for the inverse velocity kinematics, we require the differential change in joint coordinates expressed in terms of the Cartesian translation and angular velocities of the end-effector. If the J acobian matrix is nonsingul ar at the moment of calculation, the inverse Jacobian J - 1 exists and we are able to find t he requir ed joint velocity vector as (8.143) Singular configuration is where the determinant of the Jacobian matrix is zero and therefore, J- 1 is indeterminate. The Equation (8.143) det ermines the velocity requir ed at the individual joints to produce a desired endeffector velocity X. Since the inverse velocity kinematics is a consequence of the forward velocity and needs a matrix inversion, the probl em is equivalent to th e solution of a set of linear algebraic equations. To find J - 1 , every matrix inversion method may be appli ed. Example 219 Inv erse velocity of a 2R plana r manipulator. Forward and inverse kin ematics of a 2R planar manipulato r have been analy zed in Example 132 and Example 168. It s Ja cobian and forward ve-
8. Velocity Kinematics
365
locity kin ematics are also found in Example 212 as
Jq -hS01 - 12s (01 + O2) [ h C01 + 12 c (01 + O2) The inverse velocity kin ematics n eeds to find the in verse of the Jacobian . Therefore,
J-1X
-h S01 - 12s (01 + O2) [ hC01 +12C(01 +0 2) where,
and henc e,
ih
=
Xc(01 + O2 ) + Ys (01 + O2 ) h S02
82 = X (hC01 + 12C(01 + O2)) + Y(hS01 + bs (01 + O2)) . -h 12 s02
(8.149)
(8.150)
Singularity occurs when the determinant of the Jacobian is zero. (8.151)
Th erefore, the singula r configurations of the manipulator are O2
= 180deg
(8.152)
corresponding to the fully extended or fully contra cted configurations, as shown in Figure 8.5. At the singular configurations, the value of 0 1 is in determinate and may have any real value. Th e two columns of the Jacobian matrix become parallel because the Equation (8.145) becomes
(8.153)
In this situation, the endpoint can only mo ve in the direction perpendicular to the arm links .
366
8. Velocity Kinematics
Example 220 Analytic method for inverse velocity kinematics. Theoretically, we must be able to calculate the joint velocities from the equations describing the forward velocities, however, such a calculation is not easy in a general case. As an example , consider a 2R planar manipulator shown in Figure 8.2. The endpoint velocity of the 2R manipulator was expressed in Equation (8.58) as O·
.
0'
d 2 = fh ko x (h
0, 21
+h
0, 22)
.
+ O2
0'
kl
X l2
0, 22·
(8.154)
A dot product of this equation with °i 2 , gives
(8.155)
and therefore,
o;d · 0'22 . 2 01 = ....,....-"-"""";:' h sin O2 .
Now a dot product of (8.154) with
Oil
(8.156)
reduces to
and therefore, (8.158)
*
Example 221 Inverse Jacobian matrix for a robot with spherical wrist . The Jacobian matrix for an articulated robot with a spherical wrist is calculated in Example 215. (8.159)
The upper right 3 x 3 submatrix of J is zero. This is because of spherical wrist structure and having the last three position vectors as zero. Let us split the Jacobian matrix into four 3 x 3 submatrices and write it as (8.160)
8. Velocity Kinematics
367
where
[AJ
(8.161)
[C]
(8.162)
[D]
(8.163)
Inversion of such a Ja cobian is simpler if we tak e advantage of B = O. Th e forward velocity kin ematics of the robot can be written as
X = Jq
(8.164)
Th e upper half of the equation is
(8.165) which can be inverted to
(8.166) Th e lower half of th e equation is
e, [C D]
[G1 [
fh fh
04 05 06
i:] i:] + [D] [
(8.167)
and th erefore,
(8.168)
368
8. Velocity Kinematics
8.5 Summary Each link of a serial robot has an angular and a translat ional velocity. The angular velocity of link (i) in th e global coordinat e frame can be found as a summation of th e global angular velocities of its lower links
(8.169) Utilizing DH paramet ers, th e angular velocity of link (j) with respect to link (j - 1) is if joint i is R (8.170) if joint i is P. Th e translational velocity of link (i) is the global velocity of th e origin of coordinate frame B, attached to link (i) if joint i is R if joint i is P
(8.171)
where () and d are DH parameters, and d is th e frame's origin position vector. Th e velocity kinematics of a robot is defined by the relationship between joint coordinate velocity
qn ] T
(8.172)
and global velocity of the end-effector
(8.173) Such a relati onship introduces Jacobian matrix [J]
X=Jq.
(8.174)
Having [J] , we are able to find th e velocity of th e end-effector for a given joint coordinate velocity and vice versa. Jacobi an is a function of joint coordinates and is the main tool in velocity kinematics of robots . Th eoretically, [J] is defined by
J (q) =
aT (q) aq
(8.175)
where T (q) = »i; (q) is th e t ransformation matrix for the end-effector
(8.176)
8. Velocity Kinematics
However practically, we calculate noted by c, (q )
.( ) _ q -
C,
369
[J] by Jacobian generating vectors de-
[ 0k'
i- 1 0'
x
ki -
0 ]
i- l d n
(8. 177)
1
where c, (q ) makes the column i- I of t he J acobian matrix.
J =
[ co
Cl
C2
...
Cn- l ]
(8.178)
8. Velocity Kinematics
371
Exercises 1. Notation and symbols. Describe the meaning of a-
i - 1d i
b-
i -01 d ,
g-
i- 1 d i
h-
i -1 di
m-
°ki
°.
0'
n- ki -
1
i-!l d i
d-
. i d 1- i - 1 i
J-
c-
°'
.
0- i - 1 ki - 2
p-
i - 1d i i -1
e-
i- 1 d i-1 i
f
i - 1d ·
i- i ' i d i- 1
k-
i - 1d · z-l z
1-
i e / d i'
q-
i i -1 V i
r- i V i
°
i - 1Vi
-°
z
2. 3R planar manipulator velocity kinematics. Figure 5.21 illustrates an R IIRIIR planar manipulator. The forward kinematics of the manipulator provides the following transformation matrices:
'TF
[ ~7: ~~;o~3 ~ ~:~f::
]
'T,
~ [ ~1:: ~~fo~' ~ ::~f:: ]
"T,
~ ~1:: -~t ~ ~:
l
1::]
Calculate the Jacobian matrix, J, using dire ct differentiating, and find the Cartesian velocity vector of the endpoint for numerical values.
81 82 83
ih 82 83
56deg - 28 deg - lO deg
h
100cm
l2
55cm
b
30cm 30deg / sec 10deg / sec - 10 deg / sec
372
8. Velocity Kinematics
3. Spherical wrist velocity kinematics. Figure 5.22 illustrat es a schematic of a spherical wrist . The associated transformation matrices are given below. Assume that t he frame B 3 is the base frame. Find the angular velocity vector of the coordinate frame B 6 . 0 - sin 84 [ 0059, sin 84 0 cos 84 3T4 = - 1 0 0 0 0 0 [ cos95 4T5
sin 85 0 0
=
'T, ~
[
COO06
Sir'
0 0 1 0
sin8 5 - cos 85 0 0
- sin8 6 cos 86 0 0
o
~] ~]
0]
0 1 0 o 1
4. Spher ical wrist and tool's frame velocity kinematics. Assume that we attach a tools coordinate frame, with the following transformatio n matr ix, to the last coordinate frame B 6 of a spher ical wrist.
The wrist transformation matrices are given in Exercise 3. Assume that the frame B 3 is the base frame and find the translational and angular velocities of the tools coordinate frame B 7 . 5. SCARA manipulator velocity kinematics. An RIIR!!RIIP SCARA manipulator is shown in Figure 5.27 with the following transformation matrices . Calculate the Jacobian matrix using the Jacobian-generat ing vector techn ique. [ 005 B,
°T1 =
'T, ~
sin 81 0 0
[ coon,
Si1
B2
- sin 8l cos 8 1 0 0
0 l,cooO, ] 0 h sin8 1 1 0 1 0
(8.179)
- sin 82 cos 82 0 0
0 l2 cos 82 0 l2 sin 82 1 0 1 0
(8.180)
]
8. Velocity Kinematics
l
373
- sinB3
cos(h sinB3
COSB3
o o
(8.181)
o
o
(8.182)
6. Rf-RI IR articulated arm velocity kinematics. Figure 5.25 illustrates a 3 DOF Rf-RIIR manipulator with the following transformation matrices. Find the J acobian matrix using direct differentiating, Jacobian-generating vector methods.
°T1
~
Sht
[ oosO, IT2
=
0 - sinBI cos8 1 0 - 1 0 0 0
[ COOOI
- sin 82
cosB2 0 0
sinB 2 0 0
[ COO03
'7:3 ~ Sir'
0 0 1 0
l2 l2
0 sinB 3 0 - cosB3 1 0 0
7. Rigid link velocity.
0
{I
1
cos 82 sin 82 d2 1
1
~1
Figure 8.6 illustrates the coordinate frames and velocity vectors of a rigid link (i) . Find
di - l in terms of di and di - l
(a) velocity "v, of the link at C, in terms of d, and (b) angu lar velocity of the link (c) velocity
°Vi
° Wi
of the link at C, in terms of proximal joint i velocity
(d) velocity "v , of the link at C, in terms of distal joint i + 1 velocity (e) velocity of proximal joint i in terms of distal joint i + 1 velocity (f) velocity of distal joint i 8.
+ 1 in terms of proximal joint i
velocity
* Jacobian of a PRRR manipulator. Determine t he Jacobian matrix for the manipulator shown in Figure 6.5.
374
8. Velocity Kinematics
d t-. }
x
y
FIGURE 8.6. Rigid link velocity vectors. 9.
* Spherical robot velocity kinematics. A sph erical manipulator Rf-Rf-P, equipped with a spherical wrist , is shown in Figure 5.26. The t ra nsformation matrices of the robot are given in Example 149. Solve the robot's forward and inverse velocity kinemati cs.
10.
* Space station remot e manipulator syst em velocity kinemat ics. The transform ation matrices for the shut tle remot e manipulator system (SRMS) , shown in Figure 5.28, are given in the Example 153. Solve the velocity kinematics of the SRMS by calculat ing the J acobian matrix.
9 •
Numerical Methods Kinematics
In
9.1 Linear Algebraic Equations In robotic ana lysis, there exist problems and situations, such as inverse velocity kinematics, that we need to solve a set of coupled linear or nonlinear algebraic equations. Every numerical met hod of solving nonlinear equations also works by iteratively solving a set of linear equations. Consider a system of n linear algebraic equations with real constant coefficients,
+ a12 X2 + a21 Xl + a22 X2 +
+ alnXn + a2nXn
allXl
b1 b2
(9.1) which can also be writ ten in matrix form
(9.2)
[A]x =b.
There are numerous methods for solving t his set of equations. Among the most efficient met hods is the LU factorization met hod . For every nonsingular matrix [A] there exists an upper tr iangular mat rix [U] wit h nonzero diagonal elements and a lower triangular matrix [L] with unit diagonal elements, such that
[A] = [L] [U] a12
(9.3)
a'n ]
[ a21 an
a22
a2n
an l
a n2
ann
~ l~~
0 1
[A] =
ILJ [
lnl
IUJ ~
[ Un
.~
ln2
:J
U1 2
Uln
U22
U2n
0
Unn
]
(9.4)
(9.5)
(9.6)
376
9. Numerical Methods in Kinematics
The process of factoring [AJ into the [L] [U] is called LU factor ization. Once t he [L] and [U] matrices are obtained, t he equation
[L] [U]x = b
(9.7)
can be solved by transforming into
[L]y= b
(9.8)
[U] x = y.
(9.9)
and Since t he Equations (9.8) and (9.9) are both a triangular set of equations, their solut ions are easy to obtain by forward and backward substitutio n. Proof. To show how [A] can be transformed into [L] [U], we consider a 4 x 4 matrix. a ll a21 [
a12 a22
a13 a23
a14 ] a24
a31
a 32
a33
a34
a4 1
a42
a43
a44
[ 1
hi
0 1 lez
0 0 1
l41
l 42
l43
l21
-
0] 0 0 1
[u0n
U12 U22
U13 U2 3
0 0
U 33
U34
0
U44
0 0
U14 ] U24
(9.10) Employing a dummy matrix [B], we may combine the elements of [L] and [U] as (9.11)
The elements of [B] will be calculated one by one, in the following order :
(1) (2) (3) (4) ] (5) (8) (9) (10) [B] = (6) (11) (13) (14) [ (7) (12) (15) (16)
(9.12)
T he process for generat ing a matrix [B], associated to an n x n matrix [A], is performed in n - 1 iterations. After i - I iter ations, the matrix is in t he following form: Ul ,l
Ul ,2
l 2,1
U2 ,2
Ul ,i-l
Ul ,n U2 ,n
[B]=
Ui -l ,n
Di In ,l
In ,2
In ,i -l
1 I J
(9.13)
9. Numerical Methods in Kinematics
377
The unprocessed (n - i + 1) x (n - i + 1) submatrix in the lower right corner is denoted by [Di ] and has the same elements as [A]. In the ith step, th e LU factor ization meth od converts [Dd
[Di ] =
[ dii
-t
[Hi~l]
]
Si
(9.14)
[Di ] =
[ Ui i
u Tt [Di +1 ]
]
(9.15)
to a new form Ii
.
Direct mult iplicatio n shows that Ull
a ll
U12
a 12
U 13
a1 3
U1 4
a1 4
(9.16)
a2 1
b
Ull a 31
l31
Ull a4 1
l41
(9.17)
Ull l21 U 12
U22
a 22 -
U2 3
a 23 - l 2 1U1 3
U 24
a24 - l 21U1 4
(9.18)
a32 - l3 1U 12 U22 a42 -
l4 1U 12
(9.19)
U22
a33 a 34 l 43
=
a43 -
+ h2 u23) (l 31U1 4 + l 32 U 24 )
(hI U1 3
(9.20)
+ 142 u 23 )
(9.21)
+ 14 2 u 24 + 143 u 34 ) '
(9.22)
( 141 U13 U3 3
U44
=
a44 -
(14 1U14
Th erefore, the general formula for getting elements of [L] and [U] corresponding to an n x n coefficients mat rix [A] can be written as i- I
Ui j = a i j -
L
k= 1
l ikUk j
j
= 1, · · ·n
(9.23)
378
9. Numerical Methods in Kinematics
j
~
i = 1, ·· · n.
i
(9.24)
For i = 1, the rule for U reduces to
(9.25) and for j = 1, the rule for l reduces to li l
_ -
a il .
(9.26)
Un
Th e calculation of element (k) of th e dummy matrix [B], which is an element of [L] or [U], involves only the elements of [Aj in the same position and some previously calculated elements of [B] . The LU factorization technique can be set up in an algorit hm for easier numerical calculat ions.
Algorithm 9.1. LU factori zation technique for an n x n matrix [Aj . 1- Set the initial count er i = 1. 2- Set [D1 ] = [A] . 3- Calculat e [D i H ] from [Dd according to
di i r Tz 1
Ui i
uT
•
i,
[Di H
(9.27) (9.28) (9.29)
- Si U ii
[Hi + 1 ]
]
-
t, uT.
(9.30)
4- Set i = i + 1. If i = n then LU fa ctorizat ion is completed. Otherwis e return to step 3. After decomposing th e matrix [A] into the matrices [L] and [U], the set of equat ions can be solved based on th e following algorithm.
Algorithm 9.2 . LU solution technique. 1- Calculat e y from [L] y = b by Yl
b1
Y2
bz - Ylb
Y3
b3 - Yl b - Y2l32
Yi
b, - LYjlij .
i- I
j=1
(9.31)
9. Numerical Methods in Kinematics
379
2- Calculate x from [U] x = y by Yn Un ,n
Yn-1 - X n Un - 1,n
X n-1
Un-
~
Xi
1,n - 1
t
( Yi -
Un
XjU ij )
.
(9.32)
j =i+1
• Example 222 Solution of a set of four equations. Consider a set of four linear algebraic equations
(9.33)
[A] x=b
where,
[AI
~[~
1 0
2 1
3 -1 2
-- 32
j
1 -2
0
(9.34)
and
[JJ
b=
(9.35)
Following the LU factorization algorithm we first set i
1
(9.36)
[A]
(9 .37)
to find
8,
[Hz]
=
2
(9.38)
[ 1 3 - 3]
(9 .39)
~ ~
02 [ 1
[
(9.40)
]
1 2] -2 -1 0
-2
(9.41 )
380
9. Numerical Methods in Kinematics
and calculat e d ll
ri h = _1
=2 = [1 3 -3] 81
U ll
=[
(9.42) (9.43)
~]
(9.44)
3
2
[D,I
~ [H,I - I, ur ~
[!: !: -~ ] 1
.
(9.45)
5
2
In the second step we have
i=2
(9.46)
and (9.47) (9.48) (9.49)
(9.50)
and calculat e U 22
ur rr =
h
1 2
(9.51 )
-~]
(9.52)
= d22 = - =
[- ~
1
[ -4 ] 1
= U 22 8 2 =
[D3 ] = [H3 ] - h u T2 =
[-8 -2
(9.53)
-1 ]
3
.
(9.54)
In the third st ep we set
i= 3
(9.55)
and find
d33
-8
(9. 56)
rf
[-1]
(9. 57)
[-2]
(9.58)
[H4 1 = [3]
(9.59)
83 =
9. Numerical Methods in Kinematics
381
and th erefore, U33
(9.60)
= d33 = -8
ur= rr= [-1] h=
(9.61)
[~]
_1 8 3 =
(9.62)
4
U33
[D4 ] = [H4 ] - 13 u T3 =
[1 43] .
(9.63)
After th ese calculations, the matrix [B], [L], and [U] become
IBI~P
3 5 -2 -8
1 1 - 2
4
3
1
1
2
l ~ l~ ~ ~
0 1
ILJ
IUJ
4"
-4
0 0 1
1
4"
1 1 -2 0 0
1
3 5 - 2 -8 0
=:1 - 1
(9.64)
13
'4
~j =: j -1
(9.65)
.
(9.66)
13
'4
Now a vector y can be found to satisfy
y
[L] y =b
(9.67)
~
(9.68)
[
i1 13
- 2
and finally the unknown vector x should be found to satisfy
[U] x =y
(9.69)
(9.70)
Example 223 LU factori zation with p iv otin g. In the process of LU factori zation, the situation U ii = 0 gen erat es a division by zero, which must be avoid ed. In this situation, pivoting must be applied. By pivoting, we chang e the order of equations to have a coeffic ient
382
9. Numerical Methods in Kinematics
matrix with the largest eleme nts, in absolut e value, as diagonal elem ents . Th e largest eleme nt is called the pivot element. A s an example, consider the fo llowing set of equations:
[A] x
[ ~ ~ ~1
}: ]
b
(9.71)
Ui ] [JJ
(9.72)
however, we mo ve the largest elem ent to d l l by interchanging row 1 with 4, and column 1 with 3.
[ ~ ~ ~1 [
•
~1 ~ ~
}: ] [
~i ]
[1
(9.73)
~~ ]
1
(9.74)
2
]
2
[
]
Then the largest element in the 3 x 3 submatrix in the lower right corner will move to d 22
[
~1
[J
- 2 3 2 1 1 0 -3 2
~ ] U~ ] [r]
(9.75)
-2 -3
~ ][ ~~ ] [n
(9.76)
1
3 2 0
2
1
=
and finally the largest eleme nt in the 2 x 2 in the lower right corne r will mo ve to d22 .
l
~ ~q ~ ]
- 1
2
0
1
l::] l~2 =
Xl
]
(9.77)
2
To apply the LU fa ctorization algorithm and L U solution algorithm, we
9. Numerical Methods in Kinematics
383
define a new set of equations. [A'] x'
[~
-1
b'
(9.78)
~q ~ ] [:1] [T] 2
x~
0 1
(9.79)
2
Based on the LU factorization algorithm, in the first step we set i= l
(9.80)
[A' ]
(9.81) (9.82) (9.83)
and find
[D 1 ] = d l1
=
rf
4
[ -2 1 3]
(9.84)
(9.85) to calculate d ll = 4
rf =
(9.86) (9.87)
[-2 1 3 ]
(9.88)
-io ] .
(9.89)
7
4"
(9.90)
i= 2
and 3
(9.91)
2
rf
=
[i
-i ]
(9.92)
384
9. Numerical Met hods in Kinematics
(9.93) (9.94) and th en
3 2
= d22 = - -
U 22
(9.95) (9.96)
h = -1
82
U 22
= [ _ 13 ] -1
(9.97)
13
T
[D3 ] = [H3 ] - 12 U 2 =
-
;
[
(9.98)
In th e third st ep, we set
i=3
(9.99)
and find
r§
13 6
(9.100)
[-~]
(9.101)
=
83
[~]
=
[~]
(9.103)
13 d33 = 6
(9.104)
[H4 ] = an d calculate U 33 =
u§ = r§ = 13
(9.102)
[-~]
(9.105)
= _1 8 3 = [~] 13
U 33
[D 4 ] = [H4 ] - h u§ =
[~~] .
(9.106)
Th eref ore, th e matri ces [L ] and [U J are
[L] =
[
1
0
8
~1
1
- '4
3
- 1
(9.107)
9. Numerical Methods in Kinematics
[U]=
[~
1 1
~
"6
o
~J ]
385
(9.108)
13
and now we can find the vector y
[L]y=b'
(9.109)
(9.110)
Th e unknown vector x' can then be calculated,
[U]x'=y
(9.111)
(9.112)
and therefore,
(9.113)
*
Example 224 Uniqueness of solution. Cons ider a set of n lin ear equati ons, [A] x = b. If [A] is square and non1 singular, then there exists a uniqu e solution x = [Ar b . However, if the linear system of equati ons inv olves n variables and m equati ons aU x1 a21 X1
+ a12 X2 + + a 22 x 2 +
+ a1n X n + a 2n X n
h b2
(9.114) then, three classes of solut ions are possible. 1. A uniqu e soluti on exists and the system is called consistent.
2. No solution exists and the system is called in consistent . 3. Multiple solutions exist and the system is called und eterm in ed.
386
9. Numerical Methods in Kinematics
*
Example 225 III conditioned and well conditioned. A system of equations, [Aj x = b, is considered to be well conditioned if a small change in [Aj or b results in a small change in the solution vector x . A system of equations, [Aj x = b , is considered to be ill conditioned if a small change in [AJ or b results in a big change in the solution vector x. The syst em of equations is ill conditioned when [Aj has rows or columns so nearly dependent on each other. Consider the following set of equations:
(9.115) The solution of this set of equations is Xl ] [ X2
= [ -1.0 ] .
1.0
(9.116)
Let 's make a small change in the b vector 2 3.99 ] [ [ 1 2
Xl ]
= [ 1.98 ] 1.01
X2
(9.117)
and see how the solution will change. [
~~
~9 ]
] = [ - :..
(9.118)
Now we make a small change in [Aj matrix
[~:~~ ~:~~] [ ~~
] = [ 1.;9 ]
(9.119)
and solve the equations Xl ] _ [ X2 -
[
0.1988 ] 0.3993 .
(9.120)
Therefore, the set of equations (9.115) is ill conditioned and is sensitive to perturbation in [AJ and b. However, the set of equations (9.121)
is well conditioned because small changes in [Aj or b cannot change the solution drastically. The sensitivity of the solution x to small perturbations in [Aj and b is measured in terms of the condition number of [Aj by
IIL>xll < con (A) IIL>AII Ilxll IIAII
(9.122)
9. Numerical Methods in Kinemati cs
387
where con (A) =
IIA-IIIIIAII .
(9.123)
and IIAII is a norm of [A] . If con (A) = 1. Then [A] is called perfectly condition ed. The matrix [A] is well conditi oned if con (A) < 1 and it is ill condition ed if con (A) > 1. In fact , the relative change in the norm of the coefficient matrix, [AJ, can be amplifi ed by con (A) to make the upper limit of the relati ve change in the norm of the solut ion vector x. Proof. Start with a set of equati ons
[Aj x = b
(9.124)
and change the matrix [Aj to [A']. Th en the solution x will change to x' such that [A'lx' =b. (9.125) Th erefore,
[A'] x'
[A] x
([A]
+ 6 A) (x + 6x)
(9.126)
where 6A
6x
[A']-[A] x'-x.
(9.127) (9.128)
Expand ing {9.126}
[AJ x
=
[AJ x + [AJ L:.x + L:.A (x + 6 x)
(9.129)
and simplif ying (9.130)
shows that
116xll ~
IIA- 1 11116 All llx + 6 x ll ·
(9.131)
Multiplying both sides of {9.131} by the norm IIAllleads to Equat ion {9.122}.
•
*
Example 226 No rm of a matrix. Th e norm of a matrix is a scalar positive number, and is defin ed for every kind of matrices including square, rectangular, in vert ible, and noninverti ble. Th ere are seve ral definitions fo r the norm of a matrix. Th e most important ones are n
IIAIII =
M ax
L laij l
(9.132)
I ::OJ ::o n i=1
IIAI12= }.Max (ATA)
(9.133)
388
9. Numerical Methods in Kinematics (9.134) n
IIAIIF = L
n
L aTj'
(9.135)
i= l j=l
Th e norm- infinity, IIAll oo ' is the one we accept to calculate the con (A) in Equat ion (9.1 23). Th e norm-infinity, IIAll oo ' is also called the row sum norm and uniform norm. To calculate IIAll oo ' we find the sum of the absolut e of the eleme nts of each row of the matrix [A] and pick the largest sum. As an example, the norm of (9.136)
is n
IIAll oo
t1.i~x,. L laij I - -
j=l
Max {(Ill + 1 31 Max{7,4,8}
+ 1-31) ,(1-11 + 1-11 + 121) ,(121 + 141 + 1- 21 n
8.
(9.137)
W e may check the follo wing relations between norms of matrices.
II[A] + [B]II < IHA]II + II[B]II II[A] [B]II :::; II[A]IIII[B]11
(9.138) (9.139)
9.2 Matrix Inversion There are num erous techniques for matrix inversion. However t he method bas ed on the LU factorization can simplify our numerical calculations since we have already appli ed the method for solving a set of linear algebrai c equ ations. Assum e a matrix [A] could be decomposed into
[A] = [L][U]
(9.140)
where (9.141)
9. Numerical Methods in Kinematics
:]
o 1
[L] =
Un
389
(9.142)
n
lUI ~:
Ul ] U2n
[
o
(9.143)
Un n
then its inverse would be (9.144)
Because [L] and [U] are tri angular matrices, their inverses are also triangular. The elements of the matrix [M]
1
0
~~I
.'
mnl
m n2
l
[M] = [Lr = [
(9.145)
are i- I
m ij
=
- li j -
L
j
lik m kj
i = 2,3, · · ·n-1
(9.146)
k=j+1
and the elements of th e matrix [V] VI 2
VI 3
V22
Vn
o o
V33
(9.147)
0
are 1
j
=i
i = n , n - 1, . . . , 1
j
2: i
i=n-1 ,· · · ,2 .
Uij
-1
-
(9.148)
j
L
U ik V k j
U i i k= i+1
Example 227 Solution of a set of equations by matrix inversion. Consider a set of four linear algebraic equations. (9.149)
(9.150)
390
9. Numerical Methods in Kinemat ics
Following the L U factorization algorithm, we can decompose the coefficient matrix to [A] = [L][U] (9.151) where
[i
[L] =
~ ~
[UI [
0 1 - 4 1
~]
0 0 1 1
4
1
3
1
=:]
5
- 2 - 2 0 0
-1
-8 0
(9. 152)
.
(9.153)
13
"4
The inverse of matrices [L ] and [U] are
[Lr '
~ [ !~
- 2 1
- 2
0 1 4 -2
0 0 1 1
-4
~]
(9.154)
(9.155)
and therefore the solution of the equations is (9.156)
(9.157)
*
Example 228 LU factorization method compared to other methods. Every nonsingular matrix [A] can be decomposed into lower and upper triangular matrices [A] = [L] [U]. Then, the solution of a set of equations [A]x = b is equivalent to [L] [U]x = b . (9.158) Multiplying both sides by L -
1
shows tha t
[U]x = [Lr
1
b
(9. 159)
and the problem is broken into two new sets of equations
[L]y = b
(9.160)
9. Numerical Methods in Kinematics
391
40
30 :::::>
i
c.J
20
10
0
4
3
n FI GURE 9.1. T he number of calculatio ns for t he Gaussian eliminatio n subtracted by t he LU factorization methods, as a function of t he size of t he matrix.
and
[U]x=y.
(9.161)
Th e computational time required to decompose [A ] into [L] [U] is proportional to n 3/3 , where n is the number of equations. Then, the computational tim e for solving each set of [L] y = b and [U] x = y is proportional to n 2 /2. Therefore, the tota l compu tational time for solving a set of equations by the L U f actorization me thod is proportional to n 2 + n 3 /3 . However, the Gaussian elimination me thod takes a computational time proportional to n 2 /2 + n 3 /3 , forward elimination takes a time proportional to n 3 /3, and back substitution takes a time proportional to n 2 /2 . On the other hand, the tota l computational time required to inverse a ma trix using the L U factorization method is proportional to 4n 3 /3 . However, the Gaussian elimination method needs n 4 /3 + n 3 /2 , and
n > 2.
(9 .162) 4
3
3
Figure 9. 1 depicts a plot of the function G - LU = r~ + r~ - 4~ and shows how fas t the number of calculations for the Gaussian elimination, compared to the LU factorization methods, increases . As an examp le, for a 6 x 6 matrix inversion, we need 540 calculations for the Gaussian elimination method, compared to 288 calculations for the L U factorization method.
*
Example 229 Partitioning inverse method. Assume that a matrix [T] can be partitioned into
[T] =
[ ~ ~]
(9.163)
392
9. Numerical Methods in Kinemati cs
then, T- 1 can be calculat ed by
(9.164) where
[E]
[A - BD- 1
[H]
[D -A-
[F] [G]
r
1
C A - 1Br 1 1BH
-D- 1CE .
(9.165) (9.166) (9.167) (9.168)
Sometimes, it is a shortcu t inve rse m ethod .
*
Example 230 Analyt ic inversion m ethod . If th e n x n matrix [A] = [aij ] is n on-singular, that is det( A)
i= 0, we may com pute the inverse, A - I , by dividing the adjoint matrix A a by the determin ant of [A]. A -I
=~ det(A)
(9.169)
Th e adjoint or adjugate matrix of the matrix [AJ, is the transpose of the cof actor matrix of [A ].
A a = AcT
(9.170)
Th e cofactor matrix, denoted by A C, for a matrix [AJ, is made of the matrix [A] by replacing each of its eleme nts by it s cofactor. Th e cofactor associated with the eleme n t aij is defin ed by
(9.171) where A ij is th e i j -m inor of [A] . A ssociated with each element aij of the matrix T , th ere exists a minor A ij which is a number equal to the value of th e determinant of the subm atrix obtain ed by deleting row i and colum n j of th e matrix [A ]. Th e determinant of [A] is calculated by n
det (A) =
L aijA~j .
(9.172)
j=1 Th erefore, if
[A] =
[
a ll
a12
a21
a 22
a31
a32
(9.173)
th en th e elements of adjoint matrix Aa are A a = A C = (-1) 21 a22 11 11 a32
(9.174)
9. Numerical Methods in Kinematics
393
(9.175)
A33 = A~h = (-1) 61
(9.176)
and the determinant of [A] is det (A)
= a11a22a33 - a11a23a32 - a12a21a33 +a12a31a23 + a21 a13 a32 - a13 a22 a31 ·
(9.177)
As an example, consider a 3 x 3 matrix as below
The associated adjoint matrix for [A] is - 28
AcT = - 28 38 [ 24
~4
[
44 - 69 18
and the determinant of [AJ is det [A]
= 260
and therefore,
[Ar
1
Aa det(A)
[-lis
11
1
6g g
~~
1~0
-g60
26PI
65
130
-130
]
*
Example 231 Cayley-Hamilton matrix inversion. The Cayley-Hamilton theorem says: Every non-singular matrix satisfies its own characteristic equation. The characteristic equation of an n x n matrix [A] = [aij] is det (A - AI)
IA- >..II P(A) = An + an_1An-1
+ ... + al A + ao = O.
(9.178)
394
9. Numerical Methods in Kinematics
Hence, th e charact eristic equation of an n x n matrix is a polynomial of degree n . Ba sed on the Cayley-Hamilton theorem, we have (9.179)
Multiplying both sides of this polyno mial by A-I and solving for A-I provides (9.180)
Th erefore, if (9.181)
and n
det (A) =
L aijAfj
(9.182)
j=1
then th e characteristi c equation of [A ] is
P(A) = det (A - AI) = A3 + (-all -
+ (alla 22 -
a 12 a2 1
+ alla33 -
a22 - a 33) a1 3 a 31
A2
+ a 22a33 -
a23a32)
A
+all a 23a32 + a1 2 a 21 a 33 + a1 3 a22 a31 (9.183)
As an exam ple, consider a 3 x 3 m atrix
(9.184)
with following characteristic equati on: (9.185)
B ecause [A] sat isfies its own characte ristic equation, we have A3
-
16A 2
-
lOA - 2
= O.
(9.186)
Multiplying both sides by A-I (9.187)
9. Numerical Methods in Kinematics
395
provides the inverse matrix.
9.3 Nonlinear Algebraic Equations Consider a set of nonlinear algebrai c equat ions
f'(q) = 0 or
I, (ql , qz , .. . , qn) = 0 !2(ql, in , ': , qn) = 0
(9.189)
(9.190)
f n(ql ' q2 , ' " , qn) = 0 where the function and variable vectors are
(9.191)
To solve the set of equations (9.189), we begin with a guess solution vector q(O ), and employ the following iteration form ula to search for a better solution q (i+l) = q (i) _ J -l(q(i)) f(q(i )) (9.192) where J- 1 (q (i)) is t he J acobian matrix of the system of equati ons evaluate d at q = q (i). (9.193) Utilizing the iteration formula (9.192), we can approach an exact solution as desired. Th e iteration method based on a guess solution is called NewtonRaphson m ethod, which is the most common method for solving a set of nonlinear algebr aic equat ions. A set of nonlin ear equations usuall y has multiple solutions and t he ma in disadvant age of t he Newto n-Ra phson method for solving a set of nonlin ear
396
9. Numerical Methods in Kinematics
equat ions is that th e solution may not be the solution of interest. The solution th at the method will provide depends highly on th e initial estimat ion. Hence, having a correct est imate helps to detect the proper solution. Proof. Let us define the increment c5(i) as (9.194) and expand t he set of equat ions around q (i+ 1) (9.195) Assume that q (i+l) is th e exact solut ion of Equation (9.189). Th erefore, f(q (i+l ») = 0 and we may use (9.196) to find th e increment c5(i) (9.197) and determine the solution q (i) + c5 (i) q (i) _ J-l(q(i»)f(q(i»).
(9.198)
The Newton-Raphson iterat ion method can be set up as an algorithm for better application. Algorithm 9.3. Newton-R aphson iteration m ethod for f'(q) = O. 1. Set the initial counter i
= O.
2. Evaluate an estim ate soluti on q
= q (i).
3. Calculat e the Jacobian m atrix [J ] =
[~] at
q = q (i).
4. Solve forc5(i) from the set of lin ear equati ons J (q (i») 15(;) = _f(q(i»). 5. If /c5(i) I < e, where
€
is an arbitrar y tolerance then, q (i) is the solu-
tion. Otherwise calculate q (i+l ) 6. Set i = i
•
+ 1 and
= q (i) + l5(i).
return to st ep 3.
9. Numerical Met hods in Kinemati cs
397
Example 232 In verse kin ematics problem for a 2R plana r robot. Th e en dpoint of a 2R planar manipulator can be described by two nonlin ear algebraic equati ons. (9.199) A ssuming
h =b = 1
(9.200)
and th e en dpoint is at (9.201) we are looking for the associat ed variables (9.202) that provide the desired posit ion of the endpoint. Du e to simplicity of th e syst em of equati ons, the Ja cobian of the equations and its inv erse can be found in closed fo rm
[~t,] = [~ ~] aO a0
J (0) =
J
I
2
-i. sin 8 1 - l2 sin (8 1 + 82 ) [ h cos 81 + l2 cos (81 + 82 ) J -1
=
~ h l2s82
[
-l2C(8 1 + 82 ) h c8 1+l 2c(8 1+8 2)
-l2 sin (8 1 + 82 ) l2 cos (81 + 82 )
-i» (81 + 82 )
ll S8 1 +l2 S(8 1 +8 2 )
] •
]
(9 203) .
(9 0 ) .2 4
Th e N ewton-Raphson it eration algorithm may no w be started by setti ng i = 0 and evaluating an estim ate solution q (O) = [
Th erefore,
81 82
] (0) = [
Jr/3 ] . - Jr/3
[-J3 -~J3] f(3 '3)= J3-1
J(~3' ~) = 3
Jr Jr
6(0)
0
[
(9.205)
_12
(9.206)
]
(9.207)
-1.3094 ] 1.4641
(9.208)
-1
= _J- 1 (O(0)) f(O(O )) = [
and a bett er solution is 81 ] [ 82
(1)
= [
Jr/ 3 ] Jr/ 3
+[
-1.3094 ] 1.4641
= [
-0.2622 ] 2.5113 ·
(9.209)
398
9. Numerical Methods in Kinematics
In the next iterations we find
[:~ r [:~ r
[:~ f4)
[:~ r [ :~ f6)
[:~ r
-0.2622] [ 2.5113
+
-0.3317] [ 1.7079
+
-0.0176] [ 1.63958
+
- .0013] [ 1.5722
-.3 X 10[ 1.571
8
-.49 x 10[ 1.571
]
10
+
[ - .06952 ] = [ -0.3317 ] - .80337 1.7079 [
.31414] [-0.0176] - .068348 = 1.63958
[ .016275] [ - .0013] - .06739 = 1.5722
[ .1304] = [ - .295 X 10-.139 1.571 8
[ .29 X 10- .85 x 10- 6
+ ]
]
]
= [ -.49 X 10- 10
]
1.571
19
+
8
[-.41 X 10- .2 x 10- 9
]
= [ -.49
101.571 X
10
]
and this ans wer is close enough to the exact elbow down answer
(9.210)
*
Example 233 Alternative and expanded proof for Newton-Raphson iteration m ethod. Consider the following set of equations in which we are searching for the exact solutions qj
f i( qj)
Yi
(9.211)
1" " , n
j
=
1"" , m
where j is the numb er of unknowns and i is the numb er of equations. Assume that , for a given Yi an approxim ate solution q; is available. Th e difference between the exact solution qj and the approximate solution q; is
(9.212) where the value of equations for the approximate solution q; is denot ed by
(9.213) Th e iteration m ethod is based on the mi ni mization of 5j to make the solution of
(9.214)
9. Numerical Methods in Kinematics
399
be as close as possible to the exact solution. A first-order Taylor expansion of this equation is (9.215)
We may define (9.216)
and the residual quantity (9.217)
to write (9.218)
where J is the Jacobian matrix of the set of equations J=[Ofi].
(9.219)
oqj
The method of solution depends on the relative value of m and n. Therefore, three cases must be considered. 1- m=n Provided that the Jacobian matrix remains non-singular, the linearized equation (9.220) r = J8q possesses a unique solution, and the Newton- Raphson technique may then be utilized to solve equation (9.211). The stepwise procedure is illustrated in figure 9.2. The effectiveness of the procedure depends on the number of iterations to be performed, which depends on the initial estimate of and on the dimension of the Jacobian matrix. Since the solution to nonlinear equations is not unique, it may generate different sets of solutions depending on the initial guess. Furthermore, convergence may not occur if the initial estimate of the solution falls outside the convergence domain of the algorithm. In this case, much effort is needed to attain a numerical solution. 2- m
q;
(9.221)
400
9. Numerical Methods in Kinem atics
q=q(k)
FIGURE 9.2. Newton-Raphson iteration method for solving a set of nonlinear algebr aic equations.
or, in matrix form , (9.222)
where
w = diag( WI ... W n )
(9.223)
is a set of weighting factors giving a relativ e importan ce to each of the kin ematic equations. Th e error is minimum when (9.224)
or, in matrix form , JTW
[y - f(q)] = O.
(9.225)
A Taylor expansion of the third facto r shows that the lin ear correction to an estim ated solution q is
*
JTW
[y -
f(q*)] - J IS = O.
(9.226)
Th e correction equation is (9.227)
where r is the residual vector defined by Equation (9.217).
9. Numerical Methods in Kinematics
401
The weighting factor W is a positive definite diagonal matrix, and therefore , the matrix JTWJ is always symmetric and invertible. It provides the generalized inverse to the Jacobian matrix (9.228)
which verifies the property (9.229)
When the Jacobian is revertible, the solution for (9.222) is the solution to the nonlinear system (9.211) . 3- m > n This is the redundant case for which an infinity of solutions is generally available. Selection of an appropriate solution can be made under the condition that it is optimal in some sense . For example, let us find a solution for (9.211) which minimizes the deviation from a given referenc e configuration q(O) . The problem may then be formulated as that of finding the minimum of a constrained function min
(F = ~ [q _ q(O)]
T
W [q _ q(O)] )
(9.230)
subject to
y-f(q)=O.
(9.231)
Using the technique of Lagrangian multipliers, problem (9.230) and (9. 231) may be replaced by an equivalent problem
8G =0
oq
(9.232)
8G =0
(9.233)
8)" with the definition of the functional
(9.234)
It leads to a system of m
+n
equations with m
+n
W[q_q(O)] -JT).,=O y-f(q)=O.
unknowns (9.235)
Linearization of equations (9.235) provid es the system of equations for the displacement corrections and variations of Lagrangian multipliers (9.236)
WO=r
(9.237)
402
9. Numerical Methods in Kinematics
where >..' is the increme nt of >.. . Substitution of the soluti on 8 obtain ed from the first equation of (9.235) in to the second one yields
(9.238) or, in terms of the displacem ent correction
(9.239) Th e m atrix
(9.240) has the meaning of a pseudo- in verse to the singular Ja cobian matrix J . It verifies the ident ity JJ+ = I (9.241) and, whenever J is inve rti ble,
(9.242)
*
9.4
Jacobian Matrix From Link Transformation Matrices
In robot motion, we need to calculat e the J acobian matrix in a very short t ime for every configuration of the robot . The J acobian matrix of a robot can be found easier and in an algorithmic way by evaluating columns of the J acobian
J (9.243) ] (9.244) where c, is called t he Jacobian generating vector (9.245)
°
°
and ki - I is the vector associat ed to t he skew matrix ki - I . This method is solely based on link transformation matrices found in forward kinematics and does not involve differenti ation. The matrix °ki _ I is 0k-i -
I
= oR i -
I
i - I ki- I
0R iT-
I
(9.246)
9. Numerical Methods in Kinematics
403
which means °ki _ 1 is a unit vector in the direction of joint axis i in the global coordinate frame . For a revolute joint, we have (9.247)
and for a prismatic joint we have Ci
= [ OkOi-l ] .
(9.248)
Proof. Transformation between two coordinate frames (9.249)
is based on a transformation matrix that is a combination of rotation matrix R and the position vector d .
T=[~ ~].
(9.250)
Introducing the infinitesimal transformation matrix (9.251)
leads to (9.252)
where (9.253)
and therefore, Je is the matrix of infinitesimal rotations,
(9.254)
and Jv is a vector related to infinitesimal displacements,
Jv = Jd - Jed.
(9.255)
Let 's define a 6 x 1 coordinate vector describing the rotational and translational coordinates of the end-effector (9.256)
404
9. Numerical Met hods in Kinemat ics
which its variation is 6X
= [
~~
].
(9.257)
The J acobian mat rix J is then a matrix t hat maps different ial joint variab les to different ial end-effector motio n. 6X
= aT( q ) 6q = J
aq
6q
(9.258)
The transformation mat rix T , generated in forward kinemati cs, is a function of joint coord inat es »t;
T (q )
(9.259)
°T1(qd 1T2(q2) 2T3(q3 )3T4(q4 ) ... n- 1Tn(qn) therefore, t he infinitesimal trans format ion matrix is (9.260)
Interestingly, the partial derivative of the transform ation matrix can be arran ged in t he form
6 (i- T )
1 --:..-:--_'~ J:
-
oq,
i -1 6
,- 1
i - 1T ,
(9.261)
where according to DR t ra nsformat ion matrix (5.11) [ 000 8; i - 1T
sin ()i 0 0
i
[
- sin ()i cos O:i cos () i cos O:i sin O:i 0
i- ~u, i- ~di
sin () i sin O:i - cos ()i sin O:i cos O:i 0
co, o, 1
a; a; sin ()i
di 1
]
(9.262)
we can find t he velocity coefficie nt matrices matrix 6 i for a revolut e joint to be
i- 1
6 ,-
-6 _
1 -
R -
[ i-
1
fi- 1
0
~]
=
[~
- 1 0 0 0
0 0 0 0
and for a prismatic joint to be
i -1 6
i- 1
=
6 P = [ 00
i - l ie i- 1
0
] =
[!
0 0 0 0
0 0 0 0
n
n
(9.263)
(9.264)
9. Numerical Methods in Kinematics
405
We may now express each term of (9.260) in the form (9.265) where
i-2T] [OT 1 1,." 12' " i- l 12 [OT 1 1,." 12 [OT1 1,."
0T1 1,." 12
'" '" .. .
i- 2Ti -l ]
6 (i-iT) , [OT1 1,." i- 1T]-1 5< 12'" i oq,
i- l " L.l. i -l
i-iTi [OT 1 1,." 12
'"
i- 1T]-1 i
i-2T] i- l " [OT 1 1,." i- 2T]-1 i-l L.l.i-l 12 ' " i -l 1 1 2T 2 i- ,.-1 i - l L.l.,-1 " . i- T i-l - . . . lr.°T-l 1' (9.266) 2
The matrix Ci can be rearranged for a revolute joint, in the form (9.267) and for a prismatic joint in the form
~]
(9.268)
Ci has six independent terms that can be combined in a 6 x 1 vector. This vector makes the ith column of the Jacobian matrix and is called the generating vector c. . The Jacobian generating vector for a revolute joint is (9.269) and for a revolute joint is (9.270) The position vector °d i indicated the origin of the coordinate frame B, in the base frame B o. Hence, i~l d., indicated the origin of the end-effector coordinate frame B n with respect to coordinate frame B i - 1 and expressed in the base frame B o. Therefore, the Jacobian matrix describing th e instantaneous kinematics of the robot can be obtained from (9.271)
•
406
9. Numerical Methods in Kinematics
Ex ample 234 Jacobian matrix for art iculated robots. Th e fo rward and inverse kin ematics of th e art iculat ed robot has been ana lyzed in Example 163 with th e foll owing in divi dual transformation matrices:
»t,
=
' T3
~
~1
0 0
0 0
1
S(}l
[
[
0
~3 S~3
0 0
1 0
416
~
[
~5 S~5
0 0
1 0
~1
s(h
- C(}l 0 0
S(}3 - C(}3 0 0 S(}5 - C(}5 0 0
l T2 =
~]
~1
' T4
'16
~
[~, [
~4 S~4
0 0
-1 0
[~, ~ S~6
1 '~' 1
- S(}2 0 C(}2 0 0 1 0 0
S(}2 0 0
-S(}6
dh 0 0
l2s(}2 d2 1
- S(}4 C(}4 0 0 0 0
1 0
~1
~1
(9.272)
Th e art iculated robot has 6 DOF and th erefore, its Ja cobian matrix is a 6 x 6 matrix
J (q) = [ Cl(q) C2(q) .. .
C6(q) ]
(9.273)
th at relates th e translation al an d angular velociti es of the end-effector to th e jo ints' velocities q .
[ : ] = J (q) q
(9.274)
Th e i th colum n vect or c, (q) f or a revolute j oint is given by (9.275)
and for a prismatic jo int is given by (9.2 76)
Column 1. Th e first colum n of th e Jacobian m atrix has the simplest calculati on , since it is based on th e contri buti on of the zo-axis and th e position of th e en d-effec tor fram e °d 6 • Th e direction of the zo-axis in the base coordinate frame is (9.277)
9. Numerical Methods in Kinematics
407
and the position vector of the end-effector frame B 6 is given by °d 6 directly determined from °T6 o~
=
0nl~2~3~44 5~
~ °7 6
[ 0
]
= [
°d 6 = [
:~:
t l2 h3 t22 t23 t14 t24 ] t32 t33 t34 1 0 0
~~: ]
(9.278)
(9.279)
t34
where, tl4
d6 (sfhs04S05 + COl (C04S05C (02 + 03) + C05S (02 + 03))) (9.280) +l3COIS (02 + 03) + d2s01 + l2COlC02
t24
d6 (-COIS04S05 + SOl (C04S05C (02 + 03) + C05S (02 + 03))) +SOIS (02 + 03) l3 - d2COl + l2C02S01 (9.281)
t34
=
d6(C04S05S(02 + 03) -C05C( 02 +03)) +l2S02 + bc (02 + 03) .
(9.282)
Therefore,
(9.283)
and the first Jacobian generating vector is - t24 t l4 CI
=
o o o
(9.284)
1
Column 2. The
Zl -axis
in the base frame can be found by
o o
SOl - COl 1 0
]U] (9.285)
408
9. Numerical Methods in Kinematics
Th e second half of C2 n eeds the cross product ork l and position vecto r ld 6. Th e vector ld 6 is the position of the en d-effe ctor in th e coordin ate fram e Bv , however it must be described in th e base fram e to be able to perform the cross product. An easie r m ethod is to find lk l x ld 6 and transform the resultant into the base fram e.
(9.286)
Th erefore, C2 is found as a 6 x 1 vector, COSOI (-l 2 sin O2 + l3 cos (02 + ( 3)) sinOl (-l 2 sin02 + l3 cos (02 +(3)) l2 cos O2 + l3 sin (02 + ( 3) sinOl - cos 0 1
(9.287)
o
Column 3. Th e
Z2 -axis
in the base fram e can be found using the same
m ethod :
Ok,
~ °R,'k, ~ OR, 'R, [
n[ ] ~ -'~~,
(9288)
Th e second half of C3 can be found by find ing 2k 2 x 2d 6 and tran sforming th e resultant into the base coordinate fram e. (9.289)
°R2 (2k 2 X 2d 6 ) =
hCoSOl Sin (02 + 03) ] hsin Ol sin (02 + 03) [ -l3 cos (02 + ( 3)
(9.290)
Th erefore, C3 is h cos 0 1 sin (02 + ( 3 ) h sinO l sin (02 + ( 3 ) -h cos (02 + ( 3 ) sinOl - COSOI
o
(9.291 )
9. Numerical Methods in Kinematics
Col um n
4. Th e z3-axi s in
-i.
~
409
the base fram e is
0R I IR , 'R, [
~]
COS (J 1 (cos (J2 sin (J3 + cos (J3 sin (J2) sin (Jl (cos (J2 sin (J3 + sin (J2 cos (J3) [ - cos ( (J2 + (J3) and the second half of C4 can be found by finding the resultant into the base coordin ate fram e.
3
k3 X 3 d 6
]
(9.292)
and tran sforming
(9.293) Th erefore, C4 is
o o o
(9.294)
cos (J 1 (cos (J2 sin (J3 + cos (J3 sin (J2) sin (Jl (cos (J2 sin (J3 + sin (J2 cos (J3) - cos ( (J2 + (J3 )
Column 5. Th e
Ok,
Z4 -axis
~
n
in the base fram e is
oR, [
C(J4 S(J l -
- C(} l C(}4 [
C(J l S(J4 C ((J2 + (}3) S(}l S(}4C ((J2 + (}3)
- S(}4S ((}2
]
(9.295)
+ (}3)
and th e second half of C5 can be f ound by finding th e resultant into th e base coordinate frame .
4
t:
X 4d 6
and transforming
(9.296) Th erefore,
C5
is
o o o
COS(J4 sin (} l - COS (}l sin (J4 cos ((J2 + (J3) - COS(}l COS (}4 - sin (} l sin (}4 cos ((}2 + (}3) - sin (}4 sin ((}2 + (J3 )
(9.297)
410
9. Numerical Methods in Kinematics
m
Column 6. The zs-axis in the base frame is
°k5
~
°R 5
(9.298)
-C01C04 S (fh + 03 ) - S04 (SOl S04 + C01C04C (02 + 03 ) ) - SOl C04S(02 + 03) - S04 (- C0 1S04 + SOl C04C(02 + 03)) [ C04C (02 + 03) - ~s (02 + 03) s204
]
and the second half of C6 can be found by finding s ks x sd 6 and transforming the resultant into the base coordinate frame. (9.299)
Therefore,
C6
is
o o o
- C01 C04S (02 + 03) - S04 (SOl S04 + C0 1C04C (02 + 03)) - SOl C04S (02 + 03) - S04 (- C01 S04 + SOl C04C (02 + 03)) C04C (02 + 03) - ~ s (02 + 03) s204 (9.300)
9.5 9.5.1
* Kinematics Recursive Equations * Recursive Velocity in Base Frame
The time derivative of a homogeneous transformation matrix [T] can be arranged in t he form T=[V] [T] (9.301) where [T] may be a link t ransformation matrix or the result of the forward kinemati cs of a robo t as
(9.302) Therefore, t he tim e derivative of a t ransformation matrix can be computed when t he velocity t ra nsformat ion matri x [V] is calculate d . The transformation matrix »t: is
"t: = 0T1 1T2 2T3 . . .i - 1 T;
(9.303)
and t he mat rices 0v; and 0V;+1 are OlT. _ V2 -
0T·2. 0Ti- 1
(9.304)
9. Numerical Methods in Kinematics
o.
411
0-1
TH 1 Ti+l
d (OT dt i 0 (
i '
-t.H I ) 0T-1 HI O· i
t: Ti+l + t: Ti +1
) 0
- 1
(9.305)
Ti +1 ·
These two equati ons can be combin ed as a recursive formula
o
Vi+l = 0 Vi + O'T; i Vi+l 0 Ti-1 .
(9.306)
The recursive velocity transformation matrix formula can be simplified according to the type of joints connect ing two links. For a revolut e joint , the velocity t ra nsforma tion mat rix is
. [i-lk. 0] 0
= 8i + 1
Ot -l
(9.307)
then we have
(9.308)
which shows th at t he angular velocity of frame B H 1 is the angular velocity of frame B , plus the relative angular velocity produced by joint variable QHl. Furthermor e, O· o: o:d d, + i dH 1 H 1 o:d , + OWHI (0d H 1 - 0) d, (9.309) which shows t he transl ational velocity is obtained by adding the t ra nslational velocity of frame B , to t he cont ribution of rotation of link Bi+l ' For a prismati c joint, the velocity coefficient matrix formula is
. [00
= dH 1
i - 1k ' i- I
o
]
(9.310)
and t herefore, we have (9.311)
and O·
O'
-
0
0
.
0'
d i +1 = d , + OWHI ( d i+l - d i ) + di+l i ki+l (9.312) which shows that the angular velocity of fra me B H 1 is the same as the angular velocity of frame Bi. Furthermore, t he t ra nslat ional velocity is obt ained by adding the translational velocity due to di + 1 to t he relative velocity du e to rotation of link B i + 1 .
412
9, Numerical Methods in Kinematics
9.5.2
*
Recursive Acceleration in Bas e Frame
The accelerat ion of connecte d links can also be calculated recursively, Let us st art with computing OT·T. _ 0T " . 0T- 1 (9.313) Vi Z i for the absolute accelerati ons of link (i) , and th en calculating the acceleration matrix for link (i + 1)
o'
0 "
Vi+!
0-1
TH 1 0 "
(
t:
TH 1
i
TH 1 + 2
0T·' . 0T- 1 1
+ ot:
i
O·
t:
i '
TH 1 +
0
t:
i "
T i+!
) 0
-1
TH 1
1 0T· 'i: iT - 1 0T- 1 + 2 0T'1, 0Ti 1 1+1 i+ 1 i
i .. i -1 0 Ti +! TH 1
t;-1 .
(9.314)
These two equations can be put in a recursive form (9.315)
For a revolute joint, we have
(9.316)
and therefore, iVi + 1
..
£Ii+! D.R -
Bi+!
[~
·2
T
BH 1 D.R D.R -1 0 ·2 0 0 - B1 +! 0 0 0 0
n
[~
0
0 0 0 0 0 0 1
n(9 317)
9.6 Summary There are some general numerical calculat ions needed in robot kinematics. Solutions to a set of linear and nonlinear algebraic equations are th e most important ones for calculat ing a matrix inversion and a J acobian matrix. An applied solution for a set of linear equat ions is LV fact orizati on, and a practic al method for a set of nonlinear equations is th e Newton-Raphson method. Both of these methods are cast in applied algorit hms.
9. Numerical Methods in Kinematics
413
Exercises 1. Notation and symbols.
Describe the mean ing of
2.
c- [B]
a- [L]
b- [U]
g- con (A)
h-
I Alloo
i-
m- J
n-
q
0- i-I ~i -I
IIAIII
d-
[Dil
e-
Uii
f- d«
j-
I AI1 2
k-
Ci
1- X
q-
V
r- ().
p- T
LU factorization method. Use the LU factorization method and find the associated [L] and [U] for the following matrices. (a)
[A] =
[~
n
4 2 6
(b)
[AJ
~ r~
-1 3
3 - 1
2
2 5
1
~3
-2 4 -2
j
(c)
~ ~
[ -2
[AJ
-1
3
3 2
-1
1
-5
2 5 2
-3 6 -2 0 4 -2 -2 -1 1
1
3. Gaussian elimination method. There are two situations where the Gaussian elimination method fails: division by zero and round-off errors. Examine the LU factorization method for the possibi lity of division by zero. 4. Number of subtractions as a source of round-off error. Round-off error is common in numerical techniques, however it increases by increasing the number of subtractions. App ly the Gaussian
414
9. Num erical Methods in Kinema ti cs
elimin ation and LU factoriz ati on methods for solving a set of four equations
and count the numb er of subt ractions in each method. 5. J acobian matrix from t ransformation matrices. Use t he J acobian matrix technique from links' transformation matrices and find t he J acobian matrix of the RIIRIIR plan ar manipulator shown in Figure 5.21. Choose a set of sample data for the dimensions and kinematics of the manipulato r and find the inverse of t he J acobian matrix.
6. J acobian matrix for a spherical wrist . Use the J acobian matrix technique from links' tr ansformation matri ces and find the J acobian matrix of th e spherical wrist shown in Figure 5.22. Assume t hat th e frame B 3 is the base frame.
7. J acobian mat rix for a SCARA manipulator. Use t he J acobian matrix technique from links' transformat ion matrices and find the J acobian matrix of the R IIRIIRIIP robot shown in Figure 5.27.
8. J acobian matrix for an m -R IIR articulated manipulator . Figur e 5.25 illustrates a 3 DOF m -RIIR manipulator. Use t he J acobian matri x technique from links' t ransformat ion matrices and find the Jacobian matrix for the manipulator. 9.
* Partitioning inverse method. Calculate t he matrix inversion for t he matrices in Exer cise 2 using t he partitioning inverse method.
10.
* Analytic matrix inversion. Use the analyt ic and LU factorization methods and find the inverse of
or an arbitrary 3 x 3 matrix. Count and compare t he number of arit hmetic operat ions.
9. Numerical Methods in Kinematics
11.
415
* Cayley-Hamilton matrix inversion. Use th e Cayley-Hamilton and LU factori zation methods and find t he inverse of
[A]
n
~ [~ ~
or an arbitrary 3 x 3 matrix. Count and compare th e number of arit hmetic operations . 12.
*
Norms of matrices.
Calculate the following norms of the matrices in Exercise 2.
n
IIAlloo = lM< tax L laij l
-
n
j=l
n
IIAIIF = LLa;j i= l j = l
Part II
Dynamics
Dynamics is the science of motion. It describ es why and how a motion occurs when forces and moments are applied on massive bodies . The motion can be considered as evolut ion of th e position, orientation, and th eir tim e derivatives. In robotics, th e dynamic equat ion of motion for manipulators is utilized to set up th e fund amental equations for cont rol. The links and arms in a robotic system are modeled as rigid bodies. Therefore, th e dynami c prop erties of t he rigid bod y takes a central place in robot dynamics. Since th e arms of a robot may rot at e or translate with respect to each other, translational and rot ational equations of motion must be developed and described in body-attached coordinate frames B s, B 2 , B 3 , ' " or in the global reference frame G. There are basically two problems in robot dynamics. Problem 1. We want the links of a robot to move in a specified manner. Wh at forces and moment s are requir ed to achieve th e motion? Problem 1 is called direct dynamics and is easier to solve when th e equations of mot ion are in hand because it needs differentiating of kinematics equations. Th e first prob lem includes robots statics because th e specified motion can be th e rest of a robot. In this condition, th e prob lem reduces to finding forces such th at no motion takes place when th ey act . However, th ere are many meaningful problems of the first type that involve robot motion rather than rest . An important example is that of finding th e requir ed forces that must act on a robot such that its end-effector moves on a given path and with a prescribed time history from th e start configuration to the final configuration. Problem 2. The applied forces and moments on a robot are completely specified. How will th e robot move? The second prob lem is called inverse dynamics and is more difficult to solve since it needs integration of equations of motion. However, th e variet y of the applied probl ems of th e second type is interesting. Problem 2 is essenti ally a prediction since we wish to find the robot motion for all future times when the initial state of each link is given. In this Part we develop techniques to derive th e equat ions of motion for a robot.
10 Acceleration Kinematics 10.1 Angular Acceleration Vector and Matrix Consider a rotatin g rigid bod y B(Oxyz) with a fixed point 0 in a reference frame G(OXYZ) as shown in Figure 10.1.
,
x~
y
.... y
FIG URE 10.1. A rot ating rigid body B(Oxy z) with a fixed point 0 in a reference frame G(OXYZ).
Equation (7.5), for th e velocity vector of a point in a fixed origin body frame,
Gv (t ) GWB Gr(t ) GWB x Gr(t )
(10.1)
can be utili zed to find the acceleration vector of the body point G r··
:~ Gr (t ) + GW B x (1)11+ ¢u) X G r + l GQB x
Gr
( GW B x
71 x (u
Gr)
X G r)
(10.2)
.
(10.3)
G Q B is the angular acceleration vector of th e body with respect to t he G frame.
(10.4)
422
10. Accelerat ion Kinematics
The acceleration trix
G aB
G
r can also be defined using t he angular acceleration ma-
= GWB G ··
-
r = Ga B
G
(10.5)
r
where [¢u
+ ¢iL + ¢2U2]
(10.6)
.
Proof. Differenti ating Equation (10.1) gives Goo
r
• GW B
x
G aB
x
G
+ G W B x G r· G r + GWB x ( GW B r
x
(10.7)
Gr)
and since W
¢u
a
¢u +¢u
(10.8) (10.9)
we derive the Equ ation (10.3), which can also be written in a matrix form
(10.10) and the angular acceleration matrix ·2
a=
[
a can be found .
·2
·
··
·2
+ U2¢· + U2¢··
- (UI - 1)¢
UIU2¢ - U3¢ - U3¢
UIU3¢
UIU2~:+U3~ +U3~
-: ~U~_1.) ¢2 .. U2U3¢ - Ul¢ +U l¢
U2U3l ~ Ul ¢ -:;Ul ¢
UIU3¢ - U2¢ - U2¢
]
- (U3- 1)¢
(10.11) Therefore, t he position, velocity, and acceleration vectors of a body point are Brp=xi + V] +zk (10.12) G.
rp
GWB
" v» = G r p G aB
x
Ga B
x
=
x
Gd
di
rp
(10.13)
Gr
Gd2 = dt 2
+ GWB G r + GWB
Gr
B
Br p
x
G i-
x
(GW B
x
G r ).
(10.14)
The angul ar acceleration expressed in the body frame is t he bod y derivative of t he angular velocity vector. To show thi s, we use t he derivati ve transport
10. Acceleration Kinematics
423
formula (7.154)
B
GWB
(10.15) The angular accelerat ion of B in G can always be expressed in th e form (10.16) where ua is a unit vector parallel to Ga B. The angular velocity and angular acceleration vectors are not par allel in general, and therefore, Ua
GaB
i i
Uw
(10.17)
GWB·
(10.18)
However , the only specia l case is when the axis of rot ation is fixed in both G and B frames. In t his case
GaB = au = WU = cP u.
(10.19)
Using th e Rodrig uez rot ation formula, it can also be shown t hat . Gd2 GWB = ¢lim -d 2 Rit A--->O t
GaB
, ,!,
Gd2
lim -d 2 (_u 2 cos cP + u sin cP + u 2 + I) t
¢--->O
..
. .
·2
cPu + cPu + cP uu.
(10.20)
• E xample 235 Velocity and acceleration of a simple pendulum. A point mass attached to a massless rod and hanging from a revolut e joint is called a simple pendulum. Figur e 10.2 illustrates a simple pendulum. A local coordinat e fram e B is atta ched to the pendu lum that rotates in a global fram e G. Th e position vector of the bob and the angular velocity vector GWB are
B
B
r
n
G
r
GRB B r =
GWB GWB
(10.21) [
lsin f ] -1~os cP
;Pk GRTB GBw B = ;P i<.
(10.22) (10.23) (10.24)
424
10. Acceleration Kinematics
x FIG URE 10.2. Illustration of a simple pendulum .
(10.25) It s velocity is therefore given by ~v
B i- +
~WB x ~ r
0+ ¢k x u l ¢j G V
=
G
RB
B V
(10.26)
=
l ¢cos ¢ ] . [
l ¢~n ¢
(10.27)
.
Th e accelerati on of the bob is then equal to
~a
~ v + ~WB X ~ v l ¢j + ¢k x l ¢j
l ¢j-l ¢2i
.
G
a =
G
RB
B
a =
[
(10.28)
.2 ]
l ¢ cos ¢ - l ¢ sin ¢ .. ·2 l ¢ sin ¢ ~ l ¢ cos ¢
.
(10.29)
Example 236 Motion of a vehicle on the E arth. Consi der the motion of a vehicle on the Earth at latitude 30 deg and headin g north, as shown in Figure 10.3. Th e vehicle has the velocity v =
10. Acceleratio n Kinematics
Z
z
Z
I
!
............... ..............
x
E
! ! X [:J-------__
G
425
~" ~
--~=3--e - Y
...
x
B
x (b)
(a)
FIGURE 10.3. Motion of a vehicle at lat itude 30deg and headin g nort h on t he Earth. ~r
= 80 km / h = 22.22 m/ s and acceleration a = ~ r = 0.1 m / s2, both with respect to the road. Radius of the Earth is R , and hen ce, the vehicle's kin ematics are B Er
Rkm
(10.30)
B·r
E
22.22£ m/ s
(10.31)
B ··
0.12 m/ s2
(10.32)
Er
v
0
R rad / s
B
R rad/ s .
a
(10.33)
2
(10.34)
Th ere are three coordinate fram es inv olved. A body coordinate fram e B is attached to the vehicle as shown in the Figure. A global coordinate G is set up at the center of the Earth. Another local coordina te fram e E that is rigidly attac hed to the Earth and tu rns with the Earth. Th e f rames E and G are assumed coincident at the moment . Th e angular velocity of B is e WE
+ aEW B
BRe
(WE i{ +(1)
(wE cos e) £ + (WE sin e) k + OJ
(WE cos e) £ + (WE sine) k + ~j.
(10.35)
426
10. Acceleration Kinemati cs
Th erefore, the velocity and acceleration of the vehicle are
~V
B
r + ~WB
x ~r
B
A
0+ eW B x Rk vi - (RwE cosB))
~a
B·
eV+
B e WB
B
x eV
e
ai + (Rw E sin B) ] +
ai + (Rw E
(10.36)
*.
WE COSB ] [ v ] x - RwE cos B [ wE smB 0
Rw~ cos Bsin B
esin B) ] + [
vw E sin B 2 2 2 _1. R v - RwE cos B
]
a + Rw~
=
~osBsinB ] 2RwEBsinB . [ _1. 2 - Rw2 cos 2 B v R E
(10.37)
e
Th e term ai is the acceleration relativ e to Earth, (2RwE sin B)] is the 2 Coriol is acceleration , - ~ k is the centrifugal acceleration due to traveling, and -(Rw~ cos2 B) is the centrifugal acceleration due to Earth 's rotat ion . Substituting the numerical values and accepting R = 6.3677 X 106 m provide A
B
eV
6 (
22 .22~ - 6.3677 x 10 A
2n 366.25) n 24 x 3600365 .25 cos 6J A
22.22i - 402.13] rn/ s ~ a = 1.5662 x 1O- 2 i
+ 1.6203 x 10- 3]
(10.38) -
2.5473 x 1O- 2 k rn/
S2 .
(10.39)
Example 237 Combination of angular accelerations . It is shown that the angular velocity of several bodies rotat ing relative to each oth er can be related according to (7.68)
However, in general, there is no such equation for angular acceleration s
(10.40) To show this, consider a pair of rigid links conn ect ed by revolut e joints. Th e angular velocities of the links are
(10.41) (10.42)
10. Acceleration Kinematics
427
(10.43)
howev er, the angular accelerations show that
(10.44)
(10.45)
and therefore, (10.46)
Equat ion (10.45) is the relativ e acceleration equation. It expresses the relati ve accelerations for a multi-link robot. As an example, consider a 6R articulated robot with six revolut e joints. Th e angular acceleration of the end-effector fram e in the base frame would be
(10.47)
Example 238 Angular acceleration and Eul er angles. B in terms of Euler angles is The angular velocity
gw
ce WB
Wx
]
Wy [
wz
[00 cos sin sp
ip
1
0
sin 0 sin
~ cos sp + ~ sin 0 sin sp ]
[
osin sp -
'l/J cos
rp +;P cos 0
.
(10.48)
428
10. Acceleration Kinemati cs
The angular acceleration is then equal to Cd c -
dt
(10.49)
CW B
(e + tP;p sin 0) + sin ip (;j; sin 0 + iJ;P cos 0 - iJtP ) sin
[
sp
] .
The angular acceleration vector in the body coordinate frame is then equal to C RT
«:
(10.50)
B C \.A.B
c
'ljJ
cos'ljJ [
c'ljJs
gaB
(e + tP;p sin.0) + sin 'ljJ. (
Example 239
SO s'ljJ] sOc'ljJ cO
iJ;P) ]
O'ljJ) - sin 'ljJ (0 + tP'ljJ sin 0)
.
;j; - iJtP sin ()
* Angular jerk.
a = w is a skew symmetric Angular second acceleration matrix X matrix associated to the angular jerk X = it = w. The angular jerk can be found by differentiating either (10.10) or (10.11). C'r'
[;;C r
[1>u + 2¢~ + ¢ri + 3¢¢uu + 2¢2~u + ¢2u~ + l uuu] c r (10.51 ) Hence, the angular jerk matrix would be
(10.52) where j ll
=
3UIUl l + 3 (ui - 1) ¢¢
j21
(2U2Ul + U2Ul) l + 3U2Ul ¢¢ + (u3¢ + 2U3¢ + U3'¢ - u3l)
131
(2U3Ul + u3ud l + 3U3Ul ¢¢ + (u2¢ + 2U2¢ + U2 '¢ - u2¢3)
10. Acceleration Kinemati cs
(2Ul'U 2 + UI U2) ·2
3U2U2
l
2
+ 3 (U2 -
(2U3U2 + U3 U2 ) ¢2
(2UI U3
+ 3UIU2¢¢ + ( U3¢ + 2U3¢ + U3'¢ - u3l ) . ..
1)
+ 3U3U2¢¢ + (UI¢ + 2UI¢ + UI'¢ - UI¢3)
+ UIU3) ¢2 + 3UIU3¢¢ + ( U2¢ + 2U2¢ + U2'¢ - U2¢3)
(2U2U3 + U2U3 ) ¢2 ·2
3U3U3
429
2
+ 3 (U3 -
+ 3U2U3¢¢ + ( UI ¢ + 2UI¢ + UI'¢ - uI l ) . ..
1)
(10.53)
*
Example 240 Angular acceleration in term s of quat ernion and Euler param eters. Utilizing the definition of the angular velocity based on rotation al quaternion
(10.54) ~ = 2+-;;'+---:-7 e e
(10.55)
e WE
we are able to defin e the angular acceleration quaternion ~~
GCiB = 2 e
e"
~~
+ 2 e e*
(10.56) (10.57)
10.2
Rigid Body Acceleration
Consider a rigid body with an at tached local coordinate fram e B (oxyz) mov ing freely in a fixed global coordinate fram e G(OXYZ) . The rigid body ca n rot at e in the global fram e, while the origin of t he body fram e B ca n tran slat e relat ive t o the origin of G. The coordinates of a body point P in local and global fram es, as shown in Figure 10.4, are relat ed by the following equation
(10.58) where a d E indi cat es the positi on of the moving origin fixed origin O . The acceleration of point P in G is
0
relative t o the
(10.59)
430
10. Acceleration Kinematics
x
4
' y
x~ F IGURE 10.4. A rigid body with coordinate fram e B (oxy z) moving freely in a fixed global coordinate fram e G(OXYZ) .
Proof. T he acceleration of point P is a consequence of differentiating the velocity equation (7.191) or (7.192).
cd
_
cv p
dt C B rp
C aB
x
CaB
x
+ CW B C B rp + C WB
CaB
x
(C r p -
+ CW B X
· x cBrp X
+ c"dB ( C C WB x B rp ) + C d" B
Cd B)
(C WB x (c rp -
C
d B) )
+ C"d B.
(10.60)
The term C W B x (C W B X ~ r p) is called centripetal acceleration and is independent of the angular acceleration. T he te rm C aB x ~ r p is called tangential acceleration and is perpendicular to ~ r p . •
Example 241 Acceleration of a body point. Consider a rigid body is moving and rotating in a global frame . Th e acceleration of a body point can be found by taking twice the time derivative of its position vector Cr p
=
a,r p =
c RB B r p
+ c dB
(10.61)
C ' B RB
+ C'd B
(10.62)
rp
c"r p (10.63)
10. Acceleration Kinematics
431
Differentiating the angular velocity matrix
(10.64) shows that CWB
(10.65) and therefore,
c" RB
C
T
,: CWB -
RB =
--T CWB cWB'
(10.66)
Hence, the acceleration vector of the body point becomes G.. rp
-T ) (Cr
,: B = ( CW
- B CW B CW
P -
Cd) B
+ c d" B
(10.67)
where CWB
=
CaB
=
[
0
-W3
W2
W3
0
-Wj
- W2
Wj
0
]
(10.68)
]
(10.69)
and -
- T
CWBCWB
=
[ wj +wj
- W jW2
-Wj W2
wi +w~
- W jW3
-W2W3
-Wj W3 - W 2W 3
wi +w~
Example 242 Acceleration of joint 2 of a 2R planar manipulator. A 2R planar manipulator is illustrated in Figure 5.9. The elbow joint has a circular motion about the base joint. Knowing that
(10.70) we can write
(10.71) (10.72) (10.73) and calculate the acceleration of the elbow joint
(10.74)
432
10. Acceleration Kinematics
Example 243 Acceleration of a moving point in a moving body frame. Assume the point P in Figure 10.4 is indicated by a time varying local position vector Brp(t) . Then, the velocity and acceleration of P can be found by applying the derivative transformation formula (7.154). Gd' B Gd' B Gap
Gd B +
Br p
B + B·r p + GWB x Br p B B x Br p + B v P + GW
+
~WB x Br p
+
(10.75)
~WB x Br p
+~WB x (B r p + ~WB x B r p) G"d + B ap+2 B B B· B B GwBX Vp+GWBX 'rp + ~WB x (~WB x B r p) .
(10.76)
It is also possible to take the derivative from Equation (7.188) with the assumption B r polO and find the acceleration of P. G rp = GR B B rp
G·rp
G··
rp
+
Gd B
GRB B r p + GRB B r p + Gd B GWB x GR B B r P + GR B B·r p + Gd'B
(10.77)
(10.78)
GWB x GRBBrp+ GWB x GRBBrp+ GWB x GRBBrp +GR B B r p
+ GRB
Br p
+ Gd B
GWB x ~rp+ GWB x (GWB x Gr p) +2GWB x ~rp •• + Gd" B + GBrp
(10.79)
The third term on the right-hand side is called the Coriolis acceleration. The Coriolis acceleration is perpendicular to both GWB and Br p.
10.3
Acceleration Transformation Matrix
Consider the motion of a rigid body B in the global coordinate frame G, as shown in Figure lOA. Assume the body fixed frame B(oxyz) is coincident at some initial time to with the global frame G(OXYZ). At any time t # to, B is not necessarily coincident with G, and therefore, the homogeneous transformation matrix GTB(t) is time varying. The acceleration of a body point in the global coordinate frame can be found by applying a homogeneous acceleration transformation matrix (10.80)
10. Acceleration Kinematics
433
where , hereafter GAB is the acceleration transformation matrix - -T) Gd B - - ww - -T Gd" B - (GaB GA = GaB - ww B OO [
]
.
(10.81)
Proof, Based on homogeneous coordinate transformation we have, (10.82)
Gv p
GTB GTB1 Grp(t) G [ G RB R~ GdB O
[G~B
Gd B -
G
R~ G R~ GdB ] Grp(t)
~WB Gd B
] Grp(t)
GVBGrp(t) .
(10.83)
To find the acceleration of a body point in the global frame , we take twice the time derivative from Gr p (t ) = GTB Br p
(10.84)
and substitute for
Brp
(10.85)
Substituting for GTB and GTB1 provides
where G "
RB
G
T
RB
:. GWB
- -T -ww
- -T GaB - WW
(10.87)
Following the sam e pattern we may define a jerk transformation as (10.88)
434
10. Acceleration Kinematics
where,
(10.89) and
• Example 244 Velocity , acceleration, and jerk transformation matrices. The velocity transformation matrix is a matrix to map a position vector to its velocity vector. Assume p and q denote the position of two body points P and Q. Then,
Gq _ Gp =
GWB
(Gq _ Gp )
(10.91)
which can be converted to
[ G~B Gp - ~WB Gp GVB
[ G1q
] [
G1q
]
]
(10.92)
and [V] is the velocity transformation matrix. Similarly, we obtain the acceleration equation G--
q-
G--
P
- (G q- G) - B (G q- - G-) p + GW P (G G) (G GaB q p + GWB GWB q - G) P - (G q - G) - GWB - T (Gq - G) GaB p - GWB P - - GWB - GWB -T) (G q - G) (10.93) (GaB P GaB
which can be converted to
(10.94)
[AJ
~
[ GaB -
~WB GW);
G
p-
(GaB -
~WB GW);)G P
] [ Gjq
]
(10.95) where [AJ is the acceleration transformation matrix for rigid motion.
10. Acceleration Kinematics
435
The jerk or second acceleration matrix can be found after anot her differentiation G '"
q-
G '"
P
-:GaB
(G q - G) - (G q. - G.) - (G q. - G.P ) p + 2 GaB p + GWB
Git B (G q - Gp ) ( GitB -
(2GaB
+ GWB) GW~ (G q _ Gp )
(2GaB + GWB) GW'h )
(G q _ Gp )
(10.96)
which can be converted to
[ ': ] = [J] [
i]
(10.97)
where [J] is the second acceleration or jerk transformation matrix. (10.98)
Git B - (2 GaB + GW B) GW~
(10.99)
Gp _ ( GitB - (2 GaB + GWB) GW~ ) Gp
(10.100)
lOA Forward Acceleration Kinematic s The forward acceleration kinematics problem is the method of relating the joint accelerations, q, to the end-effector accelerations X. It is (10.101)
where J is t he Jacobian matrix, q is the joint variable vector, q is t he joint velocity vector, and q is the joint acceleration vector q
= [ qn qn qn
q = [ qn
qn
qn
q = [ iin
iin
iin
f qn 'f qn "f.
(10.102)
qn
However , X is the end-effector configuration vector and effector configuration velocity vector.
(10.103) (10.104)
X
is the end -
(10.105)
436
10. Acceleration Kinematics
[ oWn °Vn
X
]= [
[ Xn Yn
X
[
Dan
Dan
=
(10.106) T
Zn
]= [
[ x; Yn
~~: ] WXn
WYn
WZ n ]
(10.107)
WYn
WZ n
(
(10.108)
~~: ]
Zn
WXn
[j] ,
is not Since calculating the time derivative of the Jacobian matrix, simple in genera l, to find the forward acceleration kinematics it is better to use Equations (8.69) to (8.72) if joint i is R if joint i is P if joint i is R if joint i is P
(10.109)
(10.110)
and take a derivative to find the acceleration of link (i) with respect to its previous link (i - 1). if joint i is R
if joint i is P (10.111) if joint i is R if joint i is P
(10.112)
Therefore, the acceleration vectors of the end-effector frame are n 0"
'"
od n = LJ
(10.113)
0" i-l d ,
i=l
and (10.114) The acceleration relationships can also be rearranged in a recursive form. if joint i is R if joint i is P
(10.115)
10. Acceleration Kinematics
437
Example 245 Forward acceleration of the 2R planar manipulator. The forward velocity of the 2R planar manipulator is found as
Jq - it S(}l - l2s ((}l + (}2) [ it C(}l + l2c ((}l + (}2) The differential of the Jacobian matrix is (10.117)
where
jn
(-it cos (}l - l2 cos ((}1 + (}2)) 81 - l2 cos ((}1 + (}2) 82
jl2
+ (}2) 81 -l2 cos ((}1 + (}2) 82 (-h sin (}1 - l2 sin ((}1 + (}2))8 1 - l2 sin ((}1 + (}2) 82 -l2 sin ((}1 + (}2) 81 - l2 sin (81 + (}2) 82 (10.118)
j21 j22
-l2 cos ((}1
and therefore , the forward acceleration kinematics of the manipulator can be rearranged in the form (10.119)
However, for the 2R manipulator it is easier to show the acceleration in the form
(10.120)
10.5
* Inverse Acceleration Kinematics
The forward acceleration kinematics shows that (10.121)
Assuming that the Jacobian matrix, J , is square and nonsingular, the joint acceleration vector q can be found by matrix inversion. (10.122)
438
10. Acceleration Kinemati cs
However, calculating j and J - 1 becomes more tedious by increasing the DOF of robots. The alternative technique is to write the equat ion in a new form (10 .123 )
which is similar to Equation (8.160) for a robot with a spherical wrist (10.124)
or
[:] =[~ ~]
(10.12 5)
where (10 .126)
Therefore, the inverse acceleration kinemati cs problem can be solved as
(10 .127)
and (10 .128)
Th e matri x j q = X- J q is called the acceleration bias vector and can be calculated by differentiating from (10 .129)
(10 .130)
to find o
oa6
0"
od 6 3
L
i=l
(gWi
X
i_~\ di
+ gWi X (gWi X i..?ldi))
(10 .131)
10. Acceleration Kinematics
439
and
(10.132)
ga6
The angular acceleration vector is the second half of X. Then , subprovides the second half of t he t ract ing t he second half of J q from bias vect or .
ga6
6
'" L.J °
.
x ei 0 k' i -
OWi
(10.133)
1
i= 1
We subst it ute
and
gWi
gW i
(10.134)
(10.135) in Equation (10.131) 3
i
e
0" 6 od
'L.J " 'L.J " ( .. j 0 k' j - 1
+° oWj - l
e.j 0'kj - 1 )
x
x
°d ,
i- I
i = l j= l
3
.j °'kj + '" L.J '" L.J e
1
x
(0OWi
0) d,
X i- I
i = l j=1 i
3
°
" '' 8 .. j 0 k' j - 1 X i -I d , L.J' 'L.J
3
;
+ "'''' L.J L.J (0
oW j - 1
; = l j= l
x
e. j 0 k' j - 1)
x
;= l j = l
3
+
e
" '' . j 0k 'j- 1 L.J' 'L.J
x
(10.136)
(0OWi X i -0 I d) i
i = l j =l
to find t he first half of t he bias vector 3
3
i
LL (8
Wj - 1
x iJ jOkj _ 1)
i = 1 j= 1
X i!\ d i
+ LL iJ/ kj - 1 x
(8 W i
X i _(\ d i ).
i = l j =l
(10.137) Example 246 Inverse acceleration of a 2R planar m anipulator. Th e fo rward velocity and acceleration of the 2R planar manipulator is found as
Jq
[;t ]
[
-ll S81 -12s (81 + 82 ) hce 1 + 12C (e1 + 82 )
(10.138)
- 12s (8 1 + 82 ) 12c (81 + 82 )
][t~ ]
440
10. Acceleration Kinematics
Jq+jq -h sin Ol [ h cos 01
[~ ]
(10.139)
along with the following derivative and inve rse Jacobian m atrices.
j=
[-h~lCOl-12(~1+~2) C(fh+02) -12(~1+ ~2) C(01+02)] -h01 SO l -1 2 (0 1 + O2) S (01 + O2)
-1 2 (0 1 + O2) S (01 + O2) (10.140) (10.141)
Th e inverse acceleration kinem atics of the manipulator
(10.142) can be arranged to
(10.143)
10.6
Summary
Wh en a body coordinate frame B and a global frame G have a common origin, the global accelerat ion of a point P in frame B is G ..
r
where,
= -G d
C aB
dt
G
Vp
=
C aB
x
G
r
+ G WB
X
(
G WB
x
G )
r
(10.144)
is t he angular acceleration of B with respect to G
(10.145)
10. Acceleration Kinematics
441
However, when the body coordinate frame B has a rigid motion with respect to G, then
(10.146) where Gd B indicates the position of the origin of B with respect to the origin of G. Angular accelerations of two links are related according to (10.147) The acceleration relationship for a body B having a rigid motion in G may also be expressed by a homogeneous acceleration transformation matrix GAB
(10.148) where , GTBGT l
E
[ GOB
~ ww
T
G"dB
-
(- -T) G dB GaB 0- ww
]
. (10.149)
The forward acceleration kinematics of a robot is defined by (10.150) that is a relationship between joint coordinate acceleration
q = [ijn ijn
s:
(10.151)
and global acceleration of the end-effector (10.152) Such a relationship introduces the time derivative of Jacobian matrix
[j].
10. Acceleration Kinemat ics
443
Exercises 1. Notation and symbols.
Describe th eir meaning . a-
GaB
b-
g-
gal
h- ~a2
m-
0-
2IXl
n-
BaG
2lW 2
G c- GaB
d- ~aB
e-
. 1 1- 2 a l
J-
.
k- ~a l
1-
0- GAB
p-
q- j
r-
2 2 al
G'
VB
B BaG
f- ~aG ka i
J
X
2. Local position, global acceleration. A body is turning about the Z-axis at a const ant angular acceleration ii = 2 rad / sec'' . Find th e global velocity of a point, when 0: = 2 rad/ sec, IX = 7f / 3 rad and
3. Global position , constant angular acceleration. A body is t urning about th e Z-axis at a constant angular acceleration ii = 2 rad/ sec". Find t he global position of a point at
after t = 3 sec if t he body and global coordinate frames were coincident at t = 0 sec. 4. Turning about x-axis. Find the angular acceleration matrix when the body coordinate frame is t urning - 5 deg / sec", 35 deg / sec at 45 deg about the x-axis. 5. Angular acceleration and Euler angles. Calc ulate t he angu lar velocity and accelerat ion vectors in body and global coordinate frames if the Euler angles and t heir derivatives are : 'P = .25 rad
(} = - .25rad 7/J = .5 rad
ep = 2.5 rad / sec
e=
;p =
- 4.5 rad / sec 3 rad / sec
ip = 25 rad / sec2 B= 35 rad / sec2 ~ = 25 rad/ sec2
444
10. Acceleration Kinematics
Y4
y .. X
0 X ,(
p
o R
FIGURE 10.5. A roller in a circular path . 6. Combined rotation and angu lar acceleration. Find t he rotation matrix for a body frame after 30 deg rotation abo ut t he Z-axis, followed by 30 deg about th e X -axis , and then 90 deg about the Y-axis. T hen calculate the angu lar velocity of t he body if it is turning with a = 20 deg / sec, ~ = - 40 deg / sec, and 1 = 55 deg / sec about the Z, Y , and X axes respectively. Fin ally, calculate t he angular acceleration of the body if it is turning with (i = 2 deg / sec", $ = 4 deg / sec", and l' = - 6 deg / sec2 about th e Z, Y, and X axes. 7.
* Differentiation and coordinate fram e. How can we define these?
8. A roller in a circle. Figure 10.5 shows a roller in a circular path . Find t he velocity and acceleration of a point P on the circumference of the roller. 9. An RPR manipulator. Lab el the coordinate frames and find the velocity and acceleration of point P at the endpoint of the manipulator shown in Figure 10.6.
10. Acceleration Kinematics
445
.. x
y )0,
FIGURE 10.6. A planar RPR manipulator.
10. Rigid link acceleration. Figure 10.7 illustrates the coordinate fram es and kinematics of a rigid link (i). Assume that t he angular velocit y of the link , as well as the velocity and acceleration at proximal and distal joints, are given. Find (a) velocity and acceleration of the link at C, in terms of proximal joint i velocit y and acceleration (b) velocity and acceleration of the link at C, in terms of distal joint i + 1 velocity and acceleration (c) velocity and acceleration of th e of proximal joint i in terms of distal joint i + 1 velocity and acceleration (d) velocity and acceleration of the of dist al joint i proximal joint i velocity and accelerat ion. 11.
*
+ 1 in
terms of
Jerk of a point in a roller.
Figure 10.8 shows a rolling disc on an inclined flat surface. Find the velocity, accelera t ion, and jerk of point P in local and global coordinate frames.
446
10. Acceleration Kinematics
&2 ~ ~.
d i_1
0 d 1-' l
~
d
_ i 1
X
F IGURE 10.7. Kinematics of a rigid link.
FIGURE 10.8. A rolling disc on an inclined flat.
~
y
11
Motion Dynamics 11.1
Force and Moment
From a Newtonian viewpoint , the forces act ing on a rigid body are divided into in tern al and exte rn al for ces. Int ernal forces are act ing between par ticles of the body, and exte rnal forces are act ing from outside th e bod y. An external force is either contact f orce, such as act uat ing force at a joint of a robot , or body force, such as gravitational force on th e links of a robot . Extern al forces and moment s are called load, and a set of forces and moments act ing on a rigid body is called a for ce syst em . The resulta nt or total force F is the sum of all t he exte rnal forces act ing on a bod y, and the resultant or total moment M is the sum of all the moments of the exte rnal forces about an origin. (11.1)
(11 .2) Consider a force F act ing at a point P indicat ed by position vector r p . The moment of the for ce about a direction al line l passing through the origin is M, = liL · (rp x F) (11. 3) where iL is a unit vector indicating the direct ion of l . The moment of a force may also be called torque or moment . The moment of a force F , acting at a point P , about a point Q at r Q is (11.4)
and th erefore, th e moment of F about th e origin is
M =rp xF.
(11.5)
The effect of a force syst em, acting on a rigid body, is equivalent to th e effect of the result ant force and resultant moment of th e force system. Any two force systems are equivalent if t heir resultant forces and resultant moments are equal. It is possible th at t he resultant force of a force system be zero. In this condit ion the result ant moment of the force system is independent of the origin of the coordinate frame. Such a result ant moment is called torqu e.
448
11. Motion Dynami cs
When a force syste m is redu ced to a resultant F p and M» with respect to a reference point P, we may chan ge the reference point to anot her point Q and find t he new resultants as Fp
(11.6)
M p+ (rp-rQ)xF p
M»
+ ot » x F».
(11.7)
The momentum of a rigid bod y is a vector quantity proportion al to th e total mass of the bod y times t he translational velocity of the mass cente r of the body.
(11.8)
p=mv
The momentum is also called lin ear m omentum or translat ional momentum. Consider a rigid bod y with momentum p . Th e moment of momentum, L, ab out a direct ional line I passing t hrough th e origin is
L,
= lii - (rc
x p)
(11.9)
where iL is a unit vector indicatin g the direction of t he line, r c is the position vector of the mass cente r (eM ), and L = r c x p is the moment of momentum about the origin. The moment of momentum is also called angular momen tum to justify the word linear momentum. A bound ed vector is a vector fixed at a point in space . A sliding or lin e vec tor is a vector free to slide on its lin e of action. A free vector can move to any poin t as long as it keeps it s direction. Force is a sliding vector and moment is a free vect or. However , t he momen t of a force is depend ent on t he dist an ce between t he origin of t he coordinate frame and t he line of action. T he applicat ion of a force system is emphasized by Ne wton's secon d and th ird laws of motion. The second law of motion, also called Newton equation of motion, says th at the global rat e of cha nge of lin ear momentum is proportion al to th e global applied f orce.
(11.10) The third law of motion says th at the acti on and reaction forces acting between two bod ies are equal and opposite. The second law of motion can be expanded to includ e rot ational moti ons. Hence, the second law of motion also says that the global rate of cha nge of angular momentum is proportion al to the global appli ed moment.
(11.11)
11. Motion Dynamics
449
Proof. Differentiating from angular mom entum shows that Gd
Gd
-(rexp) dt
_ GL
dt
Gdr e
(
--;It x P + re x
GdP )
ill
Gdp
G
re x - dt G re x GF
GM .
(11.12)
• Kineti c ene rgy of a point P from a movin g frame B, with mass m at a position pointed by
K
Gr p
and having a velocity
Gv p ,
is
1 -m Gv~
2
1 2m
(G·dB +
B
Vp
+ GB WB x Brp )2
(11.1 3)
The work don e by the applied force GF on m in going from point 1 to point 2 on a path indi cated by a vector r is
1 2
1 W2
=
GF .
dr .
(11.14)
However,
(11.15) that shows 1 W 2 is equal t o the difference of the kinetic energy between terminal and initial points. (11.16)
Example 247 Position of center of ma ss. Th e posit ion of th e mass cen ter of a rigid body in a coordinate fram e is indicat ed by re and is usually m easured in the body coordin ate fram e. B re
[
~zc~ ]
~
r rdm
mJB [
~£f:J~ ~~: ] z dm
(11 .17)
(11.18)
450
11. Motio n Dynamics
y
y
• • a
45 deg '%----+----I--L.J~
X
FIGURE 11.1. Principa l coordinate frame for a symmetric L-section.
Applying the mass center integral on th e symmetric L-section rig id body with p = 1 shown in Figure 11.1 provides the C M of th e section. The x position of C is
r
~
Xc
xdm
miB ~l x dA
+ ab - a2 4ab + 2a 2
b2
(11.19)
and becaus e of symmetry, we have uc
=
- Xc
=
b2
+ ab -
a2
(11.20)
4ab+ 2a 2 .
E x ample 24 8 Every force sy st em is equiv alen t to a wrench. Th e Poinsot th eorem says: Every fo rce system is equivalent to a single force, plus a moment parallel to the forc e. Let F and M be th e resultant force and moment of th e fo rce system. We decompose th e moment in to parallel and perpendicular components, M il and M j , to th e fo rce axis. Th e force F and th e perpendicular moment M j can be replaced by a single forc e F ' parallel to F . Th erefore, th e fo rce sy st em is reduced to a force F' and a moment M il parallel to each oth er . A forc e and a moment about th e force axis is a w rench. Po insot th eorem is similar to th e Chasles theorem that says eve r y rigid body motion is equivalent to a screw, whi ch is a translation plus a rotation about th e axis of translation. Ex ample 249 Motion of a moving point in a moving body fram e. Th e ve locity and acceleration of a moving point P as shown in Figure 7.5 are found in Example 243. Gv P
=
Gd" B
+ GR B
(B v P
+ GB W B
x
B r P)
(11. 21)
11. Motion Dynamics
Cap
C"dB
+ CR B
+cR B (~WB
(B ap X
B + 2 CWB X
BV P
B. X + CWB
(~WB x Br p))
451
B r p)
(11.22)
Therefore, the equation of motion for th e point mass P is mCap
m (Cd B+ cR B (Bap+2~wB x B y p+ ~WB x +m cR B (~WB x (~WB x B r p)) . Example 250 N ewton equation in a rotating frame . Consider a spherical rigid body (such as Earth) with a fixed point that is rotating with a constant angular velocity. The equation of motion for a moving point P on the rigid body is found by setting CdB = gWB = 0 in the equation of motion of a moving point in a moving body frame (11.23) mBap+m~wB x (~WB x Br p) +2mgwB x Br p (11.24)
BF -=I=-
mBap
which shows that the N ewton equation of motion F a rotating frame.
=ma
is not correct in
Example 251 Coriolis force. The equation of motion of a moving point on the surface of th e Earth is (11.25) which can be rearranged to
(11.26) Equation (11.26) is the equation of motion to an observer in the rotating frame, which in this case is an observer on the Earth. The left-hand side of this equation is called the effective force,
F eff
=
BF -mCwB B B x (B CWB x B rp) - 2mCwB
X
Bv r
(11.27)
because it seems that the particle is moving under the influence of this force . The second term is negative of the centrifugal force and pointing outward. The maximum value of this force on the Earth is on the equator
6378.388 3.3917
X
103 x (
21f 366.25) 2 24 x 3600365.25
x 10- 2 rn/ 82
which is about 0.3% of the acceleration of gravity. If we add the variation of the gravitational acceleration because of a change of radius from
452
11. Motion Dyn ami cs
R = 6356912 m at the pole to R = 6378388 m on the equator, then the variation of the acceleration of gravity becomes 0.53%. So, generally speaking, a sportsm an such as a pole-vault er practi cing in the north pole can show a bett er record in a competition held on the equator. Th e third term is called the Coriolis effect, Fc , which is perpendicular to both wand B v p. For a mass m moving on the north hemisphere at a latitude towards the equator, we should provide a lateral eastward force equal to the Coriolis effec t to for ce the mass, keeping its direction relative to the ground.
e
Th e Coriolis effect is the reason of wearing the west side of railways , roads, and rivers. Th e lack of providing Cori olis fo rce is the reason fo r turning the direction of winds, proj ectil es, and falling obj ects westward.
Example 252 Work, for ce, and kin etic energy in a unidirectional motion. A mass m = 2 kg has an initial kin eti c energy K = 12 J . Th e mass is under a constant fo rce F = F 1 = 41 and mov es from X (0) = 1 to X (t f) = 22 m at a terminal time t f . Th e work done by the fo rce during this motion is W
=
t:
F· dr
=
J22 4dX = 21 N m = 21.J
r (O)
1
Th e kin eti c energy at the terminal time is K(tf)
=
W - K(O)
= 9J
which shows that the terminal speed of the ma ss is
V2 = J2K~f)
= 3m/ s.
Example 253 Time varying for ce. When the applied for ce is tim e varying, F(t)
= mr
(11.28)
then there is a general solution fo r the equati on of motion because the charact eristi c and category of the motion differs fo r different F(t) .
f(t) r(t)
f (to) + -1
m
it
F(t)dt
to
r(t o) + f( to)(t - to)
(11.29 )
.c:
+-
m
to
to
F(t)dt dt
(11.30)
11. Motion Dynamics
x
453
y
~
FIGURE 11.2. A body point mass moving with velocity force dr.
Gvp
and acted on by
11.2 Rigid Body Translational Kinetics Figure 11.2 depicts a moving bod y B in a global frame G. Assume t hat the body frame is at tac hed at the center of mass (C M) of t he body. Point P ind icates an infinitesimal sphere of th e body which has a very sma ll mass dm . The point mass dm is acted on by an infinit esimal force df and has a global velocity G v p . Accord ing to Newton law of motion we have df
=
Ga p dm .
(11.31)
however , the equation of motion for t he whole bod y in global coord inate fram e is GF= mG aB (11.32) which can be expressed in t he body coordinat e frame as BF
B B B =m Ga B+ m GWB X VB ·
(11.33)
In these equations, Ga B is t he acceleration vector of t he body C M in global frame, m is th e tot al mass of t he bod y, and F is the resultant of the ext ernal forces act ed on t he body at CM. Proof. A bod y coordinate frame at center of mass is called a central fram e. If the frame B is a cent ral frame, t hen the center of m ass, C M, is defined such t hat (11.34)
454
11. Motion Dynamics
The global position vector of dm is relat ed to its local position vector by
(11.35) where G d B is the global position vector of the cent ral body frame, and therefore,
l l:
GdB dm + GR B
L
Br dm dm
GdB dm
Gd B
l
dm
(11.36)
m GdB .
The tim e derivative of both sides shows that (11.37) and another derivative is m
However, we have df
G· G v B =m e e
=
G " p dm
>
r G v»; · iB
dm
.
(11.38)
and,
(11.39) The inte gral on t he right-hand side accounts for all t he forces acti ng on particles of mass in t he body. The int ern al forces cancel one anot her out , so the net result is the vector sum of all t he extern ally applied forces, F , and t herefore, (11.40) In th e bod y coordinate fram e we have BRGG F m BRG GaB B
m GaB B
m
•
e»
+m
B
GWB x
B
VB .
(11.41)
11. Motion Dynamics
455
11.3 Rigid Body Rot ational Kinetics The rigid body rot ational equation of motion is the Eul er equation c d BL dt B i +~ W B x BL B1
~wB
+
~w B
X ( B1
~w B )
(11.42)
where L is the angular momentum B L =B 1~W B
(11.43)
and 1 is th e moment of in ertia of th e rigid body. (11.44) The elements of 1 are only functions of th e mass distribution of th e rigid bod y and may be defined by i , j = 1,2 ,3
(11.45)
where 5i j is Kronecker 's delt a. The expanded form of the Euler equation is
Mx =
My
M,
1xxwx + 1xywy + I xzwz - (1yy - 1zz) wywz - I yz (w; - w~ ) - Wx (wzlxy - wyl xz )
(11.46)
1yxwx + 1yywy + Iyzwz - (1zz - 1xx) WzWx - 1xz (w; - w; ) - wy (wx1yz - wz1xy )
(11.47)
I zxwx + 1zywy + 1zzwz - (1XX - I yy) wxwy -1xy (w~ - w; ) - Wz (wyIxz - wx1yz ).
(11.48)
which can be reduced to
hWl - (12 - 12) w2w3 12W2 - (13 - h) W 3Wl h W3 - (h - 12 ) Wlw2
(11.49)
in a special Cartesian coordin at e frame called the principal coordinate frame. The principal coordinate frame is denot ed by numbers 123 to indicate the first, second , and third principal axes. The parameters I i j , i i=- j
456
11 . Motion Dynamics
are zero in th e prin cipal frame. The bod y and prin cipal coordinate fra me are assumed to sit at eM of th e bod y. Kineti c energy of a rot atin g rigid body is K
(11.50) (11.51) (11.52) that in the prin cipal coordinate frame redu ces to (11.53) Proof. Let mi be t he mass of t he ith particle of a rigid bod y B , which is made of n particles and let r i = B r i = [Xi Yi Zi ]T be t he Cart esian position vector of m ; in a central body fixed coordinate fra me O.'EY z. Assume t ha t = B = [w x wy W z is t he angular velocity of t he rigid body with respect to t he ground, expressed in the bod y coordinate frame. The angular momentum of m i is
f
w gw
r, x miti m , [r , x (w x r i)j mi [(ri . r i) w - (r , . w) ri] m-r t2 w - m · (r z· . w) r ,to
L,
~
1,
(11.54)
Hence, the angular momentum of the rigid body would be n
n
L
= w I:mir T- I:mi (r, i=1
· w ) ri'
(11.55)
i=1
Substi tution r , and w gives us n
L
(wxi+ wy]+ wzk)I:mi(xT+ YT+ zT) i=1 n
- I: m;(xiwx + YiWy + ZiWz) . (xii + Yi] + zik) (11.56) i=1
11. Motion Dynamics
457
and therefore, n
L
n
L
m i
(x ;
+ Y; + Zl) wxi +
i= 1
L m , (x; + Y; + Zl) Wy] i= 1
n
+
n
'"" ~ m,
(Xi2
+
Yi2
+
Zi2)
wzk,
-
i= 1
'"" ~m i
(XiWx + YiWy
+ ZiW z) Xi t'
i= 1
n
- L tn; (XiWx + YiWy + ZiWz) y;J i= 1 n
-L
m i
(XiWx + YiWy
+ ZiWz) zik
(11.57)
i= 1
or n
L
L tn; [(X; + Y; + Zl) Wx -
(XiWx + YiWy
+ ZiWz ) Xi] i
i= 1
n
+L
m, [(X;
+ Y; + Zl) Wy -
(XiWx + YiWy
+ ZiWz) Yi]J
+ Y; + Zl) Wz -
(XiWx + YiWy + ZiWz ) Zi ] k (11.58)
i= 1 n
+L m i
[(x;
i= 1
which can be rearranged as
L n
n
i= 1
i =1
- L (miYixi) Wx ] + L
[m i (z ;
+ x D ] Wy ]
n
(11.59)
The angular moment um can be written in a concise form
i : ». + Ixywy + i,» ,
(11.60)
I yxwx + Iyywy + Iyzwz I zxwx + Iz ywy + i. , » ,
(11.61 ) (11.62)
458
11. Motion Dynamics
or in a matrix form L
[~: ]
I ·w
(11.63)
[ I ," Iyx Izx
Ix y I yy Iz y
i . , ] [ w, ] Iyz wy
t.,
(11.64)
Wz
by int roducing the moment of inertia matrix I where n
t.;
L [mi (y; + zn ]
(11.65)
i=l n
Iyy
L [mi (z; + xm
(11.66)
i=l n
Izz
L [mi (x; + yn ]
(11.67)
i=l
n
Ix y
Iy x
=-
L (mixiYi)
(11.68)
i =l n
Iyz
t.;
=-
L (miYizi)
(11.69)
i =l n
Izx
Ix z
=-
L (mizixi).
(11.70)
i=l
For a rigid body that is a continuous solid, the summations must be replaced by integrations over t he volume of t he body as in Equation (11.45). T he Euler equation of motio n for a rigid body is
(11.71) where BM is t he resultant of t he external moments ap plied on t he rigid body. T he ang ular momentum BL is a vector defined in t he bo dy coordinate frame . Hence, its time derivat ive in t he global coordinate frame is
(11.72) Therefore, dL
.
di =L+w xL
Iw +w x (Iw )
(11.73)
11. Motion Dynamics
BM
459
o,», + Ixywy + Ixzwz) i + (I yxWx + Iyywy + Iyzwz ) j + (I zxwx + I zywy + I zzwz) k +wy (I xzwx + I yzwy + Izzw z) i - Wz (I xywx + I yywy + Iyzw z) i +w z (Ixxwx + Ixywy + Ixzw z) j -W x (I xzwx + I yzwy + I zzwz ) j +wx (I xywx + I yywy + I yzwz) k - wy (I xxwx + Ixywy + Ixzwz) k
(11.74)
and t he most genera l form of the Eul er equations of motion for a rigid body in a body frame attached to eM of t he body are
M;
=
My
M,
Ixxwx + Ixywy + Ixzw z - (Iyy - I zz ) wyw z -Iyz (w; - w~ ) - Wx (wzI xy - wyI xz )
(11.75)
Iyxwx + Iyywy + I yzwz - (I zz - I xx) WzW x -Ixz (w; - w; ) - wy (wxIyz - wzI xy )
(11.76)
Izxwx + I zywy + I zzw z - (I xx - I yy) wxwy -Ixy (w~ - w; ) - Wz (wyI xz - wxI yz ) .
(11.77)
Assum e t ha t we can rotate the body frame about its origin to find an orient ation t hat makes I i j = 0, for i -I- j . In such a coordinate fram e, which is called a prin cipal frame, t he Euler equatio ns reduce to
M1 M2 = M3 =
h Wl - (I2 - 12) W2W3 h W2 - (h - h) W3W l h W3 - (h - 12) WIW2 .
(11.78) (11.79) (11.80)
The kinetic energy of a rigid body may be found by t he int egral of the kineti c energy of a mass element dm , over the whole body. K
r y dm 2 in ~ r (w x r) . (w x r) dm 2 iB w; r (y 2 + z2) dm + w; r (z2 + x 2) dm + w; r (x 2 + y2) dm 2 iB 2 iB 2 iB
= ~
2
- wxwy
l
x y dm - wywz
l
yz dm - WzW x
l
zx dm
:21 (Ixxwx2 + I yyw2y + Izzw2) z -Ixywxwy - Iyzwywz - Izxwzwx
(11.81)
460
11. Motion Dynamics
The kinetic energy can be rearranged to a matrix multiplication form 1
-w T1w
K
(11.82)
2
1
-w 2 ·L .
(11.83)
Wh en the body frame is principal, the kineti c energy will simplify to
(11.84)
• Example 254 St eady rotat ion of a freely rotating rigid body. Th e Newton-Euler equati ons of motion for a rigid body are G
m
"
1 ~WB
+ ~W B
x BL.
(11.85) (11.86)
Consider a situati on where the resulta nt applied fo rce and moment on the body are zero.
o o
F
M
(11.87) (11.88)
Ba sed on the Newton equati on, the velocity of the mass center will be constant in the global coordina te f rame . However, the Euler equation reduces to 12 - 13 -1-1-W 2W 3
(11.89)
h-h - I - - W 3W 1
(11.90)
22
t, -12 ----y;;-w 1 w2
(11.91 )
showing that the angular velocity can be constant if h=h=h
(11.92)
or if two principal moments of in ertia , say hand 12 , are zero and the third angular velocity, in this case W3 , is initially zero, or if the angular velocity vect or is ini tially parallel to a principal axis.
Example 255 Angular m ome ntum of a two-link manipulator. A two-link manipulator is shown in Figure 11.3. Link A rotat es with angular velocity rp about the z- axis of its local coordinate fram e. Link B is atta ched to link A and has angular velocity ;p with respect to A about the
11. Motio n Dyn amics
461
x ~ 7
-8
X
A
FIG URE 11.3. A two-link manipulat or.
-axis. We assum e that A and G were coincident at
XA
- sin
o
0] 0 1
= 0, therefore, the
.
(11.93)
Th e fram e B is related to the frame A by Euler angles sp = 90 deg, () = 90deg, and u. = 'l/J, hence, [ emf; - err'K' "
C'ljJS1r + C1rC1rs'l/J S1rS'l/J
A RB
[
- cos 'l/J sin 'l/J
sin'l/J cos 'l/J
0
0
- C1rs'l/J - C1rC'IjJS1r - S1rs'l/J + C1rC1rC'l/J S1rC'l/J
~1
sttstt ] - C1rS1r C1r
(11.94)
]
and therefore,
(11.95)
G RA A RB
[
- cos sp cos 'l/J - sin sp sin 'l/J cos
o
cos
o
~
- 1
].
462
11. Motion Dyn amics
Th e angular velocity of A in G , and B in A is
(11.96) (11.97) Mom ent of inertia matrices for the arm s A and B can be defined as
(11.98)
(11.99) Th ese moment s of in ertia must be transformed to the global fram e
(11.100) (11.101) Th e total angular momentum of the manipulator is
(11.102) where G I A GWA
(11.103)
GIBG wB G IB
(~W B
+ GWA) .
(11.104)
Example 256 Poinsot 's constru ction. Consider a freely rotating rigid body with an atta ched prin cipal coordinate fram e. Having M = 0 provides a motion und er constant angular momentum and kin etic energy L
K
I W = cte 1 2wT Iw = cie .
(11.105) (11.106)
B ecause the length of the angular momentum L is constant, the equation
(11.107) introduces an ellipsoid in the (WI , W 2 , W3 ) coordinate fram e, called the momentum ellipsoid. Th e tip of all possible angular velocity vectors must
11. Motion Dynamics
463
Momentum ellipsoid Energy ellipsoid \
FIGURE 11.4. Intersection of the momentum and energy ellipsoids .
lie on the surface of the momentum ellipsoid. The kinetic energy also defines an energy ellipsoid in the same coordinate frame so that the tip of angular velocity vectors must also lie on its surface . (11.108)
In other words, the dynamics of moment-free motion of a riqid body requires that the corresponding angular velocity w(t) satisfy both Equations (11.107) and (11 .108) and therefore lie on the intersection of the momentum and energy ellipsoids. For a better visualization, we can define the ellipsoids in the (Lx , L y , L z) coordinate system as (11.109) 1.
(11.110)
Equation (11.109) is a sphere and Equation (11.110) defines an ellipsoid with V2IiK as semi-axes. To have a meaningful motion, these two shapes must intersect. The intersection may form a trajectory, as shown in Figure 11·4· It can be deduced that for a certain value of angular momentum there are maximum and minimum limit values for acceptable kinetic energy . Assuming (11.111)
464
11. Motion Dynamics
the limits of possible kin etic energy are
«-:
(11.112)
=
K m ax
(11.113)
and the corresponding motions are turning about the axes hand tiv ely.
Example 257
h
respec-
*
Alternative derivation of Eul er equations of motion. Th e moment of the sm all for ce df is dm dm
Crdm x df c rdrn x c " drn dm.
(11.114)
Th e in ertial angular momentum dl of dm is equal to
(11.115) and according to (11.11) we have dm
(11.116) (11.117)
Integrating over the body is
r dt«« (C drn c d r (C dt } dm
}B
B
r
X
CV drn dm )
r
X
CV dm dm ) .
(11.118)
How ever, ut ilizing
(11.119) where c d B is the global posit ion vector of the central body fram e, may sim plif y th e left-hand side of th e integral to
L
(Cd B +
1
G RB B
c d B x df +
rdm ) x df
l ~rdm
c d B x cF+ c M c
x df (11.120)
11. Motion Dynamics
465
where M e is the resultant extern al moment about the body mass cente r C . The right-hand side of the Equation (11 .118) is
««
dt
1 (G B
rdrn x
GYdrn dm )
r
Gd ((G d B + GRB B rdrn) x GYdrn dm ) dt JB
1
(G d B -Gd dt B
:~ ( Gd B X G. dB
X
l
+ -dGd
G Ydrn) dm
t
GYdrndm)
1 B
(GBrdrn x G Ydrn) dm
+ :~Le
JrB GYdrn dm + Gd B X JrB G·Ydrn dm + dtd L c-
X
(11.121)
W e use L e for angular momentum about the body mass center. Since th e body frame is at center of mass, we have
l l l
Grdrndm
mG d B = m Gr e
(11.122)
GYdrndm
m
G· G d B = m Ye
(11.123)
G"drndm
m
G··
G d B =m a e
(11.124)
G G Gd G d B x F+ Le .
(11.125)
and th erefore,
r
Gd (G diJB r drn
X
G
Ydrn dm
)
=
di
Substituting (11.120) and (11 .125) in (11. 118) provides the Eu ler equation of motion in the global fram e, indicating that the resultant of extern ally applied moments about eM is equal to the global derivative of angular momentum about C'M , G Gd G M e = di L e · (11.126) Th e Eul er equation in th e body coordinate can be found by transf orm ing (11 .126)
(11.127)
466
11. Motion Dynamics
11.4 Mass Moment of Inertia Matrix In analyzing t he motion of rigid bodies, two types of integrals arise th at belong to the geomet ry of th e body. The first type defines the center of mass and arises when t he translati on motion of the body is considered. The second is the moment of inertia that appears when th e rotation al motion of t he body is considered . The moment of inertia is also called centrifugal m oments, or deviation moments. Every rigid body has a 3 x 3 moment of inertia matrix I , which is denot ed by
(11.128)
The diagonal elements I i j
,
i = j are called polar moments of inertia
l + =l + = l (x +
Ixx
= t; =
(y 2
z2) dm
(11.129)
Iy y
= t,
(z2
x 2) dm
(11.130)
2
y2) dm
(11.131)
t. , = t, and th e off-diagonal element s I i j
,
i
#j
-l t., l = t., = -l
I x y = I yx =
I yz
i .;
are called products of inertia
=
=-
x y dm
(11.132)
yz dm
(11.133)
zx dm .
(11.134)
The elements of I for a rigid bod y, made of discret e point masses, are defined in Equ ation (11.45) . The elements of I are moments of inertia about a body coordinate frame at tac hed to th e eM of th e bod y. Therefore, I is a frame-dependent quantity and must be written like B I to show th e frame it is computed in. -ZX
-yz
x 2 + y2
]
dm
(11.135)
(11.136) (11.137)
11. Motion Dynamics
467
Moments of inertia can be transformed from a coordinat e frame B 1 to anot her coordinate frame B 2 , bot h install ed at the mass cent er of the body, according to the rule of t he rotated-axes theorem (11.138) Tra nsformation of the moment of inertia from a central frame B 1 located at B2 rC to anot her frame B 2 , which is parallel to B 1 , is, according to the rule of parallel-axes theorem , (11.139) If t he local coordinate frame Oxyz is located such that t he products of inertia vanish, the local coordinate frame is called the prin cipal coordinate fram e and t he associat ed moments of inertia are called prin cipal moments of ine rti a. Prin cipal axes and prin cipal moments of inertia can be found by solving the following equat ion for I
Ix x - I Iyx Izx
Ix y Ix z Iy y - I Iyz Iz z - I Iz y det ([Iij ]- I [6ij ])
(11.140)
0
= O.
(11.141)
Since Equ ation (11.141) is a cubic equation in I , we obtain t hree eigenvalues (11.142) which are th e principal moments of inertia. We may ut ilize the homogeneous position vectors and define a more general moment of inertia, called pseudo inert ia matrix (11.143)
T his pseudo inerti a matrix can be expanded to show th at
- Ix x
+ Iy y + Iz z
Izx
Ix y Iy y 2 Izy
m x r.
m zc
2 B-
1=
Iy x
Ix x
-
Ix z
+ t ., Ix x
Iy z + Iy y 2 myc
mxc myc -
Izz
m zc m
(11.144)
468
11. Motion Dynamics
FIGURE 11.5. Two coordinate frames with a common origin at mass center of a rigid body. wher e,
~~
T{: ~~: ]
[ zc ] = [ ~ IB zdm
(11.145)
is the position of the mass center in the body frame. This vector is zero if the body fram e is central. Proof. Two coordinate frames with a common origin at the mass center of a rigid body are shown in Figure 11.5. The angular velocity and angular momentum of a rigid body transform from the fram e B I to a the frame B 2 by vecto r transformation rule (11.146) (11.147) However, Land ware related according to Equation (11.43) B IL
=
BII B I W
(11.148)
and therefore, B 2 RBI B I
B2 I
B 2W
I
B 2 R~l B 2 W
(11.149)
which shows how to transfer the moment of inertia from the coordinate frame B I to a rotated frame B 2 (11.150) Now consider a cent ra l frame B I , shown in Figure 11.6, at B 2rC , which rotates about the origin of a fixed fram e B 2 such that their axes remain
11. Mot ion Dynami cs
........... ..............
469
I
<.,
I
" " "
~
.>
"<, ~ ....,....
FIGURE 11.6. A cent ral coordinate frame B 1 and a translated frame B 2 .
parallel. T he angular velocity and angular momentum of the rigid body transform from the fram e B 1 to the fram e B 2 by
BlW BIL
(11.151)
+ (rc x mvc) .
(11.152)
Therefore,
Bl L +mB2 rc x (B2wxB2rc) BlL +
(B
l
(m
B2 r~)
B2 rc
1+ m B2rc
B2r~)
B2 W B2W
(11.153)
which shows how to transfer th e moment of inertia from frame B 1 to a parallel frame B 2 (11.154) The parallel-axes t heorem is also called the Huygens-Steiner theorem. Referring to Equation (11.150) for transformation of the moment of inertia to a rot ated frame , we can always find a fram e in which B2 I is diagonal. In such a frame, we have
B2RB I BII = B2 I B2R Bl
(11.155)
or
[ t., Iyx Izx
Ix y Iyy Izy
iI.y z, ]
['11
1'12
1'21
1'22
' 13 ] 1'23
1'31
1'32
1'33
t. ,
[~
0
oo ][
'11
1'12
[2
1'21
1'22
1'23
0
h
7'31
7'32
1'33
1'13
] (11.156)
470
11. Motion Dynamics
which shows t hat l i , !Z , and h are eigenvalues of BII. These eigenvalues can be found by solving the following equa tion for A.
= 0.
(11.157)
T he eigenvalues h , 12 , and h are principal moments of inertia, and their assoc iated eigenvectors are called principal directions. The coordinate frame made by the eigenvectors is the principal body coordinate fram e. In the principal coordinate frame , the rigid body angular momentum is (11. 158)
• Example 258 Principal moments of inertia. Consider the inertia matrix 20 1=
[
~2
(11.159)
we set up the determinant (11.141) 20 - A
-2
-2
30 - A
o
o
o o
=0
(11.160)
40 - A
which leads to the characteristic equation
o.
(20 - A)(30 - A) (40 - A) - 4 (40 -),) =
(11.161)
Three roots of Equation {11.161} are
12 = 19.615,
h = 30.385 ,
h = 40
(11.162)
and therefore, the principal moment of inertia matrix is 1=
[
30.385 0
o
0 19.615
0
~
40
]
.
(11.163)
Example 259 Principal coordinate frame . Consid er the inertia matrix 20 1=
[
~2
(11.164)
11. Motion Dynamics the direction of a principal axis
[
I~ I t.
i: - t, I yx
i:
is established by solving
Xi
i;
I yy
] [
l yz
cos Qi ] _ [ 0 ] cos f3 i 0 COSIi 0
i . . - t,
I zy
(11.165)
for direction cosines, which must also sati sfy 2 2 cos Qi + cos f3 i + cos2 Ii = 1.
-2
-2
30 - 30.385
[
oo
o
o
(11.166)
= 30.385 we have
For the first principal moment of inertia I ,
20 - 30.385
471
]
[ COSQ1 ]
cosf3 1
40 - 30.385
cos 1 1
=
[~ ]
(11.167)
or
- 10.385COSQ1- 2cosf31 +0
0
(11.168)
=
0
(11.169)
0 + 0 + 9.615cos ' 1 =
0
(11.170)
- 2COSQ1 - 0.385cos f31 + 0
and we obtain
Q1
79.1 deg
(11.171)
f3 1
169.1 deg
(11.172)
11
90.0 deg .
(11.173)
Using 12 = 19.615 for the second princ ipal axis
20 - 19.62 -2
[
o
-2 30 - 19.62 0
oo
] [ cos Q2 ] cos f3 2
40 - 19.62
cos 12
[~ ]
(11.174)
we obtain
Q2
10.9deg
(11.175)
f32
79.1 deg
(11.176)
12
90.0 deg.
(11.177)
Th e third prin cipal axis is for
[
20 - 40
-2
-2
30 - 40
o
o
h
= 40
oo
]
40 - 40
[ COSQ3 ] cos f3 3 cos 13
(11.178)
which leads to
Q3 f3 3
13
=
90.0 deg
(11.179)
90.0 deg
(11.180)
O.O deg .
(11.181)
472
11. Motion Dynamics
z
y HI
x
FIGURE 11.7. A homogeneous rect angular link.
Example 260 Moment of inertia of a rigid rectangular link. Consider a homogeneous rectangular link with mass m , length I, width w, and height h , as shown in Figure 11.7. The local central coordinate fram e is attached to the link at its mass center. The moments of in ertia matrix of the link can be found by integml method. We begin with calculating I x x
(11.182)
which shows I y y and I z z can be calculated similarly
I yy
(11.183) (11.184)
Since the coordinate frame is central, the products of inertia must be zero.
11. Motion Dynamics
473
z'
y' X'
FIGURE 11.8. A rigid rectangular link in th e principal and non principal fram es.
To show this, we exam ine
l xy .
=
lyx
1
-l
xydm
x ypdv
m jh /2 jW /2 jl /2 x y dx dy dz lwh -h/2 - w/ 2 -1/2
o
(11.185)
Th erefore, the moment of inertia matrix for the rigid rectangular link in its cen tral fram e is
(11.186)
Example 261 Translation of the inertia matrix. Th e moment of inertia matrix of the rigid link shown in Figu re 11.8, in the principal fram e B(oxy z) is give in Equation (11.186) . Th e moment of inertia matrix in the non principal fram e B' (ox' y' z' ) can be found by applying the parallel-axes transformation formula (11.154) . (11.187)
Th e cent er of mas s position vector is
(11.188)
474
11. Motion Dynamics
and therefore, B'
ic =
1
2[
0 h
(11.189)
-1J)
that provides
(11.190) Example 262 Principal rotation matrix. Consider a body in ertia matrix as
1=
2/3 -1 /2 [ -1 /2
-1 /2 5/3 -1 /4
-1 /2 ] -1 /4 . 5/3
Th e eigenvalues and eigenvectors of I are
0.2413 ,
1.8421 ,
1.9167 , Th e normalized eigenvector matrix W is equal to the transpose of the required transformation matrix to make the in ertia matrix diagonal
W
J [J1 I I
+]~
0.8569 0.36448 [ 0.36448
-0.5156 0.60588 0.60588
2
'Rf 0.0 ] -0.70711 . 0.70711
We may verif y that 2 R 1 1 I 2 R[ =
0.2413 -1 X 10- 4 [ 0.0
wT
1I
W -1 x 10-4 1.842 1 0.0
0.0 ] -1 X 10- 19 1.9167
.
11. Motion Dynamics
475
*
Example 263 R elative diagonal moments of inertia. Using the definition s for m om ent s of inertia 11.129, 11.130, and 11.131 it is seen that the inertia matrix is symmetri c, and (11.191) and also
i:
+ I yy I y y + t ., t. , + i:
(11.192) (11.193) (11.194)
Not ing that it is evident that and therefore (11.195) and similarly
> u .; t.. > 2lx y .
I yy
(11.196) (11.197)
*
Example 264 Coeffi cients of the charact erist ic equati on . Th e determ inant (11.157)
=0
(11.198)
fo r calculating the prin cipal moment s of inerti a, leads to a third degree equati on of A, called the characteristic equation. A3 - alA 2 + a2A - a3 = 0 (11.199) Th e coeffi cients of the characteris tic equation are called the principal in variants of [I] . Th e coeffic ien ts can directly be found f rom the following equati ons: I xx tr
+ I y y + t .,
[I]
(11.200)
(11.201)
476
11. Motion Dynamics
a3
= l xxlyylzz + l xylyzlzx + l zylyxlxz - (Ixxlyzlzy + l yylzxlxz + l zzlxylyx) l xxlyylzz + 2Ixylyzlzx - (Ixx 1; z + l yyl;x + l zzi;,y)
det [I]
(11.202)
*
Example 265 Th e prin cipal mom ent s of in ertia are coordin ate inv ariants. Th e roots of the inertia characteris tic equati on are the principal m oments of inert ia and all real but not necessarily different . Th e prin cipal m ome nts of inertia are extreme . That is, the prin cipal moments of inertia determ ine the smallest and the largest values of I i i fo r a rigid body. Sin ce the smallest and largest values of h do not depend on the choice of the body coordin ate fram e, the solution of the characteris tic equati on is no t dependent of the coordinate fram e. In oth er words, if h , 12 , and 13 are the prin cipal m om ent s of ine rtia for B 1 I , the prin cipal mom ents of inertia for B 2 I are also II , 12 , and 13 when
We conclude that h , h , and 13 are coordinate invariants of the m atrix [I]' and therefore any quant ity that depends on h , h , and 13 is also coordinate in variant. Th e matrix [I] has only three in dependent in variants and every oth er in variant can be expressed in terms of h , 12 , and h . Sin ce h , 12 , and 13 are the soluti ons of the characteris tic equation of [I] given in (11.199) , we m ay write the determinant (11.157) in the form
(11.203) Expand ed form of this equati on is
By comparing (11 .204) and (11.199) we conclude that al
l xx + l yy + l zz
= I, + 12 + h
a2
l xxlyy + l yylzz + l zzlxx - l ; y - l;z - l ;x
hh + 1213 + h h a3
(11.205) (11. 206)
l xxlyylzz + 21xyly zlzx - (Ixx 1; z + l yyl;x + l zzl; y) h12 h (11.207)
B eing able to express the coefficie nts aI , a2, and a3 as functions of h, 12, and h determines that the coefficie nts of the characteri stic equati on are coordinate in variant .
11. Motion Dynamics
477
*
Example 266 Sh ort notation of the eleme n ts of in ertia matrix. Taking advantage of the Kronecker's delta (2.133) we may write the eleme nts of the moment of ine rtia matrix I ij in short notation f orms.
t.,
t., t., =
is ( (xi+ x~+ xD bij
- x ix j ) dm
is(
r 2bij - XiXj ) dm
(11.209)
r (t XkXkbij - XiXj ) dm
l»
(11.208)
(11.210)
k =l
where we utili zed the fo llowing notations: X2 = Y
X3
= z.
(11.211)
*
Example 267 Moment of inertia with respect to a plan e, a lin e, and a point. Th e moment of ine rti a of a system of particles may be defin ed with respect to a plane, a line, or a point as the sum of the products of th e mass of the part icles into the square of the perpendi cular distance f rom the particle to the plan e, line, or point. For a continuous body, the sum would be defin it e integral over the volume of the body. Th e moment s of inertia with respect to the x y , yz , and zx -plane are
is
I z2
2 z dm
L
Iy2
y 2dm
L
2 x dm .
I x2
(11.212) (11.213) (11.214)
Th e moments of ine rtia with respect to the x , y , and z -axis are
is is
Ix Iy
is
t,
(y2 + z2 ) dm
(11.215)
2 (z 2 + x ) dm
(11.216)
(x
2
+ y2) dm
(11.217)
+ Iz + Ix + Iy
2
(11.218)
2
(11.219)
2 .
(11.220)
and th erefore,
Ix Iy Iz
Iy 2 I z2 I x2
478
11. Motion Dynamics
Th e moment of ine rti a with respect to the origin is
l
Ix2 1
(x2 + y 2 + z 2) dm
+Iy +Iz 2
2
2 (Ix + I y + I z) .
(11.221)
B ecause th e choice of th e coordinate fram e is arbitrary , we can say that th e moment of inertia with respect to a lin e is the sum of th e moments of inerti a with respect to any two mutually orthogonal plan es that pass through the lin e. Th e moment of in ert ia with respect to a point has similar m eaning fo r three mutually orthogonal plan es in tersecting at the point.
11.5 Lagrange's Form of Newton's Equations of Motion Newt on's equation of motion can be transformed to r=1 ,2 , · · ·n
(11.222)
where Fr =
Ln
i= l
(
Ofi agi Ohi ) FiXa + FiYa + Fiz & qi q2 qn
.
(11.223)
Equ ation (11.222) is called t he Lagrange equati on of motion, where K is th e kinet ic energy of t he n DOF syst em, qr, r = 1,2 , . .. , n are the generalized coordinates of the syste m, F = [FiX F iy Fiz] T is the ext ern al force acting on the it h par ticle of t he system, and F; is the generalized force associated to qr' Proof. Let m , be the mass of one of the particles of a system and let (Xi , Yi , Zi) be its Cart esian coordinates in a globally fixed coordinate frame. Assum e that the coordinates of every individual particl e are function s of anot her set of coordinates q1 , q2, Q3, . .. , qn and possibly tim e t.
Yi
fi (q1, in , q3, . . . , qn , t )
(11.224)
gi(q1 ,q2 ,q3, ' " , qn, t )
(11.225)
h i(q1 ,Q2,q3, ' " , Qn , t )
(11.226)
If F xi , Fyi, F zi are components of t he total force acting on th e part icle m , then, the Newton equations of mot ion for the par ticle would be
F xi
m ix i
(11.227)
Fyi
m iYi
(11.228)
r;
m izi ·
(11.229)
11. Motion Dynamics
479
We multiply both sides of thes e equations by afi aqr agi aqr
m; Bq; respectively, and add them up for all the particles to have
~
s: mi i=l
.. agi ah i) ~ (F ot, F agi F ah i) + z;.. a = s: xi a + yi a + zi a a + »s: qr qr i=l qr qr qr
( .. afi Xi qr
(11.230) where n is the total number of particles. Taking a time derivative of Equation (11.224), (11.231) we find
(11.232) and th erefore, .. aXi Xiaqr d ( . aXi) . d (aXi) dt Xi aqr - Xi dt aqr .
(11.233)
However,
and we have (11.235)
480
11. Motion Dynamics
which is equal to (11.236) Now substituting (11.233) and (11.236) in the left-hand side of (11.230) leads to
(11.237) where
1
n
2 L mi (x; + ill + i =l
in =
(11.238)
K
is the kinetic energy of the system . Therefore, the Newton equations of motion (11.227), (11.228), and (11.229) are converted to
d (OK) etc -;:;-:- - ~ dt oq; oq;
=
~ O fi + Fyi~ Ogi + FZi~ Ohi) . Z:: (FXi~ i= l
uqr
oq;
oq;
(11.239)
Because of (11.224), (11.225), and (11.226), the kinetic energy is a function of q1 , qz , q3,' " , qn and time t. The left-hand side of Equation (11.239) includes the kinetic energy of the whole system and the right-hand side is a generalized force and shows the effect of changing coordinates from X i to qj on the external forces. Let us assume that the coordinate qr alters to qr + Sq; while the other coordinates q1,q2, q3, ... , qr- 1, qr+l, . . . , qn and tim e t are unaltered. So, the coordinates of m; are changed to (11.240) (11.241) (11.242)
11. Motion Dynamics
481
and the work done in this virtual disp lacement by all forces act ing on the particles of t he syst em is (11.243) Since the work done by int ernal forces appears in opposi te pairs, only the work done by exte rn al forces remains in Equation (11.243) . Let's denote the virtual work by (11.244) Then we have (11.245) where (11.246) Equation (11.245) is the Lagrange form of equations of motion. This equation is true for all values of r from 1 to n. We th us have n second ord er ordinary differential equations in which ql, qz, q3, .. . , qn are the dep endent variab les and t is t he ind ependent variable. The coordinates ql , qz , q3, ' " , qn ar e called generalized coordinat es and can be any measurab le parameters to prov ide the configuration of the system . Since the number of equations and t he number of dependent variables ar e equal, the equations are theoreti cally sufficient to determine t he motion of all mi. • Example 26 8 A simple pendulum. A pendulum is shown in Figure 11.9. Using x and y for Cart esian position of m , and using 8 = q as the generalized coordinat e, we have
x
f(8) = Isin8
Y
g(8) 1
K
-m
2
(11.247)
= l cos 8 2
2
(11.248)
(x + y ) =
1 2 ·2 - ml 8
(11.249)
2
and therefore,
d(O K) o« d 00 - 08 = dt (ml 8) = m l 8. 2'
dt
2"
(11.250)
The external force compon ents, acting on m , are
o
(11.251)
mg
(11.252)
482
11. Motion Dynamics
pivot
x
y ,
~I~
-------------
--------- ---------
»->
0 '--m
--
FIGURE 11.9. A simple pendulum .
and therefore, of
Fe = Fx oe
og
+ Fy oe =
. -mgl sm e.
(11.253)
Hence, the equation of motion for the pendulum is ml 2
e= - m gl sin e.
(11.254)
Example 269 A pendulum attached to an oscillating mass. Figure 11.10 illustrates a vibrating mass with a hanging pendulum. The pendulum can act as a vibration absorber if designed properly. Starting with coordinate relationships XM
fM =X
(11.255)
YM Xm
gM =0 fm = x + lsine
(11.256)
Ym
gm = l case
(11.258)
(11.257)
we may find the kinetic energy in terms of the generalized coordinates x and e.
K (11.259)
Then, the left-hand side of Lagrange equations are
d(O - K) -e« ox ox d(OK) etc dt oj) - ae
dt
(M
..
+ m)x + ml()cose -
mz2e + ml.i: cos e.
·2
ml() sine
(11.260) (11.261)
11. Motion Dyn amics
483
x
m
,y
FIGURE 11.10. A vibr ation absorber.
Th e extern al forces acting on M and mare
-kx
(11.262)
o o
(11.263)
mg.
(11.26 5)
(11.264)
Th erefore, the generalized forces are
Fo
and finally the Lagrange equations of motion are ..
·2
(M +m) x+mW cosO-mW sinO ml 2 (j + mlii: cos 0
-kx
(11.268)
-mglsinO.
(11.269)
*
Example 270 Potential fo rce field. If a system of masses mi are moving in a potential fo rce field F m i = -'\liV
(11.270)
the ir N ewton equations of motion will be m iri
= -'\li V
i
= 1,2"" n .
(11.271)
484
11. Motion Dyn amic s
Inner product of equations of motion with n
ri
and adding the equations
n
Lmiri · i\
= -
i =l
Lri · V'Y
(11.272)
i= l
and then, integrating over time (11.273)
shows that K
-
J
n
(OV
sv
OV)
" - Xi+-Yi + - Zi L.J ox oy·, iiz,, i=l '
-V+E
(11.274)
where E is the constant of int egration . E is called mechanical energy of the system and is equal to kin etic plus potential energies.
Example 271 Kin etic energy of the Earth . Earth is approximately a rotating rigid body about a fixed axis. The two motions of the Earth are called revolution about the sun, and rotation about an axis approximately fix ed in the Earth . The kin etic energy of the Earth due to its rotation is = = =
1
2
2I w1 ~~ (5.9742 X
1024) (6356912 + 6378388) 2 ( 21f 366.25) 2 2 24 x 3600 365.25 2.5762 x 1029 J 25
and the kin etic energy of the Earth due to its revolution is
where r is the distan ce from the sun and W2 is the angular speed about the sun. The total kin etic energy of the Earth is K = K 1 + K 2 . However, the ratio of the revolutionary to rotational kin etic energies is K2 K1
2.6457 2.5762
X
X
10 ~ 10000. 1029 33
11. Motion Dynamics
Example 272
485
*
Non Cartesian coordinate system . Th e parabolic coordin ate sy st em and Cartesian coordin ate sy st ems are relat ed according to x
TJ~ cos c.p
(11.275)
y
TJ~ sin c.p
(11.276)
(e-TJ 2
z
e TJ2 c.p
2)
(11.277)
J x 2 + y2 + Z2 + z J x 2 + y2 + z 2 - z t an -
1
(11.278) (11.279)
-y .
(11.280)
x
An elect ron in a uniform electri c fi eld along th e posit iv e z -axis is also under th e action of an attractive central force fi eld due to th e nuclei of th e atom.
(11.281) Th e infl uence of a uniform electric fi eld on th e motion of th e electrons in atoms is called th e Stark effe ct and it is easier to analyze its motion in a parabolic coordinate system . Th e kinetic ene rgy in a parabolic coordin ate system is
~m (x2 + il + i 2 ) 2
K
~m [(TJ2 + e)
(1]2+ ~2) + TJ 2e02]
(11.282)
and th e force acting on th e electron is F
=
- V
(-~ + eEZ )
-v( e +
2k
TJ2
+
eE (C2 _
2
<"
TJ
2) )
(11.283)
whi ch leads to th e following gen eralized [orces:
(11.284) (11.285) (11.286)
486
where,
11. Motion Dynamics b~ ,
b Tl' and b
b~
~~ = 1] cos .pi + 1] sin cpj + ~k
(11.287)
b ry
or = <"c cos cp2' C " 01] + <" sin CPJ -
1]k'
(11.288)
b
or ocp
' + 1]<"C cos CPJ·
(11.28 9)
" = - 1]<"C sin cp2
Th erefore , following the Lagrange method, the equations of motion of the electron are Fry
:t [m1] (e + 1]2)] - (1]2+ ~2) - m1]erp2 :t [m~ (e + TJ2) ] - m1] (1]2+ ~2) - m~TJ2rp2 :t (m1]2erp2) . ttu;
F~ r;
*
(11.290) (11.291)
(11.292)
Example 273 Explicit form of Lagrange equations. Assume that the coordinates of every particle are functions of the coordinates qi , qz , q3, ' " ,qn but not the time t . The kinetic energy of the system made of n massive particles can be written as 1 n K = '2 'L..J " m, ('2 ·2+ Zi.2) X i + u; i=i
1
n
n
'2 L L ajkitjiJk
(11.293)
j=ik=i where the coeffici ents ajk are functions of qi , q2, q3, " ' , qn and (11.294)
ajk = akj ' Th e Lagrang e equations of motion
d(O K) etc _ F oitr - oqr -
dt
r
r
= 1,2, ' "
n
(11.295)
are then equal to (11.296)
or
n n n L amrqm + L L r;;,nitkitn = m=i k=i n=i where fj ,k is called the Ch ristoffel operato r
ri k = ~ J,
2
( oa ij Oqk
+ oaik oqj
t;
(11.297)
_ oa kj ) . Oqi
(11.298)
11. Motion Dynamics
11.6
487
Lagrangian Mechanics
Assum e for some forces F = [Fix Fiy Fiz] T there is a function V , called pot ential energ y, such that t he force is derivable from V F = -\lV.
(11.299)
Such a force is called pot ential or cons ervative force. Then , the Lagr ange equat ion of motion can be written as
r = 1,2 , · · ·n
(11.300)
wher e
£=K -V
(11.301)
is the Lagrang ean of the system and Qr is the nonpotential generali zed force. P roof. Assume the external forces F = [FXi Fyi F zi] T acting on the syst em are conservative. F = - \lV (11.302) The work done by these forces in an arbitrary virtual displacement 8ql , 8q2 , 8q3, . . . , 8qn is
av - -av8q2 aw = --8ql 8ql 8q2
»v
.. . -8qn 8qn
(11.303)
then the Lagrange equation becomes
:t (~~) - ~:
~~
= -
r = 1,2" · · n .
Introducing the Lagrangean function E equation to d dt 8qr - 8qr -
(11.304)
= K - V converts the Lagrange
(8£) 8£ _0
r = 1,2" " n
(11.305)
for a conservative system. The Lagrangean is also called kin eti c potential. If a force is not conservative, then t he work done by the force is
L n
8W
~l
(
8 Ii
F Xi a
~
8gi
+ Fyi a
~
8h i
+ FZi a
)
~
o; 8qr
8qr (11.306)
and the equation of motion would be
i
dt
(8£) _8£ = o, 8qr
8qr
r = 1, 2, · · · n
(11.307)
where Q r is the nonpotenti al genera lized force doing work in a virtual displacement of the rth gener alized coordinate qr' •
488
11. Motion Dynamics
z
" y
x~
FIGURE 11.11. A spheric al pendulum.
Example 274 Spherical pendulum. A pendulum analogy is utilized in modeling of many dynamical problems . Figure 11.11 illustrates a spherical pendulum with mass m and length l . The angles c.p and () may be used as describing coordinates of the system. The Cartesian coordinates of the mass as a function of the generalized coordinates are
~ ] = [ ~~~~:s~~; ] -rcos()
(11.308)
[ Z
and therefore , the kinetic and potential energies of the pendulum are K
~m (l 2 il + z2tj} sin 2 ())
(11.309)
V
-mgl cos().
(11.310)
The kinetic potential function of this system is then equal to
I:- =
~m (l 2il + l2(p2 sin 2 ()) + mgl cos ()
(11.311)
that leads to the following equations of motion:
t
iJ - rp2 sin () cos () + sin () if? sin 2 () + 2rpB sin () cos () =
0
(11.312)
O.
(11.313)
Example 275 A one-link manipulator. A one-link manipulator is illustrated in Figure 11.12. Assume that there is viscous friction in the joint where an ideal motor can apply the torque Q to move the arm. The rotor of an ideal motor has no moment of inertia by assumption.
11. Motion Dynamics
489
y
Q
FIGURE 11.12. A one link manipulator . The kinetic and potential energies of the manipulator are
~ Iil
K
2 1
2 (Ie + ml
v=
2
·2
)0
- m g cos 0
(11.314) (11.315)
where m is the mass and I is the moment of inertia of the pendulum about
O. The Lagrangean of the manipulator is
K-V 1 ·2 210 + m g cos O
(11.316)
and therefore, the equation of motion of the manipulator is
d(0£) ec 00 - 00
M
dt
Ie+ mgl sin e'.
(11.317)
The generalized force M is the contribution of the motor torque Q and the viscous friction torque - cO. Hence , the equation of motion of the manipulator is Q= + cO + mgl sin O. (11.318)
Ie
Example 276 The ideal 2R planar manipulator dynamics. A n ideal model of a 2R planar manipulator is illustrated in Figure 11.13. It is called ideal because we assumed that the links are massless and there is no friction . Th e masses ml and m2 are the mass of the second motor
490
11. Motion Dynamics
~'fF\---'----'----------~
X
FIGURE 11.13. An ideal model of a 2R planar manipulator. to run th e second link and th e load at th e en dpoint. We take the absolute angl e 0 ] and the relati ve angle O2 as th e gen eralized coordina tes to express the configuration of th e manipulator. Th e global position of m] and m 2 are [
~:
] = [
~: ~~:~~
(11.319)
]
X2 ] = [!r COSO] +l2 cos (O] +( 2) ] [ Y2 l] sinO] + l2 sin (0] + ( 2)
(11.320)
and th erefore, th e global velocity of th e m asses are
(11.321)
[1:]
=
[
-i». sin O] -i. (0] + O
+ (2 ) !r0] cos O] + h (0] + O2 ) cos (0] + ( 2 ) 2)
sin (0]
]
(11.322) •
Th e kin eti c ene rgy of this manipulator is made of kin etic energy of the masses and is equal to
K
K] +K2
~ml
(X; +
1
2· 2
yn + ~m2 (Xi + Yl)
2m]l ]0]
+~m2 (liO~ + l~ (0
1
f
+ O2 + 2!rl201 (0 1 + O2) cos O2)
.(11.323)
11. Motion Dynamics
491
The potential energy of the manipulator is
V
V1+V2 m1gY1 + m2gY2 m1gh sin 81 + m 2g (h sin 81 + l2 sin (81 + 82)), (11.324)
The Lagrangean is then obtained from Equations (11.323) and (11.324)
c
K - V 1 2·2 "2m1l181
(11.325)
f
+~m2 (liO~ + l~ (01 + O2
+ 2h l2 01 (01 + ( 2) cos82)
- (m1ghsin8 1 +m2g(hsin8 1 +l2sin(8l +8 2))) ,
which provides the required partial derivatives as follows:
o£
08 = - (m1 + m 2)gh cos8 l - m2gl2 cos (81 + 82)
(11.326)
1
(ml + m2) liol +
m2l~
(0
1
+ ( 2)
+m2hl2 (2ih + ( 2) cos 82
d(0£) 00
dt
(ml + m 2) li01 +
1
+m2hl 2
(20
1
m2l~ (0 1 + O2)
+ O2) cos 82
-m2h l202 (2ih + ( 2) sin 82
o£
. (.
.)
08 = -m2h l28l 81 + 82 sin 82 - m 2gl2 cos (81 + 82) 2 . -ec . = m 2l22 ( 8. 1 + 8.2) + m 2hl 28l cos 82
08 2
d dt
(0£) 00 =m2l2 8 2 ( "
.. )
1+82
(11.327)
(11.328) (11.329) (11.330)
" •• (11.331) +m2hh8l cos82-m2hl 28l82sin82
2 Therefore, the equations of motion for the 2R manipulator are d
dt
(0£) 00 1
oc
-
08 1
(ml + m 2) liOl +
+m 2h12
(20
1
m2l~
(0
1
+ O2)
+ O2) cos (h - m 21l1202 (20 1 + ( 2) sin 8 2
+ (m 1 + m 2) gh cos 81 + m2gh cos (81 + 82)
(11.332)
492
11. Motion Dynamics
d dt
(f)£) oc f)fh - f)(h
m2l~ (8 1 + 82)
+ m2h l281 cos (}2 - m2 h l2fh e 2 sin (}2
+m2hl2el (e 1 + ( 2) sin(}2 + m 2gl2 cos ((}1 + (}2)
(11.333)
Th e gen eralized forces Q l and Q2 are the required for ces to drive the gen eralized coordinates. In this case, Ql is the torque at the base motor and Q2 is the torque of the motor at mI . Th e equati ons of motion can be rearranged to have a more systematic fo rm
((ml + m 2) li + m 2l2 (l2 + 2h cos (}2)) 81 +m2l2 (l2 + h COS (}2 ) 82 .
.
·2
-2m2hl2sin(}2 (}1(}2 -m2h l2sin(}2 (}2
+ (m l +m2)gllcOS(}1 +m2gl2 coS((}1 +(} 2) "
(11.334)
2"
m2l2 (l2 + h cos (}2) (}1 + m2l2(}2 ·2
+m2h l2sin(}2 (}1 + m2gl2 COS((}1 +(}2) .
11.7
(11.335)
Summary
The t ranslat ional and rotat iona l equat ions of motion for a rigid body, expressed in the global coordina te fram e, are cd C
ill Cd
P
(11.336)
cL
(11.337)
dt
where cF and c M indi cate t he resultant of t he exte rnal forces and moments applied on the rigid body and measured at CM. The vector c p is the momentum and c L is the moment of mom entum for the rigid bo dy at
CM P L
mv
(11.338)
rc x p .
(11.339)
The expression of the equat ions of motion in the body coordinate fram e ar e
(11.340)
11. Motion Dynamics
493
(11.341)
where 1 is t he moment of inertia for the rigid body.
(11.342)
The elements of 1 are only functio ns of the mass distribution of the rigid body and are defined by i,j
= 1,2,3
(11.343)
where bij is Kronecker's delta. Every rigid body has a principal body coordinate frame in which the moment of inertia is in the form
0]
o 0 o h
h
(11.344)
.
The rotational equation of motion in the princ ipal coordinate frame simplifies to
/rWl -
(12 -
12 ) W2 W 3
12 w2
(h -
Id W3 W l
-
13 w3 -
(11.345)
(h - h) WIW2 ·
Utilizing homogeneous position vectors we also define pseudo inertia matrix I with app lication in robot dynamics as
BI =
l
T
(11.346)
rr dm
[ -,,,+;,,,+1,.
where ,
B re
I xy
l xz l yz
truer;
l yx l zx
I x x -Iyy+lzz 2
l zy
I xx + l yy - I zz
2
mYe mze
mxr.
mze
mYe
m
]
is the position of the mass center in the body frame.
Bre
=
xc ] [
uc zc
=
*
[ . !. IB xdm ] IB ydm :;nIB zdm
(11.347)
494
11. Motion Dynami cs
Th e equations of motion for a mechanical system having n DOF can also be found by th e Lagrange equat ion r = 1,2 , ·· ·n
£=K-V
(11.348) (11.349)
where E is th e Lagrangean of the syst em, K is th e kinetic energy, V is the pot enti al energy, and Qr is the nonpo tential generalized force. (11.350) The parameters qr , r = 1,2 , · · · , n are the generalized coordinates of the syste m, Q = [QiX Qiy Qiz] T is t he external force act ing on the ith particle of the system, and Qr is the generalized force associated to qr . Wh en (Xi ,Yi, Zi ) is th e Cartesian coordinates in a globally fixed coordinat e frame for th e particle m i , th en its coordinates may be function s of another set of coordinates qi , q2, q3,· ·· , qn and possibly tim e t.
Yi
fi( ql, q2,q3, · ·· , qn, t )
(11.351)
gi(ql , qz , q3, · · · , qn, t)
(11.352)
hi(ql , q2, q3, · · · , qn, t)
(11.353)
11. Motion Dynamics
495
Exercises 1. Notation and symbols .
Describe their meaning. a- c p p
b- L,
c- c F
d- IW2
g- dm
h- I
i- I i j
j-
m- 02(XI
n-
2IW 2
0-
CAB
J
p- c'VB
e- K
f- V
k- c M
1- It
q- j
r-
X
2. Kinetic energy of a rigid link. Consider a straight and uniform bar as a rigid link of a manipulator. The link has a mass m . Show that the kinetic energy of the bar can be expressed as K =
where
VI
and
V2
1
13m (VI ' VI + VI ' V2 + V2 . V2)
are the velocity vectors of the endpoints of th e link.
3. Discrete particles. There are three particles
ml
= 1 kg, m2 = 2 kg, m 3 = 3 kg, at
Their velocities are
Find the position and velocity of th e system at eM. Calculate the system's momentum and moment of momentum. Calculate the system's kinetic energy and determine th e rotational and translational parts of the kinetic energy. 4. Newton's equation of motion in the body frame . Show th at Newton's equation of motion in the body frame is
496
11. Motion Dynamics
5. Work on a curved path.
A particle of mass m is moving on a circular path given by Gr p =
e + sin ej + 4 k .
cos j
Calcul at e the work done by a force GF when the particle moves from = 0 to = I'
e
e
(a) G
F=
2 2 - y2 Y2 - x 2 X -Y 2 /+ 2J + 2K (x+y) (x+y) (x+ Z) Z
2
A
A
A
(b) G
F=
2Y - y2 X 2 -y 2 2/ + - - J + 2K (x+y) x+y (X+ Z) Z2
A
A
A
6. Newton's equat ion of motion.
Find the equations of motion for the syste m shown in Figure 11.10 based on Newton method. 7. Acceleration in the body frame . Find the acceleration vector for the endpoint of t he two-link manipulator shown in Figure 11.3, expr essed in frame A. 8. Principal moment s of inerti a. Find the prin cipal moments of inert ia and directions for the following inertia matrices: (a)
[I]
=
[l ~ ] [! n 2 2
0
(b)
2
[I ] =
0 2
(c)
[I]
=
[ 100
20f
20J3 60 0
l~O
]
9. Moment of inertia. Calculate the moment of inertia for t he following obj ects in a principal Cartesian coordinate frame at eM.
11. Motion Dynamics
497
r
h
FIGURE 11.14. A cylinder.
(a) A cylinder similar to a uniform arm with a circular cross section. (b) A rectangular box similar to a uniform arm with a rectangular cross section.
b h a
FIGURE 11.15. A box.
(c) A house similar to a prismatic bar with a nonsymmetric polygon cross section.
b FIGURE 11.16. A solid house.
10. Rotated moment of inertia matrix. A principal moment of inertia matrix
[~~ [H
B2I
is given as
n
The principal frame was achieved by rotating the initial body coordinate frame 30 deg about the x-axis, followed by 45 deg about the z-axis . Find the initial moment of inertia matrix B'I.
498
11. Motion Dynamics
11. Rotation of moment of inertia matrix. Find the required rotation matrix that transforms the moment of inertia matrix [I] to an diagonal matrix.
2 2 0.1 12. Pseudo inertia matrix. Calculate the pseudo inertia matrix for the following objects in a principal Cartesian coordinate frame at eM. (a) A uniform arm with a rectangular cross section.
II
FIGURE 11.17. A prismatic bar.
(b) A compound shape as a housing for wrist joints .
II
c FIGURE 11.18. A housing for wrist joints.
(c)
*
A gripper, which is a compound shape made of some 3D geometrical shapes.
13. Cubic equations. The solution of a cubic equation ax 3
where a
+ bx 2 + ex + d =
0
f::. 0, can be found in a systematic way.
11. Motion Dynamics
499
h/3
FIG URE 11.19. A gripper.
Tr ansform the equat ion to a new form with discriminant 4p3 + q2 , y3
using the transformation x
+ 3py + q = 0
=y-
b 3a '
where,
3ac - b2 9a2 3 2b - 9abc + 27a2d 27a3
p q
The solutions are then
u,
ija- ~
Y2
e 2;;i ija _ e 4;;i ~
Y3
e 4;;i ija _ e 2;;i ~
where,
(3
=
-q +
J q2 +4p3 2
For real values of p and q, if the discriminant is positive, then one root is real, and two roots are complex conjugates . If the discriminant is zero, th en there are three real roots, of which at least two are equal. If the discrimin ant is negat ive, t hen there are three unequal real roots. Appl y thi s theory for the characteristic equation of the matrix [1] and show th at the principal moment s of inerti a are real. 14. Kinematics of a moving car on the Earth. The location of a vehicle on the Earth is described by its longitude rp from a fixed merid ian , say, t he Greenwich meridian , and its latitude f) from t he equator, as shown in Figur e 11.20. We at tac h a coordinat e
500
11. Motion Dynamics
z
z y
, " , , - _ - . L ._ _-'-+~
Y
(0)
(b)
FIGURE 11.20. The location on the Earth is defined by longitude 'P and latitude B.
frame B at the center of the Earth with z-axis on the equator's plane and y-axis pointing the vehicle. There are also two coordinate frames E and G where E is attached to the Earth and G is the global coordinate frame. Show that the angular velocity of B and the velocity of the vehicle are
gw B gyP
in B + (WE +
Calculate the acceleration of the vehicle. 15. Global differentiating of angular momentum. Convert the moment of inertia B I and the angular velocity gw B to the global coordinate frame and then find the differential of angular momentum. It is an alternative method to show that
Cd (B B ) dt I CWB
Bt+gwB x BL Iw
+ wx (Iw) .
16. Equations of motion for a rotating arm . Find the equations of motion for the rotating link shown in Figure 11.21 based on the Lagrange method.
11. Motion Dynamics
501
FIGURE 11.21. A rotating link. 17. Lagrange method and nonlinear vibrating system. Use the Lagrange method and find the equation of motion for the link shown in Figure 11.22. The stiffness of the linear spring is k.
FIGURE 11.22. A compound pendulum attached with a linear spring at the tip point.
18. Forced vibration of a pendulum. Figure 11.23 illustrates a simple pendulum having a length l and a bob with mass m. Find the equation of motion if
502
11. Motion Dynamics
e---~----.. X
FIG URE 11.23. A pendulum with a vibratin g pivot .
(a) the pivot 0 has a dict at ed motion in X direction
X o = asin wt (b) t he pivot 0 has a dictat ed moti on in Y direction
Yo = bsin wt (c) t he pivot 0 has a uniform motion on a circle
r o = R cos wt
i + R sin wt J.
19. Equ ations of mot ion from Lagrangean. Consider a physical syst em with a Lagr angean as
I:- =
~m (ax + biJ )2 - ~k (ax + by)2 .
The coefficients m , k, a, and b are constant . 20. Lagrangean from equat ion of motion.
Find t he Lagr angean associa ted to the following equations of motions:
(a)
(b) 2 r.. - r e·
r
2
B+ 2r i: iJ
o o
11. Motion Dynamics
503
x FIGURE 11.24. A mass on a rotating ring.
21. A mass on a rotating ring. A particle of mass m is free to slide on a rotating vertical ring as shown in Figure 11.24. The ring is turning with a constant angular velocity w = rp. Determine the equation of motion of the particle. The local coordinate frame is set up with the x-axis pointing the particle, and the y-axis in the plane of the ring and parallel with the tangent to the ring at the position of the mass.
22.
*
Particle in electromagnetic field.
Show that equations of motion of a particle with mass m with a Lagrangian L ,.. 1 .2 ;f,. • J.- = -mr - e"" + er . A 2 are 3
(8<1> 8Ai) mqi = e - 8qi - at ..
where
", . (8A j 8Ai) + e ~ qj 8qi - 8qj
q~[~:] [~]
Then convert the equations of motion to a vectorial form
mr = eE (r , t)
+ ei
x B (r , t)
where E and B are electric and magnetic fields.
8A
E
-\7<1>--
B
\7 x A .
8t
12 Robot Dynamics 12.1
Rigid Link Recursive Acceleration
Figure 12.1 illustrates a link (i) of a manipulator and shows its velocity and acceleration vectorial characteristics. Based on velocity and acceleration kinematics of rigid links we can write a set of recursive equations to relate the kinematics information of each link in the coordinate frame at t ached to the pr evious link. We may then generalize the method of analysis to be suitable for a robot with any numb er of links.
.. d 1-. /
y
FIGURE 12.1. Illustration of vectorial kinematics information of a link (i).
Translational acceleration of the link (i) is denoted by "a, and is measured at the mass center Ci . Angular acceleration of the link (i) is denoted by Q i and is usually shown at the mass center Ci , although Qi is the same for all points of a rigid link. We calculate the translational acceleration of the link (i) at C, by referring to the acceleration at the distal end of the
°
°
506
12. Robot Dynamics
link. Hence, 0 ··
d,
+ OO i
0
X (
r, -
x
+ OWi X ( OWi
0
d i) °d i))
(12.1)
i d i ) ).
(12.2)
if joint i is R if joint i is P
(12.3)
(Ori -
which can also be written in the form i (L
+ OO i
X Cri -
x
+bWi X ( OWi
id i)
(ir i -
The angular acceleration of the link (i) is .. 0 '
OO i = {
OO i-l
+ Bi
ki -
1
+ OWi -l
. 0'
X
Bi ki -
1
OO i-l
that can also be writ ten in the t he local frame .L i-l ( i -l OO i-l
irr»
.Li-l + irr: i rr:
.L i-l
+ B·· i i - 1 k' i- I )
( i - 0l W i-l X B · i i - 1 k' i - I )
if joint i is R
i- I
(12.4)
if joint i is P.
0 0 i -l
Proof. Accordin g to rigid body accelerati on in Equat ion (10.59), t he acceleration of the point C, is
(12.5) The accelerat ion of Ci may be t ransformed to the body fram e B , by
'a,
°Ti- 1 "a, b Oi
x
(12.6)
(ir i -
We define the posit ion of
id i)
Oi - l
+ bWi
and
0i
X ( bWi
x
Cri -
id i ) )
+
i d i.
with respect to C, by
°d i_ 1 °di -
° ri
"r,
(12.7) (12.8)
to simplify the dynamic equation of robots. Using Equation (10.45) or (10.47) as a rule for adding relative angular accelerations, we find
(12.9)
12. Robot Dyn amics
however, the angular velocity i - Ol W i and angular acceleration
° °
i - Ol Q:i
507
are
i - l Wi
if joint i is R
(12.10)
i - I Q:i
if joint i is R
(12.11)
or
o o
if joint i is P
(12.12)
if joint i is P.
(12.13)
Therefore, OQ:i -l
- 0" + (Ji k i - 1 + OWi -l
.
X (Ji
0"
k i- 1
OQ:i -l
if joint i is R if joint i is P
(12.14)
and it can be transformed to any coordinate frame, including Bi , to find the Equation (12.4). 1 °Ti
OQ:i
+ Bi bki- 1 + bWi-l x Bi oki - 1 i ( i-l Ti - 1 °Q:i-l + (Ji.. i -I°k" i - 1 + i-°I W i -l
bQ:i -l
°
. i -I " ) X (Ji k i- 1
(12.15)
• Example 277 Recursive angular velocity equation for link (i) . Th e recursive global angular velocity equation for a link (i) is if jo int i is R if joint i is P.
(12.16)
W e may transform this equation to the local fram e B , 0T -1
i
OWi
i
OWi
°Ti
- 1 ( OWi -l
+ (Ji. 0 k" i - 1 )
OWi-l
+ Bi iki _ 1
i
( i-l
Ti -
1
°
W i-I
+ (Ji.
i-I"
ki -
1
)
.
(12.17)
Th erefore, we can use a recursiv e equation to find the locally expressed angular velocity of link (i) by having the angular velocity of its lower link (i - 1) . In this equation, every vector is expressed in its own coordinate fram e. i
. _
OW , -
iTi . { 'T
I
i- I
°w i -l + (J. °W i- l
( i- l
.
, -1
i
i- 1 k" ) i- I
if joint i is R if joint i is P.
(12.18)
508
12. Robot Dynamics
Example 278 Recursi ve translational velocity equation for link (i). Th e recurs ive global translational velocity equation fo r a link (i) is 0d' . _ z -
0' d i- 1
0 X i - l di
+ OWi o' d i- 1 + OWi
{
X
o
' 0' i- I d , + d i k i -
if jo int i is R if jo int i is P.
1
(12.19)
W e may transform this equation to the local f ram e B, 1 d' · °Ti 0,
0-1(0' Ti d i- 1
+ OWi
~di-l
+ ~Wi
i
( i- I 0 d i- 1
Ti -
1
X
0 X i- Id,
i~l di
+ di
+ d'i0 k' i - 1 )
iki_ 1
i + d;. i -I k' i - 1) + OWi
i X i - I d. .
(12.20)
Th erefore, we may use a recursive equation to find the locally expressed tran slational velocity of link (i) by having the translational velocity of its lower link (i - 1). In this equati on, ever y vector is expressed in its own coordinate fram e. i
., "d, =
Ti -
1
. i - I' ) ( i-l i 0 d i - 1 -; d, ki - 1
+ OWi {
't:i -
if joint i is R
X i - l di
1 I i - 0 d i- I
+ OWi i
X i - iI d i
(12.21)
if jo int i is P.
Th e angular velocity equati on is a consequence of the relative velocity equation (7.67) and the rigid link 's angular velocity based on DH parameters (8.2) . Th e translational velocity equati on also come s from rigid link velocity analysis (8.3) .
Example 279 Recursive translational acceleration equations for link (i) . Th e recursive translational acceleration equations for a link (i) are
if joint i is R 0"
di
=
0"
+ OW• i X i-0I d , + OWi X (OWi X i _~\ di ) + di °ki_ 1 + 20Wi X di
(12.22)
d i- 1
°k i_ 1
if jo int i is P.
where (12.23)
12. Robot Dynamics
509
W e may tran sform this equation to th e local fram e B,
od i
°Ti-
1
°d i
id i _ 1 + ~Wi
+ OWi X (~Wi X i~ l di ) + di i k i _ 1 + 2 OWi X di i k i _ 1 iT i _ 1 ( i- 1(ii_ l + di i- I k i - 1 ) + 2 ~Wi X di iTi _ 1 i-I k i - 1 0 + ~Wi
i~ l di
X
X
(~Wi
X
i~ l di )
+ ~Wi
X
i~ l di'
(12.24)
Th erefore, we m ay use a recurs ive equati on to find the locally expressed translational acceleration of link (i) by having the translational acceleration of its lower link (i - 1). In this equation, every vect or' is expresse d in its own coordinate fram e.
i
Ti -
i
Ti -
1
i-I .. di -
+ + 1
°
1
&Wi X (&Wi X i .
OWi X
°
( i- I ·· di -
i d i- I i
i~ l di ) if joint i is R
+ d;.. i- I kc-s A
1
(12.25)
)
i . i i- I '"' + 20W i X d i T i- 1 ki + &Wi X (&Wi X i~ l di ) i . i d + OWi X i- I i
I
if jo int i is P .
Equation (12.22) is th e result of differentiating (12.19), and Equation (12.25) is th e result of transforming (12.22) to the local fra m e B i .
12.2 Rigid Link Newton-Euler Dynamics Figure 12.2 illustrat es free body diagram of a link (i) . The force F i - 1 and moment M i - 1 are the result ant force and mom ent t ha t link (i -1) applies t o link (i) at joint i . Similarl y, F , and M , are the resul t ant force and moment that link (i) applies t o link (i + 1) at joint i + 1. We measure and show the force syste ms (F i - 1 , M i -d and (F, , M r) at the origin of the coordinate fram es B i - I and B, respe cti vely. The sum of t he extern al loads act ing on the link (i) are shown by I: F e i and I: M e i • The N ewton-Euler equations of motion for the link (i) in the global coordinate fram e are (12.26)
°Mi _ 1
-
°M i +
+ (Odi_ 1 -
L
°M e i
° ri ) x °F i_ 1
-
(Odi - °ri) x °F i = ° I, Oni . (12.27)
510
12. Robot Dynamics
o y
FIGURE 12.2. Force system on link (i).
Proof. The force syst em at the dist al end of a link (i) is mad e of a force F , and a moment M , measured at the origin of B s. The right subscript on F, and M , is a number indic ating t he number of coordinate frame B i, At joint i + 1 th ere is always an action force F i , that link (i) applies to link (i + 1), and a reaction force - F i, t hat the link (i + 1) applies to th e link (i). Therefore, on link (i) t here 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 for ce, and reaction force is called driven for ce. Similarl y, at joint i + 1 there is always an action moment M i , that link (i) applies to the link (i + 1), and a reaction moment - M i , t ha t link (i + 1) applies to the link (i) . Hence, on link (i) t here is always an action moment M i - 1 coming from link (i - 1), and a reaction moment -vM, coming from link (i + 1). The acti on moment is called th e driving moment , and th e reaction moment is called the driven mom ent. Therefore, t here is a driving force syst em (F i - 1 , M i- d at t he origin of the coordinate frame B i - 1 , and a driven force syste m (F, , M i ) at the origin of t he coordinate frame Bi. The driving force system (F i - 1 , M i -d gives motion to link (i) and t he driven force syste m (F , , M r) gives motion to link (i + 1). In addit ion to t he act ion and reaction force syst ems, there might be some ext ern al forces act ing on the link (i) t hat their resultant makes a force system (2: F ei , 2:M eJ at the mass center Ci . In roboti c application, weight is usually t he only extern al load on middl e links, and reactions from
12. Robot Dynamics
511
the environment are extra exte rn al force syst ems on t he base and endeffect or links . The force and moment that the base act uat or applies to the first link are F o and M o, and the force and moment that the end-effect or applies to the environment are F n and M n . If weight is t he only exte rn al load on link (i) and it is in - ° ko direction, t hen we have
°
mi g =
-mig
°k A
(12.28)
o
"r , x m ; 0g = - or i x
m igOkO
(12.29)
where g is t he grav itat ional acceleration vector. As shown in Figur e 12.2, we indicate the global position of t he mass center of t he link by °ri , and t he global position of t he origin of body fram es B, and B i - 1 by °d i and °d i_ 1 resp ecti vely. The link 's velociti es °Vi ' OWi and accelerations °ai , OQi are measured and shown at Gi . The phy sical properties of t he link (i) are specified by its mom ent of inerti a ° I, about t he link 's mass cent er Gi , and its mass m i . The Newton's equation of motion det ermines t hat t he sum of forces applied t o the link (i) is equal to the mass of t he link times its accelera t ion at c; (12.30) For t he Eul er equat ion , in addit ion to t he acti on and reaction mom ent s, we must add th e mom ents of the action and reaction forces about Gi . The mom ent of -F i and F i - 1 are equa l t o -mi x F , and n, x F i - 1 where m , is the posit ion vector of o, from C, and n , is the position vector of 0i-l from C i . Therefore, the link 's Euler equation of mot ion is
°M i _ 1
-
°M i
+ °lli
X
+L
°M e i °F i _ 1 - ami x °F i = ° I, OQi
(12.31)
however , n, and m , can be expressed by °d i _ 1 °d i
-
"r, "r,
-
(12.32) (12.33)
to derive Equat ion (12.27). Since t here is one t ranslationa l and one rotational equation of motion for each link of a rob ot , t here are 2n vectorial equ ations of motion for an t i link robot . However , t here are 2(n + 1) forces and mom ents involved . Therefore, one set of force syste ms (usually F n and M n ) must be sp ecified t o solve t he equa t ions and find th e joints' force and moment . • Example 280 On e-lin k manipulator. Figu re 12.;] depict s a link atta ched to th e ground via a jo in t at O . Th e f ree body diagram of th e link is m ade of an extern al for ce an d momen t at
512
12. Robot Dynamics
FIGURE 12.3. One link manipulator.
the en dpoint, gravity , and the driving force and moment at the joint. Th e N ewton-Eul er equations fo r the link are
F o +F e +mgK Mo + Me + n
X
F° + m
X
Fe
ma
(12 .34)
/0..
(12.3 5)
Example 281 A four-bar linkag e dynami cs. Figu re 12.4(a) illustrates a closed loop four-b ar linkag e along with the free body diagrams of th e links , shown in Figure 12.4(b). Th e position of th e mass centers are given, and therefore th e vectors ani and am i for each link are also kno wn. Th e Ne wton-Euler equations fo r the link (i) are
°F °M i_ I - °M i + ani
X
°
'
F , +mig J °F i_ I - ami x F ,
i- I -
m , "a, t, Oo. i
(12.36) (12 .37)
an d therefore, we have three sets of equations.
°F o - °F
+mIgJ
'
(12.38)
°Mo - °M I + °nI x °F o - "m, x F I
(12.39)
°F
I
°F z + mzgJ'
(12.40)
°M I - °M z + °nz x °F I - °mz x F z
(12.41)
°F z - °F 3 + m 3g J'
(12.42)
°M z - °M 3 + °n3 x °F z - °m3 x F 3
(12.43)
I -
W e assume that there is no f r-iction in joints and the m echani sm is planar. Th erefore, the for ce vectors are in the XY plan e, and the moments are
12. Robot Dynamics
513
y
d (a)
-F, ~ -
FIGURE 12.4. A four-bar linkage, and free body diagram of each link.
parallel to the Z-axis. In this condition, the equations of motion simplify to,
a
°M o +
a F a - F I + mIgJ anI x °F o - "m, x F I a
A
a F I- F 2 + m 2g J °n2 x °F I - °m2 x F 2 a
A
a F 2 - F 3 + m3g J °n3 x °F 2 - °m3 x F 3 A
mi °aI
(12.44)
It
(12.45)
Oal
m2 °a2
(12.46)
12 Oa2
(12.47)
m2 °a2
(12.48)
13 Oa3
(12.49)
where °M o is the driving torque of the mechanism. The number of equations reduces to 9 and the unknowns of the mechanism are
E"ox,E"Oy ,E"Ix ,E"Iy,E"2x,E"2y,E"3x,E"3y,AiO. The set of equations can be arranged in a matrix form [A] x= b
(12 .50)
514
12. Robot Dynamics
where
1
0
1 0 - n l y nI x
[A] =
0 0 0 0 0 0
0 0 0 0 0 0
- 1
0
0 ml y
-1 - m Ix
0 0 0
1
0
-1
0 -n2y 0 0 0
1 n2x 0 0 0
0 m2y
0 0 0 0
1
-m2x 0
-1
0 0 0 0 0 0 0
0 -n3y
1 n3x
0 m 3y
- m 3x
-1
0 0 0 0 0 0
- 1
0 0
1 0 0 0 0 0 0
(12.51) Fox Foy FI x Fly F 2x F 2y F 3x F3y Mo
(12.52)
mlal x mlal y - mIg hal m 2a2x m2a2y - m2g ha2 m3 a 3x m3 a3y - m 3g ha3
(12.53)
x=
b=
Th e matrix [A] describes the geom etry of the m echanism, the vector x is th e unknown forces, and the vector b indicates the dynamic terms. To solve th e dynamics of the four-bar m echanism, we must calculat e th e accelerations "a, and °ni and then find the required driving moment °M o and the joints ' forc es. The force
E, = F 3
-
Fo
(12.54)
is called the shaking f orce and shows th e reaction of th e m echani sm on th e ground.
Example 282 2R planar manipulator N ewton -Euler dynamics. A 2R planar manipulator and its free body diagram are shown in Figure 12.5. Th e torques of actuators are parallel to the Z -axis and are shown by
12. Robot Dynamics
515
»,« FIGURE 12.5. Free body diagram of a 2R palanar manipulator.
Qo and QI. The Newton-Euler equations of motion for the first link are
o
F o - F I + mlgJ Q o - Q I + °nl x F o - " m, x °F I A
(12.55) (12 .56)
and the equations of motion for the second link are o
m2 0a2 012 002.
A
F I + m 2g J Q I + °n2 x °F I
(12 .57) (12.58)
There are four equations for four unknowns F o, F I , Q o, and QI . These equations can be set in a matrix form
[A]x= b
(12 .59)
where
[A] =
1 0
0 1
nl y
- n Ix
0 0 0
0 0 0
0 0 1 0 0 0
- 1 0
0 - 1
-mly
mIx
1 0
0 1
n2y
- n 2x
0 0 - 1 0 0 1
(12 .60)
516
12. Robot Dynamics
Fox Foy
x=
Qo FIx
(12.61)
Fly
Q1 m1 a1x m1 a1y - mIg
°h a l
b=
(12.62)
m2 a2x m2a2y - m2 g I 2a 2
°
Example 283 Equations for joint actuators. In robot dynamics, we usually do not need to find the joint forces. Actuators' torque are much more important since we use them to control a robot. In Example 282 we found four equations
°F o - °F
0Q ° - 0Q 1 +
+ m 1gJ ° n 1 x OF ° - ° m 1 x of 1 F 1+ m 2g J 1
A
°
A
° Q1 + °n2 x °F 1
m1 0a1
(12.63)
°h
(12.64)
0 01
m2 0a2 1200 2'
°
(12.65) (12.66)
for the joints' force system of a 2R manipulator. However, we may elimi nate the joint forces Fa, F 1 , and reduce the number of equations to two for the two torques Qo and Q 1' Eliminating F 1 between (12.65) and (12.66) provides (12.67) °Q1 = °12002 - °n2 x ( m 2 °a2 - m2g
J)
and eliminating F
° and F
1
between (12.63) and (12.66) gives
°Q1 + aIi 001
°Qo
-
+ °m1 x
°n 1 x (m1 0a1 -
( m 2 °a2 - m2g
J)
°
mIg JA+ m2 a 2 - m2g JA) .
(12.68)
°
The forces F and F 1, if we are interest ed, are equal to
°F
°
1
°F o
=
m2 a 2 - m2g J m1 ° a 1 + m2 ° a2 - (m1
(12.69)
A
+ m2) 9 J . A
(12.70)
The position of C, an d the links ' angular velocity and acceleration are
°n1 -on1 + "m, - °n2
(12.71)
- on 1 + °m1
(12.73)
- on2 + °m2
(12.74)
- on1 + "m, - °n2 + °m2
(12.75)
(12.72)
12. Robot Dyn amics OW l
B1 k
00:1
OW l
OW2
(B 1 + ( 2 ) k =
0 0:2
(12.76)
= 01 k
(12.77) (12.78)
= (0 1 + ( 2 ) k
OW2
517
(12.79)
and the local position vectors of C; are a R 1 1n 1
an I
R Z ,Ih
1n 1
R
2
- i. 1,
Z, O!
21
[ -tl, cosB, sin01
(12.80)
- 'i l l
J
0
a R 2 2n 2
°n2
R z ,o! R Z ,02 2n 2 R
Z ,O!
[
R
Z ,02
22
+C~S (B'+B2} 1 - 2 12 Sill
am I
2 2,
2- 1
(01 + ( 2 ) 0
(12.81)
° R 1 1m l R z ,O, l m 1 R
11 1, Z ,O' "2 21
[ tl, cosB, sin 0 'i ll
(12.82)
1
0
J
a R 2 2m 2
°m 2
R
=
12 2 , Z .O! R Z ,02"2 22
[ tl2sin (B, ++ (B2) J COS
"2 12
~1
2)
.
(12.83)
518
12. Robot Dynamic s
Th e translational acceleration of C1 is
(12.84)
because 0··
0
d1 = 2 a j ,
(12.85)
The translational acceleration of C2 is
(12.86)
- hOI sinOl
-
~l2 (01 + O2 ) sin ( 01 + ( 2 )
-h e~ cos 0 1 - ~l2 (01 + O2 ) 2 cos (0 1 + ( 2 ) ll(h
COSOI
+ ~l2 (0 1 + O2 )
cos
f
-h e~ sinOl + ~l2 (0 1 + O2
(0 1
sin
(12.87)
+ (2 )
(01
+ (2 )
(12.88)
because
(12.89)
-l 10 1 sin
e1 -
l20:2
sin
(e 1 + ( 2 )
-hei cose 1 - l2W~ cos (e1 + ( 2 )
(12.90)
+ l 20:2 cos (8 ! +( 2 ) -hoi sinO! -l2W~ sin (O! + ( 2 )
(12.91)
h81 COSOl
12. Robot Dynamics
519
and the moment of inertia matrices in th e global coordinate fram e are
°I,
=
R ZJh
OR!
1
h R~,el
[~ ~
i]
oRr
t, sin 2 8 1 [ 012
-i. cosg1 sin8 1
0 R 2 212 0
oR 2
- h cos 8 1 sin 8 1 h cos2 8 1
(12.92)
o
Rf
[ o~ ~0 1~] °Rf 2
2
sin 8 12 cos 8 12 sin 812
12 - 12 [
- 12
cos 8 12 sin 8 12
h cos2 8 12
o
0] O . (12.93)
h
0
Substituting th ese results in Equations (12.67) and (12.68) , and solving for Qo and Q1, provid es th e dynamic equations for the 2R manipulator in an appropriate form . 0Q !
°Q1 z
12 (B 1 + B2) 1
~
[
oJJ
+ ~l~m2 1
·2
+"2l1hm281 sin 82
-
(B 1 + B2)
(12.94)
+ ~hl2m2B1 cos 82
"2ghm2 cos (8 1
+ 82 )
(12.95) (12.96)
°Qo z
(12.97)
(12.98)
520
12. Robot Dynamics m 2 0 a2 x
(12.99)
o m2 a2 y
- m 29
(12.100)
(12.101)
oP Ox
(12.102)
12.3
Recursive Newton-Euler Dynamics
An advantage of the Newton-Euler equat ions of motion in robotic applicat ion is t hat we can calculate th e joint forces one link at a tim e. Therefore, start ing from t he end-effector link, we can analyze th e links one by one and end up at t he base link. For such an analysis, we need to reform t he Newton-E uler equations of mot ion to work in t he int erested link's frame . T he backward Newton-Euler equations of motion for th e link (i) in t he the local coordinate frame B, are (12.104)
iM i -
L iM
+ (id i -
ei
(id i _ 1
-
ir i) x iF i
+ 't,
id i _ 1
-
i r i ) x iF i_ 1
-
bO'.i ir i
id i - i r i.
+ bWi X 't, bWi
(12.105) (12.106) (12.107)
12. Robot Dyn amics
521
When the driving force system (iF i _ 1, iMi _ t) is found in fra me Bs, we can transform t hem to the frame B i - 1 and apply th e Newton-Euler equat ion for link (i - 1). i- 1F i _ 1
i-1 T
i- 1M i _ 1
i -1 T
i
iF i_ 1
(12.108)
i
iM i _ 1
(12.109)
The negative of the converted force system acts as the driven force system (- i- IF i-1, - i- IM i-t) for t he link (i - 1). Proof. Th e Euler equation for a rigid link in body coordinate frame is Gd BL
dt B t + g WB x BL i B . I, iQi + GWB x ' I, iWi
(12.110) (12.111)
where L is th e angular momentum of th e link BL --
BI Bw G B·
(12.112)
We may solve th e Newton-Euler equat ions of motion (12.26) and (12.27) for t he act ion force system
L °F +mioai °M i - L °M (Od i _ 1 °F i -
(12.113)
ei
ei -
+ (Od i - °ri) x °F i +
°ri) x °F i _ 1
:~ °L i
(12.114)
and then, transform the equat ions to the coordinate frame B, att ached to the link's (i) to make t he recursive form of th e Newton-Euler equat ions of motion. 0T i - 1 OF.,-1
iF i °Ti-10M,·iM i -
L
L
iM e i
-
(id i_ 1 - i r i ) x iF i _ 1
L iM
ei
-
:~ iLi
(id i-1 - i r i) x iF i_ 1
+ (id i - ir i) x iF i +
•
(12.115)
I
+ (idi - i r i ) x iF i + iM i -
iF e i + m, bai
'i, (Pi +
bWi X i Ii OWi .
(12.116)
522
12. Robot Dynamics
FIGURE 12.6. A 2R planar manipulator carrying a load at th e endpoint .
Example 284 R ecursive dynamics of a 2R planar manipulator. Consider the 2R planar manipulator shown in Figure 12.6. Th e manipulator is carr ying a fo rce system at the endpoint. We use this manipulator to show how we can, step by st ep, develop the dynamic equations for a robot. Th e backward recursive Newton-Euler equations of motion for the first link are
(12.117)
(12.118)
and the backward recursiv e equations of motion for the second link are
(12.119)
12. Robot Dynamics
523
(12.120)
Th e manipulator consists of two RIIR(O) links , therefore their transformation matrices i-lTi are of class (5.32). Substituting di = 0 and a; = li, produces the following transformation matrices
[ 00,0, sinOl 0 0
»r,
[ CO,02
sin O2 0 0
lT2
- sin 0 1 0 eos Ol 0 1 0 0 0
I, CO, 0, ]
- sin02 eos02 0 0
l,
0 0 1 0
II sin 0 1 0 1
(12.121 )
COO 0,
]
l2 sin O2
o
.
(12.122)
1
The homogeneous moments of in ertia matrices are
'I
1
~ m,ll [ : 12
0 0
0 1 0 0
~]
0 0 1 0
2I _ m2l~ 212
[~
0 1 0 0
0 0 1 0
n
(12.123)
The homogeneous moment of inertia matrix is obtained by appending a zero row and column to the I matrix. Th e position vectors involved are
l nl =
r
l ml =
-r]
[If]
2n 2 =
2m 2 =
-r]
(12.124)
[T]
(12.125)
r
(12 .126)
The angular velocities and accelerations are
(12 .127)
524
12. Robot Dynami cs
(12 .128)
Th e tran slational acceleration of C1 is
(12.129)
because
Th e tran slational acceleration of C2 is 2 Oa2
2 ) 002 x (- 2 lli2
2 + OW2
2 )) X ( OW2 X (- 2 lli2
f
2d •. +O 2
-!l2 (B 1 + B2
!l2 (0 1 + O2)
(12.130)
o o
because (12 .131)
Th e gravitational acceleration vector in the links ' fram e is
(12.132)
(12.133)
The extern al load is usually given in the global coordinate fram e. We must transform them to the interested link 's fram e to apply the recursive equa-
12. Robot Dynamics
525
tions of motion. Th erefore, the extern al for ce system expressed in B 2 is
(12.134)
(12.135)
No w, we start from th e jinallink and calculate its action for ce syst em . Th e backward Newton equation for link (2) is
(12.136)
1
-212m 2
(.
.)2-
01 + O2
Fex cos (01 + ( 2)
- (Fey - gm2) sin (01 + ( 2)
(12.137)
~12m2 (B1 + B2) + Fex sin (01 + ( 2) - (Fey + gm2) cos (01 + ( 2)
(12.138)
and th e backward Euler equation fo r link (2) is
(12.139)
where
(12.140)
526
12. Robot Dynamics
Finally the action force on link (1) is
(12. 141)
where
and the action mom ent on link (1) is
where
1Moz
1
2
( ..
.. )
- Me+ "3 12m 2 01+02
1
2
..
+ "311m101
- (Feyl2 + ~912m2) cos (01 + O2) 1
.
- "2 h m 19 cos 01 + Fe xl 2 sin (01
+ O2 ) ,
(12.145)
12. Robot Dynamics
527
Example 285 Actuator's force and torque. Applying a backward recursive force analysis ends up with a set of known force systems at joints. Each joint is driven by a motor known as an actuator that applies a force in a P joint, or a torque in an R joint. When the joint i is prismatic, th e for ce of the driving actuator is along the Zi_ 1-axis (12. 146)
showing that the ki - 1 component of the jo int force F, is supported by th e actuator. The 2i -1 and ] i -1 components of F , must be supported by th e bearings of the joint. Similarly, when th e joint i is revolute, the torque of the driving actuator is along the Zi-1 -axis (12.147)
showing that the ki - 1 component of the joint torque M, is supported by the actuator. Th e 2i -1 and ]i -1 components of M, must be supported by th e bearings of the joint.
*
Example 286 Forward Newton-Euler equations of motion. Th e N ewton-Euler equations of motion (12.104) and (12.105) can be written in a forward recursive form in coordinat e fram e B , attached to the link (i) . (12.148)
iM i
iM i_ 1
+
- Cdi -
L iM ir i)
x
ei
+ ( i d i_ 1 -
iF i -
-t,
id
-
id
i_ 1 i
-
bni -
ir i
ir i
ir i)
x
bWi
iF i_ 1 X
't,
bWi' (12.149) (12.15 0) (12.151)
Using the forward Newton-Eu ler equations of motion (12.148) and ( 12.14 g), we can calculate the reaction force system (i F i, i M i ) by having the action forc e system (i F i_ 1, i M i _ 1) . Wh en the reacti on force system (i F i , i M i) is found in frame Bi, we can transform them to th e fram e Bi.;1
iT - 1
iF . i+ 1 2 iT-i +l1 i M i ·
(12.152) (12.153)
Th e negative of the converted force system acts as the action for ce system for th e link (i + 1) and we can apply the Newton-Eul er equation to the link (i + 1). Th e forward Newton-Euler equations of motion allows us to start from a known action force system f F a, 1 M a), that the base link applies to th e 1 ( - i+ 1 F i , - i+ M i )
528
12. Robot Dynamics
link (1), and calculate the action force of the next link. Th erefore, analyzing the links of a robot, one by one, we end up with the for ce system that the end-effec tor applies to the environme nt. Using the fo rward or backward recursive Newton-Euler equations of motion depends on the m easurem ent and sensory system of the robot.
12.4 Robot Lagrange Dynamics The Lagrange equat ion of motion provides a systematic approach to obt ain the dynamics equations for robots. T he Lagr angean is defined as th e difference between t he kinet ic and potential energies
L =K-V.
(12.154)
The Lagrange equation of motion for a robotic system can be found by applying the Lagrange equation i
= 1,2, ' "
n
(12.155)
where qi is the coord inat es by which t he energies are expressed, and Q i is t he corresponding generalized nonpotenti al force t hat drives qi . The equations of motion for an n link serial manipulator can be set in a matrix form (12.156) D(q) q + H(q, q) + G(q) = Q or in a summation form n
L j=1
n
D ij(q) ijj
+L
n
L
H ikm([k(jm
+ c, =
o;
(12 .157)
k=l m=1
D( q) is an n x n inerti al-type symmet ric matrix (12.158)
H ikm
is the velocity coupling vector (12.159)
and G, is the gravitat ional vector n
" T J Dj ei } ' G i = 'L...mjg j=1
(12.160)
12. Robot Dynamics
529
Proof. Kin etic energy of link (i) may be written as
°
10 T
1
T i
tc. ="2 vi mi Vi + "20w i
(12.161)
I i OWi
where m; is the mass of the link, i I, is the moment of inerti a matrix of the link in the link's frame B i , °Vi is the global velocity of t he CM of the link, and OWi is the global angular velocity of t he link. The translational and angular velocity vect ors can be expressed based on the joint coordinat e velocities, utilizing the Jacobian of the link J i
X· i
=
OVi] [ 0W i
=
[J
].q J Di Ri
=
J' i
(12.162)
q.
The link 's J acobian J , is a 6 x n matrix that t ransforms th e inst ant aneous joint coordinate velocities into the inst antaneous link 's C M translational and angular velocities. The jth column of J i is made of cg~ and c~l, where for j :::; i C
U)
Di
and
, = ~j-I = kj - I c~l
°
for a R joint for a P joint
X j -I r ,
= kj -
(12.163)
for a R joint for a P joint
I
= 0
(12.164)
and j _OI ri is the position of CM of the link (i) in the coordinate frame B j - I expressed in the base frame. The columns of J i are zero for j > i. The kinetic energy of the whole robot is th en
i= 1
n
=
-12: 2 i= 1
~
°
( Ov T· m · v ·
t ((J
"'
Di Cti) T
1 OW T· OI OW ' ) + -2 '"
m ;
~qr (t, (J];, "'
(J Di Cti) +
J DO + ~ J~,
~ (J R i Ctif °t, o/;
Jm )
)
q,.
(J Ri Cti))
(12 165)
where I i is the inerti a matrix of the link (i) about its C M and expressed in the base fra me. 01t· -- OR · 't.1, 0RT (12.166) i 1,
The kinetic energy may be written in a more convenient form as (12.167)
530
12. Robot Dynamics
where D is an
ti
x n matrix called the manipulator inertia matrix (12 .168)
The potenti al energy of the link (i) is du e to gravit y (12.169)
and therefore, the total potenti al energy of t he manipulator is n
V
=
n
LVi
LmiOgTOri.
= -
i= 1
(12.170)
i= 1
where "s is the gravitational accelerat ion vector expressed in the base frame . The Lagrangean of the manipulator is
L
K - V
~qr Dqi+ tmi OgTOri i= 1
"21 ~~ L.J L.J
o., ..
qiqj
0 TO +~ L.J m , g r io
i = 1 j= 1
(12.171)
i= 1
Based on the Lagrangean E we can find fJ£ fJq i
(12.172)
(12.173)
and
d dt
oc fJq i
n
LD
n
i j qj
j =1
j=1
L
dD . . 2J dt qj
j=1
~D " L.J
+
ij qj
~ fJD +~ L.J L.J 8
ij
j = 1 k=1
. . qk qj '
(12.174)
qk
The generalized force of the Lagr ange equat ions are (12.175)
12. Robot Dynamics
531
f
where Qi is the it h act uator force at joint i, and F e = [-FIn -M In is the external force system app lied on the end-effector. Finally, the Lagrange equations of motion for an n-link man ipulator are n
L Dij(q) iiJ + Hikmqkqrn + c. = o.
(12 .176)
j=l
where
t t (a: j=l k=l
ij
~ a~Jk )
_
qk
(12.1 77)
q,
n
L m jgT J~~
(12.178)
j=l
The equations of motion for a manipulator may be shown in a more concise form to simplify matrix calculations.
D (q) q + H (q, 4) + G (q) = Q
(12.1 79)
The term G(q ) is called the gravitational force vector and the term H (q , 4) is called the velocity coupling vector . The velocity coupling vector may sometimes be written in the form
H (q , 4) = C (q , 4)4·
(12.18 0)
• Example 287 2R planar manipulator' Lagrangian dynamics. The 2R planar manipulator is shown in Figure 5.9 and its homogeneous transformation matrices are given in Equations (5.29) and (5.30).
°R1 =
1
R2 =
[
[
C~O,
sine 1
- sin(h cose 1
0
0
C~O,
- sine 2 cos e 2
si~e2
0
n n
(12.181)
(12.182)
Assuming that the links are made of homogeneous material in a bar shape, the C!vI position vectors are at i
r, =
[
- ld 2 ] 0
o
i
=
1,2
(12.183)
532
12. Robot Dynamics
and the in ertia matrices are (12 .184)
Therefore,
°h
oR 2 212 1 12 m2l~
0
Rf . 20 12
sm
[
- cos O~ sin 012
- cos 012 sin 012
cos2012
o
~ ] . (12.186)
The gravity is assum ed to be in - ]0 direction (12 .187)
and the link Jacobian matrices are
r: ~ ]
-1.h sin 01 2 J Dl
=
1
[
Jm J D2 =
[
21]
~ [~
(12.188)
n
(12. 189)
-h sin 01 - ~l2 sin 012 -~h sin 012 h COS01 + ~l2 COS O]2 ~l2 COS 012
o
]
(12 .190)
0
Jm~[~~]
(12191)
We can calculate the manipulator inertia matrix by substituting and J Ri in Equation (12.168) D
=
t, (ibi T
J Dl [
m1
m;
J Di
J Dl +
i m 1li + m2
+ ~ iki 0 t, J Ri )
T "2I J TO Rl t, JRl +J D2
(li + hh c02 + ilD (~hl2 C02 + itD
m2
0Ii ,
J Di ,
(12.192)
m2 m2
J D2 +
"2I J TO R 2 12 J R2
(~hhc02 + il~) ] im2l~
12. Robot Dynamics
533
The velocity coupling vector H has two elements that are
(12.193)
(12.194) The elements of the gravitational force vector G are GI
1 1 "2mlgh cos 0 1 + m2gh cos 0 1 + "2m2gl2 cos 0 12 (12.195)
G2 =
1 "2m2gl2 COSOI2.
(12.196)
Now we can assemble the equations of motion for the 2R planar manipulator utilizing the matrix and vectors we found above. Assuming no external force on the end-effector, the equations of motion are
QI
(~mlli +
m2
(1
(li
+m2 l2 "2hC02
+ u-». + ~l~) ) 81
1)..
+ 312
+ (~ml + m 2) gll
(. 1.) .
O2 - m2 l1l2 0 1 + "202
cos 01 +
~m2gl2 cos 012
O2 sin O2 (12 .197)
(12.198)
Example 288
*
Christoffel operator. The symbol is called the Christoffel symbol or Christoffel operator with the following definition:
q,k
(12.199)
534
12. Robot Dynamics
Th e velocity coupling vector H ij k is a Christoffel operator.
(12.200)
Example 289 No gravity and no external force. Assume there is no gravity and there is no ext ernal for ce applied on the end-effe ct or of a robot. In these conditions, the Lagrangean of th e manipulator sim plifies to 1
L =
n
n
2 LLDijq/lj
(12.201)
i=l j=l and th e equations of motion reduce to (12.202)
12.5
*
Lagrange Equations and Link
Transformation Matrices The matrix form of the equations of motion for a robotic manipulator, based on t he Lagrange equat ions, is
D(q) q + H(q, q) + G(q) = Q
(12.203)
which can be written in a summat ion form as n
L Dij(q) qj j=l
+ Hikmqkqm + c.
=
o..
(12 .204)
The matrix D( q) is an n x n inertial-type symmetric matri x D ' J -_
~ L
r=max
i,j
tr
({) OTr
Bo,
q,
-t
r
()OTr () .
T)
(12.205)
qJ
and H ikm is the velocity coupling term (12.206)
12. Robot Dynamics
535
and G, is the gravitational vector
a0'7'
n
r
Gi = - ""' LJ mrgT
l. ~
r=i
T
(12.207)
rT•
q,
Proof. Position vector of a point P of the link (i) at i r p in the body coordinate B i , can be t ransformed to the base frame by
°r p = °T
(12.208)
i i r p.
Therefore, its velocity and squar e of velocity in the base frame are
~ ovt: .
0.
i
r p = LJ - - qj r p
(12.209)
j=1 aqj
and 0 ·2 rp
(12.210) The kinetic energy of point P having a small mass dm is then equal to
dK p
1 tr = 2 1
2t r
i
(
i
e»
Ti LL T j=1 k=1 q)
(iLLi
j =1 k=1
i
aOT
i T e» T i T . . ) r p r P 8 qjqk qk .
.
dm
aOT T
)
~ ('rpdmtr~) ~ qjqk (12.211) q)
qk
an d t he kinetic energy of t he link (i) is
tc. =
Li
dK p
~ tr
(tt
Ti ( ( . irp aaO j=1 k=1 q) } B,
ir~ dm ) aaO~i T qj qk ) qk
. (12.212)
T he int egral in Equation (12.212) is t he pseudo inertia matrix (11.143) for t he link (i) (12.213)
536
12. Robot Dynamics
Hence , the kinetic energy of link (i) becomes
(12.214)
The kinetic energy of a robot having n links is a summation of the kinetic energies of each link
(12.215)
We may also add the kinetic energy due to the actuating motors that are installed at the joints of the robot . However, we may assume that the motors are concentrated masses at joints and add the mass of the motor at joint i to the mass of the link (i - 1) and adjust the inertial parameters of the link. The motor at joint i will drive the link (i). For the potential energy we assume the gravity is the only source of potential energy. Therefore, the potential energy of the link (i) with respect to the base coordinate frame is
Vi
-mi "s "r, -mi 0gT
"r: i r i
(12.216)
where 0g = [gx gy gz 0] T is the gravitational acceleration usually in the direction -zo and or; is the position vector of th e C'M of link (i) in the base frame . The potential energy of the whole robot is then equal to (12.217) The Lagrangean of a robot is found by substituting (12.215) and (12.217) in the Lagrange equat ion (12.154) .
n
+L
mi 0gT
»i; i r i .
(12.218)
i =l
The dynamic equat ions of motion of a robot can now be found by applying the Lagrange equ ations (12.155) to Equation (12.218). We develop the equations of motion term by t erm. Differentiating the [ with respect
12. Robot Dynamics
537
to qr is 1 ~ ~ ( OOTi -t o OTi - ~~ tr - - i - 2 i=l k=l oqr oqk
+-1 ~~ ~ ~ tr 2 i=l j=l
(OOTi 't.i -OOTi oqj Bq;
~ ~ ( OOTi i1~~ t r - i i=r
j=l
T) qk.
oqj
ovt: oqr
-
T )
. qj
T) qj.
(12.219)
because for r
and tr
OOTi i 1- ovt: -- i - ( oqj Oqk
>i
T) -_tr ( -OO-Ti i/- oOTi T) i --
Oqk
oqj
(12.220)
.
(12.221)
Time derivative of of: joqr is d o f: dt oqr
~ ~ ( OOTi -t. oOT- i T) qj. ~~ t r - - i j=l
i= r
oqj
Bq;
02 Or . _ 0 Or T) +LLLtr d'/i~ qAk j=l k=l qJ qk qr n
i
i
(
i
i
(
i =r
n
+ LLLtr i= r
j=l k=l
02 Or . _ 0 OTT ) d ' /iT qAk . (12.222) qr qk %
The last term of t he Lagrange equation is
(12.223)
which can be simplified to
(12.224)
538
12. Robot Dynamics
Interestingly, the third term in Equation (12.222) is equal to the first term in (12.224). So, substituting these equations in the Lagrange equation can be simplified to
d(0£) -Bq, oc -
-
dt
-
Oqi
(12.225) Finally, the equations of motion for an n link robot are
(12.226) The equations of motion can be writ ten in a more concise form n
Qi
=
L Diijj + H ijkqj qk + c,
(12.227)
j =l
where (12.228)
(12.229)
(12.230)
• Example 290 2R manipulator mounted on ceiling. Figure 12.7 depicts an ideal 2R planar manipulator mounted on a ceiling. Ceiling mounting is an applied method in some robotic operated assembly lines.
12. Robot Dynamics
539
'------'1-----------' X
FIG URE 12.7. 2R manipulat or mount ed on a ceiling.
Th e Lagrangean of th e manipulator is
L
K- V 1 2·2 2" m l l l (} l
f
+~m2 (liO~ + l~ (01 + O2
+ 2h l20l
(01 + ( 2) COS (}2 )
+m l gh cos (} l + m2g (il cos (}l + l2 cos ((}l + (}2))
(12.231)
whic h leads to th e f ollowing equations of motion:
Ql
=
( (m l+m2)
2
2
) ..
ll +m2l2 + 2m2 h l2coS (}2 (}l
+ m 2l2 (l2 + h COS (}2) B2 .
.
·2
- 2m 2llh sin (}2(}1 (}2 - m 2hl2 sin (}2(}2
+ (ml + m2) gh sin (}l + m2gl2 sin ((}l + (}2) ..
(12.232)
2··
m2h (l2 + li cos (}2) (} l + m 2l2(}2 - 2m2h h sin(}20l
(0 1 + ( 2)
-m2gl2 sin ((}1 +(}2) . (12.233)
T he equations of motion can be rearran ged to
Ql
=
-
-
·2
·2
D U(} l + D 12(} 2 + H U1(}1 + H 122(}2
+ D ll2 0l O2 + D l2l 02 01 + G 1
(12.234)
540
12. Robot Dynamics
Yo
•
__7 - - '' --
-
-
-
--.. X
o
FIGURE 12.8. A 2R planar manipulator with massive links. -
Q2
-
·2
·2
= D 12 (h + D 22 (}2 + H 211 (}1 + H 222 (}2 +D 212 ih ih + D 22182ih + G2
(12.235)
where, (12.236)
D 12
= m2 l2 (h + h COS(}2)
D 21 = D 12 =
m2l2 (l2
+ h COS(}2)
(12.237) (12.238) (12.239)
H ll 1 = 0
(12.240)
(12.241 ) (12.242)
H 222 = 0
(12.243) (12.244) (12.245) (12.246) (12.247)
Example 291 2R manipulator with massive links .
12. Robot Dynamics
541
A 2R planar manipulator with massive links is shown in Figure 12.8. We assume the eM of each link is in the middle of the link and the motors at each joint is massless. The links' transformation mat rices are
or1 -_
[
eas 81 - sin 81 sin81 eas8 1 0 o
o
(12.248)
o
- sin 82 eas8 2
(12.249)
o o
(12.250) - 8
(81 + 82 )
c (81 +8 2 )
=
o o
o 11C81 + lac (81 + 82 ) o u». + 128 (81 + 82 ) 1
0
o
1
]
.
Employing the velocity coefficient matrix AR for revolute joint s, we can write
aOT1 = a8 1
AR °T1
l~ l-
=
-1 0 0 0 sinOI
eas8 1 0 0
o o o o
0][
0 0 0
(12.251) COSOI
sin8 1 0 0
- eas 81 0 - sin 81 0 0 0 0 0
- sin 81 eas8 1 0 0
0 0 1 0
II COS 01 h sin 81
]
0 1
-II sin 01 ]
i, eas8 1 0 0
and similarly,
aOT2 a8 1
=
AR °T2
(12.252)
12. Robot Dynamics
542
{)OT2 {)(h
°T1!:i. R IT2
(12.253)
[-8(0,+ 0,) c (8 + 8 2)
1
0 0
-c(8 1 +8 2 ) 0 -1 2 8 (OJ +0,) ] - s (81 + 82 ) 0 l2c (81 + 82 ) 0 0
0 0
o
.
0
Assuming all the product of inerti as are zero, we find
(12.254)
(12.255)
Utilizing inerti a and derivative of transformation matrices we can calculate the inertial-type symme tric ma trix D(q)
Dll
(12.256)
(12.257)
12. Robot Dynamics
543
~ ~]
- cos 012 sin 0 12
COS2012
o o
(12.258)
2
2
LL
H1
H 1kmqkqm k=lm=l H 1l1 q1q1 + H 1l2q1q2 + H l2l q2q1 + H 122 q2q2 (12.259) 2
2
LL
H2 =
H 2kmqkqm k=l m=l H 21l q1q1 + H 212q1q2 + H221q2q1 + H 222q2q2 (12.260)
where (12.261)
(12.262)
o sin01 o -i. h COSOl o 0 o 0
]
[-It ] 0
0 1
1 "2m1gh cos 01 1 +"2m2gh cos (0 1 +( 2) + m2gh cos 0 1
(12.263)
544 G2
12. Robot Dynamics
= =
T 8 0T2 2 -m2g - - r2 8q2
-m2
[ -g 0 0
o =
r[
12
CB 12 -,8 0
0
-cfh2 -SB 12 0 0
0 0 0 0
s8'2 -12 ] l2CB12 0 0
[T]
1 "2m2gl2 cos (B 1 + B2) .
(12.264)
Finally the equations of motion for the 2R planar manipulator are =
}12.265)
12.6 Robot Statics At the beginning and at the end of a rest-to-rest mission, a robot must keep the specified configurations. To hold the position and orientation, the actuators must apply some required forces to balance the external loads applied to the robot. Calculating the required actuators' force to hold a robot in a specific configuration is called robot statics. In a static condition, the globally expressed Newton-Euler equations for the link (i) can be written in a recursive form
°F i °Mi
-
L °F L °M
(12.266)
ei
-
ei
+ i_Pdi
X
(12.267)
°Fi
where (12.268) Therefore, we are able to calculate the action force system (F i - 1 , Mi-d when the reaction force system (-F i , -Mi ) is given. The position vectors and force systems on link (i) are shown in Figure 12.9. Proof. In a static condition, the Newton-Euler equations of motion (12.26) and (12.27) for the link (i) reduce to force and moment balance equations =
0 (12.269) O. (12.270)
12. Robot Dynamics
545
y
FIGURE 12.9. Illustration of position vectors and force system on link (i).
These equations can be rearranged into a backward recursive form
I: °F °M i - I: °M °F i
-
(12.271)
ei
ei -
°ni x °F i _ 1 + "m, x °F i. (12.272)
However, we may transform the Euler equation from C, to Oi-1 and find the Equation (12.267) . Practically, we measure the position of mass center r, and the relative position of B, and B i - 1 in the coordinate frame B, attached to the link (i). Hence, we must transform i r i and i-Idi to the base frame.
°ri i_Pdi
=
(12.273)
=
(12.274)
The external load is usually the gravitational force mig and hence, mi 0g
(12.275)
°r, x mi °g .
(12.276)
Using the DH parameters, we may express the relative position vector i-Idi
546
12. Robot Dynamics
by . td i-I i -
[
ai di sinai d, cos ai 1
]
.
(12.277)
The backward recursive equations (12.266) and (12.267) allow us to start with a known force system (F n , M n) at B n, applied from the end-effector to the environment, and calculate the force system at B n - I .
OFn _ 'L...J " of en
°M n
-
L
(12.278)
OMen
+ n_~dn
X
°F n
(12.279)
Following the same procedure and calculating force system at proximal end by having the force system at distal end of each link, ends up to the force system at the base. In this procedure, the force system applied by the end-effector to the environment is assumed to be known. It is also possible to rearrange the static Equations (12.269) and (12.270) into a forward recursive form °F i _ 1 +
oM i-I
L
(12.280)
°F ei
" oM e, + °ni + 'L...J
X
°mi
ofi-I -
X
of i· (12.281)
Transforming the Euler equation from C, to O, simplifies the forward recursive equations into the more practical equations
L of + L °Mei -
°F i _ 1 + °M i_ 1
(12.282)
e,
i_Pd i
X
°F i_ l .
(12.283)
Using the forward recursive Equations (12.282) and (12.283) we can start with a known force system (F ° , M o) at B o, applied from the base to the link (1), and calculate the force system at B I .
L °F °Mo + L °M
°F o +
(12.284)
e1
e1 -
°d l
X
°F o
(12.285)
Following this procedure and calculating force system at the distal end by having the force system at the proximal end of each link, ends up at the force system applied to the environment by the end-effector. In this procedure, the force system applied by the base actuators to the first link is assumed to be known. • Example 292 Statics of a 4R planar manipulator.
12. Robot Dynamics
547
FIGURE 12.10. A 4R planar manipulator.
Figure 12.10 illustrates a 4R planar manipulator with the DH coordinate frames set up for each link . Assume the end-effector force system applied to the environment is measured as
(12.286)
In addition, we assume that the links are uniform such that their C'M are located at the midpoint of each link, and the gravitational acceleration is (12.287)
g = -gjo ·
Th e manipulator consist s of four RIIR(O) links, therefore their transformation matrices i-1Ti are of class (5.32) that because d; = 0 and ai = li, simplifies to -sinB i
0
cosB i
0 1
o o
0
i, cosB i l, sin Bi ~
]
.
(12.288)
548
12. Robot Dynami cs
Th e CM position vectors
iri
and the relative position vectors i_ pd i are
(12.289)
(12.290)
and therefore, (12.291) (12.292)
where (12.293)
Th e static force at joints 3, 2, and 1 are
°F 3
°F 4
-
L
°F e4
°F 4 + m4g0jo
[
[
~:
] +m'9
F,~m49
]
[n (12.294)
(12.295)
12. Robot Dynamics
549
(12.296)
The static moment at joints 3, 2, and 1 are
°M4 - LOMe; + gd4 x °F4
°M3
=
°M4 + °r4 x m4g0jo + gd 4 x °F4 °M4 + °r4 x m4g0jo + gd 4 x °F4 °M4 + m4g (OT4 4r4 x °jo) + (OT4 ~d4)
[
~
~3Z
]
X
°F4 (12.298)
(12.300)
550
12. Robot Dynamics
M 2z = M,
+ l4Fy cos 01234 -
l4Fx sin 01234 + ~gl4m4 COS 01234
1 . +"2gl3m3 cos 0123 - l3Fx sm 0123
+b (Fy + gm4) cos 0 123
=
M 1z =
(12.301)
[~.]
(12.302)
M z + l4Fy cos 01234 - l4Fx sin 01234 + ~gl4m4 COS 01234
+~gl3m3 COS 0123 -
l3Fx sin 0123 + l3 COS 0123 (Fy + gm4)
1 . +"2ghm2 cos 012 - hFx sm012
+l2 cos 012 (Fy + 9 (m 3 + m4))
(12.303)
[~. ] M oz
=
M;
(12.304)
.
1
+ 14Fy cos 81234 - l4Fx SIn ()1234 + '2gl4m4 cos 81234
+~gl3m3 cos 0123 -l3Fx sin 0123 + l3 COS 0123 (Fy + gm4) -l2 Fx sin 012 +
~gl2m2 cos 012 + h cos 012 (Fy + 9 (m3 + m4))
+~gh m1 COS 01 - i.r; sin 01 +ll COS01 (Fy + 9 (m2
+ m 3 + m4))
(12.305)
01 + O2
(12.306) (12.307) (12.308)
where
+ 03 + 04
01 + O2 + 03 (h + O2 •
12. Robot Dynamics
551
Example 293 Recursive force equation in link's frame . Practically, it is easier to measure and calculate the force systems in the kink's frame . Therefore, we may write the backward recursive Equations {12.266} and (12.267) in the following form : iF i iM i -
L iF L iM
(12.309)
ei
ei
+ i_{di
X iF i
(12.310)
and calculate the proximal force system from the distal force system in the link's frame. The calculated force system, then, may be transformed to the previous link 's coordinate frame by a transformation i-1F i-1M
i_ 1 i_ 1
=
i-1T iF _ i i 1
(12.311)
i-1T i M _ i i 1
(12.312)
or they may be transformed to any other coordinate frame including the base frame . (12.313) (12.314)
Example 294 Actuator's force and torque. Applying a backward or forward recursive static force analysis ends up with a set of known force systems at joints. Each joint is driven by a motor or generally an actuator that applies a force in a P joint, or a torque in an R joint. When the joint i is prismatic, the actuator force is applied along the axis of the joint i . Therefore, the force of the driving motor is along the Zi-l- axis (12.315) showing that the ki - 1 component of the joint force F, is supported by the actuator, while the ii-l and ji-l components of F, must be supported by the bearings of the joint. Similarly, when joint i is revolute, the actuator torque is applied about the axis of joint i . Therefore, the torque of the driving motor is along the Zi-l -axis (12.316) showing that the ki - 1 component of the joint torque M, is supported by the actuator, while the ii-l and ji-l components of M, must be supported by the bearings of the joint.
12.7 Summary Dynamics equations of motion for a robot can be found by both NewtonEuler and Lagrange methods. In the Newton-Euler method, each link (i)
552
12. Robot Dynamics
is a rigid body and therefore, its translational and rotational equations of motion in the base coordinate frame are (12.317)
°M i_ 1 - °Mi +
L
+ (Odi_ 1 -
°M e i
°ri) x °F i_ 1
- (Odi - °ri) x °F i = ° I, OO:i·
(12.318)
The force F i - 1 and moment M i - 1 are the resultant force and moment that link (i - 1) applies to link (i) at joint i. Similarly, F, and M, are the resultant force and moment that link (i) applies to link (i + 1) at joint i + 1. We measure the force systems (F i - 1 , M i- 1) and (F, , M i) at the origin of the coordinate frames B i - 1 and B, respectively. The sum of the external loads acting on the link (i) are L:: F ei and L:: M ei . The vector or i is the global position vector of C, and "d, is the global position vector of the origin of Bi. The vector °O:i is the angular acceleration and °ai is the translational acceleration of the link (i) measured at the mass center Ci • (12.319) .. 0'
OO:i = {
00:i-1 00:i-1
+ (}i
ki -
1
+ OWi-1
. 0'
X
(}i k i - 1
if joint i is R if joint i is P
(12.320)
Weight is usually the only external load on middle links of a robot , and reactions from the environment are extra external force systems on the base and end-effector links. The force and moment that the base actuator applies to the first link are F o and M o, and the force and moment that the end-effector applies to the environment are F nand M n . If weight is the only external load on link (i) and it is in - ko direction , then we have
°
°
m; g =
' -mig 0 k o
(12.321 )
°ri x m, 0g= -ori x m i g Ok O
(12.322)
where g is the gravitational acceleration vector. The Newton-Euler equation of motion can also be written in link's coordinate frame in a forward or backward method. The backward Newton-Euler equations of motion for link (i) in the the local coordinate frame B, are (12.323)
iM i -
L iM
+ (id i -
ei
-
(id i- 1 - i r i ) x iF i_ 1
i r i) x iF i + iIi bO:i + bW i
X
' i,
bWi
(12.324)
12. Robot Dynamics
553
where in i
=
im i
id _ i 1 id i
ir i
(12.325)
ir i.
(12.326)
and
(12.327)
(12.328)
In this method , we search for the driving force system ( iF i_ l , iMi_I) by having the driven force system (iF i, iM i) and the resultant external force system ( iF e p iM e ;) . When the driving force system eF i- l , iM i_ l ) is found in frame Bi , we can transform them to the frame B i - I and apply the Newton-Euler equation for link (i - 1). i-IF i-IM
i-IT iF i i_ 1 i - I T iM i_ 1 i
i_ 1 i_ 1
(12.329) (12.330)
The negative of the converted force system acts as the driven force system ( - i-I F i- I , - i - I M i- I) for the link (i - 1). The forward Newton-Euler equations of motion for link (i) in the the local coordinate frame B, are (12.331)
(12.332)
in i 'm,
id
=
i_ 1
'd, -
-
ir i
(12.333) (12.334)
ir i
Using the forward Newton-Euler equations of motion, we can calculate the reaction force system (iF i, iMi) by having the action force system (iFi_l, iM i_ I) . When the reaction force system (iF i, iM i) is found in frame B i , we can transform them to the frame B i+l iT- I iF . i+ 1 , iT- I iM
HI
(12.335) i-
(12.336)
554
12. Robot Dynamics
The negative of the converted force system acts as the action force system (- i+l F i , - HI M i ) for the link (i + 1) and we can apply the Newton-Euler equation to the link (i+ 1). The forward Newton-Euler equations of motion allows us to start from a known action force system F o, 1 M o), that the base link applies to the link (1), and calculate the action force of the next link. Therefore, analyzing the links of a robot , one by one, we end up with the force system that the end-effector applies to the environment. The Lagrange equation of motion
e
d(0£) -oc Bq,
-
= Qi
dt Oqi
c
i=1,2 , · · ·n
K-V
(12.337) (12.338)
provides a systematic approach to obtain the dynamics equations for robots. The variables qi are the coordinates by which the energies are expressed and the Qi is the corresponding generalized nonpotential force. The equations of motion for an n link serial manipulator, based on Newton-Euler or Lagrangian, can always be set in a matrix form
D(q)
q + H(q, it) + G(q)
=
Q
(12.339)
or in a summation form n
n
n
L Dij(q) iiJ + L L Hikmqkqm + c. o. =
j=1
(12.340)
k=1 m=1
where, D( q) is an n x n inertial-type symmetric matrix (12.341)
Hikm is the velocity coupling vector (12.342)
G, is the gravitational vector n
Gi = "~mjgTJ{i) Dj
(12.343)
j=1
and J i is the Jacobian matrix of the robot X· i
=
[
OYi] 0Wi
=[
J Di J Ri
].
q
= J. i q.
(12.344)
12. Robot Dynamics
555
To hold a robot in a st ationary configuration, th e act uato rs must apply some required forces to balan ce th e external loads applied to the robot. In th e stat ic condition, th e globally expressed Newt on-Euler equat ions for the link (i), can be writ ten in a recursive form
°F i °M i
-
L °F L °M
(12.345)
ei
-
ei
+ i _ Pd i
X
°F i .
(12.346)
Now we are able to calculate th e act ion force syste m (F i - 1 , M i - d when th e reaction force system (- F i , -M i ) is given.
12. Robot Dynamics
557
Exercises 1. Notation and symbols. Describe their meaning.
a- F z
.
°d i
1- i - I
p-
x,
q- V i
2. Even order recursive translational velocity.
Find an equation to relate the velocity of link (i) to the velocity of link (i - 2), and the velocity of link (i) to the velocity oflink (i + 2).
3. Even order recursive angular velocity. Find an equation to relate the angular velocity of link (i) to the angular velocity of link (i - 2), and the angular velocity of link (i) to the angular velocity of link (i + 2). 4. Even order recursive translational acceleration.
Find an equation to relate the acceleration of link (i) to the acceleration of link (i - 2), and the acceleration of link (i) to the acceleration of link (i + 2). 5. Even order recursive angular acceleration. Find an equation to relate the angular acceleration of link (i) to the angular acceleration of link (i - 2), and the angular acceleration of link (i) to the angular acceleration of link (i + 2).
6. Acceleration in different frames. For the 2R planar manipulator shown in Figure 12.5, find ~az, &az,
°zaI, oal, z oaz, z an dOlaI ·
7. Slider-crank mechanism dynamics. A planar slider-crank mechanism is shown in Figure 12.11. Set up the link coordinate frames, develop the Newton-Euler equations of motion, and find the driving moment at the base revolute joint. 8. PR manipulator dynamics. Find the equations of motion for the planar polar manipulator shown in Figure 5.37. Eliminate the joints' constraint force and moment to derive the equations for the actuators' force or moment.
558
12. Robot Dynamics
-------'"----------t--------L..j~yo
d
FIGURE 12.11. A planar slider-crank machanism. 9. Global differential of a link momentum. In recursive Newton-Euler equations of motion , why we do not use the following Newton equation? .
'F
cd .
cd
dt
dt
.
..
.
= - ' F = -m'v = m'v + 'w ' x m':v 0
•
10. 3R planar manipulator dynamics. A 3R planar manipulator is shown in Figure 12.13. The manipulator is attached to a wall and therefore, g = 9 °io. (a) Find the Newton-Euler equations of motion for the manipulator. Do your calculations in the global frame and derive the dynamic force and moment at each joint. (b) Reduce the number of equations to three for moments at joints. (c) Substitute the vectorial quantities and calculate the moments in terms of geometry and angular variables of the manipulator. 11. A planar Cartesian manipulator dynamics. Determine the Newton- Euler equations of motion for the planar Cartesian manipulator shown in Figure 5.38. 12. Polar planar manipulator dynamics. A polar planar manipulator with 2 DOF is shown in Figure 5.37. (a) Determine the Newton-Euler equations of motion for the manipulator. (b) Reduce the number of equations to two, for moments at the base joint and force at the P joint.
12. Robot Dynamics
559
(c) Substitute the vectorial quantities and calculate the action force and moment in terms of geometry and angular variables of the manipulator. 13.
* Dynamics of a spherical manipulator. Figure 5.26 illustrates a spherical manipulator attached with a spherical wrist. Analyze the robot and derive the equations of motion for joints action force and moment. Assume g = gO ko and the endeffector is carrying a mass m.
14.
* Dynamics of an articulated manipulator. Figure 5.25 illustrates an articulated manipulator Rf--RIIR. Use g = gO ko and find the manipulator's equations of motion .
15.
* Dynamics of a SCARA robot.
Calculate the dynamic joints' force system for the SCARA robot RIIRIIRIIP shown in Figure 5.27 if g = gOko. 16.
* Dynamics of an SRMS manipulator.
Figure 5.28 shows a model of the Shuttle remote manipulator system (SRMS). (a) Derive the equations of motion for the SRMS and calculate the joints' force system for g = O. (b) Derive the equations of motion for the SRMS and calculate the joints' force system for g = gO ko. (c) Eliminate the constraint forces and reduce the number of equations equal to the number of action moments . (d) Assume the links are made of a uniform cylinder with radius r = .25 m and m = 12 kg/ m. Use the characteristics indicated in Table 5.11 and find the equations of motion when the endeffector is holding a 24 kg mass. 17. 3R planar manipulator recursive dynamics . The manipulator shown in Figure 12.13 is a 3R planar manipulator attached to a wall and therefore , g = 9 °io. (a) Find the equations of motion for the manipulator utilizing the backward recursive Newton-Euler technique. (b)
*
Find the equations of motion for the manipulator utilizing the forward recursive Newton-Euler technique.
18. Polar planar manipulator recursive dynamics . Figure 5.37 depicts a polar planar manipulator with 2 DOF.
560
12. Robot Dynamics (a) Find the equations of motion for the manipulator utilizing the backward recursive Newton-Euler technique. (b)
19.
*
Find the equations of motion for the manipulator utilizing the forward recursive Newton-Euler technique.
* Recursive dynamics of an articulated manipulator. Figure 5.25 illustrates an articulated manipulator Rf-RIIR. Use g gO ko and find the manipulator's equations of motion
=
(a) utilizing the backward recursive Newton-Euler technique. (b) utilizing the forward recursive Newton-Euler technique . 20.
* Recursive dynamics of a SCARA robot. A SCARA robot RIIRIIRIIP is shown in Figure 5.27. If g = gOkO determine the dynamic equations of motion by (a) utilizing the backward recursive Newton-Euler technique. (b) utilizing the forward recursive Newton-Euler technique .
21.
* Recursive dynamics of an SRMS manipulator. Figure 5.28 shows a model of the Shuttle remote manipulator system (SRMS). (a) Derive the equations of motion for the SRMS utilizing the backward recursive Newton-Euler technique for g = O. (b) Derive the equations of motion for the SRMS utilizing the forward recursive Newton-Euler technique for g = O.
22. 3R planar manipulator Lagrange dynamics . Find the equations of motion for the 3R planar manipulator shown in Figure 12.13 utilizing the Lagrange technique . The manipulator is attached to a wall and therefore , g = 9 °io. 23. Polar planar manipulator Lagrange dynamics. Find the equations of motion for the polar planar manipulator, shown in Figure 5.37, utilizing the Lagrange technique. 24.
* Lagrange dynamics of an articulated manipulator. Figure 5.25 illustrates an articulated manipulator Rf-RIIR. Use g = gO ko and find the manipulator's equations of motion utilizing the Lagrange technique .
12. Robot Dynamics
t-+----------~
561
X
FIGURE 12.12. A 2R planar manipulator attached to a ceiling in static condition.
25.
* Lagrange dynamics of a SCARA robot. A SCARA robot RIIRIIRIIP is shown in Figure 5.27. If g = gOko determine the dynamic equations of motion by applying the Lagrange technique .
26.
* Lagrange dynamics of an SRMS manipulator. Figure 5.28 shows a model of the Shuttle remote manipulator system (SRMS). Derive the equations of motion for the SRMS utilizing the Lagrange technique for
(a) g = 0 (b) g = gOko.
27.
* Work done by actuators. Consider a 2R planar manipulator moving on a given path. Assume that the endpoint of a 2R manipulator moves with constant speed v = 1 m/ sec from PI to P2 , on a path made of two semi-circles as shown in Figure 13.16. Calculate the work done by the actuators if h = l2 = 1 m and the manipulator is carrying a 12 kg mass. The center of the circles are at (0.75 m, 0.5 m) and (-0.75 m, 0.5 m) .
28. Statics of a 2R planar manipulator. Figure 12.12 illustrates a 2R planar manipulator attached to a ceiling.
562
12. Robot Dynamics
~---------~
Yo
FIGURE 12.13. A 3R planar manipulator attached to a wall. The links are uniform with 24kg 18kg 1m 1m 0,
-g Jo ·
There is a load Fe = -14g 030 N at the endpoint. Calculate the static moments Q1 and Q2 for 01 = 30 deg and O2 = 45 deg. 29. Statics of a 2R planar manipulator at a different base angle.
In Exercise 28 keep O2 = 45 deg and calculate the static moments Q1 and Q2 as functions of 01. Plot Q1 and Q2 versus 01 and find the configuration that minimizes Q1, Q21 Q1 + Q21 and the potential energyV. 30. Statics of a 3R planar manipulator.
Figure 12.13 illustrates a 3R planar manipulator attached to a wall. Derive the static force and moment at each joint to keep the configuration of the manipulator if g = 9 °io.
12. Robot Dynamics 31.
*
563
Statics of an articulated manipulator.
An articulated manipulator Rf-RIIR is shown in Figure 5.25. Find the static force and moment at joints for g = gO ko. The end-effector is carrying a 20 kg mass. Calculate the maximum base force moment. 32.
* Statics of a SCARA robot . Calculate the static joints ' force system for the SCARA robot RIIRIIRIIP shown in Figure 5.27 if g = gOko and the end-effector is carrying a 10kg mass.
33.
* Statics of a spherical manipulator. Figure 5.26 illustrates a spherical manipulator attached with a spherical wrist . Analyze the robot and calculate the static force system in joints for g = gO ko if the end-effector is carrying a 12kg mass.
34.
* Statics of an SRMS manipulator.
A model of the Shuttle remote manipulator system (SRMS) is shown in Figure 5.28. Analyze the static configuration of the SRMS and calculate the joints' force system for g = s"ko. Assume the links are made of a uniform cylinder with radius r = .25 m and m = 12kg/ m. Use the characteristics indicated in Table 5.11 and find the maximum value of the base force system for a 24 kg mass held by the end-effector. The SRMS is supposed to work in a no-gravity field.
Part III
Control
567
Control is the science of desired motion. It relates the dynamics and kinematics of a robot to a prescribed motion . It includes optimization problems to determine forces so that the system will behave optimally. A typical example is the situation in which the initial and terminal configurations of the robot are given and the forces acting on the robot must be found to have the motion in minimum time . Path or trajectory planning is a part of control, in which we plan a path followed by the manipulator in a planned time profile. Paths can be planned in joint or Cartesian space. Joint path planning directly specifies the time evolution of the joint variables. However, Cartesian path specifies the position and orientation of the end frame. So, path includes attaining a desired target from an initial configuration. It may include avoiding obstacles. Joint path planning is simple because it does not involve inverse kinematics , but it is hard to digest the motion of the manipulator in Cartesian space. However, Cartesian coordinates make sense but need inverse kinematics calculation. In this Part, we develop techniques to derive the required commands to control the robot's task.
13
Path Planning 13.1 Joint Cubic Path A cubic path in joint space for t he joint variable qi (t ) between two points qi (to) and qi (t f) is qi (t)
= ao + alt + a2t2 + a3t3
(t~ -
3t5t f )
(13.1)
where ql ao
+ qo ( 3t ot }
- t J)
(t f - to)3 qb (tot J - t 5t })
+ q~
(t5t} -
t~tf)
(13.2)
(t f - t o)3
al
6qototf - 6ql tOtf (t f - to )3 qb (tJ
+ a2
+ t ot} -
2t5t f )
+ q~
(t f - t o) qo ( 3t o + 3t f)
+ ql (-
( 2tot} -
t5 tf )
3
(13.3)
3t o - 3t f )
(t f - t o)3
q~
(to t f - 2t5 + t })
+ qb (2 t }
- t5 - tot f )
(13.4)
(tf - t o)3 a3
t~ -
2qo - 2ql
+ qb (t f
- t o)
+ q~ (t f
(tf - to)3
- to)
(13.5)
and qi(tO)
Ii i( to ) qi (t f ) ili(tf)
= =
qo
,
qo qf
,
qf '
(13.6)
Proof. A cubic polynomial has four coefficients . Therefore, it can satisfy th e position and velocity constraints at t he init ial and final points. For simplicity, we call th e value of th e joint variable, the position, and the rate
570
13. Path Planning
of the joint variable, the velocity. Assume that the position and velocity of a joint variable at the initial time to and at the final time t f are given as (13.6). Substituting the boundary conditions in the position and velocity equations of the joint variable ao + alt al
+ a2e + a3t 3
(13.7)
+ 2a2t + 3a3t2
(13.8)
generates four equations for the coefficients of the path t1o
t6 2;0
t~ 3;6
1 tf [ o 1
tf 2t f
tf 3t}
oI
j[ j ao al a2 a3
=
[qo qb qf qj
j
.
(13.9)
Their solutions are given in (13.2) to (13.5) . In case that to = 0, the coefficients simplify to qo
(13.10)
qb
(13.11)
3 (qf - qo) - (2qb a2
=
+ qj )
tf
t} -2 (qf - qo) + (qb
+ qj) tf
t}
(13.12)
(13.13)
It is also possible to employ a time shift and search for a cubic polynomial of the form (13.14) Now, the boundary conditions (13.6) generate a set of equations
1
o
0 1
[ 1 (t f - to) o 1
o o (tf - to)2
2 (t f - to)
with the following solutions:
(13.15)
13. Path P lanning
571
40 30 20 10 0 - 10 -20 0
0.2
0.4
0.6
0.8
FIGURE 13.1. Kinemati cs of a rest-to -rest cubic path in joint space.
A disadvantage of cubic paths is t he accelera tion jump at boundaries that introduces infinit e jerks . •
Exa m ple 295 Rest-to-rest cubic path. Assume q(O) = lOdeg, q(l) = 45deg, and q(O) = q(l) = O. Th e coefficients of the cubic path are
10
o 105 -70
(13.17)
th at generat e a path fo r' the jo int variable as q(t) = 10 + 105t 2
-
70t 3 deg .
(13.18)
Th e path information is shown in Figure 13.1.
Example 296 To-rest cubic path . A ssume the angle of a jo int starts from 0(0) = 10deg, 8(0) = 12deg / s and ends at 0(2 ) = 45 deg, qi (0) = O. Th e coefficients of a cubic path for this motion are ao
10
al
12 81 2 - 29 -2
a2 a3
(13.19)
572
13. Path Plannin g
40 30
20 10 O-l------~oc___-------~
-10
-20
L...-
o
~
0.2 0.4 0.6 0.8
I
1.2 1.4 1.6 1.8
2
t
FIGURE 13.2. Kinemat ics of a to-rest cubic path in joint space.
Th e kin ematics of this path are
B(t) B( t) 8(t)
10 + 12t + 40.5t 2 - 14.5t 3 deg 8It - 43.5e
+ 12 deg / 8
81 - 87t deg /
8
2
(13.20) (13.21) (13.22)
and are shown graphically in Figure 13.2.
Example 297 Rest-to-rest path with a constant velocity in the m iddle. Assume we n eed a rest-to-rest path with a const ant given velocity q = qc fo r tI < t < t 2 where to < tI < t2 < t f . W e show the boundary condition s to be sat isfied as q(to)
qo
q(to)
qo
q(t)
qc
q(t f)
qf
q(t f)
qf ·
I I
tI < t < t 2
I
(13.23)
Th e path has three parts : rest-to, constant-velocity, and to-rest. We need an equation for the rest-to part of th e motion to achieve th e given velocity. A quadratic path has three coeffi cients and can be utilized to sat isfy three conditions. ao + alt
+ a2t2
a1 + 2a2t
(13.24) (13 .25)
Th e conditions are the initial position and velocity, and the final const ant
13. Path Pl annin g
velocity. Assuming to
=0
573
th e con diti ons for the rest-to path are
ql(O) ql(0) Ih(tl)
(13.26)
that gen erate the following equations: (13.27 ) (13.28) (13.29)
Th erefore, the rest-to path is
0 < t < h.
(13.30)
Given the specific cons tant velocity q~ shows that th e path in the middle part is (13 .31)
h < t
(13.32)
Th e constant of integrati on can be found by ut ilizing th e posit ion conditi on at t = tl q~iI
+ C1
(13.33)
1 I qo - "2t1qc·
(13 .34)
Th ere are fo ur condit ions fo r th e to-rest part of th e path . Th erefore, it can be calculated uti lizi ng a cu bic equati on b« + bIt
e
+ b2 + b3t 3
b1 + 2b2t
(13.35)
+ 3b3e
(13 .36)
and th e follow ing boundary cond itions: q3(t f)
qf
(13 .37)
q3(t f)
o
(13.3 8)
q3(t 2)
q2( t2 )
= q2 = qct2 + qo -
q(t2)
q2(t2)
= q~ .
I
1 I "2iIqc
(13. 39) (13.40)
574
13. Path Planning
These conditions generat e three equations
(13.41)
with the following solutions:
bo
=
t t2 2 f - 2t 2t f
q~
t} - 3t 2t}
+ t~ + t} + qz -t~ + t } - 3t2t} + 3t~tf - t~ + 3t~tf + qf -t~ + t} - 3t 2t} + 3t~t f tf , 2t2tf + t} t2 qc -2M f + t~ + t} + 6q2 -t~ + t} - 3t2t} + 3t~t f -
6
tf 3t t 2 + 3t 2tf 2 f 2 , - t 2 - 2tf -3t2 - 3tf qc - 2t 2t f + t~ + t} + q2 -t~ + t} - 3t 2t} + 3t~tf
+ t 3f 2
- t 2qf -t3
+ 3tf
3t2
+qf
-t~ + t} -
3t2t}
q' - -----='Oc'---,;: 2-
- 2t 2t f
"' 2
+ t2 + tf
+2
+ 3t~t f q2 3
3
2
-t 2 + t f - 3t2tf
qf
2
+ 3t 2tf (13.42)
for t2 < t
< tf .
A graph of the path for th e following valu es is illustrated in Figure 13.3.
it
O.4s
t2
0.7s
tf
Is
qo
0
qf
60deg
qc
50deg / s
,
13. Path P lanning
575
6050 40-
q 3020 10
0.2
0.4
0.6
t
0.8
FIGURE 13.3. A piecewise rest-to-rest path with a const ant velocity in th e middle.
Ex ample 298 A quadratic path through three points. A quadratic path passing through three points (ql' h) , (q2 ' t2) , and (q3' t3) is
(t - t 2)(t - t 3) (t 1 - t2) (t 1 - t3) ql (t -t3)(t-tl) ~ q2 (t 2 - t 3) (t 2 - td (t - td (t - t2)
q(t)
(13.43)
~ (t3 - td (t3- td q3 '
As an exampl e, the path passing through (lOdeg,O), (25deg ,0 .5), and (45 deg, 1) is
q(
t)
=
(t-O .5)(t-l) 0
- 0.5 (- 1)
-~ (14 .0t 2 -
1
(t -l)t
19.0t - 4.0) .
Th e velocity of the path at both ends are
q(O) q(l)
2
~ (-0.5)(0.5) 5 ~
47.5 deg j s -22.5deg js.
t(t -0.5)
1
45 (13.44)
576
13. Path Planning
13.2 Higher Polynomial Path T he number of required condit ions determines t he degree of the polynomial for q = q(t). In general , a polynomial path of degree n,
(13.45) needs n + 1 conditions . The conditions may be of two types: posit ions at a series of points, so that the trajectory will pass through all specified points; or position, velocity, acceleration, and jerk at two points , so that the smoothness of the path can be contro lled. The problem of searching for the coefficients of a polynomial reduces to a set of linear algebraic equations and may be solved numerically. However, the path planning can be simplified by splitting the whole path into a series of segments and utilizing combinations of lower order polynomials for different segments of the path. The polynomia ls must th en be joined together to sat isfy all the required boundary conditions. E xample 299 Quintic path. Forcing a joint variable to have specific position, velocity, and acceleration at boundaries introduces six conditions: q(t o)
q(to) q(to) q(t f) qi (tf) q(t f)
(13.46)
A five degree polynomial can satisfy these conditions (13.47)
and generates a set of six equations: 1 to 0 1
t 02 2to
0 1 0 0
1
t 2f 2tf
0
2
0
tf
2
t 30 t 40 t 05 3t6 4t 03 5t6 6to 12t6 20t6 t 3f t 4f t 5f 2 3 3t f 4t f 5t 4f 2 6tf 12t f 20t3f
ao al a2 a3 a4 a5
qo qb q~
qf qj q'j
(13.48)
A rest-to-rest path with no acceleration at the rest positions with the
13. Path Planning
577
following conditions:
can be found by solving mial 1 0 o 1 o 0 1 1 o 1 o 0
q(O) q(O) q(O)
lOd eg
o o
q(1)
45deg
qi(l) q(l)
o o
(13.49)
a set of equations for the coeffi cients of the polyno0 0 2 1 2 2
10
0 0 0 0 0 0 0 0 0 1 1 1 3 4 5 6 12 20
o o 45
(13.50)
o o
which shows 10
o o
350 -525 210
(13.51)
The path equation is then equal to q(t ) = 10 + 350t3 - 525t 4 + 210t5
(13.52)
which is shown in Figure 13.4. Ex am ple 300 A j erk zero at a start -stop path. To mak e a path start and stop with zero j erk, a seven degree polynomial
q(t ) = ao + alt + a2t2 + a3t3 + a4t4 + a5t5 + a6t6 + a7t7
(13.53)
and eight boundary conditions must be employed. q(O) q(O) q(O) '(]" (0)
qo 0 0 0
q(l)
qf
qi (1) q(l) 'g'(1)
0 0 0
(13.54)
578
13. Path P lanning
q
40 30
q/2
20 10
0 - 10 -20
0
0.2
0.6
0.4
0.8
FIGURE 13.4. A quintic rest -to-rest pat h.
Suc h a zero jerk start-stop path for q(O) = 10 deg and q(l) = 45 deg , can be found by solving the fo llowing set of equations for the unknown coefficients ao, aI , . . . ,a7 1 0 0 0 1 0 0 0
0 0 1 0 0 2 0 0 1 1 1 2 0 2 0 0
0 0 0 6 1 3 6 6
0 0 0 0 1 4 12 24
0 0 0 0 1 5 20 60
0 0 0 0 1 6 30 120
0 0 0 0 1 7 42 210
ao al
a2
a3 a4 a5 a6 a7
10 0 0 0 45 0 0 0
(13.55)
which provides q(t ) = 10 + 1225t 4 - 2940t 5 + 2450t 6 - 700e .
(13.56)
A graph of this path is illustrated in Figure 13.5. Figures 13.1, depicts the path of a rest-to-rest mo tion with no condition on the acceleration and jerk. Figure 13.4 shows an improvement by forcing the motion to have zero accelerations at start and stop . In Figure 13.5, the motion is forced to have zero acceleration and zero jerk at start and stop. Hen ce, it shows the smoothest start and stop. However, increasing the smoothness of the start and stop increases the peak value of acceleration.
Example 301 Cons tant acceleration path . A constant acceleration path has two segments with positive and negative acceleratio ns . Let 's assume the absolute value of th e positive an d negative accelerations are given . (13.57)
13. Path Pl anning
579
40 30 20 I 0 I---,L-,----"lr
o fL.~----,,----'\-------r-----=-?t -10
-20
o
0.2
0.4
0.6
t
0.8
FIGURE 13.5. A jerk zero at st art -stop path.
Th e first half of the motion has a positi ve acceleration that needs a second degree polynomial
(13.58) (13.59) for
1
0 < t < 2tf'
(13.60)
Th e constants of integration are found based on the initial conditions. ql(O)
=
qo
(h(O)
=
0
(13 .61)
For the second half of th e path, we may start with a secon d degree polynomial
(13.62) and impose the following boundary conditions:
q2 (tf)
qf
q2 (t f)
o
tf
ql( 2)
tf 1 2 qz( 2) = sactf
+ qo
(13.63)
Thes e conditions generate three equations for the unknown coeffi cients
(13.64)
580
13. Path Planning
60 40 20 0
q/1O -20 0
0.2
0.4
0.6
0.8
FIGURE 13.6. A constant acceleration path.
with the following solution:
(13.65)
A constant acceleration path is shown in Figure 13.6 for the conditions qo = lOdeg, qf = 45deg, tf = 1, and a.; = 200 deg /s 2 .
E x a mple 30 2 Point sequence path . A path can be assigned via a series of points to approximate a trajectory. Consider an example path specified by four points qo, ql, q2, and q3, such that the points are reached at times to, i}, t2, and t3 respectively. In addition to positions, we usually impose constraint on initial and final velocities and accelerations. The conditions for such a sequence of points can be q(to)
qo
q(to)
0
ij(to)
0
q(t 1 )
ql
q(t2)
q2
q(t3) t 3)
M
q3 0
ij(t3)
O.
(13.66)
13. Path Planning
581
A seven degree polynomial can be utilized to satisfy these eight conditions.
e
ao + alt + a2 + a3t3 + a4t4 +a5t5 + a6t6 + a7
q(t)
e
(13.67)
The set of equations for the unknown coefficients is
t o5 1 to t6 t6 t6 o 1 2to 3t6 4t6 5t6 o 0 2 6to 12t6 20t6 t 51 1 tl tt t 52 1 t2 t~ t~ t~ 1 t3 t5 t~ t~ t~ o 1 2t3 3t5 4t~ 5t~ o 0 2 6t3 12t5 20t33
t7o 7t 6o 42to5
tr tr
t71
a a3 2
7t 63 42t53
qo
o o
ql q2 q3
o o
(13.68)
that can be simplified to
1 0 0 010
o
0
2
1 0.4 0.42 1 0.7 0.72 1 1 1 012 002
o o o
000 0 000 0 000 0 0.43 0.44 0.45 0.46 0.47 0.73 0.74 0.75 0.76 0.77 111 11 3 4 5 6 7 6 12 20 30 42
10
o o 20 30 45
o o (13.69)
for the example data given below.
q(O)
10deg
q(O) q(O)
o o
q(O.4) q(0.7)
q(l) qi(l) q(l)
20deg 30 deg 45deg
o O.
(13.70)
582
13. Path Planning
40 30
20 10
o ~-'<---'< -10
-20
o
0.2
0.4
t
0.6
0.8
FIGURE 13.7. A point sequence path.
The solution for the coefficients is
10
o o
1500 .5 - 7053 12891 -10380 3076 .9
(13.71)
These coefficients generate a path as shown in Figure 13.7. This method provides a continuous and differentiable function for joint variables . Continuity and differentiability of q = q(t) is an advantage that provides a continuous velocity, acceleration , and jerk. However, the number of equations increases by increasing the number of points, which needs larger data storage and increases the calculating time.
Ex ample 303 Splitting a path into a series of segments. Instead of using a single high degree polynomial for the entire trajectory, we may prefer to split the trajectory into some segments and use a series of low order polynomials.
13. Path Planning
583
Consider a path for the following boundary conditions: q(to) =
qo
q(to) = q(to)
a a
q(t 4 )
q3
q(t4) = q(t4)
a (13.72)
O.
which must also pass through three middle points given below. q(t])
ql
q(t2) = q2 q(t3) = q3
(13.73)
Let 's split the entire path into four segments, namely ql (t) , qz(t), q3 (t), and q4(t) . ql(t) q2(t) q3(t) q4(t)
for for for for
q(to) < ql(t) < q(td q(td < q2(t) < q(t2) q(t 2) < q3(t) < q(t3) q(t3) < q4(t) < q(t4)
and and and and
to < t tl < t t.z < t t3 < t
The boundary conditions for the first segment are ql (to)
ql (to) ql (to) (13.74)
ql(h)
which can be satisfied by a cubic fun ction. ql(t) = ao
+ al (t -
to) + a2 (t - to)2 + a3 (t - to)3
(13.75)
The coefficients can be calculated by solving a set of equations
[
~
~
a
a
1
(tl-to)
(13.76)
that provides qo
a a ql - qo 3. (tl-tO)
(13.77)
584
13. Path P lanning
The path in the second segment must satisfy the following boundary conditions: ql
(h(td al
+ 2a2 (tl
- to)2 ql - qo qo + 3 - tl - to q2.
+ 3a3 (h
- to)2
(13.78)
A quadratic polynomial will satisfy these conditions: (13.79) T he coefficients are the solutions of
(13.80)
that provide
bo
(13.81) Th e boundary conditions in th e third segment are q3(t2)
q2
(i3(t2)
(h (t2)
q3(t 3)
h + 2b2t2 a«
(13.82)
We can satisfy these conditions with a quadratic equation
(13.83)
13. Path Pl anning
585
that provides three equations for the unknown coefficients
(13.84)
The coefficients are Co
t -
2
t3 (b + 2b t ) + t~ - t2 + t3 1 2 2 q3- 2t2t 3 + t~
+ t~
-2t2t3 + t~ +q2- 2t 2t 3 + t~ + t~ t2 + t3 (b 2b t ) + 2 t2 q2 - 2t 2t 3 + t~ - t2 + t3 1 + 2 2 2 t2 - q3 - 2t 2t 3 + t~ 1
-t2 + t3
+
(b
+ t~
+ 2b t 22
1
q3 - 2t2t3 + t~
+ t~
) -
q2 - 2t2 t3+ t~+ t~ (13.85)
+ t~ '
The boundary conditions for the fourth segment are q3 (h(t3) = q4
q4(t3) 44(t3) q4 (t4) 44(t4) q4(t 4)
C1
+ 2C2 t3
o o
(13.86)
which needs a fourth degree polynomial to be satisfied. (13.87)
Substituting the boundaTY conditions generates a set of [our equations for the coefficient
t 34
4t~
t 44
4t~ 1 2t~
(13.88)
586
13. Path Planning
45 40
q
35 30 25 20 15 10-
0
0.2
0.4
0.6
t
0.8
FIGURE 13.8. Spliting a path into a series of segments .
As an example, a set of conditions given by
q(O)
lOdeg
q(O) q(O)
o o
q(O.4)
20deg
q(0.7) q(0.9)
30deg
q(1) qi(1) q(1)
45deg
35deg
o o
(13.89)
provides ql (t)
10 + 156.25t 3
q2(t) q3(t) q4(t)
148.99 - 321.67t
(13.90)
-41.56 + 222.78t - 172.2e
(13.91)
+ 216.67t 2
(13.92)
198545 - 827166 .6672t
+1290500t 2 -
893500t 3
+ 231666.67t 4
(13.93)
which is shown in Figure 13.8 graphically. Th e disadvantage of the segment method is the lack of a smooth overall path and having a discontinuous acceleration . To in crease the smoothness of the path, we need to us e higher degree polynomials and put constraints on acceleration and possibly j erk. A piecewise path with continuo us acceleration is called spline.
13. Path Pl ann ing
587
13.3 Non-Polynomial Path Planning A path of motion in eit her joint or Cartesian spaces may be defined based on different mathematical functions . Harmonic and cycloid functions are the most common paths,
q(t) q(t)
ao + al cos a2t + a3 sin a2t ao + alt - a2 sin a3t
(13.94) (13.95)
however, Fourier, Legendre, Chebyshev, and other series methods may also be ut ilized for path planning. Example 304 Harmonic path. Consider a harmonic path between two points q(to) and q(t f) (13.96)
with the rest-to-rest boundary conditions q(to) (j(to) q(tf) q(tf)
(13.97)
Applying the conditions to the harmonic equation (13.96) provides the following solution: q(t) = -21 ( qf
+ qo - (qf - qo) cos 7r (t - to) ) tf - to
.
(13.98)
A plot of the solution is depicted in Figure 13.9 for the following numerical values: to tf qo
0
qb qf
0
qj
O.
0
lOdeg 45deg (13.99)
588
13. Path Planning
40 30 20
10
o
t<-------~;:__-------'1
- 10
o
0.4
0.2
t
0.6
0.8
FIGURE 13.9. A harmonic path.
Ex ample 305 A cycloid path. A cycloid path between two points q(to) and q(t f) with rest-to-rest boundary conditions
q(to) q(to) q(t f) q(t f)
(13 .100)
is
qf -qo (7r(t-to) q() t = qo + - 7r t f - to
- -1.Sill 27r(t -to)) . 2
t f - to
(13.101)
A plot of the cycloid path is illustrat ed in Figure 13.10 for the following numerical values: to tf
0
qo
lOdeg
qo qf
0
,
, qf
0
45deg
O.
(13.102)
13. P ath Pl anning
589
40 30
20
0 +-"---
-
-
-
-
-
"'<--
-
-
-
-
-
=-:/
-10 -20
'--- 0.2 - - -------==-----' o 0.4 0.6 0.8 t
FIGURE 13.10. A cycloid path .
13.4 Manipulator Motion by Joint Path Having the joint variables as functions of tim e, and employing the forward kinematics of manipulators, allows us to calculat e the path of motion for the end-effector. E x a m ple 306 2R manipulator motion based on jo ints ' path. Assume that we hav e calculated the paths of the two joints of a 2R planar manipulato r according to cubic functions, and they are
10 + 105t 2
-
10 + 350t
-
3
70t 3 deg 525t 4 + 21Ot 5 deg .
(13.103) (13.104)
Th e joints ' paths sati sfy the following conditions:
81(0)
10deg
81(1)
45deg
19 1 (0)
0
19 1 (1)
0
82(0)
10 deg
19 1 (0)
0
81 (0)
0
8 1 (I) 19 1 (1)
45deg
81 (I)
0 0
(13.105)
(13.106)
590
13. Path Planning
1.8
y
1.6 1.4 1.2
0.8 0.6 0
0.4
0.2
0.6
0.8
FIGURE 13.11. X and Y components of the tip point position of a 2R planar manipulator.
The forward kinematics of a 2R manipulator are found in Example 132 as below.
-s (81 + 82) 0 i-«. + lac (81 + 82) ] c (81; 82) ~ hs8 1 + l2~ (8 1 + 82 ) (13 .107)
001 The fourth column of °T2 indicates the Cart esian position of the tip point of the manipulator in the base frame . Therefore, the X and Y components of the tip point are
x Y
=
h cos 8 1 + l2 cos (8 1 + 82 ) hsin8 1 +l2sin(81 + 82 )
(13.108) .
(13.109)
Substituting 81 and 82 from (13 .103) and (13.104) provides the tim e variation of the tip position. These variations, for h = l2 = 1 m are shown in Figure 13.11, while the configurations of the manipulator at initial and final positions are shown in Figure 13.12.
13.5 Cartesian Path Cartesian path planning is similar to joint space path planning. The pointto-po int path can be planned by connecting the points, or designing a path to pass close to but not necessarily t hro ugh the points. A practical met hod
13. Path Pl anning
591
2 ,--- - - -- - - - - ----, 1.8 1.6 1.4 1.2
Y 1 0.8 0.6 0.4 0.2 0.4
0.8
2
FIGURE 13.12. Configuration of a 2R manipulator at initial and final positions.
is to design a path utilizing straight lines with constant velocity, and deform the corners to have a smooth transition. The path connecting points ro to r2, and passing close to the corner rl on a transition curve, can be designed by a piecewise motion. to ::; t ::; h - t'
(13 .110)
h - t' ::; t ::; h + t f
The path starts from ro at tim e to and moves with const ant velocity along a line until a point at switching time tl - t' , At this time , the path switches to a constant acceleration parabola. At another switching point at time h +t f , the path switches to the second line and moves with const ant velocity toward the destination at point r2 . The tim e t 1 - to is the required tim e to move from ro to rl and t2 - h is the required time to move from rl to r2 , if there were no transition path. The path is shown in Figure 13.13 schematically. Proof. The first line segment starts from a point ro at time to and, without any deformation, it arrives at point rl at time h via a const ant velocity. The second line ends with a constant velocity at point r2 at time t2 and,
592
13. Path Planning
FIGURE 13.13. Transition parabola between two line segments as a path in Cartesian space. without deformation, it would st art from point r l at time it .
r(t) r(t)
it - t r l - - - (rl - ro) it - to it - t r l - - - (r2 - r d t2 - it
to ~ t
~
tl
~
~
t2
tl
t
(13.111)
We introduce an interval tim e t' before arriving at r l to switch from the line to a transition curve. The transition curve is then between times tl - t' and tl + t' . The simplest transition curve is a par abol a, which, at the end points, has t he same speed as the lines. The boundary positions of th e t ra nsit ion curve on the first and second lines are respectively at
r(it - t') r (t 1 + t')
t' rl - - - 15 1 it - to t' r l + -- Ih t2 - tl
(13.112) (13.113)
where
15 1 15 2
rl - ro r2 - rl .
(13.114) (13.115)
The velocity at t he beginning and final points of the transition curve are resp ectively equal to
t(tl - t')
(13.116)
t(tl + t')
(13.117)
13. Path Planning
593
Assum e the acceleration of motion along the transition curve is const ant
(13.118)
r (t) = r e = cie and therefore, th e transition curve after integration is equal to
~(t -
r(t) = r (t 1 - t') + (t - h + t') f(tl - t') +
tl + t')2r e.
(13.119)
Substituting (13.112) and (13.116) provides t - t1 A r (t ) = r l + - u l + -I .• r e(t - h + t ') 2. tl - to 2
(13.120)
The transition curve r (t) must be at the end point when t = h + t'
r (h
t' r l + - - d2 t2 - h t' A •• 2 r l + - - u l + 2r e t 1
+ t')
i, -to
therefore, the acceleration on the curve must be ..
1
r e = 2t'
(d t2 - t
d 1
2
1 -
)
t 1 - to .
(13.121)
Hence, the curve equation becomes
2 d (t - t' - t d 2 d (t + t' - h) r (t)=rl- 1 4t'(t 1 -t O) + 2 4t'(t2 - tl)
(13.122)
showing that the path between ro and r 2 has a piecewise character given in (13.110). A Cartesian path followed by the manipulator, plus the tim e profile along the path, specify the position and orientation of th e end frame. Issues in Cartesian path planning include attaining a specific target from an initi al st arting point , avoiding obstacl es, and staying within man ipulator capabilities. A path is modeled by n points called control points. Th e control points are connected via straight lines and th e transient parabolas will be imp lemented to exclude th e sharp corners. An alternat ive method is app lying an interpolating method to design a continuous path over the control points , or close to them. • Example 307 A path in 2D Cartesian space. Consider a line in the XY plane connecting (1,0) and (1, 1), and anoth er line connecting (1,1) and (0,1) . Assume that the time is zero at (1,0) , is t = 1 at (1,1), and is t = 2 at (0,1) . For an interval time t' = 0.1, the position vector at contro l points arc
ro
£+3 3
(13.123) (13.124) (13.125)
594
13. Path Pl anning
1.4 1.2
y
1 1--------.......
0.8 0.6 0.4
0.2
o0
0.2 0.4 0.6 0.8
I
1.2 1.4
X FIGURE 13.14. A t ransition parabol a connecting two lines.
r(tt - t f )
=
(13.126) r(t 1 + t
f )
(13.127) where
rl - ro r2 - rl
=j = - i.
(13.128) (13.129)
Th e path of motion is then expressed by the follo wing piecewise fun ction as shown in Figure 13.14: r(t) r(t)
= i + tj = (t - 0 9)2 ) i + 04
(1 -
r(t)=(2-t) i+j
(1 _ 0\1)2) j (t -
o::; t < 0.9 0.9 ::; t
< 1.1
(13.130)
1.1 ::; t ::; 2
Th e velocity of motion along th e path is also a piecewis e function given below. o::; t ::; 0.9 f(t) = j r(t) = t- 0.9i _ t-l.l j (13.131) 0.9 ::; t < 1.1 0 .2 0. 2 r(t) = - i 1.1 ::; t < 2
13.6
*
13. Path Planning
595
Rotational Path
Consider an end-effector frame to have a rot ation matrix G R o at an initial orient ation at tim e to. Th e end-effector must be at a final orient ation G R f at tim e t f . The rotation al path is defined by th e angle-axis rotation matrix
R u, R ou ,
=
°Rf
=
GRT;GRf
(13.132)
that tr ansforms the end-effector frame from the final orient ation G R f to th e initi al orientation G Ro. Th e axis of rot at ion °u is defined by a unit vector expressed in th e init ial frame. Therefore, th e desired rot ation matrix for going from initial to th e final orientation, would be R Tau,
=
GRT GR f O:
(13.133)
To control a rot ation, we may define a series of cont rol orientations G R 1 , between the init ial and final orient ations, and rot ate th e end-effector frame through th e cont rol orientations. When there is a control orient ation G R 1 between the initi al and final orient ations, th en th e initi al orient ation G Ro transforms to the cont rol orient at ion G R 1 using an angleA. , and then it transforms from the control orient ation axis rot ation R ou, '1'0 G R 1 to the final orient ation using a second-angle axis rotation R 1 U'l • G R 2 , " ', G R n
(13.134) (13.135) Proof. According to the Rod riguez rot ation formula (3.4), (13.136)
the angle and axis th at tr ansforms a frame B f to anot her frame B o are found from cos¢
(13.137)
0-
(13.138)
u
If G R o is th e rotation matrix from B o to the global frame G, and th e rot ation matrix from B f to G, th en
G Rf
is
(13.139)
and th erefore, (13.140)
596
13. Path Planning
We define a linearly time dependent rotation matrix by varying the angle of rotation ab out the axis of rotation (13.141)
to
:s: t :s: t f
where, to is the tim e when the end-effector frame is at orientation G ~ and tf is the tim e at which the end-effector fram e is at orientation G R f , and r ll (t)
t -to ) ¢ + cos ( t -to ) ¢ uivers ( t f - to
t f - to
t -to ) ¢ + u3 sin ( t -to ) ¢ U1U2vers ( tf - to
tf - to
t -to ) ¢ - u2 sin ( t -to ) ¢ U1U3 vers ( t f - to
tf - to
t -to ) ¢ - u3 sin ( t -to ) ¢ U1U2vers ( t f - to
t f - to
t -to ) ¢ +cos ( t -to ) ¢ u§vers ( t f - to t f - to U2 U3
t -to ) ¢ + Ul sin ( t -to ) ¢ vers ( tf - to tf - to
to ) ¢ + u2 sin ( t-to ) ¢ t-U1U3vers ( tf - to t f - to U2U3
t -t o ) ¢ - Ul sin ( t -to ) ¢ vers ( tf - to
tf - to
t -to ) ¢ + cos ( t -to ) ¢. u~ vers ( t f - to t f - to
The matrix aRf( t) can turn the final frame about the axis of rotation onto the initial frame, and therefore,
au
(13.142) If there is a cont rol orient ation frame orient at ions, then
G
R 1 between the initial and final (13.143) (13.144)
13. Path Pl anning
597
and therefore, oR 1 -- GRT GR 1 0
(13.145)
lR f -- GRTG R f· 1
(13.146)
The rotation mat rices 0 R 1 and 1 Rf may be defined as linearly tim e var ying rot ation matrices by °R1(t) 1 Rf(t)
= R Oft, ( ~ )'" t I -t o '-Po = tc., , (...!=!L )", if - i t '+'1
to ::; t ::; h
(13.147)
h ::; t ::; t f ·
Utilizing these variable matri ces, we can turn the end-effector fram e from t he initi al orient ation GRo about oil to achieve the cont rol orient ation GR 1 , and then turn t he end-effector frame about 1iL to achieve the final orientation GRf . Following th e par abol a transition technique, we may define an orient at ion path connect ing GR o and GR f ' and passing close to the corner orient ation GR 1 on a transient rot ation path. The path st arts from GRoat tim e to and turns with constant angular velocity along an axis until t = i , - t' . At thi s t ime, the path switches to a parabolic path with constant angular accelera t ion. At another switching orientation at ti me t = h + t' , the path switches to th e second path and turns with constant velocity toward t he destinat ion orientation GRf . Th e tim e t 1 - to is t he required time to move from GR o to GR 1, and t z - tl is the requir ed time to move from GR 1 to GRf if there were no transition path. We introduce an interval tim e t' before arr iving at orient ation GR 1 to swit ch from t he first path segment to a transition path. Th e transition path is then between t imes tl - t' and t l + t' . At t he second switching orient at ion, t he transition path ends at t he same angular velocity as t he third path segment . The bound ary positions of t he t ransition path between t he first and t hird segments are respectively G R 1 (h
- t')
GRo °R 1(h - t' ) G Ro
t
R Oft, (1_ _ " _)'" i t - t o '+'0
=h -
t' (13.148)
GR 1 1R f (h + t' ) t
GR1 R ,ft (_t j"- _) '" t l '+'1
= tl + t' .
(13.149)
l
The transit ion path is t hen equal to
»n
GR
o
1
GRo R o •
(t 1 - t' - t _ (t - t' - h )Z ) 2t' 4t' (t l - to)
u,
( ' 1-'; -' _( ' -" -' 1)2 ) 4' / U,
2'
t1
-
'0)
1>0
t' ::; t ::; h
R ,o
u ,(
+ t'
1R
f
((t + t' - h )Z ) 4t' (t f - h)
' +t' -' 1 2 '( )1>,
4t
if - it
(13.150)
598
13. Path Planning
and the entire path is R(t)
=
0 R 1 (t)
= R ou (..!=!lL)q, ' t l -to
R(t) = Rt(t) R(t) = lRf(t)=Rlu(~)q, ' t f- t l
to ~ t ~ t 1
0
-
tf
h - tf ~ t ~ h
+t
t, 1
f
+t
f
(13.151)
~ t ~ t2.
• 13.7 Manipulator Motion by End-Effector Path Cartesian path planning has more application in robotics , because it can control the level of force and jerk inserted by the hand of a robot to the carrying object. Path planning in Cartesian space also determines the geometric constraints of the external world. However, a Cartesian path needs inverse kinematics to determine the time history of the joint variables. Example 308 Joint path for a designed Cartesian path. Consider a rest-to-rest Cartesian path from point (1, 1.5) to point (-1 ,1.5) on a straight line Y = 1.5. A cubic polynomial can satisfy the position and velocity constraints at initial and final points .
X(O)
Xo = 1
X(l)
X f =-1
X(O) X(l)
x, =0
Xo =0
(13.152)
The coefficients of the polynomial are 1
o -6 4
(13.153)
and the Cartesian path is
x
1 - 6t 2
Y
1.5.
+ 4t 3
(13.154) (13.155)
The inverse kinematics of a 2R planar manipulator is calculated in Example 168 as
(13.156) (13.157)
13. Path Planning
599
y
FIGURE 13.15. Illustration of a 2R panipulator when th e tip point moves on a straight line y = 1.5.
where the sign (±) indicates the elbow-up and elbow-down configurations of the manipulator. Depending on the initial configuration at point (1,1.5), the manipulator is supposed to stay in that configuration. Let 's consider an elbow-up configuration. Therefore , we accept only those values of the joint valuables that belong to the elbow-up configuration. Substituting (13.154) and (13.155) in (13.156) and (13.157) provides the path in joint space
A graphical illustration of the manipulator at every 1/30 th of the total time is shown in Figure 13.15.
13.8 Summary A serial robot may be assumed as a variable geometrical chain of links that relates the configuration of its end-effector to the Cartesian coordinate frame in which the base frame is attached. Forward kinematics are mathematical-geometrical relations that provide the end-effector configuration by having the joint coordinates. On the other hand, the inverse kinematics are mathematical-geometrical relations that provide joint coordinates for a given end-effector configuration.
600
13. Path Planning
The Cartesian path of motion for the end-effector must be expressed as a function of time to find the links' velocity and acceleration. The first applied path function that can provide a rest-to-rest motion is a cubic path for the joint variable qi( t) between two points qi (to) and qi (tf) (13.160) By increasing the requirements, such as zero acceleration or jerk at some points on the path, we need to employ higher polynomials to satisfy the conditions. An n degree polynomial can satisfy n + 1 conditions. It is also possible to split a multiple conditional path into some intervals with fewer conditions. The interval paths must then be connected to satisfy their boundary conditions. A path of motion may also be defined based on different mathematical functions. Harmonic and cycloid functions are the most common paths. Non-polynomial equations introduce some advantages, due to simpler expression, and some disadvantages due to nonlinearities. When a path of motion either in joint or Cartesian coordinates space is defined, forward and inverse kinematics must be utilized to find the path of motion in the other space. Rotational maneuver of the end-effector about the wrist point needs a rotational path. A rotational path may mathematically be defined similar to a Cartesian path utilizing the Rodriguez formula and rotation matrices.
13. Path Pl anning
601
Exercises 1. Notation and symbols . Describ e their meaning. a- to
b- t f
c- qi (t)
d- t f
2. Rest-to-rest cubic path. Find a cubic path for a joint coordinate to satisfy the following conditions:
q(O) = -10deg, q(l) = 45deg, q(O) = q(l) = O. 3. To-rest path. Find a quadratic path to satisfy t he following conditions:
q(O) = -10deg, q(l) = 45deg, q(l) = O. Calculate the initial velocity of the path using the quadratic path. Then, find a cubic path to satisfy the same boundary conditions as the quadratic path. Compare the maximum accelerations of the two paths. 4. Constant velocity path. Calculate a path to satisfy the following conditions:
q(O) = -10deg, q(l) = 45deg, q(O) = q(l) = 0 and move with constant velocity 35deg.
q=
25 deg / sec between 12 deg and
5. Constant acceleration path. Calculat e a path with constant acceleration ij = 25 deg / sec2 between 12 deg and 35 deg, and satisfy the following condit ions:
q(O) = -lOdeg, q(l) = 45deg, q(O) = q(l) = O. 6. Zero jerk path. Find a path to satisfy the following boundary conditions:
q(O) =0, q(l) = 66deg, q(O)=q(l)=O and have zero jerk at the beginning, middl e, and end points.
602
13. Path Planning
7. Control points. Find a path to satisfy the conditions
q(O) = 10deg, q(l) = 95 deg, q(O) = q(l) = 0 and pass through the following control points :
q(0.25) = 30 deg, q(0.5) = 65deg 8. A jerk zero at start-middle-stop path. To make a path have jerk as close to zero as possible, an eight degree polynomial
and nine boundary conditions can be employed. Find th e path. 0
q(O) = 0
ij(O) = 0
'q'(O) = 0 ·q·(0.5) = 0
q(l) = 120 deg
qi(l) = 0
ij(l) = 0
'q'(I) = 0
q(O)
=
9. Point sequence path. The conditions for a sequence of points are given here. Find a pat h to satisfy the conditions given below. (a)
q(O) = 5deg q(0.4) = 35 deg q(0.75) = 65deg q(l) = 100deg
q(O) = 0
ij(O) = 0
qi(l) = 0
ij(l) =0.
(b)
q(O) = 5 deg q(2) = 15 deg q(4) = 35deg q(7.5) = 65 deg q(lO) = 100deg
q(O) = 0
q(O) = 0
qi(lO) = 0
ij(lO) = O.
10. Splitting a path into a series of segments . Find a path for the following conditions:
q(O) = 5deg q(2) = 15 deg q(4) = 35deg q(7.5) = 65 deg q(10) = 100deg
q(O) = 0
qi(10)
=0
ij(O) = 0
ij(lO) = o.
13. Path Planning
603
by the splitting method and breaking th e entire path into four segments. qi (t)
q2(t) q3(t) q4(t) 11.
*
for for for for
q(O) < q1(t) < q(2) q(2) < q2(t) < q(4) q(4) < q3(t) < q(7.5) q(7.5) < q4(t) < q(lO)
and and and and
0
Extra conditions .
To have a smooth overall path in the splitting method, we may add extra conditions to match the segments. Solve Exercise 10 having a zero jerk transition between segments. 12. 2R manipulator motion to follow a joint path. Find the path of the endpoint of a 2R manipulator, with II = l2 = 1 m, if the joints' variable follow the given paths:
(h(t) (Jz(t)
=
10+ 156.25t3 -41.56 + 222.78t - 172.2t2.
13. 3R planar manipulator motion to follow a joint path. Find the path of the endpoint of a 3R manipulator, with
h l2 =
ls
1m 0.65m 0.35m
if the joints ' variable follow these given paths
(}l(t) = (}2(t) = (}3(t) =
-41.56+222.78t-I72.2tz 148.99 - 321.67t + 216.67t2 198545 - 827166.6672t +1290500e - 893500t 3 + 231666.67t4 .
Calculate the maximum acceleration and jerk of the endpoint. 14. Rf-RIIR articulated arm motion . Find the Cartesian trajectory of the endpoint of an arti culat ed manipulator, shown in Figure 5.25, if the geometric parameters are d1
1m
d2
0
l2 = l3
1m 1m
604
13. Path Planning
,/
/
",.,. - - - -.
"
.....
"
I
I I
~p
2
I
PI
I I /
" . 0:--'------ ,/.--.- X ,-
"
FIGURE 13.16. A 2R manipulator moves on a path made of two semi-circles. and the joints' paths are 8 I (t)
-41.56 + 222.78t -172.2t 2
82(t) 83(t)
148.99 - 321.67t + 216.67e 198545 - 827166 .6672t
+1290500t 2 -
893500t3
+ 231666.67t4 .
15. Cartesian path for a 2R manipulator. Calculate a cubic rest-to-rest path in Cartesian space to join the following points with a straight line.
PI P2
(1.5,1) (-0.5,1.5) .
Then calculate and plot the joint coordinates of a 2R manipulator, with h = l2 = 1 m, that follows the Cartesian path. What is the maximum angular acceleration of the joints' variable? 16.
* Joint path for a given Cartesian path. Assume that the endpoint of a 2R manipulator moves with constant speed v = 1 m/ sec from PI to P2 , on a path made of two semicircles, as shown in Figure 13.16. Calculate and plot the joints' path if h = l2 = 1 m. Calculate the value and position of the maximum angular acceleration and jerk in joint variables. The center of the circles are at (0.75m, 0.5m) and (-0.75m, 0.5m).
17.
* Obstacle avoidance and path planning. Find a path between PI = (1.5,1) and P2 = (-1 ,1) to avoid the obstacle shown in Figure 13.17. The path may be made of two straight lines with a transition circular path in the middle . The radius of the
13. Path Planning (-0.25,1)
605
(0.25.1)
FIGURE 13.17. An obstacle in the Cartesian space of motion for a 2R manipulator. circle is r = 0.5 m and the center of the circle is at a point that makes the total length of the path minimum. The lines connect to the circle smoothly. The endpoint of a 2R manipulator, with h = l2 = 1 m, starts at rest from PI and moves along the first line with constant acceleration. The endpoint keeps its speed constant v = 1 tu] sec on the circular path and then moves with constant acceleration on the final line segment to stop at P2 . Calculate and plot the joints' paths. Find the value and position of the maximum angular velocity, acceleration, and jerk for both joints' variable . 18.
* Joint path for a given Cartesian path. Connect the points PI = (1.1,0.8,0.5) and P2 = (-1 ,1,0.35) with a straight line. Find a rest-to-rest cubic path and plot the Cartesian coordinates X, Y, and Z as functions of time. Then, calculate the joints' path for an articulated manipulator, shown in Figure 5.25, if the geometric parameters are:
Find the value and position of the maximum angular velocity, acceleration, and jerk for the joints' variable . 19.
* Transition parabola. In Exercise 17, connect the points PI = (1.5,1) and P2 = (-1 ,1) with two straight lines, using Po = (0,0.6) as a corner . Design a parabolic
606
13. Path Planning
transition path to avoid the corner if the interval time is t' = 5 sec and the total time of motion is 12sec. 20.
* Euler angles rotational path. Assume that the spherical wrist of a 6 DOF robot starts from rest position and turns about the axes of the final coordinate frame B 6 in order z-x-z for sp = 15 deg, 0 = 38 deg, and 'ljJ = 77deg. The frame B 6 is installed at the wrist point. Design a rest-to-rest cubic rotational path for the angles ip, 0, and 'ljJ, if each rotation takes 1sec. Then, find the axis and angle of rotation, (it, ¢), that moves the wrist from the initial to the final orientation. Design a cubic rotational path for the axis-angle rotation if it takes 3 sec. Calculate the Euler angles path cp(t), O(t) , and 'ljJ(t) for this motion . Calculate and compare the maximum angular velocity, acceleration, and jerk for cp, 0, and 'ljJ in the first and second motions. What is the maximum angular velocity, acceleration, and jerk of ¢ in the second motions?
14
* Time Optimal Control 14.1
* Minimum Time and Bang-Bang Control
The most important job of industrial robots is moving between two points rest-to-rest. Minimum time control is what we need to increase industrial robots productivity. The objective of time-optimal control is to transfer the end-effector of a robot from an initial position to a desired destination in minimum time. Consider a system with the following equation of motion :
x = f (x(t) , Q(t))
(14.1)
where Q is the control input, and x is the state vector of the system (14.2) The minimum time problem is always subject to bounded input such as
IQ(t)1 ::; QMax '
(14.3)
The solution of the time-optimal control problem subject to bounded input is bang-bang control. The control in which the input variable takes either the maximum or minimum values is called bang-bang control. Proof. The goal of minimum time control is to find the trajectory x( t) and input Q(t) starting from an initial state xo(t) and arriving at th e final state xf(t) under the condition that the whole trajectory minimizes the time integral J=
i
tl
dt.
(14.4)
to
The input command vector Q(t) usually has the constraint (14.3). We define a scalar function H, and a vector p
H(x, Q, p) = pT f (x(t) , Q(t))
(14.5)
that provide the following two equations:
aH T
x p
ap
=
aH T ax
(14.6) (14.7)
608
14.
* Time Optimal Control
Based on the Pontryagin principle, the optimal input Q(t) is the one that minimizes the function H . Such an optimal input is to apply the maximum effort, QMax or -QMax, over the entire time interval. When the control command takes a value at the boundary of its admissible region, it is said to be saturated. The function H is called Hamiltonian, and the vector p is called a co-state. •
*
Example 309 A linear dynamical system. Consider a linear dynamical system given by
Q= x or where
x
= [ ~~ ]
x= [A]x+bQ
(14.8)
[~ ~]
(14.9)
[Aj
=
along with a constraint on the input variable
Q:::;1.
(14.10)
By defining a co-state vector (14.11)
the Hamiltonian {14.5} becomes
H(x, Q , p) = pT ([Aj x
+ bQ)
(14.12)
that provides two first-order differential equations
aH T
ap
= [Ajx+ bQ
aH T
- ax
= - [A]p.
(14.13) (14 .14)
Equation (14.14) is (14.15)
which can be integrated to find p (14.16)
The Hamiltonian is then equal to H
Qp2 + PIX2 (-CIt
+ C 2) Q + PIX2
(14.17)
14.
Th e control command
* Tim e Optimal Control
609
Q only appears in pTbQ = (- 0 1t + 02) Q
(14.18)
which can be maximized by
Q(t )
=1
if if
= -1
-01 t + O2 2: 0 -01t+ 0 2 <0
(14.19)
Th is solution implies that Q(t ) has a ju mp point at t = ~ . Th e j ump point, at which the control com m and suddenly changes f rom maximum to min imum or from min im um to m aximum, is called the switching poin t. Substituting the control input (14.19) into (14. 8) gives us two first-order differential equations (14.20)
Equation (14.20) can be integrated to find the path x(t).
[
~~
]=
[ ~~ ] =
[fit ~3 0
3
)2
+04
Q= 1
if
]
(14.21)
[=fi\;30 3)2+ 0 4]
if
Q= - 1
Th e constants of integration, 0 1 , O2, 0 3 , and 0 4 , must be calculated based on the follo wing boundary conditions: x o = x (t o)
(14.22)
Eliminating t between equati ons in (14. 21) provides the relationship between the state variables Xl and X2 . Xl = ~ X~ + 0 4 Xl = - ~ x~ + 0 4
if if
Q= 1 =-1
Q
(14.23)
Th ese equations show a series of parabolic curves in the Xlx2-plane with
0 4 as a parameter. Th e parabolas are shown in Figure 14.1 (a) and (b) with the arrows indicating the direction of motion on the paths. Th e Xlx2 -plane is called the phase plane. Con sidering that there is one swit ching point in this system, the overall optimal paths are shown in Figure 14.1(c) . A s an examp le, assume the state of the system at initial and fina l times are x (to) and x (t f) respectively. The motion starts with Q = 1, which forces the system to mov e on the control path Xl = ~ x~ + (XlO - ~ x~o) up to the intersection point with (Xlf + ~ x~f ). Th e intersection is the switching point at which the control input changes to Q = -1 . Th e swit ching point is at
Xl
=
-~x~ +
Xl
41 (2XlO + 2x f -
2 2 ) X20 + x2f
(14.24) (14.25)
610
14.
* Time Optimal Control
Q=l
~---+-I--+---+--I-- I---I----+----.---f-
-6
XJ
(b)
Q=-l
Q=l
-6
FIGURE 14.1. Optimal path for Q = paths in phase plane.
x in phase
plane and the mesh of optimal
* Time Optimal Control
14.
611
*
Example 310 Robot equations in state equations. The vector form of the equations of motion of a robot is
D(q)
q + H(q, q) + G(q)
= Q.
(14.26)
We can define a state vector
x=[~]
(14.27)
and transform the equations of motion to an equation in state space
x=
f (x(t) , Q(t))
where f (x(t) , Q(t)) = [ D- 1 (Q
(14.28)
~ H _ G) ] .
(14.29)
*
Example 311 Time-optimal control for robots. Assume that a robot is initially at
x(t o) = Xo = [
~~
]
(14 .30)
and it is supposed to be finally at (14.31)
in the shortest possible time. The torques of the actuators at each joint is assumed to be bounded (14.32) The optimal control problem is to minimize the time performance index (14.33)
The Ham iltonian H is defined as H(x, Q, p) = pT f (x(t), Q(t))
(14.34)
which provides the following two sets of equations : (14.35) (14.36)
612
14.
* Time Optimal Control
The optimal control input Qt(t) is the one that minimizes the function H . Hamiltonian minimization reduces the time-optimal control problem to a two-point boundary value problem. The boundary conditions are the states of the robot at times to and t f . Due to nonlinearity of the robots' equations of motion, there is no analytic solution for the boundary value problem. Hence, a numerical technique must be developed.
*
Example 312 Euler-Lagrange equation. To show that a path x = x (t) is a minimizing path for the functional J
*
=
J(x)
i
t!
(14.37)
f(x , x, t)dt
to
with boundary conditions x(to)
= Xo, x( t f) = x f ' we need to show that
J(x) ~ J(x*)
(14.38)
for all continuous paths x(t) satisfying the boundary conditions. Any path x(t) satisfying the boundary conditions x(to) = xo, x(tf) = xf, is called admissible. To see that x (t) is the optimal path, we may examine the integral J for every admissible path. An admissible path may be defined by
*
x* + Ey(t)
(14.39)
= y(tf) = 0
(14.40)
E«
(14.41 )
x(t) = where y(to) and
1
is a small number. Substituting x(t) in J and subtracting from (14.37) provides 6.J 6.J
J (x*
i t!
+ Ey(t))
f(x*
- J (x*)
+ Ey, x* + ey,t)dt
~
Let us expand f(x*
+ Ey, x* + ey, t)
(14.42)
_it!
f(x* , x* ,t)dt.
~
about (x* , x*)
and find (14.44)
14.
* Time Optimal Control
613
where VI
V2 =
i
t!
to
(ay axf + y. aaxf ) dt
t! ( to
i
2 a f y2 ax2
(14.45)
2 a f + 2YiJ ax ax
2 a f) + iJ2 ax2
(14.46)
The first integral, VI is called the first variation of J, and the second integral, V2 is called the second variation of J . All the higher variations are combined and shown as 0 (10 3) . If x* is the minimizing curve, then it is necessary that f::>.J ;::: 0 for every admissible y(t) . If we divide f::>.J by 10 and make 10 --t 0 then we find a necessary condition for x to be the optimal path as VI = O. This condition is equivalent to
*
i
t!
to
(ay axf + y.aaxf ) dt
= O.
(14.47)
By integrating by parts we may write t! . af _ ( af)t! _it! ~ (a f) Ya' dt - Ya ' Yd a ' dt . x x to to t x i to
(14.48)
Since y(to) = y(tf) = 0, the first term on the right-hand side is zero. Th erefore, the minimization integral condition (14 .47), for every admissible y(t) , reduces to
i
t!
to
y(aaxf _ ~dt a~) dt ax
= O.
(14.49)
The terms in the parenth eses are continuous functions of t, evaluated on the optimal path x* , and they do not involve y(t) . So, the only way that the
--it M) , mult iplied by a non zero
bounded integral of the parenth eses (~ fun ction y(t) , from to and tf to be zero, is that
(14.50)
Equation (14.50) is a necessary condition for x = x*(t) to be a solution of the minimization problem (14 .37). This differential equation is called the Euler-Lagrange equation. It is the same Lagrange equation that we utilized to derive the equations of motion of a robot. The second necessary condit ion to have x = x (t) as a minimizing solution is that the second variation, evaluated on x (t) , must be negative.
* *
*
Example 313 The Lagrange equation for extremizing J = The Lagrange equation for extremizing the functional
1
112 x 2dt .
2
J =
2
i; dt
(14.51)
614
14.
* Time Optimal Control P I ."....--
-
- - - - - - - - -- . X
y FIGURE 14.2. A curve joining points Pl and P2 , and a frictionless sliding point .
is
aj _.!!:.- aj = -x =
ax
dt
ax
°
(14.52)
that shows the optimal path is
(14.53) Considering the boundary condit ions x(l) = 0, x(2) = 3 provides x = 3t - 3.
*
Example 314 Brachistochrone problem. We may utilize the Lagrange equation and find the frictionless curve joining two points as shown if Figure 14.2, along which a particle falling from rest due to gravity, travels from the higher to the lower point in the minimum time. This is the well-known brachistochrone problem . If v is the velocity of the falling point along the curve, then the time required to fall an arc length ds is ds / v . Then, the objective function to find the curve of minimum time is
J=
1 2
1
However, ds
=
ds - .
(14.54)
V
VI + y/2dx
(14.55)
and according to the conservation of energy v
= V2gy .
(14 .56)
Therefore , the objective function simplifies to
J=
2fif +Y/2
1 1
- - d x. 2gy
(14.57)
14.
* Time Optimal Control
615
Applying the Lagrange equations we find (14.58)
where r is a constant. The optimal curve starting from y(O) = 0 can be expressed by two parametric equations x
r ((3 - sin (3)
(14.59)
y
r (1 - cos (3) .
(14.60)
The optimal curve is a cycloid. The name of the problem is derived from the Greek word "(3paxu7To(," meaning "shortest," and "XPOl/o(," meaning "time." The brachistochrone problem was originally discussed by Galilei in 1630 and later solved by Johann and Jacob Bernoulli in 1696.
*
Example 315 Lagrange multiplier. Assume f (x) is defined on an open interval (a, b) and has continuous first and second order derivatives in some neighborhood of Xo E (a, b) . The point Xo is a local extremum of f(x) if
df(xo) = 0 dx .
(14.61)
Assume f(x) = 0, x E IR n and gi(X) = 0, i = 1,2" " ,m are functions defined on an open region IR n and have continuous first and second order derivatives in IR n . The necessary condition that Xo be an extremum of f(x) subject to the constraints gi(X) = 0 is that there exist m Lagrange multipliers Ai, i = 1,2, . . . , m such that (14.62)
As an example, we can find the minimum of
f subject to g=
by finding the gradient of f \7 (1
-
= 1-
xi - x~
xi + X2 -
(14.63)
1= 0
(14.64)
+ Ag
xi - x~ + A(xi + X2 - 1)) = O.
That leads to (14.65) -2X2
+ A=
O.
(14.66)
616
14.
* Time Optimal Control
To find the three unknowns, Xl, X2, and A, we employ Equations (14.65) , (14.66), and (14.64). There are two sets of solutions as follows : Xl Xl
= 0 = ±1/v'2
X2 X2
= 1 = 1/2
A=2 A=l
(14.67)
*
Example 316 Admissible control function . The components of the control command Q(t) are allowed to be piecewise continuous and the values they can take may be any number within the bounded region of the control space. As an example, consider a 2 DOF system Q(t) = [QI Q2 with the restriction IQil < 1, i = 1,2 . The control space is a circle in the plane QI Q2. The control components may have any piecewise continuous value within the circle. Such controls are called admissible.
f
Example 317 Description of the time optimal control problem. The aim of minimum time control is to guide the robot on a path in minimum time to increase the robot's productivity. Except for low order, autonomous, and linear problems, there is no general analytic solution for the time optimal control problems of dynamical systems. The problem of time optimal control is always a bounded input problem. If there exists an admissible time optimal control for a given initial condition and final target, then, at any time, at least one of the control variables attains its maximum or minimum value. Based on Pontryagin's principle, the solution of minimum time problems with bounded inputs is a bang-bang control, indicating that at least one of the input actuators must be saturated at any time. However , finding the switching points at which the saturated input signal is replaced with another saturated signal is not straightforward, and is the main concern of numerical solution methods . In a general case, the problem reduces to a two-points boundary value problem that is difficult to solve. The corresponding nonsingular, nonlinear two-point boundary value problem must be solved to determine the switching times. A successful approach is to assume that the configuration trajectory of the dynamical system is preplanned, and then reduce the problem to a minimum time motion along the trajectory.
14.2
* Floating Time Method
Consider a particle with mass m, as shown in Figure 14.3, is moving according to the following equation of motion: mii
= g(x, ±) + f(t)
(14.68)
where g(x,±) is a general nonlinear external force function , and f(t) is the unknown input control force function. The control command f(t) is
14.
f -
..-J
* Time Optimal Control
617
m
II I JI
I---------------I~
X
FIGURE 14.3. Rest-to-rest motion of a mass on a straight line time-optimally. bounded to
If(t)1 :S F.
(14.69)
The particle starts from rest at position x(O) = Xo and moves on a straight line to the destination point x( t f) = x f at which it stops. We can solve this rest-to-rest control problem and find the required f(t) to move m from Xo to xf in minimum time utilizing the floating time algorithm.
Algorithm 14.1. Floating time technique. 1. Divide the preplanned path of motion x(t) into s + 1 intervals and specify all coordinate values Xi , (i = 0,1 ,2 ,3, ..., s + 1) 2. Set fo = +F and calculate 70
= J2m(x~ - xo)
(14.70)
3. Set fs+! = -F and calculate (14.71)
4. For i from 1 to s - 1, calculate
5. If
Ifil :S F,
6. Calculate 7. If 6
7i
such that
then stop, otherwise set j
7j-1
IfJ-11 :S F,
such that
fJ
Ii
= +F and
= s,
= -F
then stop, otherwise set j
=j
- 1 and return to step
618
14.
* Time Optimal Control x Xi+1 f - - - - - - - - - - - - : ; ; 7 ( ' Xi f - - - - - - - - : ; " r
I
I 1-. 1
FIGURE 14.4. Time history of motion for the point mass m .
,, ,, ,, ,, , I
1:s_1
IS _1 FIGURE 14.5. Introducing two extra points, after the final points.
X-I
and
I
1:s
Is X s+2,
, 1:s+1
+
I.
IS +1
IS +2
before the initial and
°
Proof. Assume g(Xi, Xi) = and x(t) , as shown in Figure 14.4, is the time history of motion for the point mass m . We divide the path of motion into s + 1 arbitrary, and not necessarily equal, segments. Hence, the coordinates Xi , (i = 0,1 ,2, ..., S + 1) are known. The floating-time Ti = t, - t i-I is defined as the required time to move m from Xi to Xi+! . Utilizing the central difference method, we may define the first and second derivatives at point i by Xi+! - Xi-I Ti 2
Ti
(14.73)
+ Ti-I 4
2
+ T i_ 1
(Ti-I
Ti
+ Ti-I
Xi+!
+ Ti +r,Ti-I Xi-I -
Xi
) . (14.74)
These equations indicate that the velocity and acceleration at point i depend on Xi , and two adjacent points Xi-I, and XHI, as well as on the floating times T i, and Ti-I . Therefore, two extra points, X-I and X s+2 , before the initial point and after the final point are needed to define velocity and acceleration at Xo and X s +! ' These extra points and their corresponding floating times are shown in Figure 14.5.
14.
*
Time Optimal Control
619
The rest conditions at the beginning and at the end of motion require X-I
=
Xl
70
X s+2
=
Xs
7 s+1=7 s ·
7-1
(14.75) (14.76)
Using Equation (14.74), the equation of motion, fi = mXi, at the initial point is
fo
mxo =
4m
27 2 (Xl -
o
The minimum value of the first floating time
xo) . 70
(14.77)
is found by setting fo = F . (14.78)
It is the minimum value of the first floating time because, if 70 is less than the value given by (14.78), then fo will be greater than F and breaks the constraint (14.69). On the other hand, if 70 is greater than the value given by (14.78), then fo will be less than F and the input is not saturated yet. The same conditions exist at the final point where the equation of motion is
m XsH 4m -22 (z, - Xs-1) .
f sH
7s
The minimum value of the final floating-time ,
7sv
(14.79) is achieved by setting
fsH = -F.
r, = . ! 2m (x s
V
-Xs -
-F
1)
(
)
14.80
To find the minimum value of 71 , we develop the equation of motion at
Xl
(14.81) which is an equation with two unknowns II and 71. We are able to find numerically by adjusting 71 to provide II = F . Applying this procedure we are able to find the minimum floating times 7H1 by applying the maximum force constraint fi = F , and solving the equation of motion for 7iH numerically. When 7i is known and the maximum force is applied to find the next floating-time, 7Hl, we are in the forward path of the floating time algorithm. In the last step of the forward path, 7 s-l is found at X s-l ' At this step, all the floating-times 7i, (i = 0,1,2, ..., s) are known, while fi = F for i = 0,1,2, ..., s -1, and Ii = -F for i = s. Then, the force fs is 71
620
14.
* Time Optimal Control o
x= !
!
f
m
x
FIGURE 14.6. A rectilinear motion of a rigid mass m under the influence of a control force f(t) and a friction force f.lmg . the only variable that is not calculated during the forward path by using the equation of motion at point X s . It is actually dictated by the equation of motion at point x s , because 7 s-1 is known from the forward path procedure, and 7 s is known from Equation (14.80) to satisfy the final point condition. So, the value of fs can be found from the equation of motion at i = s and substituting 7 s, 7 s-1, X s-1, X s, and X s+1. fs
=
2 7s
4m
+
2 7 s- 1
(
7 s-1
7s
+ 7 s- 1
X s+1
+
r, 7s
+ 7 s- 1
X s-1 - X s
)
(14.82)
Now, if fs does not break the constraint If(t)1 :S F, the problem is solved and the minimum time motion is determined. The input signals, Ii, (i = 0,1,2, ..., s + 1), i -=I- s, are always saturated, and also none of the floatingtimes 7i can be reduced any more. However, it is expected that is breaks the constraint If(t)1 :S F because accelerating in s - 1 steps with f = F produces a large amount of kinetic energy and a huge deceleration is needed to stop the mass m in the final step. Now we reverse the procedure, and start a backward path. According to (14 .82), fs can be adjusted to satisfy the constraint I, = -F by tuning 7 s-1. Now fn-1 must be checked for the constraint If(t)1 :S F . This is because 7 s-2 is already found in the forward path, and 7 s-l in the backward path. Hence, the value of fn-1 is dictated by the equation of motion at point X s-1. If fs-1 does not break the constraint If(t)1 :S F, the problem is solved and the time-optimal motion is achieved. Otherwise, the backward path must be continued to a point where the force constraint is satisfied. The position Xk in the backward path, where Ilkl :S F, is called switching point because !J = F for j < k , 0 :S j < k and !J = - F for j > k, k<j:Ss+1. _
*
Example 318 Moving a mass on a rough surface. Consider a rectilinear motion of a rigid mass m under the influence of a variable force f(t) and a friction force umq, as shown in Figure 14.6. The force is bounded by If(t)1 :S F, where ±F is the limit of available force. It is necessary to find a function f(t) that moves m, from the initial conditions x(O) = 0, v(O) = 0 to the final conditions x(tf) = l > 0, v(tf) = 0
14.
* Time Optimal Control
621
10
..... V')
f".l
f(t)
0
~ ..... V') ..... 0
~
5
V')
00
..... 0\
~'-, ~'-,
":t-
00 00 ":t-
c:::::;
c:::::;
~'-,
~'-,
~
c:::::;'
f".l'
f".l'
f".l'
II
c:::::;
c:::::;
c:::::;
:t
-5
r-, f".l ..... V')
~
'0 c:::::;
V')
c:::::;
o
r-,
'0 00 0 r-, '0
II
II
II
:t
:t
t
:t
-10 0.1
0.2
0.3
0.4
0.5
0.6
0.7
FIGURE 14.7. Time history of th e optimal input f(t) for different friction coefficients /.1.
in minimum total time t = t i : The motion is described by the following equation of motion and boundary conditions: f = ttii: - p,mg v(O) = 0 v(tj) =0.
(14.83) (14.84)
Using the theory of optimal control, we know that a time optimal control solution for p, = 0 is a piecewise constant funct ion where the only discontinuity is at the switching point t = T = t f /2 and f(t) = {F -F
iif!
t
>T.
(14.85)
Therefore, the time optimal control solution for moving a mass m from x(O) = Xo = 0 to x( t j) = x j = l on a smooth straight line is a bangbang control with only one switching tim e. The input force f(t) is on its maximum , f = F , before the switching point x = (Xj - xo)/2 at T = t J!2 , and f = -F after that. Any asymmetric characteristics, such as friction, will make the problem asymm etric by moving the switching point. In applying the floating-tim e algorithm, we assume that a particle of unit mass, m = 1 kg , slides under Coulomb friction on a rough horizontal surface. The magnitude of the friction force is p,mg, where p, is the friction coefficient and g = 9.81 tis ] s2. We apply the floating-time algorithm using the following num erical values: F= ION
l
= 10m
s + 1 = 200
(14.86)
622
* Tim e Optimal Control
14.
xtt)
f,.l =O. tl =0.591857502 0.8 f,.l =O.2. ~= 0. 644 67086 7 0.6
f-- - - - --+-r--+- -+'--- - - - - -f-.
t
0.4
f,.l=0.2, ~= 0. 60353 625 1
0.2 0='---0.1 0.2
f,.l=O.2. tr 0.734885127 ----"----' 0.3
0.4
0.5
0.6
0.7
F IGURE 14.8. Time history of th e optimal mot ion x (t ) for different frict ion coefficients J-L.
10
~
((t)
""l
r-, .,..,
.......
e-, 'C 00 c:::> r-, 'C "t-
""l <"l 'C
e-, <"l
.......
""l 00 00 "tr-,
5
.,.., 00 """ .... o, c:::> """
.,..,
'C
'C
o
C
C
C
""" c:;:;
~'-,
~'-,
~'-,
"i C
"i C
"i C
11 ::I.
::I.
::I.
~'-, r::5 II
::I.
-5
- 10
o
0.2
0.4
~
0.6
II
p
x(t)
II
0.8
FIGURE 14.9. Position history of the optimal force f(t) for different friction coefficients J-L.
*
14.
Time Optimal Control
623
Figures 14.7, 14.8, and 14.9 show the results for some different values of u: Figure 14.7 illustrates the time history of the optimal input force for different values of p,. Each curve is indicated by the value of p, and the corresponding minimum time of motion t f. Time history of the optimal motions x(t) are shown in Figure 14.8, while the time history of the optimal inputs f(t) are shown in Figure 14.9. The switching times and positions are shown in Figures 14.7 and 14.9, respectively. If p, = 0 then switching occurs at the midpoint of the motion x( T) = l/2 and halfway through the time T = tt/2 . Increasing p, delays both the switching times and the switching positions. The total time of motion also increases by increasing u ,
*
Example 319 First and second derivatives in central difference method. Using a Taylor series, we expand x at points Xi-l and Xi+! as an extrapolation of point Xi .
1 ..
.
+ 2'XiTi-l
2
+ XiTi + '2XiTi + ...
=
Xi
=
Xi -
XiTi-l
1 ..
Accepting the first two terms and calculating
(14.87)
2
-
(14.88)
... .
X i+! - X i-l
provides
Now, accepting the first three terms of the Taylor series and calculating + Xi-l provides
Xi+l
(14.89)
*
Example 320 Convergence. The floating time algorithm presents an iterative method hence, convergence criteria must be identified. In addition, a condition must be defined to terminate the iteration. In the forward path, we calculate the floatingtime Ti by adjusting it to a value that provides Ii = F . The floating-time Ti converges to the minimum possible value, as long as OXi/OTi < 0 and OXi/OTi-l > O. Figure 14.10 illustrates the behavior of Xi as a function of Ti and Ti-l' Using the Equation (14·74), the required conditions are fulfilled within a basin of convergence, Zl Xs+l Z4 Xs+l
+ Z2 Xs + Z3 X s-l < + Z5 Xs + Z6 X s-l >
0 0
(14.90) (14.91)
624
14.
* Time Optimal Control
80 ..
X
40
o -40
o
0.04 0.08 0.12 0.16 't Io
FIGURE 14.10. Behavior of Xi as a function of r , and
7 i -1 .
where,
+ 8TtT T-1 + 6TTTt-1) 3 (TT + TT_1)3 (Ti + Ti_d
8 (6TfTi-1
(14.92)
8 (TL1 - 3Tf - 8Tt TT-1 - 9Tf Ti- 1 + 3TiTL 1)
(14.93) (TT + TT-1)3 (Ti + Ti_1)3 8 (-Tt l + 3Tt + 3Tt Ti-1 - 3TiTL1 - 6TTTY-1) (14.94) 3 (TT + TT-1 )3 (Ti + Ti_d
Z4 =
Z6 =
Tf - 6TtT T-1 -
3Tf Ti- 1 + 3TiTL l ) (14.95) 3 (TT + TLl) 3 (Ti + Ti_d 8 (-3T f_l + Tf + 3Tt Ti-l - 9TiTL I - 8TT Tt- l ) ( 14.96) 3 (TT + TT-1)3 (r , + Ti_ d 8 (3TL1 -
+ 8TTTt-1 + 6TiTLl) (TT + TT-l) 3 (Ti + Ti_1)3
8 (6TtT T-1
(14.97)
The convergence conditions guarant ee that Xi decreases with an increase in Ti, and increases with an increase in Ti-l ' Therefore, if either Ti or Ti - I is fixed, we are able to find the other floating time by setting fi = F. Convergence conditions for backward path are changed to
+ Z 2 X s + Z 3 X s-1 > 0 Z4X s +l + Z5X s + Z6X s - 1 < o.
ZI Xs+l
(14.98) (14.99)
A termination criterion may be defined by (14.100)
14.
* Time Optimal Control
625
where E is a user-specified number. The termination criterion provides a good method to make sure that the maximum deviation is within certain bounds.
*
Example 321 Analytic calculating of floating times. The rest condition at the beginning of the motion of an m on a straight line requires X-I
TO
=
Xl
(14.101)
T-l·
(14.102)
The first floating time TO is found by setting fo = F and developing the equation of motion fi = mii, at point Xo· (14.103)
Now the equation of motion at point
II =
4m ( 2 2 "i +T O
Tl
TO X2 +TO
Xl
is
) + Tl Tl Xo + Xl +TO
Substituting TO from (14·103) into (14·104) and applying the following equation
F=
4mF 2mxl - 2mxo
+ FTr
(14.104)
.
II
= F provides
(14.105)
that must be solved for Tl . Then substituting Tl from (14.105) into the equation of motion at X2, and setting h = F leads to a new equation to find T2. This procedure can similarly be applied to the other steps . However, calculating the floating times in closed form is not straightforward and getting more complicated step by step, hence , a numerical solution is needed. The equations for calculating Ti are nonlinear and therefore have multiple solutions. Each positive solution must be examined for the constraint fi = F. Negative solutions are not acceptable.
*
Example 322 Brachistochrone and path planning. The floating-time method is sometimes applicable for path planning problems. As an illustrative example, we considered the well-known brachistochrone problem. As Johann Bernoulli says : "A material particle moves without friction along a curve . This curve connects point A with point B (point A is placed above point B). No forces affect it, except the gravitational attraction. The time of travel from A to B must be the smallest. This brings up the question: what is the form of this curve '?"
626
14.
* Time Optimal Control y
0.8 0.6
0.4 0.2
2
FIGURE 14.11. Time optimal path for a falling unit mass from A(O,l) to two different destinations.
The classical solution of the brachistochrone problem is a cycloid and its parametric equation is x
=
r
Y
=
r
(13 - sin 13) (1 - cos 13) .
(14.106) (14.107)
where r is the radius of the corresponding cycloid and 13 is the angle of rotation of r . When 13 = 0 the particle is at the beginning point A(O,O). The particle is at the second point B when 13 = 13 B' The value of 13 B can be obtained from XB
=
YB
r
(13 B - sin 13 B)
r (1 - cosj3B) '
(14.108) (14.109)
The total time of the motion is
(14.110) In a path-planning problem, except for the boundaries, the path of motion is not known. Hence, the position of Xi in Equations (14·73) and (14.74) are not given. Knowing the initial and final positions, we fix the Xi coordinates while keeping the Yi coordinates free. We will obtain the optimal path of motion by applying the known input force and searching for the optimum Yi that minimizes the floating times. Consider the points B 1 (1,0) and B2(2, 0) as two different destinations of motion for a unit mass falling from point A(O, 1). Figure 14.11 illustrates
14.
*
Time Optimal Control
627
the optimal path of motion for the two destinations, obtained by the floatingtime method for s = 100. The total time of motion is tfJ = 0.61084s, and t h = 0.8057s respectively. In this calculation, the gravitational acceleration is assumed g = 10m/ S2 in - Y direction. An analytic solution shows that (3 B1 = 1.934563rad and (3 B2 = 2.554295 rad. The corresponding total times are t fJ = 0.6176s, and t h = 0.8077s respectively. By increasing s, the calculated minimum time would be closer to the analytical results, and the evaluated path would be closer to a cycloid. A more interesting and more realistic problem of brachistochrone can be brachistochrone with friction and brachistochrone with linear drag. Although there are analytical solutions for these two cases, no analytical solution has been developed for brachistochrone with nonlinear (say second degree) drag. Applying the floating-time algorithm for this kind of problem can be an interesting challenge.
14.3
* Time-Optimal Control for Robots
Robots are multiple DOF dynamical systems. In case of a robot with n DOF, the control force f and the output position x are vectors.
r x The constraint on the input force vector can be shown by (14.111) where the elements of the limit vector F E IRn may be different. The floating time algorithm is applied similar to the algorithm 14.2, however, at each step all the elements of the force vector f must be examined for their constraints. To attain the time optimal control, at least one element of the input vector f must be saturated at each step, while all the other elements are within their limits . Algorithm 14.2. Floating time technique for the n DOF systems . 1. Divide the preplanned path of motion x(t) into s + 1 intervals and specify all coordinate vectors Xi, (i = 0,1 ,2,3, ..., s + 1). 2. Develop the equations of motion at Xo and calculate TO for which only one component of the force vector £0 is saturated on its higher limit, while all the other components are within their limits.
k E {O, 1, 2, = 0,1,2,
r
,n} ,n
628
14.
* Time Optimal Control
3. Develop the equations of motion at X s +1 and calculate 7 s for which only one component of the force vector f S +1 is saturated on its higher limit, while all the other components are within their limits. k E {O, 1,2" " ,n} = 0,1,2"" , n
= -Fk fS+l r ~ r;
fs+1k
r
4. For i from 1 to s - 1, calculate 7i such that only one component of the force vector f i is saturated on its higher limit , while all the other components are within their limits. fik
k E {0,1,2, ' " ,n } r = 0,1,2, . . . , n
= -Fk
fir cr. 5. If
Ifsl
~
F, then stop, otherwise set j = s.
6. Calculate 7j-l such that only one component of the force vector fj is saturated on its lower limit, while all the other components are within their limits. 7. If jrj-ll ~ F, then stop, otherwise set j 6.
=j
- 1 and return to step
*
Example 323 2R manipulator on a straight line. Consider a 2R planar manipulator that its endpoint moves rest-to-rest from point (1, 1.5) to point (-1,1.5) on a straight line Y = 1.5. Figure 14.12 illustrates a 2R planar manipulator with rigid arms. The manipulator has two rotary joints, whose angular positions are defined by the coordinates 8 and ip, The joint axes are both parallel to the Z-axis of the global coordinate frame, and the robot moves in the XY -plane. Gravity acts in the - Y direction and the lengths of the arms are II and l2. We express the equations of motion for 2R robotic manipulators in the following form: P
Q =
AB + By; + COif + Dlji + M ..
·2
E8+Fy;+C8 +N
(14.112) (14.113)
14.
*
Time Optimal Control
629
FIGURE 14.12. A 2R planar manipulator with rigid arms.
where P and Q are the actuator torques and A =
A(
B = C D E
B(
F
m2l~
=
G = M N =
D(
Following Equations (14.73) and (14.74), we define two functions to discretize the velocity and acceleration. (14.115)
Then, the equations of motion at each instant may be written as Pi(t) Qi(t)
=
=
Aia(B)+Bia(rp)+CiV(B)v(ep)+DiV2(ep)+Mi (14.117) Eia(B) + Fia(rp) + Giv 2(B) +Ni (14.118)
where Pi and Qi are the required actuator torques at instant i. Actuators are assumed to be bounded by (14.119)
630
14.
* Time Optimal Control y
---------,~------ x
FIGURE 14.13. A 2R planar manipulator, moving from point (1,1.5) to point (-1,1.5) on a straight line Y = 1.5.
To move the manipulator time optimally along a known trajectory, motors must exert torques with a known time history. Using the floating-time method, the motion starts at point i = 0 and ends at point i = s + 1. Introducing two extra points at i = -1 and i = n + 2, and applying the rest boundary conditions, we find 7"0
=
7"s
= 7"s+l
7"1
(14.120)
All inputs of the manipulator at instant i are controlled by the common floating-times 7"i and 7"i-1 . In the forward path, when one of the inputs saturates at instant i, while the others are less than their limits, the minimum 7"i is achieved. Any reduction in 7"i increases the saturated input and breaks one of the constraints (14.119). The same is true in the backward path when we search for 7"i-1. Consider the following numerical values and the path of motion illustrated in Figure 14.13. (14.121)
To apply the floating time algorithm, the path of motion in Cartesian space must first be transformed into joint space using inverse kinematics. Then, the path of motion in joint space must be discretized to an arbitrary interval, say 200, and the algorithm 14.2 should be applied. Figure 14.14 depicts the actuators ' torque for minimum time motion after applying the floating time algorithm. In this maneuver, there exists one switching point, where the grounded actuator switches from maximum to minimum. The ungrounded actuator never saturates, but as expected, one
14.
*
Time Optimal Control
631
100 . , . . - - - - . . . - - - - - - - - - - ,
75 50
'?
25
III
0
~ ;::
~ -25
~
-50
P(t)
-75 -100
.L-.
-..
o
0.098
~
0.197
0.296
0.395
FIGURE 14.14. T ime optimal cont rol inputs for a 2R manipulator moving on line Y = 1.5, -1 < X < 1.
of the inputs is always saturated. Calculating the floating times allows us to calculat e the kinematics inf orm ati on of motion in j oint coordinate space. Time histories of the j oint coordin ates can be ut ilized to determ in e the kinematics of the end- effector in Carte sian space.
*
Example 324 Multiple switching points. Th e 2R manipulator shown in Figure 14.12 is made to follow the path illustrated in Figure 14.15. Th e float ing time algorithm is run for the following data: ml
= m 2 = 1kg
X(O) = 1.9m
h
=
h
X(tf)
= QM =
= 1m
PM
= 0.5m
Y=O
100Nm
(14.122)
which leads to the solut ion shown in Figure 14.16. A s shown in the Figure, there are three switching points for this motion. It is seen that the optimal motion stasis while the ground ed actuator is saturated and the ungrounded actuator applies a positive torque with in its limits . At the first switching point, th e ungrounded actuator reaches its negative lim it . Th e ground ed actuator shows a chang e from positive to negative until it reaches its negative limit when the second switching occurs. Between the second and third switching points, the grounded actuator is saturated. Finally, when the un grounded actuator tou ches its negative lim it for the second time, th e third switching occurs.
632
14.
* Time Optimal Control
y 0.8 0.6 0.4 0.2
o
0.2
0.4
0.6
0.8
1.2
1.4
1.6
1.8
F IGURE 14.15. Illustrati on of motion of a 2R planar manipulator on line y = O.
100
..,...------a-- - - - --,
75 50 ";::-
~ 25
0-...:.
~
0
s:r~ -25
Q(t)
-50 -75 -100 0
0.079
0. 157
0.236
0.3 15
FIGURE 14.16. Time optimal control inputs for a 2R manipulator moving on line Y = 0, 0.5 < X < 1.9.
14.
* Time Optimal Control
633
14.4 Summary Practically, every actuator can provide only a bounded output. When an actuator is working on its limit, we call it saturated. Time optimal control of an n DOF robot has a simple solution: At every instant of time , at least one actuator must be saturated while the others are within their limits . Floating-time is an applied method to find the saturated actuator, the switching points, and the output of the non-saturated actuators. Switching points are the points that the saturated actuator switches with another one. The floating-time method is based on discrete equations of motion, utilizing variable time increments. Then, following a recursive algorithm, it calculates the required output for the robot's actuators to follow a given path of motion.
* Time Optimal Control
14.
635
Exercises 1. Notation and symbols.
Describe th eir meaning.
2.
a-TO
b-Ti
c-Ii
d- .»,
e-Xi
f- Xi
g-
h- T s
i- xs H
j- is
T -1
* Time optimal control of a 2 DOF system. Consider a dynamical system
+ 2 X2 + 5Q
Xl
-3XI
X2
2 XI - 3 X 2
that must st art from an arb itrary initi al condit ion and finish at = 0, with a bounded control input IQI :::; 1.
Xl
=
X2
Show th at th e function s
II h h
=
-
3 XI
+ 2 X 2 + 5Q
- 2 XI
- 3 X2
1
along with the Hamiltonian function H
H = -1 + P I (-3XI + 2 X 2 + 5Q) + P 2 (2XI and th e co-state variables 3.
PI
and
P2
3 X 2)
can solve th e problem.
* Nonlinear objective function. Consider a one-dimensional control problem X= - x
+Q
where Q is th e cont rol command. Th e variable the bound ary condit ions
X(O)
a
x(t f )
b
and minimize the objective function J.
X
= x(t ) must satisfy
636
14.
* Time Optimal Control
Show that th e functions
~Q2 2
- x+Q
o along with th e Hamiltonian function H 1 2 H = "2 PoQ + pd - x
+ Q)
and the co-st ate variables Po and PI can solve the problem. 4.
* Tim e opt imal cont rol to origin. Consider a dynamical system
that must start from an arbitrary initi al condit ion and finish at the origin of th e phase plane, Xl = X2 = 0, with a bounded control IQI ~ 1. Find th e cont rol command to do thi s motion in minimum tim e. 5.
* A linear dynamical syst em. Consider a linear dynami cal system
x=
[A]x+ bQ
where
[AJ =
[~ ~]
subj ect to a bounded constraint on th e control command Q ~1.
Find th e time optimal control command Q to move from the system from Xo to xj, (a)
Xo= [
=~ ]
Xj =
[~ ]
(b)
xo =[=~]
Xl = [
~]
14.
6.
* Time Optimal Control
637
* Constraint minimization. Find the local minima and maxima of
f (x) = xi + x~ + x~ subject to the constraints
(a) 91
= Xl + X2 + X3
3= 0
-
(b) 91
=
92
7.
xi + X~ + X~ - 5 = 0 xi + X~ + X~ 2Xl - 3 = O. -
* A control command with different limits. Consider a rectilinear motion of a point mass m = 1 kg under the influence of a control force f(t) on a smooth surface. The force is bounded to F l :::; f(t) :::; F2 • The mass is supposed to move from the initial conditions x(O) = 0, v(O) = 0 to the final conditions x(t f) = 10 m, v(t f) = 0 in minimum total time t = t f . Use the floating time algorithm to find the required control command f(t) = mi: and the switching time for
(a) Fl
= ION
F2
= ION
Fl
= 8N
F2
= ION
Fl
= ION
(b)
(c)
8.
F2
= 8N.
* A control command with different limits. Find the optimal control command If(t)1 :::; 20N to move the mass m = 2 kg from rest at point PI to P2 , and return to stop at point P3 , as shown in Figure 14.17. The value of /1 is
(a) /1=0 (b)
/1 = 0.2.
638
14.
* Time Optimal Control
10 rn
f
m
x ~-- 5
rn- - -"
F IGURE 14.17. A rectilinear motion of a mass m from rest at point PI to P2 , and a return to stop at point P3.
o
10 rn
EE= "========:::==r~. pmg
X
FIGURE 14.18. A rectilinear motion of a mass m on a rough sur face and at t ached to a wall with a spring.
14.
* Tim e Optimal Control
~-__.
_ .......! - L
639
X
FIGURE 14.19. A 2R manipulator moves between two points on a line and a semi-circle.
9.
* Motion of a mass under friction and spring forces. Find the optimal control command If(t)1 ~ lOON to move the mass m = 1kg rest-to-rest from x(O) = 0 to x(tf) = 10m. The mass is moving on a rough surface with coefficient J.L and is attached to a wall by a linear spring with stiffness k, as shown in Figure 14.18. The value of J.L and k are
(a)
= 2N/m
J.L
= 0.1
k
J.L
=0.5
k = 5N/m.
(b)
10.
* Convergence conditions. Verify Equations (14.92) to (14.97) for the convergence condition of the floating-time algorithm.
11.
* 2R manipulator moving on a line and a circle. Calculate the actuators' torque for the 2R manipulator, shown in Figure 14.19, such that the end-point moves time optimally from P 1(1.5m , 0.5m) to P2(0 , 0.5m) . The manipulator has the following characteristics: ml
h IP(t)1 IQ(t)1 The path of motion is
m2
= 1kg
l2 = 1m < 100Nm < 80Nm
640
14.
* Time Optimal Control
FIGURE 14.20. A polar manipulator, controlled by a torque Q and a force P .
(a) a straight line (b) a semi-circle with a center at (0.75m, 0.5m). 12.
* Time optimal control for a polar manipulator. Figure 14.20 illustrates a polar manipulator that is controlled by a torque Q and a force P . The base actuator rotates the manipulator and a force P slides the second link on the first link. Find the optimal controls to move the endpoint from PI (1.5 m, 1 m) to Pz(-1, 0.5 m) for the following data: 5kg IQ(t)1 :::; 100Nm
ml =
13.
mz = 3kg IP(t)1 :::; 80Nm
* Control of an articulated manipulator. Find the time optimal control of an articulated manipulator, shown in Figure 5.25, to move from H = (1.1,0.8,0.5) to Pz = (-1,1,0.35) on a straight line. The geometric parameters of the manipulator are given below. Assume the links are made of uniform bars . d l = 1m lz = 1m ml = 25 kg IQI(t)l:::; 180Nm
mz = 12kg IQz(t)1 < 100Nm
h = 1m = 8kg
m3
IQ3(t)1 :::; 50Nm
15 Control Techniques 15.1
Open and Closed-Loop Control
A robot is a mechanism with an actuator at each joint i to apply a force or torque to derive the link (i). Th e robot is instrumented with position , velocity, and possibly accelerat ion sensors to measur e the joint variables' kinemati cs. The measur ed values are usually relative. Th ey are kinematics information of the frame Bi, attached to the link (i) , relative to the frame B i - 1 or B o. To cause each joint of the robot to follow a desired motion , we must provide the required torque command. Assume that the path of joint variables qd = q(t) are given as functions of tim e. Th en, the required torques th at cause the robot to follow th e desired motion are calculated by th e equat ions of motion and are equal to (15.1) where the subscripts d and c st and for desired and controlled respectively. In an ideal world, th e variables can be measur ed exactly and the robot can perfectly work based on the equations of motion (15.1). Then, the actuators' control command Q c can cause the desired path qd to happen. This is an open-loop control algorithm, that th e control commands are calculated based on the equat ions of motion and a known desired path of motion . Then, the control commands are fed to the syst em to generate th e desired path. Therefore, in an open-loop control algorithm, we expect the robot to follow the designed path, however, there is no mechanism to compensate any possible error. Now assume that we are watching the robot during its motion by measuring the joints' kinematics. At any instant there can be a difference between the actual joint variables and the desired values. The difference is called error and is measured by e
(15.2)
e
(15.3)
Let 's define a cont rol law and calculate a new control command vector by (15.4) where k p and k D are constant control gains. Th e control law compares the actual joint variables (q , q) with the desired values (qd' qd), and generat es
642
15. Control Techniques
q
FIGURE 15.1. Illustration of feedback control algorithm .
m
f
c FIGURE 15.2. A linear mass-spring-damper oscillator .
a command proportionally. Applying the new control command changes the dynamic equations of the robot to produce the actual joint variables q. Q c + kDe + kpe
= D(q) q + H(q, it) + G(q)
(15.5)
Figure 15.1 illustrates the idea of this control method in a block diagram. This is a closed-loop control algorithm, in which the control commands are calculated based on the difference between actual and desired variables. Reading the actual variables and comparing with the desired values is called feedback, and because of that, the closed-loop control algorithm is also called a feedback control algorithm . The controller supplies a signal proportional to the error. This signal is added to the predicted command Q c to compensate the error . The principle of feedback control can be expressed as: Increase the control command when the actual variable is smaller than the desired value and decrease the control command when the actual variable is larger than the desired value. Example 325 Mass-spring-damper oscillator. Consider a linear oscillator made by a mass-spring-damper system shown in Figure 15.2. The equation of motion for the oscillator under the effect of an external force f is mx+ cx+kx = f
(15.6)
where, f is the control command, m is the mass of the oscillating, c is the viscous damping , and k is the stiffness of the spring . The required force to
15. Control Techniques
Ie
- - - - 1...1
Dynamics I---I.~
643
Xd
(a)
x
Dynamics f---r--l~
d X
(b)
·r
c ontrollerH Dynamics
T
X
(c) FIGURE 15.3. Open-loop and closed-loop control algorithms for a linear oscillator . achieve a desired displacement motion.
Xd
= x(t) is
calculated from the equation of (15.7)
The open-loop control algorithm is shown in Figure 15.3(a). However, we may use the difference between the desired and actual outputs e = x - Xd, and define a control law
f ko
f e + kDe
> 0
+ kpe kp
(15.8)
>0
to compensate a possible error. The new control law uses a feedback command as shown in Figure 15.3(b). It is also possible to define a new control law only based on the error signal such as f = - k De - kpe. (15.9) Employing this law, we can define a more compact feedback control algorithm and change the equation of motion to (15.10)
The equation of the system can be summarized in a block diagram as shown in Figure 15.3(c). A general scheme of a feedback control system may be explained so that a signal from the output feeds back to be compared to the input. This feedback
644
15. Control Techniques
signal closes a loop and makes it reasonable to use the words feedback and close-loop. The principle of a closed loop control is to detect any error between the actual output and the desired input. As long as the error signal is not zero, the controller keeps changing the control command so that the error signal converges to zero.
Example 326 Stability of a controlled system. Consider a linear mass-spring-damper oscillator as shown in Figure 15.2 with the equation of motion given by
mi: + eX + kx =
f.
(15.11)
We define a control law based on the actual output
f = -kDx - kpx ko > 0
, kp
(15.12)
>0
and transform the equation of motion to
mx+(c+kD)x+(k+kp)x=O.
(15.13)
By comparison with the open-loop equation (15.11), the closed loop equation shows that the oscillator acts as a free vibrating system under the action of new stiffness k + k p and damping c + ko - Hence, the control law has changed the apparent stiffness and damping of the actual system. This example introduces the most basic application of control theory to improve the characteristics of a system and run the system to behave in a desired manner. A control system must be stable when the desired output of the system changes, and also be able to eliminate the effect of a disturbance. Stability of a control system is defined as: The output must remain bounded for a given input or a bounded disturbance function . To investigate the stability of the system, we must solve the closed loop differential equation (15.13). The equation is linear and therefore, it has an exponential solution.
(15.14) Substituting the solution into the equation (15.13) provides the characteristic equation ni);
2
+ (c + kD) A + (k + k p)
= 0
(15.15)
with two solutions c v kt»
A12
'
= ---- ± 2m
V(c+k D)2 - 4m(k+kp) . 2m
(15.16)
15. Control Techniques
645
The nature of the solution (15.14) depends on Al and A2' and therefore on ko and k p. The roots of the characteristic equation are complex
(15.17)
-a±bi c+k D 2m
a
b
=
J4m(k
(15.18)
+ kp) -
(c+ kD)2
2m
(15.19)
provided the gains are such that (c+ k D)2 < 4m(k + kp).
(15.20)
In this case, the solution of the equation of motion is x
= Ce-~wnt sin
wn
Jk:k
(wn Ht + sp)
(15.21 )
where,
~
=
p
= Ja 2+b2
c+k D 2Jm(k+kp)
a 2 va + b2
(15.22) (15.23)
The parameter W n is called natural frequency, and ~ is the damping ratio of the system. The damping ratio controls the behavior of the system according to the following categories: 1. If ~ = 0, then the characteristic values are purely imaginary, Al,2 = ±bi = ±i2~J4m(k+kp) . In this case, the system has no damping, and therefore, it oscillates with a constant amplitude around the equilibrium, x = 0, forever. 2. If 0 < ~ < 1, then the system is under-damped and it oscillates around the equilibrium with a decaying amplitude. The system is asymptotically stable in this case. 3. If ~ = 1, then the system is critically-damped. A critically damped oscillator has the fastest return to the equilibrium in an unoscillatory manner.
4. If ~ > 1, then the system is over-damped and it slowly returns to the equilibrium in an unoscillatory manner. The characteristic values are real and the solution of an over-damped oscillator is
(15.24)
646
15. Control Techniques
5. If ~
< 0, then the system is unstable because the solution is (15.25)
and shows a motion with an increasing amplitude.
Example 327 Solution of a characteristic equation. Consider a system with the following characteristic equation :
),2+ 6),+10=0 .
(15.26)
Solutions of this equation are ),1 ,2 =
-3 ± i
(15.27)
showing a stable system because Re (),1 ,2) = -3 < 0 . Characteristic equations are linear polynomials. Hence, it is possible to use numerical methods, such as Newton-Raphson, to find the solution and determine the stability of the system.
Example 328 Complex roots. In case the characteristic equation has complex roots ),1 ,2 =
a ± bi
(15.28)
we may employ the Euler formula ei B = cos() + isin()
(15.29)
and show that the solution can be written in the form x
+ isinbt) + C2 eat (cosbt eat (A cos bt + Bsinbt) Cl eat (cosbt
- isinbt)
(15.30)
where, C 1 and C2 are complex, and A and B are real numbers according to
*
A
(15.31)
B
(15.32)
Example 329 Robot Control Algorithms. Robots are nonlinear dynamical systems, and there is no general method for designing a nonlinear controller to be suitable for every robot in every mission. However, there are a variety of alternative and complementary methods, each best applicable to particular class of robots in a particular mission. The most important control methods are as follows: Feedback Linearization or Computed Torque Control Technique. In feedback linearization technique, we define a control law to obtain a linear
15. Control Techniques
647
differential equation for error command, and then use the linear control design techniques. The feedback linearization technique can be applied to robots successfully, however, it does not guarantee robustness according to parameter uncertainty or disturbances . This technique is a model-based control method, because the control law is designed based on a nominal model of the robot. Linear Control Technique. The simplest technique for controlling robots is to design a linear controller based on the linearization of the equations of motion about an operating point. The linearization technique locally determines the stability of the robot. Proportional, integral, and derivative, or any combination of them, are the most practical linear control techniques. Adaptive Control Technique. Adaptive control is a technique for controlling uncertain or time-varying robots. Adaptive control technique is more effective for low DOF robots. Robust Control Technique, Adaptive In the robust control method, the controller is designed based on the nominal model plus some uncertainty. Uncertainty can be in a lot of parameters, such as the load carrying by the end-effector. For example, we develop a control technique to be effective for loads in a range 1 - 10 kg. Gain-Scheduling Control Technique. Gain-scheduling is a technique that tries to apply the linear control techniques to the nonlinear dynamics of robots. In gain-scheduling, we select a number of control points to cover the range of robot operation . Then at each control point, we make a linear timevarying approximation to the robot dynamics and design a linear controller. The parameters of the controller are then interpolated or scheduled between control points .
15.2 Computed Torque Control Dynamics of a robot can be described in the form Q = D(q) q + H(q, 4)
+ G(q)
(15.33)
where q is the joint variable vector, and Q(q, 4, t) is the torques applied at joints. Assume a desired path in joint space is given by a twice differentiable function q = qd(t) E C 2. Hence, the desired time history of joints' position, velocity, and acceleration are known. We can control the robot to follow the desired path, by introducing a computed torque control law as below
(15.34) where e is the error vector
(15.35) and kD and kp are constant gain diagonal matrices. The control law is stable and applied as long as all the eigenvalues of the following matrix e = q - qd
648
15. Control Techniques
have negati ve real part. (15.36) P roof. The required Q c to track qd(t) can directl y be found by substituting the path function into the equations of motion. (15.37) The calculated torques are called control inputs, and th e control is based on th e open-loop control law. In an open-loop control , we have the equations of motion for a robot and we need the required torques to move the robot on a given path. Open-loop control is a blind contro l method, since th e current state of the robot is not used for calculating the inputs. Due to non-modeled parameters and also errors in adjustment, there is always a difference between th e desired and actual paths. To make the robot's actual path converge to th e desired path, we must introduce a feedback control. Let us use th e feedback signal of th e actual path and apply the computed torque control law (15.34) to th e robot. Substituting the control law in th e equations of motion (15.33), gives us (15.38) This is a linear differential equation for the error variable between the actual and desired outputs. If the n x n gain matrices k D and k p are assumed to be diagonal, then we may rewrite the error equation in a matrix form
(15.39) The linear differential equation (15.39) is asymptotically st able when all th e eigenvalues of [A] have negative real part . The matrix k p has the role of natural frequency, and k D acts as damping.
kp
kD =
~ ~'
[ w'
[ 2,~w, 0
0
a a
a a a a a 0
2 w2
0 2~2W2
0 2
wn
a a 0
(15.40)
0
a
0
0
a
]
2~n wn
]
(15.41)
15. Cont rol Techniques
649
Since k D and k p are diagonal, we can adjust the gain matrices k D and k p to cont rol the response speed of the robot at each joint independently. A simple choice for the matrices is to set ~i = 0, i = 1,2 , ... ,n, and make each joint response equal to the response of a critically damped linear second order syst em with natural frequency Wi . The computed torque cont rol law (15.34) has two components as shown below.
Q=
D(q)
"
+
D(q)(-kDe-kpe)
.f'
V'
v
Q ff
(15.42)
.f
Qfb
The first term, Qff , is the f eedforward command, which is the required torques based on open-loop cont rol law. When there is no error, the cont rol input Qff makes the robot follow the desired path qd. The second term , Qfb , is the f eedback command, which is the correct ion torques to redu ce the err ors in t he path of the robot. Computed torque cont rol is also called f eedback lin earizat ion , which is an applied techniqu e for robots' nonlinear cont rol design. To apply the feedb ack linearization technique, we develop a control law to eliminate all nonlinearities and redu ce the problem to the linear second-order equat ion of error signal (15.38) •
Example 330 Comp ut ed for ce con trol for an oscillator. Figure 15.2 depicts a lin ear ma ss-spring-damper oscillator under th e action of a control for ce. Th e equation of motion for the oscillator is m /i + ex
+ kx = f.
(15.43)
Applying a com puted force control law
f
m (Xd - kDe - kp e)
e
X - Xd
+ ex + k x
(15.44) (15.45)
reduces th e error differenti al equati on to
e + kDe + kp e = O.
(15.46)
Th e soluti on of the error equati on is
e
+ B eA2t
(15.47)
-k D ± Jk'b - 4k p
(15.48)
AeA1t
where A and B are f un ctions of initial conditions, and of th e characteri stic equati on
), 1,2
are soluti ons
(15.49) Th e solutio n (15.41) is stable and e
--+
0 exponentially as t
--+ 00
if ko > O.
650
15. Control Techniques
o FIGURE 15.4. A controlled inverted pendulum.
Example 331 Inverted pendulum. Cons ider an inverted pendulum shown in Figure 15.4. Its equation of motion is ml 2 0- mgl sine = Q. (15.50) To control the pendulum and bring it from an initial angle e = eo to the vertical-up posit ion, we may employ a feedback control law as Q = -kDO - kpe - mgl sin e.
(15.51 )
The parameters ko and k p are positive gains and are assumed constants. The control law (15.51) transforms the dynamics of the system to 2"
.
ml e + kDe
+ kpe = 0
(15.52)
show ing that the system behaves as a stable mass-spring-damper. In case the desired position of the pendulum is at a nonzero angle, e = ed, we may em ploy a feedback control law based on the error e = e - ed as below, " Q = ml 2 ed - kDe - kpe - mglsine.
(15.53)
Substituting this control law in the equation of motion (15 .50) shows that the dynamic of the controlled system is governed by
mz2e + kDe + kpe =
O.
(15.54)
Example 332 Control of a 2R planar manipulator. A 2R planar manipulator is shown in Figure 15.5 with dynamic equations given below. =
(15.55)
15. Control Techniques
651
FIGURE 15.5. A 2R planar manipul ator with massive links.
where
D ll D2l D22
= =
Ii + m2 (li + lrr2 cos(h + r~) + 12 D 12 = m2lr r2 cos O2 + m2r~ + h m2r~ +h
(15.56)
-m2lr r 2ih sin O2 -m2lrr2(ih +B2)sin02 m2lrr2Bl sin O2 0
(15.59) (15.60) (15.61) (15.62)
mlri +
Cl l C2l C12
C22
(15.57) (15.58)
Gl = mlgrl cos 01 + m2g (lr cos 01 + r2 cos (01 + O2)) (15.63) G2 = m2gr2 cos (01 + O2) . (15.64) Let's write the equations of motion in the following form:
D (q) q + C(q, q)q + G (q) = Q
(15.65)
and multiply both sides by D - l to transform the equations of motion to (15.66) To control the manipulator to follow a desired path q = qd(t ), we apply the following control law: Q = D (q) U
+ C (q, q)q+ G (q)
(15.67)
qd - 2ke - k2 e
(15.68) (15.69)
where U
e
q
- qd·
652
15. Control Techniques
The vector U is the controller input, e is the position error, and k is a positive constant gain number. Substituting the control law into the equation of motion shows that the error vector satisfies a linear second-order ordinary differential equation (15.70) and therefore, exponentially converges to zero.
15.3 Linear Control Technique Linearization of a robot 's equations of motion about an operating point while applying a linear control algorithm is a practical robot control. This technique works well in a vicinity of the operating point. Hence, it is only a locally stable method. The linear control techniques are proportional, integral, derivative, and any combination of them. The idea is to linearize the nonlinear equations of motion about some reference operating points to make a linear syst em, design a controller for the linear system, and then, apply the control to the robot. This technique will always result in a stable controller in some neighborhood of the operating point. However, the stable neighborhood may be quite small and hard to be determined. A proportional-integral-derivative (PID) control algorithm employs a position error , derivative error, and integral error to develop a control law. Hence, a PID control law has the following general form for the input command: Q
= kpe + k j
it
edt + kDe
(15.71)
where e = q-qd is the error signal , and kp , kt , and kt: are positive constant gains associated to the proportional, integral, and derivative controllers. The control command Q is thus a sum of three terms: the P-term ,which is proportional to error e, the l-term, which is proportional to the integral of the error, and the D-term, which is proportional to the derivative of the error.
15.3.1 Proportional Control In the case of proportional control , the PID control law (15.71) reduces to (15.72) The variable Qd is the desired control command, which is called a bias or reset factor. When the error signal is zero, the control command is equal to the desired value. The proportional control has a drawback that results in a constant error at st eady state condition.
15. Control Techniques
15.3.2
653
Integral Control
The main function of an integral control is to eliminate the steady state error and make the system follow the set point at steady state conditions. The integral controller leads to an increasing control command for a positive error, and a decreasing control command for a negative error. An integral controller is usually used with a proportional controller. The control law for a PI controller is (15.73)
15.3.3
Derivative Control
The purpose of derivative control is to improve the closed-loop stability of a system. A derivative controller has a predicting action by extrapolating the error using a tangent to the error curve. A derivative controller is usually used with a proportional controller. The PD control law is Q
= kpe+kDe.
(15.74)
Proof. Any linear system behaves linearly if it is sufficiently near a reference operating point. Consider a nonlinear system
q=
f'(q, Q)
(15.75)
where qd is a solution generated by a specific input Qc (15.76)
Assume 8q is a small change from the reference point qd because of a small change 8Q from Q. q
(15.77)
Q
(15.78)
If the changes 8q and 8Q are assumed small for all times, then the equation (15.75) can be approximated by its Taylor expansion and q be the solution of . Br Bf (15.79) q = Bqd q + BQd Q. The partial derivative matrices
[::d] and
[8(~L]
are evaluated at the ref-
erence point (qd ' Qd)' • Example 333 Linear control for a pendulum. Figure 11.12 illustrates a controlled pendulum as a one-arm manipulator. The equation of motion for the arm is
Q=
Ie + cO + mgl sin ()
(15.80)
654
15. Control Techniques
where I is the arm 's moment of inertia about the pivot joint and m is the mass of the arm. The joint has a viscous damping c and kinematic length, the distance between the pivot and C'M, is l. Introducing a new set of variables
e
(15.81)
it
(15.82)
converts the equation of motion to
(15.83)
X2
Q-
mgl sinxl
CX2 -
I
(15.84)
The linearized form of these equations is
(15.85) Assume that the reference point is [
~~
] = [
62 ]
1r
(15.86) (15.87)
mgl.
The coefficient matrices in Equation (15.85) must then be evaluated at the reference point. We use a set of sample data
m
=
1kg
l
0.35m
I
0.07kg. m 2
c
0.01 N s] m
(15.88)
and find
::d
=
of
[-4~.05 -0~01
oQc =
]
(15.89)
[00 14.286 0 ].
(15.90)
Now we have a linear system and we may apply any control law that applies to linear systems. For instance, a PID control law
Q = Qc - kDe - kpe + k[ where e=q-qd=
Xl [
1r
I
t
/2 ]
X2
can control the arm around the reference point.
edt
(15.91)
(15.92)
15. Control Techniques
655
Example 334 PD control. Let us define a PD control law as
Q
-kDe - kj-e
(15.93)
e
q-qd
(15.94)
Applying the PD control to a robot with dynamic equations as
Q
D(q) q + H(q, q) + G(q) D(q) q + C(q, q)q + G(q)
(15.95)
will produce the follo wing control equation:
D(q) q + C(q, q)q + G(q)
+ k D (q-
qd) - k p (q - qd) = O.
(15.96)
This control is ideal when qd is a constant vector associat ed with a specific configuration of a robot, and therefore qd = O. In this case the PD controller can make the configuration qd globally stable. In case of a path given by q = qd(t) , we define a modified PD controll er in the form
Q = D(q)qd + C(q, q)qd + G(q) - kDe -
kj-e
(15.97)
and reduce the closed-loop equation to
D(q)e + (C(q, q) + k D ) e + kj-e = O. The linearization of this equation about a control point q vides a stable dynamics for the error signal
(15.98)
= qd = cte pro(15.99)
15.4 Sensing and Control Position, velocity, acceleration, and force sensors are th e most common sensors used in robotics. Consider the inverted pendulum shown in Figur e 15.6 as a one DOF manipulator with the following equat ion of motion : ml 2 (j - cO - mgl sin () = Q.
(15.100)
From an open-loop control viewpoint , we need to provide a moment Qc(t) to force the manipulator to follow a desired path of motion (}d(t) where (15.101) In robotics, we usually calculate Qc from th e dynamics equation and dictate it to th e actuator.
656
15. Control Techniques
o FIGURE 15.6. An inverted pendulum.
The manipulator will respond to the applied moment and will move. The equation of motion (15.100) is a model of the actual manipulator. In other words, we want the manipulator to work based on this equation. However, there are so many unmodeled phenomena that we cannot include them in our equation of motion, or we cannot model them. Some are temperature, air pressure, exact gravitational acceleration, or even the physical parameters such as m and l that we think have good accuracy. So, applying a control command Qc will move the manipulator and provide a real value for e, e, and B, which are not necessarily equal to ed, ed, and Bd. Sensing angular is now important because we need to measure the actual angle velocity B, and angular acceleration to compare them with ed, Bd' and Bd and make sure that the manipulator is following the desired path. This is the reason why the feedback control systems and the error signal e = e- ed were introduced. Robots are supposed to do a job in an environment, so they can interact with the environment. Therefore, a robot needs two types of sensors : l-sensing the robot's internal parameters, which are called proprioceptors , and 2-sensing the robot's environment al parameters, which are called exterocepiors. The most important interior parameters are position, velocity, acceleration, force, torque, and inertia.
e
e,
15.4.1 Position Sensors. Rotary encoders. In robotics, almost all kinds of actuators provide a rotary motion. Then we may provide a rotation motion for a revolute joint, or a translation motion for a prismatic joint, by using gears . So, it is ideally possible to sense the relative position of the links connect ed by the joint based on the angular position of the actuator. The error in this position sensing is due to non-rigidity and backlash. The most common position sensor is a rotary encoder that can be optical, magnetic, or electrical. As an example, when the encoder shaft rotates, a disk counting a pattern of
15. Control Techniques
657
fine lines interrupts a light beam. A photo detector converts the light pulses into a countable binary waveform . The shaft angle is then determined by counting the number of pulses. Resolvers. We may design an electronic device to provide a mathematical function of the joint variable. Th e mathematic al function might be sine, cosine, exponent ial, or any combinat ion of mathemati cal functions . The joint variable is then calculat ed indirectly by resolving th e mathematical functi ons. Sine and cosine functions are more common. Potentiometers. Using an electrical bridge, the pot entiom eters can provide an electric voltage proportional to th e joint position . LVDT and RVDT. LVDT/RVDT or a Linear /Rotary Variable Differential Transformer operates with two transformers sharing the same magnetic core. When the core moves, th e output of one transformer increases while the other's output decreases. Th e difference of the curr ent is a measure of t he core position.
15.4.2 Speed Sensors. Tachometers. Generally speaking, a t achometer is a name for any velocity sensor. Tachometers usually provide an analog signal proportional to the angul ar velocity of a shaft . There are a vast amount of different designs for t achometers , using different physical characterist ics such as magnetic field. Rotary encoders. Any rot ary sensor can be equipped with a tim e measuring system and become an angular velocity sensor. The encoder counts the light pulses of a rot ating disk and the angular velocity is th en determined by time between pulses. Differentiating devices. Any kind of position sensor can be equipped with a digital differenti ating device to become a speed sensor. Th e digital or numerical differentiating needs a simple processor. Numerical differentiating is generally an erroneous process. Integrating devices. Th e output signal of an accelerometer can be num erically integrat ed to provide a velocity signal. Th e digital or numerical integrating also needs a simple processor. Numerical differentiat ing is genera lly a smooth and reliable pro cess.
15.4.3 Acceleration Sensors. Acceleration sensors work based on Newton 's second law of motion . Th ey sense the force t hat causes an acceleration of a known mass. Th ere are many types of accelerometers. Str ess-strain gage, capacitive, inducti ve, piezoelectric, and micro-accelerometers are t he most common. In any of th ese types, force causes a propor tion al displacement in an elast ic mat erial, such as deflection in a micro-cantilever beam, and th e displacement is proportional to the acceleration.
658
15. Control Techniques
Applications of accelerometers include measurement of acceleration, angular acceleration, velocity, position, angular velocity, frequency, impulse, force, tilt, and orientation. Force and Torque Sensors. Any concept and method that we use in sensing acceleration may also be used in force and torque sensing. We equip the wrists of a robot with at least three force sensors to measure the contact forces and moments with the environment. The wrist's force sensors are important especially when the robot's job is involved with touching unknown surfaces and objects. Proximity Sensors. Proximity sensors are utilized to detect the existence of an object, field, or special material before interacting with it. Inductive, capacitive, Hall effect, sonic, ultrasonic, and optical are the most common proximity sensors. The inductive sensors can sense the existence of a metallic object due to a change in inductance. The capacitive sensors can sense the existence of gas, liquid, or metals that cause a change in capacitance. Hall effective sensors work based on the interaction between the voltage in a semiconductor material and magnetic fields. These sensors can detect the existence of magnetic fields and materials. Sonic, ultrasonic, and optical sensors work based on the reflection or modification in an emitted signal by objects.
15.5 Summary In an open-loop control algorithm, we calculate the robot's required torque commands Qc for a given joint path qd = q(t) based on the equations of motion (15.102)
However, there can be a difference between the actual joint variables and the desired values. The difference is called error e e
(15.103)
e
(15.104)
By measuring the error command, we may define a control law and calculate a new control command vector (15.105)
to compensate for the error. The parameters k p and k D are constant gain diagonal matrices. The control law compares the actual joint variables (q, q) with the desired values (qd ' qd), and generates a command proportionally. Applying the new control command changes the dynamic equations of the robot to Q c + kDe
+ kj-e = D(q) q + H(q, q) + G(q).
(15.106)
15. Control Techniques
659
This is a closed-loop control algorithm, in which the control commands are calculated based on the difference between act ual and desired variables. Computed torqu e control
Q = D(q) (qd - kDe - kj-e)
+ H(q, q) + G(q)
(15.107)
is an applied closed-loop control law in roboti cs to make a robot follow a desired path.
15. Control Techniques
661
Exercises 1. Response of second-order systems.
Solve the characte ristic equat ions and determine the response of the following second-order systems at x(l) , if they st art from x(O) = 1, X(O) = O.
(a)
x + 2x + 5x =
0
(b) x + 2x + x = 0
(c) x + 4x + x = O 2. Modified PD control. Apply a modified PD control law
e
x - Xd
to a second-ord er linear system
m x+cx+ kx =
f
and reduce the system to a second-order equat ion in an error signal.
Then , calculate the steady state error for a step input
x = Xd = cte . 3. Modified PID control. Apply a modified PD control law
f
-kpe - kdX - k j
e
x - Xd
to a second-order linear syst em
mx+ cx+kx = f
it
edt
662
15. Control Techniques
and reduce the system to a third-order equation in an error signal.
Then, find the PID gains such that the characteristic equation of the system simplifies to
4. Linearization. Linearize the given equations and determine the stability of the linearized set of equations. =
2
+ Xl COSX2 X2 + (1 + Xl + X2)Xl + Xl sin z-, X2
5. Expand the control equations for a 2R planar manipulator using the following control law:
6. One-link manipulator control. A one-link manipulator is shown in Figure 15.6. (a) Derive the equation of motion. (b) Determine a rest-to-rest joint path between 8(0) = 45 deg and 8(0) = -45 deg. (c) Solve the time optimal control of the manipulator and determine the torque Qc(t) for m
=
l
IQI <
1kg 1m 120Nm.
(d) Now assume the mass is m = 1.01kg and solve the equation of motion numerically by feeding the calculated torques Qc(t). Determine the position and velocity errors at the end of the motion. (e) Design a computed torque control law to compensate the error during the motion . 7.
* Mass-spring control. Solve Exercise 14.9 and calculate the optimal control input. Increase the stiffness %10, and design a computed torque control law to eliminate error during the motion.
15. Control Techniques
8.
663
* 2R manipulator control. (a) Solve Exercise 13.16 and calculate the optimal control inputs. (b) Increase the masses by 10%, and solve the dynamic equations numerically. (c) Determine the position and velocity error in Cartesian and joint spaces by applying the calculated optimal inputs. (d) Design a computed torque control law to eliminate error during the motion .
9.
* PR planar manipulator control. (a) Solve Exercise 14.12 and calculate the optimal control inputs. (b) Increase the gravitational acceleration by 10%, and solve the dynamic equations numerically. (c) Determine the posit ion and velocity error in Cartesian and joint spaces by applying the calculated optimal inputs. (d) Design a computed torque control law to eliminate error during the motion.
10. Sensing and measurement. Consider the one DOF manipulator in Figure (15.100). To control the manipulator, we need to sense the actual angle B, angular velocity 8, and angular acceleration Band compare them with Bd , 8d , and Bd to make sure that the manipulator is following the desired path. Can we measure the actual moment Q, that the actuator is providing, and compare with the predicted value Qc instead? Does making Q equal to Q c guarantee that the manipulator does what it is supposed to do?
References Chapter 1 Asimov , 1., 1950, I, Robot, Doubleday & Comp any, Inc. , New York. Aspragathos, N. A., and Dimitros, J . K., 1998, A comparative study of three methods for robot kinematics, IEEE Transaction on Systems, Man and Cybernetic-PART B: CYBERNETICS, 28(2), 115-145. Chernousko, F. L., Bolotnik, N. N., and Gradetsky, V. G., 1994, Manipulation Robots: Dynamics, Control, and Optimization , CRC Press, Boca Raton, Florida. Denavit, J. , and Hartenberg, R S., 1955, A kinematic not ation for lowerpair mechanisms based on matrices, Journal of Appli ed Mechanics, 22(2), 215-221. Dugas, R , 1995, A History of Mechanics (English translation) , Switzerland, Editions du Griffon, Central Book Co., New York. Erdman, A. G., 1993, Modern Kinematics: Developed in the Last Forty Years, John Wiley & Sons, New York. Hunt, K. H., 1978, Kinematic Geometry of Mechanisms , Oxford University Press, London. Milne, E. A., 1948, Vectorial Mechanics, Methuen & Co. LTD. , London. Niku, S. B., 2001, Introduction to Robotics: Analysis, Systems, Applications, Prentice Hall, New Jersey. Rosheim , M. E., 1994, Robot Evolution: The Development of Anthrobotics, John Wiley & Sons, New York. Tsai, L. W ., 1999, Robot Analysis, John Wiley & Sons, New York. Veit, S., 1992, Whatever happened to ... personal robots?, The Computer Shopper, 12(11), 794-795.
Chapter 2 Buss , S. R , 2003, 3-D Computer Graphics: A Math emat ical Introdu ction with OpenGL , Cambridge University Press, New York. Cheng, H., and Gupta, K. C., 1989, A historical note on finit e rotations, Journal of Applied Mechanics , 56, 139-145. Coe, C. J. , 1934, Displacement of a rigid body, American Mathematical Monthly, 41 (4), 242-253. Denavit, J. , and Hartenberg, R S., 1955, A kinematic notation for lowerpair mechanisms based on matrices, Journal of Applied Mechanics, 22(2), 215-221. Hunt, K. H., 1978, Kin ematic Geometry of Mechanisms , Oxford University Press, London.
665 References Mason , M. T., 2001, Mechanics of Robotic Manipulation, MIT Press , Cambridge, MA. Murray, R M., Li, Z., and Sastry, S. S. S., 1994, A Mathematical Introduction to Robotic Manipulation, CRC Press , Boca Raton, Florida. Nikravesh, P., 1988, Computer-Aided Analysis of Mechanical Systems, Prentice Hall, New Jersey. Niku, S. B., 2001, Introduction to Robotics: Analysis, Systems, Applications, Prentice Hall, New Jersey. Paul, B., 1963, On the composition of finite rotations, American Mathematical Monthly, 70(8), 859-862. Paul, R P., 1981, Robot Manipulators: Mathematics, Programming, and Control, MIT Press, Cambridge, Massachusetts. Rimrott, F . P. J ., 1989, Introductory Attitude Dynamics, Springer-Verlag, New York. Rosenberg , R., M. 1977, Analytical Dynamics of Discrete Systems, Plenum Publishing Co., New York. Schaub, R., and Junkins, J . L., 2003, Analytical Mechanics of Space Systems, AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc., Reston, Virginia. Suh, C. R., and Radcliff, C. W., 1978, Kinematics and Mechanisms Design, John Wiley & Sons, New York. Spong, M. W., Hutchinson, S., and Vidyasagar, M., 2006, Robot Modeling and Control , John Wiley & Sons, New York. Tsai , L. W., 1999, Robot Analysis, John Wiley & Sons, New York.
Chapter 3 Buss, S. R, 2003, 3-D Computer Graphics: A Mathematical Introduction with OpenGL , Cambridge University Press, New York. Denavit, J ., and Hartenberg, R S., 1955, A kinematic notation for lowerpair mechanisms based on matrices, Journal of Applied Mechanics, 22(2), 215-221. Hunt , K. H., 1978, Kinematic Geometry of Mechanisms, Oxford University Press, London U.K. Mason , M. T ., 2001, Mechanics of Robotic Manipulation, MIT Press , Cambridge, Massachusetts. Murray, R M., Li, Z., and Sastry, S. S. S., 1994, A Mathematical Introduction to Robotic Manipulation, CRC Press , Boca Raton, Florida. Nikravesh, P., 1988, Computer-Aided Analysis of Mechanical Systems, Prentice Hall, New Jersey. Paul, B., 1963, On the composition of finite rotations, American Mathematical Monthly, 70(8), 859-862. Paul, R P., 1981, Robot Manipulators: Mathematics, Programming, and Control , MIT Press , Cambridge, Massachusetts. Rimrott, F . P. J ., 1989, Introductory Attitude Dynamics, Springer-Verlag , New York.
References 666 Rosenberg , R , M. 1977, Analytical Dynamics of Discret e Systems, Plenum Publishing Co., New York. Schaub, H., and Junkins, J . L., 2003, Analytical Mechanics of Space Sys tems, AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc., Reston , Virginia. Spong , M. W., Hutchinson, S., and Vidyasagar, M., 2006, Robot Modeling and Control, John Wiley & Sons, New York. Suh, C. H., and Radcliff, C. W., 1978, Kinematics and Mechanisms Design, John Wiley & Sons, New York. Tsai , L. W., 1999, Robot Analysis, John Wiley & Sons, New York. Wittenburg, J. , and Lilov, L., 2003, Decomposition of a finite rotation into three rotations about given axes, Multibody System Dynamics, 9, 353375.
Chapter 4 Ball, R S., 1900, A Treatise on the Theory of Screws, Cambridge University Press , USA. Bottema, 0. , and Roth, B., 1979, Theoret ical Kinematics, North-Holland Publication, Amsterdam , Th e Netherlands. Chernousko, F. L., Bolotnik, N. N., and Gradetsky, V. G., 1994, Manipulation Robots: Dynamics , Control, and Optimization, CRC press, Boca Raton, Florida. Davidson , J . K., and Hunt , K. H., 2004, Robots and Screw Theory: Applications of Kinematics and Statics to Robotics, Oxford University Press, New York. Denavit, J ., and Hartenberg, R S., 1955, A kinematic notation for lowerpair mechanisms based on matrices, Journal of Applied Mechanics, 22(2), 215-221.
Hunt, K. H., 1978, Kinematic Geometry of Mechanisms, Oxford University Press, London . Mason, M. T ., 2001, Mechanics of Robotic Manipulation , MIT Press, Cambridge, Massachusetts. Murray, R M., Li, Z., and Sastry, S. S. S., 1994, A Mathematical Introduction to Robotic Man ipulation, CRC Press , Boca Raton, Florida. Niku, S. B., 2001, Introduction to Robotics: Analysis, Systems, Applications, Prentice Hall, New Jersey. Plucker, J. , 1866, Fundamental views regarding mechanics, Philosophical Transactions, 156, 361-380. Selig, J . M., 2005, Geometric Fundamentals of Robotics, 2nd ed., Springer, New York. Schaub , H., and Junkins, J . L., 2003, Analytical Mechanics of Space Systems , AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc., Reston, Virginia. Schilling, R J ., 1990, Fundamentals of Robotics: Analysis and Control , Prentice Hall, New Jersey.
667 References
Suh, C. H., and Radcliff, C. W., 1978, K in ematics and M echanisms Design, John Wiley & Sons, New York. Spong , M. W., Hutchinson, S., and Vidyasagar , M., 2006 , Robot Modeling and Con trol, John Wiley & Sons, New York.
Chapter 5 Asad a, H., and Slotine, J . .1 . E ., 1986, Robot Analysis and Control, John Wiley & Son, New York. Ball, R. S., 1900, A Treatise on the Th eory of Sc rews, Cambridge University Press, USA. Bernhardt , R. , and Albright , S. L., 2001, Robot Calibrati on, Sprin ger, New York. Bottema, 0 ., and Roth, B., 1979, Theoretical Kinema ti cs, North-Holland Publication , Amsterdam , The Net herlands. Davidson, J. K. , and Hunt, K. H., 2004, Robots and Sc rew Th eory : Applications of Kin ematics and Statics to Robotics, Oxford University Press, New York. Denavit , .1 ., and Har tenberg, R. S., 1955, A kinematic not ation for lowerpair mechanisms based on matri ces, Journal of Applied Mechan ics, 22(2), 215-221. Hunt , K. H., 1978, K in em atic Geometry of Mechanisms, Oxford University Press, London . Mason, M. T ., 2001 , M echanics of Robotic Man ipulation , MIT Press, Camb ridge, Massachusetts. Paul , R. P., 1981, Robot Man ipulators: Math em atics, Programm ing, and Control, MIT Press, Cambridge, Massachusetts. Schilling, R. .1 ., 1990, Fundamentals of Robotics: Analysis and Con trol; Prentice Hall, New J ersey. Schroe r, K. , Albright , S. L., and Grethlein, M., 1997, Complete, minimal and model- continuous kinematic models for robot calibrat ion, Rob. Com p.Integr. Manufa ct., 13(1) , 73-85. Spong, M. W., Hut chinson, S., and Vidyasagar , M., 2006 , Robot Modeling and Control, John Wiley & Sons, New York. Suh , C. H., and Radcliff, C. VV ., 1978, K inematics and M echanism s Design, John Wiley & Sons, New York. Ts ai, 1. W., 1999, Robot Analysis, John Wiley & Sons, New York. Wang, K. , and Lien, T ., 1988, St ruct ure, design & kinematics of robot manipulato rs, Robotica, 6, 299-306. Zhuang, n., Roth, Z. S., and Hamano , F., 1992, A complete , minimal and model-continuous kinematic model for robot manipulator s, IEEE Trans . Rob. A ut om ation , 8 (4), 451-463.
Chapter 6 Asada, H., and Slotine, .1 . .1. E., 1986, R obot Analysis an d Control, John Wiley & Sons, New York.
References 668 Paul , R. P., 1981, Robot Manipulators : Math ematics, Programming, and Control , MIT P ress, Cambridge, Massachusetts. Spong, M. W., Hut chinson, S., and Vidyasagar, M., 2006, Robot Modeling and Control , John Wiley & Sons, New York. Tsai , L. W., 1999, Robot Analysis, John Wiley & Sons, New York. Wang, K , and Lien, T ., 1988, Structure, design and kinematics of robot manipulators, Robotica, 6, 299-306.
Chapter 7 Bottema , 0. , and Roth , B., 1979, Theoretical Kinematics, North-Holland Publications, Amsterdam, The Netherlands. Geradin, M., and Cardonna, A. , Kinematics and Dynamics of Rigid and Flexible Mechanisms Using Finit e Elements and Quaternion A lgebra, Comp utational Mechanics, 1987. Hunt, K H., 1978, Kin ematic Geom etry of Mechanisms, Oxford University Press, London. Mason , M. T. , 2001, Mechanics of Robotic Manipulation , MIT Press , Cambridge, Massachusetts. Schaub, H., and J unkins, J . L., 2003, Analytical Mechanics of Space Sys tems, AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc., Reston , Virginia . Spong , M. W., Hutchinson, S., and Vidyasagar , M., 2006, Robot Modeling and Control , John Wiley & Sons, New York. Suh, C. H., and Radcliff, C. W., 1978, Kin ematics and Mechanisms Design , John Wiley & Sons, New York. Tsai , L. W., 1999, Ro bot Analysis , Jo hn Wiley & Sons, New York.
Chapter 8 Hunt, K H., 1978, Kinematic Geometry of Mechanisms, Oxford University Press , London. Kane , T. R. , Likins, P. W., and Levinson, D. A., 1983, Spacecraft Dynamics, McGraw-Hill, New York. Kane , T . R. , and Levinson, D. A., 1980, Dynamics: Theory and App lications, McGraw-Hill, New York. Mason, M. T ., 2001, Mechanics of Robotic Manipulation , MIT Press , Camb ridge, Massachusetts. Rimrott , F . P. J ., 1989, Introductory Attitude Dynamics, Springer-Verlag, New York. Schilling, R. J ., 1990, Fundam entals of Roboti cs: Analysis and Control , Prent ice-Hall, New Jers ey. Spong , M. W., Hutchinson, S., and Vidyasagar, M., 2006, Robot Modeling and Control, John Wiley & Sons, New York. Suh , C. H., and Radcliff, C. W., 1978, K inematics and Mechan isms Design , John Wiley & Sons, New York.
669 References Talman , R., 2000, Geom etri c Mechan ics, John Wiley & Sons, New York. Tsai, L. W., 1999, Robot Analysis, John Wiley & Sons, New York.
Chapter 9 Carnahan , B., Luther, H. A., and Wilkes, J . 0. , 1969, Applied Nu merical M ethods , John Wiley & Sons, New York. Eich-Soellner, E., and Fuhrer, C ., 1998, Numerical M ethods in Multibody D yn am ics, B.G. Teubn er Stu ttgart . Gerald, C. F., and Wheatl ey, P.O., 1999, Applied Num erical Analysis, 6th ed., Addison Wesley, New York. Nikravesh, P., 1988, Computer-A ided A naly sis of M echani cal Sy st ems , Pr enti ce Hall, New Jersey.
Chapter 10 Mason, M. T ., 2001, M echan ics of Roboti c Manipulation , MIT Press, Cambridge, Massachusetts. Nikravesh, P., 1988, Computer-A ided Analysis of M echanical System s, P rentice Hall, New Jer sey. Rimrot t , F. P. .1., 1989, Introductory Attitude Dynamics, Springer-Verlag, New York. Spong, M. W., Hut chinson, S., and Vidyasagar , M., 2006, Robot Modeling and Control, John Wiley & Sons, New York. Suh, C. H., and Radcliff, C. W., 1978, Kinem ati cs and M echanism s Design, John Wiley & Sons, New York. Tsai, L. W., 1999, Robot Analysis, John Wiley & Sons, New York.
Chapter 11 Goldstein, H., Poole, C ., and Safko, .1., 2002, Classical M echanics, 3rd ed., Addison Wesley, New York. MacMillan, W. D., 1936, Dynam ics of R igid Bodies, McGraw-Hill, New York. Meirovitch, L., 1970, M ethods of Analytical Dynamics, McGraw-Hill, New York. Nikravesh, P., 1988, Computer -Aided Analysis of M echanical Sy stems, Pr enti ce Hall, New Jersey. Rimrott, F. P. .1 ., 1989, Introductory Attitud e Dynamics, Springer-Verlag, New York. Rosenberg, R. M., 1977, A nalytical Dynamics of Discrete System s, Plenum Publishing Co., New York. Schaub , H., and Junkins, J. L., 2003, Analytical M echan ics of Space Sy stems, AlAA Education al Series, American Institute of Aeronauti cs and Astro nautics, Inc., Reston , Virginia. Thom son, W. T., 1961, Introduction to Space Dyn am ics, John Wiley & Sons. New York.
References 670 Tsai , L. W., 1999, Robot Analysis, John Wiley & Sons, New York. Wittacker, E. T., 1947, A Treatise on the A nalytical Dynamics of Particles and Rigid Bodies, 4th ed. , Cambridge University Press, New York.
Chapter 12 Brady, M., Hollerbach, J . M., Johnson, T . L., Lozano-Prez, T ., and Mason, M. T ., 1983, Robot Mot ion : Planning and Control, MIT Press, Cambridge, Massachusetts. Murray, R M., Li, Z., and Sastry, S. S. S., 1994, A Math ematical Introduction to Robotic Manipulation, CRC Press, Boca Raton, Florid a. Nikravesh, P., 1988, Computer-A ided Analysis of Mechanical Systems, Prentice Hall, New Jersey. Niku, S. B., 2001, Introduction to Robotics: Analysis, Systems, Applications, Prentice Hall, New Jersey. Paul, R P., 1981, Robot Man ipulators: Math emat ics, Programming, and Control , MIT Press, Cambridge, MA. Spong, M. W., Hutchinson, S., and Vidyasagar , M., 2006, Robot Modeling and Control , John Wiley & Sons, New York. Suh , C. H., and Radcliff, C. W., 1978, K in ematics and Mechanisms Design, John Wiley & Sons, New York. Tsai, L. W., 1999, Robot Analysis, John Wiley & Sons, New York.
Chapter 13 Asad a, H., and Slotine, J ..J. E. , 1986, Robot Analysis and Control, John Wiley & Sons, New York. Murray, R M., Li, Z., and Sastry, S. S. S., 1994, A Math ematical Int roducti on to Robotic Man ipulat ion , CRC Press, Boca Raton, Florid a. Niku, S. B., 2001, Introduction to Robotics: Analysis, Syst ems, Applications , Prentice Hall, New Jersey. Spong, M. W., Hutchinson, S., and Vidyasagar , M., 2006, Robot Modeling and Cont rol, John Wiley & Sons, New York.
Chapter 14 Ailon, A., and Langholz, G., 1985, On th e existence of tim e optimal control of mechanical manipulators, Journal of Optimization Th eory and Applications , 46(1), 1-21. Bahr ami M., and Nakhai e Jazar G., 1991, Optimal control of roboti c manipulators: optimi zation algorithm, Amirkabir Journal, 5(18), (in Persian) . Bahr ami M., and Nakhaie Jazar G., 1992, Optimal control of robotic manipulators: application, Amirkabir Journal, 5(19), (in Persian) . Bassein, R , 1989, An Optimization Problem, American Math ematical Monthly, 96(8), 721-725.
671 References Bobrow, J . E., Dobowsky, S., and Gibson, J . S., 1985, Time optimal control of robotic manipulators along specified paths , The International Journal of Robotics Research, 4 (3) , 495-499. Courant, R., and Robbins, H., 1941, What is Math ematics ?, Oxford University Press, London. Fotouhi, C. R. , and Szyszkowski W. , 1998, An algorithm for tim e optimal cont rol problems, Transaction of the ASME, Journal of Dynamic Systems, Measurements, and Control, 120, 414-418 . Fu, K S., Gonzales, R. C., and Lee, C. S. G., 1987, Robotics, Control, Sensing, Vision and Intelligence, McGraw-H ill, New York. Gamkrelidze, R. V., 1958, Th e theory of tim e optimal processes in linear syst ems , Izvestiya Akademii Nauk SSSR, 22, 449-474. Garg, D. P., 1990, The new time opt imal motion control of robotic manip ulators, The Franklin Institute, 327(5) , 785-804. Hart P. E. , Nilson N. J ., and Raphael B., 1968, A forma l basis for heuristic det ermination of minimum cost pat h, IEEE Transaction Man, System 8 Cybern etics, 4, 100-107. Kahn, M. E., and Roth B., 1971, The near minimum time control of open loop articulated kinematic chain, Transaction of the ASME, Journ al of Dynamic Syst ems , Measurements, and Contro l, 93, 164-172. Kim B. K, and Shin KG ., 1985, Suboptimal control of industrial manipulators with weighted minimum time-fuel criterion, IEEE Transactions on Automatic Control , 30(1), 1-10. Krotov, V. F ., 1996, Global Methods in Optimal Cont rol Theory , Marcel Decker, Inc. Kuo , B. C., and Golnaraghi, F ., 2003, Automatic Control Systems , John Wiley & Sons, New York. Lee, E . B., and Markus, L., 1961, Optimal Control for Nonlinear Processes, Archive for Rational Mechanics and Analysis, 8, 36-58. Lee, H. W . J ., Teo, K L., Rehbock, V., and Jennings, L. S., 1997, Control paramet erization enha ncing techniqu e for time optimal control prob lems, Dynamic Systems and App lications, 6, 243-262. Lewis, F . L., and Syrmos , V. L., 1995, Optimal Control , John Wiley & Sons, New York. Meier E. B., and Bryson A. E. , 1990, Efficient algorithm for time optima l control of a two-link manipulator, Journal of Guidance, Control and Dynamics, 13(5) , 859-866. Mita T ., Hyon, S. H., and Nam , T . K , 2001, Ana lytical time optimal control solution for a two link planar acrobat with initial angu lar momentum , IEEE Transactions on Robotics and Automation, 17(3) , 361-366. Nakamura, Y. , 1991, Advanced Robotics : Redundancy and Optimization, Addison Wesley, New York. Nakh aie J azar G., and Naghshinehpour A., 2005, Time opt imal control algorit hm for multi-body dynamical systems, IMechE Part K: Journal of Multi-Body Dynamics, 2 19(3), 225-236.
References 672 Pinch E. R , 1993, Optimal Control and the Calculus of Variations, Oxford Univer sity Press, New York. Pontryagin , L. S., Boltyanskii, V. G., Gamkrelidze, R V., and Mishchenko , E. F ., 1962, Th e Math ematical Th eory of Optimal Processes, John Wiley & Sons , New York. Roxin , E., 1962, The existe nce of optimal controls, Michigan Math ematical Journal, 9, 109-119. Shin K. G. , and McKay N. D., 1985, Minimum ti me cont rol of a robotic manipulator with geomet ric path const rai nts , IEEE Transaction Automatic Control , 30(6), 531-541. Shin K. G., and McKay N. D., 1986, Selection of near minimum tim e geomet ric paths for robo tic man ipulators , IEEE Transaction Automatic Control, 31(6), 501-511. Skowronski , J . M., 1986, Cont rol Dynamics of Robotic Manipulator, Academic Press Inc., U.S. Slotine, J . J . E. , and Yang, H. S., 1989, Improving the efficiency of time optimal path following algorithms, IEEE Trans. Robotics Automation, 5(1) , 118-124. Spong, M. W. , Thorp, J . S., and Kleinwaks , J ., M., 1986, The cont rol of robot manipulators with bounded input , IEEE Journ al of Automatic Control, 31 (6), 483-490. Sundar , S., and Shiller , Z., 1996, A genera lized sufficient condit ion for time optimal cont rol, Transaction of the ASME, Journal of Dynamic System s, Measurem ent s, and Cont rol, 118(2), 393-396. Takegaki , M., and Arimoto, S., 1981, A new feedb ack method for dynami c cont rol of manipulators, Transact ion of the ASME, Journal of Dynam ic Sy stems, Measurem ent s, and Control , 102, 119-125. Vincent T . L., and Grantham W . J ., 1997, Nonlin ear and Optimal Control Sy stems, John Wiley & Sons, New York.
Chapter 15 Astrom, K. J ., and Hagglund, T ., PID Controllers, 2nd ed. , 1995, Instrument Society of America, Resear ch Triangle Park, North Car olina. Fu , K. S., Gonzales, R C., and Lee, C. S. G., 1987, Robotics, Control, Sensing, Vision and Intelligence, McGraw-Hill, New York. Kuo, B. C., and Golnaraghi , F ., 2003, Automatic Cont rol Systems, John Wiley & Sons, New York . Lewis, F . L., and Syrmos, V. L., 1995, Optimal Control, John Wiley & Sons, New York. Paul, R P., 1981, Robot Man ipulators: Math ematics, Programming, and Control, MIT Press, Cambridge, Massachusetts. Spon g, M. W ., Hutchinson, S., and Vidyasagar , M., 2006, Robot Modeling and Control, John Wiley & Sons, New York.
673 References Takegaki, M., and Arimoto, S., 1981, A new feedback method for dynamic control of manipulators, Transaction of the ASME, Journal of Dynamic Systems, Measurements , and Coturo I, 102, 119-125. Vincent T . L., and Grantham W. J ., 1997, Nonlin ear and Optimal Control Systems, John Wiley & Sons, New York.
Appendix A Global Frame Triple Rotation In this appendix, the 12 combinat ions of triple rot ation about global fixed axes are presented.
- c(3sa
cac(3 C"{sa + cas(3s"( [ sas"( - caC"{s(3
=
caC"{ - sas(3s"( cas"(
s(3
- c(3s"(
+ c"(sa s(3
]
(A.1)
c(3C"{
2-QY,,,!Qz ,(3Qx,o. c(3C"{
sas"( - cac"(s(3
s(3
cac(3
cas"( + C"{sas(3 ] - c(3sa
+ cas(3s"(
caC"{ - sas(3s"(
= [
- c(3s"(
cvso
(A.2)
3-Qz,,,!Qx,(3 Qy,o. cocv - sas(3s"( cas"( + C"{sas (3 [ - c(3sa
=
- c(3s"( c(3C"{
C"{sa + cas(3s"( ] sas"( - cac"(s(3
s(3
(A .3)
cac(3
4-Q z ,,,! QY,(3 QX,o.
=
- ca s"( + C"{sas (3 cac"( + sas(3s"( c(3sa
c(3C"{ c(3s"( [ - s(3
sas"( + caC"{s(3 ] - c"(sa + cas(3s"( cac(3
(A.4)
5-QY,,,!Qx ,(3 Qz,o. caC"{
=
[
= [
+ sas(3s"( ctieo:
- cas"(
+ C"{sas(3
~~C"{s(3
sas"( - c"(sa
+ cas(3s"(
- C"{sa + cas(3s"( cac(3 sas"(
- s(3 c(3C"{ c(3s"(
+ cac"(s(3
C(3S"( ] - s(3
c(3sa ] - ca s"( + C"{sas(3 caC"{
(A.5)
c(3C"{
(A.6)
+ sas(3s"(
7-QX,,,! QY,(3Q X,o.
= [
S~"(
- c"(s(3
(A.7)
676
App endix A. Global Frame Tr iple Rotation
-sas"( + cac;3q cas;3 [ - qsa - cac;3s"(
- q s;3 cas"( + c;3qsa ] c;3 sas;3 s;3s"( caq - c;3sas"(
(A.S)
caq - c;3sa s"( - q sa - cac;3s"( s;3s"( ] = cas"( + c;3qsa - sa s"( + cac;3q - q s;3 [ sas;3 cas;3 c;3
(A.9)
=
9-Qz ,,,/QX,(3Q z ,o:
lO-Qx,"/Qz ,(3Q x ,o:
=
- cas;3 c;3 c"(s;3 -sas"( + cactic» [ s;3s"( cvso + cac;3s"(
=
cocv - c;3sa s"( sas;3 [ - ca s"( - c;3qsa
s;3s"(
sas;3 ] - ca s"( - c;3qsa cac"( - c;3sas"(
(A.I0)
cv so: + cac;3s"( ]
- cas;3 c;3 q s;3 - sa s"( + coctic»
(A.H)
I2-Q z,,,/QY,/3Q z, o:
=
- sa s"( + coctic» c"(sa + coctie» [ - cas;3
- ca s"( - c;3c"(sa caq - c;3sa s"( sas;3
q s;3 ] s;3s"(
c;3
(A.12)
Appendix B Local Frame Triple Rotation In t his appendix, t he 12 combinations of tri ple rotation about local axes are presented.
=
cBc
(B.1)
2-A y,1j,A z,o A x ,cp __ [ C_BC S'lj;B cBs'lj;
s
- c
(B.2)
- c'lj;s
(B.3)
s
(BA)
3-A z,1j,A x,oAy,cp =
c
cBs'lj; cBc'lj; - sB
4-A z,,pAy,oA x ,cp
=
dJc'lj; - cOs'ljJ [ sO
c
5-A y,,p A x,o A z,cp
+ c
c
c'ljJs
=
- CBS'ljJ] sB cBc'lj;
(B.5)
=
cOc
sO - CBS< P ] cBc'lj; c
(B.6)
7-A x ,,pAy,oA x ,cp
=
cB sBs'ljJ [ sBc'ljJ
sOs
- C
(B.7)
678
App endix B. Local Frame Triple Rot ation
=
- Si.ps'ljJ + cBci.pc'ljJ - ci.psB [ c'ljJSi.p + cBci.ps'ljJ
sBc'ljJ cB sBs'ljJ
=
Ci.pc'ljJ - cBsi.ps'ljJ - Ci.ps'ljJ - cBc'ljJsi.p [ sBsi.p
c'ljJSi.p + cBci.ps'ljJ - Si.ps'ljJ + cBci.pc'ljJ - ci.psB
ci.psB - Si.ps'ljJ + cBci.pc'ljJ - c'ljJSi.p - cBci.ps'ljJ
=
cB -sBc'ljJ [ sBs'ljJ
=
Ci.pc'ljJ - cBsi.ps'ljJ sBsi.p [ Ci.ps'ljJ + cBc'ljJsi.p
sBs'ljJ cB - sBc'ljJ
- Ci.ps'ljJ - cBc'ljJsi.p ] sBsi.p cipcsb - cBsi.ps'ljJ
SBS'ljJ ] sBc'ljJ cB
(B.8)
(B.9)
SBSi . p ] Ci.ps'ljJ + cBc'ljJsi.p cipcsb - cBsi.ps'ljJ
(B.l O)
- c'ljJSi.p - cBci.ps'ljJ ] ci.psB - Si.ps'ljJ + cBci.pc'ljJ
(B.ll)
l2-A z ,,pAy,oAz ,
- Si.ps'ljJ + cBci.pc'ljJ - c'ljJSi.p - cBci.ps'ljJ [ ci.psB
Ci.ps'ljJ + cBc'ljJsi.p cipcsb - cBsi.ps'ljJ sBsi.p
- SBC'ljJ] sBs'ljJ cB
(B.12)
Appendix C Principal Central Screws Triple Combination In this app endix , the six combinat ions of t riple principal central screws are presented.
l -s(hx ,"(, i) s(hy , 13, J) s(hz, a,
= [
cacj3 c"(sa + casj3s"( sas"( - ca qsj3
o
K)
- cj3sa caq - sas j3s"( cas"( + qsasj3
0
sj3 - cj3s"( cj3q
o
"(px + apzs j3 ] j3py q - apzcj3s"( j3py s"(\apzcj3q
(C.1) 2-s(h y , 13, J) s(h z , a, K) s(hx ,"(, i) cac j3 sa - [ - c~sj3
sj3s"( - cj3qsa c"(sj3 + cj3sa s"( apzs j3 + "(Px cacj3 ] cocv - cas"( j3py + "(Px sa cj3s"( + q sasj3 cj3q - sas j3s"( apzcj3 - "(Pxcas j3 o o 1
(C.2) 3-s(hz , a, K) s(hx, "(, i) s(hy, 13, J) _ -
r
ca c{3 - sas{3s"( ctiso: + cas j3s"( - qsj3
- q sa caq
s"(
ca s{3 + c{3sas"( "(pxca - {3pyqsa ] sas{3 - cac{3s"( "(pxsa + j3py cac"( c{3q opz + {3py s"(
000
1
(C.3)
4-s(hz , a , K) s(hy, (3, J) s(hx, "(, i) _ -
[
ca cj3 ciie« - sj3
o
cas j3s"( - q sa caq + sas j3s"( cj3s"( 0
sas"( + caqsj3 "(Pxcacj3 - j3pysa ] q sa sj3 - cas"( j3py ca + "(Pxcj3sa cj3q apz - "(Px sj3 0 1
(C.4) 5-s(hy , 13, J) s(hx, "(, i) s(hz , a, K) _ -
r
ca cj3 + sas j3s"( qsa c{3sa s"( - cas{3
o
cas j3s"( - cj3sa c"(sj3 "(Px cj3 + apzc"(sj3 ] caq - s "( j3py - apz s,,( sasj3 + ca cj3s"( c{3q apz cj3q - "(Px sj3
o
o
1
(C.5)
680
App endix C. P rincipal Cent ra l Screws Trip le Combination
6-s(h x ,"(, i ) s(hz , a, k ) s(h y , (3, J)
= [
mc(3 s(3s"( + c(3qsa c(3sas "( - qs(3
a
ms(3 ,,(P X - (3py sa ] q sa s(3 - c(3s"( (3py mq - apzs,,( cas"( c(3q + sas(3s"( apz q + py m s "( - sa
cocv
a
a
t
(C.6)
Appendix D Trigonometric Formula Definitions in terms of exponenti als.
(D.l)
cos z =
ei z _ e- i z sinz= - - - 2i ei z _ e- i z tanz = (.t Z . i e + e- t z ) iz
e
= cosz + isinz
e- i z = cos z - isin z
(D.2)
(D.3) (D.4) (D.5)
Angle sum and difference. sin(0: ± (3) = sin 0:cos (3
± cos 0:sin (3
(D.6)
cos(0: ± (3) = cos 0:cos (3 =f sin 0:sin (3
(D.7)
tan 0: ± tan (3 tan (0: ± (3) = ----....:...1 =f tan 0:tan (3
(D.8)
cot 0:cot (3 =f 1 cot (0: ± (3) = ------,,-.:....-:....cot (3 ± cot 0:
(D.9)
sin( - 0:) = - sin 0:
(D.lO)
cos(- 0:) = cos 0:
(D.ll)
tan( - 0:) = - tan 0:
(D.12)
Symmetry.
Multiple angle. . (2) sm 0: = 2' sm 0:cos 0: =
2 tan 0: 2 1 + tan 0: cos(20:) = 2 cos2 0: - 1 = 1 - 2 sirr' 0: = cos20: - sin 2 0:
(D.13) (D.14)
tan(20:) =
2 tan 0: 2 1 - tan 0:
(D.15)
cot (2) 0: =
cot2 0:- 1 2 cot 0:
(D.16)
682
Appendix D. Trigonometric Formula
sin(3a) = -4sin3a +3sina
(D.17)
cos(3a ) = 4cos 3 a - 3cosa
(D.18)
- tan" a + 3 tan a -3tan a + 1
(D.19)
tan (3) a =
---~- 2
sin(4a) = - 8 sin3 o cos o + 4sinacosa
(D.20)
cos(4a) = 8 cos'' a - 8 cos2 a + 1
(D.21)
- 4 tan' a + 4 tan a tan (4) a = ---,-------,,-tan? a - 6 tan 2 a + 1
(D.22)
sin(5a) = 16 sin'' a - 20sin" a + 5 sin a
(D.23)
cos(5a) = I fi cos'' a - 20cos 3 a + 5cosa
(D.24)
sin(na) = 2sin((n - l)a) cos a - sin((n - 2)a)
(D.25)
cos(na) = 2c os((n - l )a ) cos a - cos((n - 2)a)
(D.26)
tan((n - l )a ) + tan a tan (na ) = ----'-',..-,..-:..-.;,,..--1 - tan((n - l )a ) ta n a
(D.27)
Half angle. cos
(~) = ±) 1 + ~os a
(D.28)
. (a) '2 = ± )1 - 2cos a
sin
(a) =
tan 2
1 - cos sin o
a= sin o'
sin o = ± 1 + cos a 2tan Q = 2 1 + tan 2 -I
cos a =
1 - ta rr'
1 + tan
2
1 - cos a 1 + cos a
Q 2 Q
(D.29) (D.30)
(D.31) (D.32)
2
Powers of funct ions. . 1 sm2 a = 2 (1 - cos(2a))
sin a cos a = cos2 a = sirr' a =
~ sin(2a)
1
2 (1 + cos(2a))
~ (3s in(a) -
sin(3a))
(D.33) (D .34)
(D.35) (D.36)
Appendix D. Trigonometric Formula
sm2acosa= o
41 (cosa-3cos(3a))
(Do37)
~ (sin a + sin(3a))
(Do38)
sin a cos2 a = cos'' a = o
sin" a =
1
4 (cos(3a) + 3 cos a))
1
8 (3 -
sin3 o cos o =
4cos(2a) + cos(4a))
1
8 (2sin(2a) -
sm2 acos 2 a = o
1
8 (1 -
sin(4a))
cos(4a))
(Do39) (D.40) (D.41) (D.42)
~ (2 sin(2a) + sin(4a))
(D.43)
81 (3 + 4 cos(2a) + cos(4a))
(D.44)
sin a cos" a = cos" a =
683
sin'' a = 11 (10sin a - Ssin(3a) + sin(Sa)) 6 1 sin" o cos o = 16 (2 cos a - 3cos(3a) + cos(Sa)) o
(Do4S) (D.46)
sin3 acos 2 a = 11 (2sina+sin(3a)-sin(Sa)) 6
(D.47)
sirr' o cos'' a = 11 (2 cosa - 3cos(3a) - Scos(Sa)) 6 1 sin a cos" a = 16 (2 sin a + 3 sin(3a) + sin(5a))
(D.48)
1 cos'' a = 16 (Iu cos o + 5 cos(3a) + cos(Sa))
tarr' a = 1 - cos(2a) 1 + cos(2a)
(Do49) (DoSO) (DoS1)
Products of sin and cos. cos a cos J3 = o
1
1
1
1
1
1
"2 cos(a - J3 ) + "2 cos(a + J3 )
(Do52)
sm o sm J3 =
"2 cos(a - J3 ) - "2 cosfo + J3 )
sin a cos J3 =
"2 sin(a - J3 ) + "2 sin(a + J3 )
(Do54)
cos a sin J3 =
~ sin(a + J3 ) - ~ sin(a - J3 )
(DoSS)
0
sin(a + J3 ) sin(a - J3 ) = cos2 J3 - cos2 a = sin2 a - sin 2 J3
(DoS3)
(DoS6)
684
App endix D. Trigonometric Formul a
cosfo
+ 13) cosfo
-
13) = cos2 13 + sin 2 a
(D.57)
Sum of functions . sin a
.
± sin . 13 = 2 sm ' a ±fJ a ±fJ - 2 - cos -2-
cos a
+ cos 13 = 2 cos - 2 - cos -2-
(D.59)
· a +fJ . a- fJ cos a -cos 13 = - 2 s m - - s m - 2 2
(D.60)
a+ fJ
tan o ± t an 13 =
a- fJ
sin( a ± 13) 13 cos a cos
(D.58)
(D.61)
sin(fJ ± a )
= sm . o sm . 13
(D.62)
sin a + sin 13 _ tan ~ sin a - sin 13 - t an 0:- +13
(D.63)
sin o + sin 13 -a + 13 = cot - - cos a - cos 13 2
(D.64)
cot a ± cot 13
2
---~-
sin o + sin 13 a + 13 -----'-:- = t an - cos a + cos 13 2 a - 13 sin o - sin 13 -----'--::-=ta n - cos a + cos 13 2
(D.65) (D.66)
Trigonometric relations. sin 2 a - sin 2 13 = sin(a + 13) sin(a - 13) cos2 a - cos 2 13 = - sin( a
+ 13) sin(a
- 13)
(D.67) (D.68)
Index 2R plan ar manipulat or cont rol, 652 DR t ransformation matri x, 212 dynam ics, 491, 540 equations of motion, 494 forward accelerat ion, 439 ideal, 491 inverse acceleration, 441 inverse kinematics, 281, 286, 399 inverse velocity, 366, 368 Jacobian matrix, 350, 352 joint 2 acceleration, 433 joint path , 591 kinetic energy, 492 Lagrange dynamics , 533, 542 Lagrangean , 493 Newton-Eul er dynamics, 516 potential energy, 493 recursive dynami cs, 524 time-opt imal cont rol, 630 with massive links, 542 3R planar manipulato r DR transformation matrix, 204 forward kinematics , 227 4R planar manipulat or statics, 548 Acceleration angular, 423, 428, 429, 431, 432 bias vector, 440 body point , 317, 432, 434, 452 cent ripetal, 432 constant parabola, 593 constant path , 580 Coriolis, 454 discontinuous path, 588
discrete equat ion, 620, 631 end-effector, 429 forward kinematics, 437, 439 gravitational, 532, 538, 549 inverse kinematics, 439 jump, 573 matrix, 414, 423, 434-436 recursive, 507, 510 sensors, 659 tangential, 432 Active transformation, 72 Actu ator, 7, 12 force and torque, 513, 529, 553 optimal torque, 632, 633 torque equation, 518, 630 Algorit hm floatin g-time, 619, 629 inverse kinemati cs, 286 LV factorization, 380 LV solut ion, 380 Newton-Raphson, 398 Angle-axis rotat ion, 106 Angular accelerat ion, 423, 431, 432 combination, 428 end-effector, 429 in terms of Euler parameters, 429, 431 in terms of quaternion, 431 recursive, 414 Angular momentum 2 link manipulator, 462 Angular velocity, 53, 56, 57, 86, 299,306 alternative definition, 318 combination, 305 coordinate transformat ion, 308 decomposition, 305
686
Index
elements of matri x, 311 in terms of Euler parameters, 310 in terms of quaternion, 309 in terms of rotation matri x, 307 instantaneous, 301 inst antaneous axis, 302 matrix, 300 principal matrix, 304 recursive, 412, 509 Articulated arm, 8, 231, 267, 357, 408 Atan2 function, 272 Automorphism, 102 Axis-angle rotation, 81, 84, 85, 90, 91, 94 Block diagram , 644 Brachisto chro ne, 616, 627 Bryant angles, 58 Cardan angles, 58 frequencies, 58 Cartesian angular velocity, 56 end-effector posit ion , 365 end-effector velocity, 366 manipulat or, 8, 11 path, 592 Cent ra l difference, 625 Chasles theorem, 154, 166 Christoffel operator, 488, 535 Co-state variable, 610 Cont rol ada pt ive, 649 admissible, 618 bang-bang , 609, 610 characteristic equation, 646 closed-loop, 643 command, 643 computed force, 651 computed torque, 648, 649 derivati ve, 655
desired path , 643 error, 643 feedback, 644 feedback command, 651 feedback linearization, 648, 651 feedforward command, 651 gain-scheduling, 649 input , 650 integral, 655 linear, 649, 654 minimum time, 609 modified PD , 657 open-loop, 643, 650 path points, 595 PD ,657 proportional, 654 robots, 13 sensing, 657 stability of linear, 646 time-opt imal, 618, 622, 629, 630, 633 time-opt imal description, 618 time-opt imal path , 627 Contro ller, 7 Coordinate cylindr ical, 152 frame, 17 non-Car tesian , 487 non-orthogonal, 117 parabolic, 487 spherical, 153, 332 system, 17 Coriolis acceleration, 428, 434 effect, 454 force, 453 Cycloid, 617 Denavit-Hartenberg, 31 meth od, 199, 202, 248 nonstand ard method , 223, 283 notation, 199 parameters, 199, 334, 345, 510, 548
Index transformation, 208, 212-21 8, 220, 222, 243 Differential manifold, 71 Differentiating, 312 B-derivative , 312, 314 G-derivative, 312, 317 second, 320 transformation formula, 317 Distal end, 199, 548 Dynamics , 421, 507 2R planar manipulator, 516, 524 4 bar linkage, 514 actuator's force and torque, 529 backward Newton-Euler, 522 forward Newton-Euler, 529 global Newton-Eul er, 511 Lagrange , 530 Newton-Euler , 511 one-link manipulator, 513 recursive Newton-Eul er, 511, 522 Earth effect of rotation, 453 kinetic energy, 486 revolution , 486 rot ation , 486 rotation effect, 428 Eigenvalue, 87 Eigenvector , 87 Ellipsoid energy, 465 momentum, 464 End-effector , 6 acceleration, 437 angular acceleration , 429 angular velocity, 363 articulated robot , 267 configuration vector , 348, 405, 437 configuration velocity, 437 force, 530 frame, 207, 231
687
inverse kinematics, 265 kinemati cs, 237 link, 199 orientation, 271, 364 path , 591, 600 position, 231 position kinematics, 226 position vector, 358 rot ati on, 597 SCARA position, 149 SCARA robot, 240 space station manipulator, 243 spherical robot , 247 tim e optimal control, 609 velocity, 348, 354, 365 velocity vector, 348 Energy Ear th kinetic, 486 kinetic rigid body, 461 kinetic rot ational, 458 link's kinetic, 531, 537 link's pot ential, 532 mechanical, 486 point kinetic, 451 potenti al, 489 robot kinetic, 531, 538 robot potential, 532, 538 Euler -Lexell-Rodri guez formula, 83 angles, 18, 48, 51, 53, 107 integrability, 57 coordin at e frame, 56 equation of motion , 457, 460, 461, 466, 467, 513, 523 frequencies, 53, 56, 306 inverse matrix, 69 parameters, 88- 92, 96-98 , 100, 111,309,310 rotation matrix, 51, 69 th eorem, 48, 88 Euler equation body frame, 460, 467 Euler-Lagrange equat ion of motion , 614, 615 Eulerian
688
Index viewpoint , 326
Floating time , 620 1 DOF algorithm, 619 analytic calculation, 627 backward path, 622 convergence, 625 forward path, 621 method,618 multi DOF algorithm, 629 multiple switching , 633 path planning, 627 robot control, 629 Force, 449 action, 512 actuator, 529 conservative, 489 Coriolis, 454 driven , 512 driving, 512 generalized , 483, 532 gravitational vector, 533 potential, 489 potential field, 485 reaction, 512 sensors, 660 shaking, 516 time varying , 454 Forward kinematics, 32 Frame central, 455 final, 207 goal, 207 principal, 457 reference, 16 special, 206 station, 206 tool, 207 world, 206 wrist, 207 Generalized coordinate, 480, 483,484, 490 force, 482,483,485,487,489, 491, 494, 530
inverse Jacobian, 403 Grassmanian, 177 Group properties, 70 Hamiltonian, 610 Hand , 231 Hayati-Roberts notation, 224 Helix, 154 Homogeneous compound transformation, 145 coordinate, 133, 138 direction, 138 general transformation, 139, 143 inverse transformation, 139, 141, 142, 146 position vector , 133 scale factor , 133 transformation, 131, 134-137, 139, 141 Integrability, 57 Inverse kinematics, 32, 265 decoupling technique, 265 inverse transformation technique,272 iterative technique, 284 Pieper technique, 274 Inverted pendulum, 652 Jacobian analytical, 365 elements, 363 generating vector, 353, 404 geometrical, 365 inverse, 287, 403 matrix, 285, 287, 290, 348, 352, 355, 357, 365, 368, 397, 401, 407, 408, 437, 439, 534 oflink,531 polar manipulator, 349 Jerk
355,
292, 361, 404, 442,
Index angul ar , 430 matrix, 436 transformation, 435, 437 zero path , 579 Joint, 3 acceleration vector, 437 act ive, 4 coordinate, 4 cylindrical, 252 inactive, 4 orthogonal, 8 parallel, 8 passive , 4 path, 591 perp endicular , 8 screw, 4 vari able vector, 348 velocity vector, 348, 355 Joint angle, 200 Joint distance, 200 Joint parameters, 202 Kinematic length, 200 Kin ematics, 31 acceleration, 423 forward , 32, 226 forward acceleration, 437 forward velocity, 348 inverse, 32, 265, 272 inverse acceleration, 439 inverse velocity, 365 num erical methods, 377 velocity, 345 Kineti c energy, 451 Earth, 486 link , 537 parabolic coordinate, 487 rigid body, 461 robot , 531, 538 rotation al body, 458 Kronecker 's delt a, 65, 457, 479 Lagrange dyn amics, 530 equation, 536
689
equation of motion, 480, 489 mechanics , 489 multiplier, 617 Lagrange equation explicit form, 488 Lagrangean , 489, 538 robot, 538 viewpoint, 326 Law motion , 450 motion second, 450, 455 motion third , 450 robotics, 1 Levi-Civita density, 96 Lie group, 71 Link , 3 angular velocity, 346 class 1 and 2, 213 class 11 and 12, 218 class 3 and 4, 214 class 5 and 6, 215 class 7 and 8, 216 class 9 and 10, 217 classification, 219 end-effector, 199 Euler equat ion, 523 kinetic energy, 531 Newton-Euler dyn amics, 511 recursive accelera tion, 507, 510 recursive Newton-Euler dynamics, 522 recursive velocity, 509, 510 rotational acceleration, 508 translational accelera tion, 508 translational velocity, 347 velocity, 345 Link length, 200 Link offset , 200 Link par ameters, 202 Link twist, 200 Location vector, 156, 158 LV factorization method , 377, 392 Manipulator 2R plan ar , 491, 533
690
Index
3R planar , 227 ar ti culat ed, 205 definition, 5 inertia matri x, 532 one-link, 490 one-link cont rol, 655 one-link dynamics, 513 P UMA , 204 SCARA, 8 t ra nsformat ion matri x, 267 Mass center, 450, 451, 455 Matrix skew symmet ric, 68, 69, 82, 89 Moment , 449 action, 512 dri ven, 512 dri ving, 512 reaction, 512 Moment of inerti a about a line, 479 ab out a plane, 479 about a point , 479 cha racteristic equation, 477 diagonal elements , 477 Huygens-Steiner t heorem, 471 matri x, 468 parallel-axes t heorem, 469 pola r , 468 principal, 469 principal axes, 458 principal invariants, 477 produ ct , 468 pseud o matrix, 469 rigid body, 457 rot ated-axes t heorem, 469 Moment of momentum, 450 Moment um, 450 angular, 450 ellipsoid, 464 linear, 450 Motion, 14 Newton equation of mot ion, 480
Newton equation body frame, 456 global frame, 455 Lagrange form, 482 rotatin g frame, 453 Newto n-Euler backward equations, 522 equation of motion, 523 forward equations, 529 global equations, 511 recursive equations, 522 Numerical methods, 377 analytic inversion, 394 Cayley-Hamilton inversion, 395 condition number, 388 ill-conditioned, 388 J acobian matrix, 404 LU facto rization, 377 LU factorization with pivoting, 383 matrix inversion, 390 Newton-Ra phson, 398, 400 nonlinear equations, 397 norm of a mat rix, 389 partition ing inversion, 393 uniqueness of solut ion, 387 well-conditioned, 388 Optim al control, 609 a linear syste m, 610 descript ion, 618 first variation, 615 Hamiltonian , 610, 613 Lagran ge equation, 614 obj ecti ve function, 609, 613 performance index, 613 second variation, 615 switchi ng point , 611 Orthogonality condition, 64 Passive transformation, 72 Path Brachistochrone, 627 Cartesian, 592 constant acceleration, 580
Index constant angular acceleration, 599 cont rol points, 595 cubic , 571 cycloid, 590 har monic, 589 higher polynomial, 578 jerk zero, 579 joint space, 591 non-polynomial, 589 planning, 592 point sequence, 582 quad ratic, 577 quintic, 578 rest-to-rest , 573, 574 rot ati onal, 597 splitting, 584 to- rest , 573 Pendulum cont rol, 652 inverted, 652, 657 linear contro l, 655 oscillat ing, 484 simple, 425, 483 spherical, 490 Permutation symbol , 96 Phase plane, 611 Pieper technique, 274 Plucker ang le, 181 classification coord inate, 178 dist an ce, 181 line coordinate, 173, 175- 177, 181, 185-187, 247, 248 moment , 180 ray coordinate , 175, 177 recipr ocal produ ct , 181 screw , 186 virtual product , 181 Poinsot 's const ruction, 464 Po int at infinity, 138 Pole, 163 P osit ion sensors, 658 Positioning, 14 Potent ial
691
force, 489 Potenti al energy robot, 532, 538 P roximal end , 199, 548 Quatern ions, 99 addition, 99 compos it ion rotation, 102 flag form, 99 inverse rotation, 101 multipli cation , 99 rot ation, 100 Rigid body acceleration, 431, 508 angular momentum, 458 angular velocity, 86 Euler equation of mot ion , 461, 466 kinematics, 127 kineti c energy, 461 moment of inertia, 457 motion, 127 mot ion classification, 167 motion compos ition , 131 principal rotation matrix , 476 rotational kineti cs, 457 steady rot ation, 462 translat ional kinetics, 455 velocity, 321, 323 Robot applicat ion, 13 articulated, 8, 231, 238, 267, 357, 361 Cartesian , 11 classificat ion, 7 control, 13, 14 cont rol algorit hms, 648 cylind rical, 11, 259 dy na mics, 14, 19, 507, 533 end-effector path, 600 equation of mot ion, 540 forward kinematics, 226, 246 gravitational vector , 533 inertia matrix, 532
692
Index
kinemati cs, 14 kinetic energy, 531, 538 Lagrange dynamics, 530, 536 Lagrange equation, 533 Lagrangea n, 532, 536 link classification, 245 modified PD control, 657 Newton-Euler dynamics, 511 PD control, 657 pot ential energy, 532, 538 recursive Newton-Euler dynamics, 522 rest position , 200, 203, 231, 235, 239 SCARA , 149, 239 spherical, 10, 205, 235, 246, 274,355 state equat ion, 613 st atics, 546 time-opt imal contro l, 613, 629 velocity coupling vector, 533 Robotic geometry, 8 histo ry, 1 law, 1 Rodriguez rot at ion formula, 83, 84, 89, 92- 95, 101, 106, 114, 128, 158, 161, 167, 172, 302, 337, 597 vector, 95, 113 Roll-pitch-yaw frequency, 60 global angles, 41, 59 global rotation matri x, 41, 59 Rot ati on, 32, 83 about global axis, 33, 38, 40 about local axis, 43, 47, 48 angle-axis, 106 axis-angle, 81, 83-85, 90, 91, 94, 106 composition, 113 decomposition, 113 eigenvalue, 87 eigenvect or, 87
exponential form, 93 general, 63 infinitesimal, 92 local versus global, 61 matr ix, 18, 105 pole, 326 quaternion, 100 stanley met hod , 98 X-matri x, 33 x-matrix, 43 Y-matrix, 33 y-mat rix, 43 Z-matrix, 33 z-matrix, 43 Rotational path , 597 Rotator, 83, 102 SCARA mani pulator , 8 robot , 149, 239 Screw, 154, 157, 166 axis, 154 centra l, 155, 156, 159, 160, 173, 187, 202, 243, 245, 247 combination, 170, 172 coordinate, 154 decomposition, 172, 173 exponential, 171 forward kinematics, 243 inst antaneous, 187 intersection, 248 inverse, 169, 170, 172 left-handed, 155 link classificati on, 245 location vector, 156 motion, 202, 327 parameters, 155, 164 pitch, 154 Plucker coordinate, 186 prin cipal, 166, 172, 173 reverse central, 156 right-h anded, 15, 155 special case, 162 transformation, 158, 165
Index twist , 154 Second derivative, 320 Sensor accelerat ion, 659 position, 658 rotary, 658 velocity, 659 Sheth not ation , 248 Singular configuration, 291 Spherical coordinate , 153 Spinor , 83, 102 Splin e, 588 St an ley method, 98 St ark effect, 487 Symbols, xi Ti lt vector, 231 Time derivative, 312 Top , 53 Tr ansform ation , 31 active and passive, 71 general, 63 homogeneous, 131 Tr ansformation matrix derivat ive, 332 differential, 336, 337 elements, 66 velocity, 327 Tr anslati on, 32 Tri ad , 15 Trigonometric equation, 271 Turn vector , 231 Twi st vector , 231 Unit syste m, xi Unit vectors, 16
693
Vector gravitational force, 533 velocity coupling, 533 vector grav itational force, 537 velocity coupling, 536 Vector decomp osition, 117 Velocity body point , 452 discrete equation, 620, 631 end-effector, 348 inverse tr ansformation, 330 matrix, 436 operator matrix, 333 prismatic transformat ion, 335 revolut e transformation, 335 sensors, 659 transform ation matrix, 327 , 329, 331, 333 Work, 451, 454 virtual, 483 Work-energy principle, 451 Workspace, 11 Wrench, 452 Wrist , 12-1 4, 231 decoupling kinematics, 266 forward kinematics, 229 frame, 207 kinematics assembly, 238 point , 6, 229, 271 position vector, 270 spherical, 6, 205, 231, 235, 361 transformation matrix , 230 , 267 Zero velocity point , 326
