Opis algorytmu PD

Z tego wpisu dowiesz się:

  • Jak płynnie podążać za linią?
  • Jak przetwarzać sygnały z czujników by sterować szybkością poszczególnych silników?
  • Co to jest tzw. „pętla zwrotna”?

A wszystko po to, by oprogramować Line Followera (w skrócie LF)!

Regulator PD to “ucięta” wersja regulatora PID (ang. Proportional-Integral-Derivative). Posłuży nam on do płynnego sterowania linefollower’em. Regulator ten posiada 3 człony:
liniowy (P),
całkowy (I),
różniczkowy (D).

Człon P odpowiada za to, jak szybko silniki LF’a będą się obracać na podstawie położenia linii. Jeżeli linia będzie po prawej stronie czujnika to robot “wie”, że musi skręcić w tym kierunku i zmniejszy szybkość prawego silnika (lub zwiększy prędkość lewego, ale z reguły lepiej zastosować to pierwsze rozwiązanie). Jednak sama zależność liniowa nie wystarcza, żeby płynnie podążać za linią (chyba, że bardzo powoli). Żeby “przewidzieć” zmiany w kierunku ruchu i odpowiednio na nie zareagować przydaje się nam człon D regulatora. Jest to prosta różnica pomiędzy poprzednią a aktualną pozycją linii. W przypadku prostej linii różnica będzie nieznaczna, a podczas nagłej zmiany kierunku robot szybciej zmieni szybkość silników. Człon I pomijamy, bo nie jest on zbyt ważny w przypadku linefollower’ów (z doświadczenia).

Może i jest to na początek trochę skomplikowane, ale implementacja jest bardzo prosta i nie wymaga zbyt wiele pisania:

error_P = pozycja_linii * KP; // zależność liniowa
error_D = (pozycja_linii - poprzednia_pozycja_linii) * KD; // różnica pomiędzy pomiarami
Error = error_P + error_D; // suma członów regulatora
poprzednia_pozycja_linii = pozycja_linii;

Uzyskany w ten sposób Error możemy bezpośrednio wykorzystać do ustawienia prędkości silników. Współczynniki KP i KD określają jaki wpływ mają poszczególne człony na ruch LF’a i będą się znacząco różnić w zależności od algorytmu wykrywania pozycji linii czy samej budowy robota (np. pozycji czujnika). Właśnie w konkretnych wartościach KP, KD leży sekret działania dobrego regulatora PID. Żeby uzyskać płynny ruch trzeba się trochę “pobawić” z tymi współczynnikami i znaleźć najlepsze wartości. Można znaleźć wiele metod strojenia takiego regulatora (np. Metoda Zieglera-Nicholsa), jednak w tym przypadku najprościej jest to zrobić ręcznie czyli metoda prób i błędów, eksperymentalnie zmieniając KP i KD i sprawdzając na torze jak to wpływa na ruch LF.

Na początku ustawiamy KD = 0, i szukamy najmniejszego KP, żeby robot przy małej prędkości w miarę dobrze podążał po prostej linii lub lekkim łuku. Po znalezieniu odpowiedniego KP możemy stopniowo zwiększać współczynnik KD i prędkość, do uzyskania płynnego ruchu. Jako, że różnice w pomiarach są stosunkowo małe w porównaniu do położenia linii, często następuje zależność: KD > KP.

Określanie pozycji linii, wspólne działanie 5-ciu czujników

Jak przy pomocy wielu czujek jednoznacznie określić, gdzie znajduje się linia? Każda czujka zwraca tylko informację o ilości odbitego światła (wartości od 0 do 1023). Można by zastosować algorytm który wybiera jedną pozycję z największą wartością ale prowadzi on tylko do niepłynnego ruchu. Powinniśmy zastosować bardziej precyzyjny algorytm wykrywania linii ze średnią ważoną.

Każdemu czujnikowi przypisujemy wagi, na których postawie będziemy określać pozycję (np. -2000, -1000, 0, 1000, 2000 dla kolejnych czujek). Następnie czytamy wartości z czujników, mnożymy je razy wagę i sumujemy. Na koniec dzielimy otrzymaną wartość przez sumę wartości z czujek. W ten sposób dokładną informację nawet kiedy linia znajduje się na granicy dwóch czujników.

pozycja = (cz1*waga1+cz2*waga2+cz3*waga3+cz4*waga4+cz5*waga5)/(cz1+cz2+cz3+cz4+cz5)

W przypadku zastosowania wag: -2000, -1000, 0, 1000, 2000 dla kolejnych czujników, po przeliczeniu średniej ważonej otrzymamy jedną wartość od -2000 do 2000 (dla -2000 linia znajduje się po lewej stronie, dla 0 – na środku, a dla 2000 – po prawej).

Sterowanie prędkością silników

Jak wykorzystać wcześniej otrzymany Error do sterowania prędkością silników? Do ustawienia prędkości pojedynczego silnika używamy sygnału PWN. Będziemy tą wartość zmieniać w zależności od uzyskanego Error’a.

Jeżeli wartość Error’a jest ujemna to w naszym przypadku pozycja linii znajduje się po lewej stronie LF’a. Aby skorygować ruch będziemy zmniejszać prędkość lewego silnika. Analogicznie postępujemy w przypadku dodatniej wartości:

if(Error > 0){
pwn1=maxSpeed; //maxSpeed to wczesniej ustalona maksymalna wartość PWN LF’a
pwn2=maxSpeed-Error;
} else {
pwn1=maxSpeed+Error;
pwn2=maxSpeed;

Warto pamiętać, że Error może być znacznie większy od wartości maxSpeed. Jeżeli wartość nowego pwn1 lub pwn2 jest mniejsza od 0 to musimy ją ustawić na 0 (pwn powinien wynosić od 0 do 255). Po tych wszystkich operacjach uzyskujemy pwn’y do sterowania silnikami.

Mój LF w akcji:

(c) Bytler, 2019