Transcript Inverse kinematica

design
learn
share
Inverse kinematica
is hier de schrijfstift.
Inverse kinematica houdt hier in dat
we aan de hand van
een gewenste positie van de schrijfstift (x6,y6) de hoeken van de actuatoren (= stand van
de servomotoren) θ1
en θ2 gaan berekenen. Aangezien de
scharniergewrichten slechts één vrijheidsgraad hebben
en er maar twee actuatoren zijn, valt
dit qua wiskundige
vergelijkingen nog-
end effector
actuator
actuator
Een kinematische keten in de robotica
is een wiskundig model van een mechanisch systeem bestaande uit een geheel
van starre lichamen en gewrichten. De
bewegingen van een dergelijke kinematische keten kunnen dan weer verder gemodelleerd worden aan de hand van wiskundige vergelijkingen.
Voorwaartse kinematica houdt in dat de
configuratie van de kinematische keten
berekend wordt aan de hand van de posities van de gewrichten. Inverse kinematica is exact het tegenovergestelde,
waarbij met een gewenste configuratie
van de kinematische keten als uitgangspunt de posities van de gewrichten worden berekend, dit meestal met als doel
de positie en het traject van de eindeffector/manipulator te bepalen.
Buiten het domein van de robotica wordt
inverse kinematica ook toegepast bij
3D-animaties en computergames. Ook
de bewegingen van het menselijk lichaam en dieren kunnen op dergelijke
wijze gemodelleerd worden.
Wanneer we deze theorie toepassen op
de zandklok, dan bestaat de kinematische keten hier uit vier starre lichamen
en vijf scharniergewrichten, zoals getekend in de afgebeelde figuur. Twee van
de vijf scharniergewrichten hebben een
actuator (= servomotor). De eindeffector
40
januari/februari 2017
al mee en is dit nog
te behappen met
een op een Atmega328p gebaseerd
Arduino-board.
www.elektormagazine.nl
Om de berekeningen te vereenvoudigen
hebben de linker en rechter servomotor
dezelfde Y-positie: y1 = y2. De grafiek
toont verscheidene posities, lengtes en
hoeken. Sommige zijn constant, andere
zijn variabel.
Constant: (x1,y1), (x2,y2), L13, L24, L35,
L45, L46, L56, α564
Variabel: (x5,y5), (x6,y6), L15, L26, α1, β1,
θ1, α2, β2, θ2, α5, α6
Eerst bepalen we hoek θ2. Deze hoek
leiden we af van α2 en β2. α2 is de hoek
tussen de X-as en de lijn die door P2
(rechter servomotor) en P6 (pen) gaat:
⎛ y − y ⎞
α 2= arctan 2 ⎜ 6 2 ⎟
⎝ x 6− x 2 ⎠
β2 berekenen we op basis van de zijdes
van driehoek P2-P4-P6. Lengte L46 is
bekend. Lengte L26 berekenen we als
volgt:
L 26=
( x 6− x 2 )2+ ( y 6− y 2 )2
Nu kunnen we β2 bepalen:
⎛ L 2+ L 2− L 2 ⎞
24
46
β 2= arccos ⎜ 26
⎟
⎝ 2 ⋅ L 26⋅ L 24 ⎠
θ2 is dan:
Θ2= α 2− β 2
Hoek θ1 berekenen we op basis van
driehoek P1-P3-P5. (x5,y5), de positie van
P5 is een variabele die we eerst moeten
berekenen.
Driehoek P4-P5-P6 is een star lichaam,
hoeken en zijdes zijn constant. Om
(x5,y5) te berekenen gaan we er van
uit dat driehoek P4-P5-P6 roteert met
zijde L56 rond P6 over een hoek α5 zoals
aangegeven in de grafiek.
Eerst bepalen we hoek α6:
⎛ L 2+ L 2− L 2 ⎞
46
24
α 6= arccos ⎜ 26
⎟
⎝ 2 ⋅ L 26⋅ L 46 ⎠
Hoek α2 was al bekend. Nu kunnen we
hoek α5 bepalen:
α 5= π + α 2 + α 6− α 564
De positie van P5 is dan als volgt:
x 5= x 6 + L 56 ⋅ cos(α 5 )
y 5= y 6 + L 56 ⋅ sin(α 5 )
Terug naar hoek θ1. Deze hoek leiden
we af van α1 en β1. α1 is de hoek tussen
de X-as en de lijn die door P1 (linker
servomotor) en P5 gaat:
⎛ y − y ⎞
α 1= arctan2 ⎜ 5 1 ⎟
⎝ x 5 − x1 ⎠
β1 berekenen we op basis van de zijdes
van driehoek P1-P3-P5. Lengte L35 is
bekend. Lengte L15 berekenen we als
volgt:
L15=
( x 5− x1 )2+ ( y 5− y1 )2
β1 is dan:
⎛ L 2+ L 2− L 2 ⎞
13
35
β 1= arccos ⎜ 15
⎟
⎝ 2 ⋅ L15⋅ L13 ⎠
En ten slotte volgt θ1:
Θ1= π − β 1− α 1
Deze berekeningen zijn
geïmplementeerd in de functie pen_
calc in de sketch.