obot klub Rychnov
RoboCar 07
robot pro soutěž Robotour 2007
Robot RoboCar07 byl stavěn, stejně jako robot pro minulý ročník, s ohledem na maximální jednoduchost a minimální náklady. Po poměrně dobré zkušenosti z loňského roku s použitím odometrie a kompasu, bylo použito stejných prostředků i pro tento ročník. Zásadní nedostatek minulého robota byl jeho malý podvozek bez odpružení, který se ukázal na venkovní použití jako nevhodný. Proto byl nový robot postaven na novém podvozku z hračky malého terénního automobilu.
Mechanické řešení:
Jako základ robota je použit podvozek z terénního autíčka. Tento podvozek, i když
je poměrně malý, má několik zásadních výhod. Má náhon všech čtyř kol, přičemž jen na
přední nápravě je diferenciál. Díky tomu je určení ujeté vzdálenosti z otáčení kol
poměrně přesné, i když dojde k prokluzu některého kola. Další výhodou je vysoké
zpřevodování motoru, i tím i poměrně nízká rychlost autíčka, tj. vhodná pro robotické aplikace.
Pro použití podvozku jako základu robota, byl doplněn inkrementální enkodér pro regulaci
rychlosti otáčení elektromotoru a měření ujeté vzdálenosti. Dále bylo původní řízení typu
"doraz-doraz" nahrazeno modelářským servem pro proporcionální řízení zatáčení.
Na podvozek byla namontována plastová deska pro montáž všech elektronických částí robota
a pro kompas byl instalován stojánek, aby nebyla ovlivňována jeho funkce magnetickým
polem pohonného elektromotoru. Poslední úpravou byla montáž držáku pro akumulátory,
které nahrazují původní s nedostatečnou kapacitou.
Pohon:
Pohon je realizován stejnosměrným elektromotorem třídy 280 s maximálními otáčkami
12 000 ot/min při napětí 7,2V s odběrem 2A při řežimu maximální účinnosti.
Pro regulaci otáček je přímo na hřídel elektromotoru instalován optický inkrementální enkodér.
Clonka se 40 štěrbinami na otáčku je použita z počítačové myši. Snímání realizuje dvojice
optických závor Sharp GP1S56, jejich signál je následně upraven do úrovní CMOS komparátory
s hysterezí. Tyto optické závory byly vybrány z důvodu jejich malých rozměrů, bohužel ne
zcela vyhovují rychlostí (frekvence při maximálních otáčkách motoru je více než 8 kHz).
Napájení robota zajišťují 2 bloky LiON akumulátorů o 2 článcích s kapacitou 2000 mAh,
jeden napájí pohon a druhý elektroniku robota.
Elektronika:
Veškeré elektronické části robota tvoří osvědčené moduly použité na robotech pro soutěž Eurobot.
Řízení pohonu:
Výkonové řízení elektromotoru je zajištěno jednoduchým galvanicky odděleným H-můstkem
(http://www.micer.wz.cz/clanky/c007/regser.html).
Řízení elektromotoru realizuje modul s mikrokontrolerem AVR ATmega8 běžícím na
frekvenci 16 MHz. Můstek je ovládán PWM výstupem s 8 bitovým rozlišením a frekvencí 2 kHz.
Stavu enkodéru je čten a pohyb je dekódován v přerušením s frekvencí 50kHz
(max. frekvence impulsů při vyhodnocování všech čtyř hran je 32 kHz).
Senzory:
Jediným použitým senzorem pro navigaci je kromě enkodéru pro zjištění ujeté vzdálenosti
kompas. Je použit modul elektronického kompasu CMPS03 připojený přes sběrnici I2C.
Ve fázi testování byly zkoušeny také modelářské rate-gyro a GPS modul, které však
nakonec nebyly použity.
Hlavní řídicí jednotka:
Funkci nadřízeného systému zajišťuje modul s mikrokontrolerem AVR ATmega 128
taktovaným frekvencí 16 MHz. Mimo mikrokontroleru obsahuje informační LCD displej,
pamět EEPROM připojenou přes sběrnici I2C o kapacitě 64kB, rozhraní pro připojení
dalších modulů přes sběrnici RS485 a konektor s vyvedenými I/O piny procesoru včetně
PWM výstupů a vstupů A/D převodníku.
Parametry robotického podvozku:
- - Hmotnost: cca 1 kg
- - Maximální rychlost: 1,6 m/s
- - Rozlišení měření ujeté vzdálenosti: cca 0,05 mm/impuls
- - Rozměry délka x šířka x výška: 300 x 160 x 250 mm
Popis použitého software:
Software modulu pro řízení motoru:
Modul řízení motoru pracuje nezávisle na nadřízeném systému. Vlastní software je
tvořen třemi samostatnými procesy.
Proces 1, který je vyvoláván časovým přerušením s frekvencí 50kHz zajišťuje skenování stavu enkodéru, jeho dekódování a čítání impulsů. Je použito vyhodnocení všech čtyř hran kvadraturního signálu enkodéru, čemuž odpovídá frekvence impulsů při maximální rychlosti 32 kHz. Tento způsob snímání, je oproti použití přerušení od vstupních signálů, odolnější proti zákmitům, které mohou na signálech z enkodéru vznikat. Počet impulsů z enkodéru je uchováván ve 32 bitové proměnné, jejíž hodnota je používána pro určení ujeté vzdálenosti.
Proces 2 zajišťuje regulaci otáček elektromotoru PI regulátorem rychlosti,
jehož výpočet probíhá s frekvencí 500 Hz. Tomu odpovídá rozsah rychlostí 0 - 64 tiků/periodu,
což je dostatečné pro kvalitní regulaci rychlosti. Kromě regulace rychlosti
regulátor realizuje i plynulé zrychlování a brždění při skokové změně požadované hodnoty rychlosti.
Výstup pro řízení H-můstku je řízen PWM výstupem s 8 bitovým rozlišením a frekvencí 2 kHz.
Proces 3 realizuje komunikaci s nadřízeným systémem. Komunikace je realizována fyzickou
vrstvou RS485. Použitý paketově orientovaný komunikační protokol je popsán na wiki.robotika.cz.
Modul přijímá požadavky na rychlost pohybu a zpětně zasílá hodnoty aktuální polohy dané
čítačem impulsů enkodéru.
Software řídicího modulu:
Celý řídicí algoritmus probíhá ve smyčce s periodou 25 Hz, která je dána nejpomalejším
zařízením, tj. modulem kompasu.
Cyklus se sestává z několika částí:
- získání hodnot ze všech modulů a senzorů (čtení směru z kompasu, hodnoty čítače enkodéru, v případě ručního řízení čtení vstupů A/D převodníku, na který jsou připojeny výstupy analogového joysticku)
- výpočet odhadu aktuální pozice robota
- určení "cílové" bodu pro další pohyb
- výpočet regulačního zásahu pro řízení směru a rychlosti
- nastavení akčních veličin, tj. polohy serva řízení a požadované rychlosti motoru
Popis algoritmu řízení robota
Robot používá používá jednoduchý algoritmus jízdy po předem určené trajektorii.
Tzn. že robot nereaguje na prostředí a pouze "slepě" vykonává svůj úkol.
Trajektorie je dána množinou souřadnic bodů, které definují lomenou čárů.
Souřadnice bodů trajektorie je možné zadat ručně nebo získat záznamem při ručním řízení robota.
Tímto způsobem byla připravena data pro soutěž RoboTour 2007. Data získaná při
ručním projetí trasy byla ručně upravena a zredukován jejich počet, protože vnitřní
EEPROM pamět mikrokontroleru má velikost pouze 4kB, a může tak pojmout pouze 200 bodů
definujících trajektorii.
Výpočet aktuální polohy robota používá znalosti ujeté vzdálenosti z enkodéru a směru,
získaného z elektronického kompasu.
Nová pozice robota je určována pomocí metody Dead Reckoning, tj. připočítávání změny
polohy na základě znalosti délky a směru pohybu.
Poloha robota je definována záznamem tvořeným dvojicí souřadnic v mm a směrem natočení
robota v 1/100°.
Část kódu výpočtu nové pozice robota:
Zmena_smeru = Limit((Kompas - Kompas_stary)/2) // vypocet zmeny smeru s omezenim na max. 90°
Zmena_polohy = (Enkoder - Enkoder_stary)/Tick_mm // vypocet ujete vzdalenosti v mm
Pozice.a += Zmena_smeru // zmena aktualniho smeru natoceni o polovinu zmeny smeru
Pozice.x += Zmena_polohy * sin (Pozice.a * RAD) // vypocet X souradnice
Pozice.y += Zmena_polohy * cos (Pozice.a * RAD) // vypocet Y souradnice
Pozice.a += Zmena_smeru // uprava smeru o druhou polovinu zmeny smeru
Kompas_stary = Kompas // uschova aktualnich hodnot snimacu
Enkoder_stary = Enkoder
Zmena_polohy = (Enkoder - Enkoder_stary)/Tick_mm // vypocet ujete vzdalenosti v mm
Pozice.a += Zmena_smeru // zmena aktualniho smeru natoceni o polovinu zmeny smeru
Pozice.x += Zmena_polohy * sin (Pozice.a * RAD) // vypocet X souradnice
Pozice.y += Zmena_polohy * cos (Pozice.a * RAD) // vypocet Y souradnice
Pozice.a += Zmena_smeru // uprava smeru o druhou polovinu zmeny smeru
Kompas_stary = Kompas // uschova aktualnich hodnot snimacu
Enkoder_stary = Enkoder
Cílová pozice robota je v každém kroku výpočtu určena posunutím po lomené čáře
definující trajektorii o hodnotu rychlosti pohybu. Tento bod "předbíhá" robota
o několikanásobek dráhy, kterou robot v každém kroku ujede. Tím je zajištěno,
že robot nepojede po lomené čáře, ale po trajektorii, která se jí pouze přibližuje.
Díky tomu lze řídit i robota, který není schopen měnit směr pohybu na místě, což je
právě případ automobilového podvozku s ackermanovým řízením.
Vlastní řízení robota je realizováno dvěmi proporcionálními regulátory. Jeden zajišťuje
regulaci směru a druhý výpočet požadované rychlosti pohybu.
Regulační odchylka pro regulaci směru je určena jako rozdíl mezi azimutem daným spojnicí
aktuální pozice robota a vypočteným cílovým bodem a aktuálním směrem robota (získaným z kompasu).
Základem pro výpočet požadované rychlosti je vzdálenost robota do vypočteného cílového bodu.
V ideálním případě je tato vzdálenost rovna součinu hodnoty "předbíhání" cílového bodu
(konstanta AHEAD) a nominální rychlosti robota. Potom je vypočtená rychlost rovna nominální.
Pokud je vzdálenost do cílového bodu vetší, tj. robot se opožďuje, je vypočtená rychlost
úměrně vyšší s omezením na maximální povolenou rychlost (tzn. robot postupně dohání cílový bod).
Pokud je vzdálenost menší, tak výsledná rychlost úměrně nižší. Toto nastane v případě, že
se robot přebíhá nebo v případě lomení na předepsané trajektorii. Díky tomu robot
v zatáčkách automaticky zpomaluje, a to úměrně "ostrosti" zatáčky (viz. obr.).
Část kódu regulátorů rychlosti a směru:
dx = CilovyBod.x - Pozice.x; // x slozka vektoru smeru
dy = CilovyBod.y - Pozice.y; // y slozka vektoru smeru
d = sqrt(dx * dx + dy * dy); // vzdalenost do ciloveho bodu
if (d > MAX_VELOCITY * AHEAD) // pokud je vetsi nez ...
d = MAX_VELOCITY * AHEAD; // tak omezit
a = atan2(dx, dy) * 18000 / M_PI; // smerovy uhel, tj. pozadovany smer
a -= Pozice.a; // vypocet regulacni odchylky smeru
if (a > 18000) a -= 36000; // uprava do rozsahu +-180
if (a < -18000) a += 36000;
Speed = d * SPEED_GAIN / AHEAD; // vypocet reg. zasahu pro rychlost
Ride = -a * RIDE_GAIN; // vypocet reg. zasahu pro smer
dy = CilovyBod.y - Pozice.y; // y slozka vektoru smeru
d = sqrt(dx * dx + dy * dy); // vzdalenost do ciloveho bodu
if (d > MAX_VELOCITY * AHEAD) // pokud je vetsi nez ...
d = MAX_VELOCITY * AHEAD; // tak omezit
a = atan2(dx, dy) * 18000 / M_PI; // smerovy uhel, tj. pozadovany smer
a -= Pozice.a; // vypocet regulacni odchylky smeru
if (a > 18000) a -= 36000; // uprava do rozsahu +-180
if (a < -18000) a += 36000;
Speed = d * SPEED_GAIN / AHEAD; // vypocet reg. zasahu pro rychlost
Ride = -a * RIDE_GAIN; // vypocet reg. zasahu pro smer
Závěr
Realizované řešení robota předčilo očekávání. Výsledek v prvním kole soutěže,
kdy robot ujel více než 200 m bez opuštění cesty, bylo více než jsem doufal.
Ale již v tomto kole se projevila závada, která znemožnila tento výkon v dalších
kolech opakovat. Byla to nedostatečná odolnost optického enkodéru proti vibracím,
které při jízdě po kostkách vznikaly. Při testování na asfaltu se tato závada neprojevila.
Důsledkem byly náhodné výpadky kroků enkodéru, se kterými si software pro jeho
dekódování neporadil. Vzhledem k tomu, že mi vždy hardware fungoval na 100%,
jsem nepovažoval toto za nutné v softwaru ošetřit. Výsledek byla nefunkční regulace
pohonu a ztráta informace o ujeté vzdálenosti. Z toho plyne ponaučení, i když hardware
funguje, v softwaru počítat s možností, že za některých okolností fungovat nebude.
Dalším připravovaným rozšířením robota je přidání čidel na detekci překážek (sonar).
A následné vyřešení problému objetí překážky a návratu na definovanou trajektorii.