Kursy • Poradniki • Inspirujące DIY • Forum
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.
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ą.
Kontrola silnika na ogół jest opisywana i przeprowadzana na dwa sposoby:
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.
Wniosek - odpowiednio zadając położenie - przesuwając je wraz z upływem czasu - możemy jednocześnie panować nad prędkością!
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:
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ą:
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.
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 nasz procesor potrafi sprzętowo obsługiwać zmienne typu float - możemy spokojnie pominąć ten temat i w naturalny sposób przechowywać w nich potrzebne dane.
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.
|
1 2 3 4 5 6 7 8 9 10 11 12 |
u16 int1 = 100; // liczba 100 w tradycyjnym zapisie liczb całkowitych u16 fr1 = 100 << 8; // liczba 100 w zapisie stałoprzecinkowym 8.8 // de facto zmienna2 jest teraz równa 25600, ale jest przez nas interpretowana jako liczba 100 u16 fr2 = (100 << 8) + 128; // liczba 100,5 w zapisie 8.8 // część ułamkowa w zmienna3 to 128/256 = 0,5 u16 dodawanie1 = (int1 << 8) + fr1; // dodanie u16 do u8.8 z wynikiem w formie u8.8 u16 dodawanie2 = int1 + ((fr2 + 128) >> 8); // dodanie u16 do u8.8 z wynikiem w formie u16 // dodanie 128 do zmiennej przed przesunięciem bitowym powoduje zaokrąglenie od 0,5 w górę, zamiast obcięcia części ułamkowej |
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.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 |
stan = JAZDA; // stan profilera; możliwe wartości: JAZDA, HAMOWANIE, KONIEC docelowa_V_8 = max_V; // docelowa_V - docelowa prędkość, do której stopniowo dąży profiler // max_V - maksymalna dozwolona prędkość next_V_8 = XX; // prędkość w następnym ruchu (po przejechaniu zadanej odległości) aktualna_V_8 = 0; // aktualnie zadana prędkość przysp_8 = XX; // zadane przyspieszenie zad_S = 0; // wyliczona droga do przejechania - wykorzystywana jako setpoint regulatora PD target_S = XX; // zadana całkowita droga do przejechania profiler() { if(stan == JAZDA) { if(trzeba_hamowac()) { stan = HAMOWANIE; docelowa_V_8 = next_V_8; } } if(dojechal_do_celu()) { stan = KONIEC; docelowa_V_8 = next_V_8; } if(aktualna_V_8 < docelowa_V_8) { aktualna_V_8 += przysp_8; if(aktualna_V_8 > docelowa_V_8) aktualna_V_8 = docelowa_V_8; } if(aktualna_V_8 > docelowa_V_8) { aktualna_V_8 -= przysp_8; if(aktualna_V_8 < docelowa_V_8) aktualna_V_8 = docelowa_V_8; } zad_S += (aktualna_V_8 + 128) >> 8; // zad_S jest typu całkowitego - należy odpowiednio przekonwertować zmienną aktualna_V_8 przed dodaniem } |
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łą:
|
1 |
if(((aktualna_V_8 - next_V_8)*((aktualna_V_8 + next_V_8 + 128) >> 8))/(2*przysp_8) >= target_S - zad_S) |
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.
Co prawda tracimy na tym odrobinę precyzji, ale unikamy możliwości przepełnienia się zmiennej - przemnożenie dwóch liczb 8.8 daje w wyniku zmienną 16.16, a więc wymagającą aż 32-bitów.
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:
|
1 |
if(set_S >= target_S || (stan == HAMOWANIE && MYSZ.prof_mV == 0)) |
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.
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.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 |
stan_T = JAZDA; // stan profilera; możliwe wartości: JAZDA, HAMOWANIE, KONIEC docelowa_T_8 = max_T; // docelowa_T - docelowa prędkość, do której stopniowo dąży profiler // max_T - maksymalna dozwolona prędkość aktualna_T_8 = 0; // aktualnie zadana prędkość przyspT_8 = XX; // zadane przyspieszenie zad_T = 0; // wyliczony kąt do przejechania - wykorzystywany jako setpoint regulatora PD target_T = XX; // zadany całkowity kąt do przejechania profiler() { if(stan == JAZDA) { if(aktualna_T_8 * ((aktualna_T_8 + 128) >> 8)/(2 * przyspT_8) >= abs(target_T) - abs(zad_T)) { stan = HAMOWANIE; docelowa_T_8 = next_T_8; } } if(stanT == HAMOWANIE && aktualna_T_8 == 0) { stan = KONIEC; docelowa_T_8 = next_T_8; } if(aktualna_T_8 < docelowa_T_8) { aktualna_T_8 += przysp_8; if(aktualna_T_8 > docelowa_T_8) aktualna_T_8 = docelowa_T_8; } if(aktualna_T_8 > docelowa_T_8) { aktualna_T_8 -= przysp_8; if(aktualna_T_8 < docelowa_T_8) aktualna_T_8 = docelowa_T_8; } if(target_T > 0) zad_T += ((aktualna_T_8 + 128) >> 8); else zad_T -= ((aktualna_T_8 + 128) >> 8); } |
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.
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.
Nazwy zmiennych powinny być wystarczające do zrozumienia działania regulatora.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
reg() { regV_aktE = zad_S - enk_totalS; // obliczenie aktualnego błędu translacji regT_aktE = zad_T - enk_totalT; // obliczenie aktualnego błędu rotacji // zmienne enk_totalS oraz enk_totalT zawierają aktualne pomiary z enkoderów, zgodnie ze schematem zamieszczonym powyżej reg_newPWML = regV_P * regV_aktE + regV_D * (regV_aktE - regV_prevE) + regT_P * regT_aktE + regT_D * (regT_aktE - regT_prevE); reg_newPWMP = regV_P * regV_aktE + regV_D * (regV_aktE - regV_prevE) - regT_P * regT_aktE + regT_D * (regT_aktE - regT_prevE); regV_prevE = regV_aktE; regT_prevE = regT_aktE; setPWM(reg_newPWML, reg_newPWMP); } |
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.
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.
Artykuł ten nie zawiera gotowego przepisu na zaprogramowanie robota i powinien raczej być traktowany jako opis możliwego podejścia do samego jego wysterowania. Z tego powodu nie wszystkie zmienne zostały opisane (choć ich nazwy nie powinny budzić wątpliwości w ich przeznaczeniu), a także mogło wystąpić kilka niekonsekwencji w nazewnictwie. Mam nadzieję, że nie przeszkodzi to w zrozumieniu tekstu oraz zastosowaniu opisanych tu metod w swojej konstrukcji.
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.
Przeczytaj powiązane artykuły oraz aktualnie popularne wpisy lub losuj inny artykuł »
micromouse, PD, PID, profiler, regulacja, regulatory, sterowanie
Dołącz do 20 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY z Arduino i RPi.
Trwa ładowanie komentarzy...