Hexapod
Roboty ·Sześcionogi stwór z kamerkami.
Projekt, którym zajmowałem się przez pierwsze 2 lata studiów w ramach działalności koła naukowego Integra. Moim wkładem było oprogramowanie do sterowania robota oraz stereowizji.
Do poruszania konstrukcją wykorzystane zostało 18 serw Hitec po 3 na każdą nogę. Jako główny komputer robota wykorzystalismy Raspberry Pi 2. Komunikowała się ona z sterownikiem serw Pololu poprzez interfejs UART umożliwiając zadawanie odpowiednich pozycji serw. Całą konstrukcję zasila bateria LiPol o pojemności 4000 mAh, co pozwalało na ok. 1h użytkowania robota. Napięcie z akumulatora dostowywaliśmy za pomocą przetwornic (3 do zasilania serw oraz 1 dla Raspberry). Wykorzystaliśmy także przetwornik ADC MCP3008, aby móc mierzyć aktualne napięcie na baterii. Komunikował się on z Raspberry za pomocą SPI, a następnie napięcie było wysyłane i wyświetlane w aplikacji klienta.
Robot jest sterowany z poziomu laptopa, który komunikuje się z Raspberry poprzez WiFi przez protokół TCP. Do poruszania robotem można używać zarówno pada jak i klawiatury. W celu odpowiedniego sterowania najpierw napisałem aplikację obrazującą model matematyczny robota. Na tym modelu została zaimplementowana kinematyka odwrotna, na której następnie stworzyłem modele poruszania się robota. Odpowiednio przeliczone uzyskane kąty na sygnały PWM dla serwonapędów były wysyłane do sterownika.
Sterowanie robotem
Aplikacja uruchamiana na laptopie zawiera wizualizacje modelu robota oraz odbiera sygnały sterowania od użytkownika, które następnie przekazuje do programu na Raspberry. On natomiast dokonuje odpowiednich przeliczeń na sygnały PWM, które następnie przekazuje do sterownika serw. Z Raspberry wysyłana jest także informacja zwrotna do aplikacji użytkownika z aktualnym napięciem na baterii, dzięki czemu jej poziom jest na bieżąco monitorowany. Osobny program służy do realizacji stereowizji. Raspberry wysyła obrazy z obu kamer przy użyciu programu gstreamer. Na laptopie oba obrazy są odbierane i następnie przy użyciu odpowiednich macierzy (uzyskanych podczas kalibracji) przekształcana oraz liczona jest macierz obrazu dysparcji. Zastosowałem także filtr po przeliczeniu dysparcji, który dodatkowo poprawia rezultaty.
Aplikacje modelu Hexapoda można pracować w 2 trybach:
Tryb modelu
W trybie modelu można poruszać się po środowisku za pomocą klawiszy W/S/A/D/Q/E oraz barów alfa, beta i gamma służących do zmiany kąta widzenia. Modelem robota poruszać można za pomocą klawiszy w/s/a/d/q/e oraz numerów służących do wyboru odpowiedniego trybu chodzenia robota.
Tryb połączenia z Hexapodem
Uruchamia się jeśli dodatkowo przekażemy do programu przy uruchomieniu adres IP Raspberry. Sterowanie aplikacją na laptopie nie zmienia się. Różnica z poprzednio omówionym trybem (tryb modelu) jest taka, iż teraz odpowiednie komendy wysyłane są także do Hexapoda. Dodatkowo wyświetlane na ekranie laptopa jest również napięcie na baterii.
Bliższy opis elementów zrealizowanej aplikacji
GUI
Zadaniem najbardziej oddalonym od samej idei Hexapoda był moduł wyświetlania, czyli GUI. Do wyświetlania użyto wyłącznie prostego okienkowego trybu wyświetlania dołączonego do biblioteki OpenCV w celu debugowania. Zastosowałem rzutowanie perspektywiczne zgodnie ze wzorami zawartymi w 1, aby otrzymać symulację w 3D. Zdefiniowałem płaszczyznę kamery, na którą odbywało się rzutowanie świata 3D symulacji. Dodałęm także przesuwanie płaszczyzną kamery za pomocą przycisków oraz możliwość jej obracania przy pomocy toolbarów.
Robot
To jest główny moduł odpowiedzialny za obliczenia związane ze sterowaniem robota. Zawarłem w nim implementację kinematyki odwrotnej w podejściu trygonometrycznym opisaną w 2. Główna klasa robota zawiera klasę nóg robota, które są wydzielone. Jest osobna klasa, w której znajdują się różne algorytmy chodzenia.
Chodzenie
Najlepsze efekty dawało chodzenie po paraboli. Dobierane są kolejne punkty paraboli, która zaczyna się w miejscu, gdzie aktualnie znajduje się końcówka nogi robota. Kończy się tam, gdzie ma się ostatecznie znaleźć. Przy użyciu kinematyki odwrotnej wyliczane są kąty tak, aby końcówka danej nogi znalazła się w punkcie docelowym.
Pozostałe moduły
Kontroler, który odpowiednio interpretuje wysłane komendy na funkcje, np.: chodzenia lub poruszania bazą robota. Znajduje się tutaj również moduł do komunikacji z serwami (tylko w wersji programu dla Raspberry). Zamieniane są wyliczone kąty dla każdej nogi na odpowiednie sygnały PWM, dla każdego serwa (jedna noga składa się z trzech serwonapędów). Następnie są one wysyłane.
TCP
W module TCP znajduję się cały kod do komunikacji pomiędzy Raspberry a laptopem. Kod został zaczerpnięty z 3. W ramach komunikacji laptop łączy się z Raspberry i na porcie 8081 odbiera aktualne napięcie baterii, które Raspberry stale udostępnia. Komunikacja w ramach odczytu napięcia baterii odbywa się w osobnym wątku, aby nie zakłócać pozostałych operacji. Natomiast na port 8080 aplikacja z laptopa wysyła podane komendy.
Stereowizja
Poniżej przedstawie krótki opis etapów realizacji stereowizji:
Wykonanie zdjęć kalibracyjnych
Aby skalibrować parę stereowizyjną wykonywane są zdjęcia ustalonego wzoru, np. szachownicy. W tym celu napisałem aplikację, która wyświetlała obraz z obu kamer. Co ustalony czas zapisywała zdjęcie pod odpowiednią nazwą oraz tworzyła listę utworzonych obrazów. Wykonałem około 50 zdjęć tak, aby jak najlepiej pokryć cały obszar widoku kamer.
Widoczny na zdjęciach zamieszczonych powyżej jest także następny napotkany problem - obiektywy w obudowach kamer zamontowane były nierówno. Oczywiście to przesunięcie eliminowane jest podczas kalibracji, jednak w rezultacie podczas właściwej stereowizji ograniczone jest pole widzenia od góry i dołu (część pixeli widoczna jest tylko przez jedną kamerkę).
Kalibracja
Do samej kalibracji użyłem przykładu z książki Learning OpenCV. W parametrach wywołania umieszczano wysokość oraz szerokość szachownicy (liczba pól), a także długość boku kwadratu w centymetrach. Bardzo ważne jest, aby szachownica miała kwadratowe pola, gdyż przy użyciu szachownicy o bokach różniących się nieznacznie cały proces kalibracji zostałby przeprowadzony niepoprawnie wraz z listą zdjęć, powstałych w trakcie tej błędnej kalibracji.
W wyniku działania kalibracji otrzymałem parametry zewnętrzne oraz wewnętrzne kamer, które następnie używane były do właściwej stereowizji. Uzyskano błędy: błąd średniokwadratowy (RMS) = 0.0503053 średni błąd epipolarny = 0.517146 Są to wartości niskie, co świadczy o poprawnie wykonanej kalibracji.
Wybór algorytmu oraz dobranie parametrów
Najpierw przetestowałem dostępne w bibliotece OpenCV algorytmy BM i SGBM, lepsze rezultaty uzyskałem dla SGBM. Jest on bardziej wymagający obliczeniowo, aczkolwiek przy rozdzielczości 320x240 można było uzyskać dobre przetwarzanie w czasie rzeczywistym. Następnie dobrałem optymalne parametry dla tego algorytmu. W tym celu napisałem aplikację na podstawie przykładu użycia SGBM zawartego w bibliotece OpenCV. Dodałem do niej suwak tak, aby można było wygodnie zmieniać parametry i obserwować uzyskiwane rezultatu.
Dobranie parametrów dla postfiltracji
W celu uzyskania gładszego obrazu stereowizyjnego zastosowałem postfiltrację, która dodana została stosunkowo niedawno do modułów dodatkowych biblioteki OpenCV - contrib.
Ostateczna aplikacja
W ostatecznej aplikacji połączone zostały wszystkie opisane elementy. W wyniku czego otrzymano przesył obrazu z obu kamer na laptopa. Tam obraz był rektyfikowany, a pixele były dopasowywane za pomocą algorytmu SGBM. Przy użyciu posfiltracji obraz dysparcji jest poprawiany i na koniec wyświetlany. Dalszym etapem rozwojowym może być przeliczenie chmury punktów na podstawie dysparcji oraz następnie wykrywanie i omijanie przeszkód.
Poza samą realizacją stereowizji ważnym elementem był także optymalny przesył obrazu z obu kamer. Pierwszym podejściem było użycie MJPGstreamera. Efekty jednak nie były zadowalające. Klatki, które nie zostały jeszcze wysłane, były składowane w buforze. W wyniku czego, im dłużej działała aplikacja, tym większe było opóźnienie. Dodatkowo występowała różnica czasu pomiędzy klatkami z obu kamer, co w przypadku stereowizji jest nieakceptowalne. Dlatego zdecydowano się użyć gstreamera. Jest on bardziej zaawansowany, przez co uruchomienie go zajęło więcej czasu. Jednak uzyskany efekt jest bardzo dobry. W jego działaniu, jeśli nie zdąży się wysłać klatki przed przybyciem następnej, to zostaje ona porzucona. W wyniku tego wyeliminowane zostało rosnące opóźnienie. Dlatego też nie występowało już opóźnienie pomiędzy klatkami. Rozdzielczość przesyłanych obrazów wynosi 320x240. Przeprowadzono także testy dla rozdzielczości 640x480, ale spadek FPS był nieproporcjonalny do wzrostu jakości.
Osiągnięcia
Zawody robotyczne
- Robocomp 2017 w Krakowie (kategoria Freestyle)
- Robotic Arena 2017 we Wrocławiu (kategorie Freestyle oraz wyścig robotów kroczących). Pierwsze miejsce w kategorii wyścig robotów kroczących.
- Robomaticon 2018 w Warszawie (kategoria Freestyle)
- Robotic Tournament 2018 w Rybniku (kategoria Freestyle)
- Robotic Arena 2019 we Wrocławiu. Drugie miejsce w kategorii wyścig robotów kroczących.
Wydarzenia
- Targi pracy Kariera IT
- TEDxAGHUniversity
