Ball & Beam (1)

Czwartek, 01 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. Często spotykanymi są różne wariacje na temat odwróconego wahadła. Innym popularnym rozwiązaniem jest kulka na belce, częściej spotykana pod angielską nazwą Ball & Beam. Koncepcja jest bardzo prosta, mamy belkę, po której toczy się piłka. Mierzymy pozycję kulki i sterujemy kątem nachylenia belki. Najprostszym celem jest umieszczenie kulki w zadanej odległości.

Lista elementów zastosowanych do budowy modelu:

serwomechanizm SG90,
czujnik CJVL53L0XV2,
Arduino Uno,
nakrętka 4 mm × 2,
śruba o średnicy 4 mm i długości 25 mm,
nakrętka 2 mm × 4,
śruba o średnicy 2 mm i długości 10 mm × 4,
drut stalowy o średnicy 1 mm około 10 cm,
piłeczka do tenisa stołowego.

Przybliżę czytelnikom sposób, w jaki mogą sami zbudować własny egzemplarz. Składa się on z kilku łatwo dostępnych podzespołów. Dzięki zastosowaniu powszechnie dostępnego druku 3D sama budowa także nie jest trudna. Na końcu zaprezentuję eksperymenty, jakie wykonałem, a działanie samego modelu można obejrzeć na filmie [1].

Mechanika

Dla sprawdzenia koncepcji, najpierw wykonałem prosty model z kartonu. Widzimy go na fotografii 1. Gdy przeszedł on pozytywnie wstępne testy, zaprojektowałem docelowy model. Część mechaniczna składa się z dwóch elementów. Zostały one zaprojektowane w programie Oneshape [2]. Obydwa z nich zostały wydrukowane na drukarce Prusa Mini. Pliki STL znajdują się w repozytorium [3] w folderze model.

Fotografia 1. Pierwszy prototyp sklejony z kartonu

Pierwszym elementem mechanicznym jest belka, po której toczy się kulka. Prezentuje ją rysunek 1. W początkowej części widzimy otwory montażowe dla czujnika odległości. Pod nimi znajduje się miejsce do zamocowania nakrętki, która będzie osią obrotu belki. W drugiej części widoczny jest otwór o średnicy 1 mm na ramię połączone z serwomechanizmem. Została ona wydrukowana z grubością warstwy 0,15 mm.

Rysunek 1. Model belki

Drugi element to podstawa. Pokazuje ją rysunek 2. Mieszczą się na niej podpora dla belki oraz mocowania dla serwomechanizmu i płytki Arduino Uno. Początkowo sama podpora była cieńsza, jednak powodowało to jej wygięcie w czasie druku. Ponieważ nie zawiera ona drobnych detali, została wydrukowana z grubością warstwy 0,25 mm.

Rysunek 2. Podpora belki

Montaż

Rozpoczynamy go od przykręcenia czujnika odległości do belki. Następnie na wcisk umieszczamy w niej nakrętkę o średnicy 4 mm. Wciągamy ją do otworu poprzez wkręcenie śruby z drugiej strony. Serwomechanizm przykręcamy do podstawy. Ostatnie oczko orczyka powiększamy do średnicy 1 mm.

Za pomocą śruby o średnicy 4 mm mocujemy belkę do podstawy. Zwracamy uwagę, aby była ona zamocowana na jej środku. Z drutu stalowego o średnicy 1 mm wyginamy kształt pokazany na rysunku 3, za pomocą którego łączymy orczyk serwomechanizmu z belką.

Rysunek 3. Ramię z drutu łączące belkę z serwomechanizmem

Na końcu do belki naklejamy miarkę z pliku model/beam_sticker.odg (rysunek 4).

Rysunek 4. Naklejka z linijką

Jej początek jest przesunięty tak, aby 0 oznaczało końcówkę czujnika odległości. Będziemy jej używać do kalibracji modelu, a później do wizualnej oceny jego pracy. Gotowy model pokazuje fotografia tytułowa oraz fotografia 2.

Fotografia 2. Gotowy model

Funkcję kulki pełni standardowa piłeczka do tenisa stołowego.

Elektronika

Schemat elektroniczny składa się z dwóch komponentów podłączonych do Arduino Uno. Rysunek 5 pokazuje sposób ich połączenia. Czujnik odległości zasilany jest napięciem 3,3 V i jest podłączony do interfejsu I²C na płytce Arduino Uno. Drugim komponentem jest serwomechanizm. Jest on zasilany napięciem 5 V i sterowany z pinu cyfrowego D9. Do połączenia używamy przewodów z wtykami goldpin. Nie jest potrzebna dodatkowa płytka stykowa. Wszystkie komponenty są podłączone bezpośrednio do Arduino.

Rysunek 5. Połączenie komponentów do Arduino

Kalibracja czujnika

Po zakończeniu budowy możemy przejść do eksperymentów. Pierwszym krokiem jest sprawdzenie działania czujnika odległości. Pomoże nam w tym program soft/VL53L0X_test/VL53L0X_test.ino, który znajdziemy w repozytorium [3]. Do komunikacji możemy użyć monitora portu szeregowego dostępnego w Arduino IDE. Program po odebraniu na porcie szeregowym litery s przesyła wyniki 100 kolejnych pomiarów odległości. Ja wykonałem pomiary dla położenia środka kulki od 30 do 180 milimetrów, przesuwając ją co 10 milimetrów. Położenie było wyznaczane względem podziałki naklejonej na belkę.

Zebrane dane znajdują się w notatniku Jupyter. Znajdziemy go w repozytorium jako jupiter/Distance sensor VL.ipynb [4]. Zmienna y to tablica położeń piłki, a x to dwuwymiarowa tablica, która zawiera po 100 pomiarów dla każdej odległości.

Dla każdego pomiaru została obliczona wartość średnia. Wyniki prezentuje rysunek 6. Widzimy na nim, że dla położenia 40 mm znajdujemy się poza zakresem czujnika, dlatego punkt ten nie zostanie użyty do kalibracji.

Rysunek 6. Średnia wartość zwracana przez czujnik dla różnych położeń kulki wraz z dopasowaną funkcją liniową

Następnie do zebranych danych dopasujemy wielomian pierwszego stopnia, czyli funkcję afiniczną:

Będziemy jej używać w sterowniku do wyrażenia położenia kulki w milimetrach. W tym celu użyjemy funkcji polyfit z biblioteki Numpy, np.

polyfit(xm[1:], y[1:], deg=1)

Dopasowana prosta jest także widoczna na rysunku 6. Uzyskane przeze mnie współczynniki to:

Znając współczynniki funkcji, możemy zobaczyć, jakie odległości otrzymaliśmy dla poszczególnych pomiarów. W tym celu za pomocą wyznaczonych parametrów przeliczymy uzyskane pomiary na milimetry, a następnie narysujemy ich histogramy. Uzyskany wynik prezentuje rysunek 7. Widzimy, że im większa odległość piłki od czujnika, tym otrzymujemy większy rozrzut uzyskanych wyników.

Rysunek 7. Histogram uzyskanych odległości dla różnych położeń kulki

Na końcu uzyskane parametry wpisujemy w programie soft/controler/controler.ino do funkcji:

int adc_to_mm(int x) {
return (94*x-801)/100;
}

Do obliczeń używamy liczb całkowitych, dlatego aby uzyskać lepszą dokładność, wpisujemy wartości współczynników pomnożone przez 100, a na końcu uzyskany wynik z powrotem konwertujemy na milimetry, dzieląc go także przez 100. Uzyskujemy w ten sposób liczby stałoprzecinkowe z dwoma dziesiętnymi miejscami po przecinku. Gdybyśmy zamiast potęgi dziesiątki użyli potęgi 2, moglibyśmy zastąpić dzielenie przez dużo prostsze obliczeniowo przesunięcie bitowe. Stracimy jednak wtedy na czytelności kodu. Gdyby się okazało, że nasze obliczenia zajmą zbyt dużo czasu, jest to jedno z miejsc, gdzie możemy go trochę zaoszczędzić.

Aplikacja controler

Podczas kolejnych eksperymentów na Arduino będzie wgrana aplikacja soft/controler/controler.ino. Dlatego teraz przyjrzymy się jej ogólnej strukturze. Pozwala na sterowanie oraz odczytywanie aktualnego stanu układu. Jak każdy program na Arduino składa się z dwóch części. Funkcja setup jest wywoływana raz na początku, a następnie w nieskończonej pętli wywoływana jest funkcja loop.

W funkcji setup następuje konfiguracja peryferiów. Prędkość transmisji portu szeregowego jest ustawiana na 115200B, inicjalizowany jest serwomechanizm oraz czujnik odległości. Na końcu konfigurowane jest przerwanie od Timera2. Jest ono wywoływane co 1 ms. Co 50 wywołań przerwania następuje ustawienie flagi next_cycle oraz zmiana stanu wyjścia 12 na przeciwny. Pozwala to za pomocą oscyloskopu zbadać okres regulatora.

Na początku pętli głównej program czeka na ustawienie flagi next_cycle, a następnie ją resetuje. Dzięki temu kolejne kroki naszego regulatora są wykonywane co Ts=50 ms. Przebieg jednego cyklu pokazuje rysunek 8.

Rysunek 8. Pojedynczy cykl pracy regulatora

Na początku następuje sprawdzenie, czy przyszły nowe dane z portu szeregowego. Listę przyjmowanych rozkazów pokazuje tabela 1. Następnie odczytywany jest aktualny stan czujnika odległości. Tu wywoływana jest funkcja adc_to_mm, której współczynniki dobraliśmy w poprzednim akapicie.

Kolejnym etapem jest obliczenie sterowania. Obecnie przygotowane są trzy algorytmy. Pierwszy z nich to sterowanie w układzie otwartym. Ideę jego działania prezentuje rysunek 9. Polega ono na zadawaniu przez operatora położenia serwa i nie używa w ogóle sprzężenia zwrotnego od położenia kulki.

Rysunek 9. Schemat sterowania w otwartej pętli

Drugi z nich to klasyczny regulator proporcjonalno-różniczkujący określany też od angielskiej nazwy skrótem PD. Jego schemat widzimy na rysunku 10. Blok z-¹ oznacza opóźnienie sygnału o jeden krok regulatora. Operator zadaje wartość zadaną z. Na jej podstawie jest wyznaczany uchyb regulacji e. Człon różniczkowalny jest przybliżony jako różnica obecnej i poprzedniej wartości uchybu.

Rysunek 10. Schemat regulatora PD

Sterowanie obliczane jest wzorem:

pid_u = (pid_p*e + pid_d*de) / 10

Obie nastawy są zapisane jako liczby dziesiętne z jedną cyfrą po przecinku, dlatego na końcu pojawia się dzielenie przez 10. Do wyliczonego sterowania dodawana jest wartość u0 odpowiadająca stanowi równowagi belki. Ostatni regulator korzysta ze stanu układu, który jest wyznaczany za pomocą filtru Kalmana. Przyjrzymy się mu w kolejnej części artykułu.

Ostatnim krokiem pętli regulatora jest przesłanie aktualnego stanu układu do komputera. Dane mają stały format. W każdym takcie regulatora jest przesyłanych jedenaście liczb oddzielonych spacjami. Po kolei są to:

nastawa P,
nastawa D,
wartość zadana,
aktualny pomiar położenia,
1 składowa estymowanego stanu,
2 składowa estymowanego stanu,
3 składowa estymowanego stanu,
uchyb regulacji,
wyliczona wartość sterowania,
zadana pozycja równowagi u0,
wartość sterowania u wysłana do serwomechanizmu.

Do komunikacji na komputerze można użyć dowolnego monitora portu szeregowego, na przykład tego wbudowanego w Arduino. Przygotowałem także aplikację w postaci strony WWW, którą można znaleźć w repozytorium w folderze gui albo pod adresem [5]. Jej wygląd pokazuje rysunek 11. W lewym górnym rogu znajduje się przycisk Connect pozwalający na połączenie płytki. Po kliknięciu pokaże się okno dialogowe z listą dostępnych portów szeregowych. Poniżej znajduje się przełącznik get data, pozwalający na włączenie/wyłączenie przesyłania danych. Dalej znajdują się pola tekstowe pozwalające na zmianę nastaw oraz przyciski do zmiany trybu pracy regulatora. W środkowej części ekranu prezentowane są wykresy. Pokazują one aktualny stan modelu. Na górze jest to wartość zadana z oraz aktualne dane z czujnika x. Niżej pokazane jest sterowanie, a na samym dole estymata stanu modelu (zostanie ona omówiona dokładniej w kolejnej części).

Rysunek 11. Aplikacja webowa do komunikacji z modelem

Na samym dole znajduje się pole tekstowe z kolejnymi stanami modelu zapisanymi jako tablica w języku Python. Jeżeli chcemy przeanalizować zebrane dane, możemy skopiować jego zawartość, dodać na początku i końcu nawias kwadratowy, usunąć ostatni przecinek i otrzymujemy dwuwymiarową tablicę.

Możemy ją wtedy wkleić do notatnika Jupyter. Przycisk Clear data pozwala na wyczyszczenie tego pola.

Kolejne zagadnienia i opis aplikacji będą kontynuowane w następnym wydaniu "Elektroniki Praktycznej".

Rafał Kozik
rafkozik@gmail.com

Bibliografia: