PATRONI ROBOTYKI:
 

TEORIA ROBOTYKI

 5.5. Kinematyka prosta

Do opisu kinematyki prostej niezbędne jest podanie równań kinematyki robota. Zadanie kinematyki prostej można określić następująco: posiadając dane o zmiennych przegubowych należy określić pozycję i orientację końcówki roboczej. Jak wspomniano wcześniej równania kinematyki mogą zostać wyznaczone wykorzystując metody stosowane w mechanice klasycznej lub wykorzystując notację Denavita-Hartenberga. Poniżej przedstawiono kilka przykładów, w których dokonano analizy zadania prostego kinematyki.

 

Przykład 5.9

http://www.robotyka.com/teoria/grafika/image338.gif

Rysunek 5.22 Manipulator 2-członowy

Wyznaczyć położenie chwytaka w przestrzeni dla manipulatora 2-członowego przedstawionego na rys.5.22. Poszczególne człony wykonują ruch w płaszczyźnie płaskiej. Dana jest długość członów manipulatora oznaczona przez a1 i a(długość a2 jest liczona od punktu B do C). 

Rozwiązanie:

I sposób - Dla prostego manipulatora płaskiego można w sposób klasyczny wyznaczyć położenie punktu C.Przyjęto stały układ, nazywany układem bazowym lub odniesienia, względem, którego rozpatruje się wszystkie obiekty łącznie z manipulatorem. Układ ten został zaczepiony w punkcie O0x0y0 leżącym w podstawie robota. Sposób przyjęcia układu i oznaczenia kątów przedstawiono na rys. 5.23. 

 

http://www.robotyka.com/teoria/grafika/image339.gif

Rysunek 5.23 Manipulator 2-członowy z przyjętym układem współrzędnych

Współrzędne x0,y0 narzędzia w tym układzie współrzędnych zostały wyrażone następującymi wzorami (rzutowanie punktu C na poszczególne osie):

http://www.robotyka.com/teoria/grafika/image340.gif

W przypadku prostych manipulatorów takie podejście do rozwiązania tego typu zadań jest wygodne, jednak w przypadku bardziej złożonych struktur, może okazać się trudne do zastosowania.  

II sposób - Innym sposobem rozwiązania tego zadania jest zastosowanie notacji Denavita-Hartenberga.

http://www.robotyka.com/teoria/grafika/image341.gif

Rysunek 5.24 Manipulator 2-członowy z przyjętym układem współrzędnych

 

W tym celu należy również przyjąć bazowy układ współrzędnych x0,y0,z0 oraz układy współrzędnych związane z każdym członem. Dodatkowo należy zachować zasadę, iż obrót poszczególnego członu odbywa się względem osi z, a przemieszczenia względem osi z i x. Sposób przyjęcia tych układów przedstawiono na rys.5.24.  Należy wspomnieć, iż wszelkie obliczenia związane z konstruowaniem i analizą kinematyki wykonuje się przy użyciu pakietów obliczeniowych takich jak np. MapleTM, MatlabTM. Poniżej obliczeń w tabeli 5.10 przedstawiono zapis funkcji w programie MapleTM umożliwiający rozwiązanie powyższego zadania. 

Pierwszy krok, jaki należy wykonać zgodnie z notacją to przyjęcie układów współrzędnych związanych z każdy członem oraz przygotowanie tabeli (tabela 5.9) z parametrami kinematycznymi, takimi jak to kąty obrotu, przemieszczenia członów oraz długości poszczególnych ramion. Dodatkowo przy tych parametrach które ulegają zmianie znajduje się indeks var oznaczający, iż ten parametr jest zmienny w czasie.   

układ

qi

di

ai

ai

1

q1,var

0

a1

0

2

q2,var

0

a2

0

Tabela 5.1 Parametry kinematyczne dla przykładu 5.9

Analizowany płaski manipulator oczywiście posiada 2 stopnie swobody, co można łatwo określić na podstawie tabeli 5.1. Zawsze należy tak wiązać układy współrzędnych aby w każdym przekształceniu jednorodnym występował tylko jeden parametr zmienny. Następnym krokiem jest zapisanie macierzy przekształcenia jednorodnego dla poszczególnych członów na podstawie równania (5.33) w oparciu o dane zawarte w tabeli 5.1. 

dla układu I

http://www.robotyka.com/teoria/grafika/image342.gif

http://www.robotyka.com/teoria/grafika/image343.gif

dla układu II

http://www.robotyka.com/teoria/grafika/image344.gif

http://www.robotyka.com/teoria/grafika/image345.gif

Końcowym etapem rozwiązania tego typu zadania jest podanie macierzy transformacji układu ostatniego do układu O0 x0 y0, którą zapisano następująco:

http://www.robotyka.com/teoria/grafika/image346.gif

http://www.robotyka.com/teoria/grafika/image347.gif

Dokonując porównania obu wariantów rozwiązania powyższego zadania oczywiście można stwierdzić, iż wyznaczone położenia chwytaka są identyczne (wektor położenia w macierzy T2,0 oraz współrzędne x0,y0 wyznaczone wykorzystując sposób I). Poniżej zapis funkcji w programie Maple.

>     restart:

>     with(linalg):

>     Rot[z,theta[1]]:=matrix(4,4,[cos(theta[1]),-sin(theta[1]),0,0,

sin(theta[1]),cos(theta[1]),0,0,0,0,1,0,0,0,0,1]);

>     Trans[z,a[1]]:=matrix(4,4,[1,0,0,a[1],0,1,0,0,0,0,1,0,0,0,0,1]);

>     A1:=multiply(Rot[z,theta[1]],Trans[z,a[1]]);

>     Rot[z,theta[2]]:=matrix(4,4,[cos(theta[2]),-sin(theta[2]),0,0,

sin(theta[2]),cos(theta[2]),0,0,0,0,1,0,0,0,0,1]);

>     Trans[z,a[2]]:=matrix(4,4,[1,0,0,a[2],0,1,0,0,0,0,1,0,0,0,0,1]);

>     A2:=multiply(Rot[z,theta[2]],Trans[z,a[2]]);

>     T[2,0]:=map(combine,multiply(A1,A2));

 

Przykład 5.10

http://www.robotyka.com/teoria/grafika/image348.gif

Rysunek .25 Manipulator 2-członowy

Wyznaczyć położenie i orientację chwytaka w przestrzeni dla manipulatora 2-członowego przedstawionego na rys. 5.25. Poszczególne człony wykonują ruch w płaszczyźnie płaskiej (pierwszy ruch jest obrotowy, natomiast drugi postępowy). Dana jest długość członów manipulatora oznaczona przez a1 i d3(długość d3 jest mierzona od punktu B doC).

Rozwiązanie:

Wykorzystując notacje Denavita-Hartenberga dąży się do takiego ustawienia współrzędnych oby ruch poszczególnych członów odbywał się zawsze względem osi z. Zgodnie z opisaną notacją w jednym przekształceniu jednorodnym można wykonywać obroty i przesunięcia zgodnie z równaniem (5.33) w kolejności takiej jak zostały zapisane poszczególne macierze w tym równaniu. Bardzo często w jednym przekształceniu nie da się odpowiednio zorientować układu współrzędnych tak, aby oś z była w osi ruchu. Należy wtedy przyjąć następny układ odpowiednio zorientowany względem poprzedniego. Na rys. 5.26 przedstawiono analizowany manipulator z przyjętymi układami współrzędnych.

 

http://www.robotyka.com/teoria/grafika/image349.gif

Rysunek 5.26 Manipulator 2-członowy z przyjętymi układami współrzędnych zgodnie z notacją Denavita-Hartenberga

 

układ

qi

di

ai

ai

1

q1,var

0

a1

0

2

-90°

0

0

-90°

3

0

d3,var

0

0

Tabela 5.2 Parametry kinematyczne dla przykładu 5.10

 Znaki minus w tabeli 5.10 przy kątach 90° wynika z reguły śruby prawoskrętnej.

dla układu I

http://www.robotyka.com/teoria/grafika/image342.gif

http://www.robotyka.com/teoria/grafika/image343.gif

dla układu II

http://www.robotyka.com/teoria/grafika/image350.gif

http://www.robotyka.com/teoria/grafika/image351.gif

dla układu III

http://www.robotyka.com/teoria/grafika/image352.gif

http://www.robotyka.com/teoria/grafika/image353.gif

Macierz transformacji układu T3,0 ma postać:

http://www.robotyka.com/teoria/grafika/image354.gif

http://www.robotyka.com/teoria/grafika/image355.gif

Zgodnie z równaniem (5.30) w powyższej macierzy można wyróżnić orientację i pozycję chwytaka. Poniżej zapis funkcji w programie Maple.

>          restart:

>          with(linalg):

>          Rot[z,theta[1]]:=matrix(4,4,[cos(theta[1]),-sin(theta[1]),0,0,

sin(theta[1]),cos(theta[1]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[z,a[1]]:=matrix(4,4,[1,0,0,a[1],0,1,0,0,0,0,1,0,0,0,0,1]);

>          A1:=multiply(Rot[z,theta[1]],Trans[z,a[1]]);

>          Rot[z,-90]:=matrix(4,4,[cos(-Pi/2),-sin(-Pi/2),0,0,sin(-Pi/2), cos(-Pi/2),0,0,0,0,1,0,0,0,0,1]);

>          Rot[x,-90]:=matrix(4,4,[1,0,0,0,0,cos(-Pi/2),-sin(-Pi/2),0,0, sin(-Pi/2),cos(-Pi/2),0,0,0,0,1]);

>          A2:=multiply(Rot[z,-90],Rot[x,-90]);

>          A3:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[3],0,0,0,1]);

>          T[3,0]:=map(combine,multiply(A1,A2,A3)); 

 

 

Przykład 5.11

http://www.robotyka.com/teoria/grafika/image356.jpg

Rysunek 5.27 Manipulator o strukturze cylindrycznej

Wyznaczyć położenie i orientację chwytaka w przestrzeni dla manipulatora o 4 stopniach swobody (struktura cylindryczna), przedstawionego na rys. 5.27. Dane są poszczególne długości członów manipulatora, zilustrowane na rys. 5.28.

Rozwiązanie:

Dane są długości poszczególnych członów manipulatora odpowiednio: d1, d2, d3, d4, d5, mierząc od podstawy (rys.5.28).

 

http://www.robotyka.com/teoria/grafika/image357.gif

http://www.robotyka.com/teoria/grafika/image358.gif

Rysunek 5.28 Schemat z wymiarami

Rysunek 5.29 Schemat z układami

Należy podkreślić, iż bardzo często istnieje wiele sposobów orientowania układów współrzędnych (zgodnie z notacją Denavita-Hartenberga), jednak celem analizy kinematyki prostej jest przyjęcie najprostszej formy opisu ze względu na zmniejszenie komplikacji związanych z obliczeniami.  W przypadku przedstawionej struktury manipulatora najprostszy sposób przyjęcie układów współrzędnych został przedstawiony na rys. 5.29, natomiast w tabeli 5.3 zamieszczono odpowiednie operacje obrotów i przesunięć.

układ

qi

di

ai

ai

1

q1,var

d1

0

0

2

0

d2,var

0

-90°

3

0

d3,var

0

-90°

4

q4,var

d4+ d5

0

0

Tabela 5.3 Parametry kinematyczne dla przykładu 5.11.

 

dla układu I

http://www.robotyka.com/teoria/grafika/image359.gif

http://www.robotyka.com/teoria/grafika/image360.gif

dla układu II

http://www.robotyka.com/teoria/grafika/image361.gif

http://www.robotyka.com/teoria/grafika/image362.gif

dla układu III

http://www.robotyka.com/teoria/grafika/image363.gif

http://www.robotyka.com/teoria/grafika/image364.gif

dla układu IV

http://www.robotyka.com/teoria/grafika/image365.gif

http://www.robotyka.com/teoria/grafika/image366.gif

tak więc:

http://www.robotyka.com/teoria/grafika/image367.gif

http://www.robotyka.com/teoria/grafika/image368.gif

 

Poniżej przedstawiono kod przygotowany w programie MapleTM

 >         restart:

>          with(linalg):

>          Rot[z,theta[1]]:=matrix(4,4,[cos(theta[1]),-sin(theta[1])

,0,0,sin(theta[1]),cos(theta[1]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[z,d[1]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[1],0,0,0,1]);

>          A[1]:=multiply(Rot[z,theta[1]],Trans[z,d[1]]);

>          Trans[z,d[2]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[2],0,0,0,1]);

>          Rot[x,-90]:=matrix(4,4,[1,0,0,0,0,cos(-Pi/2),-sin(-Pi/2),0,0 ,sin(-Pi/2),cos(-Pi/2),0,0,0,0,1]);

>          A[2]:=multiply(Trans[z,d[2]],Rot[x,-90]);

>          Trans[z,d[3]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[3],0,0,0,1]);

>          A[3]:=multiply(Trans[z,d[3]],Rot[x,-90]);

>          Rot[z,theta[4]]:=matrix(4,4,[cos(theta[4]),-sin(theta[4]) ,0,0,sin(theta[4]),cos(theta[4]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[z,d[4]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[4]+ d[5],0,0, 0,1]);

>          A[4]:=multiply(Rot[z,theta[4]],Trans[z,d[4]]);

>          T[4,0]:=map(combine,multiply(A[1],A[2],A[3],A[4])); 

Przyjęte układy współrzędnych zostały również przedstawione na trójwymiarowym modelu manipulatora, zilustrowanym na rys.5.30. W celu oznaczenia poszczególnych osi wykorzystano odpowiednie oznaczenie ich kolorami: czerwony - oś z, zielony - oś x oraz żółty - oś y.

 

http://www.robotyka.com/teoria/grafika/image369.jpg

http://www.robotyka.com/teoria/grafika/image370.jpg

http://www.robotyka.com/teoria/grafika/image371.jpg

http://www.robotyka.com/teoria/grafika/image372.jpg

http://www.robotyka.com/teoria/grafika/image373.jpg

http://www.robotyka.com/teoria/grafika/image374.jpg

http://www.robotyka.com/teoria/grafika/image375.jpg

Rysunek 5.30 Trójwymiarowe ujęcia podczas ruchu manipulatora z układami

 

Posiadając macierz transformacji T4,0 dla analizowanego manipulatora w postaci symbolicznej można podstawiając wartości parametrów konstrukcyjnych i kinematycznych wyznaczyć położenie i orientację manipulatora w danej chwili czasowej.

 

 

Przykład 5.12

http://www.robotyka.com/teoria/grafika/image376.jpg

Rys.5.31 Manipulator o strukturze SCARA

Wyznaczyć położenie i orientację chwytaka w przestrzeni dla manipulatora o 4 stopniach swobody (struktura SCARA), przedstawionego na rys. 5.31. Dane są poszczególne długości członów manipulatora, zilustrowane na rys. 5.32.

 Rozwiązanie:

 

Dane są długości poszczególnych członów manipulatora odpowiednio: d1, a2, a3, d4, d5, mierząc od podstawy (rys. 5.32). W przypadku przedstawionej struktury manipulatora najprostszy sposób przyjęcie układów współrzędnych został przedstawiony na rys. 5.33, natomiast w tabeli 5.4 zamieszczono odpowiednie operacje obrotów i przesunięć.

 

układ

qi

di

ai

ai

1

0

d1

0

0

2

q2,var

0

a2

0

3

q3,var

0

a3

180°

4

0

d4,var

0

0

5

q5,var

d5

0

0

Tabela 5.4 Parametry kinematyczne dla przykładu 5.12

 

http://www.robotyka.com/teoria/grafika/image377.gif

http://www.robotyka.com/teoria/grafika/image378.gif

Rysunek 5.32 Schemat z wymiarami

Rysunek 5.33 Schemat z układami

 

dla układu I

http://www.robotyka.com/teoria/grafika/image379.gif

http://www.robotyka.com/teoria/grafika/image380.gif

dla układu II

http://www.robotyka.com/teoria/grafika/image381.gif

http://www.robotyka.com/teoria/grafika/image382.gif


dla układu III

http://www.robotyka.com/teoria/grafika/image383.gif

http://www.robotyka.com/teoria/grafika/image384.gif

http://www.robotyka.com/teoria/grafika/image385.gif

dla układu IV

http://www.robotyka.com/teoria/grafika/image386.gif

http://www.robotyka.com/teoria/grafika/image387.gif

dla układu V

http://www.robotyka.com/teoria/grafika/image388.gif

http://www.robotyka.com/teoria/grafika/image389.gif

ostatecznie:

http://www.robotyka.com/teoria/grafika/image390.gif

http://www.robotyka.com/teoria/grafika/image391.gif

 

Otrzymana macierz może posłużyć do obliczenia pozycji i orientacji w dowolnej chwili czasowej. Poniżej przedstawiono kod w programie MapleTM.

>          restart:

>          with(linalg):

>          A[1]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[1],0,0,0,1]);

>          Rot[z,theta[2]]:=matrix(4,4,[cos(theta[2]),-sin(theta[2]),0,0, sin(theta[2]),cos(theta[2]),0,0,0,0,1,0,0,0,0,1]);

>          trans[x,a[2]]:=matrix(4,4,[1,0,0,a[2],0,1,0,0,0,0,1,0,0,0,0,1]);

>          A[2]:=multiply(Rot[z,theta[2]],trans[x,a[2]]);

>          Rot[z,theta[3]]:=matrix(4,4,[cos(theta[3]),-sin(theta[3]),0,0, sin(theta[3]),cos(theta[3]),0,0,0,0,1,0,0,0,0,1]);

>          trans[x,a[3]]:=matrix(4,4,[1,0,0,a[3],0,1,0,0,0,0,1,0,0,0,0,1]);

>          Rot[x,180]:=matrix(4,4,[1,0,0,0,0,cos(Pi),-sin(Pi),0,0,sin(Pi), cos(Pi),0,0,0,0,1]);

>          A[3]:=multiply(Rot[z,theta[3]],trans[x,a[3]],Rot[x,180]);

>          A[4]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[4],0,0,0,1]);

>          Rot[z,theta[5]]:=matrix(4,4,[cos(theta[5]),-sin(theta[5]),0,0, sin(theta[5]),cos(theta[5]),0,0,0,0,1,0,0,0,0,1]);

>          trans[z,d[5]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[5],0,0,0,1]);

>          A[5]:=multiply(Rot[z,theta[5]],trans[z,d[5]]);

>          T[5,0]:=map(combine,multiply(A[1],A[2],A[3],A[4],A[5])); 

Układy współrzędnych zostały również przedstawione na trójwymiarowym modelu manipulatora, zilustrowanym na rys. 5.34. W celu oznaczenia poszczególnych osi wykorzystano odpowiednie oznaczenie ich kolorami: czerwony - oś z, zielony - oś x oraz żółty - oś y. Każdy z układów współrzędnych został odpowiednio dołączony do modelu robota zgodnie z przyjętym schematem rozwiązania (rys. 5.33).

 

http://www.robotyka.com/teoria/grafika/image392.jpg

http://www.robotyka.com/teoria/grafika/image393.jpg

http://www.robotyka.com/teoria/grafika/image394.jpg

http://www.robotyka.com/teoria/grafika/image395.jpg

http://www.robotyka.com/teoria/grafika/image396.jpg

http://www.robotyka.com/teoria/grafika/image397.jpg

http://www.robotyka.com/teoria/grafika/image398.jpg

Rysunek 5.34 Trójwymiarowe ujęcia podczas ruchu manipulatora z układami

 

Przykład 5.13

http://www.robotyka.com/teoria/grafika/image399.jpg

Rysunek 5.35 Manipulator o 2 stopniach swobody

Wyznaczyć położenie i orientację chwytaka w przestrzeni dla manipulatora o 2 stopniach swobody, przedstawionego na rys. 5.35. Dane są poszczególne długości członów manipulatora.

Rozwiązanie:

http://www.robotyka.com/teoria/grafika/image400.jpg

Rysunek 5.36 Model 3D manipulatora z układami współrzędnych

 

W przypadku przedstawionej struktury manipulatora najprostszy sposób przyjęcie układów współrzędnych został przedstawiony na rys. 5.36, natomiast w tabeli 5.5 zamieszczono odpowiednie operacje obrotów i przesunięć.

układ

qi

di

ai

ai

1

0

0

a1

0

2

q2,var

d2

0

90°

3

0

d3,var

0

0

Tabela 5.5 Parametry kinematyczne dla przykładu 5.13

 

dla układu I

http://www.robotyka.com/teoria/grafika/image401.gif

http://www.robotyka.com/teoria/grafika/image402.gif

dla układu II

http://www.robotyka.com/teoria/grafika/image403.gif

http://www.robotyka.com/teoria/grafika/image404.gif

http://www.robotyka.com/teoria/grafika/image405.gif

dla układu III

http://www.robotyka.com/teoria/grafika/image406.gif

http://www.robotyka.com/teoria/grafika/image407.gif

ostatecznie:

http://www.robotyka.com/teoria/grafika/image408.gif

http://www.robotyka.com/teoria/grafika/image409.gif

 

Podobnie jak w poprzednich przykładach otrzymana macierz może posłużyć do obliczenia pozycji i orientacji w dowolnej chwili czasowej. Poniżej przedstawiono kod w programie MapleTM.

 >         restart:

>          with(linalg):

>          Trans[x,a[1]]:=matrix(4,4,[1,0,0,a[1],0,1,0,0,0,0,1,0,0,0,0,1]);

>          A[1]:=Trans[x,a[1]];

>          Rot[z,theta[2]]:=matrix(4,4,[cos(theta[2]),-sin(theta[2]),0,0, sin(theta[2]),cos(theta[2]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[z,d[2]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[2],0,0,0,1]);

>          Rot[x,90]:=matrix(4,4,[1,0,0,0,0,cos(Pi/2),sin(Pi/2),0,0,

sin(Pi/2), cos(Pi/2),0,0,0,0,1]);

>          A[2]:=multiply(Rot[z,theta[2]],Trans[z,d[2]],Rot[x,90]);

>          Trans[z,d[3]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[3],0,0,0,1]);

>          A[3]:=Trans[z,d[3]];

>          T[3,0]:=map(combine,multiply(A[1],A[2],A[3])); 

http://www.robotyka.com/teoria/grafika/image410.jpg

http://www.robotyka.com/teoria/grafika/image411.jpg

http://www.robotyka.com/teoria/grafika/image412.jpg

http://www.robotyka.com/teoria/grafika/image413.jpg

http://www.robotyka.com/teoria/grafika/image414.jpg

http://www.robotyka.com/teoria/grafika/image415.jpg

http://www.robotyka.com/teoria/grafika/image416.jpg

Rysunek 5.37 Trójwymiarowe ujęcia podczas ruchu manipulatora z układami

 

Układy współrzędnych zostały przedstawione na trójwymiarowym modelu manipulatora, zilustrowanym na rys.5.37. W celu oznaczenia poszczególnych osi wykorzystano odpowiednie oznaczenie ich kolorami: czerwony - oś z, zielony - oś xoraz żółty - oś y.

Przykład 5.14

http://www.robotyka.com/teoria/grafika/image417.jpg

Rys.5.38 Manipulator robota IRB 2400

Wyznaczyć położenie i orientację chwytaka w przestrzeni dla manipulatora IRB 2400 o 5 stopniach swobody, przedstawionego na rys. 5.38. Dane są poszczególne długości członów manipulatora.

 

Rozwiązanie:

http://www.robotyka.com/teoria/grafika/image418.jpg

Rysunek 5.39 Model 3D manipulatora  z układami współrzędnych

W przypadku przedstawionej struktury manipulatora przyjęto układy współrzędnych w sposób zilustrowany narys. 5.39, natomiast w tabeli 5.6 zamieszczono odpowiednie operacje obrotów i przesunięć.

układ

qi

di

ai

ai

1

q1,var

d1

a1

90°

2

q2,var+90°

0

0

0

3

0

0

a3

0

4

q4,var

0

a4

90°

5

0

d5

0

-90°

6

q6,var

0

0

90°

7

0

d7

0

0

8

q8,var

d8

0

0

Tabela 5.6 Parametry kinematyczne dla przykładu 5.14

 

dla układu I

http://www.robotyka.com/teoria/grafika/image419.gif

http://www.robotyka.com/teoria/grafika/image420.gif

http://www.robotyka.com/teoria/grafika/image421.gif

dla układu II

http://www.robotyka.com/teoria/grafika/image422.gif

http://www.robotyka.com/teoria/grafika/image423.gif

dla układu III

http://www.robotyka.com/teoria/grafika/image424.gif

http://www.robotyka.com/teoria/grafika/image425.gif

dla układu IV

http://www.robotyka.com/teoria/grafika/image426.gif

http://www.robotyka.com/teoria/grafika/image427.gif

http://www.robotyka.com/teoria/grafika/image428.gif

dla układu V

http://www.robotyka.com/teoria/grafika/image429.gif

http://www.robotyka.com/teoria/grafika/image430.gif

dla układu VI

http://www.robotyka.com/teoria/grafika/image431.gif

http://www.robotyka.com/teoria/grafika/image432.gif

dla układu VII

http://www.robotyka.com/teoria/grafika/image433.gif

http://www.robotyka.com/teoria/grafika/image434.gif

dla układu VIII

http://www.robotyka.com/teoria/grafika/image435.gif

http://www.robotyka.com/teoria/grafika/image436.gif

ostatecznie:

http://www.robotyka.com/teoria/grafika/image437.gif

 

Ze względu na dużą złożoność rozwiązania symbolicznego postać macierzy T8,0 jest dostępna po wykorzystaniu dołączonego kodu w programie MapleTM zamieszczonego poniżej.

>          restart:

>          with(linalg):

>          Rot[z,theta[1]]:=matrix(4,4,[cos(theta[1]),-sin(theta[1]),0,0, sin(theta[1]),cos(theta[1]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[z,d[1]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[1],0,0,0,1]);

>          Trans[x,a[1]]:=matrix(4,4,[1,0,0,a[1],0,1,0,0,0,0,1,0,0,0,0,1]);

>          Rot[x,90]:=matrix(4,4,[1,0,0,0,0,cos(Pi/2),sin(Pi/2),0,0,

sin(Pi/2), cos(Pi/2),0,0,0,0,1]);

>          A[1]:=multiply(Rot[z,theta[1]],Trans[z,d[1]],Trans[x,a[1]],Rot[x,90]);

>          Rot[z,theta[2]+Pi/2]:=matrix(4,4,[cos(theta[2]+Pi/2),-sin(theta[2]+Pi/2)0,0,sin(theta[2]+Pi/2),cos(theta[2]+Pi/2),0,0,0,0,1,0,0,0,0,1]);

>          A[2]:=Rot[z,theta[2]+Pi/2];

>          Trans[x,a[3]]:=matrix(4,4,[1,0,0,a[3],0,1,0,0,0,0,1,0,0,0,0,1]);

>          A[3]:=Trans[x,a[3]];

>          Rot[z,theta[4]]:=matrix(4,4,[cos(theta[4]),-sin(theta[4]),0,0, sin(theta[4]),cos(theta[4]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[x,a[4]]:=matrix(4,4,[1,0,0,a[4],0,1,0,0,0,0,1,0,0,0,0,1]);

>          A[4]:=multiply(Rot[z,theta[4]],Trans[x,a[4]],Rot[x,90]);

>          Trans[z,d[5]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[5],0,0,0,1]);

>          Rot[x,-90]:=matrix(4,4,[1,0,0,0,0,cos(-Pi/2),sin(-Pi/2),0,0,

sin(-Pi/2), cos(-Pi/2),0,0,0,0,1]);

>          A[5]:=multiply(Trans[z,d[5]],Rot[x,-90]);

>          Rot[z,theta[6]]:=matrix(4,4,[cos(theta[6]),-sin(theta[6]),0,0, sin(theta[6]),cos(theta[6]),0,0,0,0,1,0,0,0,0,1]);

>          A[6]:=multiply(Rot[z,theta[6]],Rot[x,90]);

>          Trans[z,d[7]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[7],0,0,0,1]);

>          A[7]:=Trans[z,d[7]];

>          Rot[z,theta[8]]:=matrix(4,4,[cos(theta[8]),-sin(theta[8]),0,0, sin(theta[8]),cos(theta[8]),0,0,0,0,1,0,0,0,0,1]);

>          Trans[z,d[8]]:=matrix(4,4,[1,0,0,0,0,1,0,0,0,0,1,d[8],0,0,0,1]);

>          A[8]:=multiply(Rot[z,theta[8]],Trans[z,d[8]]);

>          T[8,0]:=map(combine,multiply(A[1],A[2],A[3],A[4],A[5],A[6],A[7],A[8]));.

 

Układy współrzędnych zostały przedstawione na trójwymiarowym modelu manipulatora, zilustrowanym na rys.5.40. W celu oznaczenia poszczególnych osi wykorzystano odpowiednie oznaczenie ich kolorami: czerwony - oś z, zielony - oś xoraz żółty - oś y.

 

http://www.robotyka.com/teoria/grafika/image438.jpg

http://www.robotyka.com/teoria/grafika/image439.jpg

http://www.robotyka.com/teoria/grafika/image440.jpg

http://www.robotyka.com/teoria/grafika/image441.jpg

http://www.robotyka.com/teoria/grafika/image442.jpg

http://www.robotyka.com/teoria/grafika/image443.jpg

http://www.robotyka.com/teoria/grafika/image444.jpg

Rys.5.40 Trójwymiarowe ujęcia podczas ruchu manipulatora z układami

 

Należy podkreślić, iż w przypadku rozwiązywania zadania prostego kinematyki, co przedstawiono powyżej, zawsze otrzymuje się jednoznaczne rozwiązanie.