Po wyeliminowaniu błędów w PID opisanych w poprzednim artykule, mogłem przejść do kolejnych poprawek w module silników. Moją uwagę przykuł regulator prędkości kątowej. Postanowiłem wprowadzić w nim zmiany, aby zwiększyć precyzję obrotu robota i zwiększyć jego stabilność. Do tej pory zdarzało mi się, że robot się wzbudzał i stojąc w miejscu wykonywał czasem niewielkie skręty.

Wybór mierzonego sygnału

Dotychczasowy regulator prędkości obrotowej bazował na odczytach z żyroskopu. Jego dryft sprawiał, że gdy robot stał w miejscu, była odczytywana prędkość kątowa. Przez to uniemożliwiał bazowanie na bezpośredniej wartości. Można odczytywać wartość czujnika przy inicjalizacji i potem jej używać jako dryft, albo zahardkodować jakąś wartość. Nie podobały mi się te rozwiązania. Dryft może się zmienić w czasie, możemy odczytać początkową wartość, kiedy robot jest w ruchu, zahardkodowana wartość może się mocno rozjeżdżać z rzeczywistością. Może do pierwszego prototypu to by jeszcze przeszło, ale na pewno nie chciałem czegoś takiego używać jako docelowe rozwiązanie.

Tak się jednak składa, że mam już gdzie indziej wyliczaną prędkość kątową. Filtr Kalmana estymuje prędkość i dryft, używa do tego żyroskopu i enkoderów. Jednak takie rozwiązanie też ma wady. Po pierwsze regulator silnika potrzebuje wtedy działającego Kalmana, który wymaga skomplikowanych obliczeń i nie jest prosty do nastrojenia. Aktualnie mam jakieś nastrojone wartości, ale musiałbym zrobić dodatkowe testy, czy na pewno dryft jest dobrze rozpoznawany zarówno gdy robot stoi, jak i jedzie. Poza tym każda zmiana będzie wymagała ponownego strojenia tworząc deadlocka. Do nastrojenia Kalmana potrzebuję działających silników, do działania silników potrzebuję nastrojonego Kalmana. Być może sprawdziłby się drugi prostszy Kalman tylko do kąta. Jednak dublowałby on funkcjonalność. Więc to rozwiązanie również odrzuciłem.

Ostatecznie wymyśliłem, że najlepiej sprawdzi się najprostsze rozwiązanie. Do regulatora silnika chcę użyć jedynie odczytów z enkoderów. Dzięki temu otrzymam szybki feedback z minimalną ilością obliczeń. Enkoderów i tak już używam do regulatora ruchu postępowego, więc nie ma żadnych dodatkowych zależności. Największą wadą enkoderów jest błąd pozycji kumulujący się w czasie. Tutaj będą wykorzystywane tylko do określania chwilowej prędkości, a z tym zadaniem poradzą sobie dobrze.

Obliczanie kąta obrotu

Przenalizujmy teraz jak obliczyć kąt obrotu z odczytów enkoderów. Posłużymy się w tym celu niezwykle profesjonalnym rysunkiem w paincie. Robot posiada pewną szerokość d i wykonuje obrót o pewien kąt \alpha . Promień skrętu wewnętrznego koła wynosi r. Łuki kreślone przez oba koła przedstawia rysunek. Zakładamy, że robot jedzie do góry i skręca w lewo.

Długości łuków dla poszczególnych kół można wyznaczyć z wzorów:

L = 2 \cdot \pi \cdot r \cdot \frac{\alpha}{360^\circ} 

R = 2 \cdot \pi \cdot (r + d) \cdot \frac{\alpha}{360^\circ} 

Jeżeli odejmiemy od siebie te wartości, otrzymamy:

R - L = 2 \cdot \pi \cdot d \cdot \frac{\alpha}{360^\circ} 

Czyli różnica odległości przebytych przez poszczególne koła jest wprost proporcjonalna do pokonanego kąta. Tak więc wystarczy odjąć od siebie odczyty z obu enkoderów i podać je do algorytmu PID. Jedyną trudnością będzie wtedy określenie wartości zadanej. Chcielibyśmy zadawać prędkość kątową w stopniach na sekundę, a aktualnie operujemy w tickach enkodera na czas próbkowania. W tym celu musimy posłużyć się wzorem:

\frac{\alpha}{360^\circ} \cdot 2 \cdot \pi \cdot d \cdot dt \cdot k = R - L

Gdzie dt to czas próbkowania, a k to przelicznik z milimetrów na ticki enkodera. W moim wypadku wynosi on:

k = \frac{40 \cdot 64}{10 \cdot 69}

Pewnym problemem może być tutaj rozdzielczość, ponieważ zgodnie z powyższymi wzorami kąt \alpha = 20^\circ / s daje różnicę jednego ticka. Jednak nie powinno być to aż takim problemem, ponieważ nawet małe korekty kąta mogą być wykonywane po prostu z większą prędkością kątową.

Pierwsze testy

Po wprowadzeniu opisanych poprzednio zmian w samym algorytmie PID oraz zmianie na odczyty z enkodera nawet bez specjalnego strojenia widać, że regulator działa dokładniej, ma mniejsze zakłócenia i nie traci stabilności. Wcześniej stabilność była problemem szczególnie z członem I. Symulacje wykazały, że regulator PD ma spory uchyb ustalony, więc niezbędne będzie dodanie członu całkującego. Człon różniczkujący niewiele wnosi, więc ostatecznie będzie to pewnie regulator PI.