1
4. Kinematyka odwrotna
Zadanie kinematyki odwrotnej jest przeciwieństwem zadania kinematyki prostej;
moŜna opisać je następująco: mając daną pozycję i orientację końcówki technologicznej,
wyznacz zmienne złączowe, które zapewniają osiągnięcie docelowej pozycji i orientacji. W
ogólnym przypadku jest to zadanie trudniejsze, niŜ zadanie kinematyki prostej, poniewaŜ na
ogół nie istnieje jednoznaczne rozwiązanie nieliniowych równań kinematyki.
Problem kinematyki odwrotnej moŜna przedstawić następująco: dana jest jednorodna
macierz przekształcenia 4x4
=
=
44
43
42
41
34
33
32
31
24
23
22
21
14
13
12
11
1
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
0
p
R
H
,
naleŜy znaleźć jedno lub wszystkie rozwiązania równania
H
H
=
)
,
,
,
(
2
1
0
n
n
q
q
q
K
(1)
gdzie:
=
=
−
44
43
42
41
34
33
32
31
24
23
22
21
14
13
12
11
1
2
2
1
1
1
0
2
1
0
)
(
)
(
)
(
)
,
,
,
(
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
q
q
q
q
q
q
n
n
n
n
n
H
H
H
H
K
K
Równanie (1) daje 12 równań z n niewiadomymi, które moŜna zapisać jako
4
,
3
,
2
,
1
;
3
,
2
,
1
;
)
,
,
,
(
2
1
=
=
=
j
i
h
q
q
q
H
ij
n
ij
K
gdzie:
ij
ij
h
H ,
oznaczają 12 nietrywialnych elementów macierzy, odpowiednio
n
H
0
i H .
PoniewaŜ ostatni wiersz obu macierzy jest równy (0,0,0,1), więc spośród 16 równań
reprezentowanych przez zaleŜność (1) cztery są trywialne.
Zadanie odwrotne kinematyki moŜna próbować rozwiązać na wiele sposobów np.
wykorzystując podejście geometrycznie, analitycznie lub numerycznie.
4.1. Podejście geometryczne
4.1.1. Manipulator płaski z łokciem
Dla manipulatora płaskiego z łokciem (rys. 1) zadanie odwrotne kinematyki polega na
znalezieniu zmiennych złączowych
θ
1
i
θ
2
dla znanych długości członów manipulatora
odpowiednio a
1
i a
2
oraz danego połoŜenia chwytaka (x, y).
2
Rys. 1. Wyznaczenie kątów w złączach dwuczłonowego manipulatora płaskiego z łokciem
PoniewaŜ równania kinematyki prostej są najczęściej nieliniowe, przez co znalezienie
rozwiązania kinematyki odwrotnej moŜe nie być łatwe i w ogólności nie będzie to
rozwiązanie jednoznaczne. Na przykład dla rozwaŜanego przypadku nie ma rozwiązania gdy
dane współrzędne (x,y) są poza zasięgiem manipulatora. Istnieje jedno rozwiązanie, gdy
ramiona są całkowicie wyprostowane. Gdy współrzędne (x,y) mieszczą się w zasięgu
manipulatora, mogą istnieć dwa rozwiązania (rys. 2), czyli konfiguracje „łokieć u góry” i
„łokieć u dołu”.
Rys. 2. Rozwiązanie niejednoznaczne kinematyki odwrotnej
Stosując prawo cosinusów, moŜna wyznaczyć kąt
θ
2
w następujący sposób:
)
180
cos(
2
)
(
2
2
1
2
2
2
1
2
2
θ
−
−
+
=
+
o
a
a
a
a
y
x
)
cos(
)
180
cos(
2
2
θ
θ
−
=
−
o
D
a
a
a
a
y
x
=
−
−
+
=
:
2
)
cos(
2
1
2
2
2
1
2
2
2
θ
(2)
D
arccos
2
=
θ
θ
1
θ
2
a
1
a
2
y
x
łokieć u góry
łokieć u dołu
3
JednakŜe lepszym sposobem znalezienia kąta
θ
2
jest zauwaŜenie, Ŝe jeśli cos
θ
2
jest dany
wzorem (2), to sin
θ
2
jest dany odpowiednio wzorem
2
2
1
sin
D
−
±
=
θ
stąd
−
±
=
D
D
2
2
1
arctan
θ
(3)
Zaletą tego podejścia jest rozróŜnienie obu konfiguracji „łokieć u góry” i „łokieć u dołu”
przez wybór odpowiedniego znaku w równaniu (3). Kąt
θ
1
moŜna wyznaczyć w następujący
sposób:
Rys. 3. Manipulator płaski z łokciem – wyznaczenie kąta
θ
1
Na podstawie rys. 3 moŜna napisać:
β
α
θ
−
=
1
gdzie:
=
x
y
arctan
α
Stosując prawo sinusów moŜna wyznaczyć kąt β według wzorów
1
2
1
2
)
sin(
)
sin(
sin
a
a
a
β
θ
γ
β
−
=
=
β
θ
β
θ
β
θ
sin
cos
cos
sin
)
sin(
2
2
2
−
=
−
β
θ
β
θ
β
sin
cos
cos
sin
sin
2
2
2
2
1
a
a
a
−
=
β
θ
θ
β
cos
sin
)
cos
(
sin
2
2
2
2
1
a
a
a
=
+
2
2
1
2
2
cos
sin
tan
cos
sin
θ
θ
β
β
β
a
a
a
+
=
=
+
=
2
2
1
2
2
cos
sin
arctan
θ
θ
β
a
a
a
β
θ
γ
θ
γ
β
θ
γ
β
γ
−
=
⇒
=
+
=
−
+
+
2
2
2
180
180
o
o
y
x
a
2
1
a
1
2
4
+
−
=
2
2
1
2
2
1
cos
sin
arctan
arctan
θ
θ
θ
a
a
a
x
y
(4)
ZauwaŜmy, Ŝe kąt
θ
1
zaleŜy od kąta
θ
2
. Ma to znaczenie fizyczne, gdyŜ oczekujemy róŜnych
wartości kąta
θ
1
w zaleŜności od tego, które rozwiązanie wybierzemy dla kąta
θ
2
.
Zatem, zmienne złączowe manipulatora płaskiego z łokciem moŜna wyznaczyć przy uŜyciu
następującego algorytmu:
2
1
2
2
2
1
2
2
2 a
a
a
a
y
x
D
−
−
+
=
(5a)
−
±
=
D
D ,
1
atan2
2
2
θ
(5b)
( )
(
)
2
2
1
2
2
1
cos
,
sin
atan2
,
atan2
θ
θ
θ
a
a
a
x
y
+
−
=
(5c)
gdzie atan2(y,x) jest dwuargumentową funkcją obliczającą wartość arctan(y/x) z
uwzględnieniem znaków zarówno przy x jak i y, w celu znalezienia ćwiartki, w której leŜy
szukany kąt. Na przykład:
Na rys. 4 przedstawiono rozwiązania odwrotnego zadania kinematyki manipulatora płaskiego
o wymiarach a
1
= 5, a
2
= 5 i pozycji zadanej x = 6, y = 6 dla konfiguracji „łokieć u dołu” (rys.
4a) i „łokieć u góry” (rys. 4b). Obliczenia wykonano w programie Matlab.
Rys. 4. Rozwiązanie odwrotnego zadania kinematyki dla manipulatora płaskiego z łokciem
4
)
1
,
1
(
atan2
π
−
=
−
4
3
)
1
,
1
(
atan2
π
+
=
−
Y
X
1
-1
= /4
Y
X
1
-1
= +3 /4
th2= atan2(sqrt(1-D^2),D);
th1 = 13.0519°
th2 = 63.8961°
th2= atan2(-sqrt(1-D^2),D);
th1 = 76.9481°
th2 = -63.8961°
a)
b)
5
4.1.2. Manipulator z łokciem (manipulator stawowy)
Schemat kinematyczny manipulatora stawowego z przyporządkowanymi do niego
układami współrzędnych przedstawiono na rys. 5.
Rys. 5. Schemat kinematyczny manipulatora stawowego.
Rzutując wektor
p
O
na płaszczyznę
0
X
0
Y zmienną złączową
θ
1
moŜna wyznaczyć z równania:
)
,
(
atan2
1
Ox
Oy
p
p
=
θ
W celu znalezienia kątów
θ
2
i
θ
3
rozwaŜamy płaszczyznę utworzoną przez ogniwa 2 i 3.
PoniewaŜ ruchy tych członów są płaskie, dalsze rozwaŜania są identyczne jak dla
manipulatora płaskiego z łokciem. Na podstawie (5) moŜna napisać:
(6a)
−
±
=
,D
D
2
3
1
atan2
θ
(6b)
( )
(
)
=
+
−
=
,
cos
,
sin
atan2
,
atan2
3
3
2
3
3
2
θ
θ
θ
a
a
a
r
s
O
(
)
,
cos
,
sin
atan2
,
atan2
3
3
2
3
3
2
2
1
θ
θ
ε
a
a
a
p
p
p
Oy
Ox
Oz
+
−
+
−
=
(6c)
0
X
0
Y
0
Z
1
Z
2
Z
1
X
p
O
p
Oy
p = s+
Oz
1
p
Ox
s
1
1
r
O
s
0
Z
r
O
a
2
a
3
θ
3
θ
2
s
0
Z
r
O
a
2
a
3
θ
3
θ
2
(
)
3
2
2
3
2
2
2
1
2
2
3
2
2
3
2
2
2
2
2
2
a
a
a
a
p
p
p
a
a
a
a
s
r
D
Oz
Oy
Ox
O
−
−
−
+
+
=
−
−
+
=
ε
6
Gdy
p
Ox
= p
Oy
= 0 równanie
)
,
(
atan2
1
Ox
Oy
p
p
=
θ
jest nieokreślone, a manipulator znajduje
się w pozycji osobliwej (rys. 6).
0
Z
Rys. 6. Konfiguracja osobliwa manipulatora stawowego
W tej pozycji efektor (opisany wektorem
p
O
) leŜy na osi
0
Z. Dla kaŜdej wartości kąta
θ
1
połoŜenie efektora jest stałe, tzn. istnieje nieskończenie wiele rozwiązań dla
θ
1
.
JeŜeli dla rozwaŜanego manipulatora wprowadzi się przesunięcie
a
1
≠ 0 (rys. 7), wówczas
ś
rodek kiści nie moŜe leŜeć na osi
0
Z.
0
Z
a
1
Rys. 7. Manipulator z łokciem i przesuniętym barkiem
W tym przypadku istnieją dwa rozwiązania dla kąta
θ
1
, tzw. konfiguracja lewego i prawego
ramienia.
7
4.1.3. Manipulator z łokciem (stawowy) i przesuniętym barkiem
Schemat kinematyczny manipulatora stawowego z przesuniętym barkiem wraz z
przyporządkowanymi do niego układami współrzędnych, zgodnie z reprezentacją D-H,
przedstawiono na rys. 8.
Rys. 8. Schemat kinematyczny manipulatora z łokciem i przesuniętym barkiem
W tab. 1 zamieszczono parametry D-H rozwaŜanego manipulatora.
Tab. 1. Parametry D-H manipulatora stawowego z przesuniętym barkiem
θ
i
ε
i
a
i
α
i
1
θ
1
*
ε
1
0
+90
°
2
0
−
ε
2
0
0
3
θ
3
*
0
a
3
0
4
θ
4
*
0
a
4
0
5
+90
°
0
0
+90
°
6
0
ε
6
0
0
RozwaŜmy konfigurację lewego i prawego ramienia.
0
Z
a
3
6
2
1
a
4
0
X
0
Y
1
X
2
X
3
X
5
X
6
X
4
X,
1
Z
2
Z
3
Z
4
Z
5
Z,
6
Z
8
Konfiguracja lewego ramienia - na podstawie (6) moŜna napisać
(
)
(
)
(
)
(
)
6
4
3
2
6
4
2
3
2
1
2
2
2
2
2
ε
ε
ε
ε
+
+
−
−
−
+
−
+
=
a
a
a
a
p
p
p
D
Oz
Oy
Ox
−
±
=
D
D ,
1
atan2
2
4
θ
(
)
(
)
(
)
4
6
4
3
4
6
4
2
2
2
2
1
3
cos
,
sin
atan2
,
atan2
θ
ε
θ
ε
ε
ε
θ
+
+
+
−
−
+
−
=
a
a
a
p
p
p
Oy
Ox
Oz
Konfiguracja prawego ramienia - wyznacz
θ
4
i
θ
3
jak dla konfiguracji lewego ramienia i
zmień je w następujący sposób
4
4
-
θ
θ
=
3
3
-
180
θ
θ
°
=
Konfiguracja lewego ramienia
Konfiguracja prawego ramienia
Z rysunku widzimy Ŝe:
α
φ
θ
−
=
1
)
,
(
atan2
Ox
Oy
p
p
=
φ
gdzie:
2
2
2
2
2
2
2
2
2
)
tan(
ε
ε
ε
ε
α
−
+
=
−
=
Oy
Ox
O
p
p
r
−
+
=
2
2
2
2
2
,
atan2
ε
ε
α
Oy
Ox
p
p
0
X
p
Ox
p
Oy
r
O
0
Y
2
1
0
X
p
Ox
p
Oy
r
O
0
Y
2
1
Z rysunku widzimy Ŝe:
°
+
+
=
180
1
α
φ
θ
9
Przykładem manipulatora z łokciem i przesunięciem w barku jest robot PUMA. Dla tego
robota moŜliwe są cztery rozwiązania zadania kinematyki odwrotnej: a) ramię z lewej strony
u dołu (rys. 9a), b) ramię z lewej strony u góry (rys. 9b), c) ramię z prawej strony u dołu (rys.
9c), d) ramię z prawej strony u góry (rys. 9d).
Rys. 9. Rozwiązanie odwrotnego zadania kinematyki dla manipulatora PUMA.
Zmienne złączowe wyznaczono dla p
Ox
= p
Oy
= p
Oz
= 6.
Wymiary manipulatora:
ε
1
=5;
ε
2
=2; a
3
=5; a
4
=5;
ε
6
=1.
4.2. OdsprzęŜenie kinematyczne
Zadanie odwrotne kinematyki jest raczej problemem trudnym, jednak okazuje się, Ŝe
dla manipulatorów o sześciu stopniach swobody, których ostatnie trzy osie przecinają się w
jednym punkcie, jest moŜliwe
odsprzęŜenie powodujące podział kinematyki odwrotnej na
dwa prostsze zadania, nazwane kinematyką odwrotną pozycji i kinematyką odwrotną
orientacji. Pierwsze polega na znalezieniu punktu przecięcia osi kiści (zwanego środkiem
kiści) drugie na znalezieniu orientacji kiści. Przedstawmy zaleŜność (1) dla manipulatora o
sześciu stopniach swobody w następującej postaci
°
=
°
=
°
=
-82.3377
52.6286
31.3670
4
3
1
θ
θ
θ
°
=
°
=
°
=
82.3377
-38.7998
31.3670
4
3
1
θ
θ
θ
°
=
°
=
°
=
-82.3377
218.7998
238.6330
4
3
1
θ
θ
θ
°
=
°
=
°
=
82.3377
127.3714
238.6330
4
3
1
θ
θ
θ
a)
b)
c)
d)
10
p
p
R
R
=
=
)
,
,
(
)
,
,
(
6
1
6
,
0
0
6
1
6
0
q
q
q
q
K
K
gdzie: p i R opisują odpowiednio daną pozycję i orientację układu narzędzia (układu {6},
patrz rys. 10).
Rys. 10. OdsprzęŜenie kinematyczne
Dla rozwaŜanej kiści osie
4
Z,
5
Z,
6
Z przecinają się w punkcie O, stąd początki układów {4} i
{5} umieszczonych zgodnie z konwencją D-H będą zawsze w środku kiści - punkt O. Często
początek {3} będzie równieŜ w O, ale nie jest to konieczne do dalszych rozwaŜań.
WaŜnym punktem przedstawionego załoŜenia dla kinematyki odwrotnej jest to, Ŝe ruch
ostatnich trzech osi nie zmienia połoŜenia punktu O. Zatem połoŜenie środka kiści jest
funkcją tylko pierwszych trzech zmiennych złączowych q
1
, q
2
, q
3
.
0
p
O
oznacza wektor od
początku układu bazowego do środka kiści. Układ współrzędnych końcówki roboczej ({6}),
względem układu bazowego, opisany jest przez wektor p i orientację R = (r
ij
). PoniewaŜ
początek układu {6} jest przesunięty o odległość
6
ε
wzdłuŜ osi
5
Z od punktu O moŜna
napisać
k
R
p
p
6
0
ε
−
=
O
(7)
JeŜeli współrzędne wektora p oznaczymy przez p
x
, p
y
, p
z
a współrzędne wektora środka kiści
przez p
Ox
, p
Oy
, p
Oz
to równanie (7) przyjmuje postać:
−
−
−
=
33
6
23
6
13
6
r
p
r
p
r
p
p
p
p
z
y
x
Oz
Oy
Ox
ε
ε
ε
(8)
0
p
O
p
0
X
0
Y
0
Z
4
Z
5
Z
6
Z
ś
rodek kiści - punkt O
11
Znając połoŜenie środka kiści (8) moŜna wyznaczyć wartości pierwszych trzech zmiennych
złączowych q
1
, q
2
, q
3
stosując podejście geometryczne. Na podstawie wyznaczonych
pierwszych trzech zmiennych złączowych naleŜy obliczyć macierz obrotu
3
0
R która zaleŜy
tylko od tych wielkości. Następnie z wyraŜenia
6
3
3
0
R
R
R
=
(9)
moŜna określić orientację końcówki roboczej (ostatnie trzy zmienne złączowe) względem
układu {3} jako
( )
( )
R
R
R
R
R
T
3
0
1
3
0
6
3
=
=
−
(10)
To, Ŝe konstrukcje manipulatorów rozwinęły się do takiego, a nie innego stanu
obecnego, spowodowane było częściowo trudnościami z odwrotnym zadaniem kinematyki.
Ogólnie moŜna powiedzieć, Ŝe stopień skomplikowania zadania kinematyki odwrotnej
wzrasta ze wzrostem liczby niezerowych parametrów kinematycznych. Dla większości
manipulatorów wiele parametrów spośród
i
i
a
ε
,
jest równych zeru, wartości parametrów
i
α
wynoszą 0 lub
2
π
±
itd.
4.3. Kinematyka odwrotna kiści – podejście analityczne
ZałóŜmy, Ŝe dana jest macierz U = (u
ij
)
3x3
, gdzie UU
T
= 1
, det(U)=1
oraz przekształcenie
3
R
6
kiści (patrz rozdział 3, str. 8 i 9). Zadaniem jest znalezienie kątów
θ
4
,
θ
5
,
θ
6
, spełniających równanie macierzowe
U
R
=
6
3
=
−
+
−
+
−
−
−
33
32
31
23
22
21
13
12
11
5
6
5
6
5
5
4
6
4
6
5
4
6
4
6
5
4
5
4
6
4
6
5
4
6
4
6
5
4
c
s
s
c
s
s
s
c
c
s
c
s
s
c
c
c
s
s
c
c
s
s
c
c
s
s
c
c
c
u
u
u
u
u
u
u
u
u
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
(11)
1) Jeśli u
13
i u
23
≠ 0 wówczas s
θ
5
≠ 0 i wtedy elementy u
31
i u
32
nie są jednocześnie
równe zeru. PoniewaŜ c
θ
5
= u
33
stąd
2
33
5
1
s
u
−
±
=
θ
, zatem
−
=
33
2
33
5
,
1
atan2
u
u
θ
lub
12
−
−
=
33
2
33
5
,
1
atan2
u
u
θ
Dla s
θ
5
>
0 otrzymujemy:
(
)
13
23
4
,
atan2
u
u
=
θ
(
)
31
32
6
,
atan2
u
u
−
=
θ
Dla s
θ
5
<
0 otrzymujemy:
(
)
13
23
4
,
atan2
u
u
−
−
=
θ
(
)
31
32
6
,
atan2
u
u
−
=
θ
2) Jeśli u
13
i u
23
= 0 (s
θ
5
=
0) wówczas u
33
= ±1 i u
31
= u
32
= 0. Dla tego przypadku
±
=
1
0
0
0
0
22
21
12
11
u
u
u
u
U
Dla u
33
=
1 → c
θ
5
= 1 i s
θ
5
= 0 więc
θ
5
= 0. W tym przypadku (11) przyjmuje postać:
=
+
+
+
−
+
=
+
−
+
−
−
−
1
0
0
0
0
1
0
0
0
)
c(
)
s(
0
)
s(
)
c(
1
0
0
0
c
c
s
s
s
c
c
s
0
c
s
s
c
s
s
c
c
22
21
12
11
6
4
6
4
6
4
6
4
6
4
6
4
6
4
6
4
6
4
6
4
6
4
6
4
u
u
u
u
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
Sumę
θ
4
+
θ
6
moŜna wyznaczyć z:
(
)
(
)
11
,
12
11
21
6
4
atan2
,
atan2
u
u
u
u
−
=
=
+
θ
θ
(12)
Jeśli
θ
5
= 0 osie złącz 3 i 5 są współosiowe (rys. 11). Jest to konfiguracja osobliwa i
moŜna wyznaczyć jedynie sumę
θ
4
+
θ
6
. Dla tego przypadku istnieje nieskończenie
wiele rozwiązań. Jednym z moŜliwych działań w takiej sytuacji jest wybranie dowolnej
wartości kąta
θ
4
i wyznaczenie
θ
6
z (12).
13
Rys. 11. Kiść sferyczna
Dla u
33
=
– 1 → c
θ
5
= – 1 i s
θ
5
= 0 więc
θ
5
=
π
. W tym przypadku (11) przyjmuje
postać:
−
=
−
−
−
−
−
−
−
1
0
0
0
0
1
0
0
0
)
c(
)
s(
0
)
s(
)
c(
22
21
12
11
6
4
6
4
6
4
6
4
u
u
u
u
θ
θ
θ
θ
θ
θ
θ
θ
RóŜnicę
θ
4
-
θ
6
moŜna wyznaczyć z:
(
)
(
)
22
21
11
12
6
4
,
atan2
,
atan2
u
u
u
u
=
−
−
=
−
θ
θ
RównieŜ dla tego przypadku uzyskuje się nieskończenie wiele rozwiązań.
Literatura:
[1] Craig J. J.: Wprowadzenie do robotyki. Mechanika i sterowanie, Wydawnictwa
Naukowo-Techniczne, 1995
[2] Jezierski E.: Dynamika robotów, Wydawnictwa Naukowo-Techniczne, 2006
[3] Spong M. W., Vidyasagar M.: Dynamika i sterowanie robotów, Wydawnictwa
Naukowo-Techniczne, 1997
Informacja o prawach autorskich
O ile nie zaznaczono inaczej, rysunki i teksty pochodzą z pozycji podanych w literaturze.
Niniejsze opracowanie stanowi pomoc do wykładu „Podstawy Robotyki”.
X
6
Z
6
,
,
,
5
4
3
X
X
X
Z
Z
5
3
,
Z
4
6
ε