ENG4406 ROBOTICS AND MACHINE VISION KINEMATICS ANALYSIS OF ROBOTS (Part 2) PART 2 LECTURE 9.

ENG4406 ROBOTICS AND MACHINE VISION

KINEMATICS ANALYSIS

OF ROBOTS(Part 2)

PART 2 LECTURE 9

This lecture continues the discussion on the analysis of the forward and inverse kinematics of robots.

After this lecture, the student should be able to:•Put into practice the concept of inverse kinematics analysis of robots•Derive inverse transformation matrix between coupled links•Formulate the inverse kinematics of articulated robots in terms of the link transformation matrices•Solve problems of robot inverse kinematics analysis using transformation matrices

Kinematics Analysis of Robots II

Inverse Transformation

1000

)cos()cos()cos()sin()sin()sin(

)sin()sin()cos()cos()sin()cos(

0)sin()cos(

1111

1111

1

1

iiiiiii

iiiiiii

iii

ii d

d

a

T

A typical transformation matrix can be partitioned as follow:

to give

10001 PRTii

where

)cos()cos()sin()sin()sin(

)sin()cos()cos()sin()cos(

0)sin()cos(

111

111

iiiii

iiiii

ii

R

)cos(

)sin(

1

1

1

ii

ii

i

d

d

a

P

and

Inverse Transformation

100011 PRR

TTT

ii

The inverse transformation is given by:11 Tii

1000

0100

00)cos()sin(

00)sin()cos(

11

11

01

T

For example, find the inverse of : 10

1T

Inverse Transformation of

1000

0100

00)cos()sin(

00)sin()cos(

11

11

01

T

100

0)cos()sin(

0)sin()cos(

100

0)cos()sin(

0)sin()cos(

11

11

11

11

TRR

0

0

0

0

0

0

100

0)cos()sin(

0)sin()cos(

0

0

0

11

11

PRP T

101

T

1000

0100

00)cos()sin(

00)sin()cos(

11

11

01

T

100011 PRR

TTT

ii

100

0)cos()sin(

0)sin()cos(

11

11

TR

0

0

0

PRT

Therefore

Inverse Transformation of 10

1T

1000

0100

00)cos()sin(

00)sin()cos(

11

11

101

T

100

0)cos()sin(

0)sin()cos(

100

0)cos()sin(

0)sin()cos(

22

22

22

22

TRR

0

)sin(

)cos(

0

0

100

0)cos()sin(

0)sin()cos(

0

0 21

211

22

221

A

AA

PR

A

P T

Find the inverse of

1000

0100

00)cos()sin(

0)sin()cos(

22

122

12

A

T

Inverse Transformation of 11

2T

Therefore

1000

0100

)sin(0)cos()sin(

)cos(0)sin()cos(

2122

2122

112

A

A

T

100

0)cos()sin(

0)sin()cos(

22

22

TR

0

)sin(

)cos(

21

21

A

A

PRT

and

Inverse Transformation of 11

2T

100

0)cos()sin(

0)sin()cos(

100

0)cos()sin(

0)sin()cos(

33

33

33

33

TRR

0

)sin(

)cos(

0

0

100

0)cos()sin(

0)sin()cos(

0

0 32

322

33

332

A

AA

PR

A

P T

Find the inverse of

1000

0100

00)cos()sin(

0)sin()cos(

33

233

23

A

T

Inverse Transformation of 12

3T

Therefore

1000

0100

)sin(0)cos()sin(

)cos(0)sin()cos(

3233

3233

123

A

A

T

100

0)cos()sin(

0)sin()cos(

33

33

TR

0

)sin(

)cos(

32

32

A

A

PRT

and

Inverse Transformation of 12

3T

Inverse Kinematics Problem

Robot forward kinematics involve finding the gripper position and orientation given the angles of rotation of the linkages

Now consider the following problem for the planar robot:

The arm moves to a new location and the position along with the orientation of the gripper w.r.t. frame {0} is known. Can you determine the total rotations of 1, 2, and 3 required for the arm to reach that new position?

This problem is called the robot inverse kinematics problem.

Inverse Kinematics of the Planar Robot

The inverse kinematics problem for the planar robot can be stated as follow:Given the global gripper orientation and position, i.e. given

1000

03

zzzz

yyyy

xxxx

paon

paon

paon

T

Find the joint angles 1, 2, and 3 required for the arm to reach that new position

Inverse Kinematics

To solve the inverse kinematics problem, we know that:

TTTTpaon

paon

paon

zzzz

yyyy

xxxx

03

23

12

01

1000

1000

0100

)sin()sin(0)cos()sin(

)cos()cos(0)sin()cos(

21211321321

21211321321

03

AA

AA

T

Or

We can equate the elements in the above two matrices to try to determine the joint angles

1000

0100

)sin()sin(0)cos()sin(

)cos()cos(0)sin()cos(

1000

21211321321

21211321321

AA

AA

paon

paon

paon

zzzz

yyyy

xxxx

Inverse Kinematics example

Let = 1+2+3. Equate elements (1,1) and (2,1):

)cos()cos( 321 xn

)sin()sin( 321 yn

Provided that 1)( 22 yx nn

We can find using =arctan(ny, nx):

x

y

n

n1321 tan)(

)cos(

)sin()tan(

Inverse Kinematics example

Next, equate elements (1,4) and (2,4):)cos()cos( 21211 AApx

)sin()sin( 21211 AApy

Square both of these equations to get:

)cos()cos(2)(cos)(cos 211212122

2122

12 AAAApx

)sin()sin(2)(sin)(sin 211212122

2122

12 AAAApy

Adding the above two equation yields:

)cos(2 22122

21

22 AAAApp yx

21

22

21

22

2 2)cos(

AA

AApp yx

)cos(

)sin(tan

)(cos1)sin(

)(cos1)(sin

2

212

22

2

22

22

Inverse Kinematics example

To solve for 1, we again reuse the following equations:)sin()cos()cos()cos( 1121211 AApx

)cos()sin()sin()sin( 1121211 AApy

where

)sin(

)cos(

22

221

A

AA

Let)sin(

)cos(

r

r

1

22

tan

rwhere

and can be found as 2 was previously determined.

Substitute r and into px and py to get:

)sin()sin()cos()cos( 11 r

px

)cos()sin()sin()cos( 11 r

py

r

pr

p

y

x

)sin(

)cos(

1

1

Inverse Kinematics example

We can then find 1 using the known :

111

1 tantantanx

y

x

y

p

p

p

p

x

y

p

p1

1

111 tan

)cos(

)sin(tan)(

Summary of inverse kinematics for the planar robot:1) Find = 1+2+3 from

x

y

n

n1321 tan)(

2) Find 2 from )(cos1)sin(,

2)cos( 2

22

21

22

21

22

2

AA

AApp yx

3) Find 1 from

4) Find 3 from 3=-(1+2)

111

1 tantantanx

y

x

y

p

p

p

p

Inverse Kinematics exercise 1

The gripper position and orientation for the planar robot is at:

1000

0100

010

001

1000

1

2

A

A

paon

paon

paon

zzzz

yyyy

xxxx

Where A1=3, A2=2. Find the joint angles and hence determine the robot configuration.

00tantan)( 11321

x

y

n

n

90)cos(

)sin(tan

1)(cos1)sin(

02

)cos(

2

212

22

2

21

22

21

22

2

AA

AApp yx

Inverse Kinematics exercise 1

Assuming an elbow down configuration for link 2:902

X2

Y2

2

Remember, positive 2 means clockwise rotation as follow:

0)( 213

Results:

0

90

90

3

2

1

3

2

2)sin(

3)cos(

22

1221

y

x

p

p

A

AAA

90tantantan 1111

x

y

x

y

p

p

p

p

Inverse Kinematics exercise 1

After 2 has been found:

Visualization of Inverse Kinematics exercise 1

To visualize the arm movement, the robot should looks like this after rotating 1 = 90° (1 is the angle from X0 to X1 measured along Z1).

Y1

X1

X0

Y0,

X2

Y2

X3

Y3

1=90°

To visualize the arm movement, the robot should looks like this after rotating 1 = 90° (1 is the angle from X0 to X1 measured along Z1),

and after rotating 2 = -90° (2 is the angle from X1 to X2 measured along Z2)Note that 3 = 0°

2=-90°

Y1

X1

X0

Y0,

X2

Y2

X3

Y3

Visualization of Inverse Kinematics exercise 1

The orientation of the gripper is as follow:

X3 is in the positive X0 direction Tn 001

Y3 is in the positive Y0 direction To 010

Z3 is in the positive Z0 direction Ta 100

Y1

X1

X0

Y0,

X2

Y2

X3

Y3

A1

A2

TAAp 012 Location of gripper in frame {0}

1000

0100

010

001

1000

1

2

A

A

paon

paon

paon

zzzz

yyyy

xxxx

Visualization of Inverse Kinematics exercise 1

Inverse Kinematics exercise 2

The gripper position and orientation for the planar robot is at:

1000

0100

5001

0010

1000zzzz

yyyy

xxxx

paon

paon

paon

Find the joint angles and hence determine the robot configuration.

900

1tantan)( 11

321

x

y

n

n

0)cos(

)sin(tan

0)(cos1)sin(

12

)cos(

2

212

22

2

21

22

21

22

2

AA

AApp yx

0)( 213

Results:

0

0

90

3

2

1

5

0

0)sin(

13)cos(

22

1221

y

x

p

p

A

AAA

90tantantan 1111

x

y

x

y

p

p

p

p

Inverse Kinematics exercise 2

After 2 has been found:

To visualize the arm movement, the robot should finally looks like this after rotating 1 = 90° (1 is the angle from X0 to X1 measured along Z1), 2 = 0° (2 is the angle from X1 to X2 measured along Z2) and 3 = 0°.

1=90°

Y1

X1

X0

Y0,

X2

Y2

X3

Y3

Visualization of Inverse Kinematics exercise 2

Inverse Kinematics – the general approach

For a more complex robot with n>3 links, it is generally not possible to solve for all the joint variables by just equating the elements in

To solve for the other joint angles, it may be necessary to find

Tpaon

paon

paon

nzzzz

yyyy

xxxx

0

1000

1000

101

23

12

23

12

01

101

03

101

zzzz

yyyy

xxxx

paon

paon

paon

TTTTTTTTT

The elements in the above equation are compared to solve for the other joint angles. It may be necessary to repeat the process with premultiplication of the LHS and RHS with more inverse transformation matrices.

Inverse Kinematics – Summary

• Robot inverse kinematics involve finding the joint variables given the robot arm global position and orientation

• This problem can be solved by equating the overall transformation matrix with the matrix containing the given information and comparing their elements

• The process may need to be repeated with premultiplication of the above equation with inverse transformation matrices

• Although we have use a planar robot to illustrate the concept, the approach can be applied to any robot moving in 3-D space.

•The approach involves the following:

1. Solve for joint variables by comparing elements in

Inverse Kinematics – Summary

Tpaon

paon

paon

nzzzz

yyyy

xxxx

0

1000

2. Repeat step 1 with

,

1000

101

2

zzzz

yyyy

xxxx

n paon

paon

paon

TT

1000

101

112

3

zzzz

yyyy

xxxx

n paon

paon

paon

TTT

etc. until all the joint variables are obtained

Summary

This lecture continues the discussion on the analysis of the forward and inverse kinematics of robots.

The following were covered:•The concept of inverse kinematics analysis of robots•Inverse transformation matrix between coupled links•Inverse kinematics of articulated robots in terms of the link transformation matrices•Robot inverse kinematics analysis using transformation matrices