Robot karmiący, czyli połączenie mechaniki, elektroniki i oprogramowania

   W ostatnich miesiącach, w związku z epidemią COVID-19, dużo się zmieniło jeśli chodzi o sytuację społeczną nas wszystkich, a szczególnie osób samotnych i niepełnosprawnych. Wiele osób potrzebuje pomocy, ale nie może jej otrzymać ze względu na sytuację epidemiologiczną.

Pomyślałem, że wykorzystując swoją wiedzę, umiejętności, kompetencje, mógłbym zrobić coś, co ułatwiłoby życie innym osobom, szczególnie samotnym i niezdolnym do samodzielniej egzystencji. Jednocześnie chciałem, aby to coś było nie tylko użyteczne, ale też wyjątkowe (unikalne).

Pomyślałem, że stworzę ROBOTA, który byłby w stanie nakarmić osoby niepełnosprawne.

 

Rys.1: Pierwsze testy

 

Wiedziałem, że nie będzie to proste. Znajomi mówili, żebym spróbował czegoś prostszego, że przecież osoby niepełnosprawne można karmić dożylnie, itp.

Mimo wszystko uznałem, że spróbuję stworzyć takiego ROBOTA.

Zacząłem od spisania wszystkich elementów potrzebnych do realizacji projektu.

Aby sprawdzić czy moja koncepcja jest w ogóle realna, możliwa do wykonania, zamówiłem na próbę kilka elementów, tj. silniki serwo, czujniki odległości, kamerkę do Raspberry Pi, itp. Pierwsze próby to mnóstwo przewodów, elektroniki i kilka prostych skryptów. Podczas testów wszystko wyglądało i działało rewelacyjnie.

Pierwotna wersja mojego ROBOTA była bardzo prosta. Opierała się na tzw. silnikach serwo. Silniki serwo to specyficzny rodzaj silników, które można za pomocą odpowiedniego sygnału obrócić do pożądanej pozycji. Ramiona zrobiłem z lekkich aluminiowych płytek, a do sterowania użyłem Raspberry Pi 3B. Oprogramowanie napisałem w C/C++, gdyż uznałem że to zapewni wydajność aplikacji, a ponadto pozwoli na oprogramowanie wrażliwych funkcjonalności ROBOTA (np. reakcję ROBOTA na przeszkodę, czy szybkie podjęcie przez niego „właściwej” decyzji). Silniki serwo podłączyłem do osobnego zasilania, zaś sterowanie do Raspberry Pi.

Okazało się, że podłączenie sterowania silników bezpośrednio do Raspberry Pi nie sprawdza się. Raspberry Pi powodowało bardzo duże opóźnienia, przez co sygnał sterujący silnikami był mało precyzyjny. To skutkowało, że ramię ROBOTA zamiast płynnie się poruszać, „tańczyło”. Zmodyfikowałem więc projekt i dodałem sterownik silników. To znacznie poprawiło sytuację. Jednak pojawiły się kolejne wyzwania. Po wyłączeniu i ponownym włączeniu zasilania aplikacja nie wiedziała w jakiej pozycji ustawione są poszczególne silniki. Nie była w stanie określić czy ROBOT aktualnie jest w stanie spoczynku, czy jest zawieszony nad talerzem. Problemem stało się również ponowne włączenie ROBOTA. Nie mogłem ustawić ROBOTA w pozycji startowej, ponieważ silniki tuż po włączeniu, gwałtownym ruchem ustawiałyby się na pozycję startową. Takie niekontrolowane ruchy mogłyby doprowadzić do uszkodzenia urządzenia, a także zranić osobę znajdującą się w pobliżu. Jedna z prób zakończyła się spaleniem silników, gdyż ramię zaczepiło o podstawę, a przez silniki popłynął prąd o zbyt dużym natężeniu.

Zgłębiłem więc wiedzę na temat budowy silników i dołączyłem dodatkowy przewód, który pozwolił mi na odczyt aktualnej pozycji silnika serwo.

Krótki filmik z pierwszego uruchomienia po wprowadzeniu w/w zmian w działaniu znajduje się pod adresem: https://youtu.be/JEfh78zjKZQ 

Niestety w silnikach występowały „luzy” co sprawiało, że podczas ruchu ramię trzęsło się. Jako, że ramię było złożone z 3 odcinków połączonych silnikami, łyżka na końcu ramienia również drżała. Dodatkowy problem stanowiła nieliniowa charakterystyka sygnału podawanego do silników wobec ustawienia kąta silnika.

 

Rys. 2: Charakterystyka silników serwo; wpływ sygnału wejściowego na kąt wychylenia ramienia silnika

 

W tej sytuacji rozwiązania były dwa: zakup i wymiana silników na bardziej stabilne lub przebudowa całego projektu poprzez wymianę silników serwo na silniki krokowe (takie, jak są używane w precyzyjnych robotach przemysłowych a także w drukarkach 3D).

Zdecydowałem się na rozwiązanie z silnikami krokowymi. W Internecie znalazłem projekt wydruku 3D ramienia robota wzorowany na ramieniu robota UR3 (https://www.thingiverse.com/thing:3327968), wykorzystałem z tego sam szkielet. Przy pomocy programu Blender 3D zmodyfikowałem ramię tak, aby spełniało moje potrzeby.

 

Rys. 3: Projekt ROBOTA w aplikacji Blender 3D

 

Następnie zamówiłem wydruk elementów.

 

Rys. 4: Wydrukowane elementy konstrukcji

 

Tym razem kupiłem sprawdzone, solidne silniki. Obecnie każdy silnik jest sterowany osobnym sterownikiem, który posiada w sobie już oprogramowanie.

W trakcie realizacji projektu odświeżyłem sobie wiedzę z trygonometrii, macierzy, rozwiązywania równań z wieloma niewiadomymi, itp. Wszystko po to, aby móc ustawić ramiona ROBOTA wraz z łyżką w odpowiedniej pozycji. Kolejna rzecz to nabranie na łyżkę porcji oraz późniejsze przemieszczanie się łyżki w przestrzeni (to złożony proces). Dodam, że ogromnie ważne jest, aby łyżka przemieszczała się w sposób bezpieczny i przewidywalny, czyli po linii prostej (talerz-głowa), a nie jak ramię koparki (po łuku). Ważna jest również prędkość i przyspieszenie. Łyżka nie może gwałtownie zatrzymać się przed ustami osoby, bo mogłoby to spowodować wylanie się porcji na osobę karmioną. Inną niedogodnością mogłoby być obijanie łyżką o talerz, szczególnie jako „walor” słuchowy. To wszystko trzeba było rozwiązać i oprogramować.

Tworząc oprogramowanie, w pierwszej kolejności rozpisałem wzorami matematycznymi ustawienie wszystkich kątów ramion oraz położenie łyżki ROBOTA.

Widząc, że uproszczony model robota wygląda następująco:

 

Gdzie:

rA, rB, rC – długości poszczególnych części ramienia robota [cm]

q1 – kąt silnika (obracającego ramieniem) [rad]

q2 – kąt silnika podnoszącego ramię [rad]

q3 – kąt silnika zginającego ramię [rad]

q4 – kąt silnika zginającego końcówkę ramienia (prostowanie położenia łyżki)

Dla uproszczenia, w poniższym opisie nie uwzględniam dwóch silników znajdujących się na końcu ramienia. Służą one do głównie do pozycjonowania łyżki podczas nabierania porcji z talerza.

Położenie końcówki takiego ramienia można wyliczyć wg. następującego wzoru:

X = cos(q1) * (rC * cos(q2 + q3 + q4 - PI) + rB * cos(q2 + q3 - PI) + rA * cos(q2))

Y = sin(q1) * (rC * cos(q2 + q3 + q4 - PI) + rB * cos(q2 + q3 - PI) + rA * cos(q2))

Z = rC * sin(q1 + q2 + q3 – PI) + rB * sin(q2 + q3 - PI) + rA * sin(q2)

Znając odległość i położenie w przestrzeni dwóch elementów - talerza oraz głowy, mogłem określić drogę łyżki. Jest to dość trudne i pracochłonne, więc nie będę opisywał szczegółowo tej metody.

Dodam tylko, że pozycję głowy (jej skierowanie, ułożenie) określiłem wykorzystując oprogramowanie OpenCV oraz Dlib na podstawie położenia oczu, nosa oraz kącików ust.

 

Rys. 5: Określenie punktów charakterystycznych twarzy za pomocą oprogramowania Dlib

 

Na powyższym obrazie ważne są punkty 8, 27, 33, 36, 45, 48 i 54.

Na podstawie w/w obliczeń oraz danych miałem już wszystkie informacje niezbędne do oprogramowania elementów ROBOTA.

Do sterowania każdego silnika użyłem zestawów: sterownik TMC2130 + Arduino ProMicro. Zaś całością ROBOTA steruje aplikacja napisana w C++ uruchomiona na Raspberry Pi.

 

Rys. 6: Testy sterowania silnikiem krokowym

 

Jak wspomniałem wcześniej, każdy silnik ma swoje własne sterowanie. Po to, aby zapewnić maksymalną wydajność każdego elementu ROBOTA. Dzięki takiemu rozwiązaniu mam pewność, że ROBOT jest w stanie zareagować na przeszkodę i zadziałać w sposób zaplanowany i przewidywalny.

W/w kontrolery mają w sobie czujniki oporu (tzw. homing sensor), Dzięki takiemu rozwiązaniu, w sytuacji gdy ROBOT napotka przeszkodę to się zatrzyma, a nie będzie na siłę parł „do przodu”.

Obecna wersja ROBOTA wgląda i działa bardziej profesjonalnie:

 

Rys. 7 i 8: ROBOT w trakcie działania

 

Aktualnie pracuję nad dodatkowymi zabezpieczeniami. Wymieniam także sterowniki na wyższy model. Te obecne operują na zbyt niskich prądach i czasem ROBOT nabierając porcję jedzenia traktuje boki talerza jako przeszkodę i zatrzymuje się na 2-3 sekundy. Przy większych prądach, nie będzie aż tak bardzo czuły.

Doskonalę też metodę określania położenia i ułożenia ust, skierowania twarzy, itp.


Krzysztof

Starszy Programista Java w Nexio Management

Powrót