Leer een tweebenige robot lopen

Leer een tweebenige robot lopen

T.J. Melief DC 2012.005

Bachelor Eindproject Identiteitsnummer: 0656886 Begeleider:

Ir. Zutven, P.W.M.

Eindhoven University of Technology Department of Mechanical Engineering Dynamics & Control Eindhoven, januari, 2012

Inhoudsopgave 1

Opdrachtomschrijving

5

2 Inleiding 3

7

Het algoritme 3.1 Modellering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Stabiliteit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 De FPE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9 9 10 11

4 Absolute oriëntatie bepaling 4.1 De webcam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 De inclinometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Conclusie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

13 13 15 17

5

. . . .

19 19 20 22 24

6 Experimentele analyse 6.1 Experiment 1: een vaste hoek ϕ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Experiment 2: een stap zetten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

25 25 26

7

29

Implementatie op de experimentele opstelling 5.1 Het model . . . . . . . . . . . . . . . . 5.2 De variabelen en constanten . . . . . . . 5.3 Het Simulink schema . . . . . . . . . . 5.4 Aansluiten van de robot . . . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

Conclusie

A Posities, constanten en variabelen nodig voor de FPE A.1 Posities . . . . . . . . . . . . . . . . . . . . . . A.2 Posities . . . . . . . . . . . . . . . . . . . . . . A.3 De hoogte . . . . . . . . . . . . . . . . . . . . . A.4 Hoeken . . . . . . . . . . . . . . . . . . . . . . A.5 Snelheden . . . . . . . . . . . . . . . . . . . . A.6 Traagheid om het massamiddelpunt . . . . . .

. . . . . .

33 33 33 34 34 34 35

B Simulinkschema B.1 Het ingangssignaal omzetten naar graden . . . . . . . . . . . . . . . . . . . . . . . . B.2 De berekening van γ, θ en  . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B.3 Het States blok . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

37 37 37 38

3

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

C Matlab code voor het traceren van de markers C.1 patternmatching.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . C.2 videopatternget_template.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . C.3 videopatternplots.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

41 41 45 47

D Inclinometer NA4-70

49

E Symbolenlijst

53

4

Hoofdstuk 1

Opdrachtomschrijving Menselijke robots zullen een onderdeel worden van ons dagelijkse leven. Onderzoekers hebben al dergelijke robots ontwikkeld met verschillende menselijke kenmerken, omdat deze bedoeld zijn om verschillende taken in de industrie, het huishouden, de gezondheidszorg, et cetera van mensen over te nemen. Het menselijke uiterlijk wordt gezien als de meest gangbare connectie tussen mensen en robots. Binnen de Dynamics and Control Group van de Technische Universiteit Eindhoven wordt een menselijke robot ontwikkeld door team EINDroid [1]. Team EINDroid is onderdeel van een Nederlands initiatief om deze robots te maken [1, 2]. Het initiatief kent verschillende uitdagingen voor onderzoek: modelleren, dynamische analyses, en het regelen van robotmechanismen met een groot aantal graden van vrijheid. Op dit moment werkt team EINDroid aan drie robots: Tulip, Nao en Stickman (zie Figuur 1.1).

(a) TUlip

(b) Nao

(c) Stickman

Figuur 1.1 Dit project omvat de Stickman robot. Stickman is de meest simpele tweebenige robot denkbaar. De robot heeft twee paar benen, waardoor hij niet zijwaarts om kan vallen. In de heup zit een actuator waarmee de hoek tussen de benen aangestuurd kan worden. De hele opstelling is operationeel en de robot kan zowel passief van een hellend vlak als actief over een vlakke tafel lopen [1, 2, 4]. In de groep Dynamics and Control is een nieuw loopalgoritme ontwikkeld gebaseerd op de energiebalans van de robot. Dit algoritme werkt in simulatie en moet worden gedemonstreerd op een 5

fysieke robot. Het doel van dit onderzoek is implementatie van een nieuw loopalgoritme waarmee de Stickman over een vlakke tafel kan lopen. Dit project kent een aantal uitdagingen. Als eerste moeten parameters zoals massa’s en lengtes geschat worden. Vervolgens moet een extra sensor geïmplementeerd worden die de absolute hoek tussen de robot en de grond meet. Hiervoor wordt gedacht aan de Xsense gyroscoop of een camera systeem. Beide zijn beschikbaar. Tot slot moet het algoritme geïmplementeerd en gedemonstreerd worden. De resultaten van dit onderzoek kunnen in de toekomst gebruikt worden om ook meer geavanceerde robots zoals Tulip of Nao efficiënt te laten lopen.

6

Hoofdstuk 2

Inleiding In dit project wordt gekeken of een robot, de Stickman, geleerd kan worden te lopen aan de hand van het Foot Placement Estimator (FPE) loop-algoritme. Dat de Stickman nog moet "leren" lopen, wil niet zeggen dat ze nog niet kan lopen. Ze is namelijk zo ontworpen dat ze passief van een hellend vlak naar beneden kan lopen. Daarna is deze beweging geanalyseerd. Vervolgens is met behulp van een elektrische actuator deze beweging als aansturing gebruikt. De robot kan dus ook actief over een horizontaal vlak lopen [3, 4]. Hier moet wel bij vermeld worden dat de aansturing van te voren bedacht is. De robot reageert dus niet op verstoringen. Met dit loop-algoritme is het de bedoeling dat ze dat wel doet. Lopen kan gezien worden als elke keer één stap zetten. Dat is wat er in dit project onderzocht wordt. De onderzoeksvraag is dan ook: is het mogelijk om met behulp van de Foot Placement Estimator de Stickman een stap te laten zetten? Als het algoritme op deze robot toepasbaar is, dan kan dit wellicht gebruikt worden om een meer geavanceerde robot te laten lopen. Als eerste zal in Hoofdstuk 3 het algoritme worden uitgelegd. Daarna wordt er in Hoofdstuk 4 gekeken hoe de noodzakelijke hoeken gemeten gaan worden om zo de oriëntatie van de robot te achterhalen. In Hoofdstuk 5 wordt vervolgens uitgelegd hoe het algoritme wordt toegepast op de Stickman en hoe de link met de PC wordt gemaakt. In Hoofdstuk 6 worden de experimenten en de daarbij gevonden resultaten geanalyseerd. Als laatste volgt de conclusie.

7

8

Hoofdstuk 3

Het algoritme Het loopalgoritme dat moet worden geïmplementeerd is de Foot Placement Estimator. Het is gebaseerd op de energiebalans van de robot. Dit algoritme zoekt de plek waar de robot zijn been moet zetten, zodanig dat er een balans-situatie ontstaat die instabiel is. In dit hoofdstuk volgt een korte uitleg van deze FPE. Voor een uitgebreidere uitleg en voor de afleidingen wordt verwezen naar [6].

3.1

Modellering

De FPE is gebaseerd op het volgende model: de Stickman bestaat uit twee massaloze benen, die samenkomen in het massamiddelpunt met een bepaalde massa m en een traagheid om datzelfde punt Icom . De benen hebben een lengte L en de hoek tussen de benen is γ0 (zie Figuur 3.1). Voor de afleiding is er vanuit gegaan dat deze hoek constant is. θ

m, ICOM

L γ0

A

B

Figuur 3.1: Model van de Stickman Op het moment dat de Stickman beweegt en er één voet (bijvoorbeeld voet A) op de grond staat kan dit gezien worden als een omgekeerde slinger en geldt (3.1). Hierin is τA het koppel, IA het traagheidsmoment om punt A en θA hoek die poot A met de verticale as maakt. Als deze vergelijking wordt ingevuld (3.2) kan aan de hand hiervan de hoekversnelling θ¨A van poot A worden berekend (3.3). X

τA = IA θ¨A 9

(3.1)

mgL sin(θA ) = (ICOM + mL2 )θ¨A

(3.2)

mgL sin (θA ) θ¨A = Icom + mL2

(3.3)

Als de stickman met de klok mee beweegt zal op een gegeven moment de andere voet (voet B) de grond raken. Na deze impact zal voet A de lucht in gaan. De snelheid van de robot direct na impact θ˙2 is met behulp van het behoud van impulsmomenten (Hi )1 en (Hi )2 te bepalen als functie van de snelheid vlak voor impact θ˙1 (zie (3.4), (3.5) en (3.6)). De hoekversnelling van poot B θ¨B kan vervolgens op dezelfde manier als die van poot A bepaald worden (3.7). (Hi )1 = (Hi )2

(3.4)

mL(Lθ˙1 ) cos(γ0 ) + ICOM θ˙1 = mL(Lθ˙2 ) + ICOM θ˙2

(3.5)

(L m cos (γ0 ) + Icom )θ˙1 θ˙2 = L2 m + Icom

(3.6)

mgL sin (θB ) θ¨B = Icom + mL2

(3.7)

2

Om deze vergelijkingen samen te kunnen voegen is één enkele coördinaat noodzakelijk. Vandaar dat de hoeken θA en θB als volgt worden omgeschreven naar coordinaat θ (zie Figuur 3.1): θA = θ +

γ0 2

(3.8)

γ0 (3.9) 2 Als beide poten op de grond staan is deze hoek θ gelijk aan nul. Vul deze coördinaat in vergelijkingen (3.3) en (3.7) in. Vervolgens kunnen deze worden omgeschreven naar de volgende set van bewegingsverglijkingen:  mgL sin (θ+γ0 /2)  θ<0  Icom +mL2       sin (θ−γ0 /2)   mgL θ>0  Icom +mL2     mgL sin (θ+γ0 /2) θ¨ = (3.10) θ = 0, θ˙ < 0 Icom +mL2       mgL sin (θ−γ0 /2)   θ = 0, θ˙ > 0  Icom +mL2       0 θ = 0, θ˙ = 0 θB = θ −

Hierbij kan de initiële snelheid bepaald worden aan de hand van (3.6).

3.2

Stabiliteit

Nu de bewegingsvergelijkingen bekend zijn kan er gekeken worden naar de stabiliteit. Om dit te kunnen doen zijn de volgende drie definities nodig: • De Stickman is gevallen als θ˙ = 0 en elk punt anders dan de voeten raakt de grond 10

• De Stickman is gebalanceerd als θ˙ = 0 en de stickman is niet gevallen • De Stickman is stabiel als, voor een gegeven verzameling van initiële condities en geen andere invoer van energie, zij uiteindelijk tot rust komt in een staande positie. Als ze tot rust gekomen is zal een voldoende kleine, impulsieve, externe verstoring resulteren in een beweging, maar zal de Stickman uiteindelijk weer tot dezelfde stabiele, gebalanceerde positie komen.

Figuur 3.2: De onderbroken lijn is het pad dat de COM volgt. Er zijn drie stabiele regio’s die hieronder gedefinieerd zullen worden.

3.2.1

Stabiele Regio 1.

Als de initiële hoek θ0 zodanig is dat −γ0 /2 < θ0 < γ0 /2 (het witte gedeelte in Figuur 3.2) en de totale energie in het systeem lager is dan de piek van de potentiële energie (bij de maximale hoogte hpeak ), staat de Stickman stabiel.

3.2.2

Stabiele Regio 2.

Als ook rekening gehouden wordt met de energie die verloren gaat bij de impact met het loopvlak, mag de totale energie hoger zijn. Voor de initiële hoek θ0 geldt nog steeds −γ0 /2 < θ0 < γ0 /2.

3.2.3

Stabiele Regio 3.

Voor situaties waar θ0 buiten het gebied −γ0 /2 < θ0 < γ0 /2 (het grijze gebied in Figuur 3.2) ligt, moet de totale energie in het systeem hoog genoeg zijn om de eerste piek van de potentiële energie te overwinnen. Daarna moet er genoeg energie verloren gaan bij de impact, zodat de totale energie dan lager is dan de piek van de potentiële energie. De Stickman zal terugvallen en uiteindelijk stabiel op zijn poten komen te staan.

3.3

De FPE

De Foot Placement Estimator schat de plek waar de robot zijn voet neer moet zetten, zodanig dat deze in een gebalanceerde, instabiele situatie eindigt (zie Figuur 3.3). Als de voet iets dichter bij de robot wordt gezet, zal deze naar voren vallen. Wordt de voet iets verder weg gezet, dan zal de robot terug naar achter bewegen en weer op beide poten terecht komen. Het blijkt dat dit evenwicht exact bereikt wordt als de robot neerkomt met hoek ϕ als oplossing van (3.11): 11

Figuur 3.3: Het model van de stickman zet zijn voet (a) dichterbij en valt om. (b) De voet wordt verder weg van de robot gezet. De robot eindigt rechtopstaand in een stabiele positie. (c) De voet wordt op de FPE gezet en eindigt in een perfecte instabiele, gebalanceerde situatie.

[mh(vx cos (ϕ) + vy sin (ϕ)) cos (ϕ) + ICOM θ˙1 cos2 (ϕ)]2 + 2mgh cos (ϕ)(cos (ϕ) − 1) = 0 (3.11) mh2 + ICOM cos2 (ϕ) In Figuur 3.4 staat de betekenis van alle in te voeren waarden. θ1

m, ICOM vx vy

v h

φ

1

L

2

Figuur 3.4: Betekenis van alle in te voeren waarden van (3.11)

12

Hoofdstuk 4

Absolute oriëntatie bepaling Om de FPE toe te kunnen passen op de Stickman is het nodig dat de hoek tussen de twee benen, hoek γ, en de hoeksnelheid θ˙ bekend zijn (zie Figuur 4.1). De huidige Stickman heeft geen mogelijkheid deze hoeken te meten. Er zijn verschillende manieren om deze hoeken te weten te komen. In dit project zijn twee manieren onderzocht: de hoeken worden gemeten met behulp van een webcam (manier 1) en met behulp van twee hoekverdraaiing meters (manier 2). De volgende eisen worden gesteld: de hoekbepaling moet op de graad nauwkeurig zijn en de hoek moet met een frequentie van minimaal 100 Hz gesampled kunnen worden. Aan het einde zal aan de hand van deze twee eisen een vergelijking gemaakt worden tussen de twee manieren.

4.1

De webcam θ

(e,f)

½γ ½γ (c,d) (a,b)

Figuur 4.1: Model van de stickman

4.1.1

De markers traceren

De eerste manier om de oriëntatie van de Stickman ten opzichte van de verticale as te vinden is met behulp van een webcam. Het idee is om markers aan het eind van beide poten, waar ze de grond raken, te bevestigen. In Figuur 4.1 is dit op de coördinaten (a, b) en (c, d). Als de webcam 13

deze markers traceert en met behulp van een Matlab-script de coördinaten hiervan bekend worden, kunnen de hoeken γ en θ berekend worden. Om de coordinaten van de markers te vinden is een standaard implementatie van Matlab voor marker herkenning gebruikt. Deze implementatie is gebaseerd op "genormaliseerde kruiscorrelatie van het frequentie domein" (in het Engels: normalized frequency domain cross correlation). In het Matlab script van deze implementatie worden één of meer patronen uit een film getraceerd. Dit is zo aangepast dat er uit live beeld, twee patronen (markers) getraceerd kunnen worden.

4.1.2

Bepaling van de hoeken θ en γ

Als de coördinaten van de markers bekend zijn, kunnen de hoeken ϕ en γ berekend worden. Hiervoor is ook een stukje code aan het Matlab script toegevoegd. In het begin, als de stickman met beide poten op de grond staat, wordt er gekeken naar de posities van de markers. Noem de coordinaten van deze posities (a0 , b0 ) en (c0 , d0 ). De lijn tussen de markers zou nu precies horizontaal moeten zijn. Naar alle waarschijnlijkheid is dit, aangezien de markers niet met voldoende precisie op gelijke hoogte bevestigd kunnen worden, niet het geval. Hier wordt rekening mee gehouden in de code. De lijn tussen deze coördinaten wordt gelijk gesteld aan de vector ~h: deze lijn wordt als horizontaal verondersteld. De vector die loodrecht op ~h staat is de verticale as ~v .   ~h = c0 − a0 (4.1) d0 − b0   b0 − d0 ~v = (4.2) c0 − a0 Hoek θ is de hoek tussen ~r en ~v (zie (4.3) en Figuur 4.1). Op het moment dat een van beide poten de lucht in gaat en (a, b) en (c, d) getraceerd worden, kan deze hoek θ berekend worden (4.4). Vervolgens kunnen ook de coördinaten van het draaipunt (e, f ) berekend worden (4.5) en (4.6).   b−d ~r = (4.3) c−a   ~v · ~r (4.4) θ = arccos |~v ||~r| s r a+c a+c b+d e= − L− ( − a)2 + ( − b)2 sin (θ) (4.5) 2 2 2 s r b+d a+c b+d f= + L− ( − a)2 + ( − b)2 cos (θ) (4.6) 2 2 2 Zie de poten van de robot als de vectoren l~1 en l~2 ((4.7) en (4.8)). De hoek γ tussen deze twee lijnen kan dan berekend worden volgens (4.9).   e−a l~1 = (4.7) f −b   e−c l~2 = (4.8) f −d ! l~1 · l~2 γ = arccos (4.9) |l~1 ||l~2 | 14

4.1.3

Het resultaat

Figuur 4.2: Screenshot van de lopende m-file Na de aanpassingen die hierboven genoemd zijn is het Matlab-script geschreven dat te vinden is in Bijlage C. Met dit Matlab-script kunnen twee markers gevolgd worden. Eerst is dit geprobeerd met een videoformaat van 640×480. De posities van de markers worden bij dit formaat nauwkeurig getraceerd (zie Figuur 4.2). Probleem is echter dat dit veel te langzaam gaat: er is meer dan een seconde vertraging. Aangezien de robot vrij snel beweegt is dit een groot probleem. Wordt vervolgens het video-formaat verkleind naar 320×240, dan is de vertraging wel minder, maar is deze nog steeds meer dan een halve seconde (wat nog steeds veel te veel is). Bovendien wordt het beeld dan zo onscherp dat de markers niet meer goed getraceerd worden. Niet alleen de code is te langzaam. Ook de gebruikte webcam is langzaam. Er worden 30 beelden per seconde gemaakt [7]. De eis is minimaal 100 keer per seconde.

4.2

De inclinometer

De tweede manier om de oriëntatie van de robot te weten te komen is met behulp van twee hoekverdraaiing meters. Hiervoor zijn twee inclinometers gebruikt (zie Bijlage D). Deze sensoren worden op beide poten aangebracht, op de manier die te zien is in Figuur 4.3 . Zo zijn de hoeken die de poten maken met de verticale as bekend. Met behulp van een paar geometrische formules is het berekenen van de hoeken en coördinaten van het systeem niet zo moeilijk meer (zie hiervoor Hoofdstuk 5).

Sensor

a Sensor

b

Figuur 4.3: De robot met de inclinometers a en b. 15

4.2.1

Aansluiten op de pc en Matlab/Simulink

Een voordeel van deze sensoren is dat ze via een TUe/DAc aan de pc te koppelen zijn. Dit komt goed uit, aangezien de actuator op de robot ook via een dergelijk kastje wordt aangestuurd. De TUe/DAc kan met een frequentie van 1000 Hz samplen. Voor de TUe/DAc is een koppeling met Simulink beschikbaar (zie Figuur 4.4). Hier is dan het signaal van de sensoren uit te lezen.

Figuur 4.4: De koppeling met de TUe/DAc in Simulink

4.2.2

Kalibratie

De output van de sensor is als volgt: Bij 0.5V maakt de sensor een hoek van −70◦ met de richting van de zwaartekracht. Bij een output van 4.5V is deze hoek 70◦ (zie Bijlage D en Figuur 4.5a). Om als output graden te krijgen wordt hierna het input signaal verminderd met 2.5 en vermenigvuldigd met 35 (zie Simulinkschema in Figuur 4.5b).

(a) De verhouding tussen input in Volt en output in graden

(b) Het Simulink-schema

Figuur 4.5

4.2.3

De hoeken θ en γ berekenen

Als de hoeken α en β bekend zijn, kunnen de hoek tussen de benen γ en de hoek die de robot maakt met de verticale as hoek θ berekend worden. Dit gebeurd als volgt: γ =α−β

(4.10)

θ = α − γ/2

(4.11)

16

4.2.4

Het resultaat

De sensoren zijn zeer nauwkeurig. Ook als de sensoren met grote snelheid worden gedraaid. Er zit wel een klein beetje ruis op het signaal (zie 4.6), maar dat kan worden weggefilterd. Aangezien de sensoren op de TUe/DAc worden aangesloten, kan de de informatie met 1000 Hz worden gesampled. Dit is voldoende voor dit algoritme.

Figuur 4.6: Ruis op het input signaal

4.3

Conclusie

Voor het vervolg van dit project is er voor de hoekverdraaiing sensoren gekozen. De marker-detectie met de webcam en met deze code is geen optie, omdat de geschreven Matlab-code en de webcam te langzaam zijn. Het zou wellicht kunnen dat met een andere code de hoeken wel snel genoeg te meten zijn, maar dan nog zijn de 30 frames per seconde die de webcam binnenhaalt vrij minimaal. De inclinometers werken wel goed: ze zijn snel en nauwkeurig. Hierdoor zijn ze geschikt voor dit onderzoek.

17

18

Hoofdstuk 5

Implementatie op de experimentele opstelling In dit hoofdstuk zal uitgelegd worden hoe het FPE-algoritme op deze robot zal worden toegepast. Zo wordt eerst uitgelegd hoe de stickman gemodelleerd zal worden. Ook zal worden beschreven hoe de variabelen en constanten berekend of geschat worden. Daarna zal het Simulink-schema uitgelegd worden. Om dit alles duidelijk te kunnen doen staat hieronder nog een keer de FPE-vergelijking: [mh(vx cos (ϕ) + vy sin (ϕ)) cos (ϕ) + ICOM θ˙1 cos2 (ϕ)]2 + 2mgh cos (ϕ)(cos (ϕ) − 1) = 0 (5.1) mh2 + ICOM cos2 (ϕ)

5.1

Het model

In de afleiding van het FPE-algoritme wordt een simpel model van een robot gebruikt: er is een massamiddelpunt met een bepaalde massa m en een bepaalde traagheid om het massamiddelpunt ICOM . Verder heeft de robot alleen twee massaloze benen (zie Figuur 5.1a). De in dit project gebruikte robot bestaat natuurlijk niet uit een massamiddelpunt en twee massaloze benen. Om het algoritme toch op deze robot toe te passen moet dan ook eerst het massamiddelpunt gevonden worden. Het massamiddelpunt van elke poot apart is bekend. De robot is zo ontworpen dat deze precies 10 cm onder de as van de heup ligt [3, 4]. Met behulp hiervan kan de positie van het massamiddelpunt (COM ) van de hele robot gevonden worden. Hieronder is te zien hoe de de positie van de COM en andere posities die bekend moeten zijn berekend worden. De positie van het massamiddelpunt van poot 1: p~m1 = (L1 sin(α)

L1 cos(α))

(5.2)

(L1 + L2 )cos(α))

(5.3)

p~T,y − L3 cos(β))

(5.4)

De positie van de heup: p~T = ((L1 + L2 ) sin(α) De positie van het massamiddelpunt van poot 2: p~m2 = (~ pT,x − L3 sin(β) 19

De positie van voet 1: p~v1 = (0

0)

(5.5)

p~T,y − (L3 + L4 ) cos(β))

(5.6)

De positie van voet 2: p~v2 = (~ pT,x − (L3 + L4 ) sin(β) De positie van de COM :  p~COM =

5.2

p~m1 ,x + p~m2 ,x 2

p~m1 ,y + p~m2 ,y 2



(5.7)

De variabelen en constanten θ1

θ1

m, ICOM vx vy

v h

φ

L2 m1 α

L

L3 m2 vx COM v vy γ

ε

β φ

L4

h L1 l

1

1

2 (a)

2 (b)

Figuur 5.1: In (a) de Stickman zoals die gemodelleerd is om het algoritme af te leiden. In (b) zoals de Stickman geïnterpreteerd is om het algoritme op toe te kunnen passen. In Figuur 5.1b zijn alle lengtes en hoeken die bekend moeten zijn om het algoritme toe te kunnen passen te zien. De poten hebben een lengte van 40 cm. Het massamiddelpunt van elke poot ligt precies 10 cm onder de heup. L1 is dus, net als L4 , 30 cm. L2 en L3 zijn 10 cm. Als deze lengtes en de hoeken α en β bekend zijn, zijn de hoeken ϕ, γ, θ en  te berekenen (5.8) tot en met (5.11). γ =α−β   p~v2 ,x − p~COM,x ϕ = arctan p~v2 ,y − p~COM,y α−γ 2   p~COM , x  = arctan p~COM , y θ=

20

(5.8) (5.9) (5.10) (5.11)

Ook de traagheid om het massamiddelpunt ICOM moet ingevuld worden in 5.1. Deze traagheid is om elke as verschillend. In dit geval is alleen de traagheid om de heup-as van de robot belangrijk. Dat is namelijk de enige as waar omheen gedraaid wordt. Aangezien voor elke hoek de positie van het massamiddelpunt verschillend is, is de waarde van de traagheid ook voor elke hoek γ verschillend. Deze waarde is echter niet analytisch te bepalen. Hiervoor is het CAD-model van de Stickman gebruikt (zie Figuur 5.2). De met behulp van het CAD-model berekende traagheden voor verschillende hoeken zijn te vinden in Bijlage A.6. Om het algoritme niet te ingewikkeld te maken, wordt de traagheid om het massamiddelpunt constant verondersteld. De hoek ϕ die de robot moet maken om de FPE vergelijking op te lossen wordt geschat op 20◦ . De waarde van de ICOM bij deze hoek wordt dan ook als constante ingevoerd. Deze is 7.34 ∗ 10−2 kgm2 .

Figuur 5.2: De ICOM van de robot bij 20◦ . Kijk bij de Moment of Inertia (Centroidal), Ixx. Het bepalen van de hoogte h is niet zo moeilijk: het is namelijk de hoogte van het massamiddelpunt. Deze is gelijk aan de y-coordinaat van de positie van het massamiddelpunt en dus gelijk aan (5.12). h = p~COM,y (5.12) ˙ Als laatste moeten de snelheden vx en vy en de hoeksnelheid θ berekend worden. De hoeksnelheid wordt bepaald door in Simulink het signaal van de hoek θ te differentiëren (zie Paragraaf 5.3). Vervolgens wordt deze gebruikt om de snelheden in de x- en y-richting te bepalen (zie (5.15) en (5.18)) In x richting: dx dt x = l sin(θ(t))

(5.14)

vx = θ˙ l cos(θ(t))

(5.15)

vx =

Hieruit volgt:

(5.13)

In y richting: dy dt y = l cos(θ(t)) vy =

Hieruit volgt:

vy = −θ˙ l sin(θ(t)) 21

(5.16) (5.17) (5.18)

5.3

Het Simulink schema

Om de robot real-time aan te kunnen sturen wordt deze met behulp van een TUeDAc aan de pc gekoppeld. Via Matlab/Simulink is deze koppeling makkelijk te maken. Het algoritme zal dus in Simulink gezet moeten worden. In Figuur 5.3 is te zien hoe dit is gedaan. Er is een blok waarin de Foot Placement Estimator wordt uitgevoerd, het Calculating Phi & Phi_real blok. Hier komt een volgsignaal Phi uit. Ook komt hier het real-time gemeten signaal Phi_real uit. Een regelaar stuurt vervolgens met behulp van deze twee signalen de robot aan. De robot wordt dus op ϕ geregeld. De regelaar die dit doet is rechts van het blok te zien.

Figuur 5.3: De aansturing van de robot in Simulink. In het blok Calculating Phi & Phi_real wordt het FPE algoritme toegepast. Het volgsignaal is phi, het real-time gemeten signaal is phi_real. Rechts hiervan is de regelaar te zien. In Figuur 5.4 is het Calculating Phi & Phi_real blok te zien. In1 en In2 zijn de signalen die uit de twee hoekverdraaiing meters komen. In de blokken Hoek1 en Hoek2 worden deze signalen omgezet naar graden (zie Bijlage B.1). Iets verderop in het schema staat het Gamma & Theta & Epsilon blok. Hier worden de hoeken γ, θ en  berekend (zie Bijlage B.2).

Figuur 5.4: Het Calculating Phi & Phi_real blok In Figuur 5.5a is het signaal (in graden) te zien dat uit een sensor komt. Dit signaal is gecreëerd door de robot met de hand heen en weer te bewegen. Je ziet dat hier een beetje ruis op zit (ongeveer 0.2◦ ). Verderop in het schema worden de signalen van theta en eps gedifferentieerd. Deze signalen worden gevormd uit de signalen van de twee sensoren. De ruis van deze twee inclinometers zal hier gedifferentieerd en dus versterkt worden (zie Figuur 5.5b). Om dit te voorkomen is er direct achter 22

het de binnenkomst van het signaal in graden een filter geplaatst. Dit filter is een Lowpass filter dat alle frequenties boven de ongeveer 50 Hz (300 rad/s) onderdrukt. Het blijkt dat de gedifferentieerde van het nieuwe signaal nu wel vloeiend is. In Figuur 5.6a is het effect van de filter op het inputsignaal te zien. In Figuur 5.6b zijn de hoek θ en de hoeksnelheid θ˙ hiervan te zien. Deze zijn berekend aan de hand van het gefilterde signaal.

(a) Output van de inclinometer over een kort tijdsinterval

(b) Het signaal van theta (rood) en dtheta (groen) zonder filter

Figuur 5.5

(a) Output van de inclinometer in graden met en zonder filter (b) Het signaal van theta (rood) en dtheta (groen) met filter

Figuur 5.6 Als laatste is er in Figuur 5.4 het States blok te vinden. Ook dit blok bestaat uit Matlab-code. Deze is te vinden in Bijlage B.3. In dit blok wordt het stuursignaal voor de robot gecreëerd. Dit gebeurt in stapjes (States). Zoals in de inleiding al gezegd is, wordt hier onderzocht of het mogelijk is na een duw de poot op de, volgens het PFE-algoritme, juiste plek te zetten. In dit Simulink programma wordt dan ook maar één stap gezet. Deze stap bestaat uit de volgende States: • State 0: De robot staat stil. De hoeken α en β zijn absoluut gezien gelijk aan elkaar. Zodra de robot een duw krijgt, zijn deze hoeken niet meer gelijk aan elkaar. Het programma zal nu naar State 1 springen. • State 1: De robot heeft een duw gekregen. In principe zou vanaf nu de robot bestuurd kunnen worden aan de hand van het FPE-algoritme. Dit gebeurt echter niet. Om het programma simpel te houden wordt de hoek ϕ op 3◦ ingesteld. De hoek ϕ die zal worden uitgerekend is positief. Het is dus gerechtvaardigd om dit te doen. • State 2: De robot heeft nu een hoek ϕ > 3◦ . Nu pas wordt het FPE algoritme toegepast. De vergelijking wordt iteratief opgelost. Dit gaat door totdat de hoeken α en β absoluut gezien weer gelijk aan elkaar zijn: De robot staat weer (even) met beide benen op de grond. Als het goed is met de hoek ϕ die berekend is. • State 3: De robot wordt niet meer aangestuurd. Het programma is klaar. Nu zal de robot met 23

zijn andere poot de lucht in gaan. Volgens het FPE algoritme zou de robot nu precies in een instabiele balans situatie terechtkomen. In dit blok staat bovendien ook nog een stuk code ter beveiliging van de robot. Als deze bijvoorbeeld lijkt te vallen, zal het programma stoppen. Dan wordt de actuator niet meer aangestuurd en is de robot makkelijker op te vangen.

5.4

Aansluiten van de robot

Dit project is een voortzetting van de robot die eerder al gebouwd is en waarmee eerder ook al tests uitgevoerd zijn. Hier zal dan ook geen aandacht worden besteed aan het aansturen van de motor. Hetzelfde geld voor het aansluiten van de TUe/DAc en het ’runnen’ van de Simulink-code. Hiervoor wordt verwezen naar een vorig verslag over de Stickman [4]. De twee hoekverdraaing sensoren zijn wel nieuw. Hier wordt wel aandacht aan besteed. Op elke poot wordt een hoekverdraaing sensor bevestigd. De sensoren moeten wel allebei in dezelfde richting bevestigd worden, zoals in Figuur 5.7 te zien is. De sensoren moeten zo bevestigd zijn dat ze 0◦ aangeven als de poot precies verticaal staat. Vervolgens moeten de inclinometers via een interface (Figuur 5.8) aan de TUe/DAc worden bevestigd. Ze moeten worden aangesloten op de AD1 en AD2 (Figuur 5.9).

Figuur 5.7: De manier waarom de sensoren (oranje) bevestigd moeten worden.

Figuur 5.8: De interface.

Figuur 5.9: De bevestiging aan de TUe/DAc op

24

Hoofdstuk 6

Experimentele analyse Om te kunnen zien hoe de robot reageert op het loopalgoritme moet er natuurlijk getest worden. Dit testen is verdeeld in twee fases. Als eerste wordt getest met de Stickman waarvan de hoek γ tussen de poten is vast gezet. Als tweede wordt getest of de robot een stap kan zetten aan de hand van het FPE-algoritme. De resultaten van beide experimenten zullen geanalyseerd worden.

6.1

Experiment 1: een vaste hoek ϕ

In het eerste experiment wordt de hoek tussen de poten vast gezet. De robot wordt dan op zijn achterpoten gezet en krijgt een kleine duw (zie Figuur 6.1). Het Matlab-script is zo aangepast dat het programma in State 2 begint en dat de actuator niet wordt aangestuurd. In principe is de plaats van de FPE alleen afhankelijk van de snelheid die tijdens de duw wordt meegegeven. De robot krijgt geen energie toegevoegd door de actuator. Hierdoor krijgt de robot geen ’zwiep’ mee. Dat de robot niet actief aangestuurd wordt, wil niet zeggen dat de hoek ϕ niet berekend wordt. Zo kan bepaald worden of het algoritme werkt.

Duw B

γ

A

γ

γ

A

A

B

t1

t0

B

t2

Figuur 6.1: Het eerste experiment. De hoek γ tussen de twee poten is vastgezet. In Figuur 6.1 staat op tijd t0 de robot achterover op één poot en krijgt de robot een duw. Tussen t0 en t1 wordt steeds opnieuw berekend welke hoek ϕ er gemaakt zou moeten worden om precies in een instabiele balanssituatie te eindigen. Vanaf t1 wordt het algoritme niet meer uitgevoerd en zal de robot verder bewegen. Noem de hoek die gemeten wordt hoek ϕmeting . Als op t2 de robot terug 25

valt, weet je dat de echte hoek ϕmeting groter is dan de door de FPE berekende hoek ϕ. Valt de robot verder, dan was hoek ϕmeting kleiner dan hoek ϕ. De resultaten van dit experiment zijn hieronder te zien in Figuur 6.2a en Figuur 6.2b.

(a)

(b)

Figuur 6.2: Resultaat van experiment 1. In (a) viel de robot door. In (b) viel de robot terug. Wat als eerste opvalt in beide figuren is de knik in de lijn van de gemeten hoek ϕmeting . Dit is eenvoudig te verklaren. Dit is namelijk het punt dat het massamiddelpunt zijn hoogste positie heeft. Hierna werkt de zwaartekracht mee in plaats van tegen. Hierdoor krijgt de lijn van de berekende hoek ϕ een knik. Wat ook opvalt is dat de berekende hoek ϕ niet kleiner wordt dan 4◦ . Ook dit is makkelijk te verklaren. Het algoritme is zo in Simulink geimplementeerd dat aan hoek ϕ de waarde 4◦ toegekend wordt, als deze eigenlijk lager is. In Figuur 6.2a zien we dat de hoek ϕmeting kleiner is dat de berekende hoek ϕ. De robot zou dus door moeten vallen. Dit is ook wat gebeurde. Het verschil tussen de twee hoeken van ongeveer 15◦ is erg groot. Aan de hand van dit resultaat is dus te verwachten dat de robot met flinke snelheid door viel. Dit is echter niet wat er gebeurde. De Stickman kwam namelijk maar net over zijn dode punt. Wat dat betreft is hier dus nog niet bewezen dat het algoritme de hoek ϕ goed berekend heeft. In Figuur 6.2b is wederom te zien dat de hoek ϕmeting kleiner is dat de berekende hoek ϕ. Deze keer is de robot echter terug gevallen. Eigenlijk zou de berekende hoek dus groter moeten zijn dan hoek ϕmeting . Hieruit blijkt dat de hoek ϕ die berekend wordt door het FPE algoritme niet overeenkomt met de hoek ϕ in de realiteit. De reden hiervoor zou kunnen zijn dat de gebruikte robot niet voldoende lijkt op het model dat gebruikt is voor de afleiding van de FPE-vergelijking. De robot bestaat niet uit een puntmassa met massaloze benen. Dit is een punt van nader onderzoek.

6.2

Experiment 2: een stap zetten

Als tweede deel van het experiment wordt getest of de robot een stap kan zetten. Uit het vorige experiment is gebleken dat de hoek ϕ die volgens het FPE-algoritme berekend wordt niet gelijk is aan de hoek ϕ in de realiteit. Toch wordt hier gekeken hoe de robot op het stuursignaal ϕ reageert. Het algoritme gaat er vanuit dat in initiële toestand de binnenste poot naar achter staat. In Figuur 5.7 zou de robot dus een stap naar rechts zetten. Zoals in een vorig verslag over de Stickman [4] al gezegd is, is het noodzakelijk dat de robot op blokjes staat. Aangezien deze geen knieën heeft kan de Stickman zonder niet lopen. De Simulink-simulatie wordt gestart. De robot zit nu in State 0 en zal dus stil blijven staan. Op het moment dat de stickman een duw krijgt, gaat de robot naar State 1 en State 2. Als de robot weer 26

met beide benen op de grond staat (State 3) zal de simulatie vanzelf stoppen. in Figuur 6.3 is de is de testopstelling te zien.

Figuur 6.3: De testopstelling voor experiment 2. In Figuur 6.4 is het resultaat van dit experiment te zien. Na ongeveer 4.01 seconden zie je dat de robot een hoek ϕ wil maken van 3◦ . Op dat moment zit de robot in State 1. Na 4.2 seconden begint het FPE algoritme pas te werken (State 2). Wat meteen opvalt is dat de berekende waarde van ϕ in minder dan 0.4 seconden erg varieert. De actuator kan dit signaal niet volgen. De volgende twee redenen zijn onder andere verantwoordelijk voor deze variatie van ϕ: • Op het moment dat de robot geduwd wordt, gaat zijn achterste poot zo snel mogelijk naar de 3◦ toe. Hierdoor krijgt de robot een enorme ’zwiep’. Hier is in het model geen rekening mee gehouden. • Doordat de actuator te langzaam reageert op het volgsignaal, gaat deze bewegingen maken die helemaal niet gemaakt moeten worden (bijvoorbeeld tussen de 4.40 en 4.45 seconden in Figuur 6.4).

Figuur 6.4: Resultaat van experiment 2. Een stap.

27

28

Hoofdstuk 7

Conclusie Het Foot Placement Estimator algoritme bepaalt de plek waar een tweebenige robot zijn voet neer moet zetten om precies in een instabiele balanssituatie te eindigen. Voor de afleiding van dit algoritme is uitgegaan van een model dat bestaat uit een massamiddelpunt en twee massaloze benen. Om dit algoritme toe te passen is het noodzakelijk dat de absolute oriëntatie van de Stickman bekend is. Om deze oriëntatie te achterhalen zijn twee manieren onderzocht. De eerste manier was de bepaling met behulp van een webcam. Deze manier is niet toepasbaar, omdat de gebruikte webcam niet snel genoeg was. Ook de gebruikte Matlab-implementatie was erg langzaam. De andere manier bestond uit de bepaling van de oriëntatie met behulp van twee hoekverdraaiing sensors, inclinometers. Op elke poot is een dergelijke sensor bevestigd. Deze sensoren geven de hoek terug die elke poot maakt met de verticale as. Deze manier was wel snel genoeg. Bovendien waren deze sensoren precies genoeg. Het algoritme is getest. Hieruit bleek ten eerste dat de aan de hand van de FPE berekende hoek ϕ niet goed was. De reden hiervoor is dat er voor de afleiding van de FPE-vergelijking het hierboven genoemde model gebruikt is. De Stickman lijkt niet voldoende op dit model: de robot bestaat niet uit enkel een massamiddelpunt met twee massaloze benen. Bovendien krijgt de Stickman bij een stap een ’zwiep’ mee, doordat een poot naar voren wordt bewogen. Dit heeft dezelfde reden: de benen zijn niet massaloos. Hier wordt in het model helemaal geen rekening mee gehouden. Het FPE-algoritme is dus niet toepasbaar op de Stickman.

29

30

Bibliografie [1] http://www.eindroid.nl/ [2] http://www.dutchrobotics.net/ [3] H.B. Koolmees, B. Moris, T.A.J. Thevissen "Stickman control 1"Intern minor report [4] H.B. Koolmees, B. Moris, T.A.J. Thevissen "Stickman control 2"Intern minor report [5] http://www.eminent-online.com/datasheets/generator.php?product=885&dt=1 [6] Derek L. Wight, Eric G. Kubica, David W.L. Wang Introduction of the Poot Placement Estimator: A Dynamic Measure of Balance for Bipedal Robotics. [7] http://www.eminent-online.com/datasheets/generator.php?product=885&dt=1

31

32

Bijlage A

Posities, constanten en variabelen nodig voor de FPE In deze bijlage wordt laten zien hoe de posities, constanten (bijvoorbeeld lengtes) en variabelen (bijvoorbeeld hoeken en snelheden) die in het algoritme gebruikt zijn, berekend worden.

A.1

A.2

Posities L1 = 30 cm

(A.1)

L2 = 10 cm

(A.2)

L3 = 10 cm

(A.3)

L4 = 30 cm

(A.4)

m = 6.0 kg

(A.5)

ICOM = 7.35 ∗ 10−2 kgm2

(A.6)

Posities L1 cos(α))

(A.7)

p~T = ((L1 + L2 ) sin(α)

(L1 + L2 )cos(α))

(A.8)

p~m2 = (~ pT,x − L3 sin(β)

p~T,y − L3 cos(β))

(A.9)

p~m1 = (L1 sin(α)

p~v1 = (0 33

0)

(A.10)

p~v2 = (~ pT,x − (L3 + L4 ) sin(β)  p~COM =

A.3

A.4

p~m1 ,x + p~m2 ,x 2

p~m1 ,y + p~m2 ,y 2



(A.11)

(A.12)

De hoogte h = p~COM,y

(A.13)

γ =α−β

(A.14)

Hoeken

 ϕ = arctan

p~v2 ,x − p~COM,x p~v2 ,y − p~COM,y

θ=

α−γ 2 

 = arctan

A.5

p~T,y − (L3 + L4 ) cos(β))

p~COM , x p~COM , y



(A.15)

(A.16) 

(A.17)

Snelheden

Voor het FPE algoritme is het nodig de snelheden in x- en in y-richting te weten. Deze worden als volgt berekend: In x-richting: dx vx = (A.18) dt x = l sin(θ(t))

(A.19)

vx = θ˙ l cos(θ(t))

(A.20)

Hieruit volgt: In y-richting: vy =

dy dt

(A.21)

y = l cos(θ(t))

(A.22)

vy = −θ˙ l sin(θ(t))

(A.23)

Hieruit volgt:

34

A.6

Traagheid om het massamiddelpunt

Hoek tussen de benen γ [◦ ] 0 5 10 15 20 25 30 35 40

ICOM [kgm2 ] 7.25 ∗ 10−2 7.25 ∗ 10−2 7.29 ∗ 10−2 7.34 ∗ 10−2 7.41 ∗ 10−2 7.53 ∗ 10−2 7.62 ∗ 10−2 7.76 ∗ 10−2 7.91 ∗ 10−2

35

36

Bijlage B

Simulinkschema B.1

Het ingangssignaal omzetten naar graden

Om het ingangssignaal om te zetten naar graden is het volgende Simulinkschema gebruikt:

Figuur B.1: Van ingangssignaal naar graden

B.2

De berekening van γ, θ en 

Het Gamma & Theta & Epsilon blok bestaat uit de volgende code: 1 2 3

function [eps,gamma,theta]= fcn(a,b) % This block supports an embeddable subset of the MATLAB language. % See the help menu for details.

4 5 6 7 8

L1 L2 L3 L4

= = = =

0.3; 0.1; 0.1; 0.3;

9 10 11

ar = round(a*10)/10; br = round(b*10)/10;

%rounded angle a %rounded angle b

12 13

if abs(ar)

≥

abs(br)

14 15 16

gamma = b − a; theta = a+gamma/2;

17 18

else % abs(ar) < abs(br)−0.5

19 20 21

gamma = a−b; theta = a−gamma/2;

37

22 23

end

24 25 26

arad = a/360 *2*pi; brad = b/360 *2*pi

27 28 29 30 31 32

pv1 = [0,0]; pm1 = [L1*sin(arad),L1*cos(arad)]; pt = [(L1+L2)*sin(arad), (L1+L2)*cos(arad)]; pm2 = [pt(1) − L3*sin(brad), pt(2) − L3*cos(brad)]; pcom = [(pm1(1) + pm2(1))/2 , (pm1(2) + pm2(2))/2];

33 34

eps=atan(pcom(1)/pcom(2)) /(2*pi)*360;

B.3

Het States blok

Het States blok bestaat uit de volgende code: 1

function [phi_real, phi, stop, state]= fcn(a,eps,deps, gamma,theta, dtheta, b, ... state_old)

2 3

%% Constants

4 5 6 7

m1 = 3; m2 = 3; m = m1+m2;

8 9 10 11 12

L1 L2 L3 L4

= = = =

0.3; 0.1; 0.1; 0.3;

13 14

g = 9.81;

15 16

stop = 0;

17 18 19

ar = round(a*10)/10; br = round(b*10)/10;

%rounded angle a %rounded angle b

20 21 22

arad = a/360*2*pi; brad = b/360*2*pi;

23 24 25 26

vx = NaN; vy = NaN;

27 28 29 30 31 32

eps = eps/360 *2*pi; deps = deps/360 *2*pi; theta = theta/360 *2*pi; dtheta = dtheta/360 *2*pi; %% Calculate coordinates and angles

33 34 35

switch state_old case 0

36 37 38 39

pv1 = [0,0]; pm1 = [L1*sin(arad),L1*cos(arad)]; pt = [(L1+L2)*sin(arad), (L1+L2)*cos(arad)];

38

40 41 42

pm2 = [pt(1) − L3*sin(brad), pt(2) − L3*cos(brad)]; pv2 = [pt(1) − (L3+L4)*sin(brad), pt(2) − (L3+L4)*cos(brad)]; pcom = [(pm1(1) + pm2(1))/2 , (pm1(2) + pm2(2))/2];

43 44 45 46

%gamma = a−b; phi_real = atan( (pv2(1) − pcom(1))/(pcom(2)−pv2(2)))/ (2*pi)*360 ; phi = phi_real;

47 48 49 50 51 52

if abs(br) > abs(ar) + 2; state = 1; else state = 0; end

53 54

case 1

55 56 57 58 59 60 61

pv1 = [0,0]; pm1 = [L1*sin(arad),L1*cos(arad)]; pt = [(L1+L2)*sin(arad), (L1+L2)*cos(arad)]; pm2 = [pt(1) − L3*sin(brad), pt(2) − L3*cos(brad)]; pv2 = [pt(1) − (L3+L4)*sin(brad), pt(2) − (L3+L4)*cos(brad)]; pcom = [(pm1(1) + pm2(1))/2 , (pm1(2) + pm2(2))/2];

62 63 64 65

%gamma = a−b; phi_real = atan( (pv2(1) − pcom(1))/(pcom(2)−pv2(2)))/ (2*pi)*360 ; phi = 3;

66 67 68 69 70 71

if phi_real> phi−0.5 state = 2; else state(1) = 1; end

72 73

case 2

74 75 76 77 78 79 80

pv1 = [0,0]; pm1 = [L1*sin(arad),L1*cos(arad)]; pt = [(L1+L2)*sin(arad), (L1+L2)*cos(arad)]; pm2 = [pt(1) − L3*sin(brad), pt(2) − L3*cos(brad)]; pv2 = [pt(1) − (L3+L4)*sin(brad), pt(2) − (L3+L4)*cos(brad)]; pcom = [(pm1(1) + pm2(1))/2 , (pm1(2) + pm2(2))/2];

81 82 83

%gamma = a−b; phi_real = atan( (pv2(1) − pcom(1))/(pcom(2)−pv2(2)))/ (2*pi)*360 ;

84 85 86

%FPE h = pcom(2);

87 88 89 90 91

l = sqrt(pcom(1)^2 + pcom(2)^2); %lengte van pv1 −−> pcom vx = deps * l * cos(eps); vy = −deps * l * sin(eps); Icom = 7.34e−2;

92 93 94 95

ini_degree = 4; phi = ini_degree/360*2*pi; not_zero = 1;

96 97

while not_zero == 1

98 99

phi = phi + 0.1/360*2*pi;

100 101

f = ((m*h*(vx*cos(phi)+vy*sin(phi))*cos(phi)...

39

+ Icom*dtheta*cos(phi)^2)^2)/... (m*h^2 + Icom*cos(phi)^2)+2*m*g*h*cos(phi)*(cos(phi)−1);

102 103 104

if f > −0.1 && f < 0.1 not_zero = 2; end

105 106 107 108

end

109 110 111

phi = phi/(2*pi)*360;

112 113

if abs(ar) < abs(br)+2 && abs(ar) > abs(br)−2 state = 3; else state = 2; end

114 115 116 117 118 119

case 3

120 121

pv1 = [0,0]; pm1 = [L1*sin(arad),L1*cos(arad)]; pt = [(L1+L2)*sin(arad), (L1+L2)*cos(arad)]; pm2 = [pt(1) − L3*sin(brad), pt(2) − L3*cos(brad)]; pv2 = [pt(1) − (L3+L4)*sin(brad), pt(2) − (L3+L4)*cos(brad)]; pcom = [(pm1(1) + pm2(1))/2 , (pm1(2) + pm2(2))/2];

122 123 124 125 126 127 128

%gamma = a−b; phi_real = atan( (pv2(1) − pcom(1))/(pcom(2)−pv2(2)))/ (2*pi)*360 ; phi = phi_real; stop = 1; state = 3;

129 130 131 132 133 134

otherwise phi = 0; phi_real = 0; state = 4;

135 136 137 138 139

end

140 141 142 143 144 145

% safety if theta/(2*pi)*360 < −50 || theta/(2*pi)*360 > 50 stop = 1; state = 5; end

146 147 148 149

if state == 4 stop = 1; end

40

Bijlage C

Matlab code voor het traceren van de markers In deze bijlage is de m-file te zien die de markers op de robot kan traceren. In de functie patternmatching.m worden twee hulp-funcies aangeroepen, namelijk videopattern_gettemplate.m en videopatternplots. Deze zijn hier ook te vinden.

C.1 1

patternmatching.m

clc; clear all; close all;

2 3 4 5 6 7 8

%% Pattern Matching % This demo illustrates the use of 2−D normalized cross−correlation for % pattern matching and target tracking. The demo uses predefined or user % specified target and number of similar targets to be tracked. The % normalized cross correlation plot shows that when the value exceeds the % set threshold, the target is identified.

9 10 11

% %

Copyright 2004−2010 The MathWorks, Inc. $Revision: 1.1.6.2 $ $Date: 2011/05/09 03:44:00 $

12 13 14 15 16 17 18 19 20 21 22 23

%% Introduction % In this demo you use normalized cross correlation for tracking target % patterns in the video. You implement the pattern matching algorithm as % follows: % % * Reduce the size of Image Under Test (IUT) and the Target in order to % ensure that the cross correlation is computed over lesser area thereby % saving computation time. % * Calculate the normalized cross correlation in the frequency domain. % * Track the target based on the maximum correlation value. % * Visualize the results.

24 25 26 27 28 29 30

%% Initialization % Initialize required variables such as the threshold value for the cross % correlation and the decomposition level for Gaussian Pyramid % decomposition. threshold = single(0.95); level = 2;

41

31 32 33 34 35 36 37 38

%% % Initialize video input. vid = videoinput('linuxvideo', 1); himage = preview(vid); pause(5); img1 = get(himage,'cdata'); img1 = rgb2gray(ycbcr2rgb(img1));

39 40 41 42 43 44 45 46 47

%% % Create three gaussian pyramid System objects for decomposing the target % template and decomposing the Image under Test(IUT). The decomposition is % done so that the cross correlation can be computed over a small region % instead of the entire original size of the image. hGaussPymd1 = vision.Pyramid('PyramidLevel',level); hGaussPymd2 = vision.Pyramid('PyramidLevel',level); hGaussPymd3 = vision.Pyramid('PyramidLevel',level);

48 49 50 51 52 53

%% % Create a System object to rotate the image by angle of pi before % computing multiplication with the target in the frequency domain which is % equivalent to correlation. hRotate1 = vision.GeometricRotator('Angle', pi);

54 55 56 57 58 59

%% % Create two 2−D FFT System objects one for the image under test and the % other one for the target. hFFT2D1 = vision.FFT; hFFT2D2 = vision.FFT;

60 61 62 63 64

%% % Create a System object to perform 2−D inverse FFT after performing % correlation (equivalent to multiplication) in the frequency domain. hIFFFT2D = vision.IFFT;

65 66 67 68 69

%% % Create 2−D convolution System object to average the image energy in tiles % of the same dimension of the target. hConv2D = vision.Convolver('OutputSize','Valid');

70 71 72

%% % Here you implement the following sequence of operations.

73 74 75 76 77 78 79 80

% Specify the target image and number of similar targets to be tracked. By % default, the demo uses a predefined target and finds up to 2 similar % patterns. Set the variable useDefaultTarget to false to specify a new % target and the number of similar targets to match. useDefaultTarget = false; [Img, roi, numberOfTargets, target_image] = ... videopattern_gettemplate(useDefaultTarget, img1);

81 82 83

%%

84 85 86 87 88 89 90 91

% Downsample the target image by a predefined factor using the % gaussian pyramid System object. You do this to reduce the amount of % computation for cross correlation. target_image = single(target_image); target_dim_nopyramid = size(target_image); target_image_gp = step(hGaussPymd1, target_image); target_energy = sqrt(sum(target_image_gp(:).^2));

92

42

93 94 95 96 97 98 99 100 101 102 103

% Rotate the target image by 180 degrees, and perform zero padding so that % the dimensions of both the target and the input image are the same. target_image_rot = step(hRotate1, target_image_gp); [rt, ct] = size(target_image_rot); Img = single(Img); Img = step(hGaussPymd2, Img); [ri, ci]= size(Img); r_mod = 2^nextpow2(rt + ri); c_mod = 2^nextpow2(ct + ci); target_image_p = [target_image_rot zeros(rt, c_mod−ct)]; target_image_p = [target_image_p; zeros(r_mod−rt, c_mod)];

104 105 106

% Compute the 2−D FFT of the target image target_fft = step(hFFT2D1, target_image_p);

107 108 109 110 111 112 113

% Initialize constant variables used in the processing loop. target_size = repmat(target_dim_nopyramid, [numberOfTargets, 1]); gain = 2^(level); num_rows = (ri − rt) + 1; Im_p = zeros(r_mod, c_mod, 'single'); % Used for zero padding C_ones = ones(rt, ct, 'single') ; % Used to calculate mean using conv

114 115 116 117 118 119 120 121

%% % Create a System object to calculate the local maximum value for the % normalized cross correlation. hFindMax = vision.LocalMaximaFinder( ... 'Threshold', single(−1), ... 'MaximumNumLocalMaxima', numberOfTargets, ... 'NeighborhoodSize', floor(size(target_image_gp)/2)*2 − 1);

122 123 124 125 126 127

%% % Create a System object to draw a bounding box around the tracked target. hDrawBBox = vision.ShapeInserter( ... 'BorderColor', 'Custom', ... 'CustomBorderColor', [0 1 0]);

128 129 130 131 132 133 134

%% % Create a System object to display the tracking of the pattern. hROIPattern = vision.VideoPlayer( ... 'Name', 'Overlay the ROI on the target'); hROIPattern.Position(1) = round(0.4*hROIPattern.Position(1)); hROIPattern.Position(2) = round(0.4*(hROIPattern.Position(2)));

135 136 137

hROIPattern.Position([4 3]) = [480 640]; % video window size [300 400]

was ...

138 139 140 141 142

%% % Initialize figure window for plotting the normalized cross correlation % value hPlot = videopatternplots('setup',numberOfTargets, threshold);

143 144 145 146 147

%% Stream Processing Loop % Create a processing loop to perform pattern matching on the input signal. % This loop uses the System objects you instantiated above. The loop is % stopped after 800 seconds (or if the program is stopped by the user).

148 149 150

tic n = 1;

151 152 153

while toc < 800.00 Im = get(himage,'cdata');

43

154 155 156 157 158

Im = rgb2gray(ycbcr2rgb(Im)); Im = im2single(Im); Im_gp = step(hGaussPymd3, Im); imshow(Im);

159 160 161 162 163 164 165

% Frequency domain convolution. Im_p(1:ri, 1:ci) = Im_gp; % Zero−pad img_fft = step(hFFT2D2, Im_p); corr_freq = img_fft .* target_fft; corrOutput_f = step(hIFFFT2D, corr_freq); corrOutput_f = corrOutput_f(rt:ri, ct:ci);

166 167 168 169 170 171

% Calculate image energies and block run tiles that are size of % target template. IUT_energy = (Im_gp).^2; IUT = step(hConv2D, IUT_energy, C_ones); IUT = sqrt(IUT);

172 173 174 175

% Calculate normalized cross correlation. norm_Corr_f = (corrOutput_f) ./ (IUT * target_energy); xyLocation = step(hFindMax, norm_Corr_f);

176 177 178 179 180

% Calculate linear indices. linear_index = sub2ind([ri−rt, ci−ct]+1, xyLocation(:,2),... xyLocation(:,1));

181 182 183 184 185 186 187

norm_Corr_f_linear = norm_Corr_f(:); norm_Corr_value = norm_Corr_f_linear(linear_index); detect = (norm_Corr_value > threshold); target_roi = zeros(length(detect), 4); ul_corner = (gain.*(xyLocation(detect, :)−1))+1; target_roi(detect, :) = [ul_corner, fliplr(target_size(detect, :))] ;

%bam

188 189 190

% Draw bounding box. Imf = step(hDrawBBox, cat(3,Im, Im, Im), target_roi);

191 192 193 194

% Plot normalized cross correlation. videopatternplots('update',hPlot,norm_Corr_value); step(hROIPattern, Imf);

195 196 197

% Calculation of the angle with respect to the vertical axis

198 199

if n == 1

200

% Calculation of the vertical axis V = [target_roi(1,2)−target_roi(2,2) ; target_roi(2,1)−target_roi(1,1)]; n = n+1;

201 202 203 204 205

end

206 207

R = [target_roi(1,2)−target_roi(2,2) ; target_roi(2,1)−target_roi(1,1)];

208 209 210 211

T = dot(V,R); theta = acos(T/(norm(V)*norm(R))); theta = theta *360/ (2*pi);

212 213 214 215

if target_roi(1,2) > target_roi(2,2) theta = −theta; end

44

216

hold on

217 218 219

end

220 221 222 223 224

%% Release % Here you call the release method on the System objects to close any open % files and devices. closepreview(vid); ... ... %release(hVideoSrc);

225 226 227 228 229 230 231 232 233 234 235 236

%% Summary % This demo shows use of Computer Vision System Toolbox(TM) to % find a user defined pattern in a video and track it. The algorithm is % based on normalized frequency domain cross correlation between the target % and the image under test. The video player window displays the input % video with the identified target locations. Also a figure displays the % normalized correlation between the target and the image which is used as % a metric to match the target. As can be seen whenever the correlation % value exceeds the threshold (indicated by the blue line), the target is % identified in the input video and the location is marked by the green % bounding box.

237 238 239 240 241 242

%% Appendix % The following helper function is used in this demo. % % * <matlab:edit('videopattern_gettemplate.m') videopattern_gettemplate.m> % * <matlab:edit('videopatternplots.m') videopatternplots.m>

243 244 245

displayEndOfDemoMessage(mfilename)

C.2 1

2 3

videopatternget_template.m

function [img, roi, numTargets, target_img] = ... videopattern_gettemplate(useDefaultTarget, img) %VIDEOPATTERN_GETTEMPLATE Helper function used in videopatternmatching demo %to get the template pattern to track.

4 5 6

% %

Copyright 2004−2010 The MathWorks, Inc. $Revision: 1.1.6.2 $ $Date: 2011/05/09 03:43:59 $

7 8

numTargets = 1;

9 10 11 12

% Pick some initial location for the target rectangle roi = [360 261 159 153]; target_img = imcrop(img,roi);

13 14 15 16 17

hf = figure('Color', get(0, 'defaultuicontrolbackgroundcolor'), ... 'Name', 'Target pattern', ... 'NumberTitle', 'off'); imshow(img);

18 19 20 21

if useDefaultTarget

45

22 23 24 25 26 27 28

numTargets = 2; % Show the pattern rectangle('Position', roi, 'EdgeColor',[0 1 0]); pause(2); close(hf); return; else

29

h = imrect(gca, roi); api = iptgetapi(h); api.setColor([0 1 0]); api.addNewPositionCallback(@(p) title(mat2str(p)));

30 31 32 33 34

% Don't allow the rectangle to be dragged outside of image boundaries fcn = makeConstrainToRectFcn('imrect',get(gca,'XLim'),get(gca,'YLim')); api.setDragConstraintFcn(fcn);

35 36 37 38

yshift = 10; uicontrol(hf, 'style', 'text', 'Units', 'Pixels', ... 'String', 'Number of targets:', ... 'Fontsize', 12, ... 'Position', [80 yshift 150 20]); hEditBox = uicontrol(hf, 'style', 'edit', 'Units', 'Pixels', ... 'String', '2', ... 'HorizontalAlignment', 'left', ... 'BackgroundColor', [1 1 1], ... 'Position', [230 yshift 100 20]); uicontrol(hf, 'style', 'pushbutton', 'Units', 'Pixels', ... 'String', 'Submit', ... 'Position', [340 yshift 100 20], ... 'Callback', @submitFcn); uiwait;

39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54

end

55

function submitFcn(varargin) roi = api.getPosition();

56 57 58

% Extract the template data target_img = imcrop(img,roi); % assignin('base','target_img', target_img); if any(size(target_img) < 20) || any(size(target_img) > 300) errordlg('Target height and width must be between 20 and 300 ... pixels.',... 'Invalid dimensions'); return; end numTargets = round(str2double(get(hEditBox, 'String'))); if numTargets < 1 warndlg('Number of targets must be greater than or equal to 1. ... Setting the number of targets to 1.', 'Invalid number of targets'); numTargets = 1; end close(hf);

59 60 61 62 63

64 65 66 67 68 69

70 71 72

end

73 74 75

end

46

C.3 1 2 3

videopatternplots.m

function hPlot = videopatternplots(mode, varargin) %VIDEOPATTERNPLOTS Helper function to setup plots in videopatternmatching %demo

4 5 6

% %

Copyright 2004−2010 The MathWorks, Inc. $Revision: 1.1.6.1 $ $Date: 2010/08/30 02:59:42 $

7 8

persistent numTargets firstTime;

9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38

switch mode case 'setup' figure('Name', 'Match Metric', 'Color', 'white', ... 'Visible','off', 'NumberTitle', 'off'); hold on; col = ['r','g','c','m','k','y']; numTargets = varargin{1}; threshold = varargin{2}; hPlot = zeros(1, numTargets); for ii = 1:1:numTargets, hPlot(ii) = plot(ii, NaN, col(mod(ii, 6) + 1)); end line([0 400], threshold*[1 1]); grid on; firstTime = true; case 'update' hPlot = varargin{1}; norm_Corr_value = varargin{2}; if firstTime set(get(get(hPlot(1),'Parent'),'Parent'), 'Visible', 'on'); firstTime = false; end % Plot normalized cross correlation for j = 1:numTargets ydata = [get(hPlot(j), 'YData') norm_Corr_value(j)]; set(hPlot(j), 'YData', ydata, 'XData', 1:length(ydata)); end drawnow; end

47

48

Bijlage D

Inclinometer NA4-70 R

NA2-05, NA2-10, NA3-30 NA4-45, NA4-70 Inclinometers with integrated 0...5 Volt signal conditioner for inclination measurement in the ranges of ±5, ±10, ±30, ±45, ±70 degrees

Features ƒ ƒ ƒ ƒ ƒ ƒ ƒ ƒ ƒ ƒ ƒ ƒ

ƒ unconditioned operating voltage between 9V and 30V ƒ low power consumption ƒ protection against reverse supply voltage polarity ƒ EMC certified CE certified ƒ no interference by surrounding electromagnetic fields ƒ shockproof as without moving mechanical parts ƒ hermetically sealed ƒ sensor electrically isolated from point of measurement by high quality plastic housing – no ground connection ƒ zero point adjustable through 360° using clamping ring

small size light weight linear output response high measurement accuracy small zero drift small cross sensitivity high long term stability - virtually infinite lifespan hysteresis-free output signal integrated sensor electronics including signal conditioner and low-pass filter no data memory with memory loss temperature compensated, conditioned 0...5V output signal highly stable internal voltage regulation optional 5 Volt reference voltage output

Description The capacitive, dielectric liquid based inclinometers NA2-05, NA2-10, NA3-30, NA4-45 and NA4-70 contain new sensor electronics. These are made up of a highly stable, laser-trimmed signal conditioner with electronic compensation for temperature drift, highly stable supply voltage regulation circuitry and low-pass filtering of the measurement signal to eliminate unwanted noise. The capacitive measurement principle guarantees a very stable, linear relationship between the inclination being measured and the normalised output signal. The sensor electronics require minimal power and, together with the capacitive primary transformer, are characterised by low errors, high signal-to-noise ratio and high long-term stability. Contrary to measuring inclinations using accelerometers, this measurement principle enables a linear relationship between the inclination to be measured and the output signal, independent of the constant of gravity at the place of measurement, i.e. Independently of where the measurement is taking place, whether in Europe, Australia, on Mount Everest or the Moon.

Application The NA2, NA3 and NA4 can be used for measurements requiring small and light devices, replacability, measurement of relatively large inclinations and a normalised, analogue voltage output signal. Measurements of inclinations in measuring instruments and inspection equipment, in water, land and air vehicles, in automation and safety technology, on cranes and lifting equipment, on robots, in the manufacture of scientific equipment, in medicine and telecommunication as well as navigation systems are typical examples.

Shared Technical Specifications Supply voltage 1)

9V … 30V DC

Dimensions

see dimension drawing

SEIKA Mikrosystemtechnik GmbH - Ellharter Str.10 - D-87435 Kempten - Tel: 0831-25532 Fax: 0831-25534 Internet: http://www.seika.de - http://www.seika.net - Email: [email protected] 1/3

49

R

NA2-05, NA2-10, NA3-30 NA4-45, NA4-70 3)

Current drawn

approx. 2mA

Reference voltage output

Externally regulated supply voltage 2)

5 Volt

Temperature drift of the reference voltage

5.00 Volt < 25 ppm/K

Current drawn with external supply voltage 5V

approx. 1mA

Maximum current of the reference voltage output

5mA

Maximum Operating Temperature

+85°C

Output resistance

Ca. 100 Ohm

Minimum Operating Temperature

-40°C

Protection

IP 65

Maximum Storage Temperature

+90°C

Weight (Sensor excluding mounting ring and cable)

18.5 g

Minimum Storage Temperature

-55°C

Enviroment Humiditi

0…100% r.F.

Signal-to-noise ratio at signal output (0…10kHz)

<150µVss

Electrical signal-to-noise ratio

>85dB

Transverse sensitivity at 45° tilt

<1% of measurement value

Voltage Offset (at Cero degree tilt)

2.5V

Settling time (to 98% of the actual value)

<0.3 s

Maximum output voltage range

0.05V … 4.95V

3 (4) highly flexible cables, Øca.1mm, length 18cm optionally: shielded cable Ø2.1mm

Terminals

Notes: 1) Standard supply voltage 2) optional externally regulated supply voltage 3) reference voltage output (5.00 V) only optionally connected (for wire terminals only) Each sensor will be delivered with individual calibration dates (offset, sensitivity) and calibration record.

Technical Specifications of types NA2-05 and NA2-10 Typ

NA2-05

Measuring range

±5degrees

Resolution

<0.002degrees

NA2-10 ±10degrees

<0.001degrees

Nominal sensitivity

400 mV/degree

200 mV/degree

Output voltage range

2.5V±2V at ±5°

2.5V±2V at ±10°

Linearity deviation over whole measurement range

<±0.02degrees

<±0.03degrees

sensitivity shift over a temperature range 25°C...85°C

<2%

<2%

Temperature drift of zero point

±0.002degrees/Kelvin

±0.002degrees/Kelvin

Technical Specifications of types NA3-30, NA4-45 and NA4-70 NA3-30

NA4-45

NA4-70

Measuring range

Typ

±30degrees

±45degrees

±70degrees

Resolution

<0.005degrees <0.01degrees <0.01degrees

SEIKA Mikrosystemtechnik GmbH - Ellharter Str.10 - D-87435 Kempten - Tel: 0831-25532 Fax: 0831-25534 Internet: http://www.seika.de - http://www.seika.net - Email: [email protected] 2/3

50

R

NA2-05, NA2-10, NA3-30 NA4-45, NA4-70

Nominal sensitivity

66.6.. mV/degree

44.4.. mV/degree

Output voltage range

2.5V±2V at ±30°

2.5V±2V at ±45°

28.57 mV/degree 2.5V±2V at ±70°

Linearity deviation over whole measurement range

<±0.06 degrees

<±0.14 degrees

<±0.28 degrees

sensitivity shift over a temperature range 25°C...85°C

<2%

<2%

<2%

Temperature drift of zero point

±0.002 deg/Kelvin

±0.003deg/Kelvin

±0.003deg/Kelvin

Dimensions and Connections red: supply voltage Ub +8...+30V black: GND (-Ub)

Sensor with internal supply voltage regulation (Typ: NAx-xx)

white: output signal 0.5...4.5V brown: optional Uref output 5,00V max.5mA

12.7mm

11.7mm

1.5mm

3 or 4 wires

fiberglass-reinforced, insulating plastic housing (rotatable and mounted using clamping ring)

brown: supply voltage Ub +5V regulated

Sensor with external supply voltage regulation (Typ: NAx-xx-5V)

black: GND (-Ub) white: output signal 0.5...4.5V

3 wires

fiberglass-reinforced, insulating plastic housing (rotatable and mounted using clamping ring)

M3 mounting screws

measuring angle

-

0

+

ø24.8mm

ø28.9mm

Typ (e.g. NA3-30)

ø3.2mm Inclinometer

NA3-30 001068

31mm

stainless steel ring for mounting of sensor serial number

37mm

SEIKA Mikrosystemtechnik GmbH - Ellharter Str.10 - D-87435 Kempten - Tel: 0831-25532 Fax: 0831-25534 Internet: http://www.seika.de - http://www.seika.net - Email: [email protected] 3/3

51

52

Bijlage E

Symbolenlijst Symbool m g h L ICOM , IA , IB γ, θ, ϕ,  θ˙ θ¨A , θ¨B , θ¨ τA , τB (Hi )1 , (Hi )2 vx , vy

Grootheid Massa Gravitatieconstante Hoogte Lengte Massatraagheid Hoek Hoeksnelheid Hoekversnelling Koppel Impulsmoment Snelheid

Eenheid kg m/s2 m m kg · m2 ◦ /s ◦ /s ◦ /s N ·m N ·m·s m/s

53