Robottechnika

Trajektóriatervezés

Magyar Attila

Pannon Egyetem
Műszaki Informatika Kar
Villamosmérnöki és Információs Rendszerek Tanszék
amagyar@almos.vein.hu

2009 október 8.
Trajektóriatervezés

A kinematika és a dinamika a manipulátor matematikai modelljének

felírásával foglalkozott
Irányítás
Trajektória tervezés: referencia bemeneteket generál az irányító
rendszernek, amely biztosítja, hogy a manipulátor a megadott
trajektóriát járja be

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 2 / 24

 Út és trajektória

Áttekintés

1 Út és trajektória

2 Csuklóváltozók trajektóriája

3 Műveleti térbeli trajektória

4 Trajektóriák dinamikus skálázása

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 3 / 24

 Út és trajektória

Út és trajektória
Minimális követelmény: a manipulátor egy kezdeti pózból eljusson egy (általunk
megadott) végső pózba
Különbség van út és trajektória között:
Út: a csuklóváltozók terében, vagy a műveleti térben definiált ponthalmaz (pálya),
melyet a manipulátornak érintenie kell a mozgás során - tisztán geometriai
mozgásleírás
Trajektória: olyan út, amelyhez időfüggés is tartozik (pl. bizonyos útponthoz meg
van adva a sebesség/gyorsulás értéke, vagy a teljes menetidő, sebesség-, és
gyorsuláskorlátok)
Trajektóriatervező algoritmus bemenetei
Út leírása
Útpontokhoz rendelt korlátozások, pl. akadályok a munkatérben, stb. - ezek a
műveleti térben jobban kezelhetőek
Manipulátor dinamika
Trajektóriatervező algoritmus kimenetei
A csuklóváltozók (vagy a végszerszám) trajektóriája egy időbeli sorozatként, az
egyes időpontban felvett pozíció, sebesség és gyorsulás értékekkel
Ha a műveleti térben adott a trajektória, akkor valamilyen inverz kinematikai
algoritmussal meg kell határozni a csuklóváltozó-trajektóriát

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 4 / 24

 Csuklóváltozók trajektóriája

Áttekintés

1 Út és trajektória

2 Csuklóváltozók trajektóriája
Pont-pont mozgás
Út mozgás

3 Műveleti térbeli trajektória

4 Trajektóriák dinamikus skálázása

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 5 / 24

 Csuklóváltozók trajektóriája

Csuklóváltozók trajektóriája

Ha a trajektóriatervezés a csuklóváltozók terében történik, akkor

minden nevezetes végszerszám pozícióhoz-orientációhoz meg kell
határozni a megfelelő csuklóváltozó értékeket (q 1 , q 2 , . . .)
Inverz kinematikai algoritmus
Az algoritmus egy q(t) függvényt generál, amely az adott csuklótérbeli
pontokat interpolálja a korlátozások figyelembevételével
Csuklóváltozók terében történő trajektóriatervezéssel szembeni
elvárások:
A generált trajektóriák ne legyenek túl számításigényesek
Csuklópozíciók és sebességek időben folytonos függvények

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 6 / 24

 Csuklóváltozók trajektóriája Pont-pont mozgás

Pont-pont mozgás
Feladat: egy kezdeti qi csuklókonfigurációból tf idő alatt kell eljutni egy végső qf
csuklókonfigurációba, mindegy milyen úton.
A tervező algoritmusok rendszerint valamilyen performancia kritérium szerint optimális
megoldást adnak
Például: forgó merev test
Z t
f I : tehetetlenségi nyomaték
I ω̇ = τ, ω(t)dt = qf − qi ,
0
q̇ = ω : szögsebesség
Z tf
τ 2 (t)dt → min
0 τ : motor által szolgáltatott nyomaték
A megoldás alakja: ω(t) = at 2 + bt + c
Köbös csuklópozíció: q(t) = a3 t 3 + a2 t 2 + a1 t + a0
Négyzetes sebességprofil: q̇(t) = 3a3 t 2 + 2a2 t + a1
Lineáris gyorsulásprofil: q̈(t) = 6a3 t + 2a2
Négy ismeretlen együtthatót kell meghatározni, négy egyenlet kell hozzá:
qi = a0 kezdeti csuklópozíció
qf = a3 tf3 + a2 tf2 + a1 tf + a0 végső csuklópozíció
q̇i = a1 kezdeti csuklósebesség (általában 0)
q̇f = 3a3 tf2 + 2a2 tf + a1 végső csuklósebesség (általában 0)

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 7 / 24

 Csuklóváltozók trajektóriája Pont-pont mozgás

Pont-pont mozgás - példa

Legyenek a kezdeti és végső csuklópozíció, és -sebesség adatok:

qi = 0, qf = π, q̇i = q̇f = 0, tf = 1

a0 = 0, a1 = 0, a2 = 3π, a3 = −2π

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 8 / 24

 Csuklóváltozók trajektóriája Pont-pont mozgás

Pont-pont mozgás

Ipari gyakorlatban gyakran trapéz sebességprofilt alkalmaznak (gyengébb performancia)

Adottak: qi , qf , tf , továbbá q̇i = q̇f = 0, plusz paraméter: q̈c , vagy q̇c
qf +qi tf
Szimmetrikus trajektória (qm = 2
, tm = 2
)
A parabolikus szakasz végsebességének meg kell egyeznie a lineáris szakasz
−qc
sebességével: q̈c tc = qtm −t
m c

Másrészt qc = qi + 12 q̈c tc2

Rendezve: q̈c tc2 − q̈c tf tc + qf − qi = 0

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 9 / 24

 Csuklóváltozók trajektóriája Pont-pont mozgás

Pont-pont mozgás

tc -ben másodfokú egyenlet: q̈c tc2 − q̈c tf tc + qf − qi = 0

Megoldás: s
tf 1 tf2 q̈c − 4(qf − qi ) t 4|qf − qi |
tc = − , mivel tc ≤ f , ezért |q̈c | ≥
2 2 q̈c 2 tf2
1 2


 qi + 2 q̈c t 0 ≤ t ≤ tc



továbbá q(t) = qi + q̈c tc (t − t2c ) tc < t ≤ tf − tc




qf − 21 q̈c (tf − t)2 tf − tc < t ≤ tf


Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 10 / 24

 Csuklóváltozók trajektóriája Út mozgás

Út mozgás
Kettőnél több pontból álló pontsorozattal (útpontok) van megadva a manipulátor által
bejárandó pálya
N útpontra illeszthető egy N − 1-edfokú polinom, de ez a megoldás nem előnyös
Nem lehet megadni a kezdeti és a végsebességeket
Magas fokszám esetén oszcilláló megoldások
Nehezen megoldható, nagy egyenletrendszerek is adódhatnak
Ha egy útpont megváltozik, az egészet újra kell számolni
Helyette több alacsonyabb fokú polinomot használatos, az útpontokban folytonos
Harmadfokú interpolációs polinomokkal garantálható a folytonos sebesség

A csuklópozíció időfüggvényét N − 1
harmadfokú polinom sorozatával adjuk meg
Különböző változatok
sebesség korlátozások
folytonos gyorsulás

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 11 / 24

 Csuklóváltozók trajektóriája Út mozgás

Interpoláció útpontonként adott sebességgel

Tudni kell a kívánt sebességet minden útpontban
N útpont: q1 , . . . , qN
Az interpolációs polinom qk és qk+1 között Πk (t), k = 1, . . . , N − 1
A megoldandó egyenletrendszer (4(N − 1) egyenlet, 4(N − 1) ismeretlen együttható)

Πk (tk ) = qk
Πk (tk+1 ) = qk+1
, k = 1, . . . , N − 1
Π̇k (tk ) = q̇k
Π̇k (tk+1 ) = q̇k+1

Kezdeti- és végsebességek általában nullák (q̇1 = q̇N = 0)

A folytonos sebességre vonatkozó korlátozás: Π̇k (tk+1 ) = Π̇k+1 (tk+1 )

Nem folytonos a gyorsulás

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 12 / 24
 Csuklóváltozók trajektóriája Út mozgás

Interpoláció útpontonkban folytonos gyorsulással

N útpont: q1 , . . . , qN
Az interpolációs polinom qk és qk+1 között Πk (t), k = 1, . . . , N − 1
Az alábbi feltételeknek kell teljesülniük az közbenső pontokra (4(N − 2) egyenlet)
Πk−1 (tk ) = qk
Πk−1 (tk ) = Πk (tk )
, k = 2, . . . , N − 1
Π̇k−1 (tk ) = Π̇k (tk )
Π̈k−1 (tk ) = Π̈k (tk )
További feltételek a végpontokra:
Π0 (t1 ) = qi , ΠN (tf ) = qf , Π̇0 (t1 ), Π̈0 (t1 ), Π̇N (tf ), Π̈N (tf )

Összesen 4N − 2 egyenlet a 4(N − 1) ismeretlenre

Kezdeti és végső gyorsulásra vonatkozó feltételek elhegyása
Negyedfokú polinomok használata Π1 és ΠN−1 esetében (+2 ismeretlen együttható)
Két virtuális pont bevezetése

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 13 / 24

 Csuklóváltozók trajektóriája Út mozgás

Interpoláció útpontonkban folytonos gyorsulással

N + 2 időpont: t2 és tN+1 tartozik a virtuális pontokhoz
A megoldandó egyenletrendszer
Πk−1 (tk ) = qk
Πk−1 (tk ) = Πk (tk )
, k = 3, . . . , N
Π̇k−1 (tk ) = Π̇k (tk )
Π̈k−1 (tk ) = Π̈k (tk )

Π1 (t1 ) = q1 ΠN+1 (tN+2 ) = qf Πk−1 (tk ) = Πk (tk )

Π̇1 (t1 ) = q̇1 , Π̇N+1 (tN+2 ) = q̇f , Π̇k−1 (tk ) = Π̇k (tk ) , k = 2, N + 1
Π̈1 (t1 ) = q̈1 Π̈N+1 (tN+2 ) = q̈f Π̈k−1 (tk ) = Π̈k (tk )
Egyenletek száma: 4(N + 1) - ismeretlenek száma: 4(N + 1)

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 14 / 24

 Csuklóváltozók trajektóriája Út mozgás

Szakaszonként lineáris interpoláció parabolikus átmenetekkel

Egyszerűbb trajektóriatervezési módszer
Az útpontok egyenes szakaszokkal vannak összekötve
A folytonos sebességprofil érdekében a tk időpontok körül q(t) egy parabolikus átmenettel
köti össze a szakaszokat

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 15 / 24

 Csuklóváltozók trajektóriája Út mozgás

Szakaszonként lineáris interpoláció parabolikus átmenetekkel

Legyenek:
∆tk = tk+1 − tk : az időlépés
∆tk,k+1 : az időintervallum, amelyen a qk és qk+1 közti trajektória egyenes szakasz
q̇k,k+1 : konstans sebesség az egyenes szakaszokon
q̈k : konstans gyorsulás a ∆tk0 ideig tartó átmeneti szakaszokon
Adottak: qk , ∆tk , ∆tk0
A közbenső pontok sebessége és gyorsulása
qk −qk−1
q̇k−1,k = ∆t k−1
q̇k,k+1 −q̇k−1,k
q̈k = ∆t 0
k

A végpontokban plusz feltétel szükséges a gyorsulások számításához: q̇0,1 = q̇N,N+1 = 0

így a trajektória bejárása hosszab időt vesz igénybe (tN − t1 + (∆t10 + ∆tN
0 )/2)

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 16 / 24

 Műveleti térbeli trajektória

Áttekintés

1 Út és trajektória

2 Csuklóváltozók trajektóriája

3 Műveleti térbeli trajektória

Út alapelemek
Pozíció
Orientáció

4 Trajektóriák dinamikus skálázása

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 17 / 24

 Műveleti térbeli trajektória

Műveleti térbeli trajektória

A direkt kinematikai modell nemlinearitása miatt egy adott csuklóváltozó trajektóriához
tartozó végszerszám trajektóriát nem könnyű megjósolni
Ha a végszerszámnak egy geomatriailag adott nyomvonalat kell követnie a műveleti
térben, akkor a trajektóriát közvetlenül ebben a térben kell megtervezni
Ha nem kell pontosan követni a nyomvonalat, akkor elég megadni x értékét néhány
útpontban (x(tk ), k = 1, . . . , N), és a korábbi interpolációs módszerek
valamelyikével megtervezni a trajektóriát (pl. rakodó robot néhány akadállyal a
munkatérben)
Amennyiben pontosan kell követnie, analitikusan kell megtervezni (pl. festő-,
gravírozó-, sebészeti robot)

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 18 / 24

 Műveleti térbeli trajektória Út alapelemek

Út alapelemek
Út (Γ) - paraméterezett térbeli görbe: p = f (σ), σ ∈ [σi , σf ]
Ha p(σf ) = p(σi ), akkor az út zárt, egyébként nyitott
Áttérés ívhosszparaméterre: a p-hez tartozó paraméter σ helyett a Γ út p i és p pontok
közti ívhossza (s) lesz:
p = f (s)
Minden p ponthoz tartozik 3 egységvektor (kísérő triéder)

dp
t= ds
érintő egységvektor
d 2 p/ds 2
n= ||d 2 p/ds 2 ||
főnormális egységvektor
b = t × n binormális egységvektor
O pedig a p-beli simulósík

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 19 / 24

 Műveleti térbeli trajektória Út alapelemek

Szakasz és kör paraméterezése

p i és p f pontokat összekötő egyenes szakasz paraméterezése:
s
p(s) = p i + (p − p i )
||p f − p i || f
Kísérő triéder nem egyértelmű:
dp pf − pi
= =t
ds ||p f − p i ||
2
d p
=0 =⇒ n =?
ds 2
Kör paraméterezése a következő paraméterek alapján
a tengely irányvektora: r
a tengely egy pontja: d
a kör egy pontja: p i
δ = pi − d
c = d + (δ T r )r
ρ = ||p i − c||
 
ρ cos(s/ρ)
0
a kör paraméterezése p (s) =  ρ sin(s/ρ) 
0

(x, y , z) koordinátarendszerben felírva

p(s) = c + R · p 0 (s), ahol R = [x 0 y 0 z 0 ]

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 20 / 24

 Műveleti térbeli trajektória Pozíció

Pozíció
Legyen p = f (s) a végszerszámhoz illesztett koorinátarendszer origóga által bejárandó Γ
útvonal paraméterezése
Γ a p i és a p f pontokat köti össze, miközben a paraméter s = 0 (t = 0) és
s = sf (t = tf ) között változik
Γ bejárásának időbeli lefolyását az s(t) függvény határozza meg (interpolációval
meghatározható)
Adott s(t)-vel az útvonal bejárásának dinamikája: p sebessége
dp ds
ṗ = = ṡt
ds dt
pi és pf pontokat összekötő szakasz
ṡ
ṗ = ||p −p ||
(p f − p i ) = ṡt
f i

s̈
p̈ = ||p −p ||
(p f − p i ) = s̈t
f i

Kör esetében a sebesség iránya t, a gyorsulás pedig egy n és t irányú komponensből áll elő

−ṡ 2 cos(s/ρ)/ρ − s̈ sin(s/ρ)

   
−ṡ sin(s/ρ)
ṗ = R ·  ṡ cos(s/ρ)  p̈ = R ·  2
−ṡ sin(s/ρ)/ρ + s̈ cos(s/ρ) 
0 0

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 21 / 24

 Műveleti térbeli trajektória Orientáció

Orientáció

Alapötlet: a végszerszám-koordinátarendszer n, s, a egységvektorainak lineáris

interpolációja:
nf − ni sf − si af − ai
n(s) = ni + s , s(s) = s i + s , a(s) = ai + s
||nf − ni || ||s f − s i || ||af − ai ||
Nem működik, mert nem garantált (n(s), s(s), a(s)) ortogonalitása minden s-re
Helyette Euler szögekkel írjuk le: φ = (ϕ, ϑ, ψ)
Adott φi kezdeti, és φf végorientáció, valamint s(t) mellett a pozíció (lineáris), sebesség
és gyorsulásprofilok:
φ −φ
φ = φi + s ||φf −φi ||
f i
φ −φ
φ̇ = φi + ṡ ||φf −φi ||
f i
φ −φ
φ̈ = φi + s̈ ||φf −φi ||
f i

Miután a műveleti térbeli útvonal és trajektória (azaz p(s(t)) és φ(s(t))) megvan, inverz
kinematikai algoritmussal határozzuk meg a csuklóváltozók q(t) trajektóriáját

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 22 / 24

 Trajektóriák dinamikus skálázása

Áttekintés

1 Út és trajektória

2 Csuklóváltozók trajektóriája

3 Műveleti térbeli trajektória

4 Trajektóriák dinamikus skálázása

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 23 / 24

 Trajektóriák dinamikus skálázása

Trajektóriák dinamikus skálázása

A megtervezett trajektóriának ki kell elégítenie bizonyos dinamikai feltételeket, hogy azt a

robotkar be tudja járni
a mozgáshoz szükséges τ nyomatékok nagyobbak, mint amekkorát a motorok
képesek leadni
a nem megvalósítható trajektóriát időben át kell skálázni
Adott q(t) csuklóváltozó trajektóriából az inverz dinamika segítségével meghatározható a
τ (t) nyomaték-időfüggvény
Skálázzuk át a trajektóriát egy r (t) szigorúan monoton függvénnyel, r (0) = 0 és
r (tf ) = t f
q(t) = q(r (t))

q̇(t) = ṙ q 0 (r ) ⇒ behelyettesíthető a dinamikus modellbe

q̈(t) = ṙ 2 q 00 (r ) + r̈ q 0 (r )
Lineáris skálázás esetén r (t) = c · t (c > 1)
τ (t)
τ (t) = c 2 τ (ct) ⇒ τ (ct) =
c2
1
A nyomatékok nagysága c2
-ed része lett azon az áron, hogy t f > tf

Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 24 / 24