Kenttä- ja palvelurobotiikan harjoitustyö Teemu Kemppainen, Heikki Hyyti, Olli Sjöberg, Sami Salonen, Timo Schwarte, Lotta Kannari 15.1.2011
Sisältö 1 Johdanto 4 2 Ohjelmiston rakenne 5 3 Robotin arkkitehtuuri 6 4 Mittaustieto ja robotin aistit 8 4.1 Kamera.............................. 9 4.2 Odometria............................. 9 4.3 Laserskanneri........................... 9 4.4 Turvallinen alue robotin ympärillä................ 10 5 Konenäkö 11 5.1 Kameran kalibrointi........................ 11 5.2 Objektien sijainnin määritys................... 13 5.3 Tunnistusalgoritmi........................ 13 5.4 Opettaminen........................... 15 5.5 Oppiminen ja tunnistaminen................... 16 5.6 Testaus ja suorituskyky...................... 18 6 Reeksit 18 6.1 Pysäytysreeksi.......................... 18 6.2 Kääntymisreeksi......................... 19 6.3 Nopeuden rajoittaminen toteuttamiskelpoiseksi........ 19 7 Paikannus 20 7.1 laserpaikannus........................... 20 7.2 Paikan globaali haku kartalta.................. 22 7.3 Robotin paikan oikeellisuuden valvonta............. 23 7.4 Paikannuksen sensorifuusio EKF:llä............... 24 7.5 Viiveen kompensointi ja laitteen paikan prediktointi...... 25 8 Navigointi 25 8.1 Reitinsuunnittelu......................... 25 8.2 Reitinseuraaja........................... 26 8.3 Reeksiivinen PD-nopeussäädin................. 28 9 Tehtävän suunnittelu 30 9.1 Roskien etsintä.......................... 30 9.2 Roskan hakeminen........................ 31 2
9.3 Roskan vieminen roskakoriin................... 31 10 Matlabin simulaattori 32 10.1 Dynamiikkasimulaattori..................... 33 10.2 Lasermittauksen simulaattori.................. 33 10.3 Törmäyksenhavaitsija ja puskurien simulointi.......... 33 10.4 Toimielimen simulointi...................... 34 11 Käyttöliittymä 34 11.1 Uusi ja vanha käyttöliittymä................... 34 11.2 Kommunikointi pääohjelman kanssa............... 35 11.3 Toiminnallisuus.......................... 35 12 Toimielin 38 13 Laitteiston toimivuus ja haasteet 41 13.1 Moottorinohjaimen viive..................... 41 13.2 Laskenta-algoritmien raskaus................... 42 13.3 Kameran liikkuminen ja kameran kalibroinnin epätarkkuus.. 42 13.4 IO-kortin ongelmat........................ 42 14 Yhteenveto 44 Viitteet 46 3
1 Johdanto Projektissa tehtävänä oli suunnitella ja toteuttaa robotti, joka kerää tunnetusta huoneesta roskat ja kuljettaa sekä lajittelee ne roskakoreihin. Robotin tulee pystyä toimimaan osittain itsenäisesti, kuitenkin operaattorin auttaessa ongelmatilanteiden ja vaikeampien tehtävien yli. Robotin ohjaus ei saa kuormittaa operaattoria liikaa, vaan operaattorin tulisi pystyä ohjaamaan laitetta muiden töidensä ohella. Robotin fyysinen toteutus, laitteiden ajurit sekä valmis rajapinta robotin ohjaamiseen esimerkkikoodeineen oli kurssilla valmiina. J2B2-robotteja oli kuitenkin vain yksi, ja se tuli jakaa kaikkien kilpailevien ryhmien kesken. J2B2-robotti on kuvassa 1 keräämässä roskia areenalta. Projekti tehtiin kuuden hengen ryhmätyönä. Ryhmän tuli itse suunnitella ja toteuttaa roskien poimimiseen soveltuva työväline - toimilaite, jota käyttäen J2B2-robotti voisi kerätä ja lajitella roskia. Työvälineen liittäminen J2B2-robottiin ja sen ohjaus robotin järjestelmien avulla tuli itse toteuttaa. Projektissa tuli myös suunnitella ja toteuttaa koko ohjelmallinen robottia ohjaava järjestelmä, joka annettujen komentojen ja robottiin asennettujen mittalaitteiden avulla pystyisi selviytymään tehtävästä. Tässä esitetty ratkaisu täyttää kaikki projektityön vaatimukset, mutta sen lisäksi pyrkii mahdollisimman autonomiseen suoritukseen. Ryhmämme lähti jo suunnittelupöydällä kehittämään täysin autonomista laitetta, joka pystyy toimimaan itsenäisesti annetussa työtehtävässä. Ehdimme toteuttaa suurimman osan suunnitelmista, ja lopputuloksena onkin kokolailla autonomisesti toimiva laite, jonka toimintaa käyttäjä pystyy käyttöliittymän avulla valvomaan ja tarkentamaan niin halutessaan. Laitteeseen toteutettiin jonkin verran turvaominaisuuksia, joiden avulla se esimerkiksi välttää seiniin törmäämisen. Pahimmillaan täysin autonomisesti toimiva robotti jää jumiin, eikä selviä ilman käyttäjän apua. Dokumentissa kuvataan tehdyn työn laajuuden takia käytetyt algoritmit vain pintapuoleisesti selittäen niiden idea ja yksinkertaistettu toiminnallisuus. Annettujen tietojen pohjalta on mahdollista toteuttaa samalla toimintaperiaatteella toimiva robotti, mutta algoritmit eivät esitetyssä laajuudessaan toimi täydellisesti. Monia käytettyjä algoritmeja on hierottu ja modi- oitu heuristisesti juuri tähän tarkoitukseen sopiviksi. Näitä hienosäätöjä on jätetty dokumentaatiosta pois, jotta järjestelmän toiminta olisi helpommin ymmärrettävissä. Tällä tavoin esitettynä projektia varten kehitettyjä ideoita ja algoritmeja on helpompi käyttää muissa vastaavissa käyttötarkoituksissa. 4
Kuva 1: J2B2-robotti työn toimessa kilpailuareenalla 2 Ohjelmiston rakenne Robotin ohjelmisto on suunniteltu ja toteutettu mahdollisimman modulaarisesti. Toteutetun ohjelmiston yleinen rakenne, lohkojen riippuvuudet toisistaan, ja käytettyjen ohjelmointikielten ja ohjelmistojen jaottelu on esitetty kuvassa 2. Keskeisin luokka on J2B2, joka toimii keskusyksikkönä muille luokille. Sensorit ovat jaettu omaan luokkaansa ja kukin sensorikysely on jaettu omaan säikeeseensä. Myös konenäkö on omassa luokassaan ja säikeessään. Rakenteen ideana on, että sensorit (myös konenäkö) kyselevät fyysiseltä robotilta mittauksia sopivin väliajoin ja J2B2 (luokka) lukee uusimmat sensoritiedot eri sensoriluokilta aina säätöluuppinsa alussa. Säätöluuppi etenee karkeasti seuraavanlaisesti: 1. Lue sensorit 2. Lue konenäön roskatiedot 3. Aja Simulink-malli 5
4. Lähetä tiedot käyttöliittymälle 5. Kirjoita säädöt fyysiselle robotille 6. Kirjoita kaikki data logiin Ohjelman rakenne on tehty siten, että kriittinen säätöluuppi ei koskaan jää odottamaan muita luokkia. Kaikki kriittiset osat ovat myös asianmukaisesti suojattu mutexeilla. Konenäkökoodin rakenne on myös tehty modulaariseksi. Jokainen yksittäinen kuvankäsittelyosa on jaettu omaan luokkaansa ja luokkia yhdistellään "johdoilla"ikään kuin Simulink-malleissa. Kuvankäsittelyluokkien käyttö on erillisissä "skripteissä"(header), tässä tapauksessa esikäsittely ja piirreerottelu ovat erillisissä skripteissään. Konenäön oleellisin luokka on TrashClassier, joka omassa pääluupissaan ensin esikäsittelee kuvan, erottelee piirteet ja luokittelee karkeasti, projisoi kuvakoordinaatit maailman koordinaatistoon ja päivittää sisältämänsä TrashTrackerin tilan ja luokittelee lopullisesti objektit. TrashClassier toimii sensorimaisesti siten, että siltä voi milloin tahansa kysellä uusimmat roskatiedot. 3 Robotin arkkitehtuuri Robotin arkkitehtuuri perustuu eräänlaiseen hybridiarkkitehtuuriin, jossa hierarkista ja reaktiivista arkkitehtuuria on yhdistetty. Arkkitehtuuri koostuu erillisestä reaktiivisesta osasta ja sen päällä toimivasta suunnittelu- ja ohjausosasta. Perinteisen hybridiarkkitehtuurin reaktiivista tasoa [3] on laajennettu laskentaintensiivisemmäksi, niin että melko paljon laskentaa tehdään automaattisesti joka syklillä. Tässä reaktiiviseen tasoon voidaan laskea kuuluvaksi mittausdatan käsittely, konenäkö, laserpaikannus, turvallisen alueen estimointi, sensorifuusio, viiveen prediktointi, reeksiivinen nopeussäädin ja törmäyksen estävä nopeusrajoitin. Reaktiivisen tason toimilohkot on esitetty kuvassa 3 vihreällä taustavärillä. Kuvassa ruskean katkoviivan yläpuolinen osa koostuu käyttöliittymästä, tilakoneesta, toimilaiteohjaimesta, reitinsuunnittelijasta ja reitinseuraajasta. Nämä lohkot suunnittelevat ja toteuttavat ohjauskäskyjä ja ohjearvoja, joiden avulla reaktiivista alempaa tasoa ohjataan. Robotin toiminta voidaan kiteyttää kuvan 3 toimikaavion avulla. Mittaustieto kaikista sensoreista esikäsitellään ja puskuroidaan ensin vasemman 6
Kuva 2: Ohjelmiston rakenne alanurkan lohkossa. Koska osasta mittauksista mittaustietoa ei ole saatavilla joka 20Hz laskentasykli, käytetään laskentaan tällöin edellistä saatavilla olevaa mittaustietoa, mutta niin että sen laatuarvoa sensorifuusiota varten lasketaan ajan funktiona. Laskennan nopeuttamiseksi laserpaikannus tehdään kuitenkin vain kuitenkin 10Hz laskentasyklissä, koska mittaustietoa ei tämän tiheämmin ole saatavilla. Laserpaikannuksen ja turvallisen alueen estimoinnin laskema tieto, kuten kaikki muukin mittaustieto ja edellinen asetettu nopeusarvo viedään sensorifuusio ja prediktointi -lohkolle. Tässä lohkossa tapahtuu kaikki eri mittaustietojen yhdistäminen ja näiden tietojen avulla viiveen prediktointi. Laitteen paikka pitää ennustaa viiveen verran eteenpäin sen antaman ohjauksen ja sisäisen mallin avulla, jotta laitetta voitaisiin tehokkaasti ohjata viiveellisessä järjestelmässä. J2B2-robotin tehollinen viive mitattiin vajaan sekunnin mittaiseksi. Tehollisella viiveellä tarkoitetaan todellista käsketyn ja toteutuneen robotin nopeuden välistä aikaeroa. Reeksiivinen säädin toimii tällöin täysin ennustetussa tasossa, ja näin ennusteen yleensä toimiessa hyvin, säädin pystyi säätämään mukautettua PD-säädintä käyttäen robotin nopeusarvoja. PD-säätimessä oli normaalin 7
Kuva 3: Robotin arkkitehtuuri ja toimikaavio. erosignaalin vahvistuksen ja sen derivaatan lisäksi vapaan tilan mukaan laskettu kulmanopeuteen vaikuttava termi, jolla säädin reeksinomaisesti väistää lähelle tulevia seiniä ja muita esteitä kääntymällä niistä poispäin. Tämän jälkeen ohjausnopeudet menevät rajoittimelle, joka katsoo, että edessä ei ole seiniä liian lähellä. Rajoitin laskee nopeutta jos ollaan päätymässä liian lähelle seinää. Rajoitin pysäyttää laitteen muutamaa senttimetriä ennen sen osumista seinään. Myös rajoitin toimii prediktoidulla paikkatiedolla, ja näin viiveestä tai ohjauksesta johtuen laite ei pääse ajamaan seinään tai esteisiin. Aivan viimeiseksi rajoitin tarkistaa, että annettava ohjaussignaali on fysikaalisesti toteutettavissa. Tämä tehdään vertaamalla annettavaa ohjausta nopeus ja kiihtyvyys rajoituksiin. Lopulta rajoitettu ja säädetty nopeustieto annetaan ulos ohjauksena. 4 Mittaustieto ja robotin aistit Projektityössä käytettiin J2B2-robotin sensoreista kameraa, renkaiden nopeustietoa, laserskanneria ja puskureita. Puskureita käytettiin alkuvaiheessa havaitsemaan tapahtuneet törmäykset, mutta loppuvaiheessa törmäyksiä ei enää tapahtunut joten sensorit jäivät tarpeettomiksi. Kaikkien sensorien mittaustiedolle rakennettiin puskurointijärjestelmä, joka käytti edellistä saatua mittaustulosta ja laski saatujen mittausten laatua ajan kuluessa. Tällöin van- 8
hentuneen mittauksen laatu lähestyi nollaa ja sitä ei pian otettu huomioon sensorifuusiossa ja robotin toiminnassa. Tämä kuitenkin mahdollisti robotin toiminnan silloinkin, jos jokin mittaustieto jäi langattoman verkon häiriöiden takia saamatta ajoissa. 4.1 Kamera J2B2-robotissa on kaksi webkameraa sensorina. Toinen on kytketty kääntöpäähän robotin laserskannerin yläpuolelle ja toinen on vapaasti kiinnitettävissä minne vain. Tehtävässä käytettiin vain kääntöpäähän kiinnitettyä kameraa. Kameran kalibroinnin (5.1) helpottamiseksi kääntöpään liike rajoitettiin ohjelmallisesti toimimaan vain pystysuunnassa. Kamera kuvaa 10 kuvaa sekunnissa, mikä oli käytetylle konenäköalgoritmille aivan tarpeeksi suuri määrä. Todellisuudessa vain murto-osa kamerasta saaduista kuvista ehdittiin käsittelemään (5). 4.2 Odometria J2B2-robotin kummankin renkaan kulmanopeutta mitataan inkrementtienkooderilla, tästä MaCI-rajapinnassa lasketaan valmiiksi paikka ja suuntakulma [11]. Nykyisin rajapinnasta on saatavilla myös nopeus ja kulmanopeusarvot. Valmiiksi laskettua paikkaa ja suuntakulma on vaikea käyttää sensorifuusiossa apuna, koska jos oma estimaatti suuntakulmasta eroaa odometrian suuntakulmasta, integroi odometria x ja y arvoja toteutuneista renkaiden pyörimisnopeuksista väärän kulman suhteen, jolloin x ja y sekoittuvat toisiinsa epälineaarisesti. Tällöin kunnollisen sensorifuusion tekemiseen ainoa vaihtoehto on laskea annetusta paikasta derivaatat kullakin hetkellä ja tätä kautta renkaiden nopeusarvot (tai tätä kautta robotin nopeus ja kulmanopeus). Onneksi robotista kuitenkin saatiin Antti Maulan ja Matthieu Myrskyn avustuksella ulos nopeuden ja kulmanopeuden arvot ja robotin paikan sensorifuusion ongelma helpottui ja tarkentui huomattavasti. Robotin nopeus- ja kulmanopeusmittausten perusteella tehty odometria on suhteellisen tarkka. Siihen kuitenkin kertyy koko ajan virhettä, joten mittaustietoa pitää yhdistää lasermittauksen avulla laskettuun paikkaan (7.1) sensorifuusiolla (7.4). 4.3 Laserskanneri J2B2-robotissa on SICK LMS-200 laserskanneri [9]. Se mittaa asteen välein etäisyysmittauksia -90 asteesta +90 asteeseen, yhteensä 181 mittausta. Mittaukset kerätään kymmenen kertaa sekunnissa sarjaväylän yli J2B2-9
robottiin. Lasermittalaite on robotille elintärkeä aisti. Se käyttää mittalaitetta etupäässä navigointiin laserpaikannuksen (7.1) avulla. Robotti osaa myös väistää laserskannerissa näkyviä esteitä navigoidessaan. Tämä on toteutettu turvallisen alueen avulla (4.4). 4.4 Turvallinen alue robotin ympärillä Robotin ympärille lasketaan turvallinen alue. Se mallinnetaan 90 etäisyytenä 4 asteen välein. 4 asteen väli valittiin, koska se on riittävän tarkka, mutta silti laskennallisesti riittävän nopea. Turvallisen alueen yksi etäisyys lasketaan kuhunkin kulmaan niin, että se vastaa robotin säteistä (20cm) ympyrää, joka koskettaa lähintä estettä. Tällöin tässä lasketussa suunnassa robotti mahtuu turvallisesti jokaiseen pisteeseen oman keskipisteensä ja mitatun säteen välillä. Laskettavina esteinä käytetään laserskannerin mittaustietoa, kartan seiniä sekä kamerakuvasta nähtyjä esteitä ja roskia. Koska turvallisen etäisyyden laskeminen lähimpään kohteeseen robotin säteen turvamarginaalilla ei ole aivan triviaalia, avataan algoritmia tässä hieman. Ideana on se, että ensin jokaiseen senttimetrin tarkkuudella diskretoidun kartan pisteeseen piirretään robotin säteinen ympyrä. Sen jälkeen robotista piirretään halutun säteen suuntainen suora. Suoran leikkauspisteet määritetään kaikkien leikkaavien ympyröiden kesken. Näistä valitaan robottia lähin ympyrän ja suoran leikkauspiste. Tämän jälkeen sama tehdään kaikille laserskannerin mittauspisteille ja sen jälkeen kamerakuvasta mitatuille roskille (ottaen myös huomioon roskan koon lisäämän mitan säteeseen). Käytännössä edellistä algoritmia ei voi toteuttaa suoraan noin, vaan toteutus vaatii hieman optimointia. Laskentaan olisi mennyt paljon kauemmin, kuin mitä 20Hz päivitystaajuus olisi sallinut, koska kartan seinäpisteitä oli noin 1500, laserskannerin mittauspisteitä 181 ja kameran esteitä maksimissaan 100 kappaletta. Laskentaa voidaan kuitenkin huomattavasti tehostaa ensin segmentoimalla kartasta tiettyyn rajoitettuun kulmaan osuvat seinäpisteet kuten myös laserskannerin ja kameran antamat esteiden paikat. Kaikki tämä onnistuu siististi, jos suurimman osan laskennasta tekee polaarikoordinaatistossa, jossa robotti on origossa. Lisäksi suoraa robotin säteisiin ympyröihin sovittaessa voi samalla algoritmin suorituskerralla laskea robotin etuja takapuolella olevat (180 asteen ero) lähimmät leikkauspisteet. Kuvassa 4 on esitetty loppukilpailun alussa J2B2-robotti, toimilaitteen kytkemisen jälkeen. Kuvassa näkyy karttapohjalla kolme roskaa, jotka on merkitty magentalla vinoneliöllä, laserskannerin punaiset mittauspisteet, robotti toimielimineen vihreällä viivalla ja vihreällä katkoviivalla esitettynä turvallinen alue. Se ei ole aivan täsmällisen tarkka johtuen suurehkosta 4 asteen välistä turvallisten etäisyyksien välillä. Todelliset turvallisen alueen estimoi- 10
Kuva 4: Robotin näkemä turvallinen alue vihreällä katkoviivalla. dut pisteet on esitetty vihreillä pisteillä keskelle turvallisen alueen katkoviivaa. Kuvassa nähdään myös ihmisen jalkojen heijastus laserskannerissa. Laite väistää turvallisen alueen ansiosta myös ylimääräisiä kehään tulevia esteitä, jotka näkyvät laserskannerissa. Viimeisenä vaiheena turvallisen alueen etäisyydet estimoitiin myös ennustettuun paikkaan, johon robotti vajaan sekunnin viiveen aikana sisäisen mallin ja annetun ohjauksen perusteella päätyisi. Tätä prediktoitua turvallista aluetta käytettiin robotin ohjauksessa ja turvatoiminnoissa. 5 Konenäkö 5.1 Kameran kalibrointi Robotin kamera kalibroitiin OpenCV:stä löytyvillä kalibrointialgoritmeilla. Kalibroinnilla selvitettiin kameran sisäiset parametrit sekä kameran tuotta- 11
mat kuvavääristymät, joiden avulla kuvia pystyttiin käyttämään esineiden paikantamiseen. Shakkiruutukuviosta otettiin kymmeniä kuvia eri asennoissa ja etäisyyksissä robotista. Kuvista 15 käytettiin kameran kalibrointiin. Kalibrointi tapahtuu seuraavasti: Etsitään kuvasta shakkiruutukuvio. Shakkiruutukuvion koko (ruutujen lukumääränä pysty- ja leveyssuunnassa) on määritelty. Tallennetaan jokaisen löydetyn shakkiruutukuvion ruutujen kulmapisteiden sijainnit. Suoritetaan kalibrointi kaikkien löydettyjen shakkiruutukuvioiden avulla. Kuvassa 5 nähdään robotin kameralla otettu kuva shakkiruutukuviosta, johon on merkitty algoritmin löytämä ruutukuvio. Kalibroinnin avulla saatujen kameran sisäisten parametrien ja löydettyjen kuvavääristymien avulla, robotin ottamia kuvia voidaan muuttaa sellaiseen muotoon, että niistä voidaan määrittää kuvassa olevien objektien sijaintia maailmankoordinaatistossa. Kuva 5: Kameran kalibrointi shakkiruutukuviolla 12
5.2 Objektien sijainnin määritys Objektien sijainnin määritys tapahtuu kalibroinnin avulla vääristymistä korjatusta kuvasta. Kuva oletetaan laskennassa reikäkamerakuvaksi. Objektien sijainnin määrittämistä varten tarvitaan objektien sijainti kuvassa, robotin sijainti sekä suuntaus, kameran sijainti robottiin nähden, sekä kameran kulma. Lisäksi mitattiin kameran näkökenttää. Objektien voitiin tässä tehtävässä olettaa sijaitsevan lattiatasolla, mikä helpotti sijainnin määrittämistä. Objektin sijainnista kuvassa pystyttiin edellä mainittujen tietojen avulla määrittää sijainti maailmankoordinaatistossa yksinkertaisia trigonometrisia funktioita käyttäen. Kentällä olevia roskia pystyttiin näin paikantamaan riittävän tarkasti keräämistä varten. 5.3 Tunnistusalgoritmi Roskan tunnistaminen kuvasta etenee seuraavanlaisesti. Algoritmin kohdat 1-7 ovat esikäsittelyä, kohdat 8-9 piirre-erottelua, kohta 10 karkea luokittelu ja kohdat 11-13 robusti luokittelu. 1. Canny Suodatetaan sisääntuleva harmaasävykuva canny-reunansuodattimella. Canny-reunantunnistus on pääasiassa Sobel-suodatettu kuva, josta etsitään vain maksimi arvot (non-maximum suppression) ja käytetään hystereesikynnystystä (hysteresis thresholding). 2. Integraalikuva Saadusta binäärikuvasta muodostetaan integraalikuva. Tämä nopeuttaa tietynlaisten suodattimien laskentaa huomattavasti. Esimerkiksi summan laskeminen pikselin ympäristöstä onnistuu neljällä laskutoimituksella, kun se muuten olisi n 2, jossa n on suodattimen koko. 3. Suodatus Suodatetaan reunakuva siten, että jos suodattimen alle jäävien pikselien summa on tiettyä kynnysarvoa pienempi, merkitään pikseli nollaksi, muuten ei muuteta arvoa. Tämän suodatuksen tarkoituksena on poistaa liian pienet reunat ts. kohina ja roskat. 4. Integraalikuva Muodostetaan suodatetusta kuvasta integraalikuva. 5. Blob-suodatus Ajetaan suodatetulle kuvalle uusi suodatin. Tällä kertaa jos suodattimen alle jäävien pikselien summa on suurempi kuin jokin kynnysarvo, merkitään pikseli ykköseksi, muuten nollaksi. Tässä suodatin on suurempi kuin häiriönpoisto-suodatuksessa. Tarkoituksena maalata kiinnostavien (tarpeeksi reunaa) kohteiden päälle valkoinen alue. 13
6. Labelointi Saadusta möykkykuvasta (blob image) pitää kukin yhtenäinen alue erottaa omaksi alueekseen. Tämä tehdään oodll-algoritmilla, joka etsii yhtenäiset alueet ja numeroi ne. Samalla saadaan myös alueen bounding box. 7. Bounding boxes Bounding box on minimaalinen nelikulmio, jonka sisään alue mahtuu. Saatuja nelikulmioita suodatetaan alueen pinta-alan mukaan, nelikulmion ja alueen pinta-alan suhteen mukaan ja neliömäisyyden mukaan. Tämän tarkoitus on poistaa liian pienet, hajanaiset ja pitkulaiset alueet. 8. SURF-piirteet Saatujen nelikulmioiden sisältä etsitään SURF-piirteet. Algoritmia on muokattu siten, että ei käytetä maskia opencv:n SURFalgoritmissa, vaan käytetään jaettua muistia suoraan kuvaan. Tämä nopeutti algoritmia n. 5 kertaisesti. 9. Histogrammi Muodostetaan löydetyistä piirteistä histogrammi vertaamalla piirteitä visual wordeihin. Tämä histogrammi varsinaisesti kuvaa kysesistä objektia. 10. Luokkatodennäköisyys Lasketaan kullekin luokalle (1-4) todennäköisyys viiden lähimmän naapurin mukaan. Käytetään 5-nn luokitinta ja painotetaan saatuja naapureita niiden etäisyyden mukaan ja normalisoidaan välille 0-1. Tällöin objektilla on neljä lukua, jotka kertovat sen todennäköisyyden kuulua kuhunkin neljään luokkaan. 11. Träkkäys Lasketaan objektille paikka maailmankoordinaatistossa ja verrataan sitä tietokantaan löydetyistä roskista. Jos vastaavuus löytyy, päivitetään roskan tietoja. Muuten luodaan uusi roska/muu. Vastaavuus löytyy jos uusi objekti on tarpeeksi lähellä jotain tietokannasta löytyvää objektia. 12. Tietojen päivitys Jos roska löytyy jo tietokannasta, päivitetään sen todennäköisyyksiä ja paikkaa. Todennäköisyyksissä käytetään 30 mittauksen mittaista mediaaniltteriä, josta poistetaan ltterin täyttyessä satunnainen arvo. Todennäköisyydet normalisoidaan välille 0-1. Samanlaista mediaaniltteriä käytetään myös x- ja y-koordinaatille. Träkkäyksen tarkoitus on parantaa tunnistustarkkuutta ja mediaaniltteriä käyttämällä tunnistuksesta saadaan robusti. 13. Luokittelu Objekti luokitellaan johonkin neljästä luokasta sen luokkatodennäköisyyden mukaan. 14
Kuva 6: Kuvan käsittelyn vaiheet, numero viittaa algoritmin vaiheeseen Alkuperäinen harmaasävykuva Canny-reunasuodatettu kuva (1) Suodatettu reunakuva (3) Blob-suodatettu kuva (5) Bounding boxes (7) Luokkatodennäköisyydet (10) 5.4 Opettaminen Tehtävässä robotin opettaminen aloitettiin muutamalla sadalla "liian hyvällä"kuvalla, joiden pohjalta algoritmin kehittäminen aloitettiin. Nämä kuvat 15
eivät vastanneet kovin hyvin todellisuutta, mutta mahdollistivat algoritmin kehittämisen. Kuvat olivat liian hyvälaatuisia eikä niissä ollut liikettä. Seuraavassa vaiheessa opettaminen tehtiin näillä optimaalisilla kuvilla sekä n. 4000 huonommalla kuvalla. Nämä auttoivat huomaamaan algoritmin bugeja sekä tekemään algoritmista robustimman. Lopullista opetusta varten kerättiin n. 25000 kuvaa, aikaisemmin kerättyä dataa ei käytetty lainkaan. Lopullinen data kerättiin siten, että se vastaisi mahdollisimman hyvin todellista tilannetta ts. ajettiin robotilla ympäriinsä roskien seassa. Opettaminen tapahtuu erillisellä ohjelmalla (J2B2-teach ja J2B2-realteach). Opetusohjelma kehittyi projektin edetessä. Ensimmäisessä versiossa ohjelma ei auttanut käyttäjää lainkaan, ei sisältänyt peruutusmahdollisuutta virhenäppäilyjen varalta eikä mitään muutakaan edistyksellistä. Opetusdatan kasvaessa ilmeni tarve paremmalle opetusohjelmalle. Ohjelman viimeisin versio sisältää undo-napin ilman historiarajoitusta sekä tallennusmahdollisuuden. Ohjelma avustaa käyttäjää merkkaamalla sen mielestä kiinnostavat alueet, jolloin käyttäjän tarvitsee vain kertoa alueen tyyppi. Ohjelma tuottaa tekstitiedoston, jossa kerrotaan opetuskuvan polku, kiinnostavan alueen koordinaatit ja sen tyyppi. Ohjelma käyttää ylläkuvatun algoritmin esikäsittelyvaiheita 1-7. Tallennusmahdollisuus ja undo-nappi osoittautuivat tärkeiksi, sillä n. 25000 kuvan opettamiseen kului useampi päivä useammalta käyttäjältä. Näiden ansiosta työmäärää pystyi hieman jakamaan. 5.5 Oppiminen ja tunnistaminen Opetusdatan oppiminen tapahtuu erillisellä ohjelmalla (J2B2-learn). Oppimisohjelma lukee opetusohjelman kirjoittaman tekstitiedoston ja tuottaa "codebookin". Oppiminen käyttää algoritmin kohtia 8-10. Oppiminen käyttää nk. bag of words-luokitinta SURF-piirteillä [2]. Algoritmi ei ole valmiiksi saatavilla, vaan se on kirjoitettu itse alusta asti. Algoritmi toimii myös muiden piirteiden kanssa. Bag of Words-algoritmi Algoritmin idea on se, että piirteiden välillä ei ole hierarkista yhteyttä. Idea selviää parhaiten ensin tutustumalla algoritmiin. OPETUS 1. Erotellaan piirteet (SURF-piirteet) koko opetusdatasta, ei välitetä luokkatiedosta 16
2. Klusteroidaan piirteet. Klusterin keskipiste on visual word 3. Etsitään kohdassa 1. erotelluille piirteille niitä vastaava klusterin keskipiste käyttäen lähintä naapuria. Tällöin kutakin piirrettä vastaa jokin klusterin keskipiste 4. Muodostetaan histogrammi vastaavuuksista. Huomioidaan luokkatieto. Nyt jokaista opetusdatan objektia vastaa jonkinlainen histogrammi. Tallennetaan histogrammi ja luokkatieto. 5. Vain visual wordit, histogrammit ja luokkatiedot tarvitaan tunnistamisessa Tunnistaminen tapahtuu seuraavasti (oletetaan esikäsittelyn antavan mielenkiintoinen kohde): TUNNISTAMINEN 1. Erotellaan piirteet 2. Etsitään piirteille vastaavat visual wordit 3. Muodostetaan histogrammi 4. Luokitellaan objekti histogrammiavaruudessa Algoritmikuvauksesta nähdään, että vain piirteen esiintymisellä on merkitystä objektin luokittelussa. Piirteet voivat siis objektissa olla missä tahansa, kunhan niiden lukumäärien suhde täsmää opetettuun dataan. Koska piirteillä ei ole hierarkista yhteyttä, saadaan tunnistuksesta melko robusti mm. okkluusiolle, orientaatiolle, skaalalle. Käyttämällä sopivia piirteitä voidaan em. asioita parantaa. Toteutuksessamme käytimme 50 visual wordia ja luokittelimme histogrammit 5NN-luokittimella (viisi lähintä naapuria). Painottamalla naapurien etäisyyksiä luomme kullekin objektille karkean luokkatodennäköisyyden. Lopullisessa toteutuksessa opetuspiirteitä oli n. 700000 ja niistä saatuja histogrammeja n. 50000. Piirreavaruus on 128 dimensioinen ja histogrammiavaruus 50 dimensioinen. SURF-piirteet Tarkan kuvauksen SURF-piirteistä voi lukea osoitteesta (http://www.vision.ee.ethz.ch/ surf/). Piirteiden ominaisuuksia ovat mm. invarianssi skaalalle, rotaatiolle, valaistukselle ja katselukulmalle. Lisäksi piirteet ovat hyvin tehokkaita, koska ne sisältävät lokaalin riippuvuuden naapuripikseleihin. Yhdistettynä Bag of Words-luokittimeen saamme algoritmin, jossa objektin osilla on suppea lokaali riippuvuus, mutta ei hierarkista yhteyttä objektin muihin osiin. 17
5.6 Testaus ja suorituskyky Algoritmin evaluoinnissa käytimme crossvalidationia. Tämä on sisällytettynä oppimisohjelmaan (J2B2-learn). Suorituskyvyn (ei luokittelutehokkuuden) testaamiseksi eri ohjelma (J2B2-test). N. 25000 kuvan oppiminen kestää 2.8GHz dual-core prosessorilla varustetulla tietokoneella hieman yli tunnin. Suurin pullonkaula oppimisessa on klusterointi, jossa käytetään OpenCV:n funktioita. Klusterointia ei tästä syystä voinut jakaa säikeisiin. Muuten kaikki ohjelmat ovat jaettu säikeisiin mahdollisimman joustavasti siten, että moniytimisistä prosessoreista saadaan hyötyä. Säikeiden lukumäärään voi antaa ohjelmalle parametrina. Mm. piirre-erottelu on jaettu säikeisiin, lopullisessa robotin oppimisessa käytimme 8 säiettä. Algoritmin eri osien suoritusajat 2.8GHz prosessorilla, vain yksi säie: Tehtävä Suoritusaika Kiinnostavien kohteiden löytäminen (per kuva) 18.596 ms Piirteiden erottelu kohteista (per kuva) 9.527 ms Piirteiden luokittelu (per kuva) 7.611ms Piirteiden luokittelu (per objekti) 4.228ms Kokonaisaika (per kuva) 41.261 ms Suoritusajat ovat laskettu n. 25000 kuvan keskiarvona. Nähdään, että algoritmi kykenee käsittelemään yli 20 kuvaa sekunnissa. Kilpailussa ajoimme koodia debug-moodissa, jolloin kuvia käsiteltiin arviolta 5 sekunnissa. Luokittelun tehokkuudesta emme voi antaa tarkkaa arvoa, sillä lopullinen opettaminen ja crossvalidation tehtiin datalla, joka vastasi todellista tilannetta. Tämä tarkoittaa, että opetusdata sisälsi useita hyvin samanlaisia kuvia (kun robotti oli paikallaan), jolloin crossvalidation antaa liian optimistisen kuvan (n. 97% luokittelusta oikein). Träkkäykseen yhdistettynä luokittelu oli kuitenkin yli 90% oikein (arvio). 6 Reeksit 6.1 Pysäytysreeksi Pysäytysreeksillä robotti pysäyttää itsensä ennen seinään törmäämistä. Pysäytysreeksi toteutettiin prediktoidun turvallisen alueen avulla (4.4). Pysäytysreeksi laskettiin kulloisessakin pisteessä maksimaalisen sallitun robotin nopeuden mukaan. Tämän nopeuden neliö kuhunkin prediktoidun turvallisen alueen suuntaan voidaan laskea kaavan 1 mukaan. Kaavassa ρ on prediktoitu turvallinen etäisyys kulmaan Θ. Kaavassa d on etäisyys, jolle robotin 18
haluttiin pysähtyvän ennen seinään törmäämistä. Tämä niin sanottu turvamarginaali valittiin 0.07 metriksi tunnetun agentin mukaan. v 2 MAX = ρ d λ cos( θ 2 ) (1) Kaavassa 1 λ on kokeellisesti määritetty painokerroin, jolla järjestelmän herkkyys kalibroitiin. Kaava on johdettu kiihtyvyydestä, jolla robotti tasaisesti jarruttaessaan etenisi täsmälleen ρ-d etäisyydelle, kun painokerroin λ = 2 ja Θ = 0. Kulma on lisätty kaavaan vaimentamaan jarrutusreeksiä kun mitattu vapaa etäisyys ei ole robotin kulkusuunnassa. Tällöin kosinitermi lähestyy nollaa ja sallittu maksiminopeus ääretöntä. Kokeellisesti määritetty painokerroin λ valittiin arvoon 4.0, jolloin robotti oikeasti pysähtyi riittävän nopeasti ennen seinää. 6.2 Kääntymisreeksi Kääntymisreeksin avulla robotti kääntää itseään poispäin lähimmästä esteestä. Tällöin laite osaa itsenäisesti kiertää ja väistää eteen ilmestyviä esteitä. Kääntymisreeksi toteutettiin suoraan PD-nopeussäätimen yhteyteen. Se korjasi nopeussäätimen kulmanopeusasetusarvoa niin, että robotti pyrki hylkimään seiniä ja esteitä. Reeksissä lasketaan turvallisten etäisyyksien käänteisluvut jokaiselle kulmalle. Oikean puoleiset reeksit lasketaan yhteen ja niistä vähennetään vasemman puoleisten arvojen summa. Tällöin jos lähelle tulee esteitä oikealle puolelle enemmän kuin vasemmalle, on reeksin arvo positiivinen ja laite pyrkii kääntymään positiivisen kulman suuntaan eli vasemmalle, poispäin esteestä. Reeksin suuruus rajoitettiin kokeellisesti valitulla raja-arvolla ja kerrottiin kokeellisella vahvistuskertoimella, jonka jälkeen se summattiin PD-säätimen kulmanopeuden ohjausarvoon. Tämä, kuten edellinenkin reeksi lasketaan prediktoidun turvallisen alueen etäisyyksien avulla. 6.3 Nopeuden rajoittaminen toteuttamiskelpoiseksi Viimeisenä vaiheena ennen kuin muuten säädetyt ja rajoitetut nopeusohjaukset käsketään robotin moottorinohjaimelle, tarkistetaan, että nopeudet ovat toteuttamiskelpoisia. Tätä varten mitattiin robotin fyysinen suorituskyky, eli maksiminopeudet ja kiihtyvyydet sekä hidastuvuudet. Mittausten perusteella J2B2-robotin kiihtyvyydeksi valittiin 0.3m/s2, jarrutuskiihtyvyydeksi -0.7m/s2 ja maksiminopeudeksi 0.5m/s. Koska robotin todelliset rajoitukset olivat rengaskohtaisia, laskettiin annettavat nopeuden ja kulmanopeuden asetusarvot kummankin renkaan kehänopeuksiksi, jolloin kääntymisen ja suoraan ajon komponentit voitiin rajoittaa yhdessä rengaskohtaisesti. 19
Nopeus rajoitettiin laskemalla annettavasta asetusarvosta aiheutuva kiihtyvyys edelliseen asetettuun arvoon verrattuna kummallekin renkaalle erikseen. Jos kiihtyvyys (tai hidastuvuus) ylitti sallitun rajan, niin kyseisen renkaan asetusarvo rajattiin sallittuun arvoon. Tällöin kuitenkin kulmanopeuden ja nopeuden suhde muuttuu, ja ajettava rata ei enää ole sama. Tämä korjattiin laskemalla nopeuksien suhde ja vähentämällä myös toisen renkaan nopeudesta sopiva komponentti, jotta nopeuden ja kulmanopeuden suhde olisi edelleen sama, kuin ennen rajoitusta. 7 Paikannus 7.1 laserpaikannus Laserin mittaustietoa sovitetaan kartan pisteisiin G. Weiss et al. [1] kehittämän korrelaatiomenetelmän pohjalta kehitetyllä menetelmällä. Ideana menetelmässä on verrata kahta peräkkäistä laserskannerin mittausta ja laskea niiden välinen tapahtunut rotaatio ja translaatio. G. Weiss et al. [1] esittämässä menetelmässä kulmatieto ratkaistaan ensin kulmahistogrammien välisen korrelaation avulla. Tämän jälkeen menetelmässä mitattu rotaatio poistetaan jälkimmäisestä näytteestä ja tämän jälkeen normaalilla x ja y suuntaisella korrelaatiolla lasketaan x ja y suuntaiset siirtymät laserskannerin mittausten välillä. G. Weiss et al. [1] kehittämä menetelmä toimii jokseenkin luotettavasti sisätiloissa, jossa on suoria pintoja ja paljon yhden suuntaisia seinärakenteita. Se toimii erityisen hyvin jos seinät ovat likimain x ja y akselien suuntaisia. Tässä tapauksessa heidän kehittämää menetelmää muokattiin toimimaan karttapisteiden ja uusimman skannerin mittauksen välillä. Lisäksi menetelmään kehitettiin validointimenetelmä, jolla laskettiin sovituksen hyvyysluku. Tätä hyvyyslukua voitiin suoraan käyttää sensorifuusiota tekevässä Kalmansuotimessa (7.4) Menetelmässä nykyistä laserskannerin mittausta ja karttapisteitä verrataan toisiinsa niin, että ensin kartasta lasketaan edellisen paikan ja suuntakulman estimaatin perusteella simuloitu laserskannerin mittaus. Se toteutettiin muuttamalla kartan seinäpisteet robotin keskeiseen polaarikoordinaatistoon niin että kulma 0 vastaa robotin mukana kulkevan koordinaatiston x akselia (robotista kohtisuoraan oikealle). Tällä tavoin on tehokasta valita jokaisen asteluvun kohdalta lähin etäisyys seinäpisteeseen ja saadaan lopputulokseksi etäisyysmittaukset lähimmistä kartan pisteistä aivan kuten laserskannerin mittauksetkin todellisesta ympäristöstä. Kartta ja mittauspisteet sekä simuloidut mittaukset on esitetty kuvassa 7. 20
Kuva 7: Laserodometria kartasta simuloidun skannauksen kanssa. Simuloidun ja todellisen lasermittaukselle lasketaan kulmahistogrammit ja näitä korreloidaan keskenään. Näin voidaan estimoida kartan ja todellisen lasermittauksen välinen kulmaero. Seuraavaksi tämä kulmaero päivitetään simuloituihin lasermittauksiin ja nyt muutetun simuloidun ja todellisen laserskannerin mittausten välillä suoritetaan x ja y suuntaiset korrelaatiot. Näiden korrelaatioiden perusteella päätellään oikea x ja y suuntainen siirtymä. Nyt tällä uudella paikan ja asennon estimaatilla tehdään uusi simuloitu laserskannaus kartan seinäpisteiden perusteella. Uutta simuloitua skannausta verrataan laserskannerin mittaukseen pisteittäin. Kunkin toisiaan vastaavan kulman etäisyysmittauksien eron neliöt summataan yhteen ja näin lasketaan neliövirhe laserskannerin mittauksen sopivuudesta karttaan. Sovituksessa tuli ajoittain suuria virheitä yksittäisten mittauspisteiden kanssa, jolloin neliövirhe kasvoi suureksi. Tästä päästiin eroon suodattamalla liian suuren neliövirheen saamat termit pois ja laskemalla neliövirhe vain hyväksyttyjen onnistuneiden kulmavirheiden kanssa. 21
Sovituksen onnistuminen validoidaan kaavalla 2, jossa k on hyväksyttyjen etäisyyserojen määrä, indeksit i = 1...k ovat nämä valitut etäisyyksien indeksit, ja r i on etäisyyksien välinen ero mitatun ja simuloidun skannauksen välillä indeksillä i. q = k 181 100 k k ( r i ) 2 (2) Kaava 2 perustuu ideaan, jossa 0 ja 1 välille skaalattuna 1 tarkoittaa täydellistä sovitusta ja 0 täysin epäonnistunutta sovitusta. Kun kaavassa k lähestyy maksimiaan 181, niin ensimmäinen termi nostaa hyvyysluvun maksimiarvoa lineaarisesti lähemmäs yhtä. Toinen termi sen sijaan vähentää tästä luvusta 100-kertaisena neliövirheen keskimääräisen suuruuden. Kerroin 100 on kokeellisesti valittu tähän tapaukseen sopivaksi. Kaavassa mikään ei estä nollaa pienempiä hyvyysluvun arvoja. Ne kuitenkin erikseen tarkistetaan ja muutetaan nollaksi järjestelmässä. Todellisuudessa kartasta simuloitiin 181 mittauspisteen sijaan 270 asteen välein olevaa etäisyyttä, jotta voidaan luotettavasti tunnistaa rotaatiot myös isommilla kulmilla lopullisesta kulmahistogrammista. Kuvassa 7 on esitetty keltaisella värillä simuloidut skannerin etäisyydet ja punaisella alkuperäisestä paikasta saadut laserskannerin mittaukset. Lopullinen tulos on merkitty oranssilla rasteilla siirtäen kutakin punaista laserskannerin mittausta algoritmilla estimoidun siirtymän ja kiertymän mukaan. Samoin punainen robotin kuva esittää alkuperäistä paikkaa ja asentoa ja oranssi uutta. i=1 7.2 Paikan globaali haku kartalta Robotti osaa hakea paikkansa kartalta estimaatin hukkuessa tai alkuarvauksen puuttuessa. Tämä globaali paikan ja suuntakulman haku on toteutettu tätä tarkoitusta varten ja verioitu kyseistä kilpailun karttaa varten. Se kuitenkin toiminee myös muissa tapauksissa. Sen idea perustuu samaiseen korrelaatioon ja kulmahistogrammin korrelaatioon kuin laserpaikannuskin, mutta siinä käytetään lisäksi muutamaa kartan erityispiirrettä hyväksi. Kurssin kilpailun kartta oli siinä mielessä edullinen, että siinä seinät olivat suurin piirtein suorassa kulmassa toisiinsa nähden. Koska koko kartan kaikkien seinien asento oli joko x tai y akselin suuntainen, voitiin kulma hakea vain neljän vaihtoehdon perusteella jotka olivat pääteltävissä kulmahistogrammin arvosta missä tahansa kohtaa kartalla. Menetelmässä kulmahistogrammista haetaan neljä todennäköisintä kulmaa, jotka ovat oikeassa asennossa laserskannauksen mittauksessa nähtävien suorien kanssa. Kulmahistogrammi siis lasketaan teoriassa vierekkäisten la- 22
serin mittauspisteiden välille muodostuvan kulman avulla. Käytännössä kannattaa kuitenkin käyttää vähän kauempana toisistaan olevia mittauspisteitä, jolloin etäisyysmittauksen kohina vaikuttaa vähemmän. Tässä menetelmässä käytettiin 5 asteen etäisyydellä toisistaan mitattuja etäisyyksiä kulmahistogrammin luomiseen. Näin etäisyysmittauksen kohina saatiin hyvin pieneksi. Tällöin myös oletetaan että kaikki nähtävät suorat ovat vähintään 5 astetta nähtynä mistä tahansa kohtaa karttaa. Tämä kuitenkin toteutuu hyvin yksinkertaisessa kilpailun kartassa. Menetelmässä simuloidaan ensin kartasta täyden ympyrän kokoinen asteen välein mitattu robotin laserskannerin keskipisteestä nähty simuloitu laserskannaus. Tätä verrataan laserskannerin mittaamaan skannaukseen kulmahistogrammilla. Näin lasketaan neljä mahdollista suuntaa jossa robotti on kartalla. Näillä jokaisella neljällä suunnalla tehdään erikseen x ja y suuntaiset korrelaatiot kartan seinäpisteisiin nähden. x ja y suuntaisia korrelaatiomaksimeita etsitään maksimissaan 10 kappaletta kumpiakin. Tällöin lopputuloksena saadaan maksimissaan 4*10*10 vaihtoehtoa joissa robotti voi sijaita eri asennoissa. Todellisuudessa kunnollisia x ja y suuntaisia korrelaatiomaksimeita ei useinkaan tule näin paljoa vaan päästään paljon pienemmällä määrällä. Vaihtoehtoisia paikkoja olikin usein reilusti alle sata kappaletta. Menetelmä on tähän mennessä hakenut kaikki potentiaaliset mahdolliset vaihtoehdot joissa robotti voi sijaita korrelaatiomenetelmän perusteella. Seuraavaksi käytetään laserpaikannusta varten kehitettyä validointimenetelmää, jossa lasketaan hyvyysluku paikan ja asennon estimaatin onnistumiselle. Hyvyysluku lasketaan tässä menetelmässä kaikille sallitulle alueelle (kartan sisäpuolelle) päätyville estimaateille, joita usein on kilpailun kartassa vain joitakin kymmeniä. Näistä valitaan uudeksi robotin paikaksi ja asennoksi parhaan hyvyysluvun saanut vaihtoehto. 7.3 Robotin paikan oikeellisuuden valvonta Robotillamme on keino löytää oma alkupaikkansa (7.2), mutta jotta alkupaikan hakua voitaisiin käyttää myös sekaantuneen paikan korjaamiseen, pitää havaita tilanne, jolloin robotti on hukannut oman paikkansa. Tämä tunnistetaan siitä, että laserskannerin sovituksen hyvyysluku on ollut liian pitkään liian alhainen. Tällöin on uskottavasti useimmiten käynyt niin, että robotin estimaatti paikasta ja asennosta ei vastaa todellisuutta ja tällöin laserskannerin mittaukset sopivat huonosti karttaan. Robotin paikan ja asennon valvonta toimii käytännössä niin, että jos kymmenen perättäistä laserskannerin mittausta on hyvyysluvultaan alle 0.1, niin tällöin koko odometriajärjestelmä alustetaan ja robotille haetaan uusi paikan alkuarvaus kartalta. Tämä kuitenkin aiheutti usein lähellä seinää ollessa vää- 23
riä tunnistuksia, koska usein laserskannerin mittaukset lähietäisyydeltä eivät olleet riittävän yhteneviä kartan kanssa. Tämä ongelma korjattiin rajoituksella, että edellä mainittu väärän paikan tunnistus on mahdollinen tapahtua vain, jos laserskannerin mittaukset ovat keskeltä edestä yli puolen metrin etäisyydellä. 7.4 Paikannuksen sensorifuusio EKF:llä Sensorifuusion toteuttamiseen käytettiin laajennettua Kalman suodatinta optimaalisen tilaestimaatin saavuttamiseksi. Laajennetun Kalman suodattimen yhtälöitä varten tulee järjestelmästä muodostaa kinemaattinen malli, mittausyhtälö sekä määrittää mallille ja mittaukselle kovarianssimatriisit. Käytetty kinemaattinen malli robotille on ˆx 1 (k k) + v(k) tcos(ˆx 3 (k k)) ˆx(k + 1 k) = f[k, ˆx(k k), u(k)] = ˆx 2 (k k) + v(k) tsin(ˆx 3 (k k)), (3) ˆx 3 (k k) + ω(k) t missä v(k) on robotin nopeus, ω(k) robotin kulmanopeus, t edellisestä päivityksestä kulunut aika, ˆx 1 estimaatti robotin x-koordinaatista, ˆx 2 estimaatti robotin y-koordinaatista ja ˆx 3 estimaatti robotin suuntakulmasta. Koska kinemaattinen malli on epälineaarinen, tulee se myös linearisoida laajennetun Kalman suodattimen toteuttamista varten. Mittauksen ennustusyhtälö on lineaarinen, jolloin sen linearisointia ei vaadita, vaan ennusteet paikasta ja suuntakulmasta saadaan suoraan identiteettimatriisilla. Vastaavat sensorimittaukset ennusteille saadaan laserpaikannuksesta. Kinemaattisessa mallissa vaadittavat robotin nopeus v(k) ja kulmanopeus ω(k) saadaan vastaavasti suoraan odometriamittauksista. Koska odometriamittauksista saatujen signaalien kohina oli hyvin vähäistä, oli mittausten suora käyttö kinemaattisen mallin ohjauksissa mahdollista. Kovarianssimatriisit mallille ja mittaukselle asetettiin siten, että kohina vaimentuu mahdollisimman hyvin, mutta sijainnin estimaatti seuraa kuitenkin tarkasti todellista sijaintia. Matriisien arvojen asettamisessa käytettiin robotin testiajoista saatua oikeaa mittausdataa. Perinteisen Kalman suodattimen rakenteen lisäksi sensorifuusiolohkoon on lisätty tiettyjä modikaatioita. Näillä modikaatioilla pyritään tarkastelemaan laserpaikannuksesta tulevan mittausdatan oikeellisuutta ja reagoimaan tilanteeseen, jossa mittausdata on virheellistä. Joissain tapauksissa on nimittäin mahdollista, että laserpaikannus on sovittanut laserskannerin mittaukset väärin karttaan, jolloin sensorifuusiolle tuleva mittausdata on virheellistä. Sensorifuusiolohko pyrkii tarkkailemaan laserpaikannuksesta tullutta dataa 24
laserpaikannuksen hyvyysluvun perusteella. Jos hyvyysluku laskee liian alhaiseksi, suunnistaa robotti pelkän odometrian ja kinemaattisen mallin avulla. Laserpaikannus kuitenkin resetoidaan, jos hyvyysluku on liian pitkään alhaisena, jolloin robotille etsitään uusi paikka kartalta. Tieto resetoinnista lähetetään sensorifuusiolle, jolloin tilaestimaatiksi asetetaan suoraan laserodometrialta tuleva arvo, jonka jälkeen estimointia jatketaan normaalisti. 7.5 Viiveen kompensointi ja laitteen paikan prediktointi J2B2 robotin dynaaminen käyttäytyminen mallinnettiin ja tehollinen viive estimoitiin käsketyn nopeuden ja toteutuneen nopeuden välisellä korrelaatiolla niin nopeudelle kuin kulmanopeudellekin [4]. Molemmat viiveet olivat korrelaatiolaskennan mukaan 0.8s. Parhaaseen tulokseen kuitenkin päästiin, kun käytettiin hieman pienempää viivettä paikan prediktointiin ja annettiin PD-säätimen selvitä lopusta viiveestä. Lopullisessa robotin ohjauksessa käytettiin 0.55 sekunnin viivettä prediktoitaessa robotin paikkaa. Robotin paikka 0.55 sekunnin viiveen kuluttua ennustetaan niin, että edelliset 10 ohjausta tallennetaan puskuriin järjestyksessä. Näitä edellistä ohjausta ja uusinta ohjausta käytetään robotin fysikaalisen mallin kanssa ja vanhimmasta mittauksesta alkaen integroidaan nykyistä paikkaa annetuilla ohjauksilla niin, että saadaan ennuste sille, mihin robotti päätyisi viiveen aikana tapahtuvan ohjauksen aikana. Tällä tavalla prediktoitua paikkaa ja nopeutta käytettiin kaikessa robotin säätämisessä, reeksien laskennassa ja nopeuksien rajoittamisessa. Tulevan paikan ennustamisessa tärkeätä on se, että annettu ohjaus on myös toteutettavissa. Tämä varmistetaan kohdassa (6.3) esitetyllä rajoittimella. 8 Navigointi 8.1 Reitinsuunnittelu Reitinsuunnittelija perustuu A* -algoritmiin [5], jota on tarkoitusta varten hieman muokattu ja rajoitettu. Menetelmässä kartta on muutettu 5x5cm ruudukoksi, jonka avulla reitinsuunnittelu tehdään. Reitti ei ole lyhin mahdollinen, vaan optimointikriteerejä on enemmän. Reitti on riittävän hyvä lähellä lyhintä, mutta samalla riittävän kaukana seinistä ja se myös kiertää kaikki konenäöllä tunnistetut esteet. Lisäksi alku ja loppupään asento robotille on otettu huomioon reittiä suunniteltaessa. Reitinhakualgoritmin voi yksinkertaistaa kuvan avulla prosessiksi, jossa ensin maalista aloittaen aletaan täyttää maalipaikkaa vastaavan pikselin ym- 25
päristöä tyhjästä kuvasta, kuten kuvassa 9 on esitetty. Ympäristön pisteitä painotetaan kuvan 8 mukaisella kertoimella (välillä 0...1 kirkkauden mukaan). Painokerroin on laskettu sitä suuremmaksi mitä lähempänä se on todellista seinää. Kuten kuvasta 8 nähdään, on robotin säde lisätty kaikkiin seinän pikseleihin ja näin on muodostettu kartta jolla suunniteltu reitti on aina mahdollinen robotille. Kunkin kohdan painokertoimen mukaan edellisen käsitellyn pikselin arvoa kasvatetaan painokertoimen arvolla ja tämä uusi luku talletetaan kunkin vastaavan pikselin arvoksi lopputuloksen esittävään kuvaan. Tämä toteutetaan kaikille vapaille reunimmaisille pikseleille järjestyksessä. Algoritmin eteneminen on kuin aaltorintaman eteneminen maalipisteestä alkaen ympäristöön niin, että kuva vaalenee painokertoimen suuruuden mukaan. Kuvissa täysin musta alue on kielletty ja sinne ei algoritmi pääse laajentumaan. Algoritmia on tässä tapauksessa rajoitettu muistin koolla, ja algoritmi ei enää välttämättä hae optimaalista tulosta, mutta ratkaisun se löytää aina, jos sellainen on olemassa [6]. Kun laajeneva ja väriltään kirkastuva kuvan aaltorintama saavuttaa lähtöpisteen koordinaatit, on lähtöpiste löytynyt. Nyt reitti lähtöpisteestä maaliin on löydettävissä alkupisteestä lähtien valitsemalla kulloisenkin pisteen ympäristöstä pienimmän arvon omaava pikseli ja siirtymällä siihen. Tällä perusteella on tehty reitti oikean puoleisessa kuvassa 9 Alku ja loppupäiden suunta voidaan ottaa huomioon muuttamalla sallitun alueen muotoa sopivasti ennen reitin suunnittelua. Oikean puoleisessa kuvassa 9 nähdään alku ja loppupisteiden ympärille tehty muoto, jolla pakotetaan reitinsuunnittelija hakemaan reitti vaaditun suunnan kautta. Tällöin voidaan suunnitella reitti niin että se alkaa läheltä robotin senhetkistä asentoa ja päätyy lähelle haluttua asentoa. Reitinsuunnittelija tekee reitin 5x5cm ruudukolla, joten se ei luonnollisestikaan ole kovinkaan sulava ajettavaksi robotilla. Valmis reitti suodatetaan 5 peräkkäisen mittauksen keskiarvolla, jolloin siitä tulee riittävän tasainen robotin ajettavaksi. Kuvassa 10 on esitetty valmis suodatettu reitti lähtöpisteestä maaliin kiertäen tunnetut esteet. 8.2 Reitinseuraaja Valmiiksi suunniteltu reitti ajetaan läpi niin, että robotin nopeussäätimelle annetaan kohdepiste reitiltä järjestyksessä alusta loppuun. Tätä niin sanottua porkkanaa liu'utetaan reitillä eteenpäin, kun ollaan riittävän lähellä edellistä reittipistettä. Säädin toimi hyvin kun porkkana oli aina noin 30 cm päässä robotin keskipisteestä. Tämä porkkanan etäisyys valittiin kokeellisesti. Reitinseuraaja käy reitin tällä tavoin läpi robotin samalla edetessä kohti 26
Kuva 8: Navigointikartan painokerroin sallitulla alueella. maalia. Jos reitinseuraaja ei saa robottia etenemään reitillä kymmenen sekunnin aikana, antaa reitinseuraaja suunnittelijalle käskyn suunnitella uusi reitti ja aloittaa alusta. Ennemmin tai myöhemmin robotti on saavuttanut reitinseuraajan avulla maalin. Kun on lähdetty poimimaan roskaa, tietää reitinseuraaja tämän, ja lopettaa reitin seuraamisen puoli metriä ennen roskaa. Tämän jälkeen vaihdetaan maalipisteeksi roskan koordinaatit ja annetaan ohje PD-säätimelle toimia toimilaitteen koordinaattien mukaan. Tällöin sama säädin säätää robotin paikkaa niin, että toimilaite saadaan mahdollisimman lähelle roskaa. Samalla tavalla edetään roskaa pois kuljetettaessa, ensin reitinseuraajan opastamana reitinsuunnittelijan suunnittelemalla reitillä, ja lopuksi tarkan paikan perusteella säätämällä. Roskan pois kuljettamisen sekvenssi on tarkemmin esitetty tehtävän suunnittelun yhteydessä 9. Reitinseuraajan toimintaa olisi voinut huomattavasti parantaa robotissamme. Siihen jäi loppukilpailuun harmittava ominaisuus tai bugi: jos kone- 27
Kuva 9: Vasemmalla kamerakuvassa havaitut esteet vähennettynä reitinsuunnittelijan sallitusta alueesta, ja oikealla puolella A* menetelmällä sovitettu reitti loppupäästä alkupäätä kohti. näkö löysi suunnitellulle reitille esteitä roskaa pois vietäessä, ei niitä huomattu ennen kuin oli jumitettu esteen aiheuttamaa turvallisen etäisyyden rajaa vasten kymmenen sekunnin odotteluaika, jonka reitinseuraaja odottaa todetakseen reitin epäonnistuneen. Tämä olisi helposti voitu havaita vertaamalla edessä olevia reittipisteitä esteisiin. Tällä muutoksella olisimme ehtineet poimia hyvin kaikki roskat kilpailualueelta, koska robotti jäi useammankin kerran kiinni edestään löytämiinsä roskiin osaamatta suunnitella uutta reittiä niiden ympäri. 8.3 Reeksiivinen PD-nopeussäädin Hyvin toimiva reeksiivinen esteitä väistelevä nopeussäädin on tärkeä ohjatessa robottia tarkkuutta vaativissa roskan poiminta ja säiliöihin sijoittamistehtävässä. Samalla esteitä tulee väistellä mahdollisimman ketterästi, mutta silti liikkua mahdollisimman nopeasti. J2B2-robotin ohjauksessa oli lisäksi huomattava noin 0.8 sekunnin mittaiseksi identioitu tehollinen viive (7.5). Tehollisella viiveellä tarkoitetaan viivettä joka on havaittavissa toteutuneen ja annetun nopeusarvon välillä. Säätimen suunnittelu osoittautui robotin tehtävistä lähes haasteellisimmaksi. Lopullinen säädin on idealtaan yksinkertainen PD-säädin, jossa ohjearvon ja mittauksen erosignaalin ja erosignaalin derivaatan painotetuilla arvoilla ohjataan laitetta [10]. Säädintä kuitenkin käytetään 0.55 sekuntia prediktoidun robotin paikan mukaan (7.5). Tällöin viiveestä aiheutuva värähtely saatiin pienemmäksi, ja säätimen vahvistusparametreja voitiin kasvattaa ilman, 28
Kuva 10: Lopullinen suunniteltu ja suodatettu reitti. että säädin muuttuisi epästabiiliksi. Koska robotin paikka viiveen jälkeen oli kohtuullisen tarkasti estimoitu, pystyi säädin toimimaan riittävän tarkasti ja nopeasti. PD-säätimen ohjausarvot nopeudelle ja kulmanopeudelle laskettiin asetetun porkkanan paikan perusteella toisistaan riippumatta. Järjestelmässä on siis kaksi erillistä PD-säädintä. Kulmanopeussäätimen erosignaali muodostuu nykyisen suuntakulman ja porkkanan sijainnin suuntakulman mukaan. Nopeussäätimen erosignaali muodostuu porkkanan etäisyyden mukaan. Etäisyyttä painotetaan sijaintikulman kosinilla. Tämän ansiosta säätimen nopeusarvo oli sitä pienempi mitä isompi kulmaero kohteeseen on. Kosinitermi mahdollistaa lisäksi peruuttamisen, koska robotin takana olevan kohteen kulman kosini on negatiivinen. Nopeussäätimen erosignaali R on esitetty kaavassa (PD-säädin), jossa ρ on etäisyys kohteeseen ja Θ kohteen ja oman suuntakulman ero. Termit ovat polaarikoordinaatit robotin mukana kulkevassa koordinaatistossa. 29
R = ρ cos Θ (4) Saman PD-säätimen yhteyteen toteutettiin lisäksi esteiden väistöreeksi (6.2). Siksi säädintä kutsutaan tässä reeksiiviseksi PD-säätimeksi. Lisäksi säätimeen toteutettiin useita rinnakkaisia moodeja, joista valittiin oikea kulloisenkin tehtävän mukaan. Näillä moodeilla oli kaikilla erilliset parametrit sekä lisäksi mahdollisuus kytkeä joitakin säätimen ominaisuuksia pois päältä. Esimerkiksi roskakoriin roskaa työnnettäessä väistöreeksi tuli kytkeä pois päältä, jotta laite kykeni työntämään roskan suoraan roskapönttöön. 9 Tehtävän suunnittelu Robotin kulloisenkin tehtävän suunnitteluun päätettiin käyttää Matlabin Simulinkistä löytyvää StateFlow:ta. Stateow:lla on mahdollista tehdä helposti tiloja ja vetää niiden välille erilaisia siirtymäehtoja graasen käyttöliittymän avulla ja käyttää samalla Matlabin toiminnallisuutta. Toteutuksessa lähdettiin liikkeelle siitä, että tilakone antaa muulle ohjelmalle tiedot mitä se haluaa tehtävän kertomalla tavoitekoordinaatit, halutun kameran asennon ja kulloinkin aktiivisena olevan tilan numeron, sekä sen pitääkö gripperin olla auki vai kiinni. Aktiivisena olevan tilan numeroa käytetään hyväksi esimerkiksi siinä, että tiedetään mitä reitinsuunnittelun vaihetta käytetään. Tilakone puolestaan saa muulta ohjelmalta tarvitsemansa tiedot, eli lähinnä oman paikan, roskan paikan ja tyypin. Periaatteessa tilakoneen toiminta on hyvin yksinkertainen. Aluksi etsitään roskia. Stateow saa tiedon roskan löytymisestä sen luo, ja otetaan se gripperillä kiinni. Lopuksi roska viedään oikeaan roskakoriin, ja siirrytään taas etsimiseen. Jos automaatti-moodiin siirrytään muusta moodista esimerkiksi kesken roskan kuljettamista, osaa tilakone havaita tämän, ja jatkaa roskan kuljetuksella roskakoriin. 9.1 Roskien etsintä Ensimmäiseksi automaatti-moodissa mennään 'Seach'-tilaan. Siinä robotti tutkii huoneen siirtymällä ennalta määrättyihin pisteisiin ja pyörimällä ympyrän niiden läheisyydessä. Jos pisteessä on pyöritty kokonaan ympäri siirrytään seuraavaan pisteeseen. Näitä pisteitä on neljä, ja ne sijaitsevat huoneessa siten, että kaksi on väliseinän kummallakin puolella huoneen eri päissä. Ne valittiin kokeellisesti siten, että koko huone tulisi nähtyä kun ajetaan pisteet edestakaisin. 30
Tilassa säädetään myös robotin kameran suuntausta kääntämällä sitä alaspäin aina kun lähestytään tarpeeksi seinää. Näin vältetään turha seinien tarkkailu. Tilasta siirrytään seuraavaan tehtävään, kun tilakone saa viestin että uusi roska on löytynyt. Ongelmana on että robotti liikkuu määrättyjen pisteiden välit hivenen liian nopeasti huomatakseen kaikkia kamerassa välähtäviä roskia heti. Lopulta se kuitenkin aina on huomannut ne jossain vaiheessa. Toinen huono puoli etsintä-tilassa on se, että jos laite huomaa roskan kesken reittipisteen ympäristössä pyörimistä, ja tämä roska on sellainen jonka poimimisen käyttäjä hylkää, palaa robotti pyörimään saman pisteen ympäri ja saattaa huomata uudelleen saman roskan jota ei tahdota poimia. 9.2 Roskan hakeminen Kun tilakone saa ilmoituksen uuden roskan löytymisestä, siirrytään tilaan 'GoToGarbage'. Annetaan robotille roskan koordinaatit, jolloin muu ohjelma siirtää robotin roskan luo. Kameraa säädetään tilassa siten, että mitä lähempänä ollaan kohdetta sitä alemmas kamera on suunnataan. Kun ollaan roskasta etäisyydellä, että gripperillä siihen ylletään, siirrytään tilaa 'PickUp', jossa gripperi käsketään kiinni. Jos operaattori muuttaa roskan tyypin ei-roskaksi kesken matkan roskan luo, palaa tilakone roskan etsintä-tilaan. 9.3 Roskan vieminen roskakoriin Roskan kuljettaminen oikeaan paikkaan päädyttiin toteuttamaan useamman tilan yhdistelmänä, jotta siirtyminen käytännössä olisi helpompaa. Ensimmäisenä on tila 'GoToBin'. Tämä antaa tavoitekoordinaateiksi pisteen noin metrin päässä siitä roskakorista, jonka tyyppiseksi roska on luokiteltu. Robotin ollessa tässä tilassa voi operaattori vielä manuaalisesti vaihtaa roskan tyyppiä, tai käskeä robottia avaamaan gripperin, jolloin kourassa ollut roska pudotetaan ja palataan etsimään uusia roskia. Kun robotti on n. metrin päässä oikeasta roskakorista käskee tilakone robottia siirtymään puoli metriä eteenpäin (tila 'NearBin'). Kun ollaan siirrytty osa tästä matkasta eteenpäin, siirrytään tilaan 'bin2'. Ja jatketaan siirtymistä siihen asti että ollaan siirrytty loputkin tahdotusta matkasta. Tällä kolmen erillisen tilan lähestymisellä saadaan aikaan se, että reitinsuunnittelija saa robotin käännettyä oikeaan kulmaan roskakoreihin nähden, ja ajettua sitten kohtisuorassa roskakoriin. Kun ollaan juuri roskakorin suulla siirrytään tilaan 'Drop'. Tällöin tilakone käskee avaaman gripperin, sillä se ei mahdu avautumaan kokonaan roskakorin sisäpuolella. Tämän jälkeen tila 'Push' käskee laitetta siirtymään vielä 31
vähän eteenpäin, mikä saa aikaan sen, että roska työnnetään kokonaan roskakorin puolelle. Lopuksi tila 'takeback' käskee vielä peruuttamaan takaisin pois roskakorista. 10 Matlabin simulaattori J2B2-robotin ohjelmoimista varten tarvittiin simulaattori, jossa havaittu 0.8s tehollinen viive oli olemassa. Säätimiä ei olisi pystytty kehittämään viiveelliseen järjestelmään viiveetöntä simulaattoria vastaan. Simulaattoria, jonka Matlabin Simulink -kaaviokuva on kuvassa 11, varten mallinnettiin J2B2 robotin dynaaminen käyttäytyminen, sen viiveet, kiihtyvyydet, nopeudet, ja eri sensoreiden kohinatasot. Näiden tietojen pohjalta rakennettiin Matlabin Simulinkkiin malli, jota vastaan voitiin säätimiä ja muutakin järjestelmää testata suoraan. Kuva 11: Dynamiikka- ja laserdatasimulaattori J2B2-robotista Matlabin Simulinkille. Simulaattori muodostuu neljästä eri osasta, dynamiikan, lasermittausten, puskurien ja toimilaitteen simuloinnista. Toimilaiteen simulointi ei ole kuvas- 32
sa 11, mutta muut ovat. Simulaattorin eri osa-alueet on selitetty seuraavissa alakappaleissa. 10.1 Dynamiikkasimulaattori Dynamiikkasimulaattori on kuvassa 11 usean lohkon yhdistelmästä muodostuva kokonaisuus. Se ottaa sisäänsä vasemmalta nopeusasetusarvon robotille. Tähän asetusarvoon se lisää robotin toimilaitteen mallin mukaista kohinaa. Kohina on mallinnettu erikseen nopeus ja kulmanopeusarvoihin niin, että robotin kummankin renkaan pyörimisen nopeusarvon asetus on otettu huomioon. Tähän asetusarvoon lisätään biasoitunut gaussinen kohina. Lisäksi tämä lohko viivästää asetusarvoa mitatun 0.8s viiveen verran. Viimeiseksi lohko varmistaa, että annettu arvo on toteutettavissa ja lisää lisäksi moottorien vaihteiston välyksen pyörien nopeuden vaihtaessa suuntaa. Seuraavassa lohkossa tämä kohinainen nopeusarvo integroidaan paikaksi. Lohko pitää sisällään robotin edellisen paikan ja laskee annettujen nopeusarvojen perusteella seuraavan paikan. Lohko antaa ulos kohinattoman paikan, joka myös otetaan suoraan ulos simulaattorista todellisen paikan referenssiksi. Todellinen paikka on tällöin myös samalla viivästynyt oikean määrän, koska viive on otettu ennen integrointia huomioon. 10.2 Lasermittauksen simulaattori Lasermittaukset simuloidaan kuvassa 11 alareunassa sijaitsevalla lohkolla. Lohkossa lasketaan robotin oikean paikan ja mallinnetun kartan perusteella lasermittausten iskemät kartan seinillä. Mallinnus tehdään niin, että kartan pisteet muutetaan robotin keskipisteen ja asennon mukaiseen polaarikoordinaatistoon. Tällä tavoin esitettyjen seinäpisteiden joukosta valitaan kulmasta -90 alkaen asteen välein 181 lyhintä etäisyyttä seinään kussakin kulmassa. Tämän lisäksi simuloituun kulmaan lisätään ±1cm gaussinen kohina. Lisäksi jokaisesta lasermittauksen arvosta vähennetään 2cm, koska tarkkojen kalibrointimittausten perusteella kyseinen laserskanneriyksilö antoi jokaisen seinän mittauspisteen etäisyyden 2cm liian läheltä. 10.3 Törmäyksenhavaitsija ja puskurien simulointi Törmäyksenhavaitsija on toteutettu kuvassa 11 pystysuunnassa suurimman lohkon sisään. Robotin kulloistakin paikkaa verrataan mallinnettuun karttaan. Jos kartan yksikin piste tulee robotin sädettä lähemmäksi, niin siirretään robottia sen asennon, nopeuden ja kulmanopeuden perusteella pääteltyä kulkusuuntaa vastaan. Tämä siirto tehdään antamalla oikeanlainen no- 33
peusohjausarvo dynamiikkasimulaattorin integraattorille. Tässä on pyritty matkimaan mahdollisimman fysikaalista, mutta samalla tarpeeksi yksinkertaista mallia törmäyksestä seinän kanssa. Samalla, kun törmäys havaitaan, kytketään törmäyskohtaa vastaavan puskurin mittaus päälle. Tällä tavoin myös puskurien mittaukset on voitu toteuttaa yksinkertaisesti. 10.4 Toimielimen simulointi Toimielimen simulointia varten toteutettiin yksinkertainen rajoitettu integraattori, jonka arvoa kasvatettiin ja vähennettiin kulloisenkin tarpeen mukaan toimilaitteen avautumis- tai sulkeutumisnopeudella. Tällä tavoin pystyttiin havainnollistamaan toimilaitteen avautumiseen ja sulkeutumiseen kuluva aika, ja se osattiin paremmin ottaa huomioon muuta järjestelmää suunniteltaessa. Toimilaitteen simulointi myös mahdollisti täyden järjestelmän kokeilemisen simulaattorissa ja näin kaikkia osajärjestelmiä voitiin testata ilman oikeata J2B2-robottia. 11 Käyttöliittymä 11.1 Uusi ja vanha käyttöliittymä Kaikille ryhmille oli toimitettu toimiva käyttöliittymä, jossa perustoiminnallisuus oli läsnä. Robottia pystyi liikuttamaan näppäinkomennoilla ja tärkein sensoridata (laser, robotin paikka, kamera jne.) oli visualisoitu ruudulle. Alusta lähtien oli kuitenkin selvää, että SDL:n (Simple Directmedia Layer) päälle kehitettyä käyttöliittymää olisi työlästä jatkokehittää. Tämä johtuu siitä, että SDL on melko alhaisen tason multimedia-kirjasto ja juuri sen vuoksi sillä monimutkaisten käyttöliittymien rakentaminen on monimutkaista. Vaihtoehtoinen käyttöliittymä päätettiin rakentaa Qt:n päälle. Qt on laajasti käytössä oleva yleiskirjasto, johon kuuluu olennaisena osana alustariippumattomat käyttöliittymän rakennuspalikat. Qt:lla voi rakentaa käyttöliittymiä joko graasesti QT-kehittäjätyökaluja käyttäen, skriptaten käyttäen javascriptin tyylistä skriptikieltä (QML, Qt Meta Object Language) ja/tai perinteisellä C++-koodilla. Demonstraatiot suoritimme aikataulusyistä kuitenkin vanhalla käyttöliittymällä, johon oli tehty pikalisäyksenä demoissa tarvittavat komennot. Samaan aikaan kehitimme kuitenkin QT-käyttöliittymää eteenpäin. 34
11.2 Kommunikointi pääohjelman kanssa Päätimme tehdä graasen käyttöliittymän (GUI, Graphical User Interface) erillisenä ohjelmana. Tähän ratkaisuun päädyttiin sen vuoksi ettei Qt:n kirjastoja tarvitse kääntää MaCin ja MATLABin kanssa samaan ohjelmaan. Liikenne pääohjelman ja käyttöliittymän välillä kulkee TCP/IP-protokollan yli. Käyttöliittymä toimii asiakkaana ja pääohjelma palvelimena. Datarakenteet sarjallistettiin käyttäen Boostin tarjoamaan serialisointikirjastoa. Myös TCP/IP-verkkoliikenne toteutettiin käyttäen Boostia (ASIO). Verkkoliikennetoteutuksessa toteutuksen pohjana käytettiin Boost ASIO:n sivuilla olevia esimerkkitoteutuksia. Päätavoitteena oli, että monimutkaiset laskuoperaatiot tehdään pääohjelman päässä ja tulokset vain välitetään käyttöliittymälle, joka näyttää ne. GUI:lle lähetetään kahdenlaista tietorakennetta: DataToGui ja ImageData- ToGui. DataToGui sisältää kaiken muun paitsi suoraan kuvan liittyvän datan kuten laser-skannauksen, MATLABin estimoima paikka, tunnetut roskat jne.). Kuvadata kulkee ImageDataToGui -tietorakenteessa. Nämä kaksi tietorakennetta kulkevat omissa porteissaan, jotta systeemi ei menisi epäkuntoon esimerkiksi siinä tilanteessa jos kuvadatan siirrosta tapahtuu kummia. Käytännössä tällaisia ongelmia ei kuitenkaan esiintynyt, ja lähestymistapa oli kenties turhankin varovainen. Vastaavasti käyttöliittymä lähettää pääohjelmalle ohjauskomennot DataFromGui-tietorakenteessa. Kaikki nämä kolme tietorakennetta koostuvat yksinkertaisista tietorakenteista (bool, int, vector, jne.), jotka sarjallistetaan Boost Serialization-kirjaston avulla binääriksi tiedosiirtoa varten. Erityisvaivaa nähtiin kommunikaation virhesietoisuuteen. Esimerkiksi yhteyden katketessa molemmat osapuolet (pääohjelma ja GUI) muodostavat yhteyden uudelleen ja sen jälkeen kokeilevat uudelleen. 11.3 Toiminnallisuus GUI:ssa (ks. kuva 12) on kartta, jonka päälle on piirretty seuraavat asiat: Laser-skannaus (punaiset pikku pisteet, muutettu polaarikoordinaatistosta kartan koordinaatistoon), Esteet reittisuunnittelua ajatellen, Simulink-mallin laskemat turvalliset etäisyydet vihreällä Tunnetut roskat (väri valittu roskan tyypin mukaan), Reittisuunnittelijan suunnittelema reitti valkoisella viivalla, 35
Kuva 12: Yleiskuva käyttöliittymästä. Robotin törmäyspuskurien tila (värikoodattu), Robotin tämänhetkinen estimoitu paikka ja asento (x, y, a), Robotin ennustettu paikka ja asento (pieni nuoli) ohjausviiveen päähän Näistä roskien piirto ei jostain syystä toimi todellista robottia vasten. Lisäksi käyttöliittymässä näkyy kamerakuva, jonka päälle on lisätty konenäköön liittyvää dataa: Roskien sijainti (ja koko) kuvassa, merkattuna suorakulmiolla. Väri määräytyy roskatyypin mukaan, Roskien uskottavuus eri tyyppeinä Roskien tunnistenumerot (ID:t) 36
Lisäksi GUI:ssa on melko paljon tekstimuotoista dataa, esimerkiksi robotin nopeus ja Simulink-tilakoneen tila. Seuraavaksi poimittavat roskat näkyvät Next trash-ruuduissa. Seuraavan (nykyisen) roskan tyyppiä voi myös nopeasti muuttaa painamalla roskatyyppiä vastaavaa numeronäppäintä (ks. HELP). Kuva 13: Kuva käyttöliittymästä roskiksien lähellä. Huomaa, ettei roskiksessa olevia roskia tunnisteta. Käyttöliittymän kautta voidaan myös antaa robotille komentoja. Tärkeimpinä mainittaakoon seuraavat (ks. 12): Robotin ohjaustapa (Control Mode), Roskien unohdusnappula (Forget trash) Hätäseis (Stop) Robotin paikan uudelleenhaku kartalta eli odometrian resetointi (Reset) 37