Masterproef Aansturen en afregelen van een 6-assige robot

Masterproef Aansturen en afregelen van een 6-assige robot

Studiegebied Industriële wetenschappen en technologie Opleiding Master in de industriële wetenschappen: elektrotechniek Afstudeerrich ng Automa sering Academiejaar 2012-2013

S jn Huysentruyt Academische bachelor- en masteropleidingen, Graaf Karel de Goedelaan 5, 8500 Kortrijk

Masterproef Aansturen en afregelen van een 6-assige robot

Studiegebied Industriële wetenschappen en technologie Opleiding Master in de industriële wetenschappen: elektrotechniek Afstudeerrich ng Automa sering Academiejaar 2012-2013

S jn Huysentruyt Academische bachelor- en masteropleidingen, Graaf Karel de Goedelaan 5, 8500 Kortrijk

Voorwoord

Student zijn was voor mij een zeer aangename periode en een levensfase waar ik nog vaak op zal terugblikken. Een periode waarin je dag op dag zowel je zel ennis als je kennis binnen je studiegebied kan verrijken. Een periode waarin ook bleek dat deze ontwikkelingen niet al jd hand in hand gaan. Doch, na enkele jaren breekt de dag aan dat je er klaar voor bent. Klaar om je verworven kennis naar buiten te brengen en een uitdaging aan te gaan. Ik was toen ook tevreden dat dit gevoel een invulling ging krijgen bij het kiezen van een masterproefonderwerp. Ik wou zowel een breed onderwerp als een onderwerp met enige theore sche diepgang. De kans om zelf een volledige robot controller op te bouwen en stelselma g de nodige intelligen e toe te voegen heb ik dan ook niet lang laten liggen. Dit was precies wat ik wou en ging perfect samen met mijn affiniteit om tot de kern van de zaken te gaan. Het was hoe dan ook een zeer leerrijke ervaring en een uitdagend project. Uiteraard verliep dit project niet zonder de nodige begeleiding. Daarbij wil ik vooral mijn promotoren dhr. S jn Derammelaere en dhr. Simon Houwen bedanken voor hun inzet en nauwe opvolging van mijn eindwerk. Jullie gaven mij van begin tot eind de nodige ondersteuning om dit project te doen slagen. In het bijzonder wil ik dhr. Kim Robbens van Beckhoff bedanken voor zijn hulp doorheen het project. De samenwerking met u was zowel een zeer leerrijke als een aangename ervaring. Hiernaast wil ik alle leden van het opleidingsteam bedanken voor de ps vanuit hun specifiek kennisdomein. Jullie zorgen er ook voor dat studeren aan de Hogeschool West-Vlaanderen meer is dan enkel studeren. Voor jullie zijn we meer dan zomaar studenten die komen en gaan wat echt wel zijn charmes hee . Dit alles was natuurlijk niet mogelijk zonder mijn ouders die me de kans gaven deze studies aan te va en. Zonder jullie stond ik niet waar ik nu sta. Ik ben jullie dan ook zeer dankbaar voor de kansen en ondersteuning die jullie me elke dag opnieuw gaven. Tenslo e wil ik ook de rest van het gezin en mijn vrienden bedanken voor de hulp en mooie jden die we samen reeds beleefden.

Huysentruyt S jn Juni 2013

i

Abstract

The Hogeschool West-Vlaanderen has a KUKA KR15/2 industrial robot at its disposal. However, the KUKA KRC1 controller which controls the robot is outdated and allows no adjustements in the regula on of the axes posi oning. Therefore a new controller with modern technology must take its place. This goal can be achieved by buying a new one or by building one ourselves. Although both ways have their pros and cons the choice for building our own controller was made. In this way the controller has a open architecture and every aspect of a robot controller must be self implemented. What comes with a lot of knowledge gained in the process. The main goal of this project is building and implemen ng a robot controller out of standard components. In this project these components consist of several drives and a industrial PC from Beckhoff. For their automaon pla orm TwinCAT®3 provides a MATLAB®/Simulink integra on. In this way it is possible to create a Simulink object in MATLAB®, compile it into a TwinCAT®object and run this la er in real- me on their industrial PC's. At last EtherCAT®(a real- me Ethernet protocol) provides in the communica on between the devices. Once the motors are hooked up to the drives and their configura on is done, the drives are able to regulate the current in the motor windings. Thus the desired torque can be delivered by every motor on the connected robot axis. The next step is the implementa on of de velocity and posi on controllers. Normally the drives are placed in velocity mode and the industrial PC generates the desired velocity so the desired posi on can be achieved. However, the mathema cal form of the used velocity and posi on controllers in TwinCAT®are not exactly known. Therefore the drives are placed in torque mode and a own controller object containing the velocity and posi on controller is build in Simulink. In this way the user has a complete open view of the controllers and the signals. What makes the tuning of these controllers, now both running in the industrial PC, easier. By execu ng a system iden fica on, based on a Simulink model, the torque-velocity bode characteris cs of the axes can be a ained. These are used to tune the velocity controllers and make ik possible to achieve the dynamic parameters of the axes. A erwards the posi on controller is tuned by the closed loop stepresponse. In this way the last three axes of the robot have been tuned performantly. However, the tuning of the first three axes requires more research on the occuring physical effects so that they can be compensated. When all axes of the robot are tuned the robot can be used to achieve the desired pose. Therefore the knowledge of the desired posi on for every axes is required. This is the task of the inverse kinema cs which was a ained by using a closed form solu on. On the other hand the actual pose of the robot is also needed. This is the task of the forward kinema cs which was a ained by matrix methods for 3D kinema cs. Both transforma ons have been implemented in MATLAB®/Simulink. By which it is possible to compile them into a TwinCAT®object and run them in real- me on the industrial PC. It might be clear that this open setup won't be reaching its limits any me soon and provides an excellent insight in the different aspects of a complete robot controller.

ii

Lijst met a or ngen C Cascade CPU

Parallel Central processing unit

I IGBT iPC

Insulated gate bipolar transistor industriële PC

N NOVRAM

nonvola le random access memory

P P-regelaar PI-regelaar PC PLC Pose

Propor onele regelaar Propor oneel integrerende regelaar Personal Computer Programmable Logic Controller Posi e en oriënta e

T TC3 es mate

TwinCAT® 3 Transfert func on es mate

X XAE XAR XAT

eXtended Automa on Engineering eXtended Automa on Run me eXtended Automa on Technology

iii

Inhoudsopgave 1 Inleiding 1.1 Situering . . . . . 1.2 Probleemstelling 1.3 Doelstelling . . . 1.4 Projectaanpak . .

. . . .

1 1 1 2 3

2 De KUKA KR15/2 2.1 Concept zes-assige robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4 4

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

3 Hardware en so ware controller 3.1 Hardware . . . . . . . . . . . . . . . 3.1.1 De aandrijvingen . . . . . . . 3.1.2 Het overkoepelend systeem . 3.1.3 Samenva end schema . . . . 3.2 So ware . . . . . . . . . . . . . . . . 3.2.1 TwinCAT® 3 . . . . . . . . . . 3.2.2 MATLAB®/Simulink integra e

. . . .

. . . . . . .

. . . .

. . . . . . .

. . . .

. . . . . . .

. . . .

. . . . . . .

4 Posi eregeling van de assen 4.1 De posi eregeling . . . . . . . . . . . . . . . 4.1.1 Geslotenlus en cascade werking . . . 4.1.2 De regelaars . . . . . . . . . . . . . 4.2 Situering regelaars . . . . . . . . . . . . . . 4.3 Systeemiden fica e . . . . . . . . . . . . . 4.3.1 Principe . . . . . . . . . . . . . . . . 4.3.2 Systeemiden fica e van A1 . . . . . 4.3.3 Bepalen van dynamische parameters 4.4 Afregelen van de assen . . . . . . . . . . . . 4.4.1 Methodiek . . . . . . . . . . . . . . 4.4.2 Afregelen van A6 . . . . . . . . . . . 4.4.3 Afregelen van A5 . . . . . . . . . . . 4.4.4 Afregelen van A4 . . . . . . . . . . . 4.4.5 Afregelen de eerste drie assen . . . . 4.5 Besluit . . . . . . . . . . . . . . . . . . . . . 5 Kinema sche transforma es 5.1 Pose coördinaten en joint coördinaten . . . 5.2 Voorwaartse kinema ca . . . . . . . . . . 5.2.1 Defini e . . . . . . . . . . . . . . . 5.2.2 De homogene transforma ematrix 5.2.3 Uitwerking . . . . . . . . . . . . . 5.3 Inverse kinema ca . . . . . . . . . . . . . 5.3.1 Defini e . . . . . . . . . . . . . . . 5.3.2 Uitwerking . . . . . . . . . . . . . 5.4 Besluit . . . . . . . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . .

. . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . .

. . . . . . .

5 5 5 6 8 9 9 10

. . . . . . . . . . . . . . .

11 11 11 12 13 14 14 16 17 18 18 21 25 26 27 28

. . . . . . . . .

29 29 30 30 30 32 36 36 37 41

iv

6 Conclusies 6.1 Bereikte resultaten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Sugges es voor verder onderzoek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

42 42 42

A De KUKA KR15/2 A.1 Mechanische eigenschappen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.2 Elektrische eigenschappen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

44 44 46

B Kinema ca B.1 Matrixmethodes voor 3D kinema ca . . . . B.1.1 De rota ematrix . . . . . . . . . . B.1.2 De homogene transforma ematrix B.1.3 Besluit . . . . . . . . . . . . . . . . B.2 Kinema ca van de KUKA KR15/2 . . . . . . B.2.1 Voorwaarste kinema ca . . . . . . B.2.2 Inverse kinema ca . . . . . . . . . B.2.3 Matlab code . . . . . . . . . . . .

47 47 47 52 53 54 54 59 65

C Datasheets Beckhoff

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

71

v

Lijst van figuren 1.1 1.2

De KUKA KR15/2 industriële robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Regelkringen in cascade als posi eregeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2 3

2.1

Onderdelen van de KUKA KR15/2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4

3.1 3.2 3.3 3.4 3.5 3.6 3.7

De AX5206 servo drive . . . . . . . . . . . . . . . . . . . . . . . . . . . . . De geselecteerde drives en remchopper in lijn met de AX-Bridge . . . . . . . De CX2030 iPC met de CX2100 power module en de EK1110 EtherCAT buskop Busbeze ng bij EtherCAT . . . . . . . . . . . . . . . . . . . . . . . . . . . . Samenva end schema van de geselecteerde hardware . . . . . . . . . . . . TwinCAT 3 eXtended Automa on Engineering . . . . . . . . . . . . . . . . . MATLAB/Simulink integra e in TwinCAT 3 . . . . . . . . . . . . . . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

5 6 7 8 8 9 10

4.1 4.2 4.3 4.4 4.5 4.6 4.7 4.8 4.9 4.10 4.11 4.12 4.13 4.14 4.15 4.16 4.17 4.18 4.19 4.20 4.21 4.22

Regelkringen in cascade als posi eregeling . . . . . . . . . . . . . Situering regelaars bij de verschillende drive modes . . . . . . . . Opmeten van een onbekend systeem . . . . . . . . . . . . . . . Bodekarakteris ek van een systeem . . . . . . . . . . . . . . . . Het ingangssignaal voor de systeemiden fica e . . . . . . . . . . De snelheidsresponsie van de eerste robot-as . . . . . . . . . . . Bode-karakteris ek van de koppelresponsie van de eerste robot-as Eigen snelheids- en posi eregelaar in MATLAB/Simulink . . . . . . De fasemarge ϕP M en snijpulsa e ωP M . . . . . . . . . . . . . . De stapresponsie van een eerste orde systeem . . . . . . . . . . Bode-karakteris ek koppelresponsie A6 . . . . . . . . . . . . . . Bode-karakteris ek koppelresponsie met filter en regelaar A6 . . Simulink object van de snelheidsregelkring en stapingang . . . . . Stapresponsie van de afgeregelde snelheidsregelaar . . . . . . . . De uiteindelijke stapresponsie van de posi e . . . . . . . . . . . Gewenste posi e en werkelijke posi e van A6 na het afregelen. . Bode-karakteris ek koppelresponsie A5 . . . . . . . . . . . . . . Bode-karakteris ek koppelresponsie met filter en regelaar A5 . . Bode-karakteris ek koppelresponsie A4 . . . . . . . . . . . . . . Bode-karakteris ek koppelresponsie met filter en regelaar A4 . . Bode-karakteris ek koppelresponsie A3 . . . . . . . . . . . . . . Bode-karakteris ek koppelresponsie A1 . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

11 13 14 15 16 17 17 18 19 21 22 22 23 23 24 24 25 25 26 26 27 28

5.1 5.2 5.3 5.4 5.5 5.6 5.7 5.8 5.9 5.10

Defini e van het basis- en eindeffector assenstelsel. . . . . . . . . . . . . . . . {b} beweegt t.o.v. {a}. Punt p beweegt samen met {b}. . . . . . . . . . . . . Samenstellen van verschillende poses . . . . . . . . . . . . . . . . . . . . . . De KUKA KR15/2 in nulposi e met de gedefiniëerde assenstelsels en afstanden Bepalen van ba T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van cb T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van dc T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van ed T, fe T en gf T . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van fe T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Berekening posi e van de robotpols . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

29 30 31 32 33 33 34 34 35 37

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

vi

5.11 Bovenaanzicht robotarm, berekening θ1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.12 Zijaanzicht robotarm, berekening θ2 en θ3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

38 39

A.1 A.2 A.3 A.4

De KUKA KR15/2 industriële robot Zijaanzicht KUKA KR15/2 . . . . . Bovenaanzicht KUKA KR15/2 . . . Motoren op de KUKA KR15/2 . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

44 45 45 46

B.1 B.2 B.3 B.4 B.5 B.6 B.7 B.8 B.9 B.10 B.11 B.12 B.13 B.14 B.15 B.16

Componenten Rij van een rota ematrix ba R . . . . . . . . . . . . . . . . . . . Rota e rond z a over een hoek α . . . . . . . . . . . . . . . . . . . . . . . . . De ZYZ rota e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . De XYZ rota e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . {b} beweegt t.o.v. {a}. Punt p beweegt samen met {b}. . . . . . . . . . . . . Samenstellen van verschillende poses . . . . . . . . . . . . . . . . . . . . . . De KUKA KR15/2 in nulposi e met de gedefiniëerde assenstelsels en afstanden Bepalen van ba T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van cb T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van dc T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van ed T, fe T en gf T . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van fe T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bepalen van de posematrix van de eindeffector uit de gegevens . . . . . . . . Berekening posi e van de robotpols . . . . . . . . . . . . . . . . . . . . . . . Bovenaanzicht robotarm, berekening θ1 . . . . . . . . . . . . . . . . . . . . . Zijaanzicht robotarm, berekening θ2 en θ3 . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

47 48 50 51 52 53 54 55 55 56 56 57 59 60 61 62

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

vii

1

Inleiding

Deze masterpaper is een weergave van de hard- en so warema ge ontwikkeling van een nieuwe controller voor de KUKA robot. In dit inleidende hoofdstuk wordt het onderwerp gesitueerd, waarna de probleemstelling, doelstelling en aanpak worden besproken.

1.1 Situering „If every tool, when ordered, or even of its own accord, could do the work that befits it... then there would be no need either of appren ces for the master workers or of slaves for the lords.” Aristoteles, 320 VC Al eeuwen zijn mensen op zoek naar verbeteringen op elk niveau. Zo kwam de hedendaagse technologie stap voor stap tot stand. In deze masterproef wordt een kleine stap gezet in de verdere ontwikkeling op vlak van robo ca. Robots zijn machines die taken uitvoeren waarvoor ze geprogrammeerd worden. In de volksmond worden robots vaak gezien als machinemensen. Een robot kan ook eenvoudiger. In de enge zin kan een robot omschreven worden als een combina e van starre lichamen met manipuleerbare verbindingen. Het manipuleren van deze verbindingen gebeurt vanuit het intelligente systeem van de robot: de controller. In de Hogeschool West-Vlaanderen, campus kortrijk staat een KUKA KR15/2 industriële robot. De controller van deze robot is echter verouderd en laat niet toe de posi eregeling van de individuele assen te op maliseren. Om deze reden is een nieuwe controller nodig. Deze kan ofwel aangekocht worden, ofwel zelf ontwikkeld worden. Voordelen om zelf een controller te ontwikkelen zijn: • Een „Open” controller, weg met het „Black box” gegeven. • Begrijpen van de technologie, door alles zelf te implementeren. • De studenten inzicht geven in elk aspect van een robot. Onder andere op basis van deze voordelen werd beslist zelf een controller hard- en so warema g op te bouwen en dit in het kader van een masterproef.

1.2 Probleemstelling Het vertrekpunt van deze masterproef is de KUKA KR15/2 robot met zijn motoren en hun feedback. Deze robot is een zes-assige industriële robot. Dit betekent dat er zes manipuleerbare verbindingen of joints op de robot aanwezig zijn. De joints stellen voor deze robot elk een roterende vrijheidsgraad voor, zie figuur 1.1 Een eerste eigenschap van dit mechanisch systeem is een hoge dynamische a ankelijkheid tussen de assen. De iner es van de assen zullen namelijk voor elke stand van de robot verschillend zijn. Een tweede eigenschap is de nood aan kinema sche transforma es. Elke as moet namelijk weten welke hoek deze moet aannemen zodat de robot in een gewenste stand komt. Beide eigenschappen zorgen alvast voor een mechatronisch uitdagend project.

1

+

+ A4

A5 -+

--

+

---

A6

A3

+ A2 --

--

A1 +

Figuur 1.1: De KUKA KR15/2 industriële robot

1.3 Doelstelling Het algemeen doel van de masterproef is de realisa e van een nieuwe controller voor de KUKA KR15/2 robot. Dit doel kan als volgt opgesplitst worden: 1. Verzamelen van technische data KUKA KR15/2: Vooraleer een nieuwe controller kan ontwikkeld worden moeten alle nodige data van de robot gekend zijn. Dit gaat om zowel de mechanische data van de robotarm als om de elektrische data van de motoren en hun feedback. 2. Selec e van de aandrijvingen en het overkoepelend systeem: Er zijn nieuwe aandrijvingen of drives nodig voor elke as. Deze moeten geselecteerd en geparametriseerd worden zodat deze compa bel zijn met de motoren en hun feedback. Om de aandrijvingen in gebruik te nemen en de robotsturing te implementeren moet een passend overkoepelend systeem gekozen worden. 3. Opbouw van de opstelling: De nieuwe controller moet opgebouwd worden en aangesloten worden op de robot. 4. Afregelen van de assen: De robotassen moeten afgeregeld worden zodat ze op een soepele, snelle en efficiënte manier hun wensposi e bereiken. 5. Opbouwen van kinema sche transforma es: Er zijn kinema sche transforma es nodig om de robot in een gewenste stand te brengen. Deze transforma es berekenen de overgang tussen pose coördinaten naar joint coördinaten en vice versa.

2

1.4

Projectaanpak

Vooraleer de nieuwe controller kan opgebouwd worden moeten de gegevens van de robot gekend zijn. Via datasheets, kenplaatgegevens, me ngen,... worden alle nodige gegevens van de robot verzameld. Deze gegevens zijn zowel de mechanische data van de robotarm als de elektrische data van de motoren en hun feedback, zie hoofdstuk 2 en bijlage A. Aan de hand van de verworven gegevens worden de aandrijvingen en het overkoepelend systeem uit het productgamma van Beckhoff geselecteerd. Hun automa seringspla orm (TwinCAT® 3) is namelijk in staat MATLAB®/Simulink modellen te compileren naar TwinCAT® 3 objecten en deze in real- me uit te voeren. De keuze voor Beckhoff lag dan ook reeds vast voor de aanvang van deze masterproef, zie hoofdstuk 3. Vervolgens kunnen de assen afgeregeld worden. Per as werken drie regelkringen in cascade (Fig. 1.2): (a) Stroomregelkring (b) Snelheidsregelkring (c) Posi eregelkring θ* + -

+

Positie ω* + + Regelaar -

Snelheid T* T i Regelaar

i* + -

+

Stroom Regelaar

IGBT’s

i

Motor

T

Proces

ω

integrator

θ

Figuur 1.2: Regelkringen in cascade als posi eregeling

De stroomregelaar regelt de stroom, en dus ook het gegenereerd koppel, in de motor. Deze regelaar bevindt zich in de drive, en is ingesteld volgens de elektrische parameters van de statorwikkelingen van de aangewende motor. De snelheidsregelaar zit ingebakken in de drive. Het is echter niet geweten hoe deze regelaar er wiskundig uitziet, wat de afregeling moeilijk maakt. Daarom wordt ervoor gekozen om de drives in torque mode te plaatsen. Op deze manier krijgen de drives, vanuit het overkoepelend systeem, het gewenste koppel en bezorgen ze de actuele posi e en snelheid van de motor terug aan het overkoepelend systeem. Dit betekent dat de snelheid- en posi eregelaar binnen het overkoepelend systeem moeten werken. Net dit is de kracht van de integra e van MATLAB®/Simulink binnen TwinCAT® 3. Elk Simulink model - zoals een regelkring - kan in TwinCAT® 3 in real- me uitgevoerd worden. In Simulink worden de snelheids- en posi eregelkringen opgebouwd en ingesteld via systeemiden fica e van het proces en de stapresponsie, zie hoofdstuk 4. Om de robot in een gewenste stand te brengen zijn kinema sche transforma es nodig. Ten eerste wordt de voorwaarste kinema sche transforma e bepaald. Deze berekent de pose (posi e en oriënta e) van het eindpunt of eindeffector van de robot in func e van de joint coördinaten (ashoeken). Ten tweede wordt de inverse kinema sche transforma e opgebouwd. Deze berekent de joint coördinaten van de robot in func e van de pose van de eindeffector. Via de kinema sche transforma es wordt dus overgegaan van de carthesische coördinaten van de robot naar de joint coördinaten en vice versa, zie hoofdstuk 5 en bijlage B.

3

2

De KUKA KR15/2

Het vertrekpunt van deze masterproef is de robot. In dit onderdeel wordt het concept van een zes-assige robot toegelicht. Voor extra informa e wordt verwezen naar bijlage A. Daar worden de belangrijkste gegevens van de robot opgesomd. Dit zijn zowel mechanische als elektrische gegevens.

2.1

Concept zes-assige robot

Een industriële robot is een serieketen van starre lichamen met manipuleerbare verbindingen. In het jargon wordt echter niet gesproken van starre lichamen en manipuleerbare verbindingen, maar van respec evelijk links en joints. Bij deze joints vallen twee types te onderscheiden: een roterende verbinding of een translerende verbinding. Door een varia e van het aantal joints en hun type ontstaan diverse mogelijkheden aan robotconfigura es. Ondanks deze grote varia e, zijn de zes-assige robots met roterende verbindingen toch in de meerderheid. Deze robots zijn namelijk geschikt voor een breder gamma aan toepassingen dan robots met minder vrijheidsgraden. Verder hebben de assen van een zes-assige robot elk een specifieke taak, ongeacht de robo abrikant en het kaliber van de robot. Deze taken worden hier toegelicht aan de hand van de KUKA KR15/2, zie figuur 2.1. A1: De eerste as bevindt zich op de basis van de robot en laat toe de volledige robotarm te pivoteren rond de basis. A2:

De tweede as verbindt het rad met de onderarm en laat toe de arm verder of korter te doen reiken.

A3: De derde as verbindt de onderarm met de bovenarm en laat toe de arm omlaag of omhoog te bewegen. A4:

De vierde as ligt in de lengte van de bovenarm en brengt samen met A5 de pols in de gewenste rich ng.

A5: De vijfde as verbindt de bovenarm met de pols en brengt samen met A4 de pols in de gewenste rich ng A6:

De zesde as verbindt de pols met de eindeffector en brengt de eindeffector in de gewenste oriënta e.

Bovenarm

+ A5 --

+ A4

Pols +

-A6

+

---

Eindeffector

A3

Onderarm A1 -+

+ A2 --

Rad

Basis

Figuur 2.1: Onderdelen van de KUKA KR15/2

Hieruit volgt dat de eerste drie assen de posi e van de robotpols bepalen en de laatste drie de posi e en oriënta e van de eindeffector. Deze gedachtengang geldt voor alle zes-assige robots. De opbouw van zes-assige robots zorgt dus voor twee typerende eigenschappen. Ten eerste zijn de assen dynamisch a ankelijk van elkaar. De iner es rond de assen zijn namelijk verschillend voor elke stand van de robot. Ten tweede kan er niet zomaar overgegaan worden van joint coördinaten naar pose coördinaten en omgekeerd. Hiervoor zijn kinema sche transforma es nodig, zie hoofdstuk 5.

4

3

Hardware en so ware controller

Een nieuwe controller betekent nieuwe toestellen. In dit hoofdstuk worden de hardware en so ware aspecten van deze toestellen die samen de controller vormen toegelicht. Eerst worden de drives en het overkoepelend systeem besproken. Hierna volgt uitleg over het automa seringspla orm TwinCAT® 3 en de MATLAB®/Simulink integra e.

3.1

Hardware

Om een nieuwe controller op te bouwen zijn intelligente toestellen nodig. Elk toestel hee een specifieke taak. De drives zorgen voor de vermogenstoevoer aan de motoren en de koppeling met het overkoepelend systeem. Het overkoepelend systeem zorgt op zijn beurt voor het aansturen van de drives en de uitvoering van de extra intelligen e. Dit zijn onder andere de kinema sche transforma es.

3.1.1

De aandrijvingen

Alle assen op de robot zijn geslotenlus systemen. De motoren leveren een koppel aan het mechanische systeem en lezen tegelijk de hoekposi e van de motoras uit. Deze geslotenlus systemen, of servosystemen, moeten aangedreven worden door servodrives. Het productgamma van Beckhoff voorziet hierin met hun AX5000 reeks. Na het overlopen van enkele eigenschappen volgt de selec e van de toestellen. De AX5000 serie

A ankelijk van het vermogen zijn drives beschikbaar met 1 of 2 servo kanalen. Zo bezit de AX5206 twee servo kanalen die elk zes ampère kunnen leveren aan de motor, zie figuur 3.1. Voor mul -axes toepassingen kunnen de drives in lijn geplaatst worden en eenvoudig verbonden worden met de AX-Bridge. Deze verbindt de stuurspanning, de vermogenspanning en de tussenkringspanning van de drives met elkaar. Hierdoor is een uitgebreid energie management mogelijk. Zo kan een drive zijn remenergie dissiperen in de ingebouwde remweerstanden van alle aangesloten drives. Indien dit nog onvoldoende is, kan de AX5021 ingezet worden. Deze remchopper controleert de tussenkringspanning en zal indien nodig de overtollige energie in zijn interne of oponele externe remweerstanden dissiperen. Verder bezi en de drives een EtherCAT®interface. Deze veldbus is momenteel de snelste op de markt en op maal voor drivetoepassingen. Zie 3.1.2 voor meer informa e over EtherCAT®. Figuur 3.1: De AX5206 servo drive

5

Geselecteerde toestellen Aan de hand van de nominale stroom van de motoren kunnen de nodige drives uit de AX5000 reeks geselecteerd worden. Met het oog op snelle bewegingen van de robot wordt ook de AX5021 remchopper met extra externe weerstanden gekozen. Zie tabel 3.1 voor een overzicht van de geselecteerde drives. Tabel 3.1: Geselecteerde AX5000 drives

Motor 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

AX5000 Drive AX5112 AX5112 AX5203 AX5203

De drives en remchopper worden in lijn geplaatst en verbonden via de AX-Bridge. Op deze manier moet enkel de eerste drive op de stuurspanning en vermogenspanning aangesloten worden. Zie figuur 3.2 voor een overzicht van de drive opstelling.

AX5112 A2

AX5203 A3 & A4

AX5203 A5 & A6

AX5021 Remchopper

AX-Bridge

AX5112 A1

Figuur 3.2: De geselecteerde drives en remchopper in lijn met de AX-Bridge

3.1.2

Het overkoepelend systeem

Om de drives aan te sturen is extra intelligen e nodig. Deze zal uitgevoerd worden door het overkoepelend systeem. Dit overkoepelend systeem bestaat uit een iPC (industriële PC) en een EtherCAT veldbus. De iPC zal alle berekeningen uitvoeren en de veldbus verzorgt de communica e tussen de toestellen.

6

De industriële PC De industriële PC (kortweg iPC) van Beckhoff kan gezien worden als een PLC (programmable logic controller) die gebaseerd is op PC technologie. De iPC hee Windows als besturingssysteem waarop een TwinCAT runme systeem draait. Het TwinCAT run me systeem werkt met een base clock die elke cyclus de Windows taken onderbreekt en de geprogrammeerde taken uitvoert. Dit betekent dat de iPC als real- me controller kan ingezet worden. Een robot aansturen vraagt een hoop rekenwerk. Om deze reden werd gekozen voor de CX2030 embedded iPC. Deze iPC hee een Intel® Core™i7 dual-core CPU (central processing unit), zodat er voldoende rekenkracht aanwezig is om alle berekeningen bij een korte cyclus jd uit te voeren. Deze cyclus jd is minstens één keer de laagste waarde van de base clock en bedraagt 250µs. Verder bezit de CX2030 een NOVRAM module, wat staat voor nonvola le random access memory. Hierdoor kan data - zoals de posi es van de robotassen - onthouden worden wanneer de controller spanningsloos is. De CX2030 wordt gevoed door de CX2100 power module en de EK1110 EtherCAT buskop zorgt voor de link met de drives. Zie figuur 3.3 voor de volledige iPC configura e en bijlage C voor de datasheets.

Figuur 3.3: De CX2030 iPC met de CX2100 power module en de EK1110 EtherCAT buskop

EtherCAT® Om vloeiende bewegingen van de robot te verkrijgen, moet de iPC elke 250µs zijn taken uitvoeren en data uitwisselen met de drives. De data moet echter langs beide zijden perfect gereconstrueerd kunnen worden. Hierdoor moet de updatefrequen e van de veldus minstens twee keer groter zijn dan de frequen e van de data1 . Dit betekent dat de cyclus jd van de veldbus maximaal 125µs mag bedragen. Aan deze vereiste biedt EtherCAT®de oplossing2 . EtherCAT®is een real- me ethernet veldbus. Deze maakt gebruik van het one total frame protocol, wat het volgende inhoudt: Elke I/O cyclus gaat één Ethernet II bericht, opgesteld door de master, doorheen alle slaves en keert het terug naar de master. Elke slave haalt de procesdata, die voor hem bestemd is, uit het bericht en plaatst de procesdata, die voor de master bestemd is, in het bericht. Indien één Ethernet II bericht onvoldoende is worden meerdere berichten verstuurd, wat in deze toepassing echter niet het geval is. Op deze manier is EtherCAT een Full-Duplex systeem en wordt het Ethernet II frame op maal benut. Wat samen met de 100Mbps verbinding leidt tot een snelle en efficiënte bus. 1 Bemonsteringstheorema van Nyquist-Shannon[3] 2 Cursus real-

me Ethernet [4]

7

Het real- me karakter van de veldbus wordt bepaald door de I/O cyclus jd, zie figuur 3.4. Procesdata I/O

Mailbox

Parameterdata, TCP/IP, Sercos, ... I/O

Mailbox

I/O

Mailbox

I/O Cyclustijd

I/O

tijd

Bus idle

Figuur 3.4: Busbeze ng bij EtherCAT

Elke I/O cyclus jd wordt het one total frame verzonden. In de resterende jd kan andere data verzonden worden. Dit gebeurt aan de hand van de mailbox. Deze zorgt enerzijds voor het verzenden van de parameterdata en anderzijds voor de compabiliteit met andere systemen. Dankzij dit protocol kunnen 100 servo-assen in 100µs voorzien worden van nieuwe data. Hierdoor kan de vereiste cyclus jd van 125µs voor 6 assen en wat extra I/O van deze opstelling zeker behaald worden.

3.1.3

Samenva end schema

Zie figuur 3.5 voor het samenva end schema en bijlage C voor de datasheets.

CX 2030 Industriële PC:

AX5000 Drives en remchopper: Resolver aansluitingen

Motor aansluitingen

Figuur 3.5: Samenva end schema van de geselecteerde hardware

8

3.2

So ware

De reden waarom voor Beckhoff hardware gekozen werd, is de MATLAB®/Simulink integra e binnen hun automa seringspla orm TwinCAT®3. Eerst wordt TwinCAT®3 toegelicht, hierna komt de MATLAB®/Simulink integra e aan bod.

3.2.1

TwinCAT® 3

TwinCAT®3 is het recentste automa seringspla orm van Beckhoff. Het systeem wordt ook wel eXtended Automa on Technology of kortweg XAT genoemd. De nieuwe architectuur laat het gebruik van C en C++ als programmeertaal toe. Dit maakt de expansie of integra e van reeds bestaande systemen mogelijk. Hiervan is de link met MATLAB®/Simulink een voorbeeld. De programmeeromgeving van TwinCAT®3 - eXtended Automa on Engineering of kortweg XAE - werd geïntegreerd in Microso Visual Studio®, zie figuur 3.6. Hierdoor kunnen automa seringsobjecten zowel volgens de IEC 61131-3 norm, als in C of C++ geprogrammeerd worden. Eens deze objecten of modules gegenereerd worden, kunnen ze ona ankelijk van hun programmeertaal data uitwisselen en elkaar oproepen. Door ook nog de System Manager te integreren kan een volledig project geconfigureerd, geparametriseerd en geprogrammeerd worden vanuit de XAE.

TwinCAT 3 TwinCAT 3 Engineering Environment based on Visual Studio ® System Manager Configuration – I/O – PLC – C/C++ – MC – NC – CNC – Safety – others

Programming Real-time Nonreal-time C#/.NET

IEC 61131 Objectoriented extensions

IEC Compiler

C/C++

Matlab® / Simulink®

Simulink Coder™

Third-party programming tool

C/C++

Microsoft C Compiler

TwinCAT Transport Layer – ADS TwinCAT 3 Runtime

Figuur 3.6: TwinCAT 3 eXtended Automa on Engineering

Op de iPC is de eXtended Automa on Run me of kortweg XAR werkzaam. Deze so ware zal aan de hand van de base-clock de windows taken onderbreken en de geprogrammeerde taken uitvoeren. Hierdoor wordt de real- me werking van het systeem gegarandeerd. Verder laat de modulaire opbouw van TwinCAT®3 mul -core processoren toe als resource. Hierdoor worden toepassingen die veel rekenwerk eisen - zoals het aansturen van een 6-assige robot - mogelijk gemaakt.

9

3.2.2

MATLAB®/Simulink integra e

Door de MATLAB®/Simulink integra e in TwinCAT®3 kunnen Simulink modellen gecompileerd worden naar TwinCAT®3 objecten (Fig. 3.7). Op deze manier zijn deze modellen in real- me werkzaam binnen de XAR. Het grote voordeel hiervan is dat alle Simulink toolboxes ingezet kunnen worden in een groter geheel. Enkele voorbeelden van nu ge toepassingen zijn: • Opbouwen van regelaars, wat in deze masterproef toegepast wordt. • Simuleren van processen, zodat de werking kan nagegaan worden jdens de ontwerpfase. • Op maliseren van bestaande toepassingen, ten gunste van de produc viteit. De gecompileerde objecten worden ingevoegd als een TcCOM object. Hieraan moet een task en een prioriteit toegewezen worden, conform de IEC norm. Naast het compileren van Simulink objecten naar een TcCOM object, is er ook communica e tussen TwinCAT®3 en Simulink mogelijk. Hiermee kunnen onder andere parameters van Simulink modellen on-the-fly aangepast worden. Deze interface is echter nog in ontwikkeling en kon dus nog niet ingezet worden.

MATLAB/Simulink

Simulink Coder TwinCAT 3

Figuur 3.7: MATLAB/Simulink integra e in TwinCAT 3

10

4

Posi eregeling van de assen

Aan de hand van de nieuwe controller kan de robot in bedrijf genomen worden. Om de robot echter te laten bewegen, moet elke as voorzien worden van een posi eregeling. Deze posi eregelingen moeten performant afgeregeld worden, zodat de assen op een soepele, snelle en efficiënte manier hun wensposi es bereiken. Deze topics en de hierbij toegepaste methodes en technieken, komen in dit hoofdstuk aan bod.

4.1

De posi eregeling

4.1.1

Geslotenlus en cascade werking

De assen op de robot zijn geslotenlus- of servosystemen. Algemeen bestaat een geslotenlus systeem uit een actuator, die een fysisch systeem kan beïnvloeden en een sensor, die de toestand van dit systeem opmeet. De lus wordt gesloten door de sensorwaarde als feedback te gebruiken en hiermee de actuator aan te sturen. Om aan deze vereiste tegemoet te komen, bezit de KUKA KR15/2 robot permanent magneet motoren als actuatoren en resolvers als feedback. Zo kan elke as aan de hand van een posi eregeling zijn wensposi es opvolgen. Als posi eregeling worden drie regelkringen in cascade geplaatst, zie figuur 4.1. θ* + -

+

Positie ω* + + Regelaar -

Snelheid T* T i Regelaar

i* + -

+

Stroom Regelaar

IGBT’s

i

Motor

T

Proces

ω

integrator

θ

Figuur 4.1: Regelkringen in cascade als posi eregeling

Om deze posi eregeling eenvoudig uit te leggen, wordt vertrokken vanaf de motoren. Deze zijn in staat een koppel (T) te leveren aan de robotassen via mechanische reduc es. Hiervoor moeten echter stromen (i) naar de motor gestuurd worden. Dit gebeurt door de IGBT's (insulated gate bipolar transistors) in de vermogenmodule van de drive. Deze schakelen de tussenkringspanning van de drive door naar de statorwikkelingen van de motor. In deze stroomregelkring zijn de IGBT's dus de actuatoren, terwijl een stroomme ng instaat voor de feedback. Om nu de gewenste stromen (i∗ ) in de statorwikkelingen te regelen wordt een stroomregelaar ingezet. Deze regelaar compenseert het eerste orde gedrag van de statorwikkelingen en wordt dan ook enkel door de weerstand en inductan e van deze spoelen bepaald. Verder is er een recht evenredig verband aanwezig tussen de stroom in de statorwikkelingen en het geleverde koppel aan de motor-as. Hierdoor kan besloten worden dat de stroomregelkring er telkens voor zorgt, dat het gewenste koppel (T∗ ) door de motor geleverd wordt aan de betreffende robot-as. Onder invloed van dit koppel zal de verbonden robot-as gaan versnellen met de iner e van de as als belangrijkste parameter. Een posi ef koppel versnelt de as terwijl een nega ef koppel de as vertraagt. Hierdoor kan de snelheid van de robot-as eenvoudig gemanipuleerd worden. Aan de hand van een snelheidsme ng en snelheidsregelaar wordt de snelheidslus gesloten. De snelheidsme ng gebeurt aan de hand van de resolver op de motor-as, deze leest con nu de werkelijke posi e van de motor-as (θ) uit. Hiermee bepaalt de drive de werkelijke snelheid (ω) aan de hand van een interne klok. De snelheidsregelaar zal nu een gewenst koppel uitsturen naar de stroomregelkring zodat de gewenste snelheid (ω ∗ ) telkens bereikt wordt.

11

Uiteindelijk moet de robot-as zijn gewenste posi e (θ∗ ) bereiken. Dit gebeurt door een posi eregelkring, bestaande uit een posi eme ng ter hoogte van de motor-as en een posi eregelaar. De posi eregelaar zal hierbij de gewenste snelheid uitsturen naar de snelheidsregelaar zodat de wensposi e bereikt wordt.

4.1.2

De regelaars

Met de algemene werking van de posi eregeling in het achterhoofd, kunnen de specifieke kenmerken van de regelaars toegelicht worden. Merk op dat de regelaars werken met de fout tussen de gewenste waarde en de werkelijke waarde van hun grootheid, en ze deze trachten weg te regelen. De stroomregelaar De stroomregelaar regelt de stroom in de statorwikkelingen of spoelen van de motor. Deze spoelen zijn in feite eerste orde systemen, waardoor de stroom pas enige jd na het aanleggen van de spanning, in regime komt. Bij het wegvallen van de spanning zakt de stroom terug naar nul. Hierdoor is een PI-regelaar (propor oneel integrerende regelaar) nodig. De propor onele ac e versterkt de fout en de integrerende ac e integreert de fout naar een constante waarde. Indien er enkel een propor onele regelaar aanwezig zou zijn, zal de spanning over de spoel wegvallen wanneer de fout nul wordt. Hierdoor zou de stroom afnemen, waardoor de wenswaarde niet bereikt wordt. Dit effect wordt opgelost door de integrerende ac e, wanneer de fout nul wordt zal deze een constante waarde uitsturen waardoor de stroom constant blij op de gewenste waarde. Deze processen worden ook wel integrerende processen genoemd. Zoals eerder vermeldt worden deze regelaars ingesteld door de inductan e en weerstand van de statorwikkelingen van de aangewende motor. Deze parameters werden opgenomen uit datasheets of zelf opgemeten en behoren integraal tot de parameterset van de motoren. De snelheidsregelaar Net zoals de stroomopbouw in de statorwikkelingen, behoort de snelheid van een mechanisch systeem tot de integrerende processen. Bij het wegvallen van de fout, zal er bij een propor onele regelaar geen koppel meer uitgestuurd worden, waardoor de snelheid onder invloed van wrijving naar nul gaat en de wenssnelheid niet bereikt wordt. Hierdoor wordt ook voor de snelheidsregelaar een PI-regelaar ingezet. Op deze manier zal de snelheidsregelaar het wrijvingskoppel gaan compenseren door de integrerende ac e. Het instellen van deze regelaar gebeurt via een systeemiden fica e, zie 4.3. De posi eregelaar Om de posi es van de robot-assen te bereiken is een P-regelaar (propor onele regelaar) voldoende. Bij het wegvallen van de fout is de wensposi e bereikt en hoe er geen snelheid uitgestuurd te worden naar de snelheidsregelaar. Deze regelaar wordt ingesteld via de stapresponsie van de posi e. Aangezien dit een posi eregeling is mag hierbij geen doorschot optreden.

12

4.2

Situering regelaars

De toegepaste posi eregeling bevat drie regelaars die werken in cascade. Deze regelaars kunnen echter op verschillende plaatsen in het systeem werkzaam zijn. De stroomregelaar stuurt schakelpulsen naar de IGBT's in de vermogenmodule van de drive. Deze regelaar zit dus ingebakken in de drive, waar deze zijn taak uitvoert. Naast de stroomregelaar bevat de drive ook een snelheids- en posi eregelaar, deze kunnen al dan niet gebruikt worden. Zodat de drive in drie verschillende modes kan werken (Fig. 4.2): Torque control mode: Enkel de stroomregelaar is werkzaam in de drive, deze zorgt dat het gewenste koppel door de motor geleverd wordt. Dit betekent dat de snelheids- en posi eregelaar werkzaam zijn in de iPC en ze het gewenste koppel via EtherCAT doorsturen naar de drive. De drive koppelt op zijn beurt de actuele snelheid en posi e van de motor-as terug naar de regelaars in de iPC. Velocity control mode: Zowel de stroomregelaar als de snelheidsregelaars zijn werkzaam in de drive, deze zorgen ervoor dat de motor aan de gewenste snelheid draait. Dit betekent dat enkel de posi eregelaar werkzaam is in de iPC en deze de gewenste snelheid doorstuurt naar de drive. De drive zal hierbij de actuele posi e van de motor-as terugkoppelen naar de posi eregelaar in de iPC. Pos on control mode: Alle regelaars zijn werkzaam in de drive, deze zorgen ervoor dat de motor-as in de gewenste posi e komt te staan. Hierdoor moet de iPC enkel een posi ewens voorzien en deze naar de drive sturen. Eventueel kan de drive de volgfout of een status bij het bereiken van de wensposi e terugkoppelen naar de iPC.

Torque control mode Posiie regelaar Snelheidsregelaar

Stroomregelaar

Velocity control mode Posiie regelaar

Stroomregelaar Snelheidsregelaar

Posiion control mode Wensposiie

Stroomregelaar Snelheidsregelaar Posiie regelaar

T*

ω*

θ*

ω,θ

θ

Δθ

Figuur 4.2: Situering regelaars bij de verschillende drive modes

Binnen TwinCAT 3 worden de drives standaard in velocity control geplaatst. De stroom- en snelheidsregelaar zijn dan werkzaam in de drive, en de posi eregelaar wordt in de iPC uitgevoerd als een TwinCAT NC mo on object. Om de assen af te regelen moeten de snelheids- en posi eregelaars van de assen afgeregeld worden. Indien deze TwinCAT methode toegepast wordt, moeten de TwinCAT regelaars als ”black box” gebruikt worden. Het is echter niet geweten hoe deze regelaars er wiskundig uitzien, wat de afregeling moeilijk maakt. Om deze reden wordt ervoor gekozen om de drives in torque mode te plaatsen. Hierdoor kan een eigen invulling gegeven worden aan de snelheids- en posi eregelaar via een MATLAB/Simulink model. De openheid van de nieuwe controller is dan ook deels ten gunste van deze mogelijkheid.

13

4.3

Systeemiden fica e

In 4.1.2 werd reeds aangegeven dat de snelheidsregelaars ingesteld worden via een systeemiden fica e1 . Vooraleer in te gaan op de afregeling wordt deze systeemiden fica e toegelicht. Eerst komt het principe aan bod, daarna volgt een prak sch voorbeeld aan de hand van de eerste robot-as. Tenslo e wordt aangetoond hoe dynamische systeemparameters uit een systeemiden fica e bepaald kunnen worden.

4.3.1

Principe

Met behulp van fysica en systeemtheorie kan een wiskundig model van een systeem opgebouwd worden. Dit is in de meeste gevallen voldoende om het model te simuleren. Het is echter moeilijk om parameters zoals wrijving, demping en niet-lineariteiten in te scha en. Deze onbekende parameters dienen dan ingevuld te worden door een me ng. Dit is dan ook het doel van een systeemiden fica e, deze bepaalt de beschrijving van een systeem op basis van meetdata. Hierdoor kan het wiskundige model gevalideerd en ingezet worden voor verschillende doeleinden, bijvoorbeeld: • Instellen van een regelaar • Iden fica e of ontwerp van systeemeigenschappen • Machine diagnose op basis van het model Vooraleer dit model bekomen wordt, moet het onbekende systeem opgemeten worden. Via de ingang kan het systeem geëxciteerd worden, met de responsie van het systeem tot gevolg, zie figuur 4.3.

Ingang

Onbekend systeem

Responsie

ϕ=ω.∆ t Au

Amplitude (-)

4

Ingang Responsie A M= u Ai ω=2πf

Ai

2 0 -2 -4 0

5

10

15

20

25

30

Tijd (s) Figuur 4.3: Opmeten van een onbekend systeem

Door beide signalen op te meten en hierop een spectraalanalyse uit te voeren kan de bode-karakteris ek van het systeem bepaald worden. De spectraalanalyse zorgt voor de overgang van het jdsdomein naar het frequen edomein, terwijl de bode-karakteris ek zorgt voor een eenvoudige weergave. Deze bodekarakteris ek bevat twee karakteris eken. De eerste is de amplitude-karakteris ek, deze gee de verhouding (M) weer van de amplitudes aan de uitgang (Au ) en ingang (Ai ) van het systeem. De tweede is de fase-karakteris ek, deze gee de faseverschuiving (φ) weer tussen de uitgang en ingang van het systeem. Beide karakteris eken zijn een func e van de pulsa e (ω) van de signalen. Zie figuur 4.4 voor het resultaat. 1 Slideshow systeemiden

fica e [5]

14

M(dB) A uit (ω1) A in (ω1)

ϕ [°]

ω1 ω1 ϕuit (ω1)-ϕin (ω1)

ωlog ωlog

Figuur 4.4: Bodekarakteris ek van een systeem

De bovenste karakteris ek gee de amplitudeverhouding (M) weer, terwijl de onderste karakteris ek de faseverschuiving (φ) weergee . Merk op dat de pulsa es en de amplitudeverhouding een logaritmische schaal hebben. Verder wordt niet meer gesproken van de amplitudeverhouding, maar wel van de versterking van het systeem, deze wordt uitgedrukt in decibell (dB). Om de bode-karakteris ek uit figuur 4.4 te bekomen, moet er meetdata beschikbaar zijn van alle weergegeven frequen es. Hiervoor moeten deze frequen es in het systeem aanwezig zijn op het moment van de me ng. Dit kan enkel door een signaal met deze frequen es aan te leggen aan de ingang van het systeem. De frequen e inhoud aan de ingang van het systeem is dus bepalend voor de frequen e inhoud van de responsie. Om deze reden moet er bij elke me ng een ingangssignaal voorzien worden die het gewenste frequen edomein bevat. Hiernaast moeten de signalen meetbaar zijn. Uit het bemonsteringstheorema van Nyquist-Shannon is geweten dat de maximaal meetbare frequen e van een signaal de hel van de samplefrequen e is. Dit eerste betekent dat de samplefrequen e van het mee oestel de maximaal meetbare frequen e bepaalt. Een tweede bo leneck is het aantal me ngen die het mee oestel kan opslaan. Indien er met een grote samplefrequen e gemeten wordt, raakt het meetgeheugen snel vol. Wanneer dit problemen veroorzaakt, moet het op te meten frequen edomein opgesplitst worden in meerdere segmenten. Lagere frequen es kunnen dan met een lagere samplefrequen e gemeten worden en omgekeerd. De bode-karakteris eken van de segmenten kunnen dan nadien samengesteld worden. Samengevat moeten volgende stappen doorlopen worden om een systeemiden fica e uit te voeren: (a) Kies het frequen edomein waarin het systeem moet geïden ficeerd worden. (b) Stel een ingangssignaal op die de op te meten frequen es bevat. (c) Stel de samplefrequen e in op het dubbele van de maximale op te meten frequen e. (d) Splits het op te meten frequen edomein op indien het meetgeheugen te beperkt is. (e) Voer de me ng(en) uit en pas een spectraalanalyse toe op de meetgegevens. (f) Stel de bode-karakteris eken op aan de hand van de verwerkte meetgegevens. Om dit alles aan te tonen wordt in 4.3.2 een voorbeeld uitgewerkt.

15

4.3.2

Systeemiden fica e van A1

In dit deel wordt een prak sch voorbeeld van een systeemiden fica e gegeven aan de hand van de eerste robot-as. Het doel van deze me ng is het opmeten van de koppelresponsie van de eerste as. Als ingang wordt een koppel aan de as geleverd, als uitgang wordt de snelheid van de motor-as opgemeten. Hierbij wordt het samenva end stappenplan uit 4.3.1 gevolgd: (a) Kies het frequen edomein waarin het systeem moet geïden ficeerd worden. Het frequen edomein van 0,1Hz tot 2000Hz moet opgemeten worden. (b) Stel een ingangssignaal op die de op te meten frequen es bevat. Als ingangssignaal wordt een sweep gebruikt. Dit is een sinussignaal met een lineair s jgende frequen e. De star requen e is 0Hz en na 10 minuten bedraagt de frequen e 2000Hz. Zie figuur 4.5.

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 4.5: Het ingangssignaal voor de systeemiden fica e

(c) Stel de samplefrequen e in op het dubbele van de maximale op te meten frequen e. De samplefrequen e mag minimaal 4000Hz (2×2000Hz) zijn. Aangezien de iPC de snelheid omrekent van inc/1024ms naar rad/s moet zijn cyclus jd op 1/4000Hz = 250µs ingesteld worden. De TwinCAT Scope, het mee nstrument van TwinCAT, kan dan in elke cyclus de actuele koppelvraag en snelheid opmeten en wegschrijven naar het meetgeheugen. (d) Splits het op te meten frequen edomein op indien het meetgeheugen te beperkt is. Met behulp van de TwinCAT Scope kan 10 minuten gemeten worden. Het op te meten frequen edomein moet dus niet opgesplitst worden. (e) Voer de me ng(en) uit en pas een spectraalanalyse toe op de meetgegevens. De me ng wordt gestart en het sweep signaal wordt met het wenskoppel verbonden. Figuur 4.6 gee een deel van de opgemeten snelheidsresponsie weer. Na de me ng worden de meetresultaten geïmporteerd in MATLAB. Met behulp van de es mate (tranfert func on es mate) func e wordt een spectraalanalyse uitgevoerd.

16

Snelheidsresponsie 200

Amplitude (rad/s)

150

100

50

0

-50

-100

-150

-200 0

2

4

6

8

10

12

Tijd (s) Figuur 4.6: De snelheidsresponsie van de eerste robot-as

(f) Stel de bode-karakteris eken op aan de hand van de verwerkte meetgegevens. Aan de hand van de output van de es mate func e worden de amplitude- en fase karakteris ek van de koppelresponsie in MATLAB opgesteld. Zie figuur 4.7 voor het resultaat.

Bode-karakterisiek koppelresponsie A1 Versterking (dB)

60

Resonanie piek

40

20

Ani-resonanie piek

0

-20

-40 -2 10

-1

10

0

10

200

1

10

2

10

Pulsaie (rad/s)

3

10

4

10

5

10

150

Fase (°)

100 50 0 -50 -100 -150 -200 -2 10

-1

10

0

10

1

10

2

10

3

10

4

10

5

10

Pulsaie (rad/s) Figuur 4.7: Bode-karakteris ek van de koppelresponsie van de eerste robot-as

4.3.3

Bepalen van dynamische parameters

Wanneer de bode-karakteris ek in figuur 4.7 nader bekeken wordt, zijn resonan e pieken waar te nemen. Deze zijn nadelig bij de instelling van de regelaar, maar hebben als voordeel dat ze informa e van het systeem beva en. Op deze manier kunnen de dynamische parameters van het systeem bepaald worden. Deze parameters kunnen - in een later stadium - ingevuld worden in een dynamisch model van de robot. Zo kunnen de assen deels aangestuurd worden en zal de volgfout verkleinen. Momenteel worden de assen namelijk bijgestuurd door de volgfout, zodat er jdens een beweging al jd een fout aanwezig is.

17

4.4

Afregelen van de assen

Nu de werking van de posi eregeling en het principe van een systeemiden fica e gekend zijn, kunnen de assen afgeregeld worden. Om te beginnen wordt de gebruikte methodiek toegelicht. Hierna komt de effec eve afregeling van de assen aan bod.

4.4.1

Methodiek

Eigen invulling snelheids- en posi eregelaar In 4.2 werd gekozen om de drives in torque mode te plaatsen. Hierdoor kan een eigen invulling voor de snelheids- en posi eregelaar voorzien worden, zodat de wiskundige beschrijving van de regelaars bekend is. Dit maakt het afregelen van de regelkringen eenvoudiger en brengt een open kijk in de posi eregeling van de assen met zich mee. Hiervoor moet een MATLAB/Simulink model opgebouwd worden die de snelheidsen posi eregelkring sluit en in real- me werkzaam is in de iPC. Zo werd een Simulink model opgebouwd die als mal gebruikt wordt voor de posi eregeling van de assen, zie figuur 4.8.

Figuur 4.8: Eigen snelheids- en posi eregelaar in MATLAB/Simulink

In het simulink model zijn drie delen te onderscheiden door een verschillende kleur. Het linkse deel vormt de posi eregelkring, deze vergelijkt de werkelijke posi e met de gewenste posi e en brengt de fout aan de ingang van de posi eregelaar. Deze posi eregelaar is een P-regelaar (zie 4.1.2) en versterkt de fout naar een gewenste snelheid. Om de uitgestuurde snelheid te beperken, wordt een snelheidslimiet ingesteld. Het tweede deel vormt de snelheidsregelkring, deze vergelijkt de werkelijke snelheid met de wenssnelheid en brengt de fout aan de ingang van de snelheidsregelaar. Deze snelheidsregelaar is een PI-regelaar (zie 4.1.2) en bepaald het gewenste koppel. Op de figuur is een PID-regelaar te zien, de D-ac e (differen ërende ac e) wordt echter uitgeschakeld. Om ook een mogelijk doorschot van de integrerende ac e tegen te gaan, wordt an -windup toegepast. Deze beperkt het uitgestuurde koppel tot het maximale koppel die de aangewende motor kan leveren. Hiernaast wordt een eerste orde filter op de werkelijke snelheid geplaatst, hierdoor wordt hoogfrequente ruis uit het systeem gehouden. Om binnen de regelaars met standaard eenheden te werken, worden op de ingangen en uitgangen van het model een conversie toegepast. Het derde deel staat bijvoorbeeld in voor de conversie van het koppel in Newton meter - naar het percentage van het piekkoppel van de aangewende motor. Verder is er telkens een overgang tussen de gebruikte datatypes.

18

Instelling snelheidsregelaar Via een systeemiden fica e kan de bode-karakteris ek van de koppelresponsie van de assen opgemeten worden. Aan de hand van deze karakteris ek worden de snelheidsregelaars ingesteld, in dit onderdeel wordt toegelicht hoe dit gebeurd2 . Ten eerste moet de regelaar stabiel zijn, dit wordt aangegeven door het bode-stabiliteitscriterium: „De geslotenlus is stabiel als de versterking in de openlus frequen eresponsie kleiner is dan 1 of 0dB, daar waar de fase-naijling ±180° bedraagt.” Stel dat een signaal met een fase-naijling van exact -180° uit het systeem komt. Dan zal dit signaal, door de nega eve terugkoppeling, opgeteld worden bij het verschil tussen de wenswaarde en werkelijke waarde, en terug in het systeem gestuurd worden. Indien dit signaal door het systeem verzwakt wordt, zal de amplitude dalen tot het signaal uiteindelijk uit het systeem verdwijnt. Het omgekeerde gebeurt wanneer het signaal door het systeem versterkt wordt. Hierdoor zal de amplitude van het signaal blijven s jgen, waardoor het signaal dominant wordt. Dit leidt tot instabiliteit, voor een signaal met een fase-naijling van exact +180° kan de instabiliteit op analoge wijze aangetoond worden. In feite bezi en alle uitgangssignalen met een fase-naijling vanaf ±90°, een component die door de negaeve terugkoppeling instabiliteit kan veroorzaken. Dit wordt ook wel de rela eve stabiliteit van het systeem genoemd. Om deze reden moet de fase-naijling voldoende boven -180° liggen bij een versterking van 0dB. Hierin overdrijven is ook niet goed, het systeem wordt dan overstabiel en zeer traag. Er moet dan ook een compromis gemaakt worden tussen de stabiliteit (of robuustheid) en de snelheid (of bandbreedte) van het systeem. In het jargon wordt echter niet gesproken van de fase-naijling, maar van de fasemarge ϕP M (PM = phase margin). Dit is de marge tussen de fase van het systeem bij een versterking van 0dB en de -180° instabiliteitsgrens. De pulsa e van dit punt wordt de snijpulsa e ωP M genoemd, zie figuur 4.9.

M dB

ωPM 0dB

φ

log ω

log ω φPM

-180°

Figuur 4.9: De fasemarge ϕP M en snijpulsa e ωP M

De fasemarge vormt dan ook de geschikte parameter om verschillende instellingen van een regelaar te vergelijken. Via trial-and-error kunnen verschillende fasemarges getest en vergeleken worden tot het gewenste resultaat bekomen wordt. 2 Cursus regeltechniek [6]

19

Wanneer de lus met een regelaar gesloten wordt moet het effect op de bode-karakteris ek gekend zijn. De snelheidsregelaar is een PI-regelaar, zie (4.1) voor de transferfunc e van deze regelaar. ( ) 1 Ki · 1 + Ti s

(4.1)

Deze kan herschreven worden als (4.2). Ki · Ti

(

Ti s + 1 s

) (4.2)

Hierin kunnen de integrator en het nulpunt onderscheiden worden. Het nadeel van de I-ac e is een faseverschuiving van -90° waardoor de kri sche hoek van -180° sneller bereikt wordt. Het nulpunt kan nu gebruikt worden 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 die 90° te werken, omdat deze zeer hoge frequen es sowieso niet door het systeem kunnen. Een fase voorsprong van 60° is wel realis sch. 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 vooropgesteld wordt dan kan de pulsa e ωP M bepaald worden uit (4.3).

̸

G(jω)|ω=ωP M − 90° + 60° = −180° + ϕP M

(4.3)

Aan de hand van de bode-karakteris ek van de koppelresponsie, kan ωP M dan afgelezen worden. Bij de pulsa e ωP M moet het nulpunt de fase dus 60° omhoog trekken. Zo kan Ti bepaald worden uit (4.4).

̸

(1 + Ti jω)|ω=ωP M = 60°

(4.4)

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 0dB lijn moet snijden. Op deze manier kan de versterking of verzwakking Kp bepaald worden, zie (4.5).

20log|Ki |ω=ωP M

T jω + 1 i = 20log Ti jω

− 20log|G(jω)|ω=ωP M

(4.5)

ω=ωP M

Samengevat moeten volgende stappen doorlopen worden om de snelheidsregelaar in te stellen: (a) Kies een gewenste fasemarge uit. (b) Bereken ̸ G(jω)|ω=ωP M uit (4.3). (c) Lees ωP M af op de bode-karakteris ek. (d) Bereken Ti uit (4.4). (e) Bereken Ki uit (4.5). Om de snelheidsregelaar uit te testen wordt een stap aan de wenssnelheid aangelegd. Als het systeem instabiel reageert moet een grotere fasemarge gekozen worden, indien het systeem traag reageert kan een kleinere fasemarge gekozen worden. De bovenstaande stappen worden dan opnieuw doorlopen tot de snelheidsresponsie het gewenste gedrag vertoont.

20

Instelling posi eregelaar Als posi eregelaar wordt een P-regelaar gebruikt, deze wordt enkel afgeregeld via de stapresponsie. Hierbij wordt een stap aan de ingang van de posi eregeling aangelegd en wordt de responsie van de werkelijke posi e gemonitord. Zie figuur 4.10 voor een voorbeeld.

Gewenste stapresponsie van de positie 1

Stap

Positie (rad)

0.8

Responsie

0.6

0.4

0.2

0

0

1

2

3

4

5

6

7

8

9

10

Tijd (s) Figuur 4.10: De stapresponsie van een eerste orde systeem

Er wordt met een versterking Kp = 1 begonnen. Deze wordt stelselma g opgedreven tot er een instabiliteit of doorschot waargenomen wordt. Doorschot is namelijk niet toelaatbaar binnen een posi eregeling.

4.4.2

Afregelen van A6

Nu alle nodige aspecten van de posi eregeling en technieken om de regelaars in te stellen gekend zijn, kunnen de assen afgeregeld worden. De laatste as van de robot wordt als eerste afgeregeld. Om te beginnen wordt een systeemiden fica e (zie 4.3) uitgevoerd, zie figuur 4.11 voor het resultaat. Aan de hand van de bode-karakteris ek kan de snelheidsregelaar ingesteld worden. Er wordt een fasemarge van 70° gekozen, hieruit kan ̸ G(jω)|ω=ωP M gehaald worden, zie (4.6). ̸

G(jω)|ω=ωP M = −180° + 70° + 90° − 60° = −80°

(4.6)

Vervolgens kan ωP M afgelezen worden uit de bode-karakteris ek, zie figuur 4.11. Hieruit kan Ti berekent worden, zie (4.7). Om Ki te bepalen moeten de versterking van het proces en de I-ac e bekend zijn. De versterking van het proces wordt afgelezen uit de bode-karakteris ek en bedraagt 20dB. De versterking van de I-ac e wordt bepaald met behulp van MATLAB, deze bedraagt 1,2dB. Door (4.5) om te vormen kan Ki ook berekent worden, zie (4.8). Ti = ωP M · tan(60°) = 0, 0133s

(4.7)

) = 0.087 N m rad/s

(4.8)

Ki = 10−(

20+1,2 20

21

Bode-karakterisiek koppelresponsie A6 Amplitude (dB)

80 60

ωPM= 130 rad/s

40

20 dB

Piek

20

0 dB

0 -20 -40 -2 10

-1

0

10

10

2

1

10

10

Pulsaie (rad/s)

3

10

4

5

10

10

200 150

Fase (°)

100 50 0 -50

Φ = -80°

-100

-180°

-150 -200 -2 10

-1

0

10

10

2

1

10

10

Pulsaie (rad/s)

3

10

4

5

10

10

Figuur 4.11: Bode-karakteris ek koppelresponsie A6

Naast de regelaar wordt ook een eerste orde filter ingezet. Het nut van deze eerste orde filter is tweeledig. Ten eerste om de hoogfrequente ruis uit de snelheidsresponsie te filteren, en ten tweede om de resonan epiek (zie figuur 4.11) te dempen. Om deze redenen wordt de filter geplaatst op 360 rad/s, zodat de resonan epiek in het afgeregelde systeem net onder 0dB ligt. Via MATLAB kan nu de totale openlus bode-karakteris ek van de koppelresponsie met regelaar en filter opgesteld worden, zie figuur 4.12.

Bode-karakterisiek koppelresponsie A6 met filter en regelaar Amplitude (dB)

80 60 40

ωPiek = 28 rad/s

20

piek

en

ωPM= 130 rad/s

0 dB

0 -20 -40 -60 -80 -2 10

-1

10

0

10

1

10

2

10

Pulsaie (rad/s)

3

10

4

5

10

10

150 100

Fase (°)

50 0 -50

-100 -150 -200

Φ= -130°

ΦPM = 50°

-250 -300 -2 10

-1

10

-180°

0

10

1

10

2

10

Pulsaie (rad/s)

3

10

4

10

5

10

Figuur 4.12: Bode-karakteris ek koppelresponsie met filter en regelaar A6

22

Het is meteen duidelijk dat de filter de fase 20° naar beneden hee getrokken, zodat de fasemarge van het geheel nog 50° is. Langs de andere kant zorgt de filter voor een voldoende demping van de piek, zodat deze onder het 0dB punt ligt. Om dit resultaat te toetsen aan de prak jk worden de regelaar en filter uit figuur 4.1 ingesteld met de bekomen waardes. De posi eregelkring wordt aan de kant geschoven en maakt plaats voor een stapingang, zie figuur 4.13 voor het gebruikte Simulink model.

Figuur 4.13: Simulink object van de snelheidsregelkring en stapingang

Met behulp van dit object kan de performan e van de snelheidsregelkring nagegaan worden. Aan de wensnelheid wordt een stap aangelegd en de snelheidsresponsie wordt opgemeten, zie figuur 4.14.

Stapresponsie snelheid A6 14

Werkelijke snelheid

12

Snelheid (rad/s)

10 8

250 ms

200 ms 6 4

Snelheids commando

2 0 -2 -4 0

0.5

1

1.5

2

2.5

3

3.5

4

Tijd (s) Figuur 4.14: Stapresponsie van de afgeregelde snelheidsregelaar

Uit deze stapresponsie kunnen twee conclusies getrokken worden. Ten eerste wordt de wenssnelheid binnen de 200ms bereikt en treedt er 30% doorschot op, wat neerkomt op een gewenst gedrag. Ten tweede blij een signaal met een periode van ±250ms in het systeem aanwezig, wat neerkomt op een pulsa e van 25rad/s. Wanneer deze pulsa e nader bekeken wordt in de bode-karakteris ek (zie figuur 4.12), zijn kleine pieken waar te nemen. Hierdoor blijven deze signalen onder de pieken steken en raken ze niet uit het systeem. Aangezien dit voor deze as niet meteen voor problemen zorgt, kan dit in een later stadium herzien worden. Er wordt dan ook met deze instelling verdergegaan naar de posi eregelaar.

23

Om de posi eregelaar in te stellen wordt het volledige Simulink object, weergegeven in figuur 4.1, gebruikt. Er wordt vertrokken van een versterking van 0dB, deze wordt opgetrokken tot er net geen doorschot optreedt. Op deze manier wordt een versterkingsfactor Kp van 30 rad/s rad bekomen. Zie figuur 4.15 voor de resulterende stapresponsie, een stap naar 5rad wordt in slechts 200ms bereikt.

Stapresponsie posiie A6 6

5

Posiie commando

Posiie (rad)

4

Ts = 200ms

3

2

Werkelijke posiie

1

0

-1 0

0.05

0.2

0.15

0.1

0.25

Tijd (s) Figuur 4.15: De uiteindelijke stapresponsie van de posi e

Nu de posi eregeling volledig afgeregeld is kan de werking aangetoond worden door een sinusvormige wensposi e aan te leggen en de werkelijke posi e te monitoren, zie figuur 4.16 voor het resultaat.

Posiie commando en werkelijke posiie A6 400 300

Δt = 30ms

Posiie (rad)

200 100 0

-100

Posiie commando

Werkelijke posiie

-200 -300 -400 0

1

2

3

4

5

6

7

Tijd (s) Figuur 4.16: Gewenste posi e en werkelijke posi e van A6 na het afregelen.

De werkelijke posi e volgt de wensposi e op een gewenste manier, zo zijn beide profielen iden ek mits een jdsverschil van 30ms. Dit jdsverschil kan kleiner gemaakt worden via feed forward. Daarvoor is echter nood aan extra onderzoek. Aangezien de as zijn wensposi e op een snelle, soepele en efficiënte manier bereikt, kan besloten worden dat de posi eregeling performant afgeregeld is. Vervolgens kunnen de andere assen op analoge wijze afgeregeld worden.

24

4.4.3

Afregelen van A5

Het afregelen van deze as gebeurd analoog aan de afregeling van A6, zie (4.4.2). Om te beginnen wordt een systeemiden fica e uitgevoerd, zie figuur 4.17 voor het resultaat.

Bode-karakterisiek koppelresponsie A5

Amplitude (dB)

80 60 40 20

0 dB

0 -20 -40 -60 -2 10

-1

10

0

10

1

10

2

10

3

4

10

5

10

10

Pulsaie (rad/s) 200 150

Fase (°)

100 50 0 -50

-100

-180°

-150 -200 -2 10

-1

10

0

10

1

10

2

10

3

4

10

5

10

10

Pulsaie (rad/s) Figuur 4.17: Bode-karakteris ek koppelresponsie A5

Bij het afregelen werden enkele fasemarges getest, volgende regelparameters werden bekomen: ωP M = 150 rad s

Ti = 0, 01155s

Nm Ki = 0, 1202 rad/s

Kp = 30 rad/s rad

ωf ilter = 750 rad s

ϕP M = 50°

Zie figuur 4.18 voor de totale openlus bode-karakteris ek. 100

Bode-karakterisiek koppelresponsie A5 met filter en regelaar

Amplitude (dB)

80 60 40

ωPM= 150 rad/s

20

0 dB

0 -20 -40 -60 -80 -2 10

-1

10

0

10

150

1

10

2

10

Pulsaie (rad/s)

3

10

4

5

10

10

100

Fase (°)

50 0 -50

-100

-180°

-150

ΦPM = 50°

-200 -250 -300 -2 10

-1

10

0

10

1

10

2

10

Pulsaie (rad/s)

3

10

4

10

5

10

Figuur 4.18: Bode-karakteris ek koppelresponsie met filter en regelaar A5

Zowel de stapresponsie van de snelheid, als die van de posi e, kwamen tot een gewenst resultaat. Dit betekent dat de wensposi e zonder doorschot en op een snelle, soepele en efficiënte manier bereikt wordt.

25

4.4.4

Afregelen van A4

Ook het afregelen van deze as gebeurd analoog aan de afregeling van A6, zie (4.4.2). Om te beginnen wordt een systeemiden fica e uitgevoerd, zie figuur 4.19 voor het resultaat.

Bode-karakterisiek koppelresponsie A4 Amplitude (dB)

60 40 20

0 dB

0 -20 -40 -60 -2 10

-1

10

0

10

1

10

2

3

10

4

10

5

10

10

Pulsaie (rad/s)

200 150

Fase (°)

100 50 0 -50

-100

-180°

-150 -200 -2 10

-1

10

0

10

1

10

2

3

10

Pulsaie (rad/s)

4

10

5

10

10

Figuur 4.19: Bode-karakteris ek koppelresponsie A4

Bij het afregelen werden enkele fasemarges getest, volgende regelparameters werden bekomen: ωP M = 205 rad s

Ti = 0, 00841s

Nm Ki = 0, 1047 rad/s

Kp = 40 rad/s rad

ωf ilter = 1000 rad s

ϕP M = 50°

Zie figuur 4.20 voor de totale openlus bode-karakteris ek.

Bode-karakterisiek koppelresponsie A4 met filter en regelaar Amplitude (dB)

80 60 40

ωPM= 205 rad/s

20

0 dB

0 -20 -40 -60 -80 -2 10

-1

10

0

10

150

1

10

2

10

Pulsaie (rad/s)

3

10

4

5

10

10

100

Fase (°)

50 0 -50

-100

-180°

-150

ΦPM = 50°

-200 -250 -300 -2 10

-1

10

0

10

1

10

2

10

Pulsaie (rad/s)

3

10

4

10

5

10

Figuur 4.20: Bode-karakteris ek koppelresponsie met filter en regelaar A4

Zowel de stapresponsie van de snelheid, als die van de posi e, kwamen tot een gewenst resultaat. Dit betekent dat de wensposi e zonder doorschot en op een snelle, soepele en efficiënte manier bereikt wordt.

26

4.4.5

Afregelen de eerste drie assen

Het afregelen van de eerste drie assen gaat echter minder vlot. De grote verhouding tussen de iner e van de last en de iner e van de motor-as belemmert een eenvoudige afregeling, dit uit zich in uitgesproken resonan epieken. Zie figuur 4.21 voor de bode-karakteris ek van de koppelresponsie van A3.

Bode-karakterisiek koppelresponsie A3 Amplitude (dB)

50

ωPiek = 28 rad/s

Resonanie piek

Ani-resonanie piek

0 dB

0

-50 -2 10

-1

10

0

10

200

1

10

2

10

Pulsaie (rad/s)

3

10

4

5

10

10

150

Fase (°)

100 50 0

-50

-100

-180°

-150 -200 -2 10

-1

10

0

10

1

10

2

10

Pulsaie (rad/s)

3

10

4

10

5

10

Figuur 4.21: Bode-karakteris ek koppelresponsie A3

De bovenste resonan epiek kan naar beneden getrokken worden door een zuigfilter. Op deze manier kan de as in eerste instan e afgeregeld worden zoals de laatste drie assen. De resonan epieken zijn echter niet de enigste optredende effecten. Net zoals bij A6 blijven ook hier precies signalen steken in het systeem (ωP iek = ±28rad/s). Deze kunnen bij deze as echter niet verwaarloosd worden, zie figuur 4.21. Zo lijkt de as wel soepel te bewegen, maar is een trilling van de as duidelijk voelbaar. Om de eerste drie assen performant af te regelen, moeten de oorzaken van deze effecten eerst gekend en modelleerbaar zijn. Een eerste denkpiste is dat dit effect voortvloeit uit de resonan epieken. Deze ”mini-resonan es” zijn namelijk in figuur 4.22 zowel links, als rechts van de resonan epieken te zien. Een tweede denkpiste is dat dit effect een eigentrilling van de robotarm is. Op dit moment is er echter nog geen uitsluitsel. Deze effecten moeten hoe dan ook verder onderzocht worden zodat ze gecompenseerd kunnen worden. Dit behoort dan ook tot één van de topics voor de volgende masterproef.

27

Bode-karakterisiek koppelresponsie A1 Versterking (dB)

60

40

20

0

-20

-40 -2 10

-1

10

0

10

1

10

2

10

3

10

4

10

5

10

Pulsaie (rad/s) 200 150

Fase (°)

100 50 0 -50 -100 -150 -200 -2 10

-1

10

0

10

1

10

2

10

3

10

4

10

5

10

Pulsaie (rad/s) Figuur 4.22: Bode-karakteris ek koppelresponsie A1

4.5

Besluit

De mogelijkheid om op deze manier een posi eregeling te implementeren en af te regelen, toont zowel de openheid van de nieuwe controller als de kracht van de MATLAB/Simulink integra e in TwinCAT 3 aan. In het eerste luik kwam het concept van een posi eregeling aan bod, hierna werden de technieken om de regelaars in te stellen toegelicht. Op deze manier werden de eerste drie assen van de robot performant afgeregeld. Ook werd een eerste stap gezet in de afregeling van de laatste drie assen. Deze hebben echter nood aan extra onderzoek naar de optredende fysische fenomenen zodat deze op een gewenste manier gecompenseerd kunnen worden. Eens alle assen afgeregeld zijn, laten de kinema sche transforma es de robot toe, zich in de gewenste houding te brengen. Deze komen dan ook in het volgende hoofdstuk aan bod.

28

5

Kinema sche transforma es

Wanneer de assen van de robot afgeregeld zijn kunnen deze elk apart hun wensposi e bereiken. Om vervolgens de robot in een gewenste houding te brengen, moet de gewenste posi e van elke as voor deze houding bepaald worden. Bijkomend moet ook de werkelijke houding van de robot gekend zijn uit de werkelijke posi es van de assen. De kinema sche transforma es die hiervoor instaan komen in dit hoofdstuk aan bod. Zie bijlage B voor de volledige theore sche achtergrond, wiskundige uitwerking en implementa e in MATLAB®van deze kinema sche transforma es.

5.1

Pose coördinaten en joint coördinaten

Om een assenstelsel in de ruimte te definiëren, ten opzichte van een referen e assenstelsel, zijn zes variabelen nodig. Drie voor de onderlinge posi e van hun oorspongen en drie voor de onderlinge oriënta e van hun assen. Deze worden samengebracht tot de pose van een assenstelsel (posi e en oriënta e). Nu kunnen twee assenstelsels op de robot gedefiniëerd worden, zie figuur 5.1. Een eerste assenstelsel {bb} wordt gedefiniëerd op de basis van de robot en wordt ingezet als referen e assenstelsel. Een tweede assenstelsel {ee} wordt gedefiniëerd op de eindeffector van de robot. Merk op dat deze enkel worden gebruikt om de coördinatenstelsels toe te lichten en zijn verder niet meer van toepassing.

eeθ5

y {ee} zee

xee Eindeffector + +

θ4

-+ --

θ6

+

--

--

(X,Y,Z) θ2

θ1

+ --

--

θ3

+

zbb Basis r

y p

xbb (0,0,0) {bb}

ybb

Figuur 5.1: Defini e van het basis- en eindeffector assenstelsel.

De pose van {ee} ten opzichte van {bb} kan nu op twee manieren bepaald worden. Ofwel via de pose coördinaten (X, Y, Z, r, p, y) ofwel via de hoeken van de joint coördinaten (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ). Hierbij stellen r, p en y de hoeken van een samengestelde rota e rond de assen van {bb} voor. Beide coördinatenstelsels kunnen ingezet worden om de houding van de robot voor te stellen.

29

5.2 5.2.1

Voorwaartse kinema ca Defini e

De voorwaarste kinema ca berekent de pose van de eindeffector in func e van de robotassen (5.1). Deze transforma e staat dus in voor de berekening van de pose coördinaten in func e van de joint coördinaten en wordt uitgewerkt aan de hand van homogene transforma ematrices.

(X, Y, Z, r, p, y) = V K(θ1 , θ2 , θ3 , θ4 , θ5 , θ6 )

5.2.2

(5.1)

De homogene transforma ematrix

Zoals eerder vermeld zijn er zes variabelen nodig om een assenstelsel in de ruimte te definiëren ten opzichte van een referen e assenstelsel. Eén van de manieren om deze wiskundig voor te stellen is aan de hand van de homogene transforma ematrix1 . Deze 4 × 4 homogene matrix maakt het enerzijds mogelijk om de coördinaten van een punt p volgens het ene assenstelsel {b} te transformeren naar de coördinaten van dit punt volgens een andere assenstelsel {a}. Anderszijds maakt deze een verplaatsing van het ene assenstelsel {b} naar het andere assenstelsel {a} mogelijk. Beide interpreta es worden bij de voorwaarste kinema ca van de robot toegepast, zie figuur 5.2.

p

p

b

a

p a

{b}

pa,b

{a}

Figuur 5.2: {b} beweegt t.o.v. {a}. Punt p beweegt samen met {b}.

De transforma ematrix ba T bevat een rota ematrix ba R en een posi evector a pa,b , zie formule (5.2). ( b aT

=

b aR

a,b ap

01×3

1

) (5.2)

a,b a,b T De posi evector a pa,b = (a pa,b x a py a px ) bevat de posi e van de oorsprong van {b} ten opzichte van de oorspong van {a} volgens de coördinaten van {a}. In deze posi evector worden dus telkens de drie variabelen van de onderlinge posi e ingevuld. De rota ematrix ba R is een 3 × 3 orthogonale matrix en beschrij de eenheidsvectoren van {b} volgens de eenheidsvectoren van {a}. Aan de hand van deze matrix kunnen de assen van het assenstelsel rond zichzelf geroteerd worden. Hierbij worden maximaal de drie variabelen voor de onderlinge oriënta e meegegeven. Deze stellen de hoek voor waarmee rond de assen geroteerd wordt. Aangezien de invulling van deze matrix geen belang hee om de denkwijze te volgen, wordt hiervoor verwezen naar bijlage B. Vanaf nu worden enkel de assen waarrond er geroteerd wordt en de bijhorende rota ehoeken weergegeven. 1 Cursus toegepaste mechanica 3[7]

30

Indien de onderlinge posi e en oriënta e van {b} ten opzichte van {a} en de coördinaten van het punt p volgens {b} gekend zijn kan ba T bepaald worden. Hiermee kunnen de coördinaten van het punt p volgens {a} berekend worden, zie (5.3). (

)

ap

( =

1

b aT

bp

) (5.3)

1

De vorm van deze homogene transforma ematrix is helemaal niet willekeurig. Onderaan worden drie nullen en een één toegevoegd om de matrix ”homogeen” te maken. Dit zorgt er net voor dat transforma es zoals rota es en transla es als matrix opera es kunnen geïmplementeerd worden. Dit komt tot ui ng wanneer opeenvolgende transforma es gebeuren, zie figuur 5.3.

{b}

a

pa,b pb,c

b

{a}

a

pa,c {c}

Figuur 5.3: Samenstellen van verschillende poses

Om de transforma ematrix ca T van assenstelsel {a} naar assenstelsel {c} te bekomen volstaat het om één matrixvermenigvuldiging van de tussenliggende transforma ematrices ba T en cb T uit te voeren, zie (5.4) c aT

= ba T cb T

(5.4)

De onderlinge posi e en oriënta e van {c} ten opzichte van {a} kan dan uit ca T afgeleid worden, zie (5.5). ( c aT

=

c aR

a,c ap

01×3

1

) (5.5)

a,c a,c T Zo bevat a pa,c = (a pa,c x a py a px ) de posi e van {c} volgens {a} en kunnen de rota ehoeken om {a} volgens {c} te oriënteren uit ca R afgeleid worden.

Homogene transforma ematrices zijn dus in staat om verschillende assenstelsels aan elkaar te linken. Deze kunnen eenvoudig samengesteld worden tot de totale transforma ematrix door hun matrixproduct uit te rekenen. Hieruit kunnen dan de onderlinge posi e en oriënta e van het laatste assenstelsel ten opzichte van het referen e assenstelsel afgeleid worden. Deze matrix is dan ook geschikt om de voorwaartse kinema ca van de robot te bepalen.

31

5.2.3

Uitwerking

Om de voorwaartse kinema ca van de robot te bepalen wordt gebruik gemaakt van de homogene transforma ematrix, zie 5.2.2. De oplossing van dit probleem bestaat erin verschillende assenstelsels op de robot te definiëren, zie figuur 5.4. Deze assenstelsels worden vervolgens gelinkt aan elkaar via transforma ematrices die a ankelijk zijn van de assen van de robot. Door deze serieketen uit te rekenen wordt de totale transforma ematrix bekomen waaruit de pose coördinaten van de eindeffector afgeleid kunnen worden.

θ5

{e} θ4 e ye + z

+

--

x

g

y

f

xf

xd

g

y+ E --

f

z -- z F {f } {g} θ6 g

xe

D +

y

zd

d

{d} C xc

+ θ2 --

--

zb -- b

x

θ1 +

B

yb {b}

{c} c

y

θ3

c

z

A

za xa

{a}

ya

Figuur 5.4: De KUKA KR15/2 in nulposi e met de gedefiniëerde assenstelsels en afstanden

Bepalen van de totale transforma ematrix De totale transforma ematrix wordt bekomen uit het matrixproduct van de verschillende transforma ematrices die de assenstelsels linken met elkaar, zie (5.6).

g aT

= ba T cb T dc T ed T fe T gf T

(5.6)

Merk op dat de zin van de rota ehoeken bepaalt wordt door de rechterhand regel. Hiernaast worden ook de originele draairich ngen van de assen op de robot gebruikt, zie figuur 5.4.

32

b aT

wordt als volgt opgebouwd, zie figuur 5.5 en (5.7):

(a) Een rota e rond z a over de hoek −θ1 . (b) Een transla e volgens z a over de afstand A. De hoek θ1 , aangegeven in figuur 5.5, bevindt zich momenteel in de nulposi e. Door een rota e van de motor op A1 zal deze hoek veranderen met een varia e van de rota ematrix tot gevolg. Deze gedachtengang is geldig bij alle assen en verklaart de a ankelijkheid van de joint coördinaten op de pose coördinaten.

zb θA1 +1

-- b

x

zb

yb {b}

xc

-- b

B

{c}

A

y

z

c

c

xx θ2

A1 +

-π/2

yb {b}

z y

za xa

{a}

ya Figuur 5.6: Bepalen van cb T

Figuur 5.5: Bepalen van ba T

 b aT

  = 

0 R(Z, −θ1 ) 0 A 01×3 1

    

(5.7)

Hierbij stelt R(Z, −θ1 ) de rota ematrix voor van een rota e rond de Z-as met een hoek −θ1 (rechterhand regel). Deze gedachtengang geldt ook voor de volgende rota ematrices. c bT

wordt als volgt opgebouwd, zie figuur 5.6 en (5.8):

(a) Een rota e rond xb over de hoek − π2 . (b) Een rota e rond z over de hoek θ2 − π2 . (c) Een transla e volgens xb over de afstand B.  c bT

 R(XZ, − π , θ − π )  2 2 2 =  01×3

B 0 0 1

    

(5.8)

33

d cT

wordt als volgt opgebouwd, zie figuur 5.7 en (5.9):

(a) Een rota e rond z c over de hoek θ3 . (b) Een transla e volgens xc over de afstand C.

xxdd

θ3

dd

E yy--

zdzd {d}

--

C

z

xc

-- b

x

{c} c

y

x

b

f

g

y

θ4

xf

{e} ye

xe

ze

xd D

g

y θ6

z z F {g} {f } f

g

θ5 E

yd

zd {d}

Figuur 5.8: Bepalen van ed T, fe T en gf T

c

z

Figuur 5.7: Bepalen van dc T

 d cT

e dT

  = 

C R(Z, θ3 ) 0 0 01×3 1

    

(5.9)

wordt als volgt opgebouwd, zie figuur 5.8 en (5.10):

(a) Een rota e rond xd over de hoek − π2 . (b) Een transla e volgens xd over de afstand D.  e dT

 R(X, − π )  2 =  01×3

D 0 0 1

    

(5.10)

34

f eT

wordt als volgt opgebouwd, zie figuren 5.9 en 5.8 en formule (5.11):

(a) Een rota e rond z e over de hoek −θ4 . (b) Een rota e rond y over de hoek −θ5 . (c) Een rota e rond z over de hoek −θ6 . (d) Een transla e volgens z e over de afstand E. Alhoewel assen 4, 5 en 6 zich fysisch op een verschillende loca e bevinden snijden hun rota e-assen elkaar in het centerpunt van de robotpols. Hierdoor kunnen hun rota es wiskundig voorgesteld worden door één rota ematrix gelegen in dit snijpunt, welke de oorsprong van {e} is. Om deze reden is fe T a ankelijk van de hoeken θ4 , θ5 en θ6 en bevat gf T enkel een transla e.

xe

x

y

x

ye yf

y y θ5 θ z 4

{f } E

zf

{e} θ6

ze z z

x

xf Figuur 5.9: Bepalen van fe T

 f eT

 R(ZY Z, −θ , −θ , −θ )  4 5 6 =  01×3

g fT

0 0 E 1

    

(5.11)

wordt als volgt opgebouwd, zie figuur 5.8 en (5.12):

(a) Een transla e volgens z f over de afstand F .  g fT

  = 

13 01×3

0 0 F 1

    

(5.12)

Daar alle transforma ematrices gekend zijn, kan ga T berekend worden uit (5.6).

35

Posi e en oriënta e van de eindeffector De gewenste pose coördinaten (X, Y, Z, r, p, y) kunnen vervolgens uit ga T afgeleid worden, zie (5.13). ( g aT

a,g ap

=

g aR

a,g ap

01×3

1

) (5.13)

bevat de posi e van de eindeffector {g} ten opzichte van {a} (5.14).

a,g ap

= (ga px ga py ga pz )T = (X Y Z)T

(5.14)

De rota ehoeken om {a} te oriënteren volgens {g} kunnen uit ga R afgeleid worden. Merk op dat de atan2 func e gebruikt wordt, deze kan de hoeken berekenen in alle vier kwadranten.  g aR

r

g a R12 g a R22 g a R32

g a R13 g a R23 g a R33

  

= atan2(−ga R23 , ga R33 )

p = y

 =

g a R11 g a R21 g a R31

atan2(ga R13 , cy ga R33

−

(5.15)

(5.16) sy ga R23 )

= atan2(−ga R12 , ga R11 )

(5.17) (5.18)

Met sy = sin(y) en cy = cos(y) Volgens deze methode werd de voorwaarste kinema ca van de robot geïmplementeerd in MATLAB®/Simulink, zie B.2.3 voor de MATLAB®-code. Deze kan in real- me uitgevoerd worden in TwinCAT®3.

5.3 5.3.1

Inverse kinema ca Defini e

De inverse kinema ca berekent de hoeken van de robotassen uit de pose van de robot (5.19).

(θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = IK(X, Y, Z, r, p, y)

(5.19)

Deze transforma e staat in voor de berekening van de joint coördinaten in func e van de pose coördinaten. Voor dit wiskundig probleem bestaan verschillende oplossingen. In deze uiteenze ng wordt gekozen voor de gesloten vorm. Dit wil zeggen dat de berekeningen gebeuren aan de hand van formules die rechtstreeks tot het resultaat leiden en niet op een itera eve wijze. Eerst wordt de methodiek van de berekeningen toegelicht, daarna volgt hun uitwerking. Merk op dat gedefiniëerde assenstelsels uit figuur 5.4 ook bij deze berekeningen van kracht zijn.

36

Methode De KUKA KR15/2 robot is een standaard zes-assige robot. Bij deze robots wordt de posi e van de robotpols enkel bepaald door de eerste drie assen. De laatste drie assen zorgen voor de gewenste oriënta e van de eindeffector. De berekening kan dus opgesplitst worden in volgende deelproblemen: (a) Berekenen van de transforma ematrix van de eindeffector ten opzichte van de basis ga T. (b) Berekenen van de posi e van de robotpols a p via ga T, zie figuur 5.10. (c) Berekenen van θ1 , θ2 en θ3 uit de posi e van de robotpols. (d) Berekenen van de oriënta e van de robot ea R door θ1 , θ2 en θ3 . (e) Berekenen van de oriënta e van de eindeffector ten opzichte van de oriënta e van de robot fe R. (f) Berekenen van θ4 , θ5 en θ6 uit fe R.

5.3.2

Uitwerking

De transforma ematrix ga T Uit de gegeven pose coördinaten kan de transforma ematrix van de eindeffector bepaald worden. X , Y en Z bepalen a pa,g en r , p en y bepalen ga R. Zie figuur 5.10.

(Xpols, Ypols, Zpols)

A5 + --

xg

+

A4

yg {g} gp +

zg

--

+

--

F

A6

a

p

--

A3

(X,Y,Z) + A2 --

A1 +

-a

pa,g

za r xa

y {a} (0,0,0)

p ya

Figuur 5.10: Berekening posi e van de robotpols

Zodat de posematrix ga T als (5.20) kan geschreven worden. Zie B.1.1 voor de RPY rota e. ( g aT

=

g aR

a,g ap

01×3

1

)

   = 

X R(RP Y, r, p, y) Y Z 01×3 1

    

(5.20)

37

De posi e van de robotpols Door langs z g de afstand F terug te keren, wordt het centerpunt van de robotpols bereikt, zie figuur ??. Uit (5.3) volgt (5.21). Waardoor de posi e van de robotpols gekend is. (

ap

)

(

gp

= ga T

1



)

   

of

1



Xpols Ypols Zpols 1



   g   = aT   

0 0 −F 1

    

(5.21)

Vervolgens kunnen de eerste drie hoeken van de robot bepaald worden. Berekening hoeken θ1 , θ2 en θ3 Aan de hand van goniometrische berekeningen kunnen de eerste drie hoeken uit de posi e van de robotpols afgeleid worden. Zie onderstaande procedure en figuren 5.11 en 5.12 als verduidelijking.

Y

Exy Dxy

Xpols {e}

Cxy

Fxy

{f }

{g} Ypols

{d} {c}

B

θ1 X

{a} {b} Figuur 5.11: Bovenaanzicht robotarm, berekening θ1

Procedure tot θ1 , θ2 en θ3 uit Xpols , Ypols en Zpols : Berekenen van θ1 (Fig. 5.11): θ1 = atan2(−Ypols , Xpols )

(5.22)

Berekenen van L1, L2, xy, z en r (Fig. 5.12): L1 = L2 = xy =

C √ √

(5.23) D2

+

E2

X2pols + Y2pols − B

= Zpols − A √ r = xy2 + z2

z

(5.24) (5.25) (5.26) (5.27)

38

Berekenen van α, β en γ: r2 − L12 − L22 2(L1 L2 ) sin(ϵ) = atan L1 L2 + cos(ϵ)

ϵ

(5.28)

=

α

(5.29)

= ϵ−α ( ) π D − atan = 2 E

β γ

(5.30) (5.31)

Het teken van xy bepaalt drie gevallen voor δ en θ2 : (a) xy > 0: ( δ

= atan

θ2

=

z xy

) (5.32)

π −α−δ 2

(5.33)

(b) xy = 0: δ θ2

π 2 = −α

(5.34)

=

(5.35)

(c) xy < 0: δ

=

θ2

=

atan

( xy )

z −(α + δ)

+

π 2

(5.36) (5.37)

θ3 wordt bepaald door de hulphoeken: θ3 = α + β − γ

(5.38) θ3

Z

{e}

{d}

D

γ

E π -(α+β)

C

L2 L1 β

θ2 r

α

B

{c}

{f }

δ

z

Fxyz {g}

xy

{b}

A

XY

{a}

Figuur 5.12: Zijaanzicht robotarm, berekening θ2 en θ3

39

De oriënta e van de robotpols ea R door θ1 , θ2 en θ3 Door de rota ematrices van θ1 , θ2 en θ3 samen te stellen kan de oriënta e van het centerpunt van de robotpols bepaald worden. Hieruit volgt ea R, zie (5.39). 

c1 s23  e a R =  −s1 s23 c23

−s1 −c1 0

 c1 c23  −s1 c23  −s23

(5.39)

Met s1 = sin(θ1 ), c1 = cos(θ1 ), s23 = sin(θ2 + θ3 ) en c23 = cos(θ2 + θ3 )

De rota ematrix fe R Zowel de gewenste oriënta e van de eindeffector als de oriënta e van het centerpunt van de robotpols zijn gekend. Hieruit kan het verschil in oriënta e tussen beide berekend worden. Deze wordt weergegeven door de rota ematrix fe R, zie (5.40).

f eR

= ea RT fa R

(5.40)

Berekening hoeken θ4 , θ5 en θ6 θ4 , θ5 en θ6 vormen samen een ZYZ rota e, zie B.1.1 voor meer informa e. De rota ematrix van deze samengestelde rota e moet gelijk zijn aan de rota ematrix fe R. Door deze gelijkheid kunnen de laatste drie hoeken berekend worden. Indien θ5 echter gelijk is aan nul kan enkel de som van θ4 en θ6 bepaald worden. De waarde van fe R33 bepaalt het geval (de indices rechtsonder staan voor de rij en de kolom). Geval 1: fe R33 ̸= 1 ⇒ θ5 ̸= 0 θ4 θ5 θ6

= atan2(fe R23 , −fe R13 ) −

c4 fe R13 , fe R33 )

(5.41)

=

atan2(s4 fe R23

=

atan2(−fe R32 , fe R31 )

(5.43)

θ5

= 0

(5.44)

= atan2(fe R12 , fe R11 )

(5.45)

(5.42)

Geval 2: fe R33 = 1 ⇒ θ5 = 0

θ4 + θ6

Met s4 = sin(θ4 ) en c4 = cos(θ4 ) Volgens deze methode werd de inverse kinema ca van de robot geïmplementeerd in MATLAB®/Simulink, zie B.2.3 voor de MATLAB®-code. Deze kan in real- me uitgevoerd worden in TwinCAT®3.

40

5.4

Besluit

In dit deel werd de kinema ca van de robot bepaald. Hiermee kan de robot steeds de gewenste houding aannemen en is de pose van de robot steeds bekend. Deze transforma es komen pas echt tot hun recht wanneer deze gekoppeld worden aan een interpola e generator. Zodat een wenspad op een vloeiende manier opgevolgd kan worden. Hiernaast moet ook opgemerkt worden dat er verschillende houdingen van de robot mogelijk zijn om tot dezelfde pose te komen. Hierdoor zijn verschillende oplossingen bij de inverse kinema ca mogelijk. In deze oplossing werd de keuze gemaakt om de arm al jd omhoog en in de rich ng van de pose te richten. Hiernaast zal de eindeffector zich steeds met de kleinste ne o hoek ten opzichte van de bovenarm opstellen.

41

6

Conclusies

6.1

Bereikte resultaten

Tijdens deze masterproef werd de verouderde sturing van een 6-assige KUKA robot vervangen door een open controller. Hierbij kunnen alle aspecten van een complete robotsturing zelf ingevuld worden. Alle nodige data werd verworven, hetzij door opzoeking in datasheets, me ngen of via trial-and-error. Vooral het verzamelen van de parameters van een onbekende motor vormde hierbij een uitdaging. Uiteindelijk konden alle parametersets van de motoren een invulling krijgen waardoor de controller volledig compa bel is met de motoren en hun feedback. Vervolgens werd de nodige hardware geselecteerd uit het productgamma van Beckhoff. Elke motor werd voorzien van een eigen servo kanaal dat over EtherCAT aangestuurd wordt door een krach ge industriële PC. Hierdoor zijn een zeer lage cyclus jden - zoals de gebruikte cyclus jd van 250µs - mogelijk. Na de opbouw en configura e van de opstelling werd een eerste stap gezet in de posi eregeling van de assen. Hierdoor zijn de laatste drie assen reeds performant afgeregeld. Om dit resultaat bij de eerste drie assen te bereiken, is er extra onderzoek nodig naar de optredende fysische effecten. Teneinde deze effecten op een eenvoudige manier te kunnen compenseren. Eens alle assen afgeregeld zijn kunnen de kinema sche transforma es ingezet worden. Zowel de voorwaartse kinema ca als de inverse kinema ca van de robot werden bepaald. Het feit dat deze stappen mogelijk waren toont zowel de openheid van deze controller als de kracht van de MATLAB®/Simulink integra e in TwinCAT®3 aan.

6.2

Sugges es voor verder onderzoek

Het mag duidelijk zijn dat een project van deze omvang in een reeks van masterproefonderwerpen aanwezig zal zijn. Dit basisproject kan dan ook op vele gebieden verder uitgebouwd worden. Ten eerste moeten de eerste drie assen performant afgeregeld worden. Daarna kunnen de wensposi es gekoppeld worden met de kinema sche transforma es en een interpola e generator. Zodat een wenspad op een vloeiende manier opgevolgd kan worden. Hierna kan gekozen worden om de assen dynamisch aan te sturen. Momenteel worden de assen namelijk bijgestuurd door de volgfout, zodat er jdens een beweging al jd een fout aanwezig is. Hiervoor moet een dynamisch model van de robot opgebouwd worden en moeten de nodige parameters opgemeten worden. Dit is mogelijk via de systeemiden fica e. Vervolgens kan een applica e opgebouwd worden die het mogelijk maakt om taken voor de robot te definiëren, ... Het mag duidelijk zijn dat de grenzen van deze opstelling nog lang niet bereikt zijn.

42

Literatuurlijst [1] Beckhoff, ``Website beckhoff.'' Online. [datum van opzoeking: Academiejaar 2012-2013]. [2] Beckhoff, ``Beckhoff informa on system.'' Online. [datum van opzoeking: Academiejaar 2012-2013]. [3] Wikipedia, ``Bemonsteringstheorema nyquist-shannon.'' Online, 2013. 03/05/2013].

[datum van opzoeking:

[4] H. Capoen, ``Real- me ethernet.'' Cursustekst, Academiejaar 2012-2013. [5] S. Derammelaere and B. Vervisch, ``Systeemiden fica e.'' Handouts, November 2012. [6] S. Derammelaere and I. Sweertvaegher, ``Regeltechniek.'' Cursustekst, Academiejaar 2012-2013. [7] J. D. Schu er and D. Vandepi e, ``Toegepaste mechanica 3.'' Cursustekst, 2008. [Versie: 1.4]. [8] KUKA, ``Spez kr 6, 15 de/en/fr.'' Datasheet, 1996.

43

A

De KUKA KR15/2

In deze bijlage worden de mechanische en elektrische gegevens van de KUKA KR15/2 gebundeld1 .

A.1

Mechanische eigenschappen

Volgende mechanische eigenschappen werden opgenomen in deze bijlage: • Afme ngen van de robotlinks, zie figuren A.2 en A.3. • Mechanisch bereik van de robotassen, zie tabel A.1. • Maximale hoeksnelheid van de robotassen, zie tabel A.1. • Overbrengingsverhoudingen tussen de motoren en robotassen

+

nmotor nas ,

zie tabel A.1.

+ A4

A5 -+

--

+

---

A6

A3

+ A2 --

--

A1 +

Figuur A.1: De KUKA KR15/2 industriële robot

Tabel A.1: Mechanische gegevens KUKA KR15/2

As A1 A2 A3 A4 A5 A6

Bereik van de as ( ◦ ) -185 tot 185 -55 tot 115 -210 tot 70 -350 tot 350 -130 tot 130 -350 tot 350

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

1 Datasheet KUKA [8]

44

600

140

120

155

Supplementaire last (max .10kg) +70˚

150 Zwaartepunt last (max . 15kg)

1945

650

---210˚

+115˚

675

1325

300 219

1480

---55˚

852

892 1570

R 145

Figuur A.2: Zijaanzicht KUKA KR15/2

--- 185˚

R 1570

+185˚

Figuur A.3: Bovenaanzicht KUKA KR15/2

45

A.2

Elektrische eigenschappen

De motoren op de robot zijn permanent magneet motoren, zie figuur A.4. Enkele gegevens van deze motoren worden weergegeven in tabel A.2. Extra informa e van de motoren kan in de parametersets van de motoren terug gevonden worden. Dit is zowel data van de motoren als van de feedback.

Motor 6

Motor 5

Motor 4 Motor 3 Motor 1

Motor 2

Figuur A.4: Motoren op de KUKA KR15/2

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

Motortype 1FK6081-6AF71 1FK6081-6AF71 KK4EY-YYY-033 1FK6032-6AF71 1FK6032-6AF71 1FK6032-6AF71

46

B

Kinema ca

In deze bijlage komen de kinema sche aspecten van de robot opgesomd. Eerst worden de toegepaste matrixmethodes voor 3D kinema ca toegelicht. Daarna komt de kinema ca van de robot aan bod.

B.1

Matrixmethodes voor 3D kinema ca

De posi e en oriënta e van voorwerpen worden wiskundig voorgesteld door matrices. Ten eerste komt de oriënta e van een voorwerp aan bod, deze wordt voorgesteld door de rota ematrix. Ten tweede komt de homogene transforma ematrix aan bod, deze beschrij zowel de posi e als oriënta e van een voorwerp. Deze uiteenze ng is gebaseerd op de cursustekst van het vak Toegepaste Mechanica 3 aan de KU Leuven1 .

B.1.1

De rota ematrix

De rota ematrix gee de wiskundige beschrijving van de oriënta e van een assenstelsel weer. Deze oriënta e is rela ef ten opzichte van een ander assenstelsel. In dit deel worden de defini e, eigenschappen en enkele standaard rota ematrices weergegeven. Defini e Er bestaan verschillende voorstellingen voor de rela eve oriënta e van een assenstelsel {b} ten opzichte van een assenstelsel {a}. In deze masterproef wordt de rota ematrix ba R toegepast, zie (B.1) en figuur B.1. 

xb · xa  b b b b b a a R = (Rij ) = (a x a y a z ) =  x · y xb · z a

  z b · xa R11 b a = z · y   R21 zb · za R31

y b · xa yb · ya yb · za

R12 R22 R32

 R13  R23  R33

(B.1)

De kolommen van ba R beva en de componenten van de eenheidsvectoren a xb , a yb en a zb van assenstelsel {b}, uitgedrukt volgens assenstelsel {a}. a xb is de nota e voor de driedelige vector met de coördinaten van het eindpunt van de eenheidsvector xb in assenstelsel {a}, gevormd door de drie eenheidsvectoren xa , y a en z a . Deze matrix kan dus een assenstelsel beschrijven vanuit een referen e assenstelsel.

za

zb R33

x xa

R21

R13 R23

b

R31

{a} R11

{b} R

R22 R32

12

yb

ya

Figuur B.1: Componenten Rij van een rota ematrix ba R 1 Cursus Toegepaste Mechanica 3

[7]

47

Ac eve en passieve interpreta e De rota ematrix kan op twee verschillende manieren geïnterpreteerd worden: (a) Passieve interpreta e: De rota ematrix wordt ingezet om de coördinaten van eenzelfde ruimtelijk punt of vector, volgens het ene assenstelsel, te transformeren naar coördinaten volgens het andere assenstelsel. R stelt een oriënta e voor. (b) Ac eve interpreta e: De rota ematrix stelt een beweging van een assenstelsel voor. Een ruimtelijk punt of vector van een assenstelsel wordt bewogen naar een ander ruimtelijk punt of vector. De transforma e beweegt de eenheidsvector xa , gelegen op de X-as van {a}, naar de eenheidsvector xb , gelegen op de X-as van {b}: R(xa ) = xb . Hetzelfde geldt voor de eenheidsvectoren op de Y - en Z-as. R stelt een rota e voor. Beide interpreta es worden door dezelfde matrix ba R voorgesteld. (a xb a yb a zb ) = R(a xa a ya a za ) = R = ba R

(B.2)

Rota es rond assen Rota es rond assen van een assenstelsel kunnen eenvoudig beschreven worden. Stel R(Z, α) de rota ematrix van een rota e rond de z a -as met een hoek α. Volgens de ac eve interpreta e worden vectoren xa en y a geroteerd naar xb en y b . Bij de passieve interpreta e worden de coördinaten van het punt p = ( xap ypa xap )T volgens {a} getransformeerd naar de coördinaten p = ( xbp ypb xbp )T volgens {b}. Beide worden voorgesteld door ba R, zie Fig.B.2.

za zb (0,0,1)

p {b} xa

{a} α xb

(cos(α),sin(α),0)

yb α ya (-sin(α),cos(α),0)

Figuur B.2: Rota e rond z a over een hoek α

Uit figuur B.2 kan rota ematrix (B.3) afgeleid worden. De kolommen beva en de componenten van de nieuwe eenheidsvectoren xb , y b en z b , volgens de eenheidsvectoren van het referen e assenstelsel xa , y a en z a . 

cos(α)  R(Z, α) =  sin(α) 0

 −sin(α) 0  cos(α) 0  0 1

(B.3)

Opmerking: De zin van rota ehoeken wordt telkens bepaald met behulp van de rechterhandregel.

48

Ac eve interpreta e: Rota e, zie B.4.   cos(α) xb  b    y  =  sin(α) zb 0 

   −sin(α) 0 xa    cos(α) 0  ·  y a  0 1 za

(B.4)

Passieve interpreta e: Oriënta e, zie B.5. 

     xpb cos(α) −sin(α) 0 xap  b       yp  =  sin(α) cos(α) 0  ·  ypa  xpb xap 0 0 1

(B.5)

Op analoge wijze kunnen de rota ematrices R(X, α) en R(Y, α) bepaald worden (B.6). 

1  R(X, α) =  0 0

 0 0  cos(α) −sin(α)  sin(α) cos(α)



cos(α) 0  R(Y, α) =  0 1 −sin(α) 0

 sin(α)  0  cos(α)

(B.6)

Inverse De inverse van de rota ematrix ba R is per defini e ab R. Deze transformeert namelijk de coördinaten van een punt ten opzichte van {b} naar coördinaten ten opzichte van {a}. Volgens (B.1) kan ab R geschreven worden als (B.7). 

xa · xb  a b a bR =  x · y xa · z b

y a · xb ya · yb ya · zb

 z a · xb  za · yb  za · zb

(B.7)

Het vergelijken van (B.1) en (B.7) leidt tot (B.8). a bR

= ba R−1 = ba RT

(B.8)

Of algemeen kan voor rota ematrices het volgende besloten worden (B.9). R−1 = RT

(B.9)

Opeenvolgende rota es Bij een reeks opeenvolgende rota es zijn twee mogelijkheden voor de opeenvolgende rota e: • De rota e gebeurt rond de assen van het assenstelsel na de vorige rota e. • De rota e gebeurt rond de assen van een vast referen e assenstelsel. Er wordt gesproken van rota es rond bewegende assen en rota es rond vaste assen. Een voorbeeld van rota es rond bewegende assen is de ZYZ rota e, deze beweging gebeurt in de robotpols onder invloed van A4, A5 en A6. Voor rota es rond vaste assen wordt de roll, pitch, yaw rota e toegelicht. Deze wordt gebruikt om de orıënta e van de eindeffector van de robot te definiëren ten opzichte van de basis.

49

De ZYZ rota e De ZYZ rota e is een samengestelde rota e die bestaat uit drie opeenvolgende rota es rond bewegende assen. Een eerste rond z a met een hoek α, een tweede rond y b met een hoek β en een derde rond z c met een hoek γ, zie figuur B.3. Het samenstellen van de rota ematrices gebeurt volgens de passieve interpreta e (B.10). m xn stelt de coördinaten voor van eenheidsvector xn ten opzichte van {m}.

za

za α

zb {b}

{a} xa

ya

xa

za

zc

zd

γ

β

{c}

b

y ya

xb

za

{d}

yc ya

xa

yd

{a} xa

xc

xd

ya

Figuur B.3: De ZYZ rota e

 d d d   cx = c R dx d c d ⇒ a xd = ba R cb R dc R d xd = da R d xd bx = bR cx   d b d ax = aR bx d aR

(B.10)

= ba R cb R dc R

(B.11)

Via (B.11) kan de totale rota ematrix da R bepaald worden (B.12). cα = cos(α), sα = sin(α),...

d aR

= R(ZY Z, α, β, γ) = R(Z, α)R(Y, β)R(Z, γ)    cα −sα 0 cβ 0 sβ cγ −sγ    =  sα cα 0   0 1 0   sγ cγ 0 0 1 −sβ 0 cβ 0 0   cα cβ cγ − sα sγ −sα cγ − cα cβ sγ cα sβ   =  cα sγ + sα cβ cγ cα cγ − sα cβ sγ sα sβ  −sβ cγ sβ sγ cβ

 0  0  1 (B.12)

Omgekeerd kunnen de hoeken α, β en γ uit R(ZY Z, α, β, γ) gehaald worden.

α

= atan2(R23 , R13 )

(B.13)

β

= atan2(cα R13 + sα R23 , R33 )

(B.14)

γ

= atan2(R32 , −R31 )

(B.15)

50

De roll, pitch, yaw rota e De roll, pitch, yaw rota e is een samengestelde rota e die bestaat uit drie opeenvolgende rota es rond vaste assen. Een eerste rond xa met een hoek r, een tweede rond y a met een hoek p en een derde rond z a met een hoek y, zie figuur B.4. Het samenstellen van de rota ematrices gebeurd volgens de ac eve interpreta e (B.16). za

za

za

za Yaw

zb zc {a}

Roll

y

{b}

xa

ya

xa

{a}

xb

zd

{c}

b

{d}

yc

ya Pitch

{a}

{a} xa

ya

yd ya

xa xd

xc

Figuur B.4: De XYZ rota e

 b b a   ax = aR ax c c b ⇒ a xd = dc R cb R ba R a xa = da R d xd ax = bR ax   d d c ax = c R ax d aR

= dc R cb R ba R

(B.16)

(B.17)

Via (B.17) kan de totale rota ematrix da R bepaald worden (B.18).

d 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

 

(B.18)

Omgekeerd kunnen de hoeken r, p en y uit R(RP Y, r, p, y) gehaald worden.

r

= atan2(R32 , R33 )

(B.19)

p

= atan2(−R31 , cy R11 + sy R21 )

(B.20)

y

= atan2(R21 , R11 )

(B.21)

Te onthouden: De volgorde van het matrixproduct verschilt bij rota es rond bewegende assen en rota es rond vaste assen.

Bewegende assen: Vaste assen:

RbABC (α, β, γ) = R(A, α)R(B, γ)R(C, γ) RvABC (α, β, γ) = R(C, γ)R(B, γ)R(A, α)

51

B.1.2

De homogene transforma ematrix

In het vorige onderdeel werd toegelicht hoe de oriënta e van een assenstelsel, ten opzichte van een referen e assenstelsel kan beschreven worden. Om een assenstelsel in de ruimte te definiëren moet ook de posi e gekend zijn. Dit gebeurt aan de hand van de homogene transforma ematrix. Deze matrix bevat zowel de oriënta e als de ligging van een assenstelsel, ten opzichte van een referen e assenstelsel. In dit deel komen de defini e, interpreta e en enkele eigenschappen van deze matrix aan bod. Defini e De pose (rela eve posi e en oriënta e) van {b} ten opzichte van {a} kan beschreven worden door: (a) De posi evector a pa,b van de oorsprong van {b} ten opzichte van de oorsprong van {a} en uitgedrukt ten opzichte van {a}, zie figuur B.5. (b) De rota ematrix ba R van {b} ten opzichte van {a}. Deze worden gecombineerd in een 4 × 4 posematrix of homogene transforma ematrix ba T (B.22).

p

p

b

a

p a

{b}

pa,b

{a}

Figuur B.5: {b} beweegt t.o.v. {a}. Punt p beweegt samen met {b}.

( b aT

=

b aR

a,b ap

01×3

1

) (B.22)

Deze matrixnota e maakt het mogelijk de coördinaten van p (a p) ten opzichte van {a} te berekenen indien de coördinaten van p (b p) ten opzichte van {b} gekend zijn. (

ap

1

)

( =

b aT

bp

1

) (B.23)

Merk op dat de eerder driedelige posi evectoren nu uit vier componenten bestaan. Door een constante 1 toe te voegen worden de vectoren homogeen gemaakt, vandaar de benaming van deze pose transforma e. De homogene transforma ematrix bundelt de nodige informa e voor een rota e en een transla e binnen één matrix. Ac eve en passieve interpreta e Net zoals rota es en oriënta es, kent de homogene transforma ematrix een ac eve en een passieve interpreta e. De passieve interpreta e impliceert een „pose”, de ac eve interpreta e suggereert een „verplaatsing”.

52

Inverse Gezien de inverse van de rota ematrix eenvoudig te berekenen is, kan eenvoudig ingezien worden dat de inverse van een homogene transforma ematrix ba T volgens (B.24) beschreven wordt. ( b −1 aT

b −1 aT

=

b T aR

01×3

−ba RT a pa,b 1

) (B.24)

samen stellen uit ba T vereist slechts één matrixvermenigvuldiging.

Samenstellen pose Net zoals er bij de rota ematrix opeenvolgende rota es kunnen gebeuren, is de samenstelling van verschillende pose transforma es mogelijk. Zo kan de posematrix ca T van een assenstelsel {c} ten opzichte van {a} bepaald worden indien: (a) De posematrix ba T van assenstelsel {b} ten opzichte van {a} gekend is. (b) De posematrix cb T van assenstelsel {c} ten opzichte van {b} gekend is.

{b}

a

pa,b pb,c

b

{a}

{c}

Figuur B.6: Samenstellen van verschillende poses

De posematrix ca T wordt dan uitgedrukt als (B.25). c aT

B.1.3

= ba T cb T

(B.25)

Besluit

De pose van elk assenstelsel, ten opzichte van een referen e assenstelsel, kan via matrices beschreven worden. Met deze theore sche achtergrond kan de kinema ca van de KUKA KR15/2 robot bepaald worden.

53

B.2

Kinema ca van de KUKA KR15/2

De gebruiker wenst de robot langs verschillende punten te laten bewegen. Hiervoor dient de kinema ca van de robot gekend te zijn. Ten eerste moet de pose van de eindeffector te berekenen zijn uit de hoeken van de robotassen, dit gebeurt aan de hand van de voorwaarste kinema ca. Ten tweede moeten de hoeken van de robotassen gekend zijn uit de pose van de eindeffector, dit gebeurt aan de hand van de inverse kinema ca. In dit deel wordt toegelicht hoe deze berekeningen gebeuren. Zie B.2.3 voor de toegepaste MATLAB code.

B.2.1

Voorwaarste kinema ca

De voorwaarste kinema ca berekent de pose van de eindeffector in func e van de robotassen (B.26).

(B.26)

(X, Y, Z, r, p, y) = V K(θ1 , θ2 , θ3 , θ4 , θ5 , θ6 )

De berekeningen gebeuren aan de hand van transforma ematrices. Om de transforma ematrices op te stellen moeten assenstelsels op de robot gedefiniëerd worden, zie figuur B.7. Eerst wordt de transformaematrix ga T van de eindeffector {g} ten opzichte van de basis {a} opgesteld. Hierna worden de posi e en oriënta e van de eindeffector ten opzichte van de basis uit deze matrix afgeleid.

θ5

{e} θ4 e ye + z

+

--

x

g

y

f

xf

xe xd

g

y+ f z -- z F {f } {g} θ6

E --

g

D +

zd

yd {d} C xc

+ θ2 --

--

zb -- b

x

θ1 +

B

yb {b}

{c} c

y

θ3

zc

A

za xa

{a}

ya

Figuur B.7: De KUKA KR15/2 in nulposi e met de gedefiniëerde assenstelsels en afstanden

54

Opstellen transforma ematrix In dit deel wordt de transforma ematrix ga T bepaald. Uit (B.25) volgt (B.27). g aT

= ba T cb T dc T ed T fe T gf T

(B.27)

Deze transforma es kunnen uit B.7 afgeleid worden. ba T wordt als volgt opgebouwd, zie figuur B.8: (a) Een rota e rond z a over de hoek −θ1 . (b) Een transla e volgens z a over de afstand A.

zb θA1 +1

-- b

x

zb

yb {b}

xc

-- b

B

{c}

A

y

z

c

c

xx θ2

A1 +

-π/2

yb {b}

z y

za xa

{a}

ya Figuur B.9: Bepalen van cb T

Figuur B.8: Bepalen van ba T

 b aT

c bT

  = 

0 R(Z, −θ1 ) 0 A 01×3 1

    

(B.28)

wordt als volgt opgebouwd, zie figuur B.9:

(a) Een rota e rond xb over de hoek − π2 . (b) Een rota e rond z over de hoek θ2 − π2 . (c) Een transla e volgens xb over de afstand B.  c bT

 R(XZ, − π , θ − π )  2 2 2 =  01×3

B 0 0 1

    

(B.29)

55

d cT

wordt als volgt opgebouwd, zie figuur B.10:

(a) Een rota e rond z c over de hoek θ3 . (b) Een transla e volgens xc over de afstand C.

xxdd

θ3

dd

E yy--

zdzd {d}

--

C

z

xc

-- b

x

{c} c

y

x

b

f

g

y

θ4

xf

{e} ye ze

g

y θ6

z z F {g} {f } f

g

θ5 E

yd

xe xd D zd {d}

Figuur B.11: Bepalen van ed T, fe T en gf T

c

z

Figuur B.10: Bepalen van dc T

 d cT

e dT

  = 

C R(Z, θ3 ) 0 0 01×3 1

    

(B.30)

wordt als volgt opgebouwd, zie figuur B.11:

(a) Een rota e rond xd over de hoek − π2 . (b) Een transla e volgens xd over de afstand D.  e dT

 R(X, − π )  2 =  01×3

D 0 0 1

    

(B.31)

56

f eT

wordt als volgt opgebouwd, zie figuren B.12 en B.11:

(a) Een rota e rond z e over de hoek −θ4 . (b) Een rota e rond y over de hoek −θ5 . (c) Een rota e rond z over de hoek −θ6 . (d) Een transla e volgens z e over de afstand E.

xe

x

y

x

ye yf

y y θ5 θ4 z

θ6

ze z z

{f } E

zf

{e} x

xf Figuur B.12: Bepalen van fe T

Opmerking: De robot bezit een ZYZ pols, zie B.1.1.  f eT

 R(ZY Z, −θ , −θ , −θ )  4 5 6 =  01×3

g fT

0 0 E 1

    

(B.32)

wordt als volgt opgebouwd, zie figuur B.11:

(a) Een transla e volgens z f over de afstand F .  g fT

  = 

13 01×3

0 0 F 1

    

(B.33)

Daar alle transforma ematrices gekend zijn, kan ga T berekend worden volgens (B.27).

57

Posi e en oriënta e van de eindeffector De posi e en oriënta e van de eindeffector kunnen nu uit ga T afgeleid worden. De posi e wordt voorgesteld door cartesische coördinaten t.o.v. {a}, de oriënta e door de roll, pitch en yaw hoeken t.o.v. {a}. ga T bestaat uit volgende elementen (B.34): ( g aT

a,g ap

=

g aR

a,g ap

01×3

1

) (B.34)

bevat per defini e de posi e van de eindeffector {g} t.o.v. {a}.

a,g ap

= (ga px ga py ga pz )T

(B.35)

De roll, pitch en yaw kunnen uit de rota ematrix ga R gehaald worden.

r

= atan2(−R23 , R33 )

p = atan2(R13 , cy R33 − sy R23 ) y

= atan2(−R12 , R11 )

(B.36) (B.37) (B.38)

Volgens deze methode kan de voorwaarste kinema ca van de robot geïmplementeerd worden.

58

B.2.2

Inverse kinema ca

De inverse kinema ca berekent de hoeken van de robotassen in func e van de pose van de robot (B.39).

(θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = IK(X, Y, Z, r, p, y)

(B.39)

Voor dit wiskundig probleem bestaan verschillende oplossingen. In deze uiteenze ng wordt de gesloten oplossing bepaald. Dit wil zeggen dat de berekeningen gebeuren aan de hand van formules die rechtstreeks tot het resultaat leiden, niet op een itera eve wijze. Eerst wordt de methodiek van de berekeningen toegelicht, daarna volgt hun uitwerking. Ook hier gelden de gedefiniëerde assenstelsels uit figuur B.7. Methodiek De KUKA KR15/2 robot is een standaard zes-assige robot. Bij deze robots wordt de posi e van de robotpols enkel bepaald door de eerste drie assen. De laatste drie assen zorgen voor de gewenste oriënta e van de eindeffector. De berekening kan dus opgesplitst worden in volgende deelproblemen: (a) Berekenen van de transforma ematrix van de eindeffector ten opzichte van de basis ga T. (b) Berekenen van de posi e van de robotpols a p via ga T. (c) Berekenen van θ1 , θ2 en θ3 uit de posi e van de robotpols. (d) Berekenen van de oriënta e van de robot ea R door θ1 , θ2 en θ3 . (e) Berekenen van de oriënta e van de eindeffector ten opzichte van de oriënta e van de robot fe R. (f) Berekenen van θ4 , θ5 en θ6 uit fe R. De transforma ematrix ga T Uit de gegeven x, y, z coördinaten en roll r, pitch p, yaw y hoeken kan posematrix van de eindeffector bepaald worden. De x, y en z coördinaten bepalen a pa,g . De roll, pitch en yaw hoeken bepalen ga R. Zie figuur B.13. A5 + --

xg

+

A4

yg {g} +

+

--

zg ---

A6

A3

(x,y,z) + A2 --

A1 +

-a

pa,g

za r xa

y {a} (0,0,0)

p ya

Figuur B.13: Bepalen van de posematrix van de eindeffector uit de gegevens

59

Zodat de posematrix ga T als (B.40) kan geschreven worden.

( g aT

=

g aR

a,g ap

01×3

1

)

   = 

x R(RP Y, r, p, y) y z 01×3 1

    

(B.40)

De posi e van de robotpols Door langs z g de afstand g p terug te keren, wordt het centerpunt van de robotpols bereikt, zie figuur B.14.

A5 + --

(Xpols, Ypols, Zpols) xg

+

A4

yg {g} gp +

zg

--

+

--

F

A6

a

p

--

A3

(X,Y,Z) + A2 --

A1 +

-a

pa,g

za y {a}

r xa

(0,0,0)

p ya

Figuur B.14: Berekening posi e van de robotpols

Uit (B.23) volgt (B.41). Waardoor de posi e van de robotpols gekend is.

(

ap

1

)

( = ga T

gp

1



) of

   

Xpols Ypols Zpols 1





   g   = aT   

0 0 −F 1

    

(B.41)

Vervolgens kunnen de eerste drie hoeken van de robot bepaald worden. Berekening hoeken θ1 , θ2 en θ3 Figuur B.15 gee het bovenaanzicht van de robot weer. Hieruit volgt (B.42).

60

Y

Exy Dxy

Xpols {e}

Cxy

{f }

Fxy {g} Ypols

{d} {c}

B

θ1 X

{a} {b} Figuur B.15: Bovenaanzicht robotarm, berekening θ1

θ1 = atan2(−Ypols , Xpols )

(B.42)

Opmerking: Op deze manier zal de eerste as al jd gericht zijn naar eindeffector. Het omgekeerde is ook mogelijk maar hier wordt niet op ingegaan. Het zijaanzicht van de robot wordt weergegeven in figuur B.16. Via deze figuur worden de formules opgesteld voor θ2 en θ3 . Eerst worden de hulphoeken α, β en γ bepaald. Hierna volgt de totale procedure tot θ1 , θ2 en θ3 . Bepalen van de hulphoeken α, β en γ: Voor α en β gelden volgende vergelijkingen: L1 sin(α) − L2 sin(β) =

0

(B.43)

L1 cos(α) + L2 cos(β) =

r

(B.44)

(B.43)2 en (B.44)2 : L12 sin2 (α) − 2 L1 L2 sin(α) sin(β) + L22 sin2 (β) L1 cos (α) − 2 L1 L2 cos(α) cos(β) + L2 cos (β) 2

2

2

2

= =

0 r

2

(B.45) (B.46)

(B.45) + (B.46): L12 + L22 + 2 L1 L2(cos(α) cos(β) − sin(α) sin(β)) = L12 + L22 + 2 L1 L2 cos(α + β)

=

r2 r2

(B.47)

61

Uitwerken van (B.47): r2 − L12 − L22 2(L1 L2 ) r2 − L12 − L22 acos 2 L1 L2

cos(α + β) = ⇒α+β

=

(B.48)

Opmerking: Indien (r > L1 + L2) kan het punt niet bereikt worden. Stel: ϵ

= α+β

(B.49)

⇒β

= ϵ−α

(B.50)

Omvormen van (B.43) en invullen van (B.50): L1 L2

= = =

⇒ tan(α) = ⇒α

=

⇒β

=

⇒γ

=

sin(β) sin(ϵ − α) = sin(α) sin(α) sin(ϵ) cos(α) − cos(ϵ) sin(α) sin(α) sin(ϵ) − cos(ϵ) tan(α) sin(ϵ) L1 L2 + cos(ϵ) ( ) sin(ϵ) atan L1 L2 + cos(ϵ)

(B.51)

ϵ−α ( ) D π − atan 2 E

(B.52) (B.53)

Opmerking: Op deze manier zal de arm al jd langs boven staan, het omgekeerde is ook mogelijk. θ3

Z

{e}

{d}

D

γ

E π -(α+β)

C

L2 L1 β

θ2 r

α

B

{c}

{f }

δ

z

Fxyz {g}

xy

{b}

A

XY

{a}

Figuur B.16: Zijaanzicht robotarm, berekening θ2 en θ3

62

Procedure tot θ1 , θ2 en θ3 uit Xpols , Ypols en Zpols : Berekenen van θ1 : θ1 = atan2(−Ypols , Xpols )

(B.54)

Berekenen van L1, L2, xy, z en r: L1 =

C √

L2 =

√

xy =

(B.55) D2 + E 2

(B.56)

X2pols + Y2pols − B

(B.57)

= Zpols − A √ r = xy2 + z2

z

(B.58) (B.59)

Berekenen van α, β en γ: ϵ α β γ

r2 − L12 − L22 2(L1 L2 ) sin(ϵ) = atan L1 L2 + cos(ϵ)

(B.60)

=

(B.61)

= ϵ−α ( ) π D = − atan 2 E

(B.62) (B.63)

He teken van xy bepaalt drie gevallen voor δ en θ2 : (a) xy > 0: ( δ

= atan

θ2

z xy

)

=

π −α−δ 2

δ

=

(B.64) (B.65)

(b) xy = 0:

θ2

π 2 = −α

(B.66) (B.67)

(c) xy < 0: δ

=

θ2

=

atan

( xy )

z −(α + δ)

+

π 2

(B.68) (B.69)

θ3 wordt bepaald door de hulphoeken: θ3 = α + β − γ

(B.70)

De oriënta e van de robotpols ea R door θ1 , θ2 en θ3 Via θ1 , θ2 en θ3 kan de oriënta e van de robotpols ea R bepaald worden (B.71). Met 1 = θ1 en 23 = θ2 + θ3 . 

c1 s23 π π π  e )R(X, − ) =  −s1 s23 a R = R(Z, −θ1 )R(XZ, − , (θ2 + θ3 ) − 2 2 2 c23

−s1 −c1 0

 c1 c23  −s1 c23  −s23

(B.71)

63

De rota ematrix fe R Via de inverse van de rota ematrix kan fe R bepaald worden (B.72).

e aR

= ea R fe R

f eR

of

= ea RT af R

(B.72)

Berekening hoeken θ4 , θ5 en θ6 Daar de robotpols een ZYZ rota e met hoeken θ4 , θ5 en θ6 is, kan (B.72) als (B.74) geschreven worden.



c4 c5 c6 − s4 s6   −c4 s6 − s4 c5 c6 s5 c6

R(ZY Z, −θ4 , −θ5 , −θ6 )

=

e T aR



of



−c4 s5  s4 s5  c5

s4 c6 + c4 c5 s6 c4 c6 − s4 c5 s6 s5 s6

=

R(RP Y, r, p, y)

c1 s23   −s1 c1 c23

−s1 s23 −c1 −s1 c23

(B.73)  c23 r11  0   r21 −s23 r31

r12 r22 r32

 r13  r23  r33

De hoeken θ4 , θ5 en θ6 kunnen bepaald worden uit R(ZY Z, −θ4 , −θ5 , −θ6 ). Hiervoor bestaan twee gevalZ bepaalt het geval: len: θ5 ̸= 0 en θ5 = 0. De waarde van RZY 33 Z ̸= 1 ⇒ θ5 ̸= 0 Geval 1: RZY 33

θ4

Z Z = atan2(RZY , −RZY ) 23 13 Z atan2(s4 RZY 23

−

θ5

=

θ6

Z Z = atan2(−RZY , RZY ) 32 31

Z = 1 ⇒ θ5 = 0 Geval 2: RZY 33  c4 c6 − s4 s6 s4 c6 + c4 s6   −c4 s6 − s4 c6 c4 c6 − s4 s6 0 0

 0  0  1

 =

(B.74)

Z Z c4 RZY , RZY ) 13 33

cos(θ4 + θ6 )   −sin(θ4 + θ6 ) 0

(B.75) (B.76)

 sin(θ4 + θ6 ) 0  cos(θ4 + θ6 ) 0  (B.77) 0 1

zodat θ5 θ4 + θ6

=

0

(B.78)

=

Z Z atan2(RZY , RZY ) 12 11

(B.79)

Volgens deze methode kan de inverse kinema ca van de robot geïmplementeerd worden.

64

B.2.3

Matlab code

Voorwaartse kinema ca

f u n c t i o n [ X , Y , Z , w, p , r ] = V o o r w a a r s t e K i n e m a t i c a ( i _ x E n a b l e , t1 , t2 , t3 , t4 , t5 , t 6 ) % Enable i f ~ i_xEnable X = 0; Y = 0; Z = 0; w = 0; p = 0; r = 0; return end % a b c d e f

Afstanden : = 0.675; = 0.3; = 0.65; = 0.155; = 0.6; = 0.14;

% Goniometrische berekeningen : c1 = c o s ( t 1 ) ; s1 = s i n ( t 1 ) ; c2 = c o s ( t 2 ) ; s2 = s i n ( t 2 ) ; c3 = c o s ( t 3 ) ; s3 = s i n ( t 3 ) ; c4 = c o s ( t 4 ) ; s4 = s i n ( t 4 ) ; c5 = c o s ( t 5 ) ; s5 = s i n ( t 5 ) ; c6 = c o s ( t 6 ) ; s6 = s i n ( t 6 ) ; % Opstellen tranformatiematrices : % AB : T_ab = [ c1 s 1 0 0 ; −s 1 c1 0 0 ; 0 0 1 a

; 0 0 0 1 ];

% BC : T_bc = [ s 2 c2 0 b ; 0 0 1 0 ; c2 −s 2 0 0 ; 0 0 0 1 ] ; % CD : T_cd = [ c3 −s3 0 c ; s 3 c3 0 0 ; 0 0 1 0 ; 0 0 0 1 ] ;

65

% DE : T_de = [ 1 0 0 d ; 0 0 1 0 ; 0 −1 0 0 ; 0 0 0 1 ] ; % EF : T_ef = [

c4 * c5 * c6−s4 * s6 s4 * c6 + c4 * c5 * s 6 −c4 * s 5 0 ; −c4 * s6−s 4 * c5 * c6 c4 * c6−s 4 * c5 * s 6 s 4 * s 5 0 ; s5 * c6 s 5 * s 6 c5 e ; 0 0 0 1];

% FG ; T_fg = [ 1 0 0 0 ; 0 1 0 0 ; 0 0 1 f ; 0 0 0 1 ] ; % AG : T_ag = T_ab * T_bc * T_cd * T_de * T _ e f * T _ f g ; % X Y Z

P o s i t i e bepalen : = T_ag ( 1 , 4 ) ; = T_ag ( 2 , 4 ) ; = T_ag ( 3 , 4 ) ;

% O r i e n t a t i e bepalen : i f a b s ( a b s ( T_ag ( 3 , 1 ) ) − 1 ) < 2* eps p = −p i / 2 * s i g n ( T_ag ( 3 , 1 ) ) ; w = 0; r = a t a n 2 (− T_ag ( 1 , 2 ) , T_ag ( 2 , 2 ) ) ; else w = a t a n 2 (− T_ag ( 2 , 3 ) , T_ag ( 3 , 3 ) ) ; s r = s i n (w ) ; c r = c o s (w ) ; p = a t a n 2 ( T_ag ( 1 , 3 ) , c r * T_ag ( 3 , 3 ) − s r * T_ag ( 2 , 3 ) ) ; r = a t a n 2 (− T_ag ( 1 , 2 ) , T_ag ( 1 , 1 ) ) ; end

66

Inverse kinema ca

f u n c t i o n [ q _ x P o s i t i o n O k , q _ x S i n g u l a r i t y , t1 , t2 , t3 , t4 , t5 , t 6 ] = I n v e r s e K i n e m a t i c a ( i _ x E n a b l e , X , Y , Z , w, p , r ) % Enable i f ~ i_xEnable t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; t6 = 0; return end % Initialisatie q_xPositionOk = true ; q_xSingularity = false ; i f Z < 0.1 q_xPositionOk = f a l s e ; t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; t6 = 0; return end

% a b c d e f

Afstanden = 0.675; = 0.3; = 0.65; = 0.155; = 0.6; = 0.14;

% Goniometrische berekeningen cw = c o s (w ) ; sw = s i n (w ) ; cp = c o s ( p ) ; sp = s i n ( p ) ; cr = cos ( r ) ; sr = sin ( r ) ;

67

% B e p a l e n van de p o s e m a t r i x T_ag = [ c r * cp −s r *cw+ c r * sp *sw s r *sw+ c r * sp *cw X ; s r * cp c r *cw+ s r * sp *sw −c r *sw+ s r * sp *cw Y ; −sp cp *sw cp *cw Z ; 0 0 0 1 ]; % B e p a l e n van h e t c e n t e r p u n t van de p o l s p _ p o l s = T_ag * [ 0 0 −f 1 ] ' ; x_pols = p_pols ( 1 ) ; y_pols = p_pols ( 2 ) ; z_pols = p_pols ( 3 ) ; % B e p a l e n van t1 , t 2 en t 3 t 1 = a t a n 2 (− y _ p o l s , x _ p o l s ) ; i f a b s ( t 1 ) > 175/180* p i q_xPositionOk = f a l s e ; t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; t6 = 0; return end l1 = c ; l 2 = s q r t ( d ^2 + e ^ 2 ) ; x y = s q r t ( x _ p o l s ^2 + y _ p o l s ^ 2 ) − b ; z = z_pols − a ; r = s q r t ( x y ^2 + z ^ 2 ) ; e p s i l o n = a c o s ( ( r ^2 − l 1 ^2 − l 2 ^ 2 ) / ( 2 * l 1 * l 2 ) ) ; i f ~ isreal ( epsilon ) q_xPositionOk = f a l s e ; t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; t6 = 0; return end 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 − a t a n ( d / e ) ;

68

i f ( x y > 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 ( x y < −eps ) d e l t a = atan ( xy / z ) + p i / 2 ; t 2 = −( a l p h a + d e l t a ) ; e l s e % xy = 0 t 2 = −a l p h a ; end t 3 = a l p h a + b e t a − gamma ; i f t 2 < −55/180* p i || t 2 > 115/180* p i || t 3 < −210/180* p i || t 3 < 70/180* p i q_xPositionOk = f a l s e ; t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; t6 = 0; return end % B e p a l e n o r i e n t a t i e p o l s t e n o p z i c h t e van de arm R_ag = T_ag ( 1 : 3 , 1 : 3 ) ; t_23 = t2 + t3 ; s1 = s i n ( t 1 ) ; c1 = c o s ( t 1 ) ; s23 = s i n ( t _ 2 3 ) ; c23 = c o s ( t _ 2 3 ) ; R _ f a = [ c1 * s23 −s1 * s23 c23 ; −s 1 −c1 0 ; c1 * c23 −s 1 * c23 −s23 ] ; R _ f g = R _ f a * R_ag ; i f a b s ( a b s ( R _ f g ( 3 , 3 ) ) − 1 ) < eps % S i n g u l a r i t e i t , as4 b l i j f t staan , as6 doet verder q_xSingularity = true ; t 4 = 0 ; % Som van 4 en 6 wordt met a s 6 meegegeven t 5 = 0 ; % en wordt b u i t e n v e r w e r k t t6 = atan2 ( R_fg ( 1 , 2 ) , R_fg ( 1 , 1 ) ) ; else t4 = atan2 ( R_fg (2 ,3) , − R_fg ( 1 , 3 ) ) ; t5 = atan2 ( s i n ( t4 )* R_fg ( 2 , 3 ) − cos ( t4 )* R_fg ( 1 , 3 ) , R_fg ( 3 , 3 ) ) ; t 6 = a t a n 2 (− R _ f g ( 3 , 2 ) , R _ f g ( 3 , 1 ) ) ; end

69

i f a b s ( t 4 ) > 350/180* p i || a b s ( t 5 ) > 130/180* p i || a b s ( t 6 ) > 350/180* p i q_xPositionOk = f a l s e ; t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; t6 = 0; return end

70

C

Datasheets Beckhoff

In deze bijlage zijn volgende datasheets te vinden: (a) AX51xx Drives (b) AX52xx Drives (c) CX2030 Indstriële PC (d) CX2100 Power module (e) EK1110 EtherCAT buskop

71

AX51xx

AX5118, AX5125, AX5140

Drive Technology

AX51xx | Digital Compact Servo Drives (1-channel)

748

The AX5000 Servo Drive series is available in single- or multichannel form and is optimised in terms of function and costeffectiveness. Integrated control technology supports fast and highly dynamic positioning tasks. EtherCAT as a high-performance communication system enables ideal interfacing with PC-based control technology. The AX51xx 1-channel Servo Drives are designed for rated currents up to 170 A (additional variants up to 315 A are in preparation). The AX5000 system enables simple and fast connection of several AX5000 devices to form a multi-axis system through the “AX-Bridge” quick connection system. The pluggable supply and connection module combines power supply, DC-Link

and 24 V DC control and braking voltage. A wide range of motor types can be connected to the AX5000. Motors of different size and type can be connected without additional measures. Examples include synchronous, linear, torque and asynchronous motors. The multi-feedback interface supports all common standards. The AX5000 was developed specifically for use with the EtherCAT real-time Ethernet system. The outstanding features of EtherCAT are particularly beneficial for Drive Technology. They include short cycle time, synchronicity and simultaneity. EtherCAT enables very short cycle times, even in networks containing a large number of devices.

BECKHOFF New Automation Technology

Features – high-speed EtherCAT system communication – rated current (1-channel Servo Drive): 1.5 A, 3 A, 6 A, 12 A, 18 A, 25 A, 40 A – wide voltage range: 1 x 100 -10 %… 3 x 480 V AC +10 % – active DC-Link and brake energy management – multi-feedback interface – flexible motor type selection – scalable, wide range motor current measurement – high-speed capture inputs – diagnostic and parameter display – integrated mains filter Cat. C3, according to EN 61800-3 – optional safety functions: restart lock, intelligent TwinSAFE safety functions

– –

–

compact design for simple control cabinet installation AX-Bridge – the quick connection system for power supply, DC-Link and control voltage optimised cooling

We reserve the right to make technical changes.

AX51xx

Technical data

AX5101

AX5103

AX5106

AX5112

AX5118

AX5125

AX5140

Rated output current at 50 °C

1 x 1.5 A

1x3A

1 x 4.5 A

–

–

–

–

1 x 1.5 A

1x3A

1x6A

1 x 12 A

1 x 18 A

1 x 25 A

1 x 40 A

0.35 A

1A

1A

6A

12 A

12 A

18 A

(1-phase connection) Rated output current at 50 °C (3-phase connection) Minimum rated motor current at full current resolution Rated supply voltage

3 x 100 V AC -10 %…3 x 480 V AC +10 %

3 x 100 V AC -10 %…

3 x 100 V AC -10 %…

1 x 100 V AC -10 %…1 x 240 V AC +10 %

3 x 480 V AC +10 %

3 x 480 V AC +10 %

DC-Link voltage

max. 890 V DC

Peak output current (1)

4.5 A

7.5 A

13 A

26 A

36 A

50 A

80 A

120 V (1-/3-phase connection)

0.3 kVA

0.6 kVA

1.2 kVA

2.5 kVA

3.4 kVA

4.8 kVA

8.3 kVA

230 V (1-/3-phase connection)

0.6 kVA

1.2 kVA

2.4 kVA

4.8 kVA

7.2 kVA

10.0 kVA

16.0 kVA

400 V (only 3-phase connection)

1.0 kVA

2.1 kVA

4.2 kVA

8.3 kVA

12.5 kVA

17.3 kVA

28.0 kVA

480 V (only 3-phase connection)

1.2 kVA

2.5 kVA

5.0 kVA

10.0 kVA

15.0 kVA

20.8 kVA

33.0 kVA

Continuous braking power (2)

50 W

50 W

150 W

90 W

200 W

200 W

200 W

Max. braking power (2)

14 kW

14 kW

14 kW

14 kW

27 kW

27 kW

27 kW

Power loss (3)

35 W

50 W

85 W

160 W

255 W

340 W

550 W

System bus

EtherCAT

Weight

4.0 kg

4.0 kg

5.0 kg

5.0 kg

11.0 kg

11.0 kg

14.0 kg

Further information

www.beckhoff.com/AX51xx

Rated apparent power for S1 operation (selection)

RMS for max. 7 seconds, (2)internal brake resistor, (3)S1 operation, incl. power supply, without brake chopper

Dimensions

AX5101

AX5103

AX5106

AX5112

AX5118

AX5125

AX5140

Height without connectors

274 mm

274 mm

274 mm

274 mm

274 mm

274 mm

300 mm

Width

92 mm

92 mm

92 mm

92 mm

185 mm

185 mm

185 mm

Depth without connectors

232 mm

232 mm

232 mm

232 mm

232 mm

232 mm

232 mm

Ordering information

AX51xx-0000-0x00

AX5101-0000-0x00

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 1.5 A, EtherCAT interface

AX5103-0000-0x00

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 3 A, EtherCAT interface

AX5106-0000-0x00

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 6 A, EtherCAT interface

AX5112-0000-0x00

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 12 A, EtherCAT interface

AX5118-0000-0x00

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 18 A, EtherCAT interface

AX5125-0000-0x00

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 25 A, EtherCAT interface

AX5140-0000-0200

Digital Compact Servo Drive, 1-axis module, 100…480 V AC, rated output current 40 A, EtherCAT interface

x = 0: HW version 1.0 (compatible with AX5801) x = 2: HW version 2.0 (compatible with AX5805, AX57xx and AX5021)

Ordering information

AX5xxx | Options

AX5021-0000

ballast unit with internal braking resistor and connection option for an external ballast resistor (up to 6 kW) as well as an additional DC link expansion capacity for storing of brake energy Only compatible with HW version 2.0 (AX5xxx-0000-0200)

We reserve the right to make technical changes.

BECKHOFF New Automation Technology

Drive Technology

(1)

749

AX52xx

Drive Technology

AX52xx | Digital Compact Servo Drives (2-channel)

750

The AX5000 Servo Drive series is available in single- or multichannel form and is optimised in terms of function and costeffectiveness. Integrated control technology supports fast and highly dynamic positioning tasks. EtherCAT as a high-performance communication system enables ideal interfacing with PC-based control technology. The AX52xx 2-channel Servo Drive enables operation of two motors with identical or even with different power, up to a total current of 12 A. The multiaxis drives with variable motor output allocation offer optimised packaging density and costs per drive channel. The AX5000 system enables simple and fast connection of several AX5000 devices to form a multi-axis system through the “AX-Bridge” quick connection system. The plug-

gable supply and connection module combines power supply, DC-Link and 24 V DC control and braking voltage. A wide range of motor types can be connected to the AX5000. Motors of different size and type can be connected without additional measures. Examples include synchronous, linear, torque and asynchronous motors. The multi-feedback interface supports all common standards. The AX5000 was developed specifically for use with the EtherCAT real-time Ethernet system. The outstanding features of EtherCAT are particularly beneficial for Drive Technology. They include short cycle time, synchronicity and simultaneity. EtherCAT enables very short cycle times, even in networks containing a large number of devices.

BECKHOFF New Automation Technology

Features – high-speed EtherCAT system communication – rated current (2-channel Servo Drive): 2 x 1.5 A, 2 x 3 A, 2 x 6 A – wide voltage range: 1 x 100 -10 %… 3 x 480 V AC +10 % – active DC-Link and brake energy management – multi-feedback interface – flexible motor type selection – scalable, wide range motor current measurement – high-speed capture inputs – diagnostic and parameter display – integrated mains filter Cat. C3, according to EN 61800-3 – optional safety functions: restart lock, intelligent TwinSAFE safety functions – compact design for simple control cabinet installation

–

–

AX-Bridge – the quick connection system for power supply, DC-Link and control voltage optimised cooling concept

We reserve the right to make technical changes.

AX52xx

Technical data

AX5201

AX5203

AX5206

Rated output current at 50 °C

2 x 1.5 A

2x3A

2 x 6 A (*)

Minimum rated channel current

0.35 A

1A

1A

3A

4.5 A

9A

3A

6A

9A

at full current resolution Max. rated channel current at full current resolution (1-phase connection) Max. rated channel current at full current resolution (3-phase connection) Rated supply voltage

3 x 100 V AC -10 %…3 x 480 V AC +10 % 1 x 100 V AC -10 %…1 x 240 V AC +10 %

DC-Link voltage

max. 890 V DC

Peak output current (1)

2x5A

2 x 10 A

2 x 13 A

Peak output current as total

10 A

20 A

26 A

120 V (1-/3-phase connection)

0.6 kVA

1.2 kVA

2.5 kVA

230 V (1-/3-phase connection)

1.2 kVA

2.4 kVA

4.8 kVA

400 V (only 3-phase

2.1 kVA

4.2 kVA

8.3 kVA

2.5 kVA

5.0 kVA

10.0 kVA

50 W

150 W

90 W

85 W

160 W

6.0 kg

6.0 kg

device current (1) Rated apparent power for S1 operation (selection)

connection) 480 V (only 3-phase connection) Continuous braking power (2) Max. braking power

(2)

14 kW

Power loss (3)

55 W

System bus

EtherCAT

Weight

5.0 kg

Further information

www.beckhoff.com/AX52xx

(1)

RMS for max. 7 seconds, (2)internal brake resistor, (3)S1 operation, incl. power supply, without brake chopper

(*)

Dimensions

AX5201

AX5203

AX5206

Height without connectors

274 mm

274 mm

274 mm

Width

92 mm

92 mm

92 mm

Depth without connectors

232 mm

232 mm

232 mm

Ordering information

AX520x-0000-0x00

AX5201-0000-0x00

Digital Compact Servo Drive, 2-axis module, 100…480 V AC, rated output current 2 x 1.5 A, EtherCAT interface

AX5203-0000-0x00

Digital Compact Servo Drive, 2-axis module, 100…480 V AC, rated output current 2 x 3 A, EtherCAT interface

AX5206-0000-0x00

Digital Compact Servo Drive, 2-axis module, 100…480 V AC, rated output current 2 x 6 A, EtherCAT interface

x = 0: HW version 1.0 (compatible with AX5801) x = 2: HW version 2.0 (compatible with AX5805, AX57xx and AX5021)

Ordering information

AX5xxx | Options

AX5021-0000

ballast unit with internal braking resistor and connection option for an external ballast resistor (up to 6 kW) as well as an additional DC link expansion capacity for storing of brake energy Only compatible with HW version 2.0 (AX5xxx-0000-0200)

We reserve the right to make technical changes.

BECKHOFF New Automation Technology

Drive Technology

For 1-phase mains the total current is limited to 9 A.

751

CX2030

Ethernet and USB connection

Status LEDs

Battery compartment (behind the flap)

DVI connection

CFast

Multi optional interface (e.g. RS232, PROFIBUS, CANopen)

CX2030 | Basic CPU module The CX2030 has a 1.5 GHz Intel® Core™ i7 dual-core CPU, it is fanless and has no rotating components. In addition to the CPU and chipset the CX2030 also contains the main memory with 2 GB RAM as standard. 4 GB is possible as option. The controller boots from the CFast flash memory card. The basic configuration of the CX2030 includes a CFast memory card, two independent Gbit Ethernet interfaces, four USB 2.0 interfaces and a DVI-I interface. The CPU has a 128 kB NOVRAM persistent data memory for situations where no UPS is used. The unit can optionally be ordered with a fieldbus or serial interface. Other components from the CX2000 family can be connected via the multi-pin terminals on either side. The operating system is Microsoft Windows Embedded Standard 7 P. TwinCAT automation software transforms a CX2030 system into a powerful PLC and Motion Control system that can be operated with or without visualisation. Due to its high-performance the CX2030 can be used for interpolating 3-D path movements with TwinCAT NC I. Up to four modules can be connected to the basic CPU module. The connection order is irrelevant. Internally the modules are connected via PCI Express and can be connected subsequently to the CPU "in the field". The power supply for the CPU module comes from a CX2100 power supply module, which is connected on the right-hand side of the CPU. Two further CFast memory card modules (CX2550-0010) can be connected between the power supply unit and the CPU, so that a total of up to three CFast cards can be used. RAID can be used in situations where more than one CFast card is used. The extended operating temperature range between -25 and +60 °C enables application in climatically demanding situations. The order identifier of the basic CPU module is derived as follows:

76 BECKHOFF New Automation Technology

We reserve the right to make technical changes.

CX2100-0xxx

Power LEDs E-bus/ K-bus interface Power supply

Status LCD display

Power supply unit with E-bus/K-bus interface

CX2100-0xxx | Power supply units and and UPS modules for CX2000 The CX2100 power supply and UPS modules are used for supplying power to the components (CPU modules, system and extension modules) of a CX20x0 system. The power supply modules are controlled via TwinCAT. A FSTN LC display with 2 x 16 characters and a selector switch as well as an enter key allow the querying of internal status values as well as self-created display parameters or menus with and without input options. K-bus or alternatively EtherCAT terminals can be connected on the right-hand side – the respective bus system is detected automatically and operated by the CPU. Power is supplied to the E-bus or K-bus electronics in the I/O terminals by an integrated, but internally separate power supply unit with 2 A at 5 V DC. All power supply units from the CX2000 series are in principle passively cooled, have no fans and are intended for operation in the extended temperature range from -25…+60 °C. A choice of four different power supply units is available for the CX2000 CPUs: The standard power supply CX2100-0004 provides a maximum output of 45 W. The more powerful CX2100-0014 power supply unit offers a maximum output of 90 W and has to be used for CX2040 systems equipped with quad-core CPUs. Thanks to its the wider housing front the CX2100-0014 also allows passive ventilation through the front and is thus also suitable for horizontal mounting positions. Optionally the CX2100-0014 can be equipped with active ventilation (fan option) to provide the normally fanless CX2020 or CX2030 systems with a better heat dissipation for operation in different ambient conditions. In addition to the power supply functionality, the CX2100-0904 module also features an integrated capacitive UPS. In the event of a power failure this enables the system to save data on the storage medium and then shut down in an orderly manner. The CX2100-0914 module can be used to charge external battery packs in order to provide backup power for the system and external components such as Control Panels. Technical data

CX2100-0004

Power supply

24 V DC (-15 %/+20 %)

CX2100-0014

CX2100-0904

CX2100-0914

Dielectric strength

500 V (supply/internal electronics)

I/O connection

E-bus (EtherCAT Terminals) or K-bus (Bus Terminals), automatic recognition

Current supply I/O terminals

2A

UPS

–

–

capacitively integrated

external Smart Battery

Charge

–

–

120 As

dependent on battery

Type of connection

spring-loaded technique (adapter terminal)

Display

FSTN display 2 lines x 16 characters of text, illuminated

Diagnostics LED

1 x PWR, 1 x I/O Run, 1 x I/O Err

Max. power consumption

3.5 W

Dimensions (W x H x D)

39 mm x 100 mm x 91 mm

64 mm x 100 mm x 91 mm

117 mm x 100 mm x 91 mm

73 mm x 100 mm x 91 mm

Weight

approx. 200 g

approx. 750 g

approx. 200 g

approx. 200 g

Operating/storage temperature

-25…+60 °C/-40…+85 °C

-25…+60 °C/-40…+85 °C

-25...+50 °C/-40...+60 °C

-25…+60 °C/-40…+85 °C

Relative humidity

95 %, no condensation

Vibration/shock resistance

conforms to EN 60068-2-6/EN 60068-2-27

EMC immunity/emission

conforms to EN 61000-6-2/EN 61000-6-4

Protection class

IP 20

Approvals

CE

77 BECKHOFF New Automation Technology

We reserve the right to make technical changes.

EtherCAT

EK1110

EtherCAT signal output Link/Act

EK1110 | EtherCAT extension Like the E-bus end terminal, the EK1110 EtherCAT extension is connected to the end of the EtherCAT Terminal block. The terminal offers the option of connecting an Ethernet cable with RJ 45 connector, thereby extending the EtherCAT strand electrically isolated by up to 100 m. In the EK1110 terminal, the E-bus signals are converted on the fly to 100BASE-TX Ethernet signal representation. Power supply to the EK1110 electronics is via the E-bus. No parameterisation or configuration tasks are required. Technical data

EK1110

Task within EtherCAT system

conversion of the E-bus signals to 100BASE-TX Ethernet for extension of the EtherCAT network

Data transfer medium

Ethernet/EtherCAT cable (min. CAT 5), shielded

Distance between stations

100 m (100BASE-TX)

Protocol

any EtherCAT protocol

Delay

approx. 1 µs

Data transfer rates

100 Mbaud

Configuration

not required

Bus interface

1 x RJ 45

Power supply

from E-bus

Current consumption E-bus

typ. 130 mA

Electrical isolation

500 V (supply voltage/Ethernet)

Weight

approx. 50 g

Operating/storage temperature

0…+55 °C/-25…+85 °C

Relative humidity

95 %, no condensation

Vibration/shock resistance

conforms to EN 60068-2-6/EN 60068-2-27

EMC immunity/emission

conforms to EN 61000-6-2/EN 61000-6-4

Protect. class/installation pos.

IP 20/variable

Approvals

CE, UL, Ex

Accessories Cordsets

cordsets and connectors

78 BECKHOFF New Automation Technology

We reserve the right to make technical changes.