Bangenerering för industrirobot med 6 frihetsgrader

Bangenerering f¨

or industrirobot med 6

frihetsgrader

Examensarbete utf¨ort i Reglerteknik vid Tekniska H¨ogskolan i Link¨oping

av

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

Bangenerering f¨

or industrirobot med 6

frihetsgrader

Examensarbete utf¨ort i Reglerteknik vid Tekniska H¨ogskolan i Link¨oping

av

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

Handledare: Erik Wernholt Examinator: Mikael Norrl¨of Link¨oping 14 maj 2004.

Avdelning, Institution Division, Department Institutionen för systemteknik 581 83 LINKÖPING Datum Date 2004-05-14 Språk

Language RapporttypReport 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

Sammanfattning

Rapporten handlar om bangenerering f¨or industrirobotar med sex frihetsgrader – position och orientering. En bana byggs upp av banprimitiver som linjer och cirkelb˚agar, vilka fogas samman i s k zoner d¨ar en viss avvikelse fr˚an banan till˚ats f¨or att g¨ora denna mjukare. Zonen g¨or det m¨ojligt att f¨olja banan med konstant hastighet, beroende p˚a hur banan ser ut i zonen kan dock den acceleration som kr¨avs variera.

F¨orst presenteras n˚agra s¨att att best¨amma zonbanan s˚a att accelerationen blir kontinuerlig. Sedan beskrivs hur den kartesiska banan kan transformeras till mo-torvinkelkoordinater d¨ar banan beskrivs m h a kubiska splinefunktioner. Den senare banan kommer att ha kontinuerliga f¨orstaderivator, men mellan splinefunktionerna kommer andraderivatorna att vara diskontinuerliga.

N˚agra exempel ges d¨ar zonbanan ber¨aknats p˚a olika s¨att varefter de resulteran-de splinefunktionernas f¨orsta- och andraresulteran-derivator j¨amf¨ors. Dessa exempel anv¨ands ¨

aven till att unders¨oka hur m˚anga splinefunktioner som beh¨ovs f¨or att man vid transformering tillbaka till kartesiska koordinater ska f˚a en avvikelse fr˚an t¨ankt bana 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 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.

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

F¨

orord

Jag vill tacka min examinator Mikael Norrl¨of, LiTH, f¨or att ha givit mig en intres-sant uppgift och bra v¨agledning under arbetets genomf¨orande.

Jag vill ¨aven tacka min handledare Erik Wernholt, LiTH, f¨or all hj¨alp vid rappor-tens slutf¨orande.

Notation

Symboler

Oi, Oi-xyz Ortonormerat h¨oger koordinatsystem.

q Enhetsquaternion.

qi Ledvariabel i.

q Exakt funktion f¨or ledvariablerna (fr˚an inverskinematik). ˆ

q Splinefunktion f¨or ledvariablerna. r, R Vektorer och matriser skrivs med fetstil. rj_i Vektorn fr˚an rj till ri.

Rj_i Rotationsmatris f¨or Oi relativt Oj.

T, Tj_i Homogen transformationsmatris.

Operatorer och funktioner

Slerp (S pherical l inear interpolation) Funktion som utf¨or en sf¨arisk linj¨ar interpolering mellan tv˚a enhetsquartenioner.

arctan2(y, x) Funktion som f¨or tan θ =y_x returnerar ett korrekt v¨arde p˚a θ.

s(·) sin(·)

c(·) cos(·)

F¨

orkortningar

DH Denavit-Hartenberg IK Inverskinematik

SCARA Selective Compliance Assembly Robot Arm TCP Tool Center Point

Inneh˚

all

1 Inledning 1

1.1 Bakgrund . . . 1

1.2 Syfte och uppl¨agg . . . 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

5.1 Linje-Linje . . . 35

5.2 Linje-Cirkelb˚age . . . 36

5.3 Linje-Helix . . . 37

5.4 Cirkelb˚age-Helix . . . 38

5.5 Linje-Linje, linj¨ara l-kurvor . . . 39

5.6 Kommentar . . . 42

6 Interpolering i vinkelkoordinater 43 6.1 Splines . . . 43

6.2 Algoritm f¨or bangenerering . . . 44

6.3 Felutv¨ardering . . . 45

6.3.1 N˚agra exempel . . . 45

6.3.2 Antal splines f¨or ett visst fel . . . 48

7 Exempel p˚a 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

Kapitel 1

Inledning

1.1

Bakgrund

Normalt har en industrirobot ett verktyg som man vill ska f¨olja en given bana. F¨orutom att best¨amma dess position l¨angs banan vill man ¨aven styra ¨over dess orientering i rummet. Ett exempel ¨ar om verktyget m˚aste h˚allas vinkelr¨att mot en kr¨okt yta samtidigt som positionen f¨or¨andras.

I kartesiska koordinater konstrueras banan genom att koppla samman enkla seg-ment som linjer och cirkelb˚agar. Kring de sammanbindande punkterna definieras ett omr˚ade, en zon, d¨ar en mjukare ¨overg˚ang mellan segmenten till˚ats. Zonen g¨or det m¨ojligt att f¨olja banan med konstant hastighet; i annat fall m˚aste roboten stannas innan den ¨andrar riktning fr˚an ett segment till n¨asta. ¨Aven om hastighe-ten h˚alls konstant kan, beroende p˚a hur ¨overg˚angen mellan segmenten ber¨aknas, accelerationen bli diskontinuerlig vid zongr¨anserna.

F¨or reglering av roboten m˚aste den kartesiska banan transformeras till motorvin-kelkoordinater. N¨ar den transformerade banan anv¨ands f¨or styrning av roboten kommer man, i b˚ade position och orientering, att f˚a en avvikelse fr˚an den ¨onskade kartesiska banan. Denna avvikelse m˚aste tas i beaktande vid transformationen. Detta examensarbete kan ses som en forts¨attning p˚a ett tidigare arbete av Maria Nystr¨om [17]. D¨ar unders¨oks hur den resulterande banan p˚averkas n¨ar kubiska splinefunktioner anv¨ands f¨or att beskriva banan i motorvinkelkoordinater. Olika metoder f¨or att skapa mjukare banor tas ocks˚a fram. En skillnad mellan arbetena ¨ar att man i [17] inte studerar robotens orientering.

het i zonerna, s˚a att denna kan genoml¨opas p˚a ett mjukt s¨att utan diskontinuiteter i hastighet eller omorientering.

Rapporten ¨ar indelad som f¨oljer:

• Kapitel 2 ger en beskrivning av orientering och olika s¨att att representera denna p˚a (t ex med enhetsquaternioner eller rotationsmatriser).

• I kapitel 3 tas mer robotspecifik teori upp. En modell av ABB-roboten IRB 1400 tas fram f¨or direkt- och inverskinematik.

• Kapitel 4 beskriver en banas uppbyggnad och hur position och orientering ber¨aknas efter banan och speciellt i zonerna.

• I kapitel 5 ges n˚agra exempel p˚a olika banor och olika s¨att att ber¨akna position och orientering i zonen.

• Kapitel 6 beskriver en algoritm f¨or transformering av den kartesiska banan till motorvinkelkoordinater, vilka representeras av kubiska splinefunktioner. • I kapitel 7 har banorna i exemplen fr˚an kapitel 5 omvandlats till kubiska

splinefunktioner enligt kapitel 6 varefter splinefunktionerna f¨or varje exempel j¨amf¨ors. H¨ar g¨ors ¨aven en felutv¨ardering av splinefunktionerna.

De i rapporten beskrivna metoderna har implementerats i Matlab [12], och anv¨ants vid ber¨akning av de givna exemplen. Robotics Toolbox [2] har anv¨ants i vissa delar, och f¨or splineber¨akningar har de i [15] beskrivna funktionerna anv¨ants.

Kapitel 2

Orienteringsrepresentation

Det h¨ar kapitlet beskriver hur man kan representera orientering och position p˚a olika s¨att. Avsnitt 2.1 visar hur man samtidigt kan representera denna informa-tion i en s˚a kallad homogen transformationsmatris, vilken underl¨attar notation och ber¨akningar.

Till stor del bygger inneh˚allet p˚a [19], men med delvis f¨or¨andrad notation. I [7] beskrivs hur rotationer kan g¨oras med hj¨alp av quaternioner. Det g¨ors bl a ¨aven i [4] och [3], d¨ar den sistn¨amnda v¨aldigt utf¨orligt behandlar quaternioner och inter-polering med s˚adana. En sammanst¨allning av de artiklar som Sir William Rowan Hamilton, upphovsmannen till begreppet quaternioner, publicerade i Philosophical Magazine ˚ar 1844-1850 finns sammanst¨allda i [8].

2.1

Homogena transformationsmatriser

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

p= pxxˆ+ pyyˆ+ pzzˆ=   px py pz  

d¨ar ˆx, ˆy och ˆz ¨ar enhetsvektorerna f¨or koordinatsystemet. Om vi har tv˚a ON-system i samma origo kan man genom att projicera enhetsvektorerna p˚a l¨ampligt s¨att ˆ xj=(ˆxj·ˆxi)ˆxi+(ˆxj·ˆyi)ˆyi+(ˆxj·ˆzi)ˆzi ˆ yj=(ˆyj·ˆxi)ˆxi+(ˆyj·ˆyi)ˆyi+(ˆyj·ˆzi)ˆzi ˆ zj=(ˆzj·ˆxi)ˆxi+(ˆzj·ˆyi)ˆyi+(ˆzj·ˆzi)ˆzi 3

vilken har egenskapen (ortogonalitet)

(Rij)−1= (Rij)T = R j i

I det allm¨anna fallet beh¨over inte origo sammanfalla f¨or tv˚a koordinatsystem. Om Oj:s l¨age relativt Oi anges av vektorn oij f˚as f¨or punkten p

pi= oi j+ R i jpj och pj= Rj_ipi_{− R}j_ioi_j i respektive koordinatsystem.

Om man vill g¨ora flera transformationer i f¨oljd g¨ors detta enklast genom att inf¨ora den homogena transformationsmatrisen

Ti_j =  Ri_j oi j 0T ₁ 

samt skriva p som

˜ p=  p 1 

Detta inneb¨ar att

˜

pi= Tijp˜j

ger samma pi _{som tidigare, fast p˚a ett mer direkt s¨att.}

Noteras b¨or dock att matrisen Tij inte ¨ar ortogonal vilket betyder att

(Ti_j)−16= (Tij)T Inversen av Ti j ¨ar d¨aremot (Tij)−1=  (Rij)T −(Rij)Toij 0T ₁ 

2.2

Representation av orientering

Om positionen ges av en punkt i rummet kan orienteringen ses som en vektor utg˚aende fr˚an den punkten och kring vilken en rotation kan ske.

Flera s¨att att representera orientering existerar. Det ¨ar inte s¨akert att man all-tid har en unik representation, vilket skapar problem n¨ar man vill byta fr˚an en representation till en annan.

2.2 Representation av orientering 5

2.2.1

Rotationsmatriser

En rotationsmatris ¨ar den grundl¨aggande representationen f¨or orientering. Den ¨ar en ortogonal matris som anv¨ands f¨or att rotera vektorer och koordinatsystem (se avsnitt 2.1). Den best˚ar (i tre dimensioner) av nio element vilket ¨ar mer ¨an antalet frihetsgrader som kr¨avs f¨or att representera orientering - en icke minimal representation.

Rotationsmatriserna f¨or rotation kring de tre koordinataxlarna z-y-x ges av

Rz(α) =   cos α − sin α 0 sin α cos α 0 0 0 1   (2.1) Ry(β) =   cos β 0 sin β 0 1 0 − sin β 0 cos β   (2.2) Rx(γ) =   1 0 0 0 cos γ − sin γ 0 sin γ cos γ   (2.3)

vilka kan kombineras f¨or att generera generella rotationer.

Varje rotation sker relativt ett givet koordinatsystem. Vill man g¨ora en sam-mans¨attning av rotationer ¨ar det d¨arf¨or viktigt att dessa utf¨ors i r¨att ordning. En rotation av ett koordinatsystem Oirelativt koordinatsystemet Oj uttrycks som

Rj_i. Det finns tv˚a s¨att att g¨ora detta:

• Om efterf¨oljande rotation sker relativt det koordinatsystem som erh¨olls vid f¨oreg˚aende rotation multipliceras de enligt R02= R01R12, d¨ar R01¨ar den f¨orsta

rotationen.

• Om b˚ada rotationerna sker relativt samma koordinatsystem multipliceras de enligt R02= R12R01.

2.2.2

Eulervinklar

Eulervinklarna beskriver orienteringen med tre oberoende parametrar - en minimal representation:

Φ = [ϕ ϑ ψ]T

Den rotation som Φ beskriver erh˚alls genom att skapa en rotationsmatris samman-satt av tre p˚a varandra f¨oljande rotationer (ϕ, ϑ, och ψ) kring ett l¨ampligt val av rotationsaxlar.

N¨ar man roterar ett koordinatsystem O kring en av dess axlar f˚ar man ett nytt koordinatsystem O′_{. Rotationsaxeln ¨ar densamma i de b˚ada systemen. Om man}

O x y O′ x′ y′

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

d¨arf¨or ser till att inte rotera kring samma axel tv˚a g˚anger i rad, kommer man att f˚a den ¨onskade rotationsmatrisen. Se figur 2.1.

N¨ar man v¨al har best¨amt vilken f¨oljd av rotationsaxlar man vill anv¨anda m˚aste man h˚alla sig till detta val och inte byta, eftersom en annan ordning ger ett helt annorlunda slutresultat (rotationsmatriserna kommuterar inte).

ZYZ-vinklar

En representation som beskriver den sf¨ariska handleden p˚a ett bra s¨att ¨ar ZYZ-vinklar. Rotationerna sker f¨orst kring z-axeln, sedan kring den nya y′_{-axeln och}

sist kring z′′_{-axeln (led 4, 5 och 6 i figur 3.3) enligt}

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

vilket efter sammans¨attning 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 r13 r21 r22 r23 r31 r32 r33   (2.6)

2.2 Representation av orientering 7

• tan ϕ = sin ϕcos ϕ= sϕsϑ

cϕsϑ =

r23

r13

• tan ψ = cos ψsin ψ = sϑsψ

sϑcψ = −

r32

r31

• tan2ϑ =_cossin22ϑ

ϑ = s2 ϑ c2 ϑ = (c2 ϕ+s 2 ϕ)s 2 ϑ c2 ϑ = r2 13+r 2 23 r2 33

vilket kan anv¨andas f¨or att best¨amma de till (2.6) svarande eulervinklarna. Det g¨ors enklast genom att anv¨anda en funktion arctan2(y, x) som f¨or uttrycket tan α =

sin α cos α =

y

x ger ett korrekt v¨arde p˚a vinkeln α.

D˚a tan2_{ϑ =} r2 13+r 2 23 r2 33 blir tan ϑ = ± √ r2 13+r 2 23

r33 och vinkeln ϑ best¨ams till ϑ =

arctan2(±pr213+ r223, r33) d¨ar man f¨or ett positivt respektive negativt tecken f˚ar

en l¨osning antingen i intervallet (0, π) eller (−π, 0). Om b˚ade r13 = 0 och r23 = 0

(ϑ = nπ) kommer Ry′(ϑ) att vara enhetsmatrisen, och s˚aledes roterar vi kring

z-axeln tv˚a g˚anger i f¨oljd. I detta fall kan inte ϕ och ψ entydigt best¨ammas. L¨osningarna blir nu f¨or ϑ ∈ (0, π)

ϕ = arctan2(r23, r13) ϑ = arctan2( q r2 13+ r223, r33) (2.7) ψ = arctan2(r32, −r31) och f¨or ϑ ∈ (−π, 0) ϕ = arctan2(−r23, −r13) ϑ = arctan2(− q r2 13+ r223, r33) (2.8) ψ = arctan2(−r32, r31)

2.2.3

Vinkel/Vektor

Orientering kan ¨aven beskrivas som en rotation ϑ kring en (enhets-) vektor r (se figur 2.2). Givet ett referenskoordinatsystem kan den rotationsmatris som motsva-rar orienteringen tas fram. Det g¨ors genom att f¨orst vrida in vektorn r till en av referenskoordinatsystemets axlar, rotera kring denna enligt ϑ och sedan vrida till-baka r till det ursprungliga l¨aget. G¨or man detta kring axlarna z och y f˚ar man f¨oljande uttryck f¨or rotationsmatrisen:

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

x y 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 =<η, ǫ> d¨ar

η = cosϑ 2 ǫ= sinϑ 2 r och kqk2= η2+ ǫT_{ǫ= cos}2ϑ 2 + sin 2ϑ 2 r T_{r= 1}

eftersom r ¨ar en enhetsvektor.

Multiplikation av tv˚a quaternioner q1=<η1, ǫ1> och q2=<η2, ǫ2> g¨ors enligt

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

d¨ar × ¨ar kryssprodukten. Detta motsvarar multiplikationen R1R2 av tv˚a

rota-tionsmatriser.

Inversen av en quaternion ¨ar

q−1=<η, −ǫ> vilket ger

q∗q−1_{=<1, 0>}

2.2 Representation av orientering 9

x y

z

v vrot

r

Figur 2.3.Rotation av en vektor kring en annan.

Den mot en enhetsquaternion svarande rotationsmatrisen har utseendet

R(η, ǫ) =   2(η2_{+ ǫ}2 x) − 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 l¨osningen η = 1 2 √ r11+ r22+ r33+ 1 ǫ=1 2   sgn(r32− r23)√r11− r22− r33+ 1 sgn(r13− r31)√r22− r33− r11+ 1 sgn(r21− r12)√r33− r11− r22+ 1   (2.10)

d¨ar η ≥ 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 med quaternioner kan man g¨ora p˚a f¨oljande s¨att (se figur 2.3):

• Skapa quaternionerna qv=< 0, v_kvk > och qr=< cosθ₂, r_krksinθ₂ >

• Utf¨or multiplikationerna q =< η, ǫ >= qr∗ qv∗ qr−1

• L˚at vrot= kvk · ǫ beteckna den resulterande vektorn.

I rapporten kommer enhetsquaternioner att anv¨andas f¨or att representera oriente-ring och ber¨akna rotationer. Detta p˚a grund av den enkelhet varmed detta l˚ater sig g¨oras.

Kapitel 3

Robotspecifik teori

Industrirobotar finns i m˚anga utf¨oranden, som alla har gemensamt att de best˚ar av ett antal l¨ankar och sammanbindande leder. Lederna kan vara av tv˚a typer - prismatiska och roterande, d¨ar de prismatiska lederna utf¨or en translaterande r¨orelse. Om man kopplar l¨ankarna i en f¨oljd, fr˚an f¨orsta till sista l¨anken, kallas det f¨or en ¨oppen kinematisk kedja. Om d¨aremot n˚agra l¨ankar bildar en slinga, kallas det f¨or en sluten kinematisk kedja.

F¨or att kunna positionera ett objekt med en robotarm kr¨avs tre frihetsgrader. Vill man sedan l˚ata objektet ha en viss orientering relativt ett referenskoordinatsystem kr¨avs ytterligare tre. Totalt beh¨ovs allts˚a sex frihetsgrader (och d¨armed sex leder) f¨or en godtycklig positionering och orientering av ett objekt. Om man l˚ater antalet leder ¨overstiga antalet frihetsgrader har man en kinematiskt redundant robot. En robot delas vanligen in i en arm och en handled, d¨ar armen sk¨oter positionering och handleden orientering. Beroende p˚a vilka typer av leder man v¨aljer, och hur de placeras i strukturen, f˚ar man olika typer av robotar (med olika egenskaper): an-tropomorfisk, cylindrisk, kartesisk, sf¨arisk och SCARA-robot. Den antropomorfiska armen best˚ar av tre roterande leder enligt figur 3.1 och har i princip ett sf¨ariskt arbetsomr˚ade.

Handleden kan ¨aven den se ut p˚a olika s¨att, men den mest generella ¨ar den sf¨ariska handleden. Denna best˚ar av tre roterande leder vars rotationsaxlar sk¨ar varandra i en punkt, och d¨armed kan godtycklig orientering uppn˚as.

P˚a handledens yttersta ¨ande brukar ett verktyg eller gripdon av n˚agot slag mon-teras. Denna punkt kallas f¨or TCP (Tool Center Point) eller arbetspunkt. Ett verktygskoordinatsystem kan kopplas till TCP s˚a att man kan styra position och orientering f¨or det verktyg som anv¨ands (annars styr man TCP).

Robotens l¨ankar och leder kan beskrivas med lokala koordinatsystem och relationer mellan dessa. F¨or varje led (prismatisk eller roterande) ans¨atts en ledvariabel (qi)

som beskriver ledens tillst˚and (translaterad str¨acka eller roterad vinkel). V¨ardet av

Figur 3.1.Tre roterande leder placerade s˚a att en antropomorfisk arm f˚as.

ledvariablerna best¨ammer position och orientering f¨or roboten. Med direktkinematik och inverskinematik g˚ar man fr˚an en beskrivning till den andra.

Ett exempel p˚a en antropomorfisk robot med sf¨arisk handled ¨ar IRB 1400 fr˚an ABB. F¨or denna tas en kinematisk modell fram (och implementeras i Matlab) som anv¨ands vid ber¨akningar i efterf¨oljande kapitel.

Om modellering av industrirobotar finns i att l¨asa i bl a [14] och [19].

3.1

Direktkinematik

Med direktkinematik best¨ammer man position och orientering som en funktion av ledvariablerna. Till skillnad fr˚an det inverskinematiska problemet f˚ar man alltid en unik l¨osning.

F¨or en robot av strukturen ¨oppen kinematisk kedja ber¨aknar man f¨or varje l¨ank transformationsmatrisen relativt f¨oreg˚aende l¨ank i kedjan. N¨ar man sedan multi-plicerar ihop dessa f˚ar man den slutliga transformationsmatrisen f¨or position och orientering relativt den f¨orsta l¨anken. (Om roboten ¨ar av strukturen sluten ki-nematisk kedja f˚ar man g¨ora p˚a ett lite annorlunda s¨att, vilket inte kommer att behandlas h¨ar.)

Dessa ber¨akningar kan systematiseras genom att f¨olja Denavit-Hartenbergkonven-tionen (appendix A) som f¨or varje l¨ank skapar ett koordinatsystem vilket beskrivs

3.2 Inverskinematik 13

av ett antal parametrar. Parametrarna anv¨ands f¨or att ta fram den transforma-tionsmatris som beskriver hur en l¨ank ¨ar kopplad till n¨asta.

Ett exempel p˚a ber¨akningar f¨or roboten IRB 1400 finns i avsnitt 3.3.

3.2

Inverskinematik

Med inverskinematik (IK) s¨oker man v¨arden p˚a ledvariablerna vilka g¨or att roboten f˚ar en given position och orientering. Det ¨ar inte alltid m¨ojligt att l¨osa detta problem - l¨osning kanske saknas eller ocks˚a existerar flera l¨osningar (m¨ojligen ett o¨andligt antal). Det ¨ar heller inte s¨akert att man kan ber¨akna ett analytiskt uttryck f¨or ledvariablerna.

I vissa fall kan ber¨akningarna delas upp i ett positionerings- och ett orienterings-problem. Det g¨aller speciellt f¨or robotar av samma struktur som IRB 1400 (antropo-morfisk robot med sf¨arisk handled). Det betyder att ledvariablerna f¨or handledens position (de tre f¨orsta) och de f¨or handledens orientering (de tre sista) l¨oses separat. I [19] finns en l¨osningsg˚ang (Algoritm 1) f¨or en antropomorfisk robot med sf¨arisk handled beskriven. Denna kommer att illustreras med exemplet i n¨asta avsnitt.

Algoritm 1 Inverskinematik f¨or en antropomorfisk robot med sf¨arisk handled.

1: Ber¨akna handledens position pw(q1, q2, q3) som pw= p − d6ad¨ar p ¨ar TCP:s

position, d6en DH-parameter (appendix A) och a den enhetsvektor som

sam-manfaller med rotationsaxeln f¨or den sista leden (riktad ut˚at). Se figur 3.2.

2: L¨os IK-problemet f¨or att finna (q1, q2, q3).

3: Ber¨akna R03(q1, q2, q3) som ger handledens orientering relativt den f¨orsta leden.

4: Ber¨akna R36(q4, q5, q6) = (R03)TR, d¨ar R ¨ar den rotationsmatris som beskriver

orienteringen i TCP.

5: L¨os IK-problemet f¨or att finna (q4, q5, q6).

3.3

Exempel - Roboten IRB 1400

Som ett exempel p˚a kinematiska ber¨akningar kommer en modell av roboten IRB 1400 att tas fram, vilken sedan kommer att implementeras i MATLAB med hj¨alp av Robotics Toolbox [2]. D˚a denna bara kan hantera robotar av strukturen ¨oppen kine-matisk kedja m˚aste modellen anpassas d¨arefter. (I [16] finns beskrivet en MATLAB-toolbox f¨or bangenerering i tre frihetsgrader.)

TCP n s a p p_w

Figur 3.2.Antropomorfisk arm med sf¨arisk handled.

3.3.1

Direktkinematik

F¨or att kunna anv¨anda modellen i Robotics Toolbox f˚ar man anse att roboten ¨ar formad som en ¨oppen kinematisk kedja, med sex roterande leder och sju l¨ankar. Figur 3.3 visar ledernas placering (lederna ligger i samma plan) och rotations-riktningar. Modellen kan nu tas fram enligt Denavit-Hartenbergs (DH) metod (se appendix A). #1 #2 #3 #4 #5 #6

Figur 3.3.Axlar och sidovy med m˚atts¨attning (mm) f¨or IRB 1400.

Det f¨orsta steget ¨ar att best¨amma riktningen f¨or robotens axlar, samt att numrera dessa med b¨orjan i den f¨orsta leden (se figur 3.4). Detta har gjorts p˚a s˚a s¨att att

3.3 Exempel - Roboten IRB 1400 15

parallella leder har lika riktning. Numreringen b¨orjar med axel z0 i led nummer

ett, och forts¨atter till axel z6 som tillh¨or koordinatsystemet f¨or TCP.

0 0.5 1 −0.2 0 0.2 −0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 z₆ z₅ z₄ x Rotationsaxlarnas riktning z₃ z₂ z₁ z₀ y z 0 0.5 1 −0.2 0 0.2 −0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 z₆ y₆ z₅ y₅ z₄ x₆ x₅ x₄ y₄ x Koordinatsystemens placering z₃ x₁ y₃ z₂ z₁ x₀ x₃ x₂ y₁ z₀ y₂ y₀ y z

Figur 3.4.Rotationsaxlarnas riktning och numrering (v¨anster) samt l¨ankarnas koordi-natsystem (h¨oger).

L¨ankarnas koordinatsystem har f˚att placeringar enligt figur 3.4. Tillsammans med m˚atten fr˚an figur 3.3 kan modellens parametrar ber¨aknas (tabell 3.1). DH-parametrarna anv¨ands slutligen till att best¨amma de homogena transformations-matriser som beskriver robotens direktkinematik.

L¨ank (i) ai αi di θi 1 150 +π 2 475 0 2 600 0 0 +π 2 3 120 +π 2 0 0 4 0 −π 2 720 0 5 0 +π₂ 0 0 6 0 0 85 0

Tabell 3.1.DH-parametrarna (m˚att i mm och radianer)

Modellen kan implementeras i MATLAB med Robotics Toolbox enligt figur 3.5. Funktionen link skapar ett l¨ankobjekt utifr˚an DH-parametrarna f¨or l¨anken. Funk-tionen robot skapar sedan ett robotobjekt av angivna l¨ankar.

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 f¨or position

Om man ser roboten fr˚an sidan (som i figur 3.3) och antar att inga begr¨ansningar finns g¨allande v¨arden p˚a ledvinklarna qi, kan man f˚a upp till fyra olika l¨osningar

f¨or positionen (figur 3.6) beroende p˚a vilka intervall q1 och q2 tillh¨or. Singul¨ara

punkter existerar d¨ar en entydig l¨osning inte kan best¨ammas, s˚a ¨aven punkter d¨ar l¨osning saknas.

Av alla vinklar ¨ar q1 den enklaste att l¨osa. Genom att f¨orst projicera vektorn pw

i xy-planet (figur 3.7) och sedan anv¨anda (MATLAB-) funktionen ATAN2(y,x) f˚as ett v¨arde ˜q1i intervallet [−π, +π]. (I den singul¨ara punkten (x, y) = (0, 0) kan detta

inte entydigt best¨ammas.) Nu finns det tv˚a m¨ojliga val av q1

q1= ( ˜ q1 shoulder right ˜ q1+ π shoulder left (3.1)

Om man l˚ater lederna 2, 3 och 5 utg¨ora h¨ornen p˚a en triangel i koordinatsystemet f¨or led 2 (O2) kan vinklarna q2och q3 ber¨aknas genom betraktande av figur 3.8.

Eftersom cos(π − ˜q3) = − cos ˜q3 ger cosinussatsen (b3=pa23+ d24):

˜ q3= ( + arccos(c 2 −a2 2−b 2 3 2a2b3 ) elbow up − arccos(c 2 −a2 2−b 2 3 2a2b3 ) elbow down

Summan av ˜q2och den mellan str¨ackorna a2och c bildade vinkeln ¨ar identisk med

vinkeln mellan y-axeln och linjen fr˚an origo till punkten (px, py). Tangens f¨or dessa

vinklar ¨ar b3sin ˜q3

a2+b3cos ˜q3 respektive

px

3.3 Exempel - Roboten IRB 1400 17

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

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

Figur 3.6.De m¨ojliga l¨osningarna shoulder left/right, elbow up/down.

˜

q2= arctan2(b3sin ˜q3, a2+ b3cos ˜q3) − arctan2(px, py).

N¨ar vi nu har valt elbow up eller elbow down kan slutligen q2 och q3 best¨ammas

(δ = arctand4 a3): q2= ( π 2 + ˜q2 shoulder right π 2 − ˜q2 shoulder left (3.2) q3= ( δ − ˜q3 shoulder right δ + ˜q3 shoulder left (3.3)

0 0.2 0.4 0.6 0.8 1 −0.5 −0.3 −0.1 0.1 0.3 0.5 0 0.2 0.4 X Y Z

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

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Geometriska samband a2 a3 d4 b3 c ˜ q2 ˜ q3 (px, py) x[m] y [m ]

Figur 3.8.Geometriska samband f¨or IRB 1400 d˚a enbart axel 2 och 3 betraktas.

IK f¨or orientering

Efter ber¨akningen R36(q4, q5, q6) = (R03)TRf˚ar man en matris p˚a formen

R3₆=   n3 x s3x a3x n3 y s3y a3y n3 z s3z a3z  

3.3 Exempel - Roboten IRB 1400 19

ur vilken q4, q5 och q6 kan best¨ammas med hj¨alp av den homogena

transforma-tionsmatrisen T36=  R36 p36 0T ₁  d¨ar R3₆=   c4c5c6− s4s6 −c4c5s6− s4c6 c4s5 s4c5c6+ c4s6 −s4c5s6+ c4c6 s4s5 −s5c6 s5s6 c5  

Genom att identifiera l¨ampliga element i de b˚ada matriserna R36f˚ar man l¨osningar

med q5 i n˚agot av de tv˚a intervallen [−π, 0[ och ]0, π]. Om handleden skulle vara

rak, (a3

x)2+ (a3y)2 = 0, f˚ar man q5 = 0 samt o¨andligt m˚anga l¨osningar p˚a q4 och

q6. I annat fall blir l¨osningarna

q5∈]0, π] :        q4= arctan2(a3y, a3x) q5= arctan2( q (a3 x)2+ (a3y)2, a3z) q6= arctan2(s3z, −n3z) q5∈ [−π, 0[:       

q4= arctan2(−a3y, −a3x)

q5= arctan2(−

q (a3

x)2+ (a3y)2, a3z)

q6= arctan2(−s3z, n3z)

En MATLAB-funktion (se figur 3.9) som utf¨or beskrivna ber¨akningar har tagits fram. F¨or ett angivet robotobjekt, ¨onskad position och orientering (i en homo-gen transformationsmatris) samt ¨onskem˚al om typ av l¨osning (shoulder left/right, elbow. . .) returnerar den v¨arden p˚a ledvariablerna q1 till q6. Valet av wrist beror

p˚a vilket intervall man vill att q5 ska tillh¨ora (positiva eller negativa v¨arden).

Om en punkt ligger utanf¨or robotens arbetsomr˚ade kommer ett felmeddelande att visas. Felet indikeras med err6=0 och q blir en en tom vektor.

N¨ar en singularitet p˚atr¨affas returneras en l¨osning q som inte ¨ar entydigt best¨amd. Inget fel indikeras (err=0), men en varningstext visas.

V¨art att notera ¨ar att denna funktion g¨aller f¨or alla robotar av samma struktur som IRB 1400, f¨orutsatt att DH-parametrarna ber¨aknats utg˚aende fr˚an koordinat-systemen i figur 3.4.

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

%IK - Inverskinematik f¨or 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 f¨or 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 f¨or ber¨akning av inverskinematik f¨or robotar av samma struktur som IRB 1400

Kapitel 4

Kartesisk banrepresentation

Detta kapitel kommer att ge en beskrivning av hur en bana byggs upp och hur position och orientering f¨or roboten best¨ams l¨angs en s˚adan.

F¨or att slippa diskontinuiteter i hastighet och acceleration l¨angs banan anv¨ands s k zoner d¨ar man till˚ater avvikelser fr˚an banans ¨onskade utseende.

En metod f¨or ber¨akning av banan i en zon presenteras i kapitel 4.3.2.

(F¨or interpolering av orientering (med quaternioner) och i n˚agra fall ¨aven position finns mycket att l¨asa i t ex [1], [5], [6], [10], [11] och [13].)

4.1

Banans delar

Den bana man vill att robotens verktyg skall f¨olja byggs upp (i ett globalt koordi-natsystem) av ett eller flera segment. Ett segment ¨ar en enkel typ av kurva, en s˚a kallad banprimitiv (se figur 4.3). N¨ar segmenten kopplas samman s˚a att slutpunk-ten f¨or ett blir startpunkslutpunk-ten f¨or n¨asta f˚as en bana av mer godtyckligt utseende. Figur 4.1 visar en bana best˚aende av tv˚a linjer och tv˚a cirkelb˚agar.

Start Slut P1 P2 P3 P4 P5 P6 P7

Figur 4.1.En bana best˚aende av linjer och cirkelb˚agar. 21

genom att l˚ata Rz= 0 kommer dess inverkan p˚a banan att f¨orsvinna.

Zon

Zon

Sektion

Segment

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

Man vill ¨aven att orienteringen av verktyget skall kunna best¨ammas l¨angs banan. Detta kan g¨oras genom att till segmentens ¨andpunkter koppla information om orientering (i form av enhetsquaternioner), som sedan interpoleras fram f¨or en given position l¨angs segmentet.

4.2

Banprimitiver

4.2.1

Position

Banprimitiverna (segmenten) best¨ams av en parameterbeskrivning i ett, f¨or varje segment, lokalt koordinatsystem O′_{. Detta system best¨ams av translationsvektorn}

rc och rotationsmatrisen R s˚a 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)  =   R0cos(θ0s) R0sin(θ0s) h0s  =   cos(θ0s) − sin(θ0s) 0 sin(θ0s) cos(θ0s) 0 0 0 1     R0 0 h0s   (4.1)

d¨ar s ∈ [0, 1] best¨ammer alla punkter l¨angs kurvan (fr˚an starten i s = 0 till slutet i s = 1) och R0, θ0samt h0¨ar konstanter, kan f¨oljande fyra banprimitiver beskrivas:

• Punkt R0= 0, h0= 0 (f¨or en punkt i origo)

4.2 Banprimitiver 23

• Cirkelb˚age R06= 0, h0= 0, θ06= 0

• Helix (cirkul¨ar) R06= 0, h06= 0, θ06= 0

F¨or punkten och linjen finns andra val av konstanter. Att just dessa valts f˚ar sin f¨orklaring nedan. x y z x y z x y z x y z

Punkt Linje Cirkelbåge Helix

Figur 4.3.Banprimitiverna punkt, linje, cirkelb˚age och helix.

Val av parametrar

Valet av parametrar (R0, θ0, h0) och lokalt koordinatsystem (rc och R) g¨ors s˚a att

h0 >= 0 och R0 >= 0. Det inneb¨ar att en linje alltid utg˚ar fr˚an origo och f¨oljer

ˆ

z-axeln, samt att en helix ”v¨axer” l¨angs samma axel. En cirkelb˚age kommer alltid att b¨orja p˚a avst˚andet R0fr˚an origo l¨angs positiva ˆx-axeln och sluta n˚agonstans i

ˆ

xˆy-planet. Helixen b¨orjar i samma punkt som en cirkelb˚age skulle g¨ora och ”vrider sig” kring ˆz-axeln p˚a en t¨ankt cylinder med radien R0. Tecknet p˚a θ0 best¨ammer

om man har en h¨oger- eller v¨anstervriden helix. F¨or cirkelb˚agen g¨or valet θ0 > 0

att representationen blir entydig (annars kan man ha en rotationsmatris R med ˆ

z-riktningen omv¨and mot fallet θ0> 0 vilken ger den ¨onskade cirkelb˚agen d˚a θ0< 0

- se figur 4.4). θ0>0 θ0<0 P1 P1 P2 P2 ˆ x ˆ x ˆ y ˆ y ˆ z ˆ z

r r r r

L¨angden av ett segment

I beskrivningen ovan anv¨ands index s ∈ [0, 1] f¨or att beskriva ett segment. Detta index s¨ager dock inget om hur l˚ang str¨acka man har f¨orflyttat sig l¨angs segmentet f¨or ett visst v¨arde p˚a s. Denna l¨angd kan ber¨aknas till

l(sl) = sl Z 0 kdr_dsk ds = sl Z 0 r (dx ds) 2_{+ (}dy ds) 2_{+ (}dz ds) 2_ds = sl Z 0 q (R0θ0)2+ h20ds = sl· q (R0θ0)2+ h20

och genom att s¨atta sl= 1 f˚ar man den totala l¨angden av segmentet:

Lc=

q

(R0θ0)2+ h20 (4.2)

4.2.2

Orientering

Orientering p˚a en linje

Om man i ¨andpunkterna beskriver ¨onskad orientering med hj¨alp av enhetsquaterni-onerna q0och q1, kan man med sf¨arisk linj¨ar interpolering best¨amma orienteringen

i den position som best¨ams av indexet s ovan. Denna interpolering sker p˚a den fyrdimensionella enhetssf¨aren och de quaternioner som f˚as ligger p˚a den storcirkel som sammanbinder q0 med q1. Det betyder allts˚a att q0 ¨overg˚ar i q1 l¨angs den

kortaste v¨agen p˚a sf¨aren, vilket ger en mjuk ¨overg˚ang i orientering mellan linjens ¨andpunkter.

En funktion f¨or sf¨arisk linj¨ar interpolering finns beskriven i [20]: Slerp(q0, q1; s) = sin((1 − s)θ)

sin θ q0+ sin(sθ)

sin θ q1 (4.3)

d¨ar q0· q1= cos θ och s ∈ [0, 1]. (Slerp st˚ar f¨or S pherical linear interpolation.)

Orientering p˚a en cirkelb˚age

Interpolering av orientering p˚a en cirkelb˚age g¨ors enklast om man ser cirkelb˚agen som en r¨at linje vilken b¨ojts kring en cylinder (figur 4.5(a)). Interpoleringen sker

4.2 Banprimitiver 25

p˚a linjen som d¨arefter transformeras till en cirkelb˚age.

Om man i linjens ¨andar placerar identiska koordinatsystem kommer, n¨ar cirkelb˚agen skapats, 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) Cirkelb˚age som en linje kr¨okt runt en cylinder. (b) Cirkelb˚agen och linjen sedda fr˚an cylinderns z-axel. (c) Rotation av koordinatsystemet i cirkelb˚agens slutpunkt.

Eftersom man k¨anner cirkelb˚agen och orienteringen i dess ¨andar (q0 och q1) kan

interpoleringen nu utf¨oras i tre steg:

1. Kompensera f¨or kr¨okningen genom att ”vrida tillbaka” q1med vinkeln θ kring

cylinderns z-axel. Kalla den modifierade q1f¨or q′1.

2. Utf¨or en sf¨arisk linj¨ar interpolering mellan q0 och q1′ p˚a samma s¨att som

tidigare.

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

P˚a det h¨ar s¨attet kommer q(s) att f¨or fallet q′

1= q0alltid vara orienterad p˚a samma

s¨att relativt cirkelb˚agen i alla punkter l¨angs kurvan.

Att interpoleringen inte sker direkt mellan q0och q1beror p˚a att Slerp interpolerar

l¨angs den kortaste v¨agen p˚a sf¨aren. Det betyder att man f¨or en cirkelb˚age (eller helix) med θ0≥ 180◦ inte alltid kan f˚a q(s) enligt f¨oreg˚aende stycke. Figur 4.6 ger

ett exempel p˚a en s˚adan cirkelb˚age.

Orientering p˚a en helix

Helixen skiljer sig inte fr˚an cirkelb˚agen vad g¨aller interpolering av orientering. Ko-ordinatsystemet i slutpunkten ¨ar bara translaterat l¨angs z-axeln, och rotationen kring densamma best¨ams fortfarande av θ.

Figur 4.6. En cirkelb˚age med θ0 ≥ 180◦ med pilar som representerar orienteringen.

Orienteringen har i v¨anster figur interpolerats med och i h¨oger utan h¨ansyn tagen till kurvans kr¨okning.

4.3

Zonbanan

Banans utseende utanf¨or zonerna best¨ams helt av den f¨or varje segment angivna parameterbeskrivningen. Inuti en zon m˚aste man p˚a n˚agot s¨att ber¨akna en ny bana (zonbanan) s˚a att denna och ˚atminstone dess f¨orstaderivata blir kontinuerliga i zongr¨anserna.

F¨or att kunna g¨ora det m˚aste man f¨orst veta var segmentet och zonen sk¨ar varand-ra.

4.3.1

Sk¨

arning mellan segment och zon

F¨or att kunna ber¨akna f¨or vilket v¨arde p˚a s som ett segment passerar en viss zon m˚aste man k¨anna typen av segment, segmentets parameterbeskrivning, zonradien Rz och l¨aget av zonen (i b¨orjan eller i slutet av segmentet).

Zonen delar segmentet i tv˚a delar. Sk¨arningen med segmentet ges d˚a zonen befinner sig i b¨orjan av segmentet av

kr(s) − r(0)k = Rz, s > 0

och i slutet av segmentet ges sk¨arningen av

kr(s) − r(1)k = Rz, s < 1

Ett allm¨ant uttryck f¨or sk¨arningen mellan ett segment och en sf¨ar (zonen) med radien Rzplacerad med centrum p˚a kurvan f˚as ur

kr(s) − r(τ)k = Rz

4.3 Zonbanan 27 Med parameterbeskrivningen r(s) =   R0cos(θ0s) R0sin(θ0s) h0s  , s ∈ [0, 1] f˚ar man att kr(s) − r(τ)k2= . . . = 2R20(1 − cos(θ0(s − τ))) + h20(s − τ)2= R2z

vilket ger ekvationen

h20(s − τ)2− 2R20cos(θ0(s − τ)) + 2R20− R2z= 0

som g¨aller f¨or de ovan beskrivna segmenten. F¨or variablerna s och τ g¨aller s ∈ [0, 1] och τ ∈ [0, 1].

Sk¨arning med linje

F¨or en linje blir ekvationen f¨or sk¨arning med zonen h2

0(s − τ)2− R2z= 0

vilket f¨orenklas till

s = τ ±R_hz

0

D˚a endast segmentets ¨andpunkter ¨ar av intresse s¨atts τ till noll eller ett beroende p˚a zonens l¨age. Eftersom s bara kan anta v¨arden mellan noll och ett blir resultatet f¨oljande: s =Rz h0 (τ = 0) och s = 1 −R_hz 0 (τ = 1)

H¨arur kan ses att kvoten Rz

h0 inte f˚ar vara st¨orre ¨an ett, allts˚a Rz ≤ h0, men

i praktiska till¨ampningar anv¨ands Rz ≤ 1₂h0 f¨or att tv˚a zoner inte ska kunna

¨overlappa varandra (figur 4.7).

Sk¨arning med cirkelb˚age F¨or cirkelb˚agen f˚as ekvationen

Figur 4.7.Olika zonradie f¨or ett segment. eller omskrivet cos(θ0(s − τ)) = 2R2 0− R2z 2R2 0 = 1 −1 2  Rz R0 2 vilket ger s = τ ±_θ1 0 arccos " 1 − 1₂ R_Rz 0 2#

Zonradiens maximala storlek erh˚alls ur

−1 ≤ 1 − 1₂ R_Rz

0

2 ≤ 1

eftersom argumentet till arccos m˚aste tillh¨ora det intervallet f¨or att ett reellt v¨arde ska erh˚allas. Efter omskrivning f˚as

Rz≤ 2R0

F¨or att inte tv˚a zoner ska ¨overlappa varandra f˚ar zonradien inte ¨overstiga halva avst˚andet mellan cirkelb˚agens start- och slutpunkter (figur 4.8). Avst˚andet ¨ar

2R0sin(

θ0

2) vilket resulterar i uttrycket

Rz≤ R0sin(

θ0

2 )

Sk¨arning med helix

F¨or den cirkul¨ara helixen g¨aller den allm¨anna ekvationen h20(s − τ)2− 2R20cos(θ0(s − τ)) + 2R20− R2z= 0

som inte kan f¨orenklas mycket mer ¨an till formen

(s − τ)2− 2 R_h0 0 2 cos(θ0(s − τ)) = R 2 z− 2R20 h2 0

4.3 Zonbanan 29 R0 R0 θ0 θ0 2R0sin1₂θ0 2R0sin1₂θ0

Figur 4.8.Avst˚andet mellan cirkelb˚agens start- och slutpunkter.

men med τ konstant och

ϕ = θ0(s − τ) α = θ0R0 h0 β = θ0Rz h0 f˚ar man ϕ2− 2α2cos ϕ = β2− 2α2

Denna ekvation kan givetvis l¨osas numeriskt, men genom att anta att ϕ ¨ar liten (s n¨ara τ ) kan approximationen cos ϕ ≈ 1 −12ϕ

2

g¨oras vilken leder till ekvationen ϕ2(1 + α2) = β2

eller

ϕ = ±√ β 1 + α2

¨

Aven h¨ar b¨or zonradien uppfylla Rz ≤ 1₂kr(1) − r(0)k, men i de fall θ0 ≥ 2π

b¨or ¨aven Rz ≤ 1₂kr(2π_θ0) − r(0)k uppfyllas. Det f¨orsta f¨or att zonerna i helixens

¨andpunkter inte ska kunna ¨overlappa varandra, det andra f¨or att zonbanan inte ska kollidera med helixen utanf¨or zonen, vilket annars skulle kunna ske. (Att ingen figur ges beror p˚a sv˚arigheter med att illustrera p˚a ett bra s¨att.)

4.3.2

Interpolering i zonen

Inuti en zon ¨onskas uttryck f¨or position, r(s), och orientering, q(s), f¨or s ∈ [0, 1]. De segment som g˚ar in i respektive ut ur zonen beskrivs av r1(s) och r2(s) (kapitel

4.2.1) och sk¨arningarna med zonen f˚as enligt kapitel 4.3.1 till s1 = s1z respektive

och

˜

r2(s) = r2(s · s2z)

s˚a att ˜r1(0) = r1(s1z), ˜r2(1) = r2(s2z) och ˜r1(1) = r1(1) = r2(0) = ˜r2(0) (se figur

4.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 f¨or en zon.

D˚a b˚ada segmenten ¨ar linjer kan man anv¨anda den s k de Casteljau-algoritmen [9] f¨or att skapa en parabelformad kurva i zonen vars derivata m a p s har samma riktning som derivatan f¨or linjerna. I det enklaste fallet byggs kurvan upp utifr˚an tre punkter enligt

p1_{0(s) = (1 − s)p0}+ sp1

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

p2_{0(s) = (1 − s)p}1₀+ sp11

d¨ar man kan se att p1

0 och p11 b˚ada ¨ar ekvationer f¨or en linje och att dessa v¨ags

ihop s˚a att en ekvation i s2 _erh˚alls.

Om man ist¨allet f¨or uttrycken f¨or p1

0och p11 ovan anv¨ander

parameterbeskrivning-arna f¨or segmenten, borde man kunna skapa en zonbana f¨or valfri kombination av linje, cirkelb˚age och helix:

r_{(s) = (1 − s)˜r}1(s) + s˜r2(s) (4.4)

Denna funktion uppfyller r(0) = ˜r1(0) och r(1) = ˜r2(1) och ¨ar allts˚a (som ¨onskat)

4.3 Zonbanan 31

Tyv¨arr ¨ar dess f¨orstaderivata inte kontinuerlig i dessa punkter. Eftersom r′(s) = ˜r′1(s) + s(˜r2′(s) − ˜r′1(s)) + (˜r2(s) − ˜r1(s))

blir derivatorna

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

och

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

I (4.4) sker en linj¨ar ¨overg˚ang fr˚an ˜r1(s) till ˜r2(s). Genom att ers¨atta denna med

ett allm¨ant (olinj¨art) uttryck α(s) kan man f˚a viss kontroll ¨over egenskaperna hos r(s). Uttrycket f¨or r(s) blir d˚a

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

med funktionerna ˜r1(s) och ˜r2(s) som f¨orut.

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 f¨oljande krav p˚a α(s):

α(0) = 0, α′(0) = 0, α(1) = 1, α′(1) = 0 (4.7) Det g˚ar att finna flera funktioner som uppfyller (4.7), men ett tredjegradspolynom

α(s) =

3

X

j=0

ajsj (4.8)

¨ar det enklaste valet. Efter derivering kan ett ekvationssystem st¨allas upp och funktionen best¨ammas.

Om man ¨onskar att alla derivator dn

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

zongr¨anserna f˚ar man genom upprepade deriveringar f¨oljande villkor p˚a α(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 best¨ammas. F¨or fallen N = 1, N = 2 och N = 3 ber¨aknas α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)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.1 0.2 0.3 0.4 0.5 (a) α2N +1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.25 0.5 0.75 1 1.25 (b) α′ 2N +1

Figur 4.10.Funktionen α2N +1samt α′2N +1f¨or N = 1 (punktad linje), N = 2 (streckad

linje) och N = 3 (heldragen linje)

Orientering

Orienteringen f¨or del av respektive segment som befinner sig i zonen beskrivs (p˚a liknande s¨att som f¨or positionen) av

˜

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

och

˜

q2(s) = q2(s · s2z)

s˚a att ˜q1(0) = q1(s1z) och ˜q2(1) = q2(s2z) med qi(s) enligt kapitel 4.2.2. I zonens

centrum g¨aller ˜q1(1) = ˜q2(0).

L¨angs zonbanan kan orienteringen q(s) ber¨aknas med en variant av de Casteljau-algoritmen d¨ar sf¨arisk linj¨ar interpolering anv¨ands (orienteringen representeras av enhetsquaternioner). Genom att ers¨atta ˜ri(s) med ˜qi(s) och anv¨anda funktionen

Slerp f˚ar man

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

som motsvarighet till (4.4).

Ber¨aknar man positionen i zonen enligt (4.5) m˚aste dock uttrycket

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

4.4 Interpolering l¨angs banan 33

4.4

Interpolering l¨

angs banan

Utanf¨or zonerna

P˚a segmenten utanf¨or zonerna r˚ader det enkla (linj¨ara) sambandet lc(s) = Lc· s,

med Lc enligt (4.2), mellan index lc och s. Detta g¨or att s(lc) l¨att kan best¨ammas

i en punkt p˚a banan bara man k¨anner l¨angden av banan i b¨orjan och slutet av aktuellt segment. Om Lαoch Lβ f˚ar st˚a f¨or dessa l¨angder kan index s best¨ammas

f¨or ett givet lc som

s = lc− Lα Lβ− Lα

, lc∈ [Lα, Lβ]

d¨ar l¨angden av segmentet ges av Lβ− Lα.

Inuti zonerna

I en zon finns normalt inget linj¨art samband mellan index lc och s. Ej heller finns

n˚agot analytiskt uttryck f¨or zonbanans l¨angd, men denna kan f¨or tillr¨ackligt stora N approximeras med summan

Lz= N−1

X

n=0

kr(n + 1_N ) − r(_Nn)k

(eller n˚agon liknande summa som tar h¨ansyn till storleken av normen f¨or varje v¨arde p˚a n).

Om man f¨or varje n sparar delsumman och motsvarande v¨arde p˚a s, kan man skapa en splinefunktion (kapitel 6.1) sz(l), l ∈ [0, Lz] som (approximativt) beskriver s(lc)

i zon z.

L¨angs banan

Varje segment beskrivs av index si. Det sif¨or vilket segmentet sk¨ar en zon ben¨amns

f¨or segment i som siz# d¨ar #-tecknet st˚ar f¨or zonens nummer, t ex s3z2 f¨or det

tredje segmentets sk¨arning med den andra zonen. Figur 4.11 visar detta f¨or tre segment och tv˚a zoner.

Med index lc ∈ [0, Lc] beskrivs banans punkter fr˚an startpunkten till slutpunkten

(Lc=banans l¨angd). Genom att h˚alla reda p˚a lc f¨or start- och slutpunkt samt

zongr¨anserna kan man best¨amma vilken bandel och vilket s som beskriver en viss punkt. Om i betecknar bandelens nummer f˚ar man att

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

s1= 0

s2= s2z2 s2= 1, s3= 0

s3= s3z2

Figur 4.11.Index f¨or banans segment.

j¨amnt nummer ¨ar en zonbana (se figur 4.12). Index s kan nu best¨ammas som

s = (

lc−Li−1

Li−Li−1, i = 2n − 1

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

d¨ar sn(l) ¨ar den tidigare beskrivna splinefunktionen som kopplar samman index s

och index lc p˚a zonbanan.

#1

#2

#3

#4

#5

Figur 4.12.Numrering av banans delar.

Beskrivna funktioner och metoder har implementerats i Matlab. Dessa anv¨ands i kapitel 5 d¨ar n˚agra exempel ges p˚a zonbanor ber¨aknade enligt avsnitt 4.3.2.

Kapitel 5

Exempel p˚

a kartesisk

zoninterpolering

Flera exempel p˚a banor som kombinerar olika typer av banprimitiver kommer att ges. F¨or varje exempel har zonbanan ber¨aknats f¨or α3, α5 respektive α7 (enligt

kapitel 4.3.2). En j¨amf¨orelse med en zonbana skapad med linj¨ara l-kurvor, beskrivet i [17], kommer ocks˚a att g¨oras f¨or en bana best˚aende av tv˚a linjer.

I kapitel 7 kommer dessa banor att omvandlas till splinefunktioner f¨or robotens ledvariabler f¨or ytterligare j¨amf¨orelser.

De zonbanor som visas i varje exempel kommer att se n¨astintill identiska ut. Det beror p˚a att metoden f¨or zoninterpolering med αN fr¨amst p˚averkar derivatorna

av zonbanans parameterbeskrivning. Att det blir s˚a kan ses genom att j¨amf¨ora ekvationerna (4.5) och (4.6) med figur 4.10 d¨ar, f¨or olika N , funktionerna αN har

likartat utseende men deras derivator α′

N klart skiljer sig ˚at.

Den f¨orklaring av figur 5.2 som ges i n¨asta avsnitt g¨aller ¨aven figurerna 5.3–5.5.

5.1

Linje-Linje

Figur 5.2 ¨ar uppdelad s˚a att delfigur (a) visar hela banan (med orientering), och (b)–(d) zonbanan f¨or respektive αN. Den tjocka linjen markerar banan, de tunna

punktade linjerna markerar segment och zoner, vilkas sk¨arningar ¨aven m¨arks ut av en 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 enhetsquaternion som representerar orienteringen i punkten. Genom att translatera systemet i ne-gativ z-riktning och rita en linje fr˚an origo till z-axelns sk¨arning med banan har pilskaftet skapats. Den svarta respektive gr˚a ”fj¨adern” markerar x- och y-axlarnas

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

Banorna best˚ar av tv˚a segment med en zon d¨ar dessa m¨ots. Start-, mitt- och slutpunkterna ben¨amns p1, p2 och p3betr¨affande positionen; orienteringen i dessa

punkter ben¨amns q1, q2och q3. Orienteringen i ¨andpunkterna (q1och q3) har valts

s˚a att z-axeln (TCP) ¨ar vinkelr¨at mot respektive segment. I var och en av dessa punkter har orienteringen m¨arkts ut med en dubbelt s˚a stor pil som normalt. I figur 5.2(a) bildar tv˚a linjer av l¨angd 0,60 m en r¨at vinkel i en zon med radien 0,20 m. Startpunkten p1har koordinaterna (0,4;1) i figuren. Orienteringen har valts

s˚a att z-axeln ligger i det plan som bildas av linjesegmenten. F¨or q1 g¨aller att

y-axeln ligger i detta plan, motriktad det f¨orsta segmentet. F¨or q3 ligger x-axeln i

planet, men riktad l¨angs med segmentet. I zonen har q2valts som ”ett mellanting”

av q1 och q3.

5.2

Linje-Cirkelb˚

age

Banan i figur 5.3(a) best˚ar av en linje och en cirkelb˚age i ett plan med en zon av radie 0,15 m. Startpunkten p1 har koordinaterna (0,6;1) i figuren. Orienteringen

har valts (som i f¨oreg˚aende exempel) s˚a att z-axeln ligger i planet. F¨or q1g¨aller att

y-axeln ligger i detta plan, motriktad det f¨orsta segmentet. F¨or q3ligger x-axeln i

planet, men riktad l¨angs med segmentet. I zonen har q2valts som ”ett mellanting”

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) Tv˚a 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 f¨or α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 f¨or α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 f¨or α7.

Figur 5.2.(a) Bana best˚aende av tv˚a linjer. (b)–(d) Zonbanan f¨or α3, α5 och α7.

5.3

Linje-Helix

Denna bana, figur 5.4(a), best˚ar av en linje f¨oljd av en helix. Zonens radie ¨ar 0,10 m. Startpunkten p1har koordinaterna (0,2;0,4;0,5). Orienteringen ¨ar vald med z-axeln

vinkelr¨at mot banan. F¨or q1och q2¨ar x-axeln riktad l¨angs med linjesegmentet och

y-axeln motriktad helixens z-axel. F¨or q3 g¨aller att z-axeln sammanfaller med q2:s

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

(a) Linje och cirkelb˚age.

0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.25 0.3 0.35 0.4 (b) Zonbana f¨or α3. 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 f¨or α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 (d) Zonbana f¨or α7.

Figur 5.3.(a) Bana best˚aende av en linje och en cirkelb˚age. (b)–(d) Zonbanan f¨or α3,

α5 och α7.

5.4

Cirkelb˚

age-Helix

Genom att byta linjesegmentet i exempel 5.3 mot en cirkelb˚age och ¨andra oriente-ringen i startpunkten f˚ar man banan i figur 5.5(a). Zonradien, p1, p2 och p3 samt

q2 och q3 ¨ar identiska med f¨oreg˚aende exempel. I startpunkten har q1 vridits 180◦

kring (linje-)segmentet s˚a att z-axeln och y-axeln har f˚att motsatt riktning mot tidigare.

5.5 Linje-Linje, linj¨ara 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 (b) Zonbana f¨or α3. 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 f¨or α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 (d) Zonbana f¨or α7.

Figur 5.4.(a) Bana best˚aende av en linje och en helix. (b)–(d) Zonbanan f¨or α3, α5 och

α7.

5.5

Linje-Linje, linj¨

ara l-kurvor

H¨ar (figur 5.6) har samma bana som i exempel 5.1 anv¨ants. Zonbanan i figur 5.6(b) ¨ar ber¨aknad med de i [17] beskrivna linj¨ara l-kurvorna. Dessa anv¨andes ursprungli-gen enbart till att best¨amma position i zonen men orientering kan best¨ammas 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

(a) Cirkelb˚age och helix.

0.5 0.55 0.6 0.65 0.7 0.32 0.34 0.36 0.38 0.4 0.42 (b) Zonbana f¨or α3. 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 f¨or α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 (d) Zonbana f¨or α7.

Figur 5.5.(a) Bana best˚aende av en cirkelb˚age och en helix. (b)–(d) Zonbanan f¨or α3,

α5 och α7.

ett till¨agg. Efter anpassning till h¨ar anv¨and notation f˚ar man uttrycken

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

s2= s2z· s2

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

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

5.5 Linje-Linje, linj¨ara 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 - linj¨ara 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 - linj¨ara 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 f¨or tv˚a linjer. F¨or linj¨ara l-kurvor och f¨or α5.

Zonbanan i figur 5.6(d) ¨ar ber¨aknad enligt

s1= s1z+ (1 − s1z) · s

s2= s2z· s

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

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

Kapitel 6

Interpolering i

vinkelkoordinater

Den i kapitel 4 framtagna kartesiska banan m˚aste transformeras till motorvinkel-koordinater innan reglering av roboten kan ske. En algoritm f¨or denna transforme-ring beskrivs, d¨ar resultatet ¨ar en splinefunktion som approximerar ledvariablernas v¨arden l¨angs banan.

I kapitel 7 kommer algoritmen att appliceras p˚a banorna fr˚an kapitel 5. Det h¨ar kapitlet inleds med en beskrivning av kubiska splinefunktioner.

6.1

Splines

Att approximera en funktion f kan g¨oras p˚a flera s¨att. T ex kan ett polynom p av grad N best¨ammas s˚a att p(xi) = f (xi) f¨or N +1 punkter xi. Mellan dessa punkter

kan dock avvikelsen mellan p och f vara stor och med oscillationer i p d¨ar s˚adana saknas i f . Detta g¨aller s¨arskilt d˚a N antar stora v¨arden.

En splinefunktion s(x) best˚ar 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 g˚anger kontinuerligt deriverbar.

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

kallas inre noder. Genom att st¨alla krav p˚a s(x) i noderna kan man f˚a en spline-funktion med ¨onskade egenskaper.

F¨or en interpolerande kubisk splinefunktion tillkommer kravet s(xi) = f (xi) i

noderna, vilket ger 2(n − 1) villkor p˚a s(x). Kravet p˚a kontinuitet f¨or s′ _{och s}′′_ger

ytterligare 2(n − 2) villkor i de inre noderna. Sammanlagt f˚as 4n − 6 villkor. Eftersom man med n noder f˚ar n − 1 intervall (och polynom) blir antalet s¨okta koefficienter f¨or best¨amning av s(x) lika med 4(n − 1). Antalet koefficienter ¨ar fler ¨

an villkoren som st¨allts, s˚a ytterligare tv˚a krav p˚a s(x) m˚aste g¨oras. D˚a s′_(x

1) = f′(x1) och s′(xn) = f′(xn) f˚as en s k interpolerande kubisk

splinefunk-tion med r¨atta randvillkor och s(x) kan entydigt best¨ammas.

Kraven p˚a en interpolerande kubisk splinefunktion med r¨atta randvillkor blir f¨or approximation 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 f¨

or bangenerering

I [17] har tv˚a algoritmer f¨or bangenerering i vinkelkoordinater beskrivits. Den ena anv¨ands i zonerna, den andra p˚a banan mellan zonerna. Algoritm 2 ¨ar en samman-slagning och utvidgning (till sex frihetsgrader) av dessa, med f¨oljande f¨or¨andringar: • Ingen uppdelning av banan g¨ors, alla ber¨akningar sker fr˚an lc= 0 till lc= Lc.

• Ber¨akning av orientering har lagts till, med kontroll av maximalt ”oriente-ringsfel”.

• Uppdateringen av stegl¨angden har ¨andrats (andra, b¨attre, alternativ finns f¨orst˚as).

F¨or ett visst v¨arde p˚a lc (med b¨orjan i lc= 0) ber¨aknas position och orientering i

punkterna lc+ nh, n = 0, 1, ..., N −1. Ledvariablernas v¨arden i punkterna best¨ams

med inverskinematik och anv¨ands till att skapa en splinefunktion ˆq_tmp f¨or ledva-riablerna i intervallet. Denna utv¨arderas sedan i N −1 punkter fr˚an lc+ h/2 och

6.3 Felutv¨ardering 45

I de utv¨arderade punkterna best¨ams (direktkinematik) position och orientering som j¨amf¨ors med till˚atna maximala fel. Om n˚agon punkt avviker f¨or mycket m a p position eller orientering halveras h och en ny ˆq_tmpskapas och utv¨arderas. Annars, om ˆq_tmp ¨ar till bel˚atenhet, g¨ors en sammanslagning med splinefunktionen ˆq som beskriver ledvariablerna l¨angs hela banan och stegl¨angden h samt lc uppdateras.

Proceduren upprepas tills dess att hela banan har behandlats.

6.3

Felutv¨

ardering

Splinefunktionen ¨ar ju bara en approximation av ledvariablernas v¨arden l¨angs ba-nan, s˚a ett visst fel i position och orientering kommer att upptr¨ada n¨ar den anv¨ands vid styrning av roboten.

Om p och q st˚ar f¨or ¨onskad position och orientering och ˆpoch ˆq st˚ar f¨or den position och orientering som f˚as via splinefunktionen kan felen best¨ammas som f¨oljer.

Positioneringsfel

Mellan punkterna p och ˆp ¨ar det euklidiska avst˚andet (normen) ett m˚att p˚a felet i positionering:

p∆= kˆp − pk (6.4)

Orienteringsfel

Eftersom orienteringen beskrivs med enhetsquaternioner kan vinkeln mellan q och ˆ

q anv¨andas som ett m˚att p˚a felet i orientering. Med tidigare definition f˚ar man ˆ

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

och orienteringsfelet (radianer) kan best¨ammas som

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

6.3.1

N˚

agra exempel

Figurerna 6.1 och 6.2 visar hur felet i position respektive orientering ser ut i ett antal punkter l¨angs banorna i exemplen 5.1 (tv˚a linjer) och 5.4 (cirkelb˚age och helix).

Felet kan beskrivas av en kontinuerlig funktion, med nollst¨allen i splinefunktioner-nas brytpunkter. Att felet ¨ar kontinuerligt framg˚ar inte riktigt i figurerna eftersom det samplats i relativt f˚a punkter.

Felen i position ¨ar som st¨orst i zonen i figur 6.1(a), men i figur 6.2(a) ¨ar det mindre skillnad (f¨orutom vid enstaka punkter).

h ← h0 {initial stegl¨angd}

Lc← banans l¨angd

lc← 0 {nollst¨all banindex}

f el ← False

Skapa en tom splinefunktion ˆq. while lc < Lc do

if lc+ (N − 1)h > Lc then

h ← Lc−lc

N−1 {justera h om banans slut ¨ar n¨ara}

end if

for i = 0 to N − 1 do

Ber¨akna position och orientering i punkterna med banindex lc+ ih.

Best¨am ledvariablernas, qi, v¨arden i dessa punkter med hj¨alp av

inverski-nematik. end for Best¨am d dlcq0 och d dlcqN−1 Skapa splinefunktionen ˆq_tmp gf el ← fel f el ← False for i = 1 to N − 1 do

Utv¨ardera ˆq_tmp i punkten med banindex lc+ (i −12)h.

Ber¨akna, med direktkinematik, position och orientering i denna punkt. p∆← ber¨aknat fel i position

θ∆← ber¨aknat fel i orientering

if (p∆> p∆max) or (θ∆> θ∆max) then

f el ← True end if end for if f el then h ← 1 2h {halvera stegl¨angden} else lc← lc+ (N − 1)h if gf el then h ← h {beh˚all h} else h ← 2h {dubblera stegl¨angden} end if

G¨or en sammanslagning av splinefunktionerna ˆq och ˆq_tmp. end if

6.3 Felutv¨ardering 47 0 0.2 0.4 0.6 0.8 1 1.2 1.4 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1x 10 −7 Felet i position l_c [m] [m] (a) 0 0.2 0.4 0.6 0.8 1 1.2 1.4 0 1 2 3 4 5 6 7 8 9x 10 −8 Felet i orientering l_c [m] [rad] (b)

Figur 6.1.Felet i (a) position och (b) orientering f¨or banan i exempel 5.1 med α5. Zonen

¨

ar markerad med lodr¨ata punktade linjer.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.5 1 1.5 2 2.5 3 3.5x 10 −8 Felet i position l c [m] [m] (a) 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 1 2 3 4 5 6 7 8 9x 10 −8 Felet i orientering l c [m] [rad] (b)

Figur 6.2.Felet i (a) position och (b) orientering f¨or banan i exempel 5.4 med α5. Zonen

¨

ar markerad med lodr¨ata punktade linjer.

Figur 6.1(b) visar p˚a st¨orre fel strax f¨ore och efter zonen samt i zonens mitt ¨an ´ı ¨ovriga punkter. I figur 6.2(b) ¨ar felen som st¨orst mitt p˚a det f¨orsta segmentet (cirkelb˚agen) och i slutet av det andra (helixen), men annars p˚a en ganska j¨amn niv˚a.

Figur 6.3 sammanfattar varje exempel f¨or fallet att p∆max = θ∆max. Om en av

parametrarna fixeras och den andra till˚ats anta v¨arden st¨orre ¨an eller lika med den f¨orsta kommer N att vara st¨orst n¨ar parametrarna ¨ar lika. Allm¨ant kan s¨agas att det i exemplen beh¨ovs fler splines f¨or att f˚a ett litet orienteringsfel ¨an ett litet positioneringsfel, vilket troligen beror p˚a att de ber¨aknas p˚a olika vis. Tabell 6.1 visar hur N varierar n¨ar banan i exempel 5.1 (ber¨aknad med α5) transformeras

med olika kombinationer av felgr¨anser.

D˚a α3anv¨ands blir N alltid st¨orre ¨an f¨or α5och α7. Skillnaden mellan α5och α7¨ar

marginell och ingen ger ett l¨agre v¨arde p˚a N ¨an den andra. Om exemplen j¨amf¨ors kan man se att ju ”sn˚arigare” bana desto fler splines beh¨ovs f¨or att h˚alla felet p˚a r¨att niv˚a. θ∆max 10−4 ₁₀−5 ₁₀−6 ₁₀−7 p∆max 10−4 _{22 28 58 82} 10−5 _{28 28 58 82} 10−6 _{58 58 58 82} 10−7 85 85 85 85

Tabell 6.1.Antalet splines, N , f¨or banan i exempel 5.1 (ber¨aknad med α5) vid olika

6.3 Felutv¨ardering 49 1 2 3 4 0 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 4 0 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 beh¨ovs f¨or att approximera banorna i de givna exemplen f¨or olika v¨arden p˚a de maximala felgr¨anserna (med p∆max= θ∆max). Bilderna best˚ar av

fyra grupper med tre staplar: Ljusgr˚a stapel – banan skapad med α3. M¨orkgr˚a stapel

– banan skapad med α5. Svart stapel – banan skapad med α7. Bilderna (a)–(d) visar

data f¨or respektive exempel 5.1–5.4. Anv¨anda v¨arden p˚a felgr¨anserna ¨ar: Grupp 1 : 10−4_,