Dzisiaj omówię, w jaki sposób zaprojektować regulator obrotów silnika. Pokażę jak zebrać dane pomiarowe, a następnie dokonać ich analizy i obróbki. W trakcie tego procesu wytłumaczę jakich narzędzi używam i w jaki sposób je wykorzystuję.

Podobny artykuł napisałem kiedyś na Forbocie – link. Jednak jest kilka istotnych różnic. Przede wszystkim tam był brany pod uwagę sam układ enkoder – silnik bez kół i całej konstrukcji mechanicznej robota. Przez to nie musiałem wtedy walczyć z takimi zakłóceniami i nieliniowościami. Poza tym wtedy wyprowadziłem model na podstawie pomiaru dla jednej wartości wypełnienia. Tutaj wykonuję pomiary dla trzech wartości i patrzę jak wyznaczone modele sprawdzają się dla innych danych pomiarowych.

Opis zagadnienia

Moim celem jest stworzenie układu regulacji otrzymującego na wejście oczekiwaną wartość prędkości robota wyrażonej w tickach enkodera w zadanym okresie czasu (r(t)) i obliczającego na bazie aktualnego pomiaru z enkoderów (y(t)) oraz modelu matematycznego wartość wypełnienia PWM podawanego na silniki (u(t)). Na początek skupię się jedynie na ruchu postępowym, skręcanie zostawię sobie na później. Jest to klasyczny problem sterowania, który można przedstawić za pomocą schematu:

Do realizacji tego zadania użyję regulatora PID, który jest bardzo prosty w  implementacji i daje zadowalające rezultaty.

Identyfikacja obiektu

Identyfikacja obiektu polega na wyprowadzeniu modelu matematycznego obiektu na podstawie danych pomiarowych. Aby przeprowadzić identyfikację trzeba zaprojektować odpowiedni eksperyment pozwalający na zebranie potrzebnych danych. Posłużę się tutaj jednym z najprostszych możliwych rozwiązań – wyznaczę model transmitancyjny obiektu na podstawie pobudzenia układu skokiem jednostkowym. Czyli tłumacząc na ludzki język podam na zatrzymane silniki stałą wartość mocy i zbiorę pomiary prędkości w równych odstępach czasu obrazujące, jak prędkość wzrasta od 0 do wartości docelowej.

Program testowy

Aby wykonać pomiary musiałem napisać odpowiedni program testowy. Program ten posiada następujące funkcjonalności:

• Aktywacja zbierania charakterystyki skokowej za pomocą przycisku.
• Na silniki podawana jest stała prędkość przez 1 sekundę.
• W trakcie tej sekundy co 10 ms pobierane są odczyty z enkoderów.
• Po zakończeniu pomiaru zebrane dane są wysyłane przez UART.

Między wciśnięciem przycisku, a rozpoczęciem pomiaru są 2 s różnicy, żebym zdążył odsunąć się po włączeniu testu. Okres próbkowania enkoderów ustawiłem na 10 ms, aby przez ten czas zdążyła się wykonać jakaś sensowna ilość ticków. Początkowo miałem 1 ms i przy 20% mocy na silniki w tym czasie robiły się 3 ticki. Jest to za mała wartość, aby można było stosować płynną regulację – skok o 1 tick to zmiana sygnału wyjściowego o 33%. Kod programu testowego można znaleźć w projekcie na GitHubie, w folderze test/hw_test/motor_ident. Przy pomocy tego programu pobrałem pomiary dla 20%, 40% i 60% wypełnienia PWM.

Wyznaczanie modelu

Model silnika, który chcemy zidentyfikować ma postać transmitancji pierwszego rzędu:

gdzie K to wzmocnienie układu, a T to stała czasowa. Wzmocnienie układu to stosunek wartości pobudzenia do wartości sygnału wyjściowego w stanie ustalonym. Stała czasowa natomiast określa szybkość dochodzenia układu do stanu ustalonego w odpowiedzi na pobudzenie. Jest to czas, po którym osiągnięta zostanie wartość 0.632 * stan ustalony. Można ją estymować prowadząc styczną do sygnału w początkowej fazie. Wartość T można odczytać z wykresu odpowiedzi skokowej w miejscu przecięcia stycznej z wartością w stanie ustalonym.

Aby wyznaczyć model na podstawie danych pomiarowych napisałem skrypt w Pythonie. Wykorzystałem w tym celu bibliotekę numpy. Aby wyznaczyć wartość w stanie ustalonym, obliczyłem średnią arytmetyczną dla próbek od 0.7 s do 0.9 s. Wartości te dobrałem analizując wykres danych pomiarowych. Zadanie to realizuje poniższy kod:

steady = np.mean(data[mean_samples[0]:mean_samples[1]])

Aby wyznaczyć stałą czasową, należy poprowadzić styczną do zmierzonego przebiegu w początkowej fazie. Wybrałem więc próbki z pierwszego 0.1 s pomiarów i wykonałem aproksymację linii prostej najlepiej pasującej do tych pomiarów. Linia prosta to wielomian pierwszego rzędu, dlatego wykorzystałem funkcję aproksymacji wielomianowej z numpy. Następnie przekształciłem równanie prostej:

Na:

Jako y podstawiłem wartość w stanie ustalonym, a pod a i b współczynniki otrzymane z aproksymacji. Realizuje to następujący kod pythonowy:

    coef = np.polyfit(time_vect[0:time_const_samples], data[0:time_const_samples], 1)
    time_const = (steady - coef[1]) / coef[0]

W skrypcie umieściłem jeszcze dodatkowy kod prezentujący wyprowadzanie modelu na wykresach. Oto efekty:

Wypełnienie 20% PWM

Wypełnienie 40% PWM

Wypełnienie 60% PWM

Czerwony kolor to lewy silnik, niebieski to prawy. Jak widać, lewy silnik obraca się nieco szybciej. Poza tym dla wypełnienia 20% pojawiają się na prawym silniku zakłócenia w stanie ustalonym. Oba te efekty są wynikiem niedoskonałości konstrukcji mechanicznej.

Parametry modeli dla poszczególnych silników i wypełnień znajdują się w outpucie pythonowego skryptu:

Drawing plot from data file: ../data/motor_20.txt
Steady state velocity for motor left is 28.5.
Steady state velocity for motor right is 21.1.
Time constant for left motor velocity is 0.301442.
Time constant for right motor velocity is 0.243255.

Drawing plot from data file: ../data/motor_40.txt
Steady state velocity for motor left is 67.3.
Steady state velocity for motor right is 63.6.
Time constant for left motor velocity is 0.274276.
Time constant for right motor velocity is 0.222809.

Drawing plot from data file: ../data/motor_60.txt
Steady state velocity for motor left is 96.6.
Steady state velocity for motor right is 89.45.
Time constant for left motor velocity is 0.144905.
Time constant for right motor velocity is 0.135906.

Aby z wartości w stanie ustalonym uzyskać wzmocnienie K, należy podzielić ją przez wartość sygnału wejściowego, czyli wypełnienia PWM. Pełną postać skryptu przestawia listing:

import numpy as np
import matplotlib.pyplot as plt


def rise_time_and_time_const(time_vect, data, mean_samples, rise_time_samples, time_const_samples):
    steady = np.mean(data[mean_samples[0]:mean_samples[1]])
    coef = np.polyfit(time_vect[0:time_const_samples], data[0:time_const_samples], 1)
    rise_time = [x * coef[0] + coef[1] for x in time_vect[0:rise_time_samples]]
    time_const = (steady - coef[1]) / coef[0]

    return steady, rise_time, time_const


def draw_plots(file, mean_samples, rise_time_samples, time_const_samples):
    print("\nDrawing plot from data file: {}".format(file))

    left, right = np.loadtxt(file, dtype=int, unpack=True)
    right = -right

    step = 0.01
    time = np.linspace(step, 1, 1 / step)

    left_steady, left_rise_time, left_t = \
        rise_time_and_time_const(time, left, mean_samples, rise_time_samples, time_const_samples)
    right_steady, right_rise_time, right_t = \
        rise_time_and_time_const(time, right, mean_samples, rise_time_samples, time_const_samples)

    print("Steady state velocity for motor left is {:.6}.".format(left_steady))
    print("Steady state velocity for motor right is {:.6}.".format(right_steady))

    print("Time constant for left motor velocity is {:.6}.".format(left_t))
    print("Time constant for right motor velocity is {:.6}.".format(right_t))

    plt.figure()
    plt.plot(time, left, 'r')
    plt.plot(time, right, 'b')
    plt.plot(time, [left_steady] * len(time), 'r')
    plt.plot(time, [right_steady] * len(time), 'b')
    plt.plot(time[0:rise_time_samples], left_rise_time, 'r')
    plt.plot(time[0:rise_time_samples], right_rise_time, 'b')
    plt.plot(left_t, left_steady, 'ro')
    plt.plot(right_t, right_steady, 'bo')

motor_20 = "../data/motor_20.txt"
motor_40 = "../data/motor_40.txt"
motor_60 = "../data/motor_60.txt"

draw_plots(motor_20, [70, 90], 30, 10)
draw_plots(motor_40, [70, 90], 30, 10)
draw_plots(motor_60, [70, 90], 20, 10)

plt.show()

Walidacja modelu

Na podstawie identyfikacji otrzymałem dla każdego silnika po trzy różne modele. Teraz trzeba w jakiś sposób wybrać najlepszy. Każdy z modeli był wyznaczony dla konkretnego wypełnienia PWM. Aby sprawdzić dokładność modelu zobaczę, jak się zachowuje dla pozostałych dwóch wypełnień.

W tym momencie niestety musiałem zrezygnować z Pythona i przesiadłem się na MATLABa. Szkoda, bo chciałem sprawdzić jak numpy i scipy wraz z Python Control Library sprawdzą się w zadaniach z automatyki. Jednak aby skorzystać z tych dobrodziejstw pod Windowsem, trzeba doinstalować specjalne biblioteki optymalizujące obliczenia matematyczne dla procesorów Intela (MKL – Math Kernel Library) i zainstalować specjalną wersję numpy z wsparciem tych optymalizacji. Alternatywą jest zainstalowanie dedykowanego środowiska np. Enthaught Canopy. Może jeszcze znajdę czas, żeby pokonfigurować to środowisko i zobaczyć jak Python się sprawdza obliczeniach inżynierskich.

Aby sprawdzić zgodność modelu z danymi pomiarowymi, należy wyznaczyć dla niego odpowiedź skokową i nałożyć ją na wykres. Oto otrzymane przeze mnie rezultaty:

Po analizie wykresu stwierdziłem, że najlepsze wyniki dają modele dla wypełnienia 40% PWM i to właśnie one posłużą mi do doboru nastaw dla regulatora PID.

Dobór nastaw PID

Aby dobrać nastawy PID dla wyprowadzonego modelu transmitancyjnego wykorzystam MATLABowy pakiet Simulink. Zawiera on automatyczny tuner dla regulatora PID. Jego użycie jest bardzo proste:

• tworzymy schemat układu sterowania,
• wprowadzamy parametry poszczególnych bloków i całej symulacji,
• uruchamiamy tuner, który oblicza dla nas optymalne nastawy,
• ręcznie modyfikujemy nastawy, aby uzyskać pożądane parametry sygnału wyjściowego.

Dzięki optymalizacjom numerycznym zaszytym w bloku PID Simulinka, nie musimy całego procesu przeprowadzać ręcznie.

Schemat naszego układu wygląda następująco:

Układ jest pobudzany za pomocą skoku jednostkowego (blok Step). Następnie po odjęciu od wartości zadanej wartości wyjściowej, znajduje się dyskretnoczasowy blok PID. Sygnał sterujący z tego bloku jest zapisywany do zmiennej (To Workspace1 – control) oraz przekazywany na silnik (blok Transfer Function – Continuous). Parametry bloku silnika uzupełniamy wartościami wyznaczonymi wcześniej. Sygnał wyjściowy jest również zapisywany do zmiennej (To Workspace – out). W ustawieniach całej symulacji oraz w poszczególnych blokach ustawiamy czas próbkowania na wartość odpowiadającym próbkowaniu w rzeczywistym układzie – czyli w moim przypadku 10 ms.

Konfigurację bloku PID przedstawiają dwa powyższe screeny. Ustawiamy blok jako dyskretnoczasowy, czas próbkowania stały o wartości 10 ms. W zakładce Advanced ustawiamy górny i dolny poziom nasycenia sygnału sterującego. W moim przypadku to -100 i 100. W końcu nie da się ustawić na silnik większego wypełnienia niż 100%. Opcję windup ustawiamy na clamping – zainteresowanych tą opcją odsyłam tutaj (link).

Po dokonaniu takiej konfiguracji możemy kliknąć przycisk Tune i poczekać, aż program sam nam dobierze nastawy. W nowym oknie możemy zobaczyć wykres ze starym i nowym przebiegiem, a także suwaki umożliwiające dopasowanie wykresu. Po wstępnym wyborze nastaw przez tuner warto pobawić się jeszcze nastawami ręcznie, aby uzyskać pożądane parametry sygnału.

Po wykonaniu tych czynności otrzymałem nastawy dla mojego regulatora:

P = 2.6, I = 11.6, D = 0

Okazało się, że te same nastawy mogę wykorzystać dla obu silników.

Podsumowanie

W tym wpisie przedstawiłem proces projektowania regulatora silników od zebrania danych pomiarowych do wyznaczenia nastaw regulatora PID. Zostało jeszcze sprawdzenie tych nastaw na rzeczywistym układzie – czyli robocie – i dokonanie ewentualnych korekt. Tym się zajmę dopiero za jakiś czas, bo aktualnie nie mam dostępu do robota. W źródłach na GitHubie w katalogu scripts można podejrzeć wykorzystane skrypty Python i MATLAB – zmiany te są na gałęzi dev – na master jeszcze ich nie ma.