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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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)?
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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.
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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=′
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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 =
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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)
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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:
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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?
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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:
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
How does all this relate to the robot?
BASE
END EFFECTOR
JOINTS
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
2
1
q
Schematic draws of the joints:
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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).
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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)
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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=
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
Denavit-Hartenberg method illustrated
https://www.youtube.com/watch?v=rA9tm0gTln8
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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 =
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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.
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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.
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
φφ
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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)
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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.
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
Kinematic singularities of a complete manipulator
For the six dof robot UR5 by Universal Robots:
The singular configurations are:
Source: Alumotion
Control of industrial robots – Review of robot kinematics – Paolo Rocco
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
Control of industrial robots – Review of robot kinematics – Paolo Rocco
Inverse of the Jacobian
If we adopt the following dependence of q from e:
( )( )KexqJq += −dA 1
we obtain:
0=+ Kee
and the diagram:
.
Control of industrial robots – Review of robot kinematics – Paolo Rocco
Transpose of the Jacobian
If we adopt the following (simpler) dependence:
( )KeqJq TA=
we obtain the diagram: