Kinematyka i Dynamika Układów Mechatronicznych Raport
Wykonali: Tomasz Strzałka, Marek Miodunka Ocena:
Wykres prędkości.
Przebieg prędkości oraz sygnał z informacją czy robot porusza się po łuku czy też nie
generowany jest przed subsystem  Wygeneruj_trapez . Wyjściami z subsystemu są:  Trapez czyli
przebieg prędkości,  Wygladzony_trapez czyli przebieg prędkości wygładzony, aby funkcja była
różniczkowalna oraz  CzyLuk? czyli informacja o tym, czy robot porusza się po linii prostej czy po
łuku.
Subsystem wygeneruj trapez składa się z pięciu identycznych podsystemów: Etap 1,2,3,4 oraz
5.
Bloczki te przekazują sobie informację o stanie  aktualnej prędkości, tego czy robot znajduje
się na łuku oraz czasie. Oprócz tego wejściem do każdego subsystemu są również kluczowe
informacje o danym etapie  czas trwania etapu, prędkość końcowa, promień łuku itp. Taka budowa
pozwala na dowolne rozszerzanie przebiegu prędkości  wystarczy przekopiować subsystem tworząc
Etap 6,7&
 Wewnątrz każdego z bloczków  Etap znajduje się struktura, która pozwala na utworzenie
przebiegu danego etapu  na podstawie informacji wejściowych określa czy prędkość ma się
zwiększyć, zmniejszyć czy pozostać stała, określa również czy wysłać sygnał o znajdowaniu się na
łuku. Ponadto porównuje czas z czasem rozpoczęcia ruchu oraz zakończenia ruchu, tak aby
utworzony przebieg odpowiadał naszym oczekiwaniom.
Oprócz tego subsystem  Wygeneruj_trapez zawiera blok odpowiedzialny za wygładzenie
przebiegu. Odpowiada za to blok  Analog filter design . Filtr Bessela wygładza przebieg sprawiając, że
funkcja jest różniczkowalna. Występuje tutaj minimalne opóz
nienie, lecz jest na tyle niewielkie, że nie
powoduje komplikacji w dalszej części programu.
Kinematyka
Za część kinematyczną odpowiada subsystem  Kinematyka . Wejściami są przebieg prędkości
oraz informacja zero-jedynkowa o tym czy robot znajduję się na łuku, a wyjściami wszystkie
parametry  położenie, prędkości, kąty alfa1, alfa2, alfa3, beta i theta oraz ich pochodne.
Wnętrze tego subsystemu jest dość proste. Za każdą parę zmiennych (wartość i jej pochodną)
odpowiada Function Block oraz Integrator:
Dynamika  Równania Lagrange a
Za tą część odpowiada subsystem  Dynamika  Lagrange . Wejściami są alfa1 , alfa2 , beta
oraz beta , a wyjściami momenty oraz mnożniki.
We wnętrzu subsystemu pierwszym elementem jest wyciągnięciem średniej arytmetycznej z
alfa1 i alfa2 w celu uzyskania alfa . Następnie podobnie jak w kinematyce w blokach Function Block
liczone są odpowiednie wartości.
Dynamika  równania Maggie`go
Do tej części służy subsystem Dynamika  Maggi. Wejściami do niego są jedynie pochodne
kątów alfa1 i alfa2, a wyjściami momenty.
Podobnie jak w przypadku bloków Kinematyka i Dynamika  Lagrange wewnątrz tego bloku
znajdują się Function Blocks, które wyliczają odpowiednie wartości.
Całość
Poza wyżej wymienionymi subsystemami w programie znajdują się jedynie bloki Scope
odpowiedzialne za narysowanie wykresów oraz XY Graph odpowiedzialny za narysowanie toru ruchu
robota na płaszczyz
nie.
Oprócz powyższego programu w Simulinku konieczne jest wcześniejsze odpalenie pliku
Matlaba zawierającego wszystkie stałe.
Wykresy
Rozpoczęliśmy od przeprowadzenia symulacji dla niewygładzonego przebiegu prędkości
(trapez). Przebieg prędkości oraz informacja o tym czy robot znajduje się na łuku:
Trajektoria robota
1.2
Va[m/s]
sygnalTrajektorii
1
0.8
0.6
0.4
0.2
0
-0.2
0 2 4 6 8 10 12
czas[s]
Położenie
Polozenie
2.5
x[m]
y[m]
2
1.5
1
0.5
0
0 2 4 6 8 10 12
czas[s]
Prędkości:
Predkosci
0.35
Vxa[m/s]
Vya[m/s]
0.3
0.25
0.2
0.15
0.1
0.05
0
-0.05
0 2 4 6 8 10 12
czas[s]
Kąty obrotu kół Alfa:
Kat obrotu kol: Alfa 1,2 i 3
80
Alfa1
70
Alfa2
Alfa3
60
50
40
30
20
10
0
0 2 4 6 8 10 12
czas[s]
kat[rad]
Pochodne kątów Alfa (prędkość obrotowa kół):
Predkosc obrotu kol: AlfaPrim 1,2 i 3
9
AlfaPrim1
8
AlfaPrim2
AlfaPrim3
7
6
5
4
3
2
1
0
-1
0 2 4 6 8 10 12
czas[s]
Kąt obrotu obudowy i szybkość jego zmiany:
Kat obrotu obudowy i szybkosc zmiany kata: Beta, BetaPrim
1
0.9 Beta[rad]
BetaPrim[rad/s]
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0 2 4 6 8 10 12
czas[s]
predkosc katowa[rad/s]
Kąt obrotu koła podpierającego i szybkość jego zmiany:
Kat obrotu kola podpierajacego i szybkosc zmiany kata: Theta, ThetaPrim
1
Theta[rad]
0.9
ThetaPrim[rad/s]
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0 2 4 6 8 10 12
czas[s]
Tor robota na płaszczyz
nie:
Mnożniki Lagrange`a
Mnozniki Lagrange
1.5
Lambda1[N]
Lambda2[N]
1
0.5
0
-0.5
-1
-1.5
0 2 4 6 8 10 12
czas[s]
Porównanie momentów:
Moment na kołach - Porownanie
1.5
M1-Lagrange
M2-Lagrange
M1-Maggi
M2-Maggi
1
0.5
0
-0.5
-1
0 2 4 6 8 10 12
czas[s]
Wykresy  przebieg wygładzony
Przebieg prędkości i  CzyLuk?
Trajektoria robota - po wygladzaniu
1.2
Va[m/s]
sygnalTrajektorii
1
0.8
0.6
0.4
0.2
0
-0.2
0 2 4 6 8 10 12
czas[s]
Położenie:
Polozenie
2.5
x[m]
y[m]
2
1.5
1
0.5
0
0 2 4 6 8 10 12
czas[s]
Prędkości
Predkosci
0.35
Vxa[m/s]
Vya[m/s]
0.3
0.25
0.2
0.15
0.1
0.05
0
-0.05
0 2 4 6 8 10 12
czas[s]
Kąt obrotu kół Alfa:
Kat obrotu kol: Alfa 1,2 i 3
80
Alfa1
70
Alfa2
Alfa3
60
50
40
30
20
10
0
0 2 4 6 8 10 12
czas[s]
kat[rad]
Pochodne kątów Alfa (prędkość obrotowa kół):
Predkosc obrotu kol: AlfaPrim 1,2 i 3
9
AlfaPrim1
8
AlfaPrim2
AlfaPrim3
7
6
5
4
3
2
1
0
-1
0 2 4 6 8 10 12
czas[s]
Kąt obrotu obudowy i szybkość jego zmiany:
Kat obrotu obudowy i szybkosc zmiany kata: Beta, BetaPrim
1
Beta[rad]
BetaPrim[rad/s]
0.8
0.6
0.4
0.2
0
-0.2
0 2 4 6 8 10 12
czas[s]
predkosc katowa[rad/s]
Kąt obrotu koła podpierającego i szybkość jego zmiany:
Kat obrotu kola podpierajacego i szybkosc zmiany kata: Theta, ThetaPrim
1
Theta[rad]
0.9
ThetaPrim[rad/s]
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0 2 4 6 8 10 12
czas[s]
Tor na płaszczyz
nie XY:
Mnożniki Lagrange`a:
Mnozniki Lagrange
1.5
Lambda1[N]
Lambda2[N]
1
0.5
0
-0.5
-1
-1.5
0 2 4 6 8 10 12
czas[s]
Porównanie momentów (Maggi  na górze, Lagrange  na dole):
Moment na kołach - Porownanie
1.5
M1-Lagrange
M2-Lagrange
M1-Maggi
1
M2-Maggi
0.5
0
-0.5
-1
0 2 4 6 8 10 12
czas[s]
Wnioski
Wykresy momentów osiągnięte metodą Lagrange`a i Maggi`ego można uznać za zbliżone.
Wykresy osiągnięte w czasie laboratoriów są podobne, o ile nie takie same jak te zamieszczone w
materiale z wykładów, czyli projekt został wykonany poprawnie.
Jedynie wykresy momentów i mnożników różnią się nieznacznie od tych z wykładu. Coraz
mocniejsze wygładzanie trapezu prędkości sprawia, że wykresy coraz bardziej przypominają te z
wykładu:
Jednak wygładzanie tym filtrem powoduje również opóz
nienie w sygnale, co sprawia, że
wygładzanie sygnału niekorzystnie odbija się na osiągniętych wynikach. Jedynie niewielkie
wygładzenie, jakie zaprezentowano w tym raporcie, pozwala na przeprowadzenie symulacji bez
negatywnego wpływu na osiągnięte rezultaty.
Identyfikacja
Układ identyfikacji przyjęto w postaci:
Zaimplementowano powyższe równanie w Simulinku:
Otrzymano następujące wykresy: