Podstawy robotyki
Wykład IV
Kinematyka odwrotna manipulatora
Robert Muszyński
Janusz Jakubiak
Instytut Informatyki, Automatyki i Robotyki
Politechnika Wrocławska
Podstawy robotyki – wykład IV
Podstawy robotyki
Wykład IV
Kinematyka odwrotna manipulatora
Robert Muszyński
Janusz Jakubiak
Instytut Informatyki, Automatyki i Robotyki
Politechnika Wrocławska
Podstawy robotyki – wykład IV
Odwrotne zadanie kinematyki
Przy kinematyce prostej danej jako
K (q) = A
n
0
(q) =
n
Y
i =1
A
i
(q
i
) =
"
R
n
0
(q)
d
n
0
(q)
0
1
#
odwrotne zadanie kinematyki (OZK):
Dla danej krzywej (trajektorii zewnętrznej efektora) y (t) ∈
SE (3) (R
6
) kawałkami gładkiej dla t ∈ [t
0
, t
1
], znaleźć krzywą
(trajektorię wewnętrzną) q(t), która realizuje y (t), tzn.
y (t) = k(q(t)),
t
0
¬ t ¬ t
1
Podstawy robotyki – wykład IV
Metody rozwiązania OZK
I
metoda bezpośrednia
I
rozwiązanie OZK przez odsprzężenie kinematyczne
1
I
metoda geometryczna
I
metoda jakobianowa
1
stosowalna dla manipulatorów, których osie trzech ostatnich przegubów prze-
cinają się w jednym punkcie
Podstawy robotyki – wykład IV
Metoda bezpośrednia rozwiązania OZK
T
n
0
= A
1
A
2
· · · A
n
A
−1
1
T
n
0
= A
2
A
3
· · · A
n
A
−1
2
A
−1
1
T
n
0
= A
3
A
4
· · · A
n
· · ·
A
−1
n−1
· · · A
−1
1
T
n
0
= A
n
n×12
Podstawy robotyki – wykład IV
Rozwiązanie przez odsprzężenie kinematyczne 1
z
0
y
0
d
6
z
4
z
6
x
0
...
Dane:
R
6
0
(q
1
, . . . , q
6
) = R
d
6
0
(q
1
, . . . , q
6
) = d
Podstawy robotyki – wykład IV
Rozwiązanie przez odsprzężenie kinematyczne 2
z
0
y
0
d
6
z
4
z
6
x
0
...
Dane:
R
6
0
(q
1
, . . . , q
6
) = R
d
6
0
(q
1
, . . . , q
6
) = d
1.
Wyznaczyć q
1
, q
2
, q
3
tak aby
d
4
0
= d − d
6
Rk
k = (0, 0, 1)
T
2.
Policzyć na podstawie q
1
, q
2
, q
3
macierz
R
3
0
= R
1
0
R
2
1
R
3
2
3.
Wyznaczyć q
4
, q
5
, q
6
korzystając z zależności
R
6
3
= (R
3
0
)
−1
R
6
0
= (R
3
0
)
T
R
Podstawy robotyki – wykład IV
Metoda geometryczna rozwiązania OZK 1
z
0
z
1
z
2
y
0
(d
x
,d
y
,d
z
)
T
d
x
d
y
x
0
r
Podstawy robotyki – wykład IV
Metoda geometryczna rozwiązania OZK 2
z
0
z
1
z
2
y
0
(d
x
,d
y
,d
z
)
T
d
x
d
y
x
0
r
y
0
d
x
d
y
x
0
r
q
1
= atan2(d
y
, d
x
)
z
0
d
z
r
d
x
2
+d
y
2
podwójne wahadło
Podstawy robotyki – wykład IV
Metoda jakobianowa rozwiązania OZK – wstęp
Dla kinematyki danej równaniem:
y = k(q),
y ∈ Y, q ∈ Q,
przy dim Q = dim Y = n, warunkiem koniecznym istnienia
gładkiego k
−1
(y ) jest nieosobliwość macierzy Jacobiego
∂k
∂q
kinematyki k
rank
2
∂k
∂q
= n.
2
gdzie „rank” oznacza rząd macierzy
Podstawy robotyki – wykład IV
Algorytm Newtona w zarysie
Rozwijając kinematykę w szereg Taylora wokół q
0
y = k(q
0
+ δq) = k(q
0
) +
∂k
∂q
(q
0
)δq + o
2
(δq),
q = q
0
+ δq i pomijając składniki wyższego rzędu
3
dostajemy
algorytm Newtona rozwiązania OZK:
y − k(q
0
) =
∂k
∂q
(q
0
)δq =⇒ δq =
∂k
∂q
(q
0
)
−1
(y − k(q
0
))
Wersja dyskretna algorytmu Newtona:
q
k+1
− q
k
=
∂k
∂q
(q
k
)
−1
(y − k(q
k
))
q
k+1
= q
k
+
∂k
∂q
(q
k
)
−1
(y − k(q
k
))
3
przy założeniu, że δq jest dostatecznie mała
Podstawy robotyki – wykład IV