R-team
Rychnov nad Kněžnou
Martin Locker i tentokrát dokázal, že i s malým robotem je možné soutěžit. Použil již osvědčenou technologii z prvního ročníku Robotour (kombinace kompas a odometrie) a úspěšně dokončil hned několik segmentů.
Robot pro soutěž RoboTour 2007
RoboCar 07
R-team (Martin Locker)
Klub robotiky VOŠ a SPŠ Rychnov n. Kn.
Úvod
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
 Elektronika |
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.
Ří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 (maximální 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ěť 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 brzdě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ého” 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ěť 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:
// vypocet zmeny smeru s omezenim na max. 90° Zmena_smeru = Limit((Kompas - Kompas_stary)/2) // vypocet ujete vzdalenosti v mm Zmena_polohy = (Enkoder - Enkoder_stary)/Tick_mm Pozice.a += Zmena_smeru // zmena aktualniho 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 Ackermannovým řízením.
Vlastní řízení robota je realizováno dvěma 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
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.