Kinematyka odwrotna pozycji i
Kinematyka odwrotna pozycji i
orientacji
orientacji
Sterowanie i
Sterowanie i
Programowanie
Programowanie
Robotów
Robotów
Wykład nr 03 i 04
Wykład nr 03 i 04
Wyższa Szkoła
Wyższa Szkoła
Mechatroniki
Mechatroniki
w Katowicach
w Katowicach
dr inż. Tomasz Trawiński
Problem
Problem
• Należy znaleźć wartości zmiennych przegubowych w
zależności od pozycji i orientacji końcówki roboczej
?
?
?
?
?
?
?
?
To jest dane „z
góry”
To należy
obliczyć
To należy
obliczyć
z
z
4
4
x
x
4
4
y
y
4
4
1
1
2
2
d
d
3
3
4
4
Dana jest macierz jednorodna
Dana jest macierz jednorodna
• Należy znaleźć wszystkie rozwiązania równania macierzowego:
1
0
d
R
H
H
)
,
,
,
(
2
1
0
n
n
q
q
q
T
Gdzie:
n
n
n
q
q
q
T
A
A
1
2
1
0
)
,
,
,
(
Zadana z góry
Rozwiązaniem poniższego równania
Rozwiązaniem poniższego równania
• Jest 16 nieliniowych
równań z „n”
niewiadomymi:
H
)
,
,
,
(
2
1
0
n
n
q
q
q
T
Gdzie:
1,2,3,4
j
;
4
,
3
,
2
,
1
i
ij
n
ij
h
q
q
q
T
)
,
,
,
(
2
1
1
0
0
0
33
32
31
23
22
21
13
12
11
0
z
y
x
n
d
r
r
r
d
r
r
r
d
r
r
r
T
ale
• 12 nietrywialnych i
nieliniowych
równań
• 4 trywialne
równania
Przykład 1. Manipulator RRR - v1
Przykład 1. Manipulator RRR - v1
• Oraz manipulator, którego tablica parametrów kinematycznych:
1
0
0
0
33
32
31
23
22
21
13
12
11
z
y
x
d
r
r
r
d
r
r
r
d
r
r
r
H
Czło
Czło
n
n
a
a
i
i
i
i
d
d
i
i
i
i
1
1
0
/2
d
1
1
2
2
a
2
0
0
2
3
3
a
3
0
0
3
• Dana jest macierz jednorodna H
– opisująca jednoznacznie
pozycję i orientację końcówki
manipulatora względem układu
bazowego. Wybraliśmy jeden
punkt w przestrzeni
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Formułując równanie typu: , otrzymujemy:
1) cos(Q2+Q3)*cos(Q1)=r11,
2) -sin(Q2+Q3)*cos(Q1)=r12,
3) sin(Q1)=r13,
4) -cos(Q1)=r23,
5) cos(Q2+Q3)*sin(Q1)=r21,
6) -sin(Q2+Q3)*sin(Q1)=r22,
7) sin(Q2+Q3)=r31,
8) cos(Q2+Q3)=r32,
9) 0=r33,
10)
(cos(Q2+Q3)*a3+cos(Q2)*a2)*cos(Q1)=d
x
11)
(cos(Q2+Q3)*a3+cos(Q2)*a2)*sin(Q1)=d
y
12)
a3*sin(Q2+Q3)+sin(Q2)*a2+d1=dz
H
)
,
,
,
(
2
1
0
n
n
q
q
q
T
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z równania 1) i 2) :
1)
cos(Q2+Q3)*cos(Q1)=r11,
2)
-sin(Q2+Q3)*cos(Q1)=r12
,
Q2+Q3=atan(-r12/r11)
otrzymujem
y
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z równania 3) i 4) :
3) sin(Q1)=r13,
4) -cos(Q1)=r23,
Q1=atan(r13/(-r23))
otrzymujem
y
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z równania 5) i 6) :
5)
cos(Q2+Q3)*sin(Q1)=r21,
6)
-sin(Q2+Q3)*sin(Q1)=r22,
Q2+Q3=atan(-r22/r21)
otrzymujem
y
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z równania 7) i 8) :
7) sin(Q2+Q3)=r31,
8) cos(Q2+Q3)=r32,
Q2+Q3=atan(r31/r32)
otrzymujem
y
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z równania 9) :
9) 0=r33,
Informuje o tym, że osie „z”
bazowego układu oraz końcówki
manipulatora są do siebie zawsze
prostopadłe (ale nie muszą leżeć
na wspólnej płaszczyźnie).
otrzymujem
y
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z równań 10), 11) i 12) :
10)
(cos(Q2+Q3)*a3+cos(Q2)*a2)*cos(Q1)
=dx
11)
(cos(Q2+Q3)*a3+cos(Q2)*a2)*sin(Q1)=
dy
12)
a3*sin(Q2+Q3)+sin(Q2)*a2+d1=dz
cos(Q2) = -
(dx/r23+r32*a3)/a2
cos(Q2) = (dy/r13-
r32*a3)/a2
sin(Q2) = (dz-d1-
a3*r31)/a2
Po podstawieniach 3), 4), 7) i
8):
Ale !
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z poniższych równań:
cos(Q2) = -
(dx/r23+r32*a3)/a2
cos(Q2) = (dy/r13-
r32*a3)/a2
otrzymujemy:
dy/dx=r13/(-r23)
Q1=atan(r13/(-r23))
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z rozwiązań równań wyjściowych 5), 6), 7) i 8), tworzymy układ równań:
Q2+Q3=atan2(-r22/r21)
Q2+Q3=atan2(r31/r32)
Otrzymujemy rozwiązanie tożsamościowe
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1. Manipulator RRR -v1
Przykład 1. Manipulator RRR -v1
• Z rozwiązania równania 12) tworzymy:
Q2+Q3=atan(-r22/r21)
Podstawiamy do jednego z rozwiązań równań
5), 6) lub 7), 8),
Q2 = asin((dz-d1-a3*r31)/a2)
Q3=atan(r31/r32)-asin((dz-d1-
a3*r31)/a2)
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Q2+Q3=atan2(r31/r32)
Przykład 1.
Przykład 1.
• Równania kinematyki odwrotnej w formie:
Q1=atan(-r13/r23)
Q2 = asin((dz-d1-a3*r31)/a2)
Q3=atan(r31/r32)-asin((dz-d1-
a3*r31)/a2)
)
,
,
,
(
34
12
11
h
h
h
f
q
k
k
• Mają postać:
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Przykład 1.
Przykład 1.
• Sprawdzenie poprawności wyników:
Q1=atan(r13/(-r23))
Q2=asin((dz-d1-a3*r31)/a2)
Q3=atan(r31/r32)-asin((dz-d1-
a3*r31)/a2)
Macierz
T30
H=subs( T30, { 'Q3','Q2','Q1‘ } ,{ '-pi/2','pi/2','0‘ } );
Zadajemy
{ 'Q3','Q2','Q1‘ }
Otrzymujemy
{ 'Q3','Q2','Q1‘ }
Porównujemy
1
1
2
2
3
3
Przykład 1.
Przykład 1.
• Sprawdzenie poprawności wyników dla poniższych konfiguracji:
H=subs( T30, { 'Q1','Q2','Q3‘ } ,{ ‘0',‘0','0‘ } );
Q1=atan(r13/(-r23))
Q2=asin((dz-d1-
a3*r31)/a2)
Q3=atan(r31/r32)-
asin((dz-d1-
a3*r31)/a2)
H=subs( T30, { 'Q1','Q2','Q3‘ } ,{ ‘0’,‘pi/2',‘-pi/2‘ } );
H=subs( T30, { 'Q1','Q2','Q3‘ } ,{ ‘0',‘pi/2','0‘ } );
{ 'Q1','Q2','Q3‘ } ={‘0',‘0','0‘ }
{ 'Q1','Q2','Q3‘ } = { ‘0',‘pi/2',‘-pi/2‘ }
{ 'Q1','Q2','Q3‘ } = { ‘0',‘pi/2','0‘ }
Przykład 1.
Przykład 1.
• Sprawdzenie:
syms Q1 Q2 Q3 a2 a3 d1
T30 =[ cos(Q2+Q3)*cos(Q1), -sin(Q2+Q3)*cos(Q1), sin(Q1),
(cos(Q2+Q3)*a3+cos(Q2)*a2)*cos(Q1);...
cos(Q2+Q3)*sin(Q1), -sin(Q2+Q3)*sin(Q1), -cos(Q1),
(cos(Q2+Q3)*a3+cos(Q2)*a2)*sin(Q1);...
sin(Q2+Q3), cos(Q2+Q3), 0,
a3*sin(Q2+Q3)+sin(Q2)*a2+d1;...
0, 0, 0,
1]
TT=subs(T30,{'Q3','Q2','Q1'},{'-pi/2','pi/2','0'});
wynik=[atan(TT(3,1)/TT(3,2))-asin((TT(3,4)-d1-a3*TT(3,1))/a2), asin((TT(3,4)-d1-a3*TT(3,1))/a2),
atan(TT(1,3)/(-TT(2,3)))]
clear wyniki
wyniki=[0 0 0 0 0 0]
i=0;
for QQ1=0:pi/6:pi/2,
i=i+1
for QQ2=-pi/2:pi/6:pi/2,
for QQ3=-pi/2:pi/6:pi/2,
TT=subs(T30,{'Q3','Q2','Q1'},{QQ3,QQ2,QQ1});
wynik=[atan(TT(3,1)/TT(3,2))-asin((TT(3,4)-d1-a3*TT(3,1))/a2), asin((TT(3,4)-d1-a3*TT(3,1))/a2),
atan(TT(1,3)/(-TT(2,3)))];
wyniki=[[wyniki];[QQ3 QQ2 QQ1],[wynik]];
end
end
end; wyniki
Przykład 1.
Przykład 1.
• Sprawdzenie - Simulink:
Przykład 1.
Przykład 1.
• Sprawdzenie - Simulink:
Pierwszy wiersz macierzy H
(jego pierwsze 3 elementy)
Drugi wiersz macierzy H
(jego pierwsze 3 elementy)
Trzeci wiersz macierzy H
(jego pierwsze 3 elementy)
Czwarta kolumna macierzy H
(jej pierwsze 3 elementy)
Uwaga: zmieniono nazwy zmiennych na „u(i)”
Przykład 1.
Przykład 1.
• Sprawdzenie - Simulink:
Równania kinematyki odwrotnej
Q1=atan(r13/(-r23))
Q2=asin((dz-d1-a3*r31)/a2)
Q3=atan(r31/r32)-asin((dz-d1-
a3*r31)/a2)
Uwaga: zmieniono nazwy zmiennych na „u(i)”
Przykład 2. Manipulator RR
Przykład 2. Manipulator RR
• Oraz manipulator, którego tablica parametrów kinematycznych:
1
0
0
0
33
32
31
23
22
21
13
12
11
z
y
x
d
r
r
r
d
r
r
r
d
r
r
r
H
• Dana jest macierz jednorodna H
– opisująca jednoznacznie
pozycję i orientację końcówki
manipulatora względem układu
bazowego. Wybraliśmy jeden
punkt w przestrzeni
Czło
Czło
n
n
a
a
i
i
i
i
d
d
i
i
i
i
1
1
a
1
/2
0
1
2
2
a
2
0
0
2
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Przykład 2.
Przykład 2.
1) cos(Q1+Q2) = r11,
2) -sin(Q1+Q2) = r12,
3) sin(Q1+Q2) = r21,
4) cos(Q1+Q2) = r22,
5) a2*cos(Q1+Q2)+cos(Q1)*a1 =
dx
6) a2*sin(Q1+Q2)+sin(Q1)*a1 =
dy
H
)
,
,
,
(
2
1
0
n
n
q
q
q
T
• Formułując równanie typu: , otrzymujemy:
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Przykład 2.
Przykład 2.
1) cos(Q1+Q2) = r11,
5) a2*cos(Q1+Q2)+cos(Q1)*a1 = dx
• Do równania 5) podstawiamy równanie 1), czyli:
otrzymujemy
Q1 = acos((dx-a2*r11)/a1)
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Przykład 2.
Przykład 2.
3) sin(Q1+Q2)=r21,
• Na przykład z równania 3) wyliczamy Q1 i Q2:
otrzymujemy
Q2=asin(r21)-Q1
podstawiając Q1
Q2=asin(r21)-acos((dx-a2*r11)/a1)
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Przykład 2.
Przykład 2.
• Równania kinematyki odwrotnej w formie:
)
,
,
,
(
34
12
11
h
h
h
f
q
k
k
• Mają postać:
Q2=asin(r21)-acos((dx-a2*r11)/a1)
Q1=acos((dx-a2*r11)/a1)
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Przykład 2.
Przykład 2.
• Sprawdzenie:
Q1=acos((dx-a2*r11)/a1)
Q2=asin(r21)-acos((dx-a2*r11)/a1)
Macierz
T20
H=subs( T20, { 'Q2','Q1' } ,{ '?', '?' } );
Zadajemy
{ 'Q2','Q1' }
Otrzymujemy
{ 'Q2','Q1‘ }
Porównujemy
Przykład 2.
Przykład 2.
• Sprawdzenie:
syms Q1 Q2 a1 a2
T20 =[ cos(Q1+Q2), -sin(Q1+Q2), 0, a2*cos(Q1+Q2)+cos(Q1)*a1;…
sin(Q1+Q2), cos(Q1+Q2), 0, a2*sin(Q1+Q2)+sin(Q1)*a1;…
0, 0, 1, 0;…
0, 0, 0, 1]
TT=subs(T20,{'Q2','Q1'},{'pi/2','0'});
wynik=[asin(TT(2,1))-acos((TT(1,4)-a2*TT(1,1))/a1), acos((TT(1,4)-a2*TT(1,1))/a1)]
Przykład 2.
Przykład 2.
• Sprawdzenie - Simulink:
Podczas rozwiązania zadania kinematyki odwrotnej
Podczas rozwiązania zadania kinematyki odwrotnej
jesteśmy raczej zainteresowani rozwiązaniem typu:
jesteśmy raczej zainteresowani rozwiązaniem typu:
• Ponieważ:
– Zadanie kinematyki odwrotnej może w ogóle nie mieć rozwiązania,
– Jeśli rozwiązanie istnieje to może być niejednoznaczne,
– Jeśli rozwiązanie istnieje to może być trudne do otrzymania, ze
względu na nieliniowe związki pomiędzy zmiennymi przegubowymi,
– Metody iteracyjne znajdowania rozwiązań kinematyki odwrotnej są
wolne lub bardzo wolne,
– Mając rozwiązanie w postaci zamkniętej (wzór powyższy) możemy
dla określonego zadania, wybrać gotowy zbiór zmiennych
przegubowych do nastawienia
)
,
,
,
(
34
12
11
h
h
h
f
q
k
k
n
k
,
,
1
Odsprzężenie kinematyczne
Odsprzężenie kinematyczne
• Ma zastosowanie dla manipulatorów o 6 stopniach swobody.
• Trzy ostatnie osie z przecinają się w jednym punkcie możliwe jest
odsprzężenie powodujące podział kinematyki odwrotnej na dwa zadania:
Kinematyki
odwrotnej pozycji
Kinematyki
odwrotnej orientacji
Manipulator Stanfordzki
Manipulator Stanfordzki
z
z
0
0
z
z
1
1
z
z
2
2
z
z
3
3
z
z
4
4
z
z
5
5
z
z
6
6
d
d
3
3
0
0
d
d
6
6
p
k
=d
3
0
-d
6
Rk
Macierz opisująca
orientację środka
chwytaka w
bazowym układzie
współrzędnych
Wersor
osi z6
p
kx
=d
x
-d
6
r
13
p
ky
=d
y
-d
6
r
23
p
kz
=d
z
-d
6
r
33
Współrzędne
środka kiści
(w bazowym
układzie
współrzędnyc
h
Manipulator Stanfordzki
Manipulator Stanfordzki
z
z
0
0
z
z
1
1
z
z
2
2
z
z
3
3
z
z
4
4
z
z
5
5
z
z
6
6
d
d
3
3
0
0
d
d
6
6
p
k
=d
3
0
-d
6
Rk
Wyznaczyć z tych równań zmienne przegubowe, tzn.:
1
,
2
, d
3
Obliczyć macierz R
3
0
reprezentującą orientację środka kiści.
Manipulator Stanfordzki
Manipulator Stanfordzki
z
z
0
0
z
z
1
1
z
z
2
2
z
z
3
3
z
z
4
4
z
z
5
5
z
z
6
6
d
d
3
3
0
0
d
d
6
6
R=R
3
0
R
6
3
to z powyższego obliczamy nieznaną macierz orientacji
środka chwytaka względem środka kiści:
Z macierzy R
6
3
obliczyć kąty:
4
,
5
,
6
Pamiętając że orientacja środka chwytaka:
R
6
3
=(R
3
0
)
T
R