Projekt, wersja gotowa


Politechnika Śląska
Wydział: Mechaniczny Technologiczny
Automatyka i Robotyka
Kinematyka i Dynamika Robotów i
Manipulatorów
Projekt:
Budowa Robota: częśd obliczeniowa
Grupa I
Sekcja:
Mateusz Borkowski
Aukasz Chejduk
Jakub Cieloch
Dawid Gach
str. 1
Proste zadanie kinematyki
Lp. ęi li Śi di
T1 - - 0 50
T2 900 - 300 -
T3 -900 - - 180
T4 900 - -450+900 -
T5 - 100 - -
1.Transformacja T(0-1) Rot(z; 1)*Trans(Z;l1)
2.Transformacja T(1-2) Rot(x; 90)*Rot(Z;2)
3.Transformacja T(2-3) Rot(X;-90)Trans(Z:d3)
4.Transformacja T(3-4) Rot(X:90)*Rot(Z;3+90)
5.Transformacja T(4-5) Trans(X;l5)
1 0 0 0
ć

0 1 0 0

T1 =

0 0 1 50

0 0 0 1
Ł ł
ć0.866 0.5 0 0

0 0 -1 0

T1T2 =

-0.5 0.866 0 50

0 0 0 1
Ł ł
ć0.866 0 0.5 90

- 15
0 1 0 -1.477 10

T1T2T3 =

-0.5 0 0.866 205.885

0 0 0 1
Ł ł
ć0.966 -0.259 0 90

- 15
0 0 -1 -1.477 10

T1T2T3T4 =
0.259 0.966 0 205.885

0 0 0 1
Ł ł
ć0.966 -0.259 0 186.593

0 0 -1 0

T1T2T3T4T5 =

0.259 0.966 0 231.766

0 0 0 1
Ł ł
str. 2
1.Transformacja T(0-1) Rot(z; 1)*Trans(Z;l1)
l1 := 50
q1 := 0
ćcos 1 -sin 1 0 0
(q ) (q )
ć1 0 0 0

l2 := 80
0 1 0 0
-p

sin cos 0 0
(q ) (q ) q2 :=
1 1
RZ1 :=
TZ1 := 0 1 l1
6
0 d3 := 180
0 0 1 0
-p
q3 := l5 := 100
Ł0 0 0 1 ł
0 0 0 1
Ł ł
4
T1 := RZ1TZ1
ć1 0 0 0

0 1 0 0
T1

0 0 1 50
0 0 0 1
Ł ł
2.Transformacja T(1-2) Rot(x; 90)*Rot(Z;2)
p
ć
a2 :=

ć1 0 0 0
ćcos 2 -sin 2 0 0
(q ) (q ) 2
Ł ł


-sin 0
0 cos(a ) (a )

2 2
sin cos 0 0
(q ) (q )
2 2
RX2 :=
RZ2 :=
0 sin cos 0

(a ) (a )
2 2
0 0 1 0
0 0 0 1

0 0 0 1
Ł ł
Ł ł
T2 := RX2RZ2
3 1
ć
0 0

2 2

0 0 -1 0

T2

1 3 ć0.866 0.5 0 0

- 0 0

0 0 -1 0
2 2


T1T2 =

-0.5 0.866 0 50
0 0 0 1
Ł ł

0 0 0 1
Ł ł
str. 3
3.Transformacja T(2-3) Rot(X;-90)Trans(Z:d3)
-p
a3 :=
2
ć1 0 0 0
ć1 0 0 0

0 1 0 0
-sin 0
0 cos(a ) (a )
3 3
RX3 :=
0 sin cos 0 TZ3 := 0 1 d3
0
(a ) (a )
3 3

0 0 0 1
Ł0 0 0 1 ł
Ł ł
T3 := RX3TZ3
ć0.866 0 0.5 90

ć1 0 0 0 - 15
0 1 0 -1.477 10

T1T2T3 =
0 0 1 180

T3 -0.5 0 0.866 205.885

0 -1 0 0
0 0 0 1
Ł ł
0 0 0 1
Ł ł
p
4.Transformacja T(3-4) Rot(X:90)*Rot(Z;3+90)
a4 :=
2
ćcos ćq + p ćq + p
-sin 0 0


3 3
2 2
Ł ł Ł ł
ć1 0 0 0


ćq p ćq p

-sin 0
sin + cos + 0 0
0 cos(a ) (a ) RZ4 :=

4 4
3 3
2 2

Ł ł Ł ł
RX4 :=
0 sin cos 0

(a ) (a )
0 0 1 0
4 4

0 0 0 1
0 0 0 1
Ł ł
Ł ł
T4 := RX4RZ4
ć0.966 -0.259 0 90

2 2 - 15
ć
0 0 -1 -1.477 10
- 0 0
T1T2T3T4 =
2 2
0.259 0.966 0 205.885


0 0 -1 0

0 0 0 1
T4 Ł ł

2 2
0 0

2 2

0 0 0 1
Ł ł
5.Transformacja T(4-5) Trans(X;l5)
ć1 0 0 100
ć1 0 0 l5


ć0.966 -0.259 0 186.593
0 1 0 0
T5 0 1 0 0

TX5 :=

0 0 1 0
0 0 1 0 0 0 -1 0

T1T2T3T4T5 =
0 0 0 1

Ł ł
0.259 0.966 0 231.766
Ł0 0 0 1 ł

0 0 0 1
Ł ł
str. 4
Odwrotne Zadanie Kinematyki
ć0.966 -0.259 0 186.593

0 0 -1 0

To :=

0.259 0.966 0 231.766

0 0 0 1
Ł ł
Macierz końcowa To=T01*T12*T23*T34
tutaj oznaczona jako To
Obliczanie pierwszego kąta t1, czyli teta 1
0.966cos 0.259cos sin 186.593cos

ć
(q ) (q ) (q ) (q )
1 1 1 1

- -
2 2 2 2 2 2 2 2

cos + sin cos + sin cos + sin cos + sin
(q ) (q ) (q ) (q ) (q ) (q ) (q ) (q )
1 1 1 1 1 1 1 1


0.966sin 0.259sin cos 186.593sin

- 1
(q ) (q ) (q ) (q )
1 1 1 1
To
-
(T )
- -
1
2 2 2 2 2 2 2 2

cos + sin cos + sin cos + sin cos + sin
(q ) (q ) (q ) (q ) (q ) (q ) (q ) (q )
1 1 1 1 1 1 1 1


0.259 0.966 0 181.766

0 0 0 1
Ł ł
p p p p p p
ćcos ć ć ć ć
+ q3 + q3 + q3 + q3
cos 2 - sin sin 2 -cos sin 2 - sin cos 2 0 100cosć 2 + q3 2 - 180sin 2 - 100sinć 2 + q3 2
(q ) (q ) (q ) (q ) cos (q ) (q ) sin
(q )

2 2 2 2
Ł ł Ł ł Ł ł Ł ł Ł ł Ł ł

0 0 -1 0

T2T3T4T5
p p p p p p
ć ć ć ć ć ć
(q ) (q ) (q ) (q ) (q ) (q ) (q )
cos 2 + q3sin + sin 2 + q3cos cos 2 + q3cos - sin 2 + q3sin 0 180cos + 100cos 2 + q3sin + 100sin 2 + q3cos
2 2 2 2 2 2 2
Ł ł Ł ł Ł ł Ł ł Ł ł Ł ł

0 0 0 1
Ł ł
Aby wyznaczyd pierwszy kąt niewiadomy, decydujemy się na porównanie komórki (1,3)
sin
ć
(q )
1

- := 0

sin(1)=0 czyli 1=0
2
2 2

cos + sin
(q ) (q )
1 1
Ł ł
Po wprowadzeniu kąta do macierzy otrzymujemy :
.. To1=T01^(-1) *To
ć0.966 -0.259 0 186.593

0 0 -1 0

To1 :=

0.259 0.966 0 181.766

0 0 0 1
Ł ł
str. 5
Z powyższych macierzy wyznaczamy również zależności między kolejnymi dwoma kątami.
p p
ć ć
cos + q3 2 - sin + q3 2 := 0.966
cos sin
(q ) (q )
2
2 2
Ł ł Ł ł
-sin - cos := 0.966
(q )cos(q ) (q )sin(q )
3 2 3 2
-
cos + cos
(sin(q ) (q ) (q )sin(q ))=0.966
3 2 3 2
-sin( + )=0.966
q3 q2
sin( + )=-0.966
q3 q2
+ =arcsin(-0.966)
q3 q2
+ =-750
q3 q2
=-75o-
q2 q3
Dla 1=0
p p
ć ć
100cos + q3 2 - 180sin - 100sin + q3 2
cos sin
(q ) (q ) (q )=186.593
2
2 2
Ł ł Ł ł
100(-sin( ))cos( )-100cos( )sin( )-180sin( )=186.593
q3 q2 q3 q2 q2
-100(sin( )cos( )+cos( )sin( ))--180sin( )=186.593
q3 q2 q3 q2 q2
-100sin( + )-180sin( )=186.593
q3 q2 q2
=-75o-
q2 q3
-100sin(-75o- q3 q3
+ )-180sin(-75o- q3
)=186.593
-100sin(-75o)-180sin(-75o- q3
)=186.593
-180sin(-75o- q3
)=186.593-96.593//-180
180sin(-75o- q3
)= -0.5
-75o- q3
=arcsin(-0.5)
-75o- q3
=-30o
.
=-45o
q3
=-45o
q3
=-75o-
q2 q3
=-75o-(-450)
q2
=-300
q2
str. 6
Dla 1=180o
p p
ć ć
100cos + q3 2 - 180sin - 100sin + q3 2
cos sin
(q ) (q ) (q )=-186.593
2
2 2
Ł ł Ł ł
100(-sin( ))cos( )-100cos( )sin( )-180sin( )=-186.593
q3 q2 q3 q2 q2
-100(sin( )cos( )+cos( )sin( ))--180sin( )=-186.593
q3 q2 q3 q2 q2
-100sin( + )-180sin( )=-186.593
q3 q2 q2
=-75o-
q2 q3
-100sin(-75o- q3 q3
+ )-180sin(-75o- q3
)=-186.593
-100sin(-75o)-180sin(-75o- q3
)=-186.593
-180sin(-75o- q3
)=-186.593-96.593//-180
180sin(-75o- q3
)= 0.5
-75o- q3
=arcsin(0.5)
-75o- q3
=30o
=45o
q3
=45o
q3
=-75o-
q2 q3
=-75o-(450)
q2
=1200
q2
Manipulator może byd ustawiony w 2 różnych pozycjach aby ostatni człon był w tym samym miejscu,
jednakże dla 1=180o wartośd innego kąta 2=120o co nie zawiera się w założeniach konstrukcyjnych, i
nie może byd zrealizowane.
str. 7
Kinematyka Manipulatora metodą macierzową
Kinematykę manipulatora przedstawimy dla dokładnie takiego samego układu manipulatora jak w
prostym i odwrotnym zadaniu kinematyki.
Dla przypomnienia:
Lp. ęi li Śi di
T1 - - 0 50
T2 900 - 300 -
T3 -900 - - 180
T4 900 - -450+900 -
T5 - 100 - -
Postad macierzy kierunkowych:
Macierz R1 jest nie używana w obliczeniach, ze względu na brak ruchu w parze obrotowej
ćcos 2 -sin 2 0
(q ) (q )
ć1 0 0
0
R2 := 0 0 -1

R3 := 0 1


sin cos 0
(q ) (q )
2 2 Ł0 -1 0ł
Ł ł
cosćq + p ćq + p ł
-sin 0
ę ś
3 3 ć1 0 0
2 2
Ł ł Ł ł
ę ś
0
R5 := 1 0
R4 := 0 0 -1
ę ś

Ł0 0 1ł
ę ś
p p
ęsinćq3 + 2 ćcosćq3 + 2 0 ś

Ł ł Ł Ł łł
Postad wektorów przesuwu:
0 ćl5
ć


P5 :=
P3 :=
d3 0


0
0 Ł ł
Ł ł
Pozostałe wektory są równe zero.
str. 8
Dobrane wartości prędkości kątowych: wi=*Z
Z- wersor kierunkowy
=
0 0
ć ć
ć0

0
w2 := 0 w3 := 0
w1 :=


Ł-0.5ł Ł-0.5ł
Ł0ł
Prędkości kątowe wynikające z odpowiednich transformacji układu:
0
ć

w2 := R2Tw1 + w2 = 0

Ł-0.5ł
0
ć
0.5
w3 := R3Tw2 =

0
Ł ł
0
ć

w4 := R4Tw3 + w4 = 0

Ł-1ł
0
ć

w5 := R5Tw4 = 0

Ł-1ł
Obliczanie prędkości liniowych:
ć0
0
V1 :=

Ł0ł
ć0ł ć0
ęV 0ś 0
V2 := R2T + w1 =
1
ę ś
Ł0ł Ł0ł
Prędkośd V3 odpowiada prędkości liniowej punktu B
ć0.09

V3 := R3T + w2 P3 + V3 = 0
(V )
2

0.5
Ł ł
2 2
0.09 + 0.5 = 0.508
str. 9
ć0ł ć0.417
ęV 0ś
V4 := R4T + w3 = 0.29
3
ę ś
0
Ł0ł Ł ł
Prędkośd V5 odpowiada prędkości liniowej punktu C
ć0.417

V5 := R5T + w4 P5 + 0 = 0.19
(V)4 ł


0
Ł ł
2 2
0.417 + 0.19 = 0.458
Obliczanie przyspieszeo kątowych.
Przyspieszenie kątowe wynikające z charakterystyki silników:
0 0
ć ć
E4 :=
E2 := 0 0

Ł-0.1ł Ł-0.1ł
ć0
0
e :=
1

Ł0ł
0
ć

e := R2Te + R2Tw1 w2 + E2 = 0
2 1

Ł-0.1ł
0
ć
0.1
e := R3Te =
3 2

0
Ł ł
0
ć

e := R4Te + R4Tw3 w4 + E4 = 0
4 3

Ł-0.2ł
0
ć

e := R5Te = 0
5 4

Ł-0.2ł
Obliczanie przyspieszeo liniowych:
Przyspieszenie kątowe wynikające z charakterystyki silników:
0
ć

A2 := 0

Ł0.05ł
str. 10
ć0
0
a1 :=

Ł0ł
ć0
0
a2 := R2T P2 + w1 P2 + a1 =
e 1 ł
(w )
1


Ł0ł
Przyspieszenie a3 odpowiada przyspieszeniu liniowemu punktu B :
0.518
ć

0
a3 := R3T P3 + w2 P3 + a2 + 2w3 V3 + A2 =

e 2 ł
(w )
2

5 10- 3
Ł ł
2 2
0.518 + 0.005 = 0.518
0.37
ć

a4 := R4T P4 + w3 P4 + a3 = -0.363
e 3 ł
(w )
3


0
Ł ł
Przyspieszenie a5 odpowiada przyspieszeniu liniowemu punktu C:
0.27
ć

a5 := R5T P5 + w4 P5 + a4 + 2w5 V5 + A5 = -0.383
e 4 ł
(w )
4


0
Ł ł
2 2
0.27 + 0.383 = 0.469
str. 11
Dynamika
Aby wyznaczyd działające siły i momenty w manipulatorze, które będą przybliżone do warunków
panujących w rzeczywistości, należy przyspieszenia liniowe powiększyd o działanie grawitacji
ziemskiej. Aby tego dokonad wystarczy do wektora odpowiadającemu za przyspieszenie liniowe 1
członu dodad g=9.87m/s^2. W takim wypadku przyspieszenia przyjmują postad :
0
ć0 ć
0
ag1 := = 0

Łgł Ł9.87ł
ć-4.935

ag2 := R2T P2 + w1 P2 + ag1 = 8.548
e 1 ł
(w )
1


0
Ł ł
ć-4.417

ag3 := R3T P3 + w2 P3 + ag2 + 2 w3 V3 + A2 = 0
e 2 ł
(w )
2


8.553
Ł ł
ć2.924
9.171
ag4 := R4T P4 + w3 P4 + ag3 =
e 3 ł
(w )
3


0
Ł ł
ć2.824
9.151
ag5 := R5T P5 + w4 P5 + ag4 + 2 w5 V5 + A5 =
e 4 ł
(w )
4


0
Ł ł
Kolejnym krokiem który należy wykonać jest określenie masy danego ramienia. Ze względu na
wtyczne projektu masa musi zostać określona jako masa punktowa znajdująca się na końcu danego
ramienia. Aby, wykonać taki zabieg korzystamy z zależności energi kinetycznej. Wiadomo, że energia
kinetyczna w każdym punkcie danego ciała przyjmuje takie same wartości, dlatego:
ms Vsr mk Vk
:=
2
2
2 2
ms Vsr
V
mk :=
Vk
Gdzie:
Ms- Masa środka ciężkości
Mk- Masa zredukowana na koniec ramienia
str. 12
Vsr-prędkość liniowa środka ciężkości ramienia
Vk- prędkość liniowa końcówki ramienia: w projekcie : VB i VC
Aby wyznaczyć prędkość liniową środka ciężkości ramienia, należy określić położenie środka
ciężkości. W naszym przypadku, środek ciężkości znajduje się w połowie ramienia, czyli :
Położenie środka ciężkości dla ramienia pierwszego:
0
ć
1
0.09
Ps3 := P3 =

2
0
Ł ł
Do wyznaczenia prędkości liniowej tegoż ramienia użyjemy gotowych wzorów:
ć0.045

Vs3 := R3T + w2 Ps3 + V3 = 0
(V )
2

0.5
Ł ł
Położenie środka ciężkości dla ramienia drugiego:
ć0.05
1

Ps5 := P5 = 0

2
0
Ł ł
Wyznaczenie prędkości liniowej środka ciężkości ramienia drugiego, wykonujemy adekwatnie jak w
poprzednim kroku:
ć0.417

Vs5 := R5T + w4 Ps5 + 0 = 0.24
(V)4 ł


0
Ł ł
Zakładamy, że masy poszczególnych ramion przyjmują wartości:
M3=0.3 kg, m5=0.25kg
Po skorzystaniu z zależności na równowagę energii kinetycznej, otrzymujemy wartości mas:
m3V3
mk3 := = 0.304
Vs3
m5V5
mk5 := = 0.238
Vs5
str. 13
Wyznaczanie sił działających w manipulatorze.
Do wyznaczenia sił skorzystamy ze wzoru :
i+1
Fi+1 = mi+1i+1ai+1
Gdzie :
F- siła
Mi+1-masa aktualnego członu, zredukowana do kooca członu
Ai+1  przyspieszenie liniowe aktualnego członu, powiększone o wartośd przyspieszenia ziemskiego.
ć0
0
F1 := m1 ag1 =

Ł0ł
ć0
0
F2 := m2 ag2 =

Ł0ł
ć-1.343

F3 := mk3 ag3 = 0

2.6
Ł ł
ć0
0
F4 := m4 ag4 =

Ł0ł
ć0.672
2.178
F5 := mk5 ag5 =

0
Ł ł
Siły napędowe
Aby obliczyd siły napędowe, skorzystamy z wzoru:
i i
fi =i+1Ri+1fi+1+iFi
Gdzie,
fi- siła napędowa
Ri- macierz przekształcenia dla danego członu
Fi- siła działająca na dany człon.
str. 14
ć0.935
3.029
f := F5 =
5

0
Ł ł
ć1.903
6.065
f := R5 f + F4 =
4 5

0
Ł ł
ć-4.255

f := R4 f + F3 = 0
3 4

8.174
Ł ł
ć-6.722
12.448
f := R3 f + F2 =
2 3

0
Ł ł
0.402
ć

f := R2 f + F1 = 0
1 2

Ł19.076ł
Momenty napędowe:
ć0 ć0 ć0 ć0 ć0
0 0 0 0 0
N5 := N4 := N3 := N2 := N1 :=

Ł0ł Ł0ł Ł0ł Ł0ł Ł0ł
0
ć

n5 := N5 + P5 F5 = 0

Ł0.303ł
0
ć

n4 := N4 + R5 n5 + P4 F4 + P5 f = 0
(R )
5 5

Ł0.606ł
0
ć

n3 := N3 + R4 n4 + P4 F3 + P4 f = -0.606
(R )
4 4

0
Ł ł
0
ć

n2 := N2 + R3 n3 + P2 F2 + P3 f = 0
(R )
3 3

Ł1.372ł
0
ć

n1 := N1 + R2 n2 + P2 F1 + P2 f = -1.372
(R )
2 2

0
Ł ł
str. 15


Wyszukiwarka

Podobne podstrony:
Projekt wersja ostateczna
projekt wersja elektroniczna
ProjektD wersja1 ReadMe
ProjektD wersja2 ReadMe
projekt dobre rady wersja1 1
listing wersja karto PROJEKT ISOK
Karawana jedzie dalej, czyli kolejna wersja projektu z Rejestrem Stron i Usług Niedozwolonych

więcej podobnych podstron