Aansturen en afregelen van een robot met 6
vrijheidsgraden
Laurent Merlevede
Promotoren: dhr. Stijn Derammelaere, dhr. Simon Houwen
Masterproef ingediend tot het behalen van de academische graad van
Master of Science in de Industriële Wetenschappen: Elektromechanica
Vakgroep Industrieel Systeem- en Productontwerp
Voorzitter: prof. Kurt Stockman
Faculteit Ingenieurswetenschappen en Architectuur
Academiejaar 2013-2014
Aansturen en afregelen van een robot met 6
vrijheidsgraden
Laurent Merlevede
Promotoren: dhr. Stijn Derammelaere, dhr. Simon Houwen
Masterproef ingediend tot het behalen van de academische graad van
Master of Science in de Industriële Wetenschappen: Elektromechanica
Vakgroep Industrieel Systeem- en Productontwerp
Voorzitter: prof. Kurt Stockman
Faculteit Ingenieurswetenschappen en Architectuur
Academiejaar 2013-2014
Voorwoord
Na deze aangename en leerrijke periode als student ben ik trots om de voorbije vier jaar te kunnen bekronen
met deze masterproef. Het schrijven van deze thesis was voor mij zowel een uitdaging als een kans om mijn
grote interesse voor regeltechniek en mechatronica naar voren te brengen. Daarnaast schiep het niet alleen de
mogelijkheid om de kennis die ik jdens mijn opleiding verwierf ten volste te benu en, maar deze ook op vele
vlakken uit te breiden. Het feit dat deze masterproef niet louter zuiver theore sche kennis vereiste, maar ook
een groot deel prak sche kennis, sloot perfect aan bij mijn visie op een uitdagend project. De mogelijkheid om
een zes-assige robotarm aan te sturen en hierbij de controller volledig eigenhandig te ontwerpen kon ik dan ook
niet aan me laten voorbijgaan.
De vereiste begeleiding kon natuurlijk niet ontbreken om het uiteindelijke werk tot stand te brengen. Eerst en
vooral zou ik graag mijn promotoren dhr. S jn Derammelaere en dhr. Simon Houwen willen bedanken voor
hun inzet en de nodige steun doorheen mijn masterproef. Ik kon op elk moment bij jullie terecht voor vragen
of opmerkingen die met veel plezier werden beantwoord. Hierdoor kon ik dan ook steevast met hernieuwd
enthousiasme verder werken. Het was een genoegen om met jullie samen te werken.
Ook zou ik graag dhr. S jn Huysentruyt willen bedanken voor zijn grote bijdrage aan mijn masterproef. Dankzij
de inspanningen in het kader van zijn eindwerk, dat aan de basis lag van het mijne, kreeg ik de kans om dit project
tot een goed einde te brengen.
Verder wil ik dhr. José Vangheluwe bedanken die mij telkens bijstond bij het technische aspect van het project.
De samenwerking met u was een aangename ervaring. Ten slo e wil ik mijn ouders en vrienden bedanken voor
hun ondersteuning en advies jdens deze boeiende periode.
Merlevede Laurent
Juni 2014
i
Abstract
The University of Ghent Campus Kortrijk has a KUKA KR15/2 industrial robot at its disposal. However, its
controller is outdated and can be defined as a black box, which means there is no knowledge of its internal
func onality. Therefore the opportunity was given to master students to design and implement every aspect
of an en rely new controller based on modern technology. In the first place, an open architecture subs tutes
the black box which gives a be er apprehension of the controller's internal func onality. Secondly, abundant
knowledge on several theore cal and prac cal aspects can be obtained throughout the process.
This thesis is the follow-up of the master project wri en by S jn Huysentruyt en tled Drive and Control of a
6-axis Robot. The project in ques on describes a selec on of standard components which are indispensable to
build a real- me robot controller, such as several drives and an industrial PC. The choice was made to build the
controller by using Beckhoff components as their automa on pla orm TwinCAT®3 provides a MATLAB®/Simulink
integra on. In this way, it is possible to design a control algorithm in MATLAB®/Simulink and compile it into
a TwinCAT®object which can be run in real- me in the industrial PC. This creates the opportunity to develop
every aspect of the posi on regula on for each axis of the robot using the MATLAB®/Simulink Engineering
Environment. This first thesis also provides the incen ve for tuning the axes of the robot by designing a closed
loop regula on for the last three axes. These axes are necessary to control the orienta on of the end of the arm
(i.e. the wrist). In addi on, the forward and inverse kinema c transforma ons were examined.
This thesis, on the other hand, handles the tuning of the first three axes. These axes mainly control the posi on
of the end effector. By execu ng a system iden fica on, the necessary bode characteris cs of the axis can be
acquired. A erwards, considering the bode stability criterion, a phase margin can be premised to obtain a robust
and dynamic PI velocity controller. Once the desired closed-loop characteris cs have been achieved, a suitable
propor onal gain for the posi on controller can be found. However, the dynamic proper es of the first and the
second axis in par cular, vary in func on of the other axes' posi on. Accordingly, a posi on controller of which
the propor onal gain adjusts to these varying proper es, has been developed. In this way, all the axes of the
industrial robot have been equipped with a stable closed-loop posi on regula on.
In order for the end effector to reach a desired pose (posi on and orienta on), both the forward and inverse
kinema c transforma on need to be implemented. The first is necessary to calculate the actual pose in func on
of the actual axes' posi on, while the la er provides the desired posi on for every axis in func on of the desired
pose. The transforma ons as well as the closed-loop regula ons for each axis have been implemented in a
MATLAB®/Simulink algorithm, compiled into a TwinCAT®object and run in real- me in the industrial PC. By doing
so, the end effector of the robot can now accurately achieve the desired posi on and orienta on.
For numerous applica ons, it is requisite that the end effector delineates a certain defined path. Dependent on
the final applica on of the robot, there are two different ways in which a desired trajectory can be described. As
for a pick and place unit, the only concern is that the desired end posi on is a ained fast and accurately. This
is called a point-to-point mo on. In applica ons with welding robots on the other hand, every discrete point of
the trajectory needs to be reached as precise as possible. This is called a trajectory mo on. Both mo ons are
described in this thesis as well as the applied modifica ons to the closed-loop posi on regula ons for op mizing
the behaviour of every axis during the mo on. Finally, a trajectory generator has to be developed as to provide
the kinema c algorithm with the required pose coordinates to calculate the posi on of the axes. In this way, the
end effector is able to execute a user-defined path.
ii
Lijst met a or ngen
A
AC
Alterna ng Current
C
Cascade
Parallel
E
EtherCAT
Ethernet for Control Automa on Technology
F
FFT
FM
Fast Fourier Transform
Fasemarge
I
I-ac e
IEC
IGBT
iPC
Integrerende ac e
Interna onal Electrotechnical Commission
Insulated Gate Bipolar Transistor
industriële PC
N
NVRAM
Non-vola le random-access memory
P
P-ac e
Propor onele ac e
iii
P-regelaar
PI-regelaar
PID-regelaar
PC
PLC
Pose
Propor onele regelaar
Propor oneel integrerende regelaar
Propor oneel, integrerend en differen ërende regelaar
Personal Computer
Programmable Logic Controller
Posi e en oriënta e
T
es mate
TwinCAT
Transfer func on es mate
The Windows Control and Automa on Technology
X
XAE
XAR
XAT
eXtended Automa on Engineering
eXtended Automa on Run me
eXtended Automa on Technology
iv
Inhoudsopgave
1 Inleiding
1
2 KUKA KR15/2 industriële robot
2.1 Mechanische opbouw . . . . . . . . . . . . .
2.2 Hardware van de nieuwe sturing . . . . . . .
2.2.1 AX5000-serie servo drives . . . . . .
2.2.2 CX2030 Industriële PC . . . . . . . .
2.2.3 EtherCAT® . . . . . . . . . . . . . . .
2.2.4 Overzicht hardwarema ge opstelling
.
.
.
.
.
.
4
4
5
5
6
7
7
.
.
.
.
.
.
.
.
.
.
8
8
11
14
15
16
16
16
23
26
29
.
.
.
.
.
30
32
32
34
36
38
.
.
.
.
39
39
41
41
43
6 Conclusies
6.1 Behaalde resultaten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2 Sugges es voor verder onderzoek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
45
45
46
A KUKA KR15/2
A.1 Mechanische gegevens . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.2 Elektrische gegevens . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
48
48
50
3 Posi
3.1
3.2
3.3
3.4
3.5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
eregeling van de robot-assen
Cascaderegeling . . . . . . . . . . . . . . . . . . . . . . . . . .
Systeemiden fica e . . . . . . . . . . . . . . . . . . . . . . . .
Ontwerpen van regelaars aan de hand van bodekarakteris eken
3.3.1 Instellen snelheidsregelaar . . . . . . . . . . . . . . . .
3.3.2 Instellen posi eregelaar . . . . . . . . . . . . . . . . .
Afregelen van de assen . . . . . . . . . . . . . . . . . . . . . .
3.4.1 Afregelen van As 1 . . . . . . . . . . . . . . . . . . . .
3.4.2 Afregelen van As 2 . . . . . . . . . . . . . . . . . . . .
3.4.3 Afregelen van As 3 . . . . . . . . . . . . . . . . . . . .
Besluit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4 Kinema sche transforma e
4.1 Berekenen van de ashoeken in func e van de gewenste posi
4.1.1 Berekenen van de hoeken θ1 , θ2 en θ3 . . . . . . . .
4.1.2 Berekenen van de hoeken θ4 , θ5 en θ6 . . . . . . . .
4.2 Implementa e kinema sche transforma e . . . . . . . . .
4.3 Besluit . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5 Traject
5.1 Punt-tot-punt-beweging . . . . . . .
5.2 Baanbeweging . . . . . . . . . . . .
5.2.1 Opbouw trajectgenerator .
5.2.2 Performan e baanbeweging
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
e en oriënta
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
e van de eindeffector
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
v
B MATLAB Code
B.1 Systeemiden fica e: opstellen bodekarakteris ek . . . . . . . .
B.2 As 1: berekening afstand R . . . . . . . . . . . . . . . . . . . .
B.3 As 2: berekening afstand R . . . . . . . . . . . . . . . . . . . .
B.4 Trajectgenerator: baanbeweging X-coördinaat . . . . . . . . . .
B.5 Trajectgenerator: baanbeweging Y-coördinaat . . . . . . . . . .
B.6 Trajectgenerator: baanbeweging Z-coördinaat . . . . . . . . . .
B.7 Trajectgenerator: baanbeweging roll-, pitch-, en yaw-coördinaat
B.8 Het kinema sch model voor de KUKA KR15/2 industriële robot .
B.9 Snelheidsfeedforward . . . . . . . . . . . . . . . . . . . . . . .
B.10 Volledig MATLAB®/Simulink model . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
51
51
51
52
53
54
54
55
56
58
59
vi
Lijst van figuren
1.1
1.2
1.3
KUKA KR15/2 industriële robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Cascaderegeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Cascaderegeling, torque mode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
2
3
2.1
2.2
2.3
2.4
2.5
Standaard zes-assige robotarm . . . . . . . . . . . . .
De geselecteerde Beckhoff servo drives en remchopper
CX2030 Industriële iPC van Beckhoff . . . . . . . . . .
Overzicht eXtended Automa on Technology (XAT) . . .
Overzicht hardwarema ge opstelling . . . . . . . . . .
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
3.13
3.14
3.15
3.16
3.17
3.18
3.19
3.20
3.21
3.22
3.23
3.24
3.25
3.26
3.27
3.28
3.29
3.30
4.1
4.2
4.3
4.4
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4
5
6
6
7
Een algemeen gesloten regelsysteem . . . . . . . . . . . . . . . . . . . . . . . . . . .
Cascaderegeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Situering regelaars in posi on mode . . . . . . . . . . . . . . . . . . . . . . . . . . .
Situering regelaars in velocity mode . . . . . . . . . . . . . . . . . . . . . . . . . . .
Situering regelaars in torque mode . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Stroomgeregeld en mechanisch proces als onbekend systeem . . . . . . . . . . . . .
Opmeten van een onbekend systeem via de niet-parametrische methode . . . . . . .
Bodekarakteris ek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Sweep ingangssignaal (koppel) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Systeemresponsie (snelheid) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Bodekarakteris ek: fasemarge (FM) . . . . . . . . . . . . . . . . . . . . . . . . . . .
Snelheidsregelkring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Posi eregelkring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
As 1: bodekarakteris ek (In = koppel, uit = snelheid) . . . . . . . . . . . . . . . . . .
Toestand met een klein (links) en groot (rechts) massatraagheidsmoment t.o.v. de rota
As 1: stapresponsie van de snelheidsregelkring . . . . . . . . . . . . . . . . . . . . .
As 1: stapresponsies van de posi eregelkring (minimum iner e) . . . . . . . . . . . .
As 1: stapresponsie van de posi eregelkring (minimum iner e, Kp = 12) . . . . . . . .
As 1: stapresponsies van de posi eregelkring (maximum iner e) . . . . . . . . . . . .
As 1: stapresponsie van de posi eregelkring (maximum iner e, Kp = 5) . . . . . . . . .
As 1: afstand R in func e van θ2 , θ3 en θ5 . . . . . . . . . . . . . . . . . . . . . . . .
As 2: bodekarakteris ek (In = koppel, uit = snelheid) . . . . . . . . . . . . . . . . . .
As 2: stapresponsie van de snelheidsregelkring . . . . . . . . . . . . . . . . . . . . .
As 2: stapresponsie van de posi eregelkring (minimum iner e, Kp = 13) . . . . . . . .
As 2: stapresponsie van de posi eregelkring (maximum iner e, Kp = 9) . . . . . . . . .
As 2: afstand R in func e van θ3 en θ5 . . . . . . . . . . . . . . . . . . . . . . . . . .
As 3: bodekarakteris ek (in = koppel, uit = snelheid) . . . . . . . . . . . . . . . . . . .
As 3: stapresponsie van de snelheidsregelkring . . . . . . . . . . . . . . . . . . . . .
As 3: stapresponsie van de posi eregelkring . . . . . . . . . . . . . . . . . . . . . . .
Het posi eregelalgoritme opgebouwd in MATLAB®/Simulink . . . . . . . . . . . . . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
e-as
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
8
8
9
10
10
11
11
12
13
13
14
16
16
17
18
18
19
20
21
21
22
23
24
24
25
26
27
27
28
29
Posi e en oriënta e van de eindeffector {b} ten opzichte van de basis {a} . . .
De mogelijke rota es van het assenstelsel {b} t.o.v. het referen estelsel {a} . .
De KUKA KR15/2 in rustposi e met de gedefiniëerde assenstelsels en afstanden
Zijaanzicht robotarm, berekening θ1 en θ2 . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
30
31
31
33
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
vii
4.5
4.6
4.7
4.8
4.9
4.10
4.11
Bovenaanzicht robotarm, berekening θ1 . . . . . . . . . . . . . . . . . . . .
Oriënta e assenstelsel {g} t.o.v. assenstelsel {e} . . . . . . . . . . . . . . . .
Voorwaartse kinema ca: {e} t.o.v. {a} . . . . . . . . . . . . . . . . . . . . .
Kinema ca: robotarm in rustposi e . . . . . . . . . . . . . . . . . . . . . .
Kinema ca: aanpassen Z-coördinaat, oriënta e {g} ongewijzigd . . . . . . . .
Kinema ca: aanpassen Y-coördinaat, oriënta e {g} ongewijzigd . . . . . . . .
Kinema ca: aanpassen oriënta e van de eindeffector, posi e {g} ongewijzigd
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
33
34
34
37
37
37
37
5.1
5.2
5.3
5.4
5.5
5.6
5.7
5.8
5.9
5.10
Punt-tot-punt-beweging . . . . . . . . . . . . . . . . . . . . . . .
Posi eregelkring met prefilter . . . . . . . . . . . . . . . . . . .
As 3: stapresponsie van de posi eregelkring . . . . . . . . . . . .
As 3: stapresponsie van de posi eregelkring met 2e orde prefilter
Baanbeweging . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Traject: posi es i.f.v. de jd . . . . . . . . . . . . . . . . . . . . .
Trajectgenerator in MATLAB®/Simulink . . . . . . . . . . . . . . .
As 1: posi eresponsie baanbeweging . . . . . . . . . . . . . . .
Posi eregelkring met snelheidsfeedforward . . . . . . . . . . . .
As 1: posi eresponsie baanbeweging met snelheidsfeedforward .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
39
39
40
40
41
41
42
43
44
44
A.1
A.2
A.3
A.4
KUKA KR15/2 industriële robot
Zijaanzicht KUKA KR15/2 . . .
Bovenaanzicht KUKA KR15/2 .
Motorbeschrijving . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
48
49
49
50
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
viii
1
Inleiding
Deze masterproef beschrij het ontwerp van een controle-algoritme voor een zes-assige industriële robot (Figuur
1.1). In dit inleidend hoofdstuk wordt eerst en vooral kort het probleem geschetst, waarna de doelstellingen van
het project aan bod komen. Steeds zal voor elke doelstelling de werkwijze en methodiek toegelicht worden.
A5
+
A4
+
-+
A6
+
--
--
-A2
A3
+
--
--
+ A1
Figuur 1.1: KUKA KR15/2 industriële robot
In het kader van de vorige masterproef [1] werd de robotarm reeds voorzien van een open controller. Dit maakt
het mogelijk om de interne werking van de controller te doorgronden en deze te op maliseren met behulp van
hedendaagse technologie. Deze masterproef tracht dit veelomva end project verder uit te werken en behandelt
meer specifiek het vraagstuk over hoe de volledige zes-assige robot op een efficiënte manier op maal kan worden
afgeregeld. Eenmaal elke as voorzien is van een performante posi eregeling wordt nagegaan op welke manier de
eindeffector van de robot kan worden geposi oneerd en georiënteerd met behulp van alle assen. Bovendien
moet geverifieerd worden hoe de eindeffector in func e van de jd een gewenst traject kan uitvoeren en op
welke manier het gedrag van de robot-assen jdens deze bewegingen kan worden geop maliseerd. Met andere
woorden is het algemeen doel van deze masterproef het adequaat afregelen en aansturen van een zes-assige
robot zodat deze in staat is om snel en nauwkeurig een gewenst traject uit te voeren. Om dit te verwezenlijken
kan deze masterproef in een vij al stappen worden ingedeeld:
1. Verzamelen van informa e over de KUKA robot:
Alvorens de controller op een doeltreffende manier te kunnen opbouwen, moeten allerlei gegevens over
de robot opgezocht worden. Dit betre zowel de elektrische en mechanische informa e van de robotarm
als de hardwarema ge opbouw van de sturing.
Om dit te verwezenlijken, kunnen eenvoudigweg enkele datasheets geraadpleegd worden. Voor technische
data zoals de afme ngen en het haalbaar bereik van de robot, kan worden verwezen naar [2]. Vooraleer
de robot in gebruik te nemen, kan eveneens de informa e over het noodzakelijke onderhoud van de
robot doorgenomen worden [3]. Voor allerlei elektrische en mechanische data van de verschillende AC
1
servomotoren zoals technische data, koppel-toerental-karakteris eken, etc... kan hoofdzakelijk gerefereerd
worden naar [4] en [5]. De voornaamste gegevens kunnen eveneens worden teruggevonden in bijlage A.
Daarnaast moet een beter inzicht verkregen worden in de hardwarema ge opstelling van de controller.
Hiervoor kan voornamelijk de voorgaande masterproef geraadpleegd worden. In hoofdstuk 2 wordt echter
de volledige hardwarema ge opstelling bondig toegelicht.
2. Vertrouwd geraken met het automa seringspla orm TwinCAT®3 van Beckhoff:
De robotarm beschikt over zes assen die elk door een servomotor worden aangedreven (Figuur 1.1). Om
ervoor te zorgen dat de motoren het gewenste koppel leveren aan het mechanische systeem, worden deze
in gesloten-lus aan enkele servo drives gekoppeld. Om de robotarm in gebruik te nemen is het uiterst
belangrijk dat deze drives op een correcte wijze geparametreerd, geconfigureerd en in gebruik genomen
worden.
Om de fundamentele vaardigheden en kennis te verwerven om het systeem te configureren en in werking te
stellen, kan hoofdzakelijk het Beckhoff Informa on System [6] geraadpleegd worden. Tijdens dit leerproces
wordt ook een gebruikershandleiding opgesteld met concrete richtlijnen over het aansturen van de KUKA
KR15/2 industriële robot met behulp van het automa seringspla orm TwinCAT®3 [7].
3. Afregelen van de volledige robotarm:
Vervolgens moet iedere robot-as van een performante posi eregelkring voorzien worden zodat elke as
steeds nauwkeurig kan worden geposi oneerd.
θ* +
-
*
Positie ω +
Regelaar
-
*
Snelheid T T
i
Regelaar
i* +
-
Stroom
Regelaar
IGBT’s
i
Motor
T
Proces
ω
int
θ
Figuur 1.2: Cascaderegeling
Omdat iedere as over een servomotor met feedback beschikt, kan de cascaderegeling gebruikt worden
zoals geïllustreerd in figuur 1.2. Er wordt gekozen om de servo drives in torque mode te plaatsen
(Figuur 1.3). Op die manier zijn de snelheids- en posi eregelaar werkzaam in de industriële PC (iPC)
waardoor ze eigenhandig kunnen worden ontworpen. De cascaderegelkring wordt van binnen naar
buiten uit opgebouwd waarbij drie grote onderdelen kunnen worden herkend; de stroomregelkring
(werkzaam in de drive), de snelheidsregelkring en de posi eregelkring (werkzaam in de iPC). Met behulp
van een systeemiden fica e kunnen de nodige bodekarakteris eken van het onbekende mechanisch
proces worden opgesteld om aan de hand hiervan de op male parameters van de snelheidsregelaars
te achterhalen. Na het gedrag van de snelheidsregelkring te verifiëren, kan via een stapresponsie van
de posi eregelkring de posi eregelaar ingesteld worden. Hierbij moeten alle assen snel, vloeiend en
nauwkeurig hun wensposi e bereiken.
Voor het op maal afregelen van de posi eregelkring moet de invloed van de variërende
massatraagheidsmomenten ten opzichte van de rota e-assen onderzocht worden. Aan de hand
hiervan kan een adap eve regelstrategie worden opgezet.
4. Implementeren van een kinema sch algoritme:
Eenmaal elke as afgeregeld is, moet de volledige robotarm aangestuurd worden. Om dit te verwezenlijken
zal een kinema sch algoritme met behulp van de noodzakelijke kinema sche transforma es opgebouwd en
geïmplementeerd moeten worden. Hierbij zal gebruik gemaakt worden van de voorwaartse en de inverse
kinema sche transforma es [1]. Na het bestuderen en begrijpen van deze transforma es kan het algoritme
2
worden geïmplementeerd en geop maliseerd in MATLAB®/Simulink zodat de eindeffector door het geschikt
aansturen van elke as de gewenste posi e en oriënta e bereikt.
5. Ontwikkelen van een trajectgenerator:
Nadat het mogelijk is om de eindeffector van de robotarm op een adequate wijze te posi oneren en te
oriënteren, moet een algoritme worden ontwikkeld dat een gewenst traject voor de eindeffector vertaalt
naar concrete posi e-setpoints voor de robot-assen in func e van de jd. Om dit te realiseren zal een
trajectgenerator worden opgebouwd en geïmplementeerd in MATLAB®/Simulink. Deze generator moet
op elk moment zorgen voor het genereren van de gewenste pose coördinaten (posi e en oriënta e).
Vervolgens kunnen deze coördinaten met behulp van het geïmplementeerde kinema sch model
omgerekend worden naar de gewenste hoekverdraaiingen voor elke as. Hierdoor zal de eindeffector van
de robot na een bepaalde jd het gewenste traject hebben afgelegd.
Daarnaast is ook een analyse van de wijze waarop een traject kan uitgevoerd worden een vereiste.
A ankelijk van de toepassing kan een parcours op twee verschillende manieren afgelegd worden, namelijk
door middel van een punt-tot-punt- of een baanbeweging. In beide gevallen kan worden nagegaan in welke
mate de robot het aangelegde traject volgt en hoe het gedrag indien nodig kan worden geop maliseerd
door de nodige modifica es aan de posi eregelkring aan te brengen.
AX5000DDrivesDenDremchopper
ResolverDaansluitingen
IndustriëleDPC
ω-DθD
θ’ +
Positie
Regelaar
-
θD
ω’+
T’
Snelheid
Regelaar
-
T’
T’
ω
i’ +
T
i
Stroom
Regelaar
-
i
IGBT’s
MotorDaansluitingen
Ext.DremDR
MATLAB/Simulink
θ’
+
-
Positie
Regelaar
ω’+
-
Snelheid
Regelaar
MechanischDproces
T’
i’ +
T
i
-
Stroom
Regelaar
IGBT’s
i
Motor
T
Proces
ω
int
θD
Figuur 1.3: Cascaderegeling, torque mode
3
2
KUKA KR15/2 industriële robot
2.1 Mechanische opbouw
De voor dit project beschikbare robot is een standaard zes-assige KUKA KR15/2 industriële robot. Mechanisch
is een industriële robot een geheel van starre lichamen gekoppeld met behulp van verstelbare verbindingen.
Binnen de mechanica wordt er echter niet gesproken over een star lichaam of een verbinding, maar veeleer
over links en joints. De posi e van een bepaalde link kan op ieder moment aangeduid worden met X-, Y- en
Z-coördinaten ten opzichte van een referen epunt. Bovendien hee een link niet alleen een posi e maar ook een
oriënta e, die wordt bepaald door een hoekverdraaiing ϕr , ϕp , ϕy . De posi e en oriënta e coördinaten vormen
samen de pose coördinaten. Indien een structuur zoals een industriële robot opgebouwd is uit meerdere links,
kan het aantal vrijheidsgraden snel oplopen. Maar de joints van de robot zullen echter zorgen voor het beperken
van het aantal vrijheidsgraden. Het minimum aantal coördinaten dat nodig is om een structuur te beschrijven
bepaalt het aantal vrijheidsgraden [8]. Gezien de KUKA KR15/2 beschikt over zes assen die elk door een aparte
servomotor worden aangedreven, kan de pose (posi e en oriënta e) van het eindpunt of de eindeffector van de
robot ten opzichte van een referen epunt volledig beschreven worden met behulp van de ashoekverdraaiingen
θ1 , θ2 , θ3 , θ4 , θ5 en θ6 . Eveneens kan de posi e en oriënta e beschreven worden aan de hand van de pose
coördinaten van de eindeffector X, Y, Z, roll, pitch en yaw. Met andere woorden zijn er telkens minimum zes
coördinaten nodig om het systeem te beschrijven. Hieruit volgt dat de robot beschikt over zes vrijheidsgraden.
Doorgaans komt het aantal vrijheidsgraden van een robot overeen met het aantal beschikbare assen.
Op basis van figuur 2.1 kan de func onaliteit van
Yeind Pols
elke as van de standaard zes-assige robot verduidelijkt {b}
Bovenarm
worden. Algemeen geldt dat indien een robot beschikt
+
A4
A5
+
over twee assen, om het even welk punt in één vlak
-kan worden bereikt. Wordt hieraan een derde as
Elleboog
+
+
toegevoegd, dan kan de pols van de robot theore sch Xeind
-voor eender welk punt in het areaal van de volledige
A6
--A3
Zeind
arm in een ruimte (X, Y en Z) worden geposi oneerd.
Zbasis
Vervolgens kan dit toegepast en verduidelijkt worden
+
Eindeffector A2
+ A1
-met behulp van de eerste drie assen of joints van
{a}
-de KUKA robot (As 1, As 2 en As 3). De eerste as
(As 1) zorgt voor het rotoren rond de basis, terwijl
de tweede as (As 2) wordt gebruikt om de robotarm
Schouder
verder of dichter te laten reiken. De derde as (As 3)
Basis
daarentegen laat toe om de eindeffector omhoog of
omlaag te bewegen. Zo kan geconcludeerd worden
dat met behulp van de schouder-joint (As 2) en de
Ybasis
Xbasis
elleboog-joint (As 3) de eindeffector voor elk punt in
het XZ-vlak kan worden geposi oneerd. Wordt de
Figuur 2.1: Standaard zes-assige robotarm
robot uitgebreid met een derde as, die een rota e
rond de basis toelaat (As 1), dan kan de eindeffector in drie dimensies geposi oneerd worden. Hieruit kan
worden gededuceerd dat de eerste drie assen van de robot hoofdzakelijk zorgen voor het posi oneren van de
eindeffector (X, Y en Z) of een transla e in drie dimensies van de oorsprong van het assenstelsel {b} ten opzichte
van de oorsprong van het referen estelsel {a}. Hiernaast kan de eindeffector van de zes-assige robot eveneens
een oriënta e opgelegd worden of een rota e van het assenstelsel {b} ten opzichte van het referen estelsel {a}.
4
In het vakjargon worden de mogelijke rota es roll, pitch en yaw genoemd. Het oriënteren van de eindeffector kan
verwezenlijkt worden met behulp van de laatste drie assen As 4, As 5 en As 6.
2.2 Hardware van de nieuwe sturing
Het dimensioneren van een controller door het uitdenken en selecteren van de nodige hardware is reeds
gerealiseerd [1]. De gekozen componenten voor de controller worden hier echter nog even aangehaald en kort
besproken. Ten slo e volgt een volledig overzicht van de hardwarema ge opstelling.
2.2.1 AX5000-serie servo drives
Om alle motoren van de robot op een correcte manier in gesloten-lus aan te drijven, zijn servo drives nodig. Deze
moeten de motoren op een gepaste wijze aansturen zodat het gewenste koppel steeds aan het mechanische
systeem geleverd wordt. Om die reden moet eveneens de hoekposi e van elke robot-as in de drives binnengelezen
worden. Zo werd in het productengamma van Beckhoff gekozen uit de AX5000-serie drives.
AX5112
As 2
AX5203
As 3 & As 4
AX5203
AX5021
As 5 & As 6 Remchopper
AX-Bridge
AX5112
As 1
Figuur 2.2: De geselecteerde Beckhoff servo drives en remchopper
Zoals in figuur 2.2 [1] weergegeven is, bestaat de selec e uit vier drives, een AX-Bridge en een remchopper. De
servomotoren van de laatste vier assen (As 3 t.e.m. As 6) kunnen door hun klein vermogen (0,5 kW - 0,79 kW) en
bijgevolg hun geringe nominale stroom (1,50 A - 2,10 A) per twee op één servo drive gekoppeld worden. Op die
manier worden twee drives uitgespaard. De servomotoren van de eerste twee assen (As 1 en As 2) daarentegen,
kunnen door hun groter vermogen (2,83 kW - 6,60 A) niet aangestuurd worden door een drive met twee servo
kanalen. Daarom worden deze assen voorzien van een eigen servo drive.
De AX-Bridge zorgt voor het met elkaar verbinden van de stuurspanning, de vermogenkringspanning en de
tussenkringspanning van de verschillende servo drives. Tenslo e is het geheel uitgerust met een remchopper.
Indien de tussenkringspanning door remenergie een bepaalde grens overschrijdt, kan dit extra vermogen met
behulp van deze externe weerstanden worden gedissipeerd.
5
Figuur 2.3: CX2030 Industriële iPC van Beckhoff
2.2.2 CX2030 Industriële PC
Om de servo drives aan te sturen is er een bovenliggende intelligen e nodig. Deze func e wordt ingenomen door
een industriële PC (iPC) van Beckhoff (Figuur 2.3 [1]). Een iPC kan beschouwd worden als een Programmable Logic
Controller (PLC) die gebaseerd is op PC technologie en Windows als besturingssysteem hee . Beckhoff biedt de
mogelijkheid om PC systemen uit te breiden met een TwinCAT®3 automa seringspla orm, ook wel eXtended
Automa on Technology of kortweg XAT genoemd. Zoals figuur 2.4 illustreert, is de programmeeromgeving eXtended Automa on Engineering of kortweg XAE - van dit automa seringspla orm volledig geïntegreerd in
Microso Visual Studio®. Dit maakt het mogelijk om via een System Manager binnen deze ontwerpomgeving
verschillende onderdelen van het gehele TwinCAT®3 project te configureren, bijvoorbeeld de I/O-apparaten,
het PLC gebeuren, enz. Hiernaast kunnen objecten voor real- me toepassingen zoals het aansturen van een
zes-assige robotarm, zowel volgens de IEC 61131 norm, als in C of C++ worden geprogrammeerd. Bovendien
biedt het automa seringspla orm TwinCAT®3 de mogelijkheid om MATLAB®/Simulink algoritmes te integreren
binnen de programmeeromgeving. Eenmaal de geprogrammeerde of gemodelleerde objecten met behulp van
een geschikte compiler gecompileerd zijn, kunnen ze ona ankelijk van hun programmeertaal data met elkaar
uitwisselen.
PC System
TwinCAT 3
Matlab® /
Simulink®
TwinCAT 3 Engineering Environment based on Visual Studio ®
System Manager
Configuration of:
– I/O
– PLC
– C/C++
– MC
– NC
– CNC
– Safety
– others
Programming
NonReal-time
real-time
C#/.NET
IEC 61131 Objectoriented
extensions
C/C++
Simulink
Coder™
Third-party
programming
tool
C/C++
Microsoft C Compiler
IEC Compiler
TwinCAT Transport Layer – ADS
TwinCAT 3 Runtime
PLC
Object
CNC
Object
Safety
Object
C
Object
C++
Object
NC
Object
Simulink
Object
Fieldbus
Figuur 2.4: Overzicht eXtended Automa on Technology (XAT)
6
Vervolgens zal het TwinCAT®3 Run me systeem - eXtended Automa on Run me of kortweg XAR - binnen het
PC systeem (iPC) aan de hand van een base clock de Windows taken onderbreken en de gegenereerde objecten
behandelen om deze real- me uit te voeren. Ten slo e wordt de nodige informa e via de veldbus doorgestuurd
naar de drives zodat de gewenste ac es worden uitgevoerd.
Verder beschikt de iPC over een Non-vola le random-access memory of kortweg NVRAM geheugen. Met behulp
van dit geheugen kan data behouden worden indien de voedingsspanning wegvalt. Zo kan de posi e van iedere
robot-as steeds onthouden worden indien het systeem wordt herstart.
2.2.3 EtherCAT®
Om de communica e tussen de bovenliggende intelligen e (iPC) en de servo drives te voorzien, wordt een veldbus
gekozen. Om ervoor te zorgen dat de robotarm vloeiende bewegingen uitvoert, moet de iPC elke 250 µs de
geprogrammeerde objecten uitvoeren en data uitwisselen met de drives. Het is echter belangrijk dat de data
langs beide zijden perfect kan worden gereconstrueerd. Hierdoor moet de updatefrequen e van de veldbus
minstens twee keer zo groot zijn dan de frequen e van de data (bemonsteringstheorema van Nyquist-Shannon).
Hieruit kan worden afgeleid dat de cyclus jd van de veldbus maximaal 125 µs mag bedragen. Zo werd geopteerd
voor het gebruik van een EtherCAT®veldbus (updatefrequen e tot 10 kHz). Hoe de frames van de berichten zijn
samengesteld en volgens welk protocol deze over de veldbus worden verzonden, wordt uitgebreid toegelicht
in [1].
2.2.4 Overzicht hardwarema ge opstelling
In figuur 2.5 is de opstelling van de geselecteerde hardware weergegeven. Bovenaan de servo drives kunnen de
resolvers worden aangesloten, terwijl onderaan aanslui ngen zijn voorzien voor de servomotoren. Op die manier
kunnen de verschillende assen van de robot in gesloten-lus met de sturing worden gekoppeld.
AX5000 Drives en remchopper:
Resolver aansluitingen
CX 2030 Industriële PC:
Motor aansluitingen
Ext. rem R
Figuur 2.5: Overzicht hardwarema ge opstelling
7
3
Posi eregeling van de robot-assen
Om de robot op een snelle en accurate wijze te kunnen posi oneren, wordt elke as voorzien van een eigenhandig
ontworpen posi eregelkring. Een algemeen gesloten regelsysteem zoals in figuur 3.1 weergegeven is, bestaat
hoofdzakelijk uit een regelaar, een systeem (of proces) met een actuator en een sensor. De actuator en de
storingen kunnen de toestand van het systeem beïnvloeden, terwijl de sensor de werkelijk toestand van het
systeem opmeet. Deze sensorwaarde wordt vervolgens teruggekoppeld om zo een gesloten-lus regelsysteem
te vormen. De regelaar zorgt ervoor dat de werkelijke toestand van het systeem wordt bijgestuurd indien de
wenswaarde niet langer overeenkomt met de teruggekoppelde sensorwaarde. Meestal wordt een PID-regelaar
gebruikt als regelalgoritme van de regelaar.
storingen
wenswaarde
+
foutsignaal
-
regelaar
regelwaarde
actuator
ingang
uitgang
systeem
sensor
Figuur 3.1: Een algemeen gesloten regelsysteem
Indien het algemeen regelschema uit figuur 3.1 wordt toegepast op de beschikbare KUKA robot, kan het systeem
in de figuur vervangen worden door een as van de robot met een permanent magneet motor als actuator. De
resolver op de motoras kan echter de taak als sensor vervullen. Hierdoor kan vervolgens gededuceerd worden dat
de assen van de robotarm gesloten-lus of servosystemen zijn. Dit stelt ons in staat om per as een cascaderegeling
toe te passen.
3.1 Cascaderegeling
Een cascaderegeling (Figuur 3.2) kan beschouwd worden als drie algemeen gesloten-lus regelsystemen waarbij de
ene regelkring telkens vervat zit in het proces van de bovenliggende regelkring:
θ* +
-
*
Positie ω +
Regelaar
-
*
Snelheid T T
i
Regelaar
i* +
-
Stroom
Regelaar
IGBT’s
i
Motor
T
Proces
ω
int
θ
Figuur 3.2: Cascaderegeling
Hieronder wordt de func onaliteit van elke regelkring bondig toegelicht:
1. Stroomregelkring:
De stroomregelaar zorgt samen met de IGBT's in de vermogensmodule van de servo drive voor de gewenste
stroom in de motor. Door deze stroom zal de motor een koppel leveren dat de robot-as doet versnellen
of vertragen. De werkelijke stroom in de motor wordt via een stroomme ng teruggekoppeld om een
gesloten-lus systeem te bekomen.
8
De regelaar van deze kring staat in voor de stroom in de statorwikkelingen van de motor. Indien een
spanning aan deze spoelen wordt aangelegd, zal de stroom in de statorwikkelingen s jgen volgens een
eerste orde systeem. Indien de spanning wegvalt, zal de stroom opnieuw naar nul zakken. Daarom wordt
een PI-regelaar gebruikt als stroomregelaar voor de stroomregelkring. Deze bestaat uit een propor onele
en een integrerende ac e. De propor onele ac e zorgt voor het versterken van de fout tussen de gewenste
en de werkelijke stroom in de motor. De integrerende ac e zal zorgen voor het integreren van de fout,
waardoor er spanning over de statorwikkelingen blij staan wanneer de fout tussen de gewenste en de
werkelijke stroom wegvalt.
2. Snelheidsregelkring:
Met behulp van een resolver op de motoras kan op elk moment de werkelijke snelheid als feedback worden
teruggekoppeld. Samen met de snelheidsregelaar wordt dan een gesloten-lus systeem gevormd. Deze
regelaar zal een gewenst koppel uitsturen naar de stroomregelkring zodat de gewenste snelheid wordt
bereikt. Ook voor deze regelaar wordt een PI-regelaar gebruikt. De propor onele ac e zal zorgen voor
het versterken van de fout tussen de gewenste en de werkelijke snelheid, terwijl de integrerende ac e
zorgt voor het integreren van de fout. Ook bij de snelheidsregelaar is deze integrerende factor van belang.
Wanneer de fout tussen de werkelijke en de gewenste snelheid wegvalt, moet de snelheidsregelaar nog
steeds een wenswaarde voor het motorkoppel uitsturen zodat de werkelijke snelheid niet naar nul zakt.
3. Posi eregelkring:
De posi eregelkring zorgt er telkens voor dat de robot-as een gewenste posi e bereikt. Via de resolver
wordt de werkelijke posi e opgemeten en teruggekoppeld. Wanneer een fout optreedt tussen de gewenste
en de werkelijke posi e, zal de posi eregelaar een gewenste snelheid uitsturen naar de snelheidsregelkring.
Hierdoor wordt de gewenste posi e steeds bereikt. Als posi eregelaar wordt een P-regelaar gebruikt. Hier
is het voldoende om enkel een propor onele ac e te gebruiken. Eenmaal de gewenste posi e bereikt
wordt, moet er geen wenswaarde voor de snelheid meer worden uitgestuurd naar de snelheidsregelaar.
Drive control modes
Vooraleer de nodige stappen worden uitgevoerd om de parameters van de verschillende regelaars te achterhalen,
moet gekend zijn welke regelaars in de industriële PC ac ef kunnen zijn. Omdat de stroomregelaar zorgt voor het
sturen van de schakelpulsen naar de IGBT's in de vermogensmodule van de drive, is deze regelaar steeds werkzaam
in de drive. Naast de stroomregelaar bevat de drive ook een snelheids- en posi eregelaar, die al dan niet gebruikt
worden. Het is duidelijk dat de drive in drie verschillende opera on modes kan werken. Hieronder worden de
mogelijkheden nog even verduidelijkt.
1. Posi on mode:
Deze werkingsmodus zorgt ervoor dat alle regelaars werkzaam zijn in de drive. Hierdoor hoe enkel een
wenswaarde voor de posi e vanuit de iPC naar de drive te worden verstuurd (Figuur 3.3). Vervolgens zal
de cascaderegeling, die nu volledig ac ef is binnen de drive, ervoor zorgen dat de gewenste posi e van de
robot-as bereikt wordt. Het eigenhandig ontwerpen van de regelaars is in deze werkingsmodus beperkt.
iPC
θ*
Drive
+
-
*
Positie ω +
Regelaar
-
*
Snelheid T T
i
Regelaar
i* +
-
Stroom
Regelaar
IGBT’s
i
Motor
T
Proces
ω
int
θ
Figuur 3.3: Situering regelaars in posi on mode
9
2. Velocity mode:
Nu is de posi eregelaar werkzaam binnen de iPC. Hierdoor wordt een wenswaarde voor de snelheid vanuit
de iPC naar de drive gestuurd (Figuur 3.4). Met behulp van de snelheids- en stroomregelaar zorgt de drive
ervoor dat deze gewenste snelheid bereikt wordt. Door de werkelijke posi e van de motoras vanuit de
drive terug te koppelen naar de iPC, wordt een gesloten-lus posi eregeling gevormd. Met behulp van deze
werkingsmodus kan echter enkel de posi eregelaar volledig eigenhandig worden ontworpen.
iPC
θ* +
-
Drive
*
Positie ω
Regelaar
+
-
*
Snelheid T T
i
Regelaar
i* +
Stroom
Regelaar
-
IGBT’s
i
Motor
T
Proces
ω
int
θ
Figuur 3.4: Situering regelaars in velocity mode
3. Torque mode:
Enkel de stroomregelaar is werkzaam binnen de drive. De posi e- en snelheidsregelaar bevinden zich in
de iPC waardoor nu een wenswaarde voor het motorkoppel moet worden verstuurd naar de drive (Figuur
3.5). Daar zal de stroomregelaar samen met de IGBT's zorgen voor de gepaste stroom in de wikkelingen
van de motor zodat op elk moment het gewenste koppel wordt verkregen. Door een terugkoppeling van
de werkelijke posi e en snelheid naar de iPC, wordt een gesloten regelkring bekomen. Gezien in deze
werkingsmodus zowel de posi e- en snelheidsregelaar ac ef zijn in de iPC, is het logisch dat jdens de
uitvoer van dit project gekozen wordt om de drives in torque mode te plaatsen. Dit laat toe om zelf de
posi e- en snelheidsregelaar op te stellen.
iPC
θ*
+
-
*
Positie ω +
Regelaar
-
Drive
*
Snelheid T
Regelaar
i* +
T
i
-
Stroom
Regelaar
IGBT’s
i
Motor
T
Proces
ω
int
θ
Figuur 3.5: Situering regelaars in torque mode
10
3.2 Systeemiden fica e
Zoals in sec e 3.1 al werd aangehaald, zullen de servo drives in torque mode geplaatst worden. Hierdoor kan de
volledige stroomregelkring, samen met het proces, als onbekend systeem worden beschouwd (Figuur 3.6).
θ* +
Positie
Regelaar
-
*
ω+
Snelheid
Regelaar
-
θ* +
-
T*
i*
T
i
*
Positie ω +
Regelaar
-
+
-
Snelheid
Regelaar
T*
T
i
Stroom
Regelaar
IGBT’s
Onbekend
Systeem
ω
Motor
integrator
θ
ω
Proces
int
θ
Figuur 3.6: Stroomgeregeld en mechanisch proces als onbekend systeem
Om de snelheidsregelaar op een correcte wijze te kunnen instellen, dient een wiskundig model van het systeem
te worden opgebouwd. Om dit te verwezenlijken zijn drie verschillende methodes mogelijk. Ten eerste
kan een model ontworpen worden op basis van theore sche inzichten. Door onbekende parameters, zoals
wrijving, demping en niet-lineariteiten, kan het opstellen van zo'n wiskundig model echter een las ge taak
worden. Daarnaast kan een systeemiden fica e worden uitgevoerd door testsignalen doordacht aan het systeem
aan te leggen. Met behulp van deze methode kan zowel een parametrisch (zoals een transfer unc e) als
een niet-parametrisch model (typisch een frequen eresponsie) opgebouwd worden. Het opstellen van een
parametrisch model uit een systeemiden fica e lijkt interessant maar is niet eenvoudig omdat hiervoor de vorm
van de transfer unc e, vooral qua orde en storingen (die op het systeem inwerken), goed moet gekend zijn.
Gezien de regelaars perfect kunnen worden ingesteld op basis van een frequen eresponsie, afgebeeld in een
Nichols of bodekarakteris ek, wordt geopteerd om een model voor het onbekende systeem op te bouwen via de
niet-parametrische methode.
ingang
y(t)
responsie
Onbekend proces
fft
A in
ϕ in
t
M(dB)
A in (ω1)
ω1
ϕ in (ω1)
ω1
ω
ω
ϕ [°]
A in (ω1)
A uit (ω1)
ω1
ω1
ϕ uit (ω1)-ϕ in (ω1)
fft
A uit
ωlog
ωlog
ϕuit
t
A uit (ω1)
ω1
ω
ϕ uit (ω1)
ω1
ω
Figuur 3.7: Opmeten van een onbekend systeem via de niet-parametrische methode
11
Het model wordt opgemeten door de ingang van het onbekende systeem te exciteren met behulp van een
frequen erijk signaal. Dit hee een responsie van de uitgang als gevolg. Na het opmeten van de ingang en
de systeemresponsie wordt het onbekende systeem geïden ficeerd door een bodekarakteris ek op te stellen
met behulp van een spectraalanalyse. Door middel van figuur 3.7 kan aangetoond worden op welke manier
het iden ficeren van een proces in zijn werk gaat. Om per frequen e te achterhalen hoe het systeem reageert,
wordt een Fast Fourier Trasform (FFT) uitgevoerd op zowel het ingangs- als uitgangssignaal. Daarna kan worden
nagegaan op welke manier het proces elke frequen ecomponent zal versterken of verzwakken en eventueel in
fase zal verschuiven. De resultaten kunnen vervolgens eenvoudig weergegeven worden met behulp van een
bodekarakteris ek. Een bodekarakteris ek is verdeeld in twee aparte karakteris eken, namelijk een amplitude en fasekarakteris ek (Figuur 3.8). De amplitudekarakteris ek gee de verhouding van de amplitudes weer terwijl
de fasekarakteris ek het verschil in fase illustreert.
BodetDiagram
10
Auit(ω1)
Ain (ω1)
Magnitudet(dB)
0
−10
−20
−30
Phaset(deg)
−40
0
−45
φuit(ω1) - φin(ω1)
−90
−2
10
−1
10
0
10
Frequencyt (rad/s)
ω1
1
10
2
10
Figuur 3.8: Bodekarakteris ek
Indien het gedrag van een systeem in een bepaald frequen edomein via een bodekarakteris ek dient te worden
achterhaald, moet dit systeem ook gevoed worden door een signaal dat dit frequen edomein bevat.
Met behulp van de gekozen hardware voor de nieuwe robotcontroller is het mogelijk om een minimum cyclus jd
te behalen van 250 µs. Om deze reden kan slechts iedere 250 µs een sample of een signaalwaarde aan de
ingang van het systeem aangeboden worden. De sampleperiode is dus gelijk aan 250 µs of de samplefrequen e
bedraagt 4000 Hz. Bijgevolg biedt dit de mogelijkheid om aan het systeem een ingangssignaal met een maximum
frequen e van 4000 Hz aan te leggen. Dezelfde frequen e-inhoud zal terug te vinden zijn in de uitgang van het
systeem.
Indien het in- en uitgangssignaal wordt opgemeten om via de verkregen meetdata een bodekarakteris ek op
te stellen, moet hierbij rekening gehouden worden met het bemonsteringstheorema van Nyquist-Shannon.
Deze gee aan dat de maximaal bruikbare frequen e van een signaal de hel van de samplefrequen e moet
bedragen. Op basis van de voorgaande informa e is het duidelijk dat de bodekarakteris eken van de robot-assen
een maximum bruikbaar frequen edomein van nul tot 2000 Hz zullen bezi en.
12
De beste resultaten voor het iden ficeren van het systeem worden bekomen door het ingangssignaal van het
proces te exciteren met behulp van een sweep, startend bij de minimum frequen e en con nu oplopend tot 2000
Hz (Figuur 3.9 [1]).
Sweep
2.5
2
Amplitude (Nm)
1.5
1
0.5
0
-0.5
-1
-1.5
-2
-2.5
0
2
4
6
8
10
12
Tijd (s)
Figuur 3.9: Sweep ingangssignaal (koppel)
Indien deze sweep aan de ingang van het systeem als wenswaarde voor het koppel wordt aangelegd, kan de
snelheid van het systeem opgemeten worden. In de snelheidsresponsie (Figuur 3.10 [1]) zal dezelfde frequen e
inhoud terug te vinden zijn als in het ingangssignaal.
Snelheidsresponsie
200
Amplitude (rad/s)
150
100
50
0
-50
-100
-150
-200
0
2
4
6
8
10
12
Tijd (s)
Figuur 3.10: Systeemresponsie (snelheid)
Aan de hand van de dalende amplitude van de snelheidsresponsie kan onmiddellijk de verzwakking door het
systeem bij s jgende frequen es worden opgemerkt aangezien de amplitude van de ingang constant blij . Door
het opmeten van beide signalen kan met de verkregen meetdata via de es mate func e in MATLAB®een
bodekarakteris ek van het onbekende systeem opgesteld worden. Het MATLAB®-script dat wordt gebruikt om
een bodekarakteris ek op te stellen, kan teruggevonden worden in bijlage B.1.
13
3.3 Ontwerpen van regelaars aan de hand van bodekarakteris eken
Na de bespreking van het principe van een cascaderegeling en systeemiden fica e, wordt aangetoond hoe
de snelheidsregelaar door middel van een bodekarakteris ek kan worden ontworpen. Daarna wordt deze
methode toegepast op de assen van de robot. De bodekarakteris eken worden opgemaakt met behulp van
een systeemiden fica e. In dit onderdeel wordt het instellen van de snelheidsregelaar via deze karakteris ek
besproken [9].
Aan de hand van een bodekarakteris ek moet de regelaar ontworpen worden met het bode-stabiliteitscriterium
in het achterhoofd. Deze stelt dat een gesloten-lus systeem stabiel is als in de open-lus frequen eresponsie de
versterking kleiner is dan 1 of 0 dB daar waar de fase-naijling ± 180 ◦ bedraagt.
Een systeem moet stabiel zijn maar op zich is de stabiliteit niet voldoende om van een goed geregeld systeem
te kunnen spreken. Naast de stabiliteit zijn ook de overgangsverschijnselen belangrijk. Bij een versterking van 0
dB moet de fase-naijling voldoende boven −180 ◦ liggen. Dit wordt omschreven met behulp van de fasemarge
(FM). Dit is de marge tussen de fase van het systeem en −180 ◦ bij een versterking van 0 dB en gee dus weer
hoeveel de fase mag toenemen opdat het systeem nog stabiel zou zijn. Hoe groter de fasemarge, hoe stabieler
het systeem. Door de fasemarge te groot te kiezen, zal het systeem wel robuust zijn, maar zal het traag reageren
en voor een grote instel jd (Ts) zorgen. Het is dus belangrijk om de gulden middenweg te kiezen.
De fasemarge kan afgelezen worden op de bodekarakteris ek van het open-lus systeem (Figuur 3.11). Deze
informa e gee een beter inzicht in de responsie van het systeem in gesloten-lus.
M dB
ωPM
0dB
φ
-180°
log ω
log ω
FM
Figuur 3.11: Bodekarakteris ek: fasemarge (FM)
De snijfrequen e of pulsa e (ω)P M is de pulsa e waar het systeem een versterking van 0 dB hee .
14
3.3.1 Instellen snelheidsregelaar
Aan de hand van de voorgaande informa e blijkt de fasemarge een geschikte parameter te zijn voor het instellen
van de snelheidsregelaar. Via trial-and-error worden verschillende waarden voor de fasemarge vooropgesteld
om een stabiele en dynamische regelaar te ontwerpen. Typische waarden voor de fasemarge liggen tussen 45 ◦
en 75 ◦ [9].
Eenmaal het gedrag van het onbekende proces via systeemiden fica e achterhaald is, kan hieraan een
regelaar worden toegevoegd. Dit zal een effect hebben op de bodekarakteris ek van het systeem. Voor de
snelheidsregelaar wordt een PI-regelaar gebruikt waarvan de algemene formule er als volgt uitziet (Formule 3.1):
(
)
1
Kp · 1 +
Ti s
(3.1)
Deze formule kan herschreven worden als:
Kp
·
Ti
(
Ti s + 1
s
)
(3.2)
In formule 3.2 kan een integrator en een nulpunt waargenomen worden. De integrerende ac e zal de
fase 90 ◦ naar beneden trekken. Hierdoor zal de kri sche pulsa e, de pulsa e waarbij de fase-naijling van het
systeem −180 ◦ bedraagt (Figuur 3.11), sneller bereikt worden. Het nulpunt kan nu worden gebruikt om de
fasemarge op te krikken. Bij zeer hoge frequen es zal een nulpunt een fasevoorsprong van 90 ◦ opleveren. Het
is echter niet realis sch om met deze 90 ◦ te werken omdat de zeer hoge frequen es niet door het systeem
kunnen. Een fase voorsprong van 60 ◦ is wel realis sch [9]. De fase van het originele systeem ̸ G(jω) wordt
door de integrator dus met 90 ◦ verlaagd en door het nulpunt met 60 ◦ verhoogd bij ω = ωP M . Als een gewenste
fasemarge ϕP M wordt vooropgesteld, kan de pulsa e ωP M bepaald worden met behulp van formule 3.3:
̸
G(jω)|ω=ωP M − 90◦ + 60◦ = −180◦ + ϕP M
(3.3)
Indien de bodekarakteris ek van het te regelen systeem beschikbaar is, kan de snijpulsa e ωP M eenvoudig
afgelezen worden. Bij de pulsa e ωP M moet het nulpunt de fase 60 ◦ omhoog trekken. Met behulp van formule
3.4 kan de integra e jdsconstante Ti van de PI-regelaar bepaald worden:
̸
(1 + Ti jω)|ω=ωP M = 60◦
Hieruit volgt:
Ti =
(3.4)
tan(60◦ )
ωP M
(3.5)
Bij ωP M zal het verschil tussen de werkelijke fase en −180 ◦ dus gelijk zijn aan de fasemarge. Dit is de
pulsa e waarbij de amplitudekarakteris ek de 0 dB lijn moet snijden. Op deze manier kan de versterking of
verzwakking Kp bepaald worden zodat de volledige versterking van het systeem bij de vooropgestelde fasemarge
0 dB bedraagt:
20log|Kp |ω=ωP M
T jω + 1 i
+ 20log Ti jω + 20log|G(jω)|ω=ωP M = 0 dB
(3.6)
ω=ωP M
Hieruit volgt:
20log|Kp |ω=ωP M = −20log|G(jω)|ω=ωP M
T jω + 1 i
− 20log Ti jω (3.7)
ω=ωP M
15
Na het invullen van de berekende waarden voor de versterkingsfactor Kp en de integra e jdsconstante
Ti , kan de regelaar samen met de gekozen fasemarge gecontroleerd worden door de stapresponsie van de
snelheidsregelkring (Figuur 3.12) te bestuderen.
ω* +
Snelheidsregelaar
-
T*
Proces
ω
Figuur 3.12: Snelheidsregelkring
Wanneer bij aanleg van een stap voor de wenssnelheid het systeem teveel doorschot vertoont, kunnen de
versterkingsfactor Kp en de integra e jdsconstante Ti van de PI-regelaar opnieuw worden berekend met behulp
van een grotere fasemarge. Indien het systeem traag reageert en een grote instel jd hee , kan de fasemarge
kleiner gekozen worden.
3.3.2 Instellen posi eregelaar
θ*
+
-
*
Positie ω +
Regelaar
-
Snelheid
Regelaar
T*
Proces
ω
Int
θ
Figuur 3.13: Posi eregelkring
Eenmaal een dynamische en robuuste snelheidsresponsie verkregen is, kan nu een posi eregelaar opgesteld
worden om de posi eregelkring te vervolledigen. Als posi eregelaar wordt een P-regelaar gebruikt. Eerst wordt
een versterkingsfactor Kp gelijk aan één gebruikt, waarna deze stapsgewijs wordt opgevoerd tot het systeem
net geen doorschot vertoont. Om de posi eresponsie te kunnen opmeten wordt steeds aan de ingang van de
posi eregeling een stap aangelegd.
3.4 Afregelen van de assen
Nu de werkwijze voor het instellen van een regelaar met behulp van een bodekarakteris ek gekend is, kunnen alle
assen van de robot worden afgeregeld. In deze sec e zal meer specifiek het afregelen van de eerste drie assen (As
1, As 2 en As 3) besproken worden. Er moet echter, voornamelijk bij de eerste 2 assen, rekening gehouden worden
met het wijzigend massatraagheidsmoment ten opzichte van de rota e-as. Dit zorgt ervoor dat het afregelen van
deze robot-assen een grotere uitdaging vormt. De laatste drie assen (As 4, As 5 en As 6) worden niet verder
besproken omdat deze dezelfde methodiek volgen als As 3.
3.4.1 Afregelen van As 1
Instellen van de snelheidsregelaar
Eerst en vooral wordt een systeemiden fica e uitgevoerd zoals werd verduidelijkt in sec e 3.2. Voor het
resultaat van de me ngen wordt doorverwezen naar figuur 3.14. Aan de hand van deze bodekarakteris ek
kan de snelheidsregelaar ingesteld worden. Eerst wordt een fasemarge van 80 ◦ vooropgesteld, waaruit
̸ G(jω)|ω=ω
P M kan gehaald worden (Formule 3.8).
̸
G(jω)|ω=ωP M = −180◦ + 80◦ + 90◦ − 60◦ = −70◦
(3.8)
16
Vervolgens kan de pulsa e ωP M worden afgelezen uit de bodekarakteris ek.
bekend is, kan hieruit de integra e jdsconstante Ti worden berekend.
Ti =
Eenmaal de pulsa e ωP M
tan(60◦ )
= 0, 0163s
ωP M
(3.9)
|H| [dB]
50
5,26 dB
0
ωPM=106,3 rad/s
−50
0
10
1
10
2
10
Frequentie [Rad/s]
150
H [°]
100
50
0
−50
−100
φ = -70°
−150
0
10
1
10
2
10
Frequentie [Rad/s]
Figuur 3.14: As 1: bodekarakteris ek (In = koppel, uit = snelheid)
Om de versterkingsfactor Kp te kunnen bepalen moeten de versterking van het systeem en de versterking van
de integrerende ac e gekend zijn. De versterking van het systeem kan rechtstreeks worden afgelezen van de
bodekarakteris ek (Figuur 3.14) en bedraagt 5,26 dB. De versterking van integrerende ac e kan met behulp
van MATLAB®achterhaald worden. Hierbij wordt de integra e jdsconstante Ti in onderstaande transfer unc e
(Formule 3.10) ingevuld, waarna de versterking van dit systeem bij de pulsa e ωP M kan worden afgelezen.
(
Ti s + 1
Ti s
)
(3.10)
De versterking van de I-ac e bedraagt 1,26 dB. Nu kan met behulp van formule 3.7 de versterkingsfactor
Kp van de PI-regelaar berekend worden.
Kp = 10(
−5,26−1,26
20
) = 0, 4719 N m
◦ /s
(3.11)
Indien de gevonden waarden voor de versterkingsfactor Kp en de integra e jdsconstante Ti in formule
3.1 worden ingevuld, wordt de onderstaande PI-regelaar bekomen (Formule 3.12).
(
0, 4719 · 1 +
1
0, 0163s
)
(3.12)
Nu een snelheidsregelaar ontworpen is, kan het gedrag van het snelheidsgeregeld systeem worden nagegaan.
Hiervoor wordt aan de wenssnelheid (Figuur 3.12) een stap aangelegd waarbij de snelheidsresponsie
17
wordt opgemeten. Zoals reeds werd vermeld, moet er rekening gehouden worden met het wijzigend
massatraagheidsmoment ten opzichte van de rota e-as (As 1). Deze is hoofdzakelijk a ankelijk van de
hoekposi e van As 2 en As 3. Daarom wordt de snelheidsresponsie opgemeten voor de uiterste gevallen. Beide
toestanden zijn weergegeven in figuur 3.15.
Z
Z
As 1
As 1
XY
XY
Figuur 3.15: Toestand met een klein (links) en groot (rechts) massatraagheidsmoment t.o.v. de rota e-as
De resultaten van beide snelheidsresponsies worden weergegeven op figuur 3.16.
30
Wenssnelheid
Werkelijke snelheid min. inertie
Werkelijke snelheid max. inertie
25
Snelheid [°/s]
20
15
Ts = 420 ms
10
Ts = 620 ms
5
0
−5
0
0.2
0.4
0.6
Tijd [s]
0.8
1
1.2
Figuur 3.16: As 1: stapresponsie van de snelheidsregelkring
Aan de hand van de stapresponsie van de snelheidsregelkring kunnen een aantal zaken besloten worden. Ten
eerste kan worden vastgesteld dat beide toestanden een verschillende stapresponsie vertonen. Bij een klein
bereikt
massatraagheidsmoment ten opzichte van de rota e-as wordt de gewenste stapsnelheid van 20 graden
s
na ongeveer 420 milliseconden [ms]. Hierbij treedt een doorschot op van circa 20 procent. Daarentegen wordt bij
18
het grootst mogelijke traagheidsmoment de wenssnelheid bereikt in 620 ms met een doorschot van ongeveer 40
procent. Hoe groter het massatraagheidsmoment, hoe meer het systeem met een zekere massa een verandering
van draaisnelheid zal tegengaan. Een groter traagheidsmoment zorgt dus voor meer doorschot en een grotere
instel jd. Desondanks kan toch besloten worden dat de gesloten-lus snelheidsregeling voor beide toestanden
stabiel is. Bovendien zal de wenssnelheid in de prak jk nooit een stapfunc e zijn. Dit zou betekenen dat de
overgang tussen de twee verschillende snelheden met een oneindig grote versnelling moet plaatsvinden, wat niet
realis sch is gezien dit een oneindig groot koppel van de servomotor zou vragen. In hoofdstuk 5 zal het gedrag
van de regelkring worden nagegaan waarbij de wensposi e een traject beschrij in plaats van een stapfunc e
(Figuur 5.4). Om die reden zal het doorschot van de werkelijke snelheid eveneens minder uitgesproken zijn.
Dientengevolge is het niet meteen nodig extra maatregelen te treffen om dit gedrag te voorkomen.
Ten tweede kan een rimpel waargenomen worden op de werkelijke snelheid. Een resolver levert enkel
posi e-informa e waardoor de snelheid so warema g moet worden bekomen door de posi e-informa e af
te leiden. Indien een signaal afgeleid wordt, zal dit zorgen voor ruis waarmee het rimpeleffect op figuur 3.16
kan worden verklaard. Een mogelijke oplossing ligt in de toepassing van een filter om zo het snelheidssignaal af
te vlakken. Deze kan wel een faseachterstand veroorzaken. Vermits het rimpeleffect op dit ogenblik niet voor
problemen zorgt, wordt hier momenteel niet verder op ingegaan.
Instellen van de posi eregelaar
Voor het instellen van de posi eregelaar wordt het snelheidsgeregeld systeem beschouwd waaraan een zuivere
propor onele regelaar toegevoegd wordt en de werkelijke posi e teruggekoppeld wordt (Figuur 3.13). De
performan e van de regelkring wordt nagegaan door aan de ingang een stap voor de wensposi e aan te
leggen. Omdat het systeem de eindposi e zo snel en minu eus mogelijk moet bereiken, wordt de propor onele
ac e van de posi eregelaar langzamerhand opgetrokken tot er net geen doorschot optreedt. Opnieuw kan
de stapresponsie opgemeten worden voor de toestand van de robot met het kleinst en het grootst mogelijke
massatraagheidsmoment.
20
18
16
14
Positie [°]
12
10
8
6
Wenspositie
Werkelijke positie KP = 2
Werkelijke positie KP = 3
Werkelijke positie KP = 5
Werkelijke positie KP = 8
Werkelijke positie KP = 12
4
2
0
0
0.5
Tijd [s]
1
1.5
Figuur 3.17: As 1: stapresponsies van de posi eregelkring (minimum iner e)
19
In figuur 3.17 zijn stapresponsies met verschillende propor onele versterkingen weergegeven voor de toestand
met het kleinst mogelijke traagheidsmoment ten opzichte van de rota e-as (As 1). Met behulp van deze resultaten
kan een geschikte versterkingsfactor voor de posi eregelaar gevonden worden zodat de wensposi e zonder
doorschot, zo snel en nauwkeurig mogelijk wordt bereikt. Rekening houdend met deze eisen voor het ontwerpen
van de posi eregelaar wordt geopteerd voor een propor onele ac e van 12 graden/s
graden . Grotere versterkingen
blijken voor doorschot te zorgen wat bij een posi eregeling niet gewenst is. Indien de stapresponsie van de
gekozen versterking van naderbij bekeken wordt (Figuur 3.18), kan waargenomen worden dat de instel jd
ongeveer 400 ms bedraagt. Met behulp van de responsie kan eveneens de invloed van een snelheidslimiet
geconstateerd worden (lineair gedrag van de posi eresponsie). Indien een kleinere instel jd gewenst is, kan de
begrenzing gecorrigeerd worden. Daarnaast kan worden opgemerkt dat de motoras na overgangsverschijnselen
geposi oneerd kan worden met een nauwkeurigheid van 0,0001 graden.
20
18
16
14
Positie [°]
12
Ts = 400 ms
10
8
6
4
2
Wenspositie
Werkelijke positie
0
0
0.5
Tijd [s]
1
1.5
Figuur 3.18: As 1: stapresponsie van de posi eregelkring (minimum iner e, Kp = 12)
Vervolgens kunnen enkele stapresponsies voor de toestand van de robot met het grootst mogelijke
massatraagheidsmoment ten opzichte van de draai-as opgemeten worden. De resultaten zijn weergegeven in
figuur 3.19.
Na het bestuderen van de stapresponsies van verschillende propor onele ac es kan op het eerste ogenblik
gekozen worden voor een versterkingsfactor van 8 graden/s
graden . Grotere versterkingen blijken doorschot te vertonen
waardoor deze niet meer voldoen aan de voorwaarden van een goed ontworpen posi eregelaar. Een P-ac e
van 8 graden/s
graden vertoont echter een schokkende beweging jdens de overgangsfase. Daarom wordt geopteerd
voor een propor onele versterking die minder hevig reageert, namelijk een P-ac e van 5 graden/s
graden waarbij het
schokkend gedrag minder uitgesproken is (Figuur 3.20). Met behulp van deze versterking hee de stapresponsie
een instel jd van 800 ms. Daarnaast kan de motoras geposi oneerd worden met een nauwkeurigheid van
0,0006 graden. Meteen kan hiermee de invloed van een kleinere propor onele versterking op de sta sche fout
waargenomen worden (0,0006 graden ten opzicht van 0,0001 graden bij een grotere versterkingsfactor).
Indien de posi eregelaar van de cascaderegeling wordt uitgerust met een propor onele versterking van 12
graden/s
Een
graden zal dit voor doorschot en schokkende bewegingen zorgen bij grote traagheidsmomenten.
propor onele versterking van 5 graden/s
graden daarentegen, zal bij kleine traagheidsmomenten samengaan met een
grote instel jd en een verminderde accuraatheid (grotere sta sche fout). Om het posi egeregeld systeem te
20
20
18
16
14
Positie [°]
12
10
8
6
Wenspositie
Werkelijke positie KP = 2
Werkelijke positie KP = 3
Werkelijke positie KP = 5
Werkelijke positie KP = 8
Werkelijke positie KP = 10
4
2
0
0
0.5
1
Tijd [s]
1.5
Figuur 3.19: As 1: stapresponsies van de posi eregelkring (maximum iner e)
20
18
16
Positie [°]
14
Ts = 800 ms
12
10
8
6
4
2
0
0
0.5
1
Tijd [s]
1.5
2
2.5
Figuur 3.20: As 1: stapresponsie van de posi eregelkring (maximum iner e, Kp = 5)
op maliseren is er bijgevolg nood aan een adap eve regelaar. Dit is een regelaar waarvan de propor onele
versterking wordt aangepast naargelang de posi e van de robot. De versterkingsfactor Kp van de posi eregelaar
graden/s
zal dus moeten variëren tussen 5 graden/s
graden en 12 graden in func e van de hoekposi e van As 2, As 3 en As 5. Het
variërend traagheidsmoment ten opzichte van de rota e-as kan worden benaderd door een afstand R in func e
van de hoeken θ2 , θ3 en θ5 (Figuur 3.21).
21
Z
θ2
θ3
D
C
γ
θ2
E
B
F
A
θ5
XY
R
Figuur 3.21: As 1: afstand R in func e van θ2 , θ3 en θ5
B = 0, 3 m, C = 0, 65 m, D = 0, 155 m, E = 0, 6 m, F = 0, 14 m
RC
= C · sin(θ2 )
(3.13)
RD
= D · sin(θ2 + θ3 )
(3.14)
= 90 − (θ2 + θ3 )
(3.15)
RE
= E · sin(γ)
(3.16)
RF
= F · sin(γ − θ5 )
(3.17)
= B + RC + RD + RE + RF
(3.18)
γ
R
Met behulp van figuur 3.21 en de bovenstaande formules kan besloten worden dat de afstand R variëert tussen
een minimum lengte van 0,3 meter (lengte B) en een maximum lengte van 1,69 meter. Vervolgens kan met behulp
van MATLAB®een lineair verband opgesteld worden tussen de afstand R en de propor onele versterking voor de
posi eregelaar. Op die manier zal steeds een gepaste versterkingsfactor Kp voor de posi eregelaar verkregen
worden. Het opgestelde MATLAB®-script kan teruggevonden worden in bijlage B.2.
22
3.4.2 Afregelen van As 2
Instellen van de snelheidsregelaar
Het afregelen van deze as wordt op iden eke wijze gerealiseerd als het afregelen van As 1 (Sec e 3.4.1).
Eerst en vooral wordt een systeemiden fica e uitgevoerd. Op figuur 3.22 is de opgemeten bodekarakteris ek
weergegeven. Door een fasemarge van 70 ◦ voorop te stellen, kan uit de karakteris ek vervolgens de
nodige informa e gehaald worden om de versterkingsfactor Kp en de integra e jdsconstante Ti van de
snelheidsregelaar te bepalen. Hieronder worden de resultaten van de berekeningen weergegeven.
ωP M = 204, 2 rad
s
m
Kp = 1, 26 N
◦ /s
Ti = 0, 0085s
(3.19)
|H| [dB]
0
-3,3 dB
−10
−20
ωPM = 204,2 rad/s
−30
1
10
2
10
Frequentie [Rad/s]
200
H [°]
100
0
−100
−200
1
10
φ = - 80°
2
10
Frequentie [Rad/s]
Figuur 3.22: As 2: bodekarakteris ek (In = koppel, uit = snelheid)
Na het instellen van de PI-regelaar kan het gedrag van de snelheidsregeling worden nagegaan. Hiervoor wordt
aan de wenssnelheid (Figuur 3.12) een stap aangelegd waarbij de snelheidsresponsie wordt opgemeten. Tijdens
het aansturen is het massatraagheidsmoment ten opzichte van de draai-as (As 2) hoofdzakelijk a ankelijk van de
hoekposi e van As 3. Ook hier wordt de snelheidsresponsie van beide uitersten opgemeten. In figuur 3.23 zijn
de resultaten van de stapresponsie weergegeven.
Opnieuw kunnen enkele zaken besloten worden. Ten eerste kan worden opgemerkt dat het wisselend
traagheidsmoment prak sch geen invloed meer hee op de snelheidsresponsie. De instel jd bedraagt in beide
gevallen ruwweg 440 ms met een doorschot van 13,1 procent. Door de niet te verwaarlozen groo e van het
massatraagheidsmoment ten opzicht van de rota e-as, het stabiel karakter en het feit dat de snelheid-setpoint
in de prak jk nooit stappen zal vertonen, kan het gedrag (de uitslingering en het doorschot) van beide
snelheidsresponsie aanvaard worden.
Ten tweede kan ook hier een rimpeleffect op de werkelijke snelheid waargenomen worden. Deze kan opnieuw
verklaard worden door het afleiden van de posi e-informa e.
23
25
20
Snelheid [°/s]
15
Ts = 440 ms
10
5
0
−5
0
Wenssnelheid
Werkelijke snelheid min. inertie
Werkelijke snelheid max. inertie
0.2
0.4
0.6
Tijd [s]
0.8
1
1.2
1.4
Figuur 3.23: As 2: stapresponsie van de snelheidsregelkring
Instellen van de posi eregelaar
Voor het instellen van de posi eregelaar wordt de volledige posi eregelkring toegepast (Figuur 3.13). De
performan e van de regelkring wordt gecontroleerd door aan de ingang een stap voor de wensposi e aan te
leggen waarbij de propor onele ac e van de posi eregelaar telkens stapsgewijs wordt opgetrokken tot er net geen
doorschot optreedt. Opnieuw wordt de invloed van het wisselend traagheidsmoment ten opzicht van de draai-as
nagegaan. Deze wordt grotendeels bepaald door de posi e van As 3. Zoals bij de eerste as kan de stapresponsie
voor beide uiterste gevallen opgemeten worden. In figuren 3.24 en 3.25 zijn de responsies van het posi egeregeld
systeem weergegeven waarbij in beide gevallen reeds een op male versterkingsfactor gekozen is.
20
18
16
Ts = 350 ms
Positie [°]
14
12
10
8
6
4
2
Wenspositie
Werkelijke positie
0
0
0.2
0.4
0.6
Tijd [s]
0.8
1
1.2
Figuur 3.24: As 2: stapresponsie van de posi eregelkring (minimum iner e, Kp = 13)
Eerst en vooral kan worden opgemerkt dat bij het kleinst mogelijke massatraagheidsmoment ten opzichte van
rota e-as de stapresponsie met een propor onele versterking van 13 graden/s
graden een instel jd hee van 350
ms en er hierbij geen doorschot optreedt. Ook hier kan de instel jd eventueel verkleind worden door de
snelheidslimiet aan te passen. Hier mag echter niet in overdreven worden zodat de maximale rotorsnelheid van
24
de servomotor, rekening houdend met de doorschot van de snelheidsregelkring, niet kan worden overschreden.
De snelheid-setpoint moet dus begrensd worden zodat de werkelijke snelheid met doorschot onder de
maximale toegelaten rotorsnelheid blij . Met behulp van deze propor onele versterking kan de motoras op
0,0001 graden nauwkeurig geposi oneerd worden. Indien dezelfde versterkingsfactor wordt benut bij grote
traagheidsmomenten blijkt dat de responsie wel doorschot vertoont. Om die reden moet de versterkingsfactor
worden verkleind tot het doorschot geëlimineerd is. Zo wordt een propor onele versterking van 9 graden/s
graden
bekomen. Door middel van deze P-ac e wordt de wenswaarde bereikt na circa 500 ms (Figuur 3.25). Hierbij
kan de motoras op 0,0002 graden nauwkeurig geposi oneerd worden.
20
18
16
Ts = 500 ms
Positie [°]
14
12
10
8
6
4
2
Wenspositie
Werkelijke positie
0
0
0.2
0.4
0.6
Tijd [s]
0.8
1
1.2
Figuur 3.25: As 2: stapresponsie van de posi eregelkring (maximum iner e, Kp = 9)
Om de regelkring te op maliseren zal opnieuw een adap eve posi eregelaar moeten worden opgesteld. Aan
de hand van de onderstaande formules en figuur 3.26 kan het wijzigend massatraagheidsmoment in func e van
de hoekposi e As 3 en As 5 benaderd worden om zo de propor onele versterking van de posi eregelaar aan te
passen.
C = 0, 65 m, D = 0, 155 m, E = 0, 6 m, F = 0, 14 m
(3.20)
=
180° − θ3
√
C 2 + D2 − 2 · C · D · cos(γ)
sin(γ)
sin-1 (
· C)
RCD
90° − α
√
RCD 2 + E 2 − 2 · RCD · E · cos(ψ)
sin(β)
sin-1 (
· RCD )
RCE
°
γ − 90
=
θ5 − λ
(3.27)
γ
=
RCD
=
α
=
β
=
RCE
=
ψ
=
λ
ϵ
ϕ
=
RCF
=
°
180 − λ − ψ − ϵ
√
RCE 2 + F 2 − 2 · RCE · F · cos(ϕ)
(3.21)
(3.22)
(3.23)
(3.24)
(3.25)
(3.26)
(3.28)
(3.29)
Indien RCF ≤ C:
RCF = 0, 65 m
(3.30)
25
Z
θ3
D
α
E
β
γ
E
ψ
C
γ
ψ
RCD
F
RCE
φ
ε
F
RCE
RCF
RCF
λ
θ5
XY
Figuur 3.26: As 2: afstand R in func e van θ3 en θ5
Deze berekeningen worden vervolgens in MATLAB®ingegeven waarna op basis van het resultaat steeds een
geschikte propor onele versterking kan worden gevonden. Zo komt een P-ac e van 13 graden/s
graden overeen met een
afstand R van 0,65 meter (minimum), terwijl een versterking van 9 graden/s
graden overeenstemt met een afstand R van
1,399 meter (maximum). Met behulp van een lineair verband tussen de R-waarde en de propor onele versterking
kan voor iedere stand van de robot een keurige versterkingsfactor Kp voor de posi eregelaar gegenereerd
worden. Het MATLAB®-script voor deze berekening kan teruggevonden worden in bijlage B.3.
3.4.3 Afregelen van As 3
Instellen van de snelheidsregelaar
Het afregelen van deze as gaat op een iden eke wijze in zijn werk als het afregelen van de vorige twee assen. Ten
eerste wordt een systeemiden fica e uitgevoerd waarvan de resultaten zijn weergegeven met behulp van de
bodekarakteris ek in figuur 3.27.
Door een fasemarge van 70 ◦ te kiezen, kunnen de nodige gegevens uit de bodekarakteris ek afgelezen worden
om de versterkingsfactor Kp en de integra e jdsconstante Ti van de snelheidsregelaar te bepalen. Hieronder
zijn de resultaten van deze berekeningen weergegeven:
ωP M = 247, 7 rad
s
Ti = 0, 007s
m
Kp = 0, 0517 N
◦ /s
Nu de snelheidsregelaar van As 3 ontworpen is, kan het gedrag van de snelheidsregelkring worden nagegaan
door middel van de stapresponsie (Figuur 3.28).
26
50
|H| [dB]
40
ωPM = 247,7 rad/s
24,58 dB
30
20
10
0
0
10
1
10
2
10
Frequentie [Rad/s]
150
H [°]
100
50
0
−50
φ = - 80°
−100
−150
0
1
10
10
2
10
Frequentie [Rad/s]
Figuur 3.27: As 3: bodekarakteris ek (in = koppel, uit = snelheid)
30
25
Snelheid [°/s]
20
15
Ts = 300 ms
10
5
0
−5
0
0.5
1
1.5
Tijd [s]
2
2.5
3
Figuur 3.28: As 3: stapresponsie van de snelheidsregelkring
Uit het resultaat van de stapresponsie kan gededuceerd worden dat de gewenste snelheid van 20 graden
wordt
s
bereikt na ongeveer 300 ms met een doorschot van 50 procent. Om het doorschot te verminderen kan worden
geopteerd voor een grotere fasemarge. Hierdoor zal de snelheidsregelkring robuuster zijn, maar ook trager
reageren. Omdat een stapfunc e in de prak jk niet realis sch is, wordt momenteel met de huidige PI-regelaar
verder gewerkt aangezien eerder wordt geopteerd voor een systeem dat snel reageert. Opnieuw kan het
rimpeleffect op de werkelijke snelheid waargenomen worden.
27
Instellen van de posi eregelaar
Voor het instellen van de posi eregelaar wordt, net zoals bij de vorige assen, de performan e van de volledige
posi eregelkring nagegaan met behulp van een stapresponsie. Ook nu wordt de propor onele ac e omhoog
getrokken tot er net geen doorschot optreedt. De resultaten van de stapresponsie zijn weergegeven in figuur
3.29.
20
18
16
Ts = 430 ms
Positie [°]
14
12
10
8
6
4
2
Wenspositie
Werkelijke positie
0
0
0.2
0.4
0.6
Tijd [s]
0.8
1
1.2
1.4
Figuur 3.29: As 3: stapresponsie van de posi eregelkring
Hierbij kan geconcludeerd worden dat met behulp van een propor onele versterking van 12 graden/s
graden de
stapresponsie een instel jd hee van ongeveer 430 ms en geen doorschot vertoont. De invloed van de ingestelde
snelheidslimiet kan nogmaals worden aangetoond door middel van het lineair karakter van de posi eresponsie.
Indien een kleinere instel jd gewenst is, kan de begrenzing minder nauw gekozen worden. Hierbij moet echter
rekening gehouden worden met het doorschot van de snelheidsregeling (Figuur 3.28).
Hoewel de snelheid-setpoint begrensd is, kan de werkelijke rotorsnelheid van de servomotor toch de
maximum toegelaten rotorsnelheid overschrijden als gevolg van het doorschot. Om die reden moet de
snelheidswenswaarde voldoende begrensd worden. In hoofdstuk 5 wordt de performan e van de posi eregeling
nagegaan waarbij de aangelegde wensposi e in plaats van een stapfunc e een baanbeweging beschrij . Dit zorgt
voor minder doorschot van de werkelijke snelheid waardoor de begrenzing opnieuw kan worden gecorrigeerd.
Verder kan besloten worden dat de motoras met een nauwkeurigheid van 0,0002 graden kan worden
geposi oneerd.
28
3.5 Besluit
Aan de hand van de resultaten van de verschillende stapresponsies kunnen een aantal zaken geconcludeerd
worden. Eerst en vooral kan worden vastgesteld dat voor elke as het gesloten-lus systeem stabiel is. Voor
het ontwerpen van de snelheidsregelaar werd geopteerd voor fasemarges tussen 70 ◦ en 80 ◦ waardoor het
gesloten-lus systeem snel en robuust is. Het is mogelijk om de fasemarge kleiner te kiezen om zo de instel jd
te verkleinen, maar dit zal leiden tot een groter doorschot en een te hevige uitslingering waardoor het risico
op instabiliteit wordt vergroot. Vervolgens kan besloten worden dat op basis van een systeemiden fica e met
behulp van het bode-stabiliteitscriterium een performante snelheidsregeling kan worden ontworpen.
Tot dusver werd de posi eregelaar ingesteld door de propor onele versterking stapsgewijs te vergroten
tot de posi estapresponsie net geen doorschot vertoont. In de toekomst kan hiervoor eventueel ook een
systeemiden fica e worden uitgevoerd om de posi eregelaar op een nog efficiëntere manier te ontwerpen. Het
gedrag van het systeem kan dan door middel van een bodekarakteris ek met de wenssnelheid als ingangssignaal
en de werkelijke posi e als uitgangssignaal weergegeven en gebruikt worden om de posi eregelaar op te stellen.
Het posi eregelalgoritme werd opgebouwd in MATLAB®/Simulink waarvan de snelheids- en posi eregelaar een
gesloten kring vormen en in real- me werkzaam zijn in de iPC. Het volledig ontworpen MATLAB®/Simulink model
van één as is weergegeven in figuur 3.30.
du/dt
Derivative
S peed limit
4
R value
2
P(s)
Prefilter
Position
command Postion limit
Adaptive Position Controller
3
double
Actual
position
int32->double
-K-
PI(s)
S peed limit
int16
Torque limit Nm -> Torque double -> int16
1
Torque
command
S peed
Controller
-K-
1
double
Actual
velocity
int32->double
inc->grad
-Kinc/1024ms->grad/s
Figuur 3.30: Het posi eregelalgoritme opgebouwd in MATLAB®/Simulink
In dit model zijn hoofdzakelijk twee delen te onderscheiden, namelijk de posi eregelkring en de
snelheidsregelkring. Daarnaast is er nog een snelheidsfeedforward en een prefilter waar te nemen. Deze
laatste twee worden verduidelijkt in hoofdstuk 5. Na het uitvoeren van enkele conversies wordt aan de linkerkant
van het model eerst en vooral de fout tussen de werkelijke posi e en de wensposi e bepaald. De posi eregelaar,
al dan niet adap ef, versterkt deze fout met behulp van een zuivere P-ac e naar een gewenste snelheid. De
juiste instelling voor de P-ac e is a ankelijk van het massatraagheidsmoment ten opzichte van de rota e-as
en wordt berekend met behulp van de Rvalue (Figuur 3.30). Om de uitgestuurde snelheid te begrenzen, wordt
een snelheidslimiet ingesteld. Nadien wordt in de snelheidsregelkring de fout tussen de uitgestuurde en de
werkelijke snelheid bepaald, opnieuw na het verrichten van enkele conversies. De fout wordt aangeboden aan
de ingang van de snelheidsregelaar. Deze regelaar is een PI-regelaar en bepaalt het gewenste koppel. Vervolgens
wordt de wenswaarde begrensd door middel van een koppelbegrenzing. Deze beperkt het uitgestuurde koppel
tot het maximale koppel dat de motor kan leveren. Hierbij moet eveneens rekening gehouden worden met
an -windup. Wanneer de integrerende ac e van de PI-regelaar niet op dezelfde manier wordt begrensd als
de koppelbegrenzing, ontstaat wind-up. Daarom moet de I-ac e op dezelfde waarde begrensd worden als het
maximale koppel. Ten slo e kan opnieuw na enkele omze ngen het gewenste koppel naar de drive gestuurd
worden om zo een gesloten-lus te vormen.
29
4
Kinema sche transforma e
Nu alle assen afgeregeld zijn, kan de volledige robotarm aangestuurd worden. Om deze in een gewenste posi e
te brengen, moeten de hoeken van alle robot-assen berekend worden in func e van de gewenste coördinaten
van het eindpunt of de eindeffector van de robot. Om dit te realiseren kan gebruik gemaakt worden van twee
kinema sche transforma es, namelijk de voorwaartse en de inverse kinema sche transforma e. In dit hoofdstuk
wordt toegelicht hoe beide transforma es worden aangewend om het wiskundig model van de robot op te
stellen. Het kinema sch model werd reeds uitgewerkt in het kader van de vorige masterproef [1], maar werd in
deze masterproef geop maliseerd door enkele aanpassingen aan te brengen. Verder wordt binnen dit hoofdstuk
de implementa e van deze kinema ca toegelicht.
Vooraleer de uitwerking van het kinema sch model verduidelijkt wordt, moeten enkele essen ële zaken worden
aangehaald. Het is belangrijk om te weten dat de beschikbare robot een standaard zes-assige robot is. Dit betekent
dat de eerste drie assen voornamelijk zorgen voor de posi e van de eindeffector, terwijl de laatste drie assen deze
een gewenste oriënta e geven. In figuur 4.1 wordt de pose (posi e en oriënta e) van de eindeffector van de
robot ten opzichte van de basis gedefiniëerd met behulp van een assenstelsel. De pose van het assenstelsel {b}
met betrekking tot de basis, assenstelsel {a}, kan worden bepaald aan de hand van de pose coördinaten (X, Y,
Z, y, p, r). Hierbij beschrijven de coördinaten (X, Y, Z) de posi e van de oorsprong van het assenstelsel {b} ten
opzichte van de oorsprong van het referen estelsel {a}. De oriënta e van het assenstelsel {b} ten opzichte van het
referen estelsel {a} wordt beschreven met behulp van de hoekverdraaiingen (r, p, y).
{b}
θ5
+
y zb
Eindeffector
+
+
θ6
x
b
θ4
--
--
p
r
+
(X,Y,Z,r,p,y) θ2
--
+
--
yb
--
--
a
θ3
θ1
+
pb
za
Basis
xa
(0,0,0)
{a}
ya
Figuur 4.1: Posi e en oriënta e van de eindeffector {b} ten opzichte van de basis {a}
De pose van het assenstelsel {b} ten opzichte van het referen estelsel {a} kan wiskundig beschreven worden met
behulp van een transforma e matrix ba T:
(
b
aT
=
b
aR
b
ap
01×3
1
)
(4.1)
Deze 4 x 4 transforma ematrix bestaat uit een 3 x 3 rota ematrix ba R en een posi evector a pb (Formule 4.1).
Verder worden onderaan drie nullen en een één toegevoegd zodat een homogene matrix wordt bekomen. De
posi evector a pb = (a pbx a pby a pbz )T bevat de X-, Y-, en Z-coördinaten van de oorsprong van het assenstelsel {b}
30
ten opzichte van de oorsprong van het referen estelsel {a}. Deze vector bevat met andere woorden de transla e
van het ene assenstelsel ten opzichte van het andere assenstelsel in de X-, Y-, en Z-rich ng. De rota ematrix ba R is
een 3 x 3 orthogonale matrix en beschrij de oriënta e of verdraaiing van het assenstelsel {b} ten opzichte van het
referen estelsel {a}. Er zijn drie mogelijke rota es van het assenstelsel {b} met betrekking tot het referen estelsel
{a} mogelijk, namelijk een rota e van {b} om de X-, Y- en Z-as van {a} (Figuur 4.2). Hierdoor kan de rota ematrix
dan ook op drie verschillende manieren opgesteld worden.
Rotatie rond X
Zb
Zb
(0, -sin(θ), cos(θ))
Rotatie rond Y
Zb
Zb
θ
Rotatie rond Z
(0,0,1)
θ
(sin(θ),0, cos(θ))
Zb Zb
(0, cos(θ), sin(θ))
(1,0,0)
(0,1,0)
Yb
θ
Xa
(-sin(θ), cos(θ), 0)
Xa
Ya
Xb
Xb
Yb
θ
θ
Xa
θ
Ya
(cos(θ),0, -sin(θ))
Yb
Xb
(cos(θ), sin(θ), 0)
Ya
Figuur 4.2: De mogelijke rota es van het assenstelsel {b} t.o.v. het referen estelsel {a}

1

b
R(X,
θ)
=
0

a
0
0
cos(θ)
sin(θ)

0

−sin(θ) 
cos(θ)

cos(θ)

b
R(Y,
θ)
=
0

a
−sin(θ)

sin(θ)

0

cos(θ)
0
1
0

cos(θ)

b
R(Z,
θ)
=
sin(θ)

a
0
−sin(θ)
cos(θ)
0

0

0 
1
(4.2)
Nu het principe van de transforma ematrix gekend is, kan deze worden toegepast op de volledige robotarm. In
figuur 4.3 zijn de gedefiniëerde assenstelsels en afstanden voor de robot in rustposi e weergegeven.
θ5
--
+
ze
{e}
θ4
+
ye
xe
d
z D
E
yd
+
-- d
yf
x
{d}
-θ3
C
b
z
+
θ1
θ2
zc
--
{f }
{g} zf
zg
F
+
xf
xg
yg
θ6
--
+
--
{c}
xc
B
xb
yc
{b}
yb
A
za
xa
{a}
ya
Figuur 4.3: De KUKA KR15/2 in rustposi e met de gedefiniëerde assenstelsels en afstanden
De posi e en oriënta e van de eindeffector {g} in func e van de hoeken θ1 , θ2 , θ3 , θ4 , θ5 en θ6 kan gevonden
worden door de verschillende matrices met elkaar te vermenigvuldigen (Formule 4.3). Dit wordt de voorwaartse
kinema ca genoemd. In [1] wordt deze kinema ca uitgebreid toegelicht.
g
aT
= ba T cb T dc T ed T fe T gf T
(4.3)
31
De coördinatentransforma e van het assenstelsel {b} ten opzichte van het assenstelsel {a} (Figuur 4.3) kan
worden weergegeven met behulp van de transforma ematrix ba T. Deze bestaat enerzijds uit een vector a pb
die een transla e van het assenstelsel {b} ten opzichte van het referen estelsel {a} langs de Z-rich ng met een
afstand A bevat en anderzijds uit een rota ematrix ba R die een verdraaiing met de hoek −θ1 (kurkentrekkerregel,
rechterhand) rond de Z-as beschrij . De overige transforma ematrices kunnen op een gelijkaardige manier
worden opgesteld:
• De transforma ematrix cb T beschrij een transla e langs de X-rich ng met een afstand B en een rota e om
de Y-as ten gevolge van een hoek θ2 .
• De transforma ematrix dc T omvat een transla e langs de Z-rich ng met een afstand C en een verdraaiing
om de Y-as met een hoek θ3 .
• De transforma ematrix ed T bevat een transla e langs de Z-rich ng met een afstand D. Hier treedt geen
rota e op.
• De transforma ematrix fe T beschrij een transla e langs de X-rich ng met een afstand E en een rota e om
de X- en Y-as respec evelijk volgens een hoek −θ4 en θ5 .
• De transforma ematrix gf T omvat een transla e langs de X-rich ng met een afstand F en een verdraaiing
om de X-as met een −θ6 .
Door de beschreven transforma ematrices met elkaar te vermenigvuldigen, wordt de transforma ematrix ga T
bekomen. Deze beschrij de transla e en rota e van het assenstelsel {g} ten opzichte van het referen estelsel {a}.
In het kader van deze masterproef moeten echter de ashoeken berekend worden in func e van de gewenste
pose coördinaten (Formule 4.4).
(4.4)
(θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = f (X, Y, Z, r, p, y)
Dit kan gerealiseerd worden met behulp van de inverse kinema ca, zie sec e 4.1.
4.1 Berekenen van de ashoeken in func e van de gewenste posi e en
oriënta e van de eindeffector
4.1.1 Berekenen van de hoeken θ1 , θ2 en θ3
Omdat de posi e van de robot pols (oorsprong assenstelsel {f}, Figuur 4.3) enkel wordt bepaald door de
eerste drie assen, kunnen de hoeken rechtstreeks worden berekend met behulp van de gewenste X-, Y- en Zcoördinaten. Eerst en vooral moet de gewenste posi e van de eindeffector herleid worden naar de posi e van de
robot pols. Met behulp van een transla e langs xg met een afstand −F , wordt het centerpunt van de robot pols
bereikt. Met dit in het achterhoofd kunnen de posi e coördinaten van de robot pols (Xpols , Ypols , Zpols ) berekend
worden met behulp van formule 4.5.
(
ap
1
)
(
= ga T
gp
1

)
of




Xpols
Ypols
Zpols
1




 g 
 = aT 


−F
0
0
1





(4.5)
Vervolgens kunnen de ashoeken θ1 , θ2 en θ3 berekend worden. De volledige uitwerking van deze berekeningen
kan worden teruggevonden in [1]. Hieronder worden de resultaten van de berekeningen weergegeven.
32
θ3
Z
{d}
D
Y
{e}
γ
E
π -(α+β)
C
L1
β
θ2
{c}
{f }
r
α
B
Exy
L2
δ
Dxy
Fxyz
Xpols
{e}
{g}
z
Cxy
xy
{b}
{d}
{f }
Fxy
{g}
Ypols
{c}
A
B
XY
{a}
θ1
X
{a} {b}
Figuur 4.4: Zijaanzicht robotarm, berekening θ1 en θ2
Figuur 4.5: Bovenaanzicht robotarm, berekening θ1
Berekenen van θ1 (Figuur 4.5 [1]):
θ1 = atan2(−Ypols , Xpols )
(4.6)
Berekenen van L1, L2, xy, z en r (Figuur 4.4 [1]):
L1
=
L2
=
xy
=
z
=
r
=
C
√
D2 + E 2
√
X2pols + Y2pols − B
Zpols − A
√
xy2 + z2
(4.7)
(4.8)
(4.9)
(4.10)
(4.11)
Berekenen van α, β en γ:
ϵ
r2 − L12 − L22
2(L1 L2
=
sin(ϵ)
+ cos(ϵ)
α
=
atan
β
=
γ
=
ϵ−α
( )
π
D
− atan
2
E
L1
L2
)
(4.12)
(4.13)
(4.14)
(4.15)
Het teken van xy bepaalt drie gevallen voor δ en θ2 :
• xy > 0:
(
z
xy
)
δ
=
atan
θ2
=
π
−α−δ
2
δ
=
θ2
=
(4.16)
(4.17)
• xy = 0:
π
2
−α
(4.18)
(4.19)
• xy < 0:
δ
=
θ2
=
atan
( xy )
z
−(α + δ)
+
π
2
(4.20)
(4.21)
θ3 wordt bepaald door de hulphoeken:
θ3 = α + β − γ
(4.22)
33
4.1.2 Berekenen van de hoeken θ4 , θ5 en θ6
De laatste drie assen van de robot hebben als doel om de oriënta e van de eindeffector te manipuleren. Gezien
de rota ematrix van het assenstelsel {g} ten opzichte van het referen estelsel {a} gekend is (gewenste oriënta e)
en deze van het assenstelsel {e} ten opzichte van het referen estelsel {a} kan worden berekend met behulp van de
hoeken θ1 , θ2 , θ3 , wordt het verschil in oriënta e tussen deze twee assenstelsels volledig bepaald door de hoeken
θ4 , θ5 , θ6 .
e
z
{e} z
{f }
{g}
θ4
zf
g
E
f
θ6
Fx
xg
yg
xe
ye
yf
θ5
Figuur 4.6: Oriënta e assenstelsel {g} t.o.v. assenstelsel {e}
Eerst en vooral kan de rota ematrix ea R berekend worden met behulp van de voorwaartse kinema ca.
De rota ematrix ba R wordt bepaald door een rota e met
een hoek −θ1 rond de Z-as (Figuur 4.7):

cos(−θ1 )

b
R(Z,
−θ
)
=
sin(−θ

1
1)
a
0
−sin(−θ1 )
cos(−θ1 )
0

0

0 
1
(4.23)
De rota ematrix cb R beschrij een verdraaiing met θ2 om
de Y-as (Figuur 4.7):
cos(θ2 )

c
0
b R(Y, θ2 ) = 
−sin(θ2 )
0
1
0

sin(θ2 )

0

cos(θ2 )
cos(θ3 )

d
R(Y,
θ
)
=
0

3
c
−sin(θ3 )
0
1
0

sin(θ3 )

0

cos(θ3 )
zd D d
y
C
{d}
θ3
zb
zc
(4.24)
Vervolgens kan het assenstelsel roteren met θ3 rond de
Y-as (Figuur 4.7):

ye
xe
xd

ze
{e}
B
{c}
xc
xb
yc
θ2
Het assenstelsel {e} wijzigt echter niet in oriënta e ten
opzichte van {d} (Figuur 4.7). Hierdoor geldt:
1

e
dR =  0
0
0
1
0

0

0 
1
{b}
yb
A
(4.25)
za

θ1
xa
{a}
ya
(4.26)
Figuur 4.7: Voorwaartse kinema ca: {e} t.o.v. {a}
34
Zo kan de totale rota ematrix ea R gevonden worden door de onderlinge rota ematrices met elkaar te
vermenigvuldigen (met c1 = cos(θ1 ), s1 = sin(θ1 ), ...):

c1

e
R
=
−s

1
a
0
Hieruit volgt:
s1
c1
0

0
c2

0  0
1
−s2

0 s2
c3

1 0  0
0 c2
−s3

c1 c2 c3 − c1 s2 s3

e
a R =  s1 s2 s3 − c2 c3 s1
−c2 s3 − c3 s2
s1
c1
0

0 s3
1

1 0  0
0 c3
0
0
1
0

0

0 
1

c1 c2 s3 + c1 c3 s2

−c2 s1 s3 − c3 s1 s2 
c2 c3 − s2 s3
(4.27)
(4.28)
Als gewenste oriënta e voor de eindeffector kunnen drie verschillende verdraaiingen meegegeven worden,
namelijk een roll-, pitch-, yaw- rota e die respec evelijk een rota e rond de X-, Y- en Z-as voorstellen. Op deze
manier kan de rota ematrix ga R opgesteld worden (Formule 4.29). Voor een uitvoerige beschrijving van een roll-,
pitch- en yaw- rota e wordt verwezen naar [10].
g
aR
= R(RP Y, r, p, y) = R(Z, y)R(Y, p)R(X, r)



cy −sy 0
cp 0 sp



=  sy cy 0   0
1 0 
0
0
1
−sp 0 cp

cy cp cy sp sr − sy cr cy sp cr + sy sr

=  sy cp sy sp sr + cy cr sy sp cr − cy sr
−sp
cp sr
cp cr
1 0
0 cr
0 sr


0

−sr 
cr
(4.29)


Nu de rota ematrix ga R en ea R gekend zijn, kan de rota ematrix ge R gevonden worden:
g
aR
= ea R ge R
a
eR
= ea R−1
(4.30)
Voor een rota ematrix geldt:
R−1 = RT
g
eR
(4.31)
= ea RT ga R
Eveneens kan de rota ematrix ge R beschreven worden met behulp van een XYX-rota e (roll-, pitch- en
roll-rota e). Deze kan net zoals de rota ematrix ea R berekend worden met behulp van de voorwaartse
kinema ca. De rota ematrix fe R wordt bepaald door een rota e met een hoek −θ4 en θ5 respec evelijk rond de
X-as en Y-as (Figuur 4.6):

1

f
e R(XY, −θ4 , θ5 ) =  0
0
0
cos(−θ4 )
sin(−θ4 )

cos(θ5 )
0

0
−sin(−θ4 )  
−sin(θ5 )
cos(−θ4 )
0
1
0

sin(θ5 )

0

cos(θ5 )
(4.32)
35
De rota ematrix gf R beschrij een verdraaiing met een hoek −θ6 om de Y-as (Figuur 4.6):

1

g
R(X,
−θ
)
=
0

6
f
0
0
cos(−θ6 )
sin(−θ6 )

0

−sin(−θ6 ) 
cos(−θ6 )
(4.33)
Zo kan de totale rota ematrix ge R opnieuw gevonden worden door de onderlinge rota ematrices met elkaar te
vermenigvuldigen (met c4 = cos(θ4 ), s4 = sin(θ4 ), ...):

1

g
eR =  0
0
Hieruit volgt:
0
c4
−s4


c5
0

s4   0
−s5
c4
c5

g
e R =  −s4 s5
−c4 s5

1
s5

0  0
0
c5
0
1
0
0
c6
−s6

0

s6 
c6
(4.34)

c6 s5

c4 s6 + c5 c6 s4 
c4 c5 c6 − s4 s6
−s5 s6
c4 c6 − c5 s4 s6
−c6 s4 − c4 c5 s6
(4.35)
Nu de samenstelling van de rota ematrix ge R gekend is (Formule 4.35) en de waarde van elke element in deze
matrix met behulp van formule 4.29 kan worden achterhaald (gewenste oriënta e van de eindeffector), kunnen
de hoeken θ4 , θ5 , θ6 bepaald worden. Omdat de laatste drie assen een roll-, pitch-, roll-rota e beschrijven, moet
de singulariteit van de robot in rekening gebracht worden. Deze stelt dat meerdere assen kunnen gebruikt worden
om een iden eke beweging te bekomen. Als de pitch-rota e (As 5) van de eindeffector gelijk is aan nul, dan zijn
de twee roll-joints (As 4 en As 6) uitgelijnd ten opzichte van elkaar. Indien vervolgens een roll-rota e van de
eindeffector gewenst is, hebben de twee roll-joints in feite een oneindige flexibiliteit om de gewenste oriënta e
te bereiken. Wanneer bijvoorbeeld een roll-rota e van 40◦ nodig is, dan zullen de volgende combina es van joint
4 en joint 6 aan de gewenste roll-rota e voldoen, samen met nog een oneindig aantal andere combina es: (20◦ ;
20◦ ), (50◦ ; -10◦ ), (10◦ ; 30◦ ), (-10◦ ; 50◦ ), etc... Vervolgens bestaan er twee gevallen om de hoeken θ4 , θ5 , θ6 te
berekenen: θ5 ̸= 0 en θ5 = 0. De waarde van ge R11 bepaalt het geval omdat deze verduidelijkt of de pitch-rota e
al dan niet gelijk is aan nul.
Geval 1:
g
e R11
̸= 1 ⇒ θ5 ̸= 0
θ4
θ5
θ6
Geval 2:
g
e R11
= 1 ⇒ θ5 = 0

1
0

c4 c6 − s4 s6
 0
0 −c6 s4 − c4 s6
=
atan2(− ge R21 , − ge R31 )
=
atan2(−s4 ge R21
=
atan2(−

0

c4 s6 + c6 s4 
c4 c6 − s4 s6
−
(4.36)
c4 ge R31 , ge R11 )
(4.37)
g
g
e R12 , e R13 )
(4.38)

=
1

 0
0
0
cos(θ4 + θ6 )
−sin(θ4 + θ6 )
0


sin(θ4 + θ6 ) 
cos(θ4 + θ6 )
(4.39)
zodat
θ5
θ4 + θ6
=
=
(4.40)
0
atan2(
g
g
e R23 , e R22 )
(4.41)
4.2 Implementa e kinema sche transforma e
Nu alle hoeken kunnen worden berekend, kan het kinema sch model in MATLAB®/Simulink geïmplementeerd
worden om zo de robotarm in real- me aan te sturen. Het toegepaste MATLAB®-script kan worden teruggevonden
in bijlage B.8. In deze sec e wordt aangetoond op welke manier alle hoeken met behulp van het model berekend
36
worden zodat de eindeffector de gewenste posi e en oriënta e bereikt. Eerst en vooral kan geïllustreerd worden
dat het kinema sch model alle hoeken voor de home- of rustposi e van de robot correct berekent. Deze ruststand
wordt bereikt wanneer de hoekverdraaiingen van alle assen gelijk zijn aan 0 graden. Met behulp van figuur 4.8
kan worden aangetoond dat deze toestand verkregen wordt als de gewenste pose coördinaten (X, Y, Z, r, p, y)
gelijk zijn aan respec evelijk (1040, 0, 1480, 0, 0, 0). Met behulp van figuren 4.9 en 4.10 kan worden aangetoond
dat de oriënta e van de eindeffector (assenstelsel {g}) blij behouden indien enkel de posi e coördinaten (X, Y
en Z) gewijzigd worden. Daarnaast kan aan de hand van figuur 4.11 geïllustreerd worden dat de posi e van de
eindeffector blij behouden indien de orïenta e van het assenstelsel {g} ten opzichte van het referen estelsel
{a} wordt gewijzigd. Hierbij wordt de oriënta e van de eindeffector 90 graden geroteerd om de Y-as ten opzichte
van het referen estelsel. Merk op dat hiermee een pitch-rota e beschreven wordt. Een roll- en yaw-rota e zijn
respec evelijk een rota e rond de X- en Z-as van het assenstelsel {g} ten opzichte van het referen estelsel {a}.
Zg
{g}
Za
θ5
Xg
θ6 θ4
1480 mm
θ1
θ3= 39,52°
1000 mm
θ5= 45,71°
θ6= -180°
(1040, 0, 1000
, 0, 0, 0)
θ2
(1040, 0, 1480
, 0, 0, 0)
Za
θ4= -180°
XYg
θ3
Yg
Zg
{g}
θ2= 6,187°
{a}
θ1= 0°
Ya
XYa
1040 mm
Xa
{a}
1040 mm
Figuur 4.9: Kinema ca: aanpassen Z-coördinaat, oriënta e {g}
ongewijzigd
Figuur 4.8: Kinema ca: robotarm in rustposi e
{g}
Zg
Za
Za
1480 mm
Zg
Yg
Xg
{g}
1000 mm
(1040, 0, 1000
, 0, 90, 0)
θ1
θ3= 17,52°
θ5= 59,7°
θ4,6= 0°
XYg
θ2= 12,78°
{a}
θ1= 0°
300 mm
1040 mm
Ya
XYa
{g}
1040 mm
Xa
Figuur 4.11: Kinema ca:
aanpassen oriënta e van de
eindeffector, posi e {g} ongewijzigd
Figuur 4.10: Kinema ca: aanpassen Y-coördinaat, oriënta e
{g} ongewijzigd
37
4.3 Besluit
Na het implementeren van het invers kinema sch model in MATLAB®/Simulink kan deze real- me door de iPC
uitgevoerd worden. Hierbij kan geconcludeerd worden dat de hoeken θ1 , θ2 , θ3 , θ4 , θ5 en θ6 steeds accuraat
worden berekend in func e van de gewenste pose coördinaten. Op die manier kan door een samenwerking
van alle assen de gewenste posi e en oriënta e van de eindeffector bereikt worden. Vervolgens kan een
trajectgenerator worden opgesteld die op ieder moment de gewenste posi e en oriënta e coördinaten genereert
zodat de eindeffector van de robot een gewenst traject beschrij .
38
5
Traject
A ankelijk van de uiteindelijke toepassing kan het gewenste parcours op twee verschillende manieren afgelegd
worden. Voor applica es zoals pick & place units is enkel de gewenste eindposi e van belang. Deze moet echter zo
snel en nauwkeurig mogelijk bereikt worden. Dit soort beweging, waarbij de trajec nforma e tussen het beginen eindpunt niet van belang is, staat gekend als een punt-tot-punt-beweging (Figuur 5.1). Anderzijds is voor
toepassingen zoals een lasnaad robot het traject tussen het begin- en eindpunt wel van belang. Het is de bedoeling
dat de robot op elk moment het gewenste parcours zo nauwkeurig mogelijk volgt. Dit wordt een baanbeweging
genoemd. Beide mogelijkheden worden in dit hoofdstuk besproken.
5.1 Punt-tot-punt-beweging
Z
t1 = 0s
t2 = 5s
2
1
Y
X
Figuur 5.1: Punt-tot-punt-beweging
Bij een punt-tot-punt-beweging is enkel de fout op het eindpunt van de beweging belangrijk. De wenswaarde
voor een dergelijk systeem is een stapfunc e waarbij de stapgroo e de gewenste eindposi e aangee . Het
systeem moet proberen deze eindwaarde zo snel en zo nauwkeurig mogelijk te bereiken. Meestal is geen
doorschot gewenst [11]. Om aan deze voorwaarden te voldoen, werd de posi eregelkring in hoofdstuk 3 dan ook
met behulp van de stapresponsie afgeregeld. Als reac e op zo'n stapfunc e zal het systeem echter plots in grote
mate versnellen (Figuur 5.3). Dit veroorzaakt bruuske krachtwijzigingen die het materiaal sterk belasten. Een
oplossing kan worden gevonden in het toepassen van een tweede orde prefilter op de wensposi e (Figuur 5.2).
Deze zorgt voor het omvormen van de stapfunc e naar een signaal dat gradueel s jgt zodat de posi eregelkring
minder hevig reageert.
θ*
Prefilter
+
-
*
Positie ω +
Regelaar
-
Snelheid
Regelaar
T*
Proces
ω
Int
θ
Figuur 5.2: Posi eregelkring met prefilter
39
55
Δt = 32 ms
50
100
45
Wenspositie
40
Werkelijke positie
30
60
25
Δv = 111,7 °/s
20
40
Snelheid [°/s]
Werkelijke snelheid
35
Positie [°]
80
15
10
20
5
0
0
0
0.2
0.4
0.6
0.8
1
Tijd [s]
1.2
1.4
1.6
Figuur 5.3: As 3: stapresponsie van de posi eregelkring
De stapresponsie van As 3 met prefilter wordt weergegeven in figuur 5.4.
55
70
50
45
60
Wenspositie
Δt = 324,5 ms
Wenspositie na prefilter
Positie [°]
Werkelijke positie
Werkelijke snelheid
Δv = 70,8 °/s
35
30
50
40
25
30
20
15
Snelheid [°/s]
40
20
10
10
5
0
0
0
0.5
1
Tijd [s]
1.5
2
2.5
Figuur 5.4: As 3: stapresponsie van de posi eregelkring met 2e orde prefilter
Meteen kan worden opgemerkt dat de posi e-setpoint na de tweede orde prefilter geen stapfunc e meer
beschrij , maar een soort van s-curve. Hierdoor is de plotse versnelling kleiner waardoor de beweging vloeiender
verloopt (Figuur 5.4). Kortom, door het toevoegen van een tweede orde prefilter wordt de punt-tot-punt beweging
gemodificeerd tot een baanbeweging.
40
5.2 Baanbeweging
Bij een baanbeweging zal op discrete jds ppen telkens een wensposi e uitgestuurd worden (Figuur 5.5). Hier is
niet alleen de eindposi e van belang, maar moet elk posi ecommando ook zo goed mogelijk bereikt worden [11].
Een baanbeweging kan worden opgesplitst in kleine, kort op elkaar volgende punt-tot-punt-bewegingen.
Om dit te realiseren wordt de sturing van de robot uitgebreid met een trajectgenerator (interpolator). Deze hee
als taak om bij iedere cyclus jd de geschikte pose coördinaten aan het geïmplementeerde kinema sch model
te bezorgen. Op die manier zal de juiste hoek voor elke as iedere cyclus jd (Ts) berekend worden zodat de
eindeffector steeds het gewenste traject beschrij . In deze sec e wordt de opbouw van de trajectgenerator kort
besproken. Daarnaast wordt de nauwkeurigheid waarmee de robot een baanbeweging uitvoert en de eventuele
op malisa e hiervan nagegaan.
Z
Δt = Ts
Y
X
Figuur 5.5: Baanbeweging
5.2.1 Opbouw trajectgenerator
Om de trajectgenerator op te bouwen kan opnieuw gebruik gemaakt worden van de MATLAB®/Simulink omgeving.
Op die manier kan de generator samen met het kinema sch model en de posi eregelkringen van elke as één groot
geheel vormen dat real- me door de industriële PC wordt uitgevoerd (Bijlage B.10). Als basis voor een func onele
trajectgenerator moet het mogelijk zijn om te kunnen definiëren welke posi es het traject moet beschrijven en
op welke jds ppen de verschillende posi epunten moeten worden bereikt.
positie
(X3, t3)
(Xn - 1, tn - 1)
(X1, t1)
(X2, t2)
(X0, t0)
(Xn, tn)
tijd
Figuur 5.6: Traject: posi es i.f.v. de jd
41
In figuur 5.6 wordt met behulp van een voorbeeld aangetoond op welke manier de gedefinieerde posi epunten
samen met hun bijhorende jdsinforma e een bepaald traject vormen. Bovendien kan zo de snelheid waarmee
het traject beschreven wordt eenvoudig gewijzigd worden door de jdsinforma e van elk punt aan te passen. In
figuur 5.7 wordt het func oneel eindresultaat weergegeven. Ten eerste is het model voorzien van 1D-matrices
om de gewenste posi e-en jd-gegevens te kunnen ingeven. Ten tweede moet een jdas opgebouwd worden
zodat de iPC elke cyclus een nieuwe wensposi e tussen twee ingegeven punten kan berekenen (Figuur 5.6) in
func e van de jd t. Er moet met andere woorden tussen de gekozen posi epunten geïnterpoleerd worden. Dit
wordt gerealiseerd met behulp van formule 5.1. Om deze jdas op te bouwen wordt gebruik gemaakt van een
integrator. Zo zal een constante steeds verder geïntegreerd worden waardoor de jd t lineair zal blijven s jgen.
1
1
s
Constant
0
E nable
t
Trajectgenerator
Integrator
NOT
[X0, X1, X2, X3, ..., Xn - 1, Xn]
PosArray
X
Positiepunten
S cope
[t0, t1, t2, t3, ..., tn - 1, tn]
TijdsArray
Tijdspunten
MATLAB Function
Figuur 5.7: Trajectgenerator in MATLAB®/Simulink
(
X = Xn−1 +
Xn − Xn−1
Xt − Xt−1
)
· (t − tn−1 )
(5.1)
De snelheid waarmee het traject afgelegd wordt, kan eveneens real- me gewijzigd worden door de groo e van
de geïntegreerde constante aan te passen. Ten slo e kan een MATLAB®Func on in het model teruggevonden
worden. Deze zal met behulp van formule 5.1, de jd t en de gewenste posi e- en jd-gegevens op ieder
moment de juiste posi e genereren. De MATLAB®code voor het genereren van het X-, Y- en Z-coördinaat (posi e)
kunnen respec evelijk in bijlage B.4, B.5 en B.6 teruggevonden worden. De code voor het genereren van de
rota e-coördinaten kan gevonden worden in bijlage B.7. Door elke pose coördinaat (X, Y, Z, r, p, y) van een
dergelijke trajectgenerator te voorzien, kunnen alle assen van de robot op zo'n manier worden aangestuurd zodat
de eindeffector een gewenst traject beschrij .
42
5.2.2 Performan e baanbeweging
Nu de robotarm in staat is een gewenst traject af te leggen, kan de performan e van de baanbeweging worden
nagegaan. Zo wordt even dieper ingegaan op hoe nauwkeurig ieder punt van het gewenste traject werkelijk door
de assen van de robot in real- me gevolgd wordt. Het gedrag van de regelkring wordt geverifiëerd door aan de
ingang een sinus als wensposi e aan te leggen. In figuur 5.8 wordt de responsie van As 1 weergegeven.
50
Wenspositie
Werkelijke positie
40
30
Δ° = 1,38°
20
Positie [°]
10
0
Δt = 144 ms
−10
−20
−30
−40
−50
0
0.5
1
1.5
Tijd [s]
2
2.5
3
Figuur 5.8: As 1: posi eresponsie baanbeweging
Hierbij kunnen twee zaken geconcludeerd worden. Ten eerste kan worden besloten dat de uiteindelijke amplitude
van de sinus niet wordt bereikt. De fout tussen de werkelijke en de gewenste amplitude bedraagt namelijk 1,38
graden [°]. Ten tweede kan worden opgemerkt dat de sinus die de werkelijke posi e beschrij 144 milliseconden
[ms] naijlt op de gewenste sinus.
Met oog op de accuraatheid van de robot zullen de posi eregelkringen nogmaals moeten worden
geop maliseerd. De volgfout op de gewenste posi e kan gereduceerd worden door het toepassen van een
feedforward of voorwaartse koppeling. Hierdoor wordt a priori informa e aan de verschillende regelaars
aangeboden waardoor de volgfouten verminderen. In het ideale geval, mits perfecte modelkennis van het proces,
zou het systeem volledig gestuurd kunnen worden op basis van een feedforward [11]. Toch is een regelaar
nog steeds een vereiste omwille van de variërende traagheidsmomenten ten opzichte van de rota e-assen,
wijzigende lasten, veroudering, etc. In figuur 5.9 is weergegeven hoe de snelheidswens rechtstreeks wordt
doorgestuurd naar de ingang van de snelheidsregelaar door de posi eregelaar te overbruggen met behulp
van een snelheidsfeedforward. Zo wordt de gewenste snelheid rechtstreeks bepaald uit de afgeleide van de
posi e-setpoint. De posi eregelaar wordt dan enkel nog gebruikt om storingen weg te regelen.
43
dθ
dt
+
θ*
-
*
Positie ω +
Regelaar
-
Snelheid
Regelaar
ω
T*
Proces
θ
Int
Figuur 5.9: Posi eregelkring met snelheidsfeedforward
Nu kan het gedrag van de regelkring opnieuw worden gecontroleerd door aan de ingang dezelfde sinus als
wensposi e aan te leggen. De posi eresponsie is weergegeven in figuur 5.10.
50
Wenspositie
Werkelijke positie zonder feedforward
Werkelijke positie met feedforward
40
30
Δ° = 0,025°
20
Positie [°]
10
0
Δt = 10 ms
−10
−20
−30
−40
−50
0
0.5
1
1.5
Tijd [s]
2
2.5
3
Figuur 5.10: As 1: posi eresponsie baanbeweging met snelheidsfeedforward
Aan de hand van deze me ng kan besloten worden dat de naijling gereduceerd is tot 10 ms. Bovendien bedraagt
de fout tussen de werkelijke en de gewenste amplitude nu amper 0,025°. De posi eregelkring is zowat 55
keer nauwkeuriger geworden door het toevoegen van snelheidsfeedforward. Het is onbetwistbaar dat het
gebruik van een feedforward een grote invloed hee op de nauwkeurigheid. Daarom wordt elke as met een
snelheidsfeedforward geop maliseerd zodat de eindeffector van de robotarm met uiterste precisie een gewenst
traject kan volgen.
44
6
Conclusies
6.1 Behaalde resultaten
Op basis van de bevindingen van deze masterproef kan worden vastgesteld dat een controle-algoritme voor een
industriële robot met zes vrijheidsgraden volledig eigenhandig kan worden ontworpen met behulp van een open
controller. Daarnaast biedt dit project de mogelijkheid om extensieve kennis te verwerven in de verschillende
aspecten van de robotsturing.
Eerst en vooral werd jdens dit project een beter inzicht verkregen in de volledige hardwarema ge opbouw
van een robot controller. Zo werd duidelijk welke componenten nodig zijn voor het aansturen van real- me
toepassingen zoals een zes-assige robot. Het configureren van de servo drives vormde echter een uitdaging.
Om die reden werd een gebruikersaanwijzing [7] opgesteld met concrete richtlijnen over het aansturen van de
robot met behulp van het automa seringspla orm TwinCAT®3. Op die manier wordt een jdrovend leerproces
vermeden bij toekoms ge projecten en kan hoofdzakelijk gefocust worden op het effec ef doel van deze
projecten.
Na het verwerven van de kennis en de fundamentele vaardigheden om het systeem te configureren en in werking
te stellen, konden de vereiste controle-algoritmes ontworpen worden. Eerst werd via een systeemiden fica e
van het onbekend proces, bestaande uit mechanisch en stroomgeregeld systeem, een niet-parametrisch model
opgesteld. Dit model werd opgemeten door de ingang van het onbekende systeem te exciteren met behulp
van een frequen erijk signaal (bijvoorbeeld een sweep). Na het opmeten van zowel de in- als uitgang werd
op beide signalen een spectraalanalyse uitgevoerd om de frequen eresponsie van het systeem te achterhalen.
Deze responsie kon worden weergegeven aan de hand van een bodekarakteris ek. Vervolgens kon de
snelheidsregelaar (PI-regelaar) met behulp van het bode-stabiliteitscriterium worden ingesteld door middel van
een vooropgestelde fasemarge. Tijdens deze masterproef werden fasemarges tussen 70 ◦ en 80 ◦ gekozen om
een snel én robuust gesloten-lus systeem te verkrijgen. Als basis voor het instellen van de posi eregelaar (zuivere
P-regelaar) werd de P-ac e stapsgewijs vergroot tot net geen doorschot optrad. Daarnaast werd voornamelijk
bij de eerste 2 assen de invloed van het wijzigend massatraagheidsmoment ten opzichte van de rota e-as
onderzocht. Zo werd een adap eve posi eregelaar ontwikkeld waarvan de propor onele ac e variëert in func e
van de iner e ten opzichte van de draai-as. Uiteindelijk werd elke as van een performante posi eregeling voorzien.
Ook het noodzakelijk kinema sch model dat instaat voor het berekenen van de verschillende ashoeken in
func e van de gewenste pose coördinaten (posi e en oriënta e) van de eindeffector werd geanalyseerd en in
MATLAB®/Simulink geïmplementeerd. Het kinema sch model bespreekt de voorwaartse en inverse kinema sche
transforma e. De eerstgenoemde berekent de gewenste pose van de robot in func e van de aangelegde ashoeken
terwijl de laatste de nodige ashoeken berekent in func e van de gewenste pose van de eindeffector. Beide zijn
gebaseerd op coördinatentransforma es waarbij de rota e en transla e van een assenstelsel steeds ten opzichte
van een referen estelsel worden berekend aan de hand van een transforma ematrix. Na het implementeren
van het kinema sch algoritme in de iPC, kon worden vastgesteld dat de ashoeken in func e van de gewenste
pose coördinaten steeds real- me en correct berekend worden.
Eenmaal de eindeffector van de robotarm op een adequate wijze kon worden geposi oneerd en georiënteerd,
werd een trajectgenerator opgebouwd in MATLAB®/Simulink. Op die manier kan een gewenst traject voor de
eindeffector steeds vertaald worden naar concrete posi e-setpoints in func e van de jd voor de robot-assen.
Bovendien werd de performan e van de posi eregeling voor een punt-tot-punt- en baanbeweging nagegaan. Bij
45
een punt-tot-punt-beweging bleek het gebruik van een tweede orde prefilter interessant. Door per as zo'n filter
toe te passen op de wensposi e werd een meer soepele beweging bekomen waardoor het materiaal niet langer
werd belast door bruuske krachtwijzigingen. In het geval van een baanbeweging bleek een snelheidsfeedforward
voor het op maliseren van de posi eregeling noodzakelijk te zijn. Zo werd de volgfout van het traject gereduceerd
tot amper enkele milliseconden waardoor ook de accuraatheid aanzienlijk werd verbeterd.
6.2 Sugges es voor verder onderzoek
Het is duidelijk dat de grenzen van dit project nog lang niet bereikt zijn. Vooreerst kan de adap eve regelaar
geop maliseerd worden met behulp van een exacte berekening van het wijzigend massatraagheidsmoment.
Tot op heden is eveneens de invloed van een externe last verwaarloosd. Hierbij kan opnieuw de performan e
van de verschillende posi eregelkringen nagegaan worden.
Daarnaast kan de robotsturing voorzien worden van een bedieningsalgoritme zodat de eindeffector handma g
kan worden geposi oneerd en georiënteerd (joggen).
Bovendien kan ook een homing procedure geïmplementeerd worden. Als alterna ef hiervoor kan een NVRAM
geheugen gebruikt worden. Deze zal de posi e van elke as bewaren indien de voedingsspanning wegvalt. Ook
kan een applica e uitgedacht worden die de nauwkeurigheid en de complexiteit van de bewegingen van de robot
illustreert.
Ten slo e kan de trajectgenerator worden geop maliseerd zodat de volledige robot op een efficiënte manier wordt
aangestuurd. De meest doeltreffende methode hoe niet hoofdzakelijk de kortste weg tussen twee punten te zijn,
maar een beweging waarbij de robotsturing de servomotoren in een beperkte mate moet aansturen. Zo wordt
meteen ook energie-efficiënt met de robot omgesprongen.
46
Literatuurlijst
[1] S. Huysentruyt, ``Aansturen en afregelen van een 6-assige robot.'' Masterproef, 2013.
2013-2014.
Academiejaar
[2] K. R. Gmbh, ``Robot kr15 technical data.'' Datasheet, 1998.
[3] K. R. Gmbh, ``Robot kr6,15 onderhoud.'' Datasheet, 1997.
[4] SIEMENS, ``Simodrive 611/masterdrives mc ac servomotors 1 6.'' Datasheet, 2003.
[5] S. AG, ``Simo cs s - 1 7 motors.'' Datasheet, 2013.
[6] Beckhoff, ``Beckhoff informa on system.'' Online. [Datum van opzoeking: 26 Aug. 2013].
[7] L. Merlevede, ``Handleiding twincat 3.'' Cursustekst, 2014. Academiejaar 2013-2014.
[8] B. Vervisch, ``Mechanische trillingen.'' Cursustekst, 2013 - 2014. Academiejaar 2013-2014.
[9] S. Derammelaere and I. Sweertvaegher, ``Regeltechniek.'' Cursustekst. Academiejaar 2013-2014.
[10] J. D. Schu er and D. Vandepi e, ``Toegepaste mechanica 3.'' Cursustekst, 2008. Academiejaar 2013-2014.
[11] K. Stockman, ``Geavanceerde aandrij echniek deel 3.'' Cursustekst. Academiejaar 2013-2014.
[12] N. Nise, Control Systems Engineering 4e with Just Ask Compa Nion Set. John Wiley & Sons Canada, Limited,
2005.
[13] E. Red, ``Robo cs overview.'' Online, 1996. Academiejaar 2013-2014.
[14] J. F. A. Díaz, M. S. Dutra, and F. A. de Noronha Castro Pinto, ``Kinema cal and dynamical models of kr 6
kuka robot, including the kinema c control in a parallel processing pla orm.'' Online, 2010. Academiejaar
2013-2014.
47
A
A.1
KUKA KR15/2
Mechanische gegevens
Deze mechanische gegevens kunnen worden teruggevonden in de technische datasheet [2] van de KUKA KR15/2
industriële robot (Figuur A.1).
A5
+
+
A4
-+
A6
+
--
--
-A2
A3
+
--
--
+ A1
Figuur A.1: KUKA KR15/2 industriële robot
Tabel A.1: Mechanische gegevens KUKA KR15/2
As
A1
A2
A3
A4
A5
A6
(So warema g) bereik van de as [ ◦ ]
-185 tot 185
-55 tot 115
-210 tot 70
- tot -130 tot 130
- tot -
Hoeksnelheid as [ ◦ /s]
-152 tot 152
-152 tot 152
-152 tot 152
-250 tot 250
-357 tot 357
-660 tot 660
nmotor /nas
237/2
237/2
-237/2
-3145/31
90/1
1525/37
48
In figuur A.2 zijn de afme ngen en het bereik van de robotarm in zijaanzicht weergegeven.
600
140
120
155
Supplementaire last
+70˚
150
Zwaartepunt last
1945
650
---210˚
+115˚
675
1325
1480
---55˚
219
300
852
892
1570
R 145
Figuur A.2: Zijaanzicht KUKA KR15/2
In figuur A.3 is het bereik van de robotarm in bovenaanzicht weergegeven.
--- 185˚
R 1570
+185˚
Figuur A.3: Bovenaanzicht KUKA KR15/2
49
A.2
Elektrische gegevens
In deze bijlage kunnen de elektrische gegevens van de servomotoren teruggevonden worden [4].
Tabel A.2: Elektrische gegevens KUKA KR15/2
As
A1
A2
A3
A4
A5
A6
P [kW]
2,83
2,83
0,79
0,50
0,50
0,50
Inom [A]
6,60
6,60
2,10
1,50
1,50
1,50
nnom [tr/min]
3000
3000
3000
6000
6000
6000
1 F
K 6
Motortype
1FK6081-6AF71
1FK6081-6AF71
KK4EY-YYY-033
1FK7032-5AK71-1SB0
1FK6032-6AF71
1FK6032-6AF71
.
.
.
–
.
A
.
7
1
–
1
.
.
.
Electrical=machine
Synchronous=motor
AC=Servomotor
Series
Frame=size
Length
Pole=number
Non–ventilated
Rated=speed
F===G888=RPM
H===Bf88=RPM
K===w888=RPM
Encoder=system
A== Incremental=encoder1=sin2cos=)=Vpp 6I–D8B7I
E== Absolute=value=encoder=EnDat=6A–D8B7I
G== Basic=absolute=value=encoder=6A–GDI
S== Resolver
multi–pole
S== Resolver
D–pole
Shaft=end
A== with=keyway1=radial=eccentricity=tolerance=N1=without=holding=brake
B== with=keyway1=radial=eccentricity=tolerance=N1=with=holding=brake
G== Smooth=shaft=6no=keywayI1=radial=eccentricity=tolerance=N1=without=holding=brake
H== Smooth=shaft=6no=keywayI1=radial=eccentricity=tolerance=N1=with=holding=brake
Degree=of=protection
8== IP=wB
D== IP=wf=and=additionally1=drive–end=flange1=IP=wk
Figuur A.4: Motorbeschrijving
50
B
MATLAB Code
B.1 Systeemiden fica e: opstellen bodekarakteris ek
Ts = 0 .00025 ; %c y c l u s t i j d i n d u s t r i ë l e PC (250µs )
[ EstH , EstF ] = t f e s t i m a t e (q_rTorqueCommand , q_rVeloActual , [ ] , [ ] , [ ] , 1 / Ts ) ;
EstMag
= abs ( EstH ) ;
EstPhase = angle ( EstH ) ;
EstPhaseGraden=EstPhase *180/ p i ;
EstOmega = EstF*2* p i ; %( want omega=2*p i * f )
% Make p l o t
f i g u r e ( ’Name ’ , ’ IN = Torque , OUT = V e l o c i t y ’ , ’ NumberTitle ’ , ’ o f f ’ , ’ Color ’ , [ 1 1 1 ] ) ;
% Magnitude p l o t on top
subplot ( 2 , 1 , 1 ) ;
semilogx (EstOmega , 20* log10 (EstMag ) , ’ blue ’ , ’ LineWidth ’ , 1 ) ;
x l a b e l ( ’ Frequentie [ Rad/ s ] ’ ) ;%x l a b e l ( ’ \ omega , rad /s ’ )
y l a b e l ( ’ |H| [ dB ] ’ ) ;
t i t l e ( ’ IN = Torque , OUT = V e l o c i t y ’ ) ;
% Phase p l o t on bottom
subplot ( 2 , 1 , 2 ) ;
semilogx (EstOmega , EstPhaseGraden , ’ blue ’ , ’ LineWidth ’ , 1 ) ;
x l a b e l ( ’ Frequentie [ Rad/ s ] ’ ) ;%x l a b e l ( ’ \ omega , rad /s ’ )
y l a b e l ( ’ \ angle H [ ° ] ’ ) ;
B.2 As 1: berekening afstand R
f u n c t i o n R = I n e r t i e B e r e k e n i n g ( theta2 , theta3 , theta5 )
%afstanden
B = 0.3 ;
C = 0 .65 ;
D = 0 .155 ;
E = 0.6 ;
F = 0 .14 ;
%berekeningen afst an d
Rc = C* s i n d ( theta2 ) ;
Rd = D* s i n d ( theta2 + theta3 ) ;
gamma = 90 - ( theta2 + theta3 ) ;
Re = E* s i n d (gamma ) ;
51
Rf = F* s i n d (gamma - theta5 ) ;
Rtot = B + Rc + Rd + Re + Rf ;
i f abs ( Rtot ) <= B
R = B;
else
i f abs ( Rtot ) <= abs (B + Rc)
R = abs (B + Rc ) ;
else
i f abs ( Rtot ) <= abs (B + Rc + Rd)
R = abs (B + Rc + Rd ) ;
else
i f abs ( Rtot ) <= abs (B + Rc + Rd + Re)
R = abs (B + Rc + Rd + Re ) ;
else
R = Rtot ;
end
end
end
end
B.3 As 2: berekening afstand R
f u n c t i o n R2 =InertieBerekening_A2 ( theta3 , theta5 )
%afstanden
C = 0 .65 ;
D = 0 .155 ;
E = 0.6 ;
F = 0 .14 ;
%berekeningen afst an d
gamma = 180 - theta3 ;
Rcd = s q r t (C^2 + D^2 - 2*C*D* cosd (gamma ) ) ;
a l f a = asind ( ( s i n d (gamma)/Rcd) * C) ;
beta = 90 - a l f a ;
Rce = s q r t (Rcd^2 + E^2 - 2*Rcd*E* cosd ( beta ) ) ;
p s i = asind ( ( s i n d ( beta )/ Rce )*Rcd ) ;
lambda = gamma - 9 0 ;
e p s i l o n = theta5 - lambda ;
52
phi = 180 - lambda - p s i - e p s i l o n ;
Rcf = s q r t ( Rce^2 + F^2 - 2*Rce*F* cosd ( phi ) ) ;
i f Rcf <= 0 . 6 5
R2 = 0 . 6 5 ;
else
R2 = Rcf ;
end
B.4 Trajectgenerator: baanbeweging X-coördinaat
f u n c t i o n X = Tr a j ec t ( t , PosArray , TijdsArray ) %I n t e r p o l a t o r X- co ö r d i n a a t
global J ;
e i n d e f f e c t o r = 0 ; %l e n g t e e i n d e f f e c t o r
%t i j d t
%
i f t ==
J =
X=
end
g e r e s e t --> r e s e t J ( index )
--> X- co ördi naat r u s t p o s i t i e u i t s t u r e n
0;
1 ; %Index p o s i t i e - en t i j d s a r r a y
1040 + e i n d e f f e c t o r ; %mm
%t r a j e c t berekening : X- c oör dinaat
i f J <= l e n g t h ( PosArray ) && ( t ∼= 0)
i f ( J == 1)
X = 1040 + e i n d e f f e c t o r ;
e l s e %i n t e r p o l e r e n
X = ( PosArray ( J - 1) + ( ( PosArray ( J ) - PosArray ( J - 1 ) ) / ( TijdsArray ( J )
- TijdsArray ( J - 1 ) ) ) * ( t - TijdsArray ( J - 1 ) ) ) ;
end
i f t >= TijdsArray ( J )
J = J + 1;
end
else
X = 1040 + e i n d e f f e c t o r ;
end
53
B.5 Trajectgenerator: baanbeweging Y-coördinaat
f u n c t i o n Y = Tr a j ec t ( t , PosArray , TijdsArray ) %I n t e r p o l a t o r Y- co ö r d i n a a t
global J ;
%t i j d t
%
i f t ==
J =
Y=
end
g e r e s e t --> r e s e t J ( index )
--> Y- co ördi naat r u s t p o s i t i e u i t s t u r e n
0;
1;
0 ; %mm
%t r a j e c t berekening : Y- c oör dinaat
i f J <= l e n g t h ( PosArray ) && ( t ∼= 0)
i f ( J == 1)
Y = 0;
e l s e %i n t e r p o l e r e n
Y = ( PosArray ( J - 1) + ( ( PosArray ( J ) - PosArray ( J - 1 ) ) / ( TijdsArray ( J )
- TijdsArray ( J - 1 ) ) ) * ( t - TijdsArray ( J - 1 ) ) ) ;
end
i f t >= TijdsArray ( J )
J = J + 1;
end
else
Y = 0;
end
B.6 Trajectgenerator: baanbeweging Z-coördinaat
function Z
global J ;
%t i j d t
%
i f t ==
J =
Z =
end
= Tr a j ec t ( t , PosArray , TijdsArray ) %I n t e r p o l a t o r Z- co ö r d i n a a t
g e r e s e t --> r e s e t J ( index )
--> Z- co ördi naat r u s t p o s i t i e u i t s t u r e n
0;
1;
1480; %mm
%t r a j e c t berekening : Z- c oör dinaat
i f J <= l e n g t h ( PosArray ) && ( t ∼= 0)
i f ( J == 1)
Z = 1480;
54
e l s e %i n t e r p o l e r e n
Z = ( PosArray ( J - 1) + ( ( PosArray ( J ) - PosArray ( J - 1 ) ) / ( TijdsArray ( J )
- TijdsArray ( J - 1 ) ) ) * ( t - TijdsArray ( J - 1 ) ) ) ;
end
i f t >= TijdsArray ( J )
J = J + 1;
end
else
Z = 1480;
end
B.7 Trajectgenerator: baanbeweging roll-, pitch-, en yaw-coördinaat
%I n t e r p o l a t o r voor r o l l - , pitch - en yaw- coö r din a a t
f u n c t i o n r = Tr a j ec t ( t , PosArray , TijdsArray ) %I n t e r p o l a t o r r o l l - c o ö r d i n a a t
global J ;
%t i j d t
%
i f t ==
J =
r =
end
g e r e s e t --> r e s e t J ( index )
--> r o l l - coö rdina at r u s t p o s i t i e u i t s t u r e n
0;
1;
0;
%t r a j e c t berekening : r o l l - co ördin aat
i f J <= l e n g t h ( PosArray ) && ( t ∼= 0)
i f ( J == 1)
r = 0;
e l s e %i n t e r p o l e r e n
r = ( PosArray ( J - 1) + ( ( PosArray ( J ) - PosArray ( J - 1 ) ) / ( TijdsArray ( J )
- TijdsArray ( J - 1 ) ) ) * ( t - TijdsArray ( J - 1 ) ) ) ;
end
i f t >= TijdsArray ( J )
J = J + 1;
end
else
r = 0;
end
55
B.8 Het kinema sch model voor de KUKA KR15/2 industriële robot
f u n c t i o n [ t1 , t2 , t3 , t4 , t5 , t6 ]=Kinematica_LMV(X,Y, Z , r , p , y )
%omrekenen inp ut s [ grad --> rad ]
r = r *(3 .14159265359 / 1 8 0 ) ;
p = p*(3 .14159265359 / 1 8 0 ) ;
y = y*(3 .14159265359 / 1 8 0 ) ;
% Afstanden [mm]
eindeffector = 0;
a = 675;
b = 300;
c = 650;
d = 155;
e = 600;
f = 140 + e i n d e f f e c t o r ;
% Goniometrische berekeningen
cy = cos ( y ) ;
sy = s i n ( y ) ;
cp = cos (p ) ;
sp = s i n (p ) ;
c r = cos ( r ) ;
sr = sin ( r ) ;
% Bepalen van de posematrix
T_ga = [ cy*cp cy*sp* sr - sy * c r cy*sp* c r+sy * s r X;
sy *cp sy *sp* s r+cy* c r sy *sp* cr - cy* s r Y;
- sp cp* s r cp* c r Z ;
0 0 0 1 ];
% Bepalen van het centerpunt van de p o l s
p_pols
x_pols
y_pols
z_pols
=
=
=
=
T_ga* [ - f 0 0 1 ] ’ ;
p_pols ( 1 ) ;
p_pols ( 2 ) ;
p_pols ( 3 ) ;
% Bepalen van t1 , t2 en t3
t1 = atan2 ( - y_pols , x_pols ) ;
l1 = c ;
l 2 = s q r t (d^2 + e ^ 2 ) ;
xy = s q r t ( x_pols^2 + y_pols ^2) - b ;
z = z_pols - a ;
r = s q r t ( xy^2 + z ^ 2 ) ;
e p s i l o n = acos ( ( r ^2 - l 1 ^2 - l 2 ^2)/(2* l 1 * l 2 ) ) ;
alpha = atan ( ( s i n ( e p s i l o n ) ) / ( l 1 / l 2 + cos ( e p s i l o n ) ) ) ;
beta = e p s i l o n - alpha ;
gamma = p i /2 - atan (d/e ) ;
56
i f ( xy > eps )
d e l t a = atan ( z/xy ) ;
t2 = p i /2 - alpha - d e l t a ;
e l s e i f ( xy < - eps )
d e l t a = atan ( xy/z ) + p i / 2 ;
t2 = - ( alpha + d e l t a ) ;
e l s e % xy = 0
t2 = - alpha ;
end
t3 = ( alpha + beta - gamma ) ;
% Bepalen o r i ë n t a t i e p o l s ten o p z i c h t e van de arm
R_fa = T_ga ( 1 : 3 , 1 : 3 ) ;
s1
c1
s2
c2
s3
c3
=
=
=
=
=
=
s i n ( t1 ) ;
cos ( t1 ) ;
s i n ( t2 ) ;
cos ( t2 ) ;
s i n ( t3 ) ;
cos ( t3 ) ;
%O p s t e l l e n r o t a t i e m a t r i x R_ea
R_ba = [ c1 s1 0 ; - s1 c1 0 ; 0 0 1 ] ;
R_cb = [ c2 0 s2 ; 0 1 0 ; - s2 0 c2 ] ;
R_dc = [ c3 0 s3 ; 0 1 0 ; - s3 0 c3 ] ;
R_ed = [ 1 0 0 ; 0 1 0 ; 0 0 1 ] ;
R_ea = R_ba*R_cb*R_dc*R_ed ;
R_fe = R_ea. ’ * R_fa ;
i f abs ( abs (R_fe ( 1 , 1 ) ) - 1) < eps
%%
Singulariteit
t4 = 0 ;
t5 = 0 ;
t6 = atan2 (R_fe ( 2 , 3 ) , R_fe ( 2 , 2 ) ) ;
else
t4 = atan2 ( - R_fe ( 2 , 1 ) , - R_fe ( 3 , 1 ) ) ;
t5 = atan2 ( - s i n ( t4 )*R_fe ( 2 , 1 ) - cos ( t4 )*R_fe ( 3 , 1 ) , R_fe ( 1 , 1 ) ) ;
t6 = atan2 ( - R_fe ( 1 , 2 ) , R_fe ( 1 , 3 ) ) ;
end
t1
t2
t3
t4
t5
t6
=
=
=
=
=
=
t1 *(180/3 .14159265359 ) ;
t2 *(180/3 .14159265359 ) ;
t3 *(180/3 .14159265359 ) ;
t4 *(180/3 .14159265359 ) ;
t5 *(180/3 .14159265359 ) ;
t6 *(180/3 .14159265359 ) ;
57
B.9 Snelheidsfeedforward
f u n c t i o n [V_A1, V_A2, V_A3] = speed_feedforward (X_A1,X_A2,X_A3)
g l o b a l Xvorig_A1 ;
g l o b a l Xvorig_A2 ;
g l o b a l Xvorig_A3 ;
Ts = 0 .00025 ;
%AS1
i f X_A1 == Xvorig_A1
V_A1 = 0 ;
else
V_A1 = (X_A1 - Xvorig_A1 )/Ts ;
end
Xvorig_A1 = X_A1;
%AS2
i f X_A2 == Xvorig_A2
V_A2 = 0 ;
else
V_A2 = (X_A2 - Xvorig_A2 )/Ts ;
end
Xvorig_A2 = X_A2;
%AS3
i f X_A3 == Xvorig_A3
V_A3 = 0 ;
else
V_A3 = (X_A3 - Xvorig_A3 )/Ts ;
end
Xvorig_A3 = X_A3;
58
Rota e_yaw
[1x12]
rota e_pitch
[1x12]
Rota e_roll
[1x12]
Tijdspunten
[1x12]
Posi epunten_Z
[1x12]
Posi epunten_Y
[1x12]
Posi epunten_X
[1x12]
Traject_X
Traject_Y
Traject_Z
Tijd
yaw
Traject_yaw
yaw-rota e
Traject_pitch
Tijd
roll
Z
Y
X
pitch
Traject_roll
Pitch-rota e
Tijd
Roll-rota e
Tijd
Z-posi e
Tijd
Y-posi e
Tijd
X-posi e
Trajectgenerator
y
p
r
Z
Y
X
MATLAB Func on
Kinema ca_LMV
t6
t5
t4
t3
t2
t1
Kinematica
In out
Postion limit_As6 Prefilter_As6
Wenspositie_As5
6
Wenspositie_As4
5
Wenspositie_As3
4
Wenspositie_As2
3
Postion limit_As1 Prefilter_As1
In out
Velocity*
S peed limit2_As1
S peed
Controller_As1
PI(s)
Torque limit_As1
S peed limit_As6
S peed
Controller_As6
PI(s)
Torque limit_As6
Positieregelkringen As2 tot As5
S peed limit2_As6
Position
Controller_As6
Kp
Adaptive Position
Controller_As1
Position Error S peed limit_As1
R
Derivative_As6
du/dt
R value
3
Derivative_As1
du/dt
-K-
int16
inc to grad_As6
-K-
-K-
inc/1024ms
to grad/s_As6
2
5
Actual
position_As6
double
4
Actual
velocity_As6
Torque
command_As6
int32 to double
1_As6
double
int32 to
double_As6
Nm to Torque_As6 double to int_As6
-K-
2
Actual
position_As1
double
int32 to double
2_As1
1
Actual
velocity_As1
-K-
double
int32 to
double_As1
1
Torque
command_As1
int16
double to int16
inc to grad
-K-
inc/1024ms
to grad/s
Nm to Torque
B.10 Volledig MATLAB®/Simulink model
59

Figurenlijst (74 kB, 10 januari 2013)

Deze week zijn wij (Maud, Arne, Fé en Elke) op ArtBot week gegaan

Nieuwsbrief nr.16 15-12-2014

1. 1. A s 2. z Blauwe Sluis Aan de Noor steiger

Klik hier - Hobby elektronica

Türksat zenders zoeken met de M7 SAT-801

Klik hier voor de folder van de Extended

Eindbrochure SmartBot-AgroBot

View/Open