Infobionika
ROBOTIKA
XI. Előadás Robot manipulátorok III. Differenciális kinematika
Készült a HEFOP-3.3.1-P.-2004-06-0018/1.0 projekt keretében
Tartalom • • • • •
A forgatási mátrix időbeli deriváltja A geometriai Jacobi-mátrix Kinematikai szingularitások Differenciális kinematikai inverzió Redundáns manipulátorok
A forgatási mátrix deriváltja tekintsünk egy időfüggő forgatási mátrixot: R=R t T
ortogonalitási feltételből tudjuk: a szorzat deriváltja:
R t R t =I T T ˙ ˙ R t R t R t R t =0
vezessük be a következő jelölést: S ferdén szimmetrikus, azaz:
T ˙ St = R t R t T
St S t =0
R deriváltja kifejezhető önmaga függvényeként:
R˙ t =St R t 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
3
Ismétlés: a vektoriális szorzat legyen
ekkor
[] [] [ ]
ax a= a y az
és
bx b= b y bz
a y b z −a z b y a×b= a z b x −a x b z a x b y −a y b x
∣a×b∣=∣a∣∣b∣sin
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
4
A vektoriális szorzat fizikai jelentése Tekintsünk egy origó körül ω szögsebességel forgó merev testet: Kérdés: mi lesz a p pont v sebességvektora? v nagysága:
∣v∣=∣∣=∣∣∣r∣sin v merőleges r-re és ω-ra :
v=×r 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
5
A forgatási mátrix deriváltja Fizikai jelentés: legyen pt =R t p' derivált: pt ˙ = R˙ t p ' azaz:
pt ˙ =St R t p'
Ha ω(t) jelöli az R(t) bázis tengelyeinek alap bázishoz képesti szögsebességeit, akkor tudjuk, hogy pt ˙ =t ×R t p ' 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
6
A forgatási mátrix deriváltja T
Legyen:
t =[ x y z ]
Ekkor S a következő:
0 − z y St = z 0 − x − y x 0
Jelölhetjük tehát így:
St =St
[
]
Bebizonyítható a következő T R SR =SR összefüggés: 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
7
A forgatási mátrix deriváltja Tekintsük a következő példát:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
8
A forgatási mátrix deriváltja
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
9
A geometriai Jacobi-mátrix Tekintsünk egy n szabadságfokú manipulátort. A direkt kinematikai egyenlet a következő:
Ahol a csuklóváltozók vektora:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
10
A geometriai Jacobi-mátrix A differenciális kinematika célja: megtalálni az összefüggést a csuklóváltozók sebessége és a végberendezés sebessége (pozíció, orientáció változása) között a következő formában:
Vagy kompakt jelöléssel:
amely a manipulátor differenciális kinematikai egyenlete 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
11
A geometriai Jacobi-mátrix a 6 x n-es méretű J mátrix a manipulátor geometriai Jacobi-mátrixa
amely a csuklóváltozók általános függvénye
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
12
A forgatási mátrix deriváltja
Tekintsük a P pont 1. bázis és 0. bázis közötti koordináta-transzformációját:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
13
A forgatási mátrix deriváltja a kifejezést idő szerint differenciálva kapjuk:
felhasználva a következő összefüggést: az eredmény:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
14
A forgatási mátrix deriváltja Vezessük be a következő jelölést:
=
Ekkor: Ha p1 állandó, akkor mivel ebben az esetben
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
15
Manipulátor-struktúra i. szegmense
pi: az i. bázis origója pi-1: az i-1. bázis origója ri-1,i: az i. bázis origójának i-1. bázisra vonatkozó koordinátája 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
16
Az i. szegmens pozíciójának változása Ismétlés:
(1)
Az i+1. szegmens origójának pozíciója: Az (1) egyenletet differenciálva: Ezt alkalmazva kapjuk:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
17
A szegmens szögsebessége kiindulás: felhasználva, hogy (1) időbeli deriváltja a következő:
ahol az
2005
jelöli az i. bázis i-1. bázishoz képesti szögsebességét az i-1. bázisban kifejezve összefüggésből adódik:
HEFOP 3.3.1-P.-2004-06-18/1.10
18
A szegmens szögsebessége Tudjuk, hogy Ebből következik:
A végeredmény:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
19
Transzlációs csukló Az i. bázis i-1. bázishoz képesti orientációja az i. csukló mozgatásával nem változik, tehát:
A pozíció változása: ahol zi-1 az i. csukló mozgástengelyén fekvő egységvektor A szögsebesség a következőképp számolható:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
20
Transzlációs csukló A pozíció változása:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
21
Rotációs csukló A szögsebesség: A lineáris sebesség:
Az i. bázis i-1. bázishoz képesti forgása miatt
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
22
Rotációs csukló A szögsebesség és lineáris sebesség általános formulái:
Rotációs csuklónál a következő alakúak lesznek:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
23
A Jacobi-mátrix kiszámítása A Jacobi mátrix: Partícionáljuk 3 x 1-es oszlopvektorokra a következőképp: az i. csukló hozzájárulása a végberendezés lineáris sebességéhez: az i. csukló hozzájárulása a végberendezés szögsebességéhez: 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
24
A Jacobi-mátrix kiszámítása
A szögsebességhez való hozzájárulás: Ha az i. csukló transzlációs
Ha az i. csukló rotációs
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
25
A Jacobi-mátrix kiszámítása A lineáris sebességhez való hozzájárulás: Ha az i. csukló transzlációs:
Ha az i. csukló rotációs (a végberendezés origójának sebességéhez való hozzájárulást számoljuk):
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
26
A Jacobi-mátrix kiszámítása Összefoglalás: transzlációs csukló esetén rotációs csukló esetén
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
27
A Jacobi-mátrix kiszámítása
A zi-1, p és pi-1 vektorok a csuklóváltozók függvényei 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
28
A Jacobi-mátrix kiszámítása zi-1:
forgatási mátrix harmadik oszlopa:
ahol pa
mátrix utolsó oszlopának első három eleme
p homogén formában:
ahol: pi-1 homogén alakban: 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
29
A Jacobi-mátrix kiszámítása Az előzőekben a Jacobi-mátrixot az alap-bázisban írtuk fel. Ha az u bázisra vonatkozóan szeretnénk felírni, akkor elég ismerni az Ru forgatási mátrixot. A két bázisban felírt sebességek kapcsolata:
Behelyettesítve a
2005
összefüggésbe
HEFOP 3.3.1-P.-2004-06-18/1.10
30
A Jacobi-mátrix kiszámítása
Ha az u bázis időben nem változik:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
31
Három szegmensű síkbeli kar
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
32
Három szegmensű síkbeli kar
Az egyes szegmensek (csuklók) pozíciói:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
33
Három szegmensű síkbeli kar A rotációs csuklók mozgástengelyeihez tartozó egységvektorok:
J mátrix felbontása (volt):
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
34
Három szegmensű síkbeli kar A kiszámított J mátrix:
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
35
Kinematikai szingularitások Két szegmensű síkbeli kar a szingularitás határán
A differenciális kinematika alapegyenlete (q-tól függő lineáris leképezés): ahol
a csuklósebességek vektora, és a végberendezés sebessége
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
36
Kinematikai szingularitások Kinematikai szingularitások: azok a konfigurációk, ahol a J mátrix nem teljes rangú A kinematikai szingularitások jelentősége: A szingularitások olyan konfigurációkhoz tartoznak, ahol a szerkezet mobilitása lecsökken, azaz nem lehet a végberendezéssel tetszőleges mozgást végeztetni. Ha a szerkezet szinguláris konfigurációban van, akkor az inverz kinematikai problémára végtelen megoldás is létezhet. Szingularitás környezetében a műveleti tér kis sebességeihez nagy sebességek tartozhatnak a csuklóváltozók terében. 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
37
Kinematikai szingularitások A kinematikai szingularitások csoportosítása: Határ-szingularitások: akkor jelennek meg, ha a manipulátor teljesen kihúzott vagy összecsukott állapotban van. Elkerülhetők, ha a manipulátort nem vezéreljük ki az elérhető munkatér határáig. Belső szingularitások: az elérhető munkatér belsejében vannak, és általában a mozgástengelyek elrendezése vagy bizonyos speciális végberendezéshelyzetek okozzák őket. Súlyos problémát jelenthetnek bizonyos trajektóriák követésénél.
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
38
Differenciális kinematikai inverzió Probléma: adott egy kívánt v(t) mozgás trajektória és a pozíció ill. orientáció kezdeti értékei. Határozzunk meg egy megvalósítható
qt , q˙ t trajektóriát a csuklóváltozók terében, amelynek eredménye v(t). a v=J q q˙ egyenletből kiindulva n=r esetén a megoldás: −1
q˙ t=J q t v t
azaz egy elsőrendű nemlineáris közönséges differenciálegyenlet-rendszer 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
39
Redundáns manipulátorok Ha a manipulátor redundáns (r
v=J q q˙ egyenletre végtelen sok megoldás létezik → valahogy ki kellene választani egyet. Válasszuk ki azt, amely minimalizálja a következő célfüggvényt:
1 ˙T g q= q W q˙ ˙ 2
ahol W egy megfelelő n x n-es szimmetrikus pozitív definit súlyozómátrix 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
40
Redundáns manipulátorok A megoldás (levezetés nélkül): −1
T
−1
T −1
q=W J JW J v ˙ Speciális eset: ha W egységmátrix, akkor: T
T −1
q=J JJ v ˙ ahol
2005
T
T −1
J JJ
a J mátrix jobb oldali pszeudoinverze
HEFOP 3.3.1-P.-2004-06-18/1.10
41
Geometriai és analitikus Jacobi-mátrix geometriai Jacobi-mátrix:
[]
p˙ v= =J q q˙ analitikus Jacobi-mátrix: a végberendezés pozícióját és orientációját a műveleti tér változóinak segítségével fejezzük ki (pl. pozíció és Euler-szögek)
[]
p x= =k q
[ ]
∂ k q J p q x= q= J A q q˙ ˙ ˙ ∂q J q 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
42
Geometriai és analitikus Jacobi-mátrix Példa: két szegmensű síkbeli kar Kinematikai függvény
Az analitikus Jacobi-mátrix
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
43
Inverz kinematikai algoritmus Az inverz differenciálkinematikai egyenlet diszkretizálása időben: Probléma: a diszkrét idejű egyenlet megoldása az integráció pontatlansága miatt eltér a folytonos idejű megoldástól → a kiszámított csuklóváltozókhoz tartozó végberendezés-pozíció és -orientáció eltér az előírtaktól (drift-jelenség). Megoldás: vegyük az előírt (xd) és tényleges (x) végberendezés-pozíció és -orientáció közötti különbséget:
e=x d −x=x d −k q 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
44
Inverz kinematikai algoritmus Vegyük a hiba időbeli deriváltját:
e˙ = x˙d − x˙ = x˙d −J A q q˙ Válasszuk meg q deriváltját a következőképpen: −1 q=J ˙ A q x˙d K e
ahol K sajátértékeinek valós része negatív. Ekkor az aszimptotikusan stabil (nullához tartó) hibadinamika a következő:
e˙ Ke=0 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
45
Az algoritmus blokkvázlata
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
46
Bevezetés a manipulátorok dinamikájába
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
47
A dinamikai feladat Manipulátorok dinamikus egyenletei: leírják, hogyan mozog a manipulátor, ha adottak az aktuátorok nyomatékai és az esetleges külső erők Két fontos alapprobléma: • inverz dinamika: adott a végrehajtandó mozgás, kiszámítandók az általánosított erők (nyomatékok), mint bemenetek • direkt dinamika: adott nyomatékok és erők esetén meg kell oldani a mozgásegyenleteket
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
48
A dinamikus egyenletek általános alakja Egy n csuklóval rendelkező manipulátor egyenleteinek általános alakja: M q q¨ Cq , q˙ qF q˙ G q= ˙ q q˙ q¨ C M F G
csuklókoordináták csuklósebességek csuklógyorsulások Coriolis és centripetális erőhatások manipulátor inerciatenzora viszkózus és Coulomb súrlódás (általában nem modellezik) gravitációs hatás a q általánosított koordinátákhoz (csuklóváltozókhoz) tartozó általánosított erőhatások
A robot szerkezetéből és fizikai paramétereiből (szegmensméretek, -tömegek) algoritmikusan számolható (pl. szimbolikus algebrai szoftverek segítségével) 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
49
A dinamikus egyenletek általános alakja A dinamikus egyenletek irányítási célra legjobban használható tömör formája: Hq qhq , q˙ = ¨
ahol H(q) pozitív definit, szimmetrikus mátrix
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
50
Trajektóriakövetési feladat linearizálással Feladat: a csuklókoordináták megadott időfüggvényének (qref(t)) követése visszacsatolással. Linearizáló visszacsatolás: =H quhq , q ˙
ahol u az új referenciabemenet. A megadott bemenettel a rendszer egyenletei lineárisak és irányíthatók lesznek az új referenciabemenettel: q=u ¨ 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
51
Trajektóriakövetési feladat linearizálással Jelölés: z= q˙ Az állapottér-modell alakja ekkor: q˙ = z z˙ =u
A követési hibák (n csukló esetén): e i =qi −qi , ref , i=1,... , n f i = q˙ i − q i ˙, ref = e˙i , i=1,... , n
A követési hibadinamika: e˙i = f i
f˙ i =ui − q i ¨, ref , i=1,... , n 2005
HEFOP 3.3.1-P.-2004-06-18/1.10
52
Trajektóriakövetési feladat linearizálással A referenciakövető visszacsatolás: ui = qi ¨, ref k i1 ei k i2 f i
Ahol a k konstansokat úgy kell megválasztani, hogy az (ei, fi) állapotváltozókkal rendelkező lineáris rendszer stabil legyen
2005
HEFOP 3.3.1-P.-2004-06-18/1.10
53
