Sterowanie robotem mobilnym klasy (2,0)

ProgramowanieRobotyka baton

Problem sterowania robota wyposażonego w dwukołowy napęd różnicowy, nazywanego też robotem mobilnym klasy (2,0), prawdopodobnie dosięgnie z czasem większość osób zajmujących się robotyką amatorską.

Do tej klasy robotów zalicza się bowiem dużą cześć tworzonych konstrukcji: linefollowery, micromouse i wszelkiej wielkości sumo.

Udostępnij

Może się okazać, że dla osoby podejmującej się wysterowania takiej platformy po raz pierwszy, będzie to zadanie problematyczne. Z racji na brak materiałów traktujących o tym zagadnieniu w sposób przystępny i jednocześnie w miarę kompletny, powstał niniejszy artykuł.

Celem naszej pracy będzie uzyskanie kontroli nad robotem, która pozwoli na precyzyjne sterowanie jego ruchem. O ile nie jest to nadrzędnym zadaniem przy oprogramowywaniu linefollowera czy minisumo - choć w pierwszym przypadku, przy odpowiedniej konstrukcji, z pewnością może dać rezultaty - to w robocie typu micromouse jest to podstawą.

W odpowiednim miejscu, w odpowiednim czasie

Kontrola silnika na ogół jest opisywana i przeprowadzana na dwa sposoby:

• kontrola nad prędkością - w tym przypadku wartością, którą sterujemy, a zatem na którą mamy bezpośredni wpływ, jest prędkość obrotowa silnika. Wykorzystywana jest we wszelkich aplikacjach, w których wartość ta musi być utrzymana w danym zakresie, zaś mniej ważna (lub w ogóle nieistotna) jest informacja o położeniu silnika/przebytej drodze. To ostatnie można oczywiście poznać poprzez całkowanie prędkości w danym okresie czasu, lecz może to prowadzić to narastającego błędu wynikającego z niedokładności pomiaru chwilowej prędkości.
• kontrola nad położeniem - wartością zadaną jest oczywiście położenie, w którym znaleźć ma się silnik. Prędkość, z jaką osiągnie punkt zadany na ogół liczbowo nie jest istotna (na ogół jest to "możliwie szybko"). Najczęściej spotykanym przez robotyków przykładem będzie serwomechanizm, który dąży do ustawienia się w zadanej pozycji, nie dając jednocześnie kontroli nad prędkością obrotu. Tak jak w poprzedniej metodzie kontroli, tutaj także pośrednio możemy poznać wartość pokrewną opisującą ruch - prędkość obrotowa stanowi zmianę położenia w danym czasie.

Jak widać, do zaawansowanej kontroli nad robotem wybranie jednej z powyżej opisanych metod nie przyniesie dobrych rezultatów. Potrzebna jest nam kontrola zarówno nad jego pozycją jak i prędkością, z jaką tę pozycję zmienia. Na szczęście połączenie tych dwóch metod nie jest sprawą skomplikowaną. Z pomocą przychodzi matematyka, która mówi nam, że prędkość to zmiana położenia w czasie.

Następujący przykład pozwoli bliżej zrozumieć problem.

Zakładamy, że chcemy ze stałą prędkością 1cm/s przebyć drogę 20cm, a sterownikowi silnika możemy zadawać pożądane przez nas aktualnie położenie z częstotliwością 100Hz. Aby wykonać zadanie, wystarczy z każdym przejściem pętli sterującej zwiększać zadane położenie o 1/100 cm. Ponieważ częstotliwość pętli wynosi 100Hz, w ciągu sekundy zadane położenie przesunie się o 1cm. Teraz wystarczy kontrolować przebytą drogę i w odpowiednim momencie (po przebyciu 20cm), zaprzestać zwiększania zadanego położenia. Efektem jest ciągle "uciekający" punkt zadany, za którym stara się nadążyć regulator silnika - i mamy zapewnioną zarówno kontrolę prędkości, jak i położenia.

Ponieważ każda stworzona przez nas konstrukcja będzie charakteryzowała się określoną bezwładnością oraz ograniczoną przyczepnością do podłoża, do zrealizowania opisanej metody kontroli nad robotem potrzebny będzie dodatkowy moduł - tzw. profiler prędkości. Pozwoli on na zaimplementowanie kontroli nad przyspieszeniem (poprzez odpowiednio małe zmiany zadawanej prędkości), a zatem umożliwi łagodną zmianę prędkości silników.

Do zadań profilera należeć będzie:

• zwiększanie prędkości zgodnie z zadanym przyspieszeniem
• po osiągnięciu określonej prędkości maksymalnej - utrzymanie prędkości na stałym poziomie
• kontrola nad koniecznością hamowania i jej wykonanie (zmniejszanie zadawanej prędkości)

Wykres przedstawiający przykładowy wynik działania profilera - przyspieszenie do prędkości 5, chwilowe jej utrzymanie, przyspieszenie do 10, a po chwili zwolnienie do 3 i w końcu zatrzymanie. Dzięki łagodnym zmianom prędkości nie dochodzi do poślizgów i zostaje zachowana kontrola nad położeniem robota.

Ważnym aspektem poprawnie działającego profilera jest zatrzymanie silnika w zamierzonym momencie, tj. poprawna decyzja o chwili, w której należy rozpocząć hamowanie.

Wykres prezentujący profiler, który rozpoczął hamowanie za szybko (czerwony) i zbyt późno (pomarańczowy). Wynikiem tych działań jest niezgodna z zamierzeniami przebyta droga - przy przedwczesnym hamowaniu z takim samym opóźnieniem, jak w przypadku optymalnym, robot przemieści się oczywiście o mniejszy dystans (przebyta droga równa jest liczbowo polu pod wykresem prędkości).

Wszystkie dotychczasowe rozważania dotyczyły przypadku prostoliniowego ruchu punktu, a więc odbywającego się w jednym wymiarze. W robocie napędzanym różnicowo mamy jednak do czynienia z ruchem w dwóch wymiarach - sterujemy dwoma silnikami niezależnie. Należy więc zastanowić się, w jaki sposób można kontrolować ruch takiej platformy.

Pierwszą myślą może być chęć kontroli nad robotem jako bezpośrednie sterowanie położeniem lewego i prawego silnika. Chcemy jechać prosto - podajemy taką samą odległość do przejechania z taką samą prędkością na oba silniki. W przypadku skrętu - jeden z silników musi pokonać większą odległość z większą prędkością od drugiego. W tym momencie pojawia się jednak wyraźna niedogodność takiego potraktowania problemu sterowania. Otóż dla każdego zakrętu, konieczne będzie wyliczenie drogi oraz prędkości wymaganej do podania na każdy z silników. Dodatkowo stworzenie profilera dla każdego koła osobno doprowadzi do problemów z przejściem między odcinkiem prostym a łukiem - ruch obydwu silników musiałby kończyć się w tym samym momencie. Krótko mówiąc - jest to rozwiązanie, które rodzi więcej problemów, niż ich rozwiązuje.

Dużo lepszym tokiem myślenia okazuje się rozbicie ruchu robota na dwie składowe - translację (czyli przemieszczenie równolegle do osi podłużnej robota) oraz rotację (obrót wzdłuż osi pionowej robota). Ruch naprzód po prostej nastąpi wtedy dla dodatniej wartości translacji i rotacji równej zero, obrót w miejscu - dla zerowej translacji i rotacji różnej od zera. Wykonanie łuku także staje się łatwe - obie liczby przyjmują wartości niezerowe. Jak natomiast odnosi się to do samych silników? Nic prostszego - translacja to suma odległości przejechanych przez oba koła, zaś rotacja to różnica między nimi.

W wyniku przeprowadzonego rozumowania wiemy już, co będzie potrzebne do panowania nad naszą konstrukcją:

• enkodery podające informację o aktualnej pozycji koła
• dwa profilery prędkości - dla translacji i rotacji
• dwa regulatory silników (nieśmiertelne PD) - także osobny dla translacji i rotacji
• układ generujący sygnał PWM sterujący silnikami

Schemat sterownika przedstawiony został na poniższym grafie.

Ponieważ rozważania teoretyczne i tworzenie planu działania naszego sterownika mamy za sobą, możemy przejść do sedna sprawy - zaprogramowania mikrokontrolera odpowiedzialnego za kierowanie robotem.

int - źle, float - jeszcze gorzej

Zanim zaczniemy właściwe tłumaczenie z polskiego na C, musimy jeszcze ustalić zasadę obchodzenia się ze zmiennymi opisującymi drogę, prędkość i przyspieszenie. Niestety zmienne przechowujące liczy całkowite obsługiwane w ich "naturalny" sposób nie zdadzą egzaminu w naszym zastosowaniu.

Dlaczego? Przede wszystkim, potrzebujemy ułamków. Ponieważ pętla sterowania będzie wykonywała się bardzo szybko (1kHz to sensowna częstotliwość), a enkodery mają ograniczoną rozdzielczość (4096 impulsy na obrót koła to wartość, której raczej nie przekroczymy), wartości przebytej drogi, czy też prędkości chwilowej, w okresach między wywołaniami pętli będą niewielkie.

Dla przykładu - robot z kołami o średnicy 26mm i 12-bitowymi enkoderami na kołach jadąc z prędkością 1m/s generuje ok. 100 "tików" enkodera w ciągu 1ms. O ile pomiar prędkości z rozdzielczością 0,01m/s wydaje się być wystarczający, o tyle podawanie przyspieszenia z dokładnością 0,01m/s*ms jest niepraktyczne - powyższy przykładowy robot dla minimalnej możliwej wartości przyspieszenia musiałby rozpędzać się do 1m/s w ciągu 100ms (0,01m/s*ms = 10m/s2)!

Jeśli jednak mikrokontroler, którego używamy, z liczbami zmiennoprzecinkowymi musi radzić sobie software'owo - ich zastosowanie kilkukrotnie wydłuży czas wykonywania obliczeń oraz wykorzystaną pamięć. W tym przypadku, należy samemu zaimplementować zmienne stałoprzecinkowe.

Zasada ich działania jest prosta - umawiamy się, że określona ilość młodszych bitów oznacza cześć ułamkową. Poniższy przykład zobrazuje, jak to wygląda w praktyce. 16-bitowa zmienna całkowita bez znaku przyjmuje wartości od 0 do 65535 z rozdzielczością 1. Jednak gdyby umówić się, że traktujemy ją jako zmienną 8.8 (co w zapisie liczb stałoprzecinkowych oznacza 8 bitów części całkowitej i 8 ułamkowej), to uzyskujemy zakres od 0 do 255,996 z rozdzielczością 1/256. Operacje arytmetyczne wykonujemy na takich zmiennych bez zmian względem "tradycyjnych" intów, trzeba tylko pamiętać o odpowiedniej konwersji przy przechodzeniu między dwa typami zmiennych. Operacją tą będzie przesunięcie bitowe w lewo o 8 przy przejściu uint16 -> u8.8 i przesunięcie bitowe w prawo o 8 przy przejściu odwrotnym.

Profiler translacji

Na podstawie dotychczasowych rozważań, a w szczególności "listy zadań profilera", możemy napisać prawie-gotowy-kod opisujący jego działanie. Poprzedzony został deklaracjami wszystkich wymaganych zmiennych. Zmienne stałoprzecinkowe (w tym przypadku wszystkie traktowane jako X.8), dla łatwej identyfikacji, mają dodany suffix _8.

Powyższy kod należy jedynie uzupełnić o sprawdzenie konieczności hamowania oraz dojechania do celu. Pierwszy test polega na obliczeniu drogi wymaganej do wyhamowania z aktualnej prędkości do do wartości next_V_8. Jeśli droga ta jest mniejsza lub równa pozostałej drodze zadanej profilerowi, należy rozpocząć hamowanie. Wychodzimy od wzoru na drogę w ruchu opóźnionym:

Ponieważ chcemy porównywać drogę, z powyższego równania pozbywamy się czasu, podstawiając:

Po przekształceniach nasz wzór wygląda następująco:

Nie pozostaje więc nic innego, jak porównać potrzebną do wyhamowania drogę, z tą faktycznie pozostałą:

Ta pojedyncza linijka wymaga chwili uwagi. Ponieważ w liczniku ułamka pomnożone zostają dwie zmienne stałoprzecinkowe X.8 (a więc będące zapisane jako liczby całkowite 256 razy większe od ich wartości w naszej interpretacji), a w mianowniku znajduje się jedna taka zmienna, aby otrzymać w wyniku zmienną całkowitą, musimy ten pierwszy podzielić przez 256. Nie robimy tego jednak z całym licznikiem, a tylko jednym mnożnikiem.

Sprawdzenie, czy robot dojechał już do celu jest sprawą prostszą. Wystarczy sprawdzić, czy wyliczona przez profiler droga pokryła już zadaną całkowitą drogę. Ponieważ przy niskich prędkościach może zdażyć się, że robot nie dojedzie idealnie do zadanego punktu - warto w warunku zakończenia ruchu uwzględnić osiągnięcie prędkości równej zero przy hamowaniu:

Po uzupełnieniu kodu profilera jesteśmy gotowi do precyzyjnego wyznaczania położenia, w którym powinien znaleźć się robot w danej chwili. Możemy także przejść do tworzenia profilera rotacji, który będzie wyglądał bardzo podobnie.

Profiler rotacji

Jedyną ważną różnicą w odróżnieniu od poprzednio napisanego profilera jest możliwość wystąpienia ujemnych prędkości i odległości - obrót w lewo jest przyjmowany jako ujemny. Poniższy kod przedstawia wersję lekko uproszoną - zakłada, że po zakończeniu ruchu prędkość obrotowa zawsze będzie równa zero, tj. nie pozwala na łączenie kilku obrotów w jeden ciągły.

Tym sposobem otrzymujemy gotowy drugi profiler, który generować będzie krzywą prędkości obrotu robota. Możemy zatem przejść do stworzenia regulatora, który napędzi silniki robota bazując na danych pochodzących z profilerów prędkości.

Kręć się, kręć - regulator PD

Zgodnie ze schematem przedstawionym w części teoretycznej, potrzebne nam będą dwa regulatory PD, jeden odpowiedzialny za sterowanie translacją, drugi - rotacją. Ponieważ implementacja PD w kodzie jest sprawą wyjątkowo prostą, nie będę rozwijał tematu i przedstawię gotowe rozwiązanie.

Niniejszym główna część naszego sterownika robota jest gotowa. Po odpowiednim dostrojeniu regulatorów możemy zadać robotowi określone parametry ruchu (odległość, kąt oraz przyspieszenia i prędkości translacji i rotacji), które zostaną precyzyjnie zrealizowane.

To już wszystko

Do stworzenia kompletnego oprogramowania robota konieczny będzie jeszcze moduł obsługujący enkodery i zapisujący przejechane odległości w zmiennych enk_totalS ienk_totalT oraz moduł nadrzędny, który sterować będzie pracą profilerów. Można go zrealizować np. poprzez zaimplementowanie listy kolejnych ruchów i aktualizowanie zadanych parametrów ruchu po osiągnięciu stanu KONIEC przez oba profilery.

Tradycyjnie z góry przepraszam za ewentualne błędy, które w razie wyłapania będę poprawiał. Wszelkie uwagi odnośnie moich metod jak i samego teksu także są mile widziane.

To nie koniec, sprawdź również

Przeczytaj powiązane artykuły oraz aktualnie popularne wpisy lub losuj inny artykuł »

FPGA Hackathon 2024 – zbuduj księżycową metropolię!

Poradnik prezentowy: zestawy do nauki elektroniki (2023/24)

Autonomiczny odkurzacz DIY – open source na Arduino

Wiertarka magnetyczna z recyklingu w wersji DIY

3 narzędzia, które powinien znać każdy programista

OpenCV – #2 – Wykrywanie obiektów

Kurs budowy robotów – #2 – sterownik robota, czujniki

Filtr Kalmana od teorii do praktyki – #3 – Testy na STM32

micromouse, PD, PID, profiler, regulacja, regulatory, sterowanie