Indentifiering och reglering av industrirobot med hjälp av accelerometer

Identifiering och reglering av industrirobot med hjälp av

accelerometer

Examensarbete utfört i Reglerteknik

av Alexander Nordström

Alexander Nordström

LiTH-ISY-EX--06/3785--SE

Examensarbete utfört i Reglerteknik vid Linköpings Tekniska Högskola

av

Alexander Nordström Reg nr: LiTH-ISY-EX--06/3785--SE

Handledare: Stig Moberg Mikael Norrlöf Erik Wernholt Examinator: Svante Gunnarsson Linköping, 12 april 2006

Publiceringsdatum (elektronisk version) 2006-04-12

URL för elektronisk version

http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-6277

Publikationens titel

Indentifiering och reglering av industrirobot med hjälp av accelerometer Författare

Alexander Nordström

Sammanfattning

Dagens robotindustri har som mål att bygga billigare och lättare robotar, vilket ger allt vekare robotar. Denna rapport studerar identifiering och reglering med en extra armsensor – accelerometer – på en flexibel industrirobot. Studien har skett både simulerat och experimentellt på en industrirobot.

Vid identifiering med accelerometer befinner sig accelerometern i ett roterande koordinatsystem. Denna rapport presenterar en kompenseringsalgoritm för att transformera signalerna från det roterande koordinatsystemet till ett rumsfast

koordinatsystem.

I denna rapport presenteras regulatorstrukturerna I-PID, II2-PID och LQG, vilka alla återkopplar motorvinkel och armvinkelacceleration. Som referens har PID-regulator med endast motorvinkelåterkoppling använts. Simuleringsmässigt studeras regulatorerna både på ett fyrmassesystem och på en treaxlig modell av roboten. Vid reglering av fyrmassesystemet ger LQG-regulatorn bäst prestanda med avseende på minimerande av överslängar och insvägningstid. Vid reglering av den treaxliga modellen uppkommer endast små skillnader mellan reglermetoderna. Experimentellt studeras endast I-PID-regulatorn, beroende på dess enkelhet i strukturen och den lilla prestandaskillnaden simulerat mot övriga regulatorer. Experimenten visar att prestanda (minskade överslängar och insvägningstider) förbättras vid användandet av en I-PID-regulator istället för PID-I-PID-regulatorn och att I-PID-I-PID-regulatorn har robust prestanda med avseende på modellvariationer i framkopplingsmodellen och robust stabilitet när återkopplingsförstärkningen kan ökas utan att instabilitet uppstår. m sammanfattningen inte får plats på en sida, försök att korta ned den eller minska teckenstorleken så att hela biblioteksbladet omfattar endast en sida. Infoga biblioteksbladet sist i uppsatsen/examensarbetet.

Nyckelord

accelerometer, industrirobot, identifiering, reglering Språk

X Svenska

Annat (ange nedan)

Antal sidor 87 Typ av publikation Licentiatavhandling X Examensarbete C-uppsats D-uppsats Rapport

Annat (ange nedan)

ISBN (licentiatavhandling) ISRN LiTH-ISY-EX--06/3785--SE Serietitel (licentiatavhandling)

One of the goals for robot industry today is to decrease expenses of material, which makes more flexible robots. This thesis studies identification and control with an extra arm sensor – accelerometer – on a flexible industrial robot. The results are both from simulations and experiments of an industrial robot.

In identification the accelerometer is in a revolving coordinate system. An algorithm of compensation for transforming the signals in the revolving frame to the base frame is presented.

The introduced regulators I-PID, II2-PID and LQG will be used, which all feedback motor angle and arm angle acceleration. A PID-regulator with only motor angle feedback has been used as reference. The regulators are simulated on a system and on a three axis model of the robot. In control of the fourmass-system the LQG-regulator gives the best performance, according to minimized overshoots and settling times. In control of the three axis model only smaller differences appear. Experimental is only the I-PID-regulator studied, because of its simple structure and simulated small differences of performance with respect to other regulators. The experiments show that the I-PID-regulator has better nominal and robust performance (minimized overshoots and settling times) compared to the PID-regulator.

Dagens robotindustri har som mål att bygga billigare och lättare robotar, vilket ger allt vekare robotar. Denna rapport studerar identifiering och reglering med en extra armsensor – accelerometer – på en flexibel industrirobot. Studien har skett både simulerat och experimentellt på en industrirobot.

Vid identifiering med accelerometer befinner sig accelerometern i ett roterande koordinatsystem. Denna rapport presenterar en kompenseringsalgoritm för att transformera signalerna från det roterande koordinatsystemet till ett rumsfast koordinatsystem.

I denna rapport presenteras regulatorstrukturerna I-PID, II2_{-PID och LQG, vilka} alla återkopplar motorvinkel och armvinkelacceleration. Som referens har PID-regulator med endast motorvinkelåterkoppling använts. Simuleringsmässigt studeras regulatorerna både på ett fyrmassesystem och på en treaxlig modell av roboten. Vid reglering av fyrmassesystemet ger LQG-regulatorn bäst prestanda med avseende på minimerande av överslängar och insvägningstid. Vid reglering av den treaxliga modellen uppkommer endast små skillnader mellan reglermetoderna. Experimentellt studeras endast I-PID-regulatorn, beroende på dess enkelhet i strukturen och den lilla prestandaskillnaden simulerat mot övriga regulatorer. Experimenten visar att prestanda (minskade överslängar och insvägningstider) förbättras vid användandet av en I-PID-regulator istället för PID-regulatorn och att I-PID-regulatorn har robust prestanda med avseende på modellvariationer i framkopplingsmodellen och robust stabilitet när återkopplingsförstärkningen kan ökas utan att instabilitet uppstår.

Arbetet bakom denna rapport har gett mig stor tvärvetenskaplig kunskap inom reglerteknik och mekanik, vilket har stärkt mig i min roll som blivande civilingenjör. Flera personer har jag att tacka för deras handledning. Främst vill jag tacka min handledare industridoktorand Stig Moberg, ABB Automation Technologies – Robotics, för sitt stora kunnande inom robotik. Jag vill även tacka mina universitetshandledare, doktorand Erik Wernholt för sitt intresse och sin handledning inledningsvis i mitt arbete och docent Mikael Norrlöf som alltid ställt upp att svara på mina frågor. Jag är även tacksam över att min examinator professor Svante Gunnarsson intresserat sig över mina frågor och gett mig nya idéer. Doktor Jonas Öhr, ABB Automation Technologies – Corporate Research, har hjälp mig med laboratorieutrustning och tålmodigt lyssnat på mina antaganden och idéer. Civilingenjör Sofie Persson, ABB Automation Technologies – Robotics, hjälpte mig att implementera mina regulatorer på verklig robot och varit till stor hjälp under experiment.

Jag är tacksam att ABB Automation Technologies – Robotics erbjöd mig ett spännande examensarbete och mycket handledarstöd, vilket har utvecklat mitt självförtroende som blivande civilingenjör. Jag vill ge ett stort tack till nämnda personer och övriga på ABB Automation Technologies – Robotics som stöttat mig i mitt arbete.

Alexander Nordström Linköping, 12 april 2006

Kapitel 1 Inledning ...1

1.1 Rapportens syfte...1

1.2 Problemet som skall lösas...2

1.3 Bakgrund...2 1.4 Disposition ...3 Kapitel 2 Robotik...5 2.1 Kinematik...6 2.1.1 Positionskinematik...6 2.1.2 Hastighetskinematik...6 2.1.3 Accelerationskinematik...7 2.2 Dynamik...7 2.2.1 Stelkroppsdynamik ...7

2.2.2 Dynamikmodell med flexibiliteter...7

2.2.3 Fyrmassemodell ...8 2.2.4 Linjariserad modell ...9 2.2.5 Treaxlig modell...11 Kapitel 3 Utrustning...13 3.1 Robotar...13 3.2 Leica...13 3.3 Accelerometer ...13 3.3.1 Piezoelektrisk accelerationsgivare ...14 3.3.2 Accelerometeregenskaper ...14 3.3.3 Pris ...14

Kapitel 4 Identifiering med accelerometer...17

4.1 Val av insignal ...17 4.1.1 Chirp ...17 4.1.2 Multisinus ...18 4.2 Signalbehandling...18 4.2.1 Kinematisk transformation...19 4.2.2 Kontinuerlig transformation...21 4.2.3 Lågpassfiltrering av armvinklar...23 4.2.4 Test av rotationskompensering ...24

4.3 Multivariabel frekvensfunktions estimering (MFRFE) ...26

4.3.1 Experiment: Skattning av MFRF...27

4.4 Sammanfattning ...29

Kapitel 5 Reglering med accelerometeråterkoppling...31

5.1 Modellreduktion...31

5.2 Val av högpassfilter vid accelerationsintegrering...33

5.2.1 Integrerande verkan på högpassfilter ...35

5.4.1 Integrerande verkan ...40

5.4.2 Observatör...40

5.4.3 Kalmanfilter ...41

5.4.4 Kopplingar mellan II2-PID och LQ...42

Kapitel 6 Simuleringsresultat ...43 6.1 Reglering av fyrmassemodell ...43 6.1.1 Reglermål...43 6.1.2 Resultat från simulering...46 6.1.3 Slutsatser ...50 6.1.4 Diskussion...50

6.2 Reglering av treaxlig modell...50

6.2.1 Resultat ...51

6.2.2 Robust stabilitet ...53

Kapitel 7 Experimentella resultat ...57

7.1 Försöksuppställning ...57

7.2 Störningsundertryckning...58

7.2.1 Robust prestanda och robust stabilitet ...61

7.3 Punkt-till-punkt rörelse ...63

7.3.1 Robust prestanda och robust stabilitet ...64

Kapitel 8 Slutsatser och utvidgningar ...67

8.1 Slutsatser ...67

Symboler, operatorer och funktioner

Beteckning Betydelse t Variabel betecknande tid

s Laplacetransformvariabel z Z-transformvariabel p Deriveringsoperator,

( )

( )

dt t du t pu = q Skiftoperator, qu

( ) ( )

t = tu +1

( )

t

x& Derivata av x

( )

t med avseende på tiden

∈ Tillhör

θ

Vektor med okända parametrar

N Antal sampel

s

T Samplingstid

ϕ _{Vektor med ledkoordinater,}

₍

₎

T n

ϕ

ϕ

ϕ

= 1,...,

Förkortningar och akronymer

Förkortning Betydelse

TCP Tool Center Point

Base frame Rumsfast koordinatsystem

DOF Degrees Of Freedom, frihetsgrader

FRF Frequency Response Function (frekvensfunktion) IRB Industrial RoBot, används i namn för ABB robotar MFRF Multivariable Frequency Response Function

MFRFE Multivariable Frequency Response Function Estimate SISO Single Input Single Output

MIMO Multiple Input Multiple Output

DFT Discrete Fourier Transform LTI Linjär Tidsinvariant (system)

PID Regulator med proportionell, integrerande och deriverande verkan

Inledning 1

Kapitel 1

Inledning

Ett mål som dagens robotindustri har är att minska materialkostnaderna, vilket ger allt vekare robotar. Om mindre material används blir roboten lättare och även billigare motorer kan användas. Däremot vill robotindustrin fortfarande garantera samma reglerprestanda även då roboten blir flexiblare.

1.1 Rapportens

syfte

Traditionellt sett styrs och identifieras industrirobotar enbart med hjälp av uppmätta motorvinklar. Flexibel robot innebär vek växellåda och ibland även veka robotarmar. För en flexibel robotmodell skiljer sig verktygets position från positionen hos en helt stel robotmodell. Se figur 1.1.

motor

θ θarm

Figur 1.1 Flexibel robotarm. Armvinklar och motorvinklar skiljer sig åt enligt

θ θ

θ_arm = _motor +∆ .

Ett möjligt sätt att skatta verktygets position är att placera en accelerometer ute vid verktyget. Syftet med denna rapport är att studera accelerometern som extra sensor utöver motorvinklarna vid identifiering och reglering. Vid identifiering studeras i rapporten vilka problem (drift, gravitation och accelerometerns rotering) som kan uppkomma vid mätning med hjälp av accelerometer och det ges förslag på hur dessa kan åtgärdas. Identifieringen skall ske i MIMO-fallet.

Rapporten har även som syfte att studera reglering med accelerometern som extra sensor. Rapporten skall besvara frågan ifall det går att uppnå bättre prestanda i

SISO-fallet vid reglerproblemet med en tillförd accelerometer med hänsyn även till robust stabilitet1 och robust prestanda2.

Såväl vid identifiering som vid reglering skall teori och simuleringar även styrkas med verkliga experiment på flexibel industrirobot.

1.2 Problemet

som skall lösas

Rapporten skall:

• studera identifiering med accelerometer. Undersökningen skall ske såväl teoretiskt som experimentellt.

• studera simulerat och experimentellt ifall accelerometern kan ge bättre prestanda, i jämförelse med endast motorvinkelsreglering. Robust prestanda och robust stabilitet skall även studeras.

1.3 Bakgrund

Det finns ett antal artiklar i litteraturen som behandlar accelerometeråterkoppling vid identifiering och reglering för flexibla robotar.

M. Pham et al. (2002) har genomfört experimentella försök att skatta parametrarna i en tvåmassemodell med en accelerometer placerad, i första fallet, på motorn och i nästa fall ute vid verktyget. Metoden bygger på att excitera systemet med en chirpsignal för att med minstakvadratmetoden skatta okända parametrar X i en tidskontinuerlig modell y= D⋅X , där y är mätvärden och D är en regressor baserad på tvåmassmodellen och mätta storheter. Metoden beskrivs i M. Pham et al. (2002). Däremot hänvisas experimentella resultat vidare till författarnas experimentella undersökning utan närmare kommentarer.

D. Kostic et al. (2002) visar genomförda experimentella försök att kompensera vibrationer, för en treaxlig ”direct drive” robot, med en accelerometer för att skatta vibrationerna. Regleringen är uppbyggd av två delar, en förflyttningsregulator och en vibrationsregulator. Vibrationsregulatorn består av en PD-regulator. Accelerometer-reglering förbättrar resultaten.

Z. Tiemin et al. (1996) har genomfört teoretisk och experimentell studie över en flexibel manipulatorarm med bl.a. accelerationen ute vid spetsen som extra mätsignal och fått resultatet att en accelerationsåterkoppling för vibrationsdämpning tillsammans med en PID-regulator är effektiv.

M. Readman et al. (1991) påstår att tidigare forskning har visat att för stela robotar har accelerometeråterkoppling med hög förstärkning medfört bra störningsundertryckning, robusthet och banföljning. Artikeln beskriver jämförelse mellan teoretiska överföringsfunktioner för stela robotar och flexibla robotar. Resultaten visar att för flexibla och olinjära robotar kan accelerometeråterkoppling med hög förstärkning medföra instabila system.

B. De Jager (1993) har genomfört studier med en flexibel multivaribel XY-manipulator med accelerationsåterkoppling för två olika designmetoder: en metod med direkt återkoppling av accelerometern och en indirekt metod med observatör och

Inledning 3 metoderna förbättrar prestanda men robustheten ändras inte signifikant mycket. Artikeln påpekar också fördelarna med accelerometern att:

• accelerometern kan ersätta andra dyrare mätmetoder som inte är inbyggda i strukturen.

• accelerometersensorer kan enkelt placeras på strukturer. • accelerometrar är relativt billiga.

B. De Jager (1993) rekommenderar accelerationsåterkoppling via observatör vid reglering.

M. Östring (2002) har studerat en LQG-regulator med och utan accelerometeråterkoppling och visar simuleringsmässigt att regulatorn med återkoppling från accelerometern i det nominella fallet har bättre störningsundertryckningsprestanda.

E. Malmlöf et al. (2003) har undersökt användandet av olika typer av armgivare för att reglera treaxliga robotmodeller och kommit fram till att olika typer av armgivare förbättrar prestanda.

1.4 Disposition

Kapitel 2 behandlar robotik, beskriver kinematik- och dynamisk-robotmodell. Kapitel 3 beskriver den utrustning som används vid experimenten. Kapitel 4 beskriver Multivariabel frekvensfunktions skattning samt gravitations och rotations-kompensering vid accelerometermätningar. Kapitel 5 behandlar reglering med accelerometeråterkoppling. Kapitel 6 visar simuleringar av regulatorstrukturerna behandlade i kapitel 5. Kapitel 7 innehåller simuleringsresultat för experimentella försök för regulatorstrukturerna från kapitel 5 på verklig robot. Rapportens resultat sammanfattas i kapitel 8.

Robotik 5

Kapitel 2

Robotik

Robotik är en generell term och gäller maskiner som kan ersätta människan vid utförandet av ett arbete, både fysiskt och vid beslutstaganden i en process. I denna rapport studeras industrirobotar. Industrirobot innebär en mekanisk arm med ett antal leder, där varje led har ett ställdon i form av en motor via en växellåda. Rörelserna är reglerade av ett datorsystem. Denna form av robotar är ofta benämnda robotmanipulator eller enbart manipulator (M. W. Spong et al., 1989). Exempel över hur en ABB industrirobot ser ut visas i figur 2.1.

Figur 2.1 ABB industrirobot.3

Den mekaniska strukturen hos en manipulator är en serie av stela kroppar (länkar) kopplade med roterande4 _{eller translanterande leder, också benämnda axlar.} Manipulatorns länkar är uppdelade i en del (i regel de första tre länkarna för en sex axlig robot) kallad arm, vilken ger manipulatorn rörlighet, och en del (i regel de sista

som utför det aktuella arbetet. Vanligtvis har en robot sex axlar, d.v.s. sex frihetsgrader, och kan således nå varje punkt med olika orientering i arbetsrymden. (M. W. Spong et al., 1989)

2.1 Kinematik

Kinematik är beskrivningen om en kropps rörelse utan hänsyn till krafterna som skapade rörelsen. Inom kinematiken för en robot är det intressant att studera relationen mellan ledvariabler och verktygspositionen i ett rumsfast koordinatsystem.

Ledvariablerna är givna av en vektor

(

)

T n

ϕ

ϕ

ϕ

ϕ

= 1, 2,..., , där n är antalet leder. Verktygspositionen kan uttryckas med hjälp av en vektor _x∈ℜ3_{. Orienteringen av en}

punkt i ett roterande koordinatsystem i förhållande till det rumsfasta koordinatsystemet beskrivs av en rotationsmatris, _∈ℜ3×3

R . Rotationsmatrisen har egenskaper RTR= och I detR=+1. (M. W. Spong et al., 1989)

2.1.1 Positionskinematik

Det framåtkinematiska problemet är problemet att bestämma funktionen från ledvariabler till verktygets position och orientering.

( )

( )

ϕ

( )

ϕ ϕ f r x X _⎟⎟= ⎠ ⎞ ⎜⎜ ⎝ ⎛ = (2.1)

Funktionen kan beräknas iterativt utifrån basens koordinatsystem till första länken, vidare till andra länken och så vidare tills verktygets koordinatsystem nås.

Det inverskinematiska problemet är att utifrån en given position och rotation av verktygets koordinatsystem, bestämma ledvariablerna. Till skillnad från framåtkinematiska problemet har det inverskinematiska problemet i regel flera lösningar.

2.1.2 Hastighetskinematik

Hastighetskinematiken ger relationen mellan ledhastigheter och verktygets linjära hastigheter och vinkelhastigheter. Utgående från det framåtkinematiska problemet kan hastighetskinematiken skrivas som

( )

ϕ ϕ, ω J & v V _⎟⎟= ⎠ ⎞ ⎜⎜ ⎝ ⎛ = (2.2)

där _J

( )

ϕ ∈ℜ6×n _{är manipulatorns Jacobian och V betecknar verktygets linjära}

hastigheter och vinkelhastigheter relativt till basen. När Jacobianen är en funktion av ϕ innebär de konfigurationer där Jacobianen förlorar i rang att roboten förlorar frihetsgrader. Dessa konfigurationer kallas singulariteter. Ett sätt att beräkna

Robotik 7

2.1.3 Accelerationskinematik

Utgående från det framåtkinematiska problemet och hastighetskinematiken kan accelerationskinematiken skrivas som

( )

ϕ ϕ

( )

ϕϕ ω& J && J& &

a A _⎟⎟= + ⎠ ⎞ ⎜⎜ ⎝ ⎛ = (2.3)

2.2 Dynamik

Kinematik beskriver kroppars rörelse utan beaktande av rörelsens orsak. I detta avsnitt kommer modellen att utökas till att krafter och moment skapar rörelser.

2.2.1 Stelkroppsdynamik

Med hjälp av Euler-Lagrangeformulering kan man skapa en modell för en stelkroppsrobot med n generaliserade koordinater. I denna rapport betecknas de generaliserade koordinaterna ϕ₁,...,ϕ_n.

Modellen som ges enligt Euler-Lagrangeformulering blir den dynamiska ekvationen

( )

ϕ

ϕ

+C

( )

ϕ

ϕ

ϕ

+g

( )

ϕ

=

τ

M _&& ,_{& &} (2.4)

där M

( )

ϕ

är tröghetsmatrisen, C

( )

ϕ

,

ϕ

&

ϕ

& är en hastighetsberoende term, bestående av

centrifugal och Corioliseffekter, g

( )

ϕ

är gravitationstermen och

τ

är tillfört moment.

(M. W. Spong et al., 1989)

2.2.2

Dynamikmodell med flexibiliteter

En utveckling inom robotik är att bygga lättare robotar. Lättare robotar har fördelen att de minskar energikonsumtionen och är bättre ur säkerhetssynpunkt (inte lika stora massor i rörelse). Nackdelen med lättare robotar är att de blir vekare och det bildas flexibiliteter i de olika komponenterna. Det är inte bara lederna som blir flexiblare utan även länkarna.

Flexibla manipulatorer beskrivs egentligen med partiella differentialekvationer med ett oändligt antal frihetsgrader. Att använda sig av modeller med oändligt antal dimensionsioner är omöjligt för estimering, simulering och reglering.

Istället används en förenklad modell. Det enklaste modellerings-tillvägagångssättet är en modell med endast ledflexibiliteter (ex. elastiska effekter vid utväxling mellan motorn och leden). Antas bara en led med en frihetsgrad så blir resultatet en tvåmassemodell enligt figur 2.2.

m

J och J är trögheter för motorn respektive armen, r är utväxlingen, _a k och d är fjäderkonstant respektive dämpning (modellerar flexibiliteten), f är viskösa m

(

)

(

)

(

−

) (

− −

)

=0 − = − + − + + a m a m a a a m a m m m m m r d r k J r rd r rk f J ϕ ϕ ϕ ϕ ϕ τ ϕ ϕ ϕ ϕ ϕ ϕ & & && & & & && (2.5) (M. W. Spong et al., 1989) m J a J m f m ϕ τ, r k d a ϕ Figur 2.2 Tvåmassemodell m J J_a1 J_a2 J_a3 1 k k2 k3 1 d d₂ d3 m θ θ_a1 θ_a2 θ_a3 u w v

Figur 2.3 Olinjär fyrmassemodell av robotarmen

2.2.3 Fyrmassemodell

Tvåmassmodellen (2.5) kan utvidgas till en fyrmassemodell (se figur 2.3) av robotarmen för att ytterligare beskriva armens svägningsmoder.

Fyrmassemodellen som beskrivs kommer att ha en olinjär elasticitet i växellådan och en tidsfördröjning T i mätsystemet för motorpositionen. d

Parametrarna k1 och d1 motsvarar den olinjära styvheten (se figur 2.4) respektive dämpningen i växellådan och motsvarande parametrar för armen betecknas med k₂, d₂, k och ₃ d . Utväxlingen i växellådan betecknas ₃ n. Motorns

tröghetsmoment betecknas J och armens tröghetsmoment har delats upp i m J , a1 Ja2

samt J_a3. Parametrarna f , _m f , _a1 f och _a2 f betecknar viskös friktion för motor och _a3 arm. θm är motorvinkeln (vilken mäts) och θa1, θa2 respektive θa3 är armvinklar för

de tre armmassorna.

På sista massan är även en accelerometer placerad och mäter verktygets acceleration z&&.

Styrsignalen är motormoment u medan w och v betecknar störningar på motor

Robotik 9 -0.08-5 -0.06 -0.04 -0.02 0 0.02 0.04 0.06 0.08 -4 -3 -2 -1 0 1 2 3 4 5

Delta position [rad]

Mo me n t [ N m ] Olinjär växel

Figur 2.4 Olinjär växelstyvhet

2.2.4 Linjariserad

modell

Den linjariserade modellen för fyrmassemodellen i figur 2.3 beskrivs

(

)

(

)

(

)

(

)

(

)

(

)

(

)

(

)

(

)

(

)

(

)

k

(

)

f v d J f k d k d J f k d k d J w u f k d J a a a a a a a a a a a a a a a a a a a a a a a a a a m a m a a a m m a m a m m m = + − + − + = + − + − + − + − + = + − + − + − + − + + = + − + − + 3 3 2 3 3 2 3 3 3 3 2 2 3 2 3 3 2 3 1 2 2 1 2 2 2 2 1 1 2 1 2 2 1 2 1 1 1 1 1 1 1 1 1 1 0 0 θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ & & & && & & & & & && & & & & & && & & & && (2.6)

Reglerstorheten är verktygets position z som för små variationer kring en arbetspunkt beräknas enligt

(

)

n l l l z₌ 1

θ

a1+ 2

θ

a2 + 3

θ

a3 _(2.7)

där n är utväxlingen och l1, l2 och l är avståndet mellan de (fiktiva) massorna och 3

verktyget.

Accelerometern mäter accelerationen av verktygets position z enligt

(

)

n l l l

z_&&= 1θ&&a1+ 2θ&&a2 + 3θ&&a3 _(2.8)

( )

( )

( )

( ) ( )

( ) ( )

( )

( )

( )

t Ex

( )

t z t Cx t y t v t B t w t B t Bu t Ax t x _{w v} = = + + + = & (2.9) Om tillstånden väljs enligt

[

]

T a a a m a a a m

x= θ θ ₁ θ ₂ θ ₃ θ& θ&₁ θ&₂ θ&₂ (2.10) kan fyrmassemodellen (2.6) skrivas på tillståndsformen (2.9) med följande val av

matriser (nominella parametrar väljs enligt appendix B.1):

⎥⎦ ⎤ ⎢⎣ ⎡ = ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + − − + + − + − + + − + − + − − = 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 , 0 0 0 1 0 0 0 0 , 0 0 0 10 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 3 2 1 3 2 1 3 3 3 3 3 3 3 3 3 3 2 2 2 2 3 2 2 2 2 3 2 3 2 2 2 1 2 1 1 2 1 1 1 1 2 1 2 1 1 1 1 1 1 1 n l n l n l E n l n l n l C J B J B J B J f d J d J k J k J d J f d d J d J k J k k J k J d J f d d J d J k J k k J k J d J f d J k J k A a v m w m a a a a a a a a a a a a a a a a a a a m m m m m (2.11)

Det linjariserade fyrmassemodellen har bodediagram enligt figur 2.5. (S. Moberg et al. 2004)

Robotik 11 Bode Diagram Frequency (Hz) M a g n itu de ( d B ) ; P has e ( d eg) 100 101 102 -360 0 -90 -180 -270 0 90 180 0 20 40 60 0 20 40 60

Figur 2.5 Bodediagram för nominell modell av fyrmassesystemet. Överst visas amplitud och fas för överföringsfunktionen från motormoment till motorvinkelacceleration. Nederst visas motsvarande överföringsfunktion från motormoment till armvinkelsacceleration.

2.2.5 Treaxlig

modell

I rapporten används även en elastisk MIMO-modell. Modellen är en modell över de första tre axlarna med vekheter både i växellåda och i armen. Modellen för vekheten i armen är fjädrar och dämpare i alla koordinataxlar, d.v.s. armen innehåller även elastisiteter i rörelseriktningar som inte är huvudrörelsen runt axeln.

Motorerna approximeras som enkla roterande trögheter och att motorerna egentligen rör sig beroende på rörelser av armen är försummade. Den här förenklingen kan antas under antagandet att varje motor i huvudsak rör sig av sin egen rotation. Gyroskopiska krafter mellan varje rotor är försummade.

Utrustning 13

Kapitel 3

Utrustning

Detta kapitel ger en beskrivning över den utrustning som använts vid experimenten.

3.1 Robotar

De robotar som experimentell data är uppmätta på är två olika sexaxliga robotprototyper. Industrirobotar finns i olika storlekar och har olika tillämpningar. De robotar som experimenten är utförda på har det tänkta användningsområdet punktsvets. I denna rapport kallas robotarna för Robot 1 respektive Robot 2. MIMO-modellen som används i denna rapport har nominella parametrar identifierade på Robot 2.

3.2 Leica

Vid experimenten har ett Leica lasermätningssystem LT(D) 600, levererat av Leica Geosystems, används. I rapporten har detta system benämnts Leica. Leica är ett positionsmätningssystem. Systemet består av ett reflektoröga, vilket är placerat i ett fäste längst ute på robotarmen, och en spårare. Spåraren använder laser för att detektera positionen hos reflektorögat och kan mäta rörelser i alla frihetsgrader med maximal acceleration 2G och en noggrannhet på ca en hundradels millimeter. För mer information över Leica-systemet se Leica Geosystems (2006).

3.3 Accelerometer

En accelerometer CXL02LF3 levererat av företaget Crossbow har använts vid experimenten. Se CXL-LF Series (2005) för datablad över accelerometern.

3.3.1 Piezoelektrisk

accelerationsgivare

Den vanligaste givaren för accelerationsmätning är den piezoelektriska5, accelerationskännande givaren. I denna rapport används en ”silicon micro-machined” sensor vilket är ett material som fungerar enligt samma princip som piezoelektriskt. I princip är en sådan utformad enligt figur 3.1.

3

1 2

Figur 3.1 Principuppbyggnad för piezoelektrisk givare. 1. Seismisk massa, 2. Piezoelektriska kristaller och 3. Fjädrande bricka

En förspänd fjäderbricka pressar en rörlig massa mot de piezoelektriska kristallerna. Eftersom fjäderkonstanten är mycket hög, får man höga egenfrekvenser. Den kraft som kristallerna utsätts för blir en linjär funktion av den rörliga delens läge relativt den fasta. Utspänningen blir således proportionell mot den rörliga massans svängning vid frekvenser som är låga i förhållande till egenfrekvensen. (P. Lindahl et al. 1996) Utöver en övre gränsfrekvens som bestäms av systemets egenfrekvens tillkommer en undre gränsfrekvens som beror av läckresistansen hos spänningsförstärkaren. (P. Lindahl et al. 1996)

3.3.2 Accelerometeregenskaper

Lineariteten är ett mått på maximala avvikelsen hos kalibreringskurvan från en rak linje.

(

out g out g

)

g out V V V Linearity _{,0 , 1 , 1} 2 1 − + + − = (3.1)

(Accelerometers and how they work 2005)

Enligt datablad över accelerometern är lineariteten ±2%. Känslighet är ett mått på hur mycket utsignalen från sensorn ändras vid en ändring hos accelerationen.

g V V g V y

Sensitivit out out g out g

2 1 , 1 ,+ − − = ∆ = (3.2)

(Accelerometers and how they work 2005) Enligt datablad är känsligheten 1 V/g.

3.3.3 Pris

Utrustning 15 strukturer och inte behöver vara inbyggda som många andra armgivare. Accelerometrar levereras från ett stort antal levererantörer och kostar från några hundra till flera tusen kronor. Framförallt är det noggrannhet och mätområde som bestämmer prisnivån.

I denna rapport har accelerometern CXL02LF3 från leverantören Crossbow används. CXL02LF3 är enligt leverantören främst till för forskning och uppmätning av rörliga respektive vibrerande system. Enligt leverantören kostar motsvarande accelerometer (januari 2006) ca 500USD.

Identifiering med accelerometer 17

Kapitel 4

Identifiering med accelerometer

Detta kapitel behandlar identifiering med accelerometer. Skattningsmetoden som kommer att användas kallas Multivariabel frekvensfunktionsestimering, men inledningsvis kommer kapitlet att beskriva lämplig insignal och signalbehandling av accelerometersignalerna.

4.1 Val

av

insignal

Ofta är det önskvärt att bestämma systemets frekvensfunktion inom ett frekvensband 2

1 f f

f ≤ ≤ . Det finns olika val av insignaler som åstakommer detta. Det som skiljer

signalerna åt är bandbredd och frekvensupplösning. Vid identifiering i denna rapport adderas till insignalen även en fyrkantsvåg på hastighetsignalen för att inte hindra excitation vid vissa frekvenser skapade av friktion.

4.1.1 Chirp

Chirpsignalen är en svepande sinus innehållandes frekvenser som ändras kontinuerligt över ett frekvensband f₁≤ f ≤ f₂:

( )

(

)

_⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ + − + =

π

π

2

φ

1 2 0 1 2 sin f f t T t f A t u , 0≤t≤T₀ , (4.1)

där A är amplituden på signalen och φ fasen. (R. Pintelon et al. 2001)

En chirpsignal har inte helt platt amplitudspektrum, vilket kräver längre mätningstid för en given noggrannhet. En annan nackdel är att chirpsignalen inte enbart exciterar systemet vid ett visst antal givna frekvenser utan även mellanliggande frekvenser exciteras. Detta är inga problem för linjära system, men kan ge problem för olinjära system.

4.1.2 Multisinus

Multisinussignalen innehåller samma energiinnehåll över speciellt utvalda frekvenser. Multisinussignalen kan skrivas som

( )

_∑

(

)

= + = F k k kt f A t u 1 2 sin π φ , (4.2)

med amplitud A , faser φk och frekvenser

0 0 T l f l f k k k = = där lk ∈Ν.

Faserna väljs enligt följande

(

1

)

_{,2 k F} valfritt 1 1 ≤ ≤ − − = F k k k

π

φ

φ

φ

(4.3)

Multisinussignalen får på detta sätt amplitud A vid alla frekvenser, men användaren kan om så önskas även välja olika amplitud vid olika frekvenser.

4.2 Signalbehandling

Accelerometern är monterad på robotarmen och kommer således att rotera med robotens armvinklar, se figur 4.1. Accelerometern kommer även att mäta upp gravitationen som alltid är g =−9.81m s2⋅zˆ i det fasta koordinatsystem. För att använda accelerometern vid identifiering krävs det en kompenseringsalgoritm för accelerometerns rotation och uppmätt gravitation. Avsnitt 4.2.1 ger en kompenseringsalgoritm för statisk transformation utförd kring en arbetspunkt och avsnitt 4.2.2 ger en kompenseringsalgoritm för kontinuerlig transformation baserad på motorvinklarna. Statisk transformation utförd kring en arbetspunkt är ett specialfall av den kontinuerliga transformationen, genom att roboten antas stå still.

Identifiering med accelerometer 19

Accelerometer

Verktygspositionen

Figur 4.1 Accelerometerns koordinatsystem roterar med robotarmen samtidigt som verktygspositionen är definierat i ett rumsfast koordinatsystem.

4.2.1 Kinematisk

transformation

Accelerometern kommer att röra sig enligt (2.1), ifall fyrkantsvågen (används för att inte friktion skall hindra excitation vid vissa frekvenser) är liten föreslås i detta avsitt en kompenseringsalgoritm byggd på kinematik för att omvandla accelerometerns mätsignaler till rumsfasta koordinatsystem.

xˆ

yˆ x′ˆ

y′ˆ q1

Figur 4.2 Axel 1 uppifrån

Utifrån figur 4.2 ses att en transformation från x′ˆ och y′ˆ 6 vriden med vinkel q₁ runt

axel 1 till xˆ och yˆ följer följande matris

( )

( )

( )

( )

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ′ ′ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x q q q q z y x R ˆ ˆ ˆ 1 0 0 0 cos sin 0 sin cos ˆ ˆ ˆ 1 1 1 1 1 4 4 4 4 3 4 4 4 4 2 1 (4.4)

Axel 2 fås på liknande vis

Härledning 4.1 Härledning av transformationsmatris från rumsfasta koordinater till roterade koordinater på sexaxlig robotstruktur

z′ˆ x′ˆ 2 q zˆ xˆ

Figur 4.3 Axel 2 från sidan Transformationsmatrisen blir

( )

( )

( )

( )

⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ′ ′ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x q q q q z y x R ˆ ˆ ˆ cos 0 sin 0 1 0 sin 0 cos ˆ ˆ ˆ 2 2 2 2 2 4 4 4 4 3 4 4 4 4 2 1 (4.5)

Axel 3 fås på samma sätt som axel 2

( )

( )

( )

( )

⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ′ ′ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x q q q q z y x R ˆ ˆ ˆ cos 0 sin 0 1 0 sin 0 cos ˆ ˆ ˆ 3 3 3 3 3 4 4 4 4 3 4 4 4 4 2 1 (4.6) Axel 4: z′ˆ y′ˆ 4 q zˆ yˆ

Figur 4.4 Axel 4 bakifrån

Motsvarande transformationsmatris blir

( )

( )

( )

( )

⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ′ ′ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x q q q q z y x R ˆ ˆ ˆ cos sin 0 sin cos 0 0 0 1 ˆ ˆ ˆ 4 4 4 4 4 4 4 4 4 3 4 4 4 4 2 1 (4.7)

Axel 5 ges på samma sätt som axel 3

( )

( )

( )

( )

⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ′ ′ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x q q q q z y x R ˆ ˆ ˆ cos 0 sin 0 1 0 sin 0 cos ˆ ˆ ˆ 5 5 5 5 5 4 4 4 4 3 4 4 4 4 2 1 (4.8)

Identifiering med accelerometer 21

( )

( )

( )

( )

⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ′ ′ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x q q q q z y x R ˆ ˆ ˆ cos sin 0 sin cos 0 0 0 1 ˆ ˆ ˆ 6 6 6 6 6 4 4 4 4 3 4 4 4 4 2 1 (4.9)

Enligt M. W. Spong et al., 1989 går det att multiplicera ihop alla rotationsmatriser (4.4), (4.5), (4.6), (4.7), (4.8) och (4.9) till den totala rotationsmatrisen

6 5 4 3 2 1 R R R R R R R_tot = ⋅ ⋅ ⋅ ⋅ ⋅ (4.10)

Det kan noteras att härledning 4.1 även kan användas för att gå ifrån accelerometerns koordinatsystem till rumsfasta koordinatsystemet. Skillnaden är att använda − . q_i

Från härledning 4.1 fås följande algoritm för statisk transformation av accelerometersignalen kring en arbetspunkt:

1. Beteckna de uppmätta accelerometersignalerna som

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ⋅ ′ ′ ⋅ ′ ′ ⋅ ′ z z y y x x ˆ ˆ ˆ && && && .

2. Dra bort eventuellt offset.

3. Accelerationen i ett rumsfast koordinatsystem beräknas enligt

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ⋅ ′ ′ ⋅ ′ ′ ⋅ ′ ⋅ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ ⋅ ⋅ z z y y x x R z z y y x x tot ˆ ˆ ˆ ˆ ˆ ˆ && && && && && &&

där R är detsamma som i ekvation (4.10). tot

4.2.2 Kontinuerlig

transformation

Låt q₁

( )

t , q₂

( )

t , q₃

( )

t , q₄

( )

t , q₅

( )

t och q₆

( )

t vara en funktion av tiden. kompenseringsalgoritm byggd på kinematik för att omvandla accelerometerns mätsignaler till rumsfasta koordinatsystem. Detta avsnitt behandlar kontinuerlig kompenseringsalgoritm för att omvandla accelerometerns mätsignaler till rumsfasta koordinatsystem som använder uppmätta motorvinklar.

Vid identifieringarna i denna rapport är de uppmätta motorvinklarna differentierade7 och beskriver således: q&₁

( )

t , q&₂

( )

t , q_&3

( )

t , q&₄

( )

t , q_&5

( )

t och q_&6

( )

t . Motorvinklarna är samplade med samma samplingsfrekvens som accelerometern, det vill säga

[ ]

k q

(

k Ts

)

q& = & ⋅ (4.11)

Att ur ekvation (4.11) beskriva q

[ ]

k , vilket i det kontinuerliga fallet hade varit

beskrivet genom en integration

( )

t =

_∫

tq

( )

⋅d q 0 τ τ & (4.12)

blir i det samplade fallet en summa

[ ]

[ ]

[ ]

0 1 q Ts j q k q k j + ⋅ ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ =

∑

= & (4.13)

För att transformera motorvinklarna till armvinklar krävs en 6x6 utväxlingsmatris n

( )

1

~ _{= ⋅} −

n q

q_{arm motor} (4.14)

Armen på roboten kan modelleras som ett lågpassfilter, så för att skatta armvinklarna ute vid verktygspositionen behöver q~ lågpassfiltreras genom ett lämpligt filter, se _arm kapitel 4.2.3. Därefter roteras accelerometersignalen, a

[ ]

n , för varje q_arm

[ ]

n enligt härledning 4.1.

Eftersom mängden mätdata kan vara stor, blir rotation i varje samplingstidpunkt mycket tidskrävande. Implementeringen beskriven i denna rapport använder variabel steglängd av roteringen baserad på hur mycket armvinklarna avvikit från föregående armvinkelvärde. Algoritmen för kontinuerlig transformation av accelerometersignalen baserad på motorvinklar kan sammanfattas enligt följande:

1. Beteckna accelerometersignalen ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ⋅ ′ ′ ⋅ ′ ′ ⋅ ′ z z y y x x ˆ ˆ ˆ && && &&

och motorhastigheter som

[

k

]

motor motor

motor

motor q q q

q& = &1 &2 ... & , där k är länken accelerometern är fäst vid. 2. Integrera q&_motor och multiplicera med utväxlingen n för att få armvinklar

[ ]

[ ]

[ ] ( )

1 1 0 − = ⋅ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ + ⋅ ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ =

∑

q j Ts q n k q k j arm & 3. Lågpassfiltrera armvinklarna

4. Rotera accelerometersignalen enligt härledning 4.1, d.v.s. enligt

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ′ ⋅ ′ ′ ⋅ ′ ′ ⋅ ′ ⋅ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ ⋅ ⋅ z z y y x x R z z y y x x tot ˆ ˆ ˆ ˆ ˆ ˆ && && && && && &&

där R ges av ekvation (4.10). tot

Transformationen kan bli beräkningskrävande vid många sampel så för att ge kortare svarstider kan exempelvis variabla tidssteg användas, vilket innebär att

arm

q uppdateras när villkoret max

(

q_arm

[ ]

k −q_arm

[

k−1

]

)

≥

ε

är uppfyllt.

Kompenseringsalgoritmen skriven i Matlab finns beskriven i appendix A.1.

Algoritm 4.2 Kontinuerlig transformation av accelerometersignalen baserad på motorvinklar

Identifiering med accelerometer 23

4.2.3 Lågpassfiltrering

av

armvinklar

Vid kontinuerlig transformation baserad på uppmätta motorvinklar beskriven i avsnitt 4.2.2 innehåller de uppmätta motorvinklarna mätbrus. Detta avsnitt skall studera lämpliga filter för att ta bort mätbrus.

Studera (utväxlingen är satt till 1 för tydlighetens skull)

( )

₍

(

( )

_{( )}

)

₎

,

ω

ω

ω

motor arm q DFT q DFT G = (4.15)

d.v.s. skattning av frekvensfunktionen för motorvinklar till armvinklar. Signalerna

arm

q och q_motor är hämtade från MIMO-modellen av Robot 1. Referensexciteringen är en multisinus med frekvenser från 2-40Hz. Funktionen G

( )

ω

enligt (4.15) visas i

figur 4.5. 100 101 0 10 20 Hz Q 1a/ Q 1 m

Frekvensspektrum för DFT(q_arm)/DFT(q_motor)

100 101 0 10 20 Hz Q 2a/ Q 2 m 100 101 0 10 20 Hz Q 3a/ Q 3 m

Figur 4.5 Skattad frekvensfunktion för ekvation (4.15). Om armmodellen är helt stel blir överföringen konstant ett, men i denna veka armmodell inträffar resonanstoppar vid ungefär 3-6Hz för respektive arm.

Om armmodellen är helt stel blir överföringen konstant för alla

ω

, men figur 4.5 visar att i denna veka armmodell inträffar vid ungefär 3-6Hz beroende på elastisiteter i armen. För att undvika dessa resonanstoppar filtreras motorvinklarna genom ett lågpass nollfasfilter8 med gränsfrekvenser under de värdena. I Matlabimplementeringen beskriven i appendix A.1 har ett Butterworthfilter av ordning 5 och gränsfrekvensen 2Hz använts.

4.2.4

Test av rotationskompensering

I detta avsnitt studeras rotationskompensering med excitation på alla sex axlar samtidigt. Referenssignalen till roboten har varit en multisinussignal med amplitud 10 (frekvenser 2-40Hz) och en fyrkantsignal med amplituden 12 (frekvens 0.1Hz).

Den uppmätta accelerometersignalen visas i figur 4.6. I figuren finns en överlagrad lågfrekvent periodisk funktion på alla axlar (på mittersta yˆ -axeln tydligt men går att ana på övriga). Den diskreta Fouriertransformen (figur 4.7) visar att den överlagrade periodiska funktionen har samma frekvensegenskaper som fyrkantsvågen. Bearbetas signalen med hjälp av algoritm 4.2 ser accelerometersignalen ut enligt figur 4.8 och DFT av signalen enligt figur 4.9.

Den överlagrade sinusliknande funktionen har helt försvunnit och zˆ -axeln har ett DC-offset på –1V vilket motsvarar gravitationen och är nu enkel att subtrahera bort. 0 2 4 6 8 10 12 14 16 18 -1 0 1 t [s] [V ] 0 2 4 6 8 10 12 14 16 18 -2 0 2 t [s] [V] 0 2 4 6 8 10 12 14 16 18 -2 0 2 t [s] [V]

Figur 4.6 Mätning från accelerometerns tre ortogonala axlar. Accelerometersignalen innehåller p.g.a. fyrkantsvågen en överlagrad lågfrekvent periodisk funktion på alla axlar.

Identifiering med accelerometer 25 10-2 10-1 100 101 0 20 40 60 80 f [Hz] M agn it ud e [ d B] 10-2 10-1 100 101 0 20 40 60 80 f [Hz] M agn it ud e [ d B ] 10-2 10-1 100 101 0 20 40 60 80 f [Hz] M agn it ud e [ d B]

Figur 4.7 DFT av signalen i figur 4.6. Det finns höga amplituder vid låga frekvenser (kring 0.1Hz), vilken motsvarar fyrkantvågen.

0 0.5 1 1.5 2 2.5 3 3.5 x 104 -1 0 1 t [s] [V ] 0 0.5 1 1.5 2 2.5 3 3.5 x 104 -2 0 2 t [s] [V ] 0 0.5 1 1.5 2 2.5 3 3.5 x 104 -5 0 5 t [s] [V ]

10-2 10-1 100 101 0 20 40 60 80 f [Hz] M agn it ud e [ d B] 10-2 10-1 100 101 0 20 40 60 80 f [Hz] M agn it ud e [ d B ] 10-2 10-1 100 101 0 20 40 60 80 f [Hz] M agn it ud e [ d B]

Figur 4.9 DFT av figur 4.8. De högfrekventa amplituderna som fanns i figur 4.7 har minskats.

4.3 Multivariabel

frekvensfunktions estimering (MFRFE)

I R. Pintelon et al. (2001) beskrivs en estimeringsmetod kallad Multivariabel frekvensfunktions estimering, eller i denna rapport kallad MFRFE.

Låt U_N

( )

ω_k och Y_N

( )

ω_k vara DFT över samplade signaler, där

NT k k π ω = 2 , N

k =1,2,..., . Om de samplade signalerna är periodiska gäller att

( )

( )

N

( )

k T i k N G e U Y ω = ωk ω _(4.16) där _G

( )

_eiωkT ∈_Cny×nu, y

n är antal utsignaler och n är antal insignaler till systemet, är u

MFRF. För att skatta G

( )

ωk från data, måste åtminstone n olika experiment utföras. u

Datavektorer från olika experiment samlas in i matriser där relationen mellan in och utsignal kan skrivas

( )

( )

N

( )

k T i k N G e U Y ω = ωk ω _(4.17)

Om U_N

( )

ω_k har full rank kan estimering av G

( )

ω_k genomföras enligt

( )

N

( ) ( )

k N k T

i

N e Y U

Gˆ ωk =

ω

−1

ω

_(4.18)

Identifiering med accelerometer 27

( )

k U

( )

k U ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − − = 1 1 1 1 1 1 1 1 1 (4.19)

är ett bra val. Försök i E. Wernholt (2004) styrker även detta.

4.3.1 Experiment:

Skattning

av

MFRF

En MFRF skapas i en arbetspunkt. Se E. Wernholt (2004) för närmare beskrivning över tillvägagånsätt. Kompenseringsalgoritmen enligt algoritm 4.2 används för att kompensera bort fyrkantsvågen. Figur 4.10 visar vald arbetspunkt där MFRF:en är uppmätt, enligt figuren är led 1 vriden 90°, led 2 och 3 något framåtlutade och led 6 är framåtvriden för att vikten skall ge så stor påverkan vid exciteringen som möjligt.

Figur 4.11 visar skattad MFRF. Skattningarna är osäkra vid låga frekvenser vilket till stor del kan bero på drift. Frekvensfunktionen från motormoment på motor 1 till rumsfasta koordinatsystemets xˆ-axel är väldigt tydligt vilket kommer ifrån att

över denna axel sker huvudrörelsen när axel 1 exciteras. Även överföringsfunktionen från motor 2 till rumsfasta koordinatsystemets zˆ-axel är en huvudrörelse.

0 0.5 1 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0 0.2 0.4 0.6 0.8 1 X Y Z x y z

Figur 4.10 Arbetspunkt i vilken accelerometer MFRF är skattat.

101 -40 -20 0 20 x u1 101 -40 -20 0 20 u2 101 -40 -20 0 20 u3 101 -40 -20 0 20 u4 101 -40 -20 0 20 u5 101 -40 -20 0 20 u6 101 -40 -20 0 20 y 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20 z 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20 101 -40 -20 0 20

Figur 4.11 Figur av skattad MFRF med accelerometer. Respektive motormoment (kolumnerna u1,...,u6) ger accelerationsspektra över verktygspositionen i rumsfasta koordinatsystemets respektive axel (raderna x, y och z). Figuren längst upp till

Identifiering med accelerometer 29

4.4 Sammanfattning

Kapitlet beskriver identifiering med accelerometer enligt multivariabel frekvensfunktions estimering (MFRFE). Accelerometern befinner sig i ett roterande koordinatsystem. Kapitlet beskrev två algoritmer, en statisk och en kontinuerlig, för att transformera från det roterande koordinatsystemet till ett världsfast koordinatsystem. Den statiska är ett specialfall av den kontinuerliga, på så sätt att det antas att roboten står still.

Den kontinuerliga transformationen undersöktes experimentellt vid excitering (multisinus och fyrkantsvåg) på alla sex axlar samtidigt. Efter kompensering sjönk uppmätt accelerometerfrekvensfunktioner vid frekvenser motsvarande fyrkantsvågen och uppmätt gravitation syntes tydligt.

Slutligen skattas frekvensfunktionerna av överföringsfunktionerna från motormoment till accelerationsaignaler i en arbetsposition. Frekvensfunktioner av uppmätt acceleration med starka kopplingar från det världsfasta koordinatsystemet till ledkordinatsystemet skattas tydligt.

Reglering med accelerometeråterkoppling 31

Kapitel 5

Reglering med accelerometeråterkoppling

Traditionellt sett används endast mätning av motorvinklar vid reglering av industrirobotar. Detta kapitel beskriver reglering när man har tillgång till mätning från accelerometer. Roboten är ett MIMO-system med stark korskoppling mellan in- och utsignalerna, men i denna rapport behandlas reglering i ett förenklat fall med bara en insignal och en utsignal. Regleringen sker endast kring axel 1, som har liten korskoppling till övriga axlar. Gravitationen som påverkar accelerometern är vinkelrät mot huvudrörelsen, och behöver inte kompenseras. Ingen hänsyn tas till att accelerometern roteras med roboten eftersom endast en axel regleras.

5.1 Modellreduktion

I avsnitt 5.4 kommer tillståndsåterkoppling med observatör behandlas. För att underlätta regulatordesigen beskrivs i detta avsnitt reducering av fyrmassemodellen till en tvåmassemodell. Robotsystemet kommer fortsättningsvis att vara fyrmassemodellen men regulatorer kommer att vara designade utgående från tvåmassmodellen.

I denna rapport kommer modellreduktion att ske genom att jämföra bodediagram, polnollställediagram och stegsvar hos fyrmassemodellen med motsvarande för tvåmassesystemet och skatta parametrarna i tvåmassemodellen (ekvation (2.5)) så att alla tre figurerna liknar varandra.

Antag att:

• motortrögheten är densamma för tvåmassesystemet som för fyrmassesystemet

• armtrögheten för tvåmassesystemet är en summa av trögheterna hos armmassorna i fyrmassesystemet

• armlängden i tvåmassesystemet är en summa av armlängderna i fyrmassesystemet

• utväxlingen är densamma för tvåmassesystemet som fyrmassesystemet • motorfriktionen är densamma för tvåmassesystemet som fyrmassesystemet

fyrmass a fyrmass a fyrmass a tvåmass a fyrmass m tvåmass m fyrmass tvåmass fyrmass fyrmass fyrmass tvåmass fyrmass a fyrmass a fyrmass a tvåmass a fyrmass m tvåmass m f f f f f f n n l l l l J J J J J J 3 2 1 3 2 1 3 2 1 + + = = = + + = + + = = (5.1)

Övriga parametrar har varierats med tumregeln att de skall vara ungefär i samma storleksordning som summan eller medelvärdet av ursprungliga parametrar i fyrmassesystemet. Nominella värden för det framtagna tvåmassesystemet finns i appendix B.2.

Polnollställediagram för de två systemen från motormoment till motorvinkel illustreras i figur 5.1. Vid parametrisering av tvåmassesystemet har försök genomförts så att tvåmassesystemets poler och nollställen matchar de poler och nollställen hos fyrmassesystemet som ligger nära origo.

Pole-Zero Map Real Axis Im ag in ar y A x is -45 -40 -35 -30 -25 -20 -15 -10 -5 0 -400 -300 -200 -100 0 100 200 300 400

Figur 5.1 Polnollställediagram för fyrmassesystemet (stora kryss och cirklar) och tvåmassesystemet (små kryss och cirklar).

Polernas och nollställenas position i figur 5.1 återspeglar sig i ett bodediagram, se figur 5.2. Anmärkas kan göras att bodediagrammet har förstärkningen

total

J 1

Reglering med accelerometeråterkoppling 33

låga frekvenser och förstärkningen

m

J

1 _{för höga frekvenser, första dalen går att}

beräkna med a J k

π

2

1 9 _{och första toppen kan beräknas som}

(

)

m a m a J J J J k +

π

2 1 _. Bode Diagram Frequency (Hz) M a gni tude ( d B ) ; P h as e ( deg ) 100 101 102 -270 -180 -90 0 45 0 45 90 135 180 0 20 40 60 -20 0 20 40 60 80

Figur 5.2 Bodediagram över fyrmassesystemet (heldraget), tvåmassesystemet (streckat). De två översta diagrammen visar amplitudkurvan från motormoment till motorvinkelsacceleration och de två nedersta diagrammen visar amplitudkurvan från motormoment till armvinkelsacceleration.

5.2

Val av högpassfilter vid accelerationsintegrering

Vid regulatordesign i avsnitt 5.3 kommer accelerometersignalen att integreras och dubbelintegreras och för att undvika drift högpassfiltreras signalen. I denna rapport har uteslutande ett filter av första ordningen valts, men en intressant fråga är var gränsfrekvensen skall placeras. I figur 5.3 visas experimentell accelerometermätning, när accelerometern var monterad på en stillastående robot. Efter ca 11s har bromsarna kopplats bort och regleringen startat, vilket ger det fladdrigare utseendet på accelerationssignalen. Accelerometersignalen är integrerad och dubbelintegrerad. Signalerna driver iväg (storleksordningen centimetrar efter ett par sekunder vid positionsskattningen). Om accelerometersignalen efter integration respektive dubbelintegration högpassfiltreras med ett första ordningens Butterworthfilter med gränsfrekvens 2Hz får kurvorna utseende enligt figur 5.4. Nu är driften kraftigt dämpad (storleksordningen tiondels millimetrar efter ett par sekunder vid

0 5 10 15 20 25 -0.02 0 0.02 Po s iti o n [m ]

Expermentiell ofiltrerad accelerometersignal integrerad och dubbelintegrerad

0 5 10 15 20 25 -5 0 5 x 10-3 H a s ti g h e t [m /s ] 0 5 10 15 20 25 -2 0 2 Tid [s] A c c e le ra ti o n [m /s 2]

Figur 5.3 Accelerationssignalen (figuren nederst). Efter 11s kopplas bromsarna från. I mitten finns accelerationssignalen integrerad (hastighetsskattning) och överst dubbelintegrerad (positionsskattning). Hastighetsskattningen och positionsskattningen innehåller drift.

0 5 10 15 20 25 -0.02 0 0.02 Po s iti o n [m ]

Expermentiell högpassfiltrerad med 2Hz accelerometersignal integrerad och dubbelintegrerad

0 5 10 15 20 25 -5 0 5 x 10-3 H a s ti g h e t [m /s] 0 5 10 15 20 25 -2 0 2 Tid [s] Ac c e le ra ti o n [m /s 2 ]

Figur 5.4 Accelerationssignalen (figur nederst). I mitten finns accelerationssignalen integrerad och högpassfiltrerad med ett första ordningens Butterworthfilter, överst

Reglering med accelerometeråterkoppling 35

5.2.1 Integrerande

verkan

på

högpassfilter

Överföringsfunktionen för ett diskret Butterworthfiltrer av första ordningen med gränsfrekvens 2Hz och samplingstid 0.5ms ges som

( )

995 . 0 1 − − = z z z H (5.2)

Överföringsfunktionen (5.2) har ett nollställe vid 1 på enhetscirkeln, samtidigt som överföringsfunktionen till den diskreta integratorn

( )

1 − = z T z I s _(5.3)

har en pol vid 1 på enhetscirkeln. En ansats vid implementering skulle således vara att slå ihop (5.2) med (5.3) till lågpassfiltret

( )

995 . 0 − = z T z G s (5.4) Simuleringar har visat att lågpassfiltret G

( )

z ger bättre stabilitet än integratorn och

högpassfiltret i serie, I

( ) ( )

z ⋅H z .

5.3 PID-regulatorstrukturer

Regulatorn kan implementeras på en rad olika PID-regulatorstrukturer med accelerometer inkopplad. Denna rapport behandlar PID, I-PID och II2-PID.

5.3.1 PID-reglering

Detta avsnitt beskriver reglering från uppmätta motorvinklar med hjälp av en PID-regulator. Denna reglering kommer rapporten fortsättningsvis att referera till som jämförelse vid införande av accelerometer. En PID-regulator kan skrivas enligt

( )

= ⋅

( )

+

_∫

( )

+

( )

t D I P et dt d K ds s e K t e K t u 0 , (5.5) där u

( )

t är styrsignal, r

( )

t referenssignal, y

( )

t mätsignal och e

( ) ( ) ( )

t =rt −yt .

Modellen ser schematiskt ut enligt figur 5.5.

PID-regulatorn enligt ekvation (5.5) kan implementeras så att den får en inre hastighetsåterkoppling och en yttre positionsåterkoppling, schematiskt enligt figur 5.6.

Parametrarna i figur 5.6 kan transformeras till PID-parametrarna i ekvation (5.5) enligt

im vm pm I im pm vm P vm D T K K K T K K K K K = ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ + = = 1 (5.6) PID Robot modell ref ref ϕ ϕ , & m ϕ X τ w v

Figur 5.5 Schematisk bild över PID-reglering från uppmätta motorvinklar ϕ_m.

pm K vm K s T K im vm LP dt d ref ϕ ref ϕ& m ϕ τ

Reglering med accelerometeråterkoppling 37

5.3.2 I-PID-regulator

PID HP

∫

va K ref arm ϕ& a ϕ&& m ϕ ref ref motor motor ϕ ϕ ,& τ HP

Figur 5.7 Schematisk bild över I-PID-regulator.

För att lägga till accelerometern som återkopplingssignal ansätts en s.k. I-PID10 regulator vilket är en utvidgning av PID-regulatorn beskriven i avsnitt 5.3.1. Vinkelaccelerationen integreras, högpassfiltreras och adderas till motorhastighets-återkopplingen enligt figur 5.7. Regulatorparametrarna betecknas enligt:

• K_pm – förstärkning motorpositionsfel • Kvm – förstärkning motorhastighetsfel

• T – integralfaktor för motorregulatorn _im • K – förstärkning armhastighetsfel va

5.3.3 II

2

-PID-regulator

En ansatts att utvidga I-PID-regulatorn beskriven i avsnitt 5.3.2 är att tillföra ytterligare en integrering en s.k. II2_{-PID-regulator}11 _{(se figur 5.8).}

PID HP HP

∫

∫

pa K K_va ref arm

ϕ ϕ&arm_ref

a ϕ_&& m ϕ ref ref motor motor ϕ ϕ ,_& τ HP HP

Figur 5.8 Schematisk bild över II2-PID-regulator. Anmärk att högpassfiltrerna på referenssignalerna motsvarar högpassfiltrerna på återkopplingssignalen. Det är alltså inte samma högpassfilter på alla platser.

5.3.4 Approximativ

derivering

PID-regulator enligt ekvation (5.5) kan inte implementeras exakt. I figur 5.9 visas bodediagram för PID-regulatorn (streckat). Den negativa lutningen vid låga frekvenser kommer från den integrerande faktorn i (5.5) och den positiva lutningen vid höga frekvenser kommer från den deriverande faktorn. Amplitudkurvan växer mot oändligheten när frekvensen går mot oändligheten, vilket innebär att ideal derivering har oändligt stor förstärkning vid höga frekvenser. (T. Glad et al 1989)

Ett sätt att undvika detta fenomen är att modifiera amplitudkurvan så att den planar ut vid höga frekvenser. Exempelvis kan man använda en länk enligt

bryt D f s s K 1 1 ' + (5.7)

där fbryt betecknar brytfrekvensen. Med denna kompenseringslänk ser

bodediagrammet ut enligt figur 5.9 (heldraget). Med Tustins formel12 kan (5.7) skrivas på diskret form enligt

(

)

⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛ − − ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛ + − ⋅ ⋅ s bryt s bryt D T f z T f z K 2 2 1 2 ' (5.8)