Problemformulering Basalt set drejer projektet sig om at man skal kunne få en robot til at finde sin vej i en labyrint. Selve labyrinten er opsat af sorte og hvide streger der fører i forskellige retninger. For at kunne gennemføre banen skal robotten være i stand til at gennemføre adskillige forhindringer bla. åbne en dør, flytte en kasse og selv bestemme retning når den kommer til et vejkryds. Detaljeret løsningsmål: Siden vi ikke kender banens opstilling fra starten er det nødvendigt at programmet er fleksibel i dets struktur således at det nemt kan ændres når banen ændres. Robotten skal kunne indentificere en sort linie og følge efter den. Dette skal den kunne gøre med stor præcision så den ikke ryger ud af kurs. Den skal kunne finde en kasse, skubbe den væk og køre forbi. finde afstanden til en kasse. følge en hvid linie. Køre langs en væg, finde en åbning i vægen, køre igennem og komme tilbage til linien. Finde garagen, åbne døren og parkere inde i garagen. SMR en skal være i stand til af finde de forskellige forhindringer og veje ved hjælp af sine liniesensorer og IR-sensorer. Liniesensorerne (8 stk.) bruges til at indentificere de forskellige linier. Det kræves for at kunne klare disse forhindringer at man laver omregninger ud fra sensordata erne fra liniesensorene således data en bliver til liniegenkendelse. Smr en finder forhindringer ved hjælp af sine IR-sensorer/afstandsfølere (3 foran og i venstre side). Det er ved hjælp af IR-sensorerne at robotten skal komme igennem døren. IR-sensorene skal desuden anvendes til at bestemme afstanden til kassen. 2 Projektet Konkret De forskellige states: De forskellige states bliver brugt til at bestemme hvor SMR en skal bevæge sig hen og hvad den skal gøre i labyrinten. STATE_INIT : Startstate der primært er indsat pga. debug muligheder. STATE_DRVx : States der håndterer kørsel mellem de forskellige forhindringer. STATE_BOXx : Håndterer kasse forhindringen, ved at skubbe kassen væk og vende inden i buret. STATE_COMP : Afslutter tilstandsmaskinen. STATE_WALLx: Kører langs en væg, finder en åbning og kører igennem den. STATE_OPENx: Åbner garagen og kører ind i den. STATE_DISTx: Måler afstanden til kassen STATE_INVA : Bruges til debug(resetter state-machine) Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side af??
3 Opgave 3.6.5 I opg 3.6.5 blev square programmet modificeret således at robotten skulle køre 2 meter frem og stoppe. De første grafer viser positionen for de forskellige hastigheder. Den første graf er med 2, 4 og 6 units. De næste grafer viser fasen for de samme hastigheder. Det ses af graferne at SMR en bevæger sig med en meget regelmæssig hastighed..8.6.4.2.2.4.6.2.4.6.8.2.4.6.8 2.6.4.2.2.4.6.8.2.4.6.8.2.4.6.8 2 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 2 af??
.6.4.2.2.4.6.8.2.4.6.8.2.4.6.8 2.7.6.5 vinkel i radianer.4.3.2. 2 3 4 5 6 7 8 9 sample # Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 3 af??
6.285 6.28 6.275 6.27 vinkel i radianer 6.265 6.26 6.255 6.25 6.245 6.24 5 5 2 25 3 35 4 45 5 sample # 6.274 6.272 6.27 6.268 vinkel i radianer 6.266 6.264 6.262 6.26 6.258 6.256 5 5 2 25 3 35 sample # 4 Opgave 3.6.6 og 7 I opg 3.6.6 blev accelerationeen begrænset til.5 m/s mens i opg 3.6.7 blev decelerationen begrænset til.5 m/s. Igen viser de første grafer positionen for de respektive hastigheder og de næste for fasen. Man kan se at kurverne gælder for både accelerationen og decellerationen. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 4 af??
Positionsplot.5.5.5.5 2 2.5 Positionsplot.8.6.4.2.2.4.6.8.5.5 2 2.5 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 5 af??
Positionsplot.8.6.4.2.2.4.6.8.5.5 2 2.5 7 Faseplot 6 5 vinkel i radianer 4 3 2 5 5 2 25 3 sample # Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 6 af??
7 Faseplot 6 5 vinkel i radianer 4 3 2 2 4 6 8 2 4 6 sample # 7 Faseplot 6 5 vinkel i radianer 4 3 2 2 4 6 8 2 sample # Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 7 af??
5 Opgave 3.6.8 I opgaven blev implementeret en vinkelregulering hvor V = K (θ start θ slut ). Den har til formål at korrigere retningen SMR en kører. De første grafer er for positionen mens de 3 sidste grafer er fasen med de forskellige hastigheder: 2, 4 og 6 units. Som man kan se på positionsgraferne ændrer robotten slet ikke retning. Positionsplot.6.4.2.2.4.6.8.2.4.6.8.2.4.6.8 2 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 8 af??
Positionsplot.6.4.2.2.4.6.2.4.6.8.2.4.6.8 Positionsplot.6.4.2.2.4.6.2.4.6.8.2.4.6.8 2 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 9 af??
7 Faseplot 6 5 vinkel i radianer 4 3 2 5 5 2 25 sample # 7 Faseplot 6 5 vinkel i radianer 4 3 2 5 5 2 25 3 35 4 45 5 sample # Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side af??
7 Faseplot 6 5 vinkel i radianer 4 3 2 5 5 2 25 3 sample # 6 Opgave 4 Der udføres en UMBmark kalibrering for at undgå afvigelser, som følge af forkert hjulafstand og forskel i højre og venstre hjuldiameter. SMR eren køres langs en bane med form som en ligesidet retvinklet firkant. Afvigelserne i x- og y- retningen noteres. Testen foretages både CW og CCW. De målte afvigelser anvendes til at beregne vinklerne α og β, der er mål for det kørte kvadrats vinkeldrejning i forhold til det ønskedes. Heraf beregnes krumningsradien af kurven mellem to af kvadradets hjørner. Til sidst kan E d (Hjuldiameter korrektionsfaktoren) og E b (Hjulafstand korrektionsfaktoren) beregnes. 7 Linjesensor Linjesensorens opgaver vil være: Give det bedste estimat på hvor stregen befinder sig, sort som hvid. Oplyse om at et sporskifte passeres og returnere passende værdi til at der kan køres til højre eller venstre. Eventuelt oplyse om en tværgående streg. Ved målinger på diodernes værdi har det vist sig at den maksimale dynamik for den enkelte diode er ca. 4 enheder fra sort til hvid papir. Den dårligste diode har en dynamik på. Støjen på dioderne under kørsel er i størrelsesordenen plus/minus 5 enheder. Derfor vil der under kørsel opstå situationer hvor en kombination af støj og dårlig streglæsning vil umuligøre en fornuftig detektion Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side af??
af stregens position, endog om der overhovedet findes en streg. Under øvelserne implementeredes detektioner baseret på henholdsvis: Den maksimale værdi der detekteres afgør stregens position. Tyngdepunktet af de målte værdier afgør stregens position. Den maksimale værdi findes og dens værdi og de to dioder der ligger nærmest afgør stregens position. Af kommentarer der kan knyttes hertil er at det ikke virker fornuftigt. Den første metode giver ofte det rigtige resultat, men pga. støj vil der til tider komme resultater der vil få robotten på afveje. Metode nummer to er meget upræcis omkring det lokale punkt hvor stregen befinder sig, men den har dog ikke tendens til at give værdier der ligger langt fra stregens position. En opløsning på plus/minus stregbredde skønnes. Den sidste metode er mere præcis lokalt, men har stadig ulempen fra metode ét som også benyttes indledende. Det skønnes at det med disse metoder og en kombination af midling og en hukommelse med gamle værdier, vil være muligt at lave noget der kan køres efter. Midling vil dog ikke være en realistisk metode i særlig stor udstrækning, da én måling tager ms. En mere sikker detektering vil være at foretrække og da stregsensoren skal virke ordentligt for overhovedet at have en ærlig chance for at gennemføre banen, forsøges endnu én metode implementeret. Stregsensoren kan betragtes som et 8 pixel kamera. I omgivelser hvor signal/støj forholdet er meget dårligt er det en fordel at vide hvad der ledes efter. Alternativet er at analysere data fra bunden som i øvelsens metoder. I vores tilfælde ved vi godt hvilket billede vi leder efter i vores kamera, billedet af en streg i en uvis position under sensoren. Det antages at der i de tilfælde hvor der skal køres efter stregen, vil stregen befinde sig i en position næsten vinkelret på sensoren(herefter kamera), og derved have en nogenlunde veldefineret bredde. Metoden er så at placere en sort streg på et hvidt stykke papir. Tage et stort antal målinger, midle over dem og gemme værdien for dem i et array på 8 variable. Vi har valgt at ønske en opløsning af stregens position i kameraet på 3 værdier. Derfor tages der 3 billeder med kameraet af stregen i 3 forskellige positioner, dvs. stregen rykket med 2cm mellem hvert billede. Et array på 8 gange 3 opnås. En ringbuffer kan implementeres, men de ms er stadig en begrænsende faktor da en ringbuffer i større grad vil midle over et stykke streg end over samme punkt på stregen. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 2 af??
2 8 6 4 2 2 2 3 4 5 6 7 8 Figur : De 3 billeder af de 8 dioder På figur ses de 3 billeder af stregen. De ligger alle i samme graf, så fortolkningen er ikke let, men det giver et indtryk af at der dog er lidt system i målingerne og at dioderne efter kalibrering er nogenlunde konsistente. Alle de her viste grafer er korrigeret til at give værdier mellem og for henholdsvis sort og hvid streg. Det er dog ikke nødvendigt for metodens virkemåde, men det giver fortolkningsmæssigt et bedre resultat. På de efterfølgende 3 figurer er billederne ved henholdsvis billede, 2 og 3 pillet ud. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 3 af??
2 9 8 7 6 5 4 3 2 2 3 4 5 6 7 8 Figur 2: Billede 9 8 7 6 5 4 3 2 2 3 4 5 6 7 8 Figur 3: Billede 2 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 4 af??
2 8 6 4 2 2 3 4 5 6 7 8 Figur 4: Billede 3 2 8 6 4 2 2 5 5 2 25 3 35 Figur 5: Karakteristik for den enkelte diode midlet over målinger På figur 5 er det målte array plottet på den anden led og giver herved karakteristikken for hver diode. På figuren ses at der trods midlingen stadig er Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 5 af??
en del uregelmæssigheder. 2 8 6 4 2 2 4 3 2 2 3 4 5 6 7 8 Figur 6: karakteristik for den enkelte diode i 2 dimensionalt array På figur 6 ses det samme array, men plottet i 2 dimensioner. Metoden virker på det her punkt meget overbevisende, dog er den ønskede opløsning på 3 positioner ikke helt opnået da detektoren springer over nogle værdier. De opnåede værdier kan sammenlignes med test graferne senere i rapporten, der her yderligere er filtreret som beskrevet i næste afsnit. 7. Filtrering af kalibreringsarray For at opnå en yderligere forbedring ønskes noget af støjen på det målte kalibreringsarray fjernet. En almindelig lavpasfiltrering foretages af signalet. På figur 5 og 6 ses den støj der ønskes filtreret væk. Først forsøges en filtrering af selve diodernes karakteristik, dvs. over de 3 målte værdier og dernæst afprøves det om en yderligere filtrering over billedet, de 8 dioder, yderligere kan hjælpe. Filtreringen foretages i fourier domænet da det her er lettere at se hvor kraftig en filtrering der er tilrådelig. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 6 af??
25 2 5 5 4 3 2 2 3 4 5 6 7 8 Figur 7: Det 2 dimensionale array fourier transformeret På figur 7 ses kalibreringsarrayet fourier transformeret. Det skønnes at filteret skal have et pass-band fra ca. til 2. På næste figur ses det udarbejdede filter..8.6.4.2 4 3 2 2 3 4 5 6 7 8 Figur 8: Filteret der ønskes filtreret med over karakteristikken for dioderne Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 7 af??
25 2 5 5 4 3 2 2 3 4 5 6 7 8 Figur 9: Efter filtreringen i frekvensdomænet På figur 9 ses det filtrerede array, stadig i fourier domænet. 2 8 6 4 2 5 5 2 25 3 35 Figur : Karakteristikken af den enkelte diode efter filtrering Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 8 af??
2 8 6 4 2 4 3 2 2 3 4 5 6 7 8 Figur : Det 2 dimensionale array efter filtreringen På figur og ses karakteristikken efter filtreringen. Her er kurverne pæne og støjen undertrykt. Hjørnerne på de to dioder yderst har dog fået et knæk nedad som ikke helt kan forsvares af de fysiske målinger. De rettes derfor op manuelt. 8 Test af metode Efterfølgende er alle grafer der viser resultatet af korrelationen mellem en måling på sort streg på gulvet i forskellige situationer og data i arrayet beskrevet i foregående afsnit. Målingen på sensoren er ikke midlet endnu. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 9 af??
8 6 4 2 8 6 4 2 5 5 2 25 3 35 Figur 2: Sort streg i midten 8 6 4 2 8 6 4 2 5 5 2 25 3 35 Figur 3: Sort streg i midten Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 2 af??
3 25 2 5 5 5 5 2 25 3 35 Figur 4: Sort streg til venstre 3 25 2 5 5 5 5 2 25 3 35 Figur 5: Sort streg til venstre Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 2 af??
3 25 2 5 5 5 5 2 25 3 35 Figur 6: Sort streg til højre 3 25 2 5 5 5 5 2 25 3 35 Figur 7: Sort streg til højre Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 22 af??
22 2 8 6 4 2 8 6 4 2 5 5 2 25 3 35 Figur 8: På vej ind i stregforgrening 25 2 5 5 5 5 2 25 3 35 Figur 9: På vej ind i stregforgrening 2 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 23 af??
22 2 8 6 4 2 8 6 4 2 5 5 2 25 3 35 Figur 2: På vej ind i stregforgrening 3. Det bemærkes at den maksimale værdi er cirka konstant mens bredden forøges. 3 25 2 5 5 5 5 2 25 3 35 Figur 2: På vej ind i stregforgrening 4 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 24 af??
24 22 2 8 6 4 2 8 6 4 5 5 2 25 3 35 Figur 22: På vej ind i stregforgrening 5 8 6 4 2 8 6 4 2 5 5 2 25 3 35 Figur 23: På vej ind i stregforgrening 6. Her er forgreningen næsten blevet til 2 streger som det anes på grafen. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 25 af??
9 8 7 6 5 4 3 2 5 5 2 25 3 35 Figur 24: På vej ind i stregforgrening 7. Her er forgreningen blevet til 2 streger, hvilket tydligt ses på grafen. 22 2 8 6 4 2 8 6 4 5 5 2 25 3 35 Figur 25: På vej ind i stregforgrening 8 Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 26 af??
7 6 5 4 3 2 5 5 2 25 3 35 Figur 26: Måling kun på sort baggrund. Det bemærkes at den maksimale værdi ligger langt under, <, af hvad en normal måling giver, > 2. 8 7 6 5 4 3 2 5 5 2 25 3 35 Figur 27: Kun sort igen Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 27 af??
65 6 55 5 45 4 35 3 25 2 5 5 5 2 25 3 35 Figur 28: Måling kun på gulv. Igen bemærkes den maksimale værdi ligger under. 8 7 6 5 4 3 2 5 5 2 25 3 35 Figur 29: Kun gulv igen Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 28 af??
Som det ses er signal/støj forholdet hævet betydeligt og det er ikke længere noget problem at finde stregen selv ud fra en toppunktsbestemmelse. Stregforgreningerne har den store fordel blot at vise ét toppunkt for hver streg og bredden af stregen influerer blot på bredden af figuren. 9 IR-sensor SMR eren er udstyret med fire IR-sensorer, tre på fronten og en på venstre side. Sensorene anvendes til at måle afstanden til objekter. Målingerne foretages ved at sende infrarødt lys fra en lysdiode. Lyset reflekteres af objektet og detekteres af PSD en(position Sensetive Device). Det reflekterede lys rammer PSD en i en vinkel, der er afhængig af afstanden til objektet. Da afstanden mellem lysdiode og PSD en er kendt kan afstanden til objektet nu findes vha. en trigonometrisk beregning. Denne metode har dog en væsentlig ulempe, nemlig at sensoren ikke umiddelbart fungere når objektet er meget tæt på og når objektet reflekterer lysstrålen i en meget stumb vinkel, reflekteres den over PSD enes lysfølsomme område. PSD en opfanger istedet nogle af de afbøjede lysstråler og sensoren tror der er stor afstand til objektet. Bjarke S. Frøsig s266 Brian Frølich Christiansen s97626 Jesper Rubech Rasmussen s997 Morten Møller s234 Side 29 af??