Akadálykikerülő Robot v2.0

2018. január 09. - FizikusRobotBlog

Az akadálykikerülő robot első verziójában az akadályérzékelő szenzoroknak IR LED + IR fototranzisztor párosokat használtam. Ez az érzékelő csak nagyobb tárgyak közelségét tudja detektálni. Kisebb tárgyak érzékelésére, vagy a tárgyak távolságának pontos mérésere nem alkalmas. Ehhez vagy egy ultrahangos szenzort, vagy egy Sharp infravörös távolságmérőt kell használni.

Ezek az érzékelők csak a közvetlenül előttük lévő tartományban mérnek. Ha a robot körüli környezetet akarjuk érzékelni, akkor az egyik lehetőség, hogy több különböző irányba néző érzékelőt kell használni (az alábbi kép erre mutat egy példát). Ez azonban nem valami költséghatékony megoldás.

Egyetlen érzékelő használatával is megoldható a feladat, ha az érzékelőt forgatva végigpásztázzuk a robot környezetét. Az akadálykikerülő robot továbbfejlesztett verziójában is egy mikroszervóra erősítem a távolságérzékelő szenzort és így mérem a különböző irányokban a robot körüli akadályok távolságát. Mivel az ultrahangos érzékelőről az Ar-Du robotomról szóló cikkben részletesen írtam már, ezért ennél a robotnál a Sharp infravörös távolságérzékelő szenzor használatárá mutatok egy példát.

Sharp infravörös távolságérzékelő szenzor
A robotikában használt egyik népszerű érzékelő a Sharp infravörös tavérzékelő szenzor, amiből többféle típus létezik, különböző méréstartományokkal.

A szenzor az ún. háromszögeléses módszerrel működik. A szenzor egy keskeny infravörös fénnyalábot bocsát ki (az IR fény hullámhossza 850nm ± 70nm). A kibocsátott IR fény a tárgyakról visszaverődik. Az érzékelő egy optikával leképezi a visszavert fényt egy CCD-re. Attól függően hogy milyen messze van a céltárgy, más-más szögben érkezik vissza a visszavert fény, és ennek megfelelően más-más CCD pixelre fókuszálodik. Ebből már meghatározható a távolság.

Létezik digitális és analóg kimenetű szenzor is. A digitális kimenetű szenzor jele egy fix távolságnál vált logikai magas szintről alacsonyra (az alábbi ábra a GP2Y0D21YK digitális szenzor adatlapjából való, ez az érzékelő 24 ± 3 cm-nél vált).

Robotépítéshez azonban az analóg szenzor jobban megfelel, mert azzal a méréstartományon belüli céltárgy távolsága pontosan megmérhető. Az analóg szenzor egy a távolsággal fordítottan arányos, nemlineáris, analóg kimeneti feszültségjelet ad válaszul.

Az analóg szenzor esetében, a fenti feszültség-távolság függvény alakjából következően, ha egy tárgy a minimális érzékelési távolságnál közelebb van (a fenti ábrán ez kb. 6cm), akkor a szenzor hamis értéket adhat. A mikrokontroller azt hiheti hogy az akadály távol van, a valóságban viszont nagyon is közel (pl. a szenzor a 3cm-re és a 12cm-re lévő akadály esetén is egyaránt 2V-os válaszjelet ad). Ezt a problemát úgy küszöbölhetjuk ki a legegyszerűbben, ha az érzékelőt a roboton úgy helyezzük el, hogy semmi se kerülhessen a minimális érzékelési távolságnál közelebb a detektorhoz.

A robotépítés során az alábbi 3 szenzort alkalmazzák a leggyakrabban:

Sharp GP2D120
A gyártó által megadott méréstartomány: 4-30cm

Sharp GP2Y0A21YK
A gyártó által megadott méréstartomány: 10-80cm

Sharp GP2Y0A02YK0F
A gyártó által megadott méréstartomány: 20-150cm

A szenzor energiaigénye a mérések során elég nagy, de ez nem egyenletesen hanem impulzusszeruen jelentkezik. Ez jelentős mennyiségű zajt generálhat, ezért a gyártó az adatlapon a tápfeszültség stabilizálására egy 10µF-os, vagy annál nagyobb kondenzátor bekötését javasolja a GND és Vcc közé.

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 is a korábbi cikkeimben már ismertetett robotéval egyezik meg. Csak a 3 akadályérzékelő szenzor helyett egy mikroszervót és egy 6-80cm-es méréstartományú Sharp GP2Y0A21YK szenzort használtam.

A mikroszervót és a Sharp szenzort kétoldalú ragasztoszalaggal néhany perc alatt felszereltem, és az alábbi ábrának megfelelően rákötöttem a robotvezérlő panelre (szervót a digitális D10-re, a Sharp szenzort pedig az analóg A5-re kötöttem). Ezután következett a szenzor tesztelése és a robot programozása.

A Sharp GP2Y0A21YK szenzor mérési adatainak linearizálása
A Sharp infravörös távolságérzékelőknek az a legfőbb hátránya, hogy a távolsággal fordítottan arányos és nemlineáris a válaszjelük. Sokkal jobban tudnánk az adatokat használni, ha a mért értékeket valahogyan közvetlen távolságadatokká tudnánk alakítani. Erre mutatok most egy példát.

A robothoz egy 10-80cm méréstartományu Sharp GP2Y0A21YK szenzort használtam. Első lépésként megmértem a szenzor távolság-ADC érték görbéjét. Erről látható, hogy a mért ADC értékek kb. 0 és 650 közé esnek.

Az is látszik, hogy a fenti görbe alakja nagyon hasonlít az 1/x függvényre, ezért megpróbálhatjuk ezzel átalakítani a Sharp szenzor válaszgörbéjét egy egyenessé (linearizálás).

A koordináta-geometriában használatos egyenes általános egyenletét felhasználva és a mért ADC értékek reciprokát az egyenletbe helyettesítve a távolságra (x) egy kis átalakítás után az alábbi egyenletet kapjuk:

Ez egy jó képlet, ha tudunk a mikrovezérlővel egyszerűen floating point műveleteket végezni (tizedes jegyekkel számolni). Az Arduino program méretének csökkentése érdekében a floating point műveletek használatát mindenképpen érdemes elkerülni ha lehetséges. Ezért az ideális megoldás az lenne, ha találnánk egy olyan függvényt is, ami egész számokkal számolva, csak integer műveletekkel is jó eredményt ad. A fenti egyenlet további átalakításával kapjuk a következőt:

Ez már integer műveletekkel is egyszerűen használható. Ha a szenzor előtt nincs semmilyen céltárgy, akkor a mért értékek lecsökkenhetnek akár 0-ig is. Hogy elkerüljem a nullával való osztást, az ADC értékekhez hozzáadok 1-et. Ezzel a végleges távolságkeplet így fog kinézni:

Ezután már csak a megfelelő függvényparamétereket kell megtalálni (az én szenzorom esetén m`=6400 és b`=4 értékek használata esetén volt a legjobb a végeredmény). Az alábbi ábrán a szenzor által mért értékekből számított távolságokat ábrázoltam a valódi céltávolságok függvényében:

A fenti számítás elvégzésére a robot programjában létrehozok egy Tavmeres() nevű függvényt, amit a távolságmérés elvégzéséhez a programon belül csak meg kell hívni.

int Tavmeres (int adcPin)
{
int tavolsag = 0;
int adc = analogRead(adcPin);
tavolsag = (6400/(adc+1))-4;
return tavolsag;
}

RC szervómotorok vezérlése

A szervók működtetése három vezetéken keresztül történik:

táp (piros)
föld (fekete vagy barna)
vezérlés (sárga vagy fehér)

Az RC szervók un. impulzusszélesség vezérelt motorok. A szervók vezérlése egy egyszerű négyszögjel alakú impulzussorozattal történik. A vezérlő impulzusoknak nem a nagysága (feszültsége) számít, hanem az hogy mennyi ideig tartanak. A szervó pozícióját (hogy melyik szögbe álljon be) az impulzus szélességével tudjuk beállítani. Minden szervó 1.5ms széles impulzus hatására áll be középállásba. Az impulzusszélesség általaban 1-2ms között mozog, és az 1ms széles impulzus a bal oldali szélső pozícióba állitja be a szervót, a 2ms széles impulzus pedig a jobb oldali szélső pozícióba. A szervónak legfeljebb 20ms-onként, azaz másodpercenként legalább 50-szer kell elküldeni a vezérlőjelet.

Az Arduino fejlesztőkörnyezet beépített Servo.h függvenykönyvtárát használtam, mert ez nagymértékben megkönnyíti a szervók vezérlését. A szervót a digitális 9-es vagy 10-es lábra lehet csak kötni.

Először létrehozok egy szervó objektumot Szervo néven:

Servo Szervo;

Ezután hozzárendelem a digitális D10-es lábhoz:

Szervo.attach(10);

Az alábbi paranccsal a szervónak csak meg kell adni azt a szögértéket hogy hova álljon (0 és 180 fok közötti érték lehet csak):

Szervo.write(szog);

A robot programja
Bekapcsolás után a robot elindul előre. Beolvassa a távolságérzékelő szenzor jelét (SharpIR). Ha 20cm-nél közelebb akadályt érzékel akkor:

megáll
balra néz
távolságot mér (SharpIRbal)
jobbra néz
távolságot mér (SharpIRjobb)
ha a jobb oldalon és a bal oldalon mért távolság is kisebb mint 20cm (a robot zsákutcába került), akkor a robot hátratolat egy másodpercig majd megfordul és elindul előre.
ha a robot nincs zsákutcában, akkor arrafelé fordul 45 fokot amelyik oldalon nem érzékel akadályt, vagy az érzékelt akadály messzebb van mint a másik irányban érzékelt akadály.

A fenti algoritmus folyamatábrán ábrázolva így néz ki (minden szervoforgatás után van egy fél másodperces késleltetés, hogy a szervónak legyen elég ideje az új pozícióba álláshoz, ez a folyamatábrán nincs feltüntetve):

A robot Arduino kódja:

/* ***************************************************
Sharp szenzoros AkadalyKiKerulo Robot Arduino 1.0 kod
by: Fizikus
4/8/2012
*************************************************** */

#include <Servo.h>

#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)

Servo Szervo; // mikroszervo inicializalasa Szervo neven

int SharpSzenzor = 5; //Sharp szenzor az analog A5-re kotve
int SharpIR; //Celtargy tavolsag (elore nezve)
int SharpIRbal; //Celtargy tavolsag (balra nezve)
int SharpIRjobb; //Celtargy tavolsag (jobbra nezve)
int SzervoPin = 10; // Mikroszervo a D10 digitalis labra kotve
int Bal = 150; //Balra nezo szervopoziciohoz tartozo ertek
int Kozep = 90; //Kozepre nezo szervopoziciohoz tartozo ertek
int Jobb = 30; //Jobbra nezo szervopoziciohoz tartozo ertek

void setup() //Beallitasok
{
pinMode(EN1, OUTPUT); // Motor1 sebesseg lab: kimenet
pinMode(EN2, OUTPUT); // Motor2 sebesseg lab: kimenet
pinMode(M1, OUTPUT); // Motor1 forgasirany lab: kimenet
pinMode(M2, OUTPUT); // Motor2 forgasirany lab: kimenet
Szervo.attach(10); // Mikroszervo a D10 digitalis labra kotve
Szervo.write(Kozep); // Szervo: elore nez
}

void loop() // Foprogram - vegtelen ciklus
{
Elore(100); // teljes gozzel elore!
SharpIR = Tavmeres(SharpSzenzor); // Tavolsagmeres
if ( SharpIR < 20 ) // ha a robot elott 20cm-nel kozelebb akadaly van
{
Stop(); // Stop
Szervo.write(Bal); // Szervo: balra nez
delay(500); // Varakozas 0.5s-ig
SharpIRbal = Tavmeres(SharpSzenzor); // Bal oldali tavolsagmeres
Szervo.write(Kozep); // Szervo: elore nez
delay(500); // Varakozas 0.5s-ig
Szervo.write(Jobb); // Szervo: jobbra nez
delay(500); // Varakozas 0.5s-ig
SharpIRjobb = Tavmeres(SharpSzenzor); // Jobb oldali tavolsagmeres
Szervo.write(Kozep); // Szervo: elore nez
if (SharpIRbal < 20 && SharpIRjobb < 20) //ha mindket tavolsag kisebb mint 20cm
{
Hatra(100); // Hatramenet 1s-ig
delay(100); // Varakozas 1s-ig
Stop(); // Robot stop
Hatraarc(); // 180fokos fordulas
}
else
{
if (SharpIRbal > SharpIRjobb) //ha a bal oldali tavolsag nagyobb mint a jobb
{
FordulasBalra45(); // fordulas balra 45 fokot
delay(100); // Varakozas 0.1s-ig
}
else // ha a jobb oldali tavolsag nagyobb mint a bal oldal
{
FordulasJobbra45(); // fordulas jobbra 45 fokot
delay(100); // Varakozas 0.1s-ig
}
}
}
delay (500); // Varakozas 0.5s-ig
}

int Tavmeres(int adcPin)
{
int tavolsag = 0;
int adc = analogRead(adcPin); // az adcPin-re kotott szenzor ertekenek beolvasasa
tavolsag = (6400/(adc+1))-4; // a beolvasott ertekbol a tavolsag kiszamitasa cm-ben
return tavolsag;
}

void MotorBal(int sebesseg)
{
if (sebesseg>0) // Eloremenet
{
digitalWrite(M1,HIGH); // M1 irany (elore)
analogWrite(EN1,sebesseg*255/100); // M1 sebesseg (PWM)
}
else // Hatramenet
{
digitalWrite(M1,LOW); // M1 irany (Hatra)
analogWrite(EN1,abs(sebesseg)*255/100); // M1 sebesseg (PWM)
}
}

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

void Stop()
{
MotorBal(0); // Bal motor: stop
MotorJobb(0); // Jobb motor: stop
}

void Elore(int sebesseg)
{
MotorBal(sebesseg); // Bal motor: elore
MotorJobb(sebesseg); // Jobb motor: elore
}

void Hatra(int sebesseg)
{
MotorBal(-sebesseg); // Bal motor: hatra
MotorJobb(-sebesseg); // Jobb motor: hatra
}

void Hatraarc()
{
MotorBal(100); // Bal motor: elore
MotorJobb(-100); // Jobb motor: hatra
delay(1250); // Varakozas 1.25s-ig
MotorBal(0); // Bal motor: stop
MotorJobb(0); // Jobb motor: stop
}

void FordulasBalra45()
{
MotorBal(-100); // Bal motor: hatra
MotorJobb(100); // Jobb motor: elore
delay(400); // Varakozas 0.4s-ig
MotorBal(0); // Bal motor: stop
MotorJobb(0); // Jobb motor: stop
}

void FordulasJobbra45()
{
MotorBal(100); // Bal motor: elore
MotorJobb(-100); // Jobb motor: hatra
delay(400); // Varakozas 0.4s-ig
MotorBal(0); // Bal motor: stop
MotorJobb(0); // Jobb motor: stop
}

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

Robotpók

Nem írok külön cikket az általam épített Robotpók-ról, mert szinte teljesen megegyezik az itt ismertetett akadálykikerülő robottal két “apró” különbséget leszámítva:

A robotnál változatlanul marad a diferenciális meghajtás, de a haladáshoz nem kerekeket, hanem lábakat használ.
A másik különbség, hogy a távolságméréshez a robot nem egy Sharp szenzort, hanem egy SRF-04 ultrahangos távolságmérő szenzort használ.

Klann szerkezet ( https://en.wikipedia.org/wiki/Klann_linkage )

Mindíg is szerettem volna egy ún. Klann lépegetőt használó robotot, de sose jutott idő a szerkezet megtervezésére és megépítésére. Szerencsére találtam a neten kész megoldást, ami nekem a robotépítéshez pont megfelelt.

https://www.dfrobot.com/product-913.html

Két ilyen kit-et felhasználva, kis modosítással elkészítettem a robotalvázat:
( https://www.robotshop.com/letsmakerobots/spider-walker-robot )

Ultrahangos távolságmérő szenzor

Az Ar-Du robotomról szóló cikkben már ismertettem az SRF-04 ultrahangos távolságmérő szenzor használatát. A robotvezerlő algoritmus is teljesen megegyezik az akadálykikerülő robotéval, csak a Sharp szenzort használo Tavmeres() függvényt kellett kicserelni az alábbi, SRF-04 ultrahangos távolságmérő szenzort használó UHtavmeres() függvénnyel:

int UHtavmeres() // Ultrahangos tavolsagmeres
{
digitalWrite(TriggerPin, HIGH); // 10us-os Trigger jel
delayMicroseconds(10);
digitalWrite(TriggerPin, LOW); // Echo time merese
EchoTime = pulseIn(EchoPin, HIGH); //tavolsag kiszamitasa: mm = 10 * EchoTime / 58
return 10*EchoTime/58;
}

A Robotpók-ról egy kis videó: