Vytauto Didžiojo universitetasInformatikos fakultetas

Sistemų Analizės katedra

Jonas Andriūnaitis

Roboto lokalizavimas ir patalposplano sudarymasMagistro baigiamasis darbas

Taikomosios informatikos studijų programa, valstybinis kodas 621I13003 Informatikosstudijų kryptis

Vadovas Prof. Tomas KrilavičiusMoksl. laipsnis, vardas, pavardė parašas, data

Apginta Doc. Daiva Vitkutė-AdžgauskienėFakulteto dekanė parašas, data

Kaunas 2017

Turinys

1 ĮVADAS 61.1 Tikslas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.2 Uždaviniai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.3 Darbo planas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2 ANALIZĖ 82.1 Techninė įranga . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.1.1 Robotas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.1.2 Roboto valdymo kompiuteris . . . . . . . . . . . . . . . . . . . . . . 92.1.3 Rankinis valdymo modelis . . . . . . . . . . . . . . . . . . . . . . . 92.1.4 Ultragarso jutikliai . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.1.5 Odometrijos jutiklis . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 Giroskopas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.3 Odometrija . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.4 Giroskopo ir odometrijos palyginimas . . . . . . . . . . . . . . . . . . . . . 142.5 Roboto operacinė sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.5.1 Mazgai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.5.2 Pranešimai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.5.3 Įrašinėjimas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.5.4 Paleidimo failai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.6 SLAM algoritmų palyginimas . . . . . . . . . . . . . . . . . . . . . . . . . 172.6.1 Skirtumai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.7 EKF SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.7.1 EKF būsena . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.7.2 Observacijos modeliai . . . . . . . . . . . . . . . . . . . . . . . . . . 182.7.3 Roboto judėjimo modelis . . . . . . . . . . . . . . . . . . . . . . . . 192.7.4 Triukšmo modeliai . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.7.5 Būsenos korekcija . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.7.6 Orientyrų inicializacija . . . . . . . . . . . . . . . . . . . . . . . . . 212.7.7 Orientyrų asociacija . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3 REALIZACIJA 233.1 Roboto konfigūracija . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.2 Architektūra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.2.1 Roboto valdymo serveris . . . . . . . . . . . . . . . . . . . . . . . . 243.2.2 Jutiklių mazgas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

1

3.2.3 Rankinio valdymo mazgas . . . . . . . . . . . . . . . . . . . . . . . 253.3 PI greičio kontrolė . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.4 Odometrija . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.5 Transformacijos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303.6 EKF SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.6.1 EKF būsenos realizacija . . . . . . . . . . . . . . . . . . . . . . . . 313.6.2 Roboto lokalizavimas . . . . . . . . . . . . . . . . . . . . . . . . . . 323.6.3 Orientyrų asociacija . . . . . . . . . . . . . . . . . . . . . . . . . . . 333.6.4 Orientyrų inicializacija . . . . . . . . . . . . . . . . . . . . . . . . . 333.6.5 EKF būsenos korekcija . . . . . . . . . . . . . . . . . . . . . . . . . 34

3.7 Jakobiano matricų išvedimas . . . . . . . . . . . . . . . . . . . . . . . . . . 353.7.1 EKF-SLAM Algoritmas . . . . . . . . . . . . . . . . . . . . . . . . 36

3.8 Vizualizavimas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363.8.1 Kovariacijos dekompozicija . . . . . . . . . . . . . . . . . . . . . . . 373.8.2 Odometrijos vizualizavimas . . . . . . . . . . . . . . . . . . . . . . 373.8.3 Aplinkos vizualizavimas . . . . . . . . . . . . . . . . . . . . . . . . 39

4 EKSPERIMENTINIS TYRIMAS 414.1 Kriterijai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.2 EKF-SLAM modelio testavimas . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2.1 Pasaulio modelis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.2.2 Triukšmo signalai . . . . . . . . . . . . . . . . . . . . . . . . . . . . 424.2.3 Roboto judėjimo modeliavimas . . . . . . . . . . . . . . . . . . . . 424.2.4 Aplinkos stebėjimas . . . . . . . . . . . . . . . . . . . . . . . . . . . 424.2.5 Parametrai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.2.6 Rezultatai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.3 Roboto testavimas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 464.3.1 Testavimo aplinka . . . . . . . . . . . . . . . . . . . . . . . . . . . 464.3.2 Aplinkos plano sudarymas . . . . . . . . . . . . . . . . . . . . . . . 47

5 REZULTATAI IR IŠVADOS 505.1 Rezultatai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 505.2 Išvados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 515.3 Tęstinumas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

2

SANTRUMPŲ IR TERMINŲ ŽODYNAS

ROS Roboto operacinė sistema

EKF Išplėstinis Kalmano filtras

SLAM Roboto lokalizavimo ir plano sudarymo procesas

GPS Globalaus pozicionavimo sistema

SVD Singular value decompozition, matricos dekompozicija

PI Proporcinis integruojantis reguliatorius

3

SANTRAUKA

Autorius: Jonas Andriūnaitis

Pavadinimas: Roboto lokalizavimas ir aplinkos plano sudarymas

Vadovas: Tomas Krilavičius

Puslapių skaičius: 53

Robotui atsidūrus nežinomoje aplinkoje kur turima mažai arba jokios informacijos apieaplinką gali būti sunku planuoti tolimesnius judėjimo kontrolės veiksmus. Taikant tiksliusatstumą matuojančius jutiklius robotui sudaryti aplinkos planą ir lokalizuotis jame galistipriai išaugti realizacijos kaštai arba tokia įranga gali būti neprieinama todėl yra taikomiprognozuojantys modeliai koreguoti netikslių jutiklių duomenis.

Dažnai yra taikomas EKF-SLAM procesas aplinkos planui sudaryti tuo pat metu robo-tui lokalizuotis. Šis procesas leidžia nesunkiai apjungti skirtingus jutiklius su normaliaipasiskirsčiusiu triukšmu į EKF filtrą taip padidinant sudaromo aplinkos plano kokybę irproceso greitį nenaudojant tikslios įrangos.

Šiame darbe yra taikomas EKF-SLAM procesas sudaryti aplinkos planui ir lokalizuotimobilų robotą „Eddie” operatoriui valdant robotą išoriniu kontrolės įrenginiu. Planosudarymui yra apjungiama odometriją ir ultragarsiniai atstumo jutikliai. Dėl robotomechaninių važiuoklės problemų yra taikomi skaitmeniniai PI reguliatoriai stabilizuotiroboto judėjimui sudarant aplinkos planą.

Sudarant roboto ir EKF-SLAM proceso virtualius modelius yra realizuojamos funkci-jos su Python kurias vėliau būtų lengva integruoti į Roboto Operacinę Sistemą (ROS)realiam robotui lokalizuoti. Robotui valdyti ir sudaryti aplinkos planą buvo sudarytaROS sistema apjungianti roboto judėjimo kontrolę ir jutiklių informaciją su EKF-SLAM,valdymu ir vizualizacija.

4

ABSTRACT

Author: Jonas Andriūnaitis

Title: Robot localization and map learning

Supervisor: Tomas Krilavičius

Number of pages: 53

When robot is placed in unknown environment where are little or no informationknown about environment it could be difficult to plan next robot motion control. Hereaccurate sensor equipment could be used for localization and mapping but it could raiseproduction costs where prediction and correction models are used instead.

Often EKF-SLAM process is used for simultaneous localization and mapping whereit is possible fuse multiple sensors with normally distributed noise into EKF filtering. Itallows to produce more accurate maps and better performance in mapping process.

In this work EKF-SLAM process is used for localization and mapping with “Eddie”robot where operator controls robot using external controller. For mapping odometry andultrasonic sensors are used. Because of mechanical defect in one of robots wheels, digitalPI controllers are used to stabilize robot motion.

In robot and EKF-SLAM process virtual simulation development with Python func-tions were created for later integration into Robot Operating System (ROS). For robotcontrol and mapping the ROS system was created in combining robot motion control andsensor information with EKF-SLAM, control and visualization.

5

1 ĮVADASAplinkos plano sudarymas ir roboto lokalizavimas yra viena centrinių robotikos prob-

lemų kur robotas nežinomoje aplinkoje naudodamas jutiklių informaciją geba lokalizuotisave erdvėje tuo pat metu sudarydamas aplinkos planą kuris vėliau gali būti naudojamasrobotui naviguoti aplinkoje.

Mobilūs robotai yra plačiai naudojami atlikti uždavinius kaip pristatyti vartotojoužsakymą į namus ar tyrinėti kaimynines planetas. Roboto lokalizavimas ir aplinkosplano sudarymas yra būtinas sėkmingam roboto judėjimui ir navigavimui. Pritaikantskaitmeninius prognozės metodus atsiranda galimybė robotui naviguoti net kai nėra pa-pildomų navigacijos priemonių kaip GPS.

Dažnai jutiklių duomenys turi paklaidas dėl techninės įrangos netobulumo ir aplinkospoveikio todėl roboto pozicija ir aplinkos vaizdas gali neatitikti realybės. Sudarius netikslųaplinkos planą robotui gali kilti problemų naviguojant aplinkoje.

1.1 pav.: Robotui judant neižinamoje aplinkoje, robotą veikia jutiklių netikslumai iraplinkos poveikis kaip ratų praslydimas todėl roboto apskaičiuota pozicija skiriasi nuotikros. Robotui matuojant atstumus iki aplinkos objektų sudaromas aplinkos planas irlokalizuojamas robotas koreguojant roboto ir plano būseną.

Siekiant sudaryti tikslesnį planą galima naudoti tiksliasnę techninę įranga, tačiau taigali ženkliai padidint gamybos ir integravimo kaštus arba dėl sprendžiamos problemossudėtingumo gali būti neprieinama naudoti tikslią techninę įrangą.

Šiame darbe yra nagrinėjamas skaitmeninis metodas EKF-SLAM, paremtas netiesinioKalmano filtru (Extended Kalman Filter), sudarant tikslesnį aplinkos planą ir tiksliaulokalizuojant robotą tuo pat metu. Šis metodas leidžia apjungti kelių skirtingų jutikliųduomenis (senssor fusion) taip tiksliau prognozuojant roboto ir orientyrų pozicijas.

6

1.1 Tikslas

Sudaryti kintančios aplinkos planą panaudojant mobilų robotą „Eddie”.

1.2 Uždaviniai

Toliau yra sudaromi sudėtiniai uždaviniai:

1. Realizuoti EKF-SLAM proceso ir roboto virtualų modelį;

2. Integruoti sudarytą procesą mobiliam robotui panaudojant ROS ir „Eddie” robotą;

3. Apjungti roboto jutiklius (odometrija, giroskopas, atstumo jutikliai) į EKF-SLAM;

4. Integruoti rankinio valdymo įrenginį ir PI reguliatorius roboto judėjimo valdymui;

5. Duomenų vizualizacija (aplinkos planas, kelio paieška, odometrija);

6. Integruoti kelio paieškos algoritmą mobiliam robotui.

1.3 Darbo planas

• pirmoji dalis yra įvadas pristatantis nagrinėjamos problematikos tikslus, uždaviniusir sprendimo būdus.

• Sekančiame skyriuje yra išsami problemos analizė: techninės įrangos aprašymas,metodų ir modelių pateikimas. Rezultatų vizualizavimo metodika.

• Trečiame skyriuje pateikiama pilna realizacija „Eddie” robotui valdyti, EKF-SLAMintegracija ir vizualizacijos realiu laiku integravimas.

• Ketvirtame skyriuje pateikiamas EKF-SLAM modeliavimas virtualioje erdvėje su-darant proceso emuliatorių kuris vėliau bus integruojamas „Eddie” roboto valdymokompiuteriui.

• Penktame, paskutiniame skyriuje pateikiami rezultatai ir išvados.

7

2 ANALIZĖ

2.1 Techninė įranga

2.1.1 Robotas

Roboto platforma „Eddie” [7] leidžia apjungti periferinę įranga kaip judėjimo variklius(aktuatorius), jutiklius, maitinimo elementus į vientisą roboto sistemą valdomą išoriniukompiuteriu.

2.1 pav.: Mobilus robotas „Eddie” (1) 3D kamera, (2) nešiojamas kompiuteris robotovaldymui, (3) atstumo jutikliai, (4) judėjimo varikliai (kairė, dešinė pusės) su primontuo-tais ratais.

Dėl diferencialinės važiuoklės ir dviejų varomųjų ratų ties centrine roboto bazės linija,robotas geba atlikti pasisukimus vietoje.

Roboto periferiją apjungia kontrolės plokštė gebanti palaikyti lygiagrečius procesusdėl 8 branduolių architektūros:

1. Valdyti roboto variklius,

2. 8 kanalų analoginio signalo į skaitmeninį transliatorius (ADC),

3. 16 bendros paskirties įėjimų/išėjimų jungtys,

4. papildomo maitinimo šaltinio pajungimas.

5. „Kinect” 3D kameros maitinimo jungtis.

8

2.1.2 Roboto valdymo kompiuteris

Kadangi „Eddie” roboto mikrovaldiklio plokštė turi limituotus resursus vykdyti robotolokalizavimui ir aplinkos plano sudarymą todėl yra naudojamas išorinis nešiojamas kom-piuteris jungiamas prie roboto valdymo plokštės serijine USB jungtimi.

Roboto kompiuterio pagrindinius uždavinius sudaro:

1. kontroliuoti roboto judėjimą,

2. registruoti orientyrus pagal atstumo jutiklius,

3. lokalizuoti robotą pagal orientyrus ir odometriją,

4. esamų orientyrų korekcijoms atlikti,

5. realiu laiku vizualizuoti duomenis,

6. įrašinėti duomenis vėlesniam atkūrimui.

Integruojant išorinį kompiuterį ir robotą yra naudojamos komandos [7], kurios siunči-amos į roboto mikrovaldiklio plokštę, pagal formą:

Įvestis <komanda>[<TARPAS><param1>...<TARPAS><paramN >]<CR>

Atsakas [<param1>...<TARPAS><paramN>]<CR>

Klaida ERROR[<TARPAS>-<TARPAS><priežastis>]<CR>

Komandos yra sudaromos kaip string eilutė ir yra siunčiama į roboto kontrolės plokštęnaudojant serijinę sąsają.

2.1.3 Rankinis valdymo modelis

Rankinio valdymo įrenginys yra naudojamas valdyti roboto judėjimą tuo pat meturobotui sudarant aplinkos planą.

Valdymo įrenginys yra jungiamas į roboto valdymo kompiuterį USB jungtimi [2.2].

2.2 pav.: Rankinio valdymo įrenginio jungimas prie valdymo kompiuterio naudojant USBsąsają.

9

Integruojant rankinio valdymo įrenginį robotui valdyti yra reikalinga pritaikyti modelįroboto judėjimo kontrolės signalui u = [v, θ] kur v roboto greitis ir θ krypties kampas.

2.3 pav.: Kontrolės signalo vektoriaus (raudonai) kampas ir norma kinta pagal valdymoįrenginio įvestį.

2.1.4 Ultragarso jutikliai

Orientyrų aplinkoje radimui yra naudojami ultragarsiniai atstumo jutikliai dėl papras-to veikimo principo ir roboto aplinkos tinkamumo.

Jutikliui siunčiant aukšto dažnio garso signalą (chirp) iki aplinkoje esančio objektoir matuojamas laikas per kurį garso signalas rikošetu grįžta į jutiklį. Laikas nuo signalokelionės pradžios iki pabaigos atitinka atstumą iki matuojamo objekto 2.4.

• Jutiklio atsakas sudaro 470.2 vienetus per 1 milimetrą.

• Ultragarso jutiklio atstumo matavimas gali siekti nuo 3 cm iki 3.3 metro.

Naudojant jutiklį matuoti atstumą iki garsą sugeriančių objektų gali atsirasti didelėspaklaidos. Taip pat jutiklio naudojimas pakreipus ±10 laipsnių kampu nuo statmenokampo, signalas gali atsimušęs nukeliauti trajektorija priešinga jutikliui.

2.4 pav.: Ultragarsinio jutiklio veikimo principas: jutiklio ultra garso signalas keliauja ikiobjekto ir rikošetu grįžta į jutiklį kur signalo kelionės laikas atitinka atstumą iki objekto[6].

2.1.5 Odometrijos jutiklis

Norint registruoti roboto nukeliautą kelią ir pasisukimo kampą yra naudojami varikliopasisukimo jutikliai (quadrature encoder) [20] kiekvienam varikliui atskirai.

10

Šie jutikliai susideda iš dviejų dalių: dantytas diskas kuris montuojamas ant robotojudėjimo variklio ašies ir jutiklis (optinis) kuris registruoja 2 fazių 2.5 pokyčius dantytamedisko paviršiuje robotui judant. Tai leidžia registruoti roboto judėjimą į priekį ir atbulinėseigos judėjimą.

2.5 pav.: A ir B jutikliai registruoja tarpus dantytam diskui besisukant kur fazės skirtumastarp jutiklių yra 90° laipsnių kampas. Naudojant 2 jutiklius su paslinkta fazę galimapastebėti ašies sukimosi kryptį.

2.2 Giroskopas

Robotui susiduriant su odometriją veikiančiais aplinkos poveikiais kaip ratų praslydi-mas ar nelygus grindinys yra naudojamas giroskopas registruoti roboto pasisukimo kam-pus robotui judant aplinkoje.

Integruojant giroskopo kampus galima tiksliau apskaičiuoti roboto poziciją.

2.6 pav.: Robotui atliekant posūkį yra palyginami odometrijos ir giroskopo duomenys.Mėlynai - giroskopo apskaičiuotas kampo pokytis, raudonai - odometrijos. Dėl ratų prasly-dimo atliekant posūkį roboto odometrija neatitinka tikro roboto pasisukimo kampo.

Šiuolaikiniai „Android” įrenginiai 2.7 turi integruotus giroskopus kurių informacijągalima perduoti valdančiam kompiuteriui sudarius įrenginiui aplikaciją.

11

2.7 pav.: Nešiojamo įrenginio giroskopo kampiniai laisvės laipsniai apie ašis [x, y, z].Roboto pasisukimams yra naudojama z ašis.

Sudarant aplikaciją perduoti giroskopo duomenims yra naudojama standartinę „An-droid” programavimo sąsają [15] skaityti giroskopo kampinį pasisukimo kampą erdvėjeapie kiekvieną ašį (x, y, z) rad/s:

mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);mSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GIROSCOPE);

Kadangi naudojamas įrenginio pasisukimo kampas yra momentinis kampinis pasisukimoz ašies atžvilgiu pokytis ωz kur yra reikalinga integruoti pagal:

θz =

∫ωzdt (2.1)

Giroskopo integruoti duomenys yra perduodami valdančiam kompiuteriui panaudojantUSB serijinę sąsają ir „Android” derinimo įrankį adb priimti roboto kompiuteryje [14].

2.8 pav.: Giroskopo duomenų nuskaitymas iš android įrenginio ir perdavimas robotokompiuteriui.

12

2.3 Odometrija

Odometrijos modelis [13] naudojamas rasti roboto pozicija ir pasisukimo kampą (x, y, θ)

robotui judant aplinkoje.

2.9 pav.: Odometrijos modelis robotui atliekant posūkį. Čia dleft ir dright robotonukeliautas kelias, dbaseline roboto bazės skersmuo. Turint šiuos duomenys apskaičiuojamaroboto odometrija.

Robotui judant aplinkoje yra apskaičiuojamas roboto centro taškas dcenter pagal robotonukeliautą kelią d:

dcenter =dleft + dright

2(2.2)

Turint roboto dvejus pozicijų centrus galima apskaičiuoti roboto pasisukimo kampą φsuvedus pozicijų centrus į apskritimo centrą P 2.9:

φ =dright − dleftdbaseline

(2.3)

kur dbaseline yra roboto bazės skersmuo.Toliau integruojama nauja roboto pozicija ir pasisukimo kampas kur gaunamas naujas

roboto pozicijos ir pasisukimo kampo vektorius (x′, y′, θ′):

θ′ = θ + φ (2.4)

x′ = x+ dcentercos(θ) (2.5)

y′ = y + dcentersin(θ) (2.6)

13

2.4 Giroskopo ir odometrijos palyginimas

Išmatavus roboto pasisukimo kampus pagal odometriją ir giroskopą buvo gauti rezultataidemonstruojantys roboto pasisukimo paklaidas |ε| nuo rankiniu būdų apskaičiuoto kampoφ′ [9].

2.10 pav.: Roboto pasisukimo kampo paklaidos εodom ir εgyro (žiūrėti lentelę 2.1) robotuiatliekant judėjimą su posūkiais

n Vl Vr φodom φgyro φmatavimas εodom εgyro1 20 21 0.398 0.646 0.582 0.184 0.642 20 21 0.444 0.394 0.538 0.094 0.1443 20 21 0.411 0.628 0.506 0.095 0.1224 21 20 -0.418 -0.488 -0.426 0.008 0.0625 21 20 -0.438 -0.529 -0.523 0.085 0.006

2.1 lentelė: Roboto pasisukimo kampo (radianais) paklaidos atliekant judėjimą suposūkiais ir rankiniu būdų matuojant roboto pasisukimo kampą. Čia V žymi varikliogreičius φ pasisukimo kampus ir ε pasisukimo kampo paklaidas nuo φmatavimas.

2.11 pav.: Roboto pasisukimo kampo matavimas φ = sin−1(c/b) priklausomai nuo sienosrobotui pajudėjus erdvėje.

14

2.5 Roboto operacinė sistema

Dažniausiai kuriant programinę įranga robotams akademiniais ar taikymo tikslaisdažnai yra sudaromos specifinės problemai programinės sistemos. Norint pritaikyti tokiųsistemų funkcionalumą spręsti kitokio pobūdžio problemai (skirtingi robotai) gali būtisudėtinga faktorizuoti sistemos komponentus.

Roboto operacinė sistema (ROS) [16] leidžia atskyri roboto programinės įrangos dalis įperpanaudojamus paketus kuriuos lengva integruoti skirtingoms sistemoms kaip pavyzdžiuiroboto valdymo įrenginio valdyklės paketas „Joy” [4].

2.5.1 Mazgai

ROS mazgo koncepsija leidžia atskirti skirtingus roboto procesus (pavyzdžiui planavimo,valdymo) ir apjungti panaudojant duomenų apsikeitimą pranešimais [8].

Taip galima nesunkiai prijungti/atjungti mazgus į topologiją nestabdant pagrindinėssistemos darbo.

Mazgus realizuoti galima skirtingomis programavimo kalbomis apibrėžiant pranešimoduomenų struktūrą kuri yra nepriklausoma nuo programavimo kalbos.

2.5.2 Pranešimai

Duomenų komunikacija tarp mazgų vyksta asinchroniniu būdu: mazgas siunčia pranešimąpagal temą (topic), kur kitas mazgas užsiregistruoja į normą temą gauti tos temos pranešimus.

Mazgas-siuntėjas išsiuntęs pranešimą toliau gali dirbti nepriklausomai nuo mazgo-gavėjokuris neprivalo pateikti atsakymo apie sėkmingą pranešimų apsikeitimą.

Gavus pranešimą, mazge yra nedelsiant iškviečiama paprogramė „callback” apdorotigautus pranešime duomenis. Toliau yra laukiama blokuojant mazgo gavėjo vykdymą kolbus gautas sekantis pranešimas pagal temą į kurią mazgas gavėjas yra užsiregistravęs.

2.12 pav.: Duomenų pranešimo apsikeitimas tarp mazgų pagal temą (TOPIC)

Kadangi pranešimų siuntimas ir priėmimas vykdomi asinchroniškai norint apjungtipranešimus yra reikalinga sinchronizuoti. Pritaikant ROS message_filters::sync::ApproximateTimestrategiją galima sinchronizuoti pranešimus pagal apytikri laiko skirtumą 2.13.

15

2.13 pav.: Pranešimų laiko synchronizavimas kur kiekviena horizontali linija atitinkapranešimo temą, taškai pranešimus [10].

ROS pranešimų formato faile yra aprašoma pranešimo duomenų struktūrą kuri vėliaugali būti naudojama skirtingomis programavimo kalbomis realizuotose mazguose. Pranešimuisinchronizacijai yra būtina nurodyti pranešimo antraštę Header header su laiko žyme:

Header header<duomenų tipas> <pavadinimas>...

Sudarius pranešimo formatą yra reikalinga užregistruoti ir sukompiliuoti ROS aplikaciją.

2.5.3 Įrašinėjimas

Roboto operacinė turi įrankį skirta įrašinėti ROS pranešimus į .bag failus tarp mazgųpagal pasirinktą temą arba pagal visas seanso temas.

Įrašytus .bag failus galima vėliau naudoti pranešimų sekų atkūrimui esant neprisijungusprie roboto sistemos ir naudoti programinės įrangos derinimui, vizualizavimui, testavimui.

2.5.4 Paleidimo failai

Paprastai roboto valdymo sistemos paketas yra sudarytas iš kelių mazgų, servisų, arkitų paketų. Čia yra galimybė paleisti sistemą keliais būdais

dalinai pavyzdžiui mazgo testavimui kada kiti mazgai yra nenaudojami;

pilnai kada visi komponentai yra paleidžiami pilnam darbui.

Šias veiksenas galima aprašyti ROS paleidimų (launch) XML failuose kuriuose nurodomikomponentai kurie bus paleidžiami pagal formą:

<launch><node name="mazgo pavadinimas"

pkg="paketas kuriame yra mazgas"type="mazgo failo pavadinimas" />

</launch>

Sudarius failą su galūnę .launch galima paleisti su komandą: roslaunch paleidimofailas.launch.Tadayra paleidžiama ROS sistema ir mazgai nurodomi paleidimo faile.

Taip pat galima paleisti atskirai ROS sistemą ir mazgą su komandomis:

roscorerosrun mazgas

16

2.6 SLAM algoritmų palyginimas

Atlikus SLAM algoritmų apžvalga yra palyginami 2 dažniausiai naudojami algoritmai:EKF-SLAM ir FastSLAM [12].

EKF-SLAM ir FastSlam sprendžia tą pačią robotikos problemą kaip lokalizuoti irsudaryti aplinkos planą nežinomoje aplinkoje. Šie algoritmai yra panašūs, nes yra paremtitais pačiais tikimybiniais judėjimo ir matavimo modeliais ir naudoja Kalmano filtravimąkur:

• EKF-SLAM pritaiko filtravimą visai proceso būsenai [x, P ] vienu metu. Čia sudėtingumasyra kvadratinis priklausomai nuo orientyrų skaičiaus O(|M |2),

• FastSLAM pritaiko filtravimą kiekvienam orientyrui ir roboto būsenai atskirai naudojantbūsenos [x, P ] poaibius. Čia sudėtingumas yra logaritminis priklausomai nuo orientyrųir dalelių (iš anglų k. particle) skaičiaus O(|K| log |M |).

2.6.1 Skirtumai

• EKF-SLAM būseną sudaro vektorius ir kovariacijos matrica [x, P ] ir turi dinamiškaikisti integruojant naujus orientyrus. FastSLAM Būsena dekomponuojama į atskiraskovariacijos matricas (2x2) kiekvienam orientyrui atskirai taip sudarant daleles.

• Taikant EKF-SLAM visi matavimai veikiami atsitiktinių procesų turi būti normaliniaipasiskirstę, kur FastSLAM dekomponuoja tą pačią problemą kiekvienam matavimuiatskirai kur aplinkos poveikis nebūtinai turi būti normaliai pasiskirstęs.

• Jeigu atsiranda klaida orientyrų asociacijoje EKF-SLAM, tikėtina, kad algoritmasdiverguos. FastSLAM sudaro hipotezes kiekvienam orientyrui atskirai taip izoliuojantasociacijos klaidas.

• EKF-SLAM observacijos modelis sudaromas su normaliu pasiskirstymu N(h(x), P )

kur kiekvienu observacijos metu yra atnaujinama kovariacijos matrica. FastSLAMleidžia papildomai panaudoti orientyrų asociacijos informaciją.

17

2.7 EKF SLAM

Roboto lokalizavimui ir aplinkos plano sudarymui taikomas metodas paremtas netiesinęKalmano filtro versija (EKF) [17].

2.7.1 EKF būsena

EKF SLAM būseną [x, P ] sudaro aplinkos plano vektorius x su roboto būsena R =

[x, y, θ] (pozicija, pasisukimas) ir aplinkos orientyrų pozicijomis L = [x, y] globaliosekoordinatėse išskleista forma:

x = [R(x), R(y), R(θ), L(x)1, L(y)1, .., L(x)n, L(y)n] (2.7)

Toliau sudaroma būsenos x kovariacinė blokinė matrica P kuri nusako būsenos tikėtinumąir koreliacijas tarp roboto ir orientyrų. Čia būsenos x elementai atitinka kovariacijųvidurkius matricoje P :

P =

[PRR PRM

PMR PMM

](2.8)

kur PRR yra roboto būsenos kovariacija, PRM , PMR roboto ir orientyrų kovariacijos irPMM orientyrų tarpusavio kovariacijos.

Išskleidus kovariacijos matricą [2]:

P =

σxx σxy σxθ σxm1,x σxm1,y . . . σxmn,x σxmn,y

σyx σyy σyθ σym1,x σym1,y . . . σymn,x σymn,y

σθx σθy σθθ σθm1,x σθm1,y . . . σθmn,x σθmn,y

σm1,xx σxm1,xy σθ σm1,xm1,x σm1,xm1,y . . . σm1,xmn,x σm1,xmn,y

σm1,yx σxm1,yy σθ σm1,ym1,x σm1,ym1,y . . . σm1,ymn,x σm1,ymn,y

... ... ... ... ... . . . ... ...σmn,xx σxmn,xy σθ σmn,xm1,x σmn,xm1,y . . . σmn,xmn,x σmn,xmn,y

σmn,yx σxmn,yy σθ σmn,ym1,x σmn,ym1,y . . . σmn,ymn,x σmn,ymn,y

(2.9)

Čia vektoriaus x ir kovariacijos matricos P dydis dinamiškai kinta integruojant naujusorientyrus.

2.7.2 Observacijos modeliai

Observacijos modeliai yra taikomi transformuoti roboto matavimus y = [d, θ] lokaliuoseroboto koordinatėse į tašką globaliuose koordinatėse taip sudarant orientyrą L = [x, y].Taip pat yra reikalinga sudaryti atvirkštinį modelio variantą kur turimas orientyras Lbūtų transformuojamas į matavimą y.

18

Pirma reikalinga sudaryti jutiklio modelį S kuriuo matavimas y yra transformuojamasiš polinės koordinačių sistemos į Dekarto:

S : y → p (2.10)

Analogiškai sudaromas atvirkštinis jutiklio modelis S:

S : p→ y (2.11)

Toliau yra sudaromas observacijos modelis h() kur orientyras L yra transformuojamaspagal roboto pozicija R = [x, y, θ] į matavimą y pagal jutiklio modelį Sy lokaliose robotokoordinatėse:

h(R, Sy) : L→ y (2.12)

Analogiškai sudaromas atvirkštinės observacijos modelis transformuoti matavimus y įorientyro formą L pagal jutiklį SL globaliose koordinatėse.

h(R, SL) : y → L (2.13)

2.7.3 Roboto judėjimo modelis

Robotui judant erdvėje yra apskaičiuojama nauja roboto pozicija R pagal judėjimomodelį f :

f : (Rt−1, u, n)→ R (2.14)

kurRt−1 yra roboto būsena prieš atliekant judėjimą, n triukšmo vektorius 2.17 veikiantisroboto kontrolės signalo vektorių u = [d, θ].

Roboto kontrolės vektorius u nusako kokia turėtų būti sekanti roboto pozicija taipyra atnaujinama EKF būsena [x, P ] prognozuojant naują roboto poziciją ir pasisukimąaplinkos plane:

x← R (2.15)

P ← JrPJTr + JnNJ

Tn (2.16)

Kur Jx = ∂f(x,u)∂x

ir Jn = ∂f(x,u)∂n

jakobiano matricos roboto būsenai ir triukšmo signalui,čia N triukšmo n kovariacinė matrica.

19

2.7.4 Triukšmo modeliai

Roboto jutikliams yra sudaromi triukšmo modeliai q ir n yra paklaidos dispersijos σnormaliai pasiskirsčiusioms atsitiktinėms reikšmes apie vidurkį µ = 0 generuoti.

N(µ, σ2) (2.17)

Kiekvienam triukšmo modeliui yra sudaromos kovariacinės matricos apibūdinančiosjutiklių patikimumo laipsnį ir naudojamos EKF-SLAM prognozės ir korekcijos metu:

Q = Iij ∗ q2i (2.18)

S = Iij ∗ s2i (2.19)

Čia Q ir S yra kovariacinės triukšmo matricos kur q ir s yra vektoriai randamieksperimento būdu.

2.7.5 Būsenos korekcija

Būsenos [x, P ] EKF korekcija atliekama atlikus matavimą iki taško kuris jau yrainicijuotas. Naujas atstumo matavimas gali skirtis nuo matavimo iki turimo orientyroir yra reikalinga atlikti būsenos EKF korekciją kur robotas ir aplinkos planas tampatikslesni.

Kada matricos P reikšmės didėja yra mažesnis patikimumas roboto ir orientyrų pozicijosatžvilgiu todėl Kalmano naudingumo matrica K taip pat didėja siekiant koreguoti x.Kada P ir K reikšmės artėja į 0, roboto ir orientyrų pozicijos patikimumas didėja iratliekamos labai nedidelės korekcijos.

Korekcijos metu yra atnaujinama būsena [x, P ] pritaikant observacijos modelį h pervestiorientyrą iš x į matavimo formą yx. Tuomet yra sudaroma matuojamo orientyro y

inovacija [z, Z] nusakanti orientyrų pozicijų skirtumus lokaliose roboto koordinatėse sukovariacija Z:

z = y − yx (2.20)

Z = HxPHTx + S (2.21)

Kur Hx yra būsenos x apjungta stebėjimo jakobiano matrica Hx = ∂h(x)∂x

nuo orientyroir roboto. S jutiklio triukšmo kovariacija 2.19.

Toliau apskaičiuojamas Kalmano laipsnio matrica K ir atnaujinama būsena [x, P ]

K = PHTx Z−1 (2.22)

20

Atnaujinama EKF būsena [x, P ] su korekcija:

x← x+K ∗ z (2.23)

P ← P −KZKT (2.24)

2.7.6 Orientyrų inicializacija

Nauji orientyrai inicializuojami kada šių orientyro nėra būsenoje x. Integruojantnaujus orientyrus yra taikomas atvirkštinis stebėjimo modelis h taip pervedant robotolokalų aplinkos matavimą į orientyrą L globaliuose koordinatėse. Tuomet naujas orientyrasL yra integruojamas į aplinkos planą:

x← [x, Ln+1] (2.25)

Sudaromos kvariacinės matricos naujam orientyrui PLL ir kovariacija PLX aplinkosplano – orientyro koreliacijai:

PLL = JRPRRJTR + Jy+1SJ

Ty+1 (2.26)

PLx = JRPRx (2.27)

čia JR = ∂g(R,S,y)∂R

ir Jy = ∂g(R,S,y)∂y

[17] atvirkštinio observacijos modelio jakobianomatricos nuo roboto R ir nuo matavimo vektoriaus y.

Atnaujinama visą būsenos kovariacija su PLL ir PLX :

P ←

[PRR P T

Lx

PLx PLL

](2.28)

Integruojant naują orientyrą, būsenos vektorius x ir kovariacinė matrica P plečiasi.

2.7.7 Orientyrų asociacija

Yra reikalinga atpažinti tuos pačius orientyrus jeigu robotas apsilanko toje pačiojevietoje.

Apskaičiuojant Mahalanobis [23] [17] atstumą naujam matavimui p1 ir užregistruotammatavimui p2 galima sudaryti orientyrų asociacijos modelį.

md(~p1, ~p2, S) =√

(p1 − p2)TS−1p1 − p2 = n2 (2.29)

kur p1 naujas orientyro taškas, o p2 jau užregistruotas orientyras iš x, S = PLL

kovariacinė orientyro matrica.

21

Orientyras laikomas matytu jeigu atstumas tarp orientyrų md < 3σ, kur ir atliekamaturimo orientyro EKF korekcija.

Robotui judant aplinkoje (pvz. koridorius) orientyrus reikia lyginti globalioje koordinačiųsistemoje, nes kitu atveju nauji orientyrai gali būt laikomi matytais ir neregistruojamaiskaip nauji.

2.14 pav.: Robotui judant aplinkoje yra atliekami atstumo matavimai iki sienos irregistruojami orientyrai EKF-SLAM būsenoje. Norint rasti naujo matavimo panašumąsu jau būsenoje užregistruotais orientyrais yra reikalinga transformuoti būsenos orientyrąį matavimo vektorių, o naują matavimą transformuoti į globalias koordinates.

22

3 REALIZACIJA

3.1 Roboto konfigūracija

Roboto konfigūracija sudaro roboto varikliai su ratais, montuojami standartiškai „Eddie”roboto platformai šonuose ties centro linija.

Ultragarsiniai atstumo jutikliai montuojami roboto šonuose: kairės pusės jutiklis montuojamasroboto priekinėje dalyje pasukant jutiklį 90° laipsniu kampu į kairę nuo centro. Dešinėspusės jutiklis taip pat montuojamas priekinėje dalyje pasukant 90° laipsniu kampu į dešinę(jutiklių konfigūracija 3.1).

3.1 pav.: Roboto jutiklių konfigūracija sudaro 2 ultragarso jutikliai montuojami robotopriekinėje dalyje, šoninėse pusėse, lygiagrečiai roboto bazės centro linija.

3.2 pav.: Ultragarsinių atstumo jutiklių montavimas „Eddie” robotui.

23

Ši roboto konfigūracija tinkama vienu metu rasti kairės ir dešinės pusės sienų orientyrusrobotui judant koridoriumi (plotis apie ~2 metrai) tiesiai vidurio linija.

3.2 Architektūra

3.3 pav.: Roboto operacinės sistemos architektūra apjungianti mazgus ir techninę įrangąį vientisą robotikos sistemą. Čia raudonai žymima ROS servisai ir užklausų kryptis,mėlynai mazgai apdorojantys pranešimus pagal temą, žaliai vaizduojami naudojami ROSpaketai.

3.2.1 Roboto valdymo serveris

Panaudojant serijinę jungtį ir Python biblioteką „pySerial” [11] yra sudaromos robototvarkyklės kaip ROS servisai kiekvienai roboto valdymo komandai atskirai:

drive - roboto judėjimo kontrolės funkcija kur parametrai <left>, <right> yra sveikiskaičiai nustatantys kairės ir dešinės pusės variklių greičius (pozicijos/s);

stop - judėjimo kontrolės sustabdymas per 10 pozicijų/s;

read_distance - roboto variklių ašies pasisukimo pozicijos informacijos skaitymas, gražinakairės ir dešinės variklių pasisukimo informacijos vektorių (integruotos pozicijos);

read_speed - roboto kairės ir dešinės pusių variklių greičiai pozicijomis/s laiko momentu;

read_ping - ultragarso jutiklių nuskaitymas, gražinama kairės ir dešinės pusių atstumųnuskaitymai;

24

read_odometry nuskaito roboto nukeliautą distancija ir atnaujina odometrija, gražinamasodometrijos vektorius [x, y, θ];

Sudarius roboto valdymo ROS servisus galima kreiptis iš bet kurio mazgo valdytiroboto aktuatorius ir jutiklius.

Dėl kelių procesu vienu metu siunčiant komandas robotui, atsakas yra rašomas į buferįkur procesas gali nuskaityti kito proceso duomenis. Todėl yra reikalinga jutiklių kreipiniussinchronizuoti sudarant apjungiantį mazgą. Roboto variklių valdymui sudaroma atskiraserijinės jungties programinė sąsaja.

3.2.2 Jutiklių mazgas

Jutikliu duomenys yra apjungiami jutiklių mazge į vieną pranešimą dėl procesų lenktyniųatvejo kada procesas gali nuskaityti kito proceso duomenis.

Siekiant sinchronizuoti jutiklių duomenų nuskaitymą yra sudaromas Sensors mazgaskuriame yra kreipiamasi į roboto valdymo servisus eddie_server nuosekliai ir apskaičiuojamaodometrija. Čia naudojami servisai:

• read_distance

• read_ping

• read_speed

Toliau yra formuojamas Sensors pranešimas kuris bus prieinamas mazgams užsiregistravusiemsį temą /Sensors:

float32 ping_left # ultragarso atstumasfloat32 ping_rightfloat32 dist_left # roboto nukeliauto atstumo duomenysfloat32 dist_rightfloat32 speed_left # roboto greitisfloat32 speed_rightfloat32 odom_x # odometrijafloat32 odom_yfloat32 odom_th

3.2.3 Rankinio valdymo mazgas

Realizuojant roboto valdymą su išoriniu valdymo įrenginiu yra sudaromas ROS mazgas„teleop” kurio pagalba operatoriaus įvedamos komandos kaip mygtuko „pirmyn” yraverčiamos į komandas naudojamas roboto valdymui.

25

3.4 pav.: Rankinio valdymo įrenginio integravimas į roboto operacinę sistemą kaip teleopmazgas.

Panaudojant ROS paketą „joy” [4] kurio pagalba yra galima nesunkiai nuskaitytiišorinio įrenginio signalus jungiantis prie virtualaus įrenginio /dev/input/js0.

Paketo paleidimo konfigūracija yra nurodoma ROS paleidimo (ROS launch) XML faile:

<node name="joy" pkg="joy" type="joy_node" output="screen"><param name="dev" value="/dev/input/js0" />

</node>

Atliekant valdymą, teleop mazge yra gaunamas ROS pranešimas pagal sensors_msgs/Joytemą su įvykio duomenimis. Čia masyvo elementai sužymimi kintamaisiais:

axes: [ H V ]buttons: [ X A B Y L R 0 0 0 0 ]

H horizontalus valdymas, valdomas roboto posūkio greitis, įgyjamos reikšmės 1 - kairė,-1 - dešinė, 0 - neaktyvus;

V vertikalus valdymas, valdomas roboto greitis±5 pos/sek., įgyjamos reikšmės 1 - aukštyn,-1 žemyn, 0 - neaktyvus;

L mygtukas sustabdo robotą, 1 aktyvus arba 0;

R nustato roboto posūkio kampa 0 ir robotas grįžta į eigą judėti tiesia linija θ = 0, 1aktyvus arba 0.

Kiti mygtukai (X, A, B, Y) nenaudojami, nuliniai masyvo elementai nepalaikomiįrenginio.

3.3 PI greičio kontrolė

Robotui judant aplinkoje, dėl techninių priežasčių variklių greičiai skiriasi: dešinėspusės variklio greitis momentais yra didesnis negu kairės pusės variklio (pavyzdys 3.5).

26

3.5 pav.: Variklių greičių skirtumai robotui judant tiesia linija 20 pozicijų/sek. Raudonai:kairiojo variklio greitis. Mėlynai: dešinės pusės variklio greitis.

Siekiant stabilizuoti roboto judėjimą aplinkoje yra naudojamas PI reguliatorius 3.6norimam abiejų roboto variklių greičiams palaikyti.

3.6 pav.: PI reguliatoriaus pritaikymas valdyti roboto judėjimo variklį. Norimassignalas yr(t) yra norimas roboto variklio greitis, sistemos atsakas y(t) yra esamasvariklio greitis kur sudaroma greičio paklaida e(t). PI apskaičiuotas u kontrolės signalastransformuojamas į roboto variklio valdymo signalą.

Norint taikyti PI reguliatorių skaitmeninėms sistemoms valdyti kur reguliatoriausįėjimo ir išėjimo signalai yra diskrečios reikšmės yra naudojama rekurentinio reguliatoriausforma [1] ut = ut−1 + δut:

ut = ut−1 +Kp[et − et−1 +T0TI

et + et−12

] (3.1)

kurKp proporcinės grandies koeficientas, T0 diskretizavimo žingsnis, TI integruojančiosgrandies koeficientas, e yra variklio greičio paklaida et = yr − yt.

Siekiant palaikyti roboto judėjimą yra taikomi PI reguliatoriai kiekvieno variklio greičiamsvaldyti [9].

Operatoriui valdant robotą su išoriniu įrenginiu teleop mazgas generuoja valdymosignalą u kuris yra publikuojamas ROS \control tema.

Toliau yra sudaromas PI skaitmeninio reguliatoriaus mazgas pi_control kuris gaunatemos \control kontrolės signalo duomenis ir sudaromas norimas greičio signalas yr abiejųvarikliu reguliatoriams.

Gautas kontrolės signalas u = [d, θ] yra atvaizduojamas kaip signalas variklio valdymui

27

pagal forma:

(vl, vr)←

(v − y, v), y > 0

(v, v + y), y < 0

(v, v)

(3.2)

kur v yra kontrolės signalo vektoriaus norma ir y = sin(θ) ∗ v, θ vektoriaus kampas.Tuomet vl ir vr yra naudojami kairės ir dešinės pusės variklių reguliatoriaus norimo signaloreikšmės.

Reguliatoriaus mazgui gavus naujus variklių greičių duomenis y kaip ROS pranešimąsu tema \Sensors yra apskaičiuojama valdymo paklaida ir generuojamas naujas valdymosignalas kuris siunčiamas į eddie_server variklių darbui koreguoti.

PIDleft

PIDright

speed_refleft

Control

eright = speedref - speedright

eleft = speedref - speedleft

Eddie robot

driveright

driveright

get_speedleft

get_speedright

speed_refright

u(t)

u(t)

y(t)

y(t)

e(t)

e(t)

yref yref

3.7 pav.: PI reguliatoriaus integravimas roboto varikliams valdyti. Čia yref = v, y -variklio darbo greičiai, u - roboto variklių valdymo signalas.

28

3.8 pav.: Parinkus KP = 0.8 ir KI = 0.5 galima pastebėti variklių greičiųpanašumą, tačiau momentais greičių signalo fazės yra nesinchronizuotos kur robotuijudant momentais yra matyti nedideli pasisukimai, tačiau judėjimo kryptis išlaikoma [9].

3.4 Odometrija

Roboto pozicijos ir pasisukimo kampui rasti sudaromas odometrijos atnaujinimo metodassu būsena integruoti odometrijos pokyčius. Metodas naudoja eddie_server/read_distservisą nuskaityti roboto nukeliautos distancijos duomenims [dleft, dright] kurie reikalingiapskaičiuoti roboto odometrijai [x, y, θ].

3.9 pav.: Roboto pozicijos ir pasisukimo kampo radimas panaudojant odomet- rijosalgoritmą. Kiekvieno ciklo žingsniu yra atnaujinamas roboto pozicijos vaizdavimas [8].

Odometrijos naudojamos konstantos:

• ROBOT_BASELINE roboto bazės skersmuo tarp dešinio ir kairiojo rato lygus 452cm, naudojama odometrijoje;

• POSITION_M roboto nukeliautos distancijos pavertimas į metrus kur konstantarandama išmatavus pozicijų skaičių robotui judant 1 metro atstumą.

Turint nukeliauto atstumo duomenis yra atnaujinama roboto odometrija iškviečiantOdometry:update() funkciją kuri apskaičiuoją naują roboto poziciją.

29

3.5 Transformacijos

Roboto atliekami aplinkos matavimai su ultragarso jutikliais yra roboto lokaliuosekoordinatėse ir yra reikalinga transformuoti 3.10 į globalias koordinatės integruojantnaujus orientyrus.

3.10 pav.: Matavimo lokaliose roboto koordinatėse transformavimas į globaliaskoordinačių ašis ir atvirkščiai.

Orientyrų koordinačių transformacijoms yra sudaromos sekančios funkcijos pagal pasukimomatricą R ir transformavimo vektorių t:

• transform_global(F, p) transformuoja roboto lokalų orientyro tašką p į globaliaskoordinates priklausomai nuo roboto pozicijos F R ∗ p+ t.

• Analogiškai sudaroma funkcija transform_local(F, p) taško transformacijai iš globaliųkoordinačių į roboto lokalias koordinates RT ∗ (p− t).

Kur R yra taško koordinačių rotacijos matrica pagal roboto kampą a = θ ir t taškotransformacijos vektorius:

R =

[cos(a) −sin(a)

sin(a) cos(a)

](3.3)

t = [xF , yF ] (3.4)

Lokalaus taško transformacija į globalias koordinačių ašis sudaroma jakobiano matrica[17] funkcijai transform_global:

Jr =

[1 0 −y ∗ cos a− x ∗ sin a

0 1 x ∗ cos a− y ∗ sin a

](3.5)

Ašių transformacijos jakobiano matrica atitinka transformacijos matricą:

Jp = R (3.6)

Atliekant globalaus taško transformaciją į lokalias roboto koordinates sudaroma jakobiano

30

matrica [17] funkcijai transform_local:

Jf =

[− cos a − sin a cos a ∗ (py − y)− sin a ∗ (px − x)

sin a − cos a − cos a ∗ (px − x)− sin a ∗ (py − y)

](3.7)

Analogiškai sudaroma jakobiano matrica nuo taško p:

Jp = RT (3.8)

3.6 EKF SLAM

Realizuojant EKF roboto lokalizavimo ir aplinkos plano procesą yra sudaromos funkcijosintegruoti roboto jutiklių duomenis ir apskaičiuoti naują EKF būseną [x, P ].

Duomenų struktūrom ir matricų operacijoms yra naudojama Python numpy biblioteka[18] kurios pagalba galima atlikti veiksmus su matricom kaip skaliarinė sandauga iratvirkštinės matricos operacijos.

Integruojant naujus orientyrus yra išskiriama nauja vieta x masyve ir P matricoje,užregistruojami indeksai su identifikatoriumi State:slots sąraše kuris naudojamas duomenųpaieškai. Būsenos x elementai yra siejami su matricos P elementais pagal indeksus.

3.6.1 EKF būsenos realizacija

SLAM proceso būsenos [x, P ] duomenų saugojimui ir paieškai yra sudaroma struktūraPython State su pagalbiniais metodais, siejančiais saugomus elementus x ir P :

1. State:new_slot – dinamiškai plėsti proceso būseną integruojant naujus orientyrus,

2. State:landmark – rasti orientyrą L = [x, y] pagal identifikatorių,

3. State:P_l – rasti kovariacijos matricą P (L) pagal orientyro identifikatorių,

4. State:P_r – rasti roboto kovariacijos matricą P (R),

5. State:R – rasti roboto pozicijos ir pasisukimo kampą R = [x, y, θ] būsenoje.

Orientyrų radimui būsenoje [x, P ] yra naudojamas sąrašas slots kuriame yra saugojamosidentifikatoriaus ir indeksų į orientyro elementus L(x) poros (pavyzdys 3.11).

31

3.11 pav.: Būsenos [x, P ] elementai siejami indeksais kur |x| = |PN | = |PM |, orientyrųidentifikatoriai sudaromi kartu su orientyrų indeksais index(xM).

3.6.2 Roboto lokalizavimas

Robotui judant erdvėje yra prognozuojama roboto EKF pozicija sudarant judėjimoprognozės signalą pagal odometrijos krypties vektorių u = [δd, theta] nuo prieš tai buvusiosroboto pozicijos.

Turint naują roboto lokalią poziciją δR pagal odometriją sudaromas EKF prognozėssignalas u pritaikant atvirkštinę jutiklio funkciją inv_scan pagal modelį S kur δR transformuojamasį lokalų judėjimo vektorių.

Pritaikant judėjimo funkciją move pagal judėjimo modelį f yra sudaroma nauja robotopozicijos prognozė pagal u su jakobiano matricomis [17]:

Jr =

1 0 −y ∗ cos aF − x ∗ sin aF

0 1 x ∗ cos aF − y ∗ sin aF

0 0 1

(3.9)

Jn =

cos aF sin aF

0 0

0 1

(3.10)

Toliau atnaujinama roboto EKF būsena [x, P ] pritaikant update_robot kur yra apskaičiuojamaroboto kovariacijos matrica ir nauja roboto pozicija su pasisukimu:

PRM = Jr ∗ PRM (3.11)

PRR = JrPRRJTr + JnQJ

Tn (3.12)

32

atnaujinama būsena:

x← [R, x] (3.13)

P ←

[PRR PRM

P TRM PMM

](3.14)

3.6.3 Orientyrų asociacija

Robotui atlikus matavimą yra gaunamas ROS pranešimas turintis matavimo informacijąy = [ping, θodom]. Jeigu matavimas jau yra užregistruotas pagal Mahalanobis atstumą yraatliekama būsenos [x, P ] korekcija kitu atveju orientyras yra integruojamas į būseną.

Norint palyginti Mahalanobis atstumąmd(y, xM) naujam matavimui y su visais būsenojex esančiais orientyrais yra atliekamos transformacijos:

1. naujas matavimas y yra transformuojamas į Dekarto koordinačių tašką panaudojantjutiklio modelio S funkciją scan,

2. į globalias taškas transformuojamas į globalias koordinates pritaikant transform_globalsudarant orientyrą,

3. orientyras transformuojamas atgal į matavimą yglobal pritaikant atvirkštinio jutikliomodelio S funkciją inv_scan.

Orientyrams iš būsenos x globaliose koordinatėse yra naudojamas atvirkštinis jutikliomodelis inv_scan transformuoti į matavimą yx.

Taip pat yra reikalinga rasti orientyro kovariacijos matricas P [index(L)] apskaičiuojantMahalanobis atstumą.

Turint naują globalų stebėjimą yglobal, stebėjimus iš yx ir kovariacijos matricas PLgalima apskaičiuoti atstumą ir sudaryti matyto orientyro tikrinimą pagal kovariacijos Sreikšmių pasiskirstymą σ:TRUE,md(y, yx,i) < σ2, yx,i = Sy(xL)

FALSE(3.15)

3.6.4 Orientyrų inicializacija

Robotui atlikus matavimą yra gaunamas ROS pranešimas turintis naujo matavimo(pagal asociaciją) informaciją y = [ping, θodom] ir yra integruojamas naujas orientyras įbūseną [x, P ].

Naujas matavimas y yra transformuojamas į orientyrą L = [x, y] globaliose koordinatėsepritaikius atvirkštinės observacijos modelio funkciją inv_observe.

33

Apskaičiuojamos jakobiano matricos [17] matavimo transformacijai pagal atvirkštinįmatavimo modelį S 2.11 pritaikant funkciją inv_scan matavimui y:

Jy =

[cos(aF ) −d ∗ sin(aF )

sin(aF ) d ∗ cos(aF )

](3.16)

Toliau taškas transformuojamas į globalias koordinates sudarant L orientyrą su jakobianomatricom 3.5 Jr, Jy kur matricos yra apjungiamos:

Jy = Jr ∗ Jy (3.17)

Orientyras L integruojamas į būseną [x, P ] pritaikant landmark_creation funkciją:

x← [x, L] (3.18)

PLL = JrPRRJTr + JySJ

Ty (3.19)

PLM = JrPRM (3.20)

P ←

[PLL PLM

P TLM PMM

](3.21)

3.6.5 EKF būsenos korekcija

Robotui atlikus matavimą gaunamas ROS pranešimas turintis naujo matavimo informacijąy = [ping, θodom].

Jeigu naujas matavimas egzistuoja SLAM būsenoje yra atliekama būsenos EKF korekcija.Pritaikant observacijos funkciją observe yra randamas orientyro matavimas būsenoje

x lokaliose koordinatėse su jakobiano matricomis:

yx = h(R,Lx) (3.22)

Atliekant taško transformaciją iš globalių į lokalias koordinates apskaičiuojamos jakobianomatricos Jr, Jp pritaikant transform_local transformacijos funkciją orientyrui Lx.

Toliau pritaikoma jutiklio modelio S funkcija scan transformuoti orientyrą į matavimąkur apskaičiuojama jakobiano matrica matavimui Jy:

Jy =

[x/

√x2 + y2 y/

√x2 + y2

−y/(x2 + y2) x/(y2 + x2)

](3.23)

Toliau yra apjungiamos observacijos jakobiano matricos:

Jyr = Jy ∗ Jr (3.24)

34

Jyp = Jy ∗ Jp (3.25)

Jry = Jyr ∗ Jyp (3.26)

Turint naują matavimą aplinkoje y ir matavimą būsenoje yx galima apskaičiuoti matavimųpaklaidą (inovaciją) [z, Z] su jutiklių triukšmo kovariacija S.

Toliau apskaičiuojamas Kalmano laipsnis K su inovacijos kovariaciją ir matavimo yxjakobiano matricomis Jry:

K = PRL ∗ JTry ∗ Z−1 (3.27)

ir atnaujinama EKF būsena su numpy:

state.x = x + K.dot(z)state.P = P - K.dot(Z).dot(K.T)

3.7 Jakobiano matricų išvedimas

Jakobiano matricas galima sudaryti panaudojant SymPy paketo funkciją Matrix.jacobian[19]. Orientyro transformavimo į matavimo vektorių jakobiano matricos išvedimas suSymPy:

>>> from __future__ import division>>> from sympy import *>>> x, y = symbols(’x y’)>>> X = Matrix([sqrt(x**2 + y**2), atan2(y, x)])>>> Y = Matrix([x, y])>>> X.jacobian(Y)

Čia pirmos 2 eilutės įkelia reikalingus objektus, toliau yra sudaromi simboliniai objektaix, y ir funkcijos veiksmų matrica X. Funkcijos kintamieji nurodomi Y matricoje. Paskutinėjeeilutėje yra išvedama jakobiano matrica JX = ∂X

∂Y.

35

3.7.1 EKF-SLAM Algoritmas

Algorithm 1: EKF SLAM algoritmas modeliavimui ir realizacijaiData: R = pradinė roboto būsena (x y th)Data: q = judėjimo triukšmo pasiskirstimo vektoriusData: s = jutiklio triukšmo pasiskirstimo vektoriusData: state[x, P] = R, 3x3 nulinė matricaData: Q = 2x2 judėjimo kovariacijos matrica pagal qData: S = 2x2 jutiklių kovariacijos matrica pagal swhile robotas juda aplinkoje do

u = generuojamas roboto judėjimo kontrolės signalas;R = roboto pozicijos [x, y, θ] prognozė pagal u;x = R;P = roboto pozicijos prognozės R kovariacija;state[roboto] <= [x, P]for m← sensorData(test/ping) do

y = aplinkos stebėjimas pagal observacijos modelį h(R,m);if y egzistuoja būsenoje state then

L = surandamas orientyras būsenoje x, atitinkantis y;yx = L transformuojamas į matavimą pagal h(R,L);[z, Z] = sudaroma matavimų paklaidos inovacija su kovariacija;K = apskaičiuojama Kalmano naudingumo matrica;x = atliekama korekcija robotui ir orientyrams x+K ∗ z;P = būsenos korekcija P −KZKT ;state <= [x, P];

elseL = sudaromas orientyras pagal h(R, y);l = išskiriama nauja vieta orientyrui būsenoje state;x = L;P = apskaičiuojama kovariacijos matrica orientyrui L;state[l] <= [x, P];

3.8 Vizualizavimas

• Roboto ir aplinkos plano vizualizavimui yra naudojamas ROS „rviz” [3] paketasleidžiantis vizualizuoti duomenis trimatėje erdvėje realiu laiku.

• Modeliuojant aplinkos plano sudarymą ir lokalizavimą virtualioje erdvėje yra naudojamas„matplotlib” [5] Python paketas skirtas duomenų vizualizavimui.

36

3.8.1 Kovariacijos dekompozicija

Siekiant sudaryt kovariacijos matricų vaizdavimą elipsėmis 3.12 yra taikoma SVDmatricos dekompozicija [24] [17] roboto būsenos kovariacijoms P . Panaudojant Pythonnumpy.linalg.svd funkciją yra randami matricos komponentai [U,Σ, V T ].

3.12 pav.: Dekomponuotos matricos komponentų pritaikymas transformuoti apskritimą įkovariacijos normalinio pasiskirstymo 2D elipsės reprezentaciją.

Toliau sudaroma kovariacijos elipsė su numpy kur P yra roboto arba orientyro pozicijoskovariacijos matrica:

alpha = 2*pi/(n-2)*np.arange(1, n)circle = np.array([np.cos(alpha), np.sin(alpha)])U, s, Vh = np.linalg.svd(P, full_matrices=True)V = Vh.Td = np.sqrt(s)b = np.identity(2) * dellip = np.dot(sigma, V).dot(b.T).dot(circle)

Toliau elipsė transformuojama pagal pozicija t = [x, y] iš būsenos x atitinkančiąkovariacijos P vidurkį:

ellip+ t (3.28)

3.8.2 Odometrijos vizualizavimas

Vizualizuoti mobilų robotą „eddie” virtualioje erdvėje yra naudojama trimatė erdvėatvaizduoti roboto judėjimą dvimatėje erdvėje.

roboto pozicija yra sudaroma pagal trimatį vektorių (x, y, 0) kur judėjimas z ašiesatžvilgiu yra fiksuotas ir lygus 0.

37

kartu su pozicijos vektoriumi yra naudojamas pasisukimo kampas θ kuris atitinkąEulerio kampų sistemą ir yra transformuojamas į kvaternioną.

Ašies kampo transformacija iš Eulerio kampų sistemos į kvaternioną bendriausiuatveju yra atliekama pagal formą [21]:

q0 = cos(α/2)

q1 = sin(α/2) cos(βx)

q2 = sin(α/2) cos(βy)

q3 = sin(α/2) cos(βz)

(3.29)

kur α yra pasisukimo kampas (radianais) apie ašį, cos βx, cos βy, cos βz yra kryptiniaikosinusai naudojami nustatyti pasisukimo ašis.

Naudojant kvaternioną yra susiduriama su sudėtingesne kampų sistema tačiau yraišvengiamas laisvės laipsnio praradimo efektas [22] objektui atliekant pasisukimus trimatėjeerdvėje.

3.13 pav.: roboto vizualizavimas panaudojant ros „rviz” paketą, robotas atvaizduojamastrimis koordinačių ašimis: raudona – roboto priekis, mėlyna – roboto centro ašis.

Panaudojant sendTransform funkciją yra sudaroma galutinė struktūra naudojama„rviz” paketo ir siunčiama kaip ROS pranešimas vaizdavimui.

1. Roboto pozicijos atvaizdavimui yra sudaromas (x, y, 0) vektorius turintis 2 laisvėslaipsnius.

2. Roboto pasisukimo kampas θ yra transformuojamas į kvaternioną panaudojantquaternion_from_euler funkcija su argumentais (0, 0, θ) kur θ yra roboto pasisukimokampas y ašies atžvilgiu, kitos pasisukimo ašys nenaudojamos.

3. Priskiriama roboto tf odometrijos struktūra robo pasaulio struktūrai world.

38

Bendras odometrijos tf struktūros [25] išplatinimas ROS mazgams:

br = tf.TransformBroadcaster() # tf struktūros platinimo objektasbr.sendTransform((x, y, 0),

tf.transformations.quaternion_from_euler(0, 0, th),rospy.Time.now(), ’robo’, "world")

3.8.3 Aplinkos vizualizavimas

Aplinkos plano vizualizavimui yra naudojama „Rviz” žymeklio Marker pranešimoformatas kuris apibrėžiamas 3D pozicijos vektoriumi ir tipu kaip bus vaizduojamas 3.14.

Taip pat galima sudaryti žymeklių masyvo MarkersArray pranešimus norint vizualizuotidaugiau žymeklių.

Šiuo būdų galima vizualizuoti aplinkos planą kur žymeklių masyvą sudaro roboto rastiorientyrai.

3.14 pav.: Žymeklių tipų pavyzdžiai [3].

Robotui judant aplinkoje realiu laiku yra atnaujinamas orientyrų vaizdavimas pagalEKF-SLAM būsenos vektorių x priskiriant kiekvienam žymekliui identifikatorių atitinkantiState.slots indeksą taip atnaujinant esamų orientyrų vaizdavimą. Taip pat yra sudaromižymekliai pagal atstumo jutiklio duomenis palyginti EKF-SLAM būsenos orientyrus irorientyrus apskaičiuotus tik pagal odometriją ir atstumo jutiklį.

39

3.15 pav.: Dvimatis aplinkos planas (pavyzdys [3]).

Žymeklio pranešimas sudaromas pagal Marker formą:

marker.header.frame_id = "/map" # fixed parent frame for markersmarker.header.stamp = rospy.Time();marker.ns = "landmarks";marker.id = mid;marker.type = Marker.CUBE; # show marker as cubemarker.action = Marker.ADD; # accumulate markers on mapmarker.pose.position.x = x/METERS; # landmark to metersmarker.pose.position.y = y/METERS;marker.pose.position.z = 1;marker.pose.orientation.x = 0.0; # fixed orientation for landmarksmarker.pose.orientation.y = 0.0;marker.pose.orientation.z = 0.0;marker.pose.orientation.w = 1.0;marker.scale.x = 0.4;marker.scale.y = 0.4;marker.scale.z = 0.4;marker.color.a = 1.0;marker.color.r = 0.0;marker.color.g = 1.0;marker.color.b = 0.0;

Toliau yra sudaromas žymeklių masyvas MarkerArray ir siunčiamas kaip ROS pranešimaskuris atvaizduojamas „rviz” paketo.

40

4 EKSPERIMENTINIS TYRIMAS

4.1 Kriterijai

Rezultatų palyginimui yra sudaromi kriterijai:

1. Orientyrų atstumas nuo originalaus taško erdvėje;

2. Roboto pozicijos atstumas nuo originalios roboto pozicijos;

Atstumams tarp dviejų taškų matuojamas pagal Euklido atstumą:

d(p, q) =√

(p1 − q1)2 + (p2 − q2)2 (4.1)

čia p ir q 2 taškai Euklido erdvėje.Darbe naudojama scipy Euklido atstumo implementacija.

4.2 EKF-SLAM modelio testavimas

4.2.1 Pasaulio modelis

Pasaulio modelis yra sudaromas panaudojant taškų matricaW nx2 kur kiekviena matricoseilutė atitinka taškus [x, y] erdvėje.

− 1 0 0 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0− 1 0 0

0

1 0 0

2 0 0

3 0 0

4 0 0

5 0 0

6 0 0W

4.1 pav.: Testavimo aplinkos modelis W sudaromas 2D erdvėje turinčioje taškus kurieatitinka roboto orientyrus modeliuojant EKF-SLAM.

41

4.2.2 Triukšmo signalai

Triukšmo signalai yra sudaromi atsitiktinai generuojant normaliai pasiskirsčiusias reikšmeskur q = σ parenkama testo metu.

Čia yra naudojamas numpy.random modulis sudaryti triukšmo signalus:

n = q*np.random.randn(1, 2)[0]v = s*np.random.randn(1, 2)[0]

4.2.3 Roboto judėjimo modeliavimas

Roboto judėjimo virtualioje erdvėje modeliuoti yra sudaromas kontrolės signalas ujudėjimo funkcijai move(R, u) priklausomai nuo testavimo.

Modeliuojant roboto judėjimą, funkcija move yra naudojama dviem atvejais:

1. modeliuoti roboto judėjimą virtualioje aplinkoje kaip tikrą roboto pozicija atitinkančiąroboto judėjimą realybėje,

2. prognozuoti roboto judėjimą EKF procese kur kontrolės signalas u yra veikiamastriukšmo n.

4.2.4 Aplinkos stebėjimas

Robotui atliekant aplinkos orientyrų stebėjimams yra modeliuojamas atstumo jutiklissu ribojimais:

Y ←

observe(R,Wi), p < 100, φ > −π4, φ < π

4

∞(4.2)

Čia naudojama funkcija observe transformuotiW tašką į robotoRmatavimo vektoriausformą atstumas, kampas - [p, φ]. Jeigu matavimas yra už ribojimų yra sakoma, kadmatuojamas atstumas yra begalybė.

Modeliuojamas jutiklis sudaro Y matavimų masyvą kurio indeksai atitinkaW indeksuskurie yra susiejami vizualizavime.

42

−100 −50 0 50 100

−100

−50

0

50

100

0°

45°

90°

135°

180°

225°

270°

315°

20406080

100

4.2 pav.: Roboto jutiklio modeliavimas. Viršuje: robotas aplinkoje (mėlynas taškas) su270° laipsniu pasisukimu (žiūrima žemyn y ašimi).

4.2.5 Parametrai

Modeliuojant EKF-SLAM veikimą yra naudojami parametrai:

R roboto pradinė pozicija ir pasisukimo kampas,

q roboto judėjimo triukšmo signalo pasiskirstimas σ,

s roboto jutiklio triukšmo signalo pasiskirstimas σ,

steps roboto judėjimo kontrolė atliekant roboto lokalizacija ir aplinkos plano sudarymąiteracijų skaičius,

u roboto judėjimo kontrolės signalas u.

4.2.6 Rezultatai

4.3 pav.: Rezultatų grafikų išdėstymas. 1) modeliuojama aplinka su robotu R(x),orientyrais L(x) ir kovariacijų elipsėmis. Kampiniai triukšmo signalai matuojami radianais2) visas EKF kovariacijos P vaizdavimas. 3) EKF orientyrų atstumas nuo tikro taško.

43

0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0

x

− 1 0 0

− 5 0

0

5 0

1 0 0

1 5 0

2 0 0

2 5 0

y

1 31 41 51 61 71 8

1 92 02 12 22 32 42 5

2 8

2 9

3 0

3 1

3 5

3 6

3 7

3 8

3 9

4 0

4 1

4 5

4 6

4 7

4 8

1 71 8

2 42 5

3 9

1 6

2 3

1 5

2 2

1 4

2 1

1 3

2 0

R= [3 0 4 0 0 ], q = [ 0 .0 2 0 .0 3 5 ], s = [ 0 .0 2 0 .0 3 5 ]

rob ot

world p o in t

L(x)

R(x)

0 5 1 0 1 5 2 0 2 5 3 0

[R M]

0

5

1 0

1 5

2 0

2 5

3 0

[R M

]

P

− 2 5 0

0

2 5 0

5 0 0

7 5 0

1 0 0 0

1 2 5 0

1 5 0 0

1 0 1 5 2 0 2 5 3 0 3 5 4 0

in d e x

0

5

1 0

1 5

2 0

2 5

3 0

3 5

4 0d

ist

d is t (L, w)

EKF-SLAM s im u la t ion s te p s = 3 0

4.4 pav.: Roboto judėjimo ir orientyrų integravimo modeliavimas praleidžiant EKFkorekcijos žingsnį. Roboto ir orientyrų kovariacija didėja.

44

4 2 9 .5 4 3 0 .0 4 3 0 .5 4 3 1 .0

x

3 7

3 8

3 9

4 0

4 1

4 2

4 3

y

R= [3 0 4 0 0 ], q = [ 0 .0 1 0 . ], s = [ 0 .0 1 0 .0 1 7 ]

rob ot

world p o in t

L(x)

R(x)

0 5 1 0 1 5 2 0 2 5 3 0 3 5 4 0

[R M]

0

5

1 0

1 5

2 0

2 5

3 0

3 5

4 0

[R M

]

P

− 0 .0 0 4

0 .0 0 0

0 .0 0 4

0 .0 0 8

0 .0 1 2

0 .0 1 6

0 .0 2 0

0 .0 2 4

1 0 1 5 2 0 2 5 3 0 3 5 4 0

in d e x

0 .0

0 .1

0 .2

0 .3

0 .4

0 .5

0 .6

dis

td is t (L, w)

EKF-SLAM s im u la t ion s te p s = 4 0

4.5 pav.: Robotui judant iš kairės į dešinę neveikiant judėjimo kampinio triukšmo signaluiqθ roboto paskutinės pozicijos tikėtinumas yra pasiskirstęs pagal judėjimo kryptį.

45

1 0 0 2 0 0 3 0 0 4 0 0 5 0 0

x

0

5 0

1 0 0

1 5 0y

1 31 41 51 61 71 8

1 92 02 12 22 32 42 5

3 0

3 1

3 7

3 8

3 9

1 71 8

2 42 5

3 9

1 6

2 3

1 5

2 2

1 4

2 1

1 3

2 0

3 1

1 9

3 0

3 8

R= [3 0 4 0 0 ], q = [ 0 .0 1 0 . ], s = [ 4 . 0 .1 4 ]

rob ot

world p o in t

L(x)

R(x)

0 5 1 0 1 5 2 0 2 5 3 0 3 5 4 0

[R M]

0

5

1 0

1 5

2 0

2 5

3 0

3 5

4 0

[R M

]

P

− 5

0

5

1 0

1 5

2 0

2 5

3 0

3 5

1 0 1 5 2 0 2 5 3 0 3 5 4 0

in d e x

0 .0

0 .5

1 .0

1 .5

2 .0

2 .5

3 .0

3 .5

4 .0

dis

td is t (L, w)

EKF-SLAM s im u la t ion s te p s = 4 0

4.6 pav.: Robotui judant iš kairės į dešinę netrikdant roboto judėjimo, veikiant jutiklįdideliu s signalu, orientyrų pozicijos tikėtinumas mažėja. Paskutinio orientyro elipsė yradidesnė negu kitų orientyrų, nes buvo atlikta mažiau korekcijų.

4.3 Roboto testavimas

4.3.1 Testavimo aplinka

Testavimo aplinka yra parenkama su minimaliu aplinkos poveikių roboto judėjimui:

1. lygus grindinio paviršius,

2. be objektų sugeriančių garsą (minkšti baldai, užuolaidos),

3. nesudėtingos formos,

46

4. netoli nuo VDU informatikos fakulteto.

Pagal šiuos kriterijus yra tinkamas VDU informatikos fakulteto koridorius su foje.

4.7 pav.: Roboto testavimo aplinka

4.3.2 Aplinkos plano sudarymas

4.8 pav.: Odometrijos ir orientyrų testavimas robotui odom judant vietoje (nėra ratųkontakto su grindiniu). Matome orientyrai išlaiko vienodą atstumą nuo roboto. Čiaroboto pasisukimas yra apie mėlyną ašį, o raudona rodo kryptį.

47

4.9 pav.: Roboto judėjimas koridoriumi nuo pradžios taško (S) iki apsisukimo taško (T),atliekamas 180° laipsnių apsisukimą ir grįžtama atgal (F), čia kvadratu žymimas foje.Robotui atliekant posūkį dėl ratų praslydimo susikaupia odometrijos paklaidos ir robotuigrįžtant yra beveik 50° laipsnių paklaida nuo trajektorijos (S)(T).

48

4.10 pav.: Roboto judėjimas koridoriumi (raudonos rodyklės) kur atlikus 180° laipsniųapsukimą vietoje odometrijos kampas neatitinka roboto judėjimo trajektorijos.

4.11 pav.: Roboto judėjimas koridoriumi (rodyklės kryptimi) su PI variklių greičiųstabilizavimu. Raudonas kvadratas žymi koridoriaus foje.

49

5 REZULTATAI IR IŠVADOS

5.1 Rezultatai

1. Sudaryta Python aplikacija EKF-SLAM lokalizavimui ir aplinkos plano sudarymuitirti modeliuojant roboto judėjimą virtualioje aplinkoje;

2. Sudarytas rezultatų palyginimo funkcijos pagal kriterijus ir rezultatų vaizdavimas;

3. Integruotas PI reguliatorius stabilizuoti roboto variklių darbui pagal kontrolės signalą,parinkti PI reguliatoriau parametrai;

4. Sudarytas aplinkos planas panaudojant roboto odometriją ir ultragarsinius jutikliusoperatoriui valdant robotą išoriniu valdymo įrenginiu;

5. Aplinkos plano ir lokalizavimo vizualizavimas su rviz paketu;

6. Roboto sistemos architektūros sudarymas realizuojant ROS mazgus ir duomenųapsikeitimą tarp jų sudarant pranešimų formatus;

7. Sudarytas rankinio valdymo įrenginio ROS mazgas generuoti roboto kontrolės signalui;

8. Sudaryta Android aplikacija ir ROS serverio mazgas perduoti giroskopo duomenimsiš planšetinio kompiuterio į roboto valdymo kompiuterį;

9. Ištirta roboto pasisukimo kampų matavimai naudojant odometriją ir giroskopą,rezultatų palyginimas.

50

5.2 Išvados

Sudarant aplinkos planą dėl roboto važiuoklės mechaninio defekto kur vieno iš dvejųvariklių greičiai nesutapo ir robotas išeidavo iš norimos judėjimo trajektorijos. Siekiantstabilizuoti variklių darbą buvo sudaryti PI reguliatoriai kiekvieno variklio greičiamspalaikyti pagal operatoriaus nustatytą kontrolės signalą naudojant išorinį kontrolės įrenginį.

Sudarant PI reguliatorius buvo atliktas variklių darbo tyrimas ir parenkami reguliatoriausparametrai. Pritaikius PI reguliatorius buvo stabilizuotas roboto judėjimas ir buvo reikalingosminimalios judėjimo korekcijos dėl aplinkos poveikio.

Dėl odometrijos paklaidų atsirandančių roboto ratams praslystant posūkio metu buvomodifikuojamas roboto odometrijos modelis integruojant Android įrenginio giroskopoduomenis. Apjungiant giroskopą ir odometriją buvo eliminuotos paklaidos dėl ratų praslydimo.

Roboto lokalizavimui ir aplinkos plano sudarymui (SLAM) buvo atliktas algoritmųpalyginimas. Dėl logaritminio kompleksiškumo ir jutiklių duomenų nepriklausymo nuonormalinį pasiskirstymą turinčių atsitiktinių procesų dažniau yra taikomas FastSLAMalgoritmas. Tačiau EKF-SLAM algoritmas yra paprasčiau realizuojamas apjungiant skirtingusjutiklius (sensor fusion) ir tinkamas atlikti roboto lokalizavimą ir aplinkos plano sudarymąuždarose patalpose.

EKF-SLAM algoritmo realizavimui ir tyrimui buvo sudarytas virtualaus roboto modelisir aplinka kur buvo modeliuojamas roboto judėjimas ir aplinkos stebėjimas. EKF-SLAMfunkcijos buvo sudaromos su Python programavimo technologijomis ir vėliau integruojamosį roboto operacinę sistemą.

Siekiant sudaryti aplinkos planą su Eddie roboto platforma buvo sudaryti robotooperacinės sistemos (ROS) mazgai apjungiantys jutiklių informaciją, roboto valdymą,aplinkos plano sudarymą, PI kontrolę ir rezultatų vaizdavimą į vientisą roboto sistemą.

5.3 Tęstinumas

1. Ištirti skirtingus aplinkos plano sudarymo ir lokalizavimo algoritmus kaip FastSLAMir pritaikyti Eddie robotui.

2. Sudaryti 3D aplinkos planą modifikuojant SLAM algoritmą pritaikant erdvinę kamerą(Kinect).

3. Sudaryti lokalios navigacijos strategiją autonominiam roboto judėjimui nežinomojeaplinkoje.

4. Pritaikyti navigacijos procesą robotui naviguoti lokaliame ir globaliame aplinkosplane.

51

Literatūra[1] Vytautas Magnus University Adaptyvios ir intelektualios sistemos. Objekto valdymas

pi reguliatoriumi, 2015.

[2] Autonomous intelligent systems Cyrill Stachniss, Freiburg university. Ekf slam.

[3] Josh Faust Dave Hershberger, David Gossow. rviz, 2017. http://wiki.ros.org/rviz[Online; accessed 29-August-2017].

[4] Morgan Quigley, Brian Gerkey, Kevin Watts, Blaise Gassend. joy. http://wiki.ros.org/joy [Online, 2017].

[5] J. D. Hunter. Matplotlib: A 2d graphics environment. Computing In Science &Engineering, 9(3):90–95, 2007.

[6] Parallax Inc. Detect distance with the ping)))(tm) ultrasonic sensor.

[7] Parallax Inc. Eddie robot platform. https://www.parallax.com/downloads/eddie-board-command-set [Online; accessed 16-May-2017].

[8] Vytautas Magnus university Jonas Andriūnaitis, darbo vadovas prof.Tomas Krilavičius. Mobilaus roboto odometrija panaudojant roboto operacinęsistemą, 2015.

[9] Vytautas Magnus university Jonas Andriūnaitis, darbo vadovas prof.Tomas Krilavičius. Roboto judėjimo vizualizavimas minimizuojant robotopasisukimo paklaidas ir judėjimo netikslumus, 2015.

[10] Vijay Pradeep Josh Faust. message filters. http://wiki.ros.org/message_filters.

[11] Chris Liechti. Python serial port extension.https://pyserial.readthedocs.org/en/latest/.

[12] Lausann (EPFL) Michael Calonder, Swiss Federal Institute of Technology. Ekf slamvs. fastslam – a comparison. https://infoscience.epfl.ch/record/146805/files/ekf_fastslam_comp.pdf [Online; accessed May-2017].

[13] Edwin Olson. A primer on odometry and motor control.

[14] Android Open Source Project. Android debug bridge. http://developer.android.com/tools/help/adb.html [Online; accessed 2017 Gegužės mėn.].

52

[15] Android Open Source Project. Android platform: Sensorevent. https://developer.android.com/reference/android/hardware/SensorEvent.html [Online; accessed 2017Gegužės mėn.].

[16] Morgan Quigley, Ken Conley, Brian P. Gerkey, Josh Faust, Tully Foote, Jeremy Leibs,Rob Wheeler, and Andrew Y. Ng. Ros: an open-source robot operating system, 2009.

[17] Joan Sola. Simulataneous localization and mapping with the extended kalman filter.http://www.iri.upc.edu/people/jsola/JoanSola/eng/course.html [Online; accessed15-May-2017].

[18] S. Chris Colbert Stéfan van der Walt and Gael Varoquaux. The numpy array:A structure for efficient numerical computation. http://www.scipy.org/ [Online;accessed 2017 Gegužės mėn.].

[19] SymPy Development Team. Sympy is a python library for symbolic mathematics.http://www.sympy.org/en/index.html [Online; accessed 15-May-2017].

[20] Wikipedia. Rotary encoder — wikipedia, the free encyclopedia, 2015. https://en.wikipedia.org/w/index.php?title=Rotary_encoder&oldid=691748531 [Online;accessed 16-May-2017].

[21] Wikipedia. Conversion between quaternions and euler angles — wikipedia, thefree encyclopedia, 2017. https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles&oldid=773296783 [Online; accessed16-May-2017].

[22] Wikipedia. Gimbal lock — wikipedia, the free encyclopedia, 2017. https://en.wikipedia.org/w/index.php?title=Gimbal_lock&oldid=769439365 [Online; accessed15-May-2017].

[23] Wikipedia. Mahalanobis distance — wikipedia, the free encyclopedia,2017. https://en.wikipedia.org/w/index.php?title=Mahalanobis_distance&oldid=766325418 [Online; accessed 19-March-2017].

[24] Wikipedia. Singular value decomposition — wikipedia, the free encyclopedia,2017. https://en.wikipedia.org/w/index.php?title=Singular_value_decomposition&oldid=780411818 [Online; accessed 15-May-2017].

[25] ROS wikipedia. Setting up your robot using tf, 2015. http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF.

53