Ball & Beam (2)

Poniedziałek, 26 Czerwiec 2023

Teoria sterowania jest działem matematyki zajmującym się modelowaniem pracy obiektów oraz generowaniem dla nich sterowania. Jednak poza samą teorią ma ona także część praktyczną. Istnieje kilka prostych modeli często używanych do demonstracji różnych technik sterowania. Koncepcja Ball & Beam jest bardzo prosta, mamy belkę, po której toczy się piłka. Mierzymy pozycję kulki i sterujemy kątem nachylenia belki. W pierwszej części została opisana część mechaniczna i elektryczna całej konstrukcji. Teraz zajmiemy się opisem i identyfikacją modelu.

Model

Pełny opis dynamiki znajdziemy tu [6]. My przyjmiemy uproszczony model. Zakładamy, że możemy sterować położeniem belki oraz, że położenie kulki nie ma wpływu na jej ruch. Kąt belki oznaczamy jako α, a odległość kulki od osi obrotu belki jako x.

Rysunek 12. Siły działające na kulkę

Na rysunku 12 widzimy siły działające na kulkę znajdującą się na belce:

Fg – siła grawitacji,
Fr – siła reakcji belki na nacisk,
Ft – siła tarcia kulki o belkę,
Fo – siła odśrodkowa, ponieważ opisujemy położenie kulki w nieinercjalnym układzie odniesienia związanym z belką.

Siła grawitacja dana jest wzorem:

gdzie m to masa kulki, a g to przyspieszenie ziemskie.

Siła reakcji na nacisk równoważy prostopadłą składową ciężaru:

Siła odśrodkowa jest równa:

gdzie ά jest pochodną kąta belki po czasie, czyli jej prędkością kątową. Zakładamy jednak, że jest ona na tyle niewielka, że siła odśrodkowa jest pomijalnie mała.

Przyjmujemy, że kulka toczy się bez poślizgu, oznacza to, że przyśpieszenie brzegu piłki wynikające z ruchu obrotowego jest równe temu w ruchu postępowym. Możemy to zapisać jako:

gdzie r to promień kuli, ϵ to przyspieszenie kątowe, które jest konsekwencją momentu siły tarcia:

gdzie I jest momentem obrotowym kuli.

Stąd po podstawieniu dostajemy:

Przyspieszenie w ruchu postępowym dane jest wzorem:

Podstawiając za Ft otrzymujemy:

Zakładając, że kulka jest cienką sferą, więc jej moment bezwładności to:

Otrzymujemy więc, że przyspieszenie kulki jest równe:

Ponieważ w naszym przypadku belka wykonuje małe ruchy wokół zera, możemy przybliżyć sinα przez α wyrażone w radianach. Stąd znajdujemy, że przyspieszenie kulki jest proporcjonalne do kąta nachylenia belki:

Rysunek 13. Schemat mechanizmu sterującego belką

My jednak sterujemy kątem serwomechanizmu, a nie bezpośrednio samej belki. Schemat mechanizmu prezentuje rysunek 13. Sterujemy kątem belki l4, dlatego znamy położenie jej końcówki. Aby określić kąt belki, musimy znaleźć punkt przecięcia dwóch okręgów:

środek w (0, l1), promień l2,
środek w (l5, + l4 cosβ, l4 sinβ), promień l3.

Są dwa takie punkty, ale tylko jeden może wystąpić w naszej konfiguracji. Jego pozycję wyznaczyłem używając biblioteki do obliczeń symbolicznych SymPy [7]. Dokładny wynik znajduje się tu [8].

Rysunek 14. Położenie belki w funkcji położenia serwomechanizmu

Na rysunku 16 pokazano kąt belki w funkcji położenia serwomechanizmu. Jak widzimy, dla większości zakresu możemy przybliżyć tę zależność funkcją liniową. Uzyskana krzywa ma współczynniki:

Możemy więc zapisać model naszego układu jako:

gdzie x0 jest położeniem, a x1 prędkością kulki. Wartość α jest współczynnikiem, który obejmuje zarówno dynamikę kulki, jak i kinematykę belki. W naszym przypadku przyjmiemy:

Zakładając, że położenie serwomechanizmu jest podane w stopniach, otrzymujemy:

Taki model zakłada, że serwomechanizm natychmiast zmienia swoje położenie. Możemy go zmodyfikować, dodając na wejściu inercję pierwszego rzędu, której zadaniem będzie wprowadzenie opóźnienia pomiędzy sygnałem sterującym, a siłą działającą na kulkę. Wtedy układ równań przybierze postać:

gdzie c jest stałą czasową wprowadzanego opóźnienia.

Identyfikacja modelu

Obliczenia niezbędne do identyfikacji modelu zostały wykonane w notatniku: jupiter/Identification_3D_printed.ipynb [9].

Zbieranie danych

Kolejnym krokiem jest zebranie danych potrzebnych do identyfikacji parametrów modelu. Możemy używać monitora portu szeregowego i przesyłać podane komendy albo użyć strony www [5]. Pierwszym etapem jest ustalenie, dla jakiej wartości sterowania serwomechanizm ustawia belkę w pozycji równowagi. Wybieramy tryb sterowania w układzie otwartym. Za pomocą polecenie u wysyłamy kolejne nastawy dla serwomechanizmu. Następnie kładziemy kulkę na środku i sprawdzamy, czy się toczy. Możemy także użyć w tym celu poziomicy. W moim modelu początkowo przyjąłem u0 równe 100.

Następnie za pomocą regulatora P wprowadzimy układ w drgania, na podstawie których będziemy identyfikować parametry modelu. Położyłem kulkę w odległości 140 mm, ustawiłem wartość zadaną na 110 mm (polecenie: z 110) oraz współczynnik P na 0,4 (polecenie: p 4). Następnie włączyłem zbieranie danych, przesyłając komendę s 1. Po włączeniu regulatora (r 1) układ wszedł w narastające drgania. Wartość współczynnika P dobrałem tak, aby otrzymać kilka okresów, zanim nastąpi odbicie kulki od brzegów belki. Uzyskane wyniki skopiowałem z terminala portu szeregowego i umieściłem w notatniku jupiter/Identification_3D_printed.ipynb [9].

Rysunek 15. Położenie kulki w funkcji czasu

Na rysunku 15 zaprezentowano zarejestrowane położenie kulki w funkcji czasu. Na początku widzimy, że piłka spoczywa w odległości 140 mm. W momencie włączenia regulatora rozpoczyna się jej ruch. Widzimy drgania o narastającej amplitudzie, które stabilizują się, gdy kulka zaczyna odbijać się od brzegów belki. Odległość 40 mm to dolny zakres pracy czujnika, a 180 mm to końcówka belki. Do identyfikacji użyjemy jedynie sygnału od rozpoczęcia pracy regulatora do nasycenia amplitudy. Położenie kulki oraz sterowanie dla tego fragmentu prezentuje rysunek 16.

Rysunek 16. Położenie kulki oraz sterowanie

Identyfikacja

Dane zostały zebrane w układzie zamkniętym z regulatorem proporcjonalnym. Nasze sterowanie ma postać:

gdzie z jest wartością zadaną, u0 jest sterowaniem, dla którego belka jest pozioma. Dla uproszczenia stały czynnik możemy zastąpić jedną stałą:

Wtedy sterowanie ma postać:

Możemy je podstawić teraz do wprowadzonego wcześniej modelu:

Na potrzeby symulacji możemy zastąpić występujące w modelu stałe:

Wtedy zamknięty układ będzie dany równaniem:

Nasz model ma więc cztery parametry: A, B oraz stan początkowy (pozycja i prędkość). Warto tu zwrócić uwagę, że otrzymany przez nas układ zamknięty jest oscylatorem z drganiami nietłumionymi. Oznacza to, że sam regulator P nie wystarcza do ustabilizowania kulki.

Ruch kulki zasymulujemy w Pythonie (jupiter/Identification_3D_printed.ipynb) [9]. Aby obliczyć ruch kulki, potrzebujemy funkcji, która przyjmuje aktualny stan układu i na jego podstawie oblicza pochodne:

def model(x, t, A, B):
dx0 = x[1]
dx1 = -A*x[0]+B
dx = [dx0, dx1]
return dx

Widzimy, że funkcja po prostu implementuje prawą stronę równania dynamiki modelu zamkniętego. Następnie możemy uruchomić symulację:

x0 = [x[0], (x[1]-x[0])/T]
x_s = odeint(model, x0, t, args=(4.2, 500))

Funkcja odeint pozwala na numeryczne rozwiązanie równania różniczkowego. Przyjmuje funkcję obliczającą pochodną, początkowy stan modelu, listę chwil czasu, dla których będzie obliczony wynik oraz dodatkowe argumenty, które będą przekazane do funkcji liczącej pochodną. Jako punkt startowy przyjmujemy początkowe położenie oraz średnią prędkość pomiędzy dwoma pierwszymi położeniami. Jako parametry A i B, po kilku eksperymentach na początku przyjąłem 4,2 i 500. Uzyskany wynik prezentuje rysunek 17.

Rysunek 17. Porównanie przykładowego modelu do zebranych danych

Chcemy jednak znaleźć parametry lepszą metodą niż zgadywanie. Potrzebujemy więc sposobu na wyznaczenie odległości pomiędzy danymi z symulacji, a zebraną odpowiedzią. Będzie to błąd średniokwadratowy. Dla każdej chwili czasu policzymy różnicę pomiędzy wartościami, podniesiemy je do kwadratu i zsumujemy. Poniższy kod przedstawia funkcję liczącą odległość pomiędzy trajektoriami:

def model_rms(X, x, t):
A = X[0]
B = X[1]
x0_0 = X[2]
x0_1 = X[3]

x_s = odeint(model, [x0_0, x0_1], t, args=(A, B))
rms = np.sqrt(np.mean((x-x_s[:,0])**2))
return rms

Przyjmuje ona 3 parametry:

X – jest tablicą 4 parametrów, które przyjmuje nasz model,
x – to zapisana rzeczywista trajektoria,
t – to wektor chwil czasu, dla których przeprowadzamy obliczenia.

Teraz możemy wywołać funkcję realizującą minimalizację naszej funkcji celu poprzez dobranie parametrów. Wywołujemy ją:

minimum = optimize.fmin(model_rms, [4.2, 500, x[0], (x[1]-x[0])/T], args=(x, t))

Po kolei przyjmuje ona parametry:
funkcję celu,
tablicę z wartościami początkowymi,
dodatkowe parametry przekazywane do wywoływanej funkcji.

Ważne jest, aby wybrać w miarę dobre wartości początkowe. W przeciwnym razie zastosowana funkcja może zatrzymać się w minimum lokalnym, dalekim od najlepszego dopasowania. Ja otrzymałem dopasowanie: A=3,90; B=454,29. Ponieważ wzmocnienie było równe k=0,4, a wartość zadana z=110 uzyskujemy parametry modelu równe: a=9,76; u0=2,55.

Wartość α będąca współczynnikiem pomiędzy sterowaniem serwomechanizmu, a przyśpieszeniem kulki nie jest dalekia od wartości obliczonej teoretycznie (7,80). Różnica może być spowodowana założeniem braku poślizgu, pominięciem siły tarcia tocznego oraz niedokładnością w wykonaniu modelu. Wynik symulacji modelu dla dobranych współczynników pokazuje rysunek 18.

Rysunek 18. Dopasowanie modelu drugiego rzędu do danych

Częstotliwość została dopasowana poprawnie. Natomiast nasz model drugiego rzędu generuje drgania o stałej amplitudzie, natomiast w rzeczywistym obiekcie występują z rosnącą.

Sprawdzimy więc jaki wynik uzyskamy dla modelu z dodatkową inercją. W takim przypadku równanie różniczkowe opisujące układ zamknięty ma postać:

Więc funkcja wracająca pochodną w punkcie będzie miała postać:

def model2(x, t, A, B, C):
k = 0.4
dx0 = x[1]
dx1 = A*x[2]
dx2 = -k/C*x[0] – 1/C*x[2] + B/C
dx = [dx0, dx1, dx2]
return dx

gdzie k jest parametrem regulatora.

Jeśli Czytelnik zastosował inne nastawy, konieczna będzie tu zmiana w kodzie. Możemy uruchomić symulację dla przykładowych parametrów:

x0 = [x[0], (x[1]-x[0])/T, u[0]]
print(x0)
x_s = odeint(model2, x0, t, args=(10, 50, 0.05))

Jak widać stan początkowy, składa się teraz z trzech wartości. Uzyskany wynik prezentuje rysunek 19.

Rysunek 19. Przykładowy wynik uzyskany dla modelu trzeciego rzędu

Uruchommy jednak dopasowanie. Wykorzystany kod jest analogiczny do poprzedniego. Dobrane parametry to: a=9,77; b=46,80; c=0,048. Odejmując kz od b otrzymamy błąd ustawienia poziomu równy 2,8 stopnia. Porównując z parametrami dla modelu drugiego stopnia, otrzymaliśmy błąd wyznaczenia współczynnika a dopiero na drugim miejscu po przecinku. Jednak patrząc na rysunek 20 widzimy, że dodanie inercji o stałej czasowej 48 ms pozwoliło na uzyskanie znacznie lepszego dopasowania.