3 - Dynamik - personal-homepages.mis.mpg.de · Spatial Vector Notation • Es gibt keinen einheitlichen Standard zur Beschreibung von Roboterdynamiken (3D Vektoren, 4x4 Matrizen,

Dynamik

Dynamik

• Beschreibung von Beschleunigung und Trajektorien, die sich aus den Gelenk- und 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

v = (vx

, vy

, vz

)

v = xvx

+ yvy

+ zvz

x = (1, 0, 0)

y = (0, 1, 0)

z = (0, 0, 1)

v = (1, 2, 3)

= 1x + 2y + 3z

Spacial Velocity

dx

, dy

, dz

dOx , dOy , dOz

Einheitstranslation entlang der Achsen Ox,Oy,Oz, des kartesischen Koordinatensystems O

Einheitsrotation entlang der Achsen Ox,Oy,Oz, des kartesischen Koordinatensystems O

kartesischen Koordinaten von ω und v in dem Koordinatensystem OXYZ

v = dOx!x + dOy!y + dOz!z + dxvOx + dyvOy + dzvOz

(!x ,!y ,!z , vOx , vOy , vOz )

vO =

0

BBBBBB@

!x

!y

!z

vOx

vOy

vOz

1

CCCCCCA=

✓!vO

◆

Spacial Force

Einheitskraft entlang der Achsen Ox,Oy,Oz, des kartesischen Koordinatensystems O

Einheitsdrehmoment entlang der Achsen Ox,Oy,Oz, des kartesischen Koordinatensystems O

kartesischen Koordinaten von n und f in dem Koordinatensystem OXYZ

eOx , eOy , eOz

ex

, ey

, ez

f = exnOx + eynOy + eznOz + eOxfx + eOy fy + eOzfz

(nOx ,nOy ,nOz , fx , fy , fz)

fO =

0

BBBBBB@

nOx

nOy

nOz

fxfyfz

1

CCCCCCA=

✓nf

◆

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 Geschwindigkeits- und Kraftvektoren in diesen Koordinatensysteme, dann gilt

und

mA, mB , fA, fB 2 R6

mB = BXAmA fB = BX FA fA

BX FA = (BXA)

�T = (AXB)T

BXA =

✓1 0

S(BpA) 1

◆✓BRA 00 BRA

◆

BRABpA— 3x3 Rotationsmatrix — Position von Frame A relativ zu Frame B

S(p) =

0

@0 �p

z

py

pz

0 �px

�py

px

0

1

A

BXA =

✓ARB 00 ARB

◆✓1 0

�S(BpA) 1

◆Inverse:

Vektorprodukt

Für spacial vectors sind zwei Arten von Vektorprodukten definiert:

1. Produkt zweier Geschwindigkeitsvektoren:

Ergebnis ist ein Geschwindigkeitsvektor

2. Produkt eines Geschwindigkeitsvektors mit einem Kraftvektor

Das Ergebnis ist ein Kraftvektor

Diese Formeln werden später bei den Ableitungen und Algorithmen benötigt

m1 ⇥m2 =

✓m1

m1O

◆⇥

✓m2

m2O

◆=

✓m1 ⇥m2

m1 ⇥m2O +m1O ⇥m2

◆

m ⇥ f =

✓mmO

◆⇥

✓fOf

◆=

✓m ⇥ fO +mO ⇥ f

m ⇥ f

◆

Ableitungen

• Die Ableitung eines spacial vectors ist definiert als

• Die Ableitung eines bewegten Koordinatensystems ist definiert als

• Wenn sich nur das Koordinatensystem bewegt, dann gilt

d

dx

s(x) = lim�x!0

s(x + �x)� s(x)

�x

s — beliebiger spacial vector

✓d

dts

◆

A

=d

dtsA + vA ⇥ sA

d

dts = v ⇥ s

Spacial Acceleration ist definiert als die Änderung der spacial velocity. Die Definition unterscheidet sich von der klassischen Definition

Zusammenhang ist gegeben durch:

Vorteil: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.h. Beschleunigungen lassen sich einfach addieren. Es braucht keine besondere Behandlung der Coriolis- und Zentrifugalkräften.

Beschleunigung

a =

✓!vO

◆a0 =

✓!v 0O

◆

spacial klassisch

vOv 0O

Ableitung mit O fest im RaumAbleitung mit O fest im Körper

a0 = a+

✓0

! ⇥ vO

◆

d

dt(v2 = v1 + vrel) = a2 = a1 + arel

• 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

Wenn der Impuls nicht auf C wirkt, sondern um einen beliebigen Punkt O, dann gilt

h = mvC

hC = Imc!

Spacial Momentum & Inertia (Räumlicher Impuls & Trägheit)

vC =

✓!vC

◆

hC =

✓hhC

◆=

✓I cm!mvC

◆

hO = hC + c ⇥ h, hO =

✓hOh

◆=

✓1 S(c)0 1

◆hC

I cm = mr2

Spacial Inertia (Räumliche Trägheit)

Das spacial momentum ist das Produkt der spacial inertia und der spacial velocity:

Daraus folgt (siehe vorherige Folie) für die Trägheit an der Koordinate des Massenpunkts:

Die Trägheit von C in den Punkt O läßt sich nun wie folgt beschreiben:

hC = ICvC

IC =

✓I cm 00 m1

◆

hO =

✓1 S(c)0 1

◆✓I cm 00 m1

◆vC =

✓1 S(c)0 1

◆✓I cm 00 m1

◆✓1 0

S(c)T 1

◆vO

=

✓I cm +mS(c)S(c)T mS(c)

mS(c)T m1

◆vO

hO = IOvO

IO =

✓I cm +mS(c)S(c)T mS(c)

mS(c)T m1

◆

IO =

✓IO mS(c)

mS(c)T m1

◆IO = I cm +mS(c)S(c)T

Spacial Inertia (Räumliche Trägheit)

• Koordinatentransformation

• Trägheit von zwei verbundenen Körpern:

• Vorteil: Lösen von drei Gleichungen in einer:1. Berechnung der gemeinsamen Masse2. Berechnung des gemeinsamen Schwerpunkts3. Berechnung des Rotationsimpulses

• Bewegungsgleichung:

IB = BX FA IA

AXB

IB = (AXB)T IA

AXB

Itot

= I1

+ I2

f =d

dt(Iv) = Ia+ I v = Ia+ v ⇥ Iv

1. Verbindungsgraph

2. Gelenk- und Segmentparameter

3. Gelenkträgheitsparameter

4. Gelenkmodelle

• Ein Starrkörpermodell besteht aus vier Komponenten:

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.

Dynamisches Modell für Starrkörper (Rigid-Bodies)

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) Loop0 11 2,92 3 63 44 556 77 889 10,11,141011 1212 131314 1515 1616

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(Winkel / Position)

• Gelenkgeschwindigkeit(linear / rotatorisch)

• Beschleunigung(linear / rotatorisch)

• Gewünscht:

• Die Kraft, die jedes Gelenke aufbringen muss

q

q

q

• Prismatisches Gelenk

• Rotatorisches Gelenk

• Schraubengelenk

• Zylindrisches Gelenk

Bewegungsmatrix Φ

� =

0

BBBBBB@

000001

1

CCCCCCAvrel = � · (z) =

0

BBBBBB@

00000z

1

CCCCCCA

� =

0

BBBBBB@

001000

1

CCCCCCAvrel = � · (✓) =

0

BBBBBB@

00✓000

1

CCCCCCA

� =

0

BBBBBB@

00100h

1

CCCCCCA, vrel = � · (✓) =

0

BBBBBB@

00✓00h✓

1

CCCCCCA

� =

0

BBBBBB@

0 00 01 00 00 00 1

1

CCCCCCA, vrel = �

✓✓z

◆=

0

BBBBBB@

00✓00z

1

CCCCCCA

�i – 6⇥ ni Matrix

qi – ni ⇥ 1 Vektor

• Die Beschleunigung ergibt sich aus der Ableitung der Geschwindigkeit

Beschleunigung

a =�v

�t

arel = �q + �

� =��

�t+ v ⇥ �

��

�tmeistens= 0

v ⇥ � = diag(v)⇥ �

• Die Beschleunigung ergibt sich aus der Ableitung der Geschwindigkeit

• Beispiel: Prismatisches Gelenk:

Beschleunigung

a =�v

�t

arel = �q + �

� =��

�t+ v ⇥ �

��

�tmeistens= 0

v ⇥ � = diag(v)⇥ �

v ⇥ � =

0

BBBBBB@

00000z

1

CCCCCCA⇥

0

BBBBBB@

000001

1

CCCCCCA=

0

BBBBBBBB@

0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 z

1

CCCCCCCCA

⇥

0

BBBBBB@

000001

1

CCCCCCA=

0

BBBBBB@

00000z

1

CCCCCCA

Inverse Dynamik

• Gelenkkräfte aus Gelenkparametern berechnen ( ).

• 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

q, q, q

Recursive Newton-Euler Algorithm (coordinate free)

• Schritt 1: Geschwindigkeiten und Beschleunigungen für alle Segmente berechnen

• Rekursiv:

• Beschleunigung:

v0 = 0

vi =iX

j=1

�j qj

vi = vi�1 + �i qi

ai = ai�1 + �i qi + vi ⇥ �i qi


• Schritt 2: Für jedes Gelenk, berechne die Kraft, die sich aus der Geschwindigkeit und Beschleunigung ergibt:

• Schritt 3: Berechnung der lokalen Kraft für jedes Gelenk

f ai = Iiai + vi ⇥ Iivi


• Schritt 3:

f ai = f ei + fi �X

j2c(i)

fj

Die Kraft, die durch das Gelenk i ausgeübt wird

Alle externe Kräfte, die auf das Segment wirken

fi –

f ei –

fj –Kräfte die auf die Nachfolger übertragen werden

fi = f ai � f ei +X

j2c(i)

fj

Durch Umformen erhält man




• Schritt 4: Berechnung der Gelenkkräfte


⌧i = �Ti fi

fi = f ai|{z}Ii ai+vi⇥Ii vi

�f ei +X

j2c(i)

fj




• Schritt 4: Berechnung der Gelenkkräfte


⌧i = �Ti fi

fi = f ai|{z}Ii ai+vi⇥Ii vi

�f ei +X

j2c(i)

fj

Formulierung in Gelenkkoordinaten:

Gleichungen, die mehr als ein Gelenk umfassen müssen umgeschrieben werden:

vi =iXp(i)vp(i) + �i qi

ai =iXp(i)ap(i) + �i qi + �i qi

fi = f ai � iX F0

0f ei +X

j2c(i)

iX Fj fj


inputs: q, q, q

output: joint force variables ⌧i

v0 = 0

a0 = �agfor i = 1 to NB do

vi = vp(i) + �i qiai = ap(i) + �i qi + ˙

�i qifi = Iiai + vi ⇥ Iivi � f ei

end for

for i = NB to 1 do

⌧i = �

Ti fi

if p(i) 6= 0 then

fp(i) = fp(i) + fiend if

end for

inputs: q, q, q, model,

0f

ei

outputs: ⌧model data: NB , jtype(i), p(i), Ii

v0 = 0

a0 = �ag

for i = 1 to NB do

iXp(i) = xjcalc(jtype(i), qi )

� = pcalc(jtype(i), qi )

vi =iXp(i)vp(i) + �i qi

ai =iXp(i)ap(i) + �i qi + vi ⇥ �i qi

fi = Iiai + vi ⇥ Iivi � iX

�T0

0f

ei [

iX

F0 =

iX

�T0 ]

end for

for i = NB to 1 do

⌧i = �

Ti fi

if p(i) 6= 0 then

fp(i) = fp(i) +iX

Tp(i)fi

end if

end for

Recursive Newton-Euler Algorithm

jtype(i) – Gelenkttyp (prismatisch, . . . )

xjcalc() – Transformationsmatrix des Gelenks

pcalc() – Berechnet �i

Vorwärtsdynamik

Gegeben: Position, Geschwindigkeit und Kraft/Drehmoment für jedes Gelenk ( )Ausgabe: Beschleunigungen der GelenkeAnwendung: 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

• Aus fi läßt sich ai berechnen

q, q, ⌧

fi = IAi ai + pAi

Aus folgt

und können nun rekursiv berechnet werden:

IAi = Ii +X

j2c(i)

(IAj � UjDjUTj ) pAi = pi +

X

j2c(i)

(pAj + IAj ⇣j + UjDjuj)

pi = vi ⇥ Iivi � f ei

Articulated Body Algorithm / Herleitungen

fi = IAi ai + pAi , ⌧i = �Ti fi , ai = ap(i) + �qi + �i qi

⌧i = �Ti fi = �T

i (IAi (ap(i) + �i q + �qi ) + pAi )

) qi = Di (ui � UTi ap(i))

Ui = IAi �i

Di = (�Ti Ui )

�1 = (�Ti I

Ai �i )

�1

ui = ⌧i � UTi ⇣i � �T

i pAi

⇣i = �iqi =��i

�qiqi + vi ⇥ �i qi

IAi pAi

Beschleunigung der Gelenkparater ist eine Funktion der Beschleunigung des

Vorgängersegments

Articulated Body Algorithm

inputs: q, q, ⌧ , model,

0f eioutput: qmodel data: NB , jtype(i), p(i), Ii

v0 = 0

a0 = �agfor i = 1 to NB do

iXp(i) = xjcalc(jtype(i), qi )�i = pcalc(jtype(i), qi )vi = iXp(i)vp(i) + �i qi⇣i = vi ⇥ �i qiIAi = IipAi = vi ⇥ Iivi � iX�T

00f ei

end for

for i = 1 to NB do

Ui = IAi �i

Di = (�

Ti Ui )

�1

ui = ⌧i � UTi ⇣i � �

Ti p

Ai

if p(i) 6= 0 then

IAp(i) = IAp(i) +iXT

p(i)(IAi � UiDiUT

i )

iXp(i)

pAp(i) = pAp(i) +iXT

p(i)(pAi + IAi ⇣i + UiDiui )

end if

end for

for i = 1 to NB do

ai = iXp(i)ap(i)qi = Di (ui � UT

i ai )ai = ai + �i q + ⇣i

end for

Documents

3 - Dynamik - personal-homepages.mis.mpg.de · Spatial Vector Notation • Es gibt keinen einheitlichen Standard zur Beschreibung von Roboterdynamiken (3D Vektoren, 4x4 Matrizen,