Nicolaj Lindhard Jørgensen, s062032Lasse Thorup Olesen, s062016

REGULERING AF ROBOTARM

Bachelorprojekt, forår, 2009

Nicolaj Lindhard Jørgensen, s062032Lasse Thorup Olesen, s062016

REGULERING AF ROBOTARM

Bachelorprojekt, forår, 2009

REGULERING AF ROBOTARM,

Rapporten er udarbejded af:Nicolaj Lindhard Jørgensen, s062032Lasse Thorup Olesen, s062016

VejledereNils Andersen & Ole Ravn

DTU ElektroAutomation and controlDanmarks Tekniske UniversitetElektrovejBygning 326DK-2800 Kgs. LyngbyDenmark

www.elektro.dtu.dk/forskning/au.aspxTel: (+45) 45 25 35 50Fax: (+45) 45 88 12 95studieadministration@elektro.dtu.dk

Udgivelsesdato: 26-06-2009Klasse: 1 (offentlig)

Bemærkninger: Denne rapport er indleveret som led i opfyldelseaf kravene for opnåelse af graden Bachelor iteknisk videnskab (BScE) på Danmarks TekniskeUniversitet. Rapporten repræsenterer 15 ECTSpoint.

Rettigheder c©N.L. Jørgensen, L.T. Olesen, 2009

Abstract

This report describes how vibrations in a robot arm with one link can be damped.The method that is investigated uses an accelerometer to meassure the accel-erations at the endpoint of the arm and controls using this. Matlab is usedto simulate the actual system and set up filters and regulators. The studiesshowed that using appropriate filters and regulators, it was possible to reduceovershoot and settling time.

I

Resumé

Denne rapport beskriver hvordan svingninger i en robotarm med et led kandæmpes. Metoden der undersøges bruger et accelerometer til at måle acceler-ationerne i spidsen af armen og regulerer herefter. Der bruges Matlab til atsimulere det egentlige system og opstille filtrer og regulatorer. Det viste sig at,ved at bruge nogle passende filtrer og regulatorer, var det muligt at dæmpeoversvinget og indsvingstiden.

II

Indhold

Abstract I

Resumé II

1 Indledning 11.1 Baggrund . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Problemformulering . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.3 Metode, afgrænsning og forudsætninger . . . . . . . . . . . . . . . 31.4 Introduktion til Mechatronics . . . . . . . . . . . . . . . . . . . . . 31.5 Arbejde af andre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.6 Læsevejledning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Teori 62.1 Regulatorer og filtre i kontinuert tid . . . . . . . . . . . . . . . . . 6

2.1.1 P-regulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.1.2 PI-regulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.1.3 Lead-kompensatoren . . . . . . . . . . . . . . . . . . . . . . 72.1.4 Lowpass filter . . . . . . . . . . . . . . . . . . . . . . . . . . 82.1.5 Highpass filter . . . . . . . . . . . . . . . . . . . . . . . . . . 82.1.6 Notch-filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.2 Regulatorer og filtre i diskret tid . . . . . . . . . . . . . . . . . . . 82.2.1 Z-transformation . . . . . . . . . . . . . . . . . . . . . . . . 92.2.2 Filtre i diskret tid . . . . . . . . . . . . . . . . . . . . . . . . 10

2.3 Jung’s modulus og svingninger . . . . . . . . . . . . . . . . . . . . 10

3 Simulering 123.1 Den fysiske model i simulink . . . . . . . . . . . . . . . . . . . . . 123.2 Verificering af simulering . . . . . . . . . . . . . . . . . . . . . . . 14

3.2.1 Step på hastighedsservo . . . . . . . . . . . . . . . . . . . . 143.2.2 Step på positionsservo . . . . . . . . . . . . . . . . . . . . . 143.2.3 Step på positionsservo, acceleration af arm . . . . . . . . . 153.2.4 Systemets poler . . . . . . . . . . . . . . . . . . . . . . . . . 163.2.5 Opsamling på tests . . . . . . . . . . . . . . . . . . . . . . . 16

3.3 Implementering af reguleringen . . . . . . . . . . . . . . . . . . . . 163.3.1 Hastighedstilbagekobling med lead . . . . . . . . . . . . . . 16

III

INDHOLD

3.3.2 2. ordens highpass filter . . . . . . . . . . . . . . . . . . . . 173.3.3 1.ordens highpass filter . . . . . . . . . . . . . . . . . . . . . 183.3.4 Notch-filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.3.5 P-Lead . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.4 Filtre og regulatorer i Matlab . . . . . . . . . . . . . . . . . . . . . 203.4.1 Lead-kompensator . . . . . . . . . . . . . . . . . . . . . . . 213.4.2 Notch-filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.4.3 Highpass-filtre . . . . . . . . . . . . . . . . . . . . . . . . . 23

4 Implementering 254.1 Implementering på datamat . . . . . . . . . . . . . . . . . . . . . . 254.2 Den fysiske implementering . . . . . . . . . . . . . . . . . . . . . 26

5 Test 295.1 Positionsbestemmelse . . . . . . . . . . . . . . . . . . . . . . . . . . 295.2 Karakteristik af step på robotarmen . . . . . . . . . . . . . . . . . 295.3 Step med regulering . . . . . . . . . . . . . . . . . . . . . . . . . . 305.4 Respons på forskellige inputs . . . . . . . . . . . . . . . . . . . . . 315.5 Forskydning af egenfrekvens . . . . . . . . . . . . . . . . . . . . . 32

6 Diskussion 356.1 Anvendelighed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 356.2 Videreudvikling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

7 Konklusion 37

Litteratur 38

Appendiks 39

A Symbolliste 39

B Overføringsfunktionen for modellen 41

C RHD 43C.1 Start RHD client . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43C.2 Ret i programmet . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43C.3 Compile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43C.4 Eksekver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

D Accelerometer og sampling 45

IV

INDHOLD

D.1 Antal samples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45D.2 Sampling og tider . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

E Comedi-Board 46

F Metode til positionsbestemmelse 47

Bilag 49

I Grafer 49

II Modeller 51

III Kildekode 55

IV Indhold på vedlagte CD 61

V

Figurer

1.1 IRB 1600ID fra ABB . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 UR-6-85-5-A fra Universal Robots (her vist med et tool) . . . . . . 2

2.1 General model for et reguleringssystem . . . . . . . . . . . . . . . 72.2 Cantilever beam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.1 Fysisk system uden regulator . . . . . . . . . . . . . . . . . . . . . 123.2 Step på det fysiske system med positionstilbagekobling . . . . . . 133.3 Step på hastighedsservo . . . . . . . . . . . . . . . . . . . . . . . . 143.4 Step på positionsservo (motor) . . . . . . . . . . . . . . . . . . . . . 153.5 Step på positionsservo (endespids) . . . . . . . . . . . . . . . . . . 153.6 Hastighedstilbagekobling med Lead-kompensator . . . . . . . . . 173.7 Hastighedstilbagekobling med Highpass-filter . . . . . . . . . . . 183.8 Hastighedstilbagekobling med notch-filter . . . . . . . . . . . . . . 193.9 Step med komplet regulering . . . . . . . . . . . . . . . . . . . . . 203.10 Bodeplot af modellen uden Lead-kompensator . . . . . . . . . . . 213.11 Acceleration af endespids . . . . . . . . . . . . . . . . . . . . . . . . 233.12 Frekvensrespons for notch-filteret . . . . . . . . . . . . . . . . . . 24

4.1 Model af armen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274.2 Hardware setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

5.1 Position af endespids . . . . . . . . . . . . . . . . . . . . . . . . . . 305.2 Fysisk system med positionstilbagekobling . . . . . . . . . . . . . 315.3 Fysisk system uden regulator . . . . . . . . . . . . . . . . . . . . . 325.4 Steprespons . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 335.5 Respons på et sinus input . . . . . . . . . . . . . . . . . . . . . . . 345.6 Step med forskellige setup . . . . . . . . . . . . . . . . . . . . . . . 34

F.1 Arm setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

I.1 Pole-Zero map fra Matlab . . . . . . . . . . . . . . . . . . . . . . . 49I.2 Bodeplot af 2.ordens svingning af arm . . . . . . . . . . . . . . . . 50

II.1 Ekspanderet flex-blok. . . . . . . . . . . . . . . . . . . . . . . . . . 51II.2 Hastighedsservo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52II.3 Den basale model ekspanderet. . . . . . . . . . . . . . . . . . . . . 53

VI

FIGURER

II.4 Det samlede system med regulering . . . . . . . . . . . . . . . . . 54

VII

Tabeller

2.1 Beregnede egenfrekvenser for den fysiske arm . . . . . . . . . . . 11

3.1 Poler bestemt ud fra overføringsfunktion . . . . . . . . . . . . . . 133.2 Poler fra simulering og flexarm . . . . . . . . . . . . . . . . . . . . 163.3 Data fra reguleringsprocess . . . . . . . . . . . . . . . . . . . . . . 20

4.1 Fysiske parametre for flexarm . . . . . . . . . . . . . . . . . . . . . 27

5.1 Parametre for arm med simpel regulering . . . . . . . . . . . . . . 305.2 Resultater for regulering . . . . . . . . . . . . . . . . . . . . . . . . 305.3 Respons på forskellige inputs . . . . . . . . . . . . . . . . . . . . . 31

A.1 Liste med symboler . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

E.1 Comedi parametre . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

VIII

Kapitel1Indledning

1.1 Baggrund

I generelle automationssystemer hvor der bruges robotarme til at udføre forskel-lige opgaver, er der en hel klar tendens til at bruge store, tunge og solide robot-ter. Selv når der skal udføres opgaver som at svejse, male og tage små produk-tionsprøver bliver der brugt store robotarme.På fig.1.1 ses en robotarm fra ABB der er beregnet til at svejse. Den vejer

Figur 1.1: IRB 1600ID fra ABB

250kg, den kan håndtere 4kg og har en positions nøjagtighed på +/-0.02mm.På fig.1.2 ses en robotarm fra Universal Robots der vejer 18kg, kan håndtere5kg og har en positionsnøjagtighed på +/-0.1mm.Årsagen til at robotarmen fra ABB vejer over 10 gange så meget som robotar-

men fra Univesal Robots, skal findes i ønsket om større præcision og dermedvalget af materialer, gear og motorer. Den letteste måde at øge positions- og

1

Kapitel 1. Indledning

Figur 1.2: UR-6-85-5-A fra Universal Robots (her vist med et tool)

banenøjagtighed er ved at gøre konstruktionen mere stabil. Dette kan gøresved at bruge tykkere materialer, bruge gear uden slør i og bruge motorer, derer tilpas stærke i forhold til belastningen. Derudover kan man opnå en vis præ-cisionsforbedring ved at begrænse størrelsen af accelerationerne. Disse tiltagvil generelt resultere i, at robotarmen bliver tungere og er afhængig af en solidbase, der typisk skal monteres på et gulv eller i loftet.I applikationer hvor det ikke er muligt at montere robotarmen på en robustoverflade, er man nød til at holde vægten nede. Det gør det vanskeligt at min-imere rystelser og beholde præcisionen. Ved at bruge outputtet fra sensorer,som accelerometre, på en given robotarm, til at regulere efter vil nogle vibra-tioner kunne dæmpes og dermed vil nøjagtigheden kunne bevares. Den sammeteknik kan også anvendes generelt på robotarme, der skal fortage kraftige ac-celerationer. Også her vil sensorer kunne hjælpe med at dæmpe svingningerne.

2

1.2 Problemformulering

1.2 Problemformulering

Når robotarme i industrien laver hurtige eller pludselige bevægelser kan deropstå svingninger i robotternes led. Disse svingninger kan være til gene forden opgave robotten skal udføre og i værste fald føre til skader på de mekanis-ke dele.Ved hjælp af sensorer, som accelerometre, og passende datamater er det muligtat regulere en given robot efter klassiske reguleringsprincipper.Det er derfor målet i denne opgave at dæmpe svingninger i en fleksibel robot-arm med et led vha. en datamat og et accelerometer som sensor. Dermed ønskesdet afklaret:

• Hvor meget svingningerne kan dæmpes i forhold til en kode med en sim-pel positionstilbagekobling.

• Hvilken regulator der er mest hensigtsmæssig at implementere.

1.3 Metode, afgrænsning og forudsætninger

Der tages udgangspunkt i simuleringer og modeller i Matlab og dernæst efter-prøves disse på den givne robotarm. Data fra de foretagne test undersøges udfra typiske reguleringsparametre. Parametrene bruges dernæst til at optimereefter.Det tilsigtes at implementere en regulator, der ved hjælp af de givne sensorerkan dæmpe svingningerne i den givne robotarm. Projektet afgrænses dermedaf antallet af tilgænglige sensorer, til kun at fokusere på at dæmpe 1.ordenssv-ingningerne. Derudover afgrænses projektet til at fokusere på svingninger iet led. Disse afgrænsninger opstår som forudsætning for at kunne udvikleteknologien til mere avancerede applikationer, hvor der kan håndteres hø-jere ordens svingninger og flere led. Dermed er projektet gyldigt i forhold tilvidereudvikling af teknologier til dæmpning af svingninger i robotarme.

1.4 Introduktion til Mechatronics

Mechatronics er et begreb, som gjorde sit indtog i Japan i 1969, hvor begrebetførste gang blev beskrevet af hr. Tetsuro Mori [8]. Mechatronics dækker over

3

Kapitel 1. Indledning

en blanding af mekanik og elektronik, som er afhængige af hinanden for atfungere.

"... a synergistic combination of precision mechanical engineer-ing, electronic control and systems thinking in design of productsand manufacturing processes."- Jacob Buur 1989 [2]

Dette begreb bruges især inden for regulering, hvor det, i stigende grad kanbetale sig, at lade dele af maskinen være aktive elektroniske i stedet for me-kaniske. Dette skyldes bl.a. at elektronik er blevet billigere og mere pålideligtend tidligere.

Et udmærket eksempel på et system som anvender mechatronics, er ABS-bremser (Antiblockingsystem). For at undgå, at et hjul på en bil blokerer ogskrider under opbremsning, er der i bilen implementeret en computer, som viaen elektronisk hastighedssensor registrerer hjulets omdrejninger. Denne com-puter kontrollerer så den hydrauliske bremse, og sørger for at lette trykket påbremsen ved blokering af hjulet.

1.5 Arbejde af andre

Her følger et kort resume af de to primære kilder til dette projekt.

“Jens Rune Baungaard’s Ph.D.-afhandling"[2] omhandler regulering af sammemekaniske setup som er behandlet i denne rapport, dog med strain gauges somsensor i stedet for et accelerometer. Derved er han kommet frem til, ved et steppå 0.3 rad, at have et oversving på 0.05 rad og en indsvingstid på ca. 2 sek. Ses. 231 [2].

“Vibration Absorption Control of Industrial Robots by Acceleration Feedback"[3]beskriver vibrationsdæmpning vha. tilbagekobling af acceleration på indus-trielle robotter. Der fremvises markante forbedringer efter tilbagekobling afaccelerationen sammenlignet med det traditionelle regulering.

1.6 Læsevejledning

Teorien for de anvendte regulatorer, fysikken bag en model af robotarmen ogregulering i diskret tid gennemgås kort.

4

1.6 Læsevejledning

I Simuleringen gøres rede for den anvendte matematiske model, og denne sam-menholdes med den virkelige robotarm.Implementeringen redegøre for hvordan de, i afsnit 3, fundne regulatorer im-plementeres på den fysiske opstilling.Test på robotarmen gennemgås. Der præsenteres plots og relevante værdierfor de opnåede resultater.I diskussionen perspektivers de opnåede resultater, og der reflekteres overhvad der kunne gøres bedre i et projekt på længere sigt.Konklusionen sammenfatter kort de opnåede resultater og sammenholder demmed problemformuleringen.

For læsere der vil danne sig et hurtigt overblik over rapporten, kan man medfordel læse problemformuleringen, Step med regulering (afsnit 5.3) og konklu-sionen.For læsere der har basal forståelse for regulering kan kapitel 2 udelades Er op-stillingen helt ukendt for læseren kan afsnit 4.2 læses først for et få et overblikover den konkrete reguleringsopgave.

God fornøjelse

5

Kapitel2Teori

Sædvanligvis kan systemer med et givent input u(t) og et givent output y(t)

beskrives ved en n’te ordens differentialligning så som:

anyn(t) + an−1yn−1(t) + . . . + a0y(t) = bmum(t) + bm−1u

m−1(t) + . . . + b0u(t) (2.1)

Dette gælder både for fysiske systemer som fjedre og motorer, samt filtre og reg-ulatorer. Dog realiseres filtre og regulatorer ofte i frekvensdomænet. Ligeledestransformeres modeller for systemer i tidsdomænet som det er tilfældet i Mat-lab og simulink, til frekvensdomænet med Laplace transformation. Den unilat-erale Laplace transformation er givet ved:

L[f(t)] = F (s) =

∫ ∞

0−f(t)e−stdt (2.2)

Når differentialligningerne for et system som 2.1 Laplace transformeres kanoverføringsfunktionen for systemet opstilles som:

H(s) =Y (s)

U(s)=

bmsm + . . . + b1s + b0

ansn + . . . + a1s + a0

(2.3)

Når det er ønskeligt at have systemet på denne form, er det fordi det er be-tragtelig lettere at opstille blokdiagrammer og arbejde med disse.

Den direkte Fourier transformation(2.4) bruges til at gå fra tidsdomænettil frekvensdomænet for at beskrive hvilke frekvenser der er tilstede i denoprindelige funktion.

F [f(t)] = F (ω) =

∫ ∞

−∞f(t)e−jωtdt (2.4)

2.1 Regulatorer og filtre i kontinuert tid

Et typisk reguleringssystem kan beskrives med et blokdiagram som vist påfig.2.1. Her er G(s) = (G1(s) + d(s)) G2(s) systemet, der skal reguleres, H(s)

beskriver målesystemet, K0 er en skaleringskonstant så forholdet mellem r(s)

6

2.1 Regulatorer og filtre i kontinuert tid

og c(s) stemmer overens når reguleringen er perfekt og GC(s) er regulatoren.r(s) er referencen, e(s) er fejlen, u(s) er styresignalet, c(s) er outputtet, cm er

Figur 2.1: General model for et reguleringssystem

målesignalet og d(s) er en forstyrrelse.

2.1.1 P-regulator

Den mest basale regulator er proportionalregulatoren hvor GC(s) = Kp, hvilketsvarer til en ren forstærkning af fejlen eller målesignalet.

2.1.2 PI-regulator

En meget brugt regulator er en proportional-integralregulator, der er givet ved:

Gc(s) = Kpτi s + 1

τi s(2.5)

Denne regulator kendetegnes ved at have gode stationære egenskaber, men tilgengæld mindskes båndbredden betragteligt i forhold til P-regulatoren.

2.1.3 Lead-kompensatoren

En kendt kompensator type er Lead’en, der har overføringsfunktionen:

Gc(s) = Kpτd s + 1

ατd s + 1, α < 1 (2.6)

Den væsenligste egenskab ved Lead-kompensatoren er at den øger båndbred-den og dermed mindsker den negative fasedrejning.

7

Kapitel 2. Teori

2.1.4 Lowpass filter

Laplace repræsentationen af et 1.ordens lowpass filter er givet ved:

Gc(s) =ωc

ωc + s(2.7)

Lowpass filteret kendetegnes ved at det dæmper frekvenser over knækfrekven-sen ωc.

2.1.5 Highpass filter

Overføringsfunktionen for et typisk highpass filter fremkommer ved at sættes = ωp/s i 2.7, se [6] s.525.

Gc(s) =ωs

ωs +(ωp

s

) =ωs s

ωs s + ωp

(2.8)

Hvor ωp er passband frekvensen og ωs er stopband frekvensen. Highpass filter-ets kendetegn er selvsagt, at det dæmper frekvenser under stopband frekvensen.

2.1.6 Notch-filter

Som udgangspunkt for notch-filter design bruges følgende overføringsfunktion:

Gc(s) =s2 + 2d

cω0 s + ω2

0

s2 + 21cω0 s + ω2

0

(2.9)

Denne filtertype kaldes også Båndstop filter og bruges når man ønsker atdæmpe enkelte frekvenser eller et lille frekvensspektra.

2.2 Regulatorer og filtre i diskret tid

Som bekendt arbejder computere og mikroprocessorer i diskret tid. Derfor gen-nemgås her de grundliggende principper bag overgangen fra kontinuert tid tildiskret tid.

8

2.2 Regulatorer og filtre i diskret tid

2.2.1 Z-transformation

Denne transformation bruges til systemer i kontinuert tid, der skal trans-formeres til diskret tid. Hvis et kontinuert signal y(t) samples med sampletidenT er den unilaterale z-transformerede givet ved:

Z [y∗(t)] = Y (z) =∞∑

k=0

y(kT )z−k (2.10)

Fra [5] ligning 7.17, hvor k er et udtryk for hvilken sample, der betragtes. Ipraksis benyttes de givne regneregler som er vedlagt i både [5] og [6].Til at transformere overføringsfunktioner fra kontinuert tid til diskret tid be-nyttes c2d i Matlab. Denne funktion tager, udover funktionen der skal trans-formeres, parametrene sampletid og metode. Sampletiden er fastsat idet sys-temet kører en fast cyklus på 10ms. Som metode vælges tustin der dækker overen bilineær transformation som fra [6] ligning 12.57 er givet ved:

s =

(2

T

)z − 1

z + 1(2.11)

En typisk overføringsfunktion i diskret tid vil have formen:

H(z) =Y (z)

X(z)=

b0 + b1z−1 + b2z

−2 . . . + blz−l

1 + a1z−1 + a2z−2 + . . . + anz−n(2.12)

En given overføringsfunktion skal, som det vil vise sig senere, kunne realiserespå en datamat. Skrives 2.12 som en differens ligning fås flg. udtryk:

y(k)+a1y(k − 1) + a2y(k − 2) + . . . + any(k − n)

= b0x(k) + b1x(k − 1) + b2x(k − 2) + . . . + blx(k − l) ⇒ (2.13)

y(k) =− a1y(k − 1)− a2y(k − 2)− . . .− any(k − n)

+ b0x(k) + b1x(k − 1) + b2x(k − 2) + . . . + blx(k − l) (2.14)

Her indses det, at y(k) er outputtet i øjeblikket k. Således beregnes outputtetfor et n’te ordens system ud fra de n foregående værdier for outputtet. Systemermed denne egenskab kaldes rekursive. Denne egenskab medfører at output-tet aldrig rigtig dør ud, fordi outputtet forplanter sig så længe koefficienternea1, a2 . . . an ikke er lig nul. Dermed kaldes disse koefficienter for de rekursivekoefficienter. Mens b1, b2 . . . bn kaldes ikke-rekursive koefficienter.

9

Kapitel 2. Teori

2.2.2 Filtre i diskret tid

Gives en enhedsimpuls som input til et rekursivt filter vil outputtet h(k) for-sætte for k → ∞, hvorfor rekursive filtre kaldes infinite impulse response(IIR)filtre. Mens et n’te ordens ikke-rekursivt filter der gives en enhedsimpuls, vilgive et output h(k) lig nul for k > n og derfor kaldes finite impulse response(FIR)filtre.

2.3 Jung’s modulus og svingninger

Reguleringsobjektet i dette projekt er en fleksibel arm som vist i afsnit 4.2. Ensådan arm hedder populært en cantilever beam, som er en beam fastgjort i denene ende. Se figur 2.2. En sådan beam kan beskrives ud fra følgende param-eter: Længde, Bredde, Højde, Densitet og Youngs modulus. Sidstnævnte er etudtryk for beames stivhed. Da armens svingninger er i et plan vinkelret påtyngdekraften regnes denne ikke som en påvirkning. Geometrien for en sådan

Figur 2.2: Cantilever beam

beam kan beskrives ved Euler-Bernoulli’s ligning (2.15). Denne differential-ligning angiver sammenhængen imellem krumningen af en beam (x,u), og denbelastning den bliver påtrykt (w)

δ

δx

(EIa

δ2u

δ2x

)= w (2.15)

Løst for en belastning på endepunktet af beamen giver dette udtryk ifølge [11]:

y = − FL3

3EIa

(2.16)

10

2.3 Jung’s modulus og svingninger

der beskriver spidsens forskydning som funktion af den påtrykte kraft, hvorIa er tværsnits inertimomnetet (på engelsk second moment of inertia). For enmassiv firkantet beam, er dette udtrykt ved:

Ia =BH3

12(2.17)

Hvor B er bredden i den dimension der svinges i og H er højden.Fjernes denne kraftpåvirkning vil armen begynde at svinge. Svingningen kanudtrykkes ved ligningen:

µδ2u

δt2+

δ2

δx2

(EIa

δ2u

δ2x

)= 0 (2.18)

Under antagelse af at densiteten og tværsnitsarealet er konstant over helearmen, kan svingningen udtrykkes ved den partielle differentialligning 2.19,hvor µ = ρA:

µδ2u

δt2+ EIa

δ4u

δ4x= 0 (2.19)

Vi skal ikke her komme nærmere ind på løsningen af denne ligning, men blotnævne at den har løsningen givet i [2] ligning 4.75.

ω =

√k4EIa

ρA(2.20)

Hvorcos(kL) cosh(kL) = −1 (2.21)

k og ω kan i tabel 2.1 aflæses for de 3 første ordener af svingningerne. Det

Orden 1. 2. 3.k 1.562 3.912 6.545ω 7.30 45.81 128.22

Tabel 2.1: Beregnede egenfrekvenser for den fysiske arm

kan ses ud fra 2.20 at hvis densiteten af armen stiger vil egenfrekvensernefalde. Dette er også tilfældet hvis inertimomentet for armen ændres, f.eks. vedtilføjelse af et tool. Dette vil blive nærmere undersøgt i afsnit 5.5.

11

Kapitel3Simulering

3.1 Den fysiske model i simulink

For at kunne simulere systemet opstilles en model i simulink. På fig.3.1 sesden opstillede model. Blokken flex er taget fra MSL-pakken og er sat til steel1.Denne blok beskriver selve armen, og hvordan den reagerer. Som det ses i bilagII.1 tager flex en vinkelposition og en vinkelaccelration som input og giver etmoment og armens vinkelposition i spidsen som output. Blokken Hastigheds

Figur 3.1: Fysisk system uden regulator

servo kan ses ekspanderet i bilag II.2. Denne blok repræsenterer motoren ogden hastighedstilbagekobling systemet er sat op med. Blokken får en vinkel-hastighed og et moment som input og giver en vinkelposition og en vinkelac-celeration som output.Som modellen er konfigureret her, vil et step resultere i en hastighedsæn-dring, der bliver mindre, jo tættere armen kommer på den ønskede position.Således er inputtet en position. Reelt burde der ligge et gain mellem position-stilbagekoblingen og hastighedsindgangen på Hastigheds servo-blokken, menda faktoren i dette gain ville være 1 er det udeladt på fig.3.1. Laves der et po-sitionsstep fås graferne i fig.3.2. Øverst ses armens position, her ses det, atspidsens position svinger om den ønskede position mens positionen fra poten-tionmeteret indstiller sig på den ønskede position. Nederst ses fejlen, beregnetsom forskellen mellem potentiometerpositionen og spidspositionen.Overføringsfunktionen er udledt for modellen på baggrund af den ekspanderedemodel i bilag II.3. Selve udledningen kan ses i appendiks B. Overføringsfunk-

12

3.1 Den fysiske model i simulink

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

Position

tid

rad

0 2 4 6 8 10

−0.1

−0.05

0

0.05

0.1

0.15Fejl

tid

rad

spidsskulder

Figur 3.2: Step på det fysiske system med positionstilbagekobling

Poler Nulpunkter<(p) =(p) <(z) =(z)

-0.287 7.29 -0.324 16.3-0.287 -7.29 -0.324 -16.30.0665 45.6 -49.7 00.0665 -45.6 51.7 0

-86 0 - --1.03 0 - -

Tabel 3.1: Poler bestemt ud fra overføringsfunktion

tionen for modellen er givet ved:

H(s) =KtKpmKdmg(1+G2(s)s2)

(Jm+m2g(Jb−G1(s)))Ras2+(Kt(KpmK1es+Ke)+µf Ra)s+KtKpmKdmg

(3.1)

Her er G1(s) og G2(s) estimerede overføringsfunktioner for state space modellender ses i bilag II.1. Poler og nulpunkter for overføringsfunktionen er fundetvha. Matlab’s pzmap-funktion og ses i tabel 3.1.

Ud fra I.1 og 3.1 ses det, at polerne rammer egensvingningerne for armen,som [2] også har fundet og som yderligere er beregnet i teori afsnitet.

13

Kapitel 3. Simulering

3.2 Verificering af simulering

For at verificere at modellen beskriver det fysiske system tilstrækkeligt, lavesforskellige tests af dem begge. Målet med disse tests er, at sikre at modellenikke afviger fra virkeligheden med mere end 20 %. Systemet pilles fra hinandenog undersøges bid for bid.

3.2.1 Step på hastighedsservo

Motoren tilbagekobles med et tachometer, så den agerer hastighedsservo. Dersteppes på servoen med et step af højden 0.2 rad/sek. Det kan ses på plottene3.3(a) og 3.3(b) at simuleringen giver en hastighed på 0.198 rad/sek, og denvirkelige model en hastighed på 0.193 rad/sek. Dette svarer til en afvigelse på3 % .

(a) Simulering (b) Virkelig arm

Figur 3.3: Step på hastighedsservo

3.2.2 Step på positionsservo

Systemet tilbagekobles som en positionsservo. Der steppes herefter på sys-temet med et step af højden 1 rad. Der reguleres med en P regulator med Kp= 1. Det ses af plottene 3.4(a) og 3.4(b) at der svinges til 1 rad uden stationærfejl i begge tilfælde.

14

3.2 Verificering af simulering

(a) Simulering (b) Virkelig arm

Figur 3.4: Step på positionsservo (motor)

3.2.3 Step på positionsservo, acceleration af arm

Systemet, som nu stadig er tilbagekoblet som positionsservo, steppes som førmed 1 rad, og accelerationen af endepunktet observeres. Det ses, at udsvingenefor den fysiske arm er større end for simuleringen. Størrelsen på det førsteudsving er i simuleringen 5 rad/sek2, og i praksis 4 rad/sek2. Dette svarer tilen afvigelse på 20 %. Det ses, at armen svinger med en frekvens som er lavere,end simuleringen, som det ses på figur 3.5(b) og 3.5(a). Dog skal det bemærkesat den fysiske arm har en lavere dæmpning end simuleringen. Dette fremgårogså af polerne i tabel 3.2.

0 2 4 6 8 10−10

−8

−6

−4

−2

0

2

4

6

8

10acc tip

tid

rad/

sek2

(a) Simulering

0 2 4 6 8 10−10

−8

−6

−4

−2

0

2

4

6

8

10

tid (sek)

rad/

sek2

accx

(b) Virkelig arm

Figur 3.5: Step på positionsservo (endespids)

15

Kapitel 3. Simulering

Matlab Arm<(p) =(p) <(p) =(p)

-0,2859 7,3199 -0,2805 6,2427-0,2859 -7,3199 -0,2805 -6,2417

Tabel 3.2: Poler fra simulering og flexarm

3.2.4 Systemets poler

For at undersøge den egentlige værdi af simuleringen, beregnes de poler somgiver anledning til 1. ordens svingningen for systemet. Ud fra matlab-modellener følgende komplekse poler fundet. Realdelen af polerne afviger med 1,8 % ogimaginærdelen med 14 %. Det vil altså sige at det virkelige system vil svingeen anelse langsommere end simulationen. Se tabel 3.2.

3.2.5 Opsamling på tests

Den største afvigelse ses på accelerationen af endespidsen som afviger med20 % . Polerne for det simulerede system ligner dog polerne for det virkeligesystem så meget, at det vil være rimeligt at designe regulatorer efter dem.

3.3 Implementering af reguleringen

Her følger en step by step gennemgang af dimensioneringen af regulatorernei simulink. Systemet med en simpel positionstilbagekobling kan ses i afsnit3.1. Da hastigheden for spidsen af armen ikke er tilgængelig som sensor in-put integreres spidsens acceleration. For at simulere støj i systemet integreresen fejl ind på hastighedstilbagekoblingen. Da det er muligt at vælge antalletaf ordener, armens svingninger skal have i simuleringen, startes der med ensimulering af armen med 1. ordens svingninger. Beregning af parameterene tilde implementerede filtre og regulatorer kan ses i afsnit 3.4.

3.3.1 Hastighedstilbagekobling med lead

For at dæmpe svingningerne i armen kobles hastigheden af endespidsen tilbage.Denne er fundet ved integration af accelerationen. Ud fra bodeplottet på figur

16

3.3 Implementering af reguleringen

3.10 ses det, at systemet har en voldsom negativ fasedrejning omkring 11.5rad/sek. Denne vil bevirke, at systemet ved en hastighedstilbagekobling vilblive ustabilt. Ved simulering har det vist sig, at dette er i en grad så detikke er muligt at producere brugbare plots med matlab. For at forbedre sys-temets båndbredde og derved stabilisere det, indsættes en lead kompensator isystemet. Denne er dimensioneret, så den har en maksimal positiv fasedrejn-ing ved 11.5 rad/sek. Udregningen af denne kan ses i afsnit 3.4.1. Dette giveren regulator med overføringsfunktionen:

H(s) =0.1s + 1

0.07577s + 1(3.2)

Ved implementering af denne er det igen muligt at simulere systemet, og re-sponset på et step ser ud som på figur 3.6. Her er den blå kurve endespidsensvinkelposition og den grønne kurve er den fastgjorte endes vinkelposition

0 2 4 6 8 100

0.5

1

1.5Position

tid

rad

Figur 3.6: Hastighedstilbagekobling med Lead-kompensator

3.3.2 2. ordens highpass filter

Det ses, at positionen driver som funktion af den integrerede fejl på hastighe-den. For at kompensere for dette indsættes et 2. ordens highpass filter i tilbage-

17

Kapitel 3. Simulering

koblingen for hastigheden, med en knækfrekvens på 7.31 rad/sek. Dette bevirk-er, at de lave frekvenser som giver anledning til den stationære hastighedsfejlsorteres fra. Highpass filteret som er beskrevet i afsnit 3.4.3 har overførings-funktionen:

H(s) =s2 + 1.11 · 1014s + 2.106

s2 + 10.34s + 50.54(3.3)

Det ses på figur 3.7, at den stationære hastighedsfejl nu næsten er væk. Somtidligere er den blå kurve endespidsens vinkelposition og den grønne kurve erden fastgjorte endes vinkelposition.

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

Position

tid

rad

Figur 3.7: Hastighedstilbagekobling med Highpass-filter

3.3.3 1.ordens highpass filter

For at fjerne den lille tilbageværende hastighedsfejl tilføjes endnu et highpass-filter Denne gang af 1. orden. Dette placeres med en knækfrekvens i 0.628rad/sek. og får derfor overføringsfunktionen:

H(s) =s

s + 0.6283(3.4)

18

3.3 Implementering af reguleringen

3.3.4 Notch-filter

Herefter hæves ordenen af armens svingninger i simuleringen en orden, såder nu også er 2.ordens svingninger i systemet. Disse bevirker, at armen bliv-er ustabil. For at omgå dette laves et filter som fjerner 2.ordens svingningenfra hastighedstilbagekoblingen. Der vælges et notch-filter, som er et båndstop-filter, for dets skarpe karakteristik. Spidsfrekvensen for filteret placeres i 40rad/sek, og overføringfunktionen bliver fra afsnit 3.4.2.

H(s) =s2 + 0.8s + 1600

s2 + 112.8s + 1600(3.5)

Efter implementering viste 2. ordens svingningerne at være dæmpede i engrad, så de ikke længere udgjorde et problem.

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

Position

tid

rad

Figur 3.8: Hastighedstilbagekobling med notch-filter

3.3.5 P-Lead

Der indsættes forsøgsvis et gain foran lead-kompensatoren. Ved trial end errorfindes frem til, at et gain på 0.8 drastisk begrænser oversvinget, og forbedrerindsvingstiden en smule. Efter implementering af denne regulator ser respon-set for systemet ud som på figur 3.8. En summering af responset for systemet

19

Kapitel 3. Simulering

Regulator Oversving Indsvingstid 2% Stationær værdi Største Fejl Stat. Hast. FejlP 0.194 >10 0.9997 0.2628 0Lead ∞ >10 ∞ 0.1561 0.042.ordens Highpass ∞ >10 ∞ 0.1428 0.0011.ordens Highpass 0.0819 3.3047 1.0014 0.1897 0Notch 0.0514 2.6960 1.0014 0.2102 0P-Lead 0.0070 1.7190 1.0014 0.1801 0

Tabel 3.3: Data fra reguleringsprocess

med de forskellige regulatorer er vist i tabel 3.3.

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

Position

tid

rad

Figur 3.9: Step med komplet regulering

3.4 Filtre og regulatorer i Matlab

Til dimensionering af regulatorerne og filtrene bruges i stor udstrækning Mat-lab’s funktioner og toolboxes.En af de væsentligste toolboxes, der bruges til filtrene er Digital Filter Design,der ligger i simulink under Signal Processing Blockset og Filter Designs. Herkan Matlab designe digitale filtre ud fra nogle, af brugeren, givne specifika-tioner.

20

3.4 Filtre og regulatorer i Matlab

−80

−60

−40

−20

0

20

40

Mag

nitu

de (

dB)

10−3

10−2

10−1

100

101

102

103

−225

−180

−135

−90

−45

0

Pha

se (

deg)

Bode DiagramGm = −18.9 dB (at 7.52 rad/sec) , Pm = −22.9 deg (at 9.27 rad/sec)

Frequency (rad/sec)

Figur 3.10: Bodeplot af modellen uden Lead-kompensator

3.4.1 Lead-kompensator

For at øge systemets båndbredde og kompensere for den negative fasedrejningbenyttes en lead-kompensator med flg. overføringsfunktion:

H(s) = Kpτds + 1

αlτds + 1(3.6)

Koefficienterne τd og αl bestemmes idet ϕm = 172. Fra [5] fig.6.28 haves:

αl =1− sin (ϕm)

1 + sin (ϕm)(3.7)

Her af bestemmes αl = 0.7557. Nu benyttes ωtop = 11.5 rad/sek igen haves fra[5] fig.6.27:

τd =1√

αl ωtop

(3.8)

Dermed bliver τd = 0.100 sek. Dermed bliver overføringsfunktionen for lead-kompensatoren:

H(s) =0.1s + 1

0.07557s + 1(3.9)

21

Kapitel 3. Simulering

Når denne z-transformeres med Matlab’s c2d-funktion og der bruges en sam-pletid på Ts = 0.01 samt den bilineære approksimationsfunktion tustin fås:

H(z) =1.303z + 1.179

z + 0.8759(3.10)

3.4.2 Notch-filter

Ved implementering af den først planlagte regulering stod det klart, at armens2.ordens svingning på omkring 40 rad/sek. blev anslået. Dette bevirkede, atsystemet blev ustabilt. På figur 3.11(b) er vist et plot af armens accelerationfor et step. For at undgå disse svingninger, blev et notch-filter foreslået. Etnotch-filter er et båndstop filter, med en meget spids karakteristik. Ved at fil-trere anden ordens svingningen fra den integrerede hastighed, skulle det væremuligt at undgå at armen blev ustabil. Overføringsfunktionen for et sådantfilter er vist herunder:

H(s) =s2 + 2ζzωNs + ωN

2

s2 + 2ζpωNs + ωN2

(3.11)

Hvor ωN er spidsfrekvensen, dvs. det sted hvor filtret dæmper mest. ζz og ζp

er dæmpninger for hhv. tæller og nævner. Det skal bemærkes, at dæmpnin-gen for tælleren skal være lavere end for nævneren, for at filtret vil være an-vendeligt. Da 2.ordens svingningen er aflæst til omkring 40 rad/sek, se bilagI.2, er dette spidsfrekvensen. For at opnå en ikke alt for smal karakteristik erdæmpningsforholdene valgt som ζz = 0.01, ζp = 1.41 Dette giver et filter medoverføringsfunktionen:

H(s) =s2 + 0.8s + 1600

s2 + 112.8s + 1600(3.12)

Frekvensresponset for filtret er vist på figur 3.12. Det ses at filtret ved førsteordens svingningen på 7,3 rad/sek. forstærker med -0,04 dB, men ved anden or-dens frekvensen forstærker med -60 dB. Andenordenssvingingen kan dog flyttesig lidt ved påvirkninger fra omgiverlserne, men også ved 36 rad/sek forstærk-er filtret med -34 dB.Et step på den fysiske arm med og uden filtret implementeret er vist i figur3.11(a). Det ses at filtret effektivt fjerner anden ordens svingningerne.

For at kunne anvende filteret til digital regulering blev en z-transformation

22

3.4 Filtre og regulatorer i Matlab

0 2 4 6 8 10−10

−8

−6

−4

−2

0

2

4

6

8

10

tid (sek)

rad/

sek2

accx

(a) Med Notch-filter

0 1 2 3 4 5−15

−10

−5

0

5

10

15

tid (sek)

rad/

sek2

accx

(b) Uden notch-filter (Udsnit)

Figur 3.11: Acceleration af endespids

foretaget. Dette gav følgende overføringsfunktion i z-domænet:

H(z) =0.06509z2 − 1.197z + 0.6459

z2 − 1.197z + 0.2968(3.13)

3.4.3 Highpass-filtre

Matlab’s toolbox Digital Filter Design bruges til at udforme highpass-filteret.Hvis knækfrekvensen vælges til fcutoff = 1.1634Hz ud fra armens 1.ordensegensvingning og samplefrekvensen vælges til Fs = 100Hz fås flg. overførings-funktion i z-domænet.

H(z) =0.9496z2 − 1.899z + 0.9496

z2 − 1.897z + 0.9018(3.14)

I toolbox’en vælges parametrene highpass, IIR Butterworth og 2.orden.

På samme måde designes 1. ordens highpass filteret. Der vælges nu en knæk-frekvens på 0.1 Hz, og dette giver overføringsfunktionen

H(z) =0.9969z − 0.9969

z − 0.9937(3.15)

23

Kapitel 3. Simulering

−300

−250

−200

−150

−100

−50

0

100

101

102

103

−360

−270

−180

−90

0

Bode DiagramGm = 161 dB (at 41 rad/sec) , Pm = −180 deg (at 0 rad/sec)

Frequency (rad/sec)

Figur 3.12: Frekvensrespons for notch-filteret

24

Kapitel4Implementering

4.1 Implementering på datamat

Herunder gives et eksempel på implementering af et filter i z-domænet på data-mat. Eksemplet er taget fra Direct realisation fra [1]. Antag at den diskreteoverføringsfunktion i z-domænet er givet som:

H(z) =u(z)

e(z)=

b0 + b1z−1 + b2z

−2

1 + a1z−1 + a2z−2(4.1)

Funktionen er blevet forsinket, så der ikke forekommer ikke kausale værdieraf z. Overføringsfunktionen kan med algebra omskrives til

u(z) = e(z)(b0 + b1z−1 + b2z

−2)− u(z)(a1z−1 + a2z

−2) (4.2)

Ved herefter at lave invers z transformation på systemet, er det muligt at om-skrive udtrykket til

u(i) = b0e(i) + b1e(i− 1) + b2e(i− 2)− (a1u(i− 1) + a2u(i− 2)) (4.3)

Hvor e er inputtet og u er outputtet. e(i− 1) betyder således inputtet forsinketen sample. Da udtrykket nu kun er en funktion af tidligere samples er detmuligt at implementere det digitalt. En pseudokode for dette er vist herunder:

%Initialization

Constants a1,a2,b0,b1,b2

Dummy=0

e1=0, e2=0, u1=0, u2=0

%Periodic part repeated once every sample

e=ref-getinput()

u=b0*e+dummy

Output(u)

e2=e1

25

Kapitel 4. Implementering

e1=e

u2=u1

u1=u

Dummy=(b1*e1+b2*e2)-(a1*u1+a2*u2)

Som eksempel kan notch-filteret betragtes. Fra afsnit 3.4.2 vides det, at dettefilter i z-domænet har overføringsfunktionen:

H(z) =0.06509z2 − 1.197z + 0.6459

z2 − 1.197z + 0.2968(4.4)

Det ses af overføringsfunktionen at a1=-1.197, a2=0.2968, b0=0.6509, b1=-1.197,b2=0.6459, hvilket giver c-koden:

temp_test_notch = 0.6509 * speedFromAcc + NOdummy1;

NOZI2 = NOZI1;

NOZI1 = speedFromAcc;

NOZU2 = NOZU1;

NOZU1 = temp_test_notch;

NOdummy1 = ((-1.197) * NOZI1 + 0.6459 * NOZI2 )

- ((-1.197) * NOZU1 + 0.2968 * NOZU2);

speedFromAcc = temp_test_notch;

For at verificere filtrene blev værdierne for et step i matlab sammenlignetmed et step på robotten, inden filtrene blev taget i praktisk brug.

4.2 Den fysiske implementering

Implementeringen af systemet skete på den fleksible robotarm på IAU. På fig-ur 4.1 ses det, hvordan accelerometeret er placeret på enden af den fleksiblestålstang. Armen er specificeret ud fra parametrene i tabel 4.1.

Armen er udstyret med et accelerometer til måling af spidsens acceleration.Et tachometer til at bestemme motorens omdrejningshastighed, og et poten-tiometer til bestemmelse af armens position. For at kunne drive motoren i sys-temet er denne igennem en operationsforstærker koblet op som en hastighedsservo

26

4.2 Den fysiske implementering

Figur 4.1: Model af armen

Parameter Størrelse EnhedLængde 1.200 mHøjde 0.040 mBredde 0.002 mMateriale StålDensitet ρ 7850 kg/m3

Youngs modulus E 2.11 · 1011 N/m2

Afstand til sensor 1.197 m

Tabel 4.1: Fysiske parametre for flexarm

med en stærk hastighedstilbagekobling. Kommunikation med datamaskinensker igennem et comedi-board, som er udstyret med analoge ind- og udgange.Dette er nøjere beskrevet bilag E. Det fulde reguleringssystem er skitseret påfigur 4.2.

27

Kapitel 4. Implementering

Figur 4.2: Hardware setup

28

Kapitel5Test

5.1 Positionsbestemmelse

For at kunne bedømme parametre som oversving, stationær værdi osv. er detnødvendigt at se på positionen for et step. Dette er imidlertid ikke lige til,da den information der er tilgængelig om endepunktet, er en acceleration somskal dobbelt integreres. Dette giver anledning til en voksende fejl på positionenforårsaget af en konstant fejl på accelerationen. For at kompensere for dettelaves en position, som er en kombination af potentiometer positionen ved lavefrekvenser og accelerometer positionen ved høje frekvenser. Dette gøres som iformel 5.1. Det ses at hvis de to positioner er ens vil filtret give 1.

θ =2ζωns + ω2

n

s2 + 2ζωns + ω2n

· θPot +s2

s2 + 2ζωns + ω2n

· θAcc (5.1)

Knækfrekvensen ,ωn, for filteret er valgt til 1/3 af frekvensen for 1. ordenssvingningen, dvs. ωn = 2.5. Dæmpningen er sat til ξ = 0.6 som er fundet vedforsøg. En beskrivelse af forsøget kan findes i appendiks F.Ud fra de 2 grafer 5.1 (a) og (b) fremgår det, at den filtrerede model ikke er heltnøjagtig. Der er en afvigelse på den stationære værdi på 0.015 rad svarende til1.5 %. Derfor vil stationære fejl blive aflæst på potentiometer målingen somer en sand værdi for den stationære fejl. Indsvingstid kan med fordel aflæsespå accelerometermålingen, så plottet af positionen er hovedsagelig for at givelæseren et begreb om endespidsens bevægelse.

5.2 Karakteristik af step på robotarmen

For at have noget at sammenholde senere måledata med, laves et step påarmen. Denne er sat op som positionsservo med en proportionalregulator påKp = 1. På figur 5.2 ses positionen af armens endespids ved et step med 1 rad.Ud fra dette kan parametrene i tabel 5.1 for kvaliteten af reguleringen bereg-

nes.

29

Kapitel 5. Test

0 1 2 3 4 50

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Kombineret position af potpos og accpos

(a)

0 1 2 3 4 50

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Data fra film

(b)

Figur 5.1: Position af endespids

Oversving Indsvingstid 2% Stationær værdi Største Fejl Stat. Hast. Fejl0.0421 > 10 0.9869 0.1927 0

Tabel 5.1: Parametre for arm med simpel regulering

Vha. matlab er det muligt at lave et plot af accelerationen i frekvensdomænet.Dette er vist på figur 5.3.Det ses at 1.02 Hz er særligt repræsenteret. Dette er frekvensen for armens 1.ordens svingning. Det er denne frekvens det er målet at dæmpe.

5.3 Step med regulering

For at have et kvalitativt mål for effekten af reguleringen vises på figur 5.4 atil f respons for et enhedsstep med og uden hastighedstilbagekobling. Her be-nyttes Kp = 2.5, da resultaterne skal være sammenlignelige med det fuldt reg-ulerede system. I tabel 5.2 ses det at indsvingstiden er blevet forbedret fra > 10

til 2.3sek. Oversvinget er endvidere reduceret fra 0.2rad til 0.05rad. Det er klart,at hvis Kp = 1 i systemet uden hastighedstilbagekoblingen så vil oversvingetvære mindre men bevægelsen vil også være det langsommere.

Oversving Indsvingstid 2% Stationær værdi Største Fejl Stat. Hast. FejlUden regulering 0.2063 > 10 1.0098 0.2296 0Med regulering 0.0494 2.3404 1.0085 0.0897 0

Tabel 5.2: Resultater for regulering

30

5.4 Respons på forskellige inputs

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Kombineret position af potpos og accpos

Figur 5.2: Fysisk system med positionstilbagekobling

5.4 Respons på forskellige inputs

For at teste reguleringens evne til at følge andre funktioner end et step påtryk-kes disse i simuleringen. Et kvantitativt mål for armens evne til at følge bevæg-elsen er RMS af afvigelsen imellem den påtrykte bane og den reelle bevægelse.Forskellige inputfunktioner er vist i skema 5.3 for både simuleringen og denfysiske arm. Fejlen er her defineret som forskellen på inputtet og armens en-deposition. Det ses som forventet, at både den maximale fejl og RMS fejlen

Input Simulering FlexarmMax fejl Rms fejl Max fejl Rms fejl

Step(0.2) 0.2009 0.0408 0.2187 0.1914Step(1) 1.0070 0.1824 1.1086 0.8935Sinus(0.2 t) 0.0946 0.0586 0.0791 0.0533Sinus(0.5 t) - - 0.1661 0.1033Sinus(1 t) - - 0.4596 0.1741Rampe(0.2) 0.1003 0.0904 0.1189 0.1061Rampe(1) 0.5095 0.4641 0.4218 0.3256

Tabel 5.3: Respons på forskellige inputs

bliver større når hastigheden på inputtet vokser. Som et eksempel er plottetfor 0.5 · sin(1t) vist på figur 5.5. Det fremgår at armens position er 0.50 sek.

31

Kapitel 5. Test

0 1 2 3 4 50

20

40

60

80

100

120

140

160

180

200Frequency content of accx

frequency (Hz)

Figur 5.3: Fysisk system uden regulator

forsinket i forhold til referencen.

5.5 Forskydning af egenfrekvens

Der monteres en load på armen så dens egenfrekvensen forskydes. Målet meddette er at forskubbe egenfrekvensen for 2. ordens svingningen ud af notch-filteret og derved provokere armen til at gå i resonans. Belastningen påtryk-kes ved 2 skruetvinger på ialt 0.332 kg som placeres på spidsen, 1.20 m fraomdrejningspunktet. Plots for en svingning, kun med positionstilbagekobling,med og uden ekstra belastning er vist på figur 5.6(a). Det ses at egenfrekvensenfor 1. ordens svingningen er blevet lavere med en belastning. Til gengæld erudsvinget blevet større. Dette stemmer udmærket overens med teorien. Frekvensener blevet formindsket med 30 %. Desuden fremgå det tydeligt at indsvingsti-den er steget. Dog er det ikke lykkedes med den anvendte belastning at flytte2. ordens svingningen i en grad så den blev mærkbar.Ud fra ligning 2.20 er det herefter muligt at bestemme den teoretiske 2. or-dens svingning med denne belastning til 27.76 rad/sek. Ved denne frekvensforstærker notch-filteret med -13 dB. På figur 5.6(b) ses svingningen for et stepmed den fulde regulering inkluderet. Det ses at accelerometer tilbagekoblin-

32

5.5 Forskydning af egenfrekvens

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Potentiometer position

(a)

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Potentiometer position

(b)

0 2 4 6 8 10−15

−10

−5

0

5

10

15

tid (sek)

rad/

sek2

accx

(c)

0 2 4 6 8 10−15

−10

−5

0

5

10

15

tid (sek)

rad/

sek2

accx

(d)

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Position

(e)

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Position

(f)

Figur 5.4: Steprespons

33

Kapitel 5. Test

0 5 10 15 20 25 30 35 40−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

tid (sek)

rad

correction speed

referenceposition

Figur 5.5: Respons på et sinus input

gen formindsker udsvingene, men ikke i en grad som for en arm, der ikke erbelastet, se fig.5.6.Artikel [10] foreslår at implementere et adaptivt filter, til at fjerne svingning-

0 2 4 6 8 10−0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

tid (sek)

rad

Position

Med LoadUden Load

(a) Uden hastighedstilbagekobling, med oguden load.

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

tid (sek)

rad

Kombineret position af potpos og accpos

(b) Med regulering og load.

Figur 5.6: Step med forskellige setup

erne i en robotarm. Dette vil bevirke at filteret altid arbejder omkring denaktuelle egenfrekvens. Denne type regulering går dog ud over formålet meddenne rapport.

34

Kapitel6Diskussion

Resultaterne opnået i denne rapport er fundet under faste rammer i et labo-ratoriemiljø. Dette er tilstrækkeligt til at konkludere at teknologien fungerer,men en del arbejde er stadig nødvendigt, før dette kan implenenteres på enrobot i industrien.

6.1 Anvendelighed

Tages der tages udgangspunkt i, at det er muligt at dæmpe svingningerne ien given robotarm ved brug af en sensor som et accelerometer, vil teknolo-gien umiddelbart være anvendelig da de samlede omkostninger af en robotarmmed sensorer til dæmpning af svingninger er mindre end en robotarm der iprincippet er overdimensioneret for at undgå svingninger. Teknologien er i dethele taget anvendelig i forhold til at minimere omkostningen og massen afrobotarme generelt. Hvilket kunne tænkes fordelagtigt i industrier med sam-lebåndsarbejde, hvor det åbner op for at have mere mobile robotter. Til gengældvil teknologien have mindre relevans i applikationer hvor omkostningerne harmindre betydning og hvor robotterne alligevel er store og bevæger sig langsomt.

6.2 Videreudvikling

Til fremtidig implementering af regulering med accelerationstilbagekobling, erder flere ting, som ville kunne forbedres.Accelerometeret som er anvendt i dette projekt er et billigt multi purpose kom-ponent. Kombineret med lange signalledninger, giver dette mulighed for en delstøj, og tilsvarende dårligere regulering. Desuden er hastigheden af et udsvingbegrænset af, at accelerometeret går i mætning ved ± 2g. En oplagt forbedringville derfor være at udskifte sensoren til et mere sikkert komponent med etstørre måleområde.Setuppet i denne rapport er kraftigt simplificeret i forhold til en robot i indus-trien. Der kunne sagtens forekomme flere end 1 frihedsgrad (led), og der ville

35

Kapitel 6. Diskussion

derfor være brug for en matematisk model til at beregne vinkelaccelerationenaf et led ud fra den målte acceleration. Til dette kunne en model med Denavit-Hartenberg parametre med fordel benyttes. Dette ville også give mulighed forat svinge i et plan med påvirkning af tyngdekraft.Skulle softwaren bruges til test i et miljø, hvor det ikke var hensigtsmæssigt atcompile hele tiden, ville en GUI til systemet være på sin plads. Denne kunnef.eks. modtage en inputfunktion og en køretid som parametre.I denne rapport er der kun kort blevet behandlet step med en ekstra load på ar-men, og reguleringen er ikke forsøgt specifikt optimeret til denne. Dette kunneomfatte et projekt hvor der i stedet for filtre, optimeret til armen uden belast-ning, benyttedes adaptive filters som var i stand til at tilpasse sig efter massenaf et tool.Endelig kunne der udvikles et sæt indstillingsregler, som gjorde det muligtfor ingeniører at montere et accelerometer på deres robot, og derefter vha. enrække nedskrevne trin, at implementere reguleringen på en anden robot.Hvis en motor, som ikke er koblet tilbage, dvs. en momentstyret motor blevbenyttet i stedet for en hastighedsservo ville det være muligt at beregne en mo-mentfejl frem for en hastighedsfejl. Dette ville gøre integrationen unødvendigog derfor med stor sandsynlighed øge hastigheden af reguleringen.

36

Kapitel7Konklusion

I denne rapport er der blevet arbejdet med dæmpning af svingninger i en ro-botarm. Mere præcist har arbejdet været fokuseret på at dæmpe svingning-erne i en robotarm med en frihedsgrad ved at tilbagekoble outputtet fra etaccelerometer. Gennem arbejdet med robotarmen er der blevet sat en simpelplatform op, bestående af den mekaniske arm og motoren med en hastighed-stilbagekobling gennem en forstærker. Derudover blev der konstrueret en posi-tionstilbagekobling og en accelerationstilbagekobling gennem et Comedi-boardder var sat op på en computer, se fig.4.2.Således fungerede computeren som platform for implementering af de valgteregulatorer og filtrer.

Gennem arbejdsprocessen blev der undersøgt flere filtrer og regulatorer, samtforskellige kombinationer af disse. Designet af filtrer og regulatorer blev typiskgjort i Matlab, hvor transformationerne mellem diskret tid og kontinuert tidogså blev foretaget.

I den endelige opsætning, se bilag II.4, blev der valgt et notch-filter, et 1.or-dens og et 2.ordens Highpass filter til at filtrere hastigheden fra den integr-erede acceleration. Af regulatorer blev der valgt en proportionalregulator og enLead kompensator.

Det viste sig at være muligt at mindske indsvingstiden fra over 10sek til 2.3sek

og ligeledes reducere oversvinget fra 0.2rad til 0.05rad, se fig.5.4.

37

Litteratur

[1] Andersen N., Implementation of digital controllers.

[2] Baungaard J. R., Modelling and control of flexible robot links, Ph.D. The-sis, Department of automation, 1996.

[3] Futami S., Kyura N., Hara S., Vibration Absorption Control of IndustrialRobots by Acceleration Feedback, IEEE TRANSACTIONS ON INDUSTRI-AL ELECTRONICS, VOL. IE-30, NO. 3, AUGUST 1983, s.299-305.

[4] Hillsley K. L., Yurkovich S., Vibration Control of a Two-Link Flexible Ro-bot Arm, Proc. 1991 IEEE Conference on Robotics and Automation, Sacra-mento, California, April 1991, s.2121-2126.

[5] Jannerup O., Sørensen P.H., Reguleringsteknik, 4.udgave, 1.oplag, Poly-teknisk Forlag, 2006.

[6] Lathi B. P., Signal processing & linear systems, Oxford university press,1998.

[7] Miller, Freund, Probability and Statistics for Engineers, udgave, oplag,forlag, udgivelsesår.

[8] http://www.villanova.edu/engineering/departments/mechanical/undergrad/mechatronics.htm.

[9] Schilling R. J., Fundamentals of robotics, analysis & control, Prentice Hall,1990.

[10] Tjahyadi H., He F. , Sammut K., Vibration Control of a Cantilever BeamUsing Adaptive Resonant Control, 5th Asian Control Conference, Vol.3,s.1776-1780, 20.-23. Juli, 2004.

[11] Whitney S., Vibrations of Cantilever Beams: Deflection, Frequency, and Re-search Uses, april, 1999.

38

AppendiksASymbolliste

Symbol Beskrivelse Enhed

α Vinkelacceleration rad/s2

αl Skalering til Lead –µf Viskosfriktion Nm/(rad/s)

ω Vinkelhastighed rad/s

ωn Knækfrekvens rad/s

ωtop Vinkelhastighed rad/s

ρ Densitet kg/m3

θ Vinkelposition rad

θb Vinkelposition rad

θo Vinkelposition rad

τd Tidskonstant s

τi Tidskonstant s

ϕm Fasedrejning rad

σ Standard afvigelsen –ξp Dæmpning for poler i filter –ξz Dæmpning for nulpunkter –E Youngs modulus N/m2

Eα Maximal fejl mrad/s2

es Hastighed - spænding, tacometerforhold V · sfcutoff Cutoff frekvens for HP-filter Hz

Fs Samplefrekvensen Hz

Gm Gain magin dB

Jb Inertimoment for armen kg ·m2

Jm Inertimoment for motoren kg ·m2

mg Gear ratio –N Antal af samples –K1 Gain for for analog forstærker –Ke Motor constant A · sekKp Gain –

Fortsættes på næste side

39

Appendiks A. Symbolliste

Symbol Beskrivelse Enhed

Kpm Spænding - strømforhold I/V

Kt Strøm - momentforhold Nm/A

Pm Fasemargen grader

Ts Sampletid s

Zα/2 Kumuleret sandsynlighed –

Tabel A.1: Liste med symboler

40

AppendiksBOverføringsfunktionen for modellen

θo = mg1

sq̇ + G2(s) (mg q̈) (B.1)

a = mg (Jb (mg q̈)−G1(s) (mg q̈)) (B.2)

b = µf q̇ (B.3)

c = Keq̇ (B.4)

d = K1esq̇ (B.5)

θb = mg1

sq̇ (B.6)

q̇ =1

sq̈ (B.7)

q̈ =1

Jm

((Kt

1

Ra

(Kpm (Kd(e− θb)− d)− c)− b

)− a

)(B.8)

q̈ = KtKpmKde s2

(Jm+m2gJb−m2

gG1(s))Ra s2+(KtKpmK1es+KtKe+µf Ra) s+KtKpmKdmg(B.9)

θo =KtKpmKd(1+G2(s)s2)

(Jm+m2gJb−m2

gG1(s))Ra s2+(KtKpmK1es+KtKe+µf Ra) s+KtKpmKdmge (B.10)

H(s) =KtKpmKd(1+G2(s)s2)

(Jm+m2gJb−m2

gG1(s))Ra s2+(KtKpmK1es+KtKe+µf Ra) s+KtKpmKdmg(B.11)

41

Appendiks B. Overføringsfunktionen for modellen

G1(s) = 52319.112 s2−11339.08395 s−7.139068756·107

2500 s4+2100.425 s3+5.373523456·106 s2+8.957746871·105 s+2.79823609·108 (B.12)

G2(s) = −2908.962980 s2−1562.736275 s−4.43119284·106

2500 s4+2100.425 s3+5.373523456·106 s2+8.957746871·105 s+2.79823609·108 (B.13)

H(s) = −46.11619924 s4+60.63179697 s3+1.062607340·105 s2+1.010109129·105 s+3.155395949·107

3.228297525 s6+282.2811687 s5+7287.147946 s4+6.023380281·105 s3+1.297483648·106 s2+3.139299674·107 s+3.155395949·107

(B.14)

42

AppendiksCRHD

For at forbinde til terminalen med RTAI følges følgende trin. Programmetkøres på maskinen rt14, og er gemt i mappen simpleclient. For at kunne køreprogrammer i RHD skal programmet RHD-client køre. Logning sker til filenlogile.dat. Programmet køres i 10 sek.

C.1 Start RHD client

Start en maskine op i Linux.Start en terminal og tast:

>ssh rt4

>"password"

>cd rhd/branches/rhd2.0/build/bin

>su -c "./rhd"

>"password"

C.2 Ret i programmet

For at rette i programmet åbnes filen testprog.c i en teksteditor

C.3 Compile

Start en ny terminal og tast

>ssh rt4

>"password"

>cd rhd/branches/rhd2.0/build/bin

>make

43

Appendiks C. RHD

C.4 Eksekver

Start en ny terminal og tast

>ssh rt4

>"password"

>cd rhd/branches/rhd2.0/testprogs/simpleclient

>./simpleclient rt14 1000

44

AppendiksDAccelerometer og sampling

D.1 Antal samples

I dette projekt er der benyttet et breakoutboard af typen DE-ACCM2G fra di-mensionengineering. Denne har en ADXL322 kreds fra Analog Devices, som eret accelerometer med et måleområde på ±2g. På breakoutboardet er accelerom-eteret sat op til at have en båndbredde på 500Hz og en følsomhed på 750mV/g.ADC’en på Comedi-boardet har en opløsning på 12bit, hvilket giver mulighedfor at måle accelerationer med en opløsning på 0.3072 målinger pr. g.Accelerometeret har en målbar varians, og der kan derfor med fordel samplesflere gange og midles over disse. Standard afvigelsen er ud fra forsøg målt til3.82 mrad/sek2. Desuden kan fejlen på målingerne antages at være normal-fordelt. For at undgå at disse fejlmålinger får for stor indflydelse, er det ønske-ligt at fejlen er under 2 mrad/sek2. Det antal samples, der er nødvendigt for atsikre dette i et 95 % konfidensinterval, er givet ved1:

N =

(Zα/2 · σ

Eα

)2

=

(1.96 · 3.82

2

)2

= 13.9 ≈ 14 (D.1)

Der vil derfor i den praktiske implementering blive anvendt 14 samples sombaggrund for hver måling.

D.2 Sampling og tider

For at undgå aliasing er det vigtigt at holde samplingfrekvensen over nyquist-frekvensen på ωs = 2ω. Der samples med 100 Hz i programmets hovedløkke.Dette bevirker, at frekvenser over 50 Hz vil blive opfattet forkert. Ud fra teorieni afsnit 2.3 vides det dog, at 2.ordens egenfrekvensen for armen er på 7.3 Hz,og 3.ordens svingningen på 20.4 Hz, hvilket ikke overstiger de 50 Hz.

1Se [7] s. 231

45

AppendiksEComedi-Board

Boardet der blev anvendt er af typen PCI-DAS6025. Dette er et board til inter-face til målinger på en linux-baseret maskine. Fra et C-program er det muligtat læse og sætte analog værdier på en række porte på boardet. Syntaksen forat skrive en værdi er givet som følger:

int comedi_data_write(comedi_t * device, unsigned int subdevice,

unsigned int channel, unsigned int range, unsigned int aref,

lsampl_t data);

Parametrene for funktionen er givet i tabel E.1

Ud fra disse skrives til port 0 med ± 10V som følger:

comedi_data_write(cf, sout, 0, 0, 0, value);

hvor value er den værdi der skrives. På samme måde kan der læses fra board-et med funktionen

int comedi_data_read_delayed(comedi_t * device, unsigned int subdevice,

unsigned int channel, unsigned int range, unsigned int aref,

lsampl_t * data, unsigned int nanosec);

som læser en spænding fra en port efter at have ventet et givet tidsrum. Dettedelay er nødvendigt, for at AD-converteren på boardet skal være færdig meden måling før den næste tages.

Yderligere information kan findes på: http://www.comedi.org/

Parameter Betydningcomedi_t * device En pointer til portenunsigned int subdevice Nummeret på subdivisetunsigned int channel Den kanal der skal skrives tilunsigned int range Skalering af inputtetunsigned int aref Spændingsintervalletlsampl_t data Spændingen der skal sættes

Tabel E.1: Comedi parametre

46

AppendiksFMetode til positionsbestemmelse

For at verificere at de data der er fremkommet fra robotten igennem et høj- oglavpasfilter er tilsvarende, filmes et step. Forsøgsopstillingen ser ud som på fig-ur F.1. Der er opsat hvide stykker tape som reference til afstande. Ud fra denne

Figur F.1: Arm setup

film er det muligt for hvert frame at markere endepunktets position. Da filmener optaget med 30 frames / sek. er det muligt at observere 1. ordens svingning-en. Ud fra disse punkter kan vinklen i forhold til startpositionen bestemmesved formlen (F.1), hvor a og b begge er afstanden fra omdrejningspunktet til detobserverede punkt, og c er afstanden fra startpunktet til det aktuelle punkt.Cx og Cy er koordinaterne for centrum. Herefter kan vinklen til et givet punkt(54.1, 74.9) findes som i formel F.3

cos(θ) =a2 + b2 − c2

2ab⇒ 2r2 − c2

2r2(F.1)

c =√

(x− Cx)2 + (y − Cy)2 (F.2)

47

Appendiks F. Metode til positionsbestemmelse

cos(θ) =2 · 1.1972 − 37.742

2 · 1.1972= 0.9484 ⇒ θ = 0.571rad (F.3)

Disse punkter former grafen på figur 5.1(b). Denne graf, kan herefter sammen-holdes med den graf der er fremkommet ud fra den filtrerede acceleration.

48

BilagIGrafer

−100 −80 −60 −40 −20 0 20 40 60−50

−40

−30

−20

−10

0

10

20

30

40

500.250.50.680.80.88

0.94

0.975

0.994

0.250.50.680.80.88

0.94

0.975

0.994

20406080100

Pole−Zero Map

Real Axis

Imag

inar

y A

xis

Figur I.1: Pole-Zero map fra Matlab

49

Bilag I. Grafer

0 10 20 30 40 500

50

100

150

200

250

300

350

400Frequency content of accx

frequency (Hz)

Figur I.2: Bodeplot af 2.ordens svingning af arm

50

BilagIIModeller

Figur II.1: Ekspanderet flex-blok.

51

Bilag II. Modeller

Figur II.2: Hastighedsservo

52

Figur II.3: Den basale model ekspanderet.

53

Bilag II. Modeller

Figur II.4: Det samlede system med regulering

54

BilagIIIKildekode

Source Code: testprog.c1 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ Reguleringsprogtam t i l 1DOF robotarm med ∗3 ∗ accelerometer t i labgekobl ing . ∗4 ∗ Lasse Olesen og Nico la j Lindhard ∗5 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /67 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Version contro l information ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /8 #define VERSION "$Rev : 91 $ : "9 #define DATE " $Date : 2008−09−26 15:47:28 +0200 ( Fri , 26 Sep 2008) $ : "

10 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /11 #define const_END_POS_relative 1 / / End pos i t ion of arm in rad . 1 gives one rad12 #define const_INIT_FACTOR 100 / / Factor used for i n i t i a l i z a t i o n .13 #define const_LENGTH_OF_ARM 1.98 / / Length of arm to accelerometer14 #define const_GRAVITY 9.82 / / Gravity15 #define const_SAMPLES_PR_SECOND 100 / / Samplerate1617 #define channel_X 2 / / kanaler t i l boardet18 #define channel_Y 119 #define channel_pot 02021 #include <std io . h>22 #include < s t d l i b . h>23 #include <str ing . h>24 #include <sys / time . h>25 #include <unistd . h>26 #include <rhd . h>27 #include <comedilib . h>2829 int main ( int argc , char ∗ argv [ ] ) {3031 symTableElement ∗symTable ;32 double oldTime = 0 , newTime = 0;3334 /∗ Pointere t i l arrays t i l logging ∗ /35 double ∗timestamps = NULL , ∗accX_log = NULL , ∗pos_log = NULL , ∗accX_log_fil_digital = ←↩

NULL ;36 int ∗current_log = NULL ;3738 unsigned int accXInit = 0;39 int n = 0;4041 /∗ Pointer t i l l o g f i l e ∗ /42 FILE ∗logFile ;43 int samples = 0 , counter = 0;

55

Bilag III. Kildekode

4445 /∗Regulering ∗ /46 int correction = 0;47 double speedFromAcc = 0;48 double speedFromAcc_temp = 0;49 double const_END_POS = 0;5051 /∗Comedi variable ∗ /52 int sout ;53 int sinput ;54 int maxdata ;55 int subdev = 0; /∗ change this to your input subdevice ∗ /56 int chan = 0; /∗ change this to your channel ∗ /57 lsampl_t position ;58 lsampl_t accelerationX ;5960 /∗ Variabler t i l regulering ∗ /61 double accCalX = 0;62 double InitStartPos = 0;63 double posCal = 0;64 double angularSpeed = 0;65 int dummy_counter = 0;66 double temp_end_pos = 0;67 double compare_dummy = 0;68 double HPZI1 = 0 , HPZI2 = 0 , HPZU1 = 0 , HPZU2 = 0;69 double NOZI1 = 0 , NOZI2 = 0 , NOZU1 = 0 , NOZU2 = 0;70 double LEZI1 = 0 , LEZU1 = 0;71 double HPZ2I1 = 0 , HPZ2U1 = 0;72 double temp_test_high = 0 , HPdummy1 = 0;73 double temp_test_notch = 0 , NOdummy1 = 0;74 double temp_test_high2 = 0 , HPdummy2 = 0;75 double temp_test_lead = 0 , LEdummy1 = 0;7677 /∗Find revis ion number from SVN Revision ∗ /78 char ∗i ,versionString [64] = VERSION , tempString [ 1 0 ] ;79 i = strrchr (versionString , ’ $ ’ ) ;80 strncpy (tempString ,versionString+1 ,(i−versionString−1) ) ;81 tempString [ (i−versionString−1) ] = 0 ;82 printf ( " ∗∗∗∗∗∗∗∗∗∗ Robot Hardware Daemon Client Test %s ∗∗∗∗∗∗∗∗∗∗\n\n" ,tempString )←↩

;8384 / / Open comedi85 comedi_t ∗cf ;86 cf = comedi_open ( " / dev / comedi0 " ) ;87 i f (cf == 0)88 printf ( " c f = 0 ! ! \ n" ) ;89 e lse90 printf ( " c f connected ! ! \ n" ) ;9192 sout = comedi_get_write_subdevice (cf ) ;93 sinput = comedi_get_read_subdevice (cf ) ;94 maxdata = comedi_get_maxdata (cf ,subdev ,chan ) ;95 comedi_data_write (cf , sout , 0 , 0 , 0 , 2048) ;9697 /∗Accelerometer i n i t i a l i t i o n ∗ /

56

98 f o r (n = 0; n < const_INIT_FACTOR ; n++) {99 comedi_data_read_delayed (cf , sinput , channel_X , 0 , 0 , &accelerationX ,5000) ;

100 accXInit += accelerationX ;101 }102 accXInit /= const_INIT_FACTOR ;103104 comedi_data_read_delayed (cf , sinput , channel_pot , 0 , 0 , &position ,5000) ;105 InitStartPos = ( ( ( double ) position∗1.216 ) /1000 − 0.925) ;106 const_END_POS = const_END_POS_relative + InitStartPos ;107108 /∗Hostname given as input ∗ /109 i f (argc >= 2) {110 strncpy (versionString ,argv [ 1 ] , 64 ) ;111 printf ( "Hostname read from input : %s\n" ,versionString ) ;112 } e lse {113 strncpy (versionString , " 127 .0 .0 .1 " ,64) ;114 printf ( "No hostname in input . Connecting to loca lhos t\n" ) ;115 }116117 /∗ Allocerer arrays t i l logging ∗ /118 i f (argc >= 3) {119 printf ( " Logging enabled\n" ) ;120 samples = atoi (argv [ 2 ] ) ;121 printf ( " Logging %d samples then quitt ing\n" ,samples ) ;122 logFile = fopen ( " l o g f i l e . dat " , "w" ) ;123 timestamps = realloc (timestamps , ( samples+10) ∗ s i z e o f ( double ) ) ;124 current_log = realloc (current_log , ( samples+10) ∗ s i z e o f ( int ) ) ;125 accX_log = realloc (accX_log , ( samples+10) ∗ s i z e o f ( double ) ) ;126 pos_log = realloc (pos_log , ( samples+10) ∗ s i z e o f ( double ) ) ;127 accX_log_fil_digital = realloc (accX_log_fil_digital , ( samples+10) ∗ s i z e o f ( double ) ) ;128 }129130 i f (rhdConnect ( ’ r ’ ,versionString , 0 ) > 0) {131 printf ( " Connect completed\n\n" ) ;132 symTable = getSymbolTable ( ’ r ’ ) ;133134135 /∗−−−−−−−−−−−−−−−−−−Her starter loopet−−−−−−−−−−−−−−−−−−−−−−∗ /136 while ( (rhdSync ( ) > 0) && (counter <= samples ) ) {137 / / Print time138 symTable = getSymbolTable ( ’ r ’ ) ;139 newTime = ( double )symTable [ 0 ] . timestamp [ 0 ] . tv_sec + ( double )symTable [ 0 ] .timestamp←↩

[ 0 ] . tv_usec / 1000000;140141 i f ( (logFile != NULL ) ) {142 printf ( " Period time %6.4 f %d %d\n" ,newTime − oldTime ,counter ,samples ) ;143 oldTime = newTime ;144145 / / Print read buffer146 printf ( "Read:\n" ) ;147 / /−−−−−−Der hentes vaerdier fra sensorene−−−−−−−−−−−−−−−148 / / midler over nogle samlpes149 int temp_average = 0;150 f o r (dummy_counter=0; dummy_counter < 14; dummy_counter++) {151 comedi_data_read_delayed (cf , sinput , channel_X , 0 , 0 , &accelerationX ,5000) ;

57

Bilag III. Kildekode

152 temp_average = temp_average + accelerationX ;153 }154155 accelerationX = temp_average / 14;156157 comedi_data_read_delayed (cf , sinput , channel_pot , 0 , 0 , &position ,5000) ;158 / / accXInit = 2540; / / ! ! ! !HARDCODET! ! ! !159 accCalX = ( ( double )accelerationX − ( double )accXInit ) ∗10 / 1536; / / Magic ←↩

constants160 posCal = ( ( ( double ) position∗1.216 ) /1000 − 0.925) ;161 accCalX = accCalX ∗ const_GRAVITY / const_LENGTH_OF_ARM ;162163 / / skalerer for skaevhed i accelerometret164 i f (accCalX > 0) {accCalX = accCalX / 0 . 8 6 1 ; }165 i f (accCalX < 0) {accCalX = accCalX / 0 . 7 7 ; }166167 / / Vender koordiantsystemet korrekt168 accCalX = −accCalX ;169170171 i f (posCal > 1.900) / / armen er ude af det t i l l a d t e omraade172 {173 posCal = 1.900;174 printf ( "Arm out of bound ! ! ! ! ! ! ! ! \ n" ) ;175 }176 e lse i f (posCal < 0.010)177 {178 posCal = 0.010;179 printf ( "Arm out of bound ! ! ! ! ! ! ! ! \ n" ) ;180 }181182 /∗−−−Slut paa hentning af vaerdier fra sensore−−−−−∗ /183184 /∗−−−−−−−−−−−Regulering beregninger−−−−−−−−−−−−−−∗ /185186 /∗ Integrerer accelerationen ∗ /187 speedFromAcc_temp = speedFromAcc_temp + accCalX ;188 speedFromAcc = speedFromAcc_temp /const_SAMPLES_PR_SECOND ; / / Scalere for samples pr ←↩

second189190 /∗Laver en rampe paa indputtet for at undgaa en for kra f t ig start ∗ /191 temp_end_pos = InitStartPos + counter ∗ 0.020; / / 0.02192 i f (temp_end_pos > const_END_POS )193 {temp_end_pos = const_END_POS ; }194195 /∗Laver en sinus paa inputtet ∗ /196 / / temp_end_pos = InitStartPos + 0.5 ∗ sin ( counter ∗ 0.01) ;197198 / / Laver en pos i t ionsservo199 angularSpeed = (temp_end_pos − posCal ) ∗ 2 . 5 ;200201 /∗−−−−−−−−−Laver Z highpass−−−−−−−−−−−−−−−−∗ /202 temp_test_high = 0.9496 ∗ speedFromAcc + HPdummy1 ;203204 HPZI2 = HPZI1 ;

58

205 HPZI1 = speedFromAcc ;206 HPZU2 = HPZU1 ;207 HPZU1 = temp_test_high ;208209 HPdummy1 = (− 1.899 ∗ HPZI1 + 0.9496 ∗ HPZI2 ) − ((−1.897) ∗ HPZU1 + 0.9018 ∗ ←↩

HPZU2 ) ;210211 speedFromAcc = temp_test_high ;212 /∗−−−−−−−−−Slut paa Z highpass−−−−−−−−−−−−−∗ /213214 /∗−−−−−−−−−Laver Z Notch−−−−−−−−−−−−−−−−−−∗ /215 temp_test_notch = 0.6509 ∗ speedFromAcc + NOdummy1 ;216217 NOZI2 = NOZI1 ;218 NOZI1 = speedFromAcc ;219 NOZU2 = NOZU1 ;220 NOZU1 = temp_test_notch ;221222 NOdummy1 = ((−1.197) ∗ NOZI1 + 0.6459 ∗ NOZI2 ) − ((−1.197) ∗ NOZU1 + 0.2968 ∗ ←↩

NOZU2 ) ;223224 speedFromAcc = temp_test_notch ;225 / / compare_dummy = speedFromAcc ;226 /∗−−−−−−−−−Slut paa Z Notch−−−−−−−−−−−−−−−−∗ /227228 /∗−−−−−−−−−Laver 1 ordens Z highpass−−−−−−∗ /229 temp_test_high2 = 0.996868 ∗ speedFromAcc + HPdummy2 ;230231 HPZ2I1 = speedFromAcc ;232 HPZ2U1 = temp_test_high2 ;233234 HPdummy2 = (− 0.996868 ∗ HPZ2I1 ) − ((−0.993736) ∗ HPZ2U1 ) ;235236 speedFromAcc = temp_test_high2 ;237 compare_dummy = temp_test_high2 ;238 /∗−−−−−−−−−Slut paa 1 ordens Z highpass−−−−−∗ /239240 / / Hastighedstilbagekobling241 angularSpeed = (angularSpeed − speedFromAcc ) ∗ 0 . 8 ;242243 /∗−−−−−−−−−Laver Z Lead−−−−−−−−−−−−−−−−−−∗ /244 / / compare_dummy = angularSpeed ;245 temp_test_lead = 1.303 ∗ angularSpeed + LEdummy1 ;246247 LEZI1 = angularSpeed ;248 LEZU1 = temp_test_lead ;249250 LEdummy1 = ((−1.179) ∗ LEZI1 ) − ((−0.8759) ∗ LEZU1 ) ;251252 angularSpeed = temp_test_lead ;253 /∗−−−−−−−−−Slut paa Z Lead−−−−−−−−−−−−−−−−∗ /254255 /∗−−−−−−−Regulering slut−−−−−−−−−−−−−−−−−−−−∗ /256257 /∗−−Udskriver parametre t i l terminalen−−−∗ /

59

Bilag III. Kildekode

258 i f (accCalX > 18.000)259 {printf ( " ! ! ! ACC LIMIT ! ! ! " ) ; }260 printf ( " pos i t ion : %l f \n" ,posCal ) ;261 printf ( " acce lerat ion : %l f \n" ,accCalX ) ;262 printf ( " temp tes t high 2: %l f \n" ,compare_dummy ) ;263 printf ( " InitPos : %l f \n" ,InitStartPos ) ;264 printf ( " Acc i n i t : %u\n" , accXInit ) ;265 }266267 /∗−−−−−−−−−−−Saetter hastigheden paa motoren−−−−−−−−−−−−∗ /268 correction = ( int ) ( ( ( angularSpeed∗1000) / 2 .3 ) + 2048) ;269 printf ( " correc t ion : %d\n" ,correction ) ;270 comedi_data_write (cf , sout , 0 , 0 , 0 , correction ) ;271272 / / Log i f samples are defined273 i f ( (logFile != NULL ) ) {274 timestamps [counter ] = newTime ;275 current_log [counter ] = correction ;276 pos_log [counter ] = posCal ;277 accX_log [counter ] = accCalX ;278 accX_log_fil_digital [counter ] = temp_end_pos ;279 i f (samples > 0) counter++;280 }281 }282 /∗−−−−−−−−−−−−−−−−−−−Slut paa main loop−−−−−−−−−−−−−−−−−−−−−−−−∗ /283 } e l se {284 printf ( " Connect f a i l e d \n" ) ;285 }286287 /∗ Skriver t i l l o g f i l e n ∗ /288 i f (logFile != NULL ) {289 printf ( " Saving log to f i l e . . . \ n" ) ;290 f o r (counter = 0; counter < samples ; counter++) {291 fprintf (logFile , " %10.4 f %l f %l f %d %f l \n" ,timestamps [counter ] ,pos_log [counter ] ,accX_log [←↩

counter ] ,current_log [counter ] ,accX_log_fil_digital [counter ] ) ;292 }293 }294 /∗−−−Slukker for motoren−−−∗ /295 printf ( " Saving complete\n" ) ;296 printf ( " Turning o f f engine\n" ) ;297 comedi_data_write (cf , sout , 0 , 0 , 0 , 2048) ;298 printf ( " Shutting c l i e n t down\n" ) ;299 rhdDisconnect ( ) ;300 fclose (logFile ) ;301 return 0;302 }

60

BilagIVIndhold på vedlagte CD

• Datasheet

– ADXL322

– pci-das6025-23

• Data

– Uden regulering

– Med regulering

– Uden notch

– Step på arm (video fil)

– Data fra film

• Kilder

– Implementation of a DSP-Based, Acceleration Feedback Robot Con-troller: Practical Issues and Design Limits

– Vibration Suppression Control using the Load-side Acceleration Feed-back

– Vibration Control of a Cantilever Beam Using Adaptive ResonantControl

– Notch Filter Design

– Vibration Absorption Control of Industrial Robots by AccelerationFeedback

• rhd2.0 (med undermapper)

• Matlab

– DataPlotter

61