Cześć,
W tym artykule opowiemy wam o projekcie naszego robota typu delta z systemem wizyjnym i aplikacją do symulowania i sterowania. Jesteśmy Studenckim Kołem Naukowym „Control Engineers” Politechniki Śląskiej. Nasz robot delta to jeden z projektów, które właśnie realizujemy, przy wsparciu firmy Aiut.
Założeniem projektu jest konstrukcja robota, który ma za zadanie podnosić losowo położone elementy znajdujące się w jego polu roboczym. Zdecydowaliśmy się także na stworzenie dedykowanej aplikacji, która ma mieć funkcjonalności takie jak symulacja robota, sterowanie nim oraz programowanie – na wzór aplikacji dla robotów przemysłowych (np. Robot Studio).
Roboty delta – ogólnie
Na początek przyjrzyjmy się czym są roboty typu delta. Są to roboty o zamkniętym łańcuchu kinematycznym takim jak widać na poniższej grafice. Przeważnie wykorzystują one 3 lub 4 napędy (4. napęd jako oś obrotowa chwytaka), które mocuje się co 120 stopni na okręgu o określonym promieniu. Do napędów mocuje się ramiona. Do ramion z kolei mocuje się dwa przedramiona za pomocą przegubów kulowych. Wszystkie przedramiona łączą się z bazą efektora, również przegubami kulowymi. Roboty typu delta przeważnie umieszcza się nad taśmociągami na liniach produkcyjnych i wykorzystuje się do przenoszenia lekkich elementów – co można zobaczyć na nagraniu. Roboty typu delta to jedne z najszybszych robotów przemysłowych (o ile nie najszybsze), potrafią osiągać prędkości nawet 10 m/s. Dla porównania roboty sześcioosiowe osiągają prędkości około 2 m/s dla ruchu liniowego. Jednak mają one większe udźwigi, większe pole robocze, a także więcej stopni swobody.
Kinematyka prosta i odwrotna
Aby robot mógł wykonywać ruchy do punktów zadanych przez użytkownika, konieczne jest rozwiązanie zadania prostego i odwrotnego robotyki. Zadanie proste robotyki polega na tym, że na podstawie znajomości kątów silników określamy położenie efektora robota (np. chwytaka). Zadanie odwrotne robotyki polega natomiast na tym, że na podstawie zadanego położenia XYZ, określamy kąty jakie silniki muszą przyjąć, aby efektor robota znalazł się w zadanej pozycji. Normalnie użytkownik robota przemysłowego nie musi się przejmować rozwiązywaniem zadania prostego czy odwrotnego, ponieważ algorytmy są już zaimplementowane w robocie, który kupujemy. My jednak musieliśmy zaimplementować sami te algorytmy, skoro robota budujemy od zera. W robotach typu delta zadanie proste i odwrotne robotyki wiąże się z matematycznym rozwiązaniem równań sfer.
Matematyczne rozwiązanie zadania odwrotnego robotyki dla robota delta wiąże się z wyznaczeniem równań sfery oraz okręgu, a następnie znalezienie punktu wspólnego. Środek okręgu znajduje się w osi silnika, natomiast środek sfery w miejscu mocowania przedramienia w bazie efektora. Promień okręgu to długość ramienia robota, natomiast promień sfery to długość przedramienia robota. Po wyznaczeniu punktu wspólnego, który spełnia równanie sfery oraz okręgu jesteśmy w stanie określić kąt w jakim silnik musi ustawić ramię robota.
Grafika 2 – Kinematyka odwrotna
Zadanie proste robotyki wygląda podobnie – tutaj jednak tworzymy trzy sfery na końcach ramion robota o promieniu przedramienia i szukamy punktu przecięcia trzech sfer. Punkt przecięcia tych sfer stanowi wtedy punkt w którym znajduje się baza efektora robota.
Grafika 3 – Zadanie proste robotyki
Rozwiązanie zadania prostego i odwrotnego można znaleźć w internecie – ktoś już dawno to rozwiązał, dlatego my nie musimy zajmować się wyznaczaniem równań matematycznych. Mimo to i tak sami musimy zaimplementować ruchy liniowe, przegubowe i kołowe w naszym oprogramowaniu mikrokontrolera. Jeżeli jesteście zainteresowani bardziej zadaniem prostym i odwrotnym robotyki dla robota delta, to więcej informacji znajdziecie tutaj.
Przed rozpoczęciem projektu mechanicznego należy również wybrać parametry robota, takie jak promień bazy, promień bazy efektora, długości ramion i przedramion. To od tych parametrów zależy jakie pole robocze będzie miał robot.
Mechanika
Projekt konstrukcji wykonaliśmy w oprogramowaniu Inventor. Bazę efektora oparliśmy na trójkątnej konstrukcji z profili aluminiowych. Niestety nie mamy możliwości, aby robota przymocować do sufitu na uczelni. Wykorzystaliśmy przeguby kulowe Igus, serwonapędy przemysłowe oraz przekładnie planetarne Eldar o przełożeniu 1:8.
Grafika 4 – projekt robota w Inventorze
Układ sterowania
W układzie sterowania jako napędy wybraliśmy serwonapędy przemysłowe Inovance 400W z enkoderem absolutnym oraz protokołem CANOpen. Dokupiliśmy także baterie do enkoderów, aby nie bazować robota przy każdym uruchomieniu. Gdybyśmy nie używali przekładni, to wtedy bateria nie byłaby konieczna, ponieważ enkodery są absolutne. Jednak w trakcie jednego obrotu przekładni silnik wykonuje 8 obrotów, wobec czego chcieliśmy aby układ sterowania wiedział ile obrotów wykonał silnik – dzięki temu układ zna absolutną pozycję wału przekładni. Można tutaj dodać, że wałem przekładni obracać można (z niewielką prędkością) także, gdy serwowzmacniacze są wyłączone a i tak układ sterowania nie straci wiedzy o pozycji przekładni dzięki właśnie bateriom podłączonym do enkoderów serwosilników.
Jako jednostkę sterującą serwowzmacniaczami wybraliśmy płytkę Nucleo-F429ZI z mikrokontrolerem Stm32. Zaimplementowaliśmy sterowanie napędami z wykorzystaniem sieci CAN. Należy mieć tutaj na uwadze, że ruchy silników muszą być ze sobą zsynchronizowane, aby robot mógł wykonać ruch liniowy lub po okręgu. Oprócz wykorzystania sterowania napędami przy pomocy sieci, można także sterować za pomocą sygnałów krok/kierunek, co jest często spotykanym rozwiązaniem w hobbystycznych robotach typu delta.
Grafika 5 – Schemat układu sterowania
Ruch liniowy
Napisany przez nas algorytm, zapewniający ruch liniowy robota jako parametry przyjmuje punkt zadany oraz krok ruchu. Algorytm na początku określa położenie początkowe chwytaka robota, za pomocą zadania prostego robotyki. Następnie określa wektor jednostkowy skierowany od punktu początkowego do punktu zadanego. Wektor ten wykorzystywany jest do obliczenia punktów pośrednich, które pozwalają nam na wykonanie ruchu liniowego. Pomiędzy tymi punktami robot będzie poruszał się po nieokreślonej trajektorii, jednak jeżeli punktów tych wygenerujemy dużo, na przykład oddalone będą od siebie co 0.1 mm, to wtedy w przybliżeniu robot wykona ruch liniowy.
Poniższa grafika przedstawia wycinek kodu – funkcję calcTraceLin, która ma obliczyć wektor jednostkowy, który będzie kierował zadaną pozycję robota od punktu początkowego do punktu końcowego po linii prostej. Nowa zadana pozycja dla robota zapisywana jest tablicy p[], która potem wkładana jest do funkcji ikine() odpowiedzialnej za obliczanie zadania odwrotnego robotyki – nadpisuje ona tablicę th_rad[], która stanowi kąty zadawane potem silnikom. Funkcja calcTraceLin wywoływana jest tylko na początku ruchu. Oblicza ona tylko trzy pierwsze punkty znajdujące się na linii prostej. Początkowo obliczała ona wszystkie punkty przez jakie miał przejechać efektor, jednak spotkaliśmy się z problemami związanymi z alokacją pamięci przy zbyt dużej ilości punktów. Gdy mikrokontroler wyśle każdemu z trzech silników zadaną pozycję, to następnie oblicza sobie kolejny punkt korzystając z funkcji, która posiada tylko część zmodyfikowanego kodu z funkcji calcTraceLin.
Grafika 6 – wycinek kodu odpowiedzialnego za ruchy liniowe
Ruch Przegubowy
W wielu przypadkach nie ma dla nas znaczenia to po jakiej trajektorii robot poruszy końcówkę roboczą do punktu końcowego, lecz zależy nam na krótkim czasie ruchu. Wtedy wykorzystujemy ruch przegubowy. Dla tego trybu napisaliśmy funkcje do której, tak samo jak w trybie liniowym, podajemy pozycję końcową oraz prędkość kątową najszybszego silnika, czyli silnika, który będzie musiał obrócić się o największy kąt. W przeciwieństwie do ruchu liniowego, nie musimy wykonywać zadania odwrotnego, tylko bezpośrednio odczytujemy aktualne pozycje silników. Następnie wykonujemy zadanie odwrotnej kinematyki dla punktu końcowego. Znajdujemy o jaki kąt musi obrócić się każdy silnik a następnie przechodzimy do punktu końcowego dopasowując prędkości wolniejszych silników tak, aby wszystkie zakończyły swój ruch w jednym momencie. Zaletą tego typu ruchu jest to, że silniki muszą jedynie poruszyć się o mniejszy kąt niż w ruchu liniowym – ruch jest ze stałą prędkością co wydłuża ich żywotność, końcowa trajektoria jest optymalną trajektorią dla zadanego ruchu pod względem czasu. Algorytm obliczania kolejnych punktów dla ruchu przegubowego wygląda mniej więcej tak:
- Na podstawie zadanej pozycji oblicz dla niej kąty silników
- Znając aktualne kąty silników oblicz różnicę o ile silniki mają zmienić swoje położenie, aby efektor robota dojechał do pozycji zadanej.
- Na podstawie zadanego kroku ruchu ( np. 2 stopnie – oznacza to, że co 2ms silnik, który ma pokonać najdłuższą trasę będzie wykonywał ruch o 2 stopnie ) oblicz punkty pośrednie (kąty) dla wszystkich silników, dostosowując prędkość pozostałych silników do silnika, który ma pokonać najdłuższą trasę.
Chodzi po prostu o to, że w trakcie ruchu przegubowego silniki poruszają się ze stałą prędkością – zmieniając kąt położenia wału w sposób liniowy.
Ruch po okręgu
Bardzo ważnym typem ruchu jest ruch po okręgu. Aby robot poruszył się po okręgu musimy rozwiązać układ 4 równań, żeby uzyskać współrzędne środka sfery oraz promień. Mając trzy punkty (początkowy, pośredni i końcowy) możemy wyznaczyć równania sfer ze środkami w tych punktach oraz równanie płaszczyzny. Przecięcie tej płaszczyzny z otrzymaną sferą daje nam zbiór punktów potrzebny do realizacji ruchu. Na skutek tego uzyskujemy przybliżony ruch po okręgu (robot porusza się z punktu do punktu ruchem liniowym, zmniejszanie kąta o jaki obraca się końcówka podczas jednego kroku daje coraz większe przybliżenie).
Sterowanie napędami
Jak już wspomniano w powyższej części artykułu, mikrokontroler wykorzystuje sieć CAN do sterowania serwowzmacniaczami, które z kolei sterują serwosilnikami. Mikrokontroler co 2ms wysyła każdemu z serwowzmacniaczy zadaną pozycję. Ruch silników rozpoczyna się po uzyskaniu dwóch zadanych punktów. Dzięki temu, że serwowzmacniacz zna dwie kolejne pozycje, w których ma znaleźć się za 2 i 4 milisekundy, to oblicza z jaką prędkością powinien poruszać się silnik. Na grafice nr 7 widać jedną z funkcji serwowzmacniacza – ruch interpolowany. Czas t2 to czas co jaki wysyłamy zadane położenia robotowi, natomiast czas t1 jest to okres pętli regulacji w serwowzmacniaczu. Warto też wspomnieć o tym, że mikrokontroler wysyła serwowzmacniaczom kolejne zadane pozycje, a dopiero następnie wysyła ramkę synchronizującą do nich, która aktualizuje zadaną pozycję, co zapewnia synchronizację ruchów. Możliwe, że osoby znające techniki napędowe i sieci przemysłowe wiedzą co mamy na myśli – jest to jednak temat na inny artykuł.
Grafika 7 – Tryb ruchu interpolowanego – funkcja serwowzmacniacza
Poniżej krótkie nagranie prezentujące ruchy ręczne robotem z wykorzystaniem kontrolera steam deck 🙂 Jak widać na nagraniu mamy również wspomnianą aplikację, która wizualizuje robota i pozwala na sterowanie nim. Na pewno napiszemy o niej w jednym z kolejnych artykułów 🙂
A na kolejnym nagraniu ruchy liniowe oraz po okręgu, również z wykorzystaniem steam decka. Nie jest to jednak maksymalna prędkość naszego robota, może on poruszać się dużo szybciej. Niestety większa prędkość ruchu robota powoduje jego trzęsienie, które staramy się niwelować usztywniając jego konstrukcję, nad czym stale pracujemy.
Na koniec artykułu chcielibyśmy zachęcić was do obserwowania na naszym facebooku, gdzie czasami pojawiają się posty w których dzielimy się wiedzą oraz pokazujemy nasze działania. Jeszcze raz dziękujemy firmie Aiut za wsparcie w naszych projektach! 🙂
