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