Kinematyka odwrotna: analitycznie czy numerycznie

Kinematyka prosta jest nudna. Dane są kąty przegubów, szukamy pozycji końcówki, mnożymy macierze i gotowe. Jedno rozwiązanie, zawsze istnieje, zawsze się liczy tak samo długo.

Kinematyka odwrotna jest odwrotnie pod każdym względem. Dana jest pozycja końcówki, szukamy kątów. Rozwiązań może nie być wcale, może być osiem, a przy robocie redundantnym nieskończenie wiele. Do tego czas liczenia zależy od tego, gdzie akurat jesteśmy.

Pierwsza decyzja, przed którą staje każdy piszący sterowanie, brzmi: liczyć analitycznie czy numerycznie. Odpowiedź jest mniej oczywista, niż sugerują podręczniki.

Rozwiązanie zamknięte, gdy geometria na to pozwala

Dla części manipulatorów da się wyprowadzić wzór. Nie iterację, nie przybliżenie, tylko jawną formułę, która z macierzy pozycji docelowej daje zestaw kątów.

Klucz nazywa się warunkiem Pieppera i mówi, że jeśli trzy ostatnie osie przegubów przecinają się w jednym punkcie, to zadanie sześciu stopni swobody rozpada się na dwa niezależne zadania po trzy. Najpierw wyznaczamy położenie punktu przecięcia nadgarstka, używając trzech pierwszych przegubów. Potem, znając już orientację osiągniętą przez te trzy, dobieramy pozostałe trzy tak, żeby dopełnić obrót do zadanego.

Taką geometrię ma klasyczny Puma560 i większość klasycznych manipulatorów przemysłowych z nadgarstkiem sferycznym. Nie mają jej roboty, w których osie nadgarstka są przesunięte, a takich konstrukcji jest coraz więcej, bo przesunięcie zwiększa zakres ruchu.

Zysk z rozwiązania zamkniętego jest ogromny. Nie ma iteracji, nie ma zbieżności, nie ma pytania o punkt startowy. Czas obliczeń jest stały i przewidywalny, co w pętli sterowania działającej z częstotliwością rzędu kilkuset herców ma znaczenie zupełnie inne niż w skrypcie offline. Jeśli Twój robot spełnia warunek Pieppera, licz analitycznie. Nie ma sytuacji, w której iteracja byłaby tu lepsza.

Osiem rozwiązań i problem, o którym się nie mówi

Rozwiązanie zamknięte dla manipulatora sześcioosiowego z nadgarstkiem sferycznym daje zwykle osiem zestawów kątów, prowadzących końcówkę dokładnie w to samo miejsce i w tę samą orientację. Wynikają z trzech niezależnych wyborów:

• ramię: z lewej albo z prawej strony podstawy,
• łokieć: w górę albo w dół,
• nadgarstek: obrócony albo odwrócony.

Każdy z nich to wybór znaku w jednym miejscu wyprowadzenia. Osiem kombinacji, osiem poprawnych odpowiedzi.

I tu pojawia się problem, który w opracowaniach akademickich zajmuje jedno zdanie, a w praktyce zjada tygodnie: którą wybrać.

Naiwna odpowiedź brzmi: najbliższą aktualnej konfiguracji, w sensie sumy kwadratów różnic kątów. Działa to dobrze aż do momentu, w którym trajektoria przechodzi blisko granicy między obszarami. Wtedy przy jednym punkcie najbliższe jest rozwiązanie z łokciem w górę, a przy następnym, oddalonym o milimetr, z łokciem w dół. Różnica w przestrzeni zadania jest niezauważalna, a robot wykonuje gwałtowny przerzut całego ramienia.

Efekt na hali wygląda jak awaria i bywa niebezpieczny. Zwykle kończy się zderzeniem z osprzętem albo zadziałaniem ochrony nadprądowej.

Rozwiązanie polega na tym, żeby nie wybierać osobno dla każdego punktu. Gałąź wybiera się raz, na początku segmentu ruchu, i trzyma się jej konsekwentnie, nawet jeżeli w środku przestanie być najbliższa. Zmiana gałęzi jest wtedy świadomą decyzją, wykonywaną w zaplanowanym miejscu i z zatrzymaniem, a nie skutkiem ubocznym zaokrąglenia.

Osobny przypadek to osobliwość nadgarstka, w której kąt piątego przegubu zbliża się do zera albo do kąta półpełnego. Osie czwartego i szóstego przegubu pokrywają się wtedy, gałęzie nadgarstka schodzą się i zostaje jedno rozwiązanie zamiast dwóch. W implementacji, którą utrzymuję, solver zwraca w takim przypadku jedno rozwiązanie na parę ramię i łokieć, z kątem czwartego przegubu ustawionym na zero. Można to zrobić inaczej, ale nie można tego pominąć, bo naiwne wyprowadzenie dzieli tam przez zero.

Kiedy wzoru nie ma

Przy geometrii, która nie spełnia warunku Pieppera, zostaje iteracja. Zadanie formułuje się wtedy tak: mamy błąd między obecną a docelową pozycją końcówki i szukamy takiej poprawki kątów, która ten błąd zmniejszy.

Narzędziem jest jakobian, macierz wiążąca prędkości przegubów z prędkością końcówki. Metody różnią się tym, jak z niego korzystają.

Transpozycja. Najprostsza możliwa reguła:

Δq = α · Jᵀ · e

Nie wymaga odwracania niczego, więc nigdy nie wybuchnie. Zbiega jednak wolno i wymaga dobrania współczynnika kroku. Dobra na start, do zrozumienia mechanizmu, rzadko dobra do produkcji.

Pseudoodwrotność. Poprawka jest rozwiązaniem najmniejszych kwadratów:

Δq = J⁺ · e

Zbiega znacznie szybciej. Ma jedną poważną wadę, która ujawnia się dokładnie tam, gdzie boli najbardziej.

Osobliwości, czyli dlaczego potrzebne jest tłumienie

W pobliżu osobliwości jakobian traci rząd. Oznacza to, że istnieje kierunek w przestrzeni zadania, w którym robot nie potrafi się poruszyć, choćby nie wiem jak kręcił przegubami.

Matematycznie problem polega na tym, że pseudoodwrotność zawiera dzielenie przez wartości szczególne macierzy, a te dążą wtedy do zera. Poprawka kątów rośnie nieograniczenie. W symulacji widać eksplozję liczb, na sprzęcie widać ramię wyrzucone z pełną prędkością.

Naprawa nazywa się tłumieniem i sprowadza się do dołożenia małej wartości do mianownika:

Δq = Jᵀ (J Jᵀ + λ² I)⁻¹ · e

Współczynnik λ kupuje stabilność za cenę dokładności. Przy λ równym zeru wracamy do czystej pseudoodwrotności ze wszystkimi jej problemami. Przy λ zbyt dużym solver jest bezpieczny, ale nie dojeżdża do celu.

Praktyka podpowiada, żeby nie trzymać λ na stałe, tylko zwiększać je wtedy, gdy zbliżamy się do osobliwości. Do wykrycia tego zbliżenia służy miara manipulowalności, wprowadzona przez Yoshikawę, będąca pierwiastkiem z wyznacznika iloczynu jakobianu przez jego transpozycję. Spada do zera dokładnie w osobliwości, więc nadaje się na sygnał ostrzegawczy.

Wariantem idącym dalej jest tłumienie dobierane osobno dla każdej składowej rozkładu według wartości szczególnych. Tłumi tylko te kierunki, które faktycznie są problematyczne, i zostawia pozostałe nietknięte. Kosztuje więcej obliczeń, ale zachowuje dokładność tam, gdzie da się ją zachować.

Optymalizacja, gdy zadanie ma więcej niż jedno kryterium

Trzecia droga to potraktowanie kinematyki odwrotnej jako zadania minimalizacji. Budujemy funkcję kosztu, w której błąd pozycji jest jednym ze składników, i szukamy jej minimum.

Sens pojawia się wtedy, gdy dochodzą dodatkowe wymagania: trzymaj się z dala od granic zakresu, unikaj przeszkody, zachowaj preferowaną postawę, minimalizuj zmianę względem poprzedniej konfiguracji. Każde z nich wchodzi do funkcji kosztu jako składnik z własną wagą.

Metody bezgradientowe, na przykład sympleks Neldera i Meada, są odporne na nieciągłości funkcji kosztu i nie wymagają liczenia pochodnych. Płacą za to prędkością. Metody gradientowe są szybsze, ale wymagają, żeby funkcja kosztu dała się różniczkować, co przy karach za kolizje bywa kłopotliwe.

Wspólna wada obu jest taka sama: czas obliczeń przestaje być przewidywalny. Do pętli sterowania o twardym rygorze czasowym to dyskwalifikuje, do planowania offline jest bez znaczenia.

Jak wybrać

Robot ma nadgarstek sferyczny, sterujesz w czasie rzeczywistym. Rozwiązanie zamknięte plus konsekwentna polityka wyboru gałęzi. Nie kombinuj.

Geometria nietypowa, sterowanie w czasie rzeczywistym. Jakobian z tłumieniem, ze współczynnikiem zależnym od manipulowalności. Punktem startowym iteracji jest poprzednia konfiguracja, co daje jednocześnie szybkość i ciągłość.

Robot redundantny, więcej niż sześć osi. Rozwiązania zamkniętego nie ma z definicji, bo rozwiązań jest nieskończenie wiele. Zostaje jakobian z zadaniem dodatkowym w przestrzeni zerowej albo optymalizacja.

Planowanie offline, wiele kryteriów naraz. Optymalizacja. Nieprzewidywalny czas nie przeszkadza, a możliwość dołożenia dowolnego składnika do funkcji kosztu jest tu największą zaletą.

Trzy rzeczy, które warto sprawdzić w cudzym kodzie

Kiedy przejmuję po kimś sterowanie manipulatora, zaglądam najpierw w te same trzy miejsca.

Czy solver ma limit iteracji i co robi po jego przekroczeniu. Zaskakująco często odpowiedź brzmi: zwraca ostatnią konfigurację bez żadnej informacji, że nie zbiegł. Robot jedzie wtedy w miejsce, którego nikt nie zaplanował.

Czy tolerancja pozycji i orientacji są rozdzielone. Jedna wspólna liczba dla milimetrów i radianów nie ma sensu wymiarowego, a mimo to bywa spotykana.

Czy wybór gałęzi jest stabilny wzdłuż trajektorii, czy liczony od nowa w każdym punkcie. To jest źródło większości gwałtownych ruchów, które ludzie tłumaczą sobie awarią sprzętu.