Regeling van de kantelt afel met behulp van een vision systeem. Carl Vissers WFW rapportnummer 90.049
Hoogleraar : : Coach Faculteit : Vakgroep : : Sectie
Prof. Dr. Ir. J.J. Kok
Ir. J.P.A. Banens W WFW Regeltechniek
Technische Universiteit Eindhoven Oktober 1990
Inhoud
...
111
Samenvatting
1
Inleiding
2
Modellering 2.1 Een één-dimensionaal model
2.2
Parameters : de kogelmassa
1
3
Volledige t oest andst erugkoppelling 3.1 Inleiding . . . . . . . . . . . . . . . . . . 3.2 De optimale regelwet . . . . . . . . . . .
4
Wet Kalmanfilter 4.1 Inleiding . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Aanpassing aan het Kalmanfilter . . . . . . . . . . . . . . . . . . . . . 4.3 Meetruis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Systeemruis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
Simulat ieresultat en optimale-regelaar 5.1 Inleiding . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Vergelijking van het gewone Kalmanfilter met het aangepaste filter 5.3 Simulatieresultaten . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Invloed van parameterfouten . . . . . . . . . . . . . . . . . . . . . . . 5.5 Schatting van h f k . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
7
D e optimale PID-regelaar 6.1 Inleiding . . . . . . . . . . . . . . . 6.2 Simulatieresultaten . . . . . . . . .
4 4 4
................ ................
9
..
................... ...................
Baansturing 7.1 Inleiding . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Computed Torque . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.3 Tegenkoppelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4 De totale regeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.5 7.5 7.7
7.8 8
2 2 3
....................... ........................
Vergelijking van het gewone Kalmanfilter met het aangepaste filter . . Simulatieresultater: . . . . . . . . . . . . . . . . . . . . . . . . . . . . Invloed van parameterfouten . . . . . . . . . . . . . . . . . . . . . . . Schatting van A& met behulp van een PID-regelaar . . . . . . . . . .
Conclusies & aanbevelingen
9 9 10 12 12 15
15 15 18
18 18 19 19 20 20 21 22 24
i
Appendices
A
Het 2-dimensionale model
A.l
B
Het 1-dimensionale model
B.l
C
Implementatie van het optimaal geregeide systeem
C.I.
I
Referenties
..
11
Samenvatting Dit verslag beschrijft de regeling van de kanteltafel. Aan dit systeem worden metingen verricht van de positie van de kogel en de hoekverdraaiingen van het tafelblad, ten bate van de regeling van de positie of de trajectorie van de kogel. De positiemeting geschiedt met een vision systeem. Dit optisthe meetsysieem beeft een ÏïieetfïequeïItie vzn ongeveer 10 Hz. en een meetvertraging van ongeveer 0.1 sec.. Voor de regeling van de positie van de kogel is gekozen voor de optimale regelwet. De toestandsschatting wordt gedaan met een Kalmanfilter, dat is aangepast voor de meetvertraging van het vision systeem. Wanneer de massa van de kogel op voorhand niet exact bekend is, maken we hiervoor een globale schatting, bijvoorbeeld door een eenvoudige weging. Afhankelijk van de fout in de schatting blijft er een statische afwijking voor de positie van de kogel over. De massa is echter eenvoudig en toch vrij nauwkeurig t e schatten uit de motorkoppels en de positie van de kogel. De statische afwijking is ook weggeregeld door een integrerende actie op te nemen in de regelwet. Ook nu kan weer een schatting van de kogelmassa gemaakt worden door gebruik t e niaken van de geleverde motorkoppels en de positie van de kogel. Voor de trajectoriesturing is gekozen voor een voorwaartskoppelling in combinatie met de optimale regelwet. Ook nu is het noodzakelijk dat de massa van de kogel vrij nauwkeurig bekend is. Door de kogel vanuit een willekeurig startpunt naar een punt op de baan te sturen met behulp van de optimale PID-regelaar kunnen we een schatting maken van de massa van de kogel. Deze schatting is voldoende nauwkeurig om de kogel de gewenste baan t e laten beschrijven.
...
111
1
Inleiding
Op dit moment wordt gewerkt aan de bouw van een zgn. kanteltafel. Dit systeem bestaat uit een vlakke plaat die een hoekverdraaiing kan ondergaan om twee onderling loodrechte assen. Deze rotatieassen worden aangestuurd door twee servomotoren. Op de plaat ligt een ronde kogel. Onder invloed van de zwaartekracht en de hoekverdraaiing van de tafd zal de kogel gaan rollen. De positie van de kogel wordt gemeten met behulp van een vision systeem. Simpelweg gezegd bestaat dit systeem uit een videocamera die elke T sec. de positie van de kogel in een x- en een y-coördinaat uitdrukt en doorgeeft aan een P.C.. Verder worden ook de hoekverdraaiingen van de plaat gemeten en doorgegeven aan de P.C.. Met deze P.C. worden de servomotoren aangestuurd. In dit verslag wordt bekeken hoe de kogel van punt naar punt gestuurd kan worden en hoe we de kogel een gewenste baan (in ons geval een cirkelbaan) kunnen laten beschrijven. Problemen die zich hierbij voordoen zullen onder andere zijn : de lage meetfrequentie van het vision systeem (z10 Hz.) en de (sterk) niet-lineaire bewegingsvergelijkingen van de kanteltafel. Ook zal bij de simulaties rekening gehouden moeten worden met de fysische beperkingen van de tafel. De hoekverdraaiingen van het blad mogen niet groter worden dan 38". E r zal aandacht besteed moeten worden aan de hoekversnelling van de tafel. Deze mag niet te groot worden, zodat de kogel altijd contact houdt met het tafeloppervlak. De berekende motorkoppels moeten binnen de begrenzingen van de motoren vallen, zodat de koppels ook werkelijk geleverd kunnen worden. Verder zal ook de invloed worden bekeken van de massa van de kogel. Deze willen we als "onbekende" systeemparameter invoeren. Afhankelijk van wat we willen zullen we de massa moeten schatten of een regeling ontwerpen die het gewenste resultaat levert, ondanks een fout in de schatting van de massa.
1
2
Modellering
2.1
Een één-dimensionaal model
Als we de gelineariseerde bewegingsvergelijkingen’ (A.21) tot en met (A.24) van het tweedimensionale modei bekijken, zien we dat deze voiieciig ontkoppeld zijn. ‘We kunnen, zûndeï afbreuk te doen aan het inzicht in het 2d-systeem7 een meer overzichtelijk 1d-model als leidraad gebruiken. De tekenafspraken zijn hierbij als volgt :
Figuur 2.1 : motorkoppel (systeemingang) i k f k : massa van de kogel Ik : massatraagheidsmoment van de kogel R : straal van de kogel
K
Ig : massatraagheidsmoment van de goot x g a
: positie van de kogel
: versnelling van de zwaartekracht : hoekverdraaiing van de goot
Voor dit systeem luiden de niet-lineaire bewegingsvergelijkingen2 :
(2.1.1) (2.1.2) lZie appendix A voor de afleiding van de bewegingsvergelijkingen 2Zie appendix B voor de afleiding van de bewegingsvergelijkingen
2
De gelineariseerde bewegingsvergelijkingen luiden (Mk
:
+ -@)$ + M k g a = 0 Ik
I;& + j"fkgz = K
(2.1.3)
(2.1.4)
+ hfkx:.
Met 1; = Ig
Opm. : Alle simulaties in dit rapport zijn uitgevoerd met het tweedimensionale model. Het één-dimensionale model is alleen gebruikt als een soort verkorte schrijfwijze van het tweedimensionale systeem.
2.2
Parameters : de kogelmassa
We willen de massa van de kogel als onbekende systeemparameter invoeren. Immers door een andere kogel op de tafel te leggen is deze parameter eenvoudig te veranderen. De massa h f k is echter niet de enige systeemparameter die dan een andere waarde krijgt. De parameter I k is afhankelijk van d!fk :
(2.2.1) voor een massieve kogel. In het verdere verloop van dit verslag wordt aangenomen dat alleen met massieve kogels gewerkt wordt.
3
3 3.1
Volledige toestandsterugkoppelling Inleiding
Om de kogel van een willekeurig beginpunt naar elk gewenst eindpunt t e laten bewegen, kunnen we gebruik maken van een PD-regelaar. Veer het &n-dimemion_ale geval luidt de regelwet dan :
Hierin zijn Kpl en Kp2de proportionele acties op de positie z ( t ) van de kogel respectievelijk t ) de goot, terwijl I
3.2
D e optimale regelwet
Een manier waarmee we geschikte waarden voor de regelparameters Kpl, Kp2, Kdl en I
-
-
0
1
O 0 - 0
*;
o
o
o
1 o
o
(3.2.1)
of
Z=Ax+Bu
(3.2.2)
Nu kunnen we de optimale regelwet L gf[ Z I 12 Z3 Z4] bepalen, bijvoorbeeld met behulp van PC-Matlab. Het motorkoppel wordt bepaald volgens :
K ( t ) = -I&$) =
- 2,) - Z&)
-LX
- Z&)
- Z&)
( 3.2.3) (3.2-4)
&IT
met j i = [(z - z.) 2 Q = xT - xrT. De toestandsgrootheid x is hier vet gedrukt om onderscheid t e kunnen maken tussen de toestand x van het systeem en de positie 2 van de kogel. Het subscript ,is gebruikt om de gewenste toestand van het systeem aan t e duiden. 4
Hiermee zijn de eerder genoemde regelparameters Kpl, Kpz., Kdl en K d 2 vastgelegd. Door verandering van weegmatrices kan snel een andere regelsituatie verkregen worden. De regelwet (3.2.3)is nog niet compleet. Als de kogel het gewenste eindpunt x, # O heeft bereikt i s de ingang K = O. In dit geval is de goot niet in evenwicht vanwege de kracht die de kogel op de goot uitoefent. Om dit te compenseren schrijven we :
Men zou de term -Mhgx, voorwaartskoppelling kunnen noemen. Tevens kunnen we concluderen dat de massa van de kogel inderdaad een belangrijke parameter is. Immers : als niet nauwkeurig bekend is, zal de kogel nooit de gewenste positie kunnen bereiken, tenzij we een integrerende regelactie toevoegen. In hoofdstuk 6 komen we hierop terug.
5
4
Het Kalrnanfilter Inleiding
4.1
In het vorige hoofdstuk is gekozen voor volledige toestandsterugkoppelling. Dit houdt dan .. slechts de positie . . van - üab ’ - A 1- i.---~.not &in Tn nnq _-_-vpva1 zijn ut: wcat,afiu -v- u- l~l lJ~: um1 5LoL-mnJ u L n u u u mVUu wei in de kogel en de hoekverdraaiing van de goot cq. de tafel bekend, door metingen met een bepaalde meetonnauwkeurigheid. In dit hoofdstuk zullen we zien dat de toestand van het systeem gereconstrueerd kan worden met behulp van deze metingen en het Kalmanfilter. Voor de theorie van het Kalmanfilter wordt verwezen naar de literatuur, o.a. [i] en [2]. Doel van het filter is dat, uit de beschikbare modelkennis en de gemeten toestandsgrootheden, de overige toestandsgrootheden geschat worden. Algemeen kunnen we voor het filter de volgende vergelijking opstellen : -1
met in ons geval, voor het één-dimensionale model :
$(i)= cqt)=
[ ijt;J
hierbij is : AP
BP C
: systeemmatrix met daarin modelparameters : ingangsmatrix met daarin modelparameters : uitgangsmatrix
: versterkingsmatrix van het Kalmanfilter xm, am : metingen van de positie IC en de hoek Q
Icb
Het symbool staat voor schatting. De toestandsgrootheid x is vet gedrukt om onderscheid te kunnen maken tussen de toestand x van het systeem en de positie x van de kogel; het subscript is gebruikt om aan te duiden dat het om een meting van een toestandsgrootheid gaat. Het subscript tenslotte is gebruikt om aan te duiden dat de systeemmatrices afhankelijk zijn van de systeemparameters, die niet exact bekend hoeven te zijn.
4.2
Aanpassing aan het Kalmanfilter
In (4.1.1) is y ( t ) de informatie die van het meetapparaat, in ons geval een vision systeem en een hoekopnemer, komt. Het vision systeem levert niet een continu signaal x m ( t ) ,maar een signaal (meting) dat elke T ( z 0.1) sec. 77ververst”wordt. Deze metingen zijn, door
6
de meetvertraging in de camera, aanduidingen van waar de kogel T sec. geleden was. Om de metingen te verbeteren, d.w.z. dichter bij de huidige positie te krijgen, schrijven we :
x*(t) = xm(to)
+
/"(T) to
stelt de verbeterde meting voor, terwijl $,(to) de positie voorstelt. Vervolgens benaderen we
de laatst binnengekomen meting van
$*(i!)
Jlt
i(7)dr N
waarmee
x*(t) = xm(to)
dr
(t - t o ) i ( t )
+ (t - t o ) i ( t )
(4 - 2 . 3 )
We gebruiken x*(t) in plaats van het meetsignaal xm(to) in het filter. Opm. Eventueel zou voor (4.2.2)een betere integratiemethode gebruikt kunnen worden. Voorlopig nemen we echter aan dat deze eenvoudige methode voldoende nauwkeurig is.
4.3
Meetruis
Volgens [4] moeten we de meetonnauwkeurigheid van het vision systeem op ongeveer 2 mm. stellen. De ruisintensiteit van de meetonnauwkeurigheid in x ( t ) moeten we dan op ongeveer 2 10-3 houden. Bij de simulaties is aangenomen dat de hoeken met een nauwkeurigheid van 0.1" gemeten worden. De daaruit voortvloeiende ruisintensiteit is dan
4.4
v. 3 10-3
2
Systeemruis
Bij het lineariseren van (A.17) tot en met (A.20) zijn een aantal termen verwaarloosbaar klein verondersteld, en deze termen uiten zich onder andere in de systeemruis. Om nu de orde grootte van de systeemruis vast te stellen moeten we de grootte van de verwaarloosde niet-lineaire termen te pakken krijgen. Hiertoe is, met behulp van simulaties een aantal significante waarden van de toestandsgrootheden bepaald en vervolgens zijn deze ingevuld in de niet-lineaire termen. Bij deze simulaties is aangenomen dat alle parameters exact bekend zijn. Bijvoorbeeld : Bij het linearisatieproces is in vergelijking ( A . í r ) sin(&) gelijk gesteld aan (Y.Volgens de Taylorreeks-ontwikkelingvar, sin(a) verwaadozen we een term van orde Mkg$. Een significante waarde voor Q is 0.05 rad. Met Mk = 31.4 gram en g = 9.81 m/s2 vinden we voor de orde grootte van de meetruis : 4 IO-?. Met dezelfde methode vinden we voor de intensiteit op vergelijking (A.18) : 7 lo-'. Op (A.19)en (A.20) vinden we respectievelijk lo-' en lo-'. Met deze gegevens kunnen we nu de versterking van het Kalmanfilter bepalen, bijvoorbeeld met PC Matlab.
-
7
-
Opm. Er is ook nog systeemruis ten gevolge van parameterfouten, niet gemodelleerde (of verkeerd gemodelleerde) wrijving etc.. Zolang het systeem nog niet beschikbaar is kan hier echter weinig zinnigs over gezegd worden. De systeemparameters en wrijvingscoëfficiënten zullen door experimenten, metingen en (off-line) systeemidentificatie bepaald moeten worden [5] [7].
8
Sirnulatieresultaten optimale-regelaar
5
5.1
Inleiding
Een aantal simulaties is uitgevoerd3, op het twee dimensionale model, waarbij we het ( A .i 7) tot ei met ( A .20). systeem simuieren met de niet-lineaire bewregings~~ïge~~Kiilgeii Het vision systeem is gesimuleerd door uit de systeemvergelijkingen de coördinaten .(tj en g ( t ) van de kogel te halen, hierbij meetruis (trekkingen uit een normale verdeling met de in 4.3 bepaalde intensiteit) te voegen, en vervolgens deze waarde T sec. vast te houden en daarna door te sturen naai het filter. Hierdoor simuleren we het vision systeem, met de meetvertraging en met de meetonnauwkeurigheid. Voor de meetvertraging is 0.1 seconde genomen. De hoekverdraaiingen worden op dezelfde manier behandeld, met die uitzondering dat ze direct, zonder meetvertraging aan het filter worden geleverd. Het filter is gesimuleerd volgens de in 4.2 beschreven methode en de "normale" methode, om een vergelijking tussen deze twee mogelijk te maken De regelaar tenslotte is een optimale regelaar, zoals hierboven beschreven, (zie 3 ) , die het ingangskoppel bepaalt uit de geschatte toestandsgrootheden. In een blokschema ziet dit er als volgt uit :
Figuur 5.1 Alle simulaties in dit verslag zijn continu uitgevoerd. Voor de praktische implementatie zoals die in de toekomst moet plaatsvinden, is het nuttig om dan ook discrete simulaties te doen.
Vergelijking van het gewone Kalmanfilter met het aangepaste filter
5.2
Om te controleren of de in 4.2 aangedragen verandering aan het Kalmanfilter werkelijk een verbetering betekent, zijn simulaties uitgevoerd met elk van beide filters. We moeten dan concluderen dat de responsie van het systeem (baan van de kogel) er in beide gevallen ~~
~
~
3Zie appendix C voor de implementatie van het volledige systeem
9
vrijwel hetzelfde uitziet. In figuur 5.2 is zijn de schattingsfouten (het verschil tussen de werkelijke en de gefilterde positie) van elk filter weergegeven in één figuur.
- - : Kaimanñiier !
O
;,*,'
\,;'"-
.'
'i
,'
" l..'
'
, , , . \
I
,
\I
i-
: Aangepaste filter
I I1
-0.01
1'
' / +i /
f
.
. , .
.
...
I '(
r ;1
I
:
I
~
-
.
-
.
~~
,
Omdat het aangepaste filter de toestanden beter schat, vooral in het begin van de beweging wanneer de snelheid van de kogel relatief hoog is, is in het verdere verloop van dit verslag de aanpassing doorgevoerd. Een andere reden om te kiezen voor het aangepaste filter zullen we zien als we overstappen op trajectoriesturing. De snelheid van de kogel zal dan niet naar nul gaan, zoals bij punt-punt sturing. Waarschijnlijk zal het aangepaste filter gedurende de gehele beweging de toestanden van het systeem beter schatten dan het oorspronkelijke filter. In hoofdstuk 7 komen we terug op het verschil tussen beide schattingsmethoden.
Simulatieresultaten In de figuren 5.3 en 5.4 is voor twee verschillende eindpunten van de kogel de responsie weergegeven.
0.2
KOGELBAAN
KOGELBAAN
0.15
0.05 C
'
-0.15 -0.2
-0.0'
-7
I
-0.1
I
O
I
0.1
i
0.2
x m x m Figuur 5.3 Figuur 5.4 In beide gevallen is het startpunt van de kogel (0.2,0.2). De kogel bereikt in beide situaties het gewenste eindpunt, resp. (0,Q) en (0.1,0.1), na ongeveer 5 sec. Dit gebeurt zonder doorschot. Ook is geconstateerd dat de versnelling van de tafel niet t e groot is, zodat de kogel altijd contact houdt met de tafel, en dat de hoekverdraaiingen van de tafel niet de maximaal toegestane waarden overschrijden zodat ook wordt voldaan aan de fysische beperkingen van het mechanische systeem.
11
5.4
Invloed van parameterfouten
Vooraf zijn alle systeemparameters bekend verondersteld, behalve de kogelmassa. Alle andere parameters moeten met off-line methoden bepaald worden. Gebleken Als de kogelmassaniet exact bekend is, maken we hiervoor een schatting i&. is dat het sytzem, met de gekmer, instelling VBII de regelaar en het filter stabiel blijft voor tenminste 4 : 25 5 &kk 5 50 met h!k = 31.4 gram, de massa van de kogel uit een (Genius)muis. De resultaten van simulaties met h!fk = 50 gram en k f k = 25 gram zijn weergegeven in de figuren 5.5 en 5.6.
KOGELBAAN
KOGELBAAN
x [m] x [m] Figuur 5.5 Figuur 5.6 Te zien is dat de kogel in beide gevallen niet het gewenste eindpunt, aangegeven met bereikt. Door de fout in de schatting blijft er een statische afwijking over.
5.5
Schatting van
*,
Mh
Om deze statische afwijking weg te regelen zouden we bijvoorbeeld de massa van de kogel nauwkeuriger kunnen proberen t e schatten5. Wanneer de kogel stilligt nemen we aan dat het systeem voldoet aan de gelineariseerde bewegingsvergelijkingen. We kunnen dan bepalen volgens :
(5.5.1) *Met een andere instelling van de regelaar en het filter kan wellicht een ruimere marge gevonden worden 5Zie hoofdstuk 6 voor een andere oplossing
12
Het probleem hierbij is dat voor x alleen een schatting ?, geleverd door het (aangepaste) Kalmanfilter voorhanden is. Door de aanwezigheid van meetruis, modelfouten en parameterfouten zal deze schatting afwijken van de werkelijke positie van de kogel. We kunnen dit probleem gedeeltelijk elimineren door te middellen over een bepaald aantal schattingen A
X.
.. dan twintig . . schatUit simdaties is geV!ekm dzt r&k!elliig over twee seconden -dit. zijn tingen voor Mk- een redelijk resultaat geeft. In tabel 5.1 is het resultaat weergegeven van een reeks simulaties. Deze resultaten zijn als volgt verkregen : Gegeven is een beginschatting d k 1 voor de kogelmassa. Op grond hiervan worden de regelwet L en de versterking van K het Kalmanfilter bepaald. Bepaal, als de kogel stilligt een nieuwe schatting k k 2 op de hierboven beschreven manier (5.5.i).Met behulp hiervan wordt een nieuwe regelwet en een nieuw filter bepaald. Hiermee kunnen we dan opnieuw simuleren etc.. Schatting Massa in gram Stat. afw. in mm.
50
30.5 31.3
Tabel 5.1 Gebleken is dat de massa van de kogel na drie keer simuleren op een paar tiende gram nauwkeurig bekend is, en dat dan ook de gewenste positie voldoende nauwkeurig bereikt wordt. (Het aantal maal simuleren hangt natuurlijk ook af van de beginschatting). In de praktijk is de bovenbeschreven methode verre van ideaal : De regelwet en de versterking van het filter moeten telkens opnieuw berekend worden en de kogel moet telkens weer in de beginpositie gelegd worden. Beter is de volgende manier : Gegeven is een beginschatting 2 k 1 . Bepaal op grond hiervan de regelwet L en de versterking K van het Kalmanfilter. Bepaal, als de kogel stilligt, een nieuwe schatting A l k 2 en g a hiermee verder, zonder de regelwet of het filter aan te passen èn zonder te kogel terug te leggen. Ook dit proces zal een aantal maal herhaald moeten worden. In figuur 5.7 is de kogelbaan weergegeven, zoals deze bepaald is met behulp van een simulatie. In tabel 5.2 zijn de schattingen van de massa vermeld zoals deze achtereenvolgens bepaald zijn. A
13
1-
KOGELBAAN
x m Figuzlr 5.7
11
Massa in gram
1
50
I 32.3 I 31.2 11
Tabel 5.2 In figuur 5.7 is het gedrag van de kogel te zien : Met de beginschatting gaat de kogel niet op het gewenste punt liggen. Als de schatting van de massa aangepast wordt gaat de kogel dichter bij het gewenste punt liggen. De nieuwe schatting k k k , is echter nog niet nauwkeurig genoeg, zodat het gewenste eindpunt nog niet wordt bereikt. Pas met de tweede aanpassing van de schatting wordt het gewenste eindpunt bereikt. E r zijn zeer zeker veel elegantere methoden om de massa van de kogel te schatten, zoals bijvoorbeeld het optimale filter [5]. Nadeel hiervan is dat deze methode veel rekentijd vergt en daarom moeilijk on-line kan werken. Onderzoek heeft echter aangetoond dat een on-line implementatie wel haalbaar zou kunnen zijn als we gebruik kunnen maken van transputers [6]. Voor de schatting van de massa van de kogel is niet direct een on-line methode nodig. Het is ook denkbaar de kogel een bepaalde beweging te laten uitvoeren en de meetgegevens op te slaan. Hieruit zou dan via een off-line methode de massa van de kogel bepaald kunnen worden [71.
14
6 6.1
De optimale PID-regelaar Inleiding
In het vorige hoofdstuk is de statische afwijking weggeregeld door de massa van de kogel beter te schatten. Een andeïe maiiieï om de statische &iijking weg te regelen is het invoeren van een integrerende actie. Als regelwet krijgen we dan voor hei id-systeem :
Om de optimale regelwet te kunnen bepalen moeten we de toestand van het systeem uitbreiden tot: (J: - x,)dr x - J:, 5
&IT
De toestandsbeschrijving van het Id-systeem luidt dan :
r x-x,.
-O
-
0 -
o
O
Q
O
0
o
0
O
O
-
1 0 0 0--6.Po
- Jyx-xr) x - 2,
0
o = y o
0
o
+
It:
Mk+$
1 0- -
Q
&
-
- 0 O 0 K f O 1 I;
-
-
Met behulp van P.C. Matlab kunnen we weer de optimale regelwet bepalen.
6.2
-
O O O O -Mkgx,
I:
-
( 6 . i. 3 )
Simulat ieresultaten
Een simulatie is uitgevoerd met de optimale PID-regelaar, waarbij we de massa Mk schatten op 50 gram, terwijl de massa in werkelijkheid 31.4 gram bedraagt. Het resultaat is weergegeven in figuur 6.1.
15
KOGELBAAN
x m 1%Juur 6.2 We zien dat de kogel nu wel het gewenste eindpunt bereikt, zij het met enig doorschot. In figuur 6.2 is het resultaat weergegeven van een simulatie waarbij k f k = k f k . In feite hebben we dan geen integrerende actie nodig om de kogel op de gewenste positie te krijgen. We zien dat de kogel de gewenste positie bereikt, maar dit gebeurt nadat eerst het gewenste punt is gepasserd. Wanneer we de polen van het geregelde systeem berekenen, blijkt dat de twee dominante polen zuiver reëel zijn, dus het geregelde systeem is (boven) critisch gedempt.
KOGELBAAN
O
a
2
r
b -0.1 O 0.1
-O-$.2
x [ml F i g u u r 6.2 16
0.2
D e oplossing van de bewegingsvergelijkingen van het geregelde systeem is de som van een aantal e-machten. Bekijken we de som van twee e-machten, dan kan deze eerst positief en later negatief zijn. In figuur 6.3 is een benadering voor de positie van de kogel gemaakt op grond van de twee meest dominante polen.
- : Werkelijke baan - : Benadering
-
o.2 o.1
i
l
I
-
O _____----
-0.1
-0.2
i
I
I
Figuur 6.3 We zien dat de benadering (voor voldoende grote t ) de baan vaD de kogel naiiwkeurig volgt. Hiermee is aangetoond dat het (boven) critisch gedempte systeem geen doorschot heeft, maar wel het gewenste punt kan passeren. Verder is in figuur 6.2 te zien dat, met name het laatste stukje van de beweging nogal 'bibberig' verloopt. Dit wordt veroorzaakt door de aanzienlijke meetruis die het vision systeem produceert. De regelaar reageert op deze meetruis waardoor de kogel niet helemaal stil gaat liggen. Overigens zal dit effect in de praktijk niet zo duidelijk te zien zijn, omdat er dan sprake zal zijn van demping, waardoor de kogel minder snel zal reageren.
17
7
7.1
Baansturing Inleiding
In dit hoofdstuk wordt een regelwet afgeleid, waarmee we de kogel een gewenste baan kunnen iaten beschrijven. E r zal gebruik gemaakt worden van de hierboven afgeleide volledige toestandsterugkoppelling (zie 3), in combinatie met voorwaartskoppelling, welke we vanaf nu computed torque zullen noemen. Later zal ook een manier gezocht worden om de massa van de kogel te schatten.
7.2
Computed Torque
We zullen hier het twee-dimensionale model beschouwen, hoewel de afleidingen voor het één-dimensionale model geheel analoog verlopen. Van de te beschrijven baan, in ons geval een cirkelbaan, zijn voor de kogel de gewenste coördinaten, x,(t) en y,(t), en afgeleiden naar de tijd, als functie van de tijd bekend. Beschouw de gelineariseerde bewegingsvergelijkingen (A.21) en (A.22). Uit deze vergelijkingen volgt direct :
(7.2.1) (7.2.2) Hiermee hebben we dus ook de voor de te beschrijven kogelbaan gewenste hoekverdraaiingen a, en p, gevonden en kunnen we eenvoudig de afgeleiden bepalen :
(7.2.3)
(7.2.5) (7.2.6)
( 7.2.7) (7.2.8) Invullen in (A.23) en (A.24), waarbij we lineariseren rond z = O, y = O, a = O en ,í?= O, 18
levert ons de voorwaartskoppelling :
( 7.2.9) (7.2.10)
Net als iii het vorige hoofdstuk zijn de toestandsgrootheden niet exact bekend, maar zijn alleen schattingen hiervan bekend. Hetzelfde geldt voor
7.3
Mk.
Tegenkoppelling
De tegenkoppelling, de feitelijke regeling, is wederom een optimale regelwet. De afleiding daarvan verloopt hetzelfde als in 3 en kan daarom hier achterwege blijven. De tegenkoppelling voor het twee-dimensionale systeem luidt :
7.4
D e totale regeling
De totale regeling werkt nu als volgt : met behulp van de metingen en de beschikbare modelkennis worden in het Kalmanfilter de toestandsgrootheden van het systeem geschat. Met deze schattingen en de gewenste toestand worden Kfal en l i f b z bepaald. Met de gewenste toestand bepalen we K t f l en K i f z . We voeren vervolgens deze koppels toe aan het systeem etc. In een blokschema ziet de regeling er als volgt uit :
TORQUE
FILTER
Figuur 7.1
Voor de tegenkoppelling is gekozen voor de optimale regelwet. Er zijn natuxrlijk vee! meer mogelijkheden om de kogel een bepaalde baan te laten beschrijven. Vooral de schakelvlakmethode lijkt zeer geschikt voor baansturing, vooral wanner de parameters nog niet exact bekend zijn [û] [9].
19
7.5
Vergelijking van het gewone Kalmanfilter met het aangepaste filter
Twee simulaties zijn gedraaid, met dezelfde systeemparameters, maar eenmaal met het Kalmanfilter en eenmaal met het aangepaste filter, om nogmaals het verschil tussen beide figuur UP uchattingsfouten (het methoden aan i e onderzoeken. In figüüi 7.2 zij= in verschil tussen de werkelijke en de gefilterde x-coördinaat) van eik fiiter tegen de tijd uiigezet. Nog duidelijker dan bij punt-punt sturing is hier de verbetering in de schatting te zien. ~
x-x
-
-:
A
Kalmanfilter Aangepaste filter
-:
t
[SI
Figuur 7.2
7.6
Simulat ieresult at en
Allereerst is een simulatie uitgevoerd waarbij is aangenomen dat Mk exact bekend is. De gewenste baan is een cirkel met een straal van 10 cm., die in 6.3 seconden moet worden doorlopen. Het resultaat van deze simulatie is t e zien in figuur 7.3.
20
KOGELBAAN
--: -:
Gewenste baan Kogelbaan
0 * 1 5 1 4 -0.15
’
I
I
O
-0.1
I
0.1
x \ml Figuur 7.3 Het startpunt van de kogel is het punt x = O, y = O. Het resultaat is duidelijk : de kogel gaat vanuit het startpunt vrij snel de gewenste baan beschrijven.
7.7 Invloed van parameterfouten Vervolgens is met behulp van simulaties de invloed van een fout in de schatting A& onderzocht. Hiertoe is een simulatie uitgevoerd waarbij we && op 50 gram schatten, terwijl de werkelijke massa 31.4 gram is. Het resultaat is weergegeven in figuur 7.4.
21
KOGELBAAN
si-
-
-
: Gewenste baa
: Kogelbaan
x íml Figuur 7.4 De kogel gaat vanuit de beginpositie (O, O) wel een cirkel beschrijven, maar deze cirkel heeft niet de gewenste straal. E r blijft, net als bij de punt-punt regeling, een statische afwijking over ten gevolge van de fout in M k .
7.8
Schatting van Mk met behulp van een PID-regelaar
Om de kogel de gewenste baan te laten beschrijven op de hierboven voorgestelde manier moeten we de massa van de kogel vrij nauwkeurig schatten. Dit kan op de manier zoals die is gepresenteerd in 5.5. We gaan als volgt te werk : stuur met behulp van de optimale PID-regelaar de kogel naar een punt op de cirkel, bijvoorbeeld het punt x=O, y=O.l. Als de kogel op dit punt is gekomen kunnen we de massa op dezelfde manier schatten als in 5.5 is gedaan. In figuur 7.5 is het resultaat6 weergegeven van een simulatie die op deze manier is uitgevoerd. De beginschatting van h f k was 50 gram, terwijl de massa in werkelijkheid 31.4 gram bedroeg. Omdat de kogel het laatste stuk van de beweging naar het beginpunt van de cirkelbaan vrij langzaam rolt, is het mogelijk gebleken om de massa tijdens het rollen te schatten. Deze schatting leverde een waarde voor k k van 31.9 gram op. Met deze waarde wordt de cirkelbaan voldoende nauwkeurig beschreven. 6Met de nieuwe schatting M k is opniew de regelwet en het filter berekend
22
KOGELBAAN I
I
-- : Gewenste baan E
- : Kogelbaan
Y
h
-0.15
'
I
I
O
-0.1
I
0.1
x [ml Figuur 7.5 Ook nu moet opgemerkt worden dat er veel elegantere schattingsmethoden zijn, die in een volgend onderzoek bekeken kunnen worden.
23
Conclusies & aanbevelingen
8 o
De optimale regelwet is geschikt om de kogel van punt naar punt te sturen. Wanneer ten gevolge van een fout in l k k , een statische afwijking overblijft, kunnen we die op twee manieren wegregelen :
1. Door de massa van de kogei te schatten 2. Door het invoeren van een integrerende actie Schatting van de massa is omslachtiger, hoewel de door mij toegepaste methode zeer eenvoudig is. Als we vervolgens overstappen op baansturing is schatting van de massa toch noodzakelijk gebleken. o
De aanpassing aan het Kalmanfilter verbetert de schattingen van de toestandsgrootheden. Net is nu geen bezwaar meer dat de positiemetingen met 0.1 seconde vertraging geleverd worden door het vision systeem.
o
Het is de bedoeling dat de kanteltafel io de toekomst ook werkelijk gaat werken. Alle systeemparameters zullen dan bepaald moeten worden door metingen en (off-line) systeemidentificatie [7].
o
In het model van het systeem, dat in appendix A is bepaald, is aangenomen dat het hele systeem wrijvingsloos is. In werkelijkheid is dit natuurlijk niet het geval. De wrijvingsparameters zullen moeten worden vastgelegd, met behulp van experimenten of met behulp van systeemidentificatie (51. Eventueel moet het model dan worden aangepast. Hetzelfde geldt voor de speling, hoewel aanpassing van het model hiervoor moeilijker lijkt.
o
Bij de opstelling van het model is aangenomen dat de kogel zonder slip rolt. Bekeken moet worden of dit ook werkelijk het geval en eventueel zal weer een aanpassing aan het model gemaakt moeten worden.
o
In het model is de invloed van de massa van de motor die zorgt voor rotatie om de ,&as weggelaten. Deze motor beweegt met de plaat mee en zal invloed hebben op het massatraagheidsmoment bij rotatie om de a-as en op het koppel dat de andere motor moet leveren. Het model lijkt hiervoor vrij eenvoudig aan te passen.
Het gebruik van het woord motorkoppels is eigenlijk niet juist. Bedoeld zijn de koppels die op de plaat werken. De motoren zorgen voor deze koppels via een bepaalde overbrenging.
o In dit rapport wordt gesproken over motorkoppels.
o
Het is verstandig om de regeling vanuit een Matlab omgeving t e sturen. Op deze manier wordt voorkomen dat degene die met de tafel aan het werk gaat al te veel tijd moet besteden aan programmeerwerk.
24
o
Het is interessant om andere regelwetten toe te passen, bijvoorbeeld een schakelvlakmethode [8] [9]. Als de rekentijd het toelaat is het zeker interessant om een on-line implementatie te maken van het optimale filter. Dit zou verwezenlijkt kunnen worden door gebruik te maken van transputers [6]. Hiermee kan dan een on-line schatting van bijvoorbeeld de massa van de kogel gemaakt worden.
o
Het is de moeite waard om te onderzoeken hoe de kogel zo snel mogelijk van punt naar punt geregeld kan worden. Hiertoe moet een nieuwe regelstrategie bepaald worden.
25
Appendix A Het 2-dimensionale model In deze appendix worden de bewegingsvergelijkingen voor het systeem afgeleid. Hierbij wordt gebrtiik gemaakt v a ~ de i methode var, Lagrange [VI. Allereerst dient, vastgesteld te worden hoe de tafel precies beweegt ten opzichte van de vaste wereld. Zie fig A.I.
Figuur A.1
Het blad heeft twee rotatie-assen, die hier de Q- en ,&as genoemd zullen worden. De w a s blijft in het horizontale vlak, terwijl de ,&as met de plaat meeroteert. Definieer Zo als het vaste wereld stelsel van eenheidsvectoren en é als het met de plaat meebewegende stelsel van eenheidsvectoren. De beide stelsels hebben hun oorsprong gemeenschappelijk in het midden van de plaat. Als de plaat zich in horizontale postitie bevindt vallen de beide assen samen. De rotatie-as Q ligt op ;?i de rotatie-as ,LI valt samen met. 'g Zie figuur A.2.
A. 1
/
/
Figuur A 2 We kunnen nu de volgende relaties tussen de eenheidsvectoren bepalen :
z,'
= c o s ( a ) c - sin(a)c
ëy = - sin(@) s i n ( a ) c
e', =
cos(P)s i n ( a ) c
+ c o s ( P ) -~ sin(@) cos(a)ë:
+ sin(@)Zi - sin(@)c o s ( a ) Z ~
(A4 (A-8 (A.3)
Voor de afgeleiden naar de tijd van het meebewegende stelsel kunnen we de volgende vergelijkingen opstellen :
De positie van het massamiddelpunt van de kogel kunnen we als volgt uitdrukken :
voor de snelheid van de kogel geldt :
A.2
De kinetische energie ten gevolge van de translatie van de kogel luidt :
waarbij we de straal van de kogel verwaarloosd hebben. E r is ook nog kinetische energie ten gevolge van de rotatiesnelheid van d e kogel om zijn eigen as. Hiervoor geldt : . -
(A.ll) waarbij is aangenomen dat de kogel zonder slip rolt. Dit heeft ook als consequentie dat de wrijving zeer laag is en waarschijnlijk verwaarloosd mag worden. Verder is er kinetische energie ten gevolge van het massatraagheidsmoment van de kogel ten opzichte van de rotatie-assen a en ,û. Ook nu is de invloed van de straal van de kogel verwaarloosbaar klein verondersteld.
is de kinetische energie ten gevolge van de omwentelingssnelheden van het tafelblad. , is het massatraagheidsmoment constant. Bij rotatie om a is het massaBij rotatie om O traagheidsmoment afhankelijk van de hoekverdraaiing @.Hier is echter aangenomen dat de hoek ,û klein genoeg is om dit massatraagheidsmoment als constant te mogen beschouwen. UkIV
Verder heeft de kogel nog potentiële energie ten opzichte van het vlak dat opgespannen ; De uitdrukking voor de potentiële energie luidt : wordt door de vectoren ë,, en .ë
Noem uk
= UkI
+
UkII
+
UkIII
+
UkIV
(A.15)
de totale kinetische energie, Met behulp van Lagrange kunnen we de bewegingsvergelijkingen van het systeem bepalen : (A.16)
waarbij q = [q a
r2
@I'
de kolom met gegeneraliseerde coördinaten is en &* = [O
A.3
IC1OIC2lT
de niet van de potentiaal af t e leiden belasting. Aangenomen is dat het systeem wrijvingsloos is. In de praktijk zal er wel sprake zijn van wrijving. Dir zal dan waarschijnlijk vooral droge wrijving zijn. Deze is zeer moelijk t e modelleren. Als we echter met systeemidentificatie [5] de parameters gaan bepalen, is het mogelijk de droge wrijving in een andere systeemparameter, bijvoorbeeld een massatraagheidsmoment, te verdisconteren. Dit zal in toekomstig onderzoek bekeken moeten woden. ûmdat het üitweïkm van (A.16)n a p 1 rekenintensief is, is gebruik gemaakt van het formulemanipulatiepakket reduce. Hiermee vinden we :
(A.17) (A.18) & =
+
Mkg(r1 COS(CY) - r2sin(a) sin(@)) Ici) ~ k { 2 r cos(a) ~ r ~sin(a) sin(B) - ri sin2((a) sin2(p) r: sin2(a) 2r2 sin2(@) 2r,2) 4, (A.19)
+
+
+
+
(A.20) Lineariseren we deze vergelijkingen rond de positie rl+ r2, en
CY
=OO , = O dan volgt :
(A.21) (A.22) (A.23)
(A.24)
A.4
Appendix B Het i-dimensionale model Hier worden de bewegingsvergelijkingen van het één-dimensionale systeem bepaald, met b e h d p v m de methode van La.grange. We maken de volgende tekenafspraken : (zie figuur
B.i j
\
k,lk
Figuur B. 1
K kfk
Ik
R
: motorkoppel (systeemingang) : massa van de kogel : massatraagheidsmoment van de kogel : straal van de kogel
Ig : massatraagheidsmoment van de goot x : positie van de kogel g : versnelling van de zwaartekracht o : hoekverdraaiing van de goot
N.B. : het systeem wordt spelingsvrij verondersteld. De kinetische energie van het systeem luidt :
Veronderstel dat de kogel zonder slip rolt, dan geldt :
Invullen in
(B. i )levert voor de kinetische energie :
B.l
De potentiële energie van het systeem luidt :
P = Mkg sin(a) De niet van de potentiaal af t e leiden belasting luidt
&*=
:
[I(]
en de gegeneraliseerde coördinaten :
q=
[ n]
De niet-lineaire bewegingsvergelij kingen worden vervolgens bepaald volgens Lagrange : d dU dU dP -(-) - - + - = Q* dt dq
dq
dq
Hiermee vinden we de niet-lineaire bewegingsvergelijkingen voor dit syteem :
(Ig
+ h f k x 2 ) &+ 2hfks&&+ M k g z
COS(CY)
=K
(B.8)
Lineariseren we deze vergelijkingen rond x = x,, de gewenste eindpositie van de kogel, en CY = O dan luiden de bewegingsvergelijkingen :
B.2
Appendix C Implement at ie van het optimaal geregelde systeem In deze appendix wordt de implementatie gegeven van het programma waarmee de simulaties gedaan zijn. 'Weergeven is (een deel van) de zgn. üseï-fik. Deze f i e maakt due! uit v m .. _ _ het programma P.C.-ASP. De userfile is opgebouwd uit blokken die hieronder opgesomd worden. 1 . Ref : Hier wordt de gewenste positie of baan van de kogel weergegeven.
2.
Con : Hierin moet de regelwet worden berekend.
3. Sys : De bewegingsvergelijkingen van het systeem worden hierin gezet. 4. Maes : Hier wordt het meetinstrument gesimuleerd.
5 . Fil : Hier staat de implementatie van het filter. Elk blok bestaat uit twee delen nl. een CDE-deel en een OUT-deel. In het CDE-deel staan de differentiaal vergelij kingen en in het OUT-deel staan de uitgangsvergelijkingen.
c.1
v o i d r e f u u t ( d o i i b l e y i - [ l , d o u b l e xr[l, doiihl P pr [ I , doiible
!
1
iiit
alg)
;* 1-ri;i- . . . f c t c : . */ y r [l I - i)r 1 1 ] : / " i ) e g p w e n c t e x-cc)cir(li n a a t * / yr 12 1 -pr 121 : / * rip g e w e n s t e y - c o v t d i r i a a t * /
void c o n o i i t
I
t,
r [ 1 , t l r ) u b l e xcll , drmh1c yr I ,double yf [ 3 , dOiih1 C' pc-'f ] ,
(dc)iihl~y
dorihl e t ,i n t a l g 1 / * ycCi1-
... ;
*/
qtc.
d o u l ) 1e IJ1 1
r lil
3. r
Li13 r IJ14
I
1,21 , L 2 2 , L 2 3 , L 2 4 ,
Mkdak,g,xg,yg;
/ * D e c l a r a t i e v a n d e v a r i a b e l e n voor de o p t i m a l e r e g e l w e t * / / * Deze v a r i a b e l e n z i j n n o d i g v o o r de b e p a l i n g van h e t evenwichtck o p p e l . Z i e pag. 5 * /
c.2
. - . ; ctc.
/f d x r ; [ l ] -
m,
fl
C I O I ~ L ) ~ ~
Ik, Tfinl f a , I hbe t a , L7f
RI;;
/ * I k r i a r a t j e v a n de systeemparameters * /
I
/ " y.[t]-. int, i;
... ;
etc.
*/
f o r ( i 1i.; i.< -(f ; i + + ) 1-s [ i ]-xs [ i. J ;
c.3
1
v o i d m e a s o u t ( c l o i i b l e yiii [ , d o u b l e xrn
l c y ? [ J , doiibl pni 1 1 , d o u h 1 P t , j 11 t c3 1 q 1
i
[I ,
...
; et(:. "/ yrii [ i ] - i ~ o n x . ; ; /f' N P t j ng v a n de x - - c o o r d i n a a t yin[?,]-g<)r,y; / + N c t i r i q van d e y - - c o o r d i n a a t
/ " yrn[i]-
I
'31
J : ';ml \ 1 - ~ s [ * 7 1 : p i[
-\.?
sec. achter " /
[3
(2.4
*/ : d e z e lopen h i e r al 0.1
/f' y f [ l l =
for ( i
i i i t a s p 2 ( 3 oiiq
-1
. ..:
etc.
int i : :i < - 1 O ; i 4 4 ) yf [ i l - x € [ i ] :
ri)ti)
[
/*
Cal l w l co~,d.;, used s ~ ifar is reported. / " 1?kic?n yciii t r ~ t i l r n 11011 z e r o , ASP w i l l a b o r t / " H i p r w c l r d t liPt i i i f e i t e het vjsion systeem gesimuleerd. Door / " t i p t l r q g i i i q i n t e r v a l up s e c . t e z e t t e n wordt een $ / " r n r e t v e r t r a g i n g v a n 0 . 1 s e c . gesimuleerd.
/"
0.1
j-ríiiidom(10001) : r u i c: i - ( (doiihl P i / l O O O O . O- O . 5 ì * 2 . O ? O . O02 : j - r;iiidc>ni ( 1 O 0 0 1 ) : i 11 i s 2 - ( í dc)u 131e 1 j /I DO O O O - O 5 ) * 2 O O O O 2 : p ~ s ~ - [y1 s1 *rili c: 1 ; p o s y - y s [ 5 ] 4 r u j s2; to-t; r e i u i 11 O :
1
.
I
.
.
'I
.
c.5
f.
.
*/
*/ */ */
*/
*/
Referenties [l] 5.3. Kok
Werktuigkundige regeltechniek 11 Collegedictaat 4594, versie 1985/86 [2] H. Kwakernaak en R. Sivan Linear optimal control systems Wiley-Interscience 1972 [3] A. Sauren Multibody dynamica Collegedictaat 4659, versie 1986 [4] D. Dil Het bekijken van de regelbaarheid van een tafel/kogel opstelling met beeldherkenning als meet methode WFW rapportnr. 89061 [5] M.J.G. van de Molengraft Identification of non-linear mechanical systems Proefschrift 1990 [6] A.E.H.J. Pasnagel Transputers in control of mechanical manipulators WFW rapportnr. 90032
[7] H. Vincent Derivation and identification WFW rapportnr. 90022
of
a model of a mechanical system
[8] F.E. Veldpaus werktuigkundige regeltechniek 111 Collegedictaat 4675, versie 1989 [9] H. Asada & J.E. Slotine Robot analysis and control Wiley-Interscience 1985
I
