Ontwerp van een ball and plate systeem met een 6 DoF motion platform en vision-based feedback

Ontwerp van een ball and plate systeem met een 6 DoF motion platform en vision-based feedback Michiel De Paepe

Promotoren: dr. Bart Wyns, prof. dr. ir. Robain De Keyser Masterproef ingediend tot het behalen van de academische graad van Master in de ingenieurswetenschappen: werktuigkunde-elektrotechniek

Vakgroep Elektrische energie, Systemen en Automatisering Voorzitter: prof. dr. ir. Jan Melkebeek Faculteit Ingenieurswetenschappen en Architectuur Academiejaar 2010-2011

Ontwerp van een ball and plate systeem met een 6 DoF motion platform en vision-based feedback Michiel De Paepe

Promotoren: dr. Bart Wyns, prof. dr. ir. Robain De Keyser Masterproef ingediend tot het behalen van de academische graad van Master in de ingenieurswetenschappen: werktuigkunde-elektrotechniek

Vakgroep Elektrische energie, Systemen en Automatisering Voorzitter: prof. dr. ir. Jan Melkebeek Faculteit Ingenieurswetenschappen en Architectuur Academiejaar 2010-2011

Woord vooraf Bij aanvang van dit eindwerk zou ik graag enkele mensen bedanken die het mogelijk gemaakt hebben deze masterproef te verwezenlijken. In de eerste plaats denk ik hierbij aan mijn promotoren, dr. Bart Wyns en prof. dr. ir. Robain De Keyser, die mij de kans en middelen aangereikt hebben om dit eindwerk te realiseren. Verder wil ik Nathalie Garré en ing. Wim Maerevoet bedanken voor hun deskundig advies en taalkundige hulp bij het schrijven van deze thesis. Tevens wil ik mijn dank betuigen aan alle docenten van het KHBO departement Industriële Wetenschappen en Technologie en professoren van de UGent departement Ingenieurswetenschappen voor een goede overdracht van hun kennis zodat ik deze masterproef tot een goed resultaat heb kunnen leiden. Tenslotte wil ik mijn ouders bedanken, die mijn studies mogelijk maakten, en iedereen waar ik kon op rekenen voor de nodige steun.

Michiel De Paepe Landegem, 6 juni 2011

iii

Toelating tot bruikleen De auteur geeft de toelating deze masterproef voor consultatie beschikbaar te stellen en delen van de masterproef te kopiëren voor persoonlijk gebruik. Elk ander gebruik valt onder de beperkingen van het auteursrecht, in het bijzonder met betrekking tot de verplichting de bron uitdrukkelijk te vermelden bij het aanhalen van resultaten uit deze masterproef.

Michiel De Paepe Landegem, 6 juni 2011

iv

Ontwerp van een ball and plate systeem met een 6 DoF motion platform en vision-based feedback Michiel De Paepe Promotoren: dr. Bart Wyns, prof. dr. ir. Robain De Keyser Masterproef ingediend tot het behalen van de academische graad van Master in de ingenieurswetenschappen: werktuigkunde-elektrotechniek Vakgroep Elektrische energie, Systemen en Automatisering Voorzitter: prof. dr. ir. Jan Melkebeek Faculteit Ingenieurswetenschappen en Architectuur Academiejaar 2010-2011

Samenvatting Het ball and plate systeem is een onstabiel open-loop systeem dat bestaat uit een platform waarvan de helling in twee richtingen gewijzigd kan worden met daarop een bal. Het is de tweedimensionale uitbreiding van het ball and beam systeem, dat een schoolvoorbeeld is in de regeltechniek. Deze thesis heeft als doel het maken van zo’n ball and plate systeem waar gebruik gemaakt wordt van een 6 Degrees of Freedom Motion platform. Feedback over de positie van de bal wordt verkregen met behulp van een camera, welke boven het platform opgesteld staat. Op het einde van deze thesis worden twee regelaars ge¨ımplementeerd om de werking van het geheel te demonstreren.

Trefwoorden Ball and plate, 6 DoF motion platform, vision feedback v

A Ball and Plate system with a 6 DoF Motion Platform and Vision-Based Feedback Michiel D E PAEPE Supervisor(s): dr. Bart W YNS, prof. dr. ir. Robain D E K EYSER Abstract— The ball on plate system is an open-loop unstable system . It consists of a platform which can be tilted by servos or electric motors together with a ball rolling on top of the plate. It is the two-dimensional expansion of the ball and beam system which is a popular textbook example in control theory. This article covers the building process of a ball and plate system. The plate used is a 6 DoF motion platform. Feedback on the position of the ball is obtained from images captured by a camera positioned above the platform. At the end, two control algorithms were implemented for demonstration purposes. Keywords—Ball and plate, 6 DoF motion platform, vision feedback

III. K INEMATICS The forward kinematics are based on a paper of Webjørn Rekdalsbakken: The Use of Artificial Intelligence in Controlling a 6 Degrees of Freedom Motion Platform [1]. Here an analytical method is derived for calculating the positions of the servos for a given platform position. The main control inputs are the rotation about the x- and yaxis. The influence of the z-translation was examined and optimized in order to maximize the limits of the control inputs.

I. I NTRODUCTION HE aim of this master thesis is to build a ball and plate setup which, in later projects, can be used to test controllers on a physical system. First the platform was built. The platform is connected to the actuating servos by links. To be able to move the platform in a controlled way, an invers kinematics model of the motion platform was derived. After this, a vision based measuring system was developed. At the end, the loop was closed by a controller. Two control algorithms are proposed: a PD controller and a Lineair Quadratic controller.

T

II. H ARDWARE The setup’s main dimensions are 85cm × 85cm × 50cm. The platform has a diameter of 57, 5cm, and is actuated by six RC servos. The servos are controlled by a self-made six channel USB servo controller, shown in figure 1. The heart of the controller is a Microchip PIC18F4550. With the controller it is possible to send a position and calibrate each individual servo. The calibration data is stored in the internal EEPROM on the microcontroller.

Fig. 2: The ball and plate system setup. Fig. 1: The six chanel USB servo controller

IV. V ISION F EEDBACK

300

x (mm)

200 100 Set point Simulatie Werkelijk

0 −100

0

1

2

3

4

5

t (s) 20 10 u (°)

At this point, the platform is ready to be used as a ball and plate system. To control a ball on the platform, knowledge of the ball’s position is required, which needs to be calculated. For this purpose, a camera is placed above the platform. To obtain the location of the ball on the platform, a calibration of the camera is required. The intrinsic parameters (focal length and principal point) are provided by an existing Matlab program: Camera Calibration Toolbox for Matlab, written by Bouguet [2]. The extrinsic parameter (the relative position of the camera with respect to the platform) is determined by a selfwritten calibration program, based on three markers located on the platform. The images captured by the camera are fed into an edge detection filter. To determine the ball’s position, the Hough transformation algorithm is used. To calculate the position of the ball in the moving platform frame (see figure 2), a geometrical method is proposed, using the pin-hole camera model.

0 Simulatie Werkelijk

−10 −20

0

1

2

3

4

5

t (s)

Fig. 4: Step response of with a LQ controller

V. C ONTROLLERS To demonstrate the capabilities of the ball and plate system, two controllers were implemented. The first controller is a PD controller, short for a Proportional Derivative controller. This controller was tuned with a Matlab tool developped by prof. R. De Keyser and dr. C. Ionescu, FRtool: A Frequency Response Tool for CACSD in Matlab [3]. This CAD-software uses frequency charts of a linear model as a controller design tool. The results are shown in figure 3. 300

x (mm)

200 100

−100

0

1

2

3

4

5

t (s) 10

u (°)

5 0 Simulatie Werkelijk

−5 −10

0

1

2

3

4

With this setup, it is possible to control a ball on the platform. Nevertheless, future optimizations can be performed. Two sugestions are made: • Now the platform is positioned in a feed forward manner. Problems emerge when a PD controller is used. An imperfect horizontal position will result in a steady state offset because of the P-action. By adding sensors (gyroscopes, accelerometers, . . . ), the platform can be positioned in a feedback loop • The performance of the control can be improved by using more sophisticated controllers. The current controllers only use two out of the six available degrees of freedom of the platform. R EFERENCES

Set point Simulatie Werkelijk

0

VI. C ONCLUSIONS

5

t (s)

Fig. 3: Step response of with a PD controller The second control algorithm is a Linear Quadratic controller. The feedback gain is based upon the minimization of a quadratic cost function. The gain matrix can be transformed to an equivalent PD controller. This equivalent PD controller has similar Kp and Kd gains as the one tuned with FRTool. This is reflected in a similar step response, shown in figure 4.

[1] Webjørn Rekdalsbakken,: The Use of Artificial Intelligence in Controlling a 6DOF Motion Platform, European Council for Modelling and Simulation, 2007. [2] Bouguet, J.Y., Camera Calibration Toolbox for Matlab, [3] R. De Keyser and C. Ionescu, Matlab: modelling, programming and simulations, Sciyo, 2010.

Inhoudsopgave

1 Inleiding

1

1.1

Doelstellingen van het eindwerk . . . . . . . . . . . . . . . . . . . . . . . . .

1

1.2

Structuur van het eindwerk . . . . . . . . . . . . . . . . . . . . . . . . . . .

2

2 Platform constructie

5

2.1

Servohouders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5

2.2

Excentriek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6

2.3

Kogelgewrichten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6

2.4

Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6

2.5

Bodemplaat . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.6

Samenstelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

3 Hardware 3.1

3.2

9

Actuatoren . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

3.1.1

Types . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

3.1.2

Servo’s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

3.1.3

Signalen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

Zes kanaals USB controller . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 3.2.1

Schema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2.2

USB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.2.3

Signalen genereren . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 viii

INHOUDSOPGAVE

3.3

3.2.4

kalibratie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.2.5

Commando’s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4 Modellering 4.1

ix

24

Positie van het platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 4.1.1

Ball and beam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.1.2

Ball and plate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

4.2

Bepalen van de servohoeken . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.3

Dynamisch model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 4.3.1

Continu state space model . . . . . . . . . . . . . . . . . . . . . . . . 33

4.3.2

Discretisatie van een state space model

4.3.3

Discreet state space model . . . . . . . . . . . . . . . . . . . . . . . . 36

5 Vision feedback 5.1

5.2

5.3

. . . . . . . . . . . . . . . . 35

37

Camera model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 5.1.1

Pin-Hole camera model . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.1.2

Camera kalibratie . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

Hough Transformatie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 5.2.1

Theorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.2.2

Het algoritme voor lijndetectie . . . . . . . . . . . . . . . . . . . . . 44

5.2.3

Het algoritme voor cirkeldetectie . . . . . . . . . . . . . . . . . . . . 45

5.2.4

Hough transformatie in OpenCV . . . . . . . . . . . . . . . . . . . . 46

Circle Midpoint Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 5.3.1

Het algoritme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

5.3.2

Een nieuwe decision variable . . . . . . . . . . . . . . . . . . . . . . 49

5.3.3

Opmerking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

INHOUDSOPGAVE 5.4

x

Transformaties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 5.4.1

Implementatie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.4.2

Transformatie tussen camera frame en world frame . . . . . . . . . . 52

5.4.3

Transformatie tussen camera frame en principal frame . . . . . . . . 52

5.4.4

Transformatie tussen principal frame en image frame . . . . . . . . . 52

5.4.5

Transformatie tussen platform frame en world frame . . . . . . . . . 53

5.5

Positiebepaling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.6

Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 5.6.1

Theorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.6.2

Toepassen van het filter . . . . . . . . . . . . . . . . . . . . . . . . . 55

6 Ball and plate regelaars

57

6.1

Lineair kwadratische regelaar . . . . . . . . . . . . . . . . . . . . . . . . . . 58

6.2

PD regelaar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

6.3

Conclusie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

6.4

Controller programma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63 6.4.1

Instellingen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

6.4.2

Initialisatie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

6.4.3

Programma flow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

6.4.4

Path generator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

7 Toekomstig werk 7.1

68

Vision algoritme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68 7.1.1

Eerste oplossing: Forward kinematics

. . . . . . . . . . . . . . . . . 68

7.1.2

Tweede oplossing: Sensoren . . . . . . . . . . . . . . . . . . . . . . . 69

7.2

Werkelijke afmetingen bepalen . . . . . . . . . . . . . . . . . . . . . . . . . 69

7.3

Regelaar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

INHOUDSOPGAVE 7.4

xi

Automatisatie van kalibratie en vision-parameters . . . . . . . . . . . . . . . 70

A Constructietekeningen

71

B 6 kanaals servo USB controller

76

C platformData.xml

79

D Datasheets

81

Referenties

85

Hoofdstuk 1

Inleiding 1.1

Doelstellingen van het eindwerk

Deze thesis heeft als doel het maken van een ball and plate opstelling. Hierbij wordt gebruik gemaakt van een 6 Degrees of Freedom Motion Platform (zie figuur 1.1). De positie van de bal wordt bepaald aan de hand van een camera die boven het platform opgesteld staat.

Figuur 1.1: Het SolidWorks model van het 6 DoF Motion Platform Een ball and plate systeem op zich heeft weinig meerwaarde. De uitdaging ligt in het stabiliseren van dit onstabiel open-loop systeem. Schoolvoorbeelden van onstabiele open-loop systemen zijn het ball and beam en het ge¨ınverteerde pendumum. Het ball and plate systeem kan als een tweedimensionale uitbreiding van het ball and beam systeem beschouwd worden. Dit systeem kan gebruikt worden als een educatieve tool voor studenten uit de regeltechniek. 1

Hoofdstuk 1. Inleiding

1.2

1.2. Structuur van het eindwerk

Structuur van het eindwerk

Hieronder volgt een oplijsting van de verschillende hoofdstukken en een korte inhoud ervan.

Platform constructie Het platform bestaat uit verschillende onderdelen. De meeste onderdelen werden zelf vervaardigd. In dit hoofdstuk wordt een overzicht gegeven en de functie van de verschillende onderdelen besproken. Gedetailleerde werkstuktekeningen zijn terug te vinden in appendix A.

Hardware Het motion platform wordt in beweging gebracht door gebruik te maken van zes modelbouw servo’s. Eerst wordt dit type servo besproken, hoe de servo’s werken en hoe ze aangestuurd moeten worden. Voor het aansturen van de servo’s werd een 6 kanaals USB controller ontworpen. Met deze sturing is het mogelijk om via USB, vanop een computer, de hoeken van de zes servo’s in te stellen. Op het einde van dit hoofdstuk wordt het programma besproken dat geschreven werd om deze controller aan te sturen. Met het programma kunnen de servo’s één voor één ingesteld worden, alsook het platform in zijn geheel (translaties en rotaties). Het programma maakt het ook mogelijk om de verschillende servo’s individueel te kalibreren.

Modellering In dit hoofdstuk komen drie wiskundige afleidingen aan bod. Eerst wordt een set parameters gekozen die de positie van het platform bepalen. Aan de hand van deze parameters wordt een transformatiematrix opgesteld. Vervolgens wordt de inverse kinematica van het platform afgeleid. Deze maakt het mogelijk om de servoposities te berekenen in functie van de platformpositie. Als laatste wordt een eenvoudig dynamisch model afgeleid van een ball and plate systeem door gebruik te maken van de methode van Lagrange. Dit model wordt dan gelineariseerd en in een continue state space vorm gebracht. Dit continu model wordt daarna op analytische wijze gediscretiseerd. Dit discreet state space model wordt later gebruikt voor het Kalman filter.

2



Vision feedback De feedback voor de regelaars bestaat uit de positie van de bal. Deze positie wordt bepaald met behulp van een vaste camera, opgesteld boven het platform. Om metingen te kunnen doen aan de hand van een camera is een model van die camera nodig. De parameters voor het model worden bepaald via een kalibratie. Het cameramodel maakt gebruik van verschillende assenstelsels. In het onderdeel Transformaties worden de onderlinge transformaties tussen de verschillende assenstelsels besproken. De positie van de bal op het platform kan pas bepaald worden als het eerst de bal uit een camerabeeld kan halen. Hiervoor wordt gebruik gemaakt van de Hough transformatie. Dit is een algoritme waarmee cirkels uit een figuur gehaald kunnen worden. Na het bepalen van de bal in een camerabeeld wordt een algoritme afgeleid dat, gebruik makend van het cameramodel, de positie van de bal op het platform berekend. Aan het einde van het hoofdstuk wordt de Kalmanfilter besproken die de metingen filtert aan de hand van het discrete model dat in een vorig hoofdstuk afgeleid werd.

Ball and plate regelaars In het begin van dit hoofdstuk wordt aangetoond dat het ball and plate systeem observeerbaar en regelbaar is. Het is dus mogelijk om met de meting van de positie van de bal op het platform de volledige toestand van het systeem te bepalen. De regelbaarheid geeft aan dat met de gegeven regelingangen (x- en y-helling van het platform) het mogelijk is om de volledige toestand te regelen. Verder worden twee regelaars afgeleid: • Lineair Kwadratische regelaar • Proportioneel Differentieel regelaar Resultaten van beide regelaars worden bekomen via simulatie en experimenten op de werkelijke opstelling. Na het rudimentair bespreken van deze twee regelaars wordt een methode afgeleid om de parameters van de regelaars te vergelijken. Hierbij wordt besloten dat beide regelaars, bekomen via volkomen verschillende methodes, nagenoeg equivalent zijn. Dit is af te leiden uit de gelijkwaardige resultaten van beide regelaars. Het hoofdstuk wordt afgesloten met een uiteenzetting van het computerprogramma dat gemaakt werd om het ball and plate systeem te bedienen. Hiermee kan het platform 3



bediend worden en wordt de vision-feedback gegenereerd. Het programma vormt een basis om verschillende types regelaars te implementeren.

Toekomstig werk Aan het ball and plate systeem kan verder gewerkt worden. De grote bouwstenen zijn aanwezig maar, het geheel kan nog verfijnd worden. Enkele voorbeelden zijn het gebruiken van performantere regelalgoritmes, closed-loop positionering van het platform,. . . De lijst van onderwerpen die in dit hoofdstuk besproken wordt, is niet compleet. Andere mogelijkheden hangen af van de creativiteit van de uitvoerder ervan.

4

Hoofdstuk 2

Platform constructie In dit hoofdstuk worden de verschillende mechanische onderdelen van het motion platform besproken. De hoofdafmetingen van de opstelling zijn 85 cm × 85 cm × 50 cm en het plat-

form heeft een diameter van 57,5 cm. Alle constructietekeningen zijn terug te vinden in

appendix A.

2.1

Servohouders

Op basis van de afmetingen van de servo’s werden servohouders ontworpen. Deze houders zijn gemaakt uit 10 mm dik plexiglas. De servo’s passen in een U-vorm en worden bevestigd met 4 bouten M5 × 20 mm. Aan de onderkant van de servohouders zijn twee gaten van

M6 getapt om het geheel te kunnen bevestigen aan de bodemplaat.

Figuur 2.1: Servohouder

5

Hoofdstuk 2. Platform constructie

2.2

2.2. Excentriek

Excentriek

Op de servo’s wordt een exentriek gemonteerd. Dit is een aluminium onderdeel met hoofdafmetingen 85 mm × 20 mm × 5 mm. Voor alignering is er een blind gat gefreesd van 9 mm en een diepte van 1 mm. Hierin past

de centreerring van de bevestigingsschijf van de servo. De servo en de exentriek worden bevestigd met bouten M2,6 × 15 mm. Aan het andere uiteinde zijn drie gaten voorzien van 5 mm. Ze bevinden zich respectie-

velijk op 50 mm, 55 mm en 60 mm van de centreerring. In één van deze gaten moet een kogelgewricht komen. De mogelijkheid is open gelaten om achteraf de configuratie van het platform nog te kunnen instellen. Hoe groter de exentriciteit, hoe meer regelbereik maar ook hoe minder nauwkeurig.

Figuur 2.2: Exentriek

2.3

Kogelgewrichten

De gebruikte kogelgewrichten zijn van het merk HUCO, type 373P05.C.F, volgens de norm DIN 71802. De kogelgewrichten hebben aan de ene kant een uitwendige schroefdraad en aan de andere kant een inwendige, beiden van M5. De datasheet van de kogelgewrichten is toegevoegd in appendix D. In totaal zijn 12 kogelgewrichten nodig. 6 ervan worden bevestigd aan de excentriek, de overige komen aan het platform. Overeenkomstige kogelgewrichten die aan het platform en de excentriek bevestigd zijn, zijn onderling verbonden met een stang met lengte 35 cm.

2.4

Platform

Het platform is een plexiglas schijf van 575 mm × 5 mm. De bovenkant van de schijf is

bedekt met een zwarte folie. Hiermee wordt een goed contrast bekomen tussen de bleke bal die over het platform zal rollen en het platform zelf. 6


2.5. Bodemplaat

Figuur 2.3: Kogelgewricht Onder het platform zijn drie blokjes geplaatst. In deze blokjes is schroefdraad M5 getrokken om de kogelgewrichten op te monteren. Op elk blokje passen twee kogelgewrichten. De blokjes zelf hangen aan het platform met bouten M4 × 15 mm.

Figuur 2.4: Platform

2.5

Bodemplaat

De bodemplaat is een MDF (Medium Density Fiberboard) plaat van 850 mm × 850 mm ×

18 mm. Op deze plaat zijn 12 gaten aangebracht van 6 mm via welke de servohouders

met bouten M6 × 30 mm kunnen bevestigd worden.

2.6

Samenstelling

Een foto van het motion platform is weergegeven in figuur 2.5. Een detail van een gemonteerde servo is afgebeeld in figuur 2.6

7


2.6. Samenstelling

Figuur 2.5: Motion platform opstelling

Figuur 2.6: Detail van een servo

8

Hoofdstuk 3

Hardware Dit hoofdstuk handelt over de hardware. Eerst worden de servo’s besproken: wat zijn servo’s, waarom werden servo’s gebruikt en hoe worden ze aangestuurd. Vervolgens wordt de zes kanaals USB servo controller behandeld. Dit houdt in: de onderdelen van het elektronische schema, het gebruikte USB framework van Microchip en het genereren van de positiesignalen.

3.1 3.1.1

Actuatoren Types

Er zijn twee types actuatoren mogelijk bij het bouwen van een motion platform. Een Steward platform [3] gebruikt zes lineaire actuatoren. Deze worden vooral ingezet voor toepassingen waar grote krachten nodig zijn. Een andere mogelijkheid is gebruik maken van modelbouw servo’s [23]. Voor grotere krachten kunnen eventueel servomotoren met een excentriek gebruikt worden.

3.1.2

Servo’s

Een servosysteem is een mechanisme dat door gebruik te maken van feedback automatisch het gedrag van het systeem be¨ınvloedt [27]. RC servo’s zijn servomotoren die vooral in de modelbouw toegepast worden. Aan deze servo’s moet een positiesignaal gegeven worden. Aan de hand van dit signaal wordt de motoras naar de gewenste positie gebracht. RC servo’s bestaan uit een DC-motor, een potentiometer en een controller chip. Met de potentiometer wordt de positie van de motoras gemeten. De controllerchip verwerkt het ontvangen positiesignaal en stuurt de DC-motor aan in een feedback loop om de gewenste aspositie te bekomen. Een exploded view van een servo is weergegeven in figuur 3.1. [17] 9

Hoofdstuk 3. Hardware

3.1. Actuatoren

Figuur 3.1: Exploded view van een MS455 Servo Voor de opstelling werd gekozen voor Hitec servo’s van het type HS-805MG wegens zijn hoog koppel. Enkele eigenschappen van deze servo zijn: • • • • • •

Koppel: 1,94 Nm bij 5 V en 2,42 Nm bij 6 V Afmetingen: 65,79 mm × 29,97 mm × 57,4 mm Gewicht: 197,03 g Heavy Duty Metal Gear Train 3-polige motor Dubbele kogellagers

3.1.3

Signalen

Een servo heeft drie aansluitingen. Een voor massa (zwart), een voor de voeding (rood) en een derde voor het positiesignaal. Dit positiesignaal bestaat uit pulsen met een periode van 20 ms. De breedte van de puls bepaalt de gewenste positie. Normaliter komt een puls met een breedte van 1,5 ms overeen met de neutrale positie (0 ◦ ). Een puls van 0,5 ms komt overeen met −90 ◦ en een puls van 2,5 ms komt overeen met 90 ◦ , zie figuur 3.2. De

bewegingen van modelbouw servo’s zijn mechanisch beperkt, meestal van −90 ◦ tot 90 ◦ .

[17]

10


3.2. Zes kanaals USB controller

Figuur 3.2: Positiesignalen

3.2

Zes kanaals USB controller

Om de zes servo’s aan te sturen is er nood aan een controller die vanaf een computer aangestuurd kan worden. Het hart van deze controller bestaat uit een PIC18F4550 microcontroller van Microchip. De microcontroller genereert de positiesignalen voor de verschillende signalen en communiceert met de computer over USB. Ook is het mogelijk om via de computer de servo’s apart te kalibreren (nulpunt instellen en hoek kalibreren). Deze kalibratiegegevens worden opgeslagen in het EEPROM geheugen van de microcontroller.

3.2.1

Schema

Het volledig schema en de printplaat zijn terug te vinden in appendix B. Hieronder worden de verschillende onderdelen van het circuit besproken.

Voeding De voeding bestaat uit een UA7805 spanningsregelaar van Texas Instruments. Deze IC vormt ingangsspanningen tot 25 V om tot een gelijkspanning van 5 V. In principe is enkel condensator C2 van 100 nF nodig. Deze voorkomt oscillatorisch gedrag ten gevolge van variaties van de voedingsspanning. Er wordt aangeraden aan de uitgang ook een condensator van 100 nF te plaatsen (C3 ) [8]. Deze twee keramische condensatoren moeten zo dicht mogelijk bij de spanningsregelaar geplaatst worden. De elektrolytische condensatoren C1 en C4 van 100 nF werken trage variaties weg. [6] De diode D1 is geplaatst om beschadiging van het circuit tegen te gaan bij het verkeerd aansluiten van de voeding. Een LED geeft aan of de voeding ingeschakeld is. De serie weerstand R1 werd begroot op 11



150 Ω, gebaseerd op een stroom van 20 mA en een spanningsval van 2 V over de LED. Het circuit is zo opgebouwd dat verschillende voedingen aangesloten kunnen worden. SUPPLY-2 voedt de sturing.

Hierop moet een gelijkspanning aangelegd worden van

7 − 25 V. SUPPLY-3 is de voeding van de servo’s, die gevoed worden met een spanning tussen 5 V en 6 V. Hoe hoger de spanning, des te hoger de performantie van de servo’s wordt.

Figuur 3.3: Voedingscircuit

Outputs Het positiesignaal naar de servo’s wordt geleverd via een inverterende transistor schakelaar. De microcontroller stuurt de basis van de transistor. Als de basis hoog komt (VBE ≈ 0,7 V),

zal de transistor geleiden en zal het positiesignaal ongeveer 0,2 V bedragen (VCEsat ). Als de microcontroller de basis laag stuurt, spert de transistor. Het positiesignaal wordt nu 5 V met een impedantie van 1 kΩ.

Figuur 3.4: Output circuit

USB USB heeft 4 aansluitingen, met voeding en ground op respectievelijk pin 1 en pin 4. De ground van de USB is verbonden met de ground van het circuit, de voeding van de USB 12



wordt niet gebruikt. De overige twee aansluitingen zijn voor data: pin 2 D− en pin 3 D+. Deze vormen een differentiële bi-directionele seriële bus.

Figuur 3.5: USB circuit

Knoppen Er zijn 2 knoppen voorzien die als inputs kunnen dienen. De knoppen zijn geschakeld in een active low configuratie. Dit betekent dat de ingang van de microcontroller een laag signaal detecteert wanneer er op de knop gedrukt wordt. Condensatoren werden parallel over de knoppen geplaatst, doch het is nodig in software met contact debounce rekening te houden. Dit is een fenomeen dat optreedt bij het openen en sluiten van de drukknop, waarbij op korte tijd meerdere overgangen teruggevonden kunnen worden. Indien hier geen rekening mee gehouden wordt, is het mogelijk dat één druk op de knop meerdere keren gedetecteerd wordt.

Figuur 3.6: Knoppen

ICSP In-Circuit Serial Programming (ICSP) is een methode van Microchip om microcontrollers via enkele pinnen te programmeren. Tijdens normale werking kunnen deze pinnen gebruikt worden als I/O pinnen. Met ICSP is het mogelijk de microcontroller te programmeren terwijl hij in het circuit zit, dit vereenvoudigt het ontwikkelingsproces. Om een microcontroller te programmeren wordt een kloksignaal aangesloten (PGC), een datasignaal (PGD) en een programmeerspanning (VPP), allemaal ten opzichte van een gemeenschappelijke massa. De programmeerspanning bedraagt ongeveer 13 V. Aan de hand van deze spanning kan de microcontroller onderscheid maken tussen gewone werking en programmeren. 13



Bij gewone werking doet de VPP pin dienst als MCLR pin (active low reset pin). De microcontroller reset als deze pin laag is. Om manueel te kunnen resetten wordt een active low schakelaar gebruikt. Om de 13 V die de programmer oplegt tijdens het programmeren en de reset schakeling gescheiden te houden wordt een diode toegevoegd.

Figuur 3.7: ICSP circuit

De controller De controller printplaat is weergegeven in figuur 3.8. Bovenaan worden de servo’s aangesloten (van rechts naar links). Onderaan rechts wordt de voedingsspanning aangesloten. Hier is zwart de massa, geel 12 V en rood 5 V (de voeding van de servo’s). Links onder bevindt zich de reset knop.

3.2.2

USB

USB (Universal Serial Bus) is een seriele half-duplex host gecontroleerde bus. Het is een plug’n play bus wat betekent dat apparaten kunnen aangesloten worden zonder dat ze ingesteld moeten worden en zonder opnieuw op te starten. USB heeft een ster topolotie, net als 10BaseT Ethernet, en er kan slechts één host per bus aanwezig zijn (de On-TheGo specificatie van USB 2.0 buiten beschouwing gelaten). Deze USB host initieert alle transacties en is verantwoordelijk voor de scheduling van de bandbreedte. Wanneer een USB device aangesloten wordt, zal de host dit detecteren. De host ondervraagt het device en laadt de benodigde drivers. Dit gebeurt aan de hand van de PID (Product ID) en VID (Vendor ID). VID’s moeten worden aangekocht bij het USB Implementor’s forum. Sommige fabrikanten, zoals Microchip, stellen hun VID met bijhorende PID’s ter beschikking. Microchip’s VID is 0x04D8.

14



Figuur 3.8: Foto van de 6 kanaals USB servocontroller Connectoren Aan de host kant bevindt zich de upstream connectie en aan de device kant de downstream connectie. Hiervoor worden twee mechanisch verschillende connectoren gebruikt. Connector type A (figuur 3.9a) bevindt zich altijd aan de upstream kant, connector type B (figuur 3.9b) aan de downstream kant.

2 1

1 2 3 4

3 4

(a) USB A

(b) USB B

Figuur 3.9: USB Connectors

Communicatie In tegenstelling tot RS-232 en vergelijkbare protocollen, waar het formaat waarin de data gestuurd wordt niet vastgelegd is, is USB opgebouwd uit verschillende protocol lagen. De onderste lagen worden meestal afgewerkt door de USB controller ICs of modules in de microcontroller. Hieronder wordt kort een overzicht gegeven van hoe deze communicatie werkt.

15



Aangezien de host transacties moet initiëren wordt er gebruik gemaakt van buffers of endpoints. Er zijn IN en OUT endpoints. De host plaatst data in het OUT endpoint, die daar blijft tot ze verzonden kan worden. Een device heeft een IN endpoint. Hierin wordt data weggeschreven die van het device naar de host moet gestuurd worden. Deze data wordt pas verstuurd wanneer de host het commando geeft (door een IN pakket te versturen). Per device zijn er maximaal 16 IN en OUT endpoints. Endpoint 0 (EP0) moet steeds aanwezig zijn. De connectie tussen twee endpoints wordt een pipe genoemd. Er bestaan vier soorten endpoints: • Control Transfers: hierover worden requests van de host naar het device gestuurd. • Bulk transfer: hierover vindt dataoverdracht plaats die niet tijdskritisch is. Andere

transfers op de bus krijgen voorrang (vb. printers). • Interrupt transfer: dit zijn transfers die moeten gebeuren binnen een bepaalde tijd, een minimale delay wordt gegarandeerd (vb. toetsenborden). • Isochronous transfer: wordt gebruikt voor grote hoeveelheden aan constante snelheid

te verzenden data die binnen een bepaalde tijd moet aankomen. Hierbij mogen fouten optreden (vb. streamen van audio).

Device classes Zoals eerder vermeld ondervraagt de host het device wanneer hij aangesloten wordt. Vele USB toepassingen kunnen onder eenzelfde noemer geplaatst worden, ze worden onderverdeeld in device classes. Het voordeel van deze standaardisatie is dat besturingssystemen de meeste van deze device classes reeds ondersteunen. Hierdoor moet de device ontwikkelaar enkel drivers maken voor de zelfgemaakte functionaliteit. Gekende klassen zijn de Human Interface Divice Class (HID) voor bv. toetsenborden en de Mass Storrage Device Class voor onder andere externe harde schijven. De virtuele COM-poort devices zijn een onderdeel van de Communication Devices Class (CDC). Dit is de device class die in dit eindwerk gebruikt wordt.

Framework Voor de communicatie via USB wordt gebruik gemaakt van een voorbeeld firmware die Microchip ter beschikking stelt: USB Device - CDC - Basic Demo. Dit vormt een framework R dat compatibel is met de CDC (Communication Devices Class) driver van Microsoft TM

Windows

, waardoor de USB herkend wordt als een virtuele COM poort. Deze poorten TM

zijn eenvoudig aan te sturen vanuit het .NET

SerialPort class.

De USB Library vereist dat de functie USBDeviceTasks() continu doorlopen wordt. Deze functie onderhoudt de communicatie. De functie mUSBUSARTIsTxTrfReady() geeft een 16



boolean terug die aangeeft of de USBUSART klaar is voor communicatie. Als deze TRUE terugstuurt, wordt de functie getsUSBUSART(USB Out Buffer,64) opgeroepen. Deze functie slaat de ontvangen bytes op in de unsigned char array USB Out Buffer en geeft als resultaat het aantal ontvangen bytes.

3.2.3

Signalen genereren

De zes servo’s hebben elk hun positiesignalen nodig (zie paragraaf 3.1.3). De gebruikte microcontroller (PIC18F4550) heeft slechts twee hardware PWM kanalen ter beschikking. Om deze reden wordt geopteerd voor software PWM’s. Voor de timing van de software PWM wordt gebruik gemaakt van TIMER3, een 16-bit timer. De oscilatorfrequentie van de microcontroller bedraagt 48 MHz, wat betekent dat de instruction cycle frequentie 12 MHz is. Als voor de timer een prescaler1 gebruikt wordt van 65536 · 1/4 ≈ 21,85 ms. Als de timer overloopt 1/4 heeft deze een periode van maximaal 12 MHz van 0xFFFF naar 0x0000 genereert deze een interrupt. Hierbij wordt het ISR (Interrupt Service Routine) PWM ISR() opgeroepen (zie listing 3.1). Daarin wordt eerst een kanaal hoog gezet, waarna de overeenkomstige waarde uit de servo interrupt array in de timer geladen wordt. Als de timer een volgende interrupt genereert, wordt het respectievelijke kanaal terug laag gezet en wordt het volgende kanaal hoog gezet. Tevens wordt ook de timerwaarde aangepast. Op deze manier worden alle pulsen na elkaar gegenereerd (zie figuur 3.10). Een puls kan maximaal 2,5 ms duren. Voor zes servo’s wordt dit maximaal 15 ms. Dit betekent dat er altijd een bepaalde tijd zal resteren om aan de periode van 20 ms te komen. Deze resttijd wordt als laatste in de timer geladen terwijl alle uitgangen laag zijn (case 6). De waarden van de servo interrupt array worden opgevuld in de setPositions() routine (listing 3.2). Deze wordt aangeroepen als er nieuwe positiewaarden ontvangen worden alsook tijdens de initialisatie waarbij de servo’s in hun neutrale positie gebracht worden.

3.2.4

kalibratie

De arrays offset0[], scale[] en direction[] in listing 3.2 bevatten kalibratiegegevens. De kalibratiegegevens worden opgeslagen in het EEPROM, tijdens de initialisatie worden deze arrays opgevuld. In offset0[] zitten de waarden waarmee de neutrale positie gefinetuned kan worden. Met scale[] wordt de maximale uitwijking afgeregeld. De direction[] array bevat de waarde +1 of −1. Hiermee kan ingesteld worden welke richting als positief beschouwd wordt.

In listing 3.2 is count het aantal keer dat de timer moet tellen vooraleer een volgende 1 Een prescaler zorgt ervoor dat de timer niet elke instruction cycle met één verhoogd wordt maar, met bijvoorbeeld een prescaler van 1/4, slechts om de 4 instruction cycles.

17



Listing 3.1: PWM ISR() 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47

void PWM ISR(void) { switch (PWM ISR state) { case 0: SERVO PIN 1 = SERVO AAN; WriteTimer3(servo interrupt[1]); PWM ISR state++; break; case 1: SERVO PIN 1 = SERVO UIT; SERVO PIN 2 = SERVO AAN; WriteTimer3(servo interrupt[2]); PWM ISR state++; break; case 2: SERVO PIN 2 = SERVO UIT; SERVO PIN 3 = SERVO AAN; WriteTimer3(servo interrupt[3]); PWM ISR state++; break; case 3: SERVO PIN 3 = SERVO UIT; SERVO PIN 4 = SERVO AAN; WriteTimer3(servo interrupt[4]); PWM ISR state++; break; case 4: SERVO PIN 4 = SERVO UIT; SERVO PIN 5 = SERVO AAN; WriteTimer3(servo interrupt[5]); PWM ISR state++; break; case 5: SERVO PIN 5 = SERVO UIT; SERVO PIN 6 = SERVO AAN; WriteTimer3(servo interrupt[6]); PWM ISR state++; break; case 6: SERVO PIN 6 = SERVO UIT; WriteTimer3(servo interrupt[0]); PWM ISR state = 0; if(outputDisabled) disableOutput(); break; } }

18



Figuur 3.10: Sequentiële pulsen van de verschillende kanalen

Listing 3.2: setPositions() 1 2 3 4 5 6 7 8 9

10 11 12 13 14 15

void setPositions(int∗ positionPtr) { unsigned int pwmRest = 0xFFFF−PWM 20ms; unsigned char i; for (i=0; i<6; i++) { int position = ∗(positionPtr + i); int neutral = UNCALIBRATED NEUTRAL + offset0[i]; int deflection = direction[i] ∗ ((long)position ∗ ( UNCALIBRATED SCALE + scale[i]) ) / 10000; int count = neutral + deflection; servo interrupt[i+1] = 0xFFFF − (unsigned int)count; pwmRest += count; } servo interrupt[0] = 0xFFFF − pwmRest; }

19



interrupt te geven. Aangezien een interrupt pas gegeven wordt als de timer overloopt, moet de waarde 0xFFFF - count in de timer geladen worden.

3.2.5

Commando’s

De data die doorgestuurd wordt naar de microcontroller wordt voorafgegaan door een commando byte. Volgende commando’s worden geaccepteerd:

0x0A Hiermee wordt aangegeven dat de data servoposities bevat. Deze servoposities zijn per servo een getal van -9000 (−90 ◦ ) tot 9000 (90 ◦ ). Deze worden als 16-bit ints doorgestuurd (2 bytes per int, Little Endian2 ). Na het ontvangen van dit commando wordt setPositions() aangeroepen. Deze functie verwacht een int pointer, dit wordt gedaan door de char pointer die naar het eerste data element wijst expliciet te casten naar een int pointer.

0x0B De data die op dit commando volgt bevat nieuwe offset0 kalibratiewaarden. Wat er doorgestuurd wordt, zijn geen absolute waarden maar wel een getal dat bij de huidige kalibratiewaarde opgeteld wordt. De zes databytes worden ge¨ınterpreteerd als signed chars, één voor elke servo.

0x0C Nieuwe scale kalibratiewaarden, deze kunnen op dezelfde manier ge¨ınterpreteerd worden als de offset0 kalibratiewaarden.

0x0D Nieuwe direction waarden, ook een array van zes chars. Als er een 0x01 gestuurd wordt, zal de huidige richting omgekeerd worden.

0x0F Hierna volgt slechts 1 byte. Bij een 1 worden positiesignalen gegenereerd, bij een 0 niet. Als er geen positiesignalen gegenereerd worden, zullen de servo’s niet meer bekrachtigd worden.

Listing 3.3: Verwerken van de ontvangen pakketten switch ((BYTE)USB Out Buffer[0]) { case 0x0A: // new servo positions if(numBytesRead == 13) setPositions((int∗)(&USB Out Buffer[1])); break; case 0x0B: // change offset−0 if(numBytesRead == 7) changeOffset0(&USB Out Buffer[1]); break;

1 2 3 4 5 6 7 2

De LSB (Least Significant Byte) wordt als eerste doorgestuurd

20


case 0x0C: // change scale if(numBytesRead == 7) changeScales(&USB Out Buffer[1]); break; case 0x0D: // change direction if(numBytesRead == 7) changeDirection(&USB Out Buffer[1]); break; case 0x0F: // enable/disable outputs if(numBytesRead == 2) { // 1 on/off + 1 commando byte if(USB Out Buffer[1] == 1) enableOutput(); if(USB Out Buffer[1] == 0) disableOutput(); } break; default:;

8 9 10 11 12 13 14 15 16 17 18 19 20 21

3.3. Software

}

3.3

Software

Om de USB servo controller te bedienen werd in C# een programma geschreven. Het project servoController bevat de code voor dit programma. De GUI (Graphical User Interface) van dit programma is weergegeven in figuur 3.11. De servo’s kunnen op twee manieren ingesteld worden. De eerste mogelijkheid is het toekennen van een positie aan de afzonderlijk servo’s. Een tweede is het instellen van de drie translaties en drie rotaties. Tussen deze twee modes kan gewisseld worden met de knop Servo positions.

Figuur 3.11: Screenshot van het servocontroller programma Voor het aansturen van de servo’s werd een aparte class library gemaakt (een DLL file). Hierdoor is het mogelijk om op een eenvoudige manier functies in verschillende projecten te gebruiken. De functies werden ge¨ımplementeerd in het MotionPlatformLibrary project, in de gelijknamige namespace. Het sturen van de commando’s over de (virtuele) seriële 21


3.3. Software

poort gebeurt in de ServoDriver klasse. Verder is er ook de Platform klasse. Met deze klasse is het mogelijk de rotaties en translaties van het platform in te stellen:

• void setThetaX(double theta x) • void setThetaY(double theta y) • void setThetaZ(double theta z)

• void setDeltaX(double delta x) • void setDeltaY(double delta y) • void setDeltaZ(double delta z)

Als eerste moet daartoe de seriële poort ingesteld worden waarop het platform aangesloten is. De functie die aangeroepen moet worden is void setComPort(string comPort). In C# is het mogelijk om een lijst van de beschikbare COM poorten op te vragen met de functie: System.IO.Ports.SerialPort.GetPortNames(). De zes meters (figuur 3.11) geven de ingestelde servoposities weer. Deze werden gemaakt met behulp van de aGauge library geschreven door A.J. Bauer [1]. Indien een platformpositie gevraagd wordt die onmogelijk in te stellen is, zullen de wijzernaalden van de servo’s die een imaginair resultaat geven rood gekleurd worden, zoals deze van servo 1 in figuur 3.12.

Figuur 3.12: Imaginair resultaat bij servo 1 Op het tabblad settings is het mogelijk om de servo’s te kalibreren. Een screenshot van deze GUI is terug te vinden in figuur 3.13.

Om de servo’s te kalibreren moet eerst hun nulpositie ingesteld worden. Dit kan met behulp van de Offset 0 regeling. Door op de + en - knoppen te klikken zal de neutrale positie van de respectievelijke servo aangepast worden. Met Adjust multiplyer kan de stapgrootte ingesteld worden. Als de neutrale positie over een grote afstand verplaatst moet worden, wordt best begonnen voor een multiplayer van ×100. Voor fijnregelen kunnen de ×10 en 22


3.3. Software

Figuur 3.13: Screenshot van het servocontroller programma ×1 opties gekozen worden. Als de neutrale positie ingesteld is, moet de 90 ◦ gekalibreerd worden. Dit gebeurt aan de hand van de scale factor. Door op de + en - knoppen te klikken zal de respectievelijke servo zijn nieuwe 90 ◦ positie innemen. De servo is gekalibreerd als dit ook werkelijk met 90 ◦ overeenkomt. Als laatste kan de positieve richting van de servo’s gekozen worden. Door op flip te klikken zal de servo naar +45 ◦ gaan. Indien men de andere richting als positief wenst in te stellen, moet nogmaals op de flip knop gedrukt worden.

23

Hoofdstuk 4

Modellering De modellering omvat twee luiken: een invers-kinematisch model en een dynamisch model. Eerst wordt de kinematica besproken. Het platform neemt een positie in in de ruimte. Met behulp van servo’s kan deze positie gemanipuleerd worden. Hierbij is er nood aan parameters waarmee de positie van het platform eenduidig uitgedrukt kan worden. Vervolgens wordt met deze parameters een transformatie afgeleid waarmee het platform naar de gewenste positie getransformeerd kan worden. Uiteindelijk is het de bedoeling te achterhalen welke servo-uitwijkingen vereist zijn om deze platformpositie te bekomen. Het tweede aspect is een eenvoudig dynamisch model met slechts twee regelingangen: de x- en y-rotaties.

4.1

Positie van het platform

Als eerste wordt een set parameters gekozen waarmee de positie van het platform kan vastgelegd worden. Het platform heeft zes vrijheidsgraden, dus zijn er zes parameters nodig om de positie eenduidig vast te leggen. Deze parameters zullen bestaan uit drie translatieparameters en drie rotatieparameters. Ze zullen later de mogelijke regelingangen vormen, daarom moeten ze strategisch gekozen worden.

4.1.1

Ball and beam

Zoals eerder gesteld is de ball and plate de tweedimensionale uitbreiding van de ball and beam. Het is interessant de parameters in het eendimensionale geval te bekijken om ze dan te kunnen uitbreiden naar het tweedimensionale geval. De ball and beam opstelling ligt volledig in het vlak, de bal heeft één vrijheidsgraad. Een volledige positiebepaling van de beam vereist drie parameters. Bijvoorbeeld de positie van een punt op de beam in het vlak, (x,y) en de helling van de beam, θ, zoals op figuur 4.1. Er wordt een vast punt

24

Hoofdstuk 4. Modellering

4.1. Positie van het platform

Figuur 4.1: Positie van de beam gekozen, dit is het scharnierpunt. De enige overblijvende regelingang is de helling van de balk.

4.1.2

Ball and plate

In drie dimensies zijn rotaties, in tegenstelling tot translaties, niet commutatief. In twee dimensies vormt dit geen probleem omdat er slechts één rotatie voorkomt. In drie dimensies daarentegen worden rotaties in drie delen gesplitst. Eerst een rotatie om de x-as uitvoeren zal de y-as en z-as een nieuwe positie geven. Vervolgens een rotatie uitvoeren rond de nieuwe y-as zal de x-as en de nieuwe z-as herpositioneren. Als ingang voor de regelaar van de ball and plate zullen hoofdzakelijk rotaties om de x- en y-as van belang zijn.

Figuur 4.2: Rotatie om x en y 0 Op figuur 4.2 worden achtereenvolgens een rotatie om de x-as over een hoek α en een rotatie om de nieuwe y-as (y 0 ) over een hoek β uitgevoerd. De originele assen zijn aangeduid in streeplijn. Op de figuur is te zien dat de getransformeerde y-as onder een hoek α staat met het originele xy-vlak. Dit is niet het geval voor de nieuwe x-as. Aangezien de x-as geroteerd werd om de nieuwe y-as is niet β maar wel β 0 de hoek tussen de nieuwe x-as en het xy-vlak. Later zou dit betekenen dat de projectie van de gravitatiekracht op de nieuwe x-as niet g · sin β is, maar wel g · sin β 0 . 25



Om dit tegen te gaan worden de rotatiehoeken van de x-as en de y-as anders gedefinieerd. Beiden worden gekozen als de hoeken, θx en θy , die de nieuwe x- en y-as maken met het originele xy-vlak. Eveneens is op figuur 4.2 te zien dat de nieuwe x-as ook een rotatie ondergaan heeft rond de (niet-getekende) z-as. Dit is onvermijdelijk. Om niet alle z-as rotatie toe te schrijven aan de nieuwe x-as zal extra z-as rotatie ge¨ıntroduceerd worden om de nieuwe x- en y-as evenveel z-draaiing te geven. De transformatiematrix wordt opgebouwd door de eenheidsvectoren die het nieuwe assenstelsel bepalen uit te drukken in het oude assenstelsel. De rotatie van de x-as wordt ge¨ıllustreerd in figuur 4.3. De nieuwe x-as maakt een hoek van θx met de originele xy-vlak. De projectie van de nieuwe x-as op het oude xy-vlak maakt een hoek van θz1 met de oude x-as. Deze vector kan in het oude assenstelsel geschreven worden als: 

cos θx cos θz1



  ~ex =  cos θx sin θz1  sin θx

(4.1)

Figuur 4.3: Rotatie van de x-as Op een analoge manier wordt ~ey bekomen: 

− cos θy sin θz2



  ~ey =  cos θy cos θz2  sin θy

(4.2)

Merk op dat θz1 6= θz2 . Figuur 4.4 toon het bovenaanzicht van een draaiing waarbij θx 6= 0, θy 6= 0 en θz = 0. Het

lijkt alsof ~ex en ~ey van elkaar weg draaien. θz1 en θz2 worden als volgt gedefinieerd: θz1 = θz + ∆θz

(4.3)

θz2 = θz − ∆θz

(4.4)

26



Figuur 4.4: Betekenis van ∆θz , (θx 6= 0, θy 6= 0 en θz = 0) Nu rest er enkel nog ∆θz te bepalen. Om deze te bepalen wordt de voorwaarde gebruikt dat ~ex uit (4.1) en ~ey uit (4.2) orthogonaal moeten zijn, of nog dat hun punt product gelijk is aan nul: ~ex · ~ey = 0 Dit uitwerken levert: ∆θz =

(4.5)

arcsin(tan θx tan θy ) 2

(4.6)

De vector ~ez wordt in een rechtshandig assenstelsel gedefinieerd als [12]: ~ez = ~ex × ~ey

(4.7)

De rotatie van het assenstelsel kan met behulp van matrices als een lineaire transformatie voorgesteld worden. Stel het hoofdassenstelsel {ax , ay , az } in basis A en het geroteerde assenstelsel {bx , by , bz } in basis B, dan geldt T : R3 → R3 , [x]B 7→ [T (x)]A , [T (x)]A = R [x]B

waarin [4]:

R=

h

[T (bx )]A [T (by )]A [T (bz )]A

i

=

h

~ex ~ey ~ez

i

(4.8)

Met andere woorden is de transformatie T een transformatie van punten op het platform (uitgedrukt in een basis B, vastgehecht aan het platform), naar het hoofdassenstelsel of world frame (uitgedrukt in basis A, vastgehecht aan de aarde).

Tot nu toe werden nog geen translaties beschouwd. Aangezien een translatie een afine transformatie is en geen lineaire worden hiervoor homogene coördinaten gebruikt. Hierdoor is het mogelijk om de translatie als een lineaire transformatie en dus als een matrix transformatie voor te stellen. Een driedimensionaal punt heeft vier homogene coördinaten: P =

h

x y z

iT

⇒

h

x y z 1

iT

(4.9)

Met een homogene transforamtiematrix, zoals in 4.10, kan tegelijk een rotatie en een translatie uitgevoerd worden. De matrix R is de 3 × 3 rotatiematrix zoals eerder afgeleid. 27


4.2. Bepalen van de servohoeken

De matrix T is een 3 × 1 kolommatrix met als elementen de translaties in de x-, y- en z-richting.

 M =

Implementatie

 R

T  000 1

(4.10)

Het opstellen van de transformatiematrix werd ge¨ımplementeerd in de

Transformation klasse uit de MotionPlatformLibrary namespace. Dit object vormt een rode draad doorheen het programma van de regelaar. Het wordt zowel gebruikt in het vision gedeelte als bij het berekenen van de servoposities. Deze klasse heeft zeven public properties, de zes parameters die de transformatiematrix bepalen (θx , θy , θz , ∆x, ∆y, ∆z) en de transformatiematrix zelf. De transformatiematrix wordt uitgerekend telkens een van de zes parameter properties aangepast wordt.

4.2

Bepalen van de servohoeken

In deze paragraaf wordt het verband gezocht tussen de positie van het platform en de daarbij horende servohoeken. De afleiding is gebaseerd op een werk van Webjørn Rekdalsbakken [23]. Enkel de inverse kinematica (gegeven de positie van het platform, gevraagd de servohoeken) wordt hier afgeleid. De voorwaartse kinematica van parallelle manipulators is, in tegenstelling tot cascade manipulators, een stelsel gekoppelde niet-lineaire vergelijkingen die bijna niet analytisch op te lossen zijn [7]. De voorwaartse kinematica wordt hier niet besproken. In elke servo wordt een lokaal assenstelsel gedefinieerd zoals voorgesteld in figuur 4.5. De oorsprong van dit assenstelsel is het punt A en heeft coördinaten (xa , ya , za ) in het hoofdassenstelsel. Dit punt bevindt zich in het center van zijn servoas. Het vlak waarin de arm (AB met lengte r) van een servo beweegt bevat de z-as en maakt een hoek θ met de x-as. De hoek van de arm met het xy-vlak, tevens de hoek van de servo ten opzichte van zijn neutrale stand, wordt aangegeven door ϕ. In rust is ϕ = 0◦ . BC is de stang die de servoarm, in B, met het platform, in C, verbindt. Deze stang heeft een lengte `. In de punten B en C bevinden zich kogelgewrichten. Het punt B kan in het hoofdassenstelsel als volgt uitgedrukt worden:    xb = r cos ϕ cos θ + xa yb = r cos ϕ sin θ + ya   zb = r sin ϕ + za

28

(4.11)



Figuur 4.5: Lokaal assenstelsel van een servo Uit figuur 4.5 volgt ook: `2 = (xc − xb )2 + (yc − yb )2 + (zc − zb )2

= xc 2 + yc 2 + zc 2 + xb 2 + yb 2 + zb 2 − 2 (xb xc + yb yc + zb zc )

(4.12)

`1 2 = (xc − xa )2 + (yc − ya )2 + (zc − za )2

= xc 2 + yc 2 + zc 2 + xa 2 + ya 2 + za 2 − 2 (xa xc + ya yc + za zc )

(4.13)

r2 = (xb − xa )2 + (yb − ya )2 + (zb − za )2

= xb 2 + yb 2 + zb 2 + xa 2 + ya 2 + za 2 − 2 (xa xb + ya yb + za zb )

(4.14)

Na substitutie van 4.14 en 4.13 in 4.12 bekomt men: `2 = `1 2 − xa 2 + ya 2 + za 2 + 2 (xa xc + ya yc + za zc ) + 2 r − xa 2 + ya 2 + za 2 + 2 (xa xb + ya yb + za zb ) −

(4.15)

2 (xb xc + yb yc + zb zc )

Substitutie van 4.11 in 4.15 resulteert in: `2 = `1 2 + r2 − 2 (xa 2 + ya 2 + za 2 ) + 2 (xa xc + ya yc + za zc ) +

(4.16)

2 (xa − xc ) (r cos ϕ cos θ + xa ) + 2 (ya − yc ) (r cos ϕ sin θ + ya ) + 2 (za − zc ) (r sin ϕ + za )

29



Afzonderen van de termen in ϕ levert: `2 − `1 2 − r2 = [2 (xa − xc ) r cos θ + xa + 2 (ya − yc ) r sin θ + ya )] cos ϕ+[2 (za − zc ) r] sin ϕ | {z } | {z } | {z } M K L (4.17) Stel nu de volgende uitdrukking voorop: M = A cos(ϕ − Ψ) =A | cos {z Ψ} cos ϕ + A | sin {z Ψ} sin ϕ K L

(4.18)

Uit 4.18 volgt:  L   Ψ = arctan K   2 A = K 2 + L2

(4.19)

4.18 en 4.19 geven samen: cos(ϕ − Ψ) =

M M =√ A K 2 + L2

(4.20)

ϕ is een oplossing in het interval [−90◦ , 90◦ ] van: ϕ = ± arccos

√

M K 2 + L2

+ arctan

L K

+ k π;

∀k ∈ Z

(4.21)

Met 4.21 is nu een uitdrukking bekomen die de servohoek ϕ uitdrukt in functie van de positie van de servo (punt A en de hoek θ) en de positie van het platform (punt C). Vergelijking 4.21 bestaat uit een arcsin en een arctan. Steunend op het feit dat arccos x = α of −α en arctan y = β + kπ bekomt men voor vergelijking 4.21 vier oplossingen:

M L + arctan +π 2 2 K K +L M L + arctan • ϕ2 = − arccos √ +π 2 2 K K +L M L • ϕ3 = arccos √ + arctan 2 2 K K +L M L • ϕ4 = − arccos √ + arctan 2 2 K K +L • ϕ1 = arccos

√

Deze oplossingen worden voorgesteld in figuur 4.6. De gewenste oplossing is ϕ1 . Oplossingen 1 en 2 zijn triviaal, ze volgen uit de symmetrie van de cirkel, ze zijn elkaars 30



spiegelbeeld om de as AC. Het punt C 0 is het gevolg van het gebruik van kwadraten in de vergelijkingen 4.12, 4.13 en 4.14. 3 2 1 0 −1 −2 −3 −2

0

2

Figuur 4.6: Mogelijke oplossingen1 volgens vergelijking 4.21

Implementatie

Deze berekening is ge¨ımplementeerd in de Servo klasse en bevindt zich

in de namespace MotionPlatformLibrary. Voor elke servo moet een object van deze klasse aangemaakt worden. De gegevens van de servo (de punten A en B, de hoek θ, de lengte ` en de straal r) zijn opgeslagen in platformData.xml. Deze XML-file wordt ingelezen bij het aanmaken van een Servo object. Bij het instantiëren van de klasse wordt ook een verwijzing meegegeven naar het Transformation object welke doorheen het programma gebruikt wordt. Na het veranderen van de hoek van het platform in Transformation moet de methode calculatePosition() uit de Servo klasse aangeroepen worden voor elke servo. Deze methode berekent de nieuwe hoekposities van de servo’s, ze worden opgeslagen in de position property.

Maximalisatie van de hellingsgrenzen Met voorgaande afleiding is het mogelijk de servoposities uit te rekenen bij een gewenste positie van het platform. Indien een positie van het platform gevraagd wordt die onmogelijk in te stellen is wegens geometrische beperkingen, zal dit resulteren in een complexe uitkomst van een of meerdere servohoeken. De voornaamste positieparameters van het platform zijn de twee hellingen θx en θy . Deze vormen later de primaire regelingangen voor het ball and plate systeem. Het blijkt dat de parameter ∆z (translatie in de z-richting) een grote invloed heeft op de limieten van deze twee hellingsparameters. Hier wordt de precieze invloed van de parameter ∆z op mogelijk in te stellen platformhoeken θx en θy nagegaan. 1

l = 3, r = 1, θ = π/2, A = (0,0,0), C = (0,2,3)

31



25 20 15 10 delta z = −100mm delta z = −75mm delta z = −50mm delta z = −25mm delta z = 0mm delta z = −46mm

0 −5 −10 −15 −20 −25

−20

−10

0 θx (°)

10

20

Figuur 4.7: Grenzen van θx en θy bij verschillende ∆z

1210 1200

Oppervlak (graden2)

θy (°)

5

1190 1180 1170 1160 1150 1140 −55

−50

−45 ∆ z (mm)

−40

Figuur 4.8: Grenzen van θx en θy bij verschillende ∆z

32

−35


4.3. Dynamisch model

In figuur 4.7 staan op de x- en y-as respectievelijk θx en θy aangeduid. De gebieden geven aan welke platformposities ingesteld kunnen worden bij een gegeven ∆z. De gebieden buiten deze contourlijnen geven een imaginair resultaat en kunnen dus niet bereikt worden. Er kan opgemerkt worden dat voor kleine ∆z (absoluut gezien) de gebieden groter worden naarmate ∆z groter wordt. Voor nog grotere ∆z zal het gebied weer krimpen. Figuur 4.8 geeft het oppervlak van de bereikbare gebieden weer in functie van ∆z. Hierbij wordt een maximum bereikt rond de −46 mm. Dit gebied wordt ook weergegeven als de zwarte lijn

in figuur 4.7. Uit deze figuur blijkt dat het bij deze ∆z mogelijk is om gelijktijdig hoeken tot 13 ◦ in te stellen. Deze hoeken zullen als grenzen van de regelaar genomen worden.

4.3

Dynamisch model

Eerst zal een vereenvoudigd dynamisch model afgeleid worden in continue tijd. Uit dit model zal daarna een discreet model afgeleid worden in analytische vorm.

4.3.1

Continu state space model

De afleiding van het model is gebaseerd op de Euler-Lagrange vergelijking: d ∂L ∂L − =Q dt ∂ q˙ ∂q

(4.22)

In deze vergelijking is L de Lagrangiaan, het verschil tussen kinetische energie T en potentiële energie V . De vector q is de vector met de veralgemeende coördinaten: " q=

x

#

y

(4.23)

Dit zijn de co¨ ordinaten van de bal op het platform, zie figuur 4.9. Voor het model werden volgende veronderstellingen gemaakt: • De bal is ten allen tijde in contact met de plaat. • De bal rolt zonder glijden. • Wrijving wordt verwaarloosd. • Begrenzingen van de hoeken en de grootte van het platform worden niet in rekening gebracht.

33



Figuur 4.9: Schematisch tekening van de ball and plate opstelling De kinetische energie is de som van de translatie van het center van de bal en de rotatie van de bal rond zijn center. De rotatie van de bal ten gevolge van de rotatie van het platform wordt niet in rekening gebracht. Volgende uitdrukking voor de kinetische energie wordt bekomen:

T =

1 1 1 m x˙ 2 + y˙ 2 + J ω˙ x2 + ω˙ y2 = 2 2 2

J m+ 2 x˙ 2 + y˙ 2 r

(4.24)

De potentiële energie bevat enkel de hoogte van de bal in het gravitatieveld ten opzichte van een bepaalde referentie:

V = m g h = m g (x sin θx + y sin θy )

(4.25)

Uitrekenen van de Lagrangiaan en het invullen in de Euler-Lagrange vergelijking in 4.22 levert volgende modelvergelijkingen:  J   ¨ + m g sin θx = 0   m + r2 x  J    m + 2 y¨ + m g sin θy = 0 r

(4.26)

Linearisatie van dit model bestaat erin de sin θ termen te vervangen door θ zelf. Dit is geldig voor voldoende kleine hoeken. Volgend lineair continu state space model wordt bekomen2 :

2

Er wordt aangenomen dat de bal vol is, het massatraagheidsmoment van een volle bal is: J =

34

5 2

m r2



x˙ = A x + B u

 " x=

4.3.2

x

#

y

" u=

θx

#

θy

(4.27)

0 1 0 0



  0 0 0 0 A=  0 0 0 1  0 0 0 0

    

 5 B=− g 7

0 0



  1 0   0 0  0 1

    

Discretisatie van een state space model

In [5] wordt aangetoond dat een continu state space model zoals in 4.27 volgende algemene oplossing heeft:

x(t) = eAt x(0) +

t

Z

eA(t−τ ) B u(t) dτ

(4.28)

0

Hierin wordt de matrix-exponentiële van een vierkante matrix gedefinieerd als:

E

e =

∞ X Ek k=0

k!

(4.29)

Een discreet state space model is van de vorm:

˜ xk + B ˜ uk xk+1 = A

(4.30)

˜ x(k∆T ) + B ˜ u(k∆T ) x ((k + 1)∆T ) = A

(4.31)

Dit is de verkorte schrijfwijze van:

Veronderstel dat u(t) constant blijft tussen k∆T en (k + 1)∆T dan kan 4.28 geschreven worden als:

A ∆T

Z

∆T

xk+1 = e| {z } xk + |0 ˜ A

eA(∆T −τ ) B dτ uk {z } ˜ B

˜ en B ˜ matrices van een discreet state space model. In deze vorm herkent men de A

35

(4.32)


4.3.3


Discreet state space model

Nu gekend is hoe een continu state space model moet omgevormd worden naar een discreet state space model, kan dit toegepast worden op het ball and plate model. De toestandsmatrix wordt: 2 2 ˜ = eA ∆T = I + A ∆T + A ∆T + . . . A | 2 {z } =0

(4.33)

Er blijkt dat A2 = 0. Hieruit volgt dat A3 = A2 A = 0, of algemeen dat Ak = 0 voor k ≥ 2. 

1 ∆T

  0 ˜ A=  0  0

1 0 0

0

0



 0   1 ∆T   0 1 0

(4.34)

De ingangsmatrix wordt:     7 ˜= g B  5   

∆T 2 2 −∆T 0 0

36

0 0 ∆T 2 2 −∆T

        

(4.35)

Hoofdstuk 5

Vision feedback 5.1

Camera model

Camera’s kunnen op verschillende manieren worden gemodelleerd. Welk model ook gebruikt wordt, de camera moet eerst gekalibreerd worden. Via kalibratie worden de modelparameters bepaald. Deze parameters worden onderverdeeld in intrinsieke en extrinsieke parameters. Intrinsieke parameters zijn gerelateerd aan de camera zelf, extrinsieke worden bepaald door de positie van de camera t.o.v. een referentieframe. In de volgende paragraaf wordt een eenvoudig, veel gebruikt model beschreven: het Pin-Hole camera model [20].

5.1.1

Pin-Hole camera model

In het Pin-Hole camera model veronderstelt men dat een punt P in de ruimte wordt geprojecteerd op het afbeeldingsvlak volgens een puntprojectie. Het punt waardoor alle projectielijnen lopen wordt het optical center genoemd. Figuur 5.1 toont een projectie van het punt P in de ruimte naar het punt P 0 in het afbeeldingsvlak. Er worden vier assenstelsels gedefinieeerd: • Het camera frame C. De oorsprong van dit assenstelsel valt samen met het optical

center. Het xy-vlak is evenwijdig met het projectievlak van de afbeelding en de z-as is van de camera weg gericht.

• Het image frame I. Dit is een tweedimensionaal assenstelsel. Het komt overeen met het xy-vlak van het camera frame maar getransleerd volgens de optical axis naar

het afbeeldingsvlak. Dit vlak bevindt zich tussen het optical center en het punt P . In principe moet het vlak zich achter het optical center bevinden, maar om geen omgekeerde afbeelding te bekomen wordt het ervoor geplaatst. De afstand tussen het optical center en het afbeeldingsvlak is de focal length. 37

Hoofdstuk 5. Vision feedback

5.1. Camera model

• Het principal frame UV. Dit frame komt overeen met het image frame, maar is in het afbeeldingsvlak getransleerd naar de linker bovenhoek van de afbeelding. In dit frame worden de co¨ ordinaten als (u,v) geschreven in plaats van (x,y). Dit omdat ze als eenheid pixels hebben, in tegenstelling tot de andere assenstelsels die in meter uitgedrukt worden. • Het world frame. Dit frame is het hoofdassenstelsel. Alle vier de assenstelsels worden afgebeeld in figuur 5.1.

Figuur 5.1: Het Pin-Hole camera model principe en assenstelsels

5.1.2

Camera kalibratie

Om met behulp van camerabeelden positiebepalingen uit te kunnen voeren, moet de camera gekalibreerd worden. In de vorige paragraaf werd gesproken over twee soorten camera parameters: intrinsieke en extrinsieke parameters. De intrinsieke parameters zijn de focal length en het principal point. Deze moeten slechts één keer bepaald worden, ze zijn intrinsiek aan de camera. De extrinsieke parameter is de positie van de camera ten opzichte van het world frame.

Kalibratie van de intrinsieke parameters Voor de kalibratie van de intrinsieke parameters van de camera wordt gebruik gemaakt van de Camera Calibration Toolbox for Matlab, geschreven door Bouguet [11]. 38


5.1. Camera model

Vooreerst moeten er vanuit verschillende hoeken een aantal foto’s van een schaakbordpatroon genomen worden. Een aantal afbeeldingen zijn weergegeven in figuur 5.2. Op alle afbeeldingen worden de hoekpunten van de vakjes op het schaakbord gezocht. Het programma berekent op basis van deze gegevens de intrinsieke camera parameters. In tabel 5.1 staan de resultaten van het verwerken van twintig afbeeldingen genomen met een Logitech Cam Pro 4000. Figuur 5.3 werd ook gegenereerd door het programma, het zijn alle posities van de camera ten opzichte van het schaakbordpatroon. Tabel 5.1: Intrinsieke parameters Focal Length1 Principal point

(797,4; 798,3) ± (7,9; 8,0) pixels (337,0; 241,2) ± (10,5; 9,1) pixels

Extrinsieke parameters Met de Camera Calibration Toolbox for Matlab is het ook mogelijk om de positie van de camera ten opzichte van het world frame te bepalen. Figuur 5.3 illustreert deze functionaliteit. Er is echter voor gekozen om hiervoor gebruik te maken van een eigen kalibratie algoritme in C#, dit omdat ook alle andere software geschreven is in C#. Het zelf geschreven programma is bovendien specifiek voor het motion platform ontworpen. Het kalibratie algoritme berekent de transformatiematrix van het camera frame naar het world frame: TCW . De transformatiematrix wordt opgesteld in drie stappen. Vooreerst wordt verondersteld dat de z-as van het camera frame en de z-as van het world frame evenwijdig zijn, of nog, dat het xy-vlak van het world frame evenwijdig is met het vlak waarop de afbeelding geprojecteerd wordt. De zC -as en de zW -as hebben een tegengestelde zin. Dit wordt uitgedrukt als een rotatie rond de x-as van 180◦ . De eerste rotatiematrix wordt: 

1

0

 R1 =  0 cos π 0 sin π

0





1

0

0



    =  0 −1 0  − cos π 0 0 −1 sin π

(5.1)

Het tweede deel bestaat erin de rotatie van het camera frame rond de z-as te bepalen ten opzichte van het world frame. Hiervoor wordt gebruik gemaakt van markerpunten op het platform. De locaties van de markerpunten in het world frame zijn gekend en er wordt verondersteld dat het platform zich in zijn neutrale horizontale stand bevindt. Door de markerpunten die zich op het platform bevinden aan te duiden op de afbeelding, zijn 1 De focal length wordt in de x- en y-richting berekend. Aangezien deze twee waarden heel dicht bij elkaar liggen, zal voor de eenvoud enkel de gemiddelde waarde van de focal length gebruikt worden.

39


5.1. Camera model

(a) Positie 1 op figuur 5.3

(b) Positie 2 op figuur 5.3

(c) Positie 3 op figuur 5.3

(d) Positie 7 op figuur 5.3

(e) Positie 10 op figuur 5.3

(f) Positie 12 op figuur 5.3

Figuur 5.2: Zes afbeeldingen van een schaakbordpatroon

Extrinsic parameters (world−centered)

11 2

12

1

7

4

6

600 500

3

9

5 10

Zworld

400 8 300 200 0

100

200 0 500

400

300

400 200

100

0

−100

Yworld

Xworld

Figuur 5.3: 3D voorstelling van de camera locatie voor elke foto, gegenereerd door het camera kalibratie algoritme (De afbeeldingen uit figuur 5.2 komen overeen met de respectievelijke cameraposities in deze figuur)

40


5.1. Camera model

de projecties van deze punten op het afbeeldingsvlak ook gekend in het principal frame. Met deze drie koppels kan een transformatie gevonden worden van punten in de afbeelding (u,v)U V naar punten op het platform (x,y,z)W en omgekeerd. Aangezien deze twee vlakken evenwijdig zijn, kan volgende relatie verondersteld worden: (

x = ax u + bx v + cx

(5.2)

y = ay u + by v + cy

Deze vergelijking geldt ook voor de markerpunten. Aangezien de markerpunten in beide assenstelsels gekend zijn, kan volgende uitdrukking geschreven worden:  



u1 v1 1

         

 0 u1 v1 1   u2 v2 1 0 0 0    0 0 0 u2 v2 1   u3 v3 1 0 0 0   0 0 0 u3 v3 1 0

0

0

0

0

     ·    

ax





   ay     bx    =  by     cx    cy

x1



 y1   x2    y2   x3   y3

(5.3)

De co¨ ordinaten van de markerpunten worden aangeduid met indices 1, 2 en 3. Met deze uitdrukking kunnen de parameters ax , bx , cx , ay , by en cy berekend worden en is de transformatie tussen het principal frame en het world frame gekend. Als nu een lijn op het platform gecreëerd wordt en twee punten vanop deze lijn naar het principal frame getransformeerd worden, dan is deze lijn ook gekend in het principal frame. Door nu de hoek α tussen deze twee lijnen te bepalen, is de rotatie rond de z-as gekend. Deze rotatie wordt voorgesteld door volgende transformatiematrix: 

cos α

sin α

0



  R2 =  sin α − cos α 0  0 0 1

(5.4)

Het derde deel van de transformatie van het camera frame naar het world frame is een translatie langs een vector naar het camera frame center (het optical center). De zC -as gaat in het afbeeldingsvlak door het principal point. Door de transformatie 5.2 kan dit punt getransformeerd worden naar het world frame. Hieruit volgen de x- en y-translaties. Als laatste moet de z-positie van het optical center in het world frame bepaald worden. Uit figuur 5.4 kan, aangezien het afbeeldingsvlak en het platform evenwijdig zijn, volgende relatie gevonden worden: r d=f

41

S2 S1

(5.5)


5.1. Camera model

Figuur 5.4: Bepaling van de afstand van het optical center tot het platform Hierin is S1 het oppervlak van de driehoek bepaald door de markerpunten in de afbeelding en S2 het oppervlak van de driehoek bepaald door de markerpunten in het world frame. Het oppervlak van een driehoek met als gegeven de coördinaten van de hoekpunten kan berekend worden met de formule van Heron [26]. De hoogte van het platform, hp , in het world frame is ook gekend. De totale translatie wordt dus voorgesteld door volgende vector: 

∆x



 T =

∆y

 

(5.6)

d + hp De totale transformatiematrix van het camera frame naar het world frame wordt nu: 

TCW

Implementatie

R2 · R1 = 000

 T  1

(5.7)

De kalibratieprocedure wordt uitgevoerd met een apart programma dat

zich bevindt in het Calibration project (zie ook figuur 5.5). Eerst wordt gevraagd om de drie markers op het platform aan te duiden. Let er hierbij op dat er één marker onderaan staat en twee bovenaan. Indien dit niet het geval is moet “flip image” aangevinkt worden (rechts boven, zie figuur 5.5). Na het kalibreren wordt bij wijze van controle onderaan de muispositie omgerekend naar de positie op het platform, uitgedrukt in het world frame.

42


5.2. Hough Transformatie

Figuur 5.5: Kalibratieprogramma na voltooien van de kalibratie Ook wordt het platform frame assenstelsel getekend. De lijnen die de x- en y-richting voorstellen hebben een lengte van 10cm. De transformatiematrix wordt opgeslagen in T c2w.xml.

5.2

Hough Transformatie

De Hough Transformatie is een techniek om kenmerken uit afbeeldingen te halen. Deze techniek wordt gebruikt in image analysis, computer vision en digital image processing. Het doel van deze techniek is om imperfecte instanties van objecten te vinden in een reeks van vormen. Dit gebeurt aan de hand van een voting procedure in een parameterruimte. Het algoritme construeert een accumulatorruimte2 waarin lokale maxima mogelijke object kandidaten voorstellen. De klassieke Hough Transformatie was initieel bedoeld om lijnen uit figuren te halen. Later werd deze uitgebreid om arbitraire vormen, waaronder ook cirkels, te kunnen identificeren. De Hough transformatie welke vandaag de dag universeel gebruikt wordt, werd ontwikkeld door Richard Duda en Peter Hart in 1972, en werd de generalized Hough transform[19] genoemd. [24] Eerst wordt de originele Hough transformatie behandeld, daarna wordt de Hough trans2 Een accumulator is een n-dimensionale array, met n het aantal verschillende parameters in de parameterruimte.

43



formatie om cirkels te detecteren besproken.

5.2.1

Theorie

De basis van de Hough transformatie is dat een lijn niet beschouwd wordt als een verzameling van punten (x1 , y1 ), (x2 , y2 ), . . . maar beschreven wordt door een aantal parameters, zoals bijvoorbeeld hellingsparameter a en snijpuntsparameter b. Een lijn kan voorgesteld worden als y = a x + b. Dit betekent dat elk punt (a,b) in de parameterruimte een lijn voorstelt in de afbeeldingsruimte. Het nadeel van de hellingsparameter en snijpuntsparameter is dat deze onbegrensd zijn voor verticale lijnen. Daarom is het beter een lijn te parametriseren door ρ, de afstand van de rechte tot de oorsprong en θ, de helling van de vector vanuit de oorsprong die de kortste afstand tot de rechte geeft (zie figuur 5.6). Gebruik makende van deze parametrisatie kan een lijn in de afbeeldingsruimte geschreven worden als: y=

−1 ρ x+ tan θ sin θ

(5.8)

Deze vergelijking kan omgevormd worden tot: ρ = x cos θ + y sin θ

(5.9)

Dit betekent dat alle lijnen door een punt (x,y) in de afbeeldingsruimte voorgesteld worden door een sinus (met een bepaalde amplitude en fase) in de parameterruimte.

Figuur 5.6: ρ en θ parameters van een lijn

5.2.2

Het algoritme voor lijndetectie

De Hough transformatie start met een edge detection algoritme. Vervolgens worden alle edgepunten doorlopen. Per edgepunt worden alle lijnen geselecteerd die door dit punt gaan. Dit gebeurt door middel van een accumulator, i.e. een array die de hele parameterruimte omvat en initieel uit nullen bestaat. Voor het parametriseren van een lijn zijn twee parameters vereist: ρ en θ. De accumulator is hier dus een tweedimensionale array. In de vorige paragraaf werd aangetoond dat alle lijnen door een punt in de afbeeldingsruimte een sinus geven in de parameterruimte (zie verglijking 5.9). Alle punten in de accumulator waar deze sinus door gaat worden met één vermeerderd. Dit proces wordt 44



doorlopen voor ieder edgepunt dat gevonden wordt. Als alle edgepunten doorlopen zijn, zullen lijnen waar meerdere edgepuntenen op gelegen zijn een hogere waarde hebben in de accumulator. Dit betekent dat lokale maxima in de accumulator overeenkomen met lijnen in de beeldruimte. Figuur 5.7 geeft een voorbeeld van een figuur met twee lijnen en de resulterende accumulator. Hough Transform −200

ρ

−100 0 100 200 −90

−60

(a) Afbeelding

−30

0 θ

30

60

90

(b) Accumulator

Figuur 5.7: Voorbeeld van een Hough transformatie

5.2.3

Het algoritme voor cirkeldetectie

De Hough transformatie heeft een parametrisatie nodig van de objecten die gezocht worden. Bij een lijn waren dat ρ en θ. Hiermee kunnen alle lijnen in het vlak voorgesteld worden als een punt in de parameterruimte. Een cirkel kan geparametriseerd worden door zijn middelpunt (xc , yc ) en zijn straal r. Dit betekent dat om cirkels te detecteren een driedimensionale parametrruimte nodig is. In de verdere bespreking wordt de straal van de cirkel constant en gekend verondersteld en wordt de parameterruimte zo gereduceerd tot een tweedimensionale ruimte.

(a) Afbeelding

(b) Accumulator

Figuur 5.8: Voorbeeld van een Hough transformatie voor cirkeldetectie

45


5.3. Circle Midpoint Algorithm

Na edge detection wordt voor elk edgepunt de meetkundige plaats geselecteerd van de middelpunten van alle cirkels met straal r die door het beschouwde edgepunt gaan. Deze meetkundige plaats is een cirkel met straal r, met als middelpunt het edgepunt. Opnieuw worden in de accumulator de waarden vermeerderd met één waar deze cirkel doorgaat. Uiteindelijk zal een lokaal maximum in de accumulator overeenkomen met een cirkel in de beeldruimte met straal r. Figuur 5.8 geeft een voorbeeld van een figuur en de resulterende accumulator.

5.2.4

Hough transformatie in OpenCV

De Hough circle transformation is reeds ge¨ımplementeerd in OpenCV als de functie HoughCircles() uit de klasse Image in de Emgu.CV namespace. Deze functie moet toegepast worden op een afbeeldingsobject en heeft volgende parameters: • cannyThreshold: de hoogste drempelwaarde voor de Cannny Edge Detector3 . De

laagste drempelwaarde wordt automatisch als de helft daarvan ingesteld. • accumulatorThreshold: hoe lager deze waarde, des te meer kans op foutieve detectie

(van cirkels die geen cirkels zijn). • dp: resolutiefactor van de accumulator. Indien deze factor 1 is, is de resolutie van

de accumulator dezelfde als de resolutie van de afbeelding. Indien deze factor 2 is,

is de accumulator half zo hoog en half zo breed als de afbeelding. • minDis: de minimale afstand tussen twee waargenomen centers, uitgedrukt in pixels. • minRadius: de minimale straal die een cirkel moet hebben vooraleer ze wordt gedetecteerd. • maxRadius: de maximale straal die een cirkel mag hebben om nog gedetecteerd te worden.

De functie HoughCircles() geeft een array weer van mogelijke cirkels. De elementen van deze array zijn van de klasse Circle. Deze bevat het centerpunt van de cirkel en de straal. De meest significante cirkels (met de grootste waarde in de accumulator) zullen als eerste elementen in de array voorkomen.

5.3

Circle Midpoint Algorithm

In de Hough transformatie voor cirkeldetectie moet in elk edgepunt een cirkel geplaatst worden. Aangezien de accumulator geen discrete ruimte is, moeten de elementen gevonden worden die het best een cirkel benaderen. Hiervoor wordt een algoritme gebruikt uit de computer graphics, het circle midpoint algorithm. 3

Canny is ook een functie in de Image klasse voor edge detection. Deze functie vereist twee drempelwaarden: een die aangeeft hoe duidelijk een rand moet zijn vooraleer ze als rand weergegeven wordt en een die gebruikt wordt voor “edge linking”.

46



Het circle midpoint algorithm is een algoritme waarmee bepaald wordt welke pixels nodig zijn om een cirkel te bekomen. Het algoritme is een variant van het Bresenham’s line algorithm en wordt ook wel eens het Bresenham’s circle algorithm genoemd. Het algoritme is gebaseerd op werken van Pitteway [16] en Van Aken [10]. [25]

5.3.1

Het algoritme

Wegens symmetrie is het enkel nodig de pixels van een octant (een achtste van een cirkel) te bepalen, zie figuur 5.9.

Figuur 5.9: achtvoudige symmetrie, bepalen van één octant is voldoende

In het xy-vlak is de impliciete vergelijking van een cirkel in de oorsprong: r2 = x2 + y 2 . Vertrekkende van deze vergelijking geldt het volgende:

F (x,y) = x2 + y 2 − r2

(5.10)

• F (x,y) = 0: het punt ligt op de cirkel • F (x,y) < 0: het punt ligt in de cirkel • F (x,y) > 0: het punt ligt buiten de cirkel

Figuur 5.10: De volgende pixel, ofwel E ofwel N E. Het algoritme bepaalt het octant vanaf (0, − r) in wijzerzin. Indien de pixel (x,y) reeds

bepaald werd, is de volgende pixel E of N E (zie figuur 5.10). De keuze wordt gebaseerd

op de waarde van d = F (M ) = F (x + 1,y + 1/2). d wordt de decision variable genoemd. 47



• d = F (x + 1,y + 1/2) > 0 → E • d = F (x + 1,y + 1/2) ≤ 0 → N E De waarde van dnew , voor de keuze van de volgende pixel (x + 2,∗), kan op iteratieve wijze bepaald worden: dnew = dold + ∆d. Hierin is ∆d = dnew − dold . Indien als vorige pixel E gekozen werd, geldt: dold = (x + 1)2 + (y + 1/2)2 − R2

dnew = (x + 2)2 + (y + 1/2)2 − R2 ∆d = 2x + 3

(5.11)

Figuur 5.11: Mogelijkheden bij de keuze van E Indien als vorige pixel N E gekozen werd, geldt: dold = (x + 1)2 + (y + 1/2)2 − R2

dnew = (x + 2)2 + (y + 1/2)2 − R2 ∆d = 2x + 2y + 5

Figuur 5.12: Mogelijkheden bij de keuze van N E

Belangrijk is dat ∆d een lineaire functie is van x en y.

48

(5.12)



Nu rest er enkel nog de initiële waarde d0 te bepalen in het punt (0, − r): d0 = F (M0 ) d0 = F (1, − r + 1/2)

d0 = (1)2 + (−r + 1/2)2 − r2 d0 = 5/4 − r

(5.13)

Figuur 5.13: Initiële waarde d0 bepalen.

Dit levert uiteindelijk volgend algoritme:

x = 0; y = −r; d = 5/4 − r; % re¨ eel setPixel(x,y); while (y > x) { if (d < 0) { % E gekozen d += 2∗x + 3; x++; } else { % NE gekozen d += 2∗(x+y) + 5; x++; y++; } setPixel(x,y); }

1 2 3 4 5 6 7 8 9 10 11 12 13 14

5.3.2

Een nieuwe decision variable

Uit vorige paragraaf blijkt dat de pixels die een cirkel vormen bepaald kunnen worden met real number arithmetic. Als we in het vorige algoritme d vervangen door h + 1/4 wordt het volgende algoritme bekomen4 :

4

Merk op dat h > −1/4 kan vervangen worden door h > 0 aangezien h altijd een geheel getal zal zijn.

49


1 2 3 4 5 6 7 8 9 10 11 12 13 14

5.4. Transformaties

x = 0; y = −r; h = 1 − r; % integer setPixel(x,y); while (−y > x) { if (h < 0) { % E gekozen h += 2∗x + 3; x++; } else { % NE gekozen h += 2∗(x+y) + 5; x++; y++; } setPixel(x,y); }

Het voordeel van de nieuwe decision variable is dat deze altijd een geheel getal is i.p.v. een reëel getal, dit verhoogt de snelheid van het algoritme.

5.3.3

Opmerking

Deze methode wordt gebruikt bij de implementatie van de Hough transformatie in Simulink. Bij de implementatie in C# wordt gebruik gemaakt van de Hough transformatie die in de openCV library beschikbaar is. De implementatie van dit algoritme wordt beschreven in het boek Learning OpenCV: computer vision with the OpenCV library door Gary Bradski en Adrian Kaehler [2].

5.4

Transformaties

In paragraaf 5.1.1 werden reeds vier assenstelsels gedefinieerd. Hieraan wordt nog een vijfde assenstelsel toegevoegd, namelijk een tweedimensionaal assenstelsel op het platform. De oorsprong van dit assenstelsel ligt in het midden van het platform en de x- en y-as liggen volgens de x- en y-as van het world frame. Het world frame wordt geplaatst op de bodem van het motion platform, met de z-as naar boven gericht. Deze assenstelsels worden aangeduid op figuur 5.14.

5.4.1

Implementatie

De transformaties tussen de verschillende assenstelsels worden ge¨ımplementeerd in de VisionFeedback klasse uit de namespace MotionPlatformLibrary. Aangezien het principal frame enkel als tussenstap gebruikt wordt, wordt deze weggelaten. De gebruikte transformatie is deze tussen camera frame en image frame. De verschillende functies om 50


5.4. Transformaties

Figuur 5.14: Motion platform met aanduiding van de assenstelsels

51


5.4. Transformaties

de transformaties uit te voeren zijn: • Point camera2world(Point p) • Point world2camera(Point p) • Point camera2uv(Point p) • Point uv2camera(Point p) • Point platform2world(Point p) • Point world2platform(Point p) In de volgende paragrafen worden de transformaties tussen deze verschillende assenstelsels beschreven.

5.4.2

Transformatie tussen camera frame en world frame

De transformatie tussen het camera frame en het world frame is afhankelijk van de positie van het statief ten opzichte van het platform. De transformatiematrix wordt berekend tijdens de kalibratieprocedure die beschreven werd in paragraaf 5.1.2.

5.4.3

Transformatie tussen camera frame en principal frame

Bij deze transformatie wordt aan de tweedimensionale coördinaten van het principal frame een z-coordinaat toegevoegd, gelijk aan de focal length.

5.4.4

Transformatie tussen principal frame en image frame

Het principal frame is uitgedrukt in meter, het image frame in pixels. In principe moet de lengte die overeenstemt met een pixel dus gekend zijn. Echter blijkt dat een arbitrair verband tussen de twee volstaat. Afstanden in het image frame zijn immers enkel afhankelijk van de focal length (zie figuur 5.1). In paragraaf 5.1.2 werd de focus length, die bepaald werd via de kalibratie van de intrinsieke parameters, ook uitgedrukt in pixels. Er wordt aangenomen dat 1 pixel overeen komt met 1mm. De transformatie van het image frame naar het principal frame bestaat uit een translatie van de linker bovenhoek van de afbeelding naar het principal point. Dit punt werd ook bepaald in paragraaf 5.1.2.

52


5.4.5

5.5. Positiebepaling

Transformatie tussen platform frame en world frame

Als het platform in zijn neutrale stand staat (platform parallel met de bodem), dan zijn de x- en y-co¨ ordinaten van het world frame en het platform frame identiek. De positie van het platform wordt bepaald aan de hand van de transformatie uit paragraaf 4.1.2. Om een punt op het platform, uitgedrukt in het world frame, te kennen in het platform frame moet de inverse transformatie toegepast worden. De x- en y-waarden van het getransformeerde punt zijn dan de co¨ ordinaten in het platform frame.

5.5

Positiebepaling

Nu alle assenstelsels gedefinieerd zijn met hun respectievelijke transformaties kann de positiebepaling van de bal (die gevonden werd met de Hough transform) besproken worden. Vooreerst wordt er vanuit gegaan dat de gedetecteerde bal op het platform ligt. Het (tot nu toe onbekende) punt in de ruimte waar de bal zich bevindt, wordt aangeduid met P . Het punt op de afbeelding waar de bal gevonden werd, wordt aangeduid met P 0 . Dit punt kan nu respectievelijk naar het camera frame en het world frame getransformeerd worden.

Figuur 5.15: Positiebepaling van een punt op de afbeelding

53


5.6. Kalman filter

De mogelijke punten P die als gevolg een punt P 0 hebben op de afbeelding liggen allemaal op een rechte. Deze rechte is bepaald door het punt P 0 en het optical center OC (zie figuur 5.15). Het punt P is nu het snijpunt van deze rechte met het platform. Aangezien de positie van het platform zelf gekozen werd, kan dus een vlak bepaald worden.

Implementatie

De positiebepaling van een punt op het platform, gegeven een punt

op de afbeelding, kan uitgevoerd worden met de functie uv2world(Point p uv) in de VisionFeedback klasse uit de MotionPlatformLibrary namespace. De omgekeerde bewerking, world2uv(Point p w), is algemener. Hiermee kan de projectie van elk punt uit de ruimte op het afbeeldingsvlak bepaald worden. Beide functies maken gebruik van geometrie om een positie te bekomen. Om de implementatie te vereenvoudigen worden hiervoor drie klasses geschreven: Point, Line en Plane. Deze klasses bevinden zich in de Geometry namespace.

5.6

Kalman filter

5.6.1

Theorie

De positiemeting van de bal bevat ruis. Deze kan gereduceerd worden door de metingen te filteren. Hiervoor wordt het Kalman filter gebruikt. Het Kalman filter is een recursieve schatter die als doel heeft aan de hand van metingen (die ruis bevatten) en voorspellingen (met een model) een uitkomst te bekomen die dichter bij de werkelijke waarde ligt dan de gemeten waarde. Het filter gaat uit van volgend model: xk+1 = A xk + B uk + wk yk = C xk + vk

(5.14) (5.15)

Hierin zijn w en v respectievelijk de procesruis en de meetruis. Aan de schatter worden volgende vereisten gesteld: • De schatting moet unbiased zijn. Dit wil zeggen dat het gemiddelde van de schatting gelijk moet zijn aan het gemiddelde van de werkelijke waarde.

• De schatting moet zo weinig mogelijk verschillen van de werkelijke waarde. Dit wil zeggen dat niet enkel de gemiddelden gelijk moeten zijn, maar ook dat de variantie van de fout (schatting - werkelijke toestand) minimaal moet zijn. 54


5.6. Kalman filter

Het Kalman filter voldoet aan deze eisen indien de gemiddelde waarde van de ruis gelijk is aan nul. Verder mag er geen correlatie zijn tussen de meetruis en de procesruis. De ruis covariantiematrices worden als volgt gedefinieerd: Procesruis covariantiematrix: Wk = E wk wkT

(5.16)

Meetruis covariantiematrix: Vk = E vk vkT

(5.17)

Hierin is E( · ) de verwachte waarde (Expected value). Het Kalman filter maakt eerst een schatting van de nieuwe toestand x ˆk+1 aan de hand van het discreet lineair model: x ˆ− ˆk + B u ˆk k+1 = A x

(5.18)

Dit is een schatting zonder rekening te houden met de meting gebaseerd op de vorige geschatte toestand (ˆ xk ). Vervolgens wordt een aanpassing uitgevoerd op deze schatting door rekening te houden met het verschil van de meting en de schatting van de meting: ˆk ) x ˆk+1 = x ˆ− k+1 + Kk (yk − C x

(5.19)

Hierin is K de Kalman gain. Deze wordt als volgt bepaald: Kk = A Pk CT Vk + C Pk CT

−1

(5.20)

met P de schattingsfout covariantiematrix: Pk+1 = A Pk AT − A Pk CT (Vk + C Pk C)−1 C Pk PT + Wk

(5.21)

Om deze schatter recursief te kunnen gebruiken moet er een initiële schattingsfout covariantiematrix P0 en een schatting van de beginpositie x ˆ0 gegeven worden.

5.6.2

Toepassen van het filter

Als discreet lineair model wordt gebruik gemaakt van het model zoals uitgewerkt in paragraaf 4.3.3. Als samplefrequentie wordt 20 Hz gekozen (beperking van de camera). De optimale ruis covariantie matrices W en V kunnen bepaald worden aan de hand van metingen. Hier worden ze voor de eenvoud gezien als gewichten in plaats van fysische

55


5.6. Kalman filter

parameters. Proefondervindelijk zijn ze ingesteld op 

0,5

  0 W=  0  0

0

0

0

 "

 0   0,5 0   0 100

100

0

0 0

V=

0,1

0

0

0,1

# (5.22)

Als initiële schattingsfout covariantiematrix en begintoestand worden volgende matrices gekozen: 

100

  0 P0 =   0  0

Implementatie

0 100 0 0

0

0



 0   100 0   0 100 0



0



   0   x0 =   0    0

(5.23)

Het Kalman filter wordt ge¨ımplementeerd in de klasse KalmanFilter

die terug te vinden is in de ControlTheory namespace in het MotionPlatformLibrary project. Om het Kalman filter te gebruiken moet eerst een object van de klasse StateSpace aangemaakt worden (ook uit de ControlTheory namespace). Het Kalman filter moet elke tijdstap upgedated worden. Hierbij zijn twee mogelijkheden: • Indien de bal in de laatste afbeelding gevonden wordt, en dus zijn positie bepaald kon worden, moet de functie update(Matrix u, Vector y) aangeroepen worden. Hierin is u de aangelegde ingang en y de gemeten balpoistie. • Indien in de laatste afbeelding geen bal teruggevonden wordt, zal een schatting ge-

maakt worden enkel op basis van het model. Dit gebeurt met de functie update(Matrix u), waarin u de aangelegde ingang is.

56

Hoofdstuk 6

Ball and plate regelaars Nu het platform aangestuurd kan worden, een dynamisch model afgeleid werd en een meetsysteem ontworpen werd, is het mogelijk om een feedback regelaar te maken voor het ball and plate systeem. De toestand van het systeem wordt bepaald aan de hand van een observer. Vooraf moet nagekeken worden of het systeem wel regelbaar en observeerbaar is. Regelbaar wil zeggen dat het mogelijk is om de volledige toestand van het systeem te manipuleren met de input parameters. Observeerbaar wil zeggen dat het mogelijk is om aan de hand van de metingen de toestand van het systeem te bepalen. Deze twee voorwaarden zijn voldaan indien de regelbaarheidsmatrix (C) en de observeerbaarheidsmatrix (O) volle rang1 hebben.  C=

h

B AB

A2 B

...

An−1 B

i

7 =− g 5

0 0 1 0 0 0 0 0



  1 0 0 0 0 0 0 0   0 0 0 1 0 0 0 0  0 1 0 0 0 0 0 0

    



O=

h

C CA CA2 . . . CAn−1

     iT   =       

1 0 0 0

(6.1)



 0 0 1 0   0 1 0 0    0 0 0 1   0 0 0 0    0 0 0 0   0 0 0 0   0 0 0 0

(6.2)

1 In de lineaire algebra is rang een eigenschap van een stelsel vectoren. De rang van een matrix is gelijk aan het aantal lineair onafhankelijke kolommen, of equivalent, het aantal lineair onafhankelijke rijen van die matrix (rijenrang is gelijk aan kolommenrang). [5]

57

Hoofdstuk 6. Ball and plate regelaars

6.1. Lineair kwadratische regelaar

Beide matrices (6.1) en (6.2) hebben volle rang wat betekent dat het systeem volledig regelbaar en observeerbaar is.

6.1

Lineair kwadratische regelaar

Bij een lineair kwadratische (afgekort LQ, Linear Quadratic) regelaar tracht men de regelkwaliteit kwantitatief te kenmerken door een zogenaamde kostenfunctie. Het regelsysteem wordt dan zo ontworpen dat het kwantitatief kwaliteitskenmerk maximaal wordt (of de kostenfunctie minimaal). Deze regelaar tracht de toestand xk (positie en snelheid van de bal) naar de nul toestand te brengen, i.e. stilliggen in het midden van de plaat. Hiertoe wordt volgende kostenfunctie vooropgesteld:

η=

+∞ X

xTk Qxk + uTk Ruk

(6.3)

k=0

Indien het koppel (A,B) regelbaar is, R > 0, Q ≥ 0, Q = WT W en (A,W) waarneembaar dan is de optimale lineaire statische terugkoppeling gegeven door [5]:

uopt k =−

i −1 T R + BT MB B MA xk {z } | K

h

(6.4)

waarbij M de enig positief definiete oplossing is van:

−1 T M − AT MA + AT MB BT MB + R B MA = Q

(6.5)

Deze vergelijking wordt de algebra¨ısche Riccati-vergelijking genoemd. De oplossing van deze vergelijking is te vinden met de Matlab-functie [K,M,e] = lqr(sys,Q,R). Hierin is sys het discrete state space model, en Q en R de kostenmatrices zoals hierboven beschreven. Het resultaat van deze functie is K de terugkoppel matrix, M de positief definiete oplossing van de algebra¨ısche Riccati-vergelijking en e de closed-loop eigenwaarden2 . Volgende Q en R gewichtenmatrices worden gekozen voor het ball and plate systeem3 : " Q=

0,01

0

0

0,0001

# R=

2

h

1000

i

(6.6)

Dit zijn de eigenwaarden van A − CK De matrices worden slechts weergegeven voor één van de twee richtingen. Dit omdat de x- en y-richting dezelfde dynamica hebben en onafhankelijk zijn (lineaire beschouwing). 3

58


6.2. PD regelaar

Oplossen van de Ricatti-vergelijking in (6.5) naar M met Matlab geeft volgende resultaten:

K=

h

−0,000897 −0,000580

i

"

0,01293 0,00286

M=

#

" e=

0,00286 0,00183

0,8945 + 0,0676j 0,8945 − 0,0676j (6.7)

De simulatie en de resultaten op de werkelijke opstelling, bij het gebruik van deze regelaar, worden weergegeven in figuur 6.1. 300

x (mm)


0 −100

0

1

2

3

4

5

t (s) 20

u (°)


−10 −20

0

1

2

3

4

5

t (s)

Figuur 6.1: Resultaten met een LQ regelaar

6.2

PD regelaar

Enkel een proportionele regeling leidt tot oscillatorisch gedrag. Daarom wordt een PD regelaar gebruikt. Hiermee wordt extra demping verkregen door de afgeleide (Derivative). Een (continue) PD regelaar is van volgende vorm:

u(t) = Kp (xw(t) − x(t)) + Kd

dx(t) dt

(6.8)

Hierin is xw(t) de wenswaarde en x(t) de actuele waarde. Stel nu e(t) = xw(t) − x(t), dan

59

#


6.2. PD regelaar

kan deze vergelijking ook geschreven worden als:

u(t) = Kp e(t) + Kd

de(t) dt

(6.9)

In het Laplace-domein wordt dit: U(s) = Kp + s Kd = K (s − N1 ) E(s)

(6.10)

Het tunen van de parameters Kp en Kd wordt uitgevoerd met behulp van FRtool [18]. FRtool staat voor Frequency Response toolbox. Het is een CAD (Computer Aided Design) tool voor Matlab waarmee op een visuele manier een regelaar getuned kan worden. Om te beginnen worden volgende design parameters opgelegd: • Settling time < 2s • Overshoot < 20% • Robustness: 0,9 Vervolgens moet een lineair model van het systeem ingegeven worden. Hiervoor wordt het dynamisch model gebruikt dat afgeleid werd in paragraaf 4.3. Als laatste moet een terugkoppelregelaar ingegeven worden. Dit gebeurt aan de hand van de polen en nullen van de regelaar. Uit (6.10) blijkt dat een PD regelaar slechts één nulpunt heeft. Het tunen van de regelaar gebeurt door de positie van het nulpunt (N1 ) en de versterkingsfactor (K) te wijzigen. Door deze parameters aan te passen, verandert de closed-loop Nichols Plot (de dunne blauwe lijn op figuur 6.2). Op deze figuur zijn ook de grensgebieden van de overshoot (dikke rode lijn) en de robustness (dikke blauwe lijn) alsook een voorwaarde voor de settling time (rode cirkel) weergegeven. De regelaar moet zo afgesteld worden dat de closed-loop Nichols Plot buiten de dikke rode en blauwe gebieden blijft en de rode cirkel net boven de groene −3 dB curve blijft. Figuur 6.2 werd bekomen bij een versterkingsfactor K = −0,000514 en nulpunt N1 =

−1,459. Uit vergelijking 6.10 volgen volgende parameters:

Kp = −K N1 = −0,750 · 10−3 rad/mm

Kd = K = −0,514 · 10−3 rad s/mm

(6.11) (6.12)

Deze PD regelaar werd ge¨ımplementeerd in Simulink. De simulatie en het werkelijke verloop met de bijhorende control inputs zijn weergegeven in figuur 6.3. Bij de simulatie werd rekening gehouden met de begrenzing van de hoek (−13◦ tot +13◦ ).

60


6.2. PD regelaar

40 30 Overshoot < 20%

Versterking (dB)

20 10

-3 dB

0 -10 Settling time < 2s -20 -30 Robustness: 0.9 -40

-200

-150

-100

-50

0

Fase (°)

Figuur 6.2: Nichols Plot uit FRTool

300

x (mm)


0 −100

0

1

2

3

4

5

t (s) 10

u (°)


−5 −10

0

1

2

3

4

5

t (s)

Figuur 6.3: Resultaten met de werkelijke opstelling (enkel x-richting)

61


6.3

6.3. Conclusie

Conclusie

Een LQ regelaar kan in principe ook gezien worden als een PD regelaar. Bij een PD regelaar wordt de P-actie berekend op de fout: e(t) = wx (t) − x(t)

(6.13)

De D-actie wordt berekend op de afgeleide van de fout: e(t) ˙ = w˙ x (t) − x(t) ˙ = wv (t) − v(t)

(6.14)

De PD terugkoppeling wordt als volgt berekend: u(t) = Kp · e(t) + Kd · e(t) ˙

(6.15)

In matrixvorm komt dit neer op: u(t) =

h

Kp Kd

i

" ·

e(t)

# (6.16)

e(t) ˙

welke dezelfde vorm heeft als in (6.4). Dit betekent dat de elementen uit de terugkoppelmatrix K van de LQ regelaar gezien kunnen worden als de Kp en Kd versterkingen. Tabel 6.1 geeft een overzicht van de versterkingswaarden voor beide regelaars. Tabel 6.1: Overzicht van de versterkingswaarden Kp

rad m

Kd

rad m/s

PD

−0,750

−0,514

LQ

−0,897

−0,580

Uit de tabel blijkt dat de twee verschillende methodes om een terugkoppelregelaar te bekomen bijna gelijke parameters hebben. De resultaten van beide regelaars zullen dus ook vrij gelijklopend zijn.

62


6.4

6.4. Controller programma

Controller programma

6.4.1

Instellingen

Eerst moet de COM-poort waarop het platform aangesloten is geselecteerd worden. Als de COM-poort niet in de lijst voorkomt, kan men op de Refresh knop drukken om de lijst te vernieuwen. Indien nodig moeten de parameters voor de cirkeldetectie aangepast worden. Dit zijn de twee schuifbalken in figuur 6.4. Met de eerste schuifbalk kan de drempelwaarde voor het detecteren van cirkels ingesteld worden. Hoe duidelijker op de afbeelding, hoe hoger deze waarde ingesteld kan worden. Als ze te hoog wordt, zullen geen cirkels meer gedetecteerd kunnen worden. De tweede schuifbalk geeft de drempelwaarde voor het edge detection algoritme aan. Door het vinkje onder de schuifbalk aan te vinken wordt het resultaat van de edge detection weergegeven (zie figuur 6.5). Hoe hoger deze waarde, hoe minder edges gevonden zullen worden. Deze waarde wordt best zo hoog mogelijk ingesteld aangezien hoe minder edgepunten er gedetecteerd worden, hoe minder mogelijke cirkels in de accumulator geplaatst worden en hoe minder rekenkracht het algoritme vraagt. Met de derde schuifbalk wordt de diameter van de bal ingesteld. Deze moet ingesteld worden zodat de groene cirkel (of de grijze cirkel in edge detection mode) even groot is als de bal. De Hough transformatie zoekt naar cirkels die de ingestelde diameter hebben met een marge van ±5 pixels. Bij het opstarten van het programma wordt op de video output het platform assenstelsel getekend (de twee gele lijnen, beiden gaan ze van −200 mm tot 200 mm). Eerst moet er

voor gezorgd worden dat de marker die op de y-as ligt zich onderaan bevindt. Als dit niet het geval is, moet de afbeelding omgedraaid worden. Dit gebeurd door Flip vertical aan te vinken. Als het assenstelsel nog steeds niet klopt, moet een nieuwe kalibratie uitgevoerd worden.

6.4.2

Initialisatie

Bij het opstarten van het programma wordt eerst een Transformation object gemaakt (zie paragraaf 4.2). Dit object verzorgt de berekeningen met betrekking tot de rotaties en translaties van het platform. Vervolgens wordt een object van de klasse Platform gemaakt. Aan dit object wordt een verwijzing naar het Transformation object meegegeven. Met de klasse Platform kan het motion platform bediend worden. Deze klasse communiceert met de 6 kanaals USB servocontroller die de servo’s aanstuurt. 63



Figuur 6.4: GUI van de regelaar

Figuur 6.5: Optie edge detection aangevinkt

64



Nu wordt een nieuw Capture object aangemaakt. Dit is ook een klasse uit OpenCV. Met dit object is het mogelijk om beelden van de webcam op te vragen. Het formaat van de webcam wordt ingesteld op 480 px × 640 px. Als er een fout optreedt tijdens het aanmaken van dit object wordt een foutbericht weergegeven.

Na het opstarten van de camera wordt een VideoFeedback object aangemaakt. Als argumenten wordt een verwijzing naar het Transformation object meegegeven, alsook de discrete tijdstap deltaT. Vervolgens wordt een nieuwe thread opgestart die continu een beeld opvraagt uit de webcam, het beeld verwerkt (cirkeldetectie, positiebepaling, markers en assenstelsel tekenen,. . . ) en het uiteindelijk weergeeft in de image box op de GUI. Daarna wordt een timer aangemaakt. Met name een speciale multimedia timer met hoge precisie. In eerste instantie werd de Timer klasse uit de System.Windows.Forms namespace gebruikt. Deze timer bleek slechts een nauwkeurigheid van 55 ms [15] te hebben, wat onvoldoende is indien samplefrequenties van 20 Hz tot 30 Hz bekomen willen worden. Een klasse van timers die de grootste nauwkeurigheid biedt zijn de een multimedia timers. De Win32 multimedia timer maakt geen deel uit van het .NET framework. Leslie Sanford heeft in [21] een .NET wrapper gemaakt voor dit type timers. Aan deze timers moet een periode meegegeven worden. Wanneer een periode voorbij is, wordt het Tick event aangeroepen. Hier is de handler voor deze functie mmTimer Tick(). Deze functie wordt verder besproken. Na het aanmaken van de timer wordt de grafiek ge¨ınitialiseerd. De grafiek wordt gemaakt aan de hand van het zedGraph project, geschreven door JChampion [9]. Eerst worden de assen benoemd. Daarna wordt de schaal ingesteld en de naam van de verschillende curves meegegeven. Als de regelaar in werking treedt, worden de gemeten en de gefilterde x-waarden weergegeven op de grafiek. Vervolgens wordt een video object gemaakt. De klasse VideoWriter is ook een onderdeel van OpenCV. Met deze klasse is het mogelijk om de beelden die gemaakt werden tijdens de werking van de regelaar op te slaan in het bestand Output.wmv. Opgelet, dit bestand wordt overschreven telkens het programma afgesloten wordt. Als laatste wordt een CSV (Comma Separated Value) file aangemaakt. In dit bestand worden alle meetgegevens (de tijd, de gewenste posities, de gemeten posities, de gefilterde posities, de snelheden volgens het Kalmanfilter en platform ingangen) van de laatst sessie opgeslagen. Tijdens het afsluiten van het programma wordt de data weggeschreven in Data.csv. Het Matlab programma plots.m kan deze data verwerken. De metingen worden dan in zes overzichtelijke grafieken weergegeven, zoals bijvoorbeeld in figuur 6.6.

65


6.4. Controller programma 200 y position (mm)

x position (mm)

200 0 −200 −400

0

20

40 t (s)

60

100 0 −100 −200

80

0

−0.5

0

20

40 t (s)

60

60

80

0

20

40 t (s)

60

80

0

20

40 t (s)

60

80

20 y rotation (°)

x rotaton (°)

40 t (s)

0

−0.5

80

20 10 0 −10 −20

20

0.5 y speed (m/s)

x speed (m/s)

0.5

0

0

20

40 t (s)

60

80

10 0 −10 −20

Figuur 6.6: Het resultaat van plots.m

6.4.3

Programma flow

Tijdens de initialisatie wordt een nieuwe thread opgestart die continu de beelden van de webcam verwerkt. In deze thread loopt de functie frameProcessingLoop(). Dit is een while loop. Per iteratie wordt eerst een camerabeeld opgehaald. Dit camerabeeld wordt met de functie processFrame() uit de klasse VisionFeedback verwerkt. Deze functie levert de balpositie. Als laatste wordt de functie setImage() aangeroepen. Deze plaatst het assenstelsel op de afbeelding, alsook de markers van de gedetecteerde balpositie (groen), de gefilterde positie (blauw) en de gewenste positie (rood). Deze afbeelding wordt dan in de image box op de GUI weergegeven. Naast deze thread loopt ook nog de timer. Elke keer een periode voorbij is, wordt de functie mmTimer Tick() aangeroepen. Eerst wordt de gewenste positie berekend met de functie getPath(), welke verder besproken wordt. Daarna wordt een update uitgevoerd van het Kalman filter met de laatst bepaalde positie. Vervolgens worden de gewenste positie en de gefilterde positie aan de PID regelaar gegeven. Deze berekent een platformpositie in 66



radialen voor de x- en de y-richting. De hoeken worden begrensd tussen −10 ◦ en 10 ◦ en dan naar het platform gestuurd. Als laatste wordt de data van de huidige tijdstap naar de CSV file weggeschreven en wordt ze ook aan de grafiek op de GUI toegevoegd.

6.4.4

Path generator

Voor demonstratiedoeleinden is het mogelijk de bal een bepaald pad te laten volgen. Hiervoor werd een functie ge¨ımplementeerd die een setpoint genereert in functie van de tijd. Op figuur 6.5 is te zien dat er vier mogelijke setpoints kunnen ingesteld worden: • • • •

Center: middelpunt van het platform. Circle: cirkel met een bepaalde straal en omtreksfrequentie. Square: de vier hoekpunten van een vierkant met een bepaalde tussentijd Input: te bepalen door te klikken op het platform of door x- en y-posities in te geven.

De functie getPath() geeft de wenswaarden terug die aan de regelaar gegeven worden. Als argument verwacht deze functie de huidige tijd. Door middel van de radioknoppen op de GUI wordt in een switch statement uitgemaakt welk type setpoint geselecteerd wordt.

67

Hoofdstuk 7

Toekomstig werk De grote bouwstenen van het ball and plate systeem zijn na het afronden van dit eindwerk aanwezig, doch deze kunnen zeker nog verfijnd worden. In dit hoofdstuk worden daartoe enkele voorstellen geformuleerd. Uiteraard is deze opsomming niet-limitatief.

7.1

Vision algoritme

Het vision algoritme dat uitgewerkt werd in hoofdstuk 5 gaat uit van de huidige positie van het platform om op die manier de locatie van de bal in het world frame te bepalen. De platform positie die hiervoor gebruikt wordt is niet de werkelijke platformpositie maar de gevraagde platformpositie. Wanneer een grote verandering van de platformpositie gevraagd wordt, gaat het vision algoritme er vanuit dat het platform zich onmiddellijk in de gewenste positie bevindt. In werkelijkheid heeft het platform een eindige tijd nodig om van de ene positie naar de andere over te gaan. Dit veroorzaakt ruis op de metingen van de balpositie. Hieronder worden twee mogelijke oplossingen voorgesteld die een antwoord kunnen bieden aan dit probleem. De eerste oplossing maakt gebruik van de forward kinematics en de werkelijke servoposities. De tweede oplossing maakt gebruik van sensoren om de werkelijke platformpositie te meten.

7.1.1

Eerste oplossing: Forward kinematics

Forward kinematics zouden uitgewerkt kunnen worden. Dit betekent dat de platformpositie bepaald moet kunnen worden wanneer de zes servoposities gegevens zijn. Het forward kinematics probleem bestaat uit het oplossen van enkele hoog niet-lineaire vergelijkingen. In [14] wordt vermeld dat de volledige oplossing van een forward kine68

Hoofdstuk 7. Toekomstig werk

7.2. Werkelijke afmetingen bepalen

matics probleem uit 40 complexe oplossingen kan bestaan. Deze vergelijkingen zijn niet algebra¨ısch op te lossen waardoor een numerieke methode gehanteerd zal moeten worden. In [13] wordt een robuuste niet-lineaire solver voorgesteld voor het oplossen van het forward kinematics probleem. Een andere manier voor het oplossen van het probleem wordt voorgesteld in [22]. Hierin wordt gebruik gemaakt van neurale netwerken. Bij het gebruik van bestaande forward kinematic solvers dient in gedachten gehouden te worden dat deze meestal ontworpen zijn voor een Steward platform. Dit type platform maakt gebruik van lineaire actuatoren, terwijl in deze thesis servo’s gebruikt werden. In principe kan dit ondervangen worden door de lengte van de lineaire actuator als constant te veronderstellen en zijn aanknopingspunten aan de bodem als variabel te gebruiken. Dit komt dan overeen met het punt B in figuur 4.5. Het tweede deel van de oplossing ligt in het bepalen van de actuele servoposities. Hiervoor kan een servomodel gebruikt worden waarbij de servo-overgangen geschat worden. Een andere mogelijkheid ligt in het uitlezen van de potentiometers die zich in de servo’s bevinden. Met deze informatie kan de werkelijke servopositie bepaald worden.

7.1.2

Tweede oplossing: Sensoren

Een tweede oplossing bestaat erin gebruik te maken van sensoren die de positie van het platform bepalen. Sensoren die hiervoor gebruikt kunnen worden zijn gyroscopen en/of accelerometers. Twee gyroscopen die enkel de hoekverdraaiing in de x- en y-richting meten zullen niet volstaan. Een full 6 DoF meetsysteem is nodig omdat de tussenliggende posities niet enkel met deze twee rotaties uitgedrukt kunnen worden. Er komen ook translaties en een eventuele z-draaiing aan te pas.

7.2

Werkelijke afmetingen bepalen

Doorheen de verschillende onderdelen van het programma worden diverse geometrische eigenschappen van het platform gebruikt zoals de posities van de servo’s op het platform, de positie van de kogelgewrichten,. . . Deze waarden zijn allemaal opgeslagen in een XML-file (platformData.xml, zie ook appendix C). De afmetingen die hierin staan zijn gebaseerd op de afmetingen van het SolidWorks model. In werkelijkheid kunnen deze waarden afwijken van de theoretische waarden. Een probleem dat ten gevolge van deze afwijkingen ontstaat, is dat de gewenste positie van het platform niet de werkelijke positie zal zijn. Een gevolg hiervan werd tijdens testen met de PD regelaar waargenomen. De bal ging niet naar het gewenste positie maar naar een evenwichtspunt dat op een bepaalde afstand van het gewenste punt lag.

69

Hoofdstuk 7. Toekomstig werk

7.3. Regelaar

De bal kan enkel stil blijven liggen indien het platform horizontaal staat. In principe zijn de ingangen dan beiden 0 ◦ . Maar indien er 0 ◦ gevraagd wordt en het platform niet perfect horizontaal staat, zal de bal beginnen rollen. De P-actie van de regelaar zal dit tegenwerken door een hoek verschillend van 0 ◦ te sturen. Dit gebeurt tot de bal stil blijft liggen. De regelaar moet dus een van 0 ◦ verschillende uitgang sturen om een steady state te bekomen. Dit betekent dat de P-regelaar een van 0 verschillend error signaal moet krijgen en dus dat steady state niet overeenkomt met de gewenste positie. Stel dat het platform 1 ◦ =

π 180

rad fout zou staan en Kp = −0,00075. Om het platform in

zijn horizontale positie te krijgen zal een fout van 23 mm nodig zijn.

Een oplossing hiervoor kan het meten van de werkelijke geometrische eigenschappen van het platform zijn. Een andere oplossing kan geboden worden door gebruik te maken van sensoren die de positie van het platform meten. Met deze metingen kunnen, door gebruik te maken van een terugkoppeling, de fouten weggewerkt worden. Momenteel wordt een correctie gemaakt op de gewenste positie van het platform door een constante hoek op te tellen bij de uitgang van de PD regelaar. Dit is geen optimale oplossing aangezien dit enkel juist is wanneer het platform zich in een horizontale positie bevindt.

7.3

Regelaar

In het hoofdstuk 6 werden twee eenvoudige regelaars beschreven: een PD regelaar en een Lineair Kwadratische regelaar. Deze maken slechts gebruikt van twee ingangen: de xen y-rotatie van het platform. Echter is het een 6 DoF motion platform en misschien is het mogelijk om de overige ingangen ook te gebruiken, om op die manier performnatere regelaars te bekomen.

7.4

Automatisatie van kalibratie en vision-parameters

Tijdens het kalibreren van de extrinsieke parameters (zie paragraaf 5.1.2) moet handmatig gecontroleerd worden of het beeld niet omgedraaid moet worden. Eveneens moeten de drie markers handmatig aangeduid worden. Dit proces kan eventueel geautomatiseerd worden. Ook de parameters voor de vision-feedback (Edge detection threshold, accumulator threshold en de bal diameter) moeten momenteel handmatig ingesteld worden. Het tunen van deze parameters kan eveneens geautomatiseerd worden.

70

Bijlage A

Constructietekeningen

71

96

30 18,034

66

5

R2

25

85

74,930

65

10

6 M

UNLESS OTHERWISE SPECIFIED: DIMENSIONS ARE IN MILLIMETERS SURFACE FINISH: TOLERANCES: LINEAR: ANGULAR: NAME DRAWN Michiel De Paepe

DEBUR AND BREAK SHARP EDGES

FINISH:

SIGNATURE

DATE

DO NOT SCALE DRAWING

REVISION

TITLE:

23/05/2011

CHK'D APPV'D MFG Q.A

MATERIAL:

WEIGHT:

DWG NO.

SCALE:1:1

servobevestiging SHEET 1 OF 1

A4

60

20 50 40

A

1

20

9

R1

5 A

5 SECTION A-A

20 30 85

UNLESS OTHERWISE SPECIFIED: DIMENSIONS ARE IN MILLIMETERS SURFACE FINISH: TOLERANCES: LINEAR: ANGULAR: NAME


FINISH:

SIGNATURE

DATE


REVISION

TITLE:

DRAWN CHK'D APPV'D MFG Q.A

MATERIAL:

WEIGHT:

DWG NO.

SCALE:1:1

excentriek SHEET 1 OF 1

A4

575

35

4,5

R24 5,6 2

4

5

245



FINISH:

SIGNATURE

DATE


REVISION

TITLE:


MATERIAL:

WEIGHT:

DWG NO.

SCALE:1:10

platform SHEET 1 OF 1

A4

25 15

55 35 15

M5

10

M 4

35



FINISH:

SIGNATURE

DATE


REVISION

TITLE:


MATERIAL:

WEIGHT:

DWG NO.

SCALE:1:1

bevestigingstab SHEET 1 OF 1

A4

Bijlage B

6 kanaals servo USB controller

76

Hoofdstuk B. 6 kanaals servo USB controller

Figuur B.1: Elektrisch schema van de 6 kanaals servo USB controller

77

Hoofdstuk B. 6 kanaals servo USB controller

epeaP eD leihciM

mroftalP noitoM FoD6

Figuur B.2: Printplaat van de 6 kanaals servo USB controller

78

Bijlage C

platformData.xml

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34

<servo id="0"> <joint x="−7.5" y="−230.98" z="404.88" unit="mm" /> <servo id="1"> <joint x="−204.04" y="108.56" z="404.88" unit="mm" /> <servo id="2"> <joint x="−196.30" y="121.57" z="404.88" unit="mm" /> <servo id="3"> <joint x="196.30" y="121.57" z="404.88" unit="mm" /> <servo id="4">

79

Hoofdstuk C. platformData.xml

35 36 37 38 39 40 41 42 43 44 45

<joint x="204.04" y="108.56" z="404.88" unit="mm" /> <servo id="5"> <joint x="7.5" y="−230.98" z="404.88" unit="mm" />

46 47 48 49

<marker id="0" x="0" y="−244.96" z="415" unit="mm" /> <marker id="1" x="212.44" y="122.04" z="415" unit="mm" /> <marker id="2" x="−212.44" y="122.04" z="415" unit="mm" />

50 51 52

415

80

Bijlage D

Datasheets

81

BALL & SOCKET JOINTS Manufactured to DIN 71802, these ball & socket joints feature induction hardened balls to resist wear. Security clips retain the threaded ball studs in their sockets and spanner flats are provided on the stud. Spanner flats can be provided on the sockets to order. Available in mild steel - zinc plated and clear passivated or in stainless steel, from stock. Maximum static push/pull values are valid for both radial and axial loads but do not apply to stainless steel versions. Supplied with appropriate lock nut.

4 Ball Ø Thread d1 d2

Material

Thread

Size

373.05.C 373.06.C 373.08.C 373.10.C 373.12.C 374.05.C 374.06.C 374.08.C 374.10.C 374.12.C

Mild Steel Mild Steel Mild Steel Mild Steel Mild Steel Mild Steel Mild Steel

RH RH RH RH RH LH LH

5 6 8 10 12 5 6

M5 M6 M8 M10 M12 M5 M6

8

28.5

22

10.5

25.0

10.2

9

6.4

7

30

10 13 16 16 8 10

10 13 16 16 8 10

32.5 39.5 47.0 47.0 28.5 32.5

25 30 35 35 22 25

11.5 14.0 15.5 15.5 10.5 11.5

30.0 38.0 47.5 47.5 25.0 30.0

12.5 16.5 20.0 20.0 10.2 12.5

11 13 16 16 9 11

7.5 9.7 12.0 12.0 6.4 7.5

8 11 13 13 7 8

70 150 200 200 30 70

Mild Steel Mild Steel Mild Steel

LH LH LH

8 10 12

13 16 16

M8 M10 M12

13 16 16

39.5 47.0 47.0

30 35 35

14.0 15.5 15.5

38.0 47.5 47.5

16.5 20.0 20.0

13 16 16

9.7 12.0 12.0

11 13 13

150 200 200

Stainless Steel Stainless Steel Stainless Steel Stainless Steel Stainless Steel Stainless Steel Stainless Steel Stainless Steel Stainless Steel Stainless Steel

RH RH RH RH RH LH LH LH LH LH

5 6 8 10 12 5 6 8 10 12

8 10 13 16 16 8 10 13 16 16

M5 M6 M8 M10 M12 M5 M6 M8 M10 M12

8 10 13 16 16 8 10 13 16 16

28.5 32.5 39.5 47.0 47.0 28.5 32.5 39.5 47.0 47.0

22 25 30 35 35 22 25 30 35 35

10.5 11.5 14.0 15.5 15.5 10.5 11.5 14.0 15.5 15.5

25.0 30.0 38.0 47.5 47.5 25.0 30.0 38.0 47.5 47.5

10.2 12.5 16.5 20.0 20.0 10.2 12.5 16.5 20.0 20.0

9 11 13 16 16 9 11 13 16 16

6.4 7.5 9.7 12.0 12.0 6.4 7.5 9.7 12.0 12.0

– – – – – – – – – –

– – – – – – – – – –

373.05.C.S 373.06.C.S 373.08.C.S 373.10.C.S 373.12.C.S 374.05.C.S 374.06.C.S 374.08.C.S 374.10.C.S 374.12.C.S

8

D4

L1

L2

L3

L4

L5

L6

Spanner Max static flats A/F push/pull (kp)

Order Code

R

L1 R

L2 Working angles d2

d1 L3

D4 NOM L4

L6

L5

d2

Fax: +44 (0)1992 509890

49 ®

PIC18F2455/2550/4455/4550 28/40/44-Pin High-Performance, Enhanced Flash USB Microcontrollers with nanoWatt Technology Universal Serial Bus Features:

Peripheral Highlights:

• USB V2.0 Compliant • Low Speed (1.5 Mb/s) and Full Speed (12 Mb/s) • Supports Control, Interrupt, Isochronous and Bulk Transfers • Supports up to 32 endpoints (16 bidirectional) • 1-Kbyte dual access RAM for USB • On-chip USB transceiver with on-chip voltage regulator • Interface for off-chip USB transceiver • Streaming Parallel Port (SPP) for USB streaming transfers (40/44-pin devices only)

• • • •

•

Power-Managed Modes:

•

Run: CPU on, peripherals on Idle: CPU off, peripherals on Sleep: CPU off, peripherals off Idle mode currents down to 5.8 µA typical Sleep mode currents down to 0.1 µA typical Timer1 oscillator: 1.1 µA typical, 32 kHz, 2V Watchdog Timer: 2.1 µA typical Two-Speed Oscillator Start-up

•

• •

Special Microcontroller Features:

Flexible Oscillator Structure:

Program Memory Device

PIC18F2455 PIC18F2550 PIC18F4455 PIC18F4550

12288 16384 12288 16384

 2004 Microchip Technology Inc.

Master I2C™

Timers 8/16-bit

Y Y Y Y

Y Y Y Y

1 1 1 1

2 2 2 2

1/3 1/3 1/3 1/3

Data Memory

Flash # Single-Word SRAM EEPROM (bytes) Instructions (bytes) (bytes) 24K 32K 24K 32K

SPI™

Comparators

• C compiler optimized architecture with optional extended instruction set • 100,000 erase/write cycle Enhanced Flash program memory typical • 1,000,000 erase/write cycle Data EEPROM memory typical • Flash/Data EEPROM Retention: > 40 years • Self-programmable under software control • Priority levels for interrupts • 8 x 8 Single-Cycle Hardware Multiplier • Extended Watchdog Timer (WDT): - Programmable period from 41 ms to 131s • Programmable Code Protection • Single-Supply 5V In-Circuit Serial Programming™ (ICSP™) via two pins • In-Circuit Debug (ICD) via two pins • Optional dedicated ICD/ICSP port (44-pin devices only) • Wide operating voltage range (2.0V to 5.5V)

• Four Crystal modes including High Precision PLL for USB • Two External Clock modes, up to 48 MHz • Internal oscillator block: - 8 user-selectable frequencies, from 31 kHz to 8 MHz - User-tunable to compensate for frequency drift • Secondary oscillator using Timer1 @ 32 kHz • Dual oscillator options allow microcontroller and USB module to run at different clock speeds • Fail-Safe Clock Monitor - Allows for safe shutdown if any clock stops

EAUSART

• • • • • • • •

High-current sink/source 25 mA/25 mA Three external interrupts Four Timer modules (Timer0 to Timer3) Up to 2 Capture/Compare/PWM (CCP) modules: - Capture is 16-bit, max. resolution 6.25 ns (TCY/16) - Compare is 16-bit, max. resolution 100 ns (TCY) - PWM output: PWM resolution is 1 to 10-bit Enhanced Capture/Compare/PWM (ECCP) module: - Multiple output modes - Selectable polarity - Programmable dead time - Auto-Shutdown and Auto-Restart Enhanced USART module: - LIN bus support Master Synchronous Serial Port (MSSP) module supporting 3-wire SPI™ (all 4 modes) and I2C™ Master and Slave modes 10-bit, up to 13-channels Analog-to-Digital Converter module (A/D) with programmable acquisition time Dual analog comparators with input multiplexing

2048 2048 2048 2048

256 256 256 256

MSSP I/O

24 24 35 35

10-bit CCP/ECCP A/D (ch) (PWM) 10 10 13 13

Preliminary

2/0 2/0 1/1 1/1

SPP

No No Yes Yes

DS39632B-page 1

PIC18F2455/2550/4455/4550 Pin Diagrams

28-Pin PDIP, SOIC

1 2 3 4 5 6 7 8 9 10 11 12 13 14

PIC18F2455 PIC18F2550

MCLR/VPP/RE3 RA0/AN0 RA1/AN1 RA2/AN2/VREF-/CVREF RA3/AN3/VREF+ RA4/T0CKI/C1OUT/RCV RA5/AN4/SS/HLVDIN/C2OUT VSS OSC1/CLKI OSC2/CLKO/RA6 RC0/T1OSO/T13CKI RC1/T1OSI/CCP2(1)/UOE RC2/CCP1 VUSB

28 27 26 25 24 23 22 21 20 19 18 17 16 15

RB7/KBI3/PGD RB6/KBI2/PGC RB5/KBI1/PGM RB4/AN11/KBI0 RB3/AN9/CCP2(1)/VPO RB2/AN8/INT2/VMO RB1/AN10/INT1/SCK/SCL RB0/AN12/INT0/FLT0/SDI/SDA VDD VSS RC7/RX/DT/SDO RC6/TX/CK RC5/D+/VP RC4/D-/VM

MCLR/VPP/RE3 RA0/AN0 RA1/AN1 RA2/AN2/VREF-/CVREF RA3/AN3/VREF+ RA4/T0CKI/C1OUT/RCV RA5/AN4/SS/HLVDIN/C2OUT RE0/AN5/CK1SPP RE1/AN6/CK2SPP RE2/AN7/OESPP VDD VSS OSC1/CLKI OSC2/CLKO/RA6 RC0/T1OSO/T13CKI RC1/T1OSI/CCP2(1)/UOE RC2/CCP1/P1A VUSB RD0/SPP0 RD1/SPP1

Note 1:

DS39632B-page 2

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20

PIC18F4455 PIC18F4550

40-Pin PDIP

40 39 38 37 36 35 34 33 32 31 30 29 28 27 26 25 24 23 22 21

RB7/KBI3/PGD RB6/KBI2/PGC RB5/KBI1/PGM RB4/AN11/KBI0/CSSPP RB3/AN9/CCP2(1)/VPO RB2/AN8/INT2/VMO RB1/AN10/INT1/SCK/SCL RB0/AN12/INT0/FLT0/SDI/SDA VDD VSS RD7/SPP7/P1D RD6/SPP6/P1C RD5/SPP5/P1B RD4/SPP4 RC7/RX/DT/SDO RC6/TX/CK RC5/D+/VP RC4/D-/VM RD3/SPP3 RD2/SPP2

RB3 is the alternate pin for CCP2 multiplexing.

Preliminary

 2004 Microchip Technology Inc.

Referenties [1] Bauer A.J. A fast and performing gauge. Online, geraadpleegd op 18 november 2010. (Geciteerd op pagina 22.) [2] Kaehler A. Bradski G. Learning OpenCV: computer vision with the OpenCV library. O’Reilly Series. O’Reilly, 2008. (Geciteerd op pagina 50.) [3] Stewart D. Algorithm for drawing ellipses or hyperbolae with a digital plotter. Proc of Inst Mech Engr, 180(1), 1965. (Geciteerd op pagina 9.) [4] Lay D.C. Linear algebra and its applications. Pearson/Addison-Wesley, 2006. (Geciteerd op pagina 27.) [5] de Cooman G. Lineaire systemen. 2008. (Geciteerd op paginas 35, 57 en 58.) [6] Emasta.com. 78xx Voltage Regulators. Online, geraadpleegd op 19 maart 2011. (Geciteerd op pagina 11.) [7] Sadjadian H. and Taghirad H.D. Numerical methods for computing the forward kinematics of a redundant parallel manipulator. 2003. (Geciteerd op pagina 28.) [8] Texas Instruments. Datasheet µA7800 series, positive-voltage regulators. 2009. (Geciteerd op pagina 11.) [9] JChampion. A flexible charting library for .NET, juni 2007. Online, geraadpleegd op 25 april 2011. (Geciteerd op pagina 65.) [10] Van Aken J.R. An efficient ellipse drawing algorithm. IEEE Computerfbook Graphics and Applications, 4(9):24–35, september 1984. (Geciteerd op pagina 47.) [11] Bouguet J.Y. Camera Calibration Toolbox for Matlab. Online, geraadpleegd op 10 februari 2011. (Geciteerd op pagina 38.) [12] Riley K.F., Hobson M.P., Bence S.J., and Bence S.J. Mathematical methods for physics and engineering. Cambridge University Press, 2006. (Geciteerd op pagina 27.) [13] Dong Hwan Kim, Ji-Yoon Kang, and Kyo-Il Lee. Robust nonlinear observer for forward kinematics solution of a stewart platform: an experimental verification. Robotica, 18(6), November 2000. (Geciteerd op pagina 69.) 85

REFERENTIES

REFERENTIES

[14] Tae-Young Lee and Jae-Kyung Shim. Forward kinematics of the general 6¨ı¿ 12 6 stewart platform using algebraic elimination. Mechanism and Machine Theory, pages 1073– 1085, September 2001. (Geciteerd op pagina 68.) [15] Microsoft. Timer Class, 2011. Online, geraadpleegd op 5 mei 2011. (Geciteerd op pagina 65.) [16] Pitteway M.L.V. Algorithm for drawing ellipses or hyperbolae with a digital plotter. Computer J., 10(3):282–289, novermber 1967. (Geciteerd op pagina 47.) [17] Kachroo P. and Mellodge P. Mobile robotic car design. Tab robotics. McGraw-Hill, 2005. (Geciteerd op paginas 9 en 10.) [18] De Keyser R. and Ionescu C. Matlab : modelling, programming and simulations. Sciyo, 2010. (Geciteerd op pagina 60.) [19] Duda R.O. and P.E. Hart. Use of the hough transformation to detect lines and curves in pictures. Communications of the ACM, 15:11–15, januari 1972. (Geciteerd op pagina 43.) [20] Tsai R.Y. A versatile camera calibration technique for high accuracy 3d machine vision metrology using off-the-shelf tv cameras and lenses. IEEE journal of robotics and automation, RA-3(4):323–344, autustus 1987. (Geciteerd op pagina 37.) [21] Leslie Sanford. The Multimedia Timer for the .NET Framework, maart 2006. Online, geraadpleegd op 14 april 2011. (Geciteerd op pagina 65.) [22] Lee Hyung Sang and Myung-Chul Han. The estimation for forward kinematic solution of stewart platform using the neural network. Intelligent Robots and Systems, 1:501– 506, 1999. (Geciteerd op pagina 69.) [23] Rekdalsbakken W. The use of artificial intelligence in controlling a 6dof motion platform. In 21st European Conference on Modelling and Simulation, pages 249–254. European Council for Modelling and Simulation, juni 2007. (Geciteerd op paginas 9 en 28.) [24] Wikipedia. Hough transform — Wikipedia, The Free Encyclopedia, 2011. [Online; geraadpleegd op 11 februari 2011]. (Geciteerd op pagina 43.) [25] Wikipedia. Midpoint circle algorithm — Wikipedia, The Free Encyclopedia, 2011. [Online; geraadpleegd op 12 februari 2011]. (Geciteerd op pagina 47.) [26] Wikipedia. Heron’s formula — Wikipedia, The Free Encyclopedia, 2011. Online, geraadpleegd op 30 maart 2011. (Geciteerd op pagina 42.) [27] Wikipedia. Servomechanism — Wikipedia, The Free Encyclopedia, 2011. Online, geraadpleegd op 18 maart 2011. (Geciteerd op pagina 9.)

86

Ontwerp van een ball and plate systeem met een 6 DoF motion platform en vision-based feedback

Recommend Documents