Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Modellering, identifiering och reglering av skannern
i ett laserbatymetrisystem
Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping
av
Hanna Janeke
LITH-ISY-EX--05/3612--SE
Linköping 2005
Department of Electrical Engineering Linköpings tekniska högskola
Linköpings universitet Linköpings universitet
Modellering, identifiering och reglering av skannern
i ett laserbatymetrisystem
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan i Linköping
av
Hanna Janeke
LITH-ISY-EX--05/3612--SE
Handledare: Ingela Lind
isy, Linköpigs universitet
Andreas Axelsson
Airborne Hydrography AB
Examinator: Mikael Norrlöf
isy, Linköpigs universitet
Avdelning, Institution Division, Department
Division of Automatic Control Department of Electrical Engineering Linköpings universitet S-581 83 Linköping, Sweden Datum Date 2005-02-11 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport
URL för elektronisk version
http://www.ep.liu.se/exjobb/isy/2005/3612/
ISBN — ISRN
LITH-ISY-EX--05/3612--SE Serietitel och serienummer Title of series, numbering
ISSN —
Titel Title
Modellering, identifiering och reglering av skannern i ett laserbatymetrisystem Modeling, identification and control of the scanner in a system for laser bathymetry
Författare Author
Hanna Janeke
Sammanfattning Abstract
The purpose with this masters thesis was to model the scanner in a system for laser bathymetry. The model was then used to develop a controller for the scanner so a good search pattern was achieved.
Two different types of models have been tested, a physical model and a Black Box model of Box Jenkins type. The physical model has been derived from Lag-ranges equations. Identification experiments have been used to compute the Black Box model and to find the unknown parameters in the physical model.
Three different controllers have been tested, a PID controller, a model predicti-ve controller and a controller with feedforward. The controller with feedforward gave the best result. By softening the reference signal and using feedforward a good search pattern was achieved.
Nyckelord
Abstract
The purpose with this masters thesis was to model the scanner in a system for laser bathymetry. The model was then used to develop a controller for the scanner so a good search pattern was achieved.
Two different types of models have been tested, a physical model and a Black Box model of Box Jenkins type. The physical model has been derived from Lag-ranges equations. Identification experiments have been used to compute the Black Box model and to find the unknown parameters in the physical model.
Three different controllers have been tested, a PID controller, a model predicti-ve controller and a controller with feedforward. The controller with feedforward gave the best result. By softening the reference signal and using feedforward a good search pattern was achieved.
Sammanfattning
Målet med det här examensarbetet var att ta fram en modell för skannern i ett laserbatymetrisystem. Modellen har sedan använts till att styra skannern för att få ett bra avsökningsmönster.
Två olika modeller har tagits fram, en fysikalisk modell och en svartlådemo-dell av Box-Jenkinstyp. Den fysikaliska mosvartlådemo-dellen har tagits fram med hjälp av Lagranges ekvationer. Svartlådemodellen och de okända parametrarna i den fysi-kaliska modellen har tagits fram med hjälp av identifieringsexperiment.
Tre olika regulatorer har testats, en PID-regulator, en modellbaserad predik-tionsregulator och en regulator med framkoppling. Bäst resultat gav regulatorn med framkoppling. För att få ett bra avsökningsmönster gjordes referenssigna-len mjukare. Tillsammans med den mjukare referenssignareferenssigna-len gav regulatorn med framkoppling ett tillräckligt bra avsökningsmönster.
Tack
Ett stort tack till min handledare på Airborne Hydrography AB Andreas Axelsson, för hans stöd och djupa engagemang. Jag skulle även vilja tacka min handledare på Linköpings universitet Ingela Lind och min examinator Mikael Norrlöf.
Till sist vill jag tacka all personal på Airborne Hydrography AB för all hjälp och allt stöd under arbetets gång.
Notation
Symboler
x Bokstäver med fet stil används till vektorer.
Operatorer och funktioner
cθ cos(θ)
dij Vektorn från oi till oj uttryckt i koordinatsystem i.
Ri
j Rotationsmatris från koordinatsystem j till i.
sgn(x) Tecknet av x. sp(A) Spåret av matris A. sθ sin(θ)
Ti
j Homogen transformationsmatris från koordinatsystem j till i.
vij Linjära hastigheten för koordinatsystem j relativt koordinatsystem i uttryckt i koordinatsystem i.
ωr
s,t Vinkelhastigheten för koordinatsystem t relativt koordinatsystem s
uttryckt i koordinatsystem r.
Förkortningar
ARX AutoRegressive with eXternal input. ARMA AutoRegressive Moving Average.
ARMAX AutoRegressive Moving Average with eXternal input. BJ Box-Jenkins model structure
OE Output Error model structure
Innehåll
1 Inledning 1 1.1 Hawk Eye II . . . 1 1.2 Målbeskrivning . . . 2 1.3 Begränsningar . . . 2 1.4 Disposition . . . 2 2 Systembeskrivning 3 2.1 Skannern . . . 3 Gimbalsystemet . . . 4 Vinkelmätning . . . 6 2.2 Avsökningsmönster . . . 7 2.3 Datakommunikation . . . 9 3 Kinematik 11 3.1 Positionskinematik . . . 11 Rotationsmatriser . . . 11 Homogena transformationer . . . 12 Kinematisk kedja . . . 13 Positionskinematik för gimbalsystemet . . . 14 3.2 Hastighetskinematik . . . 16 Vinkelhastighet . . . 16 Linjär hastighet . . . 17 Hastighetskinematik för gimbalsystemet . . . 17 4 Dynamik 19 4.1 Lagranges ekvationer . . . 19 4.2 Rörelseekvationer . . . 19Uträkning av kinetisk energi . . . 20
Uträkning av potentiell energi . . . 22
Uträkning av Lagranges ekvationer . . . 22
Yttre moment . . . 23 Sammanfattning . . . 23 4.3 Rörelseekvationer för skannern . . . 24 x-axeln . . . 25 y-axeln . . . 26 ix
Spegeln . . . 27
Motorerna . . . 28
Resultat . . . 30
5 Övergång från tidskontinuerlig till tidsdiskret modell 31 5.1 Sampling . . . 31
5.2 Eulers metod . . . 31
5.3 Minsta kvadratmetoden . . . 32
5.4 Skattning av vinkelhastighet och vinkelacceleration . . . 33
6 Systemidentifiering 39 6.1 Parameterskattning i linjära dynamiska modeller . . . 39
Skräddarsydda linjära modeller . . . 39
Linjära konfektionsmodeller . . . 40
Minimering av prediktionsfelet . . . 42
Specialfallet då den skattade modellen är en linjär regression . . . 43
6.2 Experimentdesign . . . 44
Val av insignal . . . 44
Val av samplingstakt . . . 44
Experiment under återkoppling . . . 45
6.3 Modellvalidering . . . 46
6.4 System Identification Toolbox . . . 46
6.5 Systemidentifiering för skannern . . . 46
Experimentdesign . . . 46
Identifiering av parametrarna i den fysikaliska modellen . . . 48
Svartlådemodell . . . 53
Resultat . . . 55
7 Reglering 57 7.1 PID . . . 57
Diskretisering av PID-regulatorn . . . 57
Användning av PID på skannern . . . 59
7.2 Modellbaserad prediktionsreglering (MPC) . . . 62
Observatör . . . 63
Prediktion . . . 64
Med referenssignal . . . 65
Analytisk lösning vid avsaknad av bivillkor . . . 66
MPC på skannern . . . 66
7.3 Framkoppling . . . 68
7.4 Sammanfattning . . . 75
8 Slutsatser och framtida arbete 77 8.1 Slutsatser . . . 77
8.2 Framtida arbete . . . 78
Litteraturförteckning 79
Innehåll xi
A Fysikaliska data för motorerna 81
B Fysikaliska data för enkodrarna 82
C Fysikaliska data för x-axeln 83
D Fysikaliska data för y-axeln 84
Kapitel 1
Inledning
1.1
Hawk Eye II
Hawk Eye II är ett flyg- eller helikopterburet lasersystem som tillverkas av Airbor-ne Hydrography AB. Systemet används bl. a. till att göra batymetriska mätningar till havs. Med det menas att göra djupmätningar. I figur 1.1 syns hur mätningarna görs genom att låta en laserstråle söka av havsbotten.
Figur 1.1. Mätningar med Hawk Eye II.
För att styra laserstrålen används en skanner. I den sitter en spegel som reflekterar ned laserstrålen mot vattenytan.
2 Inledning
1.2
Målbeskrivning
Målet med examensarbetet är att ta fram en modell för skannern. Modellen ska sedan användas för att ta fram en styralgoritm till skannern så att ett bra avsök-ningsmönster fås.
1.3
Begränsningar
Följande begränsningar har gjorts för att ge arbetet en lagom omfattning: • De referenssignaler som behövs för att få ett bra avsökningsmönster är givna.
Någon genomgång av hur de är beräknade ges inte.
• Alla tester som redovisas i den här rapporten är gjorda på skannern då den sitter i ett fristående ställ. Inga tester har gjorts på riktiga flygningar. • Testerna av regleringen har endast gjorts med en referenssignal som har en
frekvens på 12 Hz. Skannern körs normalt med en frekvens på 10 − 12 Hz. • Det program som används för att testa hur svepmönstret kommer att bli
beskrivs inte.
• De beräkningar som finns på delarnas massa, tyngdpunkt och tröghet är givna och har antagits vara korrekta.
1.4
Disposition
Rapporten har följande upplägg:• I kapitel 2 beskrivs skannern och dess önskade rörelse lite mer noggrant. • I kapitel 3 beskrivs hur de olika delarna i skannern rör sig i förhållandet till
varandra.
• I kapitel 4 tas en fysikalisk modell för systemet fram. • I kapitel 5 skrivs modellen om till tidsdiskret form.
• I kapitel 6 skattas de okända parametrarna i den fysikaliska modellen. En BJ-modell tas också fram.
• I kapitel 7 testas olika metoder för att styra skannern. • Kapitel 8 innehåller slutsatser och framtida arbete.
Alla numeriska beräkningar som beskrivs är gjorda i Matlab. För att ta fram svartlådemodellen har System Identification Toolbox för Matlab använts.
Kapitel 2
Systembeskrivning
I det här kapitlet beskrivs skannern lite mer noggrant. Först beskrivs de olika delarna. Efter det går vi igenom hur vi vill att skannern ska röra på sig. Till slut beskrivs hur kommunikationen med skannern sker.
2.1
Skannern
De rörliga delarna i skannern kallas för gimbalsystemet. För att kunna veta hur gimbalsystemet rör sig finns två enkodrar som mäter vinklar. På gimbalsystemet sitter fyra motorer som får styrströmmar från två förstärkare. Skannern går att fästa i ett ställ så att tester kan göras på den utan att hela systemet behöver vara tillgängligt (se figur 2.1).
Figur 2.1. Skannern i sitt ställ.
4 Systembeskrivning
Gimbalsystemet
Spegeln kan rotera runt två axlar kallade x och y. En rotation kring x-axeln gör att vinkeln α ändras och en rotation kring y-axeln ändrar vinkeln β. Skannern är konstruerad så att en rotation runt en axel inte ska påverka positionen runt den andra axeln. Den här konstruktionen kallas för ett gimbalsystem (se figur 2.2).
Figur 2.2. Skannerns gimbalsystem
Gimbalsystemet består av tre rörliga delar: x-axeln, y-axeln och spegeln (se figur 2.3, 2.4 och 2.5). Två stopp sitter på varje axel för att hindra spegeln att rotera mer än cirka ±15 grader.
2.1 Skannern 5
Figur 2.4. Skannerns y-axel
Figur 2.5. Baksidan på skannerns spegel
På varje axel sitter två DC-motorer som tillsammans får spegeln att rotera. Motorerna är strömstyrda vilket innebär att momentet ut från motorn beror på styrströmmen. För att styra hur stor ström som skickas in till motorerna används två förstärkare. En förstärkare är till för motorerna på x-axeln och den andra för motorerna på y-axeln. Till förstärkaren skickas en spänning U och ut kommer en ström i som är proportionell mot spänningen.
6 Systembeskrivning
Enkodrar
För att mäta spegelns vinklar α och β används två inkrementella enkodrar. De är tillverkade av Heidenhain1 och modellen heter ROD260. Att enkodrarna är inkrementella innebär att vinkeln räknas upp och ned stegvis. På ROD260 används steg på 0.005 grader.
För att göra mätningen används en princip som kallas för bildskanning. I enkodern sitter två skivor som rör sig relativt varandra. Den ena skivan är en skanningsskiva som består av spalter som släpper igenom ljus. I enkodern sitter också en mätskiva som roterar. Mätskivan är graderad med 18000 linjer med lika stora avstånd som det är mellan spalterna på skanningsskivan. Utrymmet mellan linjerna på mätskivan är transparenta. Ljus lyser på skanningskivan. Endast om skanningsskivans och mätskivans spalter ligger över varandra släpps ljus igenom. Under mätskivan sitter fotoceller som registrerar variationerna i ljus och gör om dem till elektriska signaler (se figur 2.6).
Figur 2.6. Bildskannings principen.
Enkodern gör om fotocellssignalerna till två fyrkantsvågor Ua1 och Ua2.
Signa-lerna är fasförskjutna 90 grader i förhållande till varandra för att vi ska kunna hålla
2.2 Avsökningsmönster 7
ordning på vilket håll rotationen sker åt. På mätskivan finns också ett nollmärke. När den passeras ändras värdet av referenspulsen Ua0.
Om vi bara skulle hålla ordning på hur många gånger värdet av Ua1 har varit
högt skulle upplösningen bli 360/18000 = 0.02 grader (se figur 2.7).
Figur 2.7. Antalet gånger värdet av Ua1har varit högt under en period.
Tittar vi istället på hur ofta Ua1 eller Ua2 växlar värde så blir upplösningen
360/(18000 × 4) = 0.005 grader (se figur 2.8).
Figur 2.8. Antalet gånger Ua1eller Ua2växlar värde under en period.
2.2
Avsökningsmönster
För att få en bra uppfattning av hur mätområdet ser ut är det viktigt med en jämn fördelning av mätpunkterna. Ett optimalt mönster ser ut som det i figur 2.9. Att avsökningsmönstret är bågformat beror på att det ger laserstrålen samma infallsvinkel mot vattenytan under hela svepet.
Spegelns vinklar α och β påverkar hur laserstrålen reflekteras ned mot vatten-ytan. Hänsyn måste tas till vilken hastighet, höjd och orientering som flygplanet eller helikoptern har när referenssignalen för α och β räknas ut. Det här görs kon-tinuerligt under flygningen. Skannern körs normalt med en frekvens på 10−12 Hz. För att begränsa arbetet lite har alla tester gjorts med en referenssignal som har en frekvens på 12 Hz (se figur 2.10). Signalen är genererad utifrån en modellerad flygning på 300 meters höjd.
8 Systembeskrivning −100 −80 −60 −40 −20 0 20 40 60 80 100 60 70 80 90 100 110 120 130 140 150 160 170 [m] [m]
Figur 2.9. Önskat avsökningsmönster med en skanningsfrekvens på 12 Hz och flygning
på 300 meters höjd. 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 Tid [s] Vinkel [rad]
Figur 2.10. Referenssignalen för vinkeln α (den prickade linjen) och vinkeln β (den
2.3 Datakommunikation 9
2.3
Datakommunikation
För att kunna göra styrprogram till skannern används en dator med ett reglerkort. Det tar in signaler från enkodrarna och räknar ut aktuella vinklar och lämpliga styrsignaler. Från kortet skickas styrspänningar till förstärkarna (se figur 2.11). Styrprogrammen skrivs i C++. Alla vinkelvärden och styrspänningar går att spara undan i en textfil som sedan kan studeras i Matlab.
Kapitel 3
Kinematik
För att kunna göra en fysikalisk modell av skannern behöver vi veta hur de olika delarna i gimbalsystemet rör sig i förhållande till varandra. Först kommer vi att gå igenom positionskinematik som beskriver de geometriska samband som finns för de olika delarnas position. Efter det förklaras hastighetskinematik som används för att beräkna vilka hastigheter de olika delarna har i förhållande till varandra.
Den metod som beskrivs här är vanlig inom robotiken och finns beskriven mer utförligt av Spong och Vidyasagar (1989).
3.1
Positionskinematik
Först introduceras begreppen rotationsmatris, homogena transformationer och ki-nematisk kedja. Vi kommer sedan att beräkna positionskinematiken för gimbal-systemet.
Rotationsmatriser
Antag att koordinatsystem 0 och 1 är två ortogonala koordinatsystem med origo i samma punkt. Koordinatsystem 0 har enhetsvektorerna x0y0z0 och
koordinat-system 1 har enhetsvektorerna x1y1z1. Koordinatsystem 1 kan då skrivas som
x1 y1 z1 = x0 y0 z0 R01, (3.1)
där Ri
j är rotationsmatrisen för en rotation av koordinatsystem j relativt
koor-dinatsystem i (Forsman, 2004). Antag vidare att vi har en godtycklig punkt P . Punkten P beskrivs av koordinaterna p0xp0yp0z i koordinatsystem 0 och av koordi-naterna p1xp1yp1z i koordinatsystem 1. Då kan punkten P beskrivas som
x0 y0 z0 p0 x p0y p0 z =x1 y1 z1 p1 x p1y p1 z =x0 y0 z0 R01 p1 x p1y p1 z . (3.2) 11
12 Kinematik
Koordinatsambandet blir då (Råde och Westergren, 2001) p0x p0 y p0z = R01 p1x p1 y p1z . (3.3)
Rotationsmatriserna för rotation kring koordinataxlarna x, y och z ges av (Fors-man, 2004; Råde och Westergren, 2001):
Rx(α) = 1 0 0 0 cos(α) − sin(α) 0 sin(α) cos(α) , (3.4) Ry(α) = cos(α) 0 sin(α) 0 1 0 − sin(α) 0 cos(α) , (3.5) Rz(α) = cos(α) − sin(α) 0 sin(α) cos(α) 0 0 0 1 . (3.6)
Homogena transformationer
Läget för en stel kropp kan kompakt beskrivas av dess orientering och position i en referensram (Norrlöf, 1999; Skoglar, 2003). Betrakta en godtycklig punkt P . Inför två koordinatsystem o0x0y0z0 (koordinatsystem 0) och o1x1y1z1
(koordinat-system 1). o0och o1är origo för koordinatsystem 0 respektive koordinatsystem 1.
P beskrivs av vektor p0 i koordinatsystem 0 och av p1 i koordinatsystem 1 (se figur 3.1). Då kan p0uttryckas som
p0= R01p1+ d 0
1, (3.7)
där d01 är en vektor från o0 till o1 uttryckt i koordinatsystem 0 och R01 är en
rotationsmatris från koordinatsystem 1 till koordinatsystem 0. Om vi skriver om ekvation (3.7) på kortform får vi att
p0 1 = T10p 1 1 . (3.8) Matrisen T0
1 kallas för homogen transformationsmatris. Den beskrivs av
T10=R 0 1 d 0 1 0 1 . (3.9)
Det går lätt att visa att en sekvens av koordinattransformationer kan beskrivas som produkten (Skoglar, 2003)
p0 1 = T10T21. . . Tnn−1p n 1 . (3.10)
3.1 Positionskinematik 13
Figur 3.1. Translation och rotation.
Kinematisk kedja
En kinematisk kedja består av en eller flera länkar som är sammankopplade med leder. Varje led kan antingen vara roterande eller translaterande (se figur 3.2). Om det bara finns en väg mellan två godtyckliga länkar i kedjan kallas kedjan öppen och i annat fall sluten (Nyström, 2003).
Figur 3.2. Öppen kinematisk kedja med translaterande och roterande leder.
I fortsättningen antas att vi har en öppen kinematisk kedja med leder som är roterande eller translaterande. Antag att kedjan består av n + 1 länkar numre-rade från 0 till n med start från basen. Varje led är numrerad från 1 till n där
14 Kinematik
led i sammansätter länkarna i − 1 och i. Inför variabeln qi som är i:te ledens
rotationsvinkeln för en roterande led och förflyttningen för en translaterande led. På varje länk sitter ett koordinatsystem som är numrerat på samma sätt som länkarna. Koordinatsystemet sitter fast på varje länk så att alla punkter på länk i är konstanta då de uttrycks i koordinatsystem i. Låt koordinatsystem 0 vara fast i basen. Antag att Tii−1är en homogen transformationsmatris från koordinatsystem i till i − 1. Om alla leder antingen är roterande eller translaterande får vi att Tii−1 är en funktion av enbart qi. Enligt (3.10) får vi att (Skoglar, 2003)
Tn0= T10T21. . . Tnn−1. (3.11)
Positionskinematik för gimbalsystemet
Gimbalsystemet består som tidigare beskrivits av tre rörliga delar, x-axeln, y-axeln och spegeln. Varje del kommer här att modelleras som varsin öppen kedja. Koordinatsystem 0 sätter vi i skannern (se figur 3.3). Motorerna som sitter på skannerns x- och y-axlar påverkar gimbalsystemet genom att ändra vinklarna α och β.
3.1 Positionskinematik 15
x-axelns orientering beror endast på vinkeln α. Ekvation (3.4) ger att
R0x= 1 0 0 0 cos(α) − sin(α) 0 sin(α) cos(α) . (3.12)
Eftersom x-axelns koordinatsystem har origo i samma punkt som koordinatsystem 0 får vi att Tx0= 1 0 0 0 0 cos(α) − sin(α) 0 0 sin(α) cos(α) 0 0 0 0 1 . (3.13)
y-axelns orientering beror endast på vinkeln β. Från ekvation (3.5) får vi att
R0y= cos(β) 0 sin(β) 0 1 0 − sin(β) 0 cos(β) . (3.14)
Även y-axelns koordinatsystem har origo i samma punkt som koordinatsystem 0, vilket ger att
Ty0= cos(β) 0 sin(β) 0 0 1 0 0 − sin(β) 0 cos(β) 0 0 0 0 1 . (3.15)
Spegelns orientering och position är lite svårare att beskriva än axlarnas ef-tersom den beror på både vinkeln α och vinkeln β (se figur 2.2). Gimbalsystemet är konstruerat så att en ändring av vinkeln β gör att spegeln roterar runt skan-nerns y-axel. En ändring av vinkeln α gör att att spegeln roterar runt sin egen x-axel. För att modellera detta införs en kinematisk kedja som består av två ro-terande leder, där länk 1 antas vara masslös och länk 2 motsvarar spegeln. Först roteras spegeln β grader runt y-axeln, vilket ger att
R01= cos(β) 0 sin(β) 0 1 0 − sin(β) 0 cos(β) . (3.16)
Sedan roteras spegeln α grader runt sin egen x-axel, så
R12= 1 0 0 0 cos(α) − sin(α) 0 sin(α) cos(α) . (3.17)
Spegelns orientering beskrivs av
R0s= R 0 2= R 0 1R 1 2= cos(β) 0 sin(β) 0 1 0 − sin(β) 0 cos(β) 1 0 0 0 cos(α) − sin(α) 0 sin(α) cos(α) = (3.18) =
cos(β) sin(β) sin(α) sin(β) cos(α)
0 cos(α) − sin(α)
− sin(β) cos(β) sin(α) cos(β) cos(α)
16 Kinematik
Koordinatsystem 0, 1 och 2 är placerade så att de har origo i samma punkt. Därför får vi att spegelns orientering och position beskrivs av den homogena transforma-tionsmatrisen Ts0= T20=
cos(β) sin(β) sin(α) sin(β) cos(α) 0
0 cos(α) − sin(α) 0
− sin(β) cos(β) sin(α) cos(β) cos(α) 0
0 0 0 1 . (3.19)
3.2
Hastighetskinematik
Vi vill nu kunna uttrycka den linjära hastigheten v och vinkelhastigheten ω för koordinatsystem n i koordinatsystem 0 som som en funktion av länkarnas hastig-heter ˙q, d. v. s. v0n= Jv(q) ˙q, (3.20) och ω00,n= Jω(q) ˙q. (3.21) Vi inför Jakobianen J = Jv Jω . (3.22)
Efter att vi har gått igenom hur Jvoch Jwtas fram kommer hastighetskinematiken
för gimbalsystemet att beräknas.
Vinkelhastighet
Betrakta vinkelhastigheten ωrs,t av koordinatsystem t relativt koordinatsystem s uttryckt i koordinatsystem r. För en translaterande led fås att
ωii−1,i= 0. (3.23)
För en roterande led får vi att
ωii−1,i= ˙qiri−1. (3.24)
där ri−1är rotationsaxeln för system i uttryckt i koordinatsystem i − 1.
Vinkel-hastigheten i koordinatsystem 0 kan skrivas
ω0i−1,i= ˙qiR0i−1ri−1. (3.25)
Eftersom vinkelhastigheter kan adderas om de uttrycks i samma koordinatsystem får vi att ω00,n= n X i=1 ρiRi−10 ri−1q˙i, (3.26)
där ρi=0 om länk i är translaterande och ρi=1 om länk i är roterande. Vi jämför
med (3.22) och får att
Jω=ρ1R00r0 . . . ρnR0n−1rn−1 . (3.27)
3.2 Hastighetskinematik 17
Linjär hastighet
Definiera d0n som positionen av koordinatsystem n uttryckt i koordinatsystem 0. Den linjära hastigheten för länk n kan räknas ut genom att använda kedjeregeln
v0n= ˙d0n= n X i=1 ∂d0n ∂qi ˙ qi, (3.28)
vilket ger oss att
Jv= h ∂d0n ∂q1 . . . ∂d0n ∂qn i . (3.29) (Norrlöf, 1999; Skoglar, 2003)
Hastighetskinematik för gimbalsystemet
Eftersom alla koordinatsystem som används har sitt origo i samma punkt som koordinatsystem 0 så har varken spegeln eller axlarna någon linjär hastighet, dvs.
v0x= v0y= v0s= 0. (3.30)
Jakobianens hastighetsdel blir då
Jvx= Jvy = Jvs= 0, (3.31)
för axlarna och spegeln.
x-axeln består av en roterande led som roterar med vinkeln α. Därför får vi att
qx= α. (3.32)
Vinkelhastigheten för x-axeln uttryckt i koordinatsystem 0 ges då av
ω00,x= ˙α 1 0 0 = 1 0 0 q˙x. (3.33) Enligt (3.26) blir då Jωx= 1 0 0 . (3.34)
Jakobianen för x-axeln blir då
Jx= 0 0 0 1 0 0 . (3.35)
y-axelns läge ändras enbart då vinkeln β ändras. Med motsvarande resonemang som för x-axeln får vi för y-axeln
18 Kinematik och ω00,y= ˙β 0 1 0 = 0 1 0 q˙y. (3.37)
Jakobianen för y-axeln blir då
Jy= 0 0 0 0 1 0 . (3.38)
Spegeln påverkas både av vinkeln α och vinkeln β. Därför får vi att qs=β α , (3.39) ω00,1= ˙β 0 1 0 , (3.40) ω11,2= ˙α 1 0 0 . (3.41) För länk 1 blir då Jakobianen J1= 0 0 0 0 0 0 0 0 1 0 0 0 . (3.42)
Vi kan skriva om ekvation (3.41) med hjälp av ekvation ( 3.25)
ω01,2= ˙αR01 1 0 0 = ˙α cos(β) 0 − sin(β) . (3.43)
Eftersom vinkelhastigheter kan adderas får vi att
ω00,s= ω00,2= ω00,1+ ω01,2 = 0 cos(β) 1 0 0 − sin(β) q˙s. (3.44)
Jakobianen för spegeln blir då
Js= J2= 0 0 0 0 0 0 0 cos(β) 1 0 0 − sin(β) . (3.45)
Kapitel 4
Dynamik
I kapitel 3 togs samband fram för hur de olika delarna i gimbalsystemet rör sig i förhållande till varandra. Nu ska vi ta reda på hur motorerna påverkar systemets rörelse. För att kunna göra det behöver vi veta systemets dynamik.
Med hjälp av Lagranges ekvationer kommer rörelseekvationerna för en kine-matisk kedja att räknas ut. Den kunskapen kommer sedan att användas för att räkna ut en fysikalisk modell för skannern. Även denna metod beskrivs mer ut-förligt av Spong och Vidyasagar (1989).
4.1
Lagranges ekvationer
Lagranges ekvationer är ett redskap i analytisk mekanik som kan användas för att få fram ekvationer som beskriver mekaniska systems rörelse (Goldstein, 1980). Först väljs några generaliserade koordinater. I en kinematisk kedja är det lämpligt att välja q eftersom de beskriver länkarnas position i systemet på ett effektivt sätt. Då kan Lagrangianen L definieras som en funktion av q och ˙q
L = K − V, (4.1)
där K är den kinetiska energin och V är den potentiella energin. Lagranges ekva-tioner kan då beskrivas som
d dt ∂L ∂ ˙qi − ∂L ∂qi = τi i = 1, 2, . . . , n, (4.2)
där τi är det generaliserade momentet vid led i från motorn och friktionen. En
härledning av Lagranges ekvationer finns förklarad av Goldstein (1980).
4.2
Rörelseekvationer
Från Lagranges ekvationer ska vi nu härleda rörelseekvationerna för en kinematisk kedja. För att göra det krävs uttryck för den kinetiska och potentiella energin.
20 Dynamik
Uträkning av kinetisk energi
Vi vill räkna ut den kinetiska energin för en stel kropp. Antag att vi fäster ett koordinatsystem på kroppen med origo i masscentrum. Hastigheten av en punkt r på kroppen beskrivs av
v = vc+ ω × r, (4.3)
där vc är masscentrums hastighet. Definitionen av kinetisk energi är
K =1 2
Z
B
vT(x, y, z)v(x, y, z)dm, (4.4)
där integrationen görs över kroppen B:s massa m. Kryssprodukten kan skrivas som ω × r = S(ω)r (4.5) om S(ω) = 0 −ω3 ω2 ω3 0 −ω1 −ω2 ω1 0 . (4.6)
Då kan den kinetiska energin skrivas som K =1
2 Z
B
[vc+ S(ω)r]T[vc+ S(ω)r]dm. (4.7)
Integralen kan utvecklas till fyra termer. Den första termen blir K1= 1 2 Z B vTcvcdm = 1 2mv T cvc, (4.8)
eftersom vc är oberoende av integrationsvariabeln. Den första termen brukar
kallas den translaterande delen av den kinetiska energin. Den andra termen blir K2= 1 2 Z B vTcS(ω)rdm = 1 2v T cS(ω) Z B rdm = 0, (4.9)
därför att koordinatsystemet sitter i kroppens masscentrum. Den tredje termen blir också noll på samma sätt
K3= 1 2 Z B [S(ω)r]Tvcdm = 1 2S T(ω)v c Z B rdm = 0 (4.10)
För att kunna räkna ut den fjärde termen behöver vi några definitioner från linjär algebra. Summan av diagonalelementen i en kvadratisk matris A kallas spåret av A och betecknas med sp(A). För två vektorer gäller att
4.2 Rörelseekvationer 21
Den fjärde termen kan nu skrivas som K4= 1 2 Z B [S(ω)r]TS(ω)rdm = 1 2 Z B sp[S(ω)rrTST(ω)]dm (4.12) =1 2sp[S(ω) Z B rrTdmST(ω)]T =1 2ωIcω, (4.13)
där matrisen Ic kallas masströghetsmatris och definieras som
Ic= Icxx Icxy Icxz Icyx Icyy Icyz Iczx Iczy Iczz = R y2+ z2dm −R xydm −R xzdm −R xydm R x2+ z2dm −R yzdm −R xzdm −R yzdm R x2+ y2dm . (4.14)
Den fjärde termen brukar kallas den roterande delen av den kinetiska energin. Den kinetiska energin för en stelkropp kan skrivas som
K =1 2mv T cvc+ 1 2ω TI cω (4.15)
Är det viktigt att veta vilket koordinatsystem hastigheterna, vinkelhastighe-terna och masströghetsmatrisen är uttryckta i? För den första termen är det inte det eftersom vTcvc är längden i kvadrat och längden är den samma
obero-ende koordinatsystem. Den andra termen blir också den samma oavsett vilken koordinatsystem ω och Ic är uttryckt i. Det är dock önskvärt att använda ett
ko-ordinatsystem som är fast i kroppen för då blir masströghetsmatrisen Ickonstant.
Antag att vi har en kinematisk kedja. Den kinetiska energin för kedjan blir då K =1 2 n X i=1 [mvTcivci+ 1 2ω T i Iciωi]. (4.16) (Norrlöf, 1999; Skoglar, 2003)
Masströghetsmatrisen kan räknas om för att gälla i ett annat koordinatsy-stem än det som sitter fast i masscentrum. Med masströghetsmatrisen uttryckt i koordinatsystem i får vi att K = 1 2 n X i=1 [mvTi vi+ 1 2ω T i Iiωi]. (4.17)
Från avsnitt 3.2 fås att den linjära hastigheten och vinkelhastigheten för länk i ges av vi= Jvi(q) ˙q (4.18) ωi= (R0i) T Jωi(q) ˙q, (4.19) där matrisen (R0
i)T transformerar vinkelhastigheten från koordinatsystem 0 till
koordinatsystem i. Den kinetiska energin för kedjan blir då K = 1 2q˙ T n X i=1 [miJvi(q) TJ vi(q) + Jωi(q) T(R0 i)Ii(Ri0) TJ ωi(q)] ˙q. (4.20)
22 Dynamik
Ofta skrivs den kinetiska energin om på formen K =1
2q˙
T
D(q) ˙q. (4.21)
Uträkning av potentiell energi
Den potentiella energin för vår kinematiska kedja kan skrivas som V =
n
X
i=1
Vi, (4.22)
där Vi är den potentiella energin för länk i (Skoglar, 2003). För en stel kropp
kommer den potentiella energin endast från gravitationen och Vikan därför skrivas
Vi= −(g0)Tr0cimi, (4.23)
där g0är vektorn för tyngdaccelerationen uttryckt i koordinatsystem 0 (Goldstein, 1980). Notera att Vi endast är en funktion av q och inte av ˙q (Skoglar, 2003).
Uträkning av Lagranges ekvationer
För att få fram rörelseekvationer för vår kinematiska kedja använder vi Lagranges ekvationer. Lagrangianen (4.1) kan nu skrivas
L = K − V = 1 2 n X i=1 n X j=1 dij(q) ˙qiq˙j− V (q), (4.24) och d dt ∂L ∂ ˙qi = d dt n X j=1 dij(q) ˙qj= n X j=1 dij(q)¨qj+ n X j=1 n X k=1 ∂dij ∂qk ˙ qkq˙j. (4.25) Vi behöver också ∂L ∂qi = n X j=1 n X k=1 1 2 ∂djk ∂qi ˙ qkq˙j− ∂V ∂qi . (4.26) Insatt i (4.2) X j dij(q) ¨qj+ n X j=1 n X k=1 (∂dij ∂qk −1 2 ∂djk ∂qi ) ˙qkq˙j+ ∂V ∂qi = τi. (4.27)
Ekvation (4.27) kan skrivas om på matrisform
D(q) ¨q + C(q, ˙q) ¨q + g(q) = τ , (4.28) där gi(q) = ∂V ∂qi , (4.29)
4.2 Rörelseekvationer 23 och matriselementen i C fås från cij= n X k=1 cijk(q) ˙qk. (4.30)
Termerna cijk kallas Christoffelsymboler och definieras som
cijk= ∂dij ∂qk −1 2 ∂djk ∂qi . (4.31) (Norrlöf, 1999; Skoglar, 2003)
Yttre moment
I gimbalsystemet kommer det yttre momentet τ från motorerna och friktionen. En enkel modell av den viskösa friktionen ges av Fvq och för den statiska friktionen˙
av Fssgn( ˙q). Då får vi att
τ = τa− Fvq − F˙ ssgn( ˙q), (4.32)
där τa är momentet från motorn. (Skoglar, 2003)
Sammanfattning
Med följande steg går det att räkna ut rörelseekvationerna för en kinematisk kedja. 1. Räkna ut D(q)
(a) Lös positionskinematiken och räkna ut rotationsmatrisen R0
i för varje
länk.
(b) Lös hastighetskinematiken och räkna ut Jv och Jω.
(c) Räkna ut summan i K = 1 2q˙ T n X i=1 [miJvi(q) TJ vi(q) + Jωi(q) T(R0 i)Ii(R0i) TJ ωi(q)] ˙q. (4.33)
Detta kräver vetskap om massan mi och tröghetsmatrisen Ii för varje
länk. Då kan D(q) fås fram av K = 1
2q˙
TD(q) ˙q. (4.34)
2. Räkna ut C(q, ˙q)
(a) Räkna ut Christoffelsymbolerna cijk= ∂dij ∂qk −1 2 ∂djk ∂qi . (4.35)
24 Dynamik (b) Räkna ut elementen i C cij = n X k=1 cijk(q) ˙qk. (4.36) 3. Räkna ut g(q) (a) Räkna ut V = −(g0)T n X i=1 r0cimi. (4.37)
Detta kräver vetskap om avståndet till masscentrum r0
cioch massan mi för varje länk. (b) Räkna ut elementen i g(q) gi(q) = ∂V ∂qi . (4.38)
4. Skriv ner rörelseekvationen
D(q) ¨q + C(q, ˙q) ¨q + g(q) = τ . (4.39) (Norrlöf, 1999; Skoglar, 2003)
4.3
Rörelseekvationer för skannern
Genom att använda stegen i avsnitt 4.2 kan vi nu räkna ut en fysikalisk modell för skannern. För att kunna räkna ut rörelseekvationerna behövs tyngdaccelerationen. Vid mätningar strävar man efter att flyga i konstant hastighet och parallellt med marken. Testerna i den här rapporten är enbart gjorda på en stillastående skanner. Tyngdaccelerationen uttryckt i koordinatsystem 0 ges då av
g0= g 1/2 1/2 1/√2 , (4.40)
om skannern har en vinkel på 45 grader i förhållande till horistontalplanet. I ekvationerna nedan används flera olika index och beteckningar. Allmänt kan sägas att x, y, s och c står för x-axel, y-axel, spegel respektive masscentrum. Avståndet till masscentrum för länk i uttryckt i koordinatsystem i skrivs som
rici= rcix rciy rciz . (4.41)
Masströghetsmatrisern för länk i uttryckt i koordinatsystem i skrivs som
Ii= Iixx Iixy Iixz Iiyx Iiyy Iiyz Iizx Iizy Iizz . (4.42)
4.3 Rörelseekvationer för skannern 25
x-axeln
Ekvation (3.32) ger att
qx= α. (4.43) 1. Räkna ut D(q) Dx(α) = mxJvx(α) TJ vx(α) + Jωx(α) TR0 xIx(R0x) TJ ωx(α) = Ixxx. (4.44) Beräknade värden för mx och Ix finns i bilaga C.
2. Räkna ut C(q, ˙q)
Eftersom Dx(α) är konstant fås att
Cx(α, ˙α) = 0. (4.45)
3. Räkna ut g(q)
Från bilaga C fås att rcxy ≈ 0. Antag därför att
rxcx= rcxx 0 rcxz . (4.46) Då blir r0cx= R0xrxcx= 1 0 0 0 cos(α) − sin(α) 0 sin(α) cos(α) rcxx 0 rcxz = rcxx −rcxzsin(α) rcxzcos(α) . (4.47)
Den potentiella energin blir alltså
Vx= −(g0)Tr0cxmx= −mxg h1 2 1 2 1 √ 2 i rcxx −rcxzsin(α) rcxzcos(α) . (4.48)
Vi deriverar ekvation (4.48) och får att ∂Vx ∂α = −mxgrcxz(− 1 2cos(α) − 1 √ 2sin(α)), (4.49) vilket ger att
gx(α) =
∂Vx
∂qx1 = ∂Vx
∂α . (4.50)
4. Skriv ner rörelseekvationen
26 Dynamik
y-axeln
Ekvation (3.36) ger att
qy = β. (4.52) 1. Räkna ut D(q) Dy(β) = myJvy(β) TJ vy(β) + Jωy(β) TR0 yIy(R0y) TJ ωy(β) = Iyyy. (4.53) Beräknade värden för my och Iy kan hittas i bilaga D.
2. Räkna ut C(q, ˙q)
Eftersom Dy(β) är konstant fås att
Cy(β, ˙β) = 0. (4.54) 3. Räkna ut g(q) Antag att rycy= 0 rcyy rcyz , (4.55)
ty rcyx≈ 0 (se bilaga D). Då blir
r0cy = R0yrycy= cos(β) 0 sin(β) 0 1 0 − sin(β) 0 cos(β) 0 rcyy rcyz = rcyzsin(β) rcyy rcyzcos(β) . (4.56)
Den potentiella energin blir alltså
Vy= −(g0)Tr0cymy = −myg h1 2 1 2 1 √ 2 i rcyzsin(β) rcyy rcyzcos(β) . (4.57)
Derivering av ekvation (4.57) ger gy(β) = ∂Vy ∂β = −mygrcyz( 1 2cos(β) − 1 √ 2sin(β)). (4.58) 4. Skriv ner rörelseekvationen
4.3 Rörelseekvationer för skannern 27
Spegeln
Ekvation (3.39) ger att
qs=β α
. (4.60)
1. Räkna ut D(q)
Länk 1 är masslös och Jv2 = 0 vilket ger oss Ds(α, β) = Jω2(α, β) T R02I2(R02) T Jω2(α, β). (4.61) Vi behöver JwT2R20= 0 1 0 cβ 0 −sβ cβ sβsα sβcα 0 cα −sα −sβ cβsα cβcα =0 cα −sα 1 0 0 , (4.62) och (R02) T Jw2= (J T w2R 0 2) T = 0 1 cα 0 −sα 0 , (4.63)
där cθ och sθ står för cos(θ) respektive sin(θ). Masströghetsmatrisen I är
alltid symmetrisk därför är Iij = Iji. Länk 2 motsvarar spegeln. Då får vi
att Ds(α, β) = 0 cα −sα 1 0 0 Isxx Isxy Isxz Isyx Isyy Isyz Iszx Iszy Iszz 0 1 cα 0 −sα 0 (4.64) =Isyyc 2 α− 2Isyzsαcα+ Iszzs 2 α Isxycα− Isxzsα Isxycα− Isxzsα Isxx (4.65) =Ds11 Ds12 Ds21 Ds22 . (4.66)
Beräknade värden för msoch Is finns i bilaga E
2. Räkna ut C(q, ˙q)
Den resulterade matrisen blir
Cs(α, β, ˙α, ˙β) = Cs11 Cs12 Cs21 Cs22 , (4.67) där Cs11 = (−2Isyycαsα− 2Isyz(c 2 α− s 2 α) + 2Iszzsαcα) ˙α, (4.68) Cs12 = (−Isxysα− Isxzcα) ˙α, (4.69) Cs21 = − 1 2(−2Isyycαsα− 2Isyz(c 2 α− s 2 α)+ (4.70) + 2Iszzsαcα) ˙β + 1 2(−Isxysα− Isxzcα) ˙α, (4.71)
28 Dynamik och Cs22 = − 1 2(−Isxysα− Isxzcα) ˙β. (4.72) 3. Räkna ut g(q)
Länk 1 är masslös och ger där för ingen potentiell energi. Från bilaga E fås att rcsx≈ 0 och rcsy ≈ 0. Antag därför att
rscs= 0 0 rcsz . (4.73) Då blir r0cs= R0srscs=
cos(β) sin(β) sin(α) sin(β) cos(α)
0 cos(α) − sin(α)
− sin(β) cos(β) sin(α) cos(β) cos(α) 0 0 rcsz = rcszsin(β) cos(α) −rcszsin(α) rcszcos(β) cos(α) . (4.74)
Den potentiella energin blir alltså
Vs= −(g0)Tr0csms= −msg h1 2 1 2 1 √ 2 i rcszsin(β) cos(α) −rcszsin(α) rcszcos(β) cos(α) . (4.75)
Vi deriverar ekvation (4.75) och får att ∂Vs ∂β = −msgrcsz( 1 2cos(β) cos(α) − 1 √ 2sin(β) cos(α)) (4.76) ∂Vs ∂α = −msgrcsz(− 1 2sin(β) sin(α) − 1 2cos(α) − 1 √ 2cos(β) sin(α)). (4.77) Vilket ger att
gs(q) = "∂Vs ∂qs1 ∂Vs ∂qs2 # = ∂Vs ∂β ∂Vs ∂α . (4.78)
4. Skriv ner rörelseekvationen Ds(α, β) ¨ β ¨ α + Cs(α, β, ˙α, ˙β) ˙ β ˙ α + gs(α, β) = τs. (4.79)
Motorerna
Motorerna är strömstyrda vilket innebär att momentet ut från motorerna beror på styrströmmen i. Förhållandet mellan strömmen och momentet kallas för mo-mentkonstant Kτ. Motorernas momentkonstant är inte helt linjär (se figur 4.1).
4.3 Rörelseekvationer för skannern 29
Vi antar att de båda motorerna på varje axel är identiska och har samma mo-mentkonstant. Även om motorerna skulle skilja sig åt gör det ingenting. Det är nämligen det totala momentet ut från varje par av motorer som är det viktiga. En rimlig approximation av momentkonstanten ges då av
Kτα = kταcos(3α), (4.80)
för motorerna på x-axeln och
Kτβ = kτβcos(3β), (4.81)
för motorerna på y-axeln. kτα och kτβ antas vara konstanta.
Figur 4.1. Momentet T ut från motorn vid olika vinklar. Observera att bara
vinkelom-rådet ±15 grader används.
Varje motor har ett tröghetsmoment som antas vara Jm. Till varje axel sitter
en enkoder som har ett tröghetsmoment på Je. Vi antar att friktionen enbart
beror på ˙α för motorerna på x-axeln och ˙β för motorerna på y-axeln. En enkel modell för ταoch τβ ges av
τα= 2Kταiα− (2Jm+ Je) ¨α − Fvαα − F˙ sαsgn( ˙α), (4.82) och
τβ= 2Kτβiβ− (2Jm+ Je) ¨β − Fvββ − F˙ sβsgn( ˙β). (4.83) För att styra hur stor ström som skickas in till motorerna används två stycken förstärkare. En förstärkare är till för motorerna på x-axeln och den andra för
30 Dynamik
motorerna på y-axeln. Till förstärkaren skickas en spänning U och ut kommer en ström i som är proportionell mot spänningen. För den gäller att
i = 30
3.75U + a, (4.84)
där a är det eventuella offsetfelet och talet 3.7530 är förstärkningsfaktorn. De båda motorerna på varje axel är parallellkopplade med förstärkaren. Vi har tidigare antagit att motorerna på samma axel är identiska. Strömmen i kommer därför att fördelas lika mellan dem. Ekvation (4.82) kan då skrivas som
τα= 2kτα 1 2( 30 3.75Uα+ aα) cos(3α) − (2Jm+ Je) ¨α − Fvαα − F˙ sαsgn( ˙α), (4.85) och (4.83) blir då τβ= 2kτβ 1 2( 30 3.75Uβ+ aβ) cos(3β) − (2Jm+ Je) ¨β − Fvββ − F˙ sβsgn( ˙β). (4.86)
Resultat
Vi har modellerat gimbalsystemet som tre öppna kinematiska kedjor. På x-axeln sitter som tidigare nämnts två motorer som tillsammans ger ett moment τα. Det
pålagda momentet gör att x-axeln och spegeln roterar så att vinkeln α ändras, vilket ger att
τα= τx+ τs2= Dxα + g¨ x+ Ds21β + D¨ s22α + C¨ s21β + C˙ s22α + g˙ s2. (4.87) Om vi nu sätter in momentet från motorerna (4.85) i (4.87) får vi att
kτα( 30
3.75Uα+ aα) cos(3α) − (2Jm+ Je) ¨α − Fvαα − F˙ sαsgn( ˙α) =
= Dxα + g¨ x+ Ds21β + D¨ s22α + C¨ s21β + C˙ s22α + g˙ s2. (4.88) Vinkeln β ändras av motorerna på y-axeln. Då β ändras roterar y-axeln och spegeln. På samma sätt som för x-axeln fås för y-axeln att
τβ = τy+ τs1= Dyβ + g¨ y+ Ds11β + D¨ s12α + C¨ s11β + C˙ s12α + g˙ s1, (4.89) och kτβ( 30 3.75Uβ+ aβ) cos(3β) − (2Jm+ Je) ¨β − Fvββ − F˙ sβsgn( ˙β) = = Dyβ + g¨ y+ Ds11β + D¨ s12α + C¨ s11β + C˙ s12α + g˙ s1. (4.90)
Kapitel 5
Övergång från
tidskontinuerlig till
tidsdiskret modell
Den modell som togs fram i kapitel 4 är tidskontinuerlig. Under mätinsamling och reglering kommer vi inte att få mätvärden kontinuerligt utan enbart vid vis-sa tidpunkter. Att ta stickprov på det här sättet kallas för att vis-sampla. För att kunna ha någon nytta av vår modell måste vi därför skriva om den på tidsdis-kret form. Det handlar i vårt fall om att kunna skatta vinkelhastigheterna och vinkelaccelerationen utifrån våra samplade mätvärden på systemet.
5.1
Sampling
Vi ska i fortsättningen anta att samplingen sker ekvidistant alltså med konstant tidsmellanrum T . Det innebär att samplingstidpunkterna kan uttryckas som
tk = kT, k = 1, 2, . . . . (5.1)
Antag också att insignalen u(t) till systemet är styckvis konstant. (Glad m.fl., 2003)
5.2
Eulers metod
För att vi ska kunna få en lite mer systematisk behandling av övergången från tidskontinuerlig till tidsdiskret modell inför vi operatorerna p och q. De definieras som derivationsoperatorn
pu(t) = ˙u(t), (5.2)
32 Övergång från tidskontinuerlig till tidsdiskret modell
och förskjutningsoperatorn
qu(t) = u(kT + T ). (5.3)
En enkel skattning av derivata är Eulers metod p = 1
T(1 − q
−1). (5.4)
(Glad m.fl., 2003)
5.3
Minsta kvadratmetoden
Ett annat sätt att skatta derivatan kan man beskriva så här: Antag att vi skattar utsignalen med en andragradskurva
y(τ ) ≈ x1τ2+ x2τ + x3. (5.5)
Om vi låter τ gå från −nT till nT där T är samplingsperioden kan alla mätvärden skrivas på formen (−nT )2 −nT 1 (−nT + T )2 −nT + T 1 .. . ... ... (nT )2 nT 1 x1 x2 x3 = ˜ y(−nT ) ˜ y(−nT + T ) .. . ˜ y(nT ) . (5.6) Vi skriver (5.6) som Ax = ˜Y . (5.7)
En minsta kvadratlösning av (5.7) ges då av
x = (ATA)−1ATY .˜ (5.8)
Det här kan användas för att skatta första- och andraderivatan. Derivatan av (5.5) kan skrivas som
˙
y(τ ) ≈ 2x1τ + x2. (5.9)
Vilket innebär att då τ = 0 fås att ˙ y(0) ≈ x2, (5.10) och ¨ y(0) ≈ 2x1. (5.11) (de Boor, 1978)
5.4 Skattning av vinkelhastighet och vinkelacceleration 33
5.4
Skattning av vinkelhastighet och
vinkelacce-leration
I den dynamiska modellen som vi tog fram i kapitel 4 finns variablerna ˙α, ¨α, ˙β och ¨β. För att utvärdera vilken metod som är bäst så testar vi att skatta β:s vinkelhastighet och vinkelacceleration.
Eulersmetod fungerar bra för att skatta vinkelhastigheten ˙β (se figur 5.1). Skattningen av vinkelaccelerationen ¨β blir dock väldigt brusig (se figur 5.2).
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −6 −4 −2 0 2 4 6 Tid [s] Vinkelhastighet [rad/s]
34 Övergång från tidskontinuerlig till tidsdiskret modell 1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −300 −200 −100 0 100 200 300 400 Tid [s] Vinkelacceleration [rad/s 2]
Figur 5.2. ¨β skattat med eulersmetod.
Vi testar att anpassa β till en andragradskurva och sedan räkna ut vinkelhas-tigheten och vinkelaccelerationen från den. Brusigheten hos derivataskattningarna beror på hur parametern n väljs. Ett lågt värde på n ger mer brus än ett högt vär-de på n. Det här syns tydligt om vi tittar på skattningen av vinkelaccelerationen då n = 3, n = 4 och n = 5 i figur 5.6, 5.7 och 5.8. I figur 5.3, 5.4 och 5.5 syns att skattningen av vinkelhastigheten inte påverkas lika mycket av olika värden på n som skattningen av vinkelaccelerationen. Nackdelen med att välja ett alltför stort värde på n är att vi kan missa information om hur derivatan varierar och att den blir tung att räkna ut. Ett lämpligt val av n är 4.
Att använda en andragradskurva för att skatta vinkelhastigheten och vinkelac-celeration ger ett mindre brusigt resultat än att använda Eulers metod. I fortsätt-ningen använder vi därför den metoden.
5.4 Skattning av vinkelhastighet och vinkelacceleration 35 1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −6 −4 −2 0 2 4 6 Tid [s] Vinkelhastighet [rad/s]
Figur 5.3. ˙β skattat med minsta kvadratmetoden då n = 3.
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −6 −4 −2 0 2 4 6 Tid [s] Vinkelhastighet [rad/s]
36 Övergång från tidskontinuerlig till tidsdiskret modell 1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −6 −4 −2 0 2 4 6 Tid [s] Vinkelhastighet [rad/s]
Figur 5.5. ˙β skattat med minsta kvadratmetoden då n = 5.
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −250 −200 −150 −100 −50 0 50 100 150 200 250 Tid [s] Vinkelacceleration [rad/s 2]
5.4 Skattning av vinkelhastighet och vinkelacceleration 37 1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −250 −200 −150 −100 −50 0 50 100 150 200 250 Tid [s] Vinkelacceleration [rad/s 2]
Figur 5.7. ¨β skattat med minsta kvadratmetoden då n = 4.
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.4 1.45 1.5 −250 −200 −150 −100 −50 0 50 100 150 200 250 Tid [s] Vinkelacceleration [rad/s 2]
Kapitel 6
Systemidentifiering
I kapitel 4 togs en fysikalisk modell för skannern fram. Det finns dock vissa parametrar i modellen som har okänt värde, bl. a. motorernas momentkonstant och friktionen i systemet.
Först går vi igenom hur parametrarna kan skattas för en fysikalisk modell. En annan modelltyp som inte bygger på fysikaliska principer förklaras också. Därefter beskrivs hur ett experiment ska se ut, för att få fram så bra värden som möjligt på parametrarna. Till sist identifierar vi de okända parametrarna i den fysikaliska modellen för skannern och en svartlådemodell tas fram.
6.1
Parameterskattning i linjära dynamiska
mo-deller
Ofta när vi sätter upp en modell för ett system så saknas kunskap om vissa de-taljer för systemet. Då får vi försöka att kompensera detta genom att skatta parametrarna med statistiska metoder. Det finns anledning att skilja på två typer av modeller.
• Skräddarsydda modeller där parametrarna bygger på fysikaliska grund-principer. Parametrarna representerar okända värden som i princip har fy-sikalisk tolkning.
• Konfektionsmodeller som utgör familjer av generella och användbara modeller. Parametrarna har ingen fysikalisk tolkning. Sådana modeller kal-las också svartlådemodeller.
(Ljung och Glad, 2004)
Skräddarsydda linjära modeller
För en fysikalisk modell saknas ofta kunskap om vissa parametrar. Det kan vara t. ex. massan och tröghetsmomentet för en länk. Antag att vi har d stycken sådana
40 Systemidentifiering
okända systemparametrar. Då får vi parametervektorn
θ = θ1 .. . θd . (6.1)
Antag att modellen som beskriver systemet är linjär. Den kan då skrivas på formen d
dtx(t) = A(θ)x(t) + B(θ)u(t) + N v1(t) (6.2) y(t) = C(θ)x(t) + D(θ)u(t) + v2(t), (6.3)
där v1(t) är processbrus och v2(t) är mätbrus som påverkar utsignalen. Om
stör-ningarna v1(t) och v2(t) beskrivs av vitt brus så kan vi inte prediktera deras värde.
Då skriver vi
d
dtx(t) = A(θ)x(t) + B(θ)u(t) (6.4)
ˆ
y(t|θ) = C(θ)x(t) + D(θ)u(t), (6.5)
där ˆy(t|θ) är modellens predikterade värde eller gissning av utsignalen vid tiden t. Vi vill nu bestämma parametrarna i θ med hjälp av mätningar. (Ljung och Glad, 2004)
Linjära konfektionsmodeller
Ibland kan det vara svårt att göra en fysikalisk modell eftersom det kan vara svårt att reda ut sambanden. Då finns standardmodeller som beskriver sambandet mellan in- och utsignal utan att bry sig om underliggande fysikaliska samband. De modellerna kallas för svart låda eller konfektionsmodeller. (Ljung och Glad, 2004)
Eftersom data oftast samlas in i diskret tid är det lättast att arbeta i diskret tid. Konfektionsmodellerna består av familjer av överföringsfunktioner. En generell struktur för de linjära konfektionsmodellerna ges av (Ljung, 1999)
A(q)y(t) = B(q) F (q)u(t) + C(q) D(q)e(t), (6.6) där A(q) = a1q−1+ a2q−2+ . . . + anaq −na, (6.7) B(q) = bnkq −nk+ b nk+1q −nk−1+ . . . + b nk+nb−1q −nk−nb+1, (6.8) C(q) = c1q−1+ c2q−2+ . . . + cncq −nc, (6.9) D(q) = d1q−1+ d2q−2+ . . . + dndq −nd, (6.10) och F (q) = f1q−1+ f2q−2+ . . . + fnfq −nf. (6.11)
6.1 Parameterskattning i linjära dynamiska modeller 41
Först väljs strukturparametrarna na, nb, nc, ndoch nf. Om dynamiken innehåller
en tidsförskjutning går den att modellera genom att ändra nk. När
strukturpara-metrarna är valda återstår att anpassa parastrukturpara-metrarna ai, bi, ci, di och fi till data.
Det är de variablerna som kommer att ingå i vektorn θ. (Ljung och Glad, 2004) Det finns några vanliga specialfall av (6.6) (Ljung och Glad, 2004)
• ARX-modellen A(q)y(t) = B(y)u(t) + e(t) är enklast att skatta eftersom den är av linjär regressionstyp. Största nackdelen är att störningsmodellen följer med systemets poler (se figur 6.1). Det fungerar bra för störningarna som kommer in tidigt i processen.
Figur 6.1. ARX
• ARMAX-modellen A(q)y(t) = B(q)u(t) + C(q)e(t) har bättre störnings-modell än ARX-störnings-modellen (se figur 6.2). Det är samma poler för bruset e(t) och insignalen u(t). Modellen är bra om störningarna kommer in tidigt i processen.
42 Systemidentifiering
• OE-modellen y(t) = B(q)F (q)u(t) + e(t) har fördelen att den beskriver system-dynamiken separat utan att modellera störsignalen (se figur 6.3). Om sy-stemet arbetar utan återkoppling under datainsamlingen går det att få en korrekt beskrivning av B(q)F (q) oavsett störningen.
Figur 6.3. OE
• BJ-modellen y(t) = B(q)F (q)u(t) +D(q)C(q)e(t) beskriver störningen separat från systemdynamiken (se figur 6.4). Modellen är därför bra om störningarna kommer in sent i processen. Den kan dock kräva lite mer arbete vid skatt-ningen.
Figur 6.4. BJ
Minimering av prediktionsfelet
Med så väl de skräddarsydda modellerna och konfektionsmodellerna går det att få fram en gissning vid tiden t − 1 av hur y kommer att se ut vid tiden t. Låt
ˆ
y(t|θ) (6.12)
vara prediktionen av y(t). Vid tiden t kan vi räkna ut hur fel gissningen var genom att beräkna prediktionsfelet
(t, θ) = y(t) − ˆy(t|θ). (6.13)
Om vi gör en mätning över perioden t = 1, . . . , N på insignalen och utsignalen, kan vi utvärdera hur bra modellen med parametervärderna θ är på att prediktera.
6.1 Parameterskattning i linjära dynamiska modeller 43 Talet VN(θ) = 1 N n X t=1 2(t, θ) (6.14)
kan ses som ett mått på hur bra parametern θ är. Det naturliga valet är att välja det värde på θ som minimerar (6.14)
ˆ
θn= arg min
θ VN(θ). (6.15)
(Ljung och Glad, 2004)
Specialfallet då den skattade modellen är en linjär regression
I många fall är ˆy(t|θ) en ganska komplicerad funktion av θ. Antag dock att vi har specialfallet då den går att skriva som en linjär funktion av θ (Ljung och Glad, 2004) ˆ y(t|θ) = θTφ(t), (6.16) där θ = θ1 .. . θd , (6.17)och φ(t) är en kolonnvektor formad av gamla insignaler och utsignaler. Då kan (6.14) skrivas som VN(θ) = 1 N N X t=1 (y(t) − θTφ(t))2= 1 N N X t=1 y2(t) − 2θTfN + θTRNθ, (6.18) där fN = 1 N N X t=1 φ(t)y(t), (6.19) och RN = 1 N N X t=1 φ(t)φT(t). (6.20)
Om matrisen RN är inverterbar kan (6.18) skrivas som
VN(θ) = 1 N N X t=1 y2(t) − fNTR−1N fN + (θ − R−1N fN)TRN(θ − R−1N fN). (6.21)
De två sista termerna är oberoende av θ. Den sista termen är alltid positiv eftersom RN är positiv definit. Vnminimeras då den sista termen är noll, d. v. s. då (Ljung
och Glad, 2004)
44 Systemidentifiering
För räkningarnas skull är det lättare om vi skriver på matrisform istället (Skog-lar, 2003). Definierar matriserna
Yn= y(1) .. . y(N ) , (6.23) och φn= φT(1) .. . φT(N ) . (6.24)
Då kan (6.14) skrivas som VN = 1 N(YN − φNθ) T(Y N − φNθ). (6.25) Minimum (6.22) ges då av ˆ θN = (φTNφN)−1φTNYN. (6.26)
6.2
Experimentdesign
Vid identifiering av parametrarna i en modell behövs experiment. Det är önskvärt att experimenten utförs på ett sådant sätt att de i största möjliga mån liknar det som gäller då modellen ska användas.
Val av insignal
Det är viktigt att insignalen exciterar systemet och tvingar det att visa sina egen-skaper. För ett olinjärt system är det nödvändigt med fler än två insignalsnivåer. Insignalen bör variera över ett intervall som svarar mot önskad arbetspunkt. Fre-kvensinnehållet bör väljas så att den får väsentlig energi i frekvensbandet som är viktigt för systemet. (Ljung och Glad, 2004)
Val av samplingstakt
Valet av samplingstakt är kopplat till systemets tidskonstanter. En alltför snabb sampling kan ge numeriska problem och lågt informationsvärde hos nya datapunk-ter. En samplingstakt som är mycket långsammare än systemdynamiken gör det svårt att skatta parametrarna som beskriver dynamiken. (Ljung, 1999; Ljung och Glad, 2004)
Tumregeln är att välja en samplingstakt som är ca 10 gånger systemets band-bredd. Detta motsvarar i tidsplanet att lägga ungefär 4-8 samplingspunkter på flanken av den intressanta delen av systemets stegsvar. Vid experiment är det bättre att sampla för fort än för långsamt. Det går alltid att sampla om insamlad data efteråt så att de får en lägre samplingstakt. (Ljung och Glad, 2004)
6.2 Experimentdesign 45
Experiment under återkoppling
I vissa fall kan det vara nödvändigt att utföra experiment under återkoppling (se figur 6.5). En alltför enkel regulator kan göra ett experiment ointressant även om referenssignalen är intressant. (Ljung, 1999)
Systemidentifiering för system under återkoppling kan ske på flera sätt. De tre huvudgrupperna är direkt, indirekt och förenad insignal-utsignal. Här presenteras enbart den direkta metoden. För en genomgång av de andra metoderna rekom-menderas Ljung (1999).
Figur 6.5. Exempel på ett slutet system.
Direkt
Antag att r är okänd och använd inte r även om den är känd. Vi ser u som insignal och y som utsignal.
• Fungerar oavsett hur komplex regulator är och kräver ingen vetskap om återkopplingen.
• Ingen speciell algoritm eller programvara krävs.
• Optimal noggrannhet fås om modellstrukturen innehåller det sanna syste-met.
• Instabila system kan hanteras utan problem. Så länge som det återkopplade systemet är stabilt och prediktorn är stabil.
Enda nackdelen är att metoden kräver bra brusmodeller. En bra modellstruktur är ARX-,ARMAX- eller BJ-typ (eller en skräddarsydd modell). Den direkta me-toden ska ses som ett förstahandsval. (Ljung, 1999)
46 Systemidentifiering
6.3
Modellvalidering
F it definieras somF it = 100(1 −kˆy − yk
ky − ¯yk), (6.27)
och används för att utvärdera hur stor skillnaden är mellan modellens utsignal ˆy och det uppmätta värdet på utsignalen y. ¯y är medelvärdet av y. Fit är procent-talet av utsignalsvariationen som förklaras av modellen. En modell som har en fit på 0% ger samma resultat som att sätta modellens utsignal till medelvärdet av den uppmätta signalen. (Skoglar, 2003)
6.4
System Identification Toolbox
System Identification Toolbox (SITB) är ett programpaket som används tillsam-mans med Matlab. Det innehåller ett grafiskt gränssnitt som gör det enkelt att ta fram och jämföra modeller. SITB kan användas för att skatta parametrarna i lin-jära konfektionsmodeller eller för godtyckliga skräddarsydda linlin-jära tillståndsmo-deller. (Ljung och Glad, 2004)
6.5
Systemidentifiering för skannern
Nu ska vi ta fram en fysikalisk modell och en svartlådemodell för skannern. Först beskrivs den experimentuppställning som har använts. Efter det skattas model-lerna.
Experimentdesign
Det finns önskemål om att skannern ska köras med en samplingstakt på 1 kHz. För att se om det verkar rimligt skickar vi in ett steg på −0.03 V till 0.03 V till y-axelns förstärkare och tittar på vinkelhastigheten (se figur 6.6). Vi ska ha åtminstone 4-8 sampelpunkter på den intressanta delen av systemets stegsvar. I figur 6.6 kan man se att vinkelhastigheten har nått 63% av sin stationäranivå efter 0.04 sekunder, det är ungefär då stegsvarets lutning börjar att avta. Enligt tumregeln är då en lämplig samplingstakt 100 Hz till 200 Hz. En samplingstakt på 1 kHz är något för snabb vilket kan ge numeriska problem. Vi väljer ändå att använda en så hög samplingstakt eftersom det finns önskemål om det.
Det finns en utsignalsbegränsning i systemet. Två stopp på varje axel hindrar spegeln att röra på sig mer än ca ±15 grader. Om spegeln under experimentet rör sig så mycket att den slår i stoppen är det svårt att skatta parametrarna i modellen. Skannern kan dessutom gå sönder. För att komma bort från problemet behövs en regulator. Genom att välja lämpliga referensvärden r kan spegeln då hindras från att slå i stoppen. Som regulator används en PD där K = 4 och Td = 0.001. Det är smidigast att använda den direkta metoden vid identifiering
av parametrarna i modellen. Vi kommer att se styrspänningar som insignal och vinklarna som utsignal till systemet.
6.5 Systemidentifiering för skannern 47 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Tid [s] Vinkelhastighet [rad/s]
Figur 6.6. Skannerns vinkelhastighet för y-axeln då ett steg från -0.03 till 0.03 volt
skickas in. Markeringen visar när vinkelhastigheten har nått 63% av sin stationära nivån.
Systemet är olinjärt och ska därför ha en insignal som har fler än två nivåer. Insignalens frekvens bör dessutom variera. En signal som uppfyller dessa krav är chirpsignalen. Den ser ut som en sinussignal men frekvensen förändras konstant över ett speciellt frekvensband ω1≤ ω ≤ ω2under tiden 0 ≤ t ≤ M . Chirpsignalen
kan skrivas som (Ljung, 1999)
r(t) = A cos(ω1t +
(ω2− ω1)t2
2M ). (6.28)
Under riktiga mätningar ska skannern köras i 10 Hz till 12 Hz. Vinklarna kommer att variera mellan ungefär −8 till 8 grader. Under experimenten låter vi ω1 =
10π rad/s och ω2= 50π rad/s, vilket motsvarar ett frekvensintervall på 5 Hz till
25 Hz. För att inte få en alltför stor datamängd att hantera körs experimentet i 20 s. Ett lämpligt värde på M är då 20 s. För att inte slå i skannerns stopp låter vi referenssignalens amplitud vara A = 5π/180 rad (se figur 6.7). För att få olika dataserier för estimering och validering av modellerna så körs skannern i två omgångar.
48 Systemidentifiering 0 0.5 1 1.5 2 2.5 3 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02 0.04 0.06 0.08 0.1 Tid [s] Vinkel [rad] Figur 6.7. Referenssignalen
Identifiering av parametrarna i den fysikaliska modellen
Den fysikaliska modellen har inte linjär dynamik med den är linjär i de okända systemparametrarna. Det går att se om vi skriver om (4.88) och (4.90). Det ger att (Dx+ Ds22+ 2Jm+ Je) ¨α + Ds21β + C¨ s22α + C˙ s21β + g˙ x+ gs2 = =kτα kταaα Fvα Fsα 30 3.75Uαcos(3α) cos(3α) − ˙α −sgn( ˙α) , (6.29) och att Ds12α + (D¨ y+ Ds11+ 2Jm+ Je) ¨β + Cs12α + C˙ s11β + g˙ y+ gs1= =kτβ kτβaβ Fvβ Fsβ 30 3.75Uβcos(3β) cos(3β) − ˙β −sgn( ˙β) , (6.30)6.5 Systemidentifiering för skannern 49
där kτα, kταaα, Fvα, Fsα, kτβ, kτβaβ, Fvβ och Fsβ är de okända systemparamet-rarna. Vi kan sätt sätta in värde från experiment på α, β, ˙α, ˙β, ¨α och ¨β vid olika styrsignaler Uα och Uβ i ekvation (6.29) och (6.30). Då har vi ekvationerna
skrivna på linjär regressionsform (6.16) och kan därför använda ekvation (6.26) för att skatta de okända systemparametrarna. Då får vi att
kτα kταaα Fvα Fsα = 0.4047 −0.1075 0.0691 −0.0461 , (6.31) och att kτβ kτβaβ Fvβ Fsβ = 0.3865 0.0593 0.0667 −0.0471 . (6.32)
Det kan var någon störning eller något modellfel som gör att den statiska friktio-nen Fs blir negativ. Eftersom det är fysikaliskt omöjligt med en negativ statisk
friktion sätter vi Fstill noll i ekvation (6.29) och (6.30) och identifierar de övriga
parametrarna en gång till. Då får vi att kτα kταaα Fvα = 0.4045 −0.1076 0.0619 , (6.33) och att kτβ kτβaβ Fvβ = 0.3863 0.0592 0.0593 . (6.34)
I figur 6.8 och 6.9 syns att bidraget från C är väldigt litet jämfört med bidraget från masströgheterna D och J . En rimlig approximation är därför att sätta alla element i C till noll. Då C = 0 och Fs= 0 blir de övriga parametrarna
kτα kταaα Fvα = 0.4040 −0.1078 0.0618 , (6.35) och kτβ kτβaβ Fvβ = 0.3873 0.0601 0.0595 . (6.36)
Lägg märke till att systemparametrarna i ekvation (6.33) och (6.34) och de i ekvation (6.35) och (6.36) är ungefär lika.
50 Systemidentifiering 1 1.1 1.2 1.3 1.4 1.5 1.6 1.7 1.8 1.9 2 −5 −4 −3 −2 −1 0 1 2 3 4x 10 −3 Tid [s] Moment [Nm]
Figur 6.8. Den heldragna linjen är Cs22α + C˙ s21β och den prickade är C˙ s12α + C˙ s11β.˙
1 1.1 1.2 1.3 1.4 1.5 1.6 1.7 1.8 1.9 2 −1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 Tid [s] Moment [Nm]
Figur 6.9. Den heldragna linjen är (Dx+ Ds22+ 2Jm+ Je) ¨α + Ds21β och den prickade¨
