Programmering av styrning av tvättarm på tvättrobot.

Programmering av

styrning av tvättarm på

tvättrobot

Examensarbete inom Datateknik

av

Per Degerfält

Niklas Strömberg

29 januari 2008

Handledare:

Prof. Lars Asplund, Mälardalens Högskola

Jan Sandberg, Ramsta Robotics

Mälardalens Högskola

Institutionen för datateknik och elektronik

IDE

Sammanfattning

Ramsta Robotics är ett företag baserat i Uppsala. Man har utvecklat en robot - Clever Cleaner, som används för att göra grovrengöring av djurstallar. Roboten styrs i dagsläget av ett enkelt styr- och reglersystem för industrin, en s.k. PLC. Den styr via ett drivkort 6 motorer, dock endast en motor i taget. Ett nytt motorstyrkort är nyligen framtaget till roboten vilket gör det möjligt att styra samtliga motorer samtidigt.

Målet med arbetet var att ta fram en första version av ett nytt styrsystem som kommunicerar med de nyframtagna motorstyrkortet.

Styrsystemet ska ge användaren möjligheten att välja mellan att styra respek-tive led manuellt, till att utnyttja invers kinematik där användaren styr var munstycket ska vara beläget och styrprogrammet omvandlar den önskade posi-tionen till vinklar för respektive led.

Funktionalitet för inspelning och uppspelning av en armrörelse ska också nnas i styrprogrammet.

En ytgenererare ska också vara implementerad, med vilken operatören enkelt genom att endast ange 3 eller 4 hörnpunkter, kan få styrprogrammet att beräk-na ett rörelsemönster för tvättning av den uppmarkerade ytan.

Målen som var satta för detta examensarbete är uppnådda och en första version av styrprogrammet med de önskade funktionerna är framtagen. Dock behövs yt-terligare arbete för att förbättra funktionaliteten då tester av styrprogrammet endast skett mot lösa motorer och ej på den faktiska roboten, där belastning på motorer, slitage och andra yttre omständigheter påverkar robotens beteende.

Abstract

Ramsta Robotics is a company based in Uppsala. They have developed a robot - Clever Cleaner that is used to clean insides of livestock building's.

The robot is currently controlled by a simple control system, a so called PLC. 6 motors is controlled with a simple controller card, but only one motor can be controlled at a time. A new motor controller card has recently been developed for the robot. The new card makes it possible to control several motors at the same time.

The goal of this thesis was to develop a rst version of a control system that communicates with the new motor controller card. The control system should give the user the possibility to choose whether to control each arm separately or by using inverse kinematics, where the user is controlling the position of the nozzle and the control system calculates the desired position and then converts that to angles for each joint.

Functionality for recording and replay a pre-recorded arm movement should also be implementd in the control system.

A surface generator should also be implemented. The surface generator makes it possible for the user to select a surface using only 3 or 4 corner points. The control system would then use those points to create a path for the robot that will cover the selecteds surface when cleaning.

The goals for this thesis are fullled and a rst version of the control system with the desired functionallity has been developed. The system needs more work to make sure it works as intended to on a robot. Mainly because tests has only been made on motors that are not yet mounted on a robot. When mounted on the robot there are things like strain and wear on the motors that will aect their functionality.

Förord

Detta examensarbete valdes för att få en intressant och givande fortsättning på den projektkurs inom robotik som vi båda läst. Innehållet i arbetet verkade ock-så som en passande avslutning på studierna där en blandning av de kunskaper man samlat på sig under åren skulle komma till användning. Examensarbetet ligger till största delen inom området datateknik/robotik men innehåller även en hel del matematik, då framförallt räkning med matriser och vektorer. Examensarbetet ska förhoppningsvis resultera i att det styrsystem som är utveck-lat ska vara såpass bra att det efter tester på roboten och komplettering och uppdatering av vissa delar, skulle användas av Ramsta Robotics i deras Clever Cleaner.

Målgruppen för denna rapport är personer med viss kunskap inom ämnet datateknik samt personal på Ramsta Robotics.

I vissa kapitel har därför beskrivningar av diverse delar uteblivit då läsaren bör vara nog kunnig för att förstå vad som menas.

Vi vill passa på att tacka Jörgen Lidholm, doktorand på Mälardalens högskola för den hjälp och de råd han gett oss under examensarbetets gång. Vi vill också tacka våra handledare Lars Asplund, Proessor på Mälardalens högskola, för hans hjälp. Jan Sandberg på Ramsta Robotics för möjligheten till att skriva detta examensarbete och för den tid han ställt upp med trots att vi vet hur upptagen han är. Tack också till Håkan W och Jonas N för intressanta diskus-sioner inom området!

Innehåll

2.2 Kinematik . . . 9

2.2.1 Direkt kinematik . . . 10

2.2.2 Invers kinematik . . . 13

2.3 Automatisk uppbyggnad av yta för tvättning . . . 19

2.4 Motorkonverterare . . . 23

3 Design 24 3.1 Programmets design och uppbyggnad . . . 24

3.2 Operativsystem . . . 25 3.3 Programeringsspråk . . . 25 3.4 Dator . . . 26 3.5 Motorstyrkort . . . 27 3.6 Övrig utrustning . . . 28 3.6.1 Motorer . . . 28 3.6.2 Joystick . . . 28 3.6.3 Lagring . . . 28 3.7 Protokoll . . . 29 3.8 Simulator . . . 33 3.9 Inspelning av rörelse . . . 34 3.10 Uppspelning av rörelse . . . 34 3.11 Felövervakning . . . 34

4 Resultat 36 4.1 Resultat . . . 36 4.2 Framtida arbete . . . 41 4.3 Slutsatser . . . 42

Kapitel 1

Inledning

1.1 Bakgrund

Ramsta Robotics är ett företag baserat i Uppsala. Företaget grundades 1999 och har utvecklat en tvättrobot vars huvudsyfte är att sköta grovrengöring i djurstallar. I framförallt svinhus är arbetsmiljön i samband med tvättning di-rekt hälsovådlig, bland annat på grund av alla de kväveföreningar som löses ut när vattnet blöter upp de delar av anläggningen som skall tvättas. Detta gör att robotens arbete väsentligt förbättrar arbetsmiljön då ingen behöver vistas i svinhuset i samband med rengöringen.

Aärsidén för Ramsta Robotics har varit att utveckla en enkel och prisvärd robot och idag säljs detta system i ett ertal länder i Europa, men robotar har även sålts till bland annat Kanada och Japan. Hittills har över 150 robotar sålts, de esta för att användas i svinproduktion men även fågelproduktion och annan industri har visat sig vara en marknad.

Detta examensarbete är ett resultat av ett samarbete mellan Robotdalen, Mälardalens Högskola och Ramsta Robotics och syftar till att ge ett ökat tekniskt innehåll i företagets olika lösningar.

1.2 Syfte

Ordet robot kan härledas från det tjeckiska ordet robota som betyder tungt, monotont eller påtvingat arbete. Det nns också ett polskt ord, robotnik, som betyder arbetare.

in-dustrirobotarna utvecklades. Den första kommersiella roboten började användas 1961 på Ford Motor Company, där den kontrollerade en pressgjutningsmaskin. Under den första åren var det dock främst den amerikanska bilindustrin som använde robotar i sin produktion.

Robotar utvecklas även idag med syftet, om man bortser från de ekonomiska fördelarna, att förbättra arbetsmiljön för människan. Detta är också anlednin-gen till att Ramsta Robotics utvecklade Clever Cleaner. Att kunna låta en robot göra ett arbete som kan göra en människa sjuk eller skadad är givetvis bra. En robot kommer dessutom göra samma jobb och har heller inget emot att jobba vilken tid som helst under dygnet.

Syftet med detta examensarbete är att hjälpa Ramsta Robotics utveckla deras produkt så arbetet med att sköta roboten ytterligare förenklas och eektiviseras.

1.3 Mål

Målet med examensarbetet var att skapa en första version av ett styrsystem till Ramsta Robotics, Clever Cleaner. Styrsystemet ska styra armen med invers kinematik. Styrsystemet ska också innehålla en funktion där ett rörelsemönster för en yta kan beräknas endast genom att mata in ytans hörnpunkter till pro-grammet.

Alla rörelser som görs ska även kunna spelas in för att senare spelas upp i form av ett tvättprogram.

1.4 Arbetsmetod

Projektet började med att vi gjorde en undersökning om hur roboten fungerade i dagsläget, då framförallt hur roboten kontrolleras och hur en tvättning går till. Detta med hjälp av Jan Sandberg på Ramsta Robotics.

När vi hade den informationen påbörjades informationssökning på internet och i litteratur, om hur invers kinematik fungerar, om det skulle nnas någon metod att använda och hur man i sånt fall skulle applicera den rent praktiskt på roboten. Hänsyn till metodens komplexitet var också viktigt.

Information om hur en EIA-485 bus fungerar undersöktes också då ingen tidi-gare erfarenhet fanns av den bus-typen.

en förenklad version av roboten ritades ut med hjälp av OpenGL. Med denna var det sedan enkelt att se och analysera hur bra de olika metoderna fungerade. Eftersom vi inte hade tillgång till motorstyrkorten förrän mot slutet av pro-jektet, ck kommunikationen testas genom att koppla ihop två datorer där den ena ck agera motorstyrkort.

När motorstyrkorten var färdiga testades styrprogrammet mot motorerna så kommunikation och programmets övriga funktionalitet fungerade. Vi hoppas även kunna testa med en komplett robot inom kort.

Styrprogrammet är huvudsakligen skrivet i Ada men har också vissa funktioner skrivna i C. GNAT användes som kompilator för Ada-koden i projektet, gcc för C-koden.

Kapitel 2

Teori

2.1 Bentligt system Cleaver cleaner

Clever Cleaner är en helautomatisk tvättrobot med huvudsyftet att sköta grovrengöring av svinstallar. [7]

Roboten kontrolleras idag av ett styrsystem där man programmerar in ett städ-schema för roboten med hjälp av en joystick. På joysticken nns en väljare för vilken av lederna som ska kontrolleras, roboten kan med det bentliga styrsys-temet endast köra en motor i taget, men med de nya motorstyrkorten samt styrprogrammet kan alla motorer köras simultant. Roboten tvättar sedan au-tomatiskt enligt det inspelade städschemat.

Några tekniska specikationer (Nuvarande design)  Total bredd: 730 mm, med PVC-hjul 630 mm.

 Total längd: 2100 mm.

 Total höjd i hopfällt läge: 1610 mm.  Armens maximala-räckvidd: 4250 mm.  Eektiv arbetsräckvidd: upp till 6m.  Vikt: Totalt 270 kg.

 Kraftförsörjning: 2 st blyackumulatorer.  Elmotorer: 24 volt DC. Hitachi PLC.

Punkterna ovan är specikationer på hur roboten ser ut i dagsläget, utan den hårdvara och det styrprogram som framtagits under examensarbetet.

2.2 Kinematik

Figur 2.2: Robotens axlar

Robotarmen ska styras med invers kinematik. Med invers kinematik menas att man bestämmer hur lederna skall vrida sig för att få slutpunkten att hamna på en given plats. Ramsta robotics tvättrobot består av 5 leder:

 L1 kan rotera 360_{efter y-axeln}

 L3 är en utskjutande axel på 1.7m  L4 roterar 360 _{efter z-axeln}

 L5 roterar 360 _{efter x-axeln och har en konstant 45} _{vinkel mot L4}

I denna modell har leden L5 förbisetts då det är den som ger den sista vinkeln för spolningen och det är något användaren själv måste se och reglera. Ramsta robotics vill kunna styra roboten med hjälp av en joystick. Tanken är att det är slutpunkten på armen som är den punkt som användaren ska kunna styra med en joystick och alla leder i armen vrider sig på ett sådant sätt att slutpunkten kommer dit. Ramsta Robotics vill även kunna markera ut en yta som roboten ska tvätta. Då beräknas ett ertal målpunkter som armens slutpunkt ska röra sig mellan. Ett viktigt kriterium är att kinematiken vrider lederna så att slut-punkten rör sig i en rak linje till målet, detta är viktigt då roboten går mot en punkt efter en vägg. För att testa kinematiken och komma fram till vilken algoritm som skulle passa bäst i detta system, testades dessa i simulatorn.

2.2.1 Direkt kinematik

För att kunna hitta slutpunkten på armen så används direkt kinematik. Det går ut på att man föryttar en given punkt i ett koordinatsystem som beräknas med rotationsmatriser och translationsmatriser. Värdena i matriserna bestäms av ledernas vinklar och längder. I dessa beräkningar är origo för koordinatsys-temet i början av L1. Punkten som ska bestämmas kallas för P: Man sätter upp en matris för varje leds rotation och en för varje leds längd. Sedan mul-tiplicerar man P med dessa matriser så att P transleras ut till armens slutpunkt. Som punktlistan beskriver behövs Y-Rotationsmatrisen, Z-rotationsmatrisen samt translationsmatrisen för att nna P:

Nedan beskrivs de olika matriserna.

T ranslationsmatris =      1 0 0 x 0 1 0 y 0 0 1 z 0 0 0 1      Y Rotationsmatris =      cos  0 sin  0 0 1 0 0 sin  0 cos  0 0 0 0 1     

Z Rotationsmatris =      cos  sin  0 0 sin  cos  0 0 0 0 1 0 0 0 0 1     

För att bestämma positionen för P måste den multipliceras med följade matris-er: 1. Rotationsmatrisen för L1 rotation: Q1 2. Translationsmatrisen för L1 längd: Q2 3. Rotationsmatrisen för L2 rotation: Q3 4. Translationsmatrisen för L2 längd: Q4 5. Translationsmatrisen för L3 längd: Q5 6. Rotationsmatrisen för L4 rotation: Q6 7. Translationsmatrisen för L4 längd: Q7

Om man multiplicerar ihop alla matriser så får man en slutgiltig matris som beskriver P position. M = Q1  Q2  Q3  Q4  Q5  Q6  Q7 = 2 6 6 6 6 4 cos 1 0 sin 1 0 0 1 0 0 sin 1 0 cos 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 1 0 0 0 0 1 0 l1 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 cos 2 sin 2 0 0 sin 2 cos 2 0 0 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 1 0 0 0 0 1 0 l2 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 1 0 0 0 0 1 0 l3 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 cos 4 sin 4 0 0 sin 4 cos 4 0 0 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 1 0 0 0 0 1 0 l4 0 0 1 0 0 0 0 1 3 7 7 7 7 5= M

I denna modell så börjar P alltid i origo O : (0; 0; 0) vilket medför att det är bara kolumn 4 i matrisen M som påverkar P : s föryttning. P : s position kan därför beskrivas enligt följade:

Px = ( cos 3 cos 2 sin 3 cos 3 sin 2 c2)  l2 cos 3 sin 2 l3

cos 3 sin 2 l2;

Py= ( sin 2  sin 3 + cos 2  cos 3)  l2 + cos 2  l3 + cos 2  l2 + l1;

Pz= (sin 1  cos 2  sin 3 + sin 1  sin 2  c2)  l4 + sin 1  sin 2  l3 + sin 1 

2.2.2 Invers kinematik

Med invers kinematik är en målpunkt G:(x,y,z) känd och problemet ligger i att räkna ut vinklarna för lederna och i detta fall även längden på L3 för att P ska nå G. För Ramsta Robotics Clever Cleaner där slutpunkten P ska styras med en joystick, kan man se det som att joysticken yttar punkten G i rummet och med hjälp av invers kinematik räkna ut hur varje led ska förytta sig för att P ska nå sitt mål. Ett krav i rörelsemönstret för armen är att P måste röra sig efter en rak linje. Detta är viktigt då munstycket annars kan gå in i en vägg eller golvet.

Det nns två vanliga lösningar för invers kinematik. Den ena är en analytisk lösning. Problem med dessa är att en lösning ofta kan vara omöjliga att få fram. Samt att den kan ge oändligt många lösningar eller ingen lösning.

En annan metod är att använda en numerisk lösning där Jacobis metoden kan användas. Dessa kan vara beräkningstunga men man hittar ofta en bra lösning. CCD

Vad som sökes är en metod som är snabb men ger ändå bra löningar, en sådan metod är algoritmen Cyclic-Coordinate Descent (CCD) [1]. Den metoden är bra för att den är enkel att implementera samt för att den inte är beräkningstung. CCD ska även vara en modell som passar bra i realtidssystem.

CCD algoritmen börjar med sista leden och jobbar sig bakåt, för varje led roteras leden så att vektorn som går från basen av leden till P ligger parallellt med vek-torn mellan basen av leden och G. Se bild 2.3. Att beräkna CCD algoritmen

Figur 2.3: Cyclic-Coordinate Descent

oine så att de okända vinklarna framkommer och sedan applicera vinklarna på motorerna skulle kunna resultera i att armens slutpunkt P inte rör sig efter en rak linje mot målpunkten G utan P skulle med stor sannolikhet röra sig i en kurvig bana. Detta fenomen kommer att bli mer synligt då avståndet mellan P

och G ökar.

Att istället ha en metod som ger små förändringar i lederna och en liten rörelse i P för varje beräkning, och dessutom se till att rörelsen ligger i en vektor mot G. Skulle ge ett resultat som skulle kunna uppfylla kriteriumet att få P att röra sig i en rak linje.

En variant av CCD metoden testades. Datorn beräknade då oine hur P:s avstånd till G förändrades när man roterade eller translerade en led i taget så att P gjorde en föryttning. I ett försök i simulatorn beräknades vinkeln  för leden så att rotationen skulle ge en föryttning på 1 enhet (1 enhet = 1mm).

 = arccos(1 dist2 2  L2)

Innan rotationen applicerades på leden beräknades, med hjälp av direkt kine-matik, om rotationen skulle resultera i att avståndet mellan P och G minskade. Om avståndet inte minskade så inverterades  och ett nytt test gjordes. Om avståndet minskade så applicerades rotationen på leden. Sedan gjordes samma beräkning för nästa led.

När led L3 som är en utskjutande led skulle beräknas, så gjordes två tester där leden sköts in/ut en enhet och sedan gjordes samma beräkningar igen.

Efter en del tester i simulator framgick det att rörelserna tenderade att bli väldigt hackiga. För att rörelsen skulle bli mjukare lades ett rörelseschema, se bild 2.4, in som beskrev de rörelser som skulle testas med direkt kinematik. I schemat fanns det olika rörelserna som lederna skulle göra, och två leder kunde dessutom röra sig samtidigt.

Schemat gjordes med en 2-dimensions array som initierades med rörelserna i början. För varje föryttning gjordes alla rörelser i schemat och med direkt kinematik beräknades den nya slutpunkten P . Sedan beräknades avståndet mel-lan P och G och resultatet sparades i en lista. Sedan sorterades listan och det bästa resultatet applicerades på lederna.

Denna lösning med invers kinematik fungerade ganska tillfredställande i sim-ulatorn, men överlag är det en naiv och inte direkt eektiv lösning då direkt kinematiksberäkningen skulle behöva göras 32 ggr för varje föryttning och den-na föryttning ska beräkden-nas minst 10ggr/sekund.

Tester för att kontrollera om datorn skulle klara av att göra dessa beräkningar så ofta som nödvändigt utfördes. I testet loopades ett kodstycke som utförde direkt kinematiksberäkningen 100 gånger. För varje gång ändrades vinkeln för lederna så testet skulle vara mer veklighetstroget. Koden kördes sedan i datorn

Figur 2.4: Rörelseschema

att det skulle ta 0,704 ms att göra 32 beräkningar, och att datorn skulle utan problem klara av tidskravet på 10ggr/sekund.

Ett annat problem med denna lösning är att slutpunkten P inte rör sig i en perfekt linje mot målet G varför illustreras med bild 2.5

Figur 2.5: Problem vid användning av CCD

Här syns två lösningar för att komma närmare G där P antingen är vid p1 eller p2 som bild 2.5 visar. I detta fall så är det p1 som skulle vara den rätta lösnin-gen för att P ska förytta sig i en rak linje. Men avståndet d2 blir kortare än avståndet d1 och av den anledningen så kommer den rörelsern att appliceras på lederna. Detta ger resultatet att P inte rör sig i en perfekt linje mot G.

En lösning på Invers kinematiken med vektorgeometri söktes istället. Efter sam-tal med Lars Asplund om problemet så testades det att med hjälp av derivatan på ledernas rörelser, söka efter vilka rotationer och translationer som skulle ap-pliceras på lederna för att styra P mot G i en rak linje.

Genom att derivera formlerna för direkt kinematik ges vilken riktning som en viss led påverkar slutpunkten P .

Att derivera formeln

Px = ( cos 1 cos 2 sin 3 cos 1 sin 2 c2)  l2 cos 1 sin 2 l3

cos 1 sin 2 l2

med avseende på 1 skulle ge vilken påverkan led1 rotation ger P i x-led. Så

att derivera hela formeln för direkt kinematik med avseende på alla vinklar och sedan representera dessa som vektorer, kan man beräkna hur mycket varje led skall rotera för att P ska röra sig korrekt.

Exempel på IK lösning

Här är ett exempel med två leder med längden 200 l.e. som endast kan rotera runt Z-axeln samt en målpunkt G illustreras lösningen.

1= 54= 306

2= 66= 294

Först måste matrisen för direkt kinematik beräknas. M = 2 6 6 6 6 4 cos 2 sin 2 0 0 sin 2 cos 2 0 0 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 1 0 0 0 0 1 0 200 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 cos 2 sin 2 0 0 sin 2 cos 2 0 0 0 0 1 0 0 0 0 1 3 7 7 7 7 5 2 6 6 6 6 4 1 0 0 0 0 1 0 200 0 0 1 0 0 0 0 1 3 7 7 7 7 5

Även här så börjar P i origo [0,0,0] så när P multipliceras med M så är det endast kolumn 4 som påverkar P :s position. P kan alltså beskrivas enligt:

Px= 200 sin 2cos 1 200 sin 1cos 2 200 sin 1

Py = 200 sin 1sin 2+ 200 cos 1cos 2+ 200 cos 1

Pz= 0

Sedan beräknas derivatan för P för att få fram hur ledernas påverkar P : s position. Derivatan för P med avseende på 1

dPx

1 = 200 sin 1sin 2 200 cos 1cos 2 200 cos 1

dPy

1 = 200 cos 1sin 2 200 sin 1cos 2 200 sin 1

dPz

1 = 0

Derivatan för P med avseende på 2 blir:

dPx

2 = 200 cos 1cos 2+ 200 sin 1sin 2 200 sin 1

dPy

2 = 200 cos 1sin 2 200 sin 1cos 2+ 200 cos 1

dPz

2 = 0

Ledernas nuvarande vinklar läggs till i derivatan, och derivatans lösningar rep-resenteras som vektorer. Hädanefter så prep-resenteras bara x och y då z inte har

någon påverkan.

V 1x= 200 sin 306 sin 294 200 cos 306 cos 294 200 cos 306 = 17:6

V 1y = 200 cos 306 sin 294 200 sin 306 cos 294 200 sin 306 = 335:0

V 2x= 200 cos 306 cos 294 + 200 sin 306 sin 294 200 sin 306 = 261:8

V 2y= 200 cos 306 sin 294 200 sin 306 cos 294 + 200 cos 306 = 290:8

Beräknar !r som är riktningsvektorn mot G. !r normaliseras och multipliceras med med den längd som ska vara den minsta föryttning som P kan göra. I detta fall 20. ! r = ! G P!  !G P! 20

Px= 200 cos 306 sin 294 200 sin 306 cos 294 200 sin 306 = 335:0

Py= 200 sin 306 sin 294 + 200 cos 306 cos 294 + 200 cos 306 = 17:6

! r = ! (368:0; 131:0) (335:0; 17:6)!  (368:0; 131:0)! (335:0; 17:6)!   20 = (5:6; 19:2)

VektorernaV 1! V 2! !r Ritas in i bild 2.7.

! V 1= ( 17:6; 335:0) ! V 2= (261:8; 290:8) ! r = (5:6; 19:2)

VektorernaV 1! V 2 visar riktningen som P har i förhållande till ledernas rotation.! Att söka hur mycket varje led ska rotera för att P ska röra sig i!r riktning, kan göras med hjälp av ett ekvationssystem.

" 17:6 261:8 335:0 290:8 # " 1 2 # = " 5:6 19:2 #

Figur 2.7: Exempel med två leder

lösningen till ekvationssystemet med determinanter enlig Cramers regel 1= det_det1 D = 5:6  290:8 19:2  261:8 17:6  290:8 335:0  261:8 = 0:0366 2= det_det2 D = 17:6  19:2 335:0  5:6 17:6  290:8 335:0  261:8 = 0:0238

Längden på V1 V2 är samma som längden på radien för den axeln, detta ger att svaret på ekvationen är i radianer. För att konvertera radianer till grader multipliceras talen med 57.295. Detta ger att led 1 ska vrida sig 2:069 _{och att}

led 2 ska vrida sig 1:367_{för att P ska röra sig en längdenhet mot G i riktningen}

av!r .

2.3 Automatisk uppbyggnad av yta för tvättning

För att förenkla inspelningsprocessen så var ett mål med arbetet att Clever Cleaner ska kunna spola av en yta automatiskt efter det att ytans hörnpunkter har angivits. Ytorna kan representeras av 3 eller 4 punkter som bildar ytan som ska tvättas. Punkterna markeras genom att användaren styr armens slutpunkt P till någon punkt i rummet där den sedan markeras som en hörnpunkt. Yta med tre punkter

Om tre punkter representerar ytan så skapas en riktningsvektor Br mellan p! 0

och p1, som beskriver längs vilken riktning som munstycket ska spola av. En

riktningsvektor Hr mellan p! 1 och p2 som markerar ena kanten och en

rikt-ningsvektor V r mellan p! 0 och p2 som markerar andra kanten. Sedan beräknas

ett ertal målpunkter G efter kanterna. Dessa målpunkter sätts upp på så sätt att slutpunkten P på armen kommer att svepa över ytan i ett fördenierat sick-sack mönster vilket gör att hela ytan kommer att bli avspolad. Beräkningar för

Figur 2.8: Yta med 3 punkter

Figur 2.9: Tre punkters yta med kantpunkter

att hitta alla punkter G, görs med vektogeometri genom att först hitta en rikt-ingsvektor för!r som är vinkelrät mot basvektornBr denna beräknas fram med! hjälp av två vektorprodukter. Först vektorprodukten mellan basvektornBr och! kantvektornV r. Den vektorn som bildas av vektorprodukten är vinkelrät mot! bådeBr och! V r dvs. den pekar ut från ytan. Vektorprodukten mellan den nya! vektorn ochBr som sedan normaliseras ger den sökta riktingsvektorn.!

! r = _! BrV r!  Br!  Br! V r!  Br!

Riktningsvektorn !r används för att hitta avståndet mellan punkterna G på respektive kantvektor. Avståndet beräknas med skalärprodukten mellan!r och den kant där avstånden söks.

Yta med fyra punkter

En yta kan också representeras med 4 punkter. Med det följer två direkta prob-lem.

 Den fjärde punkten ligger inte i samma plan som de andra tre.

 Punkterna måste sättas ut i rätt ordning. Vektorerna som bildas mellan punkterna 0-1, 1-2, 2-3 och 3-0 får inte korsas.

Figur 2.10: Två problem med en yta skapad med fyra punkter

Det första problemet löses genom att bilda ett plan med de 3 första punkterna och sedan projicera ner den fjärde punkten på planet.

För att lösa det andra problemet så används en metod för att beräkna avstån-det mellan linjer i rummet. Först skapas en linje L1mellan hörnpunkten P0och

P1, och en linje L2 mellan P2 och P3. Mellan två linjer i ett rum nns alltid

minst en vektor !d som är vinkelrät mot linjerna, se bild 2.11 Den vektorns längd ger det kortaste avståndet mellan linjernaL1 och L1. För att nna!d så

Figur 2.11: Vektorn!d ger avståndet mellan linjerna

Ett ekvationssystem sätts upp som söker rätt på den punkt på vardera linje där skalärprodukten är noll. Om avståndet mellan linjerna är nära noll så korsas linjerna.

Men två linjer som ligger i samma plan kommer alltid att korsas om de inte ligger helt parallellt i förhållande till varandra. En kontroll måste utföras om korsningen ligger innanför ytan som ska spolas. Detta görs enkelt genom att jämnföra avståndet d1 mellan punkterna som bildar linjen och avståndet d2

mellan start punkten och korsnings punkten, se bild 2.12. Om avståndet till korsningen är mindre än avståndet mellan punkterna så korsas linjerna i ytan och denna punkt uppsättning är inte tillåten.

Figur 2.12: Undersöker om korsningen ligger innanför ytans gränser Samma undersökning sker sedan mellan linjerna som bildas av hörnpunkterna P1-P2 och P3-P0. Om det visar sig att två linjer korsas innanför ytan så byter

datorn plats på två punkter så att problemet löser sig. Detta gör uppsättning av hörnpunkter mycket lättare för användaren av Clever Cleaner.

Uppsättningen för alla målpunkter G efter kanterna på en yta med fyra hörn-punkter görs nästan på samma sätt som en yta med tre hörnhörn-punkter, med den skillnaden att när ena kanten tar slut under uppbyggnaden så byts kanten ut mot en ny kantvektor. Exempel ytan som bestås av punkterna P0, P1, P2 och

P3 så bildas fört bas vektorn ! P0P1, sen två kanter ! P1P2 och ! P0P3. Om kanten !

P1P2 skulle ta slut ersätt den kanten med vektorn !

Figur 2.13: G för en yta med fyra hörnpunkter

2.4 Motorkonverterare

Värdet som Inversa kinematiken (IK) ger, är den vinkel eller längd som varje led ska förytta sig för att armens slutpunkt P ska röra sig i riktningen mot målpunkten G. Signalen som ska skickas till motorerna är en PWM signal med ett värde mellan +40 till -40. Så det går inte att direkt skicka vinkeln för leden till motorerna utan den måste först konverteras till en hastighet mellan 40. Eftersom alla leder får en vinkel som ska läggas på ledens nuvarande vinkel från IK och det förväntas att alla armar ska ha tagit sig till den nya vinkeln under samma tid, så kan man dra en direkt parallell att värdet som IK ger ut är samma sak som hastigheterna sinsemellan lederna. Dvs. om led0får värdet +0.3

och led1får värdet 0.6 betyder det att dessa leder ska vrida sig det värdet under

samma tid. Man kan säga att led1 ska röra sig dubbelt så fort som led0. För

att göra en konverterare från grader till PWM signal så behövs en algoritm som konverterar upp den största vinkelförändringen till 40, vilket är PWM-max, och låter de andra vinklarna följa med på ett sådant sätt att förhållandena mellan vinklarna inte förändras.

Först söks den vinkel som ska göra största förändringen. Vmax och så skapas

en faktor f av den. f används sedan för att hitta alla motorers PWM värde. f = 1

Vmax

P W Mi= f  IKi 40

I detta fall så kommer armen att röra sig i max hastighet efter som P W Mmax

kommer att vara 40 hela tiden. Konstanten 40 byts sedan ut mot en variabel mellan 0-40 som styrs av utslaget från joysticken, vilket gör att man får en varierande hastighet för P .

Kapitel 3

Design

3.1 Programmets design och uppbyggnad

Figur 3.1: Schema över hur styrprogrammet är uppbyggt.

Mycket tid har lagts vid designen av styrprogrammet för att få den så bra som möjligt innan själva programmeringen påbörjades. Många problem löstes redan vid framtagandet av designen och har underlättat det fortsatta arbetet.

som möjligt och att paketen endast ska vara synliga för andra paket i de fall där det är nödvändigt.

Förutom paketen som nns med i designen nns också två denitionsler, Ram-sta denes och Motor denes där globala variabler som behöver vara synliga från många paket har placerats.

De olika paketens funktionalitet beskrivs mer ingående senare i rapporten.

3.2 Operativsystem

Operativsystemet som körs på styrdatorn är i grunden en Linuxkärna med Re-altids patchar1 _{som ändrar hur operativsystemet beter sig.[3] Patcharna}

kon-gurerar systemet så att så gott som hela kärnan kan bli avbruten. Systemets klocka patchas också till att bli betydligt mer högupplöst.[4]

Genom att använda en Linuxkärna som grund får man tillgång till drivrutiner för grak, TCP/IP och fönstersystem på ett enkelt sätt. Styrprogrammet i det-ta projekt använder sig till exempel av drivrutiner för joystick samt serieport vilket gör att det passar utmärkt till att köra styrprogrammet på.

3.3 Programeringsspråk

Styrprogrammet är i huvudsak skrivet i Ada men också C kod nns för mer hårdvarunära programmering. Den OpenGL simulator som använts för att tes-ta olika IK lösningar är även den skriven i C.

Ada är ett modernt programmeringsspråk som är skapat framförallt för att bygga stora långlivade inbyggda system där pålitlighet och eektivitet är vik-tigt. Ada används därför i dagsläget ofta inom yg och tågindustrin där kraven på säkerhet är höga.

Ada är ett starkt typat programmeringsspråk som lämpar sig mycket bra för utveckling av realtidssystem. Det är också enkelt att skapa trådar, även kallade tasks i ada, och att tilldela dessa prioriteter och periodtider.

Ada erbjuder också skydd av delade objekt genom att använda typen protect-ed, förhindras automatiskt med hjälp av mutual exclusion, att era tasks kan läsa/skriva samtidigt till objektet.

3.4 Dator

Datorn som används är en 3,5 EM730 Embedded SBC från AEWIN Technolo-gies2_.

Figur 3.2: Den dator som används för att köra styrprogrammet. På bilden är ett trådlöst nätverkskort monterat.

Specikationer

 Processor - Intel Celeron M ULV 600MHz Zero cache processor.  Minne - 256 MB DDR SO-DIMM, stödjer upp till 1GB.

 Serieportar - Tre EIA-232 och en EIA-232/485 4/2-tråds serieportar.  USB anslutningar - Stödjer fyra USB 2.0/1.1 portar.

Det nns också kortplatser för bland annat trådlöst nätverkskort, vilket kan komma att användas för att överföra tvättscheman och annan data från roboten. Systemet drivs av en enkel matningsspänning på 5V och kan operera i tempera-turer mellan 0 ₆₀_{. Systemet har en EIA-485 2-tråds anslutning som används}

för att kommunicera med motorkorten.

Ändringar som bör göras innan montering på robot

Datorn har i sitt grundutförande endast passiv kylning men bör kompletteras med en äkt, alternativt en bättre kyläns då klimatet med säkerhet kommer variera där systemet kommer monteras. Detta för att förlänga komponenternas livslängd.

3.5 Motorstyrkort

Motorkortet är framtaget av SMTeknik och har till uppgift att styra motorn enligt de instruktioner som kommer från styrsystemet.[SMTeknik] Kortet ska därefter svara med relevanta data om aktuell strömförbrukning, temperatur och motorposition.

Varje motordrivkort kan genom styrsystemet ges ett unikt id som då lagras i EEPROM3_{. Kortet har en anslutning för 24V matning samt en EIA-485}

anslut-ning för kommunikation.

På motorkortet sitter en processor (ATMega8) som hanterar kommunikationen, mäter aktuell motorström och temperatur samt skickar styrsignaler till motor-drivkretsen.

Signalen till motordrivkretsen innehåller en riktning och en PWM-signal. Drivkret-sen svarar sedan för nivåanpassning till FET som styr motorn. KretDrivkret-sen har även inbyggd skyddslogik för att säkerställa att endast rätt FET är öppen vid varje tillfälle.

I motorn sitter en dubbel halleektgivare som ger signal om riktning och en puls per motorvarv.

Motostyrkortet gjuts tillsammans med motorn in i polyuretangummi innan det monteras på roboten för att få ett bra skydd mot yttre påverkan av vatten och annat som kan skada komponenterna.

Figur 3.3: Motorstyrkortet från SMTeknik.

3_{EEPROM - Electrically Erasable Programmable Read-Only Memory är en typ av minne}

3.6 Övrig utrustning

Nedan redovisas för ytterligare hårdvara som använts under projektets gång.

3.6.1 Motorer

Motorerna på roboten är 24volts El-motorer och styrs via en H-brygga på mo-torstyrkortet. På motorerna sitter också en växellåda med utväxlingen 42:1. På motoraxeln sitter en tachometer som möjliggör för motorstyrkortet att avläsa riktning och motorvarvtal.

3.6.2 Joystick

För att kunna styra roboten med det nya styrsystemet har två joysticks an-vänts för att göra styrningen så enkel som möjligt. Två stycken vanliga 3-axliga konsument-joysticks inkopplade via USB har använts under testerna.

3.6.3 Lagring

Ett USB-minne har använts som boot-disk. Operativsystemet läses vid uppstart in till datorns RAM tillsammans med styrprogrammet. USB-minnet används också av styrprogrammet för att lagra de tvättprogram som användaren väljer att spela in. Det minne som har använts under testerna är av storleken 512 MiB, där mindre än 20% upptas av OS samt styrprogrammet.

3.7 Protokoll

Då styrdatorn sköter all kommunikation med styrkorten behövs det ett protokoll för att hantera den. Uppbyggnad av paket och styrkommandon denerades av konstruktören4 _{till motorstyrkorten och var därmed det som lade grunden till}

hur protokollet skulle utformas. Send Byte Innehåll 0 Start 1 Id 2 Order 3 Data 4 Checksumma 5 Slut

De order som kan skickas i dagsläget

Order Innehåll Sätt id 10 Nollställ position 20 Utstyrning av PWM 30 Skicka status 40 Recieve Byte Innehåll 0 Start 1 Id 2 Position - MSB 3 Position - LSB 4 Ström (10*A)

5 Temperatur (Grader Celcius)

6 Checksumma

7 Slut

Programmet har i huvudsak tre paket som hanterar kommunikationen i pro-grammet. Dessa är, Message handler, Communication handler samt Serial ctrl. Message handler

Message handler innehåller en array av motorobjekt, av typen protected. Ar-rayen är av samma storlek som det nns motorer och är till för att lagra det

senaste datat från varje motor.

I paketet nns en tråd vars syfte är att läsa av communication handlers motta-garbuert, för att kontrollera om det nns någon nytt meddelande i den. Om det gör det så läser den av meddelandet och lägger in de nya värdena till den motor meddelandet kom ifrån och uppdaterar även ut-index i mottagarbuer-ten.

Tråden hanterar också eventuella felmeddelanden som skickas i de fall en om-sändning har inträat. Message handler innehåller även funktioner för att skapa och placera meddelanden i sändarbuerten. Vid händelse av ett fel kommer trå-den att stoppas så att inga nya meddelantrå-den kan skickas till motorerna, i och med det kommer också ett meddelande skickas till alla motorer med ordern att stanna.

Om styrprogrammet är i inspelningsläge kommer message handler att skicka alla PWM värden, som ska sändas till motorerna, skickas till recorder också. Communication handler

Communication handler sköter sändning och mottagning av paketen till och från motorstyrkortet. Paketet innehåller därför en sänd- och mottagarbuert. Buerterna är arrayer som består av objekt av typen protected, i och med det kommer läsningar och skrivningar skyddas med semaphorer. En tråd sköter sändning och mottagning av meddelanden.

Tråden kan sättas i två lägen, sändning eller mottagning. I läget sändning står den alltid, om det inte är så att ett meddelande precis har skickats. I sänd-ning kan två saker inträa. Det första är att den kontrollerar sändbuerten om ut-index ej är detsamma som in-index. Ut-index specicerar på vilken plats ett meddelandet som ska skickas ligger och in-index på vilken plats ett nytt meddelande ska läggas. Om index skiljer sig betyder det att det nns ett nytt meddelande att skicka. Meddelandet skickas en byte i taget via skrivfunktionen som nns i paketet Serial Ctrl. Om meddelandet skickas utan några fel kommer tråden hamna i mottagarläget.

I mottagarläget kontrollerar tråden om det kommer något svar över seriebussen. Om det nns någon data så kommer funktionen som läser av meddelandet kon-trollera så att det är ett giltigt meddelande som kommer fram. Kontrollen börjar med att se om första byten är en startbyte. Är det en startbyte läses ytterligare sex byte. Sista byten kontrolleras att det är en slutbyte. Checksumman kon-trolleras sedan mot det mottagna meddelandet att det är korrekt.

Checksumman räknas fram genom att addera värdet på byte 1 till 5 modulo 128 på ett meddelande från motorstyrkortet och värdet på byte 1 till 3 modulo

128 på ett meddelande från styrdatorn.

Det kompletta meddelandet läggs i sånt fall in i mottagarbuerten där indexet uppdateras. Nu kan även ut-index i sändarbuerten uppdateras då en komplett meddelandecykel har ägt rum och inga omsändningar kommer vara nödvändiga. Tråden sätts också till sändningsläget igen.

I sändläget kan också ett statusmeddelande, som har till syfte att polla motor-erna för att se deras status, skickas till alla motorer. Statusmeddelandet kommer dock inte skickas om det inte är så att motorstyrkortens id är satta och kon-trollerade först. Statusmeddelandet kommer alltså alltid sändas till motorerna så länge det inte nns något meddelande i sändarbuerten, då meddelanden i sändarbuerten har högre prioritet. Anledningen till att statusmeddelandet nns är för att säkerställa att det alltid nns relevant data om motorerna till-gänglig. Eftersom det alltid är styrprogrammet som initierar kommunikationen så skulle ingen aktuell data från motorstyrkorten nnas så länge inget nytt med-delande skickas från styrprogrammet. Detta är inte önskvärt då exempelvis en motor kan vara igång utan att styrprogrammet har någon vetskap om det. Omsändning av meddelandet sker om inte någon svar har mottagits inom  12 ms. En felräknare kommer att räknas upp för varje gång en motor inte svarar. Varje motor har en egen felräknare vilken paketet Error övervakar. Felräknaren minskas för varje korrekt svar, men ej mindre än noll.

Tiden innan dess att en omsändning sker är vald för att ligga på en god marginal för både tiden det tar innan ett svar i bästa fall kan mottas, samt det tidskravet som nns på sampling av motorerna på 10 ggr/sekund.

Vid mottagning av ett meddelande i motorstyrkortet kommer ett svar skickas, efter det att hela meddelandet mottagits, med en fördröjning på minst 50s. Fördröjningen är nödvändig eftersom en växling från sändare till mottagare, först måste ske på styrdatorn. Växlingen har uppmätts till  20s.

Överföringshastigheten som används för kommunikationen är 38400bps vilket betyder att tiden för att skicka en bit är

1

38400 26s

Detta betyder att ett meddelande till motorstyrkortet skickas på 6  8  26 + 6  2  26 = 1560s

Vid mottagning av ett meddelande nns även en felhantering som säker-ställer att det inte ligger någon gammal data i UART buerten. Felhanteringen fungerar så att om det visar sig att det nns data i buerten och det inte är startbyte så kommer funktionen läsa av buerten meddelandestorleken+1 gånger. Detta tömmer buerten från skräpdata, alternativt kan bidra till att ett korrekt meddelande kan avläsas. I övriga fall så är buerten tömd tills dess att nästa svar anländer.

Serial Ctrl

Serial Ctrl är ett paket som sköter öppning och stängning samt skrivning och läsning av serieporten. Paketet är ett interface i ada för att komma åt funktion-erna denierade i serial interface, vilka är skrivna i c.

3.8 Simulator

En simulator av roboten med alla dess leder gjordes i OpenGL skrivet i C. Efter-som det inte fanns en robot att testa styrprogrammet på så var skapandet av en simulator nödvändig. Detta för att på ett enkelt sätt kunna provköra kine-matiken. Algoritmer för invers kinematik skrevs först i C och applicerades sedan direkt mot simulatorn. På det sättet var det tydligt om teorin för algoritmen fungerade. När algoritmen fungerade tillfredställande i simulatorn så skrevs ko-den om till att fungera i Ada. För att testa att algoritmen även fungerade i Ada så gjordes en server i C simulatorn och i Ada så skrevs en klient i paketet mes-sage handler. Med detta så kunde Ada koden köras och skicka data via sockets till simulatorn. Detta gjorde så att paket som IK, Surface, och motor converter kunde testas mot OpenGL simulatorn.

3.9 Inspelning av rörelse

Inspelning av robotarmens rörelse sker i paketet recorder. En inspelning ak-tiveras av operatören. Recorder skapar alternativt skriver över en ny programl till vilken motorernas PWM värde sparas. Recorder innehåller en array som är ett protected object vilken är till för att spara PWM-värden som skickats till motorerna. Om systemet är i inspelningsläget kommer message handler uppdat-era arrayen varje gång den mottagit en ny PWM som ska skickas till motorerna. I recorder nns en task som exekveras 10ggr/sekund[Sandberg], för varje ex-ekvering sparas värdet på PWM för varje motor, som då ligger i PWM-arrayen till l. Programlen lagras på USB-Minnet.

Storleken av PWM är en byte. En inspelning kan tidsmässigt variera mycket men på grund av att det med 6 stycken motorer där PWM lagras 10 ggr/sekund endast ger en lstorlek på 5184000=1048576  4:94MB, för ett program där inspelningen varar i 24 timmar, så spelar längden på inspelningen ingen roll. Vanligtvis är en inspelning betydligt kortare än så. Fördelen med detta är dock att man kan lagra många olika program, för olika typer av tvättningar med mera, utan att det nns någon som helst risk till att fylla minnet.

3.10 Uppspelning av rörelse

Uppspelning av robotarmens rörelse sker i paketet player. En uppspelning star-tas av operatören och består i första versionen av styrprogrammet endast av en förenklad uppspelning där inte val av tvättprogram med mera inte är imple-menterat.

I player ligger en task där läsning från en programl sker i samma frekvens som skrivning till len skett med, i det här fallet 10ggr/sekund. Player läser PWM för alla inkopplade motorer från len och skickar sedan iväg dessa till motorerna och stannar sedan när den når slutet av programlen.

3.11 Felövervakning

Ett paket för felhantering är nödvändig för att detektera fel i kommunikationen samt kontrollera så att motorernas strömförbrukning eller temperaturer inte hamnar på för höga nivåer.

Error innehåller en tråd som avläser motorvärdena som ligger lagrade i motorar-rayen i message handler.

Om något fel upptäcks kommer en agga sättas i ett skyddat objekt i error vilket main ständigt läser av och sedan hanterar det om ett fel har uppstått.

Det som kontrolleras är alltså  Error

 Temperatur  Strömförbrukning

Varje motor innehåller en variabel Error där värdet beror på hur många fel-sändningar den aktuella motorn har genererat. Error adderas med ett vid en eventuell omsändning och subtraheras med ett om en sändning har varit lyckad. Detta gör att man kan anse att en motor är defekt om error uppgår till ett visst värde. Hur stort detta värde bör vara måste testas fram beroende på hur sys-temet beter sig på roboten. Att en felsändning sker är inte helt ovanligt och kan exempelvis bero på yttre störningar, kabeltyp m.m, vilket gör att meddelandet blir felaktigt. Då inga tester utförts på roboten, nns därför inget gränsvärde framtaget i dagsläget. Skulle man löst det så att error inte hade subtraherats när en sändning lyckats skulle varje motor anses som defekt efter en viss tid. Strömförbrukningen på varje motor är också något som kontrolleras, men är i detta fall speciell. På roboten används motorernas strömförbrukning både till att veta när en led benner sig i ett ändläge eller vid eventuell kollision, då en ökad strömförbrukning är en eekt av det. Men också till att upptäcka om något är fel med motorn.

Om problemet orsakas av att leden står i ändläget bör felet hanteras på ett an-nat sätt, då det ju egentligen inte är ett fel. För att skilja på dessa är en lösning att med hjälp av motorpositionen kontrollera om leden benner sig i närheten av ändläget, då man kan anta att om motorerna då får en ökad strömförbrukn-ing, betyder det att leden med största sannolikhet står i ändläget. På det sättet kan felen separeras och hanteras korrekt.

Blir temperaturen på korten för hög är det något som inte står rätt till, al-ternativt är temperaturen där roboten arbetar för hög. I båda fall måste felet hanteras för att inte riskera att motorstyrkort eller motorer blir defekta på grund av överhettning.

Kapitel 4

Resultat

4.1 Resultat

Clever Cleaner kan med det framtagna styrsystemet kontrolleras med manuell styrning, där armen kontrolleras genom att styra varje motor för sig. Skillnaden mot det gamla styrsystemet är att man vid manuell styrning kan styra alla mo-torer samtidigt. Momo-torerna styrs med hjälp av de nyframtagna motorstyrkorten. Clever Cleaner kan också styras med hjälp av invers kinematik vilket beskrivs mer utförligt nedan.

Styrsystemet körs på ett inbyggt system där en patchad version av Linux an-vänds som realtids-operativsystem, och som kommunicerar med motorstyrko-rten via en EIA-485 serieport.

Styrprogrammet kommunicerar hela tiden med motorerna för att se till att de senaste värdena från motorerna alltid nns tillgängligt. Anledningen till det-ta är förutom att beräkningarna av inversa kinematiken blir mer korrekt ju mer aktuella värden som används, också att så snabbt som möjligt upptäcka om temperatur eller strömförbrukning är utanför de rekommenderade värdena. Styrsystemet har en enkel felhantering för att hantera temperatur och ström-förbrukning, men också hur kommunikationen mot motorstyrkortet fungerar.

Ytgenererare

En funktion för att efter inmatning av tre eller fyra punkter skapa ett rörelsemön-ster för att tvätta ytan punkterna spänner upp nns och är testad med gott resultat i simulatorn.

Inspelning och Uppspelning

En enkel version av inspelning av armrörelsen nns implementerad i styrpro-grammet. Inspelningen spelar in alla PWM som skickas till motorerna med en frekvens av 10 gånger/sekund. För tillfället kan endast ett program spelas in åt gången.

Applicera kinematik lösningen på Clever Cleaner

Eftersom Clever Cleaner har 4 leder som ska styras med hjälp av invers kine-matik och led 3 påverkar slutpunkten P i samma plan som led2 och led4, så

skulle en direkt applicering av ovanstående lösning ge oändligt många lösningar av ekvationen.

En enkel och fungerande lösning på problemet är att räkna ut led3:s rörelse före

och sedan med hänsyn till denna rörelse räkna ut led1 led2 och led4 rörelser

med metoden som presenterades ovan.

Att räkna ut led3 rörelse görs enklast med led3 rörelsevektor v!3 som fås av

derivatan för P med avseende på l3och vektorn!r mellan P och målpunkten G.

Genom att normalisera dessa och sedan ta skalärprodukten ges ett tal mellan 0 och 1. Där 0 betyder att led3 rörelse inte skulle göra någon påverkan till att

P skulle komma närmare G. Om det skulle bli 1 betyder det istället att led3

rörelse ger en direkt föryttning för P mot G. Sedan multipliceras talet med en längdenhet.

Figur 4.1: Beräkna L3:s rörelse

Som bilden till vänster i gur 12.8 visar så blir skalärprodukten 1 då rikt-ningsvektorerna har samma riktning. På bilden till höger blir skalärprodukten 0 då riktningsvektorerna istället har en 90_{vinkel mellan sig.}

l3= ! G P jG P j  ! dP 3 l

När denna beräkning är klar så beräknas rotationerna för resterande leder. Först beräknas derivatan fram. Derivatan är beräknad i förhand och är därefter en konstant formel i koden.

dPx

1 = (sin 1 cos 2  sin 4+ sin 1 sin 2  cos 4)  len3 + sin 1 sin 2 

armP os(2) + sin 1 sin 2  len1 dPy

1 = 0

dPz

1 = (cos 1 cos 2 sin 4+ cos 1 sin 2 cos 4)  len3 + cos 1 sin 2

armP os(2) + cos 1 sin 2 len1

Derivatan för P med avseende på 2: dPx

2 = (cos 1 sin 2 sin 4 cos 1 cos 2 cos 4)  len3 cos 1 cos 2

armP os(2) cos 1 cos 2 len1 dPy

2 = ( cos 2 sin 4 sin 2 cos 4)  len3 sin 2 armP os(2) sin 2 len1

dPz

2 = ( sin 1 sin 2 sin 4+ sin 1 cos 2 cos 4)  len3 + sin 1 cos 2

armP os(2) + sin 1 cos 2 len1

Derivatan för P med avseende på 3: dPx 2 = cos 1 sin 2 dPy 2 = cos 2 dPz 2 = sin 1 sin 2;

Derivatan för P med avseende på 4: dPx

2 = ( cos 1 cos 2 cos 4+ cos 1 sin 2 sin 4)  len3

dPy

2 = ( sin 2 cos 4 cos 2 sin 4)  len3;

dPz

2 = (sin 1 cos 2 cos 4 sin 1 sin 2 sin 4)  len3

Riktningsvektorn !r mellan P och G beräknas, normaliseras och multipliceras med en längenhet. Men här måste också rörelsen som led3 gjorde dras av från

!

r . Annars skulle P förytta sig mer än en längdenhet åt gången, vilket skulle resultera i att armen skulle röra sig snabbare i vissa situationer.

!

r = !G !P !G !P  l

2 6 4 dPx 1 dPx 2 dPx 4 dPy 1 dPy 2 dPy 4 dPz 1 dPz 2 dPz 4 3 7 5 2 6 4 1 2 4 3 7 5 = 2 6 4 rx ry rz 3 7 5

ekvationssystemet löses med determinanter enlig Cramers regel 1=det_det1 D 2= det2 detD 4= det4 detD

Denna lösning fungerade bra i tester mot simulatorn. Men det märktes även att när l3beräknades och skalärprodukten närmade sig 1 så gjorde led3så gott

som hela rörelsen och dom andra lederna stod still. Detta resulterade i att det i många fall bara var led3som gjorde hela jobbet.

Ett sätt att förhindra att detta sker, skulle en enkel lösning vara att använda en faktor med ett värde mellan 0..1 som multipliceras med l3. Om till exempel

l3multipliceras med 0.6 så skulle led3 maximalt kunna utföra 60 % av rörelsen

och resterande rörelse skulle led2och led4stå för. Tester i simulatorn visade att

denna faktor gjorde att rörelserna blev bättre.

Detta är ändå en ganska naiv lösning då den inte tar hänsyn till de situation-er då led3 verkligen borde användas till 100 %. En annan lösning kan istället

vara att låta denna faktor vara variabel. Till exempel på ett sådant sätt att användaren kan ställa in värdet på den via en kontroll. En annan mer praktisk lösning söktes för att få led3 att röra sig mer när det behövs men inte fullt ut

när de andra lederna kan göra jobbet. En situation när det vore önskvärt att led3skull användas till 100 % är när led2och led4ligger parallellt och G ligger i

samma linje. Då kan inte led2och led4rotera så P kommer närmare G. Se Figur

4.2 Även här skulle skalärprodukten kunna ge lösningen. Först måste ledernas

Figur 4.2: Led3 måste användas för att P ska närma sig G

!

led_!2 för led2 gör med punkten för nod1 och punkten nod2. Riktningsvektorn

led4 för led4 gör med punkten för nod3 och punkten P . Positionen för dessa

noder beräknas med direkt kinematik. Skalärprodukten för dessa riktningar ger vinkelförhållandet mellan led2 och led4. Är skalärprodukten 1 så är ligger

led-erna parallellt och led3 måste ge fullt utslag.

Formeln för l3 blir därför: l3= ! nod2 nod1 jnod2 nod1j ! P nod3 jP nod3j ! G P jG P j  ! dP 3 l

Denna lösning för invers kinematik fungerar i de esta fall, men när dP! 2 har

samma rikting som dP!

4 och G ligger vinkelrät mot dessa så kommer ekvationen

för att räkna ut 2 och 4 resultera i oändlighet. Se Figur 12.10

Figur 4.3: Problem vi ekv.lösningen

För att undvika detta görs en kontroll innan  vinklarna beräknas med ek-vationssystemet. Denna koll görs genom att beräkna skalärprodukten mellan led2och led4. Om skalärprodukten är större än 0.95 så utförs inte ovanstående

lösning med att först beräkna l3 och sedan beräkna 1; 2 och 4med Cramers

regel, utan2lämnas utanför och en lösning med 1 l3 och 4sökes istället med

ekvationssystemet.

Begränsningar i rörelser

Clever Cleaner har begränsningar i hur mycket vissa leder kan röra sig. led3

har exempelvis begränsningen att längden på leden är 1700 mm. Dessa begrän-sningar måste tas hänsyn till i beräkningarna.

Efter ekvationslösningen så görs en kontroll om appliceringen av den nya vinkeln till 2 resulterar i att dess vinkel hamnar utanför sina gränser. Om så är fallet

används inte de framräknade värdena utan en ny ekvation löses där inte led2är

en enkel kontroll innan värdet appliceras till leden. Om värdet som appliceras resulterar i att leden hamnar utanför sina gränser sätts inte värdet till leden.

4.2 Framtida arbete

Saknaden av en robot under arbetet har resulterat i att det inte har fun-nits någon möjlighet att testa inversa kinematiken annat än i simulerad miljö. Genomgående och längre tester med motorerna och styrsystemet monterat på roboten måste givetvis genomföras för att säkerställa att funktionaliteten är densamma under olika förutsättningar och att systemet är stabilt även under längre körningar.

Motorernas utväxling kommer att göra att en faktor måste beräknas för var-je arm. Denna faktor ska multipliceras med resultatet från den inversa kine-matiken.

Motorstyrkorten måste uppgraderas så det har en inbyggd felkontroll om kom-munikationen mellan motorstyrkorten och datorn av någon anledning skulle bry-tas. Utan den funktionaliteten nns det en risk att motorerna fortsätter köra med den hastighet dom mottog precis innan det att kommunikationen bröts. Felhanteringen bör förbättras för att bättre kunna hantera det olika fel som kan uppstå på roboten.

Då en enkel industriskärm/display med en knappsats ska sitta på roboten för att styra dess funktionalitet måste en passande sådan hittas och sedan imple-menteras i styrprogrammet. Paketet Screen har därför utelämnats i detta arbete. Inspelning och uppspelning bör skrivas om så att positionen för varje led sparas istället för PWM-värdet. Detta för att en korrigering av armens position, då rörelsen av någon anledning inte stämmer överens med den som spelades in, ska vara möjlig.

Struktur och funktioner för att spela in och skapa kompletta tvättprogram bestående av era mindre program måste implementeras för att få den funk-tionalitet som roboten har idag.

4.3 Slutsatser

Arbetet med att utveckla styrsystemet till Clever Cleaner har varit intressant men framförallt givande. Vi har under arbetets gång blivit tvugna att använda tidigare kunskaper för att lösa det problem vi stött på.

Målsättningen har från början varit att uppnå målen med examensarbetet och att styrprogrammet blir såpass bra, att fördelarna och den stora potentialen som nns i det nya sättet att kontrollera roboten leder till ett fortsatt arbete och en utveckling av systemet. Både styrning med hjälp av Invers Kinematik och ytgenereraren anser vi vara tidsbesparande och funktionalitet som bör förenkla arbetet för operatören.

Litteraturförteckning

[1] Invers kinematik, http://www.cs.umu.se/kurser/TDBD12/VT04/lectures/guranimering.pdf [2] Cyclic-Coordinate Descent, http://billbaxter.com/courses/290/html/img0.htm

[3] Real-Time Linux Wiki, http://rt.wiki.kernel.org/index.php/

[4] Real-Time Linux - High resolution timers,

http://rt.wiki.kernel.org/index.php/High_resolution_timers. [5] EIA-485, http://www.rs485.com/

[6] AEWIN EM730 Embedded, http://www.aewin.com.tw/ [7] Ramsta Robotics, http://www.ramstarobotics.se/

Muntliga

[Asplund] Lars Asplund, Professor Mälardalens högskola [Sandberg] Jan Sandberg, VD Ramsta Robotics