Zabawy z kinematyką odwrotną

Zagadnieniem kinematyki odwrotnej robotycy zajmują się od początku (zrobotyzowanego) świata.

Podejścia są z dwa - nazywane metodą geometryczną i numeryczną.

Metoda geometryczna bazuje na wiedzy o ograniczeniach geometrycznych układu, metoda numeryczna bazuje na funkcjach opisujących położenie i pochodną położenia końcówki, traktując zagadnienie metodami numerycznymi.

Współcześnie oba podejścia są stosowane, przy czym potęgę podejścia geometrycznego widać szczególnie w przypadku zamkniętych łańcuchów kinematycznych a podejścia numerycznego w przypadku kinematycznych “drzew”.

Do niedawna można było znaleźć opinie, że metody numeryczne są zbyt wolne, ale obecnie nie jest to prawda – kinematykę robota o sześciu stopniach swobody można obliczać online na komputerze o sile smartfona a zespoły np. Russa Tedrake’a z MIT, zajmującego się robotami kroczącymi w mniej niż milisekundowej iteracji są w stanie nie tylko obliczyć kinematykę odwrotną ale jeszcze sporo rzeczy zoptymalizować.

Zobaczmy zabawkowy przykład numerycznego rozwiązania kinematyki odwrotnej w MATLABIE - dla robota planarnego o 2 stopniach swobody.

Rozwiązanie zaczyna się od zdefiniowania funkcji przyjmujących wartości złączowe theta i generujących wektor położenia i Jakobian.

% przyjmuje ze l1,l2=1
l1=1;
l2=1;
x_obl=@(th)[ l1*cos(th(1))+l2*cos(th(1)+th(2));l1*sin(th(1))+l2*sin(th(1)+th(2))]

jaco=@(th)[ - l2*sin(th(1) + th(2)) - l1*sin(th(1)), -l2*sin(th(1) + th(2));l2*cos(th(1) + th(2)) + l1*cos(th(1)),  l2*cos(th(1) + th(2))]

Załóżmy, że chcemy obliczyć thety, dla których położenie końcówki to x=0, y=sqrt(2)

%%
q=[pi/4;pi/4] %poczatkowa wartosc q
x=[0;sqrt(2)] %polozenie koncowki dla ktorego chcemy znac wartosci zlaczowe

Samo numeryczne obliczenie kinematyki odwrotnej sprowadza się do iteracyjnego prowadzenia poniższych obliczeń

%% numeryczne obliczenie kinematyki odwrotnej
e=x-x_obl(q) %porownanie prawdziwego polozenia koncowki i tego obliczonego dla q
q=q+jaco(q)^(-1)*e %nastepne przyblizenie q

W przedstawionym przypadku wystarczy 7 iteracji, żeby błąd zszedł do wartości poniżej 10^(-15).

Kruczki

Najważniejszą słabością tej (i większości innych) metody numerycznej jest jej zależność od początkowej wartości q.

Dla przykładu, jeśli zamiast q=[pi/4,pi/4] damy początkowe q=[3*pi/4,pi/4] to kinematyka odwrotna wyliczy nam q=[2.3562, -1.5708] czyli drugie rozwiązanie tego problemu.

Jeśli zaś za drugą zmienną damy zero q=[pi/4,0] to wcale nie otrzymamy rozwiązania. Nie można bowiem policzyć odwrotności Jakobianu - trafiliśmy na jego osobliwość det(J)=0.

Ten sam program w pythonie

Do działania oprócz pythona na komputerze musi być numpy


import numpy as np
from numpy.linalg import inv
from math import sin,cos,pi
l1=l2=1
def x_obl(th):
  x=np.array([[ l1*cos(th[0])+l2*cos(th[0]+th[1])],  [l1*sin(th[0])+l2*sin(th[0]+th[1])]])
  return x

def jaco(th):
  J=np.array([[ - l2*sin(th[0] + th[1]) - l1*sin(th[0]), -l2*sin(th[0] + th[1])],
  [l2*cos(th[0] + th[1]) + l1*cos(th[0]),  l2*cos(th[0] + th[1])]])
  return J


#zdefiniowanie wartości początkowych i położenia końcówki
q=np.array([[pi/4,pi/4]]).T
x=np.array([[0,2**0.5]]).T

#rozwiazanie az do malego bledu
err=x-x_obl(q)
while err.all()>1e-15: #licz dopoki kazdy element wektora bledu bedzie mniejszy niz 10^-15
  err=x-x_obl(q)
  q=q+np.dot(inv(jaco(q)),err)
print "obliczone q",q #pokaz wynik
print "obliczone x",x_obl(q)