HEFOP 3.3.2 pályázat PPKE ITK – VE MIK - glink.huglink.hu/hallgatoi_segedletek/files/1382b7d41353d373b714c00196bc59d9.pdf · Készült a HEFOP-3.3.1-P.-2004-06-0018/1.0 projekt

Készült a HEFOP-3.3.1-P.-2004-06-0018/1.0 projekt keretében

ROBOTIKA

XI. Előadás

Robot manipulátorok III.Differenciális kinematika

Infobionika

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

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 3

A forgatási mátrix deriváltjaR=R t

R t RT t =I

R t RT t R t RT t =0

St =R t RTt

St ST t =0

R t =S t R t

tekintsünk egy időfüggő forgatási mátrixot:

ortogonalitási feltételből tudjuk:

a szorzat deriváltja:

vezessük be a következő jelölést:

S ferdén szimmetrikus, azaz:

R deriváltja kifejezhető önmaga függvényeként:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 4

Ismétlés: a vektoriális szorzat

a=[axayaz] b=[bxb yb z]legyen és

a×b=[a yb z−a zb ya zbx−axb zax b y−a ybx]ekkor

∣a×b∣=∣a∣∣b∣sin

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 5

A vektoriális szorzat fizikai jelentéseTekintsü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 6

A forgatási mátrix deriváltja

Fizikai jelentés: legyen pt =R t p '

p t =R t p 'derivált:

pt =St R t p 'azaz:

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 7

A forgatási mátrix deriváltja

t =[x y z ]T

S t =St

RS RT=SR

Legyen:

Ekkor S a következő:

Jelölhetjük tehát így:

Bebizonyítható a következő összefüggés:

St =[ 0 −z y

z 0 −x

−y x 0 ]

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 8

A forgatási mátrix deriváltjaTekintsük a következő példát:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 9

A forgatási mátrix deriváltja

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 10

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 11

A geometriai Jacobi-mátrixA 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 12

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 13

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 14

A forgatási mátrix deriváltjaa 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 15

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 16

Manipulátor-struktúra i. szegmense

pi: az i. bázis origójapi-1: az i-1. bázis origójari-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 17

Az i. szegmens pozíciójának változása

Ismétlés:

Az i+1. szegmens origójának pozíciója:

(1)

Az (1) egyenletet differenciálva:

Ezt alkalmazva kapjuk:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 18

A szegmens szögsebességekiindulás:

felhasználva, hogy

(1) időbeli deriváltja a következő:

ahol jelöli az i. bázis i-1. bázishoz képesti szögsebességét az i-1. bázisban kifejezve

az összefüggésből adódik:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 19

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 20

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égvektorA szögsebesség a következőképp számolható:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 21

Transzlációs csukló

A pozíció változása:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 22

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 23

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 24

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 25

A Jacobi-mátrix kiszámításaA 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 26

A Jacobi-mátrix kiszámításaA 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 27

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 28

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 29

A Jacobi-mátrix kiszámításazi-1: forgatási mátrix harmadik oszlopa:

ahol

p a 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 30

A Jacobi-mátrix kiszámításaAz 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 összefüggésbe

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 31

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 32

Három szegmensű síkbeli kar

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 33

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 34

Három szegmensű síkbeli karA 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 35

Három szegmensű síkbeli karA kiszámított J mátrix:

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 36

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 37

Kinematikai szingularitásokKinematikai 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 38

Kinematikai szingularitásokA 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és-helyzetek 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 39

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ó

trajektóriát a csuklóváltozók terében, amelynek eredménye v(t).

qt , q t

v=J q q

q t =J−1q t v t

a

egyenletből kiindulva n=r esetén a megoldás:

azaz egy elsőrendű nemlineáris közönséges differenciálegyenlet-rendszer

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 40

Redundáns manipulátorokHa a manipulátor redundáns (r<n), akkor a Jacobi-mátrixnak több oszlopa van, mint sora, és a

v=J q qegyenletre 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:

g q= 12qTWq

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 41

Redundáns manipulátorokA megoldás (levezetés nélkül):

q=W−1JT JW−1JT −1 v

Speciális eset: ha W egységmátrix, akkor:

q=JT JJT −1v

ahol JT JJT −1 a J mátrix jobb oldali pszeudoinverze

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 42

Geometriai és analitikus Jacobi-mátrixgeometriai Jacobi-mátrix:

v=[ p]=J q qanalitikus 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)

x=[p]=k qx=∂k q

∂qq=[J p qJq ]JAq q

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 43

Geometriai és analitikus Jacobi-mátrixPélda: két szegmensű síkbeli karKinematikai függvény

Az analitikus Jacobi-mátrix

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 44

Inverz kinematikai algoritmusAz 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=xd−x=xd−k q

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 45

Inverz kinematikai algoritmusVegyük a hiba időbeli deriváltját:

e=xd−x=xd−JAq qVálasszuk meg q deriváltját a következőképpen:

q=JA−1q xdKe

ahol K sajátértékeinek valós része negatív. Ekkor az aszimptotikusan stabil (nullához tartó) hibadinamika a következő:

eKe=0

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 46

Az algoritmus blokkvázlata

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 47

Bevezetés a manipulátorok dinamikájába

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 48

A dinamikai feladatManipulá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 49

A dinamikus egyenletek általános alakjaEgy n csuklóval rendelkező manipulátor

egyenleteinek általános alakja:M q qCq , q qF qG q=

qqq

MFG

csuklókoordinátákcsuklósebességekcsuklógyorsulásokCoriolis és centripetális erőhatásokmanipulátor inerciatenzoraviszkózus és Coulomb súrlódás (általában nem modellezik)gravitációs hatása q általánosított koordinátákhoz (csuklóváltozókhoz) tartozó általánosított erőhatások

C

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 50

A dinamikus egyenletek általános alakjaA dinamikus egyenletek irányítási célra

legjobban használható tömör formája:H q qh q , q=

ahol H(q) pozitív definit, szimmetrikus mátrix

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 51

Trajektóriakövetési feladat linearizálássalFeladat: 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 52

Trajektóriakövetési feladat linearizálássalJelölés: z=q

Az állapottér-modell alakja ekkor: q=zz=u

A követési hibák (n csukló esetén):e i=qi−qi , ref , i=1,. .. , n

f i=qi− ˙qi , ref=ei , i=1,. .. , nA követési hibadinamika:

ei= f i

f i=ui− ¨qi , ref , i=1,. .. , n

2005 HEFOP 3.3.1-P.-2004-06-18/1.10 53

Trajektóriakövetési feladat linearizálássalA referenciakövető visszacsatolás:

ui= ¨qi , refk i1eik i2 f i

Ahol a k konstansokat úgy kell megválasztani, hogy az (ei, fi) állapotváltozókkal rendelkező lineáris rendszer stabil legyen