3 - Dynamik.key - Personal Homepages

Dynamik
Dynamik
• Beschreibung von Beschleunigung und Trajektorien, die sich aus den Gelenkund Kontaktkräften ergeben
• Inverse Dynamik (inverse kinematics)
• Berechnung der notwendigen Kräfte um einer vorgegeben Trajektorie zu
folgen
• Anwendung: Roboterkontrolle
• Direkte Dynamik / Vorwärtskinematik (forward kinematics)
• Berechnung der Trajektorien als Funktion der anliegenden Kräfte
• Anwendung: Simulation
Spatial Vector Notation
• Es gibt keinen einheitlichen Standard zur Beschreibung von
Roboterdynamiken (3D Vektoren, 4x4 Matrizen, …)
• In dieser Vorlesung wird die spacial vector notation verwendet, weil sie eine
kompakte Notation bietet. Dadurch braucht man auch weniger Gleichungen,
weniger Berechnungen und ist weniger fehleranfällig
• Basisvektoren
x = (1, 0, 0)
v = (vx , vy , vz )
v = xvx + y vy + zvz
y = (0, 1, 0)
z = (0, 0, 1)
v = (1, 2, 3)
= 1x + 2y + 3z
Spacial Velocity
dx , dy , dz
Einheitstranslation entlang der Achsen Ox,Oy,Oz,
des kartesischen Koordinatensystems O
dOx , dOy , dOz Einheitsrotation entlang der Achsen Ox,Oy,Oz,
des kartesischen Koordinatensystems O
v = d Ox !x + d Oy !y + d Oz !z + d x v Ox + d y v Oy + d z v Oz
(!x , !y , !z , vOx , vOy , vOz ) kartesischen Koordinaten von ω und v in
dem Koordinatensystem OXYZ
0
1
!x
B !y C
B
C ✓
◆
B !z C
!
B
C
vO = B
=
C
v
vO
B Ox C
@ vOy A
vOz
Spacial Force
eOx , eOy , eOz Einheitskraft entlang der Achsen Ox,Oy,Oz, des
kartesischen Koordinatensystems O
ex , ey , ez
Einheitsdrehmoment entlang der Achsen
Ox,Oy,Oz, des kartesischen
Koordinatensystems O
f = e x nO x + e y nO y + e z nO z + e O x f x + e O y f y + e O z f z
(nOx , nOy , nOz , fx , fy , fz ) kartesischen Koordinaten von n und f in
dem Koordinatensystem OXYZ
0
1
nO x
B nO y C
B
C ✓
◆
B nO z C
n
B
C
fO = B
=
C
f
f
B x C
@ fy A
fz
Rechenoperationen auf spacial vectors
• Addition:
Kräfte und Geschwindigkeiten lassen sich komponentenweise addieren
und subtrahieren. Wenn f1 und f2 auf einen Körper wirken, dann ist die
Gesamtkraft f = f1 + f2. Wenn sich ein Körper mit v1 bewegt und ein
anderer mit v2, dann ist die relative Geschwindigkeit v = v1 - v2.
• Skalarprodukt
Das Skalarprodukt ist nur zwischen Geschwindigkeits- und Kraftvektoren
definiert und drückt die aufgewendete Arbeit aus (work)
m · f = mT f
Koordinatentransformation
Seien A und B zwei Koordinatensysteme und mA , mB , fA , fB 2 R6
Geschwindigkeits- und Kraftvektoren in diesen Koordinatensysteme, dann gilt mB = B X A mA
B
B
XA =
und
XAF = (B XA )
✓
1
S(B pA )
0
1
0
0
S(p) = @ pz
py
Inverse:
B
B
XA =
RA — 3x3 Rotationsmatrix
✓
A
fB = B XAF fA
RB
0
0
A
RB
B
T
= (A XB ) T
◆✓
pz
0
px
◆✓
B
RA
0
0
B
RA
1
py
px A
0
1
S(B pA )
0
1
◆
◆
pA — Position von Frame A relativ zu Frame B
Vektorprodukt
Für spacial vectors sind zwei Arten von Vektorprodukten definiert:
1. Produkt zweier Geschwindigkeitsvektoren:
m1 ⇥ m2 =
✓
m1
m 1O
◆
⇥
✓
m2
m 2O
◆
=
✓
m1 ⇥ m2
m 1 ⇥ m 2O + m 1O ⇥ m 2
◆
Ergebnis ist ein Geschwindigkeitsvektor
2. Produkt eines Geschwindigkeitsvektors mit einem Kraftvektor
m⇥f =
✓
m
mO
◆
⇥
✓
fO
f
◆
=
✓
m ⇥ f O + mO ⇥ f
m⇥f
◆
Das Ergebnis ist ein Kraftvektor
Diese Formeln werden später bei den Ableitungen und Algorithmen benötigt
Ableitungen
• Die Ableitung eines spacial vectors ist definiert als
d
s(x + x)
s(x) = lim
x!0
dx
x
s(x)
s — beliebiger spacial vector
• Die Ableitung eines bewegten Koordinatensystems ist definiert als
✓
d
s
dt
◆
A
d
= s A + vA ⇥ s A
dt
• Wenn sich nur das Koordinatensystem bewegt, dann gilt
d
s =v ⇥s
dt
Beschleunigung
Spacial Acceleration ist definiert als die Änderung der spacial velocity. Die
Definition unterscheidet sich von der klassischen Definition
a=
✓
!˙
v̇O
spacial
◆
0
a =
✓
klassisch
Zusammenhang ist gegeben durch:
Vorteil:
!˙
v̇O0
◆
0
a =a+
v̇O Ableitung mit O fest im Raum
v̇O0 Ableitung mit O fest im Körper
✓
0
! ⇥ vO
◆
Spacial acceleration ist einfacher in der Handhabung. Seien B1 und B2 zwei
Körper mit Geschwindigkeiten und sei v2 = v1 + vrel, dann folgt daraus, dass
d
(v2 = v1 + vrel ) = a2 = a1 + arel
dt
d.h. Beschleunigungen lassen sich einfach addieren. Es braucht keine
besondere Behandlung der Coriolis- und Zentrifugalkräften.
Spacial Momentum & Inertia
(Räumlicher Impuls & Trägheit)
• Körper mit Masse m und Schwerpunkt C
• Rotationsträgheit um C ist definiert als • Bewegung des Körpers
• Linearer Impuls
• Intrinsischer Rotationsimpuls
• Spacial Momentum
I cm = mr 2
✓
◆
!
vC =
vC
h = mvC
hC = I mc !
✓
◆ ✓ cm ◆
h
I !
hC =
=
hC
mvC
Wenn der Impuls nicht auf C wirkt, sondern um einen beliebigen Punkt O,
dann gilt
✓
◆ ✓
◆
hO
1 S(c)
hO = hC + c ⇥ h,
hO =
=
hC
h
0
1
Spacial Inertia (Räumliche Trägheit)
Das spacial momentum ist das Produkt der spacial inertia und der spacial
velocity: h C = I C vC
Daraus folgt (siehe vorherige Folie) für die Trägheit an der Koordinate des
Massenpunkts:
✓ cm
◆
I
0
IC =
0 m1
Die Trägheit von C in den Punkt O läßt sich nun wie folgt beschreiben:
hO =
=
✓
✓
1
0
S(c)
1
◆✓
I
cm
0
I cm + mS(c)S(c)T
mS(c)T
0
m1
◆
✓
vC =
◆
mS(c)
vO
m1
1
0
S(c)
1
◆✓
h O = I O vO
✓ cm
◆
T
I + mS(c)S(c)
mS(c)
IO =
mS(c)T
m1
✓
◆
ĪO
mS(c)
cm
T
IO =
Ī
=
I
+
mS(c)S(c)
O
mS(c)T
m1
I
cm
0
0
m1
◆✓
1
S(c)T
0
1
◆
vO
Spacial Inertia (Räumliche Trägheit)
• Koordinatentransformation
IB = B XAF IA A XB
IB = (A XB )T IA A XB
• Trägheit von zwei verbundenen Körpern:
Itot = I1 + I2
• Vorteil: Lösen von drei Gleichungen in einer:
1. Berechnung der gemeinsamen Masse
2. Berechnung des gemeinsamen Schwerpunkts
3. Berechnung des Rotationsimpulses
• Bewegungsgleichung:
d
f = (Iv ) = Ia + İ v = Ia + v ⇥ Iv
dt
Dynamisches Modell für Starrkörper (Rigid-Bodies)
• Ein Starrkörpermodell besteht aus vier Komponenten:
1. Verbindungsgraph
3. Gelenkträgheitsparameter
2. Gelenk- und Segmentparameter
4. Gelenkmodelle
Verbindungsgraph:
Ein Verbindungsgraph ist ein ungerichteter Graph in dem die Segmente
(Starrkörper) durch Knoten und die Gelenke durch die Kanten repräsentiert
werden. Bei mobilen Roboter wird zusätzlich ein 6DOF Gelenk und eine
weiteres Segment (Fixed Base) hinzugefügt.
Verbindungsgraph
• Beispiel: Mobiler humanoider Roboter
• Basis (Fixed Base) hat die Nummer 0
• Segmente werden in aufsteigender
Reihenfolge nummeriert “regular numbering
scheme”
• Gelenk i verbindet Segment i mit dessen
Vorgänger
• Bei einer Schleifenstruktur muss man sich auf
eine Baumstruktur für die Nummerierung
festlegen
Verbindungsgraph
• NB — Anzahl der Segmente
• NJ — Anzahl der Gelenke
• Loop-closing Gelenke erhalten Nummern in
beliebiger Reihenfolge mit NB + 1 ≤ i ≤ NJ • p(i) — Vorgängersegment (parent,
predecessor)
• c(i) — Menge der Nachfolgersegmente
(successor, children)
Verbindungsgraph
Body i
c(i)
0
1
1
2,9
2
36
3
4
4
5
5
6
7
7
8
8
9
10,11,14
10
11
12
12
13
13
14
15
15
16
16
Loop
Loop
1
2
3
Loop-closing
joint k
17
18
19
p(k)
0
16
0
s(k)
5
1
13
Root
0
1
0
Inverse Dynamik
• Gegeben: • Gelenkparameter
q
(Winkel / Position)
• Gelenkgeschwindigkeit
q̇
(linear / rotatorisch)
• Beschleunigung
q̈
(linear / rotatorisch)
• Gewünscht: • Die Kraft, die jedes Gelenke
aufbringen muss
Bewegungsmatrix Φ
• Prismatisches Gelenk
0 1
0
0
B 0 C
B
B C
B
B 0 C
B
C vrel = · (ż) = B
=B
B 0 C
B
B C
B
@ 0 A
@
1
0
0
0
0
0
ż
• Rotatorisches Gelenk
0
B
B
B
=B
B
B
@
0
0
1
0
0
0
1
C
C
C
C
C
C
A
0
B
B
B
˙ =B
· (✓)
B
B
@
vrel =
i
0
0
✓˙
0
0
0
1
C
C
C
C
C
C
A
1
C
C
C
C
C
C
A
– 6 ⇥ ni Matrix
• Schraubengelenk
0
1
0
B 0 C
B
C
B 1 C
C , vrel =
=B
B 0 C
B
C
@ 0 A
h
• Zylindrisches Gelenk
0
B
B
B
=B
B
B
@
0
0
1
0
0
0
0
0
0
0
0
1
1
C
C
C
C,
C
C
A
0
B
B
B
˙ =B
· (✓)
B
B
@
0
0
✓˙
0
0
h✓˙
0
vrel =
qi – ni ⇥ 1 Vektor
B
✓
◆ B
B
✓˙
=B
B
ż
B
@
1
C
C
C
C
C
C
A
0
0
✓˙
0
0
ż
1
C
C
C
C
C
C
A
Beschleunigung
v
• Die Beschleunigung ergibt sich aus der Ableitung der Geschwindigkeit a =
t
arel = q̈ + ˙
˙ =
t
meistens
t
v⇥
=
+v ⇥
0
= diag(v ) ⇥
Beschleunigung
v
• Die Beschleunigung ergibt sich aus der Ableitung der Geschwindigkeit a =
t
arel = q̈ + ˙
˙ =
+v ⇥
t
meistens
=
t
v⇥
• Beispiel: Prismatisches
= diag(v ) ⇥
Gelenk: 0
v⇥
0
B
B
B
=B
B
B
@
0
0
0
0
0
ż
1
0
C B
C B
C B
C⇥B
C B
C B
A @
0
0
0
0
0
1
1
0
B
C B
C B
C B
C=B
C B
C B
A B
@
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0 0
0 0
0 0
0 0
0 0
0 0
0 ż
1
0
C
C B
C B
C B
C⇥B
C B
C B
C @
A
0
0
0
0
0
1
1
0
C B
C B
C B
C=B
C B
C B
A @
0
0
0
0
0
ż
1
C
C
C
C
C
C
A
Inverse Dynamik
• Gelenkkräfte aus Gelenkparametern berechnen
( q, q̇, q̈ ).
• Recursive Newton-Euler Algorithm (RNEA) hat eine
Laufzeit von O(n)
• Besteht aus zwei Rekursionsschleifen:
1. Geschwindigkeiten und Beschleunigungen von
der Basis aus berechnen
2. Notwendige Kräfte von dem Endeffektor aus
berechnen
Recursive Newton-Euler Algorithm (coordinate free)
• Schritt 1: Geschwindigkeiten und
Beschleunigungen für alle Segmente
berechnen
v0 = 0
vi =
i
X
j q̇j
j=1
• Rekursiv:
vi = vi
1
+
i q̇i
• Beschleunigung:
ai = ai
1
+
i q̈i
+ vi ⇥
i q̇i
Recursive Newton-Euler Algorithm (coordinate free)
• Schritt 2: Für jedes Gelenk, berechne
die Kraft, die sich aus der
Geschwindigkeit und Beschleunigung
ergibt:
f i a = I i a i + vi ⇥ I i v i
• Schritt 3: Berechnung der lokalen
Kraft für jedes Gelenk
Recursive Newton-Euler Algorithm (coordinate free)
• Schritt 3:
a
e
fi = fi + fi
X
fj
j2c(i)
fi –
Die Kraft, die durch das
Gelenk i ausgeübt wird
fi e –
Alle externe Kräfte, die
auf das Segment wirken
fj –
Kräfte die auf die
Nachfolger übertragen
werden
Durch Umformen erhält man
X
a
e
fi = fi
fi +
fj
j2c(i)
Recursive Newton-Euler Algorithm (coordinate free)
• Schritt 2: Für jedes Gelenk, berechne
die Kraft, die sich aus der
Geschwindigkeit und Beschleunigung
ergibt:
f i a = I i a i + vi ⇥ I i v i
• Schritt 3: Berechnung der lokalen
Kraft für jedes Gelenk
a
e
fi = fi
fi +
|{z}
Ii ai +vi ⇥Ii vi
X
j2c(i)
• Schritt 4: Berechnung der
Gelenkkräfte
⌧i = T
i fi
fj
Recursive Newton-Euler Algorithm (coordinate free)
• Schritt 2: Für jedes Gelenk, berechne
die Kraft, die sich aus der
Geschwindigkeit und Beschleunigung
ergibt:
f i a = I i a i + vi ⇥ I i v i
• Schritt 3: Berechnung der lokalen
Kraft für jedes Gelenk
a
e
fi = fi
fi +
|{z}
Ii ai +vi ⇥Ii vi
X
j2c(i)
• Schritt 4: Berechnung der
Gelenkkräfte
⌧i = T
i fi
fj
Recursive Newton-Euler Algorithm (coordinate free)
inputs: q, q̇, q̈
output: joint force variables ⌧i
v0 = 0
a0 = ag
for i = 1 to NB do
vi = vp(i) + i q̇i
ai = ap(i) + i q̈i + ˙ i q̇i
f i = I i a i + vi ⇥ I i vi f i e
end for
for i = NB to 1 do
⌧i = T
i fi
if p(i) 6= 0 then
fp(i) = fp(i) + fi
end if
end for
Formulierung in Gelenkkoordinaten:
Gleichungen, die mehr als ein Gelenk
umfassen müssen umgeschrieben
werden:
vi = i Xp(i) vp(i) +
+ ˙ i q̇i
X
i F0 e
i F
X0 fi +
Xj fj
ai = i Xp(i) ap(i) +
fi = fi a
i q̇i
i q̈i
j2c(i)
Recursive Newton-Euler Algorithm
inputs: q, q̇, q̈, model, 0 fi e
outputs: ⌧
model data: NB , jtype(i), p(i), Ii
v0 = 0
a0 = ag
for i = 1 to NB do
i
Xp(i) = xjcalc(jtype(i), qi )
= pcalc(jtype(i), qi )
vi = i Xp(i) vp(i) + i q̇i
ai = i Xp(i) ap(i) + i q̈i + vi ⇥ i q̇i
fi = Ii ai + vi ⇥ Ii vi i X0 T 0 fi e [i X0F = i X0 T ]
end for
for i = NB to 1 do
⌧i = T
i fi
if p(i) 6= 0 then
T
jtype(i) – Gelenkttyp (prismatisch, . . . )
fp(i) = fp(i) + i Xp(i)
fi
xjcalc() – Transformationsmatrix des Gelenks
end if
pcalc() – Berechnet i
end for
Vorwärtsdynamik
Gegeben: Position, Geschwindigkeit und Kraft/Drehmoment für jedes Gelenk
( q, q̇, ⌧ )
Ausgabe: Beschleunigungen der Gelenke
Anwendung: Simulation
Kernidee des Articulated-Body Algorithm (ABA)
[Laufzeit von O(n)]
• Die Interaktion zwischen Segment p(i) und dem
restlichen Teilbaum findet nur über die Kraft fi statt
• Angenommen, das Gelenk wurde durchtrennt,
dann gilt fi = IiA ai + piA
• Aus fi läßt sich ai berechnen
Articulated Body Algorithm / Herleitungen
Aus folgt
fi = IiA ai + piA ,
⌧i =
T A
⌧i = T
f
=
i i
i (Ii (ap(i) +
) q̈i = Di (ui
Ui = IiA
UiT ap(i) )
T
1
U
)
i
i
⌧i UiT ⇣i
⇣i = ˆ i qi =
=(
i
qi
T A
i Ii
T A
i pi
q̇i + vi ⇥
ai = ap(i) + q̈i + ˙ i q̇i
A
˙
q̈
+
q̇
)
+
p
i
i
i )
Beschleunigung der Gelenkparater ist eine
Funktion der Beschleunigung des
Vorgängersegments
i
Di = (
ui =
T
i fi ,
i)
1
i q̇i
IiA und piA können nun rekursiv berechnet werden:
X
X
IiA = Ii +
(IjA Uj Dj UjT )
piA = pi +
(pjA + IjA ⇣j + Uj Dj uj )
j2c(i)
j2c(i)
p i = vi ⇥ I i vi
fi e
Articulated Body Algorithm
inputs: q, q̇, ⌧ , model, 0 fi e
output: q̈
model data: NB , jtype(i), p(i), Ii
v0 = 0
a0 = ag
for i = 1 to NB do
i
Xp(i) = xjcalc(jtype(i), qi )
i = pcalc(jtype(i), qi )
vi = i Xp(i) vp(i) + i q̇i
⇣i = vi ⇥ i q̇i
IiA = Ii
piA = vi ⇥ Ii vi i X0 T 0 fi e
end for
for i = 1 to NB do
Ui = IiA i
1
Di = ( T
U
)
i
i
T A
ui = ⌧i UiT ⇣i
i pi
if p(i) 6= 0 then
A
A
T
Ip(i)
= Ip(i)
+ i Xp(i)
(IiA Ui Di UiT )i Xp(i)
A
A
T
(piA + IiA ⇣i + Ui Di ui )
pp(i)
= pp(i)
+ i Xp(i)
end if
end for
for i = 1 to NB do
ai = i Xp(i) ap(i)
q̈i = Di (ui UiT ai )
ai = ai + i q̈ + ⇣i
end for