Budapesti Műszaki és Gazdaságtudományi Egyetem Villamosmérnöki és Informatikai Kar Irányítástechnika és Informatika Tanszék
Czifra Ádám (CB66WR)
NAVIGÁCIÓS CÉLÚ TÉRKÉPEZÉS ULTRAHANG SZENZOR SEGÍTSÉGÉVEL
KONZULENS
Dr. Drexler Dániel András Beágyazott és irányító rendszerek specializáció Önálló laboratórium BUDAPEST, 2015.12.15.
Tartalomjegyzék 1 Bevezetés ....................................................................................................................... 3 1.1 Cél ........................................................................................................................... 3 1.2 Az ultrahang szenzor .............................................................................................. 3 1.2.1 Tulajdonságok .................................................................................................. 3 1.2.2 Működése ......................................................................................................... 4 1.3 A szervo motor........................................................................................................ 5 1.3.1 Tulajdonságok .................................................................................................. 5 1.3.2 Működése ......................................................................................................... 5 2 A feladat kidolgozása ................................................................................................... 7 2.1 Kalibráció ................................................................................................................ 7 2.1.1 Hosszirány ....................................................................................................... 7 2.1.2 Tangenciális ..................................................................................................... 8 2.1.3 Találati arány ................................................................................................... 8 2.2 Több szenzor kezelése ............................................................................................ 9 2.3 Szoftveres megvalósítás ........................................................................................ 10 2.3.1 Arduino .......................................................................................................... 10 2.3.2 MATLAB....................................................................................................... 12 2.4 Hardveres megvalósítás ........................................................................................ 16 3 Irodalomjegyzék......................................................................................................... 18
2
1 Bevezetés 1.1 Cél Az Önálló laboratórium célja az volt, hogy egy négylábú robot számára kialakítsak egy olyan Arduino alapú környezetfelismerő rendszert, amelynek segítségével a robot el tudja majd kerülni a különböző akadályokat. A feladat elvégzésére ideális választás az ultrahang szenzor, mivel ilyen érzékelővel könnyen meghatározható, hogy van-e előttünk valamilyen objektum, illetve annak az érzékelőtől való távolsága.
1.2 Az ultrahang szenzor A feladat elvégzésére HC-SR04 Ultrahangos szenzort használtam.
1.2.1 Tulajdonságok Ez a szenzor 4 darab lábbal rendelkezik, és ezek csatlakoznak az Arduino board különböző pinjére 1. VCC = +5VDC 2. Trig = a szenzor bemenete 3. Echo = a szenzor kimenete 4. GND = GND
3
1. HC-SR04 Ultrahangos szenzor
1.2.2 Működése A mérés elkezdéséhez a távolságérzékelő bemenetére („Trig”) egy legalább 10us hosszú magas (5V) impulzust kell adni. Ennek megérkezésekor egy 40kHz-es ultrahang impulzust bocsájt ki, aminek hatására nyugalmi állapotba kerül és várja a visszaverődő hangot (echót). A lövés és fogadás között eltelt időt méri a szenzor. Ez alapján, amikor a fogadó visszaverődő ultrahangot észlel, az „Echo” kimenetet magas állapotba állítja annyi ideig, amennyi idő eltelt a lövés és fogadás között. Ebből az időből következtethetünk a távolságra (r = c*t/2). Az ultrahang terjedési sebessége 340 m/s, ebből következik, hogy az észlelt objektum távolsága: r = t / 5,88 [mm].
2. Ultrasonic sensor operation
4
1.3 A szervo motor A feladat elvégzéséhez Tower Pro SG90 szervo motort használtam.
1.3.1 Tulajdonságok A motor 3 vezetékkel rendelkezik: 1. Piros: +5VDC 2. Barna: GND 3. Sárga: jelvezeték, a szervo motor bemenete.
3. Tower Pro SG90 Servo motor
1.3.2 Működése A motor körülbelül 180°-ban képes mozogni. A sárga jelvezeték az Arduino board egy PWM kimenetére kell, hogy kapcsolódjon. PWM kimeneten nem egy 0V és 5V közé eső feszültség jelenik meg. A PWM (Pulse Width Modulation) jel a kimeneti lábra meghatározott ideig felváltva 0Vot és 5V-ot kapcsol. A szervo motor a PWM kimeneten megjelenő 20 ms hosszú PWM periódus alapján áll be a kívánt pozícióba. Az arduino beépített könyvtárának write(pos) függvényét használhatjuk a motor kívánt pozícióba állításához. „Pos = 0” esetén (~2ms impulzus) a motor a jobb szélre áll be, „pos = 90” (1,5ms impulzus) a középállás, míg „pos = 180” (~1ms impulzus) pozíció hatására a motor a bal szélre áll be.
5
4. PWM periódus
6
2 A feladat kidolgozása A feladat megvalósítása több lépésből állt. Első lépésként a távolságérzékelő kalibrálását kellett elvégezni. Ezután a szoftveres munkákat kellett megcsinálni az Arduino szoftver és a MATLAB segítségével. A végső fázisban egy stabil és jól használható hardver elkészítése volt a feladat.
2.1 Kalibráció A kalibráció azért szükséges, hogy belátásunk legyen arra, hogy különböző távolság esetén mennyire megbízható a szenzor mérési eredménye. A mért eredményeket MATLAB segítségével összegeztem.
2.1.1 Hosszirány A szenzor az adatlap szerint 20-4000 [mm] között képes mérni, én 25-825 [mm]-es tartományon vizsgáltam a szenzor pontosságát. Minden mérési pontban 5 db mérést végeztem és ezeknek az átlagát használtam fel a kiértékelésben. 3 különböző méretű, alakú és anyagú tárggyal teszteltem a szenzort. 1. Papír doboz (magasság 9cm, szélesség 8cm) 2. Műanyag cellux tartó (magasság 5,5cm, szélesség 3,5cm) 3.
Műanyag doboz (magasság 1,5cm, szélesség 16cm)
5. Deviations in calculating the distance
7
Jól látható az ábrán, hogy a távolságérzékelő mérési eredményei átlagosan 10 mm-es hibatartományon belül helyezkednek el.
2.1.2 Tangenciális A kalibrációt 500 mm-es távolságban, -25°…25°-os tartományban a műanyag cellux tartó használatával végeztem. Minden mérési pontban 5 db mérést végeztem és ezeknek az átlagát használtam fel a kiértékelésben. A mért tartományban a szenzor eredményei pontosnak tekinthetőek, míg ezen a tartományon kívül az érzékelő nem látta a tárgyat.
6. Beam pattern of ultrasonic sensor
2.1.3 Találati arány A hit rate mérését szintén 500 mm–es tartományban végeztem, viszont kettő különböző méretű és alakú tárgyat használtam. 100 mérést végeztem minden szögben és ez alapján az adott szöghöz rendeltem egy százalékos találati értéket. 2.1.3.1 Műanyag cellux tartó Ebben az esetben a már hosszirányú mérésnél is használt műanyag cellux tartót használtam és -30°…30°-os tartományban mértem. Ahogy 6. ábrán látható, a szenzor 25°..25° tartományban 100%-os hit rate-el működik, míg +- 27,5°-nál már nem mindig látja az tárgyat és +-30°-nál már egyáltalán nem érzékeli az akadályt. 8
7. Hit rate 1.
2.1.3.2 Papírguriga Ebben az esetben egy állított papírgurigát használtam, hogy látható legyen az az eset is, amikor az ultrahang hullámok nem egyenes felületre érkeznek. A 7. ábrán jól látható, hogy ebben az esetben már a +-25°-nál levő tárgyat se volt képes érzékelni a szenzor, az ultrahang hullámok más irányba verődése miatt.
8. Hit rate 2.
2.2 Több szenzor kezelése Az Arduino board képes több ultrahangos távolságérzékelőt kezelni egyszerre. Ez azért jó, mert így a későbbiekben használhatunk több szenzort akár a pontosság növelésére, akár arra, hogy a teljes környezetünket feltérképezhessük. 9
9. Measuring results of 2 ultrasonic sensors
2.3 Szoftveres megvalósítás 2.3.1 Arduino Az Arduino kód felel az ultrahang szenzor és a szervo motor helyes működéséért. Az ultrahang szenzor trigger lábát a 2-es, míg az echo lábat az Arduino 3as pinjére kötöttem, míg a motor a 9-es PWM pinre került. Az arduino board beépített LED-jét is használtam, hogy látni lehessen, mikor jelenik meg hasznos távolság adat a soros porton. Az arduino rendelkezik egy beépített „Servo” könyvtárral, amit felhasználtam én is a szervo motor konfigurálásához és irányításához. Készítettem egy myservo objektumot, aminek a használatával elértem a motort. A 0 és 180-as pozíció közelében a motor a határán van, hogy elkerüljük a motor tönkremenetelét, ezért mi csak a 10-170-es pozíció között használjuk. A szervo motort elmozdítjuk egy pozíciónyit, ez után szükséges egy 15us-os késleltetés, hogy a motor biztos elmozduljon a következő pozícióba. A forgás irány eldöntéséhez egy változót használtam (way), „0” érték esetén a motor balra fordul, míg „1” esetén jobbra. A távolságérzékelő minden pozícióban (10-170 között) megnézi, hogy van-e valami a látószögében, és a soros portra írja a pozíciót (hogy épp hol áll a motor) és a távolság adatot. Ha a mért távolság megfelelő, azaz 0-4000 [mm] között van, akkor az Arduino beépített LED-je világít. Mielőtt újra pozíciót váltunk szükséges egy 60us-os késleltetés, hogy ha van visszavert hang, akkor a szenzor biztosan érzékelje azt. 10
A feladat Arduino részét a következő kóddal oldottam meg: #include <Servo.h> #define echoPin 7 // Echo Pin #define trigPin 2 // Trigger Pin #define LEDPin 13 // Onboard LED Servo myservo; //create a servo object int pos = 10; //servo position int cnt = 0; //counter int way = 0; //servo direction int maximumRange = 4000; int minimumRange = 0; long duration, distance; void setup() { Serial.begin (9600); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(LEDPin, OUTPUT); myservo.attach(9); } void loop() { if (pos == 10) way = 0; if (pos == 170) way =1; if (way == 0) pos += 1; if (way == 1) pos -= 1; myservo.write(pos); delay(15); ultrasonicsensordata(); delay(60); } void ultrasonicsensordata(){ digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = duration/5.82; if (distance >= maximumRange || distance <= minimumRange){ Serial.print(pos); Serial.print( " "); Serial.println(distance); digitalWrite(LEDPin, HIGH); } else { Serial.print(pos); Serial.print( " "); Serial.println(distance); digitalWrite(LEDPin, LOW);
11
} }
2.3.2 MATLAB MATLAB dolgozza fel és jeleníti meg az Arduino soros porton küldött adatait. Még beolvasás előtt létrehozok MATLAB függvények segítségével egy ablakot, amiben jól követhetők lesznek a szenzor adatai és a mozgása is. A plot függvény segítségével félköríveket rajzolok a [100:2000] tartományba egymástól 100 egység távolságra. Majd 30°-onként felveszek egy-egy vonalat, az ultrahang mozgásának minél jobb követése érdekében. Ehhez 4 tömböt használtam, amik meghatározzák azokat a pontpárokat ((x0[i];y0[i]) és (x1[i];y1[i])), amik össze lesznek kötve. Hasonló módszerrel létrehoztam minden egész szög értékhez egy vonalat az [1:180] tartományon. Itt arra kellett odafigyelnem, hogy a MATLAB radiánban kezeli a szögeket nem fokban, ezért a vonalhúzás előtt át kell váltani az értékeket radiánba a degtorad() függvény segítségével. A pol2cart() függvény az ismert polár koordinátákból Descartes-koordinátákat képez, amit már ki tud rajzolni megfelelően a MATLAB a plot függvénnyel. Az itt megrajzolt vonalakat elmentem egy „lines” változóba, hogy a későbbiekben könnyen módosítani tudjam őket. Ezek után létrehozom az „arduino” serial objektumot. A serial függvénynek paraméterként szüksége van arra a COM portra, amelyiken az Arduino board csatlakozik a számítógéphez, ezt megtudhatjuk az Arduino software-ének segítségével, ami jelzi nekünk ezt a COM portot. Az fopen() függvénnyel csatlakoztatom az Arduinot az eszközhöz. A szenzor adatainak megjelenítése egy végtelen while ciklusban fog zajlani. Az fscanf() függvény segítségével beolvassuk egy „SerialData” változóba a soros porton érkező stringet. Az strtok() függvény két részre bontja a kapott stringet, egy tokenre és egy maradékra. (pl. „ Ez egy szép nap” token = „Ez”, maradék = „ egy szép nap”). Ezt a függvényt használjuk arra, hogy feldolgozzuk a „SerialData” változónkat és kiszedjük belőle a szervo motor aktuális pozícióját és a távolságérzékelő adatát. A kapott stringeket számmá kell alakítanunk, illetve a szervo motor pozícióját radiánná is. Ezután a már létrehozott „lines” változó módosul a szenzor adatainak megfelelően. Itt két eset lehetséges, hogy a szenzor érzékelt valamit 2000 mm-en vagy annál közelebb, ekkor egy vörös vonalat húzunk a szenzor által mért távolságtól a legkülső félkörívig, azaz
12
2000 mm-ig. A második eset az, amikor a szenzor vagy nem érzékelt semmit, vagy 2000 mm-nél távolabb érzékelt valamit, ekkor egy zöld pontot rakunk a legkülső félkörívre, ezzel jelezve, hogy nem található olyan akadály, ami jelenleg kritikus lenne a szempontunkból. Egy vastagabb vörös vonalat rajzolunk ki („RedLine”), ami lehetővé teszi azt, hogy folyamatosan követhessük a szervo motor mozgását és ne csak akkor rajzolódjon ki látható vonal, amikor a szenzor érzékel valamit. A drawnow függvénnyel frissítjük az általunk készített képernyőt, majd töröljük a „RedLine” változónkat, hogy a következő frissítésnél már csak az aktuális pozíció látszódjon. Végezetül le kell választanunk az arduinot az eszközünkről az fclose() függvénnyel, majd törölni a létrehozott „arduino” objektumot. A feladat MATLAB részét a következő kóddal oldottam meg: clc; clear all; figure('units','normalized','outerposition', [0 0 1 1]); %outerpos [left bottom width height] whitebg('black'); %background fi = linspace(0,pi,1000); R = 100:100:2000; %Draw the lines for distance measurment for i = 1:length(R) hold on; x = R(i)*cos(fi); y = R(i)*sin(fi); plot(x,y,'Color', 'green' ,'LineWidth',1); end x0 x1 y0 y1
= = = =
[0 -2000 0 0 0 0]; %2000*cos(30°) = 2000*sqrt(3)/2 = 1732 [0 2000 1732 1000 -1000 -1732]; %2000*cos(60°) = 2000*0,5 =1000 [0 0 0 0 0 0]; [2000 0 1000 1732 1732 1000];
for i = 1:length(x0) hold on; plot([x0(i),x1(i)],[y0(i),y1(i)],'Color','green','LineWidth',2); end for i = 1:1:180 hold on; rad = degtorad(i); [x,y] = pol2cart(rad, 2000); lines(i) = plot([0,x],[0,y],'Color','green','Linewidth',1); end cnt=0; arduino = serial('COM3','BaudRate',9600);
13
fopen(arduino); while(1) dbstop if warning SerialData = fscanf(arduino) [fi, R] = strtok(SerialData); %[token,remain] fi = str2num(fi); fi_rad = degtorad(fi); R = str2num(R); if ( R <= 2000) set(lines(fi),'color','red'); [x0, y0] = pol2cart(fi_rad, 2000); [x, y] = pol2cart(fi_rad, R); else set(lines(fi),'color','green'); [x0, y0] = pol2cart(fi_rad, 2000); [x, y] = pol2cart(fi_rad, 2000); end set(lines(fi),'XData',[x0,x]); set(lines(fi),'YData',[y0,y]); RedLine = plot([0,x0],[0,y0],'R','LineWidth',3); drawnow delete(RedLine); end fclose(arduino); delete(arduino) clear arduino
MATLAB képernyő:
10. MATLAB figure, Start
14
11. MATLAB figure, No object in range
12. MATLAB figure, The sensor perceived an object
15
2.4 Hardveres megvalósítás Az ultrahang szenzor tesztelését breadboard és jumper wire-ök használatával végeztem. Így egyszerűen meglehetett oldani a szenzor és a motor lábainak összekötését az Arduino boardal, illetve szükség esetén könnyen tesztelhetők voltak külön-külön is az egységek. Ám több probléma is akadt ezzel a megoldással:
A vezetékezés egy idő után átláthatatlanná vált.
A vezetékek nem voltak elég hosszúak és gyakran kicsúsztak a helyükről.
A szenzort úgy kellett elhelyezni, hogy a breadboard ne zavarjon be a távolságérzékelésbe (10 cm-es távolságban, illetve felette a szenzor érzékelte a breadboardot, ezért a szenzor nem adott tényleges információt a környezetünkről)
A szervo motornak stabilan kellett lennie, ahhoz hogy a mozgása esetén maga a motor egy helyben maradjon.
A végleges hardver esetében forrasztást alkalmaztam a megfelelő lábak összekötéséhez, így kevesebb kábellel, biztosabb kapcsolatokat lehetett létrehozni az egységek között. Az ultrahangos érzékelő vékony kábelek segítségével lett összeforrasztva az Arduino boarddal azért, hogy könnyen tudja forgatni a szervo motor. A motor számára 3D nyomtatva lett egy szerkezet, ami bárhova lecsavarozható, így a motor stabil helyzetbe kerül, ezért nem kell aggódni amiatt, hogy esetlegesen elmozdul működés közben. Abban az esetben, ha az Arduino board USB portról kap tápfeszültséget, az Arduino nem tudja egyszerre ellátni a szenzort és a motort megfelelő árammal, ezért külső tápfeszültséget kell alkalmazni, amivel ellátjuk a szervo motort megfelelő táppal és emiatt az Arduino boardnak, csak a szenzorról kell gondoskodnia. 12V-os külső tápfeszültség alkalmazása mellett döntöttünk. A szervo motor 5V-tal működik, tehát szükség volt egy 7805-ös IC-re a feszültségszabályozás elvégzéséhez. A kapcsolás kimeneteként megkaptuk a nekünk szükséges 5V-os feszültséget és erre már ráköthettük a motor VCC jelét.
16
13. Alkalmazott kapcsolás (C1 = C2 = 10uF; C3=10nF)
17
3 Irodalomjegyzék [1]
HC-SR04 User’s Manual, https://docs.google.com/document/d/1YyZnNhMYy7rwhAgyL_pfa39RsB-x2qR4vP8saG73rE/edit
[2]
Tower Pro SG90 Servo Motor Datasheet, http://akizukidenshi.com/download/ds/towerpro/SG90.pdf
[3]
Mathworks: plotting circles, http://www.mathworks.com/matlabcentral/answers/3058-plotting-circles
[4]
Calibration of Ultrasonic Sensors of a Mobile Robot http://www.journal.ftn.kg.ac.rs/Vol_6-3/06-Paunovic-Todorovic-BozicDjordjevic.pdf
[5]
SensorWiki.org, Ultrasound http://www.sensorwiki.org/doku.php/sensors/ultrasound
18
