Kinematics Notes - Faculty of Computing, Engineering and Sciences

copygrouperΜηχανική

13 Νοε 2013 (πριν από 3 χρόνια και 6 μήνες)

52 εμφανίσεις

Faculty of Computing, Engineering and Technology


Module Name: MSc Robotics and Automation
Forward Kinematics
Forward kinematics is the mathematics behind calculating an end point position (and
orientation) from knowing the joint lengths and angles.
Single link kinematics


Figure 1 Simple two dimension kinematics with a single link

The forward kinematics of a simple link with link length L and moving through angle Ө
are:



sin
cos
Lz
Lx

Example 1
If the link length, L=50, and the angle, Ө=45˚, the endpoint (x,z) would be (35,35).
Two link kinematics


Figure 2 Rotation of a link


z
θ
1
L
2

L
1

θ
2
End point

x

z
L

End point
Ө

x

Referring to figure 2, the geometric solution for two links can be solved as:


 
21211
21211
sinsin
coscos


LLz
LLx

The 3-D graph
From now on, we are going to use 3-D coordinates on the 3-D coordinate graph. It is
essential that you get this graph the right way round. We are going to flatten our xy graph
used above and form another axis, the z-axis to define the height. Look at the graph
below, the y-axis is going away from you, into the page.

Figure 3 The 3-D graph
Rotation
There are three possible rotations for an object, that is, every possible orientation of an
object can be described as a component of three distinct rotations. A rotation about the x-
axis, about the y-axis, and about the z-axis. Imagine that each of these axes act as an axle
running through the centre of the object, all perpendicular to each other, and the object is
allowed to rotate about each one. The effect of rotating a block about the y-axis would be
as in Figure 4.

Figure 4 Effect of rotation about the y-axis
y y
x

z
y
Direction of rotation
How do we determine which way is a positive rotation, e.g. 10˚ and negative, e.g. -10˚?
We can use the right-hand screw rule. If we imagine we are screwing a screw so it
translates in the direction the y-axis is pointing, we would have to rotate it clockwise (or
from the z-axis to the x-axis).

Figure 5 The right-hand screw convention

So, rotating from the z-axis to the x-axis is a positive angle, but rotating from the x-axis
to the z-axis is the opposite direction from your normal screw and is a negative angle.
Likewise, rotating from the x-axis to the y-axis (about z) is a positive angle and so is
rotating from the y-axis to the z-axis.

Looking at the page with the y-axis pointing away from you, a rotation of a link having
length ’r’ and rotating about the y-axis by a value, -θ, would be shown as in Figure 6.



Figure 6 Rotation of a link
x

z
y
r

z
θ
x

Homogenous Matrices
We are going to repeat the forward kinematics for this link using matrices. The
advantages of this method are that the system can work easily in 3-D space, and it is
easily adapted for a multi-link robot arm. The matrices we will use will be 2-D 4 x 4
(think of a 2-D array in programming). The general method of multiplying a 2 x 2 matrix
is as below:
dhcfdgce
bhafbgae
hg
fe
dc
ba



Example 2
Multiply two matrices to get the result:
5043
2219
87
65
43
21



We will use 4 x 4 matrices we will use consist of a rotation part and a translation part as
shown below, these are known as ‘homogenous matrices’:



The link motion is going to be described by two matrices that are multiplied together, one
to represent a rotation (about y) and another to represent a translation along x.

We will define the link direction along the x-axis because if the link had a rotation of
zero degrees it would lie along the x-axis. Refer to appendix 1 for the list of the three
translation matrices, and the three rotation matrices.

The rotation of θ about the y-axis matrix is defined as:
1000
00
0010
00


CS
SC
where Cθ = cos θ, and Sθ = sin θ

The translation along the x-axis by ‘x’ matrix is defined as:
Rotation
part of
matrix
Translation
0 0 0 1
Rotation about y X Translation along x
1000
0100
0010
001 x

Example 3-Calculating the end point position
Going back to example 1, where the link length is 50, and rotated by -45˚, the resulting
matrix would be:
1000
357.007.0
0010
357.007.0
1000
0100
0010
50001
1000
07.007.0
0010
07.007.0 




so, the link would be at the position 35 along the x-axis, and 35 along the z-axis. (the
value along the y-axis is still zero as there has been no rotation about z).
End point rotation
The rotation about the x, y and z axes can be calculated from the rotation part of the
resulting matrix, this is the 3 x 3 matrix defining rotation:

333231
232221
131211
rrr
rrr
rrr




 
 
)Ycos(r,)Ycos(rAtan2X
)Ycos(r,)Ycos(rAtan2Z
rr,r-Atan2Y
3332
1121
21
2
11
2
31





where Atan2 computes tan
-1
(y/x) but uses the signs of both x and y to determine the
quadrant of the angle from 0°to 360°. Atan2 is in the form of Atan2 (y value, x value).
For example, Atan2(-2.0,-2.0) =-135˚;wheras Atan2(2.0,2.0)=45˚. We are computing
angles in the full range of 0 to 360˚.

In example 3 above, the 3 x 3 rotation matrix is:
7.007.0
010
7.007.0 

And so the rotation is:
Y=315˚ (or -45˚)
Z=0
X=0
x
y
z
Defining multiple links
In order to really see the benefit of using matrices, a two link example is introduced.
Referring to figure 2, notice that, when both links are at rest (zero angles or rotation for
both), the links lie along the x-axis. Translation will therefore be along x.
The rotation of both links is about the y-axis with link 1 rotating in a negative direction
and link 2 rotating in a positive direction. Note that the angle of rotation of link 1 is with
respect to the current orientation of link 1 (shown with the dashed line). Notice how the
local coordinate frame moves and rotates along with each link. Each link not only
describes the position of the new frame origin but also its rotation.

The resulting endpoint will be found from multiplying the matrices in this order:

or
1000
0100
0010
001
1000
00
0010
00
1000
0100
0010
001
1000
00
0010
00
2
22
221
11
11
L
CS
SCL
CS
SC







Example using MATLAB
In figure 6 above, if θ
1
=-45°, and θ
2
=30°, whilst L1=50 and L2=40.
The matrices would be:
1000
0100
0010
40001
1000
0866.005.0
0010
05.00866.0
1000
0100
0010
50001
1000
0707.00707.0
0010
0707.00707.0





Letting each matrix be a variable a,b,c,d, we need to find a x b x c x d.
The matrix is entered with spaces between the columns, and a semi-colon separating each
row, as in:

%Forward kinematics for two link arm
a=[0.707 0 -0.707 0;0 1 0 0;0.707 0 0.707 0;0 0 0 1];
b=[1 0 0 50;0 1 0 0;0 0 1 0;0 0 0 1];
c=[0.866 0 0.5 0;0 1 0 0;-0.5 0 0.866 0;0 0 0 1];
d=[1 0 0 40;0 1 0 0;0 0 1 0;0 0 0 1];

and the answer obtained from:
r=a*b*c*d




Rotation about
y by θ
1

Translation
along x by L
1

X
Rotation
about y by θ
2
X

Translation
along x by L
2

X
which is:

r =

0.9658 0 -0.2588 73.9805
0 1.0000 0 0
0.2588 0 0.9658 45.7005
0 0 0 1.0000

Thus, the end point is at x=73.9805, y=0, z=45.7005.

The end point rotation (about Y) can be calculated thus:


22
09658.0,2588.0-Atan2 Y
=-15° or 345°

or, using MATLAB

The 4x4 matrix, ‘r’, is calculated for the end point, the orientation can be calculated using
MATLAB as:
%Find orientation of final point
Y=atan2(-r(3,1),sqrt(r(1,1)^2+r(2,1)^2));
Z=atan2(r(2,1)/cos(Y),r(1,1)/cos(Y));
X=atan2(r(3,2)/cos(Y),r(3,3)/cos(Y));

%Convert to degrees
todeg=180/pi;
Xd=X*todeg
Yd=Y*todeg
Zd=Z*todeg


Another example using MATLAB
I have made a variable, ‘t’ equal to the angle in radians. Then I have defined ‘ct’ and ‘st’
to be the cos and sin of this angle respectively.

>> t=
-
45/180*pi;

>> ct=cos(t);
>> st=sin(t);
>> roty=[ct 0 st 0;0 1 0 0;-st 0 ct 0;0 0 0 1];
>> transx=[1 0 0 50;0 1 0 0;0 0 1 0;0 0 0 1];
>> link=roty*transx

link =

0.7071 0 -0.7071 35.3553
0 1.0000 0 0
0.7071 0 0.7071 35.3553
0 0 0 1.0000

Appendix 1 Rotation and translation (homogenous) matrices
Here is a list of rotation and translation matrices useful for defining different links of a
robot:
Rotation about Z:
1000
0100
00
00
:



CS
SC
i

Translation along X:
1000
0100
0010
001
:
x
x

Rotation about X:
1000
00
00
0001
:



CS
SC
i

Translation along Y:
1000
0100
010
0001
:
y
y


Rotation about Y:
1000
00
0010
00
:



CS
SC
i

Translation along Z:
1000
100
0010
0001
:
z
z


Where
 cosC
, and
 sinS

DH Matrices
These are used to simplify the rotation and translation matrices as explained above. There
are just four parameters and represent translations and rotations in the following order:
i) rotation about the z-axis (),
ii) translation along the x-axis (a),
iii) translation along the z-axis (d), and,
iv) rotation about the x-axis ().



See my website for a DH matrix simulation using Java:
http://www.fcet.staffs.ac.uk/sow1/my_resources.htm
X
Y
a
a
i
is the distance from Z
i

to Z
i+1
along the X
i
axis.



i


is the angle between
Z
i
and Z
i+1
along the X
i
axis.
d
d
i
is the distance from X
i-1
to X
i
along the Z
i
axis.



i
is the angle between X
i-1
and
X
i
about the Z
i
axis.
i)
ii)
iii)
iv)
Z
The DH table for the four parameters for each link of a robot arm can be formed as
below:

α

a θ d
1


2


3


4


5


..










DH Example 1
Looking from above (along the negative z-axis) at a two link SCARA type robot arm,
both links rotate about the z-axis by 
1
and 
2
:


The links rotate about the z-axis, and the link lengths lie along the x-axis. The parameters
for link 1 are therefore: =0˚, a=125,d=0, =
1
. The parameters for link 2 are: =0˚,
a=60,d=0, =
1
.

The DH table would be:
α

a θ d

1

0

125


1

0

2

0

60

2

0



DH Example 2
Imagine a robot arm with two links that can rotate about the y-axis. Note that the y-axis is
going into the page. The links can rotate as in the dashed lines shown, these would be
rotating in the negative y direction.

Z
Y
X
Z
125 60

1

2


In DH matrices, there is no rotation about the y-axis, therefore we must rotate the
coordinate frame so that either the z or the x axes correspond to the rotation axis. I will
choose to rotate the x-axis by -90˚ so that the z-axis is going into the page (where the y-
axis previously was) and the y-axis will be pointing downwards.

This link can also rotate about the z-axis by ‘
1
‘and positive rotation will make the link
go upwards, the parameters for link 1 would be: =-90˚, a=0,d=0, =
1
.
Next, we have a link translation along the x-axis by 100, the end point of this link will
rotate by 
2
about the z-axis, so the parameters for link 2 are: =0˚, a=100,d=0, =
2
.
The following link has a length of 50 along the x-axis, so the parameters are:
=0˚, a=50,d=0, =0.
The DH table would be:
α

a

θ d


1

-90

0 0 0

2

0 100


1

0

3

0 50

2

0

100
X
Z
50
Y
X
Z (into page)
Y

(set the orientation of the axis to rotation about Y)
(translation along ‘a’ comes after the  rotation)
What difference would it make to the
direction of the ‘’ angles if we initially
rotate about the x-axis by +90˚?
DH Example 3

DH Example 4

X
Y
Z
A revolute robot rotates about the Z-axis base by

1

Whilst 
2
,
3
and 
4
rotate about the Y-axis.

The DH matrix would be:
α

a θ d
1

-90

0 θ
1

100

2

0 350

θ
2

0
3

0 200

θ
3

0
4

0 75 θ
4

0


1


3


4


2

100
350
200
75

(α rotation happens
after θ and
translation along ‘a’
in point 1)
θ
1

θ
2

200

150
20
100
A SCARA robot rotates abo
ut the z
-
axis
base by 
1
(shoulder)

2
is the elbow rotation about the z-axis.
The DH matrix would be:
α

a θ d
1

0

0 θ
1

200

2

0

150

0 -20
3

0

100

θ
2

0



Robotics Toolbox
To create the robot in example 2 using the ‘link’ command:


An extract from the Robotics Toolbox manual defines link:

















DH and homogenous matrices
The DH matrix is an easy method of defining a homogenous matrix. The homogenous
matrix for the DH transform is:
1000
dccscss
dsscccs
a0sc




l1=link([-pi/2 0 0 0])
l2=link([0 100 0 0])
l3=link([0 50 0 0])
L{1}=l1
L{2}=l2
L{3}=l3
R=robot(L)
drivebot(R)
L = link([alpha, a, theta, d])
Description The link function constructs a link object. The object contains kinematic and dynamic
parameters
as well as actuator and transmission parameters. The first form returns a default object, while the
second and third forms initialize the kinematic model based on Denavit and Hartenberg parameters.
By default the standard Denavit and Hartenberg conventions are assumed but a flag (mdh) can be set
if modified Denavit and Hartenberg conventions are required. The dynamic model can be initialized
using the fourth form of the constructor where dyn row is a 1 20 matrix which is one row of the
legacy dyn matrix.
The second last form given above is not a constructor but a link method that returns the link transformation
matrix for the given joint coordinate. The argument is given to the link object using parenthesis.
The single argument is taken as the link variable q and substituted for or D for a revolute or
prismatic link respectively.
The Denavit and Hartenberg parameters describe the spatial relationship between this link and the
previous one. The meaning of the fields for each model are summarized in the following table.
alpha i i 1 link twist angle
A Ai Ai 1 link length
theta i i link rotation angle
D Di Di link offset distance
sigma i i joint type; 0 for revolute, non-zero for prismatic
Note that the α angle is in radians, θ is set to zero (the
initial pose of the robot) but the command ‘drivebot’
allows the user to modify θ interactively using slider
controls. Also note that the slider for θ1 is redundant,
this alters the θ in the first column