Bangenerering för industrirobot med 6 frihetsgrader

Transkript

1 Bangenerering för industrirobot med 6 frihetsgrader Examensarbete utfört i Reglerteknik vid Tekniska Högskolan i Linköping av Daniel Forsman Reg nr: LiTH-ISY-EX Linköping 24

2

3 Bangenerering för industrirobot med 6 frihetsgrader Examensarbete utfört i Reglerteknik vid Tekniska Högskolan i Linköping av Daniel Forsman Reg nr: LiTH-ISY-EX Handledare: Erik Wernholt Examinator: Mikael Norrlöf Linköping 14 maj 24.

4

5 Avdelning, Institution Division, Department Institutionen för systemteknik LINKÖPING Datum Date Språk Language X Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling X Examensarbete C-uppsats D-uppsats Övrig rapport ISBN ISRN LITH-ISY-EX Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version Titel Title Författare Author Bangenerering för industrirobot med 6 frihetsgrader Path generation in 6DOF for industrial robots 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

6

7 Sammanfattning Rapporten handlar om bangenerering för industrirobotar med sex frihetsgrader position och orientering. En bana byggs upp av banprimitiver som linjer och cirkelbågar, vilka fogas samman i s k zoner där en viss avvikelse från banan tillåts för att göra denna mjukare. Zonen gör det möjligt att följa banan med konstant hastighet, beroende på hur banan ser ut i zonen kan dock den acceleration som krävs variera. Först presenteras några sätt att bestämma zonbanan så att accelerationen blir kontinuerlig. Sedan beskrivs hur den kartesiska banan kan transformeras till motorvinkelkoordinater där banan beskrivs m h a kubiska splinefunktioner. Den senare banan kommer att ha kontinuerliga förstaderivator, men mellan splinefunktionerna kommer andraderivatorna att vara diskontinuerliga. Några exempel ges där zonbanan beräknats på olika sätt varefter de resulterande splinefunktionernas första- och andraderivator jämförs. Dessa exempel används även till att undersöka hur många splinefunktioner som behövs för att man vid transformering tillbaka till kartesiska koordinater ska få en avvikelse från tänkt bana understigande ett givet maximalt fel. Nyckelord: bangenerering, industrirobot, sex frihetsgrader, kubisk splinefunktion 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

8 ii

9 Förord Jag vill tacka min examinator Mikael Norrlöf, LiTH, för att ha givit mig en intressant uppgift och bra vägledning under arbetets genomförande. Jag vill även tacka min handledare Erik Wernholt, LiTH, för all hjälp vid rapportens slutförande. iii

10 iv

11 Notation Symboler O i, O i -xyz Ortonormerat höger koordinatsystem. q Enhetsquaternion. q i Ledvariabel i. q Exakt funktion för ledvariablerna (från inverskinematik). ˆq Splinefunktion för ledvariablerna. r, R Vektorer och matriser skrivs med fetstil. r j i Vektorn från r j till r i. R j i Rotationsmatris för O i relativt O j. T, T j i Homogen transformationsmatris. Operatorer och funktioner Slerp (Spherical linear interpolation) Funktion som utför en sfärisk linjär interpolering mellan två enhetsquartenioner. arctan 2 (y,x) Funktion som för tanθ = y x returnerar ett korrekt värde på θ. s ( ) sin( ) cos( ) c ( ) Förkortningar DH IK SCARA TCP Denavit-Hartenberg Inverskinematik Selective Compliance Assembly Robot Arm Tool Center Point v

12 vi

13 Innehåll 1 Inledning Bakgrund Syfte och upplägg Orienteringsrepresentation Homogena transformationsmatriser Representation av orientering Rotationsmatriser Eulervinklar Vinkel/Vektor Enhetsquaternioner Robotspecifik teori Direktkinematik Inverskinematik Exempel - Roboten IRB Direktkinematik Inverskinematik Kartesisk banrepresentation Banans delar Banprimitiver Position Orientering Zonbanan vii

14 viii Innehåll Skärning mellan segment och zon Interpolering i zonen Interpolering längs banan Exempel på kartesisk zoninterpolering Linje-Linje Linje-Cirkelbåge Linje-Helix Cirkelbåge-Helix Linje-Linje, linjära l-kurvor Kommentar Interpolering i vinkelkoordinater Splines Algoritm för bangenerering Felutvärdering Några exempel Antal splines för ett visst fel Exempel på zoninterpolering, vinkelkoordinater Diskontinuiteter Derivatornas storlek Sammanfattning Sammanfattning och fortsatt arbete Slutsatser Fortsatt arbete A Denavit-Hartenbergkonventionen 63

15 Kapitel 1 Inledning 1.1 Bakgrund Normalt har en industrirobot ett verktyg som man vill ska följa en given bana. Förutom att bestämma dess position längs banan vill man även styra över dess orientering i rummet. Ett exempel är om verktyget måste hållas vinkelrätt mot en krökt yta samtidigt som positionen förändras. I kartesiska koordinater konstrueras banan genom att koppla samman enkla segment som linjer och cirkelbågar. Kring de sammanbindande punkterna definieras ett område, en zon, där en mjukare övergång mellan segmenten tillåts. Zonen gör det möjligt att följa banan med konstant hastighet; i annat fall måste roboten stannas innan den ändrar riktning från ett segment till nästa. Även om hastigheten hålls konstant kan, beroende på hur övergången mellan segmenten beräknas, accelerationen bli diskontinuerlig vid zongränserna. För reglering av roboten måste den kartesiska banan transformeras till motorvinkelkoordinater. När den transformerade banan används för styrning av roboten kommer man, i både position och orientering, att få en avvikelse från den önskade kartesiska banan. Denna avvikelse måste tas i beaktande vid transformationen. Detta examensarbete kan ses som en fortsättning på ett tidigare arbete av Maria Nyström [17]. Där undersöks hur den resulterande banan påverkas när kubiska splinefunktioner används för att beskriva banan i motorvinkelkoordinater. Olika metoder för att skapa mjukare banor tas också fram. En skillnad mellan arbetena är att man i [17] inte studerar robotens orientering. 1

16 2 Inledning 1.2 Syfte och upplägg Syftet med examensarbetet är att i sex frihetsgrader transformera en kartesisk bana till kubiska splinefunktioner som beskriver robotens motorvinkelkoordinater. Det inbegriper även hur position och orientering kan bestämmas längs banan, i synnerhet i zonerna, så att denna kan genomlöpas på ett mjukt sätt utan diskontinuiteter i hastighet eller omorientering. Rapporten är indelad som följer: Kapitel 2 ger en beskrivning av orientering och olika sätt att representera denna på (t ex med enhetsquaternioner eller rotationsmatriser). I kapitel 3 tas mer robotspecifik teori upp. En modell av ABB-roboten IRB 14 tas fram för direkt- och inverskinematik. Kapitel 4 beskriver en banas uppbyggnad och hur position och orientering beräknas efter banan och speciellt i zonerna. I kapitel 5 ges några exempel på olika banor och olika sätt att beräkna position och orientering i zonen. Kapitel 6 beskriver en algoritm för transformering av den kartesiska banan till motorvinkelkoordinater, vilka representeras av kubiska splinefunktioner. I kapitel 7 har banorna i exemplen från kapitel 5 omvandlats till kubiska splinefunktioner enligt kapitel 6 varefter splinefunktionerna för varje exempel jämförs. Här görs även en felutvärdering av splinefunktionerna. De i rapporten beskrivna metoderna har implementerats i Matlab [12], och använts vid beräkning av de givna exemplen. Robotics Toolbox [2] har använts i vissa delar, och för splineberäkningar har de i [15] beskrivna funktionerna använts.

17 Kapitel 2 Orienteringsrepresentation Det här kapitlet beskriver hur man kan representera orientering och position på olika sätt. Avsnitt 2.1 visar hur man samtidigt kan representera denna information i en så kallad homogen transformationsmatris, vilken underlättar notation och beräkningar. Till stor del bygger innehållet på [19], men med delvis förändrad notation. I [7] beskrivs hur rotationer kan göras med hjälp av quaternioner. Det görs bl a även i [4] och [3], där den sistnämnda väldigt utförligt behandlar quaternioner och interpolering med sådana. En sammanställning av de artiklar som Sir William Rowan Hamilton, upphovsmannen till begreppet quaternioner, publicerade i Philosophical Magazine år finns sammanställda i [8]. 2.1 Homogena transformationsmatriser En punkt p i ett ON-system O-xyz beskrivs med vektorn p = p xˆx + p y ŷ + p z ẑ = där ˆx, ŷ och ẑ är enhetsvektorerna för koordinatsystemet. Om vi har två ONsystem i samma origo kan man genom att projicera enhetsvektorerna på lämpligt sätt p x p y p z ˆx j =(ˆx j ˆx i )ˆx i +(ˆx j ŷ i )ŷ i +(ˆx j ẑ i )ẑ i ŷ j =(ŷ j ˆx i )ˆx i +(ŷ j ŷ i )ŷ i +(ŷ j ẑ i )ẑ i ẑ j =(ẑ j ˆx i )ˆx i +(ẑ j ŷ i )ŷ i +(ẑ j ẑ i )ẑ i 3

18 4 Orienteringsrepresentation uttrycka det ena systemets koordinater i det andras. Rotationen hos O j relativt O i uttrycks med rotationsmatrisen ˆx j ˆx i ˆx j ŷ i ˆx j ẑ i R i j = ŷ j ˆx i ŷ j ŷ i ŷ j ẑ i ẑ j ˆx i ẑ j ŷ i ẑ j ẑ i vilken har egenskapen (ortogonalitet) (R i j) 1 = (R i j) T = R j i I det allmänna fallet behöver inte origo sammanfalla för två koordinatsystem. Om O j :s läge relativt O i anges av vektorn o i j fås för punkten p och i respektive koordinatsystem. p i = o i j + R i jp j p j = R j i pi R j i oi j Om man vill göra flera transformationer i följd görs detta enklast genom att införa den homogena transformationsmatrisen ( ) T i R i j = j o i j T 1 samt skriva p som Detta innebär att p = ( p 1 ) p i = T i j pj ger samma p i som tidigare, fast på ett mer direkt sätt. Noteras bör dock att matrisen T i j inte är ortogonal vilket betyder att (T i j) 1 (T i j) T Inversen av T i j är däremot (T i j) 1 = ( (R i j ) T (R i j) T o i j T 1 ) 2.2 Representation av orientering Om positionen ges av en punkt i rummet kan orienteringen ses som en vektor utgående från den punkten och kring vilken en rotation kan ske. Flera sätt att representera orientering existerar. Det är inte säkert att man alltid har en unik representation, vilket skapar problem när man vill byta från en representation till en annan.

19 2.2 Representation av orientering Rotationsmatriser En rotationsmatris är den grundläggande representationen för orientering. Den är en ortogonal matris som används för att rotera vektorer och koordinatsystem (se avsnitt 2.1). Den består (i tre dimensioner) av nio element vilket är mer än antalet frihetsgrader som krävs för att representera orientering - en icke minimal representation. Rotationsmatriserna för rotation kring de tre koordinataxlarna z-y-x ges av cos α sin α R z (α) = sinα cos α (2.1) 1 cos β sinβ R y (β) = 1 (2.2) sin β cos β R x (γ) = 1 cos γ sin γ (2.3) sinγ cos γ vilka kan kombineras för att generera generella rotationer. Varje rotation sker relativt ett givet koordinatsystem. Vill man göra en sammansättning av rotationer är det därför viktigt att dessa utförs i rätt ordning. En rotation av ett koordinatsystem O i relativt koordinatsystemet O j uttrycks som R j i. Det finns två sätt att göra detta: Om efterföljande rotation sker relativt det koordinatsystem som erhölls vid föregående rotation multipliceras de enligt R 2 = R 1R 1 2, där R 1 är den första rotationen. Om båda rotationerna sker relativt samma koordinatsystem multipliceras de enligt R 2 = R 1 2R Eulervinklar Eulervinklarna beskriver orienteringen med tre oberoende parametrar - en minimal representation: Φ = [ϕ ϑ ψ] T Den rotation som Φ beskriver erhålls genom att skapa en rotationsmatris sammansatt av tre på varandra följande rotationer (ϕ, ϑ, och ψ) kring ett lämpligt val av rotationsaxlar. När man roterar ett koordinatsystem O kring en av dess axlar får man ett nytt koordinatsystem O. Rotationsaxeln är densamma i de båda systemen. Om man

20 6 Orienteringsrepresentation z z O O y x y x Figur 2.1. Rotation av O kring z ger O med z identisk med z. därför ser till att inte rotera kring samma axel två gånger i rad, kommer man att få den önskade rotationsmatrisen. Se figur 2.1. När man väl har bestämt vilken följd av rotationsaxlar man vill använda måste man hålla 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äriska handleden på ett bra sätt är ZYZvinklar. Rotationerna sker först 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(φ) = R z (ϕ)r y (ϑ)r z (ψ) (2.4) vilket efter sammansättning blir c ϕ c ϑ c ψ s ϕ s ψ c ϕ c ϑ s ψ s ϕ c ψ c ϕ s ϑ R(φ) = s ϕ c ϑ c ψ + c ϕ s ψ s ϕ c ϑ s ψ + c ϕ c ψ s ϕ s ϑ (2.5) s ϑ c ψ s ϑ s ψ c ϑ Om man har en rotationsmatris r 11 r 12 r 13 R = r 21 r 22 r 23 (2.6) r 31 r 32 r 33 kan man genom jämförelse med matrisen (2.5) se att

21 2.2 Representation av orientering 7 tan ϕ = sin ϕ cos ϕ = sϕs ϑ c ϕs ϑ = r23 r 13 tan ψ = sin ψ cos ψ = s ϑs ψ s ϑ c ψ = r32 r 31 tan 2 ϑ = sin2 ϑ cos 2 ϑ = s2 ϑ c 2 = (c2 ϕ +s2 ϕ )s2 ϑ = r2 ϑ c r2 23 ϑ r33 2 vilket kan användas för att bestämma de till (2.6) svarande eulervinklarna. Det görs enklast genom att använda en funktion arctan 2 (y,x) som för uttrycket tanα = ger ett korrekt värde på vinkeln α. sin α cos α = y x Då tan 2 ϑ = r2 13 +r2 23 r 2 33 blir tanϑ = ± r r 2 23 r 33 och vinkeln ϑ bestäms till ϑ = arctan 2 (± r r2 23,r 33) där man för ett positivt respektive negativt tecken får en lösning antingen i intervallet (,π) eller ( π,). Om både r 13 = och r 23 = (ϑ = nπ) kommer R y (ϑ) att vara enhetsmatrisen, och således roterar vi kring z-axeln två gånger i följd. I detta fall kan inte ϕ och ψ entydigt bestämmas. Lösningarna blir nu för ϑ (,π) och för ϑ ( π,) ϕ = arctan 2 (r 23,r 13 ) ϑ = arctan 2 ( r r2 23,r 33) (2.7) ψ = arctan 2 (r 32, r 31 ) ϕ = arctan 2 ( r 23, r 13 ) ϑ = arctan 2 ( r r2 23,r 33) (2.8) ψ = arctan 2 ( r 32,r 31 ) Vinkel/Vektor Orientering kan även beskrivas som en rotation ϑ kring en (enhets-) vektor r (se figur 2.2). Givet ett referenskoordinatsystem kan den rotationsmatris som motsvarar orienteringen tas fram. Det görs genom att först vrida in vektorn r till en av referenskoordinatsystemets axlar, rotera kring denna enligt ϑ och sedan vrida tillbaka r till det ursprungliga läget. Gör man detta kring axlarna z och y får man följande uttryck för rotationsmatrisen: R(ϑ,r) = R z (α)r y (β)r z (ϑ)r y ( β)r z ( α) där α och β är de vinklar man behöver vrida r med för att nå z-axeln.

22 8 Orienteringsrepresentation z r z β r r y ϑ y α r x x Figur 2.2. Rotation kring en axel Enhetsquaternioner En enhetsquaternion beskriver även den en rotation ϑ kring en axel r (enhetsvektor). Man skriver den som q =<η, ǫ> där η = cos ϑ 2 ǫ = sin ϑ 2 r och q 2 = η 2 + ǫ T ǫ = cos 2 ϑ 2 + sin2 ϑ 2 rt r = 1 eftersom r är en enhetsvektor. Multiplikation av två quaternioner q 1 =<η 1, ǫ 1 > och q 2 =<η 2, ǫ 2 > görs enligt q 1 q 2 =<η 1 η 2 ǫ T 1 ǫ 2, η 1 ǫ 2 + η 2 ǫ 1 + ǫ 1 ǫ 2 > där är kryssprodukten. Detta motsvarar multiplikationen R 1 R 2 av två rotationsmatriser. Inversen av en quaternion är q 1 =<η, ǫ> vilket ger q q 1 =<1, > motsvarande RR 1 =I, där I är enhetsmatrisen.

23 2.2 Representation av orientering 9 r z v v rot x y Figur 2.3. Rotation av en vektor kring en annan. Den mot en enhetsquaternion svarande rotationsmatrisen har utseendet 2(η 2 + ǫ 2 x) 1 2(ǫ x ǫ y ηǫ z ) 2(ǫ x ǫ z + ηǫ y ) R(η,ǫ) = 2(ǫ x ǫ y + ηǫ z ) 2(η 2 + ǫ 2 y) 1 2(ǫ y ǫ z ηǫ x ) (2.9) 2(ǫ x ǫ z ηǫ y ) 2(ǫ y ǫ z + ηǫ x ) 2(η 2 + ǫ 2 z) 1 och det inversa problemet (med R enligt 2.6) har lösningen η = 1 2 r11 + r 22 + r ǫ = 1 sgn(r 32 r 23 ) r 11 r 22 r sgn(r 2 13 r 31 ) r 22 r 33 r sgn(r 21 r 12 ) (2.1) r 33 r 11 r där η motsvarar en vinkel ϑ [ π,π]. Funktionen sgn(x) är definierad som { +1, x sgn(x) = 1, x < Om man vill rotera en vektor v med vinkeln θ kring en annan vektor r med quaternioner kan man göra på följande sätt (se figur 2.3): Skapa quaternionerna q v =<, v v > och q r =< cos θ 2, r r sin θ 2 > Utför multiplikationerna q =< η,ǫ >= q r q v q 1 r Låt v rot = v ǫ beteckna den resulterande vektorn. I rapporten kommer enhetsquaternioner att användas för att representera orientering och beräkna rotationer. Detta på grund av den enkelhet varmed detta låter sig göras.

24 1 Orienteringsrepresentation

25 Kapitel 3 Robotspecifik teori Industrirobotar finns i många utföranden, som alla har gemensamt att de består av ett antal länkar och sammanbindande leder. Lederna kan vara av två typer - prismatiska och roterande, där de prismatiska lederna utför en translaterande rörelse. Om man kopplar länkarna i en följd, från första till sista länken, kallas det för en öppen kinematisk kedja. Om däremot några länkar bildar en slinga, kallas det för en sluten kinematisk kedja. För att kunna positionera ett objekt med en robotarm krävs tre frihetsgrader. Vill man sedan låta objektet ha en viss orientering relativt ett referenskoordinatsystem krävs ytterligare tre. Totalt behövs alltså sex frihetsgrader (och därmed sex leder) för en godtycklig positionering och orientering av ett objekt. Om man låter antalet leder överstiga antalet frihetsgrader har man en kinematiskt redundant robot. En robot delas vanligen in i en arm och en handled, där armen sköter positionering och handleden orientering. Beroende på vilka typer av leder man väljer, och hur de placeras i strukturen, får man olika typer av robotar (med olika egenskaper): antropomorfisk, cylindrisk, kartesisk, sfärisk och SCARA-robot. Den antropomorfiska armen består av tre roterande leder enligt figur 3.1 och har i princip ett sfäriskt arbetsområde. Handleden kan även den se ut på olika sätt, men den mest generella är den sfäriska handleden. Denna består av tre roterande leder vars rotationsaxlar skär varandra i en punkt, och därmed kan godtycklig orientering uppnås. På handledens yttersta ände brukar ett verktyg eller gripdon av något slag monteras. Denna punkt kallas för TCP (Tool Center Point) eller arbetspunkt. Ett verktygskoordinatsystem kan kopplas till TCP så att man kan styra position och orientering för det verktyg som används (annars styr man TCP). Robotens länkar och leder kan beskrivas med lokala koordinatsystem och relationer mellan dessa. För varje led (prismatisk eller roterande) ansätts en ledvariabel (q i ) som beskriver ledens tillstånd (translaterad sträcka eller roterad vinkel). Värdet av 11

26 12 Robotspecifik teori Figur 3.1. Tre roterande leder placerade så att en antropomorfisk arm fås. ledvariablerna bestämmer position och orientering för roboten. Med direktkinematik och inverskinematik går man från en beskrivning till den andra. Ett exempel på en antropomorfisk robot med sfärisk handled är IRB 14 från ABB. För denna tas en kinematisk modell fram (och implementeras i Matlab) som används vid beräkningar i efterföljande kapitel. Om modellering av industrirobotar finns i att läsa i bl a [14] och [19]. 3.1 Direktkinematik Med direktkinematik bestämmer man position och orientering som en funktion av ledvariablerna. Till skillnad från det inverskinematiska problemet får man alltid en unik lösning. För en robot av strukturen öppen kinematisk kedja beräknar man för varje länk transformationsmatrisen relativt föregående länk i kedjan. När man sedan multiplicerar ihop dessa får man den slutliga transformationsmatrisen för position och orientering relativt den första länken. (Om roboten är av strukturen sluten kinematisk kedja får man göra på ett lite annorlunda sätt, vilket inte kommer att behandlas här.) Dessa beräkningar kan systematiseras genom att följa Denavit-Hartenbergkonventionen (appendix A) som för varje länk skapar ett koordinatsystem vilket beskrivs

27 3.2 Inverskinematik 13 av ett antal parametrar. Parametrarna används för att ta fram den transformationsmatris som beskriver hur en länk är kopplad till nästa. Ett exempel på beräkningar för roboten IRB 14 finns i avsnitt Inverskinematik Med inverskinematik (IK) söker man värden på ledvariablerna vilka gör att roboten får en given position och orientering. Det är inte alltid möjligt att lösa detta problem - lösning kanske saknas eller också existerar flera lösningar (möjligen ett oändligt antal). Det är heller inte säkert att man kan beräkna ett analytiskt uttryck för ledvariablerna. I vissa fall kan beräkningarna delas upp i ett positionerings- och ett orienteringsproblem. Det gäller speciellt för robotar av samma struktur som IRB 14 (antropomorfisk robot med sfärisk handled). Det betyder att ledvariablerna för handledens position (de tre första) och de för handledens orientering (de tre sista) löses separat. I [19] finns en lösningsgång (Algoritm 1) för en antropomorfisk robot med sfärisk handled beskriven. Denna kommer att illustreras med exemplet i nästa avsnitt. Algoritm 1 Inverskinematik för en antropomorfisk robot med sfärisk handled. 1: Beräkna handledens position p w (q 1,q 2,q 3 ) som p w = p d 6 a där p är TCP:s position, d 6 en DH-parameter (appendix A) och a den enhetsvektor som sammanfaller med rotationsaxeln för den sista leden (riktad utåt). Se figur : Lös IK-problemet för att finna (q 1,q 2,q 3 ). 3: Beräkna R 3(q 1,q 2,q 3 ) som ger handledens orientering relativt den första leden. 4: Beräkna R 3 6(q 4,q 5,q 6 ) = (R 3) T R, där R är den rotationsmatris som beskriver orienteringen i TCP. 5: Lös IK-problemet för att finna (q 4,q 5,q 6 ). 3.3 Exempel - Roboten IRB 14 Som ett exempel på kinematiska beräkningar kommer en modell av roboten IRB 14 att tas fram, vilken sedan kommer att implementeras i MATLAB med hjälp av Robotics Toolbox [2]. Då denna bara kan hantera robotar av strukturen öppen kinematisk kedja måste modellen anpassas därefter. (I [16] finns beskrivet en MATLABtoolbox för bangenerering i tre frihetsgrader.)

28 14 Robotspecifik teori p w p TCP a n s Figur 3.2. Antropomorfisk arm med sfärisk handled Direktkinematik För att kunna använda modellen i Robotics Toolbox får man anse att roboten är formad som en öppen kinematisk kedja, med sex roterande leder och sju länkar. Figur 3.3 visar ledernas placering (lederna ligger i samma plan) och rotationsriktningar. Modellen kan nu tas fram enligt Denavit-Hartenbergs (DH) metod (se appendix A). #3 #4 #6 #5 #2 #1 Figur 3.3. Axlar och sidovy med måttsättning (mm) för IRB 14. Det första steget är att bestämma riktningen för robotens axlar, samt att numrera dessa med början i den första leden (se figur 3.4). Detta har gjorts på så sätt att

29 3.3 Exempel - Roboten IRB parallella leder har lika riktning. Numreringen börjar med axel z i led nummer ett, och fortsätter till axel z 6 som tillhör koordinatsystemet för TCP. Rotationsaxlarnas riktning Koordinatsystemens placering x x 6 54 z 5 z 6 z z z x 3 y 4 zy y z x 2 y 3 z 3 1 z 2 1 y 2 z 2 z.8.6 z.8 y 1.6 x 1.4 z 1.4 z 1.2 z.2 z y x.2 y.2.2 x y.2.2 x.5 1 Figur 3.4. Rotationsaxlarnas riktning och numrering (vänster) samt länkarnas koordinatsystem (höger). Länkarnas koordinatsystem har fått placeringar enligt figur 3.4. Tillsammans med måtten från figur 3.3 kan modellens DH-parametrar beräknas (tabell 3.1). DHparametrarna används slutligen till att bestämma de homogena transformationsmatriser som beskriver robotens direktkinematik. Länk (i) a i α i d i θ i π π π 2 4 π π Tabell 3.1. DH-parametrarna (mått i mm och radianer) Modellen kan implementeras i MATLAB med Robotics Toolbox enligt figur 3.5. Funktionen link skapar ett länkobjekt utifrån DH-parametrarna för länken. Funktionen robot skapar sedan ett robotobjekt av angivna länkar.

30 16 Robotspecifik teori % % MATLAB-kod för roboten IRB 14 i Robotics Toolbox % L1 = link([ pi/ ], standard ); L2 = link([.6 pi/2 ], standard ); L3 = link([ pi/2.12 ], standard ); L4 = link([-pi/2.72 ], standard ); L5 = link([ pi/2 ], standard ); L6 = link([.85 ], standard ); L2.offset = +pi/2; r = robot({l1,l2,l3,l4,l5,l6}, IRB 14 ); Figur 3.5. MATLAB-modell av roboten IRB Inverskinematik IK för position Om man ser roboten från sidan (som i figur 3.3) och antar att inga begränsningar finns gällande värden på ledvinklarna q i, kan man få upp till fyra olika lösningar för positionen (figur 3.6) beroende på vilka intervall q 1 och q 2 tillhör. Singulära punkter existerar där en entydig lösning inte kan bestämmas, så även punkter där lösning saknas. Av alla vinklar är q 1 den enklaste att lösa. Genom att först projicera vektorn p w i xy-planet (figur 3.7) och sedan använda (MATLAB-) funktionen ATAN2(y,x) fås ett värde q 1 i intervallet [ π,+π]. (I den singulära punkten (x,y) = (,) kan detta inte entydigt bestämmas.) Nu finns det två möjliga val av q 1 { q 1 shoulder right q 1 = (3.1) q 1 + π shoulder left Om man låter lederna 2, 3 och 5 utgöra hörnen på en triangel i koordinatsystemet för led 2 (O 2 ) kan vinklarna q 2 och q 3 beräknas genom betraktande av figur 3.8. Eftersom cos(π q 3 ) = cos q 3 ger cosinussatsen (b 3 = a d2 4 ): q 3 = { +arccos( c2 a 2 2 b2 3 2a 2b 3 ) elbow up arccos( c2 a 2 2 b2 3 2a 2b 3 ) elbow down Summan av q 2 och den mellan sträckorna a 2 och c bildade vinkeln är identisk med vinkeln mellan y-axeln och linjen från origo till punkten (p x,p y ). Tangens för dessa b vinklar är 3 sin q 3 a 2+b 3 cos q 3 respektive px p y vilket för valt q 3 ger

31 3.3 Exempel - Roboten IRB (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öjliga lösningarna shoulder left/right, elbow up/down. q 2 = arctan 2 (b 3 sin q 3,a 2 + b 3 cos q 3 ) arctan 2 (p x,p y ). När vi nu har valt elbow up eller elbow down kan slutligen q 2 och q 3 bestämmas (δ = arctan d4 a 3 ): q 2 = { π 2 + q 2 shoulder right π 2 q 2 shoulder left (3.2) q 3 = { δ q 3 δ + q 3 shoulder right shoulder left (3.3)

32 18 Robotspecifik teori p w Z Y X Figur 3.7. Den streckade linjen visar hur p w projicerats i xy-planet. 1 Geometriska samband.9 y [m] q 2 a 3 d 4 (p x, p y) q 3 b a 2 c x [m] Figur 3.8. Geometriska samband för IRB 14 då enbart axel 2 och 3 betraktas. IK för orientering Efter beräkningen R 3 6(q 4,q 5,q 6 ) = (R 3) T R får man en matris på formen R 3 6 = n 3 x s 3 x a 3 x n 3 y s 3 y a 3 y n 3 z s 3 z a 3 z

33 3.3 Exempel - Roboten IRB ur vilken q 4, q 5 och q 6 kan bestämmas med hjälp av den homogena transformationsmatrisen ( ) T 3 R 3 6 = 6 p 3 6 T 1 där R 3 6 = c 4 c 5 c 6 s 4 s 6 c 4 c 5 s 6 s 4 c 6 c 4 s 5 s 4 c 5 c 6 + c 4 s 6 s 4 c 5 s 6 + c 4 c 6 s 4 s 5 s 5 c 6 s 5 s 6 c 5 Genom att identifiera lämpliga element i de båda matriserna R 3 6 får man lösningar med q 5 i något av de två intervallen [ π,[ och ],π]. Om handleden skulle vara rak, (a 3 x) 2 + (a 3 y) 2 =, får man q 5 = samt oändligt många lösningar på q 4 och q 6. I annat fall blir lösningarna q 4 = arctan 2 (a 3 y,a 3 x) q 5 ],π] : q 5 = arctan 2 ( (a 3 x) 2 + (a 3 y) 2,a 3 z) q 6 = arctan 2 (s 3 z, n 3 z) q 4 = arctan 2 ( a 3 y, a 3 x) q 5 [ π,[: q 5 = arctan 2 ( (a 3 x) 2 + (a 3 y) 2,a 3 z) q 6 = arctan 2 ( s 3 z,n 3 z) En MATLAB-funktion (se figur 3.9) som utför beskrivna beräkningar har tagits fram. För ett angivet robotobjekt, önskad position och orientering (i en homogen transformationsmatris) samt önskemål om typ av lösning (shoulder left/right, elbow...) returnerar den värden på ledvariablerna q 1 till q 6. Valet av wrist beror på vilket intervall man vill att q 5 ska tillhöra (positiva eller negativa värden). Om en punkt ligger utanför robotens arbetsområde kommer ett felmeddelande att visas. Felet indikeras med err och q blir en en tom vektor. När en singularitet påträffas returneras en lösning q som inte är entydigt bestämd. Inget fel indikeras (err=), men en varningstext visas. Värt att notera är att denna funktion gäller för alla robotar av samma struktur som IRB 14, förutsatt att DH-parametrarna beräknats utgående från koordinatsystemen i figur 3.4.

34 2 Robotspecifik teori function [qvec,err]=ik(r,tr,shoulder,elbow,wrist) %IK - Inverskinematik för robot av samma struktur som IRB 14. % % [q, err] = IK(r, TR, shoulder, elbow, wrist) % % r - robotobjekt (Robotics Toolbox) % % TR - homogen transformationsmatris som beskriver % position och orientering för 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, == om inget fel, ~= annars Figur 3.9. Beskrivning av funktion för beräkning av inverskinematik för robotar av samma struktur som IRB 14

35 Kapitel 4 Kartesisk banrepresentation Detta kapitel kommer att ge en beskrivning av hur en bana byggs upp och hur position och orientering för roboten bestäms längs en sådan. För att slippa diskontinuiteter i hastighet och acceleration längs banan används s k zoner där man tillåter avvikelser från banans önskade utseende. En metod för beräkning av banan i en zon presenteras i kapitel (För interpolering av orientering (med quaternioner) och i några fall även position finns mycket att läsa i t ex [1], [5], [6], [1], [11] och [13].) 4.1 Banans delar Den bana man vill att robotens verktyg skall följa byggs upp (i ett globalt koordinatsystem) av ett eller flera segment. Ett segment är en enkel typ av kurva, en så kallad banprimitiv (se figur 4.3). När segmenten kopplas samman så att slutpunkten för ett blir startpunkten för nästa fås en bana av mer godtyckligt utseende. Figur 4.1 visar en bana bestående av två linjer och två cirkelbågar. P 2 P 3 Slut P 7 P 1 Start P 4 P 6 P 5 Figur 4.1. En bana bestående av linjer och cirkelbågar. 21

36 22 Kartesisk banrepresentation I gränsen mellan två segment finns en så kallad zon inlagd. Den är en sfär med radien R z inom vilken man tillåter avvikelser från den önskade banan. Den del av banan som börjar i en zongräns, passerar zonen, och fortsätter till nästa zon kallas för en sektion. Figur 4.2 visar detta. I zonerna visar punktad linje segmenten och heldragen linje visar den i zonen beräknade banan. Zonen existerar alltid, men genom att låta R z = kommer dess inverkan på banan att försvinna. Zon Segment Sektion Zon Figur 4.2. En bana byggs upp av segment, zoner och sektioner. Man vill även att orienteringen av verktyget skall kunna bestämmas längs banan. Detta kan göras genom att till segmentens ändpunkter koppla information om orientering (i form av enhetsquaternioner), som sedan interpoleras fram för en given position längs segmentet. 4.2 Banprimitiver Position Banprimitiverna (segmenten) bestäms av en parameterbeskrivning i ett, för varje segment, lokalt koordinatsystem O. Detta system bestäms av translationsvektorn r c och rotationsmatrisen R så att r = r c + Rr transformerar den i O lokala vektorn r till vektorn r i det globala koordinatsystemet. Med parameterbeskrivningen x (s) R cos(θ s) cos(θ s) sin(θ s) R r (s) = y (s) = R sin(θ s) = sin(θ s) cos(θ s) (4.1) z (s) h s 1 h s där s [,1] bestämmer alla punkter längs kurvan (från starten i s = till slutet i s = 1) och R, θ samt h är konstanter, kan följande fyra banprimitiver beskrivas: Punkt R =, h = (för en punkt i origo) Linje R =, h (för en linje längs ẑ)

37 4.2 Banprimitiver 23 Cirkelbåge R, h =, θ Helix (cirkulär) R, h, θ För punkten och linjen finns andra val av konstanter. Att just dessa valts får sin förklaring nedan. Punkt Linje Cirkelbåge Helix z z z z y y y y x x x x Figur 4.3. Banprimitiverna punkt, linje, cirkelbåge och helix. Val av parametrar Valet av parametrar (R, θ, h ) och lokalt koordinatsystem (r c och R) görs så att h >= och R >=. Det innebär att en linje alltid utgår från origo och följer ẑ-axeln, samt att en helix växer längs samma axel. En cirkelbåge kommer alltid att börja på avståndet R från origo längs positiva ˆx-axeln och sluta någonstans i ˆxŷ-planet. Helixen börjar i samma punkt som en cirkelbåge skulle göra och vrider sig kring ẑ-axeln på en tänkt cylinder med radien R. Tecknet på θ bestämmer om man har en höger- eller vänstervriden helix. För cirkelbågen gör valet θ > att representationen blir entydig (annars kan man ha en rotationsmatris R med ẑ-riktningen omvänd mot fallet θ > vilken ger den önskade cirkelbågen då θ < - se figur 4.4). ẑ θ > θ < P2 ŷ ŷ P 2 P 1 P 1 ˆx ˆx Figur 4.4. Olika representation av samma cirkelbåge. ẑ

38 24 Kartesisk banrepresentation Om man skapar en linje genom att ange dess ändpunkter r och r 1 får man r c = r samt R = [ˆx ŷ ẑ] med ẑ = r r1 r 1 r där ˆx och ŷ väljs så att R blir en rotationsmatris enligt tidigare beskrivning. En cirkelbåge bestäms även den av en start- och en slutpunkt (r 1 och r 3 ), men dessutom av en på cirkelbågen mellanliggande punkt (r 2 ). Här blir ẑ = (r2 r1) (r3 r2) (r 2 r 1) (r 3 r 2). Längden av ett segment I beskrivningen ovan används index s [, 1] för att beskriva ett segment. Detta index säger dock inget om hur lång sträcka man har förflyttat sig längs segmentet för ett visst värde på s. Denna längd kan beräknas till l(s l ) = = s l s l dr ds ds = s l (R θ ) 2 + h 2 ds = s l ( dx ds )2 + ( dy ds )2 + ( dz ds )2 ds (R θ ) 2 + h 2 och genom att sätta s l = 1 får man den totala längden av segmentet: L c = (R θ ) 2 + h 2 (4.2) Orientering Orientering på en linje Om man i ändpunkterna beskriver önskad orientering med hjälp av enhetsquaternionerna q och q 1, kan man med sfärisk linjär interpolering bestämma orienteringen i den position som bestäms av indexet s ovan. Denna interpolering sker på den fyrdimensionella enhetssfären och de quaternioner som fås ligger på den storcirkel som sammanbinder q med q 1. Det betyder alltså att q övergår i q 1 längs den kortaste vägen på sfären, vilket ger en mjuk övergång i orientering mellan linjens ändpunkter. En funktion för sfärisk linjär interpolering finns beskriven i [2]: Slerp(q,q 1 ;s) = sin((1 s)θ) q + sin(sθ) sin θ sinθ q 1 (4.3) där q q 1 = cos θ och s [,1]. (Slerp står för Spherical linear interpolation.) Orientering på en cirkelbåge Interpolering av orientering på en cirkelbåge görs enklast om man ser cirkelbågen som en rät linje vilken böjts kring en cylinder (figur 4.5(a)). Interpoleringen sker

39 4.2 Banprimitiver 25 på linjen som därefter transformeras till en cirkelbåge. Om man i linjens ändar placerar identiska koordinatsystem kommer, när cirkelbågen skapats, systemet i linjens slutpunkt att ha roteras med en vinkel θ relativt systemet i linjens startpunkt (figur 4.5(b) och 4.5(c)). θ ŷ θ ˆx (a) (b) ˆx (c) ŷ Figur 4.5. (a) Cirkelbåge som en linje krökt runt en cylinder. (b) Cirkelbågen och linjen sedda från cylinderns z-axel. (c) Rotation av koordinatsystemet i cirkelbågens slutpunkt. Eftersom man känner cirkelbågen och orienteringen i dess ändar (q och q 1 ) kan interpoleringen nu utföras i tre steg: 1. Kompensera för krökningen genom att vrida tillbaka q 1 med vinkeln θ kring cylinderns z-axel. Kalla den modifierade q 1 för q Utför en sfärisk linjär interpolering mellan q och q 1 på samma sätt som tidigare. 3. Transformera erhållen q(s) genom rotation med θs i motsatt riktning som i 1. På det här sättet kommer q(s) att för fallet q 1 = q alltid vara orienterad på samma sätt relativt cirkelbågen i alla punkter längs kurvan. Att interpoleringen inte sker direkt mellan q och q 1 beror på att Slerp interpolerar längs den kortaste vägen på sfären. Det betyder att man för en cirkelbåge (eller helix) med θ 18 inte alltid kan få q(s) enligt föregående stycke. Figur 4.6 ger ett exempel på en sådan cirkelbåge. Orientering på en helix Helixen skiljer sig inte från cirkelbågen vad gäller interpolering av orientering. Koordinatsystemet i slutpunkten är bara translaterat längs z-axeln, och rotationen kring densamma bestäms fortfarande av θ.

40 26 Kartesisk banrepresentation Figur 4.6. En cirkelbåge med θ 18 med pilar som representerar orienteringen. Orienteringen har i vänster figur interpolerats med och i höger utan hänsyn tagen till kurvans krökning. 4.3 Zonbanan Banans utseende utanför zonerna bestäms helt av den för varje segment angivna parameterbeskrivningen. Inuti en zon måste man på något sätt beräkna en ny bana (zonbanan) så att denna och åtminstone dess förstaderivata blir kontinuerliga i zongränserna. För att kunna göra det måste man först veta var segmentet och zonen skär varandra Skärning mellan segment och zon För att kunna beräkna för vilket värde på s som ett segment passerar en viss zon måste man känna typen av segment, segmentets parameterbeskrivning, zonradien R z och läget av zonen (i början eller i slutet av segmentet). Zonen delar segmentet i två delar. Skärningen med segmentet ges då zonen befinner sig i början av segmentet av r(s) r() = R z,s > och i slutet av segmentet ges skärningen av r(s) r(1) = R z,s < 1 Ett allmänt uttryck för skärningen mellan ett segment och en sfär (zonen) med radien R z placerad med centrum på kurvan fås ur r(s) r(τ) = R z där τ beskriver sfärens placering (τ = i början av segmentet och τ = 1 i slutet).

41 4.3 Zonbanan 27 Med parameterbeskrivningen R cos(θ s) r(s) = R sin(θ s), s [,1] h s får man att r(s) r(τ) 2 =... = 2R 2 (1 cos(θ (s τ))) + h 2 (s τ) 2 = R 2 z vilket ger ekvationen h 2 (s τ) 2 2R 2 cos(θ (s τ)) + 2R 2 R 2 z = som gäller för de ovan beskrivna segmenten. För variablerna s och τ gäller s [,1] och τ [,1]. Skärning med linje För en linje blir ekvationen för skärning med zonen h 2 (s τ) 2 R 2 z = vilket förenklas till s = τ ± R z h Då endast segmentets ändpunkter är av intresse sätts τ till noll eller ett beroende på zonens läge. Eftersom s bara kan anta värden mellan noll och ett blir resultatet följande: och s = R z h (τ = ) s = 1 R z h (τ = 1) Härur kan ses att kvoten Rz h inte får vara större än ett, alltså R z h, men i praktiska tillämpningar används R z 1 2 h för att två zoner inte ska kunna överlappa varandra (figur 4.7). Skärning med cirkelbåge För cirkelbågen fås ekvationen 2R 2 cos(θ (s τ)) + 2R 2 R 2 z =

42 28 Kartesisk banrepresentation R z = 1 2 h R z = 1 4 h R z = 1 2 h R z = 1 2 h Figur 4.7. Olika zonradie för ett segment. eller omskrivet cos(θ (s τ)) = 2R2 R 2 z 2R 2 = ( ) 2 Rz R vilket ger [ s = τ ± 1 arccos 1 1 θ 2 ( Rz R ) 2 ] Zonradiens maximala storlek erhålls ur ( Rz ) R eftersom argumentet till arccos måste tillhöra det intervallet för att ett reellt värde ska erhållas. Efter omskrivning fås R z 2R För att inte två zoner ska överlappa varandra får zonradien inte överstiga halva avståndet mellan cirkelbågens start- och slutpunkter (figur 4.8). Avståndet är 2R sin( θ 2 ) vilket resulterar i uttrycket R z R sin( θ 2 ) Skärning med helix För den cirkulära helixen gäller den allmänna ekvationen h 2 (s τ) 2 2R 2 cos(θ (s τ)) + 2R 2 Rz 2 = som inte kan förenklas mycket mer än till formen (s τ) 2 2 ( R h ) 2 cos(θ (s τ)) = R2 z 2R 2 h 2

43 4.3 Zonbanan 29 2R sin 1 2 θ θ R θ R 2R sin 1 2 θ Figur 4.8. Avståndet mellan cirkelbågens start- och slutpunkter. men med τ konstant och ϕ = θ (s τ) får man α = θ R h β = θ R z h ϕ 2 2α 2 cos ϕ = β 2 2α 2 Denna ekvation kan givetvis lösas numeriskt, men genom att anta att ϕ är liten (s nära τ) kan approximationen cosϕ ϕ2 göras vilken leder till ekvationen ϕ 2 (1 + α 2 ) = β 2 eller β ϕ = ± 1 + α 2 Även här bör zonradien uppfylla R z 1 2 r(1) r(), men i de fall θ 2π bör även R z 1 2 r(2π θ ) r() uppfyllas. Det första för att zonerna i helixens ändpunkter inte ska kunna överlappa varandra, det andra för att zonbanan inte ska kollidera med helixen utanför zonen, vilket annars skulle kunna ske. (Att ingen figur ges beror på svårigheter med att illustrera på ett bra sätt.) Interpolering i zonen Inuti en zon önskas uttryck för position, r(s), och orientering, q(s), för s [,1]. De segment som går in i respektive ut ur zonen beskrivs av r 1 (s) och r 2 (s) (kapitel 4.2.1) och skärningarna med zonen fås enligt kapitel till s 1 = s 1z respektive s 2 = s 2z (se figur 4.9).

44 3 Kartesisk banrepresentation Position Den del av respektive segment som befinner sig i zonen beskrivs av r 1 (s) = r 1 (s 1z + s (1 s 1z )) och r 2 (s) = r 2 (s s 2z ) så att r 1 () = r 1 (s 1z ), r 2 (1) = r 2 (s 2z ) och r 1 (1) = r 1 (1) = r 2 () = r 2 () (se figur 4.9). r 1 (s) s r 2 (s) 1 = s 2 = 1 s 1 = s 1z r(s) s 2 = s 2z r 1 (s) r 2 (s) s 1 = 1, s 2 = Figur 4.9. Segment och bana för en zon. Då båda segmenten är linjer kan man använda den s k de Casteljau-algoritmen [9] för att skapa en parabelformad kurva i zonen vars derivata m a p s har samma riktning som derivatan för linjerna. I det enklaste fallet byggs kurvan upp utifrån tre punkter enligt p 1 (s) = (1 s)p + sp 1 p 1 1(s) = (1 s)p 1 + sp 2 p 2 (s) = (1 s)p 1 + sp 1 1 där man kan se att p 1 och p 1 1 båda är ekvationer för en linje och att dessa vägs ihop så att en ekvation i s 2 erhålls. Om man istället för uttrycken för p 1 och p 1 1 ovan använder parameterbeskrivningarna för segmenten, borde man kunna skapa en zonbana för valfri kombination av linje, cirkelbåge och helix: r(s) = (1 s) r 1 (s) + s r 2 (s) (4.4) Denna funktion uppfyller r() = r 1 () och r(1) = r 2 (1) och är alltså (som önskat) kontinuerlig i zongränserna.

45 4.3 Zonbanan 31 Tyvärr är dess förstaderivata inte kontinuerlig i dessa punkter. Eftersom blir derivatorna och r (s) = r 1(s) + s( r 2(s) r 1(s)) + ( r 2 (s) r 1 (s)) r () = r 1() + ( r 2 () r 1 ()) r 1() r (1) = r 2(1) + ( r 2 (1) r 1 (1)) r 2(1) I (4.4) sker en linjär övergång från r 1 (s) till r 2 (s). Genom att ersätta denna med ett allmänt (olinjärt) uttryck α(s) kan man få viss kontroll över egenskaperna hos r(s). Uttrycket för r(s) blir då med funktionerna r 1 (s) och r 2 (s) som förut. Derivering av (4.5) ger r(s) = r 1 (s) + α(s) [ r 2 (s) r 1 (s)] (4.5) r (s) = r 1(s) + α(s) [ r 2(s) r 1(s)] + α (s) [ r 2 (s) r 1 (s)] (4.6) som genom r () = r 1() och r (1) = r 2(1) ger följande krav på α(s): α() =, α () =, α(1) = 1, α (1) = (4.7) Det går att finna flera funktioner som uppfyller (4.7), men ett tredjegradspolynom α(s) = 3 a j s j (4.8) är det enklaste valet. Efter derivering kan ett ekvationssystem ställas upp och funktionen bestämmas. j= Om man önskar att alla derivator dn ds n r(s) upp till n = N ska vara kontinuerliga i zongränserna får man genom upprepade deriveringar följande villkor på α(s): α() = α(1) = 1 d n α () =, n = 1,2,..., N dsn d n α (1) =, n = 1,2,..., N dsn till vilket ett polynom av grad 2N + 1 kan bestämmas. För fallen N = 1, N = 2 och N = 3 beräknas α 2N+1 (s) till vilka har utseenden enligt figur 4.1. α 3 (s) = 3s 2 2s 3 (4.9) α 5 (s) = 1s 3 15s 4 + 6s 5 (4.1) α 7 (s) = 35s 4 84s 5 + 7s 6 2s 7 (4.11)

46 32 Kartesisk banrepresentation (a) α 2N (b) α 2N+1 Figur 4.1. Funktionen α 2N+1 samt α 2N+1 för N = 1 (punktad linje), N = 2 (streckad linje) och N = 3 (heldragen linje) Orientering Orienteringen för del av respektive segment som befinner sig i zonen beskrivs (på liknande sätt som för positionen) av q 1 (s) = q 1 (s 1z + s (1 s 1z )) och q 2 (s) = q 2 (s s 2z ) så att q 1 () = q 1 (s 1z ) och q 2 (1) = q 2 (s 2z ) med q i (s) enligt kapitel I zonens centrum gäller q 1 (1) = q 2 (). Längs zonbanan kan orienteringen q(s) beräknas med en variant av de Casteljaualgoritmen där sfärisk linjär interpolering används (orienteringen representeras av enhetsquaternioner). Genom att ersätta r i (s) med q i (s) och använda funktionen Slerp får man som motsvarighet till (4.4). q(s) = Slerp( q 1 (s), q 2 (s);s) Beräknar man positionen i zonen enligt (4.5) måste dock uttrycket användas för att erhålla korrekt orientering. q(s) = Slerp( q 1 (s), q 2 (s);α(s)) (4.12)

47 4.4 Interpolering längs banan Interpolering längs banan Utanför zonerna På segmenten utanför zonerna råder det enkla (linjära) sambandet l c (s) = L c s, med L c enligt (4.2), mellan index l c och s. Detta gör att s(l c ) lätt kan bestämmas i en punkt på banan bara man känner längden av banan i början och slutet av aktuellt segment. Om L α och L β får stå för dessa längder kan index s bestämmas för ett givet l c som s = l c L α L β L α, l c [L α,l β ] där längden av segmentet ges av L β L α. Inuti zonerna I en zon finns normalt inget linjärt samband mellan index l c och s. Ej heller finns något analytiskt uttryck för zonbanans längd, men denna kan för tillräckligt stora N approximeras med summan L z = N 1 n= r( n + 1 N ) r( n N ) (eller någon liknande summa som tar hänsyn till storleken av normen för varje värde på n). Om man för varje n sparar delsumman och motsvarande värde på s, kan man skapa en splinefunktion (kapitel 6.1) s z (l),l [,L z ] som (approximativt) beskriver s(l c ) i zon z. Längs banan Varje segment beskrivs av index s i. Det s i för vilket segmentet skär en zon benämns för segment i som s iz# där #-tecknet står för zonens nummer, t ex s 3z2 för det tredje segmentets skärning med den andra zonen. Figur 4.11 visar detta för tre segment och två zoner. Med index l c [,L c ] beskrivs banans punkter från startpunkten till slutpunkten (L c =banans längd). Genom att hålla reda på l c för start- och slutpunkt samt zongränserna kan man bestämma vilken bandel och vilket s som beskriver en viss punkt. Om i betecknar bandelens nummer får man att l c [L i 1,L i ] bandel i En bandel med udda nummer är ett segment utanför en zon och en bandel med

48 34 Kartesisk banrepresentation s 1 = 1, s 2 = s 3 = 1 s 1 = s 1z1 s 2 = s 2z1 s 2 = s 2z2 s 2 = 1, s 3 = s 1 = s 3 = s 3z2 Figur Index för banans segment. jämnt nummer är en zonbana (se figur 4.12). Index s kan nu bestämmas som s = { lc L i 1 L i L i 1, i = 2n 1 s n (l c L i 1 ), i = 2n där s n (l) är den tidigare beskrivna splinefunktionen som kopplar samman index s och index l c på zonbanan. #1 #2 #3 #4 #5 Figur Numrering av banans delar. Beskrivna funktioner och metoder har implementerats i Matlab. Dessa används i kapitel 5 där några exempel ges på zonbanor beräknade enligt avsnitt

49 Kapitel 5 Exempel på kartesisk zoninterpolering Flera exempel på banor som kombinerar olika typer av banprimitiver kommer att ges. För varje exempel har zonbanan beräknats för α 3, α 5 respektive α 7 (enligt kapitel 4.3.2). En jämförelse med en zonbana skapad med linjära l-kurvor, beskrivet i [17], kommer också att göras för en bana bestående av två linjer. I kapitel 7 kommer dessa banor att omvandlas till splinefunktioner för robotens ledvariabler för ytterligare jämförelser. De zonbanor som visas i varje exempel kommer att se nästintill identiska ut. Det beror på att metoden för zoninterpolering med α N främst påverkar derivatorna av zonbanans parameterbeskrivning. Att det blir så kan ses genom att jämföra ekvationerna (4.5) och (4.6) med figur 4.1 där, för olika N, funktionerna α N har likartat utseende men deras derivator α N klart skiljer sig åt. Den förklaring av figur 5.2 som ges i nästa avsnitt gäller även figurerna Linje-Linje Figur 5.2 är uppdelad så att delfigur (a) visar hela banan (med orientering), och (b) (d) zonbanan för respektive α N. Den tjocka linjen markerar banan, de tunna punktade linjerna markerar segment och zoner, vilkas skärningar även märks ut av en liten ring. Orienteringen i en viss punkt har markerats med en pil med spetsen i punkten. Denna pil är egentligen ett koordinatsystem som skapats ur den enhetsquaternion som representerar orienteringen i punkten. Genom att translatera systemet i negativ z-riktning och rita en linje från origo till z-axelns skärning med banan har pilskaftet skapats. Den svarta respektive grå fjädern markerar x- och y-axlarnas 35