Zadanie odwrotne kinematyki - przegląd metod
Aleksandra Gumieniak
12 listopada 2018
1 Wstęp
W kinematyce prostej pokazano jak określać pozycję i orientację końcówki ro- boczej w zależności od zmiennych przegubowych. Natomiast kinematyka od- wrotna polega na znalezieniu zmiennych przegubowych w zależności od pozycji i orientacji końcówki roboczej. W ogólnym przypadku jest ono trudniejsze niż zadanie kinematyki prostej ponieważ czasami nie istnieje jednoznaczne rozwią- zanie wynikające z nieliniowości równań kinematyki.
2 Zadanie odwrotne kinematyki
Zadanie odwrotne kinematyki polega na określeniu zestawu wartości współ- rzędnych konfiguracyjnych na podstawie zadanej pozycji i orientacji układu wsp´łrzędnych końcówki roboczej względem układu podstawowego.
T60→ q
Zadanie odwrotne kinematyki może mieć kilka rozwiązań w przestrzeni zmien- nych konfiguracyjnych (np. cztery).
Istnieją także wyjątkowe przypadki, które mają miejsce w tzw. punktach oso- bliwych odwzorowania kinematyki.
Osobliwości
1. Osobliwości reprezentują konfiguracje, których pewne kierunki mogą być nieosiągalne.
2. W osobliwościach ograniczone prędkości chwytaka mogą odpowiedać nie- ograniczonym prędkościom przegubów.
3. W osobliwościach ograniczone siły i momenty na chwytaku mogą odpo- wiadać nieograniczonym momentom w przegubach.
4. Zwykle (ale nie zawsze) osobliwości odpowiadają punktom na brzegu prze- strzeni roboczej manipulatora, tzn. punktom jego maksymalnego zasięgu.
5. Osobliwości odpowiadają punktom w przestrzeni roboczej manipulatora, które mogą być nieosiągalne przy małych niedokładnościach parametrów członu manipulatora, takich jak długość, przesunięcie poprzeczne itp.
6. W pobliżu osobliwości nie isnieje jednoznaczne rozwiązanie zadania kine- matyki odwrotnej. W takich przypadkach może nie być rozwiązania lub też może istnieć ich nieskończenie wiele.
3 Metody kinematyki odwrotnej
Strategie rozwiązań:
a) rozwiązania w postaci jawnej:
- metoda geometryczna,
- metody algebraiczne (przez podstawianie, przez redukcję do wielomianu), b) rozwiązania numeryczne:
- z wykorzystaniem jakobianu,
- algorytmu iteracyjnego rozwiązywania zadania odwrotnego kinematyki:
• Metoda Newtona-Raphsona,
4 Rozwiązania w postaci jawnej
4.1 Metoda algebraiczna
4.1.1 Przez podstawienie
Rysunek 1: Trójczłonowy manipulator płaski αi-1 ai-1 di θi-1
1 0 0 0 θ1
2 0 l1 0 θ2
3 0 l2 0 θ3
Tablica 1: Parametry D-H Gdzie:
ai-1 - odległość od osi ˆZi do ˆZi+1 mierzona wzdłuż osi ˆXi
αi - kąt między osiami ˆZi i ˆZi+1 mierzona wokół osi ˆXi
di - odległość od osi ˆXi−1do ˆXimierzona wzdłuż osi ˆZi
θi - kąt między osiami ˆXi−1i ˆXi mierzona wokół osi ˆZi
B
WT =03T =
c123 −s123 0 l1c1+ l2c12
s123 c123 0 l1s1+ l2s12
0 0 1 0
0 0 0 1
(1)
Ponieważ zajmujemy się manipulatorem płaskim, punkty docelowe mogą być łatwo wyszczególnione przez podanie trzech liczb: x,y,φ, gdzie φ jest orientacją członu na 3 na płaszczyźnie (względem osi ˆX). Zatem zamiast podawać dowolne
B
WT jako specyfikację celu, przyjmujemy przekształcenie o strukturze:
B WT =
cφ −sφ 0 x sφ cφ 0 y
0 0 1 0
0 0 0 1
(2)
Przyrównując (1) i (2) dochodzimy do układu czterech nieliniowych równań, które trzeba rozwiązać względem θ1, θ2, θ3:
cφ= c123 (3)
sφ = s123 (4)
x = l1c1+ l2c12 (5)
y = l1s1+ l2s12 (6)
Jeśli podniesiemy do kwadratu równania (5) i (6) i dodamy je stronami, otrzy- mamy:
x2+ y2= l12+ l22+ 2l1l2c2 (7) gdzie wykorzystujemy:
c12= c1c2− s1S2 (8)
s12= c1s2+ s1c2 (9)
Rozwiązując (7) względem c2 otrzymujemy:
c2= (x2+ y2− l21− l22)
2l1l2 (10)
Ponieważ cosinus przyjmuje wartości od -1 do 1 prawa strona musi się zawierać w tym zakresie. Przyjmując, że cel znajduje się w przestrzeni roboczej napi- szemy wyrażenie na s2 jako:
s2= ± q
(1 − c22) (11)
Wybór znakó we wzorze (11) odpowiada wielokrotności rozwiązań, z których możemy wybrać konfigurację ”łokieć u góryalbo ”łokieć u dołu”.,
Ostatecznie obliczamy θ2
θ2= Atan2(s2, c2) (12)
Mając znalezione θ2 możemy rozwiązać równania (5) i (6) względem θ1. Zapi- sujemy (5) i (6) w postaci:
x = k1c1− k2s1 (13)
y = k1s1− k2c1 (14)
gdzie
k1= l1+ l2c2 (15)
k2= l2s2 (16)
W celu rozwiązania równania tej postaci dokonujemy zamiany zmiennych. Obec- nie zmieniamy sposób, w jaki zapisujemy stałe k1 i k2.
Jeśli r = +pk12+ k22 i γ = Atan2(k1, k2) to k1= rcosγ, k2= rsinγ
Równania (13) i (14) mogą być teraz zapisane w postaci:
x
r = cosγcosθ1− sinγsinθ1 (17)
y
r = cosγsinθ1− sinγcosθ1 (18)
lub
cos(γ + θ1) =x
r (19)
sin(γ + θ1) = y
r (20)
Stosując dwuargumentowy arctg dostajemy:
γ + θ1= Atan2(y r,x
r) = Atan2(y, x) (21)
i także
θ1= Atan2(y, x) − Atan2(k2, k1) (22) Ostatecznie możemy znaleźć sumę kątów θ1 do θ3
θ1+ θ2+ θ3= Atan2(sφ, cφ) = φ (23) z której możemy wyznaczyć θ3 ponieważ znamy pozostałe dwa kąty.
4.1.2 Przez redukcję do wielomianu
W celu uproszczenia zapisu np. zmiennej θ, która na ogół występuje jako sinθ i cosθ, możemy zastosować podstawienie zmiennej u:
u = tgθ
2 (24)
cosθ = 1 − u2
1 + u2 (25)
sinθ = 2u
1 + u2 (26)
stosując te podstawienia równaniaprzestępne przekształca się w rówania wielo- mianowe względem u.
Przykład:
Zadanie: Przekształcić równanie przestępne w wielomian z tangensem kąta po- łówkowego i rozwiązać względem θ.
acosθ + bsinθ = c (27)
Podstawiając z (24),(25),(26) i mnożąc przez 1 + u2 otrzymamy:
a(1 − u2) + 2bu = c(1 + u2) (28) Ułożenie względem potęg zmiennej u daje równanie kwadratowe:
(a + c)u2− 2bu + (c − a) = 0 (29) które rozwiązujemy stosując wzory na pierwiastki:
u = b ±√
b2− a2− c2
a + c (30)
Stąd:
θ = 2arctg(b ±√
b2− a2− c2
a + c ) (31)
Jeżeli rozwiązanie na u wz wzoru (30) jest zespolone, to nie ma rzeczywistego rozwiązania wyjściowego równania przestępnego. Jeśli a + c = 0, to wartość argumentu funkcji arctg zmierza do nieskończoności, dlatego θ = π rad.
4.2 Metoda geometryczna
Metoda geometryczna zadania kinematyki odwrotnej polega glównie na zasto- sowaniu twierdzenia kosinusów do wyznaczenia kąta θ2.
Rysunek 2: Płaskie zależności geometryczne związane z trójczłonowym robotem płaskim
Na rys. 2 pokazano trójkąt utworzony przez l1, l2 oraz prostą łączącą początek układu (0) z początkiem układu (3). Liniami przerywanymi zaznaczono inną
możliwą konfigurację trójkąta, która prowadziłaby do tego samego położenia układu (3). Rozpatrując stały trójkąt, możemy zastosować tiwerdzenie kosinu- sów do wyzanaczenia θ2.
x2+ y2= l21+ l22− 2l1l2cos(π + θ2) (32) Ponieważ cos(π + θ2) = −cosθ2, mamy:
c2= x2+ y2− l21− l22 2l1l2
(33)
Aby ten trójkąt istniał, odległość punktu docelowego r = p
x2+ y2 musi być mniejsza lub równa sumie długości członów l1+l2.Równaine ma więc rozwiązanie dla θ2 między 0 i −π. Inne możliwe rozwiązanie (zaznaczone linią przerywaną) może być znalezione przez symetrię jako θ02= −θ2.
Aby wyznaczyć θ1, znajdujemy wyrażenia dla kątów ψ i β, pokazanych na rys.2 . Ponieważ β może być w każdej ćwiartce zależnie od znaków x i y, musimy użyć dwuargumentowej funkcji arctg:
β = Atan2(y, x) (34)
Znów stosujemy twierdzenie kosinusów w celu znalezenia ψ:
cosψ = x2+ y2+ l12− l22 2l1p
x2+ y2 (35)
Otrzymujemy:
θ1= β ± ψ (36)
Gdzie znak + gdy θ1< 0, a znak - , gdy θ2> 0
Wiemy, że kąty na plaszczyźnie się dodają, więc suma trzech kątów obrotu przegubów musi być orientacją ostatniego członu:
θ1+ θ2+ θ3= φ (37)
W celu uzupełnienia zadania równianie to naleźy rozwiązać względem θ3.
5 Rozwiązania numeryczne
5.1 Z wykorzystaniem jakobianu
Jakobian analityczny definiuje się jako macierz pochodnych cząstkowych kine- matyki manipulatora wyrażonej we współrzędnych:
Ja(θ) = ∂f (θ)
∂θ (38)
Poza jakobianem analitycznym definiujemy również jakobiany geometryczne. Do tej grupy jakobianów należą: jakobian geometryczny w przestrzeni, rozumiany jako przekształcenie wektora prędkości zmiennych przegubowych w wektor pręd- kości efektora w przestrzeni
vs
ws
= Js(q) ˙q (39)
oraz jakobian geometryczny w ciele, zdefiniowany jako przekształcenie wektora prędkości zmiennych przegubowych w wektor prędkości efektora w ciele
vb wb
= Jb(q) ˙q (40)
Jakobian manipulatora jest rozumiany jako macierz przekształcenia prędkości ruchu w przegubach w prędkość liniową efektora względem układu podstawo- wego i prędkość kątową efektora w przestrzeni
T˙0n ws
= Jm(q) ˙q (41)
Dla jakobianu geometrycznego w przestrzeni definiujemy kolejne jego kolumny osobno dla przegubu obrotowego, osobno dla przesuwnego. Dla i -tego przegubu obrotowego i -ta kolumna jakobianu geometrycznego jest nastepująca:
Jis(q) =
T0i−1(qi−1) × Ri−103kol(qi−1) Ri−103kol(qi−1)
(42)
natomiast dla i -tego przegubu przesuwnego i -ta kolumna ma posta’c Jis(q) =
Ri−103kol(qi−1) 0
(43) Element Ri−103kol(qi−1) oznacza trzecią kolumnę macierzy Ri−10 (qi−1) występują- cej w transformacji:
Ai−10 (qi−1) =
Ri−10 qi−1 T0i−1qi−1
0 1
(44) Dla jakobianu geometrycznego w ciele można przedstawić podobne definicje, aczkolwiek prościej jest skorzystać z jakobianu Js(q) i zależności
Jb(q) =
Rn0T(q) −Rn0T(q)[T0n(q)]
0 Rn0T(q)
Js(q) (45)
5.2 Z wykorzystaniem algorytmu iteracyjnego rozwiązy- wania zadania odwrotnego kinematyki
Algorytm: 1. Ustawić i=0.
2. Znaleźć q(0).
3. Obliczyć residuum δT (Q(i)) = J (q(i))(i) 4. Obliczyć q(i+1)= q(i)+ J−1(q(i))δT (q(i)) 5.Obliczyć dla i=i+1 od kroku 3.
Tolerancja błędu:
q(i+1)− q(i)< lub
J − I < Przykład: Dla planarnego manipulatora 2R.
Dane:
X Y
=
l1cθ1+ l2c(θ1+ θ2) l1sθ1+ l2s(θ1+ θ2)
(46) Rozwiązanie:
q =
θ1
θ2
(47) T =
X Y
(48) Otrzymujemy Jakobian:
J (q) =h ∂T
i
∂qj
i
=
" ∂X
∂θ1
∂X
∂θ2
∂Y
∂θ1
∂y
∂θ2
#
=
−l1sinθ1− l2sin(θ1+ θ2) −l2sin(θ1+ θ2) l1cosθ1+ l2cos(θ1+ θ2) l2cos(θ1+ θ2)
(49) Odwrotność otrzymanego Jakobianu:
J−1 = −1 l1l2sθ2
−l2c(θ1+ θ2) −l2s(θ1+ θ2) l1cθ1+ l2c(θ1+ θ2) l1sθ1+ l2s(θ1+ θ2)
(50) Formuła itaracji:
θ1
θ2
(i+1)
=
θ1
θ2
(i) +
X Y
−
X Y
(i)!
(51)
Załóżmy, że
l1= l2 = 1 (52)
T =
X Y
=
1 1
(53) Zacznijmy od wartości domyślnej
q(0)=
θ1
θ2
(0)
=
π/3
−π/3
(54)
dla której:
δT =
1 1
−
cosπ/3 + cos(π/3 + (−π/3)) sinπ/3 + sin(π/3 + (−π/3))
=
1 1
−
3 12 2
√3
=
−12
−12√ 3 + 1
(55) Jakobian i jego odwrotność
J =
−12√ 3 0
3
2 1
(56)
J−1=
−23√ 3 0
√3 1
(57)
θ1
θ2
(1)
=
θ1
θ2
(0)
+J(−1)δT =
π/3
−π/3
+
−23√ 3 0
√3 1
−12
−12√ 3 + 1
=
1.6245
−1.7792
(58)
Iteracja 1.
J =
−12√ 3 0
3
2 1
(59)
δT =
−12
−12√ 3 + 1
(60)
q(1)=
1.6245
−1.7792
(61) Iteracja 2.
J =
−0.844 0.154 0.934 0.988
(62) δT =
6.516 × 10−2 0.15553
(63) q(1) =
1.583
−1.582
(64) Iteracja 3.
J =
−1.00 −0.433 × 10−3 0.988 0.999
(65) δT =
0.119 × 10−1
−0.362 × 10−3
(66) q(1) =
1.570795886
−1.570867014
(67) Iteracja 4.
J =
−1.00 0.0 × 10−3 0.98850 1.0
(68) δT =
0.438 × 10−6
−0.711 × 10−4
(69) q(1) =
1.570796329
−1.570796329
(70)
Wyniki iteracji q(4) są bliskie dokładnemu wynikowi, który wynosi:
q =
π/2 −π/2 T
(71) .
5.2.1 Metoda Newtona-Raphsona
Metodę zailustrowano dla funkcji zmiennej f(x). Jak w każdej metodzie nume- rycznej, wymagane jest oszacowanie przybliżonej wartości x(1) poszukiwanego pierwiastka x0(f (x0) = 0). Niech przybliżona (oszacowana) wartość wynosi x(1) (rys. 3). Wtedy za pomocą linearyzacji funkcji f(x) w punkcie x(1) można przy- jąć, że funkcja w punkcie x(2)= x(1)+4x, ma postać:
f (x(2)) = f (x(1)+ df
dx|x(1)4 x (72)
Wówczas punkt przecieęcia stycznej t z osią odciętych (x), oddalony od x(1) o wartość 4x , wyznacza się z zależności:
0 = f (x(1)) + df
dx|x(1)4 x → 4x =−f (x(1))
df dx|x(1)
(73)
Kolejna wartość pierwiastka x(2) funkcji f (x) wynosi:
x(2)= x(1)+ 4x (74)
Następnym krokiem jest wyznaczenie wartości funkcji dla kolejnych wartości pierwiastka x(2) - sprawdzeniu warunku:
|f (x(2))| ¬ (75)
Rysunek 3: Idea metody Newtona-Raphsona
Przykład: Rozpatrzmy układ kinematyczny o ruchliwości dwa (rys. 4), któ- rego zadanie polega na przemieszczeniu punktu M po założonej trajektorii µM. Zadanie polega na wyznaczeniu konfiguracji układu, zwłaszcza współrzędnych punktu M, którą zajmie układ dla znanych wartości wymuszeń q1i q2. Według przyjętych na rys. 4 oznaczeń obowiązuje równanie wektorowe:
a + b − c − q2= 0 (76)
które skutkuje dwoma równaniami nieliniowymi (rzuty na osie układu współ- rzędnych) o postaci:
f1= acosq1+ bcosx1− c − q2cosx2= 0 (77) f2= asinq1+ bsinx1− q2sinx2= 0 (78) Dla uogólnienia metody funkcje f1i f2można zapisać łącznie jako F = [f 1f 2]T,
Rysunek 4: Generator trajektorii o ruchliwości dwa a w rozpatrywanym układzie jako:
F (q1, q2, x1, x2) = 0 (79) przy czym:
• q1, q2 - zmienne niezależne (znane wymuszenia),
• x1, x2 - zmienne zależne (niewiadome),
a pozostałe wielkości są stałe. Analogicznie do zależności (72) w przypadku układu równań F(x) wielu zmiennych mamy:
F (x(i+1)) = F (x(i)) +δF
δx|x(i)4 x (80)
Poprawkę 4x określa zależność:
0 = F (x(i)) +δF
δx|x(i)4 x → 4x =
4x1 4x2
= − δF
δx|x(i)
−1
F (x(i)) (81) Poprawka oszacowanego wektora rozwiązania prowadzi do nowej wartości:
x(i+1)= x(i)+ 4x (82)
Potrzebny w obliczeniach jakobian wyznacza się z zależności:
δF δx =
" δf
1
δx1 δf1 δx2 δf2
δx1
δf2
δx2
#
(83)
W przypadku omawianego przyk´ladu (rys. 4) jakobian ma posatć:
δF δx =
−bsinx1 q2sinx2
bcosx1 −g2cosx2
(84)
Zgodnie z (81) mamy:
4x1
4x2
=
−bsinx1 q2sinx2
bcosx1 −q2cosx2
|−1x(i)
f1(x(i)) f2(x(i))
(85) i kolejne przybliżenie pierwiastka z zależności (82). Współrzędne punktu M w przyjętym układzie współrzędnych, dla wartości zmiennych zależnych x1 oraz x2wyznacza się z zależności:
xM = acosq1+ dcos(x1+ β) (86) yM = asinq1+ dsin(x1+ β) (87) Patrząc na podany wyżej przykład widać, że łatwo tę metodę uogólnić na przy- padki układów o wielu niewiadomych - zależności (81)(82)(83) są ogólne.
Literatura
[1] Craig, J. J.: Wprowadzenie do robotyki. Mechanika i sterowanie., Warszawa, WNT, 1995
[2] Spong M.W., Vidyasagar M.: Dynamika i sterowanie robotów., Warszawa, WNT, 1997
[3] Kozłowski K., Dutkiewicz P., Wróblewski W.: Modelowanie i sterowanie robotów., Wydawnictwo Naukowe PWN, Warszawa 2003.
[4] Morecki A., Knapczyk J.: Podstawy robotyki. Teoria i elementy manipula- torów i robotów., WNT, Warszawa 1999.
[5] Tochoń K., Mazur A., Hossa R., Dulęba I., Muszyński R.: Manipulatory i roboty mobline., Warszawa, Akademicka Oficyna Wydawnicza PLJ, 2000 [6] Gracia L., Tornero J.: Tracking Trajectories with a Robotic Manipulator
with Singularities., LNCS, 4729 (2007), 595-605.
