Bangenerering f¨or industrirobot med 6 frihetsgrader19708/FULLTEXT01.pdf · 1400 tas fram f¨or direkt- och inverskinematik. •Kapitel 4 beskriver en banas uppbyggnad och hur position

Bangenerering for industrirobot med 6

frihetsgrader

Examensarbete utfort i Reglerteknikvid Tekniska Hogskolan i Linkoping

av

Daniel Forsman

Reg nr: LiTH-ISY-EX-3519-2004Linkoping 2004

Bangenerering for industrirobot med 6

frihetsgrader

Examensarbete utfort i Reglerteknikvid Tekniska Hogskolan i Linkoping

av

Daniel Forsman

Reg nr: LiTH-ISY-EX-3519-2004

Handledare: Erik Wernholt

Examinator: Mikael Norrlof

Linkoping 14 maj 2004.

Avdelning, Institution Division, Department

Institutionen för systemteknik 581 83 LINKÖPING

Datum Date 2004-05-14

Språk Language

Rapporttyp Report category

ISBN

X Svenska/Swedish Engelska/English

Licentiatavhandling X Examensarbete

ISRN LITH-ISY-EX-3519-2004

C-uppsats D-uppsats

Serietitel och serienummer Title of series, numbering

ISSN

Övrig rapport ____

URL för elektronisk version http://www.ep.liu.se/exjobb/isy/2004/3519/

Titel Title

Bangenerering för industrirobot med 6 frihetsgrader Path generation in 6DOF for industrial robots

Författare Author

Daniel Forsman

Sammanfattning Abstract This thesis studies path generation for industrial robots of six degrees of freedom. A path is defined by connection of simple geometrical objects like arcs and straight lines. About each point at which the objects connect, a region, henceforth called a zone, is defined in which deviation from the defined path is permitted. The zone allows the robot to follow the path at a constant speed, but the acceleration needed may vary. Some means of calculating the zone path as to make the acceleration continuous will be presented. In joint space the path is described by the use of cubic splines. The transformation of the Cartesian path to paths in joint space will be examined. Discontinuities in the second order derivatives will appear between the splines. A few examples of different zone path calculations will be presented where the resulting spline functions are compared with respect to their first and second order derivatives. An investigation of the number of spline functions needed when, given an upper limit of deviation, the transformation back to Cartesian coordinates is made.

Nyckelord Keyword path generation, industrial robot, 6 DOF, cubic spline function

Sammanfattning

Rapporten handlar om bangenerering for industrirobotar med sex frihetsgrader– position och orientering. En bana byggs upp av banprimitiver som linjer ochcirkelbagar, vilka fogas samman i s k zoner dar en viss avvikelse fran banan tillatsfor att gora denna mjukare. Zonen gor det mojligt att folja banan med konstanthastighet, beroende pa hur banan ser ut i zonen kan dock den acceleration somkravs variera.

Forst presenteras nagra satt att bestamma zonbanan sa att accelerationen blirkontinuerlig. Sedan beskrivs hur den kartesiska banan kan transformeras till mo-torvinkelkoordinater dar banan beskrivs m h a kubiska splinefunktioner. Den senarebanan kommer att ha kontinuerliga forstaderivator, men mellan splinefunktionernakommer andraderivatorna att vara diskontinuerliga.

Nagra exempel ges dar zonbanan beraknats pa olika satt varefter de resulteran-de splinefunktionernas forsta- och andraderivator jamfors. Dessa exempel anvandsaven till att undersoka hur manga splinefunktioner som behovs for att man vidtransformering tillbaka till kartesiska koordinater ska fa en avvikelse fran tanktbana understigande ett givet maximalt fel.

Nyckelord: bangenerering, industrirobot, sex frihetsgrader, kubisk splinefunk-tion

Abstract

This thesis studies path generation for industrial robots of six degrees of freedom.A path is defined by connection of simple geometrical objects like arcs and straightlines. About each point at which the objects connect, a region, henceforth called azone, is defined in which deviation from the defined path is permitted. The zoneallows the robot to follow the path at a constant speed, but the acceleration neededmay vary.

Some means of calculating the zone path as to make the acceleration continuouswill be presented. In joint space the path is described by the use of cubic splines.The transformation of the Cartesian path to paths in joint space will be examined.Discontinuities in the second order derivatives will appear between the splines.

A few examples of different zone path calculations will be presented where theresulting spline functions are compared with respect to their first and second orderderivatives. An investigation of the number of spline functions needed when, givenan upper limit of deviation, the transformation back to Cartesian coordinates ismade.

Keywords: path generation, industrial robot, 6 DOF, cubic spline function

i

ii

Forord

Jag vill tacka min examinator Mikael Norrlof, LiTH, for att ha givit mig en intres-sant uppgift och bra vagledning under arbetets genomforande.

Jag vill aven tacka min handledare Erik Wernholt, LiTH, for all hjalp vid rappor-tens slutforande.

iii

iv

Notation

Symboler

Oi, Oi-xyz Ortonormerat hoger koordinatsystem.q Enhetsquaternion.qi Ledvariabel i.q Exakt funktion for ledvariablerna (fran inverskinematik).q Splinefunktion for ledvariablerna.r, R Vektorer och matriser skrivs med fetstil.

rji Vektorn fran rj till ri.

Rji Rotationsmatris for Oi relativt Oj .

T , Tji Homogen transformationsmatris.

Operatorer och funktioner

Slerp (Spherical l inear interpolation) Funktion som utfor en sfarisklinjar interpolering mellan tva enhetsquartenioner.

arctan2(y, x) Funktion som for tan θ = yx

returnerar ett korrekt varde pa θ.s(·) sin(·)c(·) cos(·)

Forkortningar

DH Denavit-HartenbergIK InverskinematikSCARA Selective Compliance Assembly Robot ArmTCP Tool Center Point

v

vi

Innehall

1 Inledning 1

1.1 Bakgrund . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Syfte och upplagg . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Orienteringsrepresentation 3

2.1 Homogena transformationsmatriser . . . . . . . . . . . . . . . . . . . 3

2.2 Representation av orientering . . . . . . . . . . . . . . . . . . . . . . 4

2.2.1 Rotationsmatriser . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2.2 Eulervinklar . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2.3 Vinkel/Vektor . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.2.4 Enhetsquaternioner . . . . . . . . . . . . . . . . . . . . . . . . 8

3 Robotspecifik teori 11

3.1 Direktkinematik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.2 Inverskinematik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3.3 Exempel - Roboten IRB 1400 . . . . . . . . . . . . . . . . . . . . . . 13

3.3.1 Direktkinematik . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.3.2 Inverskinematik . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4 Kartesisk banrepresentation 21

4.1 Banans delar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4.2 Banprimitiver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4.2.1 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4.2.2 Orientering . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.3 Zonbanan . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

vii

viii Innehall

4.3.1 Skarning mellan segment och zon . . . . . . . . . . . . . . . . 26

4.3.2 Interpolering i zonen . . . . . . . . . . . . . . . . . . . . . . . 29

4.4 Interpolering langs banan . . . . . . . . . . . . . . . . . . . . . . . . 33

5 Exempel pa kartesisk zoninterpolering 35

5.1 Linje-Linje . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.2 Linje-Cirkelbage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.3 Linje-Helix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.4 Cirkelbage-Helix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

5.5 Linje-Linje, linjara l-kurvor . . . . . . . . . . . . . . . . . . . . . . . 39

5.6 Kommentar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

6 Interpolering i vinkelkoordinater 43

6.1 Splines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

6.2 Algoritm for bangenerering . . . . . . . . . . . . . . . . . . . . . . . 44

6.3 Felutvardering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

6.3.1 Nagra exempel . . . . . . . . . . . . . . . . . . . . . . . . . . 45

6.3.2 Antal splines for ett visst fel . . . . . . . . . . . . . . . . . . . 48

7 Exempel pa zoninterpolering, vinkelkoordinater 51

7.1 Diskontinuiteter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

7.2 Derivatornas storlek . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

7.3 Sammanfattning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

8 Sammanfattning och fortsatt arbete 59

8.1 Slutsatser . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

8.2 Fortsatt arbete . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

A Denavit-Hartenbergkonventionen 63

Kapitel 1

Inledning

1.1 Bakgrund

Normalt har en industrirobot ett verktyg som man vill ska folja en given bana.Forutom att bestamma dess position langs banan vill man aven styra over dessorientering i rummet. Ett exempel ar om verktyget maste hallas vinkelratt mot enkrokt yta samtidigt som positionen forandras.

I kartesiska koordinater konstrueras banan genom att koppla samman enkla seg-ment som linjer och cirkelbagar. Kring de sammanbindande punkterna definierasett omrade, en zon, dar en mjukare overgang mellan segmenten tillats. Zonen gordet mojligt att folja banan med konstant hastighet; i annat fall maste robotenstannas innan den andrar riktning fran ett segment till nasta. Aven om hastighe-ten halls konstant kan, beroende pa hur overgangen mellan segmenten beraknas,accelerationen bli diskontinuerlig vid zongranserna.

For reglering av roboten maste den kartesiska banan transformeras till motorvin-kelkoordinater. Nar den transformerade banan anvands for styrning av robotenkommer man, i bade position och orientering, att fa en avvikelse fran den onskadekartesiska banan. Denna avvikelse maste tas i beaktande vid transformationen.

Detta examensarbete kan ses som en fortsattning pa ett tidigare arbete av MariaNystrom [17]. Dar undersoks hur den resulterande banan paverkas nar kubiskasplinefunktioner anvands for att beskriva banan i motorvinkelkoordinater. Olikametoder for att skapa mjukare banor tas ocksa fram. En skillnad mellan arbetenaar att man i [17] inte studerar robotens orientering.

1

2 Inledning

1.2 Syfte och upplagg

Syftet med examensarbetet ar att i sex frihetsgrader transformera en kartesisk banatill kubiska splinefunktioner som beskriver robotens motorvinkelkoordinater. Detinbegriper aven hur position och orientering kan bestammas langs banan, i synner-het i zonerna, sa att denna kan genomlopas pa ett mjukt satt utan diskontinuiteteri hastighet eller omorientering.

Rapporten ar indelad som foljer:

• Kapitel 2 ger en beskrivning av orientering och olika satt att representeradenna pa (t ex med enhetsquaternioner eller rotationsmatriser).

• I kapitel 3 tas mer robotspecifik teori upp. En modell av ABB-roboten IRB1400 tas fram for direkt- och inverskinematik.

• Kapitel 4 beskriver en banas uppbyggnad och hur position och orienteringberaknas efter banan och speciellt i zonerna.

• I kapitel 5 ges nagra exempel pa olika banor och olika satt att berakna positionoch orientering i zonen.

• Kapitel 6 beskriver en algoritm for transformering av den kartesiska banantill motorvinkelkoordinater, vilka representeras av kubiska splinefunktioner.

• I kapitel 7 har banorna i exemplen fran kapitel 5 omvandlats till kubiskasplinefunktioner enligt kapitel 6 varefter splinefunktionerna for varje exempeljamfors. Har gors aven en felutvardering av splinefunktionerna.

De i rapporten beskrivna metoderna har implementerats i Matlab [12], och anvantsvid berakning av de givna exemplen. Robotics Toolbox [2] har anvants i vissa delar,och for splineberakningar har de i [15] beskrivna funktionerna anvants.

Kapitel 2

Orienteringsrepresentation

Det har kapitlet beskriver hur man kan representera orientering och position paolika satt. Avsnitt 2.1 visar hur man samtidigt kan representera denna informa-tion i en sa kallad homogen transformationsmatris, vilken underlattar notation ochberakningar.

Till stor del bygger innehallet pa [19], men med delvis forandrad notation. I [7]beskrivs hur rotationer kan goras med hjalp av quaternioner. Det gors bl a aven i[4] och [3], dar den sistnamnda valdigt utforligt behandlar quaternioner och inter-polering med sadana. En sammanstallning av de artiklar som Sir William RowanHamilton, upphovsmannen till begreppet quaternioner, publicerade i PhilosophicalMagazine ar 1844-1850 finns sammanstallda i [8].

2.1 Homogena transformationsmatriser

En punkt p i ett ON-system O-xyz beskrivs med vektorn

p = pxx + pyy + pzz =

pxpypz

dar x, y och z ar enhetsvektorerna for koordinatsystemet. Om vi har tva ON-system i samma origo kan man genom att projicera enhetsvektorerna pa lampligtsatt

xj=(xj ·xi)xi+(xj ·yi)yi+(xj ·zi)ziyj=(yj ·xi)xi+(yj ·yi)yi+(yj ·zi)zizj=(zj ·xi)xi+(zj ·yi)yi+(zj ·zi)zi

3

4 Orienteringsrepresentation

uttrycka det ena systemets koordinater i det andras. Rotationen hos Oj relativt Oiuttrycks med rotationsmatrisen

Rij =

xj · xi xj · yi xj · ziyj · xi yj · yi yj · zizj · xi zj · yi zj · zi

vilken har egenskapen (ortogonalitet)

(Rij)

−1 = (Rij)T = R

ji

I det allmanna fallet behover inte origo sammanfalla for tva koordinatsystem. OmOj :s lage relativt Oi anges av vektorn oij fas for punkten p

pi = oij + Rijpj

ochpj = R

jipi −R

jioij

i respektive koordinatsystem.

Om man vill gora flera transformationer i foljd gors detta enklast genom att inforaden homogena transformationsmatrisen

T ij =

(

Rij oij

0T 1

)

samt skriva p som

p =

(

p

1

)

Detta innebar attpi = T i

j pj

ger samma pi som tidigare, fast pa ett mer direkt satt.

Noteras bor dock att matrisen T ij inte ar ortogonal vilket betyder att

(T ij)

−1 6= (T ij)T

Inversen av T ij ar daremot

(T ij)

−1 =

(

(Rij)T −(Ri

j)Toij

0T 1

)

2.2 Representation av orientering

Om positionen ges av en punkt i rummet kan orienteringen ses som en vektorutgaende fran den punkten och kring vilken en rotation kan ske.

Flera satt att representera orientering existerar. Det ar inte sakert att man all-tid har en unik representation, vilket skapar problem nar man vill byta fran enrepresentation till en annan.

2.2 Representation av orientering 5

2.2.1 Rotationsmatriser

En rotationsmatris ar den grundlaggande representationen for orientering. Denar en ortogonal matris som anvands for att rotera vektorer och koordinatsystem(se avsnitt 2.1). Den bestar (i tre dimensioner) av nio element vilket ar mer anantalet frihetsgrader som kravs for att representera orientering - en icke minimalrepresentation.

Rotationsmatriserna for rotation kring de tre koordinataxlarna z-y-x ges av

Rz(α) =

cosα − sinα 0sinα cosα 0

0 0 1

(2.1)

Ry(β) =

cosβ 0 sinβ0 1 0

− sinβ 0 cosβ

(2.2)

Rx(γ) =

1 0 00 cos γ − sin γ0 sin γ cos γ

(2.3)

vilka kan kombineras for att generera generella rotationer.

Varje rotation sker relativt ett givet koordinatsystem. Vill man gora en sam-mansattning av rotationer ar det darfor viktigt att dessa utfors i ratt ordning.En rotation av ett koordinatsystem Oi relativt koordinatsystemet Oj uttrycks som

Rji . Det finns tva satt att gora detta:

• Om efterfoljande rotation sker relativt det koordinatsystem som erholls vidforegaende rotation multipliceras de enligt R0

2 = R01R

12, dar R0

1 ar den forstarotationen.

• Om bada rotationerna sker relativt samma koordinatsystem multipliceras deenligt R0

2 = R12R

01.

2.2.2 Eulervinklar

Eulervinklarna beskriver orienteringen med tre oberoende parametrar - en minimalrepresentation:

Φ = [ϕ ϑ ψ]T

Den rotation som Φ beskriver erhalls genom att skapa en rotationsmatris samman-satt av tre pa varandra foljande rotationer (ϕ, ϑ, och ψ) kring ett lampligt val avrotationsaxlar.

Nar man roterar ett koordinatsystem O kring en av dess axlar far man ett nyttkoordinatsystem O′. Rotationsaxeln ar densamma i de bada systemen. Om man


O

x

y

z

O′

x′

y′

z′

Figur 2.1. Rotation av O kring z ger O′ med z′ identisk med z.

darfor ser till att inte rotera kring samma axel tva ganger i rad, kommer man attfa den onskade rotationsmatrisen. Se figur 2.1.

Nar man val har bestamt vilken foljd av rotationsaxlar man vill anvanda masteman halla sig till detta val och inte byta, eftersom en annan ordning ger ett heltannorlunda slutresultat (rotationsmatriserna kommuterar inte).

ZYZ-vinklar

En representation som beskriver den sfariska handleden pa ett bra satt ar ZYZ-vinklar. Rotationerna sker forst kring z-axeln, sedan kring den nya y′-axeln ochsist kring z′′-axeln (led 4, 5 och 6 i figur 3.3) enligt

R(φ) = Rz(ϕ)Ry′(ϑ)Rz′′(ψ) (2.4)

vilket efter sammansattning blir

R(φ) =

cϕcϑcψ − sϕsψ −cϕcϑsψ − sϕcψ cϕsϑsϕcϑcψ + cϕsψ −sϕcϑsψ + cϕcψ sϕsϑ−sϑcψ sϑsψ cϑ

(2.5)

Om man har en rotationsmatris

R =

r11 r12 r13r21 r22 r23r31 r32 r33

(2.6)

kan man genom jamforelse med matrisen (2.5) se att


• tanϕ = sinϕcosϕ =

sϕsϑcϕsϑ

= r23r13

• tanψ = sinψcosψ =

sϑsψsϑcψ

= − r32r31

• tan2 ϑ = sin2 ϑcos2 ϑ =

s2ϑc2ϑ

=(c2ϕ+s2ϕ)s2ϑ

c2ϑ

=r213

+r223

r233

vilket kan anvandas for att bestamma de till (2.6) svarande eulervinklarna. Detgors enklast genom att anvanda en funktion arctan2(y, x) som for uttrycket tanα =sinαcosα = y

xger ett korrekt varde pa vinkeln α.

Da tan2 ϑ =r213

+r223

r233

blir tanϑ = ±√r213

+r223

r33och vinkeln ϑ bestams till ϑ =

arctan2(±√

r213 + r223, r33) dar man for ett positivt respektive negativt tecken faren losning antingen i intervallet (0, π) eller (−π, 0). Om bade r13 = 0 och r23 = 0(ϑ = nπ) kommer Ry′(ϑ) att vara enhetsmatrisen, och saledes roterar vi kringz-axeln tva ganger i foljd. I detta fall kan inte ϕ och ψ entydigt bestammas.

Losningarna blir nu for ϑ ∈ (0, π)

ϕ = arctan2(r23, r13)

ϑ = arctan2(√

r213 + r223, r33) (2.7)

ψ = arctan2(r32,−r31)

och for ϑ ∈ (−π, 0)

ϕ = arctan2(−r23,−r13)

ϑ = arctan2(−√

r213 + r223, r33) (2.8)

ψ = arctan2(−r32, r31)

2.2.3 Vinkel/Vektor

Orientering kan aven beskrivas som en rotation ϑ kring en (enhets-) vektor r (sefigur 2.2). Givet ett referenskoordinatsystem kan den rotationsmatris som motsva-rar orienteringen tas fram. Det gors genom att forst vrida in vektorn r till en avreferenskoordinatsystemets axlar, rotera kring denna enligt ϑ och sedan vrida till-baka r till det ursprungliga laget. Gor man detta kring axlarna z och y far manfoljande uttryck for rotationsmatrisen:

R(ϑ, r) = Rz(α)Ry(β)Rz(ϑ)Ry(−β)Rz(−α)

dar α och β ar de vinklar man behover vrida r med for att na z-axeln.


x

y

z

rx

ry

rz

r

α

β

ϑ

Figur 2.2. Rotation kring en axel.

2.2.4 Enhetsquaternioner

En enhetsquaternion beskriver aven den en rotation ϑ kring en axel r (enhetsvek-tor). Man skriver den som q =<η, ǫ> dar

η = cosϑ

2

ǫ = sinϑ

2r

och

‖q‖2 = η2 + ǫTǫ = cos2ϑ

2+ sin2ϑ

2rTr = 1

eftersom r ar en enhetsvektor.

Multiplikation av tva quaternioner q1 =<η1, ǫ1> och q2 =<η2, ǫ2> gors enligt

q1∗q2 =<η1η2 − ǫT1 ǫ2, η1ǫ2 + η2ǫ1 + ǫ1 × ǫ2>

dar × ar kryssprodukten. Detta motsvarar multiplikationen R1R2 av tva rota-tionsmatriser.

Inversen av en quaternion ar

q−1 =<η, −ǫ>

vilket ger

q∗q−1 =<1, 0>

motsvarande RR−1=I, dar I ar enhetsmatrisen.


x y

z

vvrot

r

Figur 2.3. Rotation av en vektor kring en annan.

Den mot en enhetsquaternion svarande rotationsmatrisen har utseendet

R(η, ǫ) =

2(η2 + ǫ2x)− 1 2(ǫxǫy − ηǫz) 2(ǫxǫz + ηǫy)2(ǫxǫy + ηǫz) 2(η2 + ǫ2y)− 1 2(ǫyǫz − ηǫx)2(ǫxǫz − ηǫy) 2(ǫyǫz + ηǫx) 2(η2 + ǫ2z)− 1

(2.9)

och det inversa problemet (med R enligt 2.6) har losningen

η = 12

√r11 + r22 + r33 + 1

ǫ = 12

sgn(r32 − r23)√r11 − r22 − r33 + 1

sgn(r13 − r31)√r22 − r33 − r11 + 1

sgn(r21 − r12)√r33 − r11 − r22 + 1

(2.10)

dar η ≥ 0 motsvarar en vinkel ϑ ∈ [−π, π]. Funktionen sgn(x) ar definierad som

sgn(x) =

{

+1, x ≥ 0

−1, x < 0

Om man vill rotera en vektor v med vinkeln θ kring en annan vektor r medquaternioner kan man gora pa foljande satt (se figur 2.3):

• Skapa quaternionerna qv =< 0, v‖v‖ > och qr =< cos θ2 ,

r‖r‖ sin θ

2 >

• Utfor multiplikationerna q =< η, ǫ >= qr ∗ qv ∗ q−1r

• Lat vrot = ‖v‖ · ǫ beteckna den resulterande vektorn.

I rapporten kommer enhetsquaternioner att anvandas for att representera oriente-ring och berakna rotationer. Detta pa grund av den enkelhet varmed detta latersig goras.


Kapitel 3

Robotspecifik teori

Industrirobotar finns i manga utforanden, som alla har gemensamt att de bestarav ett antal lankar och sammanbindande leder. Lederna kan vara av tva typer- prismatiska och roterande, dar de prismatiska lederna utfor en translateranderorelse. Om man kopplar lankarna i en foljd, fran forsta till sista lanken, kallas detfor en oppen kinematisk kedja. Om daremot nagra lankar bildar en slinga, kallasdet for en sluten kinematisk kedja.

For att kunna positionera ett objekt med en robotarm kravs tre frihetsgrader. Villman sedan lata objektet ha en viss orientering relativt ett referenskoordinatsystemkravs ytterligare tre. Totalt behovs alltsa sex frihetsgrader (och darmed sex leder)for en godtycklig positionering och orientering av ett objekt. Om man later antaletleder overstiga antalet frihetsgrader har man en kinematiskt redundant robot.

En robot delas vanligen in i en arm och en handled, dar armen skoter positioneringoch handleden orientering. Beroende pa vilka typer av leder man valjer, och hur deplaceras i strukturen, far man olika typer av robotar (med olika egenskaper): an-tropomorfisk, cylindrisk, kartesisk, sfarisk och SCARA-robot. Den antropomorfiskaarmen bestar av tre roterande leder enligt figur 3.1 och har i princip ett sfarisktarbetsomrade.

Handleden kan aven den se ut pa olika satt, men den mest generella ar den sfariskahandleden. Denna bestar av tre roterande leder vars rotationsaxlar skar varandra ien punkt, och darmed kan godtycklig orientering uppnas.

Pa handledens yttersta ande brukar ett verktyg eller gripdon av nagot slag mon-teras. Denna punkt kallas for TCP (Tool Center Point) eller arbetspunkt. Ettverktygskoordinatsystem kan kopplas till TCP sa att man kan styra position ochorientering for det verktyg som anvands (annars styr man TCP).

Robotens lankar och leder kan beskrivas med lokala koordinatsystem och relationermellan dessa. For varje led (prismatisk eller roterande) ansatts en ledvariabel (qi)som beskriver ledens tillstand (translaterad stracka eller roterad vinkel). Vardet av

11

12 Robotspecifik teori

Figur 3.1. Tre roterande leder placerade sa att en antropomorfisk arm fas.

ledvariablerna bestammer position och orientering for roboten. Med direktkinematikoch inverskinematik gar man fran en beskrivning till den andra.

Ett exempel pa en antropomorfisk robot med sfarisk handled ar IRB 1400 franABB. For denna tas en kinematisk modell fram (och implementeras i Matlab) somanvands vid berakningar i efterfoljande kapitel.

Om modellering av industrirobotar finns i att lasa i bl a [14] och [19].

3.1 Direktkinematik

Med direktkinematik bestammer man position och orientering som en funktion avledvariablerna. Till skillnad fran det inverskinematiska problemet far man alltid enunik losning.

For en robot av strukturen oppen kinematisk kedja beraknar man for varje lanktransformationsmatrisen relativt foregaende lank i kedjan. Nar man sedan multi-plicerar ihop dessa far man den slutliga transformationsmatrisen for position ochorientering relativt den forsta lanken. (Om roboten ar av strukturen sluten ki-nematisk kedja far man gora pa ett lite annorlunda satt, vilket inte kommer attbehandlas har.)

Dessa berakningar kan systematiseras genom att folja Denavit-Hartenbergkonven-tionen (appendix A) som for varje lank skapar ett koordinatsystem vilket beskrivs

3.2 Inverskinematik 13

av ett antal parametrar. Parametrarna anvands for att ta fram den transforma-tionsmatris som beskriver hur en lank ar kopplad till nasta.

Ett exempel pa berakningar for roboten IRB 1400 finns i avsnitt 3.3.

3.2 Inverskinematik

Med inverskinematik (IK) soker man varden pa ledvariablerna vilka gor att robotenfar en given position och orientering. Det ar inte alltid mojligt att losa detta problem- losning kanske saknas eller ocksa existerar flera losningar (mojligen ett oandligtantal). Det ar heller inte sakert att man kan berakna ett analytiskt uttryck forledvariablerna.

I vissa fall kan berakningarna delas upp i ett positionerings- och ett orienterings-problem. Det galler speciellt for robotar av samma struktur som IRB 1400 (antropo-morfisk robot med sfarisk handled). Det betyder att ledvariablerna for handledensposition (de tre forsta) och de for handledens orientering (de tre sista) loses separat.

I [19] finns en losningsgang (Algoritm 1) for en antropomorfisk robot med sfariskhandled beskriven. Denna kommer att illustreras med exemplet i nasta avsnitt.

Algoritm 1 Inverskinematik for en antropomorfisk robot med sfarisk handled.

1: Berakna handledens position pw(q1, q2, q3) som pw = p− d6a dar p ar TCP:sposition, d6 en DH-parameter (appendix A) och a den enhetsvektor som sam-manfaller med rotationsaxeln for den sista leden (riktad utat). Se figur 3.2.

2: Los IK-problemet for att finna (q1, q2, q3).

3: Berakna R03(q1, q2, q3) som ger handledens orientering relativt den forsta leden.

4: Berakna R36(q4, q5, q6) = (R0

3)TR, dar R ar den rotationsmatris som beskriver

orienteringen i TCP.

5: Los IK-problemet for att finna (q4, q5, q6).

3.3 Exempel - Roboten IRB 1400

Som ett exempel pa kinematiska berakningar kommer en modell av roboten IRB1400 att tas fram, vilken sedan kommer att implementeras i MATLAB med hjalp avRobotics Toolbox [2]. Da denna bara kan hantera robotar av strukturen oppen kine-matisk kedja maste modellen anpassas darefter. (I [16] finns beskrivet en MATLAB-toolbox for bangenerering i tre frihetsgrader.)


TCP

n

s

a

p

pw

Figur 3.2. Antropomorfisk arm med sfarisk handled.

3.3.1 Direktkinematik

For att kunna anvanda modellen i Robotics Toolbox far man anse att roboten arformad som en oppen kinematisk kedja, med sex roterande leder och sju lankar.Figur 3.3 visar ledernas placering (lederna ligger i samma plan) och rotations-riktningar. Modellen kan nu tas fram enligt Denavit-Hartenbergs (DH) metod (seappendix A).

#1

#2

#3#4

#5

#6

Figur 3.3. Axlar och sidovy med mattsattning (mm) for IRB 1400.

Det forsta steget ar att bestamma riktningen for robotens axlar, samt att numreradessa med borjan i den forsta leden (se figur 3.4). Detta har gjorts pa sa satt att

3.3 Exempel - Roboten IRB 1400 15

parallella leder har lika riktning. Numreringen borjar med axel z0 i led nummerett, och fortsatter till axel z6 som tillhor koordinatsystemet for TCP.

0

0.5

1

−0.20

0.2

−0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

z6z

5

z4

x

Rotationsaxlarnas riktning

z3

z2

z1

z0

y

z

0

0.5

1

−0.20

0.2

−0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

z6

y6

z5

y5

z4

x6x

5x

4

y4

x

Koordinatsystemens placering

z3

x1

y3

z2

z1

x0

x3

x2

y1

z0

y2

y0

y

z

Figur 3.4. Rotationsaxlarnas riktning och numrering (vanster) samt lankarnas koordi-natsystem (hoger).

Lankarnas koordinatsystem har fatt placeringar enligt figur 3.4. Tillsammans medmatten fran figur 3.3 kan modellens DH-parametrar beraknas (tabell 3.1). DH-parametrarna anvands slutligen till att bestamma de homogena transformations-matriser som beskriver robotens direktkinematik.

Lank (i) ai αi di θi1 150 +π

2 475 02 600 0 0 +π

23 120 +π

2 0 04 0 −π2 720 05 0 +π

2 0 06 0 0 85 0

Tabell 3.1. DH-parametrarna (matt i mm och radianer)

Modellen kan implementeras i MATLAB med Robotics Toolbox enligt figur 3.5.Funktionen link skapar ett lankobjekt utifran DH-parametrarna for lanken. Funk-tionen robot skapar sedan ett robotobjekt av angivna lankar.


%

% MATLAB-kod for roboten IRB 1400 i Robotics Toolbox

%

L1 = link([ pi/2 0.150 0 0.475 0], ’standard’);

L2 = link([ 0 0.600 pi/2 0 0], ’standard’);

L3 = link([ pi/2 0.120 0 0 0], ’standard’);

L4 = link([-pi/2 0 0 0.720 0], ’standard’);

L5 = link([ pi/2 0 0 0 0], ’standard’);

L6 = link([ 0 0 0 0.085 0], ’standard’);

L2.offset = +pi/2;

r = robot({L1,L2,L3,L4,L5,L6}, ’IRB 1400’);

Figur 3.5. MATLAB-modell av roboten IRB 1400

3.3.2 Inverskinematik

IK for position

Om man ser roboten fran sidan (som i figur 3.3) och antar att inga begransningarfinns gallande varden pa ledvinklarna qi, kan man fa upp till fyra olika losningarfor positionen (figur 3.6) beroende pa vilka intervall q1 och q2 tillhor. Singularapunkter existerar dar en entydig losning inte kan bestammas, sa aven punkter darlosning saknas.

Av alla vinklar ar q1 den enklaste att losa. Genom att forst projicera vektorn pwi xy-planet (figur 3.7) och sedan anvanda (MATLAB-) funktionen ATAN2(y,x) fasett varde q1 i intervallet [−π,+π]. (I den singulara punkten (x, y) = (0, 0) kan dettainte entydigt bestammas.) Nu finns det tva mojliga val av q1

q1 =

{

q1 shoulder right

q1 + π shoulder left(3.1)

Om man later lederna 2, 3 och 5 utgora hornen pa en triangel i koordinatsystemetfor led 2 (O2) kan vinklarna q2 och q3 beraknas genom betraktande av figur 3.8.

Eftersom cos(π − q3) = − cos q3 ger cosinussatsen (b3 =√

a23 + d2

4):

q3 =

{

+arccos(c2−a2

2−b2

3

2a2b3) elbow up

− arccos(c2−a2

2−b2

3

2a2b3) elbow down

Summan av q2 och den mellan strackorna a2 och c bildade vinkeln ar identisk medvinkeln mellan y-axeln och linjen fran origo till punkten (px, py). Tangens for dessa

vinklar ar b3 sin q3a2+b3 cos q3

respektive pxpy

vilket for valt q3 ger


(a) Shoulder right, elbow up (b) Shoulder right, elbow down

(c) Shoulder left, elbow up (d) Shoulder left, elbow down

Figur 3.6. De mojliga losningarna shoulder left/right, elbow up/down.

q2 = arctan2(b3 sin q3, a2 + b3 cos q3)− arctan2(px, py).

Nar vi nu har valt elbow up eller elbow down kan slutligen q2 och q3 bestammas(δ = arctan d4

a3

):

q2 =

{

π2 + q2 shoulder rightπ2 − q2 shoulder left

(3.2)

q3 =

{

δ − q3 shoulder right

δ + q3 shoulder left(3.3)


00.2

0.40.6

0.81

−0.5

−0.3

−0.1

0.1

0.3

0.50

0.2

0.4

0.6

0.8

1

XY

Z

pw

Figur 3.7. Den streckade linjen visar hur pw projicerats i xy-planet.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1Geometriska samband

a2

a3

d4

b3

c

q2

q3

(px, py)

x [m]

y[m

]

Figur 3.8. Geometriska samband for IRB 1400 da enbart axel 2 och 3 betraktas.

IK for orientering

Efter berakningen R36(q4, q5, q6) = (R0

3)TR far man en matris pa formen

R36 =

n3x s3x a3

x

n3y s3y a3

y

n3z s3z a3

z


ur vilken q4, q5 och q6 kan bestammas med hjalp av den homogena transforma-tionsmatrisen

T 36 =

(

R36 p3

6

0T 1

)

dar

R36 =

c4c5c6 − s4s6 −c4c5s6 − s4c6 c4s5s4c5c6 + c4s6 −s4c5s6 + c4c6 s4s5−s5c6 s5s6 c5

Genom att identifiera lampliga element i de bada matriserna R36 far man losningar

med q5 i nagot av de tva intervallen [−π, 0[ och ]0, π]. Om handleden skulle vararak, (a3

x)2 + (a3

y)2 = 0, far man q5 = 0 samt oandligt manga losningar pa q4 och

q6. I annat fall blir losningarna

q5 ∈]0, π] :

q4 = arctan2(a3y, a

3x)

q5 = arctan2(√

(a3x)

2 + (a3y)

2, a3z)

q6 = arctan2(s3z,−n3

z)

q5 ∈ [−π, 0[:

q4 = arctan2(−a3y,−a3

x)

q5 = arctan2(−√

(a3x)

2 + (a3y)

2, a3z)

q6 = arctan2(−s3z, n3z)

En MATLAB-funktion (se figur 3.9) som utfor beskrivna berakningar har tagitsfram. For ett angivet robotobjekt, onskad position och orientering (i en homo-gen transformationsmatris) samt onskemal om typ av losning (shoulder left/right,elbow. . .) returnerar den varden pa ledvariablerna q1 till q6. Valet av wrist berorpa vilket intervall man vill att q5 ska tillhora (positiva eller negativa varden).

Om en punkt ligger utanfor robotens arbetsomrade kommer ett felmeddelande attvisas. Felet indikeras med err6=0 och q blir en en tom vektor.

Nar en singularitet patraffas returneras en losning q som inte ar entydigt bestamd.Inget fel indikeras (err=0), men en varningstext visas.

Vart att notera ar att denna funktion galler for alla robotar av samma struktursom IRB 1400, forutsatt att DH-parametrarna beraknats utgaende fran koordinat-systemen i figur 3.4.


function [qvec,err]=ik(r,TR,shoulder,elbow,wrist)

%IK - Inverskinematik for robot av samma struktur som IRB 1400.

%

% [q, err] = IK(r, TR, shoulder, elbow, wrist)

%

% r - robotobjekt (Robotics Toolbox)

%

% TR - homogen transformationsmatris som beskriver

% position och orientering for TCP

%

% shoulder: +1=RIGHT, -1=LEFT

% elbow : +1=UP , -1=DOWN

% wrist : +1=POS. , -1=NEG.

%

% Returnerar:

% q - vektor med ledvinklar, q=[q1 q2 q3 q4 q5 q6]

% err - felindikator, ==0 om inget fel, ~=0 annars

Figur 3.9. Beskrivning av funktion for berakning av inverskinematik for robotar avsamma struktur som IRB 1400

Kapitel 4

Kartesisk banrepresentation

Detta kapitel kommer att ge en beskrivning av hur en bana byggs upp och hurposition och orientering for roboten bestams langs en sadan.

For att slippa diskontinuiteter i hastighet och acceleration langs banan anvands s kzoner dar man tillater avvikelser fran banans onskade utseende.

En metod for berakning av banan i en zon presenteras i kapitel 4.3.2.

(For interpolering av orientering (med quaternioner) och i nagra fall aven positionfinns mycket att lasa i t ex [1], [5], [6], [10], [11] och [13].)

4.1 Banans delar

Den bana man vill att robotens verktyg skall folja byggs upp (i ett globalt koordi-natsystem) av ett eller flera segment. Ett segment ar en enkel typ av kurva, en sakallad banprimitiv (se figur 4.3). Nar segmenten kopplas samman sa att slutpunk-ten for ett blir startpunkten for nasta fas en bana av mer godtyckligt utseende.Figur 4.1 visar en bana bestaende av tva linjer och tva cirkelbagar.

Start

Slut

P1

P2 P3

P4

P5

P6

P7

Figur 4.1. En bana bestaende av linjer och cirkelbagar.

21

22 Kartesisk banrepresentation

I gransen mellan tva segment finns en sa kallad zon inlagd. Den ar en sfar medradien Rz inom vilken man tillater avvikelser fran den onskade banan. Den delav banan som borjar i en zongrans, passerar zonen, och fortsatter till nasta zonkallas for en sektion. Figur 4.2 visar detta. I zonerna visar punktad linje segmentenoch heldragen linje visar den i zonen beraknade banan. Zonen existerar alltid, mengenom att lata Rz = 0 kommer dess inverkan pa banan att forsvinna.

Zon

ZonSektion

Segment

Figur 4.2. En bana byggs upp av segment, zoner och sektioner.

Man vill aven att orienteringen av verktyget skall kunna bestammas langs banan.Detta kan goras genom att till segmentens andpunkter koppla information omorientering (i form av enhetsquaternioner), som sedan interpoleras fram for engiven position langs segmentet.

4.2 Banprimitiver

4.2.1 Position

Banprimitiverna (segmenten) bestams av en parameterbeskrivning i ett, for varjesegment, lokalt koordinatsystem O′. Detta system bestams av translationsvektornrc och rotationsmatrisen R sa att r = rc + Rr′ transformerar den i O′ lokala vek-torn r′ till vektorn r i det globala koordinatsystemet. Med parameterbeskrivningen

r′(s) =

x′(s)y′(s)z′(s)

=

R0 cos(θ0s)R0 sin(θ0s)

h0s

=

cos(θ0s) − sin(θ0s) 0sin(θ0s) cos(θ0s) 0

0 0 1

R0

0h0s

(4.1)

dar s ∈ [0, 1] bestammer alla punkter langs kurvan (fran starten i s = 0 till slutet is = 1) och R0, θ0 samt h0 ar konstanter, kan foljande fyra banprimitiver beskrivas:

• Punkt R0 = 0, h0 = 0 (for en punkt i origo)

• Linje R0 = 0, h0 6= 0 (for en linje langs z)

4.2 Banprimitiver 23

• Cirkelbage R0 6= 0, h0 = 0, θ0 6= 0

• Helix (cirkular) R0 6= 0, h0 6= 0, θ0 6= 0

For punkten och linjen finns andra val av konstanter. Att just dessa valts far sinforklaring nedan.

x

y

z

x

y

z

x

y

z

x

y

z

Punkt Linje Cirkelbåge Helix

Figur 4.3. Banprimitiverna punkt, linje, cirkelbage och helix.

Val av parametrar

Valet av parametrar (R0, θ0, h0) och lokalt koordinatsystem (rc och R) gors sa atth0 >= 0 och R0 >= 0. Det innebar att en linje alltid utgar fran origo och foljerz-axeln, samt att en helix ”vaxer” langs samma axel. En cirkelbage kommer alltidatt borja pa avstandet R0 fran origo langs positiva x-axeln och sluta nagonstans ixy-planet. Helixen borjar i samma punkt som en cirkelbage skulle gora och ”vridersig” kring z-axeln pa en tankt cylinder med radien R0. Tecknet pa θ0 bestammerom man har en hoger- eller vanstervriden helix. For cirkelbagen gor valet θ0 > 0att representationen blir entydig (annars kan man ha en rotationsmatris R medz-riktningen omvand mot fallet θ0 > 0 vilken ger den onskade cirkelbagen da θ0 < 0- se figur 4.4).

θ0 > 0 θ0 < 0

P1P1

P2P2

xx

yy

z

z

Figur 4.4. Olika representation av samma cirkelbage.


Om man skapar en linje genom att ange dess andpunkter r0 och r1 far man rc = r0

samt R = [x y z] med z = r1−r0

‖r1−r0‖dar x och y valjs sa att R blir en rotationsmatris

enligt tidigare beskrivning.

En cirkelbage bestams aven den av en start- och en slutpunkt (r1 och r3), men dess-

utom av en pa cirkelbagen mellanliggande punkt (r2). Har blir z = (r2−r1)×(r3−r2)‖(r2−r1)×(r3−r2)‖

.

Langden av ett segment

I beskrivningen ovan anvands index s ∈ [0, 1] for att beskriva ett segment. Dettaindex sager dock inget om hur lang stracka man har forflyttat sig langs segmentetfor ett visst varde pa s. Denna langd kan beraknas till

l(sl) =

sl∫

0

‖drds‖ ds =

sl∫

0

√

(dx

ds)2 + (

dy

ds)2 + (

dz

ds)2 ds

=

sl∫

0

√

(R0θ0)2 + h20 ds = sl ·

√

(R0θ0)2 + h20

och genom att satta sl = 1 far man den totala langden av segmentet:

Lc =√

(R0θ0)2 + h20 (4.2)

4.2.2 Orientering

Orientering pa en linje

Om man i andpunkterna beskriver onskad orientering med hjalp av enhetsquaterni-onerna q0 och q1, kan man med sfarisk linjar interpolering bestamma orienteringeni den position som bestams av indexet s ovan. Denna interpolering sker pa denfyrdimensionella enhetssfaren och de quaternioner som fas ligger pa den storcirkelsom sammanbinder q0 med q1. Det betyder alltsa att q0 overgar i q1 langs denkortaste vagen pa sfaren, vilket ger en mjuk overgang i orientering mellan linjensandpunkter.

En funktion for sfarisk linjar interpolering finns beskriven i [20]:

Slerp(q0, q1; s) =sin((1− s)θ)

sin θq0 +

sin(sθ)

sin θq1 (4.3)

dar q0 · q1 = cos θ och s ∈ [0, 1]. (Slerp star for Spherical l inear interpolation.)

Orientering pa en cirkelbage

Interpolering av orientering pa en cirkelbage gors enklast om man ser cirkelbagensom en rat linje vilken bojts kring en cylinder (figur 4.5(a)). Interpoleringen sker

4.2 Banprimitiver 25

pa linjen som darefter transformeras till en cirkelbage.

Om man i linjens andar placerar identiska koordinatsystem kommer, nar cirkelbagenskapats, systemet i linjens slutpunkt att ha roteras med en vinkel θ relativt syste-met i linjens startpunkt (figur 4.5(b) och 4.5(c)).

(a)

θ

(b)

θ

x y

x′

y′

(c)

Figur 4.5. (a) Cirkelbage som en linje krokt runt en cylinder. (b) Cirkelbagen och linjensedda fran cylinderns z-axel. (c) Rotation av koordinatsystemet i cirkelbagens slutpunkt.

Eftersom man kanner cirkelbagen och orienteringen i dess andar (q0 och q1) kaninterpoleringen nu utforas i tre steg:

1. Kompensera for krokningen genom att ”vrida tillbaka” q1 med vinkeln θ kringcylinderns z-axel. Kalla den modifierade q1 for q′1.

2. Utfor en sfarisk linjar interpolering mellan q0 och q′1 pa samma satt somtidigare.

3. Transformera erhallen q(s) genom rotation med θs i motsatt riktning som i 1.

Pa det har sattet kommer q(s) att for fallet q′1 = q0 alltid vara orienterad pa sammasatt relativt cirkelbagen i alla punkter langs kurvan.

Att interpoleringen inte sker direkt mellan q0 och q1 beror pa att Slerp interpolerarlangs den kortaste vagen pa sfaren. Det betyder att man for en cirkelbage (ellerhelix) med θ0 ≥ 180◦ inte alltid kan fa q(s) enligt foregaende stycke. Figur 4.6 gerett exempel pa en sadan cirkelbage.

Orientering pa en helix

Helixen skiljer sig inte fran cirkelbagen vad galler interpolering av orientering. Ko-ordinatsystemet i slutpunkten ar bara translaterat langs z-axeln, och rotationenkring densamma bestams fortfarande av θ.


Figur 4.6. En cirkelbage med θ0 ≥ 180◦ med pilar som representerar orienteringen.Orienteringen har i vanster figur interpolerats med och i hoger utan hansyn tagen tillkurvans krokning.

4.3 Zonbanan

Banans utseende utanfor zonerna bestams helt av den for varje segment angivnaparameterbeskrivningen. Inuti en zon maste man pa nagot satt berakna en ny bana(zonbanan) sa att denna och atminstone dess forstaderivata blir kontinuerliga izongranserna.

For att kunna gora det maste man forst veta var segmentet och zonen skar varand-ra.

4.3.1 Skarning mellan segment och zon

For att kunna berakna for vilket varde pa s som ett segment passerar en viss zonmaste man kanna typen av segment, segmentets parameterbeskrivning, zonradienRz och laget av zonen (i borjan eller i slutet av segmentet).

Zonen delar segmentet i tva delar. Skarningen med segmentet ges da zonen befinnersig i borjan av segmentet av

‖r(s)− r(0)‖ = Rz , s > 0

och i slutet av segmentet ges skarningen av

‖r(s)− r(1)‖ = Rz , s < 1

Ett allmant uttryck for skarningen mellan ett segment och en sfar (zonen) medradien Rz placerad med centrum pa kurvan fas ur

‖r(s)− r(τ)‖ = Rz

dar τ beskriver sfarens placering (τ = 0 i borjan av segmentet och τ = 1 i slutet).

4.3 Zonbanan 27

Med parameterbeskrivningen

r(s) =

R0 cos(θ0s)R0 sin(θ0s)

h0s

, s ∈ [0, 1]

far man att

‖r(s)− r(τ)‖2 = . . . = 2R20(1− cos(θ0(s− τ))) + h2

0(s− τ)2 = R2z

vilket ger ekvationen

h20(s− τ)2 − 2R2

0 cos(θ0(s− τ)) + 2R20 −R2

z = 0

som galler for de ovan beskrivna segmenten. For variablerna s och τ galler s ∈ [0, 1]och τ ∈ [0, 1].

Skarning med linje

For en linje blir ekvationen for skarning med zonen

h20(s− τ)2 −R2

z = 0

vilket forenklas till

s = τ ± Rzh0

Da endast segmentets andpunkter ar av intresse satts τ till noll eller ett beroendepa zonens lage. Eftersom s bara kan anta varden mellan noll och ett blir resultatetfoljande:

s =Rzh0

(τ = 0)

och

s = 1− Rzh0

(τ = 1)

Harur kan ses att kvoten Rzh0

inte far vara storre an ett, alltsa Rz ≤ h0, men

i praktiska tillampningar anvands Rz ≤ 12h0 for att tva zoner inte ska kunna

overlappa varandra (figur 4.7).

Skarning med cirkelbage

For cirkelbagen fas ekvationen

−2R20 cos(θ0(s− τ)) + 2R2

0 −R2z = 0


Rz = 1

2h0 Rz = 1

4h0 Rz = 1

2h0 Rz = 1

2h0

Figur 4.7. Olika zonradie for ett segment.

eller omskrivet

cos(θ0(s− τ)) =2R2

0 −R2z

2R20

= 1− 1

2

(

RzR0

)2

vilket ger

s = τ ± 1

θ0arccos

[

1− 1

2

(

RzR0

)2]

Zonradiens maximala storlek erhalls ur

−1 ≤ 1− 1

2

(

RzR0

)2

≤ 1

eftersom argumentet till arccos maste tillhora det intervallet for att ett reellt vardeska erhallas. Efter omskrivning fas

Rz ≤ 2R0

For att inte tva zoner ska overlappa varandra far zonradien inte overstiga halvaavstandet mellan cirkelbagens start- och slutpunkter (figur 4.8). Avstandet ar

2R0 sin(θ02

)

vilket resulterar i uttrycket

Rz ≤ R0 sin(θ02

)

Skarning med helix

For den cirkulara helixen galler den allmanna ekvationen

h20(s− τ)2 − 2R2

0 cos(θ0(s− τ)) + 2R20 −R2

z = 0

som inte kan forenklas mycket mer an till formen

(s− τ)2 − 2

(

R0

h0

)2

cos(θ0(s− τ)) =R2z − 2R2

0

h20

4.3 Zonbanan 29

R0R0

θ0

θ0

2R0 sin 1

2θ0

2R0 sin 1

2θ0

Figur 4.8. Avstandet mellan cirkelbagens start- och slutpunkter.

men med τ konstant och

ϕ = θ0(s− τ)

α =θ0R0

h0

β =θ0Rzh0

far manϕ2 − 2α2 cosϕ = β2 − 2α2

Denna ekvation kan givetvis losas numeriskt, men genom att anta att ϕ ar liten (snara τ) kan approximationen cosϕ ≈ 1− 1

2ϕ2 goras vilken leder till ekvationen

ϕ2(1 + α2) = β2

eller

ϕ = ± β√1 + α2

Aven har bor zonradien uppfylla Rz ≤ 12‖r(1) − r(0)‖, men i de fall θ0 ≥ 2π

bor aven Rz ≤ 12‖r( 2π

θ0) − r(0)‖ uppfyllas. Det forsta for att zonerna i helixens

andpunkter inte ska kunna overlappa varandra, det andra for att zonbanan inteska kollidera med helixen utanfor zonen, vilket annars skulle kunna ske. (Att ingenfigur ges beror pa svarigheter med att illustrera pa ett bra satt.)

4.3.2 Interpolering i zonen

Inuti en zon onskas uttryck for position, r(s), och orientering, q(s), for s ∈ [0, 1].

De segment som gar in i respektive ut ur zonen beskrivs av r1(s) och r2(s) (kapitel4.2.1) och skarningarna med zonen fas enligt kapitel 4.3.1 till s1 = s1z respektives2 = s2z (se figur 4.9).


Position

Den del av respektive segment som befinner sig i zonen beskrivs av

r1(s) = r1(s1z + s · (1− s1z))

och

r2(s) = r2(s · s2z)

sa att r1(0) = r1(s1z), r2(1) = r2(s2z) och r1(1) = r1(1) = r2(0) = r2(0) (se figur4.9).

r(s)

r1(s)

r2(s)

r1(s) r2(s)

s1 = s1z

s2 = s2z

s1 = 0 s2 = 1

s1 = 1, s2 = 0

Figur 4.9. Segment och bana for en zon.

Da bada segmenten ar linjer kan man anvanda den s k de Casteljau-algoritmen [9]for att skapa en parabelformad kurva i zonen vars derivata m a p s har sammariktning som derivatan for linjerna. I det enklaste fallet byggs kurvan upp utifrantre punkter enligt

p10(s) = (1− s)p0 + sp1

p11(s) = (1− s)p1 + sp2

p20(s) = (1− s)p1

0 + sp11

dar man kan se att p10 och p1

1 bada ar ekvationer for en linje och att dessa vagsihop sa att en ekvation i s2 erhalls.

Om man istallet for uttrycken for p10 och p1

1 ovan anvander parameterbeskrivning-arna for segmenten, borde man kunna skapa en zonbana for valfri kombination avlinje, cirkelbage och helix:

r(s) = (1− s)r1(s) + sr2(s) (4.4)

Denna funktion uppfyller r(0) = r1(0) och r(1) = r2(1) och ar alltsa (som onskat)kontinuerlig i zongranserna.

4.3 Zonbanan 31

Tyvarr ar dess forstaderivata inte kontinuerlig i dessa punkter. Eftersom

r′(s) = r′1(s) + s(r′

2(s)− r′1(s)) + (r2(s)− r1(s))

blir derivatornar′(0) = r′

1(0) + (r2(0)− r1(0)) 6= r′1(0)

ochr′(1) = r′

2(1) + (r2(1)− r1(1)) 6= r′2(1)

I (4.4) sker en linjar overgang fran r1(s) till r2(s). Genom att ersatta denna medett allmant (olinjart) uttryck α(s) kan man fa viss kontroll over egenskaperna hosr(s). Uttrycket for r(s) blir da

r(s) = r1(s) + α(s) · [r2(s)− r1(s)] (4.5)

med funktionerna r1(s) och r2(s) som forut.

Derivering av (4.5) ger

r′(s) = r′1(s) + α(s) · [r′

2(s)− r′1(s)] + α′(s) · [r2(s)− r1(s)] (4.6)

som genom r′(0) = r′1(0) och r′(1) = r′

2(1) ger foljande krav pa α(s):

α(0) = 0, α′(0) = 0, α(1) = 1, α′(1) = 0 (4.7)

Det gar att finna flera funktioner som uppfyller (4.7), men ett tredjegradspolynom

α(s) =

3∑

j=0

ajsj (4.8)

ar det enklaste valet. Efter derivering kan ett ekvationssystem stallas upp ochfunktionen bestammas.

Om man onskar att alla derivator dn

dsnr(s) upp till n = N ska vara kontinuerliga i

zongranserna far man genom upprepade deriveringar foljande villkor pa α(s):

α(0) = 0

α(1) = 1

dnα

dsn(0) = 0 , n = 1, 2, . . . , N

dnα

dsn(1) = 0 , n = 1, 2, . . . , N

till vilket ett polynom av grad 2N + 1 kan bestammas.

For fallen N = 1, N = 2 och N = 3 beraknas α2N+1(s) till

α3(s) = 3s2 − 2s3 (4.9)

α5(s) = 10s3 − 15s4 + 6s5 (4.10)

α7(s) = 35s4 − 84s5 + 70s6 − 20s7 (4.11)

vilka har utseenden enligt figur 4.10.


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(a) α2N+1

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.25

0.5

0.75

1

1.25

1.5

1.75

2

2.25

2.5

(b) α′

2N+1

Figur 4.10. Funktionen α2N+1 samt α′

2N+1 for N = 1 (punktad linje), N = 2 (streckadlinje) och N = 3 (heldragen linje)

Orientering

Orienteringen for del av respektive segment som befinner sig i zonen beskrivs (paliknande satt som for positionen) av

q1(s) = q1(s1z + s · (1− s1z))

och

q2(s) = q2(s · s2z)

sa att q1(0) = q1(s1z) och q2(1) = q2(s2z) med qi(s) enligt kapitel 4.2.2. I zonenscentrum galler q1(1) = q2(0).

Langs zonbanan kan orienteringen q(s) beraknas med en variant av de Casteljau-algoritmen dar sfarisk linjar interpolering anvands (orienteringen representeras avenhetsquaternioner). Genom att ersatta ri(s) med qi(s) och anvanda funktionenSlerp far man

q(s) = Slerp(q1(s), q2(s); s)

som motsvarighet till (4.4).

Beraknar man positionen i zonen enligt (4.5) maste dock uttrycket

q(s) = Slerp(q1(s), q2(s);α(s)) (4.12)

anvandas for att erhalla korrekt orientering.

4.4 Interpolering langs banan 33

4.4 Interpolering langs banan

Utanfor zonerna

Pa segmenten utanfor zonerna rader det enkla (linjara) sambandet lc(s) = Lc · s,med Lc enligt (4.2), mellan index lc och s. Detta gor att s(lc) latt kan bestammasi en punkt pa banan bara man kanner langden av banan i borjan och slutet avaktuellt segment. Om Lα och Lβ far sta for dessa langder kan index s bestammasfor ett givet lc som

s =lc − LαLβ − Lα

, lc ∈ [Lα, Lβ ]

dar langden av segmentet ges av Lβ − Lα.

Inuti zonerna

I en zon finns normalt inget linjart samband mellan index lc och s. Ej heller finnsnagot analytiskt uttryck for zonbanans langd, men denna kan for tillrackligt storaN approximeras med summan

Lz =

N−1∑

n=0

‖r(n+ 1

N)− r(

n

N)‖

(eller nagon liknande summa som tar hansyn till storleken av normen for varjevarde pa n).

Om man for varje n sparar delsumman och motsvarande varde pa s, kan man skapaen splinefunktion (kapitel 6.1) sz(l), l ∈ [0, Lz] som (approximativt) beskriver s(lc)i zon z.

Langs banan

Varje segment beskrivs av index si. Det si for vilket segmentet skar en zon benamnsfor segment i som siz# dar #-tecknet star for zonens nummer, t ex s3z2 for dettredje segmentets skarning med den andra zonen. Figur 4.11 visar detta for tresegment och tva zoner.

Med index lc ∈ [0, Lc] beskrivs banans punkter fran startpunkten till slutpunkten(Lc=banans langd). Genom att halla reda pa lc for start- och slutpunkt samtzongranserna kan man bestamma vilken bandel och vilket s som beskriver en visspunkt. Om i betecknar bandelens nummer far man att

lc ∈ [Li−1, Li] ⇒ bandel i

En bandel med udda nummer ar ett segment utanfor en zon och en bandel med


s1 = 0

s1 = s1z1

s1 = 1, s2 = 0

s2 = s2z1

s2 = s2z2

s2 = 1, s3 = 0

s3 = s3z2

s3 = 1

Figur 4.11. Index for banans segment.

jamnt nummer ar en zonbana (se figur 4.12). Index s kan nu bestammas som

s =

{

lc−Li−1

Li−Li−1

, i = 2n− 1

sn (lc − Li−1) , i = 2n

dar sn(l) ar den tidigare beskrivna splinefunktionen som kopplar samman index soch index lc pa zonbanan.

#1

#2

#3

#4

#5

Figur 4.12. Numrering av banans delar.

Beskrivna funktioner och metoder har implementerats i Matlab. Dessa anvands ikapitel 5 dar nagra exempel ges pa zonbanor beraknade enligt avsnitt 4.3.2.

Kapitel 5

Exempel pa kartesiskzoninterpolering

Flera exempel pa banor som kombinerar olika typer av banprimitiver kommer attges. For varje exempel har zonbanan beraknats for α3, α5 respektive α7 (enligtkapitel 4.3.2). En jamforelse med en zonbana skapad med linjara l-kurvor, beskriveti [17], kommer ocksa att goras for en bana bestaende av tva linjer.

I kapitel 7 kommer dessa banor att omvandlas till splinefunktioner for robotensledvariabler for ytterligare jamforelser.

De zonbanor som visas i varje exempel kommer att se nastintill identiska ut. Detberor pa att metoden for zoninterpolering med αN framst paverkar derivatornaav zonbanans parameterbeskrivning. Att det blir sa kan ses genom att jamforaekvationerna (4.5) och (4.6) med figur 4.10 dar, for olika N , funktionerna αN harlikartat utseende men deras derivator α′

N klart skiljer sig at.

Den forklaring av figur 5.2 som ges i nasta avsnitt galler aven figurerna 5.3–5.5.

5.1 Linje-Linje

Figur 5.2 ar uppdelad sa att delfigur (a) visar hela banan (med orientering), och(b)–(d) zonbanan for respektive αN . Den tjocka linjen markerar banan, de tunnapunktade linjerna markerar segment och zoner, vilkas skarningar aven marks ut aven liten ring.

Orienteringen i en viss punkt har markerats med en pil med spetsen i punkten.Denna pil ar egentligen ett koordinatsystem som skapats ur den enhetsquaternionsom representerar orienteringen i punkten. Genom att translatera systemet i ne-gativ z-riktning och rita en linje fran origo till z-axelns skarning med banan harpilskaftet skapats. Den svarta respektive gra ”fjadern” markerar x- och y-axlarnas

35

36 Exempel pa kartesisk zoninterpolering

riktning. Eftersom orienteringen avser TCP kan man i figur 3.2 se att x, y och zmotsvarar n, s och a. Figur 5.1 visar pilen sedd ur fyra olika riktningar.

Figur 5.1. Den pil som visar orienteringen, sedd ur fyra riktningar.

Banorna bestar av tva segment med en zon dar dessa mots. Start-, mitt- ochslutpunkterna benamns p1, p2 och p3 betraffande positionen; orienteringen i dessapunkter benamns q1, q2 och q3. Orienteringen i andpunkterna (q1 och q3) har valtssa att z-axeln (TCP) ar vinkelrat mot respektive segment. I var och en av dessapunkter har orienteringen markts ut med en dubbelt sa stor pil som normalt.

I figur 5.2(a) bildar tva linjer av langd 0,60 m en rat vinkel i en zon med radien0,20 m. Startpunkten p1 har koordinaterna (0,4;1) i figuren. Orienteringen har valtssa att z-axeln ligger i det plan som bildas av linjesegmenten. For q1 galler att y-axeln ligger i detta plan, motriktad det forsta segmentet. For q3 ligger x-axeln iplanet, men riktad langs med segmentet. I zonen har q2 valts som ”ett mellanting”av q1 och q3.

5.2 Linje-Cirkelbage

Banan i figur 5.3(a) bestar av en linje och en cirkelbage i ett plan med en zon avradie 0,15 m. Startpunkten p1 har koordinaterna (0,6;1) i figuren. Orienteringenhar valts (som i foregaende exempel) sa att z-axeln ligger i planet. For q1 galler atty-axeln ligger i detta plan, motriktad det forsta segmentet. For q3 ligger x-axeln iplanet, men riktad langs med segmentet. I zonen har q2 valts som ”ett mellanting”av q1 och q3.

5.3 Linje-Helix 37

0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(a) Tva linjer.0.35 0.4 0.45 0.5 0.55 0.6

0.35

0.4

0.45

0.5

0.55

0.6

(b) Zonbana for α3.

0.35 0.4 0.45 0.5 0.55 0.6

0.35

0.4

0.45

0.5

0.55

0.6

(c) Zonbana for α5.0.35 0.4 0.45 0.5 0.55 0.6

0.35

0.4

0.45

0.5

0.55

0.6

(d) Zonbana for α7.

Figur 5.2. (a) Bana bestaende av tva linjer. (b)–(d) Zonbanan for α3, α5 och α7.

5.3 Linje-Helix

Denna bana, figur 5.4(a), bestar av en linje foljd av en helix. Zonens radie ar 0,10 m.Startpunkten p1 har koordinaterna (0,2;0,4;0,5). Orienteringen ar vald med z-axelnvinkelrat mot banan. For q1 och q2 ar x-axeln riktad langs med linjesegmentet ochy-axeln motriktad helixens z-axel. For q3 galler att z-axeln sammanfaller med q2:sy-axel och att x-axeln ar motriktad q2:s z-axel.


0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

1.1

(a) Linje och cirkelbage.0.25 0.3 0.35 0.4 0.45 0.5 0.55

0.25

0.3

0.35

0.4

0.45

0.5

0.55


0.25 0.3 0.35 0.4 0.45 0.5 0.55

0.25

0.3

0.35

0.4

0.45

0.5

0.55

(c) Zonbana for α5.0.25 0.3 0.35 0.4 0.45 0.5 0.55

0.25

0.3

0.35

0.4

0.45

0.5

0.55


Figur 5.3. (a) Bana bestaende av en linje och en cirkelbage. (b)–(d) Zonbanan for α3,α5 och α7.

5.4 Cirkelbage-Helix

Genom att byta linjesegmentet i exempel 5.3 mot en cirkelbage och andra oriente-ringen i startpunkten far man banan i figur 5.5(a). Zonradien, p1, p2 och p3 samtq2 och q3 ar identiska med foregaende exempel. I startpunkten har q1 vridits 180◦

kring (linje-)segmentet sa att z-axeln och y-axeln har fatt motsatt riktning mottidigare.

5.5 Linje-Linje, linjara l-kurvor 39

0.2

0.3

0.4

0.5

0.6

0.7

0.3

0.4

0.5

0.6

0.4

0.5

0.6

0.7

0.8

(a) Linje och helix.0.5 0.55 0.6 0.65 0.7

0.32

0.34

0.36

0.38

0.4

0.42

0.44

0.46

0.48


0.5 0.55 0.6 0.65 0.7

0.32

0.34

0.36

0.38

0.4

0.42

0.44

0.46

0.48

(c) Zonbana for α5.0.5 0.55 0.6 0.65 0.7

0.32

0.34

0.36

0.38

0.4

0.42

0.44

0.46

0.48


Figur 5.4. (a) Bana bestaende av en linje och en helix. (b)–(d) Zonbanan for α3, α5 ochα7.

5.5 Linje-Linje, linjara l-kurvor

Har (figur 5.6) har samma bana som i exempel 5.1 anvants. Zonbanan i figur 5.6(b)ar beraknad med de i [17] beskrivna linjara l-kurvorna. Dessa anvandes ursprungli-gen enbart till att bestamma position i zonen men orientering kan bestammas med


0.2

0.3

0.4

0.5

0.6

0.7

0.3

0.4

0.5

0.6

0.4

0.5

0.6

0.7

0.8

(a) Cirkelbage och helix.0.5 0.55 0.6 0.65 0.7

0.32

0.34

0.36

0.38

0.4

0.42

0.44

0.46

0.48


0.5 0.55 0.6 0.65 0.7

0.32

0.34

0.36

0.38

0.4

0.42

0.44

0.46

0.48

(c) Zonbana for α5.0.5 0.55 0.6 0.65 0.7

0.32

0.34

0.36

0.38

0.4

0.42

0.44

0.46

0.48


Figur 5.5. (a) Bana bestaende av en cirkelbage och en helix. (b)–(d) Zonbanan for α3,α5 och α7.

ett tillagg. Efter anpassning till har anvand notation far man uttrycken

s1 = 1− (1− s1z) · (1− s)2

s2 = s2z · s2

r(s) = r1(s1) + r2(s2)− r2(0)

q(s) = q1(s1) ∗ (q−12 (0) ∗ q2(s2))

for position respektive orientering i zonen (med s ∈ [0, 1]).

5.5 Linje-Linje, linjara l-kurvor 41

0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(a) Bana - linjara l-kurvor.0.35 0.4 0.45 0.5 0.55 0.6

0.35

0.4

0.45

0.5

0.55

0.6

(b) Zonbana - linjara l-kurvor.

0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(c) Bana - α5.0.35 0.4 0.45 0.5 0.55 0.6

0.35

0.4

0.45

0.5

0.55

0.6

(d) Zonbana - α5.

Figur 5.6. Bana och zonbana for tva linjer. For linjara l-kurvor och for α5.

Zonbanan i figur 5.6(d) ar beraknad enligt

s1 = s1z + (1− s1z) · ss2 = s2z · s

r(s) = r1(s1) + α5(s) · (r2(s2)− r1(s1))

q(s) = Slerp(q1(s1), q2(s2);α5(s))

med α5(s) = 10s3 − 15s4 + 6s5 och s ∈ [0, 1].


5.6 Kommentar

Som sades i inledningen blev det inte nagon synbar skillnad mellan zonbanorberaknade med α3, α5 och α7. Trots det kommer valet av funktion att spela enstorre roll nar samma banor transformeras till vinkelkoordinater i kapitel 7.

Kapitel 6

Interpolering ivinkelkoordinater

Den i kapitel 4 framtagna kartesiska banan maste transformeras till motorvinkel-koordinater innan reglering av roboten kan ske. En algoritm for denna transforme-ring beskrivs, dar resultatet ar en splinefunktion som approximerar ledvariablernasvarden langs banan.

I kapitel 7 kommer algoritmen att appliceras pa banorna fran kapitel 5. Det harkapitlet inleds med en beskrivning av kubiska splinefunktioner.

6.1 Splines

Att approximera en funktion f kan goras pa flera satt. T ex kan ett polynom p avgrad N bestammas sa att p(xi) = f(xi) for N+1 punkter xi. Mellan dessa punkterkan dock avvikelsen mellan p och f vara stor och med oscillationer i p dar sadanasaknas i f . Detta galler sarskilt da N antar stora varden.

En splinefunktion s(x) bestar av flera olika polynom si vilka approximerar funk-tionen f(x), x ∈ [a, b] i delintervallen [xi, xi+1] med a ≤ x1 < x2 < . . . < xn ≤ b.Om polynomen ar av grad 2N + 1 kommer s(x) att vara 2N ganger kontinuerligtderiverbar.

Punkterna x1 < x2 < . . . < xn kallas noder eller brytpunkter. Noderna x2, . . . , xn−1

kallas inre noder. Genom att stalla krav pa s(x) i noderna kan man fa en spline-funktion med onskade egenskaper.

43

44 Interpolering i vinkelkoordinater

Kubiska splines

Om man later N = 3 blir si kubiska funktioner, vilka bestams av fyra koefficienterper funktion. Tillsammans med kravet att s och dess derivator s′ och s′′ maste varakontinuerliga i hela intervallet [x1, xn] far man en s k kubisk splinefunktion [18].

For en interpolerande kubisk splinefunktion tillkommer kravet s(xi) = f(xi) inoderna, vilket ger 2(n− 1) villkor pa s(x). Kravet pa kontinuitet for s′ och s′′ gerytterligare 2(n− 2) villkor i de inre noderna. Sammanlagt fas 4n− 6 villkor.

Eftersom man med n noder far n − 1 intervall (och polynom) blir antalet soktakoefficienter for bestamning av s(x) lika med 4(n− 1). Antalet koefficienter ar fleran villkoren som stallts, sa ytterligare tva krav pa s(x) maste goras.

Da s′(x1) = f ′(x1) och s′(xn) = f ′(xn) fas en s k interpolerande kubisk splinefunk-tion med ratta randvillkor och s(x) kan entydigt bestammas.

Kraven pa en interpolerande kubisk splinefunktion med ratta randvillkor blir forapproximation av funktionen f(x):

s(xi) = f(xi), i = 1, 2, . . . , n (6.1)

s′i(xi+1) = s′i+1(xi+1)s′′i (xi+1) = s′′i+1(xi+1)

, i = 1, . . . , n− 2 (6.2)

s′1(x1) = f ′(x1)

s′n−1(xn) = f ′(xn)(6.3)

6.2 Algoritm for bangenerering

I [17] har tva algoritmer for bangenerering i vinkelkoordinater beskrivits. Den enaanvands i zonerna, den andra pa banan mellan zonerna. Algoritm 2 ar en samman-slagning och utvidgning (till sex frihetsgrader) av dessa, med foljande forandringar:

• Ingen uppdelning av banan gors, alla berakningar sker fran lc = 0 till lc = Lc.

• Berakning av orientering har lagts till, med kontroll av maximalt ”oriente-ringsfel”.

• Uppdateringen av steglangden har andrats (andra, battre, alternativ finnsforstas).

For ett visst varde pa lc (med borjan i lc = 0) beraknas position och orientering ipunkterna lc + nh, n = 0, 1, ..., N−1. Ledvariablernas varden i punkterna bestamsmed inverskinematik och anvands till att skapa en splinefunktion qtmp for ledva-riablerna i intervallet. Denna utvarderas sedan i N−1 punkter fran lc + h/2 ochmed samma avstand h (beskrivet i 6.3).

6.3 Felutvardering 45

I de utvarderade punkterna bestams (direktkinematik) position och orienteringsom jamfors med tillatna maximala fel. Om nagon punkt avviker for mycket m a pposition eller orientering halveras h och en ny qtmp skapas och utvarderas. Annars,om qtmp ar till belatenhet, gors en sammanslagning med splinefunktionen q sombeskriver ledvariablerna langs hela banan och steglangden h samt lc uppdateras.

Proceduren upprepas tills dess att hela banan har behandlats.

6.3 Felutvardering

Splinefunktionen ar ju bara en approximation av ledvariablernas varden langs ba-nan, sa ett visst fel i position och orientering kommer att upptrada nar den anvandsvid styrning av roboten.

Om p och q star for onskad position och orientering och p och q star for den positionoch orientering som fas via splinefunktionen kan felen bestammas som foljer.

Positioneringsfel

Mellan punkterna p och p ar det euklidiska avstandet (normen) ett matt pa feleti positionering:

p∆ = ‖p− p‖ (6.4)

Orienteringsfel

Eftersom orienteringen beskrivs med enhetsquaternioner kan vinkeln mellan q ochq anvandas som ett matt pa felet i orientering. Med tidigare definition far man

q • q = ηη + εTε = cosθ∆

och orienteringsfelet (radianer) kan bestammas som

θ∆ = arccos(q • q) (6.5)

6.3.1 Nagra exempel

Figurerna 6.1 och 6.2 visar hur felet i position respektive orientering ser ut i ettantal punkter langs banorna i exemplen 5.1 (tva linjer) och 5.4 (cirkelbage ochhelix).

Felet kan beskrivas av en kontinuerlig funktion, med nollstallen i splinefunktioner-nas brytpunkter. Att felet ar kontinuerligt framgar inte riktigt i figurerna eftersomdet samplats i relativt fa punkter.

Felen i position ar som storst i zonen i figur 6.1(a), men i figur 6.2(a) ar det mindreskillnad (forutom vid enstaka punkter).


Algoritm 2 Algoritm for bangenerering.

p∆max ← maximalt tillatet fel i position (meter)θ∆max ← maximalt tillatet fel i orientering (radianer)N ← antalet brytpunkter {i qtmp}h← h0 {initial steglangd}Lc ← banans langdlc ← 0 {nollstall banindex}fel← False

Skapa en tom splinefunktion q.while lc < Lc do

if lc + (N − 1)h > Lc thenh← Lc−lc

N−1 {justera h om banans slut ar nara}end iffor i = 0 to N − 1 do

Berakna position och orientering i punkterna med banindex lc + ih.Bestam ledvariablernas, qi, varden i dessa punkter med hjalp av inverski-nematik.

end forBestam d

dlcq0 och d

dlcqN−1

Skapa splinefunktionen qtmpgfel← felfel← False

for i = 1 to N − 1 doUtvardera qtmp i punkten med banindex lc + (i− 1

2 )h.Berakna, med direktkinematik, position och orientering i denna punkt.p∆ ← beraknat fel i positionθ∆ ← beraknat fel i orienteringif (p∆ > p∆max) or (θ∆ > θ∆max) thenfel← True

end ifend forif fel thenh← 1

2h {halvera steglangden}elselc ← lc + (N − 1)hif gfel thenh← h {behall h}

elseh← 2h {dubblera steglangden}

end ifGor en sammanslagning av splinefunktionerna q och qtmp.

end ifend while


0 0.2 0.4 0.6 0.8 1 1.2 1.40

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1x 10

−7 Felet i position

lc [m]

[m]

(a)

0 0.2 0.4 0.6 0.8 1 1.2 1.40

1

2

3

4

5

6

7

8

9x 10

−8 Felet i orientering

lc [m]

[rad

]

(b)

Figur 6.1. Felet i (a) position och (b) orientering for banan i exempel 5.1 med α5. Zonenar markerad med lodrata punktade linjer.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.5

1

1.5

2

2.5

3

3.5x 10

−8 Felet i position

lc [m]

[m]

(a)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1

2

3

4

5

6

7

8

9x 10

−8 Felet i orientering

lc [m]

[rad

]

(b)

Figur 6.2. Felet i (a) position och (b) orientering for banan i exempel 5.4 med α5. Zonenar markerad med lodrata punktade linjer.

Figur 6.1(b) visar pa storre fel strax fore och efter zonen samt i zonens mitt anı ovriga punkter. I figur 6.2(b) ar felen som storst mitt pa det forsta segmentet(cirkelbagen) och i slutet av det andra (helixen), men annars pa en ganska jamnniva.


6.3.2 Antal splines for ett visst fel

Ett forsok att bestamma felgransernas inverkan pa antalet splines, N , i den avalgoritm 2 genererade splinefunktionen har gjorts. For varje exempel (5.1–5.4) har,for α3, α5 och α7, olika varden pa p∆max och θ∆max anvants och q bestamts.

Figur 6.3 sammanfattar varje exempel for fallet att p∆max = θ∆max. Om en avparametrarna fixeras och den andra tillats anta varden storre an eller lika medden forsta kommer N att vara storst nar parametrarna ar lika. Allmant kan sagasatt det i exemplen behovs fler splines for att fa ett litet orienteringsfel an ett litetpositioneringsfel, vilket troligen beror pa att de beraknas pa olika vis. Tabell 6.1visar hur N varierar nar banan i exempel 5.1 (beraknad med α5) transformerasmed olika kombinationer av felgranser.

Da α3 anvands blir N alltid storre an for α5 och α7. Skillnaden mellan α5 och α7 armarginell och ingen ger ett lagre varde pa N an den andra. Om exemplen jamforskan man se att ju ”snarigare” bana desto fler splines behovs for att halla felet paratt niva.

θ∆max10−4 10−5 10−6 10−7

p∆max

10−4 22 28 58 8210−5 28 28 58 8210−6 58 58 58 8210−7 85 85 85 85

Tabell 6.1. Antalet splines, N , for banan i exempel 5.1 (beraknad med α5) vid olikakombinationer av felgranserna p∆max och θ∆max.


1 2 3 40

50

100

150

200

250

300

(a) Exempel 5.1.1 2 3 4

0

50

100

150

200

250

300

(b) Exempel 5.2.

1 2 3 40

50

100

150

200

250

300

(c) Exempel 5.3.1 2 3 4

0

50

100

150

200

250

300

(d) Exempel 5.4.

Figur 6.3. Antalet splines som behovs for att approximera banorna i de givna exemplenfor olika varden pa de maximala felgranserna (med p∆max = θ∆max). Bilderna bestar avfyra grupper med tre staplar: Ljusgra stapel – banan skapad med α3. Morkgra stapel– banan skapad med α5. Svart stapel – banan skapad med α7. Bilderna (a)–(d) visardata for respektive exempel 5.1–5.4. Anvanda varden pa felgranserna ar: Grupp 1 : 10−4,Grupp 2 : 10−5, Grupp 3 : 10−6, Grupp 4 : 10−7 (meter respektive radianer).


Kapitel 7

Exempel pazoninterpolering,vinkelkoordinater

For de i kapitel 5 givna exemplen har den kartesiska banan transformerats medalgoritm 2 i kapitel 6. Bilder pa ledvariablerna och deras derivator presenteras ochjamfors for att se hur valet mellan α3, α5 och α7 inverkar i zonerna. I kapitel5 orsakade valet ingen synbar skillnad mellan de beraknade zonbanorna. Att deverkligen ar olika kan latt ses nar har beraknade ledvariabler deriveras.

Beskrivning av bilderna

For exemplen 5.1–5.4 ges i figur 7.1–7.4 bilder pa q, q′ och q′′ for hela banan narα5 anvants. Plottar pa q′ och q′′ i zonen ges for α3, α5 och α7.

Figur 7.5 ger bilder pa q, q′ och q′′ for hela banan for linjara l-kurvor enligt exempel5.5. Plottar pa q′ och q′′ i zonen ges for linjara l-kurvor och α3.

Bilderna pa α5 och α7 har gjorts med p∆max = θ∆max = 10−7, men p g a nume-riska problem vid zongranserna har bilderna pa α3 och linjara l-kurvor gjorts medp∆max = θ∆max = 10−5. Detta gor att farre splinefunktioner behovs till bananvilket ger ett kantigare utseende pa q′′.

Allmant syns ingen storre skillnad mellan q′-plottarna for α3, α5 och α7. Det somskiljer dem at ar utseendet pa och strax innanfor zongranserna. Dar α3 kan ge ettskarpt veck (t ex figur 7.3(d)) ger α5 en mjukare overgang och α7 ger i sin tur enannu jamnare kurva.

51

52 Exempel pa zoninterpolering, vinkelkoordinater

7.1 Diskontinuiteter

Eftersom banans splinefunktion ar sammansatt av flera splinefunktioner till vil-ka man approximerat derivatan q′ i andpunkterna kommer andraderivatan q′′ attinnehalla (sma) diskontinuiteter i skarvarna mellan dessa funktioner. Med N bryt-punkter per funktion kan alltsa en diskontinuitet forekomma med N − 1 bryt-punkters mellanrum. Storleken av dessa varierar med vardet av de maximala fel-granserna (p∆max och θ∆max) – hog felgrans ger storre diskontinuitet och tvartom.

Vid zongranserna beror storleken aven av den metod for zoninterpolering somanvands. Med α3 (och linjara l-kurvor) fas alltid (relativt) stora diskontinuiteter.Nar α5 eller α7 anvands dampas zonens inverkan pa storleken; battre ju mindremaximala fel som tillats.

I de exempel som anvants (5.1-5.4) paverkas storleken inte markbart av zoninter-poleringen med α7, men med α5 kan den paverkas i de fall diskontinuiteten som fasmed α3 ar stor. Exempel 5.4 ar ett sadant fall vilket i figur 7.6 visas med narbildergjorda pa hoger zongrans i figur 7.4(h) respektive figur 7.4(i).

Med α5 i figur 7.6(a) syns diskontinuiteten strax till hoger om zongransen menmed α7 i figur 7.6(b) syns ingen alls. I figur 7.6(c) och 7.6(d) har brytpunkternatill splinefunktionen lagts till for att kunna jamfora med ovriga diskontinuiteter.I exemplen har N = 4 anvants, sa vid var tredje brytpunkt bor en diskontinuitetfinnas. Det gar dock inte att se i figurerna eftersom de i dessa fall ar valdigt sma.

7.2 Derivatornas storlek

Vad galler maximala storleken pa andraderivatan kan man inte saga att nagon avα5 eller α7 ar battre an den andra. I vissa fall kan den vara storre i en del av zonenmen mindre i en annan, t ex ar q′′2 i figur 7.2(h) storre (mindre) an i figur 7.2(i)for lc = 0, 6 samtidigt som q′′1 i figur 7.2(i) ar storre an i figur 7.2(h) vid lc = 0, 65.Detta galler aven da α3 jamfors (q′′1 och q′′2 ) i samma figur, men i figurerna 7.3och 7.4 ar diskontinuiteterna sa stora att de overtraffar andraderivatans maximalavarden i zonen.

Jamfors andraderivatorna i figur 7.5 verkar samma slutsats som for α3 kunna dras,men da de linjara l-kurvorna endast anvants i ett exempel kan man inte saga ifalldiskontinuiteterna i zongranserna kan bli storre an det i zonen maximala vardet paandraderivatan.

7.3 Sammanfattning

Diskontinuiteterna vid zongranserna dampas mycket bra for α7 och inte alls for α3.Storleken av andraderivatan paverkas inte pa nagot bestamt satt av valet mellanα3, α5 och α7.

7.3 Sammanfattning 53

0 0.2 0.4 0.6 0.8 1 1.2 1.4−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

(a) q (α5)0 0.2 0.4 0.6 0.8 1 1.2 1.4

−5

−4

−3

−2

−1

0

1

2

3

(b) dq

dlc(α5)

0 0.2 0.4 0.6 0.8 1 1.2 1.4−20

−15

−10

−5

0

5

10

15

20

(c) d2q

dl2c(α5)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−5

−4

−3

−2

−1

0

1

2

3

(d) dq

dlc(α3)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−5

−4

−3

−2

−1

0

1

2

3

(e) dq

dlc(α5)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−5

−4

−3

−2

−1

0

1

2

3

(f) dq

dlc(α7)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−20

−15

−10

−5

0

5

10

15

20

(g) d2q

dl2c(α3)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−20

−15

−10

−5

0

5

10

15

20

(h) d2q

dl2c(α5)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−20

−15

−10

−5

0

5

10

15

20

(i) d2q

dl2c(α7)

Figur 7.1. Ledvariablerna och deras derivator for en bana bestaende av tva linjer enligtfigur 5.2. Tjocka linjer: heldragen–q1, streckad–q2, punktad–q3. Tunna linjer: heldragen–

q4, streckad–q5, punktad–q6. (a)–(c): q, dq

dlcoch d2q

dl2cfor α5. (d)–(f): dq

dlcfor α3, α5 och α7.

(g)–(i): d2q

dl2cfor α3, α5 och α7. De maximala felgranserna ar 10−7 forutom i (d) och (g)

dar 10−5 har anvands.


0 0.2 0.4 0.6 0.8 1 1.2−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

3

(a) q (α5)0 0.2 0.4 0.6 0.8 1 1.2

−7

−6

−5

−4

−3

−2

−1

0

1

2

3

(b) dq

dlc(α5)

0 0.2 0.4 0.6 0.8 1 1.2−50

−40

−30

−20

−10

0

10

20

30

40

(c) d2q

dl2c(α5)

0.5 0.55 0.6 0.65 0.7−7

−6

−5

−4

−3

−2

−1

0

1

2

3

(d) dq

dlc(α3)

0.5 0.55 0.6 0.65 0.7−7

−6

−5

−4

−3

−2

−1

0

1

2

3

(e) dq

dlc(α5)

0.5 0.55 0.6 0.65 0.7−7

−6

−5

−4

−3

−2

−1

0

1

2

3

(f) dq

dlc(α7)

0.5 0.55 0.6 0.65 0.7−50

−40

−30

−20

−10

0

10

20

30

40

50

(g) d2q

dl2c(α3)

0.5 0.55 0.6 0.65 0.7−50

−40

−30

−20

−10

0

10

20

30

40

50

(h) d2q

dl2c(α5)

0.5 0.55 0.6 0.65 0.7−50

−40

−30

−20

−10

0

10

20

30

40

50

(i) d2q

dl2c(α7)

Figur 7.2. Ledvariablerna och deras derivator for en bana bestaende av en linje och encirkelbage enligt figur 5.3. Tjocka linjer: heldragen–q1, streckad–q2, punktad–q3. Tunna

linjer: heldragen–q4, streckad–q5, punktad–q6. (a)–(c): q, dq

dlcoch d2q


dlc

for α3, α5 och α7. (g)–(i): d2q

dl2cfor α3, α5 och α7. De maximala felgranserna ar 10−7

forutom i (d) och (g) dar 10−5 har anvands.


0 0.2 0.4 0.6 0.8 1−4

−3

−2

−1

0

1

2

3

4

(a) q (α5)0 0.2 0.4 0.6 0.8 1

−14

−12

−10

−8

−6

−4

−2

0

2

4

6

(b) dq

dlc(α5)

0 0.2 0.4 0.6 0.8 1−80

−60

−40

−20

0

20

40

60

80

(c) d2q

dl2c(α5)

0.3 0.35 0.4 0.45 0.5−14

−12

−10

−8

−6

−4

−2

0

2

4

6

(d) dq

dlc(α3)

0.3 0.35 0.4 0.45 0.5−14

−12

−10

−8

−6

−4

−2

0

2

4

6

(e) dq

dlc(α5)

0.3 0.35 0.4 0.45 0.5−14

−12

−10

−8

−6

−4

−2

0

2

4

6

(f) dq

dlc(α7)

0.3 0.35 0.4 0.45 0.5

−80

−60

−40

−20

0

20

(g) d2q

dl2c(α3)

0.3 0.35 0.4 0.45 0.5

−80

−60

−40

−20

0

20

(h) d2q

dl2c(α5)

0.3 0.35 0.4 0.45 0.5

−80

−60

−40

−20

0

20

(i) d2q

dl2c(α7)

Figur 7.3. Ledvariablerna och deras derivator for en bana bestaende av en linje och enhelix enligt figur 5.4. Tjocka linjer: heldragen–q1, streckad–q2, punktad–q3. Tunna linjer:

heldragen–q4, streckad–q5, punktad–q6. (a)–(c): q, dq

dlcoch d2q


dlcfor α3,

α5 och α7. (g)–(i): d2q

dl2cfor α3, α5 och α7. De maximala felgranserna ar 10−7 forutom i

(d) och (g) dar 10−5 har anvands. (I (a) gar q4 fran −π till +π vid 0.6 – det ar ingendiskontinuitet!)


0 0.2 0.4 0.6 0.8 1−4

−3

−2

−1

0

1

2

3

4

(a) q (α5)0 0.2 0.4 0.6 0.8 1

−14

−12

−10

−8

−6

−4

−2

0

2

4

6

(b) dq

dlc(α5)

0 0.2 0.4 0.6 0.8 1−80

−60

−40

−20

0

20

40

60

80

(c) d2q

dl2c(α5)

0.3 0.35 0.4 0.45 0.5−10

−8

−6

−4

−2

0

2

4

(d) dq

dlc(α3)

0.3 0.35 0.4 0.45 0.5−10

−8

−6

−4

−2

0

2

4

(e) dq

dlc(α5)

0.3 0.35 0.4 0.45 0.5−10

−8

−6

−4

−2

0

2

4

(f) dq

dlc(α7)

0.3 0.35 0.4 0.45 0.5−100

−80

−60

−40

−20

0

20

40

60

80

(g) d2q

dl2c(α3)

0.3 0.35 0.4 0.45 0.5−100

−80

−60

−40

−20

0

20

40

60

80

(h) d2q

dl2c(α5)

0.3 0.35 0.4 0.45 0.5−100

−80

−60

−40

−20

0

20

40

60

80

(i) d2q

dl2c(α7)

Figur 7.4. Ledvariablerna och deras derivator for en bana bestaende av en cirkelbageoch en helix enligt figur 5.5. Tjocka linjer: heldragen–q1, streckad–q2, punktad–q3. Tunna

linjer: heldragen–q4, streckad–q5, punktad–q6. (a)–(c): q, dq

dlcoch d2q


dlc

for α3, α5 och α7. (g)–(i): d2q

dl2cfor α3, α5 och α7. De maximala felgranserna ar 10−7

forutom i (d) och (g) dar 10−5 har anvands. (I (a) gar q6 och q4 fran −π till +π vid 0.2respektive 0.6 – det ar inga diskontinuiteter!)


0 0.2 0.4 0.6 0.8 1 1.2 1.4−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

(a) q (l-kurvor)0 0.2 0.4 0.6 0.8 1 1.2 1.4

−5

−4

−3

−2

−1

0

1

2

3

(b) dq

dlc(l-kurvor)

0 0.2 0.4 0.6 0.8 1 1.2 1.4−20

−15

−10

−5

0

5

10

15

20

(c) d2q

dl2c(l-kurvor)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−5

−4

−3

−2

−1

0

1

2

3

(d) dq

dlc(l-kurvor)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−5

−4

−3

−2

−1

0

1

2

3

(e) dq

dlc(α3)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−20

−15

−10

−5

0

5

10

15

20

(f) d2q

dl2c(l-kurvor)

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75−20

−15

−10

−5

0

5

10

15

20

(g) d2q

dl2c(α3)

Figur 7.5. Ledvariablerna och deras derivator for en bana bestaende av tva linjer enligtfigur 5.6 for linjara l-kurvor och α3. Tjocka linjer: heldragen–q1, streckad–q2, punktad–q3.

Tunna linjer: heldragen–q4, streckad–q5, punktad–q6. (a)–(c): q, dq

dlcoch d2q

dl2cfor linjara

l-kurvor. (d): dq

dlcfor linjara l-kurvor. (e): dq

dlcfor α3. (f): d2q

dl2cfor linjara l-kurvor. (g): d2q

dl2c

for α3. De maximala felgranserna ar satta till 10−5 i alla delfigurer.


0.49 0.495 0.5 0.505 0.51 0.515 0.52 0.525 0.5310

12

14

16

18

20

22

(a) Hoger zongrans for α5

0.49 0.495 0.5 0.505 0.51 0.515 0.52 0.525 0.5310

12

14

16

18

20

22

(b) Hoger zongrans for α7

0.49 0.495 0.5 0.505 0.51 0.515 0.52 0.525 0.5310

12

14

16

18

20

22

(c) Hoger zongrans for α5 medbrytpunkter inlagda

0.49 0.495 0.5 0.505 0.51 0.515 0.52 0.525 0.5310

12

14

16

18

20

22

(d) Hoger zongrans for α7 medbrytpunkter inlagda

Figur 7.6. Narbild pa ledvariablernas andraderivata q′′ vid hoger zongrans for banani exempel 5.4 (figur 7.4(h) och 7.4(i)) for α5 och α7. (a),(b): utan brytpunkter. (c),(d):med brytpunkter. Tjocka linjer: heldragen–q1, streckad–q2. Tunna linjer: heldragen–q4,streckad–q5.

Kapitel 8

Sammanfattning och fortsattarbete

8.1 Slutsatser

Den kartesiska banan, inklusive zonerna, beskrivs pa parameterform med en vektorfor position och en enhetsquaternion for orientering. Den metod som anvands foratt ge en mjuk zonbana tvingar de N forsta derivatorna i positionsvektorn att blikontinuerliga i zongranserna.

Valet av N spelar i det kartesiska fallet en mycket liten roll – banan blir kontinuerligfor alla N > 0. Det ar nar banan transformeras till motorvinkelkoordinater, q, somskillnaden marks. Den ar tydligast nar man betraktar andraderivatorna q′′ menkan, om an inte lika tydligt, aven ses i forstaderivatorna q′.

Mellan splinefunktionerna som beskriver motorvinkelkoordinaterna finns alltid dis-kontinuiteter i q′′. Det beror pa den approximation av q′′ som gors nar splinefunk-tionerna skapas. Storleken beror allmant pa noggrannheten i approximationen. Vidzongranserna beror den aven paN , dar de storsta diskontinuiteterna alltid intraffar.(Ovriga diskontinuiteter kan vid noggrann approximation goras godtyckligt sma.)

Tre varden pa N har anvants vid simulering – N=1, N=2 och N=3. Med N=1fas alltid stora diskontinuiteter i q′′ vid zongranserna. Diskontinuiteterna kommerfor N=3 att dampas sa pass mycket att diskontinuiteterna fran approximationenav q′′ dominerar. Det galler aven for N =2 utom i ett exempel dar diskontinuitetenmed N=1 var mycket stor. Ett hogre varde pa N ger alltsa en mindre storlek avdiskontinuiteterna vid zongranserna.

De maximala vardena pa ‖q′′‖ i zonen ar till viss del beroende av N . For N=1kan diskontinuiteterna i zongranserna vara dominerande, men bortsett fran dettakan inget varde pa N sagas vara varken battre eller samre an nagot annat.

59

60 Sammanfattning och fortsatt arbete

Simuleringar har gjorts dar de vid transformering av den kartesiska banan maximalttillatna felen i position och orientering har varierats och antal skapade splinefunk-tioner jamforts. Den lagre felgransen ar den dominerande – samma antal (medliten variation) splinefunktioner skapas oavsett vardet av den hogre felgransen.Med N=2 och N=3 fas i stort sett samma antal funktioner, vilka alltid ar farrean for N=1.

Av de testade vardena pa N ar N =3 det val som ger bast resultat. Antalet spline-funktioner blir ungefar lika manga som for N=2, men diskontinuiteterna vid zon-granserna dampas battre med storre N .

8.2 Fortsatt arbete

Mer arbete kan goras, t ex:

• Teoretisk uppskattning av det maximala felet vid berakning av den kartesiskabanan utifran splinefunktionerna.

• Att nar den kartesiska banan transformeras till motorvinkelkoordinater un-dersoka nodvandig noggrannhet i berakningarna.

• Studera tidsaspekter som hastighet och acceleration, vinkelhastighet och vin-kelacceleration langs banan.

Litteraturforteckning

[1] Samuel R. Buss and Jay P. Fillmore. Spherical Averages and Applications toSpherical Splines and Interpolation. In Proceedings of ACM Transactions onGraphics, volume 20, pages 95–126. University of California, San Diego, April2001.

[2] P.I. Corke. A Robotics Toolbox for MATLAB. IEEE Robotics and AutomationMagazine, 3(1):24–32, March 1996.

[3] Erik B. Dam, Martin Koch, and Martin Lillholm. Quaternions, Interpolationand Animation. Technical Report DIKU-TR-98/5, Department of ComputerScience, University of Copenhagen, DK-2100 Kbh Ø, Denmark, July 1998.

[4] David Eberly. Quaternion Algebra and Calculus. http://www.magic-software.com, September 2002.

[5] David Eberly. Kochanek-Bartels Cubic Splines (TCB Splines).http://www.magic-software.com, March 2003.

[6] J. Funda and R.P. Paul. A comparison of transforms and quaternions inrobotics. In IEEE International Conference on Robotics and Automation,volume 2, pages 886–891, April 1995.

[7] Peter Grogono. Rotation with Quaternions. http://www.cs.concordia.ca/ gro-gono/CUGL/, December 2001. Department of Computer Science, ConcordiaUniversity.

[8] William Rowan Hamilton. On Quaternions, or on a newSystem of Imaginaries in Algebra. http://www.maths.tcd.ie/pub/HistMath/People/Hamilton/OnQuat/, 2000. Edited by David R.Wilkins.

[9] F.S. Hill Jr. Computer graphics. Macmillan Publishing Company, 1990.

[10] Myoung-Jun Kim, Myung-Soo Kim, and Sung Yong Shin. A C2-continuousB-spline quaternion curve interpolating a given sequence of solid orientations.In Proceedings of Computer Animation ’95, pages 72–81, April 1995.

61

62 Sammanfattning och fortsatt arbete

[11] Myoung-Jun Kim, Myung-Soo Kim, and Sung Yong Shin. A general construc-tion scheme for unit quaternion curves with simple high order derivatives. InProceedings of the 22nd annual conference on Computer graphics and inte-ractive techniques, pages 369–376. ACM Press, 1995.

[12] The MathWorks, Inc., Cochituate Place, Natick, MA. USA. MATLAB: High-Performance Numeric Computation and Visualization Software, 1992.

[13] K.T. Miura, T. Nakaseko, and T. Ikedo. A New Type of Free-Form Curve Gi-ven by an Integral Form. In Proceedings of Computer Graphics International,pages 722–725, June 1998.

[14] Mikael Norrlof. Modeling of industrial robots. Technical Report LiTH-ISY-R-2208, Department of Electrical Engineering, Linkoping University, SE-581 83Linkoping, Sweden, Dec 1999.

[15] Mikael Norrlof. On path planning and optimization using splines. TechnicalReport LiTH-ISY-R-2490, Department of Electrical Engineering, LinkopingUniversity, SE-581 83 Linkoping, Sweden, Feb 2003.

[16] M. Nystrom and Mikael Norrlof. PGT - A path generation toolbox for Matlab(v0.1). Technical Report LiTH-ISY-R-2542, Department of Electrical Engine-ering, Linkoping University, SE-581 83 Linkoping, Sweden, Sep 2003.

[17] M. Nystrom. Bangenerering for industrirobot. Master’s thesis LiTH-ISY-EX-3357-2003, Department of Electrical Engineering, Linkoping University,Linkoping, Sweden, 2003.

[18] L. Elden och L. Wittmeyer-Koch. Numerisk analys - en introduktion. Stu-dentlitteratur, 1996.

[19] L. Sciavicco and B. Siciliano. Modelling and Control of Robot Manipulators.Advanced Textbooks in Control and Signal Processing. Springer-Verlag, 2ndedition, 2000.

[20] K. Shoemake. Animating rotation with quaternion curves. In Proceedings ofACM SIGGRAPH, pages 245–254, (San Francisco), 1985. The Singer Compa-ny, Link Flight Simulation Division.

Bilaga A

Denavit-Hartenberg-konventionen

Vid direktkinematiska berakningar for en robot vill man veta hur de olika lederna arrelaterade. Den av Denavit-Hartenberg (DH) framtagna metoden kallad Denavit-Hartenbergkonventionen tar pa ett systematiskt vis fram transformationsmatrisersom beskriver just detta.

Med DH-konventionen kan man ta fram en modell for en robot oavsett vilkenstruktur (oppen eller sluten kinematisk kedja) den har. Det finns aven tva olikavarianter av metoden beskrivna i [2]. Denna beskrivning galler dock enbart foren oppen struktur (dar lankarna ar kopplade i en foljd) och roterande leder, mednotation i stort sett enligt [19]. Figur A.1 visar relationerna mellan lankar och leder.

Led iLed i − 1

Led i + 1Lank i

Lank i − 1

Oixi

yi

zi

Oi−1 xi−1

yi−1

zi−1

Oi′

xi′

yi′zi′

ai

ai−1

di

αi

θi

Figur A.1. Relationerna mellan lankar och leder enligt DH-konventionen.

63

64 Denavit-Hartenbergkonventionen

DH-parametrarna

Lank i−1 och lank i ar sammankopplade i led i genom vilken axel i gar. Nagonstanslangs denna axel ar ett lokalt koordinatsystem Oi−1 placerat (med zi−1-axeln langsaxel i) i vilket lank i−1 ar ororlig. Fyra parametrar – αi, ai, θi och di – anvandsfor att beskriva koordinatsystemet Oi relativt Oi−1:

αi ar den vinkel som runt xi-axeln roterar Oi−1 sa att axlarna zi−1 och zi farsamma riktning. Positiv vid moturs vridning.

ai beskriver avstandet mellan axlarna zi−1 och zi langs den gemensamma nor-malen.

θi ar den vinkel som runt axeln zi−1 roterar Oi−1 sa att axlarna xi−1 och xi farsamma riktning. Positiv vid moturs vridning.

di beskriver avstandet mellan Oi−1 och skarningen mellan axeln zi−1 och denmen zi-axeln gemensamma normalen.

Framtagning av modell

Forst bestams DH-parametrarna enligt beskrivningen i algoritm 3 (vilken ar giltigendast for en oppen struktur och roterande leder).

Algoritm 3 Tillvagagangssatt vid framtagande av DH-parametrar.

1: Bestam riktning for alla axlar och numrera dessa i foljd (z0, z1, . . . , zn−1).2: Placera koordinatsystem O0 langs axel z0. Valj x0 och y0 sadana att ett

hogersystem bildas.3: Upprepa dessa tre steg for i = 1, . . . , n− 1:

I. Placera Oi dar zi skar den med zi−1 gemensamma normalen. Om axlarnaar parallella valjes Oi sa att di = 0.

II. Lat axel xi vara riktad fran led i mot led i + 1 langs den gemensammanormalen till axel zi−1 och axel zi.

III. Valj axel yi sa att ett hogersystem bildas.

4: Valj zn langs zn−1.5: Valj xn pa samma satt som i II.6: Valj yn som i III.7: Bestam parametrarna αi, ai, θi och di for i = 1, . . . , n.

Nar alla parametrar, som beskriver translationer och rotationer, har bestamts kanman berakna de homogena transformationsmatriser som beskriver hur koordinat-

65

systemen ar relaterade. Den matris som beskriver Oi relativt Oi−1 blir

Ai−1i (qi) =

cθi −sθicαi sθisαi aicθisθi cθicαi −cθisαi aisθi0 sαi cαi di0 0 0 1

(A.1)

dar qi ar ledvariabeln. For en roterande led galler qi = θi och att αi, ai och di arkonstanter.

Genom multiplikation av matriserna Ai−1i far man ett uttryck for position och

orientering av On relativt O0:

T 0n(q1, . . . , qn) = A0

1(q1) · · ·An−1n (qn) (A.2)

På svenska Detta dokument hålls tillgängligt på Internet – eller dess framtida ersättare – under en längre tid från publiceringsdatum under förutsättning att inga extra-ordinära omständigheter uppstår.

Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner, skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av dokumentet kräver upphovsmannens medgivande. För att garantera äktheten, säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ art.

Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman i den omfattning som god sed kräver vid användning av dokumentet på ovan beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan form eller i sådant sammanhang som är kränkande för upphovsmannens litterära eller konstnärliga anseende eller egenart.

För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/ In English The publishers will keep this document online on the Internet - or its possible replacement - for a considerable time from the date of publication barring exceptional circumstances.

The online availability of the document implies a permanent permission for anyone to read, to download, to print out single copies for your own use and to use it unchanged for any non-commercial research and educational purpose. Subsequent transfers of copyright cannot revoke this permission. All other uses of the document are conditional on the consent of the copyright owner. The publisher has taken technical and administrative measures to assure authenticity, security and accessibility.

According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected against infringement.

For additional information about the Linköping University Electronic Press and its procedures for publication and for assurance of document integrity, please refer to its WWW home page: http://www.ep.liu.se/

© Daniel Forsman

Documents

Bangenerering f¨or industrirobot med 6 frihetsgrader19708/FULLTEXT01.pdf · 1400 tas fram f¨or direkt- och inverskinematik. •Kapitel 4 beskriver en banas uppbyggnad och hur position