robot vision lesson 8: robot kinematics matthias rüther

54
Robot Vision SS 2008 Matthias Rüther 1 ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Upload: leala

Post on 19-Jan-2016

114 views

Category:

Documents


4 download

DESCRIPTION

ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther. Contents. Introduction Overview Basic Terms Typical Configurations Geometry Homogeneous Transformations Representing Orientation Direct Kinematics Open Chain Denavit-Hartenberg Typical Configurations Inverse Kinematics Problem - PowerPoint PPT Presentation

TRANSCRIPT

Page 1: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 1

ROBOT VISION Lesson 8: Robot Kinematics

Matthias Rüther

Page 2: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 2

Contents

Introduction– Overview

– Basic Terms

– Typical Configurations

Geometry– Homogeneous Transformations

– Representing Orientation

Direct Kinematics– Open Chain

– Denavit-Hartenberg

– Typical Configurations

Inverse Kinematics– Problem

– Numerical Algorithms

Page 3: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 3

Overview

An industrial robot consists of

– A manipulator that consists of a sequence of rigid bodies (links) connected by means of articulation (joints). It is characterized by an arm (for mobility), a wrist (for dexterity) and an end effector (to perform a task).

– Actuators that set the manipulator in motion by actuating the joints (electric, hydraulic or pneumatic)

– Sensors that measure the manipulators status (proprioceptive sensors) or the status of the environment (exteroceptive sensors).

– A control system that controls and supervises manipulator motion.

Page 4: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 4

Overview

An industrial robot has three fundamental capacities

– Material handling: objects are transported from one location to another, their physical characteristics are not altered (e.g. pallettizing, warehouse loading/unloading, sorting, packaging)

– Material Manipulation: physical characteristics of objects are changed or their physical identity is lost (e.g. welding, painting, gluing, cutting, drilling, screwing, assembly, …)

– Measurement: object properties are measured by sensors on the end effector, so they are able to explore a 3D environment (e.g. quality control by image processing, tactile sensors, …)

Page 5: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 5

Basic Terms

Kinematic chain: sequence of links and joints connecting the end effector to the base. Can be open (single chain) or closed (loop within the chain).

Joints: – Revolute: rotational joint– Prismatic: translational joint

Degree of mobility: each joint typ. provides an additional DOM (e.g. 6DOM necessary for 3D motion)

Accuracy: „How close does the robot get to a desired point?“, distance between desired point and actual point. (off line)

Repeatability: „How close does the robot get to a position it has reached before?“ (on line)

Page 6: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 6

Repeatability and Accuracy

),,(aaaa

zyxP

),,(tttt

zyxP

X

Z

YWhere: Pt is the target position,

Pa the average achieved position,

R is the repeatability, and A is the accuracy.

Repeatability: the measurement of how closely a robot returns to the same position n number of times.

Accuracy: the measurement of how closely the robot moves to a given target coordinate.

Page 7: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 7

Many factors can effect the accuracy of a robot such as:

– environmental factors (eg temperature, humidity and electrical noise);

– kinematic parameters (eg robot link lengths etc);

– dynamic parameters (eg structural compliance, friction);

– measurement factors (eg resolution and non-linearity of encoders and resolvers);

– computational factors (eg digital round off, steady-state control errors); and

– application factors (eg installation errors and errors in defining work-piece coordinate frames).

Page 8: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 8

Why?

Each of these can effect the robot kinematics– depending on the robot model design and manufacturing process the kinematics can

be the most significant source of error.

– if the internal kinematic model of the robot does not match the actual model we can not accurately predict where the end-effector is. 

Make them match by either– tighter manufacturing tolerances; or

– by measuring actual kinematics and incorporating this information into the kinematic model.

Page 9: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 9

Basic Terms

Joint space: end effector position and orientation given in joint coordinates.

Operational space: end effector position and orientation given in world coordinates (e.g. cartesian coordinate system)

Workspace:– Reachable WS: set of positions the EE can reach

– Dexterous WS: set of positions the EE can reach with a given orientation

Redundancy: more DOM than needed are available

Page 10: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 10

Revolute Joint1 DOF ( Variable - )

Prismatic Joint1 DOF (linear) (Variables - d)

Basic Joints

Page 11: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 11

The PUMA 560 has SIX revolute jointsA revolute joint has ONE degree of freedom ( 1 DOF) that is

defined by its angle

1

23

4

There are two more joints on the end effector (the gripper)

An Example - The PUMA 560

Page 12: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 12

Manipulator Structures

Page 13: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 13

Manipulator Structures

Page 14: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 14

Forward and Inverse Kinematics

xw

Joint 1

Joint 2

Joint 3

Link 1z1

World (Base) Coordinate Frame

Tool Coordinate Frame

Joint 1

Joint 2

Joint 3

Link 1z1

World (Base) Coordinate Frame

Tool Coordinate Frame

1

2

zw

yt

zt

Link Space

n variables (1 … n)

Tool Space

6 variables (x,y,z,x,y,z)

Forward K

Inverse K

Page 15: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 15

Co-ordinate Frames

right handed coordinate frames

used as reference frames

x

z

y

xw

World (Base) Coordinate Frame

Link frame

zw

x 2

z 2

Tool Coordinate Frame

yt

zt

Page 16: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 16

F o r w a r d K i n e m a t i c s

F o r w a r d K i n e m a t i c s

Page 17: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 17

The Situation:You have a robotic arm that

starts out aligned with the xo-axis.You tell the first link to move by 1 and the second link to move by 2.

The Quest:What is the position of the

end of the robotic arm? Solution:1. Geometric Approach

This might be the easiest solution for the simple situation. However, notice that the angles are measured relative to the direction of the previous link. (The first link is the exception. The angle is measured relative to it’s initial position.) For robots with more links and whose arm extends into 3 dimensions the geometry gets much more tedious.

2. Algebraic Approach Involves coordinate transformations.

Page 18: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 18

X2

X3Y2

Y3

1

2

3

1

2 3

Example Problem: You are have a three link arm that starts out aligned in the x-

axis. Each link has lengths l1, l2, l3, respectively. You tell the first one to move by 1 , and so on as the diagram suggests. Find the Homogeneous matrix to get the position of the yellow dot in the X0Y0 frame.

H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 )

i.e. Rotating by 1 will put you in the X1Y1 frame. Translate in the along the X1 axis by l1. Rotating by 2 will put you in the X2Y2 frame. and so on until you are in the X3Y3 frame.

The position of the yellow dot relative to the X3Y3 frame is(l1, 0). Multiplying H by that position vector will give you the coordinates of the yellow point relative the the X0Y0 frame.

X1

Y1

X0

Y0

Page 19: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 19

Slight variation on the last solution:Make the yellow dot the origin of a new coordinate X4Y4 frame

X2

X3Y2

Y3

1

2

3

1

2 3

X1

Y1

X0

Y0

X4

Y4

H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 ) * Tx3(l3)

This takes you from the X0Y0 frame to the X4Y4 frame.

The position of the yellow dot relative to the X4Y4 frame is (0,0).

1

0

0

0

H

1

Z

Y

X

Notice that multiplying by the (0,0,0,1) vector will equal the last column of the H matrix.

Page 20: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 20

More on Forward Kinematics…

Denavit - Hartenberg Parameters

Page 21: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 21

Z(i - 1)

X(i -1)

Y(i -1)

( i - 1)

a(i - 1 )

Z i Y i

X i a i

d i

i

IDEA: Each joint is assigned a coordinate frame. Using the Denavit-Hartenberg notation, you need 4 parameters to describe how a frame (i) relates to a previous frame ( i -1 ).

THE PARAMETERS/VARIABLES: , a , d,

Denavit-Hartenberg Notation

Page 22: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 22

Z(i - 1)

X(i -1)

Y(i -1)

( i - 1)

a(i - 1 )

Z i Y i

X i a i d i

i

You can align the two axis just using the 4 parameters

1) a(i-1)

Technical Definition: a(i-1) is the length of the perpendicular between the joint axes. The joint axes is the axes around which revolution takes place which are the Z(i-1) and Z(i) axes. These two axes can be viewed as lines in space. The common perpendicular is the shortest line between the two axis-lines and is perpendicular to both axis-lines.

The Parameters

Page 23: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 23

Z(i - 1)

X(i -1)

Y(i -1)

( i - 1)

a(i - 1 )

Z i Y i

X i a i d i

i

a(i-1) cont...

Visual Approach - “A way to visualize the link parameter a(i-1) is to imagine an expanding cylinder whose axis is the Z(i-1) axis - when the cylinder just touches the joint axis i the radius of the cylinder is equal to a(i-1).” (Manipulator

Kinematics)

It’s Usually on the Diagram Approach - If the diagram already specifies the various coordinate frames, then the common perpendicular is usually the X(i-1) axis. So a(i-1) is just the displacement along the X(i-1) to move from the (i-1) frame to the i frame.

Page 24: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 24

2) (i-1)

Technical Definition: Amount of rotation around the common perpendicular so that the joint axes are parallel.

i.e. How much you have to rotate around the X(i-1) axis so that the Z(i-1) is pointing in the same direction as the Zi axis. Positive rotation follows the right hand rule.

3) d(i-1)

Technical Definition: The displacement along the Zi axis needed to align the a(i-1) common perpendicular to the ai common perpendicular.In other words, displacement along the Zi to align the X(i-1) and Xi axes.

If the link is prismatic, then d(i-1) is a variable, not a parameter

4) i or i

Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi axis.

Z(i - 1)

X(i -1)

Y(i -1)

( i - 1)

a(i - 1 )

Z i Y i

X i a i d i

i

Page 25: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 25

1000

cosαcosαsinαcosθsinαsinθ

sinαsinαcosαcosθcosαsinθ

0sinθcosθ

i1)(i1)(i1)(ii1)(ii

i1)(i1)(i1)(ii1)(ii

1)(iii

d

d

a

Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a transformation matrix from one coordinate frame to the next. Using a series of D-H Matrix multiplications and the D-H Parameter table, the final result is a transformation matrix from some frame to your initial frame.

Z(i - 1)

X(i -1)

Y(i -1)

( i - 1)

a(i - 1 )

Z i Y i

X i a i d i

i

The Denavit-Hartenberg Matrix

Page 26: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 26

i (i-1) a(i-1) di

i

0 0 0 0 0

1 0 a0 0 1

2 -90 a1 d2 2

Z0

X0

Y0

Z1

X2

Y1

Z2

X1

Y2

d2

a0 a1

Denavit-Hartenberg Link Parameter Table

Notice that the table has two uses:

1) To describe the robot with its variables and parameters.

2) To describe some state of the robot by having a numerical values for the variables.

3 Revolute Joints

Page 27: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 27

Z0

X0

Y0

Z1

X2

Y1

Z2

X1

Y2

d2

a0 a1

i (i-1) a(i-1) di

i

0 0 0 0 0

1 0 a0 0 1

2 -90 a1 d2 2

1

V

V

V

TV2

2

2

000

Z

Y

X

ZYX

T)T)(T)((T 12

010

Note: T is the D-H matrix with (i-1) = 0 and i = 1.

Page 28: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 28

1000

0100

00cosθsinθ

00sinθcosθ

T 00

00

0

i (i-1) a(i-1) di i

0 0 0 0 0

1 0 a0 0 1

2 -90 a1 d2 2

This is just a rotation around the Z0 axis

1000

0000

00cosθsinθ

a0sinθcosθ

T 11

011

01

1000

00cosθsinθ

d100

a0sinθcosθ

T22

2

122

12

This is a translation by a0 followed by a rotation around the Z1 axis

This is a translation by a1 and then d2 followed by a rotation around the X2 and Z2 axis

T)T)(T)((T 12

010

Page 29: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 29

Review of Direct Kinematics

What we have:– Geometric layout of robot (constant under robot motion)– Joint positions qnx1 (variable under robot motion)

What we want:– End Effector (EE) position and orientation wrt base coordinate frame (variable

under robot motion)

A coordinate transformation T from EE coordinates to base coordinates, where T is a function of q:

– E.g. position of EE coordinate origin is:

10

)()()( T

baseEE

qpqRqT

1

0

0

0

)(qTpEE

Page 30: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 30

Review of Direct Kinematics

How we get there:– Place local coordinate frame at each joint i, i=1..n:

• Determine common normal (CN) between joint i and i-1 (i.e. line connecting both joint axes in the shortest possible way)

• Place coordinate origin oi-1 at intersection of CN and joint axis i-1– Attention Sciavicco/Siciliano: oi-1 at intersection of CN and joint axis i!

• Set z-axis in direction of joint axis• Set x-axis in direction of CN

– Model robot arm using Denavit-Hartenberg (DH) parameters:• a: shortest distance between consecutive joint axes (length of common normal

CN). : angle between consecutive joint axes around CN.• d: distance between consecutive coordinate origins along i-th joint axis (distance

between i-th coordinate origin and intersection of CN with i-th joint axis.• y or : angle between consecutive CN‘s around joint axis.

• Depending on joint type, either d or y are variables.

i (i-1) a(i-1) di

i

0 0 0 0 0

1 0 a0 0 1

2 -90 a1 d2 2

Page 31: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 31

Review of Direct Kinematics

– From DH-table, calculate T recursively:

i (i-1) a(i-1) di

i

0 0 0 0 0

1 0 a0 0 1

2 -90 a1 d2 2

1000

0100

00cosθsinθ

00sinθcosθ

T 00

00

0

1000

0000

00cosθsinθ

a0sinθcosθ

T 11

011

01

1000

00cosθsinθ

d100

a0sinθcosθ

T22

2

122

12

T)T)(T)((T 12

010

1000

cosαcosαsinαcosθsinαsinθ

sinαsinαcosαcosθcosαsinθ

0sinθcosθ

i1)(i1)(i1)(ii1)(ii

i1)(i1)(i1)(ii1)(ii

1)(iii

d

d

a

T1ii

Page 32: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 32

Inverse Kinematics

(q1 … qn) (x,y,z,x,y,z)

K-1

From Position to Angles

Page 33: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 33

Inverse Kinematics

What we have:– Geometric layout of robot (constant under robot motion)– End Effector (EE) position and orientation wrt base coordinate frame (variable

under robot motion)

What we want:– Joint coordinates qnx1 (variable under robot motion). A mapping f from EE position and orientation to joint coordinates:

Problematic, because:– EE position may be out of reachable workspace (no solution)– EE position and orientation may be out of dexterous workspace (no solution)– EE position may be reachable in several ways (multiple solutions, redundancy)– EE position may be reachable in infinitely many ways (infinite number of solutions,

redundancy)– f is highly nonlinear, no closed form solution for complicated configurations.

zyx

n

zyxf

q

q

,,,,,...1

Page 34: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 34

Inverse Kinematics

How we get there:

– Calculate closed-form solution, if possible• fast

• accurate

• unique for every arm configuration (no general concept)

– Use Iterative Method based on differential kinematics • slower

• gives approximate solution

• converges to a single solution (even if multiple solutions exist)

• general algorithm, applicable to all arm configurations

Page 35: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 35

Multiple Solutions

       Because we are dealing with transcendental functions there is often more than one solution.

eg: cos-1(0.5) = 60º and 300º

Page 36: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 36

Insoluble

Page 37: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 37

1

X

Y

S

Revolute and Prismatic Joints Combined

(x , y)

Finding :

)x

yarctan(θ

More Specifically:

)x

y(2arctanθ arctan2() specifies that it’s in

the first quadrant

Finding S:

)y(xS 22

A Simple Example

Page 38: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 38

2

1

(x , y)

l2

l1

Given: l1, l2 , x , y

Find: 1, 2

Redundancy:

A unique solution to this problem does not exist. Sometimes no solution is possible.

(x , y)l2

l1

l2

l1

Inverse Kinematics of a Two Link Manipulator

Page 39: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 39

l1

l22

1

(x , y)Using the Law of Cosines:

21

22

21

22

21

22

21

22

212

22

122

222

2arccosθ

2)cos(θ

)cos(θ)θ180cos(

)θ180cos(2)(

cos2

ll

llyx

ll

llyx

llllyx

Cabbac

2

2

22

2

Using the Law of Cosines:

x

y2arctanα

αθθ

yx

)sin(θ

yx

)θsin(180θsin

sinsin

11

22

2

22

2

2

1

l

c

C

b

B

x

y2arctan

yx

)sin(θarcsinθ

22

221

l

Redundant since 2 could be in the first or fourth quadrant.

Redundancy caused since 2 has two possible values

The Geometric Solution

Page 40: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 40

21

22

21

22

2

2212

22

1

211211212

22

1

211212

212

22

12

1211212

212

22

12

1

2222

2

yxarccosθ

c2

)(sins)(cc2

)(sins2)(sins)(cc2)(cc

yx)2((1)

ll

ll

llll

llll

llllllll

l1

l22

1

(x , y)

21

21211

21211

1221

11

θθθ(3)

sinsy(2)

ccx(1)

)θcos(θc

cosθc

ll

ll

Only Unknown

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

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

:

abbaba

bababa

Note

The Algebraic Solution

Page 41: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 41

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

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

:

abbaba

bababa

Note

)c(s)s(c

cscss

sinsy

)()c(c

ccc

ccx

2211221

12221211

21211

2212211

21221211

21211

lll

lll

ll

slsll

sslll

ll

We know what 2 is from the previous slide. We need to solve for 1 . Now we have two equations and two unknowns (sin 1 and cos 1 )

2222221

1

2212

22

1122221

221122221

221

221

2211

yx

x)c(ys

)c2(sx)c(

1

)c(s)s()c(

)(xy

)c(

)(xc

slll

llllslll

lllll

sls

ll

sls Substituting for c1 and simplifying many times

Notice this is the law of cosines and can be replaced by x2+ y2

22

222211

yx

x)c(yarcsinθ

slll

Page 42: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 42

A General Solution

A general solution to the IK problem is obtained using differential kinematics.

There exists a linear mapping between EE velocity and Joint velocity.

Joint positions can be computed by integrating velocities over time.

Page 43: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 43

Differential Kinematics

What we have:– Geometric layout of robot (constant under robot motion)– Joint velocities

What we want:– Linear velocity

– Angular velocity

A mapping J from joint velocities to EE velocities:

3 1

Tdx dy dz

pdt dt dt

11 ...

T

nn

dqdqq

dt dt

3 1

T

yx zdd d

dt dt dt

( )

( )P

O

p J q q

J q q

( )p

v J q q

6

P

O n

JJ

J

Page 44: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 44

The Jacobian

Jacobian matrix J(q) gives linear and angular velocity (p‘, ) as a function of joint velocity (q‘)

Depends on the location in joint space, hence J(q)

Can be computed via closed formula.

nO

P

O

P

T

J

JJ

qqJp

v

qqJ

qqJp

qpqRqT

6

)(

)(

)(

10

)()()(

Page 45: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 45

Derivative of a Rotation Matrix

Consider rotation matrix R(t), what is its derivative?

( )

0

( ) , 0

( ) ( ) ( )

T

T T

T T

R R t

RR I

RR RR

S t RR S S

R t S t R t

Interpretation of S: S is the cross-matrix of angular velocities

0

( ) 0 ,

0

z y

z x x y z

y x

S t

Page 46: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 46

Example: rotation about z-axis

We want to compute the time-derivative of Rz(t)

cos sin 0

( ( )) sin cos 0

0 0 1

sin cos 0 cos sin 0

( ) cos sin 0 sin cos 0

0 0 0 0 0 1

0 0

0 0 ( ( )), 0 0

0 0 0

z

T

T

R t

S RR

S t

Page 47: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 47

Velocity composition rule

Consider the coordinate transformation of a point P from coordinate frame 1 to frame 0:

0 0 0 11 1p o R p

point wrt 0 point wrt 1origin of 1 wrt 0 rotation of 1 wrt 0

0 0 0 1 0 11 1 1

0 0 0 1 0 0 11 1 1 1

0 1 01 1

( )

:

0 0 0 1 0 01 1 1 1

0 0 0 01 1 1

p o R p R p

p o R p S R p

set R p r

p o R p r

p o r

If p1 is fixed:

Page 48: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 48

Link Velocity

Consider link i connecting joints i and i+1 with their coordinate frames

Let pi and pi+1 be the coordinate frame origins wrt base coordinate frame

Let rii,i+1 be the coordinate frame origin of i+1 wrt coordinate

frame i, expressed in coordinate frame i1 , 1

1 , 1 , 1 , 1 , 1

1 , 1 , 1

ii i i i i

i ii i i i i i i i i i i i i i i

ii i i i i i i i

p p R r

p p R r R r p v r

R

Lin. velocity of i+1 Lin. velocity of i Lin. velocity of i+1 wrt i

Angular velocity of i

… angular velocity (without derivation)

Angular velocity of i+1

Angular velocity of i

Angular velocity of i+1 wrt i

Page 49: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 49

Link Velocity

For prismatic joint:

, 1

, 1 1 1

1 1 1 , 1

1

0i i

i i i i

i i i i i i i

i i

v d z

p p d z r

For revolute joint:

, 1 1 1

, 1 , 1 , 1

1 1 , 1

1 1 1

i i i i

i i i i i i

i i i i i

i i i i

z

v r

p p r

z

Page 50: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 50

Computation of Jacobian

Let the Jacobian J be partitioned into the following 3x1 vectors:

Prismatic joint:

Revolute joint:

1

1

...P Pn

O On

J J

J

J J

i Pi

i Oi

q J

q J

… contribution of joint i to EE linear velocity

… contribution of joint i to EE angular velocity

0 0i Oi Oi

i Pi i i Pi i

q J J

q J d z J z

1, , ( ) ( )

i Oi i i Oi i

i Pi i i i n i i i Pi i i

q J z J z

q J r z p p J z p p

Page 51: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 51

Computation of Jacobian

1

1

...

0

( )

P Pn

O On

i

Pi

Oi i i

i

J J

J

J J

z

J

J z p p

z

… if prismatic

… if revolute

Page 52: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 52

Example: three link planar arm

0 0 1 1 2 2

0 1 2

1 1 1 1 2 12

0 1 1 1 2 1 1 2 12

1 1 2 12 3 123

1 1 2 12 3 123

0 1 2

1 1 2 1

( ) ( ) ( )( )

0

0 ; ; ;

0 0 0

0

0

0

1

z p p z p p z p pJ q

z z z

a c a c a c

p p a s p a s a s

a c a c a c

p a s a s a s

z z z

a s a s

J

2 3 123 2 12 3 123 3 123

1 1 2 12 3 123 2 12 3 123 3 123

0 0 0

0 0 0

0 0 0

1 1 1

a s a s a s a s

a c a c a c a c a c a c

si…j, ci…j denote sin(qi+…+qj), cos(qi+…+qj)

Page 53: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 53

Iterative Inverse Kinematics

1. Specify target transformation T you want to reach.

2. Make an initial guess for joint coords at time tk: q(tk).

3. Compute T(tk) using forward kinematics.

4. Compute differential motion required to move from T(tk) to T:

5. Compute pseudo-inverse of Jacobian J-1 at the position of q(tk).

6. Add Joint motion to initial guess, make result new initial guess.

7. Repeat 2-6 until differential motion is satisfactorily small.

3 1 3 1 3 1 3 1 3 1 3 1

6 1

( )( ) ; ; ( ) ;

0.5k k k

k k k

k t t tk kt t t

p p tv t t R n s a R t n s a

n n s s a a

11( ) ( ) ( )k k kq t q t J v t t

Page 54: ROBOT VISION Lesson 8: Robot Kinematics Matthias Rüther

Robot Vision SS 2008 Matthias Rüther 54

Example: Robotics Toolbox

http://www.cat.csiro.au/cmst/staff/pic/robot/