Sterowanie silnikami - sygnał PWM (niejednakowa prędkość obrotowa)

[simba92]

Cześć !

Na wstępie dodam, że przeglądam całe forum w poszukiwaniu rozwiązania swojego problemu i może gdzieś znajdę odpowiedź. Korzystając jednak z możliwości zaczerpnięcia jakiś wskazówek od bardziej doświadczonych kolegów przedstawię krótko opis problemu z, którym się borykam: 

Mam robota z napędem różnicowym z dwoma silnikami pololu HPCB 150:1 z zamontowanym enkoderami magnetycznymi na przedłużonym wale. Silnikami steruje poprzez dwa oddzielne mostki TB6612 i mikrokontroler STM32f103VCT6. Mój problem polega na tym, że zadając identyczną wartość sygnału PWM na oba mostki H zauważyłem, że koła nie kręcą się z jednakową prędkością tzn. jedno koło spóźnia się względem drugiego. O co chodzi ? Dodam, że regulacja prędkości kół jest dla mnie bardzo istotna, ponieważ w przypadku obrotu robota w miejscu z różnymi prędkościami kół robot przesunie się względem osi obrotu co dla mojego projektu jest dużym utrudnieniem, co obrazowo przedstawiłem na załączonym rysunku. Proszę o jakieś wskazówki i pomoc 🙂

Edytowano Listopad 3, 2018 przez Treker
Poprawiłem formatowanie.

[Treker (Damian Szymański)]

Jest to zupełnie normalne, nigdy silniki nie będą kręciły się z tą samą prędkością. Zawsze będą drobne różnice, silniki nie są identyczne (w teorii są, ale to jest praktyka). Po to właśnie używa się enkodery. Jeśli zależy Ci na identycznej prędkości to zapomnij o "ręcznym" podawaniu wartości PWM. Musisz podawać oczekiwane prędkości do regulatorów, które na podstawie odczytów z enkoderów będą dążyły do konkretnej prędkości w praktyce. Dodatkowo, dzięki temu robot będzie zachowywał się poprawnie pod różnym obciążeniem i na różnej powierzchni.

[simba92]

Treker tak właśnie myślałem, nigdy nie masz 2 identycznych silników. Jeżeli dobrze rozumiem muszę zapoznać się z tym artykułem : sterowanie robotem klasy (2,0), by zrealizować 

23 minuty temu, Treker napisał:

....Jeśli zależy Ci na identycznej prędkości to zapomnij o "ręcznym" podawaniu wartości PWM. Musisz podawać oczekiwane prędkości do regulatorów, które na podstawie odczytów z enkoderów będą dążyły do konkretnej prędkości w praktyce. Dodatkowo, dzięki temu robot będzie zachowywał się poprawnie pod różnym obciążeniem i na różnej powierzchni.

tak ? Dzięki za szybką odpowiedź !

[simba92]

Witam ! 

Chciałbym reaktywować temat i poddać dyskusji wyniki moich prób związanych wysterowaniem silników w moim robocie z wykorzystaniem regulatora PID dla każdego silnika z osobna. Cel był następujący : zadać jednakową prędkość dla obu silników (regulacja PID), aby robot jechał po linii prostej do przodu.

Zgodnie z artykułem artykuł regulator PID zrealizowałem następujące kroki dla pierwszego silnika ( silnik HPCB Pololu 150 : 1 ) :

1. Zaimplementowałem kompletny program do obsługi pomiaru prędkości kątowej z silnika za pomocą enkodera magnetycznego wyrażonej w [stopień/sekundę].

a) pełen obrót wynosi 1800 zliczonych impulsów

b) prędkość kątowa obliczana jest wg. wzoru : ω [°/s]= ( 360 [°] / 1800 * nTic ) / deltaT  : gdzie deltaT = 0.01 , nTic oznacza liczbę impulsów zmierzonych w czasie deltaT

2. Pobrałem dane do matlaba w celu wyznaczenia transmitancji silnika.

a) dane obejmują prędkość kątową ω [°/s] dla zadanego sygnału PWM silnika równego 9% wypełnienia w czasie 1s. Dodam, że sygnał PWM generowany jest z częstotliowścią 100kHz zgodną ze sterownikiem TB6612 sterującym silnikiem.

Otrzymana transmitancja obiektu :

które odpowiedź układu na pobudzenie jednostkowe tj. sygnału PWM jest następujące :

Kod z matlaba poniżej : 

%Wczytywanie danych z pliku dla pomiarów z silnika - motor1L (lewy)
datatmp = importdata('pid12.txt'); 
data = datatmp.data;
pwm_input = data(:,1);
omega_output = data(:,2);


%funkcja iddata przyjmuje: dane wyjsciowe, dane wejsciowe, czas probkowania
in_data = iddata(omega_output, pwm_input, 0.01);
%funkcja tfest przyjmuje: dane wejsciowo-wyjściowe jako struktura iddata,
%ilosc biegunow, ilosc zer, nazwe i wartosc parametru do ustawienia (Ts to
%czas probkowania).
%estymacja transmitancji obiektu dla in_data
motor_tf = tfest(in_data, 1, 1, 'Ts', 0.01);

%set(0,'defaultfigureposition',[50 100 600 600])
%figure;

%Wektor czasu
dt = 0.01;
t = 0:dt:1;

plot(t, omega_output, 'b')
hold on;

%wykres odpowiedzi skokowej transmitancji silnika
[y, t] = step(motor_tf,1);
y = y*9;
plot(t, y, 'r')


axis([0 1 0 125]) 
grid on
title('Wykres odpowiedzi silnika na zadane pobudzenie PWM = 9%','FontSize',16)
xlabel('t [s]','FontSize',14,'FontWeight','bold','Color','black')
ylabel('\omega [\circ/s]','FontSize',14,'FontWeight','bold','Color','black')

legend({'Odpowiedź rzeczywista silnika - \omega(t)',' Odpowiedź modelu silnika - \omega(t)_{model}'},'Location','southeast',2,'FontSize',16)
%,'FontWeight','bold'
%%%%%%

3. Znając transmitancje obiektu (silnika) przeszedłem do kroku następnego - wyznaczenia nastaw regulatora PID.

Obiekt w pętli zamkniętej :

uzyskując przy poniższych nastawach efekty sterowania widoczne na wykresie :

  

4. Wyznaczone nastawy zaimplementowałem w programie na robocie.

Kod programu głównego z odpowiednimi podfunkcjami napisany w C :

#include "main.h" //wszystkie biblioteki

int main(void)
{
	system_init(); //inicjalizacja całego systemu sterowania dla STM32f103 tj. peryferia, porty itp
	system_IWDG_RST_check();
	iwdg_init();
    bluetooth_init(); //inicjalizacja modułu BT do przesyłnia danych do mastera tj. PC 
    encoder_Reset(); //inicjalizacja enkoderów

	robot_MOTOR1L_controller_PID_init(); // inicjalizacja regulatora PID : jego nastawy

	int pwmLL;
	
	flag = 1; //flaga ustawiona na 1 aby program wszedł do pętli zanim zegar doliczy do 10ms tj. od włączenia robota
	while(1)
	{
		IWDG_ReloadCounter();

		if(flag == 1) //program przechodzi do pętli związanej z regulacją PID co 10 ms (zmiana stanu flagi z 0 na 1 w przerwaniu dla zegaru SYSTICK)
		{
		encoder_Read(); // pomiar bieżącej prędkości kątowej : motor1L_omega
		pwmL = robot_MOTOR1L_controller_PID_calculate(120, motor1L_omega);
		pwmLL = (int)pwmL;
		
		robot_drive(pwmLL,0); // sterowanie silnikiem poprzez PWM funkcją robot_drive(lewe koło, prawe koło)
	
		flag = 0;		
		}

	}
}

Uzyskane efekty działania regulacji PID na robocie są niestety kiepskie, co przejawia się następująco :

Po pierwsze dla zadanej prędkości 120 stopni/sekundę regulatora PID koło przyspiesza w nieskończoność... a wynika to z uzyskanych nastaw

a) jeżeli uchyb jest duży, a w początkowej fazie tak jest to stała Ki, która wynosi około 3 skutkuje dużym sygnałem sterującym za regulatorem wg. wzoru dla u :

float robot_MOTOR1L_controller_PID_calculate(float set_value, float read_value)
{
	float err_d, u;

	motor1L_PID_error = set_value - read_value;
	motor1L_PID_error_sum = motor1L_PID_error_sum + motor1L_PID_error;

	if(motor1L_PID_error_sum > ERR_SUM_MAX) motor1L_PID_error_sum = ERR_SUM_MAX;
	else if(motor1L_PID_error_sum < -ERR_SUM_MAX) motor1L_PID_error_sum = -ERR_SUM_MAX;

	err_d = motor1L_PID_error - motor1L_PID_error_last;

	u =  motor1L_KP * motor1L_PID_error + motor1L_KI * motor1L_PID_error_sum + motor1L_KD * err_d; //sygnał sterujący 
	
	motor1L_PID_error_last = motor1L_PID_error;

	return u;
}

tzn. dla danych w czasie t = 0 :

motor1L_PID_error = 120, motor1L_PID_error_sum = 0,   err_d = motor1L_PID_error=120

u =  motor1L_KP * motor1L_PID_error + motor1L_KI * motor1L_PID_error_sum + motor1L_KD * err_d = 0.082957209 * 120 + 2.973355529 * 120 + 0.000466135 * 120 > 360

tu pewien ważny aspekt mojego programu : jeżeli wartość PWM > 100 np. u > 360, to program podaje na sterownik silnika wartość PWM równą PWM_MAX, stąd silnik rozpędza się do nieskończoności... i niestety to się nie chcę ustabilizować.

No i teraz pytanie i prośba o wskazówki dot. wyznaczenia nastaw regulatora PID dla silnika. Stworzyłem model silnika według artykułu, dostroiłem regulator w matlabie, przeniosłem uzyskane nastawy na regulator PID na robocie i uzyskałem takie marne wyniki ... co poszło nie tak, co robię źle ??? 

PS : jeżeli coś jest nie jasne odpowiem chętnie na dodatkowe pytania i szczegóły. Dzięki za każdą cenną wskazówkę !!!

Edytowano Styczeń 10, 2019 przez simba92

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[Lukaszm]

Podstawowe pytanie to takie, czy zbieranie danych do modelowania silnika robiłeś, gdy robot stał na podłożu? Czy raczej koła kręciły się w powietrzu?

Druga sprawa to ta odpowiedź skokowa mocno mi się nie podoba:
1) Zbyt mała częstotliwość próbkowania (w jeden okres próbkowania jest przejście od 0rad/s do praktycznie prędkości ustalonej, bez sensu)
2) Czemu najmniejsza zmiana mierzonej prędkości kątowej (rozdzielczość) to aż 20rad/s?

[simba92]

@Lukaszm dane do modelowania silnika robiłem, gdy koło kręciło się w powietrzu, więc w sumie źle, bo powinno być ono pod obciążeniem tzn. na podłożu, tak ? Myślę, że koniecznie bo w rzeczywistych warunkach sterowanie będzie abstrakcyjne.

1) częstotliwość pomiarów realizowana była z czasem co 10 ms na próbkę czyli f = 1/T = 1/10ms = 100Hz, za mało ? Z jaką częstotliwością realizować próbkowanie ??? Z większą częstotliwością model będzie bardziej dokładny ?

2) fakt rozdzielczość jest 20 stopni/s. PWM zadawany jest jako liczba całkowita z przedziału 0 - 100, pomimo ,że wartość PWM pierwotnie z algorytmu obliczana jest jako zmienna stało przecinkowa. Rozdzielczość wynika z zakresu PWM, sugerujesz np. zmienić zakres np. z 0-100 na 0-1000 lub więcej dla sygnału PWM i wówczas uzyskanie większej rozdzielczości dla prędkości kątowej ?

PS : @Lukaszm dzięki za cenne wskazówki, mam teraz trochę dodatkowych pomysłów do testów dla PID.

[Lukaszm]

Koła w momencie testów i podczas strojenia regulatora powinny być cały czas na podłożu, bez obciążenia bardzo mocno zmienia się charakter sterowanego obiektu (głównie przez masę samego robota i opory ruchu).

38 minut temu, simba92 napisał:

1) częstotliwość pomiarów realizowana była z czasem co 10 ms na próbkę czyli f = 1/T = 1/10ms = 100Hz, za mało ? Z jaką częstotliwością realizować próbkowanie ??? Z większą częstotliwością model będzie bardziej dokładny ?

Częstotliwość próbkowania trzeba dobrać do dynamiki obiektu. Chodzi o to, że na przebiegu przejściowym ma być co najmniej kilka próbek (ma być widoczne narastanie sygnału). U Ciebie jednej chwili czasowej jest 0rad/s, a już w następnej chwili czasowej jest 100rad/s. Tak nie może być. Ale zanim zaczniesz kombinować z częstotliwością próbkowania, to zrób test z robotem na kołach, przebieg będzie dużo wolniejszy (zależy od masy robota i silników, ale spodziewałbym się, że zbocze będzie trwało kilkaset ms).

38 minut temu, simba92 napisał:

2) fakt rozdzielczość jest 20 stopni/s. PWM zadawany jest jako liczba całkowita z przedziału 0 - 100, pomimo ,że wartość PWM pierwotnie z algorytmu obliczana jest jako zmienna stało przecinkowa. Rozdzielczość wynika z zakresu PWM, sugerujesz np. zmienić zakres np. z 0-100 na 0-1000 lub więcej dla sygnału PWM i wówczas uzyskanie większej rozdzielczości dla prędkości kątowej ?

Aaaaa, nie zauważyłem, że omega masz w stopniach/s, a nie rad/s. IMO za mała prędkość jest uzyskiwana.
Generalnie tutaj pierwszym krokiem jest ustalenie, jaka ma być amplituda wartości wejściowej. Teraz wybrałeś 9%. Jakiś konkretny powód takiego wyboru? Ta amplituda powinna być dobrana do docelowego punktu pracy regulatora, to znaczy określasz sobie, jaka mniej więcej jest Twoja przewidywana prędkość jazdy robota (np. v=0.5m/s). Z kinematyki odwrotnej przeliczasz to na prędkości kół robota. I wtedy na podstawie charakterystyki statycznej (albo metodą prób i błędów) wyznaczasz, jaki PWM powinien być (tzn. taka  przybliżona wartość amplitudy skoku). I zbierasz sobie dane dla takiego skoku PWM.

EDIT: zakres 0-100 dla PWM na razie może być, chociaż z moich doświadczeń wynika, że w czasie jazdy, PWM zmienia się w zakresie od około 15% do 25%, więc na dalszym etapie możesz zwiększyć zakres PWM na 0-1000, żeby mieć więcej wartości w tym zakresie 15% do 25% (ten zakres zależy od wielu parametrów: silników, napięcia zasilania, masy robota itd.)

Edytowano Styczeń 11, 2019 przez Lukaszm

[simba92]

@Lukaszm wybrałem 9% wypełnienia PWM ze względu na wymaganą małą prędkość robota (Husarz_redSLAM) w trakcie jazdy ( masa robota, łatwość sterowania i realizacji algorytmu SLAM ). Masz racje z częstotliwością, trzeba ją  dobrać w odniesieniu do dynamiki obiektu (test silnika na podłożu) i tak jak napisałeś ten czas narastanie będzie dłuższy tj.  przebieg wolniejszy stąd więcej próbek. Muszę przeprowadzić testy, dziękuje za wyczerpującą odpowiedź i udzieloną pomoc !!!