Review robot kinematics - Dipartimento di Elettronica ed

Control of industrial robots

Review of robot kinematics

Prof. Paolo Rocco ([email protected])Politecnico di MilanoDipartimento di Elettronica, Informazione e Bioingegneria

Control of industrial robots – Review of robot kinematics – Paolo Rocco

Introduction

With these slides we will cover basic elements in robot kinematics.

We will start from a basic problem of representation of a rigid body in space, and then proceed through the formal tools used in robotics till the definition of the direct, inverse and differential kinematics of the manipulator.

All this material is well covered with better detail in any introductory Robotics course at BSc level. It is reviewed here for the sole purpose of making this course self-contained for students who lack this background.

Most of the pictures in these slides are taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009


Position and orientation of a rigid body

Let us consider a rigid body in space:

unit vectors

Position of the origin of the frame (x', y ', z '):

′′′

=′⇒′+′+′=′

z

y

x

zyx

ooo

ooo ozyxo

Orientation of the frame (x ', y ', z '):zyxzzyxyzyxx

zyx

zyx

zyx

zzzyyyxxx

′+′+′=′′+′+′=′′+′+′=′

How can we characterize position and orientation of the rigid body with respect to the frame (x, y, z)?


Rotation matrix

We can gather the elements of x ', y ', z ' in a matrix:

This matrix is called rotation matrix of the frame (x ', y ', z ') with respect to the frame (x, y, z) .Since the following relations hold:

[ ]

′′′′′′′′′

=

′′′′′′′′′

=′′′=zzzyzxyzyyyxxzxyxx

zyxRTTT

TTT

TTT

zzz

yyy

xxx

zyxzyxzyx

0,0,0

1,1,1

=′′=′′=′′

=′′=′′=′′

xzzyyxzzyyxx

TTT

TTT

we have: ( )1−== RRIRR TT orthogonal matrix


Elementary rotations

Let us consider a rotation by an angle α around z axis:

The rotation matrix is thus:

=′

αα−

=′

αα

=′

100

,0

cossin

,0

sincos

zyx

( )

ααα−α

=α1000cossin0sincos

zRSimilarly for the rotations around the other axes.


Representation of a vector

Consider now a point P whose coordinates are expressed in two reference frames:

The coordinates of the same point in the two frames are:

′′′

=′

=

z

y

x

z

y

x

ppp

ppp

pp ,

Therefore:

[ ] pRpzyxzyxp ′=′′′′=′′+′′+′′= zyx ppp

The rotation matrix thus contains the transformation which maps the coordinates expressed in the frame (x', y ', z ') into the coordinates expressed in frame (x, y, z).Inverse transformation: pRp T=′


Composition of rotation matrices

jiR

Let us consider three frames (denoted with 0, 1 and 2) with a common origin.We denote with:

the rotation matrix of frame i with respect to frame j

Thus:

( ) ( )Tij

ij

ji RRR ==

−1

The coordinates of the same point in the three frames can be expressed in different ways:

212

1 pRp = 101

0 pRp = 202

0 pRp =

Rotations can be obtained by composing partial rotations.Partial rotation matrices are multiplied from left to right.

12

01

02 RRR =


Minimal representation of the orientation

A rotation matrix represents the orientation of a frame with respect to another one by means of 9 parameters, among which 6 constraints exist.

In a minimal representation the orientation is described by means of 3 independent parameters.

Possible representations are:

Euler angles (3 parameters) roll-pitch-yaw angles (3 parameters) axis/angle (4 parameters) quaternions (4 parameters)


With ZYZ Euler angles the sequence is composed as:

I) Rotation around Z(angle ϕ)

II) Rotation around Y’(angle ϑ)

III) Rotation around Z’’(angle ψ)

ZYZ Euler angles

( ) ( ) ( ) ( )

−+−+−−−

=ψϑϕ=φ

ϑψϑψϑ

ϑϕψϕψϑϕψϕψϑϕ

ϑϕψϕψϑϕψϕψϑϕ

′′′

csscsssccscsscccssccssccssccc

zyz RRRR


Homogeneous representation

101

01

0 pRop +=

How can we express coordinates of point P in frame 0, based on its coordinates in frame 1?

Homogeneous representation

010

01

10

1 pRoRp +−=

Rotation matrix of frame 1 w.r.t. frame 0

In order to represent in a compact form these transformations, it is advisable to introduce a 4-dim vector:

=

wwp

p~w is a scale factor which is always set to 1 in robotics (it is used in computer graphics)

Inverse transform:


Homogeneous transformations

=

1

01

010

1 T0oRA

Let us introduce the homogeneous transformation matrix (dimensions 4×4):

101

01

0 pRop +=

The relationship:

can be expressed, in terms of homogeneous coordinates, as :

101

0 ~~ pAp =

The inverse transformation is:010

101

01 ~)(~~ pApAp −==

−=1

01

10

101

0 T0oRRA

Composing several transformations: nnn pAAAp ~~ 11

201

0 −=

A10 relates the description (position/orientation) of a point on frame 1 with the

description in frame 0.

N.B. A is not orthogonal


Time dependent rotations

Suppose now that rotation of one frame with respect to the second one changes with time. Let us consider a point P attached to the rotating frame and expressed with the constant vector p′.The coordinates of the same point in the stationary frame are:

( ) ( )pRp ′= tt

Take now the derivative with respect to time:

( ) ( )pRp ′= tt

How can we express the derivative of a rotation matrix?


Derivative of a rotation matrix

Since the rotation matrix is orthogonal we have:

( ) ( ) ( ) ( ) ( ) ( ) 0RRRRIRR =+⇒= tttttt TTT

We conclude that the derivative of a rotation matrix is given by:

If we define the new matrix:

( ) ( ) ( )ttt TRRS =

It turns out that: ( ) ( ) 0SS =+ tt T which means that matrix S is skew symmetric.

( ) ( )( ) ( )tt RtSR ω=

( )

ωω−ω−ω

ωω−=

ωωω

=0

00

,

xy

xz

yz

z

y

x

ωω S

Matrix S then takes the following form:


Skew symmetric matrix

The derivative of vector p(t) can thus be expressed as :

On the other hand the same vector denotes the velocity of point P in the stationary frame:

( ) ( ) ( )( ) ( ) ( )( ) ( )tttt ptSpRtSpRp ωω =′=′=

( ) ( ) ( ) ( ) ( )ttttt ppRp ×=′×= ωω

Thus the skew symmetric matrix S can be interpreted as the operator that computes the cross product.

ω is the angular velocity vector of the rotating frame

symbol × denotes cross product


How does all this relate to the robot?

BASE

END EFFECTOR

JOINTS


The joints

ROTATIONAL JOINTS PRISMATIC JOINTS

Each joint allows for one (and only one) degree of freedombetween two links. We call joint variable the coordinate associated to such degree of freedom, and then we introduce the vector of joint variables:

=

nq

qq

2

1

q

Schematic draws of the joints:


Base frame and tool frame

Let us define a frame attached to the base and a frame attached to the tool.

The tool frame is defined by means of three unit vectors:

ae (approach): approach direction towards the work-piecese (sliding): orthogonal to ae in the sliding plane of the gripperne (normal): orthogonal to both the other ones

pe points to the origin of the tool frame (central point of the gripper).


Direct kinematics

The direct kinematic equation gives position and orientation of the tool frame w.r.t. the base frame, as a function of the joint variables.

( ) ( ) ( ) ( ) ( )

=

1000qpqaqsqnqT

be

be

be

beb

e

Example: planar two-links manipulator

( )

+−+

=

10000001

00

122111212

122111212

sasasccacacs

be qT

(homogeneous transformation matrix)


Direct kinematics

To proceed in a systematic way in the computation of the direct kinematics, a frame should be attached to each link:

( ) ( ) ( ) ( )nnnn qqq 1

2121

01

0 −= AAAqT

link 0 grounded

n is the last link

Every joint allows one degree of freedom

Proceeding iteratively:

( ) ( ) nen

bbe TqTTqT 0

0=


zi lies along the axis of joint i+1 Oi is at the intersection of zi axis with the common normal to axes zi e zi-1;

we denote withOi ' the intersection of this common normal with axis zi-1 xi is aligned with the common normal to axes zi e zi-1, with positive

orientation from joint i to joint i+1 yi completes a right-handed frame

Denavit-Hartenberg convention

It is a convention for the selection of the frames attached to each link.

frame i attached to link i


Denavit-Hartenberg parameters

In order to define a frame w.r.t. to the preceding one, 4 parameters are needed.

ai distance of Oi from Oi' di coordinate on zi-1 of Oi' αi angle around axis xi between axis zi-1 and axis zi computed as positive

counter clockwise ϑi angle around axis zi-1 between axis xi-1 and axis xi computed as positive

counter clockwise

ai and αi are always constant, either ϑi or di is varying

frame i attached to link i


Denavit-Hartenberg method illustrated

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


Homogeneous transformation matrix

How to construct the transformation matrix from frame i-1 to frame i:

I) In order to superimpose frame i-1 to frame i ' we translate the frame along axis zi-1 by a length dirotating by an angle ϑi around zi-1:

−

= ϑϑ

ϑϑ

−′

1000100

0000

1

i

ii d

cssc

ii

ii

A

II) In order to superimpose frame i ' to frame i we translate the frame along axis xi ' by a length ai, rotating of an angle αi around xi ' :

−

=αα

αα′

10000000

001

ii

ii

cssc

ai

iiA

( )

−

−

==αα

ϑαϑαϑϑ

ϑαϑαϑϑ

′−′

−

10000

11

i

i

i

ii

iii

ii dcs

sascccscasscsc

qii

iiiiii

iiiiii

AAA


Joint space and operational space

The joint space is defined by the vector of joint variables:

Direct kinematic relation:

=

nq

q1

qqi = ϑi (rotating joint)

qi = di (prismatic joint)

The operational space is the space where the task that the manipulator has to accomplish is specified. It is defined by the posture x :

φ

=p

x p (position)φ (minimal representation of the orientation)

m components

( )qkx =


Three d.o.f. planar manipulator

We can define the orientation with the angle φ formed by the end effector (vector x3) with axis x0

( )

ϑ+ϑ+ϑ++++

==

φ=

321

123312211

123312211

sasasacacaca

pp

y

x

qkx

33

22

11

003002001

ϑϑϑϑα

aaa

da iiii

++++−

==

10000100

00

123312211123123

123312211123123

23

12

01

03

sasasacscacacasc

AAAT


Inverse kinematics problem

Given position and orientation of the tool frame, find the corresponding joint variables.qx

qT⇒⇒

The problem may admit no solutions (if position and orientation do not belong to the workspace of the manipulator)

The analytical solution (in closed form) may not exist. In this case numerical techniques are used

Multiple or an infinite number of solutions might exist

In general the solution is found without a systematic procedure, rather relying on intuition in manipulating the equations.


Three d.o.f. planar manipulator

122113

122113

sasasappcacacapp

yWy

xWx

+=−=+=−=

φ

φ

Data: px, py, φUnknowns: ϑ1, ϑ2, ϑ3

Let:

Squaring and summing:

( )222222

21

22

21

22

2 ,2Atan

12 cs

csaa

aappc WyWx

=ϑ⇒

−±=

−−+=

Finally:( )

( ) ( )111

2222221

1

2222221

1

,2Atan cs

pp

psapcaas

pp

psapcaac

WyWx

WxWy

WyWx

WyWx

=ϑ⇒

+

−+=

+

++=

213 ϑ−ϑ−φ=ϑ

2 solutions


Anthropomorphic manipulator

Four admissible configurations exist:

right shoulder, upper elbow

left shoulder, upper elbow

right shoulder, lower elbow

left shoulder, lower elbow

For each configuration, two wrist configurations exist


Geometrical Jacobian

Let’s introduce now the linear velocity and the angular velocity of the tool frame (attached to the tool): p and ω.

The goal of differential kinematics is to express these velocities in terms of the joint velocities.

( )( )qqJ

qqJp

O

P

=

=

ω

.

In a compact form: ( )qqJp

v

=

=

ω

The (6×n) matrix: ( ) ( )( )

=

qJqJ

qJO

P

is called geometrical Jacobian of the manipulator.Systematic methods exist to compute the Jacobian.


Analytical Jacobian

Let’s go back to the direct kinematic equation of a manipulator:

Matrix: ( ) ( )( )

=

φ qJqJ

qJ PA is called analytical Jacobian of the manipulator.

( ) ( )( )

==

qqp

qkxφ

where φ is a minimal representation of the orientation. Differentiating w.r.t. time we obtain:

( ) ( )qqJqqqkx A=

∂∂

=

On the other hand:

( )( )( )( )

( )( ) qqJqJ

qqqqqqpp

x

=

∂∂∂∂

=

=

φ

P

φφ


Analytical vs. geometrical Jacobian

Let us thus express the velocity (linear and angular) of the tool frame in terms of the derivatives of p and φ:

( ) ( ) ( ) qJTxTxT0

0Ipv

AAA φφ

φω==

=

=

The relation between analytical and geometrical Jacobian follows: ( ) AA JTJ φ=

( )φφω T=

The link between the angular velocity ω and the derivative of vector φ expressing the orientation is the following one:

where T is a matrix that depends on the representation of the orientation:

( )

−=

ϑ

ϑϕ

ϑϕ

ϕ

ϕ

csssc

cs

0100

φT(for the ZYZ Euler angles)


Kinematic singularities

The equation defining the geometrical Jacobian is:

( )qqJv =

The values of q for which matrix J is rank-deficient are called kinematic singularities. At a kinematic singularity we have:

1. Loss of mobility (it is not possible to impose arbitrary motion laws)2. Possibility of infinite solutions to the kinematic inversion problem3. High velocities in joint space (around the singularity)

The singularities may happen:

1. At the borders of the manipulator work-space2. Inside the manipulator work-space

The latter are more problematic, since they can be incurred with trajectories planned in the operational space.


Kinematic singularities: example

For a two-link manipulator the Jacobian is:

These are singularities at the borders of the workspace.

In these configurations the two columns of the Jacobian are not independent.

+

−−−=

12212211

12212211

cacacasasasa

J

We can compute singularities:

( )

π=ϑ⇔==

00det 2221 saaJ


Kinematic singularities of a complete manipulator

Arm singularity Elbow singularity

Wrist singularity Source: ABB


Consequences of kinematic singularities on robot motion

https://www.youtube.com/watch?v=zlGCurgsqg8

https://www.youtube.com/watch?v=zlGCurgsqg8


Inversion of the differential kinematics

The differential kinematics is linear for a certain value of q:

( )qqJv =

Given a velocity v in the operational space and an initial condition on q we might solve the kinematic inversion problem by inverting the differential kinematics and then integrating. If the Jacobian is square (n = r, number of coordinates in the operational space needed to describe a task):

( ) ( ) ( ) ( )00

1 qqqvqJq +σσ=⇒= ∫−t

dt

However, using this expression directly, drifts of the solution may occur.

The error in the operational space made by the kinematic inversion algorithm is then introduced:

xxe −= d


Inverse of the Jacobian

If we adopt the following dependence of q from e:

( )( )KexqJq += −dA 1

we obtain:

0=+ Kee

and the diagram:

.


Transpose of the Jacobian

If we adopt the following (simpler) dependence:

( )KeqJq TA=

we obtain the diagram:

Review robot kinematics - Dipartimento di Elettronica ed

Documents

Transcript of Review robot kinematics - Dipartimento di Elettronica ed