Czech English

Cube solver

aneb Když si robot hraje, (konstruktér) nezlobí!

Impulsem k tomuto dílku bylo shlédnutí několika videí (youtube), na kterých rubikovu kostku skládají i roboti z Lega. Zároveň to byla výzva k dokončení několika odložených projektů; vyřešení kinematiky průmyslového manipulátoru a napsání SW alespoň pro jeho základní řízení, zpracování obrazu a na konec, jak se ukázalo, i řešení síťové komunikace.


Po jednom víkendu už byl robot schopen složit kostku, když se mu předala posloupnost pohybů a po dalším týdnu ji složil již zcela samostatně.
Ale teď popořádku. Celý projekt lze rozdělit na 3 samostatné úkoly: manipulace s kostkou, zpracování obrazu pro zjištění stavu kostky a vlastní algoritmus skládání.
Pro manipulaci s kostkou jsem zvolil pro mne nejjednodušší řešení a to použití průmyslového manipulátoru Mitsubishi MR-501, který jeiž delší dobu ve škole slouží k výuce základů programování průmyslových robotů. Je to však poněkud starší kousek, a tak bylo nutno vyřešit řízení z PC. Díky poměrně slušnému manuálu se mi s trochou úsilí (odposlech sériové komunikace) podařilo přimět robota ke spolupráci. Komunikační program, který jsem napsal zatím není ideální, vždy čeká na dokončení povelu robota, a tak aplikace "nekomunikuje". V plánu je přepsání jako zvláštní vlákno, které poběží na pozadí a bude pouze pro komunikaci s robotem.
Tím byl kok 0 splněn, robot se hýbe. Přichází krok 1, oprášil jsem trošku znalosti z řízení kinematických mechanizmů, abych vyřešil přímou a hlavně inverzní úlohu kinematiky. Cože to znamená? Použitý manipulátor je klasické sériové koncepce, 3 rotační osy pro polohování a 2 klouby pro orientaci chapadla, celkem 5 stupňů volnosti. Poloha manipulátoru se tedy určuje polohou jednotlivých kloubů, ale pro řízení robota je nutné umět určit polohu chapadla v kartézských souřadnicích systému XYZ. K tomu právě slouží rovnice, kterými lze tzv. kloubové souřadnice přepočítat na XYZ a naopatk (viz. schéma robota a rovnice). Dalším z dílčích úkolů bylo naprogramování pohybu robota pro úsečce. Protože, pokud zadáme robotu koncovou polohu pohybu, tak výsledný pohyb je obecná křivka vzniklá současným pohybem otočných kloubů. Pohyb po definované trajektorii je řešen tak, že jsou spočítány souřadnice bodů s definovaným rozestupem v XYZ a ty jsou přepočítány do kloubových souřadnic. Tyto souřadnice jsou použity jako posloupnost bodů, kterými manipulátor prochází a realizuje tak, přibližně požadový pohyb.
Následoval další na první pohled neřešitelný úkol. Jak pohybovat kostkou, když robot má jen jednu ruku? Druhou, i když pevnou ruku realizuje "ohrádka" pevně umístěná na stole, a přidržuje tak kostku při otáčení. Zbývalo ještě vyřešit otáčení kostky kolem vodorovné osy. Vyřešilo to snížení ohrádky na jedné straně, uchopení kostky za roh a smýknutí kostky po podložce. Tím dojde k jejímu překlopení.
Nyní jsem sestavil funkce pro manipulaci s kostkou jako posloupnosti jednoduchých povelů:
1. tah - otočení 1 patra kostky o 90°, 180° nebo -90°
2. otočení celé kostky o 90°, 180° nebo -90°
3. překlopení kostky o 90° kolem vodorovné osy
(příklad obr.):
      - dolů
      - zavřít
      - nahoru
      - otočit
      - dolů
      - otevřít
      - nahoru
Z těchto základních pohybů. lze sestavit požadovanou posloupnost pohybů. Pokud chceme otočit například L stěnu, otočíme celou kostku o 90° ve směru hodin, přklopíme požadovanou stěnu dolů a nyní již můžeme provést tak.
Pro určení posloupnosti potřebných kroků pro realizaci tahů bylo použito stavového automatu (každá poloha kostky je jedem stav a tabulka přechodů vrací kroky pro dosažení požadovaného stavu). Každý stav je definovám šesticí (čísel) určujících, která stěna kostky se nachází nahoře, vpředu, ...
Ve výchozí poloze se stěnám kostky přiřadí označení stěn F, , L, R, B a při každém otočení je stav aktualizován dle provedeného pohybu.
Postup pro složení kostky je zadáván tak, jak je běžně používán v článcích o rubikově kostce. Např. L - otočení levé stěny o 90°, T2 - otočení horní stěny o 180°, ... Robot kostku překlopí požadovanou stěnou dolů a provede příslušný tah.
Robot skládá nyní kostku, když se mu řekne jak.
Kostku sice ručně složit zvládnu, ale potřebuji na to určitě mnohem více tahů, než je třeba a zalgoritmizovat tento postup bych asi také nedokázal. Proto jsem se porozhlédl po internetu a objevil jsem spoustu stránek zabývajícíh se fenoménem rubikovy kostky. Nakonec mne zaujal program tvářící se jako server, kterému předáte aktuální rozložení kostky a on vám vrátí posloupnost kroků potřebných pro její složení. Talže další odpoledne zabrala konzultace s kamarádem programátorem o tom, jak se programuje síťová komunikace.
Zbývalo už "jen" naučit robota vidět. Do této oblasti jsem se už dlouho chystal, ale stále jsem z ní měl "přílišný" respekt. Knihovnu OpenCV jsem měl rozchozenu, stejně tak foťák připojený jako webkameru. Řešení jsem zkusil co nejjednodušší, tj. převod obrázku kostky z RGB do HSV barevného prostoru a jednoduché prahování vybraných oblastí obrazu podle barevné složky. Fungovalo to celkem uspokojivě, kromě rozpoznávání červené a oranžové. Obě barvy měly v barevné složce téměř shodnou hodnotu. Takže jsem u těchto dvou barev ještě prahoval podle jasu (oranžová je jasnější). Výsledná barva políčka je určena podle převažující barvy oblasti určené prahováním. Takto rozpoznávání barev při rozumných světelných podmínkách fungovalo na cca 95%.
Nakonec jsem však tento "výtvor" nepoužil, protože jsem objevil variantu prográmku pro skládání kostky i s interfacem pro snímání kostky webkamerou [3]. Program kromě zjištění rozložení kostky z obrazu kontroluje realnost této pozice, a pokud zjistí, že dané rozložení barev na kostce je nemožné, tak vrátí chybu snímání.
Po zkompletování všech částí projektu, sklácá robot kostku zcela samostatně. Nejprve nasnímá všech šest stěn, spočítá posloupnost kroků pro složení a pak již jen kostku složí. Vzhledem k tomu, že manipulátor není žádný mladík (1990), je jeho maximální rychlost 0,5 m/s, tak mu trvá složení kostky asi 6,5 minuty.

Použitý software:

[1] MS Visual C + OpenCV
[2] Delphi

Kinematické schéma robota

Přímá a inverzní úloha kinematiky

Stavový automat

Popis stavu kostky

Barevné prostory