Module 02 - Robot Kinematics

8/3/2019 Module 02 - Robot Kinematics

1/46

ME 8209

Robot Kinematics(Forward and Inverse)


2/46

ME 8209

Forward kinematics introduction

Challenge: given all the joint parameters of a manipulator, determinethe position and orientation of the tool frame Tool frame: coordinate frame attached to the most distal link of the

manipulator Inertial frame: fixed (immobile) coordinate system fixed to the most proximal

link of a manipulator

Therefore, we want a mapping between the tool frame and the inertialframe This will be a function of all joint parameters and the physical geometry of

the manipulator

Purely geometric: we do not worry about joint torques or dynamics (yet!)


3/46

ME 8209

Convention

A n- DOF manipulator will have n joints (either revolute or prismatic) andn +1 links (since each joint connects two links) We assume that each joint only has one DOF. Although this may seem like

it does not include things like spherical or universal joints, we can think ofmulti-DOF joints as a combination of 1DOF joints with zero length betweenthem

The o 0 frame is the inertial frame o n is the tool frame Joint i connects links i -1 and i The o i is connected to link i

Joint variables, q i

prismaticisjointifrevoluteisjointif

i d i

q i

i i


4/46

ME 8209

Convention

We said that a homogeneous transformation allowed us to express theposition and orientation of o j with respect to o i what we want is the position and orientation of the tool frame with respect to

the inertial frame An intermediate step is to determine the transformation matrix that gives

position and orientation of o i with respect to o i -1: Ai Now we can define the transformation o j to o i as:

i j j i j i

T

I AAAA

T j i

j i j i i i

j

if

if if

1

21 ...


5/46

ME 8209

Convention

Finally, the position and orientation of the tool frame with respect to theinertial frame is given by one homogeneous transformation matrix: For a n -DOF manipulator

Thus, to fully define the forward kinematics for any serial manipulator,all we need to do is create the Ai transformations and perform matrixmultiplication

But there are shortcuts

n n n n n

q Aq Aq AT o R

H 22110

00

10


6/46

ME 8209

The Denavit-Hartenberg (DH) Convention

Representing each individual homogeneous transformation as theproduct of four basic transformations:

1000

0

1000

0000

0001

1000

01000010

001

1000

1000010

0001

1000

010000

00

,,,,

i

i

i

i

i

x a x d z z i

d c s

s a s c c c s c a s s c s c

c s s c

a

d c s

s c

A

i i

i i i i i i

i i i i i i

i i

i i i i

i i

i i i i

RotTransTransRot


7/46

ME 8209

The Denavit-Hartenberg (DH) Convention

Four DH parameters: a i : link length i : link twist d i : link offset

i : joint angle Since each Ai is a function of only one variable, three of these will be

constant for each link d i will be variable for prismatic joints and i will be variable for revolute joints

But we said any rigid body needs 6 parameters to describe its positionand orientation Three angles (Euler angles, for example) and a 3x1 position vector So how can there be just 4 DH parameters?...


8/46

ME 8209

Existence and uniqueness

When can we represent a homogeneous transformation using the 4 DHparameters?

For example, consider two coordinate frames o 0 and o 1 There is a unique homogeneous transformation between these two frames

Now assume that the following holds:1. DH1:2. DH2:

If these hold, we claim that thereexists a unique transformation A:

01

z x

01

z x

10

01

01

,,,,

o R

A x a x d z z RotTransTransRot


9/46

ME 8209


Proof:1. We assume that R 10 has the form:2. Use DH1 to verify the form of R 10

Since the rows and columns of R 10 must be unit vectors: The remainder of R

1

0 follows from the properties ofrotation matrices

Therefore our assumption that there exists a unique and that will give us R 10 is correct given DH1

0

1

0

0

0

31

31

21

11

0

0

0

101

r

r

r

r

z x z x

T

,,01 x z R R R

3332

232221

131211

0

1

0 r r

r r r

r r r

R

1

12

332

32

221

211

r r

r r


10/46

ME 8209


Proof:1. Use DH2 to determine the form of o 10

Since the two axes intersect, we can represent the line between the twoframes as a linear combination of the two axes (within the plane formed byx 1 and z 0 )

d

as ac

s c

a d o

ax dz o z x

01

0

0

01

01

00

0101


11/46


12/46

ME 8209

Assigning coordinate frames

For any n -link manipulator, we can always choose coordinate framessuch that DH1 and DH2 are satisfied The choice is not unique, but the end result will always be the same

1. Choose z i as axis of rotation for joint i +1

z 0 is axis of rotation for joint 1, z 1 is axis of rotation for joint 2, etc If joint i +1 is revolute, z i is the axis of rotation of joint i +1 If joint i +1 is prismatic, z i is the axis of translation for joint i +1


13/46

ME 8209


2. Assign base frame Can be any point along z 0

3. Chose x 0 , y 0 to follow the right-handed convention4. Now start an iterative process to define frame i with respect to frame i -1

Consider three cases for the relationship of z i -1 and z i :i. z i -1 and z i are non-coplanarii. z i -1 and z i intersectiii. z i -1 and z i are parallel

z i -1 and z i are coplanar


14/46

ME 8209


i. z i -1 and z i are non-coplanar There is a unique shortest distance between the two axes Choose this line segment to be x i

o i is at the intersection of z i and x i Choose y

i by right-handed convention

ii. z i -1 and z i intersect Choose x i to be normal to the plane defined by z i and z i -1

o i is at the intersection of z i and x i Choose y i by right-handed convention

iii. z i -1 and z i are parallel Infinitely many normals of equal length between z i and z i -1 Free to choose o i anywhere along z i , however if we choose x i to be along

the normal that intersects at o i -1, the resulting d i will be zero Choose y i by right-handed convention


15/46

ME 8209

Assigning tool frame

The previous assignments are valid up to frame n -1 The tool frame assignment is most often defined by the axes n , s , a :

a is the approach direction s is the sliding direction (direction along which the grippers open/close) n is the normal direction to a and s


16/46

ME 8209

Summary - General procedure for determining fwd. kinematics

1. Label joint axes as z 0 , , z n-1 (axis z i is joint axis for joint i +1)2. Choose base frame: set o 0 on z 0 and choose x 0 and y 0 using right-

handed convention3. For i =1: n -1,

i. Place o i where the normal to z i and z i-1 intersects z i . If z i intersects z i-1, puto i at intersection. If z i and z i-1 are parallel, place o i along z i such that d i =0

ii. x i is the common normal through o i , or normal to the plane formed by z i-1 and z i if the two intersect

iii. Determine y i using right-handed convention4. Place the tool frame: set z n parallel to z n- 15. For i =1: n , fill in the table of DH parameters6. Form homogeneous transformation matrices, Ai 7. Create T n 0 that gives the position and orientation of the end-effector in

the inertial frame


17/46

ME 8209

Example 1: two-link planar manipulator

2DOF: need to assign three coordinate frames1. Choose z 0 axis (axis of rotation for joint 1, base frame)2. Choose z 1 axis (axis of rotation for joint 2)3. Choose z 2 axis (tool frame)

This is arbitrary for this case since we have described no wrist/gripper Instead, define z 2 as parallel to z 1 and z 0 (for consistency)

4. Choose x i axes All z i s are parallel Therefore choose x i to intersect o i -1


18/46

ME 8209

Example 1: two-link planar manipulator

Now define DH parameters First, define the constant parameters a i , i Second, define the variable parameters i , d i

The i terms are 0 because all z i are parallel Therefore only i are variable

link a i i d i i

1 a 1 0 0 12 a 2 0 0 2

1000

0100

0

0

1000

0100

0

0

2222

2222

21111

1111

1

s a c s c a s c

As a c s c a s c

A ,

1000

0100

0

0

122111212

122111212

210

2

10

1

s a s a c s c a c a s c

AAT

AT


19/46

ME 8209

Example 2: three-link cylindrical robot

3DOF: need to assign four coordinate frames1. Choose z 0 axis (axis of rotation for joint 1, base frame)2. Choose z 1 axis (axis of translation for joint 2)3. Choose z 2 axis (axis of translation for joint 3)4. Choose z

3 axis (tool frame)

This is again arbitrary for this case since we have described no wrist/gripper Instead, define z 3 as parallel to z 2


20/46

ME 8209

Example 2: three-link cylindrical robot


link a i i d i i

1 0 0 d 1 1

2 0 -90 d 2 0 3 0 0 d

3 0

1000

100

0010

0001

1000

010

0100

0001

1000

100

00

00

33

22

1

11

11

1 d A

d A

d c s s c

A ,,

1000

010

0

0

21

3111

3111

3210

3 d d d c c s

d s s c

AAAT


21/46

ME 8209

Example 3: spherical wrist

3DOF: need to assign four coordinate frames yaw, pitch, roll ( 4 , 5 , 6 ) all intersecting at one point o (wrist center)


22/46

ME 8209

Example 3: spherical wrist

link a i i d i i

4 0 -90 0 4

5 0 90 0 5 6 0 0 d

6 6


1000

100

00

00

1000

0010

00

00

1000

0010

00

00

6

66

66

355

55

244

44

1 d c s

s c

Ac s

s c

Ac s

s c

A ,,

10006556565

654546465464654

654546465464654

6543

6 d c c c s c s d s s s s c c s c s s c c c s

d s c s c c s s c c s s c c c

AAAT


23/46

ME 8209

Example 4: cylindrical robot with spherical wrist

6DOF: need to assign seven coordinate frames But we already did this for the previous two examples, so we can fill in the

table of DH parameters:

link a i i d i i

1 0 0 d 1 1

2 0 -90 d 2 0

3 0 0 d 3 0

4 0 -90 0 4 5 0 90 0 5 6 0 0 d 6 6

o 3 , o 4 , o 5 are all atthe same point o c


24/46

ME 8209

Example 4: cylindrical robot with spherical wrist

Note that z 3 (axis for joint 4) is collinear with z 2 (axis for joint 3), thus wecan make the following combination:

1000333231

232221

131211

36

03

06

z

y

x

d r r r

d r r r d r r r

T T T

21654

316516541

316516541

5433

5154123

5154113

6465432

651641654122

651641654112

6465431

651641654121

651641654111

d d d s s d

d c d c c d s c s d d s d c s d s c c d

s s r

c c s c s r c s s c c r c c c c s r

c s c s s s s c c s r c s s c s c s c c c r

s c c c s r c s c s s s c c c s r c s s s s c c c c c r

z

y

x


25/46

ME 8209

Example 5: the Stanford manipulator

6DOF: need to assign seven coordinate frames:1. Choose z 0 axis (axis of rotation for joint 1, base frame)2. Choose z 1-z 5 axes (axes of rotation/translation for joints 2-6)3. Choose x i axes4. Choose tool frame5. Fill in table of DH parameters:

link a i i d i i

1 0 -90 0 1

2 0 90 d 2 2

3 0 0 d 3 0

4 0 -90 0 4

5 0 90 0 5 6 0 0 d 6 6


26/46

ME 8209


Now determine the individual homogeneous transformations:

1000

100

00

00

1000

0010

00

00

1000

0010

00

00

1000100

0010

0001

1000010

00

00

10000010

00

00

6

66

66

655

55

544

44

4

33

2

22

22

2

11

11

1

d c s s c

Ac s

s c

Ac s s c

A

d A

d

c s s c

Ac s s c

A

,,

,,


27/46

ME 8209


Finally, combine to give the complete description of the forwardkinematics:

1000333231

232221

131211

610

6z

y

x

d r r r d r r r d r r r

AAT

524526322155142541621321

5412515421621321

5254233

54152542123

54152542113

65264654232

646541652646542122

646541652646542112

65264654231

646541652646542121

646542652646542111

s s c c c d d c d

s s c s s c c s s c d d c d s s d s s s s c c s c c c d d s d s c d

c c s c s r s s c c s s c c s r s s s c s s c c c r s s c c s s c c s r

s c s c s c s s s c s s c c c s r c c s c s s s s s c s s c c c c r

c s c s s c c c s r

s c c c s c c s s s s c c c c s r s c c c s d c s s s s c c c c c r

z

y

x


28/46

ME 8209

Example 6: the SCARA manipulator

4DOF: need to assign five coordinate frames:1. Choose z 0 axis (axis of rotation for joint 1, base frame)2. Choose z 1-z 3 axes (axes of rotation/translation for joints 2-4)3. Choose x i axes4. Choose tool frame5. Fill in table of DH parameters:

link a i i d i i

1 a 1

0 0

1

2 a 2 180 0 2

3 0 0 d 3 0

4 0 0 d 4 4


29/46

ME 8209

Example 6: the SCARA manipulator

Now determine the individual homogeneous transformations:

1000

100

00

00

1000

100

0010

0001

1000

0100

0

0

1000

0100

0

0

4

44

44

43

32222

2222

21111

1111

1 d c s s c

Ad

As a c s c a s c

As a c s c a s c

A ,,,

1000100

0

0

43

12211412412412412

12211412412412412

410

4

d d

s a s a c c s s s c c s c a c a c s s c s s c c

AAT


30/46

ME 8209

Forward kinematics of parallel manipulators

Parallel manipulator: two or more series chains connect the end-effector to the base (closed-chain)

# of DOF for a parallel manipulator determined by taking the total DOFsfor all links and subtracting the number of constraints imposed by theclosed-chain configuration

Grueblers formula (3D):

j n

i i j L f n n

1

6DOF#

number of links*

*excluding ground

number of joints

#DOF for joint i


31/46

ME 8209

Forward kinematics of parallel manipulators

Grueblers formula (2D):

Example (2D): Planar four-bar, n L = 3, n j = 4, f i = 1(for all joints)

3(3-4)+4 = 1DOF Forward kinematics:

j n

i i j L f n n

1

3DOF#

2tan

2

22cos

1

21

22

21

22

21

LL

LL

L


32/46

ME 8209

Inverse Kinematics

Find the values of joint parameters that will put the tool frame at adesired position and orientation (within the workspace) Given H :

Find all solutions to:

Noting that:

This gives 12 (nontrivial) equations with n unknowns

310

SE o R

H

H q q T n n ,...,10

n n n n q Aq Aq q T 1110 ,...,


33/46

ME 8209

For a given H :

Find 1, 2 , d 3 , 4 , 5 , 6 :

One solution: 1 = /2, 2 = /2, d 3 = 0.5, 4 = /2, 5 = 0, 6 = /2

1000

0001

763.0100

154.0010

H

Example: the Stanford manipulator

0763.0

154.0

0

1

0

0

0

1

1

0

0

52452632

2155142541621321

5412515421621321

52542

541525421

541525421

652646542

6465416526465421

6465416526465421

652646542

6465416526465421

6465426526465421

s s c c c d d c s s c s s c c s s c d d c d s s

s s s s c c s c c c d d s d s c

c c s c s

s s c c s s c c s

s s s c s s c c c

s s c c s s c c s

s c s c s c s s s c s s c c c s

c c s c s s s s s c s s c c c c

c s c s s c c c s

s c c c s c c s s s s c c c c s

s c c c s d c s s s s c c c c c


34/46

ME 8209

Inverse Kinematics

The previous example shows how difficult it would be to obtain aclosed-form solution to the 12 equations

Instead, we develop systematic methods based upon the manipulatorconfiguration

For the forward kinematics there is always a unique solution Potentially complex nonlinear functions

The inverse kinematics may or may not have a solution Solutions may or may not be unique Solutions may violate joint limits

Closed-form solutions are ideal!


35/46

ME 8209

Overview: kinematic decoupling

Appropriate for systems that have an arm a wrist Such that the wrist joint axes are aligned at a point

For such systems, we can split the inverse kinematics problem into twoparts:

1. Inverse position kinematics: position of the wrist center2. Inverse orientation kinematics: orientation of the wrist

First, assume 6DOF, the last three intersecting at o c

Use the position of the wrist center to determine the first three jointangles

o q q o

R q q R

6106

6106

,...,

,...,


36/46

ME 8209


Now, origin of tool frame, o 6 , is a distance d 6 translated along z 5 (sincez 5 and z 6 are collinear) Thus, the third column of R is the direction of z 6 (w/ respect to the base

frame) and we can write:

Rearranging:

Calling o = [o x o

y o

z ]T , o

c

0 = [x c y

c z

c ]T

1

0

0

606 R d o o o o c

1

0

0

6R d o o o

c

336

236

136

r d o r d o r d o

z y x

z

y

x

c

c

c


37/46

ME 8209


Since [ x c y c z c ]T are determined from the first three joint angles, ourforward kinematics expression now allows us to solve for the first three

joint angles decoupled from the final three. Thus we now have R 3 0

Note that:

To solve for the final three joint angles:

Since the last three joints for aspherical wrist, we can use a set ofEuler angles to solve for them

36

03 R R R

R R R R R T 0310336


38/46

ME 8209

Inverse position

Now that we have [ x c y c z c ]T we need to find q 1, q 2 , q 3 Solve for q i by projecting onto the x i-1, y i-1 plane, solve trig problem Two examples: elbow (RRR) and spherical (RRP) manipulators For example, for an elbow manipulator, to solve for 1, project the arm onto

the x 0 , y 0 plane


39/46

ME 8209

Background: two argument atan

We use atan2() instead of atan() to account for the full range ofangular solutions Called four -quadrant arctan

0,00,02

0,0

0,0

0,2

,2

x y x y

x y x y

x y x y

y x y

x y

undefined

atan

atan

atan

atan


40/46

ME 8209

Example: RRR manipulator

1. To solve for 1, project the arm onto the x 0 , y 0 plane

Can also have: This will of course change the solutions for 2 and 3

c c y x ,21 atan

c c y x ,21 atan


41/46

ME 8209

If there is an offset, then we willhave two solutions for 1: left arm and right arm However, wrist centers cannot

intersect z 0

If x c =y c =0, 1 is undefined i.e. any value of 1 will work

Caveats: singular configurations, offsets


42/46

ME 8209

Left arm: Right arm:

Left arm and right arm solutions

d d y x

y x

c c

c c

,2

,2

222

1

atan

atan-

d d y x

d d y x

y x

c c

c c

c c

,2

,2

,2

222

222

1

atan

atan

atan


43/46

ME 8209

Therefore there are in general two solutions for 1 Finding 2 and 3 is identical to the planar two-link manipulator we have

seen previously:

Therefore we can find two solutions for 3 :


D

a a a a d z d y x

d z s d y x r

a a a a s r

c c c

c

c c

32

23

22

21

222

3

1

2222

32

23

22

22

3

2cos

2cos

23 1,2 D D atan


44/46

ME 8209

The two solutions for 3 correspond to the elbow-down and elbow-uppositions respectively

Now solve for 2 :

Thus there are two solutions for the pair ( 2 , 3 )


333321222

333322

,2,2

,2,2

s a c a a d z d y x

s a c a a s r

c c c

atanatan

atanatan


45/46

ME 8209

In general, there will be a maximum of four solutions to the inverseposition kinematics of an elbow manipulator Ex: PUMA

RRR: Four total solutions


46/46

ME 8209

Spherical configuration Solve for 1 using same method as with RRR

Again, if there is an offset, therewill be left-arm and right-arm solutions

Solve for 2 :

Solve for d 3 :

Example: RRP manipulator

c c y x ,21 atan

1

222

2 ,2

d z s y x r

r s

c

c c

atan

212222

3

d z y x

s r d

c c c

Documents

Module 02 - Robot Kinematics