Documente Academic
Documente Profesional
Documente Cultură
Proiect de diplomă
Anul 2018
Copyright © 2018 , George-Iulian Nitroi
Toate drepturile rezervate
Autorul acordă UPB dreptul de a reproduce și de a distribui public copii pe hârtie sau electronice
ale acestei lucrări, in formă integrală sau parțială
8
Cuprins
Listă de figuri ................................................................................................................................ 11
Listă de acronime .......................................................................................................................... 15
Introducere .................................................................................................................................... 17
Capitol 1 Noțiuni teoretice necesare dezvoltării practice ....................................................... 19
1.1 Descrierea computerului Raspberry Pi ........................................................................... 19
1.1.1 Descrierea succintă a portului GPIO....................................................................... 20
1.1.2 Caracteristici generale ale ARM Cortex-A53 ........................................................ 20
1.1.3 Domeniul de utilizare.............................................................................................. 22
1.2 Descrierea senzorului LIDAR-ului ................................................................................ 22
1.2.1 Caracteristi generale ale senzorului ....................................................................... 23
1.2.2 Principiul de funcţionare ......................................................................................... 24
1.3 Comunicația Wi-Fi ......................................................................................................... 25
1.4 Locomotia vehiculului.................................................................................................... 25
1.4.1 Motorul de curent continuu ..................................................................................... 25
1.4.2 Principiul locomoției diferențiale ........................................................................... 26
1.4.3 Controlul motoarelor ............................................................................................... 27
1.4.4 Modulaţia impulsurilor în durată ............................................................................ 29
1.4.5 Encodere optice ....................................................................................................... 30
Capitol 2 Implementarea hardware a vehiculului ................................................................... 33
2.1 Sistemul de control ......................................................................................................... 34
2.1.1 Percepția ................................................................................................................. 35
2.1.2 Cunoașterea ............................................................................................................ 35
2.1.3 Interfața operator ..................................................................................................... 35
2.2 Şasiul robotului .............................................................................................................. 36
2.3 Locomotia vehiculului.................................................................................................... 36
Capitol 3 Localizare şi mapare simultană ............................................................................... 39
3.1 Odometrie ....................................................................................................................... 39
3.2 Puncte de reper (Landmarks) ......................................................................................... 39
3.3 Cartografiere................................................................................................................... 40
3.4 Filtrul de particule Rao-Blackwellized .......................................................................... 42
9
Capitol 4 Implementarea software a vehicului ....................................................................... 43
4.1 Sistemul de operare al robotului..................................................................................... 43
4.1.1 Pachetele ................................................................................................................. 43
4.1.2 Arhitectura ROS...................................................................................................... 44
4.1.3 Fişierele de tip “Launch” ........................................................................................ 45
4.1.4 ROS pe mașini multiple .......................................................................................... 46
4.2 Mediul de dezvoltare ...................................................................................................... 46
4.3 Arhitectura aplicației ...................................................................................................... 47
4.3.1 Driver-ul Hokuyo URG-04lx .................................................................................. 48
4.3.2 Navigația și determinarea distanței parcurse de vehicul ......................................... 49
4.3.3 Controlul de la distanța ........................................................................................... 51
4.3.4 Softul pentru localizare şi mapare simultană .......................................................... 51
4.3.5 Vizualizarea rezultatelor ......................................................................................... 52
Capitol 5 Testarea vehicului ................................................................................................... 53
5.1 Scenariul 1: Balconul ..................................................................................................... 53
5.2 Scenariul 2: Holul........................................................................................................... 54
5.3 Scenariu 3: Camera ........................................................................................................ 55
Concluzii ....................................................................................................................................... 57
Bibliografie ................................................................................................................................... 59
Anexa 1 Imagini cu realizarea practică ......................................................................................... 61
Anexa 2 Cod sursă –Comanda navigației ..................................................................................... 62
Anexa 3 Cod sursă – Navigația și determinarea distanței parcurse de vehicul ............................ 63
Anexa 4 Cod sursă – Scriptul pentru lansarea aplicaților pe mobil .............................................. 69
Anexa 5 Cod sursă – Scriptul pentru algorimului SLAM............................................................. 70
10
Listă de figuri
11
12
Listă de tabele
13
14
Listă de acronime
ROS: Robotic Operating System(Sistem de operare pentru roboţi)
PWM:Pulse-Width Modulation(Modulație in timp a impulsului)
SPI : Serial Peripheral Interface ( Interfață serială cu dispozitive periferice)
PDS:Procesor digital de semnal
SIMD:Single instruction, multiple data(Date multiple pe o singură instrucţiune=
SBC: Single-Board Computer (Computer pe o singură placută)
SoC:System on a chip(Sistem pe un singur chip)
LIDAR: Light Detection and Ranging(Detectare a luminii si a distanţei)
GPIO: General-purpose input/output(Pini de uz general de intrare/ieşire)
WLAN: Wireless Local Area Network(Reaţea fara fir locala)
SLAM: Simultaneous Localization And Mapping(Localizare şi mapare simultană)
SSH: Secure Shell(Shell securizat)
GPS: Global Positioning System(Sistem de Poziționare Globală)
RAM: Random Acces Memory (Memorie cu acces aleatoriu)
RISC: Reduced instruction set computing (Arhitectură de calcul cu set redus de instrucțiuni )
USB: Universal Serial Bus( Magistrală Serială Universală)
HDMI:High-Definition Multimedia Interface(Interfață multimedia de înaltă definiție)
MAC: Media Access Control(Control de acces media)
TTL : Transistor-Transistor logic (Logica tanzistor-tranzistor)
AP:Access Point(Punct de acces)
SSID: Service set identifier (Identificator al setului de servicii)
IP: Internet Protocol(Protocol de internet)
API: Application Programming Interface(Interfaţa de programare a aplicaţiilor)
15
16
Introducere
În zilele noastre dezvoltarea tehnologiei este mai rapidă pe zi ce trece. Din ce în ce mai
multe dispozitive robotizate autonome s-au infiltrat în viețile oamenilor pentru a o face mai
usoară. În foarte multe cazuri, dispozitivele robotizate autonome pot fi cea mai bună opțiune
pentru rezolvarea unei probleme. Un exemplu considerat este reprezentat de metodele de
explorare și mapare a unui spațiu interior greu de accesat omului. Utilizarea unui robot mobil
scade riscul accidentarilor sau accidentelor umane și poate returna unui utilizator date exacte
despre mediul in care acesta a fost trimis. Echipat cu dispozitive senzoriale speciale, vehiculul
autonom sau comandat de un utilizator va scana mediul și va trimite informații prețioase. În
șcenarii precum incendii, clădiri afectate sau zone lipsite de orice sursă de lumină un astfel de
robot este soluția optimală
De asemenea, pot exista și locuri în care utilizarea unui robot este singura modalitate
viabilă de acces. De exemplu, centralele nucleare, fabricile de produse chimice toxice sau orice
mediu ce ar putea fi dăunător omului, pot fi explorate de vehicule comandate de la distaţă pentru
realizarea diferitelor operațiuni. Realizarea unei harți ne poate fi de mare ajutor în aceste situații
in care pentru un individ poate fi foarte periculos sau imposibil chiar să ajungă în zona
respectivă.
Problema dezvoltării unui robot mobil de acest gen implică stăpânirea conceptului de
localizarea şi mapare simultană. În robotică localizarea și maparea simultană (SLAM -
simultaneous localization and mapping) reprezintă problema computării, construirii şi
actualizării unei hărți a unui mediu necunoscut, urmărind simultan localizarea unui agent în
cadrul acesteia. Inţial această problema pare a fi o problema de tipul „oul sau găina?” intrucât
pentru a ne putea localiza într-un loc avem nevoie de o hartă a acelui loc și, viceversa , pentru a
putea face o hartă a unui loc trebui să știm unde ne aflăm relativ de acel loc. Există mai mulți
algoritmi cunoscuți pentru rezolvarea acestei probleme, cel puțin aproximativ si în timp tractabil
pentru anumite medii. Cele mai populare soluţii aproximative includ filtrul de particule, filtrul
Kalman extins și Rao-Blackwellized.
Popularitatea problemei SLAM este corelată cu apariția robotizării mobile interioare.
Utilizarea GPS-ului nu are o soluţie de a determina eroarea de localizare pentru utilizarea în
interior. În plus, SLAM oferă o alternativă atrăgătoare pentru hărțile construite de utilizatori,
arătând că operarea robotului este accesibilă chiar și în absența unei infrastructuri de localizare a
obiectivelor.
17
Implementarea şi testarea unui algoritm care să determine harta incaperii in care
se află şi poziţia relativ cu această hartă
Proiectul este structurat în 5 capitole, fiecare prezentând detalii cu privire la diferite aspect
teoretice sau practice. Capitolul 1 prezintă o introducere in temă a proiectului si noțiunile
teoretice necesare dezvoltării acestuia. In capitolul 2 se descrie implementarea hardware a
mobilului, capitolul 3 prezintă succint câteva noțiuni legate de filtrele de particule şi cum
funcționează ele pentru a calcula poziția și harta mediului în care mobilul nostru se află, capitolul
4 vobește despre implementarea software și corectarea unor erori din hardware, iar în ultimul
capitol sunt discutate rezultatele obtinute în urma testării şi eventualele căi de dezvoltare ale
proiectului.
18
Capitol 1 Noțiuni teoretice necesare dezvoltării practice
Vehiculul nostru utilizează calculatorul Raspberry Pi un SBC produs în Marea Britanie de către
Raspberry Pi Foundation cu scopul de a oferii o platformă de învatare a informaticii în scoli. Din toate
versiunile existente de Raspberry Pi am ales să foloseşte modelul 3 B care foloseşte un SoC Broadcom
BCM2837 cu 4 nuclee de procesare ARM Cortex-A53 cu o frecvenţă de 1.2Ghz fiind îndeajuns de
potent să prelucreze datele provenite pe diferite fire de execuţie eliminând astfel necesitatea
intreruperilor.Pe langă acestea, sistemul mai inglobeaza un procesor grafic VideoCore IV. [1]
O altă caracteristica extrem de folositoare proiectului a acestei ultime versiuni este adăugarea unui
BCM43438 Wi-Fi, cip folosit pentru interfaţarea cu orice dispozitiv ce e capabil să se conecteze la o
reţea WiFi .
La capitolul conectivitate Raspberry Pi dispune de:
Un port HDMI
patru port-uri USB 2.0
un jack audio
un port pentru alimentare pe micro-USB
un port microSD
Gigabit Ethernet pe interfata USB 2.0 (latime de banda maxima 300Mbps)
MIPI DSI display port
MIPI CSI camera port
40-pin GPIO header
Ceea ce îl face ideal pentru utilizarea în aplicaţiile embedded este portul GPIO ce conţine pini care
pot fi folosiţi pentru a citi date de la senzori sau pentru a comanda diferite module cum este în cazul
proiectului nostru driver-ul de motoare.
19
1.1.1 Descrierea succintă a portului GPIO
Vcc : Se împart la randul lor in pini de 3.3V şi pini de 5V . Ambele tipuri pot fi folosite
pentru alimentare .Cei de 5V se pot folosi şi pentru alimentarea microcalculatorului.
Gnd : Masa
GPIO : Pot fi setaţi în cadrul programelor ca pini de intrare, pentru citirea datelor în
format digital sau ca pini de ieşire pentru comanda altor module. De asemena dispun şi
de alte funcţii mai specifice precum PWM (hardware doar pe pinii 12,13,18,19),
SPI,Serial (TX-GPIO14, RX-GPIO15)
20
Figura 1.2 Schema bloc generală a microcontrolerului [3]
21
1.1.2.5 Memoria cache
L1 cache
Controlerul cache de nivel 1 (L1) generează semnalele de control asociate datelor și arbitrează
între diferitele surse care solicită acces la resursele de memorie. Cache-ul de date este setat în 4 direcții
asociative și utilizează o schemă de indexare fizică ce determină un managment precis al adreselor din
sistem.
L2 cache
Alaturi de memoria de sistem aceasta are rolul de a menţine coerenta datelor intre cele 4 nuclee
ale grupului. Este important ca datele din memoria cache de nivel L1 să coincidă cu cele aflate în cea
L2.
Cerințele de alimentare exacte depind în funcţie de aplicaţia în care se doreste a fi folosit SBC-
ul. În cazul în care acesta este neutilizată, este de obicei într-o stare de putere scăzută sau complet
dezactivată. Dacă aplicatia care rulează în momentul de fată nu utilizează grafică 3D, atunci o mare
parte din logica digitală de bază nu va porni niciodată și nu are nevoie de energie.Acest lucru este
valabil și pentru interfețele de cameră și de afișare, HDMI, interfețe USB, encodere video și decodoare
și așa mai departe.
Pinii GPIO pot fi utilizaţi şi ei pentru alimentarea altor module externe. Fiecare GPIO poate
furniza până la 16mA, curentul total pe bank nu trebuie să depășească 50mA.
Acestea fiind consemnate dacă alegem să alimentam SBC-ul via micro USB va trebui sa îi
viram acestuia 5V/2.5A , iar în cazul alimentarii via GPIO header 5V.
În ceea ce priveste temperatura de operare este intre 0–50°C. [1]
22
Figura 1.3 Simulare minimalistă a robotului in Gazebo
23
Figura 1.4 Raza şi deschiderea senzorului
Principiul are la bază transmiterea unui fasciul mic de lumină la o suprafață și măsurarea
timpului necesar pentru a reveni la sursă. Când o sursa de lumină străluceşte pe o suprafață ceea ce
ochiul uman percepe este de fapt este lumina reflectată și revenind la retină. Lumina călătorește foarte
repede - aproximativ 300.000 de kilometri pe secundă, 186.000 mile pe secundă sau 0.3 metri pe
nanosecundă, astfel încât o lumină aprinsă este instantanee. Desigur, nu este! Echipamentul necesar
pentru a măsura acest lucru trebuie să funcționeze extrem de rapid.
Instrumentul LiDAR emite impulsuri rapide ale luminii laser la o suprafață, unele cu până la
150.000 de impulsuri pe secundă. Un fotodetector de pe instrument măsoară durata de timp necesară
pentru ca fiecare puls să se întoarcă înapoi. Lumina se deplasează la o viteză constantă și cunoscută,
astfel încât instrumentul LiDAR poate calcula distanța dintre ea însăși și țintă cu o precizie ridicată.
Repetând acest lucru în succesiune rapidă, insturmentul construiește o "hartă" complexă a suprafeței pe
care o măsoară. Fotodetectorul este un dispozitiv care citește și înregistrează semnalul intors la senzor.
24
Cel mai adesea întâlnit senzor de lumină este fotodiodă ce convertește semnalul luminos în curent
electric.
Wi-Fi este un mediu de difuzare ,care spre deosebire de ethernet nu are nevoie de un mediu
fizic, și care permite oricărui dispozitiv să asculte și să participe la o comunacţie prin conectarea la un
acces point. Este asadar o tehnologie radio folosită deseori la implementarea rețelelor locale de
calculatoare de tip rețea locală fără fir. Un WLAN este un sistem de comunicații implementat ca
extensie la, sau ca alternativă pentru o rețea locală cablată, combinând conectivitatea la viteză mare cu
mobilitatea utilizatorilor, într-o configurație mult simplificată. Avantajele evidente, cum ar fi
mobilitatea, flexibilitatea, simplitatea în instalare, costurile de întreținere reduse și scalabilitatea, au
impus Wi-Fi și WLAN ca o soluție tot mai mult utilizată in domeniul sistemelor integrate.
Domeniul de semnal Wi-Fi depinde de banda de frecvență, puterea de ieșire a radioului,
câștigul antenei și tipul antenei, precum și tehnica de modulare. Linia de vizibilitate a reţelei ne dă aria
reţelei, dar reflexia și refracția pot avea un impact semnificativ.
Rețelele Wi-Fi utilizează tehnologii radionumit IEEE 802.11 pentru a furniza servicii sigure
si conectivitate fără fir rapidă. O configurație- tipică Wi-Fi include unul sau mai multe puncte de acces
(AP) și unul sau mai mulți clienți. Un AP difuzează SSID-ul său (identificatorul setului de servicii sau
"numele rețelei") prin pachete ce sunt difuzate de obicei la fiecare 100 ms. Pachetele de identificare
sunt transmise la 1 Mbit / s, și sunt relativ de durată scurtă și, prin urmare, nu au un efect semnificativ
asupra performanței. 1 Mbit / s este cea mai mică rată care asigură clientul ca datele primite de la
reţeaua Wi-Fi sunt corecte. Pe baza setărilor, clientul poate decide dacă se va conecta la un AP.
Rețelele Wi-Fi funcționează la 2,4 GHz și 5 GHz, cu o frecvență de 11 Mbit / s sau 54 Mbit / s . Pentru
aplicații de tip embedded în care se doreşte trimiterea unor directive și interpretarea unor percepții
aceste viteze sunt adesea suficient de mari. [6]
1.4 Locomotia vehiculului
Vehiculul a fost realizat sub forma unui robot mobil cu locomoție diferențială concepută pe baza
unui șasiu cu 2 roți laterale și a unei roti cu rulment plasata anterior pentru echilibrarea întregului
sistem. Deplasarea sistemului diferențial a fost realizată cu ajutorul celor două motoare de curent
continuu”.
25
Figura 1.5 Motor curent continuu
26
I. Cazul S = D
Când cele două roți se rotesc în aceeași direcție și cu aceeași viteză, robotul se va deplasa înainte
sub forma unei linii dreapte.
II. Cazul S < D
Când cele două roți se rotesc în aceeași direcție, dar cu viteze diferite, robotul se va roti spre
direcția motorului cu viteza mai mică. De exemplu, în cazul în care roata din dreapta se rotește cu o
viteză mai mare față de cea din stânga, robotul va vira spre stânga.
III. Cazul S = ─ D
Când cele două roți se rotesc cu aceeași viteză, dar în direcții opuse, robotul se va roti în jurulu axei
proprii pastrânduşi aceeaşi poziţie.Numele de sistem diferențial provine din modul în care robotul este
dirijat prin schimbarea vitezei și a direcției prin diferența dintre cele două roți .
Conform structurii de mai sus, când comutatoarele S1 și S4 sunt închise, o tensiune pozitivă va
fi aplicată la terminalele motorului. Prin deschiderea comutatoarelor S1 și S4 și închiderea
comutatoarelor S2 și S3, această tensiune este inversată, permițând operarea inversă a motorului.
Comutatoarele S1 și S2 nu trebuie să fie închise în același timp, deoarece acest lucru ar provoca un
27
scurt circuit pe sursa de alimentare. Aceași condiție trebuie să fie îndeplinită de asemenea și de
comutatoarele S3 și S4. Functia de baza realizata de o punte H este de a inversa directia de deplasarea a
motorului, insa ea poate efectua si operatiuni precum franarea sau mersul in gol al acestuia. Următorul
tabel sumarizează modurile de funcționare a punții H în funcție de configurația celor 4 comutatoare:
S1 S2 S3 S4 Rezultat
1 0 0 0 Sens normal de mișcare
0 1 1 0 Sens invers de mișcare
0 0 0 0 Mers in gol
0 1 0 1 Frână
1 0 1 0 Frână
1 1 0 0 Scurt circuit alimentare
0 0 1 1 Scurt circuit alimentare
0 0 0 0 Scurt circuit alimentare
Tabela 1.1 Moduri de funcționare a unui circuit de tip punte H
28
În funcție de configurația aleasă pentru cele 2 semnale de intrare,”IN1” și “IN2”, se pot obține 4
moduri de control:
Sens de rotatie orar
Sens de rotatie anti-orar
Frana scurta
Mod de oprire
IEŞIRE
Intrare 1 Intrare 2 PWM Ieşire 1 Ieşire 2 Mod
H H H/L L L Frână scurtă
L H H L H Sens de rotaţie anti-orar
L L L Frână scurtă
H L H H L Sens de rotație orar
L L L Frână scurtă
L L H Oprit Oprit
Tabela 1.2 Tabel de adevăr pentru circuitul integrat
Simbol Comentarii
L Tranzistor inferior în conducţie, tranzistor superior în blocare
H Tranzistor inferior în blocare, tranzistor superior în conducţie
Tabela 1.3 Semnificația simbolurilor L ,H
29
Figura 1.9 Forma de undă a unui semnal PWM [11]
Formula folosită pentru a obtine valoarea factorului de umplere (D) este următoarea:
TON Latimea pulsului
D= ∗ 100% = ∗ 100%
TON + TOFF perioada
unde :
TON este perioada de timp corespunzatoare valorii ON,tensiune de 5V
TOFF este perioda de timp corespunzatoare valorii OFF,tensiune de 0V
Pe măsură ce se rotește discul, aceste sectoare decupate în interiorul discului vor permite
trecerea fluxului luminos spre fotodector, generând astfel un semnal de ieșire digital. Un encoder
incremental simplu, construit dintr-un singur detector nu ne poate oferi informația necesară obținerii
unei poziții absolute a axului motorului. [12]
30
Pentru a determina atât direcția cât și viteza de rotație a motorului se folosesc traductoare cu 2
canale, numite encodere în cuadratură. Acest tip de traductor utilizează doi senzori pentru detecția
luminii și 2 rânduri de sectoare opace decupate pe disc. Cel mai frecvent tip de traductor incremental în
cuadratură folosește două canale de ieșire ( A și B), pentru a determina poziția. Utilizând două sectoare
de cod, defazate cu 90 ̊ unul față de altul, vom determina atât poziția cât și direcția motorului, din
ieșirile celor două canale.
Dacă canalul A precede canalul B, discul se va roti în sensul acelor de ceasornic, iar în cazul în
care canalul B precede canalul A, deplasarea se va realiza în sens trigonometric(invers acelor de
ceasornic). Monitorizarea atât a numărului de impulsuri cât și a fazei relative a canalelor A și B,va
permite determinarea poziției și direcției de mișcare a discului rotativ.
În plus, unele encodere incrementale, includ și un al treilea canal de ieșire numit canal de
referință sau “semnal de zero”. Acest canal de referință determină doar un singur impuls digital pe
rotație, și poate fi utilizat pentru determinarea precisă a unei poziții de referință.
Atunci când este necesară o rezoluție mai mare, se va implementa un numărător ce va contoriza
atât fronturile pozitive cât și cele negative ale trenului de impulsuri generat de un singur canal, ceea ce
va dubla numărul de impulsuri pe rotație (x2). Numărarea ambelor fronturi ale trenului de impulsuri,
ale ambelor canale va conduce la un număr de 4 ori mai mare de impulsuri (x4).
31
Figura 1.13 Determinarea numărului de impulsuri [12]
32
Capitol 2 Implementarea hardware a vehiculului
In ceea ce privește proiectul de fața țin sa menționez că multe din componentele utilizate nu sunt
foarte precise ceea ce a afectat rezultatul final al masuratorilor efectuate de către acesta. S-a incercat
compensarea pe cât posibilă a neajunsurilor hardware prin corecții software.
Urmează să oferim o descriere amănunțită a designului funcțional al vehicului. Pentru alimentarea
driver-ului de motoare este folosit un acumulator reîncărcabil de 2200mAh în tehnologia litiu-ion.
Tensiunea nominală oferită de acest acumulator este de 11.1 V volți fiind alcătuit din trei celule
înseriate cu o capacitate de 3.7 volți fiecare, și cu o rată de descarcare de 20C. Alimentarea SBC-ului se
realizează separat de la o baterie externă ce scoate 5V și 1.5A, fiind indeajuns pentru a alimenta
senzorul LIDAR , cele 2 encodere de pe roți și pentru semnalele de comandă către driver-ul de
motoare.
Din motive de siguranță s-a ales adăugarea unei baterii externe în implementare. Modulul utilizat ce
conține driver-ul L298 are o ieșire de 5V ce in teorie ar fi fost suficientă alimentarii Raspberry Pi-ului
insă am constatat că amperajul dat de regulatorul de tensiune al modulului nu era destul de mare pentru
a susţine intregul sistem. Dat fiind costul financiar ridicat al senzorului cu care lucrăm alimentarea
prin portul GPIO ar fi reprezentat un risc intrucât aceștia nu au nici o protectie la supracurent riscând
sa ard placa și senzorul o dată cu acesta.
Plăcuța folosită pentru controlul motoarelor are la bază un circuit integrat de tip punte H. Tensiunea
logică de control a circuitului integrat este furnizată de pinii 7,11,13 și 15 ai SBC-ului, iar tensiunea de
alimentare a motoarelor vine direct de la bateria litiu-ion. Rolul punții H este de a controla viteza și
direcția celor două motoare. S-a aplicat un control PWM software cu un factor de umplere mic de la
Raspberry Pi pentru a nu distruge motoarele din cauza supratensiunii.
33
Comunicația dintre robot și laptop a fost realizată folosind un modul BCM43438 WiFi. Acest
modul a fost folosit pentru partajarea resurselor SBC-ului cu calculatorul, cât și pentru programarea,
dezvoltarea și testarea acestuia. Acest lucru permite ca robotul să fie controlat cu orice dispozitiv
capabil să se conecteze la un punctul de acces al reţelei Wi-Fi al modulului menţionat anterior, insă
pentru a obţine datele legate de poziţia curentă a robotului în incapere vom avea nevoie de o masină
care să ruleze şi ea ROS. Adresele IP au fost atribuite static fiecărei maşini în parte. Pentru a
iniţializa procesele de achiziţie a distanţelor de pe LIDAR şi de control al motoarelor, respectiv
publicare a datelor odometrice se poate folosi o conexiune SSH.
2.1 Sistemul de control
Controlul este de tip supervizat (intermitent, sau semiautonom), robotul fiind comandat printr-
o telecomanda, dar adaptandu-și comportamentul și în funcție de datele provenite de la senzori. Un
astfel de control e analog cu intercaţiunea supervizorului cu echipa subordonată.
Principiu
operatorul partajează o problema intr-o secvenţa de sub-taskuri
robotul le execută independent (fără asistenta)
b) Informaţii transmise de Percepţie H-L si Percepţie L-L referitoare la cele două nivele ale
interpretarii senzoriale. În cazul de față reglează vitezele celor 2 motoare şi le oprește dupa ce
robotul a parcurs distanța dorită.
35
2.2 Şasiul robotului
Toate elementele funcţionale prezentate anterior în diagrama bloc din Figura 2.1 au fost
montate pe şasiul din Figura 2.3 .
Singurele limitări impuse de aceasta platformă au fost pozitia motoarelor şi spaţiul redus
pentru dimensiunea componentelor utilizate. A fost adaugată încă o plaforma pentru montarea
SBC-ului şi a beteriei externe, iar în ceea ce priveşte laserul a fost proiectată şi construită cu
ajutorul unei imprimante 3D o platfromă. Amplasarea finală pe şasiul robotului se poate vedea in
anexa 1.
Poziţia laserului pe saşiu era importantă datorită ipotezelor odometrice ce s-au făcut în cod.
Pentru a presupune cu o eroare relativ mică că în cazul unei rotaţii a mobilului la stânga sau la
dreapta , acesta rămane in acelaşi loc in cameră şi doar orientarea lui se schimbă el trebuie sa fie
amplasat paralel cu cele 2 roţi ce asigură tracţiunea robotului, lucru ce interfera cu encoderele de pe
motoare.
Prevederi constructive:
Masa totală: 735 g
Lungime maximă:26 cm
Lăţime maximă:18 cm
Înaltime maximă:15 cm
Figura 2.4 Formele PWM de undă pentru mişcarea rectilinie, respectiv cea de rotaţie
Pentru a echilibra șasiul, robotului i se mai adaugă in partea dorsală un punct de sprijin ce
poate fi, incă o roată care este prinsă cu ajutorul unui rulment pentru ai oferi un grad de libertate sau
o bilă cu mișcare liberă. S-au facut incercări atât cu un suport cu bilă , cât si cu o roata prinsă cu
rulment.
38
Capitol 3 Localizare şi mapare simultană
Modelarea mediilor în care operează un robot mobil presupune determinarea formelor,
poziţiilor şi dimensiunilor obiectelor şi obstacolelor precum: pereţi, stalpi, obiecte de mobilier etc.
Utilizarea distanţelor rezultate dintr-o înregistrare laser stau la baza elaborării unei hărţi necesare
pentru planificarea traseelor de navigare. Problema navigării roboţilor mobili, inclusiv a celor
industriali, în mediile care operează, presupune parcurgerea următoarelor etape: localizare (absolută
sau relativă) bazată pe senzori (percepţie), generare hartă locală (mapare), planificarea traseului
(căii) şi urmărirea acestuia. În practica modelării mediilor de operare primele două etape sunt strans
legate. Pentru a realiza localizarea mobilului avem nevoie de o hartă a mediului şi pentru a
determina o hartă este necesar sa ştim unde ne situăm relativ cu mediul, o problemă de tipul ou sau
găina.
SLAM este alcatuit din mai multe părți. Extracția punctului de referință, asocierea datelor,
estimarea stării actuale a mobilului, actualizarea stării și actualizarea punctului de reper. Există
multe modalități de a rezolva fiecare parte. Asta înseamnă ca şi aceste parţi ale algorimului pot fi
înlocuite la răndul lor de un nou mod de a obţine rezultatele necesare cartografierii. In acest proiect
am decis să ne concentrăm asupra unui robot mobil într-un mediu interior. SLAM este aplicabil atât
pentru mișcarea 2D cât și 3D. Vom analiza numai 2D mişcare. [14]
3.1 Odometrie
Cuvântul "odometrie" este alcătuit din cuvântul grecesc odos ,adică "traseu", și metron ,adică
"măsură". Odometria este utilizarea datelor de la senzorii de mișcare pentru a estima schimbarea
poziției în timp. Este folosit în robotică de către niște roboți cu picioare sau roți pentru a-și estima
poziția față de o locație de plecare. Această metodă este sensibilă la erori datorită integrării
măsurătorilor de viteză în timp pentru a da estimări ale poziției. Colectarea rapidă și precisă a
datelor, calibrarea instrumentului și prelucrarea sunt necesare în majoritatea cazurilor pentru
utilizarea eficientă a odometriei. [15]
În navigație, odometria reprezinta utilizarea datelor de la actuatorii de mişcare pentru a estima
schimbarea poziției în timp prin dispozitive precum encodere rotative pentru măsurarea rotației
roților. Aceasta metodă este utilă pentru majoritatea vehiculelor pe roți sau pe șenile insă tehnicile
tradiționale de odometrie nu pot fi aplicate roboților mobili cu metode de locomoție nestandard,
cum ar fi roboții cu picioare. În plus, odometria suferă în mod universal de probleme de precizie,
deoarece roțile tind să derapeze și să alunece pe podea, creând o distanță neuniformă parcursă în
comparație cu rotația roților. Eroarea se complică atunci când vehiculul funcționează pe suprafețe
văluroase,accidentate. Măsurătorile odometrice devin din ce în ce mai nesigure, aceste erori
acumulânduse în timp.
Odomometria vizuală este procesul de determinare a informațiilor echivalente de odometrie
folosind imagini secvențiale ale camerei pentru a estima distanța parcursă. Odomometria vizuală
permite o precizie sporită de navigație în roboți sau vehicule care utilizează orice tip de locomoție
pe orice suprafață. [15]
u1:T={u1,u2,u3…..,uT}
Observaţiile laser-ului
z1:T={z1,z2,z3…..,zT}
Dorim sa obţinem:
Harta mediului
m
Drumul robotului
x0:T={x0,x1,z2…..,xT}
Ultimele 2 marimi conţin erori mai ales pe perioadă indelungată de utilizare şi care în timp
se vor acumula şi vor rezulta intr-o percepţie eronată a realitaţii pentru mobilul nostru. Aşa cum s-a
menţionat anterior în acest capitol, erorile pot fi datorate mecanismului de locomoţie şi a
instrumentului de determinare a distanţelor. Folosirea unei motode probabilistice pentru a ne
apropia de adevarata hartă a mediului a fost necesară în acest caz.
Utilizarea metodei probabilistice modifică asumţia “Robotul este exact aici” în “Robotul
este undeva aici”. Utilizand o distribuţie Gaussiana putem privi poziţia robotului ca pe o
probabilitate şi nu ca pe o certitudine lucru ce ne va ajuta să remediem eventualele erori ale
întregului mecanism mecanic. [16]
40
Figura 3.1 Pozitia in spaţiu a mobilului înainte si după metoda probabilistică [16]
Pentru fiecare actualizare a harţii p, sau particula, codifică ipoteza ponderată a poziţiei
robotului şi modelul harţii.
41
3.4 Filtrul de particule Rao-Blackwellized
Pachetul Gmapping utilizat in aplicația noastră folosește pentru obținerea harții un filtru de
particule Rao-Blackwellized care spre deosebire de algoritmul prezentat la punctul anterior are
câteva particularitați.
Algorimul este capabil să facă corespondența intre scanarile făcute din aceeși poziție la
momente diferite de timp pentru a corecta scurte secvențe odometrice. Acest lucru devine util mai
ales când spațiul explorat are o structură circulară si mobilul poate realiza mai multe ture în mediu,
calitatea informației crescând la fiecare tură.
Aplicația este compusă din mai multe soft-uri ce vor rula separat și care pot rula independent
unele faţă de celelalte. O parte din acestea se regasesc în pachetele ROS, fiind open-source, și au
fost instalate pe distribuția noastră. Parțile de cod care se vor descrie in detaliu sunt cele pe care le-
am conceput pentru a putea naviga mobilul de la distanță și pentru a obține informații legate de
poziția sa.
4.1 Sistemul de operare al robotului
Prezentul capitol are ca scop să ofere o descriere a instrumentului care a stat la baza întregii
functioanlitaţi a robotului. ROS sau sistemul de operare pentru roboți este o extensie a sistemului de
operare deja prezent pe o mașină, în cazul acesta este vorba de Raspian, ce oferă servicii pentru
aplicații software peste cele deja disponibile pe distribuția nativă a Raspberry Pi-ului.
Deși puterea de calcul a unui Raspberry Pi este superioară față de cea unui microcontroler
obișnuit, pentru realizarea aplicaţiei a fost necesară partajarea resurselor unui calulator cu cele ale
SBC-ului. Astfel s-a ajuns ca robotul să realizeze funcțiile de bază precum comandarea driverului
de motoare, citirea și prelucrarea datelor de pe senzorii atașați, iar calculatorului îi revin cele mai
costisitoare sarcini precum calcularea hârţii și vizualizarea ei pe baza datelor primite de la robot.
Deși ROS nu este un sistem de operare în adevaratul sens al cuvantului, precum Linux sau
Windows, acesta oferă servicii concepute pentru clusterul heterogen al calculatorului, cum ar fi
abstractizarea hardware-ului, controlul dispozitivului la un nivel scăzut, asigurarea comunicarii intre
diferite procese ,indiferent dacă acestea rulează pe maşini diferite și gestionarea pachetelor de date
transmise intre acestea. Procesele sunt numite în arhitectura ROS noduri, iar acestea sunt capabile
de a receptiona,posta şi multiplexa date legate de senzori, control, stare, planificare, actuatori sau
alte informaţii. Astfel se poate crea o rețea in care toate procesele sunt conectate intre ele. Orice nod
poate accesa rețeaua, interacționa cu celelalte noduri, utiliza informația pe care acestea o postează și
transmite propiile date in rețea.
Pentru a putea vedea harta actualizată în timp real am utilizat Rviz, un instrument din suita
ROS ce transpune datele numerice returnate de pachetul Gmapping intr-un format vizual uşor de
inteles pentru orice utilizator. Alături de Gmapping, Rviz este un pachet pe care îl putem instala
prin intermediul ROS.
4.1.1 Pachetele
Software-ul în ROS este organizat în pachete. Un pachet poate conține noduri ROS, biblioteci
independente de ROS, un set de date, fișiere de configurare, o bucată de software terță parte sau
orice altceva care constituie în mod logic un modul util. Scopul acestor pachete este să furnizeze
această funcționalitate utilă într-o manieră ușor de consumat, astfel încât software-ul să poată fi
reutilizat cu ușurință. În general, pachetele ROS respectă un principiu "Goldilocks": suficientă
funcționalitate pentru a fi utilă, dar nu prea mult ca pachetul să fie greu și dificil de folosit de alte
programe.
Pachetele sunt ușor de creat manual sau cu unelte cum ar fi catkin_create_pkg. Un pachet ROS
este pur și simplu un director derivat din ROS_PACKAGE_PATH care are un fișier package.xml în
el. Pachetele sunt cea mai atomică unitate de construcție și unitatea de eliberare. Acest lucru
43
înseamnă că un pachet este cel mai mic lucru individual pe care îl puteți construi în ROS și este
modul în care software-ul este asociat pentru instalare/downloadare exact cum ați instala un soft din
linia de comandă.
In ROS pachetele tind să urmeze o structură comună. Vom mentiona cele mai importante
directoare și fișiere prezente intr-un pachet.
include / nume_pachet: fisiere C++ de tip header
msg : Folder care conține mesaje de obicei definite de utilizator și structura acestora.
src/package_name : Fișierele sursă Python sau C++ care sunt exportate în alte pachete.
srv: Folder care conține tipuri de servicii.
scripturi: script-uri executabile scrise in Python.
CMakeLists.txt: Fișierul CMake build pentru construirea de pachete software.
package.xml: definește proprietățile pachetului, cum ar fi numele pachetului, numerele
de versiune, autori, întreținere și dependențe de alte pachete
Nodurile
Nodurile sunt procesele unde calculele sunt efectuate. Dacă dorim ca procesul nostru să
comunice cu alte noduri, suntem nevoiți sa cream un nod cu acest proces cu care să ne conectam
la rețeaua ROS. In mod uzual este bine sa avem cat mai multe noduri care să efectueze sarcini
simple pentru a asigura modularitatea si robustețea sistemului. O dată creat nodul el este
indepent din punct de vedere al funcționalitații fața de restul proiectului, el nefolosind metode
sau funcții incluse în alte fișiere,lucru ce permite reutilizarea lui în alte proiecte. ROS oferă
suport pentru limbajele de programare Python și C++. Nodurile sunt scrise cu librarii propietare
ROS cum ar fi roscpp și rospy. [18]
44
Master-ul
Acesta pune la dispoziție intregii arhitecturi numele și serviciul de cautare pentru restul
nodurilor. De asemenea stabilește conexiunile între noduri. Daca acesta nu este în sistem
comunicația cu nodurile, servicile si mesajele nu va putea fi posibila. Pentru a inițializa master-
ul se scrie comanda “roscore” într-un shell de comandă linux. Dacă sistemul este alcatuit din
mai multe computere aflate intr-o retea WLAN sau LAN vom avea Master-ul doar pe un singur
calculator. [18]
Serverul pentru parametri
Serverul pentru parametri ne oferă posibilitatea de a utiliza chei pentru a stoca date într-o
locaţie centrală. Cu aceste date stocate, avem posibilitatea de a configura comportamentul
nodurilor în timp ce se execută sau pentru a schimba parametrii de lucru a unui nod. [18]
Mesajele
Nodurile comunică intre ele cu mesaje. Mesajele reprezinta datele care sunt oferite altor
noduri din reţea. Un mesaj poate fi o structură de date simplă precum: Integer, Float, Byte,
Boolean, String sau poate avea o structură de date compusă din cele primitive. ROS pune la
dispoziție multe tipuri de mesaje, insă ele pot fi de asemenea dezvoltate de utilizator in funcție
de necesitațile proiectului. [18]
Topice
Fiecare mesaj trebuie să poarte un nume ca el sa poată fi direcționat de către rețea. Cănd un
nod trimite date in rețea se numește ca publică pe un topic. Cănd un nod citește date de pe o
rețea acest lucru se numește că subscrie la un topic. Un nod poate subscrie la un topic chiar dacă
nimeni nu publică date pe acel topic. Este important ca numele topicului sa fie unic pentru a nu
apărea confuzii intre mesajele de pe rețea. [18]
Servicii
Serviciile au apărut ca necesitate de a cere informații de la un nod. Deși topicele pot trimite
datele în multe moduri, un nod subscris la un topic nu poate sa ceară date. La fel ca topicele,
serviciile trebuie sa aibă un nume unic. Orice nod din rețea poate cere informații de la un nod ce
administreaza un serviciu lucru care asigură interacțiunea intre noduri. [18]
45
Exemplu de fisier tip .launch: [18]
<?xml version="1.0"?>
<launch>
<node name ="example1_a" pkg="chapter2_tutorials" type="example1_a"/>
<node name ="example1_b" pkg="chapter2_tutorials" type="example1_b"/>
</launch>
Pentru a se identifica nodurile pe care dorim să le lansăm în interiorul unui fișier de acest tip
este necesar să mentionam numele nodului, adică numele fișierului în care se află codul, pachetul în
care vom gasi nodul menționat anterior și numele nodului plus extensia fisierului .cpp sau .py
Orice modificari ulterioare asupra codului, o dată incărcat pe SBC, se făceau tot prin
intermediul laptopului insă printr-o conexiune SSH utilizând editorul de text VIM. VIM este un
editor de text extrem de configurabil construit pentru a permite editarea eficientă a textului. Este o
versiune îmbunătățită a editorului vi distribuit cu majoritatea sistemelor UNIX. Acesta este
proiectat pentru utilizare atât de la o interfață de linie de comandă, cât și ca aplicație independentă
într-o interfață grafică de utilizator, insă pentru proiectul de fata, editarea in linie de comanda s-a
dovedit extrem de utilă scutind necesitatea unor periferice suplimentare pentru Raspberry Pi.
47
In diagrama următoare sunt prezentate in dreptunghi-uri topicele active și în ovale nodurile.
48
Figura 4.4 Scan topic
Cele 2 funcții au fost implementate în cadrul aceluiași nod. Contrar ideologiei menționate
anterior de a ține nodurile cât mai simple cu putința și dedicate unei singure funcții, acest lucru nu a
fost posibil in situația de fața intrucât ambele funcții se folosesc de portul GPIO al Raspberry Pi-
ului iar acesta putea fi inițializat o singură dată. Ambele funcții utilizeaza metodele de citire a
encoderelor pentru a-și îndeplini sarcina și ambele sunt dependente de parametrul “directive” al
clasei Motor. Acest nod este subscris topicului direction pe care sunt publicate mesaje precum
“W,A,S,D” ce modifică directiva actuală nodului. Aceste mesaje reprezintă comenzile date de
utilizator pentru deplasare față, spate, rotorie stânga și rotire dreapta. Nodul a fost conceput ca o
clasă in care sunt ințializați mai mulți parametri ce țin de inițializarea pinilor, controlul în PWM al
motoarelor, calibrarea motoarelor și poziția inițiala a mașinii. Dupa ce a fost creat un obiect din
aceasta clasă cu o frecvența bine stabilită sunt apelate funcții ale obiectului.
4.3.2.1 Navigația
Funcțiile dedicate navigației preiau datele provenite de pe nodul de comandă ce rulează pe
laptop și efectuază deplasările. Deplasările se efectuează pe distanța fixă, aceasta fiind masurată cu
ajutorul encoderelor optice de pe motoare. De asemenea aceasta se asigură că cele 2 motoare se
rotesc cu aceeași viteză indiferent de directiva primită.
Pentru a ne asigura că roțile se deplasează cu aceeași viteză am utilizat o metodă de
controlul derivat proporțional. Folosim un motor ca motor “Master", la care se aplică o putere
constantă și un motor "slave" caruia iniţial i se va da aceeaşi putere,dar pe care o vom putea varia
ulterior a ne asigura că merge cu aceeași viteză ca primul motor.
Diferența dintre viteza master-ului și viteza slave-ului se numește "eroare". Dacă se întâmplă
exact același ritm, valoarea erorii va fi zero. Dacă slave-ul este prea lent, eroarea este pozitivă. Dacă
slave-ul merge prea repede, eroarea este negativă. Eroarea este definită ca:
Eroare = Viteza master - Viteza slave.
Se înmulțeste eroarea cu o valoare kp. Dacă kp subunitară pentru a nu supra compensa
viteza slave-ului și pentru a tinde către o diferență de viteză nula intr-un numar redus de calibrări.
49
Eroarea gasita în momentul de timp anterior se v-a inmulți cu un coeficient kd iar produsul obținut
reprezintă componenta derivativă. Viteza slave-ului va fi astfel incrementată cu cele 2 componente,
iar diferența dintre cele 2 motoare va tinde conform graficului de mai jos.
50
𝑍𝑢𝑛𝑔ℎ𝑖𝑢𝑙𝑎𝑟 = 𝑍𝑢𝑛𝑔ℎ𝑖𝑢𝑙𝑎𝑟 + 𝑣𝑍𝑡ℎ
unde vX si vZth sunt valorile fixe menționate anterior. Z liniar ,x unghiular si y unghiular vor
ramane 0. Pe langă acestea pe topicul tf se mai postează si date ce ţin de momentul achiziției acestor
date.
S-a ales numarul de particule din filtru și de asemenea s-au setat parametrii ce țin de distanța
maxima la care laserul poate să bată. Fiecare particulă reprezintă un eşantion ce conţine poziţii ale
robotului corelate cu scanări din acea instanţă oferind astfel un istoric. Codul din fişierul tip .launch
utilizat pentru a lansa nodul gmapping se poate găsi în anexa 5.
52
Capitol 5 Testarea vehicului
Pentru verificarea funcționalității implementării hardware și software a fost necesară utilizarea
mobilului în diferite scenarii pentru a compara raspunsul robotului cu modelul real al mediul.
Fiecare din mediile alese pentru testare prezintă particularitați ce testează pe rănd sau simultan
anumite aspecte ale navigaţiei și determinării poziției. Se menționeaza ca datorită suprafețelor
diferite pe care robotul s-a deplasat anumite modificări în parametrii ce țin de controlul motoarelor
au fost efectuate pentru a-l adapta. Suportul mecanic este imprevizibil în ceea ce privește aderența
asa că pentru a obține o deplasare cât mai exactă viteza trebuie adaptată la mediu. Numarul de citiri
de pe encoder pentru 45 de grade a fost și el adjustat de la un caz la altul.
Vom realiza o legendă bazată pe culori a modelelor obținute de robot
Negru: Liniile negre reprezintă suprafețele de pe care fasciculul laser s-a reflectat înapoi la
sursa. Cu negru vom găsi conturate toate suprafețele obiectelor întalnite de mobil.
Alb: Culoarea indică faptul că spațiul a fost explorat și este liber, nici un obiect solid nu se
alfă in acea zonă
Cenușiu: Reprezintă spațiul neexplorat de către robot. Este posibil să întalnim mici pete
cenușii pe zonele albe. Acestea artefacte sunt datorate nepotrivirilor la un moment de timp T
dintre mesajele de scanare și cele de odometrie.
53
Figura 5.2 Scenariu 1: Model obținut
55
Figura 5.6 Scenariu 3: Model obținut
56
Concluzii
Această lucrare prezintă dezvoltarea unui vehicul capabil să ofere infomaţii utilizatorului
despre poziţia şi structura mediului în care îl folosim. Au fost accentuate diversele probleme tehnice
apărute pe tot parcursul implementării și prezentate soluțiile de rezolvare găsite. Imagini ale
dezvoltării practice le puteți găsi in Anexa 1.
Testarea și calibrarea vehicului a fost realizată urmărind mai multe direcții de dezvoltare și
utilizare. Rezultatele testelor realizate sunt satisfăcătoare și în concordanță cu obiectivele propuse la
începutul proiectului. Conform acestora, vehiculul poate fi utilizat ca robot de recunoaștere in
timpul operațiunilor de salvare și transmiterea de informații către o bază de control aflată la
distanță.
Proiectarea și implementarea vehicului s-a axat pe obţinerea unor rezultate cat mai precise cu
putinţă utilizând componente cu un cost redus şi cu performanţe mediocre. S-au testat pe rănd mai
multe soluţii atât hardware cât şi software şi s-au ales din acestea cele care per total au avut cele mai
bune rezultate în aplicaţia noastră. Rezultatele obținute in urma testării aplicaţiei insuflă increderea
și convingerea utilizarii unui astfel de mobil în misiuni de recunoaştere a mediului.
Vehiculul este capabil să mențină o comunicație activă cu un computer echipat cu un modul
Wi-Fi, distanța maximă testată pentru menținerea unei conexiuni active intre vehicul și laptop fiind
de 10m.
Pentru a avea un astfel de vehicul capabil să exploreze orice fel de teren ar fi necesar un kit
robotic mai performant care sa pună la dispoziţie facilitaţile de natura mecanică asemanătoare cu ce
vedem pe maşinile de teren.
Eventuale îmbunătățiri ale sistemului :
Utilizarea unor senzori suplimentari pentru determinarea odometriei cum ar fi un senzor
giroscopic sau un accelerometru cu care sa prevenim înregistrarea datelor odometrice atunci
când robotul s-a ciocnit de ceva
Utilizarea unor şenile in loc de roţi pentru un control mai precis in momemntul rotaţiilor si
pentru o centrare mai eficientă al senzorului LIDAR
Echiparea vehicului cu o cameră de filmat pentru transmiterea in timp real de imagini
Utilizarea unui șasiu cu sistem de suspensie cu amortizoare pentru deplasarea indiferent de
caracteristicile mediului
Utilizarea unei singure surse de alimentare pentru Raspberry pi şi sistemul de deplasare
57
58
Bibliografie
59
[18] L. S. E. F. A. M. Anil Mahtani, Effective Robotics Programming with ROS, 2016.
60
Anexa 1 Imagini cu realizarea practică
61
Anexa 2 Cod sursă –Comanda navigației
#!/usr/bin/env python
#motorp.py
import rospy
import curses
from std_msgs.msg import String
screen = curses.initscr()
curses.noecho()
curses.cbreak()
screen.keypad(True)
rospy.init_node('Motor_Publisher')
pub = rospy.Publisher('direction', String, queue_size=10)
print 'Enter W/A/S/D to control the robot: '
while not rospy.is_shutdown():
rate = rospy.Rate(1)
print 'Input:'
try:
data = chr(screen.getch())
except:
data= 'y'
print('Invalid input')
data = data.upper()
print data
if data in ("W","S","A","D","Q"):
pub.publish(data)
rate.sleep()
62
Anexa 3 Cod sursă – Navigația și determinarea distanței parcurse
de vehicul
#!/usr/bin/env python
#motorp.py
import time
import os
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist, Quaternion, TransformStamped, Point
from nav_msgs.msg import Odometry
import RPi.GPIO as GPIO
import sys, math, tf
GPIO.cleanup()
GPIO.setmode(GPIO.BOARD)
GPIO.setup(7,GPIO.OUT)
GPIO.setup(11,GPIO.OUT)
GPIO.setup(13,GPIO.OUT)
GPIO.setup(15,GPIO.OUT)
GPIO.setup(40,GPIO.IN)
GPIO.setup(32,GPIO.IN)
class Motor():
def __init__(self):
sub = rospy.Subscriber('direction', String, self.callback)
self.d=0
#setup pwm
self.fs=GPIO.PWM(11,50)
self.fd=GPIO.PWM(15,50)
self.ss=GPIO.PWM(7,50)
self.sd=GPIO.PWM(13,50)
self.state=self.directive='X'
self.master=self.slave=0
self.kp=0.2
self.kd=0.1
self.rot=0.3925
self.trans=0.50
self.ticks=0
self.error=0
self.impD=self.impS=0
63
self.readS=GPIO.input(40)
self.readD=GPIO.input(32)
self.distS=0
self.distD=0
self.speed=0
self.turnspeed=0
self.nt=time.time()
self.pub_odom = rospy.Publisher('odom', Odometry, queue_size=10)
self.bc_odom = tf.TransformBroadcaster()
self.lastpost=time.time()
self.x, self.y, self.th = 0.0, 0.0, 0.0
self.vx, self.vth = 0.0, 0.0
self.actual=False
self.cur_time = rospy.Time.now()
self.last_time = self.cur_time
def distanceS(self):
read=GPIO.input(40)
if read==self.readS:
return 0
else:
self.readS=read
self.impS+=1
return 1
def distanceD(self):
read=GPIO.input(32)
if read==self.readD:
return 0
else:
self.readD=read
self.impD+=1
return 1
def speedCalibration(self):
self.distD+=self.distanceD()
self.distS+=self.distanceS()
t=time.time()
dt=t-self.nt
if dt>0.03:
speedS=self.distS/dt
64
speedD=self.distD/dt
print ("viteza pe roata dreapta este: %d iar pwm-ul e de %d iar pe roata stanga este: %d iar pwm-ul e
de %d " % ( speedD,self.slave,speedS,self.master))
prev_error=self.error
self.error=speedS-speedD
self.slave+=self.error*self.kp + prev_error*self.kd
if self.slave>100:
self.slave=99
elif self.slave<1:
self.slave=1
self.distS=self.distD=0.0001
self.nt=time.time()
def send_odom(self):
self.cur_time = rospy.Time.now()
currentTime=time.time()
self.x += self.vx * math.cos(self.th)
self.y += self.vx * math.sin(self.th)
self.th += self.vth
q = tf.transformations.quaternion_from_euler(0, 0, self.th)
self.bc_odom.sendTransform((self.x,self.y,0.0), q, self.cur_time,"base_link","odom")
odom = Odometry()
odom.header.stamp = self.cur_time
odom.header.frame_id = "odom"
odom.pose.pose.position = Point(self.x,self.y,0)
odom.pose.pose.orientation = Quaternion(*q)
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = self.vth
self.pub_odom.publish(odom)
self.vx, self.vth = 0.0, 0.0
def forward(self):
self.fs.start(self.master)
self.fd.start(self.slave)
self.ss.stop()
self.sd.stop()
def backwards(self):
self.ss.start(self.master)
self.sd.start(self.slave)
65
self.fs.stop()
self.fd.stop()
def turnRight(self):
self.sd.start(self.slave)
self.fs.start(self.master)
self.fd.stop()
self.ss.stop()
def turnLeft(self):
self.ss.start(self.master)
self.fd.start(self.slave)
self.fs.stop()
self.sd.stop()
def stop(self):
self.ss.stop()
self.sd.stop()
self.fs.stop()
self.fd.stop()
def CurrentPosition(self):
if self.state=="W":
self.vx+=self.trans
elif self.state == "S":
self.vx-=self.trans
elif self.state == "D":
self.vth-=0.3925
self.vx+=0.12
elif self.state == "A":
self.vth+=0.3925
self.vx+=0.12
self.state=self.directive="X"
self.impD=self.impS=0
self.stop()
def motorFunction(self):
self.d=(self.impD+self.impS)/2
if self.directive=="W" and self.d<self.ticks:
self.forward()
elif self.directive == "S" and self.d<self.ticks:
66
self.backwards()
elif self.directive == "D" and self.d<self.ticks :
self.turnRight()
elif self.directive == "A" and self.d<self.ticks:
self.turnLeft()
elif self.directive == "A" or self.directive == "D":
self.stop()
time.sleep(0.2)
self.impD=self.impS=0
self.ticks=24
self.directive="W"
else:
self.CurrentPosition()
def callback(self,msg):
self.state=self.directive=msg.data
if self.directive =="W" or self.directive=="S":
self.master=16
self.slave=16
self.ticks=100
elif self.directive =="A" or self.directive =="D":
self.master=self.slave=16
self.ticks=10
self.nt=time.time()
distD=distS=0
if __name__ == '__main__':
rospy.init_node('Motor_Subscriber')
m = Motor()
rate = rospy.Rate(500)
while not rospy.is_shutdown():
if m.directive == "Q":
GPIO.cleanup()
ros.shutdown()
currentTime=time.time()
if currentTime-m.lastpost >0.1:
m.send_odom()
m.lastpost=currentTime
if m.directive !="X":
m.motorFunction()
67
m.speedCalibration()
rate.sleep()
GPIO.cleanup()
68
Anexa 4 Cod sursă – Scriptul pentru lansarea aplicaților pe mobil
<launch>
<!--
<node pkg="robot_slam" name="motorp" type="motorp.py" required="true" />
-->
<node pkg="robot_slam" name="motors" type="motors.py" required="true" />
<!--
<node pkg="urg_node" name="urg_node" type="urg_node" required="true" >
<param name="frame_id" value="base_link"/>
</node>
-->
</launch>
69
Anexa 5 Cod sursă – Scriptul pentru algorimului SLAM
<launch>
<arg name="scan_topic" default="/scan" />
<arg name="odom_topic" default="/odom" />
70