t 0 - elka.pw

1
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Jakobiany
Kinematykę we współrzędnych możemy potraktować jako operator przekształcający funkcje czasu
(∀ t > 0) z(t) = k(x(t))
Ponieważ funkcje w powyższym równaniu są różniczkowalne, to
ż(t) =
∂k
(x(t))ẋ(t)
∂x
zwane równaniem kinematyki różniczkowej (kinematyki na prędkości).
Macierz Jacobiego (w robotyce zwana jakobianem)
J A(x) =
∂k
(x)
∂x
nazywamy jakobianem analitycznym manipulatora. Równanie kinematyki różniczkowej można zapisać
jako
ż = J A(x)ẋ
Postać jakobianu analitycznego zależy od wyboru układu współrzędnych (parametryzacji) dla określenia
odwzorowania k.
2
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Przykład 1:
Jakobian analityczny dla płaskiego manipulatora 3R. Kinematyka tego manipulatora we współrzędnych ma
postać


 
L1 c1 + L2 c12 
z1 


 










z = z2 =  L1s1 + L2s12 
z3
x1 + x2 + x3


Bezpośrednio różniczkując prawą stronę powyższego równania obliczamy jakobian analityczny dla tego
manipulatora


−L1 s1 − L2 s12 −L2 s12 0


J A(x) =  L1c1 + L2c12



1

L2c12 0
1


1
3
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Jakobian manipulatora można zdefiniować bez odwoływania się do układów współrzędnych. Wprowadza
się pojęcie jakobianu geometrycznego rozumianego jako przekształcenie prędkości ruchu w przegubach w
wektor prędkości końcówki manipulatora w przestrzeni






ν 
ω
= J(q)q̇


gdzie ν = [ν1, ν2, ν3]T – wektor prędkości liniowej, ω = [ω1, ω2, ω3]T – wektor prędkości kątowej końcówki,
a q = [q1, . . . , qn]T – wektor współrzędnych konfiguracyjnych.
Występuje, ponadto pojęcie jakobianu manipulatora rozumianego jako przekształcenie prędkości ruchu w
przegubach w wektor prędkości końcówki manipulatora w układzie bazowym robota


0 
ṗ

n


ω


= J M (q)q̇
Związek między jakobianem geometrycznym a jakobianem manipulatora jest następujący
J M ((q)) =

gdzie [p×] =








0
0
n pz
−0npy
−0npz
0
0
n px

I3




0

0
n [p×]



I3
J(q)

0
n py 


0 
−npx

jest macierzą skośniesymetryczną stowarzyszoną z wektorem 0np, który
0
opisuje położenie końcówki w układzie bazowym.

4
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Przestrzeń robocza manipulatora
Przestrzeń robocza manipulatora jest to zbiór punktów, które końcówka robocza może osiągnąć. Aby
istniało rozwiązanie OZK zadany punkt docelowy dla końcówki musi należeć do przestrzeni roboczej.
Można wyróżnić dwa rodzaje przestrzeni roboczej:
• manipulacyjna przestrzeń robocza – jest to część przestrzeni roboczej, którą końcówka robocza
może osiągnąć z dowolną orientacją,
• osiągalna przestrzeń robocza – jest to część przestrzeni roboczej, którą końcówka robocza może
osiągnąć z co najmniej jedną orientacją.
Przykład 2: Przestrzeń robocza dla płaskiego manipulatora 2R:
q2
y0
L1+L2
L2
L1
q1
x0
|L1-L2|
5
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Odwrotne zadanie kinematyki (OZK) manipulatora
Odwrotne zadanie kinematyki: Mając dane położenie i orientację efektora manipulatora w układzie bazowym
należy znaleźć odpowiadające im współrzędne przegubowe (konfiguracyjne).
Dla danego manipulatora o n stopniach swobody z przestrzenią przegubową Q oraz przestrzenią zadaniową
Z ∈ SE(3) odwrotne zadanie kinematyki można wyrazić symbolicznie jako
k −1 : SE(3) ⊃ Z ∋ z → q = k −1(z) ∈ Q
(1)
Jednakże, odwzorowanie odwrotne k −1 może nie istnieć lub być niejednoznacznie określone.
Rozwiązanie odwrotnego zadania kinematyki:
Dane są wartości liczbowe elementów macierzy przekształcenia jednorodnego H
H=






R d 
0 1


opisujących położenie i orientację końcówki w układzie bazowym związanym z nieruchomą podstawą robota.
Należy znaleźć rozwiązanie (jedno lub wszystkie) równania macierzowego
0
n T (q1 , . . . , qn )
=H
(2)
równanie to daje 12 nieliniowych równań z n niewiadomymi
tij (q1, . . . , qn) = hij ;
i = 1, 2, 3; j = 1, 2, 3, 4
gdzie tij oraz hij 12 nietrywialnych elementów macierzy 0nT i H.
6
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Wartość ai Liczba rozwiązań
a1 = a3 = a5 = 0
þ4
a3 = a5 = 0
þ8
v a3 = 0
þ 16
∀ ai Ó= 0
þ 16
Istnienie rozwiązania OZK:
• Dla robota o 6 stopniach swobody mamy 6 niewiadomych oraz 12 równań. Jednak tylko 3 równania z
9 wynikających z porównania elementów macierzy orientacji jest niezależnych, mamy zatem układ 6
równań z 6 niewiadomymi.
• Równania te są nieliniowe i przestępne.
• Brak jest ogólnych metod rozwiązywania układów równań nieliniowych.
• Może istnieć wiele rozwiązań dla danego położenia i orientacji (rozwiązania wielokrotne). W szczególnych
przypadkach (w punktach osobliwych) może być nieskończenie wiele rozwiązań.
• Liczba rozwiązań zależy od liczby przegubów, wartości parametrów D-H i dopuszczalnych zakresów
ruchu w przegubach.
• Na ogół im więcej niezerowych parametrów D-H tym więcej rozwiązań. Przykładowo dla robota 6R
zależność liczby rozwiązań od parametru ai jest następująca:
7
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Strategie rozwiązywania OZK:
• rozwiązania w postaci jawnej
• rozwiązania numeryczne (iteracyjne)
Wyróżnia się dwa podstawowe podejścia do rozwiązania OZK w postaci jawnej:
• metody algebraiczne
• metody geometryczne
Przykład 3: Rozwiązać OZK dla płaskiego manipulatora trójczłonowego pokazanego na rys.1.
y3
y2
y0
y1
q1
x3
q3
q2 L2
x2
x1
L1
x0
{0}
Rysunek 1: Manipulator płaski typu 3R
8
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Rozwiązanie PZK dla manipulatora płaskiego typu 3R:

0
3T
=













c123 −s123 0 L1c1 + L2c12
s123 c123 0 L1s1 + L2s12
0
0
1
0
0
0
0
1














(3)
Rozwiązanie algebraiczne OZK
Zadaną pozycję końcówki manipulatora można zapisać w postaci macierzy

H=














cϕ −sϕ 0 x 
sϕ cϕ 0 y
0
0
1 0
0
0
0 1











(4)
Porównując prawe strony równań (3) i (4) otrzymujemy układ 4 równań z 3 niewiadomymi θ1, θ2, θ3:
cϕ = c123
sϕ = s123
x = L1c1 + L2c12
y = L1s1 + L2s12
(5)
(6)
(7)
(8)
9
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Podnosimy do kwadratu równania (7) i (8) i dodajemy stronami
x2 + y 2 = L21 + L22 + 2L1L2c2
(9)
stąd
x2 + y 2 − L21 − L22
(10)
2L1L2
Prawa strona równania (10) musi zawierać się w przedziale [−1, 1], aby punkt docelowy należał do przestrzeni
roboczej, wtedy
ò
s2 = ± 1 − c22
(11)
c2 =
Kąt θ2 obliczamy jako
θ2 = atan2(s2, c2)
(12)
Mając obliczone θ2 można rozwiązać (7) i (8) względem θ1:
x = m1 c1 − m 2 s1
y = m1 s1 + m2 c1
(13)
(14)
gdzie
m 1 = L1 + L2 c 2
m 2 = L2 s 2
10
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
Dokonujemy zamiany zmiennych
ò
r = + m21 + m22
γ = atan2(m2, m1)
to m1 = r cos γ, m2 = r sin γ. Równania (13) i (14) można zapisać jako
x
= cos γ cos θ1 − sin γ sin θ1
r
y
= cos γ sin θ1 + sin γ cos θ1
r
lub
x
cos(γ + θ1) =
r
y
sin(γ + θ1) =
r
stąd
y x
γ + θ1 = atan2( , ) = atan2(y, x)
r r
czyli
θ1 = atan2(y, x) − atan2(m2, m1)
Jeśli x = y = 0 to (22) staje się nieokreślone i θ1 może przyjąć dowolną wartość.
Ostatecznie z (5) i (6) można znaleźć
θ1 + θ2 + θ3 = atan2(sϕ, cϕ) = ϕ
(15)
(16)
(17)
(18)
(19)
(20)
(21)
(22)
11
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
stąd
θ 3 = ϕ − θ1 − θ 2
(23)
Rozwiązanie geometryczne:
y0
q2
y
L2 O3
a
L1
b
y
O1
{0}
x
x0
Rysunek 2: Rozwiązanie geometryczne OZK dla płaskiego manipulatora 3R
Kąt α = π − (−θ2) = π + θ2. Z twierdzenia kosinusów obliczamy θ2
x2 + y 2 = L21 + L22 − 2L1L2 cos(π + θ2)
ponieważ cos(π + θ2) = − cos θ2, mamy
cos θ2 = c2 =
x2 + y 2 − L21 − L22
2L1L2
12
c
Wstęp do Robotyki – ¥W.
Szynkiewicz, 2009
√
s2 = ± 1 − c2
stąd θ2
θ2 = atan2(s2, c2)
′
Drugie rozwiązanie jest dla θ2 = −θ2.
Aby wyznaczyć θ1 należy wyznaczyć kąty β i ψ
β = atan2(y, x)
Stosując powtórnie twierdzenie kosinusów obliczamy
cos ψ =
x2 + y 2 + L21 − L22
√
2L1 x2 + y 2
gdzie arccos() musi być takie aby dla 0 6 ψ 6 π zachować relacje geometryczne. Kąt θ1





θ1 = 

Kąt θ3 obliczamy


β + ψ gdy θ2 < 0
β − ψ gdy θ2 > 0
θ1 + θ 2 + θ 3 = ϕ ⇒ θ 3 = ϕ − θ1 − θ 2