REGULERING AF ROBOTARM

Nicolaj Lindhard Jørgensen, s062032 Lasse Thorup Olesen, s062016 REGULERING AF ROBOTARM Bachelorprojekt, forår, 2009

Nicolaj Lindhard Jørgensen, s062032 Lasse Thorup Olesen, s062016 REGULERING AF ROBOTARM Bachelorprojekt, forår, 2009

REGULERING AF ROBOTARM, Rapporten er udarbejded af: Nicolaj Lindhard Jørgensen, s062032 Lasse Thorup Olesen, s062016 Vejledere Nils Andersen & Ole Ravn DTU Elektro Automation and control Danmarks Tekniske Universitet Elektrovej Bygning 326 DK-2800 Kgs. Lyngby Denmark www.elektro.dtu.dk/forskning/au.aspx Tel: (+45) 45 25 35 50 Fax: (+45) 45 88 12 95 studieadministration@elektro.dtu.dk Udgivelsesdato: 26-06-2009 Klasse: 1 (offentlig) Bemærkninger: Denne rapport er indleveret som led i opfyldelse af kravene for opnåelse af graden Bachelor i teknisk videnskab (BScE) på Danmarks Tekniske Universitet. Rapporten repræsenterer 15 ECTS point. 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 accelerations at the endpoint of the arm and controls using this. Matlab is used to simulate the actual system and set up filters and regulators. The studies showed that using appropriate filters and regulators, it was possible to reduce overshoot and settling time. I

Resumé Denne rapport beskriver hvordan svingninger i en robotarm med et led kan dæmpes. Metoden der undersøges bruger et accelerometer til at måle accelerationerne i spidsen af armen og regulerer herefter. Der bruges Matlab til at simulere 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æmpe oversvinget og indsvingstiden. II

Indhold Abstract Resumé I II 1 Indledning 1 1.1 Baggrund................................. 1 1.2 Problemformulering........................... 3 1.3 Metode, afgrænsning og forudsætninger............... 3 1.4 Introduktion til Mechatronics..................... 3 1.5 Arbejde af andre............................. 4 1.6 Læsevejledning............................. 4 2 Teori 6 2.1 Regulatorer og filtre i kontinuert tid................. 6 2.1.1 P-regulator............................ 7 2.1.2 PI-regulator........................... 7 2.1.3 Lead-kompensatoren...................... 7 2.1.4 Lowpass filter.......................... 8 2.1.5 Highpass filter.......................... 8 2.1.6 Notch-filter............................ 8 2.2 Regulatorer og filtre i diskret tid................... 8 2.2.1 Z-transformation........................ 9 2.2.2 Filtre i diskret tid........................ 10 2.3 Jung s modulus og svingninger.................... 10 3 Simulering 12 3.1 Den fysiske model i simulink..................... 12 3.2 Verificering af simulering....................... 14 3.2.1 Step på hastighedsservo.................... 14 3.2.2 Step på positionsservo..................... 14 3.2.3 Step på positionsservo, acceleration af arm......... 15 3.2.4 Systemets poler......................... 16 3.2.5 Opsamling på tests....................... 16 3.3 Implementering af reguleringen.................... 16 3.3.1 Hastighedstilbagekobling med lead.............. 16 III

INDHOLD 3.3.2 2. ordens highpass filter.................... 17 3.3.3 1.ordens highpass filter..................... 18 3.3.4 Notch-filter............................ 19 3.3.5 P-Lead.............................. 19 3.4 Filtre og regulatorer i Matlab..................... 20 3.4.1 Lead-kompensator....................... 21 3.4.2 Notch-filter........................... 22 3.4.3 Highpass-filtre......................... 23 4 Implementering 25 4.1 Implementering på datamat...................... 25 4.2 Den fysiske implementering..................... 26 5 Test 29 5.1 Positionsbestemmelse.......................... 29 5.2 Karakteristik af step på robotarmen................. 29 5.3 Step med regulering.......................... 30 5.4 Respons på forskellige inputs..................... 31 5.5 Forskydning af egenfrekvens..................... 32 6 Diskussion 35 6.1 Anvendelighed.............................. 35 6.2 Videreudvikling............................. 35 7 Konklusion 37 Litteratur 38 IV Appendiks 39 A Symbolliste 39 B Overføringsfunktionen for modellen 41 C RHD 43 C.1 Start RHD client............................. 43 C.2 Ret i programmet............................ 43 C.3 Compile.................................. 43 C.4 Eksekver................................. 44 D Accelerometer og sampling 45

INDHOLD D.1 Antal samples.............................. 45 D.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.......................... 1 1.2 UR-6-85-5-A fra Universal Robots (her vist med et tool)...... 2 2.1 General model for et reguleringssystem............... 7 2.2 Cantilever beam............................. 10 3.1 Fysisk system uden regulator..................... 12 3.2 Step på det fysiske system med positionstilbagekobling...... 13 3.3 Step på hastighedsservo........................ 14 3.4 Step på positionsservo (motor)..................... 15 3.5 Step på positionsservo (endespids).................. 15 3.6 Hastighedstilbagekobling med Lead-kompensator......... 17 3.7 Hastighedstilbagekobling med Highpass-filter........... 18 3.8 Hastighedstilbagekobling med notch-filter.............. 19 3.9 Step med komplet regulering..................... 20 3.10 Bodeplot af modellen uden Lead-kompensator........... 21 3.11 Acceleration af endespids........................ 23 3.12 Frekvensrespons for notch-filteret.................. 24 4.1 Model af armen............................. 27 4.2 Hardware setup............................. 28 5.1 Position af endespids.......................... 30 5.2 Fysisk system med positionstilbagekobling............. 31 5.3 Fysisk system uden regulator..................... 32 5.4 Steprespons............................... 33 5.5 Respons på et sinus input....................... 34 5.6 Step med forskellige setup....................... 34 F.1 Arm setup................................ 47 I.1 Pole-Zero map fra Matlab....................... 49 I.2 Bodeplot af 2.ordens svingning af arm................ 50 II.1 Ekspanderet flex-blok.......................... 51 II.2 Hastighedsservo............................. 52 II.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.............. 13 3.2 Poler fra simulering og flexarm.................... 16 3.3 Data fra reguleringsprocess...................... 20 4.1 Fysiske parametre for flexarm..................... 27 5.1 Parametre for arm med simpel regulering.............. 30 5.2 Resultater for regulering........................ 30 5.3 Respons på forskellige inputs..................... 31 A.1 Liste med symboler........................... 40 E.1 Comedi parametre............................ 46 VIII

Kapitel1 Indledning 1.1 Baggrund I generelle automationssystemer hvor der bruges robotarme til at udføre forskellige opgaver, er der en hel klar tendens til at bruge store, tunge og solide robotter. Selv når der skal udføres opgaver som at svejse, male og tage små produktionsprø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åndtere 5kg og har en positionsnøjagtighed på +/-0.1mm. Årsagen til at robotarmen fra ABB vejer over 10 gange så meget som robotarmen fra Univesal Robots, skal findes i ønsket om større præcision og dermed valget 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øres ved at bruge tykkere materialer, bruge gear uden slør i og bruge motorer, der er tilpas stærke i forhold til belastningen. Derudover kan man opnå en vis præcisionsforbedring ved at begrænse størrelsen af accelerationerne. Disse tiltag vil generelt resultere i, at robotarmen bliver tungere og er afhængig af en solid base, der typisk skal monteres på et gulv eller i loftet. I applikationer hvor det ikke er muligt at montere robotarmen på en robust overflade, er man nød til at holde vægten nede. Det gør det vanskeligt at minimere rystelser og beholde præcisionen. Ved at bruge outputtet fra sensorer, som accelerometre, på en given robotarm, til at regulere efter vil nogle vibrationer kunne dæmpes og dermed vil nøjagtigheden kunne bevares. Den samme teknik kan også anvendes generelt på robotarme, der skal fortage kraftige accelerationer. 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 der opstå svingninger i robotternes led. Disse svingninger kan være til gene for den opgave robotten skal udføre og i værste fald føre til skader på de mekaniske dele. Ved hjælp af sensorer, som accelerometre, og passende datamater er det muligt at regulere en given robot efter klassiske reguleringsprincipper. Det er derfor målet i denne opgave at dæmpe svingninger i en fleksibel robotarm med et led vha. en datamat og et accelerometer som sensor. Dermed ønskes det afklaret: Hvor meget svingningerne kan dæmpes i forhold til en kode med en simpel 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 efterprøves disse på den givne robotarm. Data fra de foretagne test undersøges ud fra typiske reguleringsparametre. Parametrene bruges dernæst til at optimere efter. Det tilsigtes at implementere en regulator, der ved hjælp af de givne sensorer kan dæmpe svingningerne i den givne robotarm. Projektet afgrænses dermed af antallet af tilgænglige sensorer, til kun at fokusere på at dæmpe 1.ordenssvingningerne. Derudover afgrænses projektet til at fokusere på svingninger i et led. Disse afgrænsninger opstår som forudsætning for at kunne udvikle teknologien til mere avancerede applikationer, hvor der kan håndteres højere ordens svingninger og flere led. Dermed er projektet gyldigt i forhold til videreudvikling 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 begrebet fø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 at fungere. "... a synergistic combination of precision mechanical engineering, electronic control and systems thinking in design of products and manufacturing processes."- Jacob Buur 1989 [2] Dette begreb bruges især inden for regulering, hvor det, i stigende grad kan betale sig, at lade dele af maskinen være aktive elektroniske i stedet for mekaniske. Dette skyldes bl.a. at elektronik er blevet billigere og mere pålideligt end tidligere. Et udmærket eksempel på et system som anvender mechatronics, er ABSbremser (Antiblockingsystem). For at undgå, at et hjul på en bil blokerer og skrider under opbremsning, er der i bilen implementeret en computer, som via en elektronisk hastighedssensor registrerer hjulets omdrejninger. Denne computer 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 samme mekaniske setup som er behandlet i denne rapport, dog med strain gauges som sensor i stedet for et accelerometer. Derved er han kommet frem til, ved et step på 0.3 rad, at have et oversving på 0.05 rad og en indsvingstid på ca. 2 sek. Se s. 231 [2]. Vibration Absorption Control of Industrial Robots by Acceleration Feedback"[3] beskriver vibrationsdæmpning vha. tilbagekobling af acceleration på industrielle robotter. Der fremvises markante forbedringer efter tilbagekobling af accelerationen sammenlignet med det traditionelle regulering. 1.6 Læsevejledning 4 Teorien for de anvendte regulatorer, fysikken bag en model af robotarmen og regulering i diskret tid gennemgås kort.

1.6 Læsevejledning I Simuleringen gøres rede for den anvendte matematiske model, og denne sammenholdes med den virkelige robotarm. Implementeringen redegøre for hvordan de, i afsnit 3, fundne regulatorer implementeres på den fysiske opstilling. Test på robotarmen gennemgås. Der præsenteres plots og relevante værdier for de opnåede resultater. I diskussionen perspektivers de opnåede resultater, og der reflekteres over hvad der kunne gøres bedre i et projekt på længere sigt. Konklusionen sammenfatter kort de opnåede resultater og sammenholder dem med problemformuleringen. For læsere der vil danne sig et hurtigt overblik over rapporten, kan man med fordel læse problemformuleringen, Step med regulering (afsnit 5.3) og konklusionen. For læsere der har basal forståelse for regulering kan kapitel 2 udelades Er opstillingen helt ukendt for læseren kan afsnit 4.2 læses først for et få et overblik over den konkrete reguleringsopgave. God fornøjelse 5

Kapitel2 Teori 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: a n y n (t) + a n 1 y n 1 (t) +... + a 0 y(t) = b m u m (t) + b m 1 u m 1 (t) +... + b 0 u(t) (2.1) Dette gælder både for fysiske systemer som fjedre og motorer, samt filtre og regulatorer. Dog realiseres filtre og regulatorer ofte i frekvensdomænet. Ligeledes transformeres modeller for systemer i tidsdomænet som det er tilfældet i Matlab og simulink, til frekvensdomænet med Laplace transformation. Den unilaterale Laplace transformation er givet ved: L[f(t)] = F (s) = 0 f(t)e st dt (2.2) Når differentialligningerne for et system som 2.1 Laplace transformeres kan overføringsfunktionen for systemet opstilles som: H(s) = Y (s) U(s) = b ms m +... + b 1 s + b 0 a n s n +... + a 1 s + a 0 (2.3) Når det er ønskeligt at have systemet på denne form, er det fordi det er betragtelig lettere at opstille blokdiagrammer og arbejde med disse. Den direkte Fourier transformation(2.4) bruges til at gå fra tidsdomænet til frekvensdomænet for at beskrive hvilke frekvenser der er tilstede i den oprindelige funktion. F [f(t)] = F (ω) = f(t)e jωt dt (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) = (G 1 (s) + d(s)) G 2 (s) systemet, der skal reguleres, H(s) beskriver målesystemet, K 0 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 G C (s) er regulatoren. r(s) er referencen, e(s) er fejlen, u(s) er styresignalet, c(s) er outputtet, c m 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 G C (s) = K p, hvilket svarer 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: G c (s) = K p τ i s + 1 τ i s (2.5) Denne regulator kendetegnes ved at have gode stationære egenskaber, men til gengæ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: G c (s) = K p τ d s + 1 ατ d s + 1, α < 1 (2.6) Den væsenligste egenskab ved Lead-kompensatoren er at den øger båndbredden 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: G c (s) = ω c ω c + s (2.7) Lowpass filteret kendetegnes ved at det dæmper frekvenser over knækfrekvensen ω c. 2.1.5 Highpass filter Overføringsfunktionen for et typisk highpass filter fremkommer ved at sætte s = ω p /s i 2.7, se [6] s.525. G c (s) = ω s ω s + ( ω p s ) = ω s s ω s s + ω p (2.8) Hvor ω p er passband frekvensen og ω s er stopband frekvensen. Highpass filterets 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: G c (s) = s2 + 2 d c ω 0 s + ω 2 0 s 2 + 2 1 c ω 0 s + ω 2 0 (2.9) Denne filtertype kaldes også Båndstop filter og bruges når man ønsker at dæ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 gennemgås her de grundliggende principper bag overgangen fra kontinuert tid til diskret 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 transformeres til diskret tid. Hvis et kontinuert signal y(t) samples med sampletiden T 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. I praksis benyttes de givne regneregler som er vedlagt i både [5] og [6]. Til at transformere overføringsfunktioner fra kontinuert tid til diskret tid benyttes c2d i Matlab. Denne funktion tager, udover funktionen der skal transformeres, parametrene sampletid og metode. Sampletiden er fastsat idet systemet kører en fast cyklus på 10ms. Som metode vælges tustin der dækker over en bilineær transformation som fra [6] ligning 12.57 er givet ved: s = ( ) 2 z 1 T z + 1 (2.11) En typisk overføringsfunktion i diskret tid vil have formen: H(z) = Y (z) X(z) = b 0 + b 1 z 1 + b 2 z 2... + b l z l 1 + a 1 z 1 + a 2 z 2 +... + a n z n (2.12) En given overføringsfunktion skal, som det vil vise sig senere, kunne realiseres på en datamat. Skrives 2.12 som en differens ligning fås flg. udtryk: y(k)+a 1 y(k 1) + a 2 y(k 2) +... + a n y(k n) = b 0 x(k) + b 1 x(k 1) + b 2 x(k 2) +... + b l x(k l) (2.13) y(k) = a 1 y(k 1) a 2 y(k 2)... a n y(k n) + b 0 x(k) + b 1 x(k 1) + b 2 x(k 2) +... + b l x(k l) (2.14) Her indses det, at y(k) er outputtet i øjeblikket k. Således beregnes outputtet for et n te ordens system ud fra de n foregående værdier for outputtet. Systemer med denne egenskab kaldes rekursive. Denne egenskab medfører at outputtet aldrig rigtig dør ud, fordi outputtet forplanter sig så længe koefficienterne a 1, a 2... a n ikke er lig nul. Dermed kaldes disse koefficienter for de rekursive koefficienter. Mens b 1, b 2... b n 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) forsætte for k, hvorfor rekursive filtre kaldes infinite impulse response(iir) filtre. Mens et n te ordens ikke-rekursivt filter der gives en enhedsimpuls, vil give 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. En sådan arm hedder populært en cantilever beam, som er en beam fastgjort i den ene ende. Se figur 2.2. En sådan beam kan beskrives ud fra følgende parameter: Længde, Bredde, Højde, Densitet og Youngs modulus. Sidstnævnte er et udtryk 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 differentialligning angiver sammenhængen imellem krumningen af en beam (x,u), og den belastning den bliver påtrykt (w) ( ) δ δ 2 u EI a = w (2.15) δx δ 2 x Løst for en belastning på endepunktet af beamen giver dette udtryk ifølge [11]: 10 y = F L3 3EI a (2.16)

2.3 Jung s modulus og svingninger der beskriver spidsens forskydning som funktion af den påtrykte kraft, hvor I a er tværsnits inertimomnetet (på engelsk second moment of inertia). For en massiv firkantet beam, er dette udtrykt ved: I a = 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 kan udtrykkes ved ligningen: ( ) µ δ2 u δt + δ2 δ 2 u EI 2 δx 2 a = 0 (2.18) δ 2 x Under antagelse af at densiteten og tværsnitsarealet er konstant over hele armen, kan svingningen udtrykkes ved den partielle differentialligning 2.19, hvor µ = ρa: µ δ2 u δt + EI δ 4 u 2 a δ 4 x = 0 (2.19) Vi skal ikke her komme nærmere ind på løsningen af denne ligning, men blot nævne at den har løsningen givet i [2] ligning 4.75. ω = k 4 EI a ρa (2.20) Hvor cos(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 egenfrekvenserne falde. Dette er også tilfældet hvis inertimomentet for armen ændres, f.eks. ved tilføjelse af et tool. Dette vil blive nærmere undersøgt i afsnit 5.5. 11

Kapitel3 Simulering 3.1 Den fysiske model i simulink For at kunne simulere systemet opstilles en model i simulink. På fig.3.1 ses den 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 bilag II.1 tager flex en vinkelposition og en vinkelaccelration som input og giver et moment 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 og den hastighedstilbagekobling systemet er sat op med. Blokken får en vinkelhastighed og et moment som input og giver en vinkelposition og en vinkelacceleration som output. Som modellen er konfigureret her, vil et step resultere i en hastighedsændring, 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 positionstilbagekoblingen og hastighedsindgangen på Hastigheds servo-blokken, men da faktoren i dette gain ville være 1 er det udeladt på fig.3.1. Laves der et positionsstep fås graferne i fig.3.2. Øverst ses armens position, her ses det, at spidsens position svinger om den ønskede position mens positionen fra potentionmeteret indstiller sig på den ønskede position. Nederst ses fejlen, beregnet som forskellen mellem potentiometerpositionen og spidspositionen. Overføringsfunktionen er udledt for modellen på baggrund af den ekspanderede model i bilag II.3. Selve udledningen kan ses i appendiks B. Overføringsfunk- 12

3.1 Den fysiske model i simulink Position rad rad 1 0.8 0.6 0.4 spids 0.2 skulder 0 0 2 4 6 8 10 tid Fejl 0.15 0.1 0.05 0 0.05 0.1 0 2 4 6 8 10 tid Figur 3.2: Step på det fysiske system med positionstilbagekobling Poler Nulpunkter R(p) I(p) R(z) I(z) -0.287 7.29-0.324 16.3-0.287-7.29-0.324-16.3 0.0665 45.6-49.7 0 0.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) = K t K pm K d m g(1+g 2 (s)s 2 ) (J m+m 2 g(j b G 1 (s)))r as 2 +(K t(k pmk 1 e s+k e)+µ f R a)s+k tk pmk d m g (3.1) Her er G 1 (s) og G 2 (s) estimerede overføringsfunktioner for state space modellen der ses i bilag II.1. Poler og nulpunkter for overføringsfunktionen er fundet vha. 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, laves forskellige tests af dem begge. Målet med disse tests er, at sikre at modellen ikke afviger fra virkeligheden med mere end 20 %. Systemet pilles fra hinanden og undersøges bid for bid. 3.2.1 Step på hastighedsservo Motoren tilbagekobles med et tachometer, så den agerer hastighedsservo. Der steppes på servoen med et step af højden 0.2 rad/sek. Det kan ses på plottene 3.3(a) og 3.3(b) at simuleringen giver en hastighed på 0.198 rad/sek, og den virkelige 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å systemet 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ær fejl 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ør med 1 rad, og accelerationen af endepunktet observeres. Det ses, at udsvingene for den fysiske arm er større end for simuleringen. Størrelsen på det første udsving er i simuleringen 5 rad/sek 2, og i praksis 4 rad/sek 2. Dette svarer til en 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ærkes at den fysiske arm har en lavere dæmpning end simuleringen. Dette fremgår også af polerne i tabel 3.2. 10 acc tip 10 accx 8 8 6 6 4 4 2 2 rad/sek 2 0 2 rad/sek 2 0 2 4 4 6 6 8 8 10 0 2 4 6 8 10 tid 10 0 2 4 6 8 10 tid (sek) (a) Simulering (b) Virkelig arm Figur 3.5: Step på positionsservo (endespids) 15

Kapitel 3. Simulering Matlab Arm R(p) I(p) R(p) I(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 som giver anledning til 1. ordens svingningen for systemet. Ud fra matlab-modellen er følgende komplekse poler fundet. Realdelen af polerne afviger med 1,8 % og imaginærdelen med 14 %. Det vil altså sige at det virkelige system vil svinge en 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 med 20 %. Polerne for det simulerede system ligner dog polerne for det virkelige system 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 regulatorerne i simulink. Systemet med en simpel positionstilbagekobling kan ses i afsnit 3.1. Da hastigheden for spidsen af armen ikke er tilgængelig som sensor input integreres spidsens acceleration. For at simulere støj i systemet integreres en fejl ind på hastighedstilbagekoblingen. Da det er muligt at vælge antallet af ordener, armens svingninger skal have i simuleringen, startes der med en simulering af armen med 1. ordens svingninger. Beregning af parameterene til de 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.5 rad/sek. Denne vil bevirke, at systemet ved en hastighedstilbagekobling vil blive ustabilt. Ved simulering har det vist sig, at dette er i en grad så det ikke er muligt at producere brugbare plots med matlab. For at forbedre systemets båndbredde og derved stabilisere det, indsættes en lead kompensator i systemet. Denne er dimensioneret, så den har en maksimal positiv fasedrejning ved 11.5 rad/sek. Udregningen af denne kan ses i afsnit 3.4.1. Dette giver en 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 responset på et step ser ud som på figur 3.6. Her er den blå kurve endespidsens vinkelposition og den grønne kurve er den fastgjorte endes vinkelposition 1.5 Position 1 rad 0.5 0 0 2 4 6 8 10 tid 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å hastigheden. 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 bevirker, at de lave frekvenser som giver anledning til den stationære hastighedsfejl sorteres fra. Highpass filteret som er beskrevet i afsnit 3.4.3 har overføringsfunktionen: H(s) = s2 + 1.11 10 14 s + 2.106 s 2 + 10.34s + 50.54 (3.3) Det ses på figur 3.7, at den stationære hastighedsfejl nu næsten er væk. Som tidligere er den blå kurve endespidsens vinkelposition og den grønne kurve er den fastgjorte endes vinkelposition. Position 1 0.8 rad 0.6 0.4 0.2 0 0 2 4 6 8 10 tid 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 highpassfilter Denne gang af 1. orden. Dette placeres med en knækfrekvens i 0.628 rad/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 bliver ustabil. For at omgå dette laves et filter som fjerner 2.ordens svingningen fra hastighedstilbagekoblingen. Der vælges et notch-filter, som er et båndstopfilter, for dets skarpe karakteristik. Spidsfrekvensen for filteret placeres i 40 rad/sek, og overføringfunktionen bliver fra afsnit 3.4.2. H(s) = s2 + 0.8s + 1600 s 2 + 112.8s + 1600 (3.5) Efter implementering viste 2. ordens svingningerne at være dæmpede i en grad, så de ikke længere udgjorde et problem. Position 1 0.8 rad 0.6 0.4 0.2 0 0 2 4 6 8 10 tid 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 error findes frem til, at et gain på 0.8 drastisk begrænser oversvinget, og forbedrer indsvingstiden en smule. Efter implementering af denne regulator ser responset 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. Fejl P 0.194 >10 0.9997 0.2628 0 Lead >10 0.1561 0.04 2.ordens Highpass >10 0.1428 0.001 1.ordens Highpass 0.0819 3.3047 1.0014 0.1897 0 Notch 0.0514 2.6960 1.0014 0.2102 0 P-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. Position 1 0.8 rad 0.6 0.4 0.2 0 0 2 4 6 8 10 tid 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 Matlab 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. Her kan Matlab designe digitale filtre ud fra nogle, af brugeren, givne specifikationer. 20

3.4 Filtre og regulatorer i Matlab Bode Diagram Gm = 18.9 db (at 7.52 rad/sec), Pm = 22.9 deg (at 9.27 rad/sec) 40 20 Magnitude (db) 0 20 40 60 80 0 45 Phase (deg) 90 135 180 225 10 3 10 2 10 1 10 0 10 1 10 2 10 3 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 fasedrejning benyttes en lead-kompensator med flg. overføringsfunktion: H(s) = K p τ d s + 1 α l τ d s + 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) = 0.100 sek. Dermed bliver overføringsfunktionen for lead- Dermed bliver τ d 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 sampletid på T s = 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 armens 2.ordens svingning på omkring 40 rad/sek. blev anslået. Dette bevirkede, at systemet blev ustabilt. På figur 3.11(b) er vist et plot af armens acceleration for et step. For at undgå disse svingninger, blev et notch-filter foreslået. Et notch-filter er et båndstop filter, med en meget spids karakteristik. Ved at filtrere anden ordens svingningen fra den integrerede hastighed, skulle det være muligt at undgå at armen blev ustabil. Overføringsfunktionen for et sådant filter er vist herunder: H(s) = s2 + 2ζ z ω N s + ω N 2 s 2 + 2ζ p ω N s + ω N 2 (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æmpningen for tælleren skal være lavere end for nævneren, for at filtret vil være anvendeligt. Da 2.ordens svingningen er aflæst til omkring 40 rad/sek, se bilag I.2, er dette spidsfrekvensen. For at opnå en ikke alt for smal karakteristik er dæmpningsforholdene valgt som ζ z = 0.01, ζ p = 1.41 Dette giver et filter med overføringsfunktionen: H(s) = s2 + 0.8s + 1600 s 2 + 112.8s + 1600 (3.12) Frekvensresponset for filtret er vist på figur 3.12. Det ses at filtret ved første ordens svingningen på 7,3 rad/sek. forstærker med -0,04 db, men ved anden ordens frekvensen forstærker med -60 db. Andenordenssvingingen kan dog flytte sig lidt ved påvirkninger fra omgiverlserne, men også ved 36 rad/sek forstærker filtret med -34 db. Et step på den fysiske arm med og uden filtret implementeret er vist i figur 3.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 10 accx 15 accx 8 6 10 4 5 2 rad/sek 2 0 2 rad/sek 2 0 4 5 6 10 8 10 0 2 4 6 8 10 tid (sek) 15 0 1 2 3 4 5 tid (sek) (a) Med Notch-filter (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 z 2 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 f cutoff = 1.1634Hz ud fra armens 1.ordens egensvingning og samplefrekvensen vælges til F s = 100Hz fås flg. overføringsfunktion i z-domænet. H(z) = 0.9496z2 1.899z + 0.9496 z 2 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ækfrekvens på 0.1 Hz, og dette giver overføringsfunktionen H(z) = 0.9969z 0.9969 z 0.9937 (3.15) 23

Kapitel 3. Simulering Bode Diagram Gm = 161 db (at 41 rad/sec), Pm = 180 deg (at 0 rad/sec) 0 50 100 150 200 250 300 0 90 180 270 360 10 0 10 1 10 2 10 3 Frequency (rad/sec) Figur 3.12: Frekvensrespons for notch-filteret 24

Kapitel4 Implementering 4.1 Implementering på datamat Herunder gives et eksempel på implementering af et filter i z-domænet på datamat. Eksemplet er taget fra Direct realisation fra [1]. Antag at den diskrete overføringsfunktion i z-domænet er givet som: H(z) = u(z) e(z) = b 0 + b 1 z 1 + b 2 z 2 1 + a 1 z 1 + a 2 z 2 (4.1) Funktionen er blevet forsinket, så der ikke forekommer ikke kausale værdier af z. Overføringsfunktionen kan med algebra omskrives til u(z) = e(z)(b 0 + b 1 z 1 + b 2 z 2 ) u(z)(a 1 z 1 + a 2 z 2 ) (4.2) Ved herefter at lave invers z transformation på systemet, er det muligt at omskrive udtrykket til u(i) = b 0 e(i) + b 1 e(i 1) + b 2 e(i 2) (a 1 u(i 1) + a 2 u(i 2)) (4.3) Hvor e er inputtet og u er outputtet. e(i 1) betyder således inputtet forsinket en sample. Da udtrykket nu kun er en funktion af tidligere samples er det muligt 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 dette filter i z-domænet har overføringsfunktionen: H(z) = 0.06509z2 1.197z + 0.6459 z 2 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 sammenlignet med 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å figur 4.1 ses det, hvordan accelerometeret er placeret på enden af den fleksible stå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 potentiometer til bestemmelse af armens position. For at kunne drive motoren i systemet 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 Enhed Længde 1.200 m Højde 0.040 m Bredde 0.002 m Materiale Stål Densitet ρ 7850 kg/m 3 Youngs modulus E 2.11 10 11 N/m 2 Afstand til sensor 1.197 m Tabel 4.1: Fysiske parametre for flexarm med en stærk hastighedstilbagekobling. Kommunikation med datamaskinen sker 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

Kapitel5 Test 5.1 Positionsbestemmelse For at kunne bedømme parametre som oversving, stationær værdi osv. er det nø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 som skal dobbelt integreres. Dette giver anledning til en voksende fejl på positionen forårsaget af en konstant fejl på accelerationen. For at kompensere for dette laves en position, som er en kombination af potentiometer positionen ved lave frekvenser og accelerometer positionen ved høje frekvenser. Dette gøres som i formel 5.1. Det ses at hvis de to positioner er ens vil filtret give 1. θ = 2ζω ns + ω 2 n s 2 + 2ζω n s + ω 2 n s 2 θ P ot + s 2 + 2ζω n s + ωn 2 θ Acc (5.1) Knækfrekvensen,ω n, for filteret er valgt til 1/3 af frekvensen for 1. ordens svingningen, dvs. ω n = 2.5. Dæmpningen er sat til ξ = 0.6 som er fundet ved forsø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 helt nøjagtig. Der er en afvigelse på den stationære værdi på 0.015 rad svarende til 1.5 %. Derfor vil stationære fejl blive aflæst på potentiometer målingen som er en sand værdi for den stationære fejl. Indsvingstid kan med fordel aflæses på accelerometermålingen, så plottet af positionen er hovedsagelig for at give læ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å K p = 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 beregnes. 29

Kapitel 5. Test Kombineret position af potpos og accpos Data fra film 1 1 0.8 0.8 rad 0.6 rad 0.6 0.4 0.4 0.2 0.2 0 0 1 2 3 4 5 tid (sek) (a) 0 0 1 2 3 4 5 tid (sek) (b) Figur 5.1: Position af endespids Oversving Indsvingstid 2% Stationær værdi Største Fejl Stat. Hast. Fejl 0.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 a til f respons for et enhedsstep med og uden hastighedstilbagekobling. Her benyttes K p = 2.5, da resultaterne skal være sammenlignelige med det fuldt regulerede 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 K p = 1 i systemet uden hastighedstilbagekoblingen så vil oversvinget være mindre men bevægelsen vil også være det langsommere. Oversving Indsvingstid 2% Stationær værdi Største Fejl Stat. Hast. Fejl Uden regulering 0.2063 > 10 1.0098 0.2296 0 Med regulering 0.0494 2.3404 1.0085 0.0897 0 Tabel 5.2: Resultater for regulering 30

5.4 Respons på forskellige inputs Kombineret position af potpos og accpos 1 0.8 rad 0.6 0.4 0.2 0 0 2 4 6 8 10 tid (sek) 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åtrykkes disse i simuleringen. Et kvantitativt mål for armens evne til at følge bevægelsen 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 den fysiske arm. Fejlen er her defineret som forskellen på inputtet og armens endeposition. Det ses som forventet, at både den maximale fejl og RMS fejlen Input Simulering Flexarm Max fejl Rms fejl Max fejl Rms fejl Step(0.2) 0.2009 0.0408 0.2187 0.1914 Step(1) 1.0070 0.1824 1.1086 0.8935 Sinus(0.2 t) 0.0946 0.0586 0.0791 0.0533 Sinus(0.5 t) - - 0.1661 0.1033 Sinus(1 t) - - 0.4596 0.1741 Rampe(0.2) 0.1003 0.0904 0.1189 0.1061 Rampe(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 plottet for 0.5 sin(1t) vist på figur 5.5. Det fremgår at armens position er 0.50 sek. 31

Kapitel 5. Test 200 Frequency content of accx 180 160 140 120 100 80 60 40 20 0 0 1 2 3 4 5 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 med dette er at forskubbe egenfrekvensen for 2. ordens svingningen ud af notchfilteret og derved provokere armen til at gå i resonans. Belastningen påtrykkes ved 2 skruetvinger på ialt 0.332 kg som placeres på spidsen, 1.20 m fra omdrejningspunktet. Plots for en svingning, kun med positionstilbagekobling, med og uden ekstra belastning er vist på figur 5.6(a). Det ses at egenfrekvensen for 1. ordens svingningen er blevet lavere med en belastning. Til gengæld er udsvinget blevet større. Dette stemmer udmærket overens med teorien. Frekvensen er blevet formindsket med 30 %. Desuden fremgå det tydeligt at indsvingstiden er steget. Dog er det ikke lykkedes med den anvendte belastning at flytte 2. ordens svingningen i en grad så den blev mærkbar. Ud fra ligning 2.20 er det herefter muligt at bestemme den teoretiske 2. ordens svingning med denne belastning til 27.76 rad/sek. Ved denne frekvens forstærker notch-filteret med -13 db. På figur 5.6(b) ses svingningen for et step med den fulde regulering inkluderet. Det ses at accelerometer tilbagekoblin- 32

5.5 Forskydning af egenfrekvens Potentiometer position Potentiometer position 1 1 0.8 0.8 rad 0.6 rad 0.6 0.4 0.4 0.2 0.2 0 0 2 4 6 8 10 tid (sek) 0 0 2 4 6 8 10 tid (sek) (a) (b) 15 accx 15 accx 10 10 5 5 rad/sek 2 0 rad/sek 2 0 5 5 10 10 15 0 2 4 6 8 10 tid (sek) 15 0 2 4 6 8 10 tid (sek) (c) (d) Position Position 1 1 0.8 0.8 rad 0.6 rad 0.6 0.4 0.4 0.2 0.2 0 0 2 4 6 8 10 tid (sek) 0 0 2 4 6 8 10 tid (sek) (e) (f) Figur 5.4: Steprespons 33

Kapitel 5. Test 0.5 correction speed 0.4 0.3 0.2 0.1 rad 0 0.1 0.2 0.3 0.4 reference position 0.5 0 5 10 15 20 25 30 35 40 tid (sek) Figur 5.5: Respons på et sinus input gen formindsker udsvingene, men ikke i en grad som for en arm, der ikke er belastet, se fig.5.6. Artikel [10] foreslår at implementere et adaptivt filter, til at fjerne svingning- 1.4 Position Kombineret position af potpos og accpos 1.2 1 1 0.8 0.8 rad 0.6 rad 0.6 0.4 0.4 0.2 0 Med Load Uden Load 0.2 0 2 4 6 8 10 tid (sek) 0.2 0 0 2 4 6 8 10 tid (sek) (a) Uden hastighedstilbagekobling, med og uden load. (b) Med regulering og load. Figur 5.6: Step med forskellige setup erne i en robotarm. Dette vil bevirke at filteret altid arbejder omkring den aktuelle egenfrekvens. Denne type regulering går dog ud over formålet med denne rapport. 34

Kapitel6 Diskussion Resultaterne opnået i denne rapport er fundet under faste rammer i et laboratoriemiljø. Dette er tilstrækkeligt til at konkludere at teknologien fungerer, men en del arbejde er stadig nødvendigt, før dette kan implenenteres på en robot i industrien. 6.1 Anvendelighed Tages der tages udgangspunkt i, at det er muligt at dæmpe svingningerne i en given robotarm ved brug af en sensor som et accelerometer, vil teknologien umiddelbart være anvendelig da de samlede omkostninger af en robotarm med sensorer til dæmpning af svingninger er mindre end en robotarm der i princippet er overdimensioneret for at undgå svingninger. Teknologien er i det hele taget anvendelig i forhold til at minimere omkostningen og massen af robotarme generelt. Hvilket kunne tænkes fordelagtigt i industrier med samlebåndsarbejde, hvor det åbner op for at have mere mobile robotter. Til gengæld vil teknologien have mindre relevans i applikationer hvor omkostningerne har mindre betydning og hvor robotterne alligevel er store og bevæger sig langsomt. 6.2 Videreudvikling Til fremtidig implementering af regulering med accelerationstilbagekobling, er der flere ting, som ville kunne forbedres. Accelerometeret som er anvendt i dette projekt er et billigt multi purpose komponent. Kombineret med lange signalledninger, giver dette mulighed for en del støj, og tilsvarende dårligere regulering. Desuden er hastigheden af et udsving begrænset af, at accelerometeret går i mætning ved ± 2g. En oplagt forbedring ville derfor være at udskifte sensoren til et mere sikkert komponent med et større måleområde. Setuppet i denne rapport er kraftigt simplificeret i forhold til en robot i industrien. 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 vinkelaccelerationen af 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 for at svinge i et plan med påvirkning af tyngdekraft. Skulle softwaren bruges til test i et miljø, hvor det ikke var hensigtsmæssigt at compile hele tiden, ville en GUI til systemet være på sin plads. Denne kunne f.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å armen, og reguleringen er ikke forsøgt specifikt optimeret til denne. Dette kunne omfatte et projekt hvor der i stedet for filtre, optimeret til armen uden belastning, benyttedes adaptive filters som var i stand til at tilpasse sig efter massen af et tool. Endelig kunne der udvikles et sæt indstillingsregler, som gjorde det muligt for ingeniører at montere et accelerometer på deres robot, og derefter vha. en række nedskrevne trin, at implementere reguleringen på en anden robot. Hvis en motor, som ikke er koblet tilbage, dvs. en momentstyret motor blev benyttet i stedet for en hastighedsservo ville det være muligt at beregne en momentfejl frem for en hastighedsfejl. Dette ville gøre integrationen unødvendig og derfor med stor sandsynlighed øge hastigheden af reguleringen. 36

Kapitel7 Konklusion I denne rapport er der blevet arbejdet med dæmpning af svingninger i en robotarm. Mere præcist har arbejdet været fokuseret på at dæmpe svingningerne i en robotarm med en frihedsgrad ved at tilbagekoble outputtet fra et accelerometer. Gennem arbejdet med robotarmen er der blevet sat en simpel platform op, bestående af den mekaniske arm og motoren med en hastighedstilbagekobling gennem en forstærker. Derudover blev der konstrueret en positionstilbagekobling og en accelerationstilbagekobling gennem et Comedi-board der var sat op på en computer, se fig.4.2. Således fungerede computeren som platform for implementering af de valgte regulatorer og filtrer. Gennem arbejdsprocessen blev der undersøgt flere filtrer og regulatorer, samt forskellige kombinationer af disse. Designet af filtrer og regulatorer blev typisk gjort i Matlab, hvor transformationerne mellem diskret tid og kontinuert tid også blev foretaget. I den endelige opsætning, se bilag II.4, blev der valgt et notch-filter, et 1.ordens og et 2.ordens Highpass filter til at filtrere hastigheden fra den integrerede acceleration. Af regulatorer blev der valgt en proportionalregulator og en Lead 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. Thesis, Department of automation, 1996. [3] Futami S., Kyura N., Hara S., Vibration Absorption Control of Industrial Robots 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 Robot Arm, Proc. 1991 IEEE Conference on Robotics and Automation, Sacramento, California, April 1991, s.2121-2126. [5] Jannerup O., Sørensen P.H., Reguleringsteknik, 4.udgave, 1.oplag, Polyteknisk 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 Beam Using 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 Research Uses, april, 1999. 38

AppendiksA Symbolliste Symbol Beskrivelse Enhed α Vinkelacceleration rad/s 2 α l Skalering til Lead µ f Viskosfriktion Nm/(rad/s) ω Vinkelhastighed rad/s ω n Knækfrekvens rad/s ω top Vinkelhastighed rad/s ρ Densitet kg/m 3 θ 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/m 2 E α Maximal fejl mrad/s 2 e s Hastighed - spænding, tacometerforhold V s f cutoff Cutoff frekvens for HP-filter Hz F s Samplefrekvensen Hz G m Gain magin db J b Inertimoment for armen kg m 2 J m Inertimoment for motoren kg m 2 m g Gear ratio N Antal af samples K 1 Gain for for analog forstærker K e Motor constant A sek K p Gain Fortsættes på næste side 39

Appendiks A. Symbolliste Symbol Beskrivelse Enhed K pm Spænding - strømforhold I/V K t Strøm - momentforhold Nm/A P m Fasemargen grader T s Sampletid s Z α/2 Kumuleret sandsynlighed Tabel A.1: Liste med symboler 40

AppendiksB Overføringsfunktionen for modellen θ o = m g 1 s q + G 2(s) (m g q) (B.1) a = m g (J b (m g q) G 1 (s) (m g q)) (B.2) b = µ f q (B.3) c = K e q (B.4) d = K 1 e s q (B.5) θ b = m g 1 s q (B.6) q = 1 s q (B.7) q = 1 (( ) ) 1 K t (K pm (K d (e θ b ) d) c) b a J m R a (B.8) q = K tk pmk d e s 2 (J m+m 2 gj b m 2 gg 1 (s))r a s 2 +(K tk pmk 1 e s+k tk e+µ f R a) s+k tk pmk d m g (B.9) θ o = K tk pmk d(1+g 2 (s)s 2 ) (J m +m 2 g J b m 2 g G 1(s))R a s 2 +(K t K pm K 1 e s +K t K e +µ f R a) s+k t K pm K d m g e (B.10) H(s) = K tk pmk d(1+g 2 (s)s 2 ) (J m +m 2 gj b m 2 gg 1 (s))r a s 2 +(K t K pm K 1 e s +K t K e +µ f R a) s+k t K pm K d m g (B.11) 41

Appendiks B. Overføringsfunktionen for modellen G 1 (s) = 52319.112 s 2 11339.08395 s 7.139068756 10 7 2500 s 4 +2100.425 s 3 +5.373523456 10 6 s 2 +8.957746871 10 5 s+2.79823609 10 8 (B.12) G 2 (s) = 2908.962980 s 2 1562.736275 s 4.43119284 10 6 2500 s 4 +2100.425 s 3 +5.373523456 10 6 s 2 +8.957746871 10 5 s+2.79823609 10 8 (B.13) H(s) = 46.11619924 s 4 +60.63179697 s 3 +1.062607340 10 5 s 2 +1.010109129 10 5 s+3.155395949 10 7 3.228297525 s 6 +282.2811687 s 5 +7287.147946 s 4 +6.023380281 10 5 s 3 +1.297483648 10 6 s 2 +3.139299674 10 7 s+3.155395949 10 7 (B.14) 42

AppendiksC RHD For at forbinde til terminalen med RTAI følges følgende trin. Programmet køres på maskinen rt14, og er gemt i mappen simpleclient. For at kunne køre programmer i RHD skal programmet RHD-client køre. Logning sker til filen logile.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

AppendiksD Accelerometer og sampling D.1 Antal samples I dette projekt er der benyttet et breakoutboard af typen DE-ACCM2G fra dimensionengineering. Denne har en ADXL322 kreds fra Analog Devices, som er et accelerometer med et måleområde på ±2g. På breakoutboardet er accelerometeret 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 mulighed for 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 samples flere gange og midles over disse. Standard afvigelsen er ud fra forsøg målt til 3.82 mrad/sek 2. Desuden kan fejlen på målingerne antages at være normalfordelt. For at undgå at disse fejlmålinger får for stor indflydelse, er det ønskeligt at fejlen er under 2 mrad/sek 2. Det antal samples, der er nødvendigt for at sikre dette i et 95 % konfidensinterval, er givet ved 1 : ( ) Zα/2 σ 2 ( ) 2 1.96 3.82 N = = = 13.9 14 (D.1) 2 E α Der vil derfor i den praktiske implementering blive anvendt 14 samples som baggrund for hver måling. D.2 Sampling og tider For at undgå aliasing er det vigtigt at holde samplingfrekvensen over nyquistfrekvensen 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 teorien i 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. 1 Se [7] s. 231 45

AppendiksE Comedi-Board Boardet der blev anvendt er af typen PCI-DAS6025. Dette er et board til interface til målinger på en linux-baseret maskine. Fra et C-program er det muligt at læse og sætte analog værdier på en række porte på boardet. Syntaksen for at 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 boardet 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. Dette delay er nødvendigt, for at AD-converteren på boardet skal være færdig med en måling før den næste tages. Yderligere information kan findes på: http://www.comedi.org/ Parameter comedi_t * device unsigned int subdevice unsigned int channel unsigned int range unsigned int aref lsampl_t data Betydning En pointer til porten Nummeret på subdiviset Den kanal der skal skrives til Skalering af inputtet Spændingsintervallet Spændingen der skal sættes Tabel E.1: Comedi parametre 46

AppendiksF Metode til positionsbestemmelse For at verificere at de data der er fremkommet fra robotten igennem et høj- og lavpasfilter er tilsvarende, filmes et step. Forsøgsopstillingen ser ud som på figur 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 filmen er optaget med 30 frames / sek. er det muligt at observere 1. ordens svingningen. Ud fra disse punkter kan vinklen i forhold til startpositionen bestemmes ved formlen (F.1), hvor a og b begge er afstanden fra omdrejningspunktet til det observerede 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 + b 2 c 2 2r2 c 2 2ab 2r 2 c = (x Cx) 2 + (y Cy) 2 (F.1) (F.2) 47

Appendiks F. Metode til positionsbestemmelse cos(θ) = 2 1.1972 37.74 2 2 1.197 2 = 0.9484 θ = 0.571rad (F.3) Disse punkter former grafen på figur 5.1(b). Denne graf, kan herefter sammenholdes med den graf der er fremkommet ud fra den filtrerede acceleration. 48

BilagI Grafer 50 0.88 0.8 0.68 Pole Zero Map 0.5 0.25 40 0.94 30 20 0.975 Imaginary Axis 10 0.994 100 0 10 0.994 80 60 40 20 20 30 40 0.975 0.94 0.88 0.8 0.68 0.5 0.25 50 100 80 60 40 20 0 20 40 60 Real Axis Figur I.1: Pole-Zero map fra Matlab 49

Bilag I. Grafer 400 Frequency content of accx 350 300 250 200 150 100 50 0 0 10 20 30 40 50 frequency (Hz) Figur I.2: Bodeplot af 2.ordens svingning af arm 50

BilagII Modeller 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

BilagIII Kildekode Source Code: testprog.c 1 / 2 Reguleringsprogtam t i l 1DOF robotarm med 3 accelerometer tilabgekobling. 4 Lasse Olesen og Nicolaj Lindhard 5 / 6 7 / Version control 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 po s i ti o n of arm in rad. 1 gives one rad 12 # define const_init_factor 100 / / Factor used f o r 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 accelerometer 14 # define const_gravity 9.82 / / Gravity 15 # define const_samples_pr_second 100 / / Samplerate 16 17 # define channel_x 2 / / kanaler t i l boardet 18 # define channel_y 1 19 # define channel_pot 0 20 21 # include <stdio. h> 22 # include < s t d l i b. h> 23 # include <string. h> 24 # include <sys / time. h> 25 # include <unistd. h> 26 # include <rhd. h> 27 # include <comedilib. h> 28 29 int main ( int argc, char argv [ ] ) { 30 31 symtableelement symtable ; 32 double oldtime = 0, newtime = 0; 33 34 / 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 ; 37 38 unsigned int accxinit = 0; 39 int n = 0; 40 41 / Pointer t i l l o g f i l e / 42 FILE logfile ; 43 int samples = 0, counter = 0; 55

Bilag III. Kildekode 44 45 / Regulering / 46 int correction = 0; 47 double speedfromacc = 0; 48 double speedfromacc_temp = 0; 49 double const_end_pos = 0; 50 51 / 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 ; 59 60 / 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; 76 77 / Find revision 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 ) ; 83 84 / / Open comedi 85 comedi_t cf ; 86 cf = comedi_open ( " / dev / comedi0 " ) ; 87 i f ( cf == 0) 88 printf ( " c f = 0!! \ n" ) ; 89 else 90 printf ( " c f connected!! \ n" ) ; 91 92 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) ; 96 97 / 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 ; 103 104 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 ; 107 108 / Hostname given as input / 109 i f ( argc >= 2) { 110 strncpy ( versionstring, argv [ 1 ], 6 4 ) ; 111 printf ( " Hostname read from input : %s\n", versionstring ) ; 112 } else { 113 strncpy ( versionstring, " 127.0.0.1 ",64) ; 114 printf ( "No hostname in input. Connecting to l o c a l h o s t \n" ) ; 115 } 116 117 / 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 quitting\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 } 129 130 i f ( rhdconnect ( r,versionstring, 0 ) > 0) { 131 printf ( " Connect completed\n\n" ) ; 132 symtable = getsymboltable ( r ) ; 133 134 135 / Her s t a r t e r loopet / 136 while ( ( rhdsync ( ) > 0) && ( counter <= samples ) ) { 137 / / Print time 138 symtable = getsymboltable ( r ) ; 139 newtime = ( double ) symtable [ 0 ]. timestamp [ 0 ]. tv_sec + ( double ) symtable [ 0 ]. timestamp [ 0 ]. tv_usec / 1000000; 140 141 i f ( ( logfile!= NULL ) ) { 142 printf ( " Period time %6.4 f %d %d\n", newtime oldtime, counter, samples ) ; 143 oldtime = newtime ; 144 145 / / Print read buffer 146 printf ( "Read:\n" ) ; 147 / / Der hentes vaerdier fra sensorene 148 / / midler over nogle samlpes 149 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 } 154 155 accelerationx = temp_average / 14; 156 157 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 constants 160 poscal = ( ( ( double ) position 1.216 ) /1000 0. 925) ; 161 acccalx = acccalx const_gravity / const_length_of_arm ; 162 163 / / skalerer f o r skaevhed i accelerometret 164 i f ( acccalx > 0) { acccalx = acccalx / 0. 8 6 1 ; } 165 i f ( acccalx < 0) { acccalx = acccalx / 0. 7 7 ; } 166 167 / / Vender koordiantsystemet korrekt 168 acccalx = acccalx ; 169 170 171 i f ( poscal > 1.900) / / armen er ude af det t i l l a d t e omraade 172 { 173 poscal = 1. 900; 174 printf ( "Arm out of bound!!!!!!!! \ n" ) ; 175 } 176 else i f ( poscal < 0. 010) 177 { 178 poscal = 0. 010; 179 printf ( "Arm out of bound!!!!!!!! \ n" ) ; 180 } 181 182 / Slut paa hentning af vaerdier fra sensore / 183 184 / Regulering beregninger / 185 186 / Integrerer accelerationen / 187 speedfromacc_temp = speedfromacc_temp + acccalx ; 188 speedfromacc = speedfromacc_temp / const_samples_pr_second ; / / Scalere f o r samples pr second 189 190 / Laver en rampe paa indputtet f o r at undgaa en f o r k r a f t i g s t a r t / 191 temp_end_pos = InitStartPos + counter 0. 020; / / 0.02 192 i f ( temp_end_pos > const_end_pos ) 193 { temp_end_pos = const_end_pos ; } 194 195 / Laver en sinus paa inputtet / 196 / / temp_end_pos = InitStartPos + 0.5 sin ( counter 0. 01) ; 197 198 / / Laver en positionsservo 199 angularspeed = ( temp_end_pos poscal ) 2. 5 ; 200 201 / Laver Z highpass / 202 temp_test_high = 0.9496 speedfromacc + HPdummy1 ; 203 204 HPZI2 = HPZI1 ; 58

205 HPZI1 = speedfromacc ; 206 HPZU2 = HPZU1 ; 207 HPZU1 = temp_test_high ; 208 209 HPdummy1 = ( 1.899 HPZI1 + 0.9496 HPZI2 ) (( 1.897) HPZU1 + 0.9018 HPZU2 ) ; 210 211 speedfromacc = temp_test_high ; 212 / Slut paa Z highpass / 213 214 / Laver Z Notch / 215 temp_test_notch = 0.6509 speedfromacc + NOdummy1 ; 216 217 NOZI2 = NOZI1 ; 218 NOZI1 = speedfromacc ; 219 NOZU2 = NOZU1 ; 220 NOZU1 = temp_test_notch ; 221 222 NOdummy1 = (( 1.197) NOZI1 + 0.6459 NOZI2 ) (( 1.197) NOZU1 + 0.2968 NOZU2 ) ; 223 224 speedfromacc = temp_test_notch ; 225 / / compare_dummy = speedfromacc ; 226 / Slut paa Z Notch / 227 228 / Laver 1 ordens Z highpass / 229 temp_test_high2 = 0.996868 speedfromacc + HPdummy2 ; 230 231 HPZ2I1 = speedfromacc ; 232 HPZ2U1 = temp_test_high2 ; 233 234 HPdummy2 = ( 0.996868 HPZ2I1 ) (( 0.993736) HPZ2U1 ) ; 235 236 speedfromacc = temp_test_high2 ; 237 compare_dummy = temp_test_high2 ; 238 / Slut paa 1 ordens Z highpass / 239 240 / / Hastighedstilbagekobling 241 angularspeed = ( angularspeed speedfromacc ) 0. 8 ; 242 243 / Laver Z Lead / 244 / / compare_dummy = angularspeed ; 245 temp_test_lead = 1.303 angularspeed + LEdummy1 ; 246 247 LEZI1 = angularspeed ; 248 LEZU1 = temp_test_lead ; 249 250 LEdummy1 = (( 1.179) LEZI1 ) (( 0.8759) LEZU1 ) ; 251 252 angularspeed = temp_test_lead ; 253 / Slut paa Z Lead / 254 255 / Regulering slut / 256 257 / Udskriver parametre t i l terminalen / 59

Bilag III. Kildekode 258 i f ( acccalx > 18.000) 259 { printf ( "!!! ACC LIMIT!!! " ) ; } 260 printf ( " p o s ition : %l f \n",poscal ) ; 261 printf ( " acceleration : %l f \n",acccalx ) ; 262 printf ( "temp t e s 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 } 266 267 / Saetter hastigheden paa motoren / 268 correction = ( int ) ( ( ( angularspeed 1000) / 2. 3 ) + 2048) ; 269 printf ( " c o r r e c t i o n : %d\n",correction ) ; 270 comedi_data_write ( cf, sout, 0, 0, 0, correction ) ; 271 272 / / Log i f samples are defined 273 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 } else { 284 printf ( " Connect f a i l e d \n" ) ; 285 } 286 287 / 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 f o r 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

BilagIV Indhold 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 Controller: Practical Issues and Design Limits Vibration Suppression Control using the Load-side Acceleration Feedback Vibration Control of a Cantilever Beam Using Adaptive Resonant Control Notch Filter Design Vibration Absorption Control of Industrial Robots by Acceleration Feedback rhd2.0 (med undermapper) Matlab DataPlotter 61