Embed Size (px)
Citation preview
A R C H I V E S O F M E C H A N I C A L T E C H N O L O G Y A N D A U T O M A T I O N
Vol. 34 no. 3 2014
NORBERT PIOTROWSKI*, ADAM BARYLSKI*
MODELLING A 6-DOF MANIPULATOR
USING MATLAB SOFTWARE
Most of studies describe manipulator simulation with a use of specialist software. Their main
disadvantage is that they have limited functions. This paper presents an alternative approach to
modelling a revolute robot in Matlab software. The manipulator in question is Kuka KR 16-2. The
main problem in robot modelling is the kinematic analysis. The revolute robot consists of six
rotary joints (6-DOF) with a base, a shoulder, an elbow and a wrist. The kinematic problem is
defined as a transformation from the cartesian space to the joint space. In this study the Denavit-
Hartenberg (D-H) model of representation was used to model links and joints. Both forward and
inverse kinematics solutions for this manipulator were presented. The kinematic equations pre-
sented have been implemented in Matlab software. The graphical model of Kuka KR 16-2 was
shown and possibilities of modelling in Matlab were described.
Key words: manipulator, robot modelling, forward and inverse kinematic, simulation
1. INTRODUCTION
The Robot Industries Association (RIA) defines robot as a reprogrammable
multifunctional manipulator which can move material, parts, tools, or special-
ized devices with the aid of variable programmed motions [7]. Robotics improve
and stabilize the quality, increase productivity and releasing from heavy and
often monotonous work. Therefore robots are being used more often in industry.
Simulation and modeling are two processes in the development phase of
a robot that reduce costs and decrease the time for development. Robot simula-
tors are useful tools for developing robot behavior which provide a fast and effi-
cient means for testing robots. Although, there are considerable differences be-
tween the behaviour of the robot in the simulator and in the real world environ-
ment. Software such as Robotics Developer Studio [8] or V-REP [10] are ad-
vanced tools for simulation, however Blender [2] can be used only for modeling.
Moreover, many robot producers delivere tools for modelling and simulation.
* Mgr inż.** Prof. dr hab. inż.
Faculty of Mechanical Engineering, Gdansk University of Technology.
N. Piotrowski, A. Barylski 46
The simulation programs of the Kuka.Sim family are designed for the offline
programming and simulation of Kuka robots. The extensive design library con-
tains all Kuka models, grippers, conveyors and safety fences [6].
In this paper, we describe the modelling and simulation of Kuka KR 16-2 in
Matlab software. A Matlab toolbox for modeling Kuka robots was already de-
veloped [3]. However, this study can be applied in any robot. First, we start with
a kinematic model of a 6-DOF (degrees of freedom) robot. Then, we present
a created graphical model. Using Matlab software to simulate manipulators al-
lows to model any components, to control any operations step by step and to
create different algorithms, such as a collision detection algorithm, a parameter-
ized control algorithm or an algorithm which integrates multiple machines.
2. KINEMATIC MODEL OF A 6-DOF ROBOT
Robots have either translational or rotational joints. A translational moves
a point in space a finite distance along given vector direction. It can be described
by the following homogeneous transformation matrix [1]:
úúúú
û
ù
êêêê
ë
é
=
1000
100
010
001
),,(Transc
b
a
cba (1)
A homogenous transformation matrix also describes the rotational displace-
ment. The first three rows correspond to the x, y, z axes of the reference frame
and the first three columns refer to the x’, y’, z’ axes of the rotated frame [1].
úúúú
û
ù
êêêê
ë
é
-=
1000
0)cos()sin(0
0)sin()cos(0
0001
),(Rotaaaa
ax (2)
úúúú
û
ù
êêêê
ë
é
-
-
=
1000
0)cos()sin(0
0)sin()cos()sin(
00)sin()cos(
),(Rotggggg
gg
gz (3)
Kinematic analysis of a robot mainly includes two aspects, namely the for-
ward kinematic analysis and the inverse kinematics analysis. Both problems are
solved in this chapter.
Modelling a 6-DOF manipulator using Matlab software 47
2.1. Forward kinematics
The forward kinematics analysis means that the location and pose of the
end of the manipulator in a given reference coordinates system can be worked
out with the given geometry parameters of the links and the variables of the
joints for a robot. This relationship can be derived using the Denavit-Hartenberg
convention – an algorithm achieving the kinematics of a chain of rigid bodies.
The coordinate axis for each joint must be chosen properly [4]. The joint dia-
gram is shown in Figure 1.
Figure 1. The joints of the robot with coordinate systems following the DH-convention
The joints of the robot are labelled as A1−A6, the lengths of the links as ai
and di. The dotted lines indicate the placement of the coordinate axes. The ar-
rows indicate the positive rotation direction. It can be observed that the first
three joints constitute an elbow manipulator, and the last three are composed into
a spherical wrist. A right handed coordinate axis is assigned to each joint in such
a manner that the zi vector is parallel with the rotation axis and the xi vector is
perpendicular to zi and zi−1 [4]. The DH parameters are given by the transfor-
mations from the coordinate system i to i+1 via the rotational and translational
transformations given in Table 1. The Range (θmin and θmax) are additional in-
formation about the axis.
N. Piotrowski, A. Barylski 48
Table 1
DH-parameters for the Kuka KR 16-2
i θ [°] d [mm] a [mm] α [°] θmin [°] θmax [°]
1 q1 d1 = 675 a1 = 260 –90 –185 185
2 q2 0 a2 = 680 0 –155 35
3 q3 – 90 0 –a3 = –35 –90 –130 154
4 q4 d4 = 670 0 90 –350 350
5 q5 0 0 –90 –130 130
6 q6 d6 = 115 0 0 –350 350
ixaixdizqiziiH a,,,,
1 RotTransTransRot ×××=+ (4)
Rotz,qi stands for a qi rotation around z axis (eq. 2), Rotx,αi stands for αi rotation
around the x axis (eq. 1) and Transz,di denotes a translation di along z axis,
Transx,ai denotes a translation ai along x axis (eq. 3). Then the homogeneous
transformation from the base coordinate system to the end effector is given by
multiplying the transformation in order:
úúúú
û
ù
êêêê
ë
é
-
-
=
=-××=
1000
0010
)sin()cos(0)sin(
)cos()sin(0)cos(
)90,(Rot),0,(Trans),(Rot
1111
1111
11110
qaqq
qaqq
xdaqzH
(5)
úúúú
û
ù
êêêê
ë
é -
=
=×=
1000
0100
)sin(0)cos()sin(
)cos(0)sin()cos(
)0,0,(Trans),(Rot
2222
2222
2221
qaqq
qaqq
aqzH
(6)
úúúú
û
ù
êêêê
ë
é
-
-
-
=
=-×-×-=
1000
0010
)cos()sin(0)cos(
)sin()cos(0)sin(
)90,(Rot)0,0,(Trans)90,(Rot
3333
3333
3332
qaqq
qaqq
xaqzH
(7)
Modelling a 6-DOF manipulator using Matlab software 49
úúúú
û
ù
êêêê
ë
é
-=
=××=
1000
010
0)cos(0)sin(
0)sin(0)cos(
)90,(Rot),0,0(Trans),(Rot
4
44
44
4443
d
xdqzH
(8)
úúúú
û
ù
êêêê
ë
é
-
-
=
=-×=
1000
0010
0)cos(0)sin(
0)5sin(0)cos(
)90,(Rot),(Rot
55
5
554
xqzH
(9)
úúúú
û
ù
êêêê
ë
é -
=
=×=
1000
100
00)cos()6sin(
00)sin()cos(
),0,0(Trans),(Rot
6
6
66
6665
d
dqzH
(10)
úúúú
û
ù
êêêê
ë
é
=×××××=
1000
65
54
43
32
21
10
61
zzzz
yyyy
xxxx
pnml
pnml
pnml
HHHHHHH (11)
where l, m, n are orthonormal vecotrs and they represent the orientation,
p is a vector which represents the position.
2.2. Inverse kinematics
For manipulators which have six joints with the last three joints intersecting
at a point, it is possible to decouple the inverse kinematics problem into two
simpler problems. They are known respectively, as inverse position kinematics,
and inverse orientation kinematics. To be more precise, the inverse kinematics
problem may be split into two simpler problems, in the case of a six-DOF ma-
nipulator with a spherical wrist. First step is to find the position of the intersec-
tion of the wrist axes. The next is to find the orientation of the wrist. The wrist
position can be found by solving equations (12–14), where px, py, pz, nx, ny, nz are
N. Piotrowski, A. Barylski 50
elements of translational matrix (eq. 11), d6 is the length of the wrist, which for
Kuka KR 16-2 is equivalent to 100 mm and l is the length of the tool.
xxx nldpw ×+-= )( 6 (12)
yyy nldpw ×+-= )( 6 (13)
zzz nldpw ×+-= )( 6 (14)
Inverse position kinematics. In the be-
ginning the first angle q1, which was la-
belled in Figure 2, can be solved. The posi-
tion of the wrist is in point C. Two another
joints, i.e. A and B, have no effect on the
value of the angle q1.
The angle q1 can be found easily:
),(2tan11 xy wwAq = 15)
π),(2tan12 += xy wwAq (16)
This gives two possible solutions for the angle of the first joint. These refer to
the front or the rear arm configuration. For both solutions of angle q1 two solu-
tions for angles q2 and q3 can be obtained, when the elbow is above or below it.
Figure 3. Four configurations of robot: a) front-above, b) front-below, c) rear-above, d) rear-below
Four configurations of Kuka KR 16-2 are shown in Figure 3. The values of
angles q2 and q3 can be found by solving equations for:
Figure 2. Calculating an angle of joint 1
a)
c)
b)
d)
Modelling a 6-DOF manipulator using Matlab software 51
a) a front-above configuration (Figure 3a)
( )ba +-=21q (17)
gd +-=2
π
31q (18)
b) a front-below configuration (Figure 3b)
( )ba --=22q (19)
)2
π3(32 gd ---=q (20)
c) a rear-above configuration (Figure 3c)
( )ba ----= π23q (21)
)2
π3(33 gd ---=q (22)
d) a rear-below configuration (Figure 3d)
( )ba +---= π24q (23)
gd +-=2
π
34q (24)
Inverse orientation kinematics. In the previous section a geometric
approach was used to solve the inverse position problem. The inverse orientation
problem is to find the values of the final three joint variables corresponding to
a given orientation with respect to the wrist frame. There are several methods to
describe the orientation of a rigid body. However, only two are routinely used to
represent the orientation of the robot's wrist, namely Euler and Roll-Pitch-Yaw
angles. In this project, Roll-Pitch-Yaw (RPY) notation was used (Figure 4). In
contrast to the second method, in RPY
angles the last rotation is made about the
x-axis instead of the z-axis [9].
The orientation inverse kinematics
can be interpreted as the problem of
finding a set of RPY angles correspond-
ing to a given rotation matrix R. The
rotation matrix obtained for the spheri-
cal wrist has the same form as the rota-
tion matrix for the RPY transformation.
Since the first three joint angles are
known, the rotation matrix can be con-
structed from the wrist center to the end
effector as a function of q4, q5 and q6.Figure 4. RPY notation
N. Piotrowski, A. Barylski 52
Where R is the given matrix, and 3
0R can be calculated from the eq. (5–7).
),(Rot),(Rot),(Rot yqf xyzRRPY ××= (25)
RRR ×= )'( 30
63 (26)
Now the matrix RRPY and the matrix 63R can be compared. There are two solu-
tions. The first one is when sin(q5) > 0 (eq. 27–29) and the second when
sin(q5) < 0 (eq. 30–32). If q5 equals 0, then other two angles also equal 0.
))3,1(),3,2((2tan 63
6341 RRAq -= (27)
))3,3(,)3,3(1(2tan 63
26351 RRAq -= (28)
))1,3(),2,3((2tan 63
6341 RRAq -= (29)
))3,1(),3,2((2tan 63
6342 RRAq = (30)
))3,3(,)3,3(1(2tan 63
26352 RRAq -= (31)
))1,3(),2,3((2tan 63
6362 RRAq -= (32)
3. GRAPHICAL MODELING AND SIMULATION
In order to create a graphical model of a robot, it is necessary to know all
the dimensions and the location of the respective joints. It can be found easily in
the robot documentation on manufacturer’s website [5]. The robot in question is
shown in Figure 5a.
Figure 5. Kuka KR 16-2: a) drawing [5], b) created graphical model in Matlab software
a) b)
Modelling a 6-DOF manipulator using Matlab software 53
Each part of the created graphical model (Figure 5b) was created by only us-
ing two functions: cylinder and block. Both these functions use Matlab’s func-
tion, which is called patch. A patch graphics object is composed of one or more
polygons. They may be but do not have to be connected. Patches are commonly
used for modelling real-world objects. The cylinder function is responsible for
drawing a cylinder with a given height, diameter and colour. Input parameters
are also an accuracy of an implementation of curvature and a coordinate system.
The coordinate system has the form of a homogeneous matrix, containing infor-
mation about the location and orientation. The block function is responsible for
the creation of a cuboid, with the specified width, length, height and colour. For
the position and orientation of a newly formed body also corresponds to the ma-
trix homogeneous passed as a parameter input function. The location and orien-
tation of a newly formed body are also contained in a homogeneous matrix. Co-
ordinate systems for following joints (A1-A6) can be found in Table 2. Origin 7
is a coordinate system in TCP (Tool Center Point), where l is as length of a tool.
Each link of the robot and a tool or gripper have to be modelled in the corre-
sponding coordinate system.
Table 2
Matlab code: coordinate systems for following joints
% Coordinate systems origin0=Trans(0,0,787);
origin1=origin0*Rot(Z,q1)*Trans(260,0,675)*Rot(X,-90);
origin2=origin1*Rot(Z,q2)*Trans(680,0,0);
origin3=origin2*Rot(Z,q3-90)*Trans(-35,0,0)*Rot(X,-90);
origin4=origin3*Rot(Z,q4)*Trans(0,0,670)*Rot(X,90);
origin5=origin4*Rot(Z,q5)*rot(X,-90);
origin6=origin5*Rot(Z,q6)*Trans(0,0,115);
origin7=origin6*Trans(0,0,l);
The interface of application was completely prepared in Matlab using
the GUIDE tool that provides a library of basic elements from Windows applica-
tions. These are the sliders, buttons, menus, etc. The interface and simulation are
shown in Figure 6. It shows a possibilty of the forward kinematics simulation.
To start the animation, the joint’s variables shoul be given and then the ‘GO’
Button must be pressed. The current coordinates and rotations of the tool and the
current joint’s angles can be followed in windows. After selecting ‘Draw trace’
option, the robot draw a red trace of the tool.
N. Piotrowski, A. Barylski 54
Figure 6. Kuka KR 16-2 robot simulation
4. CONCLUSIONS
Determined mathematical equations, which describe the kinematics of Kuka
KR 16-2 robot and created graphical model allow to control and simulate the
manipulator. It can be done with forward and inverse kinematic. The inverse
kinematics is used to obtain the corresponding joint values, by giving the desired
tool position and orientation. There is possibilty to change eight configurations
of robot and we can choose the most appropriate for a given simulation. With
inverse kinemaics there is possibility to simulate the manipulator, which follows
the path.
REFERENCES
[1] Bajd T., Robotics. Intelligent Systems, Control and Automation, in: Science and Engineer-
ing, Berlin, Springer 2010, p. 9–13.
[2] Blender software, http://www.blender.org (accessed 31.07.2014).
[3] Chinello F., Scheggi S., Morbidi F., Prattichizzo D., KCT: motion control of KUKA robot
manipulators with MATLAB, IEEE, 2011, vol. 18, no. 4, p. 69–79.
[4] Cubero S., Industrial Robotics. Theory, Modeling and Control. Robot Kinematics: Forward
and Inverse Kinematics, [S.l.]: Pro Literatur Verlag 2006.
[5] KUKA Roboter, http://www.kuka-robotics.com/res/sps/e6c77545-9030-49b1-93f5-
4d17c92173aa_Spez_KR_16_en.pdf (accessed 24.07.2014).
[6] KUKA Simulation, http://www.kuka-robotics.com/usa/en/products/software/simulation/ (ac-
cessed 30.07.2014).
Modelling a 6-DOF manipulator using Matlab software 55
[7] Robotics Developer Studio, http://msdn.microsoft.com/en-us/library/bb648760.aspx (ac-
cessed 30.07.2014).
[8] Robotic-Industries-Association: RIA, http://www.robotics.org (accessed 22.07.2014).
[9] Spong, M.W., Hutchinson S., Vidyasagar M., Robot modeling and control. Hoboken, New
Jork, Wiley 2006, p. 1–29.
[10] V-REP, http://www.coppeliarobotics.com (accessed 30.07.2014).
MODELOWANIE MANIPULATORA O SZEŚCIU STOPNIACH SWOBODY
W OPROGRAMOWANIU MATLAB
S t r e s z c z e n i e
W artykule przedstawiono alternatywne podejście do modelowania robota za pomocą opro-
gramowania Matlab. Modelowy manipulator to Kuka KR 16-2. Robot ten ma 6 stopni swobody.
Zadanie kinematyki manipulatorów, które stanowi największą trudność podczas modelowania,
polega na transformacji przestrzeni kartezjańskiej do przestrzeni złącza. Do modelu opisującego
elementy i złącza robota użyto notacji Denvita-Hartenberga. Zaprezentowano rozwiązanie zadania kinematyki prostej i odwrotnej. Równania kinematyczne wprowadzano do programu Matlab.
Zaprezentowano również model graficzny robota oraz opisano inne możliwości, jakie stwarza
Matlab.
Słowa kluczowe: manipulator, modelowanie robota, kinematyka prosta i odwrotna, symulacja
