ME 3230 Chapter 9 Spatial Modeling

7/29/2019 ME 3230 Chapter 9 Spatial Modeling

1/64

1

Lecture Slides

ME 3230 Kinematics and Mechatronics

Chapter 9

Spatial Modeling some fundamentals

for Robot Kinematics

By Dr. Debao Zhou


2/64

2

Scope and Covered Materials

Scope Rotation and homogenous transformation in 3D space

Properties of rotation and homogenous transformation

matrix

Forward kinematics Denavit - HartenbergMethod (D-H Method)

Inverse kinematics

Reading materials: Corresponding materials in Spong and Vidyasagars book,

Chapters 2, 3 and 4.

Sciavicco and Siciliano : Sections 2.1-2.4, 2.7-2.9 and 2.12

Corresponding materials in our Text book


3/64

3

3D Robot Manipulator


4/64

Robot Hand Position and Orientation Expression

4

P(x, y, z, a b g)

Rigid body - frame/vectorFrame position, orientation

- relative to difference frames

Vector position, orientation

- relative to difference frames


5/64

5

3D Vector Expressed in Different Frames

Vector rP/O is expressed asp0 in frame

ox0y0z0 and p1 in frame ox1y1z1. The i, j, kare the corresponding unit vectors

1frameto0framefrommatrixrotationtheis10R


6/64

6

3D Vector Expressed in Different Frames


7/64

7

Example #1:

Rotation qaround z1

/z0

(positive qfrom frame 0

to frame 1)

Next page: Several examples: i, j, k expressed in 1 to expressed in 0


8/64

8

Example #1:

Rotation qaround z1

/z0

(positive qfrom 0 to 1)

Meaning? Transform a vector

expression in Frame 1 to frame 0

0

0

1

100

0cossin

0sincos

0

sin

cos

1

1

00 pRp

qq

qq

q

q

0

1

0

100

0cossin

0sincos

0

cos

sin

1

1

00pRp

qq

qq

q

q

Explain q = 90, 180, 270


9/64

9

Three Simple rotations

Similarly, rotation qaround x0, y0 or z0 axis

qq

qq

q100

0cossin

0sincos

R ,z


10/64

Physical Meaning R01

Frames #0 and #1 share the same origin; R0

1 means the transformation of a vector

expressed in frame #1 to the expression in

frame #0 by using the rotation angle fromframe #0 to #1;

Rmnmeans the transformation of a vector

expressed in frame #n to the expression inframe #m by using the rotation angle from

frame #m to #n.

10


11/64

Multiple Rotations

p0

= R01

p1p1

= R12 p2

So

p0= R01 R12 p2

And

p0= R0

2 p2

Thus

R02 = R0

1 R12

and R02

(R02

)

T

= 1 11

1 2

3


12/64

12

Properties of Rotation Matrix

Transposition: Inversion:

Multiple multiplications

Any rotation matrix is Orthogonal Matrices

) ) ) )

) ) ) )

)I

RR

RRRRRRRR

RRRRRRR

RRRRRRRR

n

n

Tn

n

n

n

TTTTn

n

n

n

TTTTn

n

n

n

Tn

n

11

1

3

2

2

1

1

0

1

0

2

1

3

21

1

3

2

2

1

1

0

2

1

3

21

1

3

2

2

1

1

01

3

2

2

1

1

0

)(

)(

)()(

) ) 1000

1

3

2

2

1

1

00

isThere

Set

n

T

n

n

n

n

n

RRR

RRRRR


13/64

13

Composition of Rotation Matrices

Matrix product is not commutative

Two rotations in general do not commute and itscomposition depends on the order of the single rotations.

Mathematically, AB BA

OrR01 R12R12 R01.

) ) ) )

) ) ) )

)I

RR

RRRRRRRR

RRRRRRR

RRRRRRRR

n

n

Tn

n

n

n

TTTTn

n

n

n

TTTTn

n

n

n

Tn

n

11

1

3

2

2

1

1

0

1

0

2

1

3

21

1

3

2

2

1

1

0

2

1

3

21

1

3

2

2

1

1

01

3

2

2

1

1

0

)(

)(

)()(


14/64

Continues Rotation

Continues rotationaround the current

frame

Body-fixed framerotation

Continues rotation

around a fixed

frame

World-fixed frame

rotation

14


15/64

Continues Rotation: Euler Angle

Body Fixed Rotation Leonhard Eulerto describe the orientation of a rigid body

in 3D space

Any orientation can be described by three consequence

rotations

15
http://en.wikipedia.org/wiki/Leonhard_Eulerhttp://en.wikipedia.org/wiki/Rigid_bodyhttp://en.wikipedia.org/wiki/Dimensionhttp://en.wikipedia.org/wiki/Dimensionhttp://en.wikipedia.org/wiki/Rigid_bodyhttp://en.wikipedia.org/wiki/Leonhard_Euler


16/64

16

Continues Rotation - Euler Angles - YXZ

Rot(Y,1) Rot(X,2) Rot(Z,3)


17/64

Table of Rotation Matrix Euler Anlges

17


18/64

Homogeneous Transformation

Frames do not share the same origin!

18


19/64

11

110

11101

11101

11

2

22

0

2

22

1

1

0

2

2

0

0/1

1

1/2

0

1

1

2

0

1

1

11

0

1

1

0

0/1

1

0

0

0

2

22

1

2

2

1

1/2

2

1

1

1

222

2

2

pp

prrRRR

pprRp

pprRp

zyxpT

HHH

H

H

19

Homogeneous Transformation

Tzyxp 2222

2

1

1

1

33

1

131

31

0H

H0

H

0H

dRR

d

R

dR

TT

T

TT

2

2p

11p

0

0p

0

0/1r

1

1/2r

P

1

1/2

2

2

2

1

1

1/2

2

2

1

1 rpRrpp

0

0/1

1

1

1

0

0

0/1

1

1

0

0 rpRrpp

0

0/1

1

1/2

2

2

1

2

1

0 rrpRR

00/1

11/2

10

22

21

10 rrRpRR

2

2p

1

1p

0

0p

0

0/1r

1

1/2r

P


20/64

20

What is the benefit to us?

Robot manipulator express the end-effector

vector to the base frame. Fundamentally

different from those in Chapter 5

2

2p

1

1p

0

0p

0

0/1

r

1

1/2r


21/64

21

Basic Homogeneous Transformation

Translation

Simple Rotation


22/64

22

Defining the Homogeneous Transformation Matrix

It is a 4x4 Matrix that describes 3-Space with

information that relates Orientation and Position (pose) ofa remote space to a local space

nx ox ax dxny oy ay dynz oz az dz0 0 0 1

n vectorprojects theXrem Axis to theLocalCoordinate

System

o vectorprojectsthe YremAxis to theLocal

CoordinateSystem

a vectorprojects theZrem Axis tothe LocalCoordinateSystem

d vector isthe positionof the originof theremotespace inLocalCoordinatedimensions

This 3x3 Sub-Matrix isthe information thatrelates orientation of

Framerem to FrameLocal(This is called R therotational Submatrix)

11101

11101

1

11

0

1

1

0

0/1

1

0

0

0

222

1

22

11/2

21

11

pprRp

pprRp

H

H

2

2p

11p

0

0p

0

0/1r

1

1/2r

P


23/64

23

Defining the Homogeneous Transformation Matrix

nx ox ax dxny oy ay dynz oz az dz0 0 0 1

Perspectiveor ProjectionVector

ScalingFactor

This matrix is a transformationtool for space motion!


24/64

24

HTM A Physical Interpretation

1. A representation of a coordinate transformation

relating the coordinates of a point P between 2different coordinate systems

2. A representation of the position and orientation(pose) of a transformed coordinate frame in thespace defined by a fixed coordinate frame

3. An operation that takes avectorP and rotates

and/or translates it to anew vectorPtin the samecoordinate frame

2

2p

1

1p

0

0p

0

0/1r

1

1/2r

P

11101

1

11

0

1

1

0

0/1

1

0

0

0 pprRp

H

10

0

0/1

1

01

0

rR

H


25/64

25

Looking Closely at the T0n Matrix

T0n

matrix related theend of the arm frame

(n) to its base (0)

Thus it must contain

information related tothe several joints of the

robot

It is a 4x4 matrix

populated by complexequations for both

position and orientation

(POSE)


26/64

26


To get at the equation set, wewill choose to add a series ofcoordinate frames to each ofthe joint positions

There are n+1 frames n+1 rigid bodies = number offrames Why?

njoints

The frame attached to the 1st jointis labeled 0 the base frame!while joint two has Frame 1, etc.

The last Frame is the end ornFrame


27/64

27


As we have seen earlier, wecan define a HTM (T(i-1)i) to the

transformation between any

two consecutive frames

Thus we will find that the T0n isgiven by a product formed by:

n

n

n

TTTT 12

1

1

00


28/64

28


For simplicity, we will redefineeach of the of these transforms(T(i-1)

i) as Ai

Then, for a typical 3 DOF robot

Arm, T03 = A1*A2*A3

While for a full functioned 6 DOFrobot (arm and wrist) would be:

T0n = A1*A2*A3*A4*A5*A6

A1 to A3explain the arm jointeffect while A4 to A6 explain thewrist joint effects


29/64

29

Frame To Frame Arrangements in 3D Space

When we move from one frame to another,we will encounter:

Two translations (in a controlled sense)

Two rotations (also in a controlled sense) A rotation and translation WRT the Zi-1

These are called the Joint Parameters

A rotation and translation WRT the Xi These are called the Link Parameters


30/64

30

Relationship between Two Frames

Rotate qaround zi-1, translate dalongzi-1, translate a

alongxiaxis, and rotate a aroundxi axis.

Frame (i-1) on link (i-1)

Frame i on link i


31/64

31

Talking Specifics Joint Parameters

qi is an angle measured about the Zi-1 axisfrom Xi-1 to Xi and is a variable for arevolute joint its fixed for a Prismatic Joint

di is a distance measured from the origin ofFrame(i-1) to the intersection of Zi-1 and Xiand is a variable for a prismatic joint its

fixed for a Revolute Joint


32/64

32

Talking Specifics Link Parameters

aiis the Link length and measures the distance

from the intersection ofZi-1 to the origin of Frameimeasured alongXi

ai is the Twist angle which measures the angle

from Zi-1 to Zias measured aboutXi Both of these parameters are fixed in valueregardless of the joint type. A Further note: Fixed does not mean zero degrees or

zero lengthjust that they dont change


33/64

33


34/64

34

Returning to the 4 Frame-Pair Operators:

1st is q which is an

operation ofpure

rotation about Zor:

2nd is d which is a

translation along Zor:

os 0 0

0 0

0 0 1 0

0 0 0 1

C SinSin Cos

q q

q q

1 0 0 0

0 1 0 00 0 1

0 0 0 1

d

O


35/64

35

Returning to the 4 Frame Operators:

3rd

is a TranslationAlong Xor:

4th is awhich is a

pure Rotation

about Xor:

1 0 0

0 1 0 0

0 0 1 0

0 0 0 1

a

1 0 0 00 0

0 0

0 0 0 1

Cos Sin

Sin Cos

a a

a a

Th O ll Eff t i


36/64

36

The Overall Effect is:

os 0 0 1 0 0 0 1 0 0 1 0 0 0

0 0 0 1 0 0 0 1 0 0 0 0

0 0 1 0 0 0 1 0 0 1 0 0 0

0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1

C Sin a

Sin Cos Cos Sin

d Sin Cos

q q

q q a a

a a

aq ,,,,1 xaxdzzii

i RotTranTranRotAT

Si lif i thi M t i P d t


37/64

37

Simplifying this Matrix Product:

0

0 0 0 1

i i i i i i i

i i i i i i i

i i i

C S C S S a C

S C C C S a S S C d

q q a q a q

q q a q a q a a

This matrix is the general transformation relating each

and every of the frame pairs along a robot structure

aq ,,,,1 xaxdzzi

i

i RotTranTranRotAT

M th ti C d


38/64

38

Mathematica Code

M th ti C d


39/64

39

Mathematica Code

R b t Ki ti


40/64

40

Robot Kinematics

Foreword Kinematics Definition: Given the joint variable of the robot ,

to determine the position and orientation of the

end-effector.

Inverse Kinematics

Definition: Given a desired position and

orientation of the end-effector of a robot,

determine a set of joint variables that achievethe desired position and orientation.


41/64

41

F d Ki ti Di t Ki ti


42/64

Denavit-HartenbergMethod

or D-H Convention A Step-by-Step approach for modeling each of the

frames from the initial (or 0) frame all the way to the n(or end) frame

This modeling techniquemakes each joint axis

(either rotation or translation)

the Z-axis of the appropriate

frame (Z0 to Zn-1). The Joint motion, thus, is taken

W.R.T. the Zi-1 axis of the frame

pairmaking up the specific transformation matrix

under design42

Foreword Kinematics Direct Kinematics

A l i D H t G l C


43/64

43

Applying D-H to a General Case:

Link i:

Connect frame i -1and frame i

Connect joint iand joint i+1

Think: Link i -1as theground link (or link 0):

- Joint 1 and frame 0

D it H t b R f F L t


44/64

Denavit-Hartenberg Reference Frame Layout

http://www.youtube.com/watch?v=rA9tm0gTln8

44

Th D H M d li R l
http://www.youtube.com/watch?v=rA9tm0gTln8http://www.youtube.com/watch?v=rA9tm0gTln8http://www.youtube.com/watch?v=rA9tm0gTln8http://www.youtube.com/watch?v=rA9tm0gTln8


45/64

45

The D-H Modeling Rules:

1) Locate and mark the

motion ( using qiand di)

and label the joint axes:Z0 to Zn-1

2) Establish the Base

Frame. Set Base Origin

anywhere on the Z0axis. ChooseX0and Y0

conveniently and to

form a right hand frame.

0) Identify links and joints (motions)

n+1 links from 0 to n, n joints for n+1 links;

The D H Modeling R les


46/64

46


3) Locate the origin Oiwhere the commonnormal to Zi-1 and Ziintersects Zi. IfZi

intersects Zi-1 locate Oiatthis intersection. IfZi-1and Ziare parallel, locateOiat Joint i+1

OrXihas to be perpendicularwith Ziand Zi-1 andconnect with Ziand Zi-1

The intersection points formsthe origin of the frames

n+1 links from 0 to n, njoint from 0 to n-1;

The D H Modeling Rules:


47/64

47


4) EstablishXialong the common normalbetween Zi-1 and ZithroughOi, or in the

direction Normal to the plane Zi-1 Zi if

these axes intersect5) Establish Yi to form a right hand system

Set i = i+1, ifi


48/64

48


6) Establish the End-Effector (n) frame:OnXnYnZn. Assum ing the n

thjo int is

revolute, set kn = a along the direction Zn-1.

Establish the origin On conveniently alongZn, typically mounting point of gripper or

tool. Setjn= o in the direction of gripper

closure (opening) and set in

= nsuch that n

= oxa. Note if tool is not a simple gripper,

setXn and Yn conveniently to form a right

hand frame.

The D H Modeling Rules:


49/64

49


7) Create a table of Link parameters:1) qias angle about Zi-1between Xs

2) dias distance along Zi-1

3) aias angle aboutX

ibetween Zs

4) aias distance alongXi

8) Form HTM matricesA1,A2, An by

substituting q, d, aand a into the general

model

9) Form T0n =A1*A2**An

Some Issues to remember:


50/64

50


Xi

is perpendicular with Zi-1

and intersect with Zi-1

.

Xaxis If you have parallel Zaxes, theXaxis of the second

frame runs perpendicularly between them

Kinematic Home When working on a revolute joint, the model will besimpler if the twoXdirections are in alignment atKinematic Home i.e. Our models starting point (q=0)

To achieve this kinematic home, rotate the model aboutmoveable axes (Zi-1s) to alignXs or set q= 0

Kinematic Home is not particularly criticalforprismaticjoints



51/64

51


An ideal model will have n+1 frames However, additional frames may be

necessarythese are considered Dummy

frames since they wont contain joint axes

Three link Planar Arm


52/64

52

Three-link Planar Arm

Three link Planar Arm


53/64

53

Three-link Planar Arm

Individual Homogenous TransformationMatrix

Homogenous Transformation Matrix from 0

to 3

T03 = A 1 A 2 A 3 =

2 DOFs (Motions)


54/64

2-DOFs (Motions)

54

x0

y0

x1

z1

z2

x2

q

d

q

Example 1: 6 dofs Articulating Arm


55/64

55

Example 1: 6-dofs Articulating Arm

D H Table


56/64

56

D-H Table

Frames Link Var q d a a Sa Ca Sq Cq

0 to1 1 R q1 0 0 90 -1 0 S1 C1

1 to2 2 R q2 0 a2 0 0 1 S2 C2

2 to 3 3 R q3 0 a3 0 0 1 S3 C3

3 to 4 4 R q4 0 a4 -90 -1 0 S4 C4

4 to 5 5 R q5 0 0 90 1 0 S5 C5

5 to 6 6 R q6* d6 0 0 0 1 S6 C6

* With End Frame in Better Kinematic Home. Here,

as shown, it would be (a6 - 90), which is a problem!

A Matrices


57/64

57

A Matrices

1

1 0 1 0

1 0 1 0

0 1 0 0

0 0 0 1

C S

S CA

2

2

2

2 2 0 2

2 2 0 2

0 0 1 0

0 0 0 1

C S a C

S C a S A

3

33

3 3 0 3

3 3 0 30 0 1 0

0 0 0 1

C S a C

S C a S A

4

44

4 0 4 4

4 0 4 40 1 0 0

0 0 0 1

C S a C

S C a S A

A Matrices cont


58/64

58

A Matrices, cont.

5

5 0 5 0

5 0 5 0

0 1 0 0

0 0 0 1

C S

S CA

6

6

6 6 0 0

6 6 0 0

0 0 1

0 0 0 1

C S

S CA

d

At Better Kinematic Home!

Leads To:


59/64

59

Leads To:

Forward Kinematics is

After Preprocessing A2-A3-A4,

with (Full) Simplify function, the

FKS must be reordered as A1-

A234-A5-A6 and Simplified

6

5

2

1

1

0

6

0 TTTT

Solving for FKS


60/64

60

Solving for FKS

Pre-process {A2*A3*A4} with Full Simplify They are the planer arm issue as in the

previous robot model

Then Form: A1* {A2*A3*A4}*A5*A6 Simplify for FKS!

Simplifies to (in the expected Tabular Form):


61/64

61

Simplifies to (in the expected Tabular Form):

nx = C1(C5C6C234 - S6S234) - S1S5C6

ny = C1S5C6 + S1(C5C6C234 - S6S234)nz = S6C234 + C5C6S234

ox = S1S5S6 - C1(C5S6C234 + C6S234)

oy = - C1S5S6 - S1(C5S6C234 + C6S234)

oz = C6C234 - C5S6S234

ax = C1S5C234 + S1C5

ay = S1S5C234 - C1C5

az = S5S234

dx = C1(C234(d6S5 + a4) + a3C23 + a2C2) + d6S1C5dy = S1(C234(d6S5 + a4) + a3C23 + a2C2) - d6C1C5

dz = S234(d6S5 + a4) + a3S23 + a2S2

Verify the Model


62/64

62

Verify the Model

Again, substitute knowns (typically 0

or 0units) for the variable kinematic variables

Check each individual A matrix against

your robot frame skeleton sketch for

physical agreement

Check the simplified FKS against the

Frame skeleton for appropriateness

After Substitution:


63/64

63

After Substitution:

nx = C1(C5C6C234 - S6S234) - S1S5C6 = 1(1-0) 0 = 1

ny = C1S5C6 + S1(C5C6C234 - S6S234) = 0+ 0(1 0) = 0 nz = S6C234 + C5C6S234 = 0 + 0 = 0

ox = S1S5S6 - C1(C5S6C234 + C6S234) = 0 1(0 + 0) = 0

oy = - C1S5S6 - S1(C5S6C234 + C6S234) = -0 0(0 + 0) = 0

oz = C6C234 - C5S6S234 = 1 0 = 1

ax = C1S5C234 + S1C5 = 0 + 0 = 0

ay = S1S5C234 - C1C5 = 0 1 = -1

az = S5S234 = 0

dx = C1(C234(d6S5 + a4) + a3C23 + a2C2) + d6S1C5= 1*(1(0 + a4) + a3 + a2) + 0 = a4 + a3 + a2

dy = S1(C234(d6S5 + a4) + a3C23 + a2C2) - d6C1C5= 0(1(0 + a4) + a3 + a2) d6 = -d6

dz = S234(d6S5 + a4) + a3S23 + a2S2= 0(0 + a4) + 0 + 0 = 0

Inverse Kinematics


64/64

Inverse Kinematics

Based on the direct (forward) kinematicsand the special properties of HTM

properties to calculate the joint parameters

(angle for revolute joint and moving

distance for prismatic joint)

ME 3230 Chapter 9 Spatial Modeling

Documents

Transcript of ME 3230 Chapter 9 Spatial Modeling