Jakobiany manipulatora. Konfiguracje osobliwe

Podstawy robotyki
Wykład V
Jakobian manipulatora i osobliwości
Robert Muszyński Janusz Jakubiak
Instytut Informatyki, Automatyki i Robotyki
Politechnika Wrocławska
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda bezpośrednia uzyskania macierzy Jacobiego
Jeśli kinematyka manipulatora dana jest równaniem:
y ∈ Y, q ∈ Q,
y = k(q),
wtedy po zróżniczkowaniu względem czasu otrzymujemy
ẏ =
∂k
(q)q̇
∂q
I
wyznaczyć kinematykę manipulatora k(q),
I
wybrać współrzędne,
I
wyrazić kinematykę w wybranych współrzędnych,
I
formalnie zróżniczkować kinematykę k →
Podstawy robotyki – wykład V
∂k
∂q .
Jakobian manipulatora. Osobliwości
Metoda pośrednia uzyskania macierzy Jacobiego
Dla kinematyki danej w postaci
"
T0n
R n d0n
= 0
0
1
#
szukamy zależności prędkośći liniowej i kątowej efektora od
prędkości w poszczególnych przegubach w postaci
ẏ = J q̇,
gdzie J – jakobian manipulatora.
Szukamy jakobianu
"
J=
Jv
Jω
#
Podstawy robotyki – wykład V

n

 v0 = Jv q̇

ω n = J q̇
ω
0
Jakobian manipulatora. Osobliwości
Metoda pośrednia uzyskania macierzy Jacobiego
I
Prędkość liniowa
v0n = ḋ0n
I
Prędkość kątowa
R0n (R0n )T = I
d(R0n )T
dR0n n T
(R0 ) + R0n
=0
dq
dq
Zdefiniujmy
S,
dR0n n T
(R0 ) .
dq
Wówczas
ST =
dR0n n T
(R0 )
dq
Podstawy robotyki – wykład V
T
= R0n
d(R0n )T
.
dq
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – prędkość kątowa
dR0n n T
(R0 )
dq
d(R0n )T
= R0n
dq
S,
ST
Mamy więc
S + ST = 0


0
−ω3 ω2


n
0
−ω1  ,
S(ω0 ) =  ω3
−ω2 ω1
0


ω1
 
n
ω 0 = ω 2  .
ω3
S(ω0n )p = ω0n × p
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – prędkość kątowa
ω
I
przegub rotacyjny
i
ωi−1
= q̇i k,
I
k = [0, 0, 1]T
przegub translacyjny
i
ωi−1
=0
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – prędkość kątowa
Można pokazać, że
n
ω0n = ω01 + R01 ω12 + . . . + R0n−1 ωn−1
.
W naszym przypadku
ω0n = ρ1 q̇1 k + ρ2 q̇2 R01 k + . . . + ρn q̇n R0n−1 k =
n
X
ρi q̇i zi−1 ,
i=1
gdzie zi−1 = R0i−1 k – trzecia kolumna macierzy R0i−1 ,
ρi =
0 – przegub translacyjny
.
1 – przegub rotacyjny
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – prędkość liniowa
Wyliczając różniczkę zupełną ḋ0n otrzymujemy
ḋ0n =
n
X
∂d0n
i=1
∂qi
q̇i .
Zatem w j-tej kolumnie macierzy Jv znajduje się wektor
Przypomnijmy
i
di−1
= Rot(zi−1 , θi )Trans(zi−1 , di )
1

cθi −sθi cαi
 s θi
cθi cαi
Trans(xi−1 , ai )Rot(xi−1 , αi ) = 
0
sαi
0
0
∂d0n
∂qi .
i
Ri−1
0
Podstawy robotyki – wykład V
sθi sαi
−cθi sαi
cαi
0
Jakobian manipulatora. Osobliwości

ai cθi
ai sθi 
.
di 
1
Metoda pośrednia. . . – prędkość liniowa
Stąd
i
i
i
di−1
= di k + ai Ri−1
i = qi k + ai Ri−1
i.
Przy poruszanym tylko i-tym przegubie
i
ḋ0n = R0i−1 ḋi−1
= q̇i R0i−1 k = q̇i zi−1 .
A zatem
∂d0n
= zi−1
∂qi
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – prędkość liniowa
I
przegub rotacyjny
n
d0n = doi−1 + R0i−1 di−1
z0
zi-1
.
. . .
.
.
y0
Ponieważ d0i−1 , R0i−1 niezależna
od qi
n
ḋ0n = R0i−1 ḋi−1
.
x0
Prędkość liniowa powodowana ruchem qi
n
n
ḋi−1
= q̇i k × di−1
.
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – prędkość liniowa
Stąd
n
ḋ0n = R0i−1 (q̇i k × di−1
)=
n
q̇i R0i−1 k × R0i−1 di−1
=
q̇i zi−1 × (d0n − d0i−1 ).
Mamy zatem
∂d0n
= zi−1 × (d0n − d0i−1 )
∂qi
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda pośrednia. . . – podsumowanie
Podsumowując jakobian dany jest w postaci
h
J = J1 J2 . . . Jn
i
gdzie i-ta kolumna
"
#
z
× (d0n − d0i−1 )
Ji = i−1
zi−1
dla przegubu rotacyjnego lub
"
z
Ji = i−1
0
#
dla przegubu translacyjnego.
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Związek pomiędzy jakobianem a macierzą Jacobiego
Po wybraniu współrzędnych w przestrzeni zewnętrznej jako
współrzędne kartezjańskie + kąty orientacji dostajemy
"
#
∂k
I
0
J(q) =
(q)
0 M(y ) ∂q
h
i
gdzie M(y ) = m1 (y ) m2 (y ) m3 (y ) ,
mi (y ) – wektory jednostkowe osi obrotu wokół których mierzone są
kąty orientacji.
Np. dla kątów Eulera


− cos φ ctg θ − sin φ ctg θ 1

cos φ
0
M(y ) =  − sin φ

cos φ
sin φ
0
sin θ
sin θ
określone dla 0 < θ < π.
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda jakobianowa rozwiązania OZK
Mamy dane
ẏ =
∂k
q̇,
∂q
y (t),
t ∈ [t0 , tk ].
(1)
Szukamy q(t) dla t ∈ [t0 , tk ]. W tym celu wyliczamy y (t0 ) i przy
użyciu algorytmu Newtona znajdujemy q(t0 ). Następnie
rozwiązujemy równanie różniczkowe
q̇ =
∂k
∂q
−1
ẏ
z warunkiem początkowym q(t0 ).
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
(2)
Metoda jakobianowa – Inwers uogólniony
Równania (1) są postaci
y(m×1) = A(m×n) q(n×1) .
Przy danym y szukamy q, gdy m ¬ n, rankA = m.
Będziemy szukali rozwiązania szczegółowego minimalizującego
energię
1
H(q) = hq, qi
2
przy ograniczeniach y = Aq. Tworzymy funkcję Lagrange’a
1
1
L(q) = hq, qi+λT (y −Aq) = hq, qi+hλ, y −Aqi.
2
2
Z warunków na minimum mamy
∂L
= q T − λT A = 0
∂q
Podstawy robotyki – wykład V
⇒
q = AT λ.
Jakobian manipulatora. Osobliwości
Metoda jakobianowa – inwers uogólniony
Stąd
y = AAT λ
co prowadzi do
λ = AAT
−1
y.
Wstawiając (3) do wyniku minimalizacji dostajemy
q = AT AAT
−1
y = A+ y .
Macierz
A+ = AT AAT
−1
nazywa się pseudoinwersem Moore’a–Penrose’a.
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
(3)
Metoda jakobianowa – inwers uogólniony
Równania y = Aq będą spełnione, jeżeli do rozwiązania
szczegółowego q = A+ y dodamy element z taki, że Az = 0, co
oznacza, że
z ∈ KerA.
Stąd rozwiązanie ogólne
q = A+ y + z,
z ∈ KerA,
lub przy
z = (I − A+ A)ω,
ω ∈ Rn
ostatecznie
q = A+ y + (I − A+ A)ω,
Podstawy robotyki – wykład V
ω ∈ Rn .
Jakobian manipulatora. Osobliwości
Metoda jakobianowa – inwers uogólniony
Sprawdźmy
+
y = Aq = AA+ y + (A − AA
| {z A})ω = Iy + 0 = y .
A
Skoro ω dowolne, dobierzmy je tak by optymalizować np.
manipulowalność
q
ω = grad −
det(AAT )
.
q
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Metoda jakobianowa rozwiązania OZK. Podsumowanie.
Przy równaniu
y = k(q),
y ∈ Y, q ∈ Q,
dim Y = m, dim Q = n, i danym yd (t) dla t ∈ [t0 , tk ] (lub yd )
znaleźć q(t) (lub q) takie, że (4) spełnione dla yd (t) (lub yd ).
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
(4)
Metoda jakobianowa rozwiązania OZK. Podsumowanie.
I
gdy n = m i macierz Jacobiego nieosobliwa:
q̇(t) =
I
−1
∂k
(q(t))
∂q
ẏ (t),
k(q(t0 )) = y (t0 ),
gdy n ­ m i rank ∂k
∂q (q(t)) = m:
q̇(t) =
+
∂k
(q(t))
∂q
ẏ (t),
k(q(t0 )) = y (t0 ),
lub ogólniej
q̇(t) =
+
∂k
(q(t))
∂q
ẏ (t)+
I−
Podstawy robotyki – wykład V
+
∂k
(q(t))
∂q
!
∂k
(q(t)) ω(t).
∂q
Jakobian manipulatora. Osobliwości
Metoda jakobianowa rozwiązania OZK. Podsumowanie.
Metoda Newtona
I
gdy n = m i macierz Jacobiego nieosobliwa:
qk+1 = qk +
I
−1
∂k
(qk )
∂q
(yd − k(qk )),
gdy n ­ m i rank ∂k
∂q (q(t)) = m:
qk+1 = qk +
+
∂k
(qk )
∂q
=
+
∂k
(qk )
∂q
∂k
∂q
Podstawy robotyki – wykład V
T
(yd − k(qk )),
∂k
∂q
∂k
∂q
T !−1
(qk )
Jakobian manipulatora. Osobliwości
Osobliwości
Macierz Jacobiego kinematyki manipulatora
zależność
ẏ =
∂k
(q)q̇,
∂q
∂k
∂q (q)
określa
y ∈ Y, q ∈ Q, dim Y = m, dim Q = n,
co można zapisać jako
dy =
∂k
(q)dq,
∂q
gdzie różniczki dy i dq określają kierunki w Y i Q. Jeśli więc
rank
∂k
(q) < min(n, m)
∂q
istnieją w Y kierunki, które nie mogą zostać wygenerowane przez
jakiekolwiek kierunki w Q.
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Osobliwości
Definicje
Punktem regularnym kinematyki k nazywamy taką konfigurację
q ∈ Q, że
∂k
rank (q) = min(n, m).
∂q
Punktem osobliwym nazywamy takie q ∈ Q, że
rank
∂k
(q) < min(n, m).
∂q
Niech q ∈ Q będzie konfiguracją osobliwą. Wówczas
dy =
∂k
(q)dq,
∂q
oraz Ker ∂k
∂q (q) 6= 0. Oznacza to, że ∃dq 6= 0 która daje zerową
zmianę dy .
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Osobliwości
W otoczeniu osobliwości:
I
ruch w pewnych kierunkach w Y jest nieosiągalny;
I
pewne ruchy w Q nie powodują ruchu w Y;
I
ograniczone prędkości efektora mogą wymagać
nieograniczonych prędkości w przegubach;
I
może nie istnieć jednoznaczne rozwiązanie OZK;
I
duże siły na efektorze mogą powodować niewielkie siły w
przegubach.
Podstawy robotyki – wykład V
Jakobian manipulatora. Osobliwości
Statyka
Przy zależności
dy =
∂k
(q)dq,
∂q
wirtualna praca układu
dw = F T dy − τ T dq,
gdzie F – wektor sił i momentów na chwytaku, τ – wektor
momentów/sił w przegubach, wyraża się jako
∂k
(q) − τ T dq,
dw = F
∂q
T
co równe jest zeru jeśli manipulator pozostaje w równowadze. Stąd
τ=
T
∂k
(q)
∂q
Podstawy robotyki – wykład V
F,
Jakobian manipulatora. Osobliwości