W końcu udało mi się zakończyć pracę nad regulatorem prędkości obrotowej robota, z którym walczyłem od dłuższego czasu. W poprzednich wpisach opisywałem problemy z żyroskopem i spalenie płytki, co wiązało się z nowym projektem PCB. Nareszcie udało doprowadzić się temat do końca. We wpisie znajdziecie trochę matmy, dużo wykresów i opis kolejnych przygód z zepsutym hardwarem.

Napęd różnicowy

Budowany przeze mnie robot posiada napęd różnicowy. Oznacza to, że osobne silniki napędzają lewą i prawą stronę. Jest to często spotykane rozwiązanie w tego typu konstrukcjach, a sposób sterowania takim robotem jest całkiem prosty do zrozumienia. Jeśli koła po obu stronach kręcą się z taką samą prędkością – robot jedzie do przodu. Jeżeli jedna strona kręci się wolniej, robot wykonuje skręt w tą stronę. Im większa różnica pomiędzy obiema stronami, tym mniejszy promień skrętu. Możliwe jest również polecenie silnikom, aby kręciły się w odwrotnych kierunkach. Jeżeli robią to z taką samą prędkością – robot obraca się w miejscu. Jeżeli natomiast jeden silnik się kręci, a drugi nie – środkiem obrotu są koła, które się nie kręcą.

Znając prędkość poszczególnych silników odczytaną za pomocą enkoderów, możemy obliczyć prędkość postępową i obrotową naszego robota. Prędkość postępowa jest po prostu średnią obu odczytów:

v = \frac{v_L + v_R}{2}

gdzie v to prędkość postępowa, a v_L i v_R to odczyty prędkości dla lewej i prawej strony. Do obliczenia prędkości kątowej posługujemy się wzorem:

\omega = \frac{v_L - v_R}{L}

gdzie \omega to prędkość kątowa, a L to szerokość robota. Prędkości najlepiej obliczać bazując na pomiarach z krótkich chwil czasu np. co 1 ms. Przyjmujemy bowiem, że obliczona prędkość postępowa i kątowa utrzymywały się przez cały okres próbkowania. W rzeczywistości robot mógł w tym czasie np. jechać zygzakiem. Jeśli zmniejszymy okres próbkowania, wpływ tego błędu również się zmniejsza.

Ciekawą obserwacją jest tutaj fakt, że prędkość obrotowa nie zależy od składowej stałej. Czyli według wzoru dla wartości 1 i 2 otrzymamy taką samą prędkość kątową jak dla 9 i 10. Aby przekonać się, że tak jest, można samodzielnie wykonać obliczenia korzystając z wzoru:

\omega R = v

Akwizycja danych

Aby zaprojektować regulator ruchu obrotowego, potrzebowałem rzeczywistych danych wiążących sygnał podawany na silniki z prędkością obrotową. Mimo, że jako wyjście mogłem użyć wartości z enkoderów, zdecydowałem się skorzystać z żyroskopu. Oba pomiary są równoważne i można sobie obliczyć współczynnik skali między tickami enkodera a wartością z żyroskopu. Docelowo można będzie pomyśleć o fuzji danych z obu źródeł. Dane z enkoderów są podatne na błędy mechaniczne np. poślizg koła. Żyroskop z kolei posiada dryft, czyli błąd dodawany do pomiaru zwiększający się z czasem.

Stworzyłem aplikację testową pozwalającą zadawać bazową prędkość oraz offset dodawany do lewego silnika i odejmowany od prawego. Dzięki temu byłem w stanie nakazać robotowi skręcanie i mierzyć jego prędkość kątową odczytywaną przez żyroskop. Po wykonaniu pomiaru wyniki były zrzucane na terminal. Szybko okazało się, że używana przeze mnie rozdzielczość timera sterującego mocą silników wynosząca 100 ticków na cykl jest za mała. Zwiększyłem ją więc do 1000. Zauważyłem również, że przy tej samej mocy na oba silniki robot skręca w lewo. Oznaczało to, że lewe koła kręcą się wolniej. Doświadczalnie wyznaczyłem offset, przy którym robot jechał prosto, wynosił on 15 (dla całego okresu timera 1000).

Omównienie pomiarów

Zebrałem całkiem dużo pomiarów. Dla składowej stałej 20% i 30% mocy silnika sprawdziłem offsety 0.5%, 1% 1.5% i 2%. Wykonałem również pomiary dla obrotów w miejscu przy mocy 20% i 30% podawanej na silniki.

Obrót w miejscu 20% mocy

Obrót w miejscu 30% mocy

Dla obrotów w miejscu widać takie charakterystyczne spadki w końcowej fazie. Było to spowodowane przez baterię, która pod wpływem obrotów się odczepiała i dyndała za robotem wprowadzając takie zakłócenia.

Składowa stała 20% mocy silnika, offset 2%.

Składowa stała 20% mocy silnika, offset 1.5%.

Składowa stała 20% mocy silnika, offset 1%.

Składowa stała 30% mocy silnika, offset 1%.

Wykresów mam o wiele więcej, nie będę ich tu wszystkich wrzucać. Analizując dane zauważyłem kilka ważnych rzeczy:

  • Składowa stała ma wpływ na prędkość obrotową.
  • Zakłócenia są wyraźne, szczególnie w okolicach prędkości kątowej 0 dps.
  • Poszczególne dane pomiarowe trudno opisać jednym modelem.

Przyczyną wszystkich wymienionych punktów jest wpływ konstrukcji mechanicznej. Poza tym ważne może być to, że działam na dosyć małych offsetach. Może nieliniowości by mijały na wyższych rzędu np. 15%.

Projekt regulatora

Do wyznaczenia modelu matematycznego użyłem tej samej metody co w artykule o regulatorze prędkości postępowej. Jednak tym razem nie pisałem w tym celu skryptu w pythonie, a linie zaznaczałem ręcznie na wykresach. Wybrałem taką metodę, ponieważ nie potrzebowałem super dokładnej wartości. Bardziej chodziło mi o udowodnienie, że modele dla poszczególnych danych wejściowych istotnie się różnią. Nie będzie więc możliwe stworzenie jednego modelu wystarczająco dobrze opisującego każdy przypadek. I faktycznie udało mi się to udowodnić. Ostatecznie wybrałem jakieś uśrednione wartości i otrzymałem następujący model:

G(s) = \frac{2.1}{0.15 s + 1}

Analogicznie jak w podlinkowanym wyżej artykule użyłem autotunera z Simulinka do wyznaczenia nastaw regulatora PD. Początkowo wygenerowane wartości nie były satysfakcjonujące, więc trochę pobawiłem się parametrami. Generator chcąc uzyskać krótsze czasy ustalania preferował duże wartości P, co z kolei powodowało duże wartości sygnału sterującego i szybkie zmiany. Mnie interesowała łagodniejsza strategia i w końcu udało się taką uzyskać. Ostateczne wartości to P = 2.4320 i D = 0.1.

Testy na sprzęcie

Wartości wyznaczone analitycznie były na bieżąco sprawdzane na robocie. W tym celu użyłem kolejnego programu testowego. Miał on uruchomioną pętlę sterowania zawierającą dwa regulatory PD – jeden od ruchu postępowego, a drugi od obrotowego. Podobnie jak poprzednio dane pomiarowe były printowane na terminal. Testy na sprzęcie potwierdziły, że regulator działa, potrafi utrzymać jazdę prostą robota oraz wykonywać skręty.

Wynik symulacji działania regulatora dla wartości zadanej 20 dps.

Symulacje pokazały, że układ regulacji posiada uchyb ustalony. Zostało to potem potwierdzone w testach na sprzęcie. Zjawisko przedstawia powyższy wykres. Uchyb ustalony oznacza, że układ w stanie ustalonym nie stabilizuje się na wartości zadanej, tylko na niższej wartości.

Sposobem na wyeliminowanie uchybu ustalonego jest dodanie członu całkującego. Działa on na sumie uchybu z poprzednich iteracji algorytmu PID i dzięki temu redukuje uchyb ustalony do zera. Na razie nie chciałem dodawać członu I, ponieważ generator nastaw wybierał niskie wartości P i wysokie I, przez co robot zachowywał się niestabilnie. Natomiast ręczny dobór nastaw jest bardzo czasochłonny. Jeżeli będzie taka potrzeba – dodam ten człon później. Możliwe, że nie będzie on potrzebny, ponieważ na wyższej warstwie będzie działał moduł zadający prędkości na silnik i być może on sobie poradzi z uchybem regulatorów.

 

 

 

Nie obyło się bez ofiar

Zgodnie z tradycją wszystkie testy na sprzęcie kończą się zniszczeniem jakiegoś elementu. Nie inaczej było tym razem. Na skutek uderzenia w szafę przerwała się ścieżka zadająca kierunek na mostek H uniemożliwając w ten sposób sterowanie jednym z silników. Na szczęście na ratunek przyszedł kynar i teraz pady są połączone na pająka.

 

Docelowo będę chciał poprawić design płytki PCB. Aktualnie ścieżka ta i kilka innych jest bardzo cienka i przechodzi pomiędzy pinami złącza czujnika IMU. Spokojnie mogę zrobić ją grubszą i poprowadzić dookoła złącza. Przerwana ścieżka na poniższym screenie zaznaczona na żółto.

 

Kolejny problem również był związany z uderzeniem robota w szafę. Od uderzenia pokruszył się jeden z kondensatorów i zostały same pola lutownicze. Na zdjęciu poniżej zaznaczyłem to miejsce na żółto.

Dalsze plany

Udało się zakończyć pracę nad regulatorami prędkości robota. Z tej okazji na Githubie podbiłem wersję softu do v0.3.0. Kolejnym celem jest estymacja położenia robota, którą chcę zrealizować przy pomocy Filtru Kalmana. Będzie to element systemu SLAM (Simultaneous Localisation and Mapping) mającego na celu określanie położenia w labiryncie i zbieranie informacji o ścianach. Poza tym została kwestia czujników ścian, które również wejdą w skład SLAM, a także moduł zarządzania ruchem robota korzystający z Filtru Kalmana i wysyłający polecenia do regulatorów silników.