robotokprogramozasa.pdf

7/23/2019 RobotokProgramozasa.pdf

1/71

1

ROBOTOK PROGRAMOZSA

1. ROBOT KINEMATIKAI ALAPOK

A robot felfoghat egy irnytott mechanizmusnak, amely elrhat plyn mozog-hat, s a plya mentn elrhat feladatokat lthat el. A robot szegmensekbl ll,melyeket transzlcis vagy rotcis csuklk kapcsolnak ssze, a robot mozgsa pe-

dig a fizika trvnyeinek van alvetve. M

kdst az irnyts klnbz

szintjenklnfle modellekkel (kinematikai, dinamikus) rhatjuk le. A robotok programoz-sakor ezek kzl a legegyszerbb kinematikai (geometriai) modell jtszik szerepet.Ebben alapvetfontossg a koordinta-rendszerek kztti kapcsolat lersa.

Alkalmazi szempontbl ugyanis merev robotok esetn, amelyeknl a rugalmasalakvltozs elhanyagolhatan kicsi a robot elvrt pontossghoz kpest, a robot l-tal elvgzend mozgs definilsakor alapul vlaszthatunk egy bzis koordinta-rendszert (szoks vilg koordinta-rendszernek is nevezni), amelyhez elrjuk, hollegyen a robot munkavgzeszkze, az n. vgberendezs (end effector) a mozgssoronkvetkez fzisban a bzis koordinta-rendszerhez kpest. Mivel azonbannem csak a vgberendezs (megfog, szerszm, technolgiai eszkz: festkszrpisztoly, pont- vagy vhegesztstb.) pozcija, hanem annak orientcija is fontos

(gondoljunk arra, hogy ha a megfog egy vzzel teli pohart tart, akkor nem mindegy,hogy a pohr szja felfel vagy lefel nz), ezrt kzenfekva vgberendezshez isegy koordinta-rendszert rgzteni, s a kt koordinta-rendszer kztti relatv poz-cit s orientcit elrni. Ez azt jelenti, hogy van kt derkszg koordinta-rendszer, a BK bzis koordinta-rendszer s a EK end effector koordinta-rendszer,

s a mozgs definilsa sorn azt mondjuk meg, hol legyen EK origja s hogyan

lljanak a EK egysgvektorai a vilg koordinta-rendszer origjhoz s egysgvek-

toraihoz kpest. Az elbbit a vgberendezs pozcijanak (szerszm kzppont,TCP, Tool Center Point), az utbbit az orientcijnak nevezzk.

Vilgos, hogy a robot szegmenseinek egymshoz kpesti relatv elhelyezkedseis hasonl elven alapulva rhat le. Rgzthetnk ugyanis a kinematikai lncban egy

1iK koordinta-rendszert a megelz szegmenshez, a kinematikai lncbansoronkvetkezszegmenshez pedig egy iK koordinta-rendszert, s mozgsukat le-

rhatjuk a kt koordinta-rendszer relatv mozgsa segtsgvel. Fizikailag a szeg-menseket rendszerint motorok mozgatjk fogaskerk vagy fogaslc tttelen keresz-tl, a motor elhelyezheta megelzszegmensen, az tttel tengelye pedig tekinthe-ta kvetkezszegmenst mozgat (R rotcis, vagy T transzlcis) csukl tenge-lynek. A 1iK koordinta-rendszer 1iz tengelye vlaszthat a soron kvetkez


2/71

2 Lantos: ROBOTOK PROGRAMOZSAszegmenst mozgat motor tttelnek tengelyeknt, a iK koordinta-rendszer ix

tengelye pedig merlegesnek a megelz(sajt) s a kvetkezcsukltengelyre. A

iK koordinta-rendszer iz tengelye vlaszthat a kvetkez csukltengelynek,

amelyen ix kimetszi a iK origjt. A rszleteket a robot Denavit-Hartenberg para-

mtereinek trgyalsakor fogjuk megismerni. Elbb azonban sszefoglaljuk a fizikairendszerek (robotok, replgpek, helikopterek stb.) geometriai/kinematikai modell-jeiben szerepet jtsz klnfle orientci jellemzseket, valamint a pozci s ori-entci lerst homogn transzformcival.

1.1 Kinematikai alapfogalmakTekintsnk kt (jobb sodrs) derkszgkoordinta-rendszert (rviden keretet, an-golul frame), jellje ezeket Ks K , s legyenek a bzisvektoraik rendre kji ,, s

kji ,, (lsd 1.1. bra).

1.1. bra. Keret (frame) koncepci

Hatrozzuk meg a koordinta-rendszerek vektorai kztti kapcsolatot:

.)()(

)(

333231232221

131211

332313

322212

312111

kpaaajpaaa

ipaaaprkjir

k,pjpipp

k,ajaiak

k,ajaiaj

k,ajaiai

,kjir

zzyxyzyx

xzyxzyx

zyx

zyx

++++++++

++++=+=++=

++=

++=

++=

++=

++=

(1.1)

Ezek az sszefggsek mtrix alakban is felrhatk:

x

y

z

K

Kp

P

x

y

z

rr

x

y


3/71

1. ROBOT KINEMATIKAI ALAPOK 3

bzisban

azfelrva

,,

333231

232221

131211

kji

kji

,

p

p

p

aaa

aaa

aaa

z

y

x

z

y

x

z

y

x

444 3444 21

+

=

(1.2)

prAr += (1.3)

Hozzvve ehhez az 11= azonossgot, a Ks K keretek kztti A orientcivltozst s a keretek origja kztti p pozci vektort sszevonhatjuk egy Tho-

mogn transzformciban:

,

110001333231

232221

131211

=

z

y

x

z

y

x

z

y

x

paaa

paaa

paaa

(1.4)

=

=

=

1:

11000:

1101

T

pnmlpAT

. (1.5)

rTr = . (1.6)

Itt 4Rr s 4Rr jellik 3R s 3R homogn koordintit.

A Thomogn transzformci kt fontos jelentssel rendelkezik:

i) Tmegadja K orientcijt s pozcijt K-hoz kpest.ii) Tlehetv teszi a P pont koordintinak meghatrozst K-ban, ha ismerjk a

koordintit K -ben.

Clszera Thomogn transzformcit indexekkel elltni annak kihangslyoz-sra, hogy a K -hz tartoz pnml ,,, komponenseit a Kbzisban kell megadni:

KK

T ,

. Jl lthat, hogy az indexek sorrendje az rtelmezsben lnyeges.

Homogn transzformcik egymsutnja a homogn transzformcik szorzatvalrhat le:

+=

=

101010121212211

21 TTT

ppAAApApATT . (1.7)


4/71

4 Lantos: ROBOTOK PROGRAMOZSA1.1.1 Az orientci jellemzse forgatsokkalTekintsk elszr csak az orientcimegvltozst. Tegyk fel, hogy eredetileg Ks K egybeestek, s Kvalamelyik tengelye krl K el lett forgatva szggel (apozitv forgsirny a jobb csavar-szably szerint rtelmezett), s tekintsk az elfor-gats utni llapotot. Bevezetjk a kvetkezkonvencit a jellsekre:

cos:=C , sin:=S , )cos(: +=C , )cos(: 2112 qqC += stb. (1.8)

Akkor (1.2) szerint az elemi forgatsok (rotation) kvetkezk lesznek:

( ) ( )

=

=

=

100

0

0

100

0cossin

0sincos

bankifejezverotrotrot

CS

SC

kj,i,

kjiz,Rot4434421

(1.9)

( ) ( )

==

CS

SC

kjiy,Rot

0

010

0

rotrotrot (1.10)

( ) ( )

==

CS

SCkjix,Rot

0

0

001

rotrotrot (1.11)

Legyen tltalnos irnyvektor, 1=t . Tegyk fel, hogy eredetileg K s K

egybeestek, s forgassuk el K -t a t tengely krl szggel. A forgats egy

),( tRot lineris transzformci, amelynek mtrixa ),( tRot :

( ) sincos1cos),(rot xtxt,txxtx +>


5/71


A forgats inverze a visszaforgats, teht a forgats tkrl szggel. Tudjuk,

hogy az (1.14) egyenletben szerepl ][ tto szimmetrikus, ][t antiszimmetrikus,

)cos()cos( = s )sin()sin( = , ezrt

TtRottRot )],([)],([ 1 = . (1.15)

Ortonormlt derkszg koordinta-rendszerek esetn teht az A orientcismtrix inverze az A transzponltja, s emiatt a Thomogn transzformci inverzeis egyszeren szmthat:

TAA =1 (1.16)

=

=

1010

1

T

TT

T

pAApAT . (1.17)

Az A orientcis rsz a Thomogn transzformciban 933 = elemet tartal-maz. Ezek azonban nem fggetlenek, mert kji ,, egy ortonormlt bzis, teht

1=== kji s 0,,, >=>==


6/71

6 Lantos: ROBOTOK PROGRAMOZSA

+

+

=

=

=

===

CCSCS

SCCSSCCSSSCS

SSCSCCSSSCCC

CS

SC

CS

SC

CS

SC

xRotyRotzRotRPYA KK

0

0

001

0

010

0

100

0

0

),(),(),(),,(,

(1.19)

1.1.2 Az orientci jellemzse kvaternikkalAz orientci jellemezhet kvaternikkal is. A kvaternik )(Q a hromdimenzis

vektortr )( 3R kiterjesztst alkotjk, amelyben szorzs )( , sszeads, szmmal

szorzs, konjuglt s norma van definilva. Legyen 13 ,,, RsRwQq , akkor

definci szerint

.~),0(~),(~

),,(

),(),(),(

),(

),(

222

212112212121

2121221121

qqswqqq

swq

wwsswswswwqq

sswwswswqq

swq

swq

=+==

=

>


7/71


)2

cos,2

(sin:

tq= (1.24)

kvaternit )1( == tq , ahonnan a Rodriguez-kplet felhasznlsval kapjuk, hogy

)0,(~)0,( Arqrq = , (1.25)

ezrt qrq ~ s Ar azonosthatk. Vilgos, hogy ha az orientcit a ),( swq=

kvaternival jellemezzk, akkor ennek ismeretben A meghatrozhat, mert

][])[1()2

sin(/),(arccos2 ++=== tSttCICAwts

o , (1.26)

ahol

=

zzyzxz

zyyyxy

zxyxxx

tttttt

tttttt

tttttt

tt ][ o (1.27)

=

0

0

0

][

xy

xz

yz

tt

tt

tt

t . (1.28)

1.1.3 Az inverz orientcis feladat megoldsaHa szksg van az A orientci

=

zzz

yyy

xxx

nml

nml

nml

A (1.29)

numerikus ismeretben az orientci forgatsokkal val jellemzsnek meghatroz-sra (inverz orientcis feladat), akkor a hrom szoksos jellemzs esetn a kvet-kezkpp jrhatunk el.

Az inverz Rodriguez-feladat megoldsa:

( ) ( ) ( )

],(),(2atan2

2

1

222

=

+++=

++=

CS

mllnnmS

nmlC

xyzxyz

zyx

(1.30a)


8/71


)(sign1

)(sign1

)(sign1

xy

z

z

zx

y

y

yz

x

x

mlC

Cnt

lnC

Cmt

nmC

Clt

=

=

=

(1.30b)

A feladatnak vgtelen sok megoldsa van, ha )1(0 == C . Valban, nulla forgs-szg esetn brmilyen ttengely megfelel.

Az inverz Euler-feladat megoldsa:

i) 0022 + Snn yx

,],(

0

arctan

+

=

x

y

n

n (1.31a)

,arctan z

yx

z

yx

n

nSnC

nC

nSnCS

+

=

=

+=

(1.31b)

.arctanyx

yx

yx

yx

mCmS

lClS

mCmSC

lClSS

+

+=

+=

+= (1.31c)

ii) 00 === Snn yx , szingulris konfigurci

a) )(01 zzCnz ====

=+

=

=

x

y

y

x

l

l

lS

lCarctan

csak + hatrozhat meg (1.32)

b) )(1 zzCnz ====

=

=

=

.arctan

y

x

y

x

m

m

mC

mS

csak hatrozhat meg (1.33)


9/71

1. ROBOT KINEMATIKAI ALAPOK 9Inverz RPY feladat megoldsa:

i) 0022 + Cll yx

],,(

0

arctan

+

=

x

y

l

l (1.34a)

,arctanxy

z

xy

z

lClS

l

lClSC

lS

+

=

+=

= (1.34b)

.arctanyx

yx

yx

yx

mCmS

nCnS

mCmSC

nCnSS

+=

+== (1.34c)

ii) 00 === Cll yx , szingulris konfigurci

a) == 1Slz )(2/ xx ==

=+

=

=

x

y

x

y

n

n

nC

nSarctan

csak + hatrozhat meg. (1.35)

b) )(2/1 xxSlz ====

=

=

=

y

x

y

x

m

m

mC

mSarctan

csak hatrozhat meg. (1.36)

1.2 Robotok kinematikai modelljeSzmos, a gyakorlatban elterjedt ipari robot n. nyltlnc, elgazs nlkli merevrobot, amelynek geometriai modelljt legtbbszr a DenavitHartenberg-alakkal r-juk le. A "merev robot" elnevezs idealizls, s arra utal, hogy a robot garantltpontossgi tartomnyban a rugalmas alakvltozsok hatsa a szegmensekben vagya csuklkban elhanyagolhat.

1.2.1 Csuklkkal sszekapcsolt nyltlnc merev testek geometriaimodelljeDenavitHartenberg-alak (lsd 1.2. bra)

A robot szegmensekbl (link) ll, amelyeket 1-szabadsgfok csuklk (joint) ktnekssze. A DenavitHartenberg-alak esetn a robot tengelyei z -tengely irnyak. A


10/71

10 Lantos: ROBOTOK PROGRAMOZSAcsuklvltoz (joint variable) d(transzlcis csukl esetn) vagy (rotcis csuk-l esetn). A DenavitHartenberg-paramterek ad, (tvolsg) s , (szg).

1.2. bra.A DenavitHartenberg-alak

Tegyk fel, hogy a 1iK keretet mr felvettk. Az 1.2. brbaberajzoltuk a 1iz

s 1ix tengelyeket, ahol 1iz irnya azonos az 1i s az i szegmenst sszekt

1it csukltengely (joint axis) irnyval. Legyen it az i -edik s az 1+i -edikszegmenst sszekt csukltengely irnya. Legyen a iK keret ix tengelye merle-

ges a 1it s it tengelyekre. Legyen iK origja az a pont, ahol ix s it metszi egy-

mst. Legyen e prhuzamos ix -vel s menjen t 1iK origjn, s legyen f prhu-

zamos 1iz -gyel s menjen t iK origjn. Akkor kitr 1it s it tengelyek esetn

i) i a 1iz tengely krli forgats szge, amely az 1ix tengelyt e -be forgatja

( ixe || );

ii) id az eltols a 1iz tengely mentn, amely az e egyenest ix -be mozgatja;

iii) ia az eltols az ix tengely mentn, amely 1iz s ix metszspontjt iK orig-

jba mozgatja;iv) i az ix tengely krli forgats, amely az f egyenest iz -be forgatja.

A tvolsgok eljele pozitv, ha az eltols 1+ iz vagy ix+ irnyban trtnik. Ha-

sonlan a forgsszg pozitv, ha a forgats 1iz vagy ix krl a jobb csavar-szably

szerint pozitv. A 1iK s iK kztti homogn transzformci

2it 1it it

1iq iq 1+iq

1ix

1iz

i e

id

ia

ix

iz

f

i

2i

1i i

1+i

2it 1it it

1iq iq 1+iq

1ix

1iz

i

id

ia

ix

iz

f

i

2i

1i1i


11/71

1. ROBOT KINEMATIKAI ALAPOK 11( ) ( ) ( ) ( )

.

1000

0

1000

00

00

0001

1000

100

0010

001

1000

0100

00

00

1

=

=

=

==

i

i

i

i

i

iiiii,i

dCS

SaSCCCS

CaSSCSC

CS

SC

d

a

CS

SC

x,Rotx,aTransz,dTransz,RotT

ii

iiiiii

iiiiii

ii

iiii

ii

(1.37)

Ha 1it s it prhuzamosak, akkor vgtelen sok kzs normlisuk van. Ekkor ro-

tcis csukl esetn 0=id vlaszthat, transzlcis csukl esetn pedig iK origja

azonosra vlaszthat a 1+iK keret origjval.

Ha 1it s it metszik egymst, akkor vlaszthat 0=ia , s ix prhuzamos

ikk 1i irnyval.

A 0 szegmens esetn 0z egybeesik a 0t tengellyel s x0 merleges 0z -ra.

Az utols, m szegmens esetn mz irnya tetszleges s mx merleges a 1mz s

mz irnyokra.

Nhny specilis esetben (Descartes-fle TTT pozcionls els x tengelye,vagy RPY orientci utols x tengelye esetn) elnysebb lehet, ha nem a z , ha-nem az x tengelyt vlasztjuk azanosnak a csukltengellyel. A iiT ,1 homogn

transzformci tovbbra is rvnyben marad, de a csuklvltoz ia vagy i lesz.

A 1.1 s 1.2 tblzatok a bemutatjk a SCARA s PUMA robotok DenavitHartenberg-paramtereit.

1.1 tblzat. A SCARA RRTR robot DenavitHartenberg-paramterei

i iq i id ia i

1 1 1 1d 1a 0

2 2 2 2d 2a 0

3 3d 0 3d 0 0

4 4 4 0 0 0


12/71


1.2 tblzat. A PUMA RRRRRR robot DenavitHartenberg-paramterei

i iq i id ia i

1 1 1 0 0 90

2 2 2 0 2a 0

3 3 3 3d a3 90

4 4 4 4d 0 90

5 5 5 0 0 90

6 6 6 6d 0 0

1.2.2 Direkt geometriai feladat

Egy nyltlnc, elgazs nlkli merev robot lljon a 0 , 1 , ..., m szegmensekbl(links) s a vgberendezsbl (end effector), amely lehet megfog, szerszm stb.(gripper, tool). A szomszdos szegmenseket transzlcis vagy rotcis csuklk(prismatic or revolute joints) ktik ssze. A transzlcis csuklk a szegmensnek acsukltengely mentn trtnelmozdulst, a rotcis csuklk a tengely krli el-fordulst teszik lehetv. Legyen d vagy a a transzlcis csukl (T) mentn azelmozduls. Hasonlan jellje vagy a rotcis csukl (R) krli forgsszget.Kzs nevk a q csuklvltoz (joint variable).

1.3. bra.Nyltlnc, elgazs nlkli robot elvi smja

A "nyltlnc, elgazs nlkli robot" elnevezs arra utal, hogy a szegmensekmegszmozhatk gy, hogy minden szegmenst pontosan egy szegmens kvet a sz-

1i

m

i

0

iq

1q

mq

0t

1it

1mt

BK

0K

1iKiK

mK EK

TCP(tool center point)

1i

0

iq

1q

mq

0t

1it

1mt

BK

0K

1iKiK

mK EK


13/71

1. ROBOT KINEMATIKAI ALAPOK 13mozs sorrendjben (az m szegmenst a vgberendezs kveti), s az i szegmenspozcija s orientcija csak a megelzszegmens pozcijtl s orientcijtl,valamint az 1i s i szegmens kztti iq csuklvltoztl fgg. A megelz

szegmensek definiljk az 1i szegmenshez rgztett 1iK keret pozcijt s ori-

entcijt, mg qi definilja az i szegmenshez rgztett iK keret relatv pozcijt

s orientcijt 1iK -hez kpest, amely a i,i,KK T:T ii 11 = homogn transzformci-

val jellemezhet. Az 1i s i szegmensek kztti csukltengelyt ti1 jelli (lsd

1.3. bra).

A BK keret a robot alapzathoz van rgztve. A 0K keret a robot referencia ke-rete, amely azonos is lehetne BK -vel, de rendszerint mgis eltr attl annak rdek-

ben, hogy a csuklvtozktl val fggs egyszerbb legyen. Az irnytssal befo-lysolhat csuklvltozk a 0K s mK keretek kztt vannak. A BK s 0K , vala-

mint a mK s EK kztti kapcsolat nem fgg a csuklvltozktl. A csuklk mind

egyszabadsgfokak(one degree of freedom, 1DOF) s a teljes robot mDOF. Arobot egyik lnyeges jellemzje a csuklkplet (joint formula). Pldul RRTRRRegy 6DOF robotot jell, ahol minden csukl rotcis, kivve a harmadikat, amelyiktranszlcis. Annak rdekben, hogy a vgberendezsnek mind a pozcijt, mindpedig az orientcijt is megszorts nlkl irnytani tudjuk, legalbb 6 csuklszksges )6( m . A robot mg ebben az esetben is csak egy korltos munkatrben

(limited workspace) pozcionlhat a szegmensek vges mrete s a csuklvltozklimitlt rtktartomnya miatt.

Humn analgik alapjn beszlhetnk trzsrl ( 0 trunk), karrl ( 1 , 2 , 3 arm), kzcsuklrl ( 4 , 5 , 6 wrist) s kzrl (hand, gripper). A vgberendezsrendszerint cserlhet s az m szegmenshez rgzthet, kitntetett pontja pedig aszerszmkzppont(tool center point, TCP).

Az 1i szegmenshez rgztett 1iK keret brhol lehetne. A gyakorlatban gyak-

ran hasznljuk a DenavitHartenberg konvencit, ahol iiii ad ,,, hatrozza meg a

1iK s iK kztti iiT ,1 homogn transzformcit. Ha kivlasztjuk a j szegmens

egy jP pontjt, amelynek jjj zyx ,, a koordinti a jK keretben, akkor ugyanenneka pontnak a koordinti a 1jK keretben x y zj j j 1 1 1, , , amelyekre teljesl


14/71


=

11

11

1

1

j

j

j

j,j

j

j

j

z

y

x

Tz

y

x

. (1.38)

Lpsrl lpsre alkalmazvn ezt az sszefggst kapjuk, hogy ugyanennek a pont-nak az iii zyx ,, koordinti a iK keretben

=

=

+++

111

1211j

j

j

ji,

j

j

j

j,ji,iii,

i

i

i

z

y

x

Tz

y

x

T...TTz

y

x

, (1.39)

ahol.T...TTT j,ji,iii,ji, 1211 +++ = (1.40)

Itt jiT, rja le a koordinta-transzformcit, tovbb Kj relatv pozcijt s orien-

tcijt Ki -hez kpest. Specilisan

== 10

00121100 T

m,m,m,m,,m,

pAT...TTT , (1.41)

Em,m,B,EB, TTTT = 00 . (1.42)

Vegyk szre, hogy

m,m,,,m A...AAA 121100 = , (1.43)

T,

T,

Tm,mm, AA...AA 10211

10 = , (1.44)

11

121

11

1: +

++

== ii,i,i,jjji,ij, TT...TTT , (1.45)

1100

= Em,EB,B,m, TTTT . (1.46)

Az utols egyenlet azt sugallja, hogy a robot egy n. robot grffaljellemezhet, lsd1.4. bra.

1.4. bra.Robot grf

BK 0K mK EK0,BT m,0T Em,T

EB,T

BK 0K mK EK0,BT m,0T Em,T

EB,T


15/71


A robot grf egy cimkzett irnytott grf. A grf cscsai a keretek, a kereteketlek ktik ssze, az lek cimki a homogn transzformcik, az lek irnytsa bal-rl jobbra megfelel az indexelsnek.

0K -bl mK -be kt klnbzton juthatunk el:

i) elrefel haladva 0K , mK sorrendben,

ii) htrafel haladva mEB KKKK ,,,0 sorrendben.

Ha a nyllal szembe haladst invertlsknt interpretljuk, akkor visszakapjuk az(1.46) egyenletet, feltve, hogy rendre vesszk a homogn transzformcit vagy an-

nak inverzt az lek mentn s sszeszorozzuk azokat. A robot grf ltalnosthatsszetettebb esetekre is (robot, szlltszalag, trgy, kamera).

Szoks direkt geometriai feladatrl beszlni, ha arra keresnk vlaszt, hogyan

fgg EBT , vagy mT ,0 aT

mqqq ),,( 1 K= csuklvltoz vektortl. Rendszerint az

sszefggst szimbolikus (kpletszer) alakban keressk a robot DenavitHartenberg-paramterei s (1.41) felhasznlsval:

direkt geometriai feladat: .Tq m,0a (1.47)

A szimbolikus alak azrt szksges, hogy szmtsi mdszert (lehetleg analitikusmegoldst) tudjunk adni a ksbb vizsgland inverz geometriai feladat megolds-ra.

1.2.3 Inverz geometriai feladatAz inverz geometriai feladat clja a csuklvltozk meghatrozsa, ha adott a vg-berendezs pozcija s orientcija. A csuklvltozk felhasznlhatk referenciajelknt (idfgg alapjelknt) a tengelyek szervohajtsai szmra. A problmramint

m,Tq 0solve= (1.48)

fogunk hivatkozni. A feladat transzcendens egyenletrendszer megoldst ignyli,amelyet ltalban nem tudunk kpletszer(szimbolikus, analitikus) alakban megad-

ni. A gyakorlat szmra fontos egyik esetben azonban a feladat analitikusan is meg-oldhat.

1.1 Ttel (inverz geometriai feladat megoldsa dekompozcival):Ha az utolshrom tengely rotcis s a tengelyek egy kzs pontban metszik egymst, akkor azinverz geometriai feladat felbonthat egy pozcionl s egy orientl rszproblm-ra, amelyek szeparltan megoldhatk.


16/71

16 Lantos: ROBOTOK PROGRAMOZSABizonyts: Legyen pldul 6=m s a robot DenavitHartenberg-alakjban

0654 === aaa s = 06 , akkor

++=

=

=

=

=

=

=

10

1000

100

00100001

10

0

1000

100

00100001

1000

1000

100

00

00

10

0

10

0

0

1000

6063043060

6

63

4

30303030

6

54

4

4330

6060606060

T

,,,,

T

,,,,,

T

,

T

,,

,,,,,

ndndpA

d

A

d

pnml

d

**

**

A

d

AT

pnmlT

(1.49)

s ezrt

6066032130432130 )()( ,,,, ndp,q,qqnd,q,qqp =+ , (1.50)

[ ]6060603213032163 )()( ,,,T,, nml,q,qqA,q,qqA = . (1.51)

Az eredeti inverz problmt kt 3-DOF rszfeladatra bontottuk. Az elsfeladat(1.50), amely egy tiszta pozcionl rszfeladat, amelynek megoldsa legyen

321 ,, qqq . Miutn megoldottuk az elsfeladatot, kiszmthatjuk (1.51) jobb oldalt

s kereshetjk a msodik rszfeladat megoldst a q q q4 5 6, , vltozkban, amelyegy tiszta orientl rszfeladat. A msodik rszfeladat elterjedt ipari robotoknlgyakran vezet az inverz Euler vagy az inverz RPY feladatra, amelyek megoldstkorbban mr trgyaltuk.

1.2.4 A SCARA robot geometriai modellje

A direkt s inverz geometriai feladat megoldst a 4-DOF RRTR SCARA robot ese-tn mutatjuk be. A direkt feladat clja, hogy feldertsk a megfog pozcijnak sorientcijnak fggst a csuklvltozktl. A rotcis csuklk miatt az sszefg-gs nemlineris lesz. Msrszt az irnytsok szmra az szksges, hogy ezt a nem-lineris sszefggst megfordtsuk, mivel az irnytsi cl a csuklk olyan ssze-hangolt mozgatsa, amelynek eredmnyekppen a megfog felveszi a kvnt pozci-t s orientcit. A nemlineris sszefggsek miatt az inverz feladat egy nemline-


17/71

1. ROBOT KINEMATIKAI ALAPOK 17ris egyenletrendszer megoldsa, ami teht messze nem egy lineris algebrai (mtrixinverzis) feladat. A problmt nehezti, hogy az inverz feladat megoldsa a valsidben realizland plyatervezs szmra bemeneti adat, az inverz feladatot ezrtlehetleg itercik nlkl s robusztus numerikus szmtsok bevonsval kell meg-oldani.

A SCARA robot DenavitHartenberg-paramtereit (lsd 1.1. tblzat) az 1.5.bra koordinta-rendszerei alapjn rtelmeztk.

1.5. bra.A SCARA robotnl hasznlt keretek

Az egyes szegmensek kztti homogn transzformcik a kvetkezk:

,

1000

100

0

0

1

1111

1111

1,0

=d

SaCS

CaSC

T ,

1000

100

0

0

2

2222

2222

2,1

=d

SaCS

CaSC

T (1.52)

,

1000

1000010

0001

33,2

=d

T .

1000

010000

00

44

44

4,3

= CS

SC

T (1.53)

A homogn transzformcikat fokozatosan sszeszorozzuk az ered homogntranszformci meghatrozshoz, gyelve arra, hogy a mtrixszorzs nem kommu-tatv:

0x 0y

0z

1

1d

2d

2

1a

2a

1x 1y

1z

2x 2y

2z

3d

43 ,xx 43 ,yy

43 ,zz

4

0x 0y

0z

1

1d

2d

2

1a

2a

1x 1y

1z

2x 2y

2z

3d

43 ,xx 43 ,yy

43 ,zz

4


18/71


,d

CS

SC

T,

=

1000

100

00

00

3

44

44

42 ,dd

SaCS

CaSC

T,

+

=

1000

100

0

0

32

222424

222424

41 (1.54)

.ddd

SaSaCS

CaCaSC

T,

++

+

+

=

1000

100

0

0

321

12211124124

12211124124

40 (1.55)

Itt 40,Tq a a direkt geometriai feladat megoldsa.

A qT , a40 inverz geometriai feladat megfogalmazsakor figyelembe kell venni,

hogy a SCARA RRTR robot csak 4-szabadsgfok, ezrt csak az eredpozcit s amegfog x tengelynek irnyt a vzszintes skban rjuk el:

,12211 xpCaCa =+ (1.56)

,12211 ypSaSa =+ (1.57)

,321 zpddd =++ (1.58)

,124 xlC = (1.59)

.124 ylS = (1.60)A 1q csuklvltoz meghatrozshoz vgezzk el a kvetkeztalaktsokat:

,

,

11122

11122

SapSa

CapCa

y

x

=

=

,22

,2222

21

221111

21

2111

221

2111

2212

22

212

22

aappSpaCpa

SaSpapCaCpapSaCa

yxyx

yyxx

++=+

+++=+

ahol az utols egyenlet alakja DSBCA =+ , amelynek megoldsa

,22

222

BA

DBAABDS

+

++=

(1.61)

.22

222

BA

DBABADC

+

++=

(1.62)


19/71

1. ROBOT KINEMATIKAI ALAPOK 19Mivel teljeslnie kell, hogy 122 =+ CS , ezrt csak kt ),( CS megolds van,

amelyek a )1,1(),( = s )1,1(),( = rtkprokhoz tartoznak. Mivel

),( CS meghatrozza a trnegyedet a skon, ezrt mindkt ),( CS prhoz egy-

egy megolds van, amely( ) CS ,2arctan= , (1.63)

feltve hogy a

0222 + DBA (1.64)

munkatr felttel teljesl.Vlaszthatjuk azt a 1q megoldst, amely a megelz 1q -

hez legkzelebb van.

Ha mr 1q -et meghatroztuk, akkor

,2

1112

a

SapS

y= (1.65)

,2

1112

a

CapC x

= (1.66)

s innen( ).,2arctan 121221 CSqq =+ (1.67)

Az utols lpsek nyilvnvalak:

,213 ddpq z = (1.68)

.,2arctan421 xyllqqq =++ (1.69)

1.2.5 A PUMA 560 robot geometriai modelljeAz RRRRRR csuklkplet PUMA robot 6-szabadsgfok, kielgti adekompozcis felttelt, mivel utols hrom csukljnak tengelyei egy kzs pont-ban metszik egymst. A robot DenavitHartenberg-paramtereit az 1.2. tblzattar-talmazza, amelybl a mrtkad koordinta-rendszerek rekonstrulhatk.

A direkt geometriai feladat megoldsakor a SCARA robotnl bemutatott utat le-het kvetni, azaz a DenavitHartenberg-paramterek s az azokra pl iiT ,1 kplet

figyelembevtelvel meghatrozzuk a 6,,2,1,,1 K= iT ii homogn transzformci-

kat, majd ezekbl kpezzk a 3,22,11,03,0 TTTT = s a 6,55,44,36,3 TTTT = homogn

transzformcikat s ezek sszeszorzsval a direkt geometriai fealadat),,(),,()( 6546,33213,06,0 qqqTqqqTqT = megoldst. Ennek sorn kihasznlhat,

hogy a 3K keret szempontjbl az utols hrom csuklvltoz rendre =4q ,


20/71

20 Lantos: ROBOTOK PROGRAMOZSA=5q , =6q Euler-forgatsokat realizl, ezrt (br a komponens transzformci-

k nem azonosak az Euler-szgeknl megismertekkel, mert a DenavitHartenberg-konvenci szerint a csukltengely mindig z irny) a 3K keretet kveten az h-

rom forgats eredhatsa az orientcira:

++

==

56565

546465464654

546465464654

6546,3 ),,(

CSSCS

SSCCSCSSCCCS

SCCSSCCSSCCC

qqqEulerA (1.70)

A 3,0T homogn transzformci az elshrom homogn transzformci ssze-szorzsa utn a kvetkezalak lesz:

++

+

=

1000

0),(

233222323

1323132122311231

132313212231231

3213,0SaSaCS

CdCSaCSaSSCCS

SdCCaCCaSCSCC

qqqT (1.71)

Az inverz geometriai feladat dekompozcis algoritmusa szerint egy elsinverzpozcionl, majd egy msodik inverz orientl feladatot kell megoldani, ahol a m-sodik egy mr kidolgozott inverz Euler-problma. Az inverz feladat bemeneti adataa specifiklt (vagy a robot grf segtsgvel ms specifiklt adatokbl szmtott)

6,0T eredhomogn transzformci, amelynek numerikusan adott elemeit jellje

=

1000

6,0zzzz

yyyy

xxxx

pnml

pnml

pnml

T (1.72)

A dekompozcis algoritmus alapjn clszerbevezetni a kvetkezjellseket:

zzzyyyxxx ndppndppndpp 666 ,, ===

(1.73)

Ekkor az inverz pozcionl feladathoz a kvetkeznemlineris egyenletrend-szert kell megoldani a trigonometrikus alapsszefggsek felhasznlsval:


21/71


=+

=+++

=++

z

y

x

pCdSaSa

pSSdCdCSaCSa

pSCdSdCCaCCa

23423322

2314132313212

2314132313212

(1.74)

Az elsegyenletet )( 1S -gyel a msodikat pedig 1C -gyel megszorozva s a kt

egyenletet sszeadva egy DBSAC =+ alak egyenlethez jutunk, amelynek

megoldst a SCARA robotnl mr bemutattuk, ezrt 1q meghatrozhat:

1311 qdpCpS yx =+ (1.75)

Kt 1q megoldst kapunk annak megfelelen, hogy a PUMA robot az emberi kezet

(kart s kzcsuklt) kzelti, s adott pozci elrhetjobb kzzel, vagy bal kzzel.A megoldshoz ebbl egyet, pl. a megelz konfigurcihoz legkzelebbit kiv-laszthatjuk.

Ezutn 1q mr ismert, ezrt elemi algebrai talaktsokkal kpezhet

2

2342332

2

2342332

2342332211:

a

SdCaeC

a

CdSapS

SdCaCapSpCe

z

yx

=

+=

++=+=

(1.76)

Mivel azonban 2323 ,CS mg ismeretlen 22 ,CS jobb oldaln, ezrt az

122 =+ CS azonossg tbbszri felhasznlsval, valamint ngyzetre emelssel ssszeadssal prblkozunk:

2334234324

23

2222

232343234233223

24

223

23

2232343234233

223

24

223

23

222

22

22

22

)(2)(2

222222

SapedCdpeadapea

CSdaCdpSapCdSap

SCdaSedCeaSdCaeSaCa

zzz

zzz

++++=

+++++++=+

Bevezetve az


22/71

22 Lantos: ROBOTOK PROGRAMOZSA22

24

23

223443 ),(2),(2 adapeDapedBdpeaA zzz +++==+=

(1.77)

jellseket, ismt egy mr ismers alak egyenletre jutunk, melyet megoldva meg-kapjuk a msodik s harmadik csuklvltoz sszegt:

322323 qqDBSAC +=+ (1.78)

Ismt kt megoldst kapunk, amely kzl vlaszthatjuk pldul a megelzhz leg-kzelebbi megoldst.

Ezutn 22 ,CS jobb oldala mr ismert, ahonnan 2q meghatrozhat:

22arctan

22 ),( qCS (1.79)Vilgos, hogy 2q s 32 qq + ismeretben kpezhet 3q is. Ezzel az inverz poz-

cionl feladatot megoldottuk, a megoldsok kzl vlaszthattunk a megelzmeg-olds kzelben vagy bizonyos konfigurcikat preferlva (jobb/bal kz, als/felsknyk helyzet).

Termszetesen nincs akadlya annak sem, hogy az (1.76) egyenletbl elbb

2323 ,CS -at fejezzk ki analitikusan megoldvn egy kt ismeretlenes lineris egyen-

letrendszert, emeljk ngyzetre 2323 ,CS gy kapott kifejezst, adjuk ssze a kt

egyenletet, amikor is egy jabb DBSAC =+ 22 alak egyenletre jutunk alkalmas

DBA ,, egytthatkkal, amelynek 2q megoldst meghatrozva mr szmthat

2323 ,CS s abbl 322arctan

2323 ),( qqCS + , majd 2q ismeretben 3q is.

Az egyenletrendszer megoldsnl kihasznlhat, hogy

+

=

=

=

afce

bfde

bcadf

e

ac

bd

bcady

x

f

e

y

x

dc

ba 11,

ahonnan (1.76) alapjn kapjuk, hogy

+

=

eCa

pSa

C

S

ad

daz

22

22

23

23

34

43 ,

amelynek megoldsa:


23/71

2. A PLYATERVEZS ALAPJAI 23

)]([1

)]([1

3423224224

23

23

4324223224

23

23

eapdCaaSdada

C

edpaCdaSaada

S

z

z

+++

=

+++

=

(1.80)

224

23

234

243

24

23

22

3444332

3434342

)()()()(

)]()([2

)]()([2

daeapdedpadaaD

eapddedpaaaB

eapdaedpadaA

zz

zz

zz

++++++=

++=

+++=

(1.81)

Miutn meghatroztuk az elshrom csuklvltoz rtkt, behelyettesthetjktkket az 3,0A orientcis mtrixba s meghatrozhatjuk annak numerikus rtkt:

=

2323

2311231

2311231

3213,0

0

),,(

CS

SSCCS

SCSCC

qqqA (1.82)

Ezutn kiszmthat 6,3A numerikus rtke az

====

zzz

yyy

xxx

T

NML

NML

NML

AAAAAAAA :6,03,06,013,06,36,33,06,0 (1.83)

sszefggs felhasznlsval.

Az erednumerikus rtkeket szndkosan zx NL ,,K nagybetk jellik, utalva

arra, hogy numerikusan eltrnek 6,0A kis betvel jellt elemeitl, de ettl eltekintve

az inverz orientcis feladat megoldsakor felhasznlhatk az inverz Euler problmakidolgozott egyenletei (a kisbets elemek helyett a nagybetvel jellteket hasznlva,pl. 4),(2arctan qNN xy == stb.):

6546,3 ,, qqqA Eulerinverz === (1.84)


24/71


2. A PLYATERVEZS ALAPJAIAz irnytsok szmra meg kell fogalmazni, mi legyen a szablyozott jellemzel-rt rtke (alapjel), vagy kvetendidfggvnye (kvetjel), vagyis az n. referen-cia jel. A szablyoz ehhez kpest kpzi a hibajelet (a referencia jel s a szablyo-zott jellemzaktulis rtke kztti eltrst), s a szablyozsi algoritmus clja a hi-bajel hatsnak mrsklse a tranziensek alatt s llandsult vagy kvzi-stacioner l-lapotban a hiba lehetsg szerint teljes megszntetse. Tovbbi cl mg a zavar je-lek hatsnak cskkentse s a szablyozsi rendszer robusztuss ttele a szablyo-zott szakasz (folyamat stb.) paramtereinek vltozsa ellenre.

Mozg rendszerek (robotok, jrmvek stb.) esetn a referencia jelnek a rendszermozgsa szmra elrt plya felel meg, amely idben vltoz, megvalstsra ha-tkony irnytsi algoritmusok szksgesek. A plyatervezs teht a robotirnytsokszmra elengedhetetlen, ezrt nhny alapveteljrst mutatunk be az ipari robotokkrben elterjedt plyatervezsi mdszerek kzl.

2.1 A plyatervezs adtstruktrjaTekintsk elszr a 2.1. brbanbemutatott esetet, amikor a robot megfogjval ko-rbban mr megfogott egy trgyat, s a feladat a hengerszer trgy elhelyezse egyszerelvny blokk furatba. A problmt csap-furat problmnak is szoks nevezni(peg-into-hole problem).

A clunk annak bemutatsa, hogy az alkalmazi oldalon megfogalmazott feladat

egy ltalnos 1,0= TOOLPOSCOORDT m adatstruktrv konvertlhat. En-

nek jobb oldalt kiszmtva megkapjuk az m -szabadsgfok robot erdhomogntranszformcijt, majd erre megoldva az inverz geometriai feladatot a kapott q

vektor tekintheta referencia jel aktulis rtknek a csuklvltoz vektor rtkbenkifejezve. Ennek komponensei elkdhetk egy csukl-filozfij decentralizlt ir-nyt rendszer alrendszereinek alapjel bemeneteire, s ha az alrendszerek szablyo-zsai ezt megvalstjk, akkor a robot a trben az elrt helyzetben (pozciban sorientciban, rviden helyzetben, POSE) lesz.

Tekintsk teht a 2.1. brbanspecifiklt feladat koordinta-rendszereit (rvidenkereteit, frames). A feladatban BK a bzis keret (world frame), 0K a robot referen-

cia kerete, elhelyezkedse a bzishoz kpest az ismert0,B

T homogn transzformci-

val adhat meg. mK a robot utols szegmensnek kerete, az utols szegmenshez

van rgztve az end effector (megfog), amelynek kerete EK , s a vgberendezs a

mozgs egy korbbi fzisban mr megfogott trgyat tart, amelynek kerete objK . A

korbbi mozgsi fzis specifikciibl kinyerhet EobjT , , amely megmondja

numerikusan a vgberendezs elhelyezkedst a trgyhoz kpest.


25/71

2. A PLYATERVEZS ALAPJAI 25

2.1. bra.A csap-furat problma. Koordinta-rendszerek s robot grf


26/71

26 Lantos: ROBOTOK PROGRAMOZSAAz aktulis mozgsi fzisban elvan rva, hogy helyezkedjel el a trgy a szerel-

vny furathoz kpest, amelyet a objHOLET , homogn transzformciban adhat meg

numerikusan a felhasznl. A bzis koordinta-rendszerhez kpest a szerelvny pl-dul egy munka asztalon helyezkedhet el, a hozz rgztett keret BLOCKK , helyzett

numerikusan a BLOCKBT , ismert homogn transzformci reprezentlja. A szerel-

vnyhez kpest a furat meghatrozott pozciban s orientciban van, a furathozrgztett keret legyen HOLEK , elhelyezkedse a szerelvnynek a gpszmrnki gya-

korlatban hasznlt CAD-rendszerben kszlt szerelsi rajzbl kinyerhet s a

HOLEBLOCKT , homogn transzformciv konvertlhat.

Jl lthat, hogy a objHOLET , felhasznli informci helyett a robotirnyts p-

lyatervezsi feladathoz a mT ,0 meghatrozsa szksges, amely a robot grfbl a

korbban megismert interpretcival s 11,,1,, )(

= EobjEmEmEobj TTTT figyelembe-

vtelvel a kvetkezlesz:

44 344 214342144444 344444 211

11,,,,,

10,,0 )(

=

TOOL

EobjEm

POS

objHOLE

COORD

HOLEBLOCKBLOCKBBm TTTTTTT (2.1)

Az itt vizsglt csap-furat problma s szmos ms plyatervezsi feladat a

1,0 **

= TOOLPOSCOORDT m (2.2)

smra vezet, ahol az adatstruktrban szerepl homogn transzformcik kzlCOORD szmos koordinta-rendszertl fgg, POS a felhasznli elrs sTOOL a fizikai vagy logikai szerszm. Esetnkben

objEEmEobjEm TTTTTOOL ,,1,, ==

a logikai szerszm, ahol a fizikai EmT , szer-

szmhoz hozz lett vve a objET , trgy, s a kettegyttesen alkotja a logikai szer-

szmot. Megjegyezzk, hogy magas szint robotprogramozsi nyelvekben vannakutastsok a fizikai szerszm lecserlsre logikai szerszmm.

ltalban a robot mozgsa fzisokra bonthat, pldul a csap-furat problma fel-bonthat a furat megkzeltsre gy, hogy a csap tengelye s a furat tengelye egy-

beessen, de a relatv pozci ne legyen nulla, ezt kvetheti a mozgs a furat kezdet-hez, majd innen a furat tengelye mentn a mozgs a furat aljig. Az elskt fzismozgsirnytssal megvalsthat (szabad mozgs), a harmadik azonban a csap s afurat tengelye kztt mindig meglvkisebb irnytsi pontatlansgok s a szoros il-lesztsek miatt ltalban csak n. hibrid pozci/er irnytssal valsthat meg(korltozott mozgs). Az utbbi sorn tengelyirny ert is ki kell fejteni a szab-lyozs keretben a mozgs mellett, s az er/nyomatk rzkelltal mrt hirtelen


27/71

2. A PLYATERVEZS ALAPJAI 27megnverbl lehet kvetkeztetni arra, hogy elrte a csap a furat aljt, s a csapbetolst a furatba le kell lltani. A hibrid irnyts a csuklvltozkat mrbelsrzkelkn kvl (6-komponens) er/nyomatk rzkelalkalmazst is ignyli,ami jelents kltsgnveltnyez.

A tovbbiakban megfogalmazunk egy plyatervezsi smt egyetlenskalrvltozban, majd kihasznlvn, hogy a csuklvektor s a trben n. Descartes-koordintkban a pozci s az orientci is skalr komponensekkel jellemezhet, acsuklvltozkban s a Descartes-koordintkban trtn plyatervezst a smatbbszri alkalmazsval oldjuk meg.

2.2 Plyatervezs skalrvltozban folytonos gyorsulssalA kvetkezkben egy smt (primitvet) fogunk kidolgozni egyetlen skalrvltoz)(y esetn, amely sorn feltesszk, hogy adottak a skalrvltoz ltal befutand sa-

rokpontok s az elrsi n. abszolt idrtkek (ksbb relatv idre trnk t), to-vbb a gyorsuls abszolt rtke korltozva van egy megengedett maximlis rtkrvn:

},,,,,{}{ KK DCBAyi = , maxy&& adott (2.3a)

},,,,,{}{ KK DCBAi ttttt = abszolt id (2.3b)

Az indul hipotzis a sarokpontok sszektse lineris szakaszokkal, azaz szaka-

szonknt lineris )(ty fggvny megclzsa. Ekkor az id szerinti els derivlt)(ty& szakaszonknt konstans lpcss fggvny lenne, amelynek derivltja, azaz

)(ty -nek a msodik derivltja azonosan nulla, kivve az elsderivlt ugrs helyeit,

ahol Dirac-deltkat tartalmazna, teht nem lenne korltos, szemben az elvrsokkal.Ennek felszmolsra kt lehetsg terjedt el a robotikban.

Az elsszakaszonknt lineris elsderivltat cloz meg , amikor is a msodikderivlt lpcss fggvny (trapz alak sebessgprofil). A mozgstrvnyek alapjnekkor a gyorsuls lpcss fggvnynek ugrshelyein jelents er/nyomatk vlto-zs szksges, ami komoly feladatot r a szablyozsok szmra (nagy hibajel snagy ersts esetn telts a hajtsokban, ami az ersts visszavtelvel kerlhetel, ami viszont nagyobb tranziens s marad hibval jr).

A msodik folytonos gyorsulst cloz meg. Mivel azonban az n. utaz szaka-szon a sebessg konstans s a gyorsuls nulla, ezrt a szakaszok hatrn a sebessg-vltozs megvalstshoz a gyorstsnak nullbl nem nullba kell mennie, tartanikell az eljelet (a gyorstsnak vagy lasstsnak megfelelen), majd ismt nullbakell mennie. Ez a gyorsts szmra folytonos fggvnnyel a legegyszerbben m-sodfok polinommal (parabolval) valsthat meg.


28/71


2.2. bra.A plyatervezs smja folytonos gyorsulssal egy skalvltozban

A plyatervezs teht egy BBB gyorst szakaszra s egy CB uta-z szakaszra bonthat. A knnyebb kezels rdekben t relatv idt vezetnk be,amelynek nulla helye a gyorst szakasz kzepe, a gyorst szakasz teljes hossza pe-dig a B rtk ktszerese:

CBB : ],[ BBBabs Tttt = relatv id

43

2

2

3

1

4

0

322

13

0

212

0

2612)(

23)(

)(:

atat

at

at

aty

atatataty

atatatyBB

++++=

+++=

++=

&

&&

(2.4)


29/71

2. A PLYATERVEZS ALAPJAI 29Mivel az )(ty negyedfok polinomnak 5 egytthatja van, ezrt meghatroz-

sukhoz 5 felttel szksges, amelyek a kvetkezk:

BBCB

B

BCB

B

ABB

B

B

vBy

T

BCvy

BBvy

y

y

+=

==

==

=

=

)(

)(

)(

0)(

0)(

&

&

&&

&&

(2.5)

Behelyettestve a fggvnyekbe ezeket az rtkeket egy lineris egyenletrendszer ke-letkezik az ismeretlen egytthatkra, amelynek analitikus megoldsa a kvetkez:

BABBC

ABBC

B

ABBC

B

ABBC

vvBa

vva

vva

a

vva

)(16

32

4

3

0

4

3

4

3

2

1

30

+=

+=

=

=

=

(2.6)

A paramterek ismeretben egy skalrvltozban a plya a gyorst szakaszonegy negyedfok, az utaz szakaszon pedig egy elsfok polinom lesz:

+

++++=),[,

),[,2612)( 432

2

3

1

4

0

CBBBC

BB

TttvB

tatat

at

at

aty

(2.7)

Az )(ty fggvnyt ),[ CBB Tt kztt hasznlhatjuk, CB elrsekorjra el kell vgezni a plyatervezst az j DCC szakaszra, arra alkalmazvna CBB szakaszra kidolgozott plyatervezsi primitvet.


30/71

30 Lantos: ROBOTOK PROGRAMOZSAVegyk szre, hogy a plya nem pontosan a Bt abszolt idpontban halad t az

elrt B ponton, az eltrs ekkor

BABBC vvaByB )(16

3)0( 4 == (2.8)

A hiba abszolt rtkt B cskkentsvel lehetne cskkenteni, ennek azonban

az elrsok szerint hatrt szab, hogy

max

2max 4

3

4

3)0( y

vvvvayy

ABBC

B

B

ABBC

&&&&&&

== (2.9)

Az elrt sarokpontokon val thaladst a plyatervezsi primitv megtartsa mel-lett egyszereszkzkkel lehet biztostani, ehhez elegendugyanis a kritikusnak te-kintett sarokpontot megduplzni. Ennek ra, hogy a plya befutsi ideje B2 rtk-

kel megn, ha B a kritikus sarokpont, tovbb a sebessg egy pillanatra nullv v-lik. Vilgos, hogy a start s end sarokpontot, mivel azt garantltan be kell tartani, asorozatban mindig meg kell duplzni: },,,,,,,,,{ EEDCBASS KK . Egy specilis

felttel olvashat mg le a 2.1. brbl, nevezetesen teljeslni kell a CBBT +

felttelnek is.

2.3 Plyatervezs csuklkoordintkbanA csuklvektor komponensei skalrok, ezrt azokra komponensenknt alkalmazhataz egyetlen skalrvltozra kidolgozott primitv. Ennek sorn azonban be kell tarta-ni, hogy a csuklhajtsok motorjainak eltr teljestmnyviszonyai miatt az egyescsuklvltozk derivltjaira eltr

maxmax, ii qq &&& maximlis rtkek vannak elrva.

Ezen kvl az egyes csuklk BiCi qq megvltozsnak rtke is eltrlehet, ezrt

amaxi

q& maximlis sebessggel val befutsukhoz csuklnknt eltr id szks-

ges. Msrszt viszont csak azt a csuklt clszermaximlis sebessggel mozgatni,amelyre a sajt befutsi idmaximlis. Nincs rtelme ugyanis ms csuklkat hama-

rabb a clhelyzetbe kldeni, mert hiba rkezik egy csukl korn a clhelyzetbe, haa msik csukl mg nincs a clhelyzetben, mivel a teljes robot mg nem rte el cl-konfigurcit, s nem lehet pldul egy trgy megfogst stb. indtani.

Mivel a flslegesen gyorsan mozg csuklk mozgshoz flslegesen nagyobbkinetikus energia szksges, ezrt clszer n. koordinlt mozgst megvalstani,amikor is csak a legnagyobb idt ignylcsukl mozog maximlis sebessggel, a


31/71

2. A PLYATERVEZS ALAPJAI 31tbbiek pedig ugyanennyi idalatt, de a sajt maximlis sebessgknl lassabban skevesebb energit fogyasztva mozognak (koordinlt mozgs elve).

A plyatervezs smjt csuklvltozkban a 2.3. bramutatja.

2.3. bra.A plyatervezs smja csuklvltozkban

Az itercik vals idben val elkerlsnek rdekben minden rtket (2.9)alapjn egysgesen egyformnak vlasztottunk figyelembe vve, hogy a legnagyobb

iABiBC vv ,, sebessgvltozs rtke a maxiq& sebessgmaximum ktszerese is le-

het:

max

max

2

3max

i

i

i q

q

&&

&= (2.10)

Plyatervezsi algoritmus csuklvltozkban:

}:);({:

:,,,

:

:

}2},{max{max:

,:

}{:

:

)(:

40

max

1

+=, =,


59/71

5. Az ARPS nyelv sszefoglalja 59

MONITOR PARANCSOK

Pontok meghatrozsa

CHANGE locListzza a pont rtkt, meg lehet vltoztatni (j pont generlsa).loc: precz- vagy koordinta pont

HERE loc

Pont rtknek bevitele a memriba (aktulis robothelyzet szerinti jpont generlsa). loc: precz-, koordinta- vagy relatv pont

WHERE[#]Folyamatosan listzza az aktulis pontot. Kilps: RETURN billenty-vel.

LTEACH locPont betantsi zemmd, a betant pulton lv STEP gomb meg-nyomsakor mindig egy j pont troldik el. loc: precz-, koordinta-vagy relatv pont. Pl: LTEACH A s a STEP hromszori hasznlatautn az eredmny A0, A1, A2

Floppy kezels

STORE file[=prg][,prg]...Floppy-ra trol a megadott filenven programo(ka)t s/vagy pontokat.Kiterjeszts: .P programok, .L pontok, kiterjeszts nlkl mindkett

LOAD fileProgram(ok) s/vagy pontok betltse floppy-rl. Kiterjeszts: .P prog-ramok, .L pontok, kiterjeszts nlkl mindkett

FDIRFloppy directory - programok neve, mrete, szabad kapacits

FPACKLemezen lvadatok tmrtse, trendezse

FDEL fileFile trlse floppy-rl. Kiterjeszts: .P programok, .L pontok, kiterjesz-ts nlkl mindkett


60/71

60 Lantos: ROBOTOK PROGRAMOZSAZERO DISK

Floppy lemez formzsa. Vgrehajts eltt megerstst vr: Are yousure (y/n)?

Memria kezels

PDIRMemriban lvprogramok nevnek listzsa

PLIST [prg][,prg]...

Program(ok) kilistzsa (nv nlkl az sszes memriban lv

prog-ramot)

LLIST [loc][,loc]...Pontok kilistzsa (nv nlkl az sszes memriban lvpontot)

LDEL loc[,loc]...Pontok trlse a memribl

DLOAD fileAzokat a programokat s/vagy pontokat trli ki a memribl, amelyeka floppy-n a file-ban vannak

ZERO MEMORYTeljes memria trlse. Vgrehajts eltt visszakrdez: Are you sure(y/n)?

Program vezrls

RUNprg[,nnn]A megadott nevprogram indtsa.nnn nincs, vagy 0 egyszer fut le a program

n n-szer fut le a program=0 vagy nincs a kvetkezlpstl folytatja


61/71

5. Az ARPS nyelv sszefoglalja 61EXIT

A program befejezse (a program vgn lltja le s a RUN szmlljtnullzza)

ENABLE switProgramkapcsol engedlyezse

DISABLE switProgramkapcsol tiltsa

DIA file

Diagnosztikai programok betltse, indtsaCOMprg

Parancsprogram indtsa (parancsprogram: olyan program, amely csakmonitorparancsokat tartalmaz)

Sebessg megadsa

MAXSPEED nn.nMaximlis sebessg megadsa [mm/s]

SPEED nn.n

Aktulis sebessg megadsa [mm/s]SPEED% var

Sebessgtnyezmegadsa (3%..300%)

Egyb

EDIT [prg]Program szerkesztse. Ha nem adunk meg programnevet, akkor azutoljra szerkesztettet hozza fel

C strMegjegyzs parancsprogramokban

LIST COMMANDSLista az rvnyes parancsokrl s utastsokrl

LIST STATUSSttusz listzsa (BASE, TOOL rtke, kapcsolk llsa, sebessgek,ppen fut program aktulis lpsszma)


62/71

62 Lantos: ROBOTOK PROGRAMOZSACAL [nnn],[nnn],[nnn],[nnn],[nnn],[nnn]

Kalibrls.nnn >0 az adott csuklt pozitv irny mozgssal kalibrl-

ja


63/71

5. Az ARPS nyelv sszefoglalja 63

PROGRAM UTASTSOK

Pontokkal kapcsolatos utastsok

HERE locAz aktulis robothelyzetet a megadott nven eltrolja a memriban.loclehet precz, koordinta vagy relatv pont

LOCATE loc=[INVERSE] loc

Pont msolsa, talaktsa. loc lehet precz, koordinta vagy relatvpont

FRAME loc=loc,loc,loc[,loc]zemi keret megadsa. Paramterek sorrendben: orig, x tengely pozi-tv pontja, xy sk pozitv y oldali pontja, eltolt (vgleges) orig

SHIFT loc=[dis],[dis],[dis]Pont eltolsa x, y, z tengelyek mentn

DISTANCE var=loc,locKt pont tvolsgt adja meg. Az eredmny egsz!

MIRROR locTkrzpontot definil

NO MIRRORA tkrzst tiltja

SCALE loc=var,var,varA sklzs bzispontjt s nagytst definilja. Pldul:'SCALE BASE=2000,2000,2000' utn 'BASE(A)' esetn A pont mind-hrom tengely irnyban ktszer olyan messze lesz BASE-tl, mintsklzs nlkl

NO SCALE

A sklzst tiltja

Program elgaztatsok

JUMP lblFelttel nlkli ugrs a megadott cmkre


64/71

64 Lantos: ROBOTOK PROGRAMOZSAIF [INGROUP] varcmp[INGROUP] varTHEN JUMP lbl

Feltteles ugrs

IF IN var[,var][,var][,var]Digitlis bemenetek llapottl fgg feltteles ugrs (maximum 4bemenet S kapcsolata)

CALLprgSzubrutinhvs (paramtertads nincs, minden vltoz s pont glob-lis)

RETURN [var]Szubrutinbl val visszatrs. Ha van var, akkor annyi darab prog-ramsort kihagy a hv programbl visszatrskor. Ha szubrutin vgnnincs RETURN, a program vgrehajts a szubrutin vgn befejezdik.

Mozgat utastsok

GO locA megadott pontba megy interpollt plyn. Specilis eset: 'GOREADY' home pozciba megy

GOS loc

Egyenes plyn megy a kvnt pontba

GO&OPEN locA megadott pontba megy interpollt plyn, ekzben nyitja a megfogt

GO&CLOSE locA megadott pontba megy interpollt plyn, ekzben zrja a megfogt

GOS&OPEN locA megadott pontba megy egyenes plyn, ekzben nyitja a megfogt

GOS&CLOSE locA megadott pontba megy egyenes plyn, ekzben zrja a megfogt

GONEAR [loc],disInterpollt plyn megkzelti a clpontot. A mozgs vgpontja az apont, amely a megadott locponttl a szerszm -z tengelye irnybana megadott tvolsgra van. Ha a pontot nem adjuk meg, az aktulispontbl megy a szerszm -z tengelye irnyban az adott tvolsgra


65/71

5. Az ARPS nyelv sszefoglalja 65GOSNEAR [loc],dis

Abba a pontba megy egyenes plyn, amelytl a megadott pont a szer-szm z tengely irnyban az adott tvolsgra van. Ha a pontot nem ad-juk meg, az aktulis pontbl megy a szerszm -z tengelye irnyban azadott tvolsgra

GOS&WEAVE locA megadott pontba megy egyenes plyn s kzben psztz mozgstvgez a szerszm xy skjban

MOVE [dis],[dis],[dis]

Relatv mozgs az alap koordinta-rendszer x, y, z irnyban interpo-llt plyn

MOVES [dis],[dis],[dis]Relatv mozgs az alap koordinta-rendszer x, y, z irnyban egyenesplyn

TMOVE [dis],[dis],[dis]Relatv mozgs a szerszm koordinta-rendszer x, y, z irnyban inter-pollt plyn

TMOVES [dis],[dis],[dis]Relatv mozgs a szerszm koordinta-rendszer x, y, z irnyban egye-

nes plyn

MOVE JOINT jnt,angA megadott csuklt az adott szggel mozgatja

ALIGNA szerszm z tengelyt a legkzelebbi bzis koordinta-tengely ir-nyba beforgatja gy, hogy kzben a pozci vltozatlan marad. A be-forgats a kt tengelyre merleges egyenes menti forgatssal trtnik.(A mvelet elvgzse nem jelenti azt, hogy a forgats utn a megfogx s y tengelyei is prhuzamos helyzetbe kerlnnek a bzis brmelyiktengelyvel.)

Sebessg belltsa

SPEED nn.nAktulis sebessg megadsa [mm/s]


66/71

66 Lantos: ROBOTOK PROGRAMOZSASPEED% var

Sebessgtnyezmegadsa (3%..300%)

SPEED NEXT nn.nA kvetkezmozgat utastst ezzel a sebessggel hajtja vgre

Szerszm megfog vezrlse

OPENNyitja a megfogt

CLOSEZrja a megfogt

ODELAY n.nnAz OPEN utastst paramterezi, nyits utn a megadott ideig vr

CDELAY n.nnA CLOSE utastst paramterezi, zrs utn a megadott ideig vr

I/O utastsok

OUT var[,var][,var][,var]Digitlis kimenetek lltsa egyedileg

OUTGROUP var=[INGROUP] var[aopr[INGROUP] var]Digitlis kimenetek lltsa csoportosan

RUNOUT var[,var][,var][,var]A megadott kimeneteket adott llapotba lltja, ha a program futsamegszakad

WAIT IN var[,var][,var][,var]Adott bemeneti kombincira vr (S kapcsolat). Specilis esetben jel-re vr. Az esemny bekvetkeztig a program tovbbi futsa fel vanfggesztve.

INCALL var,prg[NOBREAK]Megszakts engedlyezse. Megszakts esetn prg-ra addik a ve-zrls (a megszaktsbl val visszatrskor a megszaktott utastsvgrehajtsra tr vissza.)


67/71

5. Az ARPS nyelv sszefoglalja 67NO INCALL var

Megszakts tiltsa

ZERO OUTS varTO varDigitlis kimenetek nullzsa

Koordinta-rendszer mdostsa

BASE [dis],[dis],[dis],[ang]Alap koordinta-rendszer transzformlsa, a paramterek sorrendben:

dx, dy, dz, do (utbbi az alap z tengelye krl fordul)TOOL [dis],[dis],[dis],[ang],[ang],[ang]

Szerszm koordinta-rendszer transzformlsa. A paramterek sorren-ben: dx, dy, dz, do, da, dt

LTOOL locUgyanaz mint az elbb, csak egy ponttal lehet megadni a transzform-cit

Konfigurci mdostsa

J2 RIGHTKettes csukl (vll) jobbkezes

J2 LEFTKettes csukl (vll) balkezes

J3 UPHrmas csukl (knyk) felsllsban

J3 DOWNHrmas csukl (knyk) als llsban

J5 PLUSts csukl (csuklizlet) pozitv

J5 MINUSts csukl (csuklizlet) negatv


68/71

68 Lantos: ROBOTOK PROGRAMOZSALellt utastsok

STOP ['str'][,var]A programot gy lltja meg, hogy CONTINUE paranccsal tovbb fut-tathat, kirja 'str'-t s 'var' rtkt

HALT ['str'][,var]Ugyanaz mint az elbb, de nem folytathat a programvgrehajts

EXITProgram futs befejezse, a program a vgn ll meg (a RUN szml-

ljt nullzza)

Floppy kezels

LOAD file[NOBREAK]Betlt egy file-t a floppy-rl

DLOAD file[NOBREAK]Azokat a programokat s/vagy pontokat trli a memribl, melyek afloppy-n a file-ban vannak

WAIT LOAD

A floppy mvelet vgrehajtsa utn fut tovbb a program

Egyb utastsok

C [str]Megjegyzs

COMMonitor parancs hasznlata programon bell, pldul: COM CAL

DELAY n.nnMegadott idt ksleltet

ENABLE switProgramkapcsol engedlyezse

DISABLE switProgramkapcsol tiltsa


69/71

5. Az ARPS nyelv sszefoglalja 69PRINT ['str'][,var]

Kpernyre val kirats

SET var=[INGROUP] var[aopr[INGROUP] var]Vltoz rtkadsa

TOL NARROWSzerszm belltsi pontossg vezrlse, pontos

TOL WIDEUgyanaz, mint az elbb, kevsb pontos

WEAVE dis[,n.nn][,n.nn]A psztz mozgst paramterezi. A paramterek sorrendben: am-pltd, peridusid, tartzkodsi id

EDITOR PARANCSOK

EKilps (exit)

IBeszrs (insert). A kurrens sor el szr be, kiszlls: res sor

D [nnn]Trls (delete). Ha nnnnincs megadva, akkor csak a kurrens sor trl-se

P [nnn],[nnn]Listzs. Ha nincs paramter, akkor az els sort adja. Paramterek:kezdsor, sorok szma

R str^strA kurrens sorban cserli az elsstring-et a msodikra (replace)

RA str^strUgyanaz mint az elbb, de az egsz file-ra vonatkozik (replace all)

T GO locA kvetkezutastsokat generlja a kzi betantpult STEP gombjamegnyomsra:

ha a megfog nyitott, akkor 'GO&OPEN loc'ha a megfog zrt, akkor 'GO&CLOSE loc'

a pontok koordinti troldnak a memriban nvekvindexszel


70/71

70 Lantos: ROBOTOK PROGRAMOZSAT GOS loc

A kvetkezutastsokat generlja a kzi betantpult STEP gombjamegnyomsra:

ha a megfog nyitott, akkor 'GOS&OPEN loc'ha a megfog zrt, akkor 'GOS&CLOSE loc'

a pontok koordinti troldnak a memriban nvekvindexszel


71/71

FGGELK 71

FGGELKAz albbiakban nhny utasts hatst, mkdsnek rszleteit mutatjuk be, mate-matikai formba ntve.

Koordintapont eltrolsa s rtelmezse

=

1000

),,(,,,,,

z

y

x

taoEulerTtaozyx BE

HERE B(REL)

Tegyk fel, hogy az aktulis pozci AKTT . Az utasts hatsra a

AKTBREL TTT = 1: pozcit fogja eltrolni.

GO B(REL)

Az utasts hatsra a RELB TT pozci lesz a mozgs clpontja.

BASE z,y,x, (Az ,,, zyx rtkek kizrlagnumerikus rtkek lehetnek!)

Az utasts vgrehajtsa utn a 0BT transzformci rtke a kvetkezlesz:

=

=

1000

100

0

0

1000

),( 10

z

ycs

xsc

z

y

x

zRotTB

TB0 T06 T6E

KB K0 K6 KE

TBE

GONEAR PONT,d(d rtk kizrlagnumerikus rtk lehet!)Az utasts vgrehajtsa vgn a megfog pozcija:

=

1

*T

PONTPONTPONTBE

dT

0

npA

robotokprogramozasa.pdf

Documents