Sterowanie prędkością – regulator PID

Następny artykuł z serii sterowania napędami będzie opisywał sterowanie prędkością obrotową silnika DC przy zastosowaniu regulatora PID. W materiale postaram się przedstawić podstawy teoretyczne działania regulatora oraz implementację sterowania na zestawie Nucelo-L476RG.

Jest to trzeci artykuł z serii. Jeżeli jeszcze nie zapoznałeś się z poradnikiem „Sterowanie silnikiem DC” oraz „Obsługa enkodera inkrementalnego„, zachęcam do tego przed przeczytaniem poniższego tekstu. Artykuł będzie bazował na opisanym sterowaniu oraz wykorzystywał zastosowane wcześniej podzespoły: silnik Micro Pololu, moduł ze sterownikiem DRV8835, enkoder inkrementalny oraz płytkę Nucelo-L476RG.

Regulator PID

Regulator PID, czyli regulator proporcjonalno-całkująco-różniczkujący (z j. angielskiego proportional–integral–derivative controller) jest jednym z najpopularniejszych regulatorów stosowanych w automatyce i robotyce. Według statystyk wynika, że nawet ok. 90% regulatorów używanych w przemyśle to regulatory typu PID. Stosowane są do sterowania praktycznie każdą wielkością fizyczną, w tym m.in. temperaturą, ciśnieniem, prędkością czy siłą, W robotyce amatorskiej najpopularniejszym zastosowaniem algorytmu PID jest sterowanie robotem typu Linefollower, gdzie regulator wykorzystywany jest do sterowania napędami na podstawie odczytów z czujników linii.

Regulator PID to rodzaj algorytmu, który wykorzystuje sprzężenie zwrotne, czyli informację z czujnika, na podstawie której oblicza tzw. uchyb. Jest to różnica między wartością zadaną (docelową wartością, jaką chcemy osiągnąć), a wartością zmierzoną (daną z czujnika). Na podstawie uchybu obliczany jest sygnał sterujący, który ma na celu osiągnąć wartość zadaną, czyli zredukować uchyb do minimum.

Jak sama nazwa wskazuje, regulator PID składa się z trzech członów:

• proporcjonalnego – podstawowy człon, który zwiększa bądź zmniejsza sygnał sterujący proporcjonalnie do uchybu; kompensuje uchyb bieżący
• całkującego – wykorzystuje całkowanie sygnału; kompensuje sumę uchybów z przeszłości
• różniczkującego – wykorzystuje różniczkowanie sygnału; kompensuje uchyby , które wystąpią w przyszłości

Wzór regulatora przedstawia się następująco (ta postać wydaje mi się najbardziej zrozumiała):

gdzie:

• u(t) – sygnał sterujący,
• e(t) – uchyb,
• Kp – wzmocnienie członu proporcjonalnego,
• Ki – wzmocnienie członu całkującego,
• Kd – wzmocnienie członu różniczkującego.

Schemat blokowy algorytmu PID przedstawia się jak na rysunku poniżej.

• 

Pełny regulator PID jest formą najbardziej zaawansowaną, jednak co za tym idzie nie zawsze łatwą do implementacji i dostrojenia. W zależności od sterowanego procesu, często stosuje się uproszczone wersje regulatorów z dwoma bądź jednym członem np. P, PI lub PD.

Zaprojektowany regulator PID wymaga jeszcze doboru współczynników wzmocnienia Kp, Ki oraz Kd, czyli tzw. strojenia. Istnieje kilka metod doboru nastaw:

• testy symulacyjne – tzw. strojenie ręczne, czyli dobór nastaw na podstawie wiedzy i doświadczenia o działaniu poszczególnych członów regulatora. Nie wykonujemy tutaj żadnych obliczeń i nie tworzymy modeli procesu, a nastawy dobieramy doświadczalnie na podstawie obserwacji działania obiektu.
• metody inżynierskie:
  • metoda Zieglera-Nicholsa – polega na początkowym wyłączeniu członów I i D. Stopniowo zwiększa się wartość Kp do momentu, aż oscylacje będą miały stałą amplitudę. Następnie w zależności od tego, jaki regulator chcemy użyć (P, PI, PID), mnożymy współczynnik Kp oraz okres oscylacji przez odpowiednie liczby i uzyskujemy wartości wzmocnień Ki oraz Kd.
  • metoda Cohena-Coona – wymaga stworzenia modelu procesu i zaawansowanych obliczeń matematycznych
  • metoda Passena – modyfikacja metody Zieglera-Nicholsa
  • metoda Hassena i Offereissena – modyfikacja metody Zieglera-Nicholsa
• metody wykorzystujące specjalistyczne oprogramowanie – oprogramowanie zbiera dane z działania układu i na ich podstawie dobiera optymalne wartości nastaw. Najczęściej stosowania metoda w przemyśle ze względu na szybkość i niezawodność.
• automatyczny dobór nastaw – czasami ze względu na specyfikę procesu wykorzystuje się metodę, w której nastawy są automatycznie dobierane w trakcie procesu pod wpływem zmian dynamiki obiektu.

Do oceny działania regulatora wykorzystuje się odpowiedź skokową. najważniejszymi parametrami odpowiedzi jest:

• przeregulowanie – różnica między wartością ustaloną a największą wartością zadaną przez regulator
• czas ustalania – czas do osiągnięcia wartości zadanej, najczęściej określa się „akceptowalny” błąd dla wartości osiągniętej względem wartości zadanej np. 5%

• 

Przy strojeniu regulatora zależy nam na tym, aby przeregulowanie było jak najmniejsze, a czas ustalania jak najkrótszy.

Jak widać, regulator PID jest dość zaawansowanym algorytmem, który w praktyce wymaga dodatkowych modyfikacji i usprawnień. Jednym z problemów, jaki zaobserwowano w trakcie badań nad regulatorem PID, a konkretnie nad członem całkującym, jest to, że może on magazynować błąd, który przekracza maksymalną wartość regulowanej zmiennej, a jego powrót do „normalnych” wartości zajmuje niepotrzebnie czas. Jest to tzw. windup. Aby zapobiec temu zjawisku, najczęściej stosuje się ograniczenie maksymalnej i minimalnej wartości, jaką może osiągnąć człon całkujący, wprowadzając odpowiednią granicę.

Konfiguracja układów peryferyjnych

Znając podstawy działania regulatora PID możemy przejść do jego implementacji. Jak wspomniałem wcześniej, zastosujemy go do sterowania prędkością obrotową silnika DC ze sprzężeniem zwrotnym w postaci odczytu z enkodera inkrementalnego. W ćwiczeniu zastosuję silnik DC Micro Pololu z przekładnią 150:1, moduł Pololu ze sterownikiem DRV8835 oraz enkoder inkrementalny z czujnikiem Halla, a program napiszemy z wykorzystaniem zestawu Nucelo-L476RG. Poniżej przedstawię tylko pobieżnie konfigurację mikrokontrolera i jego układów peryferyjnych. Dokładny opis znajduje się w poprzednich częściach poradnika: „Sterowanie silnikiem DC” oraz „Obsługa enkodera inkrementalnego„.

Tworzymy zatem nowy projekt wybierając „File->New->STM32 Project”. Przechodzimy przez wstępną konfigurację projektu i zabieramy się za konfigurację wyjść mikrokontrolera. Ja wygenerowałem projekt z domyślną konfiguracją dla płytki Nucleo, dlatego zegary i część pinów mam już skonfigurowane.

Do sterowania w trybie PHASE/ENABLE będziemy potrzebowali trzech wyjść: dwóch wyjść cyfrowych (do sterowanie pinem MODE oraz pinem PHASE) oraz wyjścia PWM. Wykorzystamy piny PA8 i PA9 jako wyjścia GPIO oraz pin PB10 jako wyjście PWM (TIMER 2, kanał 3). Przypiszemy im również etykiety, co ułatwi nam pracę z kodem. Następnie ustawiamy parametry pracy TIMER 2 tak, aby na wyjściu generowany sygnał PWM miał częstotliwość 20 kHz, a prędkością będziemy mogli sterować z rozdzielczością 1000. Zwiększyłem tutaj rozdzielczość sterowania sygnałem PWM względem projektu przedstawianego w poprzednich częściach ze względu na potrzebę lepszej kontroli wyjścia za pomocą regulatora PID.

• 

Do obsługi enkodera wykorzystamy TIMER 3. Aby go skonfigurować, wybieramy TIMER 3 w trybie „Combined Channels -> Encoder Mode”. W przypadku trybu „Encoder Mode”, dzielenie zegara nie będzie nam potrzebne, dlatego wartość Prescaler-a ustawiamy na 0. Counter Period, czy wartość do jakiej będzie zliczał TIMER zostawimy na wartości maksymalnej. Przy odczycie prędkości będziemy samodzielnie resetowali licznik. Dodatkowo chcemy, aby układ reagował na impulsy z obu kanałów, zatem ustawiamy tryb „Encoder Mode TI1 and TI2” oraz Polarity jako „Rising Edge”. Wykorzystamy najwyższy poziom filtracji, zatem „Input Filter” ustawimy na 15. Wybierając tryb „Encoder Mode” na pinach PA6 oraz PA7 naszego mikrokontrolera pojawiły się wyjścia TIM3_CH1 i TIM3_CH2. Możemy na tych pinach ustawić etykiety – OUT_A i OUT_B

• 

Pomiar prędkości obrotowej silnika chcemy zrealizować w ten sposób, że co określony odcinek czasu (w naszym przypadku co 100 ms) odczytamy wartość zliczoną przez TIMER 3 pracujący w trybie „Encoder Mode” i odpowiednio przeliczymy ją na prędkość obrotową silnika. Potrzebujemy zatem jeszcze jednego TIMER-a, który będzie generował nam przerwanie z częstotliwością 10 Hz. Do tego celu użyjemy dodatkowego układu – TIMER 6. Aby uzyskać częstotliwość 10 Hz musimy podzielić zegar przez 8 000 000. Biorąc pod uwagę maksymalne wartości, jakie możemy wpisać do ustawień Prescaler i Counter Period, konfiguracja TIMER 6 będzie wyglądała następująco.

• 

Pełna konfiguracja pinów wraz z ustawionymi etykietami będzie przedstawiała się jak na poniższym obrazku.

• 

Mając tak skonfigurowany mikrokontroler, możemy wygenerować projekt („Project->Generate Code” lub „Alt+K„) i przejść do napisania algorytmu regulatora PID.

Implementacja

Do projektu dodajemy pliki „drv8835.h”, „drv8835.c”, „motor.c” oraz „motor.h” do sterowania silnikiem i obsługi enkodera, które opisane są szczegółowo w pierwszej i drugiej części poradnika – „Sterowanie silnikiem DC” i Obsługa enkodera inkrementalnego„, Stworzymy też dwa pliki do obsługi regulatora PID: „pid_controller.c” (w folderze Core->Src) oraz „pid_controller.h” (w folderze Core->Inc).

W pliku „pid_controller.h” tworzymy strukturę do obsługi regulatora. Do implementacji pełnego regulatora będziemy potrzebowali przechowywać oprócz wzmocnień i ograniczenia anti-windup, dwie wartości:

• previous_error – poprzednią wartość błędu potrzebną do realizacji różniczkowania
• total_error – sumę zliczonych błędów potrzebną do realizacji całkowania

typedef struct
{
	int previous_error; 		//Poprzedni błąd dla członu różniczkującego
	int total_error;		//Suma uchybów dla członu całkującego
	float Kp;			//Wzmocnienie członu proporcjonalnego
	float Ki;			//Wzmocnienie członu całkującego*/
	float Kd;			//Wzmocnienie członu różniczkującego*/
	int anti_windup_limit;		//Anti-Windup - ograniczenie członu całkującego*/
}pid_str;

W pliku „pid_controller.c” tworzymy funkcję inicjalizującą „pid_init()”, w której przypisujemy wartości wzmocnień i anti_windup oraz funkcję „pid_reset()”.

void pid_init(pid_str *pid_data, float kp_init, float ki_init, float kd_init, int anti_windup_limit_init)
{
	pid_data->previous_error = 0;
	pid_data->total_error = 0;

	pid_data->Kp = kp_init;
	pid_data->Ki = ki_init;
	pid_data->Kd = kd_init;

	pid_data->anti_windup_limit = anti_windup_limit_init;
}

void pid_reset(pid_str *pid_data)
{
	pid_data->total_error = 0;
	pid_data->previous_error = 0;
}

Następnie implementujemy funkcje „pid_calculate()”, która będzie obliczała wyjście regulatora. Tworzymy zmienne lokalne do obliczenia błędu i odpowiedzi poszczególnych członów. Następnie liczmy błąd oraz dodajemy go do sumy błędów. Dalej obliczamy odpowiedzi członu proporcjonalnego, całkującego i różniczkującego. Na koniec realizujemy zabezpieczenie anti-windup, przypisujemy aktualną wartość błędu jako poprzednią i zwracamy wyjście regulatora jako liczbę całkowitą.

int pid_calculate(pid_str *pid_data, int setpoint, int process_variable)
{
	int error;
	float p_term, i_term, d_term;

	error = setpoint - process_variable;		//obliczenie uchybu
	pid_data->total_error += error;			//sumowanie uchybu

	p_term = (float)(pid_data->Kp * error);		//odpowiedź członu proporcjonalnego
	i_term = (float)(pid_data->Ki * pid_data->total_error);	//odpowiedź członu całkującego
	d_term = (float)(pid_data->Kd * (error - pid_data->previous_error));//odpowiedź członu różniczkującego

	if(i_term >= pid_data->anti_windup_limit) i_term = pid_data->anti_windup_limit;	//Anti-Windup - ograniczenie odpowiedzi członu całkującego
	else if(i_term <= -pid_data->anti_windup_limit) i_term = -pid_data->anti_windup_limit;

	pid_data->previous_error = error;	//aktualizacja zmiennej z poprzednią wartością błędu

	return (int)(p_term + i_term + d_term);		//odpowiedź regulatora
}

Teraz w pliku „motor.h” dodajemy definicje parametrów dla silnika A. Jak dobrać wartości wzmocnień przedstawiam w dalszej części artykułu. Modyfikujemy również strukturę, która potrzebuje teraz przechowywać informację o regulatorze PID oraz zarówno o zmierzonej prędkości, jak i zadanej (w RPM, a nie jako wypełnienie PWM jak używaliśmy wcześniej).

#define MOTOR_A_Kp					4.5
#define MOTOR_A_Ki					0.8
#define MOTOR_A_Kd					0.5
#define MOTOR_A_ANTI_WINDUP			        1000

typedef struct
{
	TIM_HandleTypeDef *timer;	//timer obsługujący enkoder silnika

	uint16_t resolution;		//rozdzielczość silnika

	int pulse_count;		//zliczone impulsy
	int measured_speed;		//obliczona prędkość silnika
	int set_speed;			//zadana prędkość silnika

	int actual_PWM;			//wartość PWM

	pid_str pid_controller;
}motor_str;

W pliku „motor.c” modyfikujemy funkcję inicjalizującą:

void motor_str_init(motor_str *m, TIM_HandleTypeDef *tim)
{
	m->timer = tim;
	m->resolution = ENCODER_RESOLUTION * TIMER_CONF_BOTH_EDGE_T1T2 * MOTOR_GEAR;

	m->pulse_count = 0;
	m->measured_speed = 0;
	m->set_speed = 0;
        m->actual_PWM = 0;
}

W funkcji „motor_calculate_speed()” dodajemy obsługę wyjścia z regulatora PID. Wywołujemy algorytm PID na podstawie zadanej prędkości obrotowej podanej w RPM oraz wartości zmierzonej. Wyjście dodajemy do wartości wypełnienia PWM. Następnie w zależności od wyjścia PWM (może być ujemne), sterujemy silnikiem.

void motor_calculate_speed(motor_str *m)
{
	motor_update_count(m);

	m->measured_speed = (m->pulse_count * TIMER_FREQENCY * SECOND_IN_MINUTE) / m->resolution;

	int output = pid_calculate(&(m->pid_controller), m->set_speed, m->measured_speed);

	m->actual_PWM += output;

	if(m->actual_PWM >= 0)
	{
		drv8835_set_motorA_direction(CW);
		drv8835_set_motorA_speed(m->actual_PWM);
	}
	else
	{
		drv8835_set_motorA_direction(CCW);
		drv8835_set_motorA_speed(-m->actual_PWM);
	}
}

Funkcję „motor_calculate_speed” wywołujemy tak jak dotychczas w przerwaniu od TIMER 6 w pliku „main.c”.

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance == TIM6)
	{
		motor_calculate_speed(&motorA);
	}
}

W pliku „motor.c” dodajemy też funkcję pozwalającą na zadanie prędkości w RPM.

void motor_set_speed(motor_str *m, int set_speed)
{
	if(set_speed != m->set_speed)
		pid_reset(&(m->pid_controller));

	m->set_speed = set_speed;
}

Pamiętać należy, żeby dodać deklaracje nowych funkcji w plikach „*.h”, aby były widoczne w innych plikach.

W funkcji głównej main() wywołujemy inicjalizację sterownika, silnika oraz regulatora PID. Następnie Uruchamiamy TIMER odpowiedzialny za obsługę enkodera TIMER 3 oraz bazowy TIMER 6. Następnie w pętli while(1) piszemy fragment kodu, który będzie nam zadawał prędkości z tabeli w odstępem 5 s.

/* USER CODE BEGIN 2 */
  drv8835_init();
  motor_str_init(&motorA, &htim3);
  pid_init(&(motorA.pid_controller), MOTOR_A_Kp, MOTOR_A_Ki, MOTOR_A_Kd, MOTOR_A_ANTI_WINDUP);

  HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
  HAL_TIM_Base_Start_IT(&htim6);

  int speed_table[] = {50, 100, -100, 10};

  int i = 0;
  uint32_t time_tick = HAL_GetTick();
  uint32_t max_time = 5000;

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  if((HAL_GetTick() - time_tick) > max_time)
	  {
		  time_tick = HAL_GetTick();

		  motor_set_speed(&motorA, speed_table[i++]);

		  if(i >= 4)
			  i = 0;
	  }

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */

Przed uruchomieniem projektu powinniśmy jeszcze połączyć sterownik i enkoder z zestawem Nucleo i podłączyć silnik. Robimy to według poniższego schematu.

• 

Dobór parametrów regulatora PID – strojenie metodą „ręczną”

Do doboru parametrów wykorzystamy aplikację STM32CubeMonitor. Tworzymy diagram przepływu danych wg schematu. Do prezentacji danych wybieramy obiekt „chart”, czyli wykres liniowy.

• 

Następnie konfigurujemy zmienne, jakie chcemy wyświetlić. Klikamy dwa razy w pole „myVariables” i ustawiamy plik „Executable” klikając podając ścieżkę i plik „*.elf”. Wybieramy zmienne motorA.measured_speed oraz motorA.set_speed. Następnie w polu „my_Probe_Out” i „myProbe_In” ustawiamy nasz debugger ST-Link. Zatwierdzamy wszystko przyciskiem „Deploy” i otwieramy klikając w „Dashboard”.

Strojenie regulatora może być żmudnym zajęciem, ponieważ czasami ciężko jest dobrać parametry tak, aby silnik pracował właściwie. Poniżej przedstawiam kilka wykresów dla różnych wartości parametrów Kp, Ki, oraz Kd.

• 

• 

• 

• 

• 

Jak możemy zauważyć:

• zwiększanie Ki i Kd powoduje zwiększenie czasu regulacji oraz nieznaczny wzrost przeregulowania
• zwiększanie Kp nieznacznie wpływa na czas regulacji, znacząco jednak zwiększa przeregulowanie

Na ostatnim wykresie widać odpowiedź regulatora przy parametrach Kp = 1,2, Ki = 0,1 oraz Kd = 0,2, którą możemy uznać za dość satysfakcjonującą. Działanie regulatora PID można jeszcze spróbować poprawić np. poprzez zwiększenie częstotliwości pracy TIMER 6, co spowoduje, że regulator będzie częściej wykonywał obliczenia, a co za tym idzie będzie mógł szybciej reagować na zmiany. Zachęcam do samodzielnych eksperymentów.

Na koniec przedstawię jeszcze wykres zmiany prędkości wg. algorytmu napisanego w pętli while(1) (na potrzeby generowanie powyższych wykresów program był zmodyfikowany – zadawana była tylko prędkość 100 rpm).

• 

Podsumowanie

W poradniku przedstawiłem implementację regulatora PID na STM32, który pozwala nam na sterowanie prędkością obrotową silnika DC ze sprzężeniem zwrotnym od enkodera inkrementalnego. Choć jest to tylko podstawowa wersja algorytmu, daje nam możliwość w dość efektywny sposób sterować prędkością silnika i zadawać ją w postaci obrotów na minutę. Takie sterowanie dodatkowo będzie reagowało na zmiany w otoczeniu, np. wzrost oporów tarcia na kole i automatycznie dostosowywało się do panujących warunków. Algorytm PID przedstawiony w artykule jest uniwersalny – może służyć do sterowania temperaturą czy ciśnieniem. Mam nadzieję, że sprawdzi się jeszcze nie raz w Twoich projektach.

Do pobrania

Repozytorium GitHub