W przypadku 6-osiowego robota, przy danej pozycji efektora końcowego i zakresie orientacji, jak znaleźć optymalne wartości połączeń

Biorąc pod uwagę sześcioosiowe przegubowe ramię robota utrzymujące narzędzie w jego efektorze końcowym, jeśli mam pożądaną pozycję narzędzia i orientację narzędzia, będzie dokładnie 1 rozwiązanie odwrotnego równania kinematyki dla robota, aby osiągnąć tę pozycję.
(a raczej do 16 różnych rozwiązań, w zależności od zakresu połączeń)

Ale jeśli robot trzyma coś w rodzaju długopisu i chcę, aby robot zaznaczył określony punkt tym piórem na celu, to nie dbam o to, jak pióro jest zorientowane, o ile jest ono prostopadłe do zaznaczonej powierzchni.

Zatem równanie kinematyki odwrotnej będzie miało nieskończenie wiele rozwiązań.

Jak wybrać spośród tych rozwiązań konfigurację połączenia, która jest najbliższa obecnej konfiguracji: taka, która będzie wymagała najmniejszego ruchu do osiągnięcia?
(lub konfiguracja złącza, która jest optymalna zgodnie z innym podobnym kryterium, takim jak że wszystkie kąty złącza są najdalsze od ich maksimum i minimum?)

Najpierw musimy zdefiniować optymalne . Ponieważ nie mówisz, co uważasz za optymalne, większość ludzi wybiera wyrażenie kwadratowe. Załóżmy na przykład, że twoje obecne kąty połączenia są podane przez wektor$\vec{\alpha}$. Możemy rozważyć zminimalizowanie wymaganego ruchu - z błędem$\vec{x} = \vec{\alpha} - \vec{\alpha}_{start}$, możesz zdefiniować funkcję kosztu $J=\vec{x}^TQ\vec{x}$ dla jakiejś matrycy $Q$. Zwykle używamy macierzy diagonalnej, ale zrobi to każda macierz z dodatnim wynikiem.

W uproszczonym przykładzie z dwoma kątami połączenia, jeśli połączenie $a$ mieliśmy tańszy silnik (być może bliższy efektora końcowego), moglibyśmy mieć funkcję kosztu

$J=\left[\begin{matrix}x_a\\x_b\end{matrix} \right]\left[\begin{matrix}1&0\\0&2\\\end{matrix} \right]\left[\begin{matrix}x_a&x_b\end{matrix} \right]$, tj. ruch stawu$b$ jest dwa razy droższy niż połączenie $a$.

Teraz równanie kinematyczne jest formułą macierzową, aw notacji Denavit-Hartenberg może być:

$\prod{T_i}=\left[\begin{matrix}1&0&0&x\\0&1&0&y\\0&0&1&z\\0&0&0&1\end{matrix} \right]$, gdzie prawa strona reprezentuje pozycję $(x,y,z)$ i orientacja (obecnie ustawiona jako obrót zerowy), biorąc pod uwagę kąty połączenia.

Ponieważ nie dbamy o orientację, a jedynie o pozycję, możemy obciąć pierwsze 3 kolumny ostatniej macierzy transformacji i ostatni wiersz pierwszej macierzy transformacji. Możemy równoznacznie wyrazić tę formułę jako:

$\left[\begin{matrix}1&0&0&0\\0&1&0&0\\0&0&1&0\end{matrix}\right]\prod{T_i}\left[\begin{matrix}0\\0\\0\\1\end{matrix}\right]=\left[\begin{matrix}x\\y\\z\end{matrix} \right]$

Pomnożąc lewą stronę, otrzymujemy trzy równania. Gdyby parametry były liniowe, rozwiązanie byłoby proste. Dzieje się tak, gdy wszystkie siłowniki są siłownikami liniowymi. W tym przypadku problemem jest program kwadratowy . Możemy ponownie ustawić lewą stronę, aby uzyskać równanie:

$K\vec{x}=\left[\begin{matrix}x\\y\\z\end{matrix}\right]$, dla niektórych matryc $K$.

Program kwadratowy to problem, który można wyrazić w postaci:

Zminimalizować $J=\frac{1}{2}\vec{x}^TQ\vec{x}+\vec{c}^T\vec{x}$

Z zastrzeżeniem $A\vec{x}\leq\vec{b}$, $E\vec{x}=\vec{d}$

Aby rozwiązać ten problem, istnieje wiele algorytmów, których można użyć, na przykład punkt wewnętrzny, zestaw aktywny, ... Po prostu znajdź odpowiednią bibliotekę, a ona ją rozwiąże.

Nieliniowy układ równań jest trudniejszy do rozwiązania. Nazywa się to programowaniem nieliniowym , ale tak właśnie jest, jeśli masz obrotowe złącza.

Zasadniczo zamiast równań macierzowych masz funkcje nieliniowe.

Zminimalizować $f(x)$ z zastrzeżeniem $\vec{h}(x) = 0$, $\vec{g}(x) \leq 0$ (w razie potrzeby zmień ustawienie, aby wartość RHS ograniczeń była równa zero)

Algorytmy zastosowane do rozwiązania tego problemu są jeszcze bardziej złożone, ale obejmują punkty wewnętrzne, sekwencyjne programowanie kwadratowe (SQP), aktywne zestawy, algorytmy refleksyjne regionu zaufania. Oczywiście wyjaśnienie, w jaki sposób działają te algorytmy, jest bardzo długie i nie będę uwzględniał zakresu tej odpowiedzi. Wystarczy powiedzieć, że ilość treści w algorytmach wykorzystywanych do programowania kwadratowego mogłaby sama w sobie stanowić cały kurs.

Powinieneś po prostu znaleźć bibliotekę, aby rozwiązać problem, zajęłoby dużo czasu kodowanie wydajnej implementacji, a wydajne implementacje mogą obsługiwać 100 (lub więcej) zmiennych jednocześnie. Na przykład, jeśli używasz MATLAB, istnieje dokumentacja na temat korzystania z funkcji fmincon z Optymalizatora.

Aby rozwiązać to online, możesz chcieć C ++ lub innej implementacji natywnej, na przykład NLopt. Zauważ, że może to nie być coś, co mikrokontroler może szybko rozwiązać, a wiele bibliotek może mieć inne zależności, które nie są łatwe w użyciu na mikrokontrolerze (ponieważ są przeznaczone dla komputera).

---

Jeśli nie martwisz się wydajnością i po prostu chcesz czegoś, co możesz samemu zakodować, zakładając, że istnieje funkcja, którą możesz wywołać w celu rozwiązania odwrotnego problemu kinematycznego , możesz po prostu wykonać metodę zejścia gradientowego. Na przykład arbitralnie wybierając losową orientację początkową, rozwiąż odwrotny problem, a następnie sprawdź funkcję kosztu. Następnie możesz użyć analizy zakłóceń, aby sprawdzić, jak zmienić orientację. Na przykład, jeśli sprawdzisz podobne orientacje wokół bieżącej orientacji (tj. 8 punktów na sześciennej siatce), możesz uzyskać przybliżenie drugiego rzędu, w jaki sposób funkcja kosztu zmienia się w każdym kierunku.

Korzystając z aproksymacji drugiego rzędu (znanej jako macierz Hesji, ponieważ jest ona wielowymiarowa - trójwymiarowa dla orientacji), można znaleźć przejście przez zero gradientu funkcji kosztu (tj. Przewidywanych minimów lokalnych).

W nowej przewidywanej orientacji po prostu ponownie włóż ją przez odwrotny solver i powtarzaj, aż dokładność będzie wystarczająca.

Zauważ, że prawdopodobnie nie będzie to tak wydajne, ponieważ sam odwrotny problem kinematyczny musi być iteracyjnie rozwiązany (więc wielokrotnie używasz funkcji, której rozwiązanie zajmuje trochę czasu). Ponadto kod może być mniejszy niż pełnoprawny algorytm optymalizacji, ale nadal jest dość znaczny i nie jest niewielką inwestycją czasu.

---

Stosując którąkolwiek z metod (formalnie rozwiązując jako program nieliniowy lub stosując iteracyjnie funkcję, aby rozwiązać odwrotny problem), rozwiązanie może nie być optymalne, jeśli istnieje wiele lokalnych minimów. W takim przypadku możesz spróbować znaleźć globalne minima przy użyciu różnych metod. Nawet w przypadku nieliniowego solvera programującego oczekuje się, że zostanie on zapoczątkowany wartościami początkowymi (np. Kątami połączenia). Możesz wielokrotnie uruchamiać dowolną metodę z ziarnem generowanym na różne sposoby:

• losowy restart (jest generowany losowo)
• oparty na siatce

lub inne niestandardowe metody.

Pamiętaj jednak, że jeśli istnieje wiele minimów, nie ma dobrego sposobu, aby zagwarantować, że znajdziesz minima globalne. Możesz tylko zwiększyć swoje szanse.

Ponieważ pytanie dotyczy robota przemysłowego, prawdopodobnie nie mamy modelu dynamiki robota, więc zakładam, że szukamy rozwiązań, które zoptymalizują tylko kryterium kinematyczne.

Robot ma rozwiązanie w formie zamkniętej dla swojej odwrotnej kinematyki, ale na szczęście efektor końcowy ma dodatkowy stopień swobody obrotu, co oznacza, że ​​robot ma zasadniczo 7 stopni swobody. Ale ponieważ jest to tylko jedno dof , nie jest to tak duży problem, jak mogłoby się wydawać.

Powszechną sztuczką w przypadku prawie nie redundantnych robotów jest zablokowanie dodatkowego stopnia swobody i analityczne rozwiązanie pozostałych wartości połączeń. Załóżmy, że piszemy solver IK w zamkniętej formie dla robota 6 dof , który zwraca rozwiązania$0.05$ms średnio. Poprzez iterację od$1$ do $360$ dlatego możesz znaleźć optymalne rozwiązanie dla dyskretyzacji kąta pióra wynoszącego $1^\circ$ w około $18$ ms, co biorąc pod uwagę aplikację jest prawdopodobnie więcej niż wystarczające.

Jeśli przez większość czasu pióro porusza się tylko nieznacznie (np. Podczas rysowania linii), inną sztuczką w celu przyspieszenia wyszukiwania jest użycie numerycznego IK, na przykład metoda pseudoinwersyjna:

Pozwolić $q_1$ niech będzie obecna konfiguracja robota, niech $J$ niech będzie jakobianem i niech $\Delta x$być przemieszczeniem celu w stosunku do bieżącej transformacji efektora końcowego. Rozwiązać$\Delta x = J \, \Delta q$ dla $\Delta q$ i obliczyć nową konfigurację $q_2 = q_1 + \Delta q$. Pomijam tutaj szczegóły, ale rozwiązanie$\Delta q$ powinien zminimalizować $\|\Delta q\|$ dla właściwie wybranego wskaźnika.

Odbywa się to dla robota 7 dof i znów powinno zająć tylko ułamki milisekundy. Mimo że$q_2$ może nie być prawidłową konfiguracją (łączone wartości mogą być poza zakresem) i może nie być dokładnym rozwiązaniem IK (możesz jednak wykonać więcej pseudo-odwrotnych kroków), przez większość czasu będzie to dobry punkt wyjścia do wyszukiwania za pomocą solver w formie zamkniętej.

Jest w tym ładna, zamknięta forma. Powiedzmy, że nie obchodzi nas co$r_z$jest (tzn. nie obchodzi nas, jak to zmieniamy ).

$$\mathbf{J}^{-1}\dot{\mathbf{X}}= \dot{\mathbf{\Theta}}= \begin{bmatrix} \vec{j_{1}} & \vec{j_{2}} & \vec{j_{3}} & \vec{j_{4}} & \vec{j_{5}} & \vec{j_{6}} \end{bmatrix} \begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{z}\\ \dot{r_x}\\ \dot{r_y}\\ \dot{r_z} \end{bmatrix} = \begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2}\\ \dot{\theta_3}\\ \dot{\theta_4}\\ \dot{\theta_5}\\ \dot{\theta_6}\\ \end{bmatrix}$$ Gdzie $\vec{j_{i}}$ jest $i^{th}$ kolumna z $\mathbf{J}^{-1}$. Możemy się zerwać$\dot{\mathbf{\Theta}}$ do części, która zależy od $\dot{r_z}$ i część, która nie. $$\dot{\mathbf{\Theta}}= \dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}} \\ \dot{\mathbf{\Theta}}_{\dot{r_z}}= \vec{j_{6}} \dot{r_z}$$ Więc teraz gra się stała, zminimalizujmy $$(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})^T D(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})$$ dla niektórych matryc diagonalnych $D$jak ronalchn powiedział wyżej. Zamierzam użyć$A=\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}}$ i $B=\dot{\mathbf{\Theta}}_{\dot{r_z}^*}$ dla łatwiejszego przeglądania.

Możemy to rozwinąć do

$$A^TDA+2B^TDA+B^TDB\\ \text{or}\\ A^TDA+2\dot{r_z}\vec{j_{6}}^TDA+\dot{r_z}^2\vec{j_{6}}^TD\vec{j_{6}}$$

Teraz mamy łatwe równanie do rozróżnienia $\dot{r_z}$. Znajdujemy pochodną w odniesieniu do$\dot{r_z}$ i ustaw na $0$.

$$2\vec{j_{6}}^TDA+2\dot{r_z}\vec{j_{6}}^TD\vec{j_{6}}=0\\ \dot{r_z} = \frac{-\vec{j_6}^TDA}{\vec{j_6}^TD\vec{j_6}}$$ Minimalizuje to „odległość” kąta połączenia między dwoma pozycjami.