Odwrotne zadanie kinematyki

Odwrotne zadanie kinematyki jest zagadnieniem niełatwym, a jednocześnie tak niezbędnym w przemyśle, że trudno byłoby sobie wyobrazić proces produkcyjny bez jego zastosowania. W tej notce skupię się na przypadku w miarę prostym: robot o ramieniu składającym się z dwóch części (jeden przegub), poruszający się w dwóch wymiarach.

Podstawy

Przede wszystkim należy wyjaśnić co to tak naprawdę jest odwrotne zadanie kinematyki. Otóż jest to wyznaczenie zbioru położeń (kątów i przemieszczeń) manipulatora tak, aby końcówka mogła osiągnąć zadane położenie i orientację. Bardziej po polsku: wyznaczenie ruchu ramion robota (przez ich orientację kątową i przemieszczenia) tak, aby przesunąć trzymany przedmiot (np. szklankę z wodą) w żądane miejsce i określoną orientację (czyli tak, aby nie wylać wody). Jest to zadanie trudniejsze niż tzw. proste zadanie kinematyki, w którym na podstawie danych kątów i przemieszczeń ramion robota należy wyznaczyć położenie i orientację końcówki manipulatora.

Robot ćwiczebny

Poniżej rysunek przedstawiający wirtualnego robota, którego ruch będziemy programować:

Czerwone linie to elementy ramienia robota, zielonymi okręgami (dla wygody użytkownika) zaznaczono zasięg poszczególnych części ramienia. Ramię robota może się więc poruszać tylko w obrębie większego okręgu. Każde poprawne położenie ramienia ma jedno rozwiązanie (jeżeli końcówka leży na zewnętrznym okręgu) lub dwa rozwiązania. Położenie/położenia końcówki pierwszej części ramienia (wychodzącej ze środka układu współrzędnych) można obliczyć znajdując punkty przecięcia dwóch okręgów (zaznaczone poniżej na zielono i brązowo).

Przestrzeń konfiguracji

Przechowywanie położenia robota za pomocą współrzędnych końca pierwszej części ramienia oraz współrzędnych końcówki manipulatora byłoby nadmiarowe: potrzebowalibyśmy na to 4 współrzędnych (po dwie na każdy punkt). Należy też zauważyć, że nie ma tu orientacji końcówki manipulatora, którą najłatwiej zapisać w postaci kwaternionu. Rozwiązanie jest proste, przechodzimy na współrzędne biegunowe i przypisujemy każdej części ramienia kąt, o który jest obrócona w stosunku do wektora [1, 0]. Zakładając, że możemy efektywnie obliczyć położenie części ramienia w przestrzeni przy kącie zmieniającym się o 1 stopień, powstaje dwuwymiarowa przestrzeń konfiguracji robota. Każdy z wymiarów symbolizuje kąt jednej z części ramienia. Punkt w takiej przestrzeni definiuje konkretne wartości kątów, o które są obrócone kolejne części ramienia, a więc definiuje też położenie końcówki manipulatora. Po co to wszystko? Zauważmy, że z początkowej idei przestrzeni czterowymiarowej (po dwie współrzędne na każdy z punktów położenia końcówki części ramienia) zeszliśmy do dwóch wymiarów (jeden kąt na jedną część ramienia), w których liczy się zdecydowanie wygodniej.

W aplikacji przestrzeń konfiguracji wizualizowana jest na oddzielnej formie w postaci kwadratu 360 x 360. Na początku wszystko wypełnione jest kolorem zielonym, ponieważ wszystkie konfiguracje są dozwolone. Na czerwono będą zmieniać się punkty, które przekładają się na zabronione położenia ramienia. Do gry wchodzą przeszkody.

Przeszkody

Przeszkody wprowadzamy na scenę, żeby urealnić sytuację, a także urozmaicić algorytm. Przeszkoda jest zdefiniowana jako prostokąt, przeszkody mogą na siebie nachodzić, mogą być przesuwane i modyfikowane pod względem rozmiaru. Przykład sceny z umieszczonymi w niej przeszkodami poniżej (pozycja początkowa ramienia kolorem czerwonym, pozycja końcowa – niebieskim):

Po dodaniu przeszkód, przestrzeń konfiguracji robota przestaje być jednolicie zielona, pewne pozycje ramion są zabronione – następuje kolizja ramienia z przeszkodą. Przeliczenie potencjalnych kolizji oraz wizualizacja przestrzeni konfiguracji daje następujący rezultat:

Oprócz dwóch dominujących kolorów: czerwonego i zielonego widoczne są dwa jasne punkty: biały to pozycja początkowa ramienia w przestrzeni konfiguracji, żółty punkt – pozycja końcowa.

Wyszukiwanie drogi

Głównym zadaniem od początku projektu jest przemieszczenie robota z punktu startowego do końcowego. Przestrzeń konfiguracji jest do tego zadania idealna, ponieważ przejście między dwoma sąsiednimi punktami w przestrzeni konfiguracji oznacza obrócenie ramienia robota o 1 stopień, a ponieważ jest to przestrzeń niewielka (przypominam, 360 x 360 punktów) można zastosować prosty algorytm oparty o nierekursywny algorytm „Flood Fill”.

Droga w przestrzeni konfiguracji:

Złożenie wszystkiego w całość daje następujące efekty:

i ostatni przykład, według mnie najciekawszy – droga jest nieco poplątana przez przeszkody, ale widać efekty:

Be Sociable, Share!
czoper opublikowano dnia 2010-5-4 Kategoria: Programowanie, PUSN | Tagi:, , ,

Zostaw odpowiedź

(Ctrl + Enter)