Theory of Applied Robotics: Kinematics, Dynamics, and Control

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

= 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

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 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,

" 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

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 """

.,..,

'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(