6. Kinematyka odwrotna
W poprzednim wykładzie omówiliśmy, jak określić pozycję i orientację końcówki
roboczej w zale\
ności od zmiennych przegubowych. Treść tego wykładu jest skoncentrowana
na problemie odwrotnym, tzn. znalezieniu zmiennych przegubowych w zale\
ności od pozycji
i orientacji końcówki roboczej. Jest to zadaniem kinematyki odwrotnej i w ogólności jest ono
trudniejsze ni\
zadanie kinematyki prostej. Ogólnie problem kinematyki odwrotnej mo\
na
postawić następująco. Dana jest jednorodna macierz przekształcenia 4x4
R d
îÅ‚ Å‚Å‚
H = " E(3) (209)
ïÅ‚
0 1śł
ðÅ‚ ûÅ‚
gdzie R " SO(3). Nale\
y znalez
ć (jedno lub wszystkie) rozwiązania równania
n
T0 (q1,...,q6 ) = H (210)
gdzie
n
T0 (q1,...,q6 ) = A1 Å"...Å" An (211)
Równanie (210) daje 12 nieliniowych równań z n niewiadomymi, które mogą być
zapisane jako
Ti,j (q1,...,q6) = hi, j gdzie i =1,2,3; j =1,2,3,4 (212)
gdzie Tij, hij oznaczają 12 nietrywialnych elementów macierzy, odpowiednio T0n i H.
(Poniewa\
dolne wiersze obu macierzy T0n i H wynoszą (0.0.0.1), więc spośród 16 równań
reprezentowanych przez zale\
ność (210) cztery są trywialne).
Przykład 18.
Wróćmy do manipulatora stanfordzkiego z poprzedniego wykładu. Załó\
my, \
e
pozycja i orientacja ostatniego układu współrzędnych są określone przez macierz
r11 r12 r13 dx
îÅ‚ Å‚Å‚
ïÅ‚r r22 r23 dy śł
6 21
ïÅ‚ śł
T0 = A1 Å" A2 Å" A3 Å" A4 Å" A5 Å" A6 = (213)
ïÅ‚ śł
r31 r32 r33 dz
ïÅ‚ śł
0 0 0 1
ðÅ‚ ûÅ‚
W celu znalezienia odpowiednich zmiennych przegubowych ¸1, ¸2, ¸3, ¸4, ¸5 i ¸6 nale\
y
rozwiązać następujący układ nieliniowych równań trygonometrycznych
r11 = c1[c2 (c4c5c6 - s4s6 ) - s2s5c6 ]- s1(s4c5c6 + c4s6 )
Å„Å‚
ôÅ‚r = s1[c2 (c4c5c6 - s4s6 ) - s2s5c6 ] + cÕ1(s4c5c6 + c4s6 )
21
ôÅ‚
ôÅ‚ -s2 (c4c5c6 - s4s6 ) - c2s5c6
r31 =
ôÅ‚
12
ôÅ‚r = c1[-c2 (c4c5s6 + s4c6 ) + s2s5s6 ]- s1(-s4c5s6 + c4c6 )
ôÅ‚r22 = s1[-c2 (c4c5s6 + s4c6 ) + s2s5s6 ]+ c1(-s4c5s6 + c4c6 )
ôÅ‚
ôÅ‚r32 = s2 (c4c5s6 + s4c6 ) + c2s5s6
(214)
òÅ‚r = c1(c2c4s5 + s2c5) - s1s4s5
13
ôÅ‚
ôÅ‚r23 = s1(c2c4s5 + s2c5) + c1s4s5
ôÅ‚
= -s2c4s5 + c2c5
ôÅ‚r33
ôÅ‚dx = d3c1s2 - d2s1 + d6 (c1c2c4s5 + c1c5s2 - s1s4s5 )
ôÅ‚
d = d3s1s2 + d2c1 + d6 (c1s4s5 + c2c4s1s5 + c5s1s2 )
ôÅ‚
y
ôÅ‚
= d3c2 + d6 (c2c5 - c4s2s5)
ółdz
Równania te są zbyt skomplikowane, aby je rozwiązywać wprost, otrzymując wynik w
postaci zamkniętych, analitycznych wzorów. Potrzebujemy tu specjalnej efektywnej i
systematycznej techniki rozwiązywania, która wykorzysta szczególną strukturę kinematyczną
manipulatora. Bowiem podczas gdy zadanie kinematyki prostej ma zawsze jednoznaczne
rozwiązanie, które mo\
na otrzymać w prosty sposób przez przeliczenie równań, to zadanie
kinematyki odwrotnej mo\
e w ogóle nie mieć rozwiązania. Jeśli zaś rozwiązanie istnieje, to
mo\
e ono nie być jednoznaczne. W dodatku, poniewa\
równania kinematyki prostej są
przewa\
nie skomplikowanymi nieliniowymi funkcjami zmiennych przegubowych, więc
rozwiÄ…zanie zadania odwrotnego, nawet gdy istnieje, mo\
e być trudne do otrzymania.
Podczas rozwiązywania zadania kinematyki odwrotnej jesteśmy zainteresowani raczej
znalezieniem zamkniętej postaci rozwiązania równań ni\
rozwiÄ…zaniem numerycznym.
Znalezienie rozwiązania w postaci zamkniętej oznacza podanie następującej zale\
ności
qk = fk(h11, & , h34), gdzie k = 1, & , n (215)
Rozwiązania w postaci zamkniętej są wygodniejsze z dwóch powodów. Po pierwsze, w
pewnych zastosowaniach, takich jak np. śledzenie spoiny, której poło\
enie jest określane
przez system wizyjny, równania kinematyki odwrotnej muszą być rozwiązywane bardzo
szybko, powiedzmy co 20 milisekund. W tym celu praktycznie niezbędne jest posiadanie
gotowych równań, a nie korzystanie z metod iteracyjnych. Po drugie, równania kinematyki
mają na ogół wiele rozwiązań. Mając rozwiązanie w postaci zamkniętej, mo\
na przygotować
zasady wyboru konkretnych rozwiązań spośród wielu istniejących.
W praktyce, aby odpowiedzieć na pytanie o istnienie rozwiązań równań kinematyki
odwrotnej, nale\
y przeprowadzić zarówno rozwa\
ania matematyczne, jak te\
in\
ynierskie.
Przykładowo, ruch przegubu obrotowego mo\
e być ograniczony do zakresu mniejszego ni\

peÅ‚ne 360°. W ten sposób nie wszystkie matematyczne rozwiÄ…zania równaÅ„ kinematyki bÄ™dÄ…
odpowiadały fizycznie realizowalnym konfiguracjom manipulatora. Będziemy zakładać, \
e
dana pozycja i orientacja sÄ… takie, \
e istnieje co najmniej jedno rozwiązanie równania (210).
Gdy zostanie wyznaczone rozwiązanie matematyczne równań, nale\
y jeszcze sprawdzić, czy
spełnia ono wszystkie ograniczenia dotyczące zakresów mo\
liwych ruchów. Dla potrzeb
naszych rozwa\
ań będziemy odtąd zakładać, \
e dana w równaniu (210) macierz jednorodna H
odpowiada konfiguracji wewnÄ…trz przestrzeni roboczej manipulatora z osiÄ…galnÄ… orientacjÄ….
To zagwarantuje, \
e otrzymane rozwiÄ…zanie matematyczne odpowiada konfiguracji
osiÄ…galnej.
6.1. OdsprzÄ™\
enie kinematyczne
Chocia\
ogólnie problem kinematyki odwrotnej jest raczej trudny, jednak okazuje się, \
e dla
manipulatorów o sześciu stopniach swobody, których ostatnie trzy osie przecinają się w
jednym punkcie (tak jak we wcześniej opisanym manipulatorze stanfordzkim) jest mo\
liwe
odsprzÄ™\
enie powodujące podział kinematyki odwrotnej na dwa prostsze zadania, nazywane
odpowiednio kinematykÄ… odwrotnÄ… pozycji i kinematykÄ… odwrotnÄ… orientacji. Mo\
na wyrazić
to inaczej. Dla manipulatora z kiścią sferyczną i o sześciu stopniach swobody zadanie
kinematyki odwrotnej mo\
na podzielić na dwa prostsze zadania, mianowicie na znalezienie
najpierw poło\
enia punktu przecięcia osi kiści, zwanego dalej środkiem kiści, a następnie
znalezienie orientacji kiści.
Załó\
my, \
e mamy dokładnie sześć stopni swobody i \
e trzy ostatnie osie przecinajÄ…
siÄ™ w punkcie O. Wyrazimy zale\
ność (210) w postaci układu dwóch równań
reprezentujących równania obrotu i równania poło\
enia
 R6 (q1,...,q6) = R (216)
0
n
T0 (q1,...,q6 ) = d (217)
gdzie d i R opisują odpowiednio daną pozycję i orientację układu narzędzia.
Zało\
enie o kiści sferycznej oznacza, \
e osie z4, z5, z6 przecinajÄ… siÄ™ w punkcie O, a
stąd początki układów O4 i O5 umieszczonych zgodnie z konwencją Denayita-Hartenberga
będą zawsze w środku kiści O. Często początek O3 będzie równie\
w poczÄ…tku O, ale to nie
jest konieczne do dalszych rozwa\
ań. Wa\
nym punktem tego zało\
enia dla kinematyki
odwrotnej jest to, \
e ruch ostatnich trzech osi nie zmienia poło\
enia punktu O. Tak więc
poło\
enie środka kiści jest funkcją tylko pierwszych trzech zmiennych przegubowych.
Poniewa\
początek O6 układu narzędzia jest po prostu przesunięty o odległość d6 wzdłu\
osi
z5 od punktu O (patrz tab. 3), zatem wektor o6 w układzie O0x0y0z0 mo\
na zapisać w postaci
zale\
ności
o6  o = -d6 R k (218)
Niech pc oznacza wektor od początku układu bazowego do środka kiści. Wówczas do
uzyskania pozycji końcówki roboczej w punkcie reprezentowanym przez wektor d z
orientacją opisaną macierzą R = (rij) jest konieczne i wystarczające, aby środek kiści O był
zlokalizowany w punkcie wyznaczonym z równania
pc = d  d6 R k (219)
oraz aby orientacja układu O6x6y6z6 względem układu bazowego była opisana macierzą R.
Jeśli współrzędne wektora poło\
enia d są oznaczone przez dx, dy, dz a współrzędne wektora
środka kiści pc są oznaczone przez px, py, pz to równanie (219) opisuje zale\
ność
px dx
îÅ‚ Å‚Å‚ îÅ‚ - d6r13
Å‚Å‚
ïÅ‚ śł ïÅ‚d - d6r23śł
py = (220)
y
ïÅ‚ śł ïÅ‚ śł
ïÅ‚ śł ïÅ‚ - d6r33 ûÅ‚
śł
pz
ðÅ‚ ûÅ‚ ðÅ‚dz
Teraz, korzystając z równania (220), mo\
na znalez
ć wartości pierwszych trzech
zmiennych przegubowych. Określą one macierz obrotu R03 która zale\
y tylko od tych trzech
pierwszych zmiennych. Następnie z wyra\
enia
R = R03 R36 (221)
mo\
na teraz określić orientację końcówki roboczej względem układu O3x3y3z3 jako
R36 = (R03)-1 R = (R03)T R (222)
Ostatnie trzy kąty przegubowe są obliczane jako zbiór kątów Eulera odpowiadających
przekształceniu R36. Zauwa\
my, \
e prawa strona równania (222) jest całkowicie znana,
poniewa\
macierz R jest dana, a macierz R03 mo\
e być obliczona, gdy będą znane pierwsze 3
zmienne przegubowe. Idea odsprzÄ™\
enia kinematycznego została przedstawiona na rys. 49.
Rys. 49. OdsprzÄ™\
enie kinematyczne
Podsumowanie powy\
szych rozwa\
ań
Dla omówionej klasy manipulatorów rozwiązanie zadania kinematyki odwrotnej mo\
e
być przedstawione w postaci następującego algorytmu.
Krok l: Znalez
ć zmienne przegubowe q1, q2, q3 takie, \
e środek kiści o wektorze pc jest
zlokalizowany równaniem
pc = d  d6 R k (223)
Krok 2: Wykorzystując zmienne przegubowe wyznaczone w kroku l, obliczyć macierz R03
Krok 3: Znalez
ć zestaw kątów Eulera odpowiadających macierzy obrotu
R36 = (R03)-1 R = (R03)T R (224)
6.2. Kinematyka odwrotna pozycji  podejście geometryczne
W rozwa\
anych przez nas układach kinematycznych do znalezienia zmiennych
q1, q2, q3 odpowiadajÄ…cych wektorowi pc danemu wzorem (219) mo\
emy wykorzystać
podejście geometryczne. Nasz sposób postępowania ograniczymy do podejścia
geometrycznego z dwóch powodów. Po pierwsze, jak ju\
powiedzieliśmy, większość
projektowanych obecnie manipulatorów ma prostą kinematykę, składającą się zwykle z jednej
spośród pięciu przedstawionych we wcześniejszych wykładach konfiguracji oraz z kiści
sferycznej. W rzeczywistości to, \
e projekty manipulatorów rozwinęły się do takiego, a nie
innego stanu obecnego, spowodowane było częściowo właśnie trudnościami z ogólnym
zadaniem kinematyki odwrotnej. Po drugie, istnieją techniki, które mo\
na stosować do
rozwiązywania problemów kinematyki odwrotnej w ró\
nych konfiguracjach. Poniewa\

jednak Czytelnik najczęściej spotka się z konfiguracjami robotów tutaj rozwa\
anymi, dodanie
utrudnień związanych z analizą ogólnego przypadku wydaje się nieuzasadnione. Ogólne
metody rozwiązywania przedstawianych problemów Czytelnik znajdzie w obszernej
literaturze dotyczÄ…cej tego tematu.
Ogólnie mo\
na powiedzieć, \
e stopień skomplikowania zadania kinematyki odwrotnej
wzrasta wraz ze wzrostem liczby niezerowych parametrów kinematycznych. Dla większości
manipulatorów wiele parametrów spośród ai di jest równych zeru, wartości parametrów ąi
wynoszą 0 lub ąĄ/2 itd. Właśnie w takich przypadkach podejście geometryczne jest
najprostsze i najbardziej naturalne.
6.3. Konfiguracja stawowa
Rozwa\
my manipulator z łokciem przedstawiony na rys. 50. Elementy składowe
wektora pc oznaczamy przez px, py, pz. Rzutujemy wektor pc na płaszczyznę x0y0 jak to
pokazano na rys. 51. Z tego rzutowania widzimy, \
e
¸1 = arctan(px, py) (225)
gdzie arctan (px, py) oznacza dwuargumentowÄ… funkcjÄ™ arcus tangens zdefiniowanÄ… dla
wszystkich (x, y) `" 0, wyznaczajÄ…cÄ… jednoznacznie kÄ…t ¸ taki, \
e
x y
cos¸ = , sin¸ = (226)
x2 + y2 x2 + y2
Na przykład arctan(1, -1) = -Ą/4, a arctan(-1, 1) = +3Ą/4.
Rys. 50. Manipulator z Å‚okciem
Rys. 51. Rzut środka kiści na płaszczyznę x0y0
Zauwa\
my, \
e drugim dopuszczalnym rozwiÄ…zaniem dla kÄ…ta ¸1 jest
¸1 = Ä„ + arctan(px, py) (227)
gdy rozwiÄ…zanie ¸2 odpowiadajÄ…ce równaniu (225) zostanie zastÄ…pione przez Ä„  ¸2.
RozwiÄ…zania dla kÄ…ta ¸1 istniejÄ…, chyba \
e px = py = 0. W tym przypadku równanie
(225) nie jest określone, a manipulator znajduje się w pozycji osobliwej przedstawionej na
rys. 52. W pozycji tej środek kiści opisany wektorem pc le\
y na osi z0, a więc dla ka\
dej
wartoÅ›ci kÄ…ta ¸1 poÅ‚o\
enie środka jest stałe. Mamy więc do czynienia z nieskończoną liczbą
rozwiÄ…zaÅ„ dla ¸1.
Rys. 52. Konfiguracja osobliwa
Je\
eli w omawianym manipulatorze wprowadzi się przesunięcie d1 `" 0, jak na rys. 53,
to wtedy środek kiści nie mo\
e le\
eć na osi Z0. Ogólnie w tym przypadku będą istniały dwa
rozwiÄ…zania dla kÄ…ta ¸1. BÄ™dÄ… one odpowiadać tzw. konfiguracjom lewego i prawego
ramienia, przedstawionym na rys. 54 i rys. 55.
Rys. 53. Manipulator z łokciem z przesuniętym barkiem
Rys. 54. Konfiguracja lewego ramienia
Rys. 55. Konfiguracja prawego ramienia
Rysunek 54 ilustruje konfiguracjÄ™ lewego ramienia. Widzimy zale\
ność tutaj
następującą zale\
ność geometryczną
¸ = Ć  Ä… (228)
gdzie
 Ć = arctan(px, py) (229)
2
Ä… = arctan(d1, r2 - d12)= arctan(d1, px + p2 - d12) (230)
y
Drugie rozwiÄ…zanie, dla konfiguracji prawego ramienia, pokazanej na rys. 55, jest
następujące
2 2
¸1 = arctan( px , py ) - arctan(d1,- px + py - d12) (231)
W celu znalezienia kÄ…tów ¸2, ¸3 dla manipulatora z Å‚okciem, majÄ…c dany kÄ…t ¸1,
rozwa\
my płaszczyznę utworzoną przez człony 2 i 3, jak to przedstawiono na
rys. 56.
Rys. 56. Rzutowanie na płaszczyznę utworzoną przez człony 2 i 3
Poniewa\
ruchy tych członów są płaskie, rozwiązanie jest analogiczne do rozwiązania
dla dwuczłonowego manipulatora, omawianego podczas pierwszego wykładu. Zatem na
podstawie tych mo\
na stwierdzić, \
e
2 2 2
2 2 2
r + s2 - a2 - a3 px + p2 + ( pz - d1)2 - a2 - a3
y
cos¸3 = = := D (232)
2a2a3 2a2a3
a stÄ…d kÄ…t ¸3 jest równy
¸3 = arctan(D,Ä… 1- D2 ) (233)
Podobnie otrzymujemy kÄ…t ¸2 w postaci
¸2 = arctan(r,s) - arctan(a2 + a3 cos¸3,a3 sin¸3) =
(234)
2 2
= arctan( px + py , pz - d1) - arctan(a2 + a3 cos¸3,a3 sin¸3 )
Dwa rozwiÄ…zania dla ¸2 odpowiadajÄ… dolnej i górnej pozycji Å‚okcia. PrzykÅ‚adem
manipulatora z łokciem z przesunięciem jest manipulator PUMA zilustrowany na rys. 57. Jak
widać, mo\
liwe sÄ… cztery rozwiÄ…zania zadania kinematyki odwrotnej. OdpowiadajÄ… one
sytuacjom: ramię z lewej strony u góry, ramię z prawej strony u góry, ramię z lewej strony u
dołu, ramię z prawej strony u dołu. Zobaczymy dalej, \
e dla orientacji kiści istnieją dwa
rozwiÄ…zania, co daje w sumie a\
osiem mo\
liwych rozwiązań kinematyki odwrotnej dla
manipulatora PUMA.
b)
d)
c)
Rys. 57. Cztery rozwiÄ…zania kinematyki odwrotnej pozycji manipulatora PUMA: a) ramiÄ™
z lewej strony u góry, b) ramię z prawej strony u góry, c) ramię z lewej strony u dołu, d)
ramię z prawej strony u dołu
6.4. Konfiguracja sferyczna
Teraz zajmiemy siÄ™ rozwiÄ…zaniem kinematyki odwrotnej pozycji dla manipulatora
sferycznego o trzech stopniach swobody przedstawionego na rys. 58.
Rys. 58. Manipulator sferyczny
 Tak jak w przypadku manipulatora z Å‚okciem pierwsza zmienna przegubowa jest
obrotem podstawy i mo\
na ją wyznaczyć z równania
¸1 = arctan(px, py) (235)
przy zało\
eniu, \
e składowe px i py nie są jednocześnie równe zeru. Jeśli px i py są równe zeru,
to podobnie jak poprzednio konfiguracja jest osobliwa i kÄ…t ¸1 mo\
e mieć dowolną wartość.
KÄ…t ¸2 na podstawie rys. 58, jest dany wzorem
¸2 = arctan(r, s) (236)
gdzie r2 = px2 + py2, s -= pz  d1. Tak jak w przypadku manipulatora z Å‚okciem drugie
rozwiÄ…zania dla kÄ…tów ¸1, ¸2 sÄ… postaci
¸1 = Ä„ + arctan(px, py) (237)
¸2 = Ä„  arctan(r, s) (238)
Odległość liniową d3 mo\
na wyznaczyć ze wzoru
(d3 + a2)2 = r2 + s2 (239)
jako
2 2
d3 = r2 + s2 - a2 = px + py + ( pz - d1)2 - a2 (240)
Rozwiązania, w których argument pierwiastka jest ujemny, są pomijane. W sumie
otrzymujemy dwa rozwiązania kinematyki odwrotnej pozycji, oczywiście jeśli środek kiści
nie le\
y na osi z0. Je\
eli dodamy przesunięcie, to podobnie jak w omawianym manipulatorze z
Å‚okciem otrzymamy konfiguracjÄ™ lewego i prawego ramienia.
6.5. Kinematyka odwrotna orientacji
W poprzednich wykładach wykorzystaliśmy podejście geometryczne do rozwiązania
problemu kinematyki odwrotnej pozycji. Daje nam to wartości trzech pierwszych zmiennych
przegubowych, odpowiadające danej pozycji środka kiści. Teraz zajmiemy się problemem
kinematyki odwrotnej orientacji, czyli znalezieniem dla ostatnich trzech zmiennych
przegubowych wartości odpowiadających danej orientacji względem układu O3x3y3z3. Dla
kiści sferycznej mo\
na to zinterpretować jako problem znalezienia kątów Eulera
odpowiadajÄ…cych danej macierzy obrotu R, jak to ju\
wcześniej omawialiśmy.
6.6. KÄ…ty Eulera
Załó\
my, \
e dana jest macierz U = uij " SO(3) oraz przekształcenie R36 kątów Eulera
zgodne z wzorem (83). Zadaniem jest znalezienie kÄ…tów Eulera Ć, ¸, È speÅ‚niajÄ…cych
równanie macierzowe
îÅ‚ - sĆ sÈ - cĆc¸ sÈ - sĆcÈ cĆ s¸ u11 u12 u13
cĆc¸ cÈ Å‚Å‚
îÅ‚ Å‚Å‚
ïÅ‚s c¸ cÈ + cĆ sÈ - sĆc¸ sÈ + cĆcÈ sĆ s¸ śł
ïÅ‚u
= u22 u23 śł (241)
Ć 21
ïÅ‚ śł
ïÅ‚ śł
ïÅ‚
ïÅ‚ śł
- s¸ cÈ s¸ sÈ c¸ śł ðÅ‚u31 u32 u33 ûÅ‚
ðÅ‚ ûÅ‚
Załó\
my, \
e elementy u13, u23 nie są oba równe zeru. Wtedy z powy\
szego równania
widać, \
e s¸ `" 0 i stÄ…d elementy u13, u23 nie sÄ… jednoczeÅ›nie równe zeru. JeÅ›li oba elementy ^
u13, u23 sÄ… równe zeru, to u33 `" Ä…1 i wówczas C¸ = u33, a s¸ = Ä…(1  u332)1/2, a wiÄ™c
2
¸ = arctan(u33, 1- u33 ) (242)
lub
2
¸ = arctan(u33,- 1- u33 ) (243)
JeÅ›li wybierzemy pierwszÄ… wartość kÄ…ta ¸, to s¸ > 0 oraz
Ć = arctan(u13,u23) (244)
È = arctan(-u31,u32 ) (245)
Natomiast gdy wybierzemy drugÄ… wartość kÄ…ta ¸, to s¸ < 0 oraz
Ć = arctan(-u13,-u23) (246)
È = arctan(u31,-u32 ) (247)
Mamy więc dwa rozwiązania w zale\
noÅ›ci od wybranego znaku zmiennej ¸.
Jeśli u13 = u23 = 0, to fakt, \
e macierz U jest ortogonalna, implikuje równości u33 = ą1 i
u13 = u32 = 0. W ten sposób U ma postać
u11 u12 0
îÅ‚ Å‚Å‚
ïÅ‚u śł
U = u22 0 (248)
21
ïÅ‚ śł
ïÅ‚ śł
0 0 Ä…1ûÅ‚
ðÅ‚
Gdy u33 = 1, wtedy C¸ = l i s¸ = 0, a wiÄ™c ¸ = 0. W tym przypadku równanie (241)
przyjmuje postać
cĆcÈ
îÅ‚ - sĆ sÈ - cĆ sÈ - sĆcÈ 0 cĆ+È - sĆ+È 0 u11 u12 0
Å‚Å‚ îÅ‚ Å‚Å‚ îÅ‚ Å‚Å‚
ïÅ‚s cÈ + cĆ sÈ - sĆ sÈ + cĆcÈ 0śł = ïÅ‚s cĆ+È 0śł = ïÅ‚u u22 0śł (249)
Ć Ć +È 21
ïÅ‚ śł ïÅ‚ śł ïÅ‚ śł
ïÅ‚ śł ïÅ‚ śł ïÅ‚ śł
0 0 1ûÅ‚ ðÅ‚ 0 0 1ûÅ‚ ðÅ‚ 0 0 1ûÅ‚
ðÅ‚
Suma Ć + È mo\
e być wyznaczona jako
Ć + È = arctan(u11, u21) = arctan(u11, -u12) (250)
Poniewa\
mo\
na wyznaczyć tylko sumÄ™ Ć + È , w tym przypadku istnieje
nieskończenie wiele rozwiązań. Mo\
emy przyjąć jako zasadÄ™ Ć = 0 i zdefiniować È na
podstawie wzoru (248).
Gdy u33 = -1, wtedy C¸ = -l i s¸ = 0, tak wiÄ™c ¸ = Ä„. W tym przypadku otrzymujemy
równanie (241) w następującej postaci
îÅ‚- cĆ -È - sĆ-È 0 u11 u12 0
Å‚Å‚ îÅ‚ Å‚Å‚
ïÅ‚ śł ïÅ‚u śł
sĆ -È cĆ-È 0 = u22 0 (251)
21
ïÅ‚ śł ïÅ‚ śł
ïÅ‚ śł ïÅ‚ śł
0 0 -1ûÅ‚ ðÅ‚ 0 0 -1ûÅ‚
ðÅ‚
W ten sposób rozwiązaniem jest
Ć  È = arctan(-u11, -u12) = arctan(-u21, -u22) (252)
Przykład 19. Manipulator stawowy
Macierz R03 dla manipulatora stawowego lub z łokciem łatwiej jest policzyć, gdy
zapiszemy jÄ… w postaci
c1c23
îÅ‚ - c1s23 - s1
Å‚Å‚
ïÅ‚s
R3 = c23 - s1s23 c1 śł (253)
0 1
ïÅ‚ śł
ïÅ‚- s23 - c23 0
śł
ðÅ‚ ûÅ‚
Macierz R36 = A4 A5 A6 jest dana wzorem
c4c5c6 + s4s6 - c4c5c6 + s4c6 c4s5
îÅ‚ Å‚Å‚
6 ïÅ‚s
R3 = c5c6 - c4c6 - s4c5s6 - c4c6 s4s5 śł (254)
4
ïÅ‚ śł
ïÅ‚ śł
s5c6 - s5s6 c5 ûÅ‚
ðÅ‚
Równanie, które nale\
y rozwiązać dla trzech ostatnich zmiennych, jest więc
następującej postaci
 R36 = (R03)T R := U (255)
i do jego rozwiÄ…zania mo\
e być zastosowane poprzednie rozwiązanie dla kątów Eulera. Na
przykład elementy (l, 3) i (2, 3) w powy\
szym równaniu macierzowym są równe
u13 = c4s5 = c1c23r13 + s1c23r23 - s23r33 (256)
u23 = s4s5 = -c1s23r13 - s1s23r23 - c23r33 (257)
oraz
u33 = s1r13 - c1r23 (258)
StÄ…d, gdy oba wyra\
enia (256) i (257) nie są jednocześnie równe zeru, wtedy z
wzorów (242) i (243) otrzymujemy ¸5 w postaci
¸5 = arctan(s1r13 - c1r23,Ä… 1- (s1r13 - c1r23)2) (259)
JeÅ›li w równaniu (259) zostanie wybrana dodatnia wartość pierwiastka, to kÄ…ty ¸4 i ¸6
obliczamy na podstawie wzorów (244) i (245) odpowiednio jako
¸4 = arctan(c1cÕ23r13 + s1c23r23 - s23r33,-c1s23r13 - s1s23r23 - c23r33 (260)
¸6 = arctan(s1r11 - c1r21,s1r12 + c1r22 ) (261)
Inne rozwiązania są otrzymywane analogicznie. Jeśli s5 = O, to osie przegubów 3 i 5
są współliniowe. Jest to konfiguracja osobliwa i mo\
na wtedy wyznaczyć jedynie sumę
¸4 + ¸6. Jednym z mo\
liwych działań w takiej sytuacji jest wybranie dowolnej wartości kąta
¸4 i wyznaczenie ¸6 z wzoru (250) lub (252).
6.7. Podsumowanie rozwiÄ…zania dla manipulatora z Å‚okciem
W celu podsumowania wcześniejszych wyników spiszemy teraz pełne rozwiązanie
kinematyki odwrotnej dla manipulatora z łokciem o sześciu stopniach swobody,
przedstawionego na rys. 50, który nie ma przesunięcia przegubów i ma zamontowaną kiść
sferycznÄ….
Jeśli są dane
dx
îÅ‚ Å‚Å‚
ïÅ‚d śł
d = (262)
y
ïÅ‚ śł
ïÅ‚ śł
ðÅ‚dz ûÅ‚
r11 r12 r13
îÅ‚ Å‚Å‚
ïÅ‚r
R = r22 r23śł (263)
21
ïÅ‚ śł
ïÅ‚ śł
r32 r33 ûÅ‚
ðÅ‚r31
oraz
px dx
îÅ‚ Å‚Å‚ îÅ‚ - d6r13
Å‚Å‚
ïÅ‚ śł ïÅ‚d - d6r23śł
py = (264)
y
ïÅ‚ śł ïÅ‚ śł
ïÅ‚ śł ïÅ‚ - d6r33 ûÅ‚
śł
pz
ðÅ‚ ûÅ‚ ðÅ‚dz
to zmienne przegubowe w notacji D-H wynikajÄ… z zale\
ności określonych poni\
szym wzorem
¸1 = arctan( px , py )
Å„Å‚
ôÅ‚
2 2
ôÅ‚¸2 = arctan( px + py , pz - d1) - arctan(a2 + a3c3,a3s3)
ôÅ‚
ôÅ‚¸3 = arctan(M ,Ä… 1- M 2 ),
ôÅ‚
2 3 2
px + p2 + ( pz - d1)2 - a2 - a3
ôÅ‚
y
(265)
òÅ‚gdzie M =
2a2a3
ôÅ‚
ôÅ‚¸4 = arctan(c1cÕ23r13 + s1c23r23 - s23r33,-c1s23r13 - s1s23r23 - c23r33)
ôÅ‚
ôÅ‚¸5 = arctan(s1r13 - c1r23,Ä… 1- (s1r13 - c1r23)2)
ôÅ‚
ôÅ‚¸ = arctan(s1r11 - c1r21,s1r12 + c1r22 )
6
ół
Przykład 20. Manipulator SCARA
Jako inny przykład rozwa\
my manipulator SCARA, którego kinematyka prosta jest
określona przez macierz T04 z równania (207). Kinematyka odwrotna jest dana jako zbiór
rozwiązań równania
c12c4 + s12s4 - c12s4 + s12c4 0 a1c1 + a2c12
îÅ‚ Å‚Å‚
ïÅ‚s
R d c4 - c12s4 - s12s4 - c12c4 0 a1s1 + a2s12 śł
îÅ‚ Å‚Å‚
12
ïÅ‚ śł
= (266)
ïÅ‚ śł
ïÅ‚ śł
0 1 0 0 -1 - d3 - d4
ðÅ‚ ûÅ‚
ïÅ‚ śł
0 0 0 1
ðÅ‚ ûÅ‚
Zauwa\
my na poczÄ…tku, \
e poniewa\
manipulator SCARA ma tylko cztery stopnie
swobody, nie wszystkie elementy macierzy H " E(3) dopuszczają rozwiązanie równania
(266). Rzeczywiście, łatwo mo\
na stwierdzić, \
e równanie (266) nie ma rozwiązania, chyba
\
e macierz R jest postaci
cÄ… sÄ… 0
îÅ‚ Å‚Å‚
ïÅ‚s śł
R = - cÄ… 0 (267)
Ä…
ïÅ‚ śł
ïÅ‚ śł
0 0 -1ûÅ‚
ðÅ‚
W tym przypadku suma ¸1 + ¸2  ¸4 jest okreÅ›lona wzorem
¸1 + ¸2  ¸4 = Ä… = arctan(r12, r11) (268)
Po zrzutowaniu ramion manipulatora na płaszczyznę x0y0 otrzymujemy postać jak na rys. 59.
Rys. 59. Manipulator SCARA
Widać stąd, \
e
¸2 = arctan(Ä… 1- r2 ,r) (269)
2 2 2 2
dx + d - a1 - a2
y
2
r = (270)
2a1a2
¸1 = arctan(dx,dy ) - arctan(a1 + a2c2,a2s2 ) (271)
Mo\
emy teraz wyznaczyć kÄ…t ¸4 z zale\
ności (268)
¸4 = ¸1 + ¸2  Ä… = ¸1 + ¸2  arctan(r12, r11) (272)
Na koniec d3 dane jest wzorem
d3 = dz + d4 (273)


Wyszukiwarka

Podobne podstrony:
odwrotne zadanie kinematyki projekt
Kinematyka i Dynamika Układów Mechatronicznych
kinematyka
Odwrotna notacja polska
Wyklad 9 Kinematyka relatywistyczna
C03 Kinematyka PM (01 08)
Biomechanics of the cervical spine I Normal kinematics
KINEMATYKA CIECZY
Kinematyka
Przemieszczenia model kinematyczny sem I mgr stud

więcej podobnych podstron