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óź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łaszczyź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:
Położenie
0
2
4
6
8
10
12
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
Trajektoria robota
czas[s]
Va[m/s]
sygnalTrajektorii
0
2
4
6
8
10
12
0
0.5
1
1.5
2
2.5
Polozenie
czas[s]
x[m]
y[m]
Prędkości:
Kąty obrotu kół Alfa:
0
2
4
6
8
10
12
-0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
Predkosci
czas[s]
Vxa[m/s]
Vya[m/s]
0
2
4
6
8
10
12
0
10
20
30
40
50
60
70
80
Kat obrotu kol: Alfa 1,2 i 3
czas[s]
k
a
t[
ra
d
]
Alfa1
Alfa2
Alfa3
Pochodne kątów Alfa (prędkość obrotowa kół):
Kąt obrotu obudowy i szybkość jego zmiany:
0
2
4
6
8
10
12
-1
0
1
2
3
4
5
6
7
8
9
Predkosc obrotu kol: AlfaPrim 1,2 i 3
czas[s]
p
re
d
k
o
s
c
k
a
to
w
a
[r
a
d
/s
]
AlfaPrim1
AlfaPrim2
AlfaPrim3
0
2
4
6
8
10
12
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Kat obrotu obudowy i szybkosc zmiany kata: Beta, BetaPrim
czas[s]
Beta[rad]
BetaPrim[rad/s]
Kąt obrotu koła podpierającego i szybkość jego zmiany:
Tor robota na płaszczyźnie:
0
2
4
6
8
10
12
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Kat obrotu kola podpierajacego i szybkosc zmiany kata: Theta, ThetaPrim
czas[s]
Theta[rad]
ThetaPrim[rad/s]
Mnożniki Lagrange`a
Porównanie momentów:
0
2
4
6
8
10
12
-1.5
-1
-0.5
0
0.5
1
1.5
Mnozniki Lagrange
czas[s]
Lambda1[N]
Lambda2[N]
0
2
4
6
8
10
12
-1
-0.5
0
0.5
1
1.5
Moment na kołach - Porownanie
czas[s]
M1-Lagrange
M2-Lagrange
M1-Maggi
M2-Maggi
Wykresy – przebieg wygładzony
Przebieg prędkości i „CzyLuk?”
Położenie:
0
2
4
6
8
10
12
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
Trajektoria robota - po wygladzaniu
czas[s]
Va[m/s]
sygnalTrajektorii
0
2
4
6
8
10
12
0
0.5
1
1.5
2
2.5
Polozenie
czas[s]
x[m]
y[m]
Prędkości
Kąt obrotu kół Alfa:
0
2
4
6
8
10
12
-0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
Predkosci
czas[s]
Vxa[m/s]
Vya[m/s]
0
2
4
6
8
10
12
0
10
20
30
40
50
60
70
80
Kat obrotu kol: Alfa 1,2 i 3
czas[s]
k
a
t[
ra
d
]
Alfa1
Alfa2
Alfa3
Pochodne kątów Alfa (prędkość obrotowa kół):
Kąt obrotu obudowy i szybkość jego zmiany:
0
2
4
6
8
10
12
-1
0
1
2
3
4
5
6
7
8
9
Predkosc obrotu kol: AlfaPrim 1,2 i 3
czas[s]
p
re
d
k
o
s
c
k
a
to
w
a
[r
a
d
/s
]
AlfaPrim1
AlfaPrim2
AlfaPrim3
0
2
4
6
8
10
12
-0.2
0
0.2
0.4
0.6
0.8
1
Kat obrotu obudowy i szybkosc zmiany kata: Beta, BetaPrim
czas[s]
Beta[rad]
BetaPrim[rad/s]
Kąt obrotu koła podpierającego i szybkość jego zmiany:
Tor na płaszczyźnie XY:
0
2
4
6
8
10
12
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Kat obrotu kola podpierajacego i szybkosc zmiany kata: Theta, ThetaPrim
czas[s]
Theta[rad]
ThetaPrim[rad/s]
Mnożniki Lagrange`a:
Porównanie momentów (Maggi – na górze, Lagrange – na dole):
0
2
4
6
8
10
12
-1.5
-1
-0.5
0
0.5
1
1.5
Mnozniki Lagrange
czas[s]
Lambda1[N]
Lambda2[N]
0
2
4
6
8
10
12
-1
-0.5
0
0.5
1
1.5
Moment na kołach - Porownanie
czas[s]
M1-Lagrange
M2-Lagrange
M1-Maggi
M2-Maggi
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óź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:
