Trajectvolging en synchronisatie van mobiele robots N.J. Dirkx DCT 2006.110
Traineeship report .
Coach(es):
H. Nijmeijer
.
Supervisor:
. . .
Technische Universiteit Eindhoven Department Mechanical Engineering Dynamics and Control Technology group
.
Eindhoven, September, 2006
H. Nijmeijer
1
Inhoudsopgave 1 Inleiding
2
2 Positiebepaling 2.1 Introductie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Positiebepaling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3 3 3
3 Het 3.1 3.2 3.3 3.4 3.5 3.6
volgprobleem Introductie . . . . . . . . . Modelvorming . . . . . . . . Trajectvolging . . . . . . . . Parkeren . . . . . . . . . . . Master-slave synchronisatie Wederzijdse synchronisatie .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
7 . 7 . 7 . 8 . 14 . 19 . 26
4 Conclusies en aanbevelingen 35 4.1 Conclusies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 4.2 Aanbevelingen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 5 Bibliografie
37
6 Appendix 38 6.1 Regelbaarheid voor parkeren . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 6.2 Simulink modellen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
2
Hoofdstuk 1 Inleiding De laatste eeuw is het gebruik van robots sterk toegenomen in allerlei sectoren. De vraag naar steeds geavanceerdere multidisciplinaire robots heeft er toe geleid dat ze meer en meer als zelfstandige individuen kunnen opereren. Een recentelijke ontwikkeling is de interesse in robots die niet alleen zelfstandig te werk kunnen gaan, maar ook kunnen samenwerken als groep om hun taak te volbrengen. Robots die kunnen samenwerken zullen gezamenlijk taken sneller en effici¨enter kunnen uitvoeren. Ook zullen robots voor meer verschillende doeleinden ingezet kunnen worden, als gevolg van de nieuwe mogelijkheden die ontstaan. Hierbij kan gedacht worden aan de inzet voor defensieve doeleinden, productieprocessen of onbemande mobiele robots. In dit verslag wordt ´e´en bepaald type robot onder de loep genomen: een mobiele robot met twee afzonderlijk bestuurbare achterwielen en een enkel zwenkend voorwiel. De beschouwde robot is niet-holonoom wat wil zeggen dat er een niet-holonome voorwaarde geldt die stelt dat er geen wielslip aanwezig is. Doelstelling van het onderzoek is allereerst het ontwerpen van een non-lineaire regelaar die de robot in staat stelt een referentietraject te volgen. Vervolgens wordt als doel gesteld regelaars te ontwerpen waarmee meerdere robots kunnen worden gesynchroniseerd volgens verschillende synchronisatie-principes, te weten het master-slave en het wederzijdse synchronisatie-principe. Om als groep te kunnen opereren zullen de robots informatie met elkaar moeten kunnen uitwisselen en dus ten alle tijden met elkaar in contact moeten staan. Het verslag is grofweg opgedeeld in twee delen: in Hoofdstuk 2 wordt gekeken naar de informatie overdracht tussen de verschillende robots waarna Hoofdstuk 3 zich richt op het regeltechnische aspect. Preciezer geformuleerd houdt dit laatstgenoemde hoofdstuk ten eerste het opstelling van het dynamisch model van de robot in. In de tweede paragraaf wordt het ontwerp van een regelsysteem waarmee de robot een referentietraject kan volgen behandelt. De derde paragraaf beschouwd een speciaal geval: een vast punt als referentie. Paragraaf vier legt zich toe op het ontwerp van een regelsysteem ter synchronisatie van meerdere robots volgens het master-slave principe. Als laatste punt wordt in de vijfde paragraaf een regelsysteem ontworpen om de robots volgens het wederzijdse synchronisatie-principe te laten opereren. In elke paragraaf zullen de ontwerpen worden getest met behulp van simulaties in Matlab/Simulink waarmee inzicht wordt verkregen in de prestaties en stabiliteit van de ontworpen systemen.
3
Hoofdstuk 2 Positiebepaling 2.1
Introductie
Wil je een robot laten rijden over een gewenst traject, dan zal deze moeten beschikken over informatie betreffende zijn positie. Wanneer gevraagd is robots als groep aan te sturen dan zal het tevens noodzakelijk zijn dat de robots kennis hebben over de onderlinge posities ten opzichte van elkaar. In dit hoofdstuk zal, voorafgaand aan het ontwerp van de uiteindelijk regelsystemen, eerst worden gekeken naar de middelen waarover de robots beschikken om informatie te vergaren over hun toestand, dat wil zeggen hun positie en ori¨entatie in de omgeving waarin ze zich bevinden. Naast deze globale positiebepaling wordt ingegaan op de manier waarop de communicatie tussen de robots verloopt, zodat ze rekening met elkaar kunnen houden bij het volbrengen van hun taak.
2.2
Positiebepaling
Bij het bepalen van de positie wordt er onderscheidt gemaakt tussen de positie in het globale co¨ordinatenstelsel en in het lokale co¨ordinatenstelsel. De eerstgenoemde betreft de positie van de robots beschouwd vanuit een vast punt in de omgeving waarin ze zich bevinden. Dit kan een open terrein zijn of een afgesloten ruimte. De globale co¨ordinaten (x, y) kunnen worden verkregen met behulp van bijvoorbeeld een GPS-systeem wanneer de robots zich in open terrein bevinden of door middel van plaatsing van referentiepunten wanneer ze zich bevinden in een ruimte. De hoek θ wordt gevonden door gebruikmaking van een elektronisch kompas. Het lokale co¨ordinatenstelsel zit bevestigd aan de robot zelf. Bij het bepalen van de onderlinge afstanden tussen de robots zal iedere robot vanuit zijn eigen co¨ordinatenstelsel meten. De techniek die wordt gebruikt voor het meten van de afstanden is gebaseerd op de techniek zoals die voorkomt bij draadloze sensornetwerken. Hierbij worden bakens ingezet die een tweevoudig signaal uitzenden.
Dit signaal bestaat uit radiogolven en ultrasone golven en met gebruikmaking van het feit dat deze golven een andere voortplantingssnelheid hebben, kan er een onderlinge afstand tussen de bakens worden bepaald. Na plaatsing van bakens op de robots kan er dus een onderlinge afstand
4
Figuur 2.1: Positiebepaling
worden gevonden. Echter alleen een afstand is niet voldoende: voor positieregeling is het nodig om over volledige kennis te beschikken betreffende de positie ´en ori¨entatie van de robots ten opzichte van elkaar, oftewel de co¨ordinaten (∆x, ∆y, ∆θ). Dit kan verkregen worden door meerdere bakens in combinatie met elkaar te gebruiken. Beschouw hiertoe figuur 2.1, waarin wordt getoond hoe een robot M de positie van een robot S bepaalt. De bakens M1 en M2 kunnen de relatieve positie (xS1 , yS1 ) van baken S1 vastleggen. Echter, deze positie ligt nog niet eenduidig vast: het is tevens mogelijk dat baken S1 zich bevindt op de gespiegelde positie (xS1 , −yS1 ). Om de unieke positie vast te leggen is dus de plaatsing van extra bakens per robot vereist. Na plaatsing van deze extra bakens M3 en M4 kan de de positie van punt S1 w´el eenduidig worden vastgelegd. Immers, als de afstand tussen M4 en S1 kleiner is dan de afstand tussen M3 en S1 , dan geldt dat de positie wordt gegeven door (xS1 , −yS1 ). Wanneer het tegenovergestelde het geval is bevindt punt S1 zich op positie (xS1 , yS1 ).
5
Figuur 2.2: Positieberekening punt S1
Met eenvoudige driehoeksvergelijkingen kan de ligging van punt S1 nu gevonden worden. Zie ook figuur 2.2. 2 = x2 − x2 yS1 2 S1 2 yS1
=
x21
− (L + xS1
)2
(2.1)
zodat: x22 −x21 +L2 −2L
xS1 = c ·
(2.2) q yS1 =
x22
−
x2 −x2 +L2 2 ( 2 2L1 )
met:
c=
1,
∀ |M4 − S1 | ≥ |M3 − S1 |
−1,
∀ |M4 − S1 | < |M3 − S1 |
(2.3)
In vergelijking (2.2) geeft L de afstand |M1 − M2 | en (x1 , x2 ) de afstand die door bakens wordt gemeten. De relatieve co¨ordinaten (xS2 , yS2 ) = (∆x, ∆y) van baken S2 kunnen op analoge wijze worden gevonden. Vervolgens kan de relatieve hoek ∆θ, zoals aangegeven in figuur 2.3, berekend worden door:
6
∆θ = arcsin
yS2 − yS1 + θd L
(2.4)
met
θd =
π,
∀ xS1 < xS2 ∧ yS2 < yS1
−π,
∀ xS1 < xS2 ∧ yS2 > yS1
0,
(2.5)
∀ xS1 ≥ xS2
Hiermee beschikken alle robots over informatie over hun positie (∆x, ∆y, ∆θ) in de formatie waarvan ze deel uitmaken. Met behulp van een regelsysteem kan deze positie naar wens worden geregeld, met als doel het zelfstandig volgen van een andere robot en hun gewenste positie in de formatie te handhaven. Dit volgprobleem komt aan de orde in het volgende hoofdstuk.
Figuur 2.3: Hoekberekening ∆θ
7
Hoofdstuk 3 Het volgprobleem 3.1
Introductie
Dit hoofdstuk richt zich op de uitwerking van het volgprobleem. Allereerst wordt een dynamisch model opgesteld van de robot en zullen de regelbare in- en uitgangen worden aangegeven. Hierna wordt het volgprobleem opgesplitst in vier delen: namelijk trajectvolging en parkeren voor ´e´en robot, en wordt er vervolgd met master-slave synchronisatie en wederzijdse synchronisatie met meerdere robots.
3.2
Modelvorming
Het model van de robot dat gebruikt gaat worden is een zogenaamde unicycle robot, zoals weergegeven in figuur 3.1. Het regelsysteem dat wordt ontworpen zal zich verder op dit model baseren. De robot maakt gebruikt van twee afzonderlijk aangedreven achterwielen en een enkel zwenkwiel. De wielsnelheden ϕi leveren in combinatie met elkaar een voorwaartse gerichte snelheid v en een hoeksnelheid ω volgens de vergelijkingen: v= ω=
r 2 (ϕ1 + ϕ2 ) r 2B (ϕ2 − ϕ1 )
(3.1)
Hierbij is r de wielradius en B de wielbasis. In het vervolg wordt de snelheid v en hoeksnelheid ω als regelbare motoringangen genomen, wat betekent dat het kinematische model van de robot wordt gebruikt. Omdat beide motoren onafhankelijk regelbaar zijn is het mogelijk de robot ieder gewenst traject af te laten leggen, zolang er wordt voldaan aan de niet-holonome voorwaarde voor geen wielslip: x˙ sin θ − y˙ cos θ = 0
(3.2)
Het traject wordt vastgelegd door het co¨ordinatenstelsel (x, y, θ) waarbij x en y respectievelijk de horizontale en vertikale positie van het midden van de aandrijfas aangeven en θ de hoek die
8
Figuur 3.1: Een unicycle robot
de robot maakt met de x-as. Dit levert de volgende vergelijkingen: x˙ = v cos θ y˙ = v sin θ θ˙ = ω
(3.3)
waarmee na integratie het afgelegde traject van de robot bepaald kan worden.
3.3
Trajectvolging
Deze paragraaf legt zich toe op het regelsysteem dat nodig is om een robot een vooraf bepaald traject te laten volgen. De ontworpen regelaar wordt getest in Matlab/Simulink om te kijken of hij naar behoren functioneert.
Ontwerp regelaar Het traject dat door de robot gevolgd dient te worden, wordt gegeven door de referentieco¨ordinaten (xr , yr , θr ). Deze worden gegenereerd door de ingangen (vr , ωr ) die voldoen aan: x˙r = vr cos θr y˙r = vr sin θr θ˙r = ωr
9
(3.4)
Nu het traject bekend is kan de fout (xe , ye , θe ) ten opzichte van de referentiegrootheden worden gedefine¨erd als: cos θ sin θ 0 xr − x xe ye = − sin θ cos θ 0 yr − y . (3.5) 0 0 1 θr − θ θe Hiermee is de fout [xr − x, yr − y] uit het globale co¨ordinatenstelsel omgezet in [xe , ye ] uitgedrukt in een nieuw lokaal co¨ordinatenstelsel, zoals te zien is in figuur 3.2. Deze transformatie wordt vaker gebruikt in volgproblemen en is voorgesteld door Kanayama (1990). Het voordeel van het gebruik van deze co¨ordinaten is dat het nieuwe stelsel bevestigd is aan de robot en meeroteert met de hoekverdraaiing θ. Zodoende hebben de fout-co¨ordinaten geen invloed van tekenwisselingen wanneer de robot x- of y-as in het globale co¨ordinatenstelsel passeert.
Figuur 3.2: Fout-co¨ordinaten Wanneer de fout (xe , ye , θe ) naar nul gebracht kan worden, wordt het gewenste traject gevolgd en is het probleem van trajectvolging opgelost. De vraag is nu wat voor regelaarvergelijking voor de ingangen v en ω hiervoor opgesteld dienen te worden. Beginnend met het probleem ter regeling voor de ingang ω kan er worden gedacht aan een regelaar die ten eerste zijn referentiesignaal ωr volgt en ten tweede compenseert voor de fout in de hoek θe . Dit levert een regelaarvergelijking in de vorm van: ω = ωr + k1 θe
k1 > 0
10
(3.6)
De verkregen regelaar betreft hier dus een proportionele regelaar met toestandsterugkoppeling. Echter, niet voor alle situaties zal deze regelaar een goed volggedrag geven. Beschouw daartoe figuur 3.3 waar een rechte lijn gevolgd dient worden. In situatie 1 en 2 zal de fout in de hoek θe gecorrigeerd worden doordat de term k1 θe in de regelaarvergelijking wordt aangesproken. In situatie 3 is dit niet het geval: omdat θe = 0 wordt de regelaar niet geactiveerd waardoor ye e niet naar nul zal convergeren. Daarom zal er een term met ye opgenomen moeten worden in de regelaarvergelijking.
Figuur 3.3: Verschillende situaties in het volgprobleem
De vergelijking komt er dan uit te zien als volgt: ω = ωr + k1 θe + k2 ye
k1 > 0, k2 > 0
(3.7)
Hiermee is de regeling voor de hoek θ in staat gesteld de fout in hoek θe in alledrie situaties naar nul te laten convergeren. Het tweede proces dat geregeld moet worden is de snelheid v. Ook hierbij geldt dat de robot de referentiesnelheid vr zal moeten volgen om de gewenste x- en y-posities te verkrijgen. Om te corrigeren voor de fouten xe en ye worden deze termen opgenomen in de regelaarvergelijking die hierdoor de volgende vorm krijgt: v = vr + k3 xe + k4 ye
k3 > 0, k4 > 0
(3.8)
Wederom levert dit dus een proportionele regelaar met toestandsterugkoppeling. Met de verkregen set regelaarvergelijkingen zou het trajectvolgingsprobleem opgelost moeten zijn. Om na te gaan of dit werkelijk het geval is wordt het systeem in Matlab/Simulink gesimuleerd.
Simulaties In dit deel wordt er gekeken naar de prestaties van het ontworpen regelsysteem (3.7) en (3.8). Hiervoor zal het volgprobleem worden gesimuleerd in Matlab/Simulink. Het Simulink-model is bijgevoegd op de cd-rom. Hieronder zijn de regelaarvergelijkingen nogmaals weergegeven: ω = ωr + k1 θe + k2 ye k1 > 0, k2 > 0 (3.9) v = vr + k3 xe + k4 ye k3 > 0, k4 > 0
11
Het referentietraject is gekozen als een Lissajoux figuur en wordt gegeven door de vergelijkingen: xr = −50 sin 0, 1t yr = 20 sin 0, 2t dy θr = arctan( dx )
(3.10)
met de aangepaste arctan voor vier kwadranten waardoor geldt θ [−π, π]. In het vervolg van het verslag zal overal waar de arctan wordt gebruikt deze aangepaste variant worden bedoelt. In figuur 3.4 is de is het resultaat van de simulatie getoond in een x-y-plot. De regelaarwaarden ki bij deze simulatie zijn gekozen als
k1 k2 k3 k4
T
=
20 10 0, 3 0, 3
T
(3.11)
Deze zijn, vanwege het ontbreken van tuningsregels voor een non-lineair systeem als dit, verkregen door middel van trial-and-error. Hierbij is wel getracht de waarden ki zodanig te kiezen dat de ingangssignalen vr en ωr van realistische grootte blijven. Omdat de regelaarwaarden ki zijn getuned op het gekozen testtraject (3.10) is het mogelijk dat bij een ander gekozen traject andere regelaarwaarden de voorkeur krijgen. De begincondities van de master robot worden gegeven door de ingestelde initi¨ele fout van:
xe ye θe
T 0
=
−30 −40 0
12
T
.
(3.12)
Figuur 3.4: Stabiliteit van het systeem
Uit figuur 3.4 blijkt dat de robot het referentietraject goed volgt en dat het systeem voor het geteste traject stabiel is. De oplossing van het trajectvolgingsprobleem ligt in het naar nul laten convergeren van de foutco¨ordinaten zoals eerder deze paragraaf is gesteld. In figuur 3.5 is de fout (xr − x, yr − y, θr − θ) tegen de tijd weergegeven. Bij de begincondities zoals opgegeven in (3.12) is te zien dat de fout na 15 seconden naar nul is gebracht. In figuur 3.5 is rond de 7 seconden echter een piek waar te nemen in de hoekfout θr − θ. De verklaring hiervoor ligt in het gebruik van de arctan. Zoals gezegd geldt θ [−π, π] wat wil zeggen dat de waarde π niet overschreden kan worden. Wanneer θ > π heeft dit dus een van −2π tot gevolg. Deze sprong met oneindige steilheid is niet te volgen voor de robot wat leidt tot de piek in hoekfout θr − θ. Verder kan er wel geconcludeerd worden dat de ontworpen regelaar stabiel is en naar behoren presteert.
13
Figuur 3.5: Positie- en hoekfout bij regelaar (3.9)
14
3.4
Parkeren
Een punt dat extra aandacht verdient in het trajectvolgings probleem is het parkeren van een robot, wat wil zeggen het tot stilstand brengen op een gewenst punt met een gewenste ori¨entatie. Dit zal met het regelsysteem (3.9), dat is ontworpen voor het volgen van een bewegend punt, niet zomaar lukken. Dit wordt bewezen in Appendix 6.1. Deze paragraaf beschrijft een methode waarop een robot ’geparkeerd’ kan worden. Deze methode is gebaseerd op het werk van Evers (2005) zoals aangegeven in de Bibliografie. Na het ontwerp worden de resultaten getoond aan de hand van simulaties in Matlab/Simulink.
Ontwerp regelaar In de vorige paragraaf diende de robot een referentietraject (xr , yr , θr ) te volgen. Bij het probleem een robot te laten parkeren is het doel de robot tot stilstand te brengen op een gewenst punt Q met een gewenste orientatie. De co¨ordinaten van punt Q worden (xQ , yQ , θQ ) genoemd en zijn dus constant. Het idee achter de parkeermethode is de robot eerst in een rechte lijn met hoek θQ naar punt Q te laten rijden, zodat ye en θe naar nul kunnen worden gebracht. Als deze rechte lijn eenmaal bereden wordt is het enkel nog zaak de fout xe te regelen, zoals te zien is in onderstaande figuur. De rechte lijn, het hulptraject met instelbare lengte L, wordt verkregen met gebruik van
Figuur 3.6: Parkeermethode
de co¨ordinaten (xQ , yQ , θQ ) en gegeven door de vergelijkingen:
xr,Q −L cos θQ cL cos θQ xQ yr,Q = K −L sin θQ cL cos θQ 1 + yQ t θr,Q 0 0 θQ
15
(3.13)
waarbij t staat voor tijd en de constante c gebruikt wordt om de snelheid van het traject in te stellen. Over de constante K later meer. Vanwege het feit dat het constante referentiepunt (xQ , yQ , θQ ) met vergelijking (3.13) omgezet is in een te volgen traject kan bij toepassing van deze parkeermethode dezelfde regelaar gebruikt worden voor de trajectvolging als besproken in vorige paragraaf, namelijk: ω = ωr + k1 θe + k2 ye v = vr + k3 xe + k4 ye
k1 > 0, k2 > 0 k3 > 0, k4 > 0
(3.14)
Echter hiermee is de robot nog niet op het gewenste punt Q tot stilstand gebracht, daar het hulptraject (xr,Q , yr,Q , θr,Q ) geen eindpunt bevat. Wanneer de robot zich bevind op het traject en de fout ye en θe nagenoeg nul is zal het over moeten schakelen van het volgen van het hulptraject (xr,Q , yr,Q , θr,Q ) naar het positioneren op het referentiepunt (xQ , yQ , θQ ). Hierin speelt de constante K een rol. Er geldt namelijk:
K=
1,
∀
0,
∀
ye > z ∧ θ e > z (3.15) ye < z ∧ θ e < z
met de constante z een getal zeer dicht bij nul. Als de robot zich goed heeft opgesteld ten opzichte van punt Q, wat wil zeggen ye < z ∧ θe < z, dan vervalt de eerste term in (3.14) door het nul worden van constante K. Als referentie blijft over het referentiepunt Q. Met de term k3 xe uit regelaarvergelijking (3.15) kan nu de positie van de robot over de rechte lijn geregeld worden totdat de robot op het gewenste punt tot stilstand is gebracht. De simulatieresultaten van Matlab/Simulink zullen in het volgende deel getoond worden.
16
Simulaties Het regelsysteem met regelaarvergelijkingen (3.14) zoals ontworpen in deze paragraaf zal in dit deel worden getest met behulp van Matlab/Simulink. Het bijbehorende Simulink model is te vinden op de cd-rom. De regelaarvergelijkingen nogmaals: ω = ωr + k1 θe + k2 ye v = vr + k3 xe + k4 ye
k1 > 0, k2 > 0 k3 > 0, k4 > 0
(3.16)
De co¨ordinaten van referentiepunt Q zijn gekozen als (4, 6, 14 π). Gebaseerd op deze co¨ordinaten wordt het hulptraject bepaald, gegeven door de vergelijking (3.13) met constanten L en c gesteld op respectievelijk 5 en 0,05. Voor constante z wordt een getal dicht bij nul gekozen, namelijk 0,001. De regelaarwaarden worden hetzelfde gehouden als in de vorige paragraaf:
k1 k2 k3 k4
T
=
20 10 0, 3 0, 3
T
(3.17)
Ook de begincondities zijn gelijk en worden gegeven door de ingestelde initi¨ele fout van:
xe ye θe
T 0
=
−30 −40 0
De resultaten zijn weergegeven in figuren 3.7 en 3.8.
17
T
.
(3.18)
Figuur 3.7: Prestatie van het systeem
Te zien is het referentiepunt Q en het hulptraject dat een rechte lijn vormt door het punt Q heen. De robot nadert vanuit zijn beginpositie en volgt vervolgens het hulptraject, waardoor de hoekfout θe en fout ye wordt geminimaliseerd. Dit is duidelijk te zien in figuur 3.8. De hoekfout is naar nul gebracht binnen 15 seconden terwijl de (globale) positie (x, y) nog niet gelijk is aan de referentie (xr , yr . De robot rijdt verder over de rechte lijn totdat (x, y) = (xr , yr ) waarna de robot tot stilstand wordt gebracht. Dit is gebeurd in ongeveer 20 seconden. Er kan geconcludeerd worden dat het ontworpen systeem functioneert voor het parkeren van de robot.
18
Figuur 3.8: Prestatie van het systeem
19
3.5
Master-slave synchronisatie
Nadat trajectvolging voor ´e´en robot is besproken kan er worden gekeken naar de co¨operatie tussen meerdere robots. In deze paragraaf wordt er gekeken naar het regelsysteem dat gebruikt gaat worden om de robots te synchroniseren volgens het master-slave principe. Hierna worden de resultaten van de simulaties getoond.
Master-slave synchronisatie principe Er zijn verschillende manieren waarop robots gesynchroniseerd kunnen worden. De methode die toegepast wordt in deze paragraaf is de master-slave synchronisatie. Bij dit principe spelen niet alle robots een gelijkwaardige rol: in een groep robots zal er ´e´en robot aangewezen worden die tot master wordt verheven. De overige robots worden dan de slaves genoemd. De master robot krijgt de beschikking over het referentietraject (xr , yr , θr ) en zal daardoor ook moeten beschikken over positie-informatie van zijn eigen globale co¨ordinaten (xM , yM , θM ). Het is de taak van de master robot het gegeven traject te volgen. De overige slave robots hebben geen informatie over het referentietraject ter beschikking en ook niet over hun globale posities. W´el over hun globale ori¨entatie. In hun af te leggen traject zijn ze geheel afhankelijk van de positie van de master robot. Deze zullen ze moeten volgen en wel met vaste afstanden tot de master robot zodat er een formatie ontstaat met een vaste vorm.
Figuur 3.9: Master-slave synchronisatie
20
Ontwerp regelaar Zoals duidelijk is geworden in het vorige deel is de af te leggen weg van de slave robot geheel afhankelijk van het traject die de master aflegt en beschikt de slave niet over informatie over zijn eigen positie (xs , ys ) in het globale co¨ordinatenstelsel, enkel over zijn ori¨entatie θs hierin. Als referentie gebruikt de slave de onderlinge afstand (∆x, ∆y) tussen zichzelf en de master en het verschil in de hoek ∆θ, zoals gedefinie¨erd hieronder in figuur 3.3.
Figuur 3.10: Co¨ordinaten punt Q
Het punt Q ligt op een vaste positie vastgelegd door (r1 , r2 ) in het co¨ordinatenstelsel van de master, dat meedraait met de hoek θM . Dit is het punt dat de slave zal moeten volgen, wat wil zeggen dat het pad dat punt Q aflegt als referentietraject voor de slave zal dienen. De co¨ordinaten (∆xQ , ∆yQ ) van punt Q, geprojecteerd op het assenstelsel van de slave (∆x, ∆y), worden gegeven door:
∆xQ cos θM ∆yQ = sin θM ∆θQ 0
− sin θM cos θM 0
0 r1 ∆x 0 r2 + ∆y −1 θS θQ
(3.19)
met θM = θS + ∆θ
21
(3.20)
De oplossing van het synchronisatieprobleem ligt in het naar nul laten convergeren van deze co¨ordinaten (∆xQ , ∆yQ , ∆θQ ). De methode is analoog aan die van de trajectvolging besproken in de vorige paragraaf, waarbij de fout-co¨ordinaten (xe , ye , θe ) worden vervangen door de verschilco¨ordinaten (∆xQ , ∆yQ , ∆θQ ). Hiermee komen de regelaarvergelijkingen eruit te zien als: ω = ωQ + k5 ∆θQ + k6 ∆yQ v = vQ + k7 ∆xQ + k8 ∆yQ
k5 > 0, k6 > 0 k7 > 0, k8 > 0
(3.21)
Wat opvalt is dat de referentiesnelheden vQ , ωQ en verschilhoek ∆θQ nog niet bekend zijn. Differentiatie van de verkregen termen (∆xQ , ∆yQ ) en projectie hiervan op de globale co¨ordinaatassen geeft een horizontale en vertikale snelheidscomponent, relatief ten opzichte van de slave. Hierbij opgeteld de snelheidscomponenten van de slave levert de globale snelheidscomponenten van de master, zoals weergegeven in figuur 3.9:
vx vy
=
cos θS − sin θS sin θS cos θS
∆x˙ Q ∆y˙ Q
+
cos θS sin θS
vS
(3.22)
Hieruit kan de hoek θQ gevonden worden door: θQ = arctan(
vy ) vx
(3.23)
en zodanig ook ωQ met: ωQ = θ˙Q
(3.24)
Uitgaande van een voorwaarts gerichte snelheid kan hieruit ook de referentiesnelheid vQ afgeleid worden, met:
vQ =
q vx2 + vy2 .
(3.25)
Hiermee zijn alle termen in de regelaarvergelijkingen (3.21) bekend en kan het systeem getest worden.
22
Figuur 3.11: Snelheidsvectoren vQ , ∆vQ en vS
23
Simulaties Net als in de vorige paragraaf zal het ontworpen regelsysteem getest worden in Matlab/Simulink. Hiervoor is een extra Simulink model opgesteld dat te vinden is op de cd-rom. Ter verbetering van het overzicht zijn de twee regelsystemen (3.9) en (3.21) nogmaals onder elkaar gezet. Allereerst het regelsysteem van de master robot: ω = ωr + k1 θe + k2 ye v = vr + k3 xe + k4 ye
k1 > 0, k2 > 0 k3 > 0, k4 > 0
k5 > 0, k6 > 0 k7 > 0, k8 > 0
(3.26)
en voor de slave robot: ω = ωQ + k5 ∆θQ + k6 ∆yQ v = vQ + k7 ∆xQ + k8 ∆yQ
(3.27)
Het testtraject is hetzelfde als in vorige paragraaf, gegeven door vergelijkingen (3.10). In deze simulatie zal er getest worden met drie robots: ´e´en master robot en twee identieke slave robots. De regelaarwaarden ki zijn gekozen als:
k1 k2 k3 k4
T
=
20 10 0, 3 0, 3
T
(3.28)
voor de master robot en voor de slaves robots zijn gelijke waarden aangenomen:
k5 k6 k7 k8
T
=
20 10 0, 3 0, 3
T
(3.29)
De begincondities worden gegeven door de ingestelde initi¨ele fout van:
xe,M ye,M θe,M −30 −40 xe,S1 ye,S1 θe,S1 = −30 40 30 40 xe,S2 ye,S2 θe,S2
0 0 0
(3.30)
De plaats in de formatie Qi voor de i-de slave robot is vastgelegd door:
r1,S1 r2,S1 r1,S2 r2,S2
T
24
=
0 −2 0 2
T
.
(3.31)
Figuur 3.12: Stabiliteit van het systeem
In bovenstaande figuur 3.12 is te zien hoe de slave robots vanaf hun beginposities de master robot benaderen en hun positie in de formatie innemen. Uit figuur 3.13 blijkt dat dit binnen 15 seconden is gebeurd; de fout (xr − x, yr − y, θr − θ) is dan naar nul geconvergeerd. Er kan dus gezegd worden dat het ontworpen regelsysteem stabiel is en functioneert naar behoren.
25
Figuur 3.13: Positie- en hoekfout voor de slaves bij regelaar (3.26) en (3.27)
26
3.6
Wederzijdse synchronisatie
Een volgende stap die gemaakt wordt om de robots te synchroniseren is het ontwerp van een regelsysteem dat wederzijdse synchronisatie mogelijk maakt. Dat gebeurt in deze paragraaf. Na de ontwerpfase wordt het systeem gesimuleerd en de resultaten hiervan getoond.
Wederzijdse synchronisatie principe Wederzijdse synchronisatie is een synchronisatievorm die een stap verder gaat dan het in de vorige paragraaf behandelde principe. Waar bij master-slave synchronisatie nog sprake was van een rolverdeling met een duidelijke leider en volgelingen, is bij de wederzijdse methode de rolverdeling volkomen gelijkwaardig. De robots zullen moeten samenwerken als groep en als groep bij elkaar moeten blijven terwijl ze ook allen hun referentietraject in het oog houden. Dit is wat wederzijdse synchronisatie geschikter maakt voor toepassing in de realiteit: wanneer een robot niet op koers of op snelheid kan blijven door bijvoorbeeld een verstoring of defect in een actuator, dan zullen de andere robots hun koers bijstellen zodat de formatie behouden blijft.
Figuur 3.14: Wederzijdse synchronisatie
27
Ontwerp regelaar Het principe van wederzijdse synchronisatie is na het lezen van de uitleg in het vorige deel bekend. In tegenstelling tot het behandelde master-slave principe, is de rolverdeling tussen de robots bij wederzijdse synchronisatie volkomen gelijkwaardig. Dit heeft tot gevolg dat de termen master en slave komen te vervallen en plaats maken voor de benamingen robots Ri en Rj . Het indexcijfer i geeft de beschouwde i-de robot aan en j een andere robot in een groep van n robots, dus (i = 1, ..., n) en (j = 1, ..., n, j 6= i). Een tweede verschil met het master-slave principe is dat ditmaal alle betrokken robots Ri beschikken over volledige kennis over hun eigen globale positie (xi , yi , θi ), het referentietraject (xr , yr , θr ), hun positie ten opzichte van de andere robots (∆xi→j , ∆yi→j , ∆θi→j ) en zo ook de globale positie (xj , yj , θj ) van de andere robots, volgens: ∆xi→j xi xj cos θi − sin θi 0 yj = sin θi cos θi 0 ∆yi→j + yi 0 0 1 ∆θi→j θi θj
Zie ook figuur 3.15.
Figuur 3.15: Verschil-co¨ordinaten (∆xi→j , ∆yi→j , ∆θi→j )
28
(3.32)
Bij wederzijdse synchronisatie zullen de robots enerzijds hun fout (xe,i , ye,i , θe,i ) met het referentietraject naar nul laten convergeren, anderzijds hun positiefout in de gewenste formatie minimaliseren. Dit zorgt ervoor dat de robots zich gedragen als groep en zich zullen aanpassen aan elkaar. Omdat het verkleinen van de ene fout kan leiden tot het (tijdelijk) vergroten van de andere fout, houdt wederzijdse synchronisatie altijd een bemiddeling in tussen enerzijds het te volgen referentietraject en anderzijds de synchronisatie met de n robots. Het is niet wenselijk dat elke robot op dezelfde positie rijdt maar dat er een bepaalde onderlinge afstand wordt aangehouden. Daarom dient elke robot Ri een individueel referentietraject (xr,i , yr,i , θr,i ) te krijgen. Dit traject is gebaseerd op het algemene referentietraject (xr , yr , θr ) en is zodanig gevormd dat de robots op een vaste positie ten opzichte van elkaar blijven, oftewel in een vaste formatie blijven. De vorm van de formatie dient van te voren bepaald te worden en kan in een raster worden geplaatst, zoals te zien is in figuur 3.16. Hiernais is de formatie uit te drukken in de constanten (r1,i , r2,i ) ten opzichte van een punt Qf . De positie van punt Qf wordt gevonden door de gewenste posities van de robots in de formatie (xf,i , yf,i ) te middelen:
xQ,f yQ,f
n
1X = n i
xf,i yf,i
(3.33)
Figuur 3.16: Gewenste formatie en ligging punt Qf
Nu zijn de constanten (r1,i , r2,i ) te vinden met:
r1,i r2,i
=
yi − yQ,r −xi + xQ,r
29
(3.34)
Met gebruikmaking van deze constanten kan dan het individuele referentietraject (xr,i , yr,i , θr,i ) gegeven worden door:
xr,i yr,i
=
cos θr − sin θr sin θr cos θr
r1,i r2,i
+
xr yr
(3.35)
en θr,i = arctan(
y˙ r,i ) x˙ r,i
(3.36)
Zoals gezegd speelt bij wederzijdse synchronisatie het groepsverband ook een belangrijke rol: wanneer een robot Ri afdwaalt door bijvoorbeeld een verstoring van buitenaf zullen de andere robots Rj zich moeten aanpassen aan de baan van Ri zodat de formatie bewaard blijft. Dit roept de vraag op naar een punt Q dat als referentie dient voor de formatie wanneer de robots in beweging zijn. Omdat de robots als groep bij elkaar dienen te blijven kan dit punt worden gevormd door de posities (xi , yi ) van alle robots te middelen, zodat de ligging van punt Q wordt beschreven met:
xQ yQ
n
1X = n i=1
xi yi
(3.37)
en θQ = arctan( Beschouw hiertoe ook figuur 3.17.
30
x˙ Q ) y˙ Q
(3.38)
Figuur 3.17: Ligging formatiepunten Q en Qi
Het verkregen traject (xQ , yQ , θQ ) dat de robots zullen moeten volgen is dus afhankelijk van de posities van de robots zelf: wanneer een robot Ri van zijn gewenste positie afwijkt zal na middeling punt Q van baan veranderen waarna de andere robots Rj volgen en de formatie wordt weer hersteld. Zoals het referentietraject (xr , yr , θr ) van algemene aard is, zo is het traject (xQ , yQ , θQ ) ook van algemene aard. Het individuele traject (xQ,i , yQ,i , θQ,i ) is te vinden met:
xQ,i yQ,i
=
cos θQ − sin θQ sin θQ cos θQ
r1,i r2,i
+
xQ yQ
(3.39)
en θQ,i = arctan
y˙ Q,i x˙ Q,i
(3.40)
Hiermee hebben de robots beschikking gekregen over twee referentietrajecten. Dit lijkt wellicht tegenstrijdig. Echter wanneer de formatiefout
xe,Q,i cos θi sin θi 0 xQ,i − xi ye,Q,i = − sin θi cos θi 0 yQ,i − yi . θe,Q,i 0 0 1 θQ,i − θi
(3.41)
naar nul is gebracht, geldt (xr , yr , θr ) = (xQ , yQ , θQ ). De twee trajecten zijn dan dus identiek waardoor de positie eenduidig wordt opgegeven en de twee regeldoelen elkaar normaal gesproken niet tegenwerken. Echter blijft wederzijdse synchronisatie een bemiddeling waarbij het ene
31
regeldoel boven het andere gesteld kan worden. Wanneer een robot niet in staat is op koers te blijven zal het regeldoel met de hoogste prioriteit de overhand krijgen waardoor het andere doel in mindere mate nagestreefd wordt. Wanneer de foutco¨ordinaten (3.39) en de fout met het referentietraject (xr,i , yr,i , θr,i ): cos θi sin θi 0 xr,i − xi xe,i ye,i = − sin θi cos θi 0 yr,i − yi . 0 0 1 θr,i − θi θe,i
(3.42)
beiden naar nul kunnen worden gebracht is het volgprobleem opgelost. Er kan dan gedacht worden aan een regelaar volgens de volgende vergelijkingen: ωi = ωr,i + k1,i θe,i + k2,i ye,i + k3,i ye,Q,i vi = vr,i + k4,i xe,i + k5,i ye,i + k6,i xe,Q,i + k7,i ye,Q,i
met alle constanten k > 0. De regelaar kan nu getest worden in Matlab/Simulink.
32
(3.43)
Simulaties In het vorige deel is een regelaar ontworpen voor het wederzijds synchroniseren van n robots. De regelaarvergelijkingen voor een robot Ri worden gegeven door de regelaarvergelijkingen: ωi = ωr,i + k1,i θe,i + k2,i ye,i + k3,i ye,Q,i vi = vr,i + k4,i xe,i + k5,i ye,i + k6,i xe,Q,i + k7,i ye,Q,i
(3.44)
Het gebouwde Matlab/Simulink model is wederrom te vinden op de cd-rom. De simulatie van dit systeem zal hetzelfde verlopen als bij de master-slave synchronisatie zodat de twee principes goed te vergelijken zijn. Het algemeen traject (xr , yr , θr ) wordt dan gegeven door (3.10). Met vergelijkingen (3.33) en (3.34) kan het individueel traject (xr,i , yr,i , θr,i ) worden gevonden met daarbij genomen waarden (r1,i , r2,i ):
r1,1 r2,1 r1,2 r2,2 r1,3 r2,3
T
=
0 0 0 −2 0 2
T
.
(3.45)
Ook de corresponderende regelaarwaarden k en de begincondities zullen hetzelfde worden gekozen. Allereerst de regelaarwaarden:
k1,i k2,i k3,i k4,i k5,i k6,i k7,i
T
=
20 10 10 0, 3 0, 3 0, 5 0, 5
En de begincondities wederom: xe,1 ye,1 θe,1 −30 −40 xe,2 ye,2 θe,2 = −30 40 xe,3 ye,3 θe,3 30 40
33
0 0 0
T
(3.46)
(3.47)
Figuur 3.18: Stabiliteit van het systeem
Bovenstaande figuur 3.18 toont de drie individuele referentietrajecten, waarvan het traject van robot R1 als algemeen referentietraject is gebruikt. Ook wordt het algemene traject van punt Q weergegeven. Te zien is dat de robots toenadering tot elkaar zoeken en tot hun referentietraject. Gaandeweg verschuift het traject (xQ , yQ , θQ ) van punt Q steeds dichter naar het referentietraject (xr , yr , θr ) totdat het er precies overheen valt. Op dit moment is de formatie tot stand gebracht en is de fout met het referentietraject naar nul geconvergeerd. In figuur 3.19 wordt de fout (xr,i − x, yr,i − y, θr,i − θ) tegen de tijd getoond. Er valt te zien dat de tijd die nodig is om deze fout naar nul te brengen ongeveer 20 seconden duurt, dus 5 seconden meer dan bij master-slave synchronisatie. Dit is eenvoudig te verklaren doordat de robots niet meer alleen zo snel mogelijk hun referentietraject volgen maar eerst trachten de formatie bij elkaar te brengen. De twee figuren maken duidelijk dat het regelsysteem stabiel is en goed presteert.
34
Figuur 3.19: Positie- en hoekfout bij regelaar (3.42)
35
Hoofdstuk 4 Conclusies en aanbevelingen 4.1
Conclusies
In dit verslag is gezocht naar oplossingen voor verschillende trajechtvolgings- en synchronisatieproblemen voor mobiele robots. Alvorens dit probleem aan te pakken is het van belang te weten wat de kennis is van de robot van zijn positie in de omgeving waarin hij zich bevind, daar dit mede bepaald wat voor regelaar gebruikt kan worden. Voor een trajectvolgingsprobleem dient de robot te beschikken over informatie over zijn globale positie, wat verkregen kan worden door het gebruik van een Global Positioning System of de plaatsing van bakens op de robot en in de omgeving. Bij synchronisatieproblemen is het noodzakelijk dat de robots met elkaar kunnen communiceren. Hiertoe kunnen tevens bakens worden gebruikt waarmee de posities van de robots ten opzichte van elkaar kunnen worden bepaald. Hierna is het volgprobleem behandelt. Bij het aanbod van een referentietraject (xr , yr , θr ) kan het probleem worden opgelost met een proportionele regelaar met toestandsterugkoppeling. Om een robot op een gewenst punt te laten stilstaan, oftewel te laten parkeren, kan eenzelfde regelaar worden gebruikt, mits de robot eerst via een hulptraject in een juiste baan naar het punt wordt gestuurd. Na deze verschillende vormen van trajectvolging is het probleem van synchronisatie van meerdere robots aangepakt, waarbij als doel gesteld is de robots in formatie met elkaar te laten rijden. Deze synchronisatie is gerealiseerd via twee principes: het master-slave principe en het wederzijdse principe. Bij de eerstgenoemde vorm kan een zelfde soort regelaar worden toegepast als bij de trajectvolging, met het verschil dat het af te leggen traject van de slave robots geheel afhankelijk is van de master robot. Wederzijdse synchronisatie kan verkregen worden door alle robots een gelijkwaardige rol te geven en hun posities invloed te laten hebben op het traject dat als groep afgelegd dient te worden. Hiervoor is een nieuwe regelaar bedacht, die gezien kan worden als uitbreiding op de eerder ontworpen regelaar. Het betreft wederom een proportionele regelaar met toestandsterugkoppeling.
4.2
Aanbevelingen
Alle ontworpen systemen zijn gebouwd in Matlab/Simulink en getest aan de hand van simulaties. Deze toonden aan dat de regelsystemen inderdaad in staat zijn de volgproblematiek op te
36
lossen, tenminste voor simulaties waarop getest is. In dit verslag is niet wiskundig bewezen dat de voorgestelde regelaars in alle situaties een oplossing geven voor het regelprobleem. Ook is het belangrijk op te merken dat simulaties andere resultaten zullen opleveren dan wanneer de systemen in de praktijk getest worden. In het opgestelde dynamisch model van de robot is namelijk geen rekening gehouden met factoren als wrijving, traagheden van bewegende massa’s en massatraagheidsmomenten. Ook de robuustheid van de regelaars tegen meetruis, tijds-vertragingen, input verzadiging etcetera is in de simulaties niet meegenomen. Het is dus te verwachten dat bij toepassing in de praktijk andere regelaars gebruikt dienen te worden. In een eventueel vervolgonderzoek kan bestudeert worden wat de mogelijkheden zijn bij toepassing van de ontworpen regelsystemen op auto’s en schepen, gezien het feit dat hun dynamica vergelijkbaar is met die van een mobiele ’unicycle’ robot. Zo zou trajectregeling toegepast kunnen worden om onbemande transportvoertuigen aan te sturen over een afgeschermd terrein of om een schip een haven in te leiden.
37
Hoofdstuk 5 Bibliografie S. P. M. Noijen, P. F. Lambrechts en H. Nijmeijer, ”An observer-controller combination for a unicycle mobile robot”, in International Journal of Control vol. 78, No 2, 2005 F. Kamalizadeh, ”Formation and Trajectory Control of multiple dynamical systems”, DCT 2005.152, Eindhoven, 2005 A. Rodriquez-Angeles en H. Nijmeijer, ”Mutual synchronization of Robots via Estimated State Feedback: A Cooperative Approach”, in IEEE Transactions on control systems technology,vol. 12, No 4, 2004 A. A. J. Lefeber, ”Tracking control of nonlinear mechanical systems”, Twente, 2000 W. J. Evers, ”Practical stabilization of a mobile robot with n trailers using saturated control”, DCT Report No 2005.77, Eindhoven, 2005
38
Hoofdstuk 6 Appendix 6.1
Regelbaarheid voor parkeren
In deze bijlage volgt een bewijs waaruit blijkt dat de regelaar (3.9), ontworpen om robots een referentietraject te laten volgen, niet in staat is een robot te laten parkeren. Het robotmodel wordt gegeven door de vergelijkingen: x˙ = v cos θ y˙ = v sin θ θ˙ = ω
(6.1)
Dit systeem kan in de state-space vorm gebracht worden: ~x˙ = A~x + B~u
(6.2)
waarbij geldt:
x˙ 1 ~x˙ = x˙ 2 = x˙ 3
x˙ x1 x y˙ , ~x = x2 = y x3 θ θ˙
en
~u =
u1 u2
=
v ω
(6.3)
Dan kan het systeem worden geschreven als:
x˙ u1 cos x3 y˙ = u1 sin x3 u2 θ˙
(6.4)
Bij het parkeren wordt de robot stilgezet op een stationair punt. Er geldt dus u ¯ = 0 en dusdanig ˙ ook x ¯ = 0. Wanneer er gelineariseerd wordt rondom dit punt levert dit:
39
0 0 0 ∂f = 0 0 0 , A= ∂x x¯,¯u 0 0 0
1 0 ∂f B= = 0 0 ∂u x¯,¯u 0 1
(6.5)
Met een toestand van het systeem n = 3 wordt de regelbaarheidsmatrix gelijk aan:
P =
B AB A2 B
1 0 0 0 0 0 = 0 0 0 0 0 0 0 1 0 0 0 0
De rang van P is 3 < n = 4 wat betekent dat het proces niet volledig regelbaar is.
40
(6.6)
6.2
Simulink modellen
In deze bijlage worden enkele Simulink schema’s getoond. Vanwege de complexiteit van de gebouwde systemen zijn ze niet volledig weergegeven maar zijn ze wel bijgevoegd op de cd-rom. Hier zijn tevens filmpjes van de simulaties op te vinden.
Figuur 6.1: REFERENTIETRAJECT
41
Figuur 6.2: ALGEMEEN MODEL VAN DE ROBOT
42
Figuur 6.3: MASTER-SLAVE SYNCHRONISATIE
43
Figuur 6.4: WEDERZIJDSE SYNCHRONISATIE 44
