Akadálykikerülő Robot v1.0

2018. január 06. - FizikusRobotBlog

Ebben a cikkben bemutatom, hogy hogyan alakítható át egyszerűen a Vonalkövető robot egy akadálykikerülő/falnakmnemütköző robottá.

A robot feladata:
A robot feladata egyszerűnek hangzik: Egy területen véletlenszerűen haladva kerülje ki az akadályokat. A robot beolvassa az érzékelők adatait, ezek alapján döntéseket hoz, majd úgy vezérli a robotot meghajtó motorokat, hogy a robot kikerülje az útjába kerülő akadályokat. A Vonalkövető robothoz hasonlóan ez a robot is egy komplett autonóm robot lesz majd.

A robot felépítése:
A robot konstrukciója, alváza, meghajtása, energiaellátása, az Arduino Nano-ra épülő robotvezérlő panel és a motorvezérlő panel a korábban ismertetett Vonalkövető robotéval egyezik meg. Csak az érzékelők típusa, elhelyezkedése és száma változott. Ezért lényegében az építés is csak a szenzorok elkészítéséből, felszereléséből, teszteléséből és a robot programozásából áll majd. Ha minden szükséges alkatrész rendelkezésre áll, akkor az átalakítás kb 2-3 óra alatt elvégezhető.

Az infravörös akadályérzékelő szenzor:
A robot az útjába kerülö akadályok és a falak érzékelésére egy infravörös fényforrásból és egy infravörös fényt érzékelő detektorból álló párost használ. A fényforrás egy IR LED, a detektor pedig egy az IR LED-hez illő IR fototranzisztor (a LED által kibocsátott infravörös fény hullámhossza és a fototranzisztor legérzékenyebb hullámhoszatartománya egybeesik)

Infravörös fényforrásnak egy GaAs-ből készült, 5mm-es IR LED-et használok, amely kibocsátási spektrumának a maximuma 940nm-es hullamhossznál van. A LED és a fototranzisztor polaritására figyelni kell. Hogy könnyebb legyen a lábakat megkülönböztetni, a LED anódnál lévő lába hosszabb mint a katódnál lévő. Ráadásul a LED katód felöli oldala nem kerek, hanem lapos. Az akadályérzékelő detektornál az anódot kell a pozitív tápfeszültségre kötni (a LED hosszabbik lába).

Az akadályról visszaverődött infravörös fény detektálására egy IR fototranzisztort fogok használni (ami külsőre teljesen ugyanúgy néz ki mint egy LED). Akárcsak a LED-nek, az IR fototranzisztornak is két kivezetése van (emitter és kollektor). Az emitterhez tartozó láb hosszabb mint a kollektorhoz tartozó láb. A kapcsolásban a kollektort kell a pozitív tápfeszültségre kötni (a fototranzisztor rövidebb lába).

Az alábbi ábrán látható az infravörös akadályérzékelő szenzor kapcsolási rajza:

AZ IR LED-nél lévő áramkorlátozó ellenállás értéke 150 ohm. A fototranzisztornál lévő ellenállás értéke pedig 22k. A fototranzisztor bázisárama a rá eső infravörös fény intenzitásával arányos, és ez az emitter-kollektor áramot is meghatározza. Ezt a változó erősségű áramot alakítjuk át feszültségváltozássá a tranzisztorral sorba kötött 22k-s ellenállással.

A szenzor a kimenetén a ráeső infravörös fény intenzitásától függően egy 0V és 5V közötti analóg feszültségjelet ad ki. A detektorra visszaverődött infravörös fény mennyisége függ az akadály távloságától és típusától (mennyire nyeli el és mennyire veri vissza az infravörös fényt).

A vonalkövető robothoz hasonlóan, az IR LED-et és a fototranzisztort úgy helyeztem el egymáshoz viszonyítva, hogy a fototranzisztor az IR LED-hez képest előrébb legyen. Így a LED fénye a fototranzisztor mögül jön, ezért a LED közvetlenül nem tud rávilágítani, ezért csak a robot előtt lévő akadályról visszaverődött infravörös fény éri el a fototranzisztort.

A próbanyákra összeépített infravörös akadályérzékelő szenzor:

A robotra három ilyen akadályérzékelő szenzort raktam. Egyik egyenesen előre, a másik kettő pedig 45 fokban balra és jobbra néz.

Az Arduino Nano-n a bal oldali érzékelő kimenetét az A4-es, a középső szenzor kimenetét az A5-ös, a job oldali szenzor kimenetét pedig az A6-os analóg bemeneti lábra kötöttem.

Az akadályérzékelő szenzor tesztelése (szenzor kalibráció)
Az akadályérzékelő szenzor kimenetén lévő jelet a mikrovezérlő ADC bemeneteire kötve, és a mért értéket az alábbi Arduino kóddal a PC-re küldve és ott megjelenítve teszteltem a detektor viselkedését.

void setup()
{
Serial.begin(9600); //9600 baud-sebessegu soros port beallitasa
}

void loop()
{
balertek = analogRead(BalSzenzor); //Bal szenzor ertekenek beolvasasa
kozepsoertek = analogRead(KozepsoSzenzor); //Kozepso ertekenek beolvasasa
jobbertek = analogRead(JobbSzenzor); //Jobb szenzor ertekenek beolvasasa
Serial.print("Bal: ");
Serial.print(balertek, DEC);
Serial.print(", Kozep: ");
Serial.print(kozepsoertek, DEC);
Serial.print(", Jobb: ");
Serial.println(jobbertek, DEC); //a mert ertekek kiiratasa a PC-re
delay(500);
}

Különböző anyagú és méretű tárgyak különböző mértékben nyelik el és verik vissza az infravörös fényt. Ez az érzékelő hatékonyságát nagymértékben befolyásolja. Egy fekete és egy fehér papírlappal teszteltem a szenzor viselkedését. Ezt mutatja az alábbi ábra:

Az ábráról látható, hogy kb 7-8 cm-nél nagyobb céltávolság után a mért értékek hirtelen lecsökkennek (900-as ADC értékről kb 500 körüli értékre). Ezért egy 700 körüli határérték megfelelőnek tűnik (ez kb 9-10 cm-es céltávolságnak felel meg).

A fenti vizsgálatot külön-külön el kell végezni mindhárom detektorra, hogy megismerjük az érzékelők egyedi viselkedését és meghatározzuk a hozzájuk tartozó határértékeket. Az általam használt fototranzisztorok karakterisztikája nagyon hasonló, ezért a középső, a bal és a jobb oldalra néző szenzoroknak is ugyanazt a 700-as értéket választottam határértéknek.

Megfigyeltem azt is, hogy a kompakt fényforrások nem nagyon befolyásolják az érzékelők teljesítményét. A hagyományos izzószálas fényforrások ellenben teljesen megbolondítják az érzékelőket a nagymértékű kisugárzott infravörös fény miatt.

Robotvezérlő program
A robotot vezérlő program ADC-vel beolvassa az érzékelők adatait, ezek alapján döntéseket hoz, majd vezérli a robotot meghajtó motorokat. A robot fel-alá bolyong az útjába kerülő akadályokat kerülgetve.

Robotvezérlő algoritmus
A programban három változót definiálok, amelyek az egyes érzékelőkhöz tartozó mérések eredményeit tárolják. Ugyancsak definiálok három konstanst is, amelyekkel a szenzorokhoz tartozó határértékeket adom meg. A program ez alapján dönti el, hogy mikor van egy akadály az adott érzékelő előtt. Ha a mért érték ennél a határértéknél nagyobb, akkor a robot akadályt érzékel.

Ha a robot elindult előre, akkor egy végtelen cikluson belül megvizsgálom hogy a robot előtt van-e akadály (beolvasom a bal oldali, a középső és a jobb oldali érzékelők értékeit). Ha a beolvasott értékek a határértékeknél nagyobbak, akkor akadály van az adott szenzor előtt, és ettől függően kell a robotot irányítani.

Az akadálykikerülő algoritmus elkészítéséhez érdemes két határértéket definiálni (a programomban ez a hatarertek és a hatarertek-50). Az alábbi ábrán a szürkével jelölt körív azt a távolságot mutatja, amihez közelebb kerülő akadályok esetén az érzékelők a hatarertek-nél nagyobb értéket adnak válaszul. A kékkel jelölt nagyobb körív pedig azt a távolságot mutatja, amelynél az érzékelők a hatarertek-50-nél nagyobb értéket adnak. Nézzük meg kicsit részletesebben, hogy erre miért is van szükség.

Ha akadály van a robot előtt (A), akkor a középső szenzor értéke nagyobb mint a középső határérték. Amint az a fenti ábráról látható, ekkor a bal és a jobb oldali szenzorok által mért értékek a határérték alatt vannak (mindkét szenzor távolabb van az akadálytól mint a középső szenzor, ráadásul ferdén is látják az akadályt, ezért a jelük még kisebb). Azt kellene eldönteni, hogy meddig forduljon a robot pl jobbra, mielőtt elindulhatna előre. Ennek eldöntéséhez nem elég csak a középső szenzor jeleit vizsgálni. Jobbra fordulás esetén a bal oldali szenzor jeleit is vizsgálni kell (a jobbra fordulás során a bal oldali szenzor fog az akadály felé nézni). Ha elkezd fordulni a robot, akkor a középső szenzor jele csökkenni fog (a szenzor elfordul az akadálytól), a bal oldali szenzor jele pedig növekedni fog (a szenzor az akadály felé fordul és közeledik is hozzá). Amikor a bal oldali szenzor pont az akadály felé néz (B), akkor a robot még nem indulhat el előre, mert még mindíg eléggé az akadály felé néz. Ha a robot tovább fordul addig amíg a bal szenzoron mért érték a hatarertek-50-nél kisebb nem lesz (C), akkor már a robot eléggé elfordult az akadálytól és elindulhat előre. Minél nagyobb értékkel csökkentjük a beállított határértéket, a robot annál jobban elfordul az akadálytól. Az 50-es értéket próbálgatással határoztam meg.

A fentiek alapján az akadálykikerülő algoritmus röviden így foglalható össze:

Ha akadály van a robot előtt (a középső szenzoron mért érték nagyobb mint a középső határérték), akkor a robot folyamatosan jobbra fordul amíg a középső VAGY a bal oldali érzékelő jele nagyobb mint a szenzorokhoz tartozó határérték-50. Ha a középső és a bal oldali szenzor jele is kisebb már mint a határérték-50, akkor a robot már eléggé elfordult az akadálytól és indulhat előre.

Ha csak a robot bal vagy jobb oldali érzékelője érzékel akadályt, akkor a robot elfordul az akadálytól, amíg az akadályt érzékelő oldalsó szenzoron mért érték kisebb nem lesz mint a szenzorhoz tartozó határérték-50. Ezután a robot elindulhat előre.

Az akadálykikerülő algoritmus folyamatábrája:

A robot Arduino kódja:

#define M1 4 //Pin4 : Motor1 forgasirany (Bal Motor)
#define M1 4 //Pin4 : Motor1 forgasirany (Bal Motor)
#define EN1 5 //Pin5 : Motor1 sebesseg (PWM)
#define EN2 6 //Pin6 : Motor2 sebesseg (PWM)
#define M2 7 //Pin7 : Motor2 forgasirany (Jobb Motor)

int BalSzenzor = 4; //Balra nezo szenzor az A4 analog labra kotve
int KozepsoSzenzor = 5; //Elore nezo szenzor az A5-re kotve
int JobbSzenzor = 6; //Jobbra nezo szenzor az A6-ra kotve
int balertek; //Bal szenzoron mert ertek
int kozepsoertek; //Kozepso szenzoron mert ertek
int jobbertek; //Jobb szenzoron mert ertek
int BalHatarertek = 700; //Bal szenzor hatarerteke
int KozepsoHatarertek = 700; //Kozepso szenzor hatarerteke
int JobbHatarertek = 700; //Jobb szenzor hatarerteke

void setup()
{
pinMode(EN1, OUTPUT); // Motor1 sebesseg (PWM)
pinMode(EN2, OUTPUT); // Motor2 sebesseg (PWM)
pinMode(M1, OUTPUT); // Motor1 forgasirany (Bal Motor)
pinMode(M2, OUTPUT); // Motor2 forgasirany (Jobb Motor)
MotorBal(100); // Robot: Elore
MotorJobb(100);
}

void loop()
{
balertek = analogRead(BalSzenzor); //Bal szenzor ert. beolv.
kozepsoertek = analogRead(KozepsoSzenzor); //Kozepso szenzor ert. beolv.
jobbertek = analogRead(JobbSzenzor); //Jobb szenzor ert. beolv.

if (kozepsoertek>KozepsoHatarertek)
{
// Fordulas balra
MotorBal(100);
MotorJobb(-100);

while ((jobbertek>(JobbHatarertek-50)) || (kozepsoertek>(KozepsoHatarertek-50)))
{
kozepsoertek = analogRead(KozepsoSzenzor); //Kozepso szenzor ert. beolv.
jobbertek = analogRead(JobbSzenzor); //Bal szenzor ert. beolv.
delay(100);
}
MotorBal(100); //Elore
MotorJobb(100);
}

if (jobbertek>JobbHatarertek)
{
// Balra
MotorBal(-100);
MotorJobb(100);

while (jobbertek>(JobbHatarertek-50))
{
jobbertek = analogRead(JobbSzenzor); //Jobb szenzor ert. beolv.
delay(100);
}

MotorBal(100); //Elore
MotorJobb(100);
}

if (balertek>BalHatarertek)
{
// Jobbra
MotorBal(100);
MotorJobb(-100);

while (balertek>(BalHatarertek-50))
{
balertek = analogRead(BalSzenzor); //Bal szenzor ert. beolv.
delay(100);
}

MotorBal(100); //Elore
MotorJobb(100);
}
delay(100);
}

void MotorBal(int sebesseg)
{

if (sebesseg>0)
{
digitalWrite(M1,HIGH);
analogWrite(EN1,sebesseg*255/100);
}
else
{
digitalWrite(M1,LOW);
analogWrite(EN1,abs(sebesseg)*255/100);
}
}

void MotorJobb(int sebesseg)
{
if (sebesseg>0)
{
digitalWrite(M2,HIGH);
analogWrite(EN2,sebesseg*255/100);
}
else
{
digitalWrite(M2,LOW);
analogWrite(EN2,abs(sebesseg)*255/100);
}
}

A végeredményről egy kis videó: