Sterowanie PID z TurtleSim

Repozytorium GitLab projektu

Streszczenie

Move Turtle to prosty skrypt Pythona dla ROS, który przesuwa żółwia w TurtleSim ROS do określonych współrzędnych (punktów). Projekt ten rozpoczął się jako projekt zaliczeniowy na zajęcia CSCI473 w Colorado School of Mines (CSOM), a później przekształcił się w projekt, którego używałem, aby nauczyć się, jak można tworzyć układy zamkniętej pętli, a wiedza z tego zakresu została wykorzystana, by pomóc mi w pracy w laboratorium Human Center Robotics (HCR) na CSOM.

Kontekst

Dla pewnego tła, ROS oznacza Robot Operating Systems. ROS to oprogramowanie pośredniczące typu open-source dla robotyki, zwykle używane razem z systemem operacyjnym Ubuntu, które służy do lepszego zarządzania komponentami/klastrami oprogramowania systemu robotycznego. Możesz dowiedzieć się więcej o tym, jak używać ROS, korzystając z oficjalnej wiki. Kod, który piszesz w ROS, jest w C++ albo Pythonie.

TurtleSim to prosty symulator robota 2D używany jako wprowadzenie do ROS i pakietów ROS dla nowych użytkowników ROS. TurtleSim robi tylko tyle, że otwiera prosty wyświetlacz 2D z żółwiem. Ten żółw działa jak „robot” i możesz wysyłać wiadomości do tego „robota”, aby się poruszał i/lub obracał. Podczas ruchu TurtleSim generuje pewien losowy błąd, aby w prosty sposób symulować fizykę świata rzeczywistego.

Skoro wiadomo, czym są ROS i TurtleSim, dlaczego ich używałem? Cóż, wiosną 2020 roku brałem udział w zajęciach o nazwie „Human Centered Robotics” w Colorado School of Mines jako część mojego zakresu przedmiotów na studiach licencjackich z informatyki. Zajęcia prowadził dr Hao Zhang. Kurs składał się z trzech projektów, a pierwszy polegał na skonfigurowaniu ROS, nauczeniu się korzystania z ROS oraz zaimplementowaniu algorytmu otwartej pętli albo zamkniętej pętli, który sprawiłby, że żółw w TurtleSim narysuje literę M. Oryginalny opis projektu można zobaczyć TUTAJ.

Kiedy byłem na tych zajęciach, zrealizowałem ten projekt, implementując algorytm otwartej pętli. To rozwiązanie działało, ale było naprawdę kiepskie i narysowało A, które było „wystarczająco dobre”, ale wcale nie było „świetne”. Po oddaniu projektu około połowy lutego 2020 roku zapomniałem o nim aż do roku później, około lutego 2021 roku.

Wyzwanie

W lutym 2021 roku zostałem zatrudniony jako asystent badawczy w laboratorium Human Centered Robotics (HCR) w Colorado School of Mines, gdzie pracowałem pod kierunkiem dr. Hao Zhanga. Celem laboratorium jest prowadzenie badań nad „całożyciową współpracującą autonomią, z celem umożliwienia robotom działania i adaptacji przez długie okresy czasu”. Na dzień 1-9-2022 (1 września 2022) laboratorium przeniosło się z Colorado School of Mines do University of Massachusetts Amherst. Jednak kiedy tam pracowałem, nadal znajdowało się w Kolorado.

W laboratorium HCR miałem za zadanie pracować nad projektem Triton. Projekt Triton składał się z wielu, dosłownie dziesiątek, robotów o nazwie Tritons. Tritony były trójkątnymi naziemnymi robotami z kołami omni, które wyglądały tak:

Oprócz Tritonów, osiem kamer IR firmy OptiTrack rozmieszczono w „układzie ośmiokąta” kilka stóp nad ziemią. Korzystając z tych kamer, oprogramowania Motive firmy OptiTrack, komputera PC z systemem Windows 10 oraz ROS, tworzona jest przestrzeń 2 metry na 2 metry na 2 metry (x, y, z), w której można określić dokładne rzeczywiste położenie obiektów z markerami motion capture.

Moim pierwszym zadaniem przy projekcie Triton było przygotowanie skryptów, które sprawiały, że robot Triton poruszał się do określonej rzeczywistej pozycji. W tamtym czasie nie byłem pewien, jak to osiągnąć, ale pamiętałem pierwszy projekt z CSCI473, w którym wprowadzono mi ideę systemu zamkniętej pętli, więc zacząłem naprawdę zgłębiać systemy zamkniętej pętli, a w konsekwencji także systemy sprzężenia zwrotnego.

Badania

Podczas moich badań odkryłem ten świetny film autorstwa AerospaceControlsLab:

W tym filmie pokazano mi i wyjaśniono ideę systemu PID. System PID to system zamkniętej pętli, układ sterowania, który generuje określone wyjścia na podstawie danych z rzeczywistego świata. Mówiąc prosto, to system sprzężenia zwrotnego, który dostosowuje swoje wyjście na podstawie różnicy między wartością zadaną a wartością zmierzoną. Oto wzór na regulator PID:

$$ u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{de(t)}{dt} $$

  • $u(t)$ to sygnał sterujący.
  • $K_p$, $K_i$, $K_d$ to odpowiednio wzmocnienia proporcjonalne, całkujące i różniczkujące.
  • $e(t)$ to sygnał błędu (różnica między pożądaną a rzeczywistą wartością wyjściową).
  • $\int e(t) dt$ to całka błędu w czasie.
  • $\frac{de(t)}{dt}$ to pochodna błędu.

Aby dowiedzieć się więcej, sprawdź film AerospaceControlsLab albo ten niesamowity artykuł w Wikipedii, z którego pochodzi ten wzór.

Po przeprowadzeniu moich badań oraz uzyskaniu pomocy od moich mądrych znajomych i profesorów ustaliłem, że użycie regulatora proporcjonalnego, czyli regulatora P, będzie najlepsze, aby pomóc Tritonowi dotrzeć do określonej współrzędnej.

Ponieważ Triton jest robotem naziemnym, muszę martwić się tylko o współrzędne X i Y. Wiedząc to, musiałem następnie ustalić, jakie wartości zadane i jakie wartości zmierzone mierzyć oraz przetwarzać w regulatorach P. W moich badaniach znalazłem ten niesamowity wpis na wiki ROS o nazwie Go to Goal, w którym skupiono się na następujących błędach:

  • Błąd odległości: Różnica w odległości między pożądaną lokalizacją (X, Y) a bieżącą lokalizacją (X, Y).
  • Błąd theta: Różnica między pożądaną orientacją a bieżącą orientacją.

Proponowane rozwiązanie

Mając to wszystko na uwadze, rozwiązaniem umożliwiającym przesunięcie Tritona do określonej rzeczywistej współrzędnej jest użycie regulatora proporcjonalnego dla błędu odległości Tritona i błędu theta względem współrzędnej celu oraz bieżącej współrzędnej Tritona. Ale zanim wdrożyłem to w systemie fizycznym, chciałem przetestować tę ideę w symulacji i w tamtym czasie uważałem, że TurtleSim jako symulacja jest najlepszym wyborem.

Testowanie w symulacji

Mając nakreślone teoretyczne rozwiązanie, zdecydowałem się początkowo zweryfikować je poprzez symulację, a nie w rzeczywistym scenariuszu. Rzeczywisty świat często jest pełen szumu, który mógłby zamazać skuteczność rozwiązania i prowadzić do zajmowania się nieistotnymi problemami. Mając to na uwadze, wróciłem do mojego projektu z CSCI473 i zaadaptowałem go jako pole testowe dla tego teoretycznego rozwiązania.

Po zaktualizowaniu starego kodu z ROS Melodic do ROS Noetic oraz po kilku iteracyjnych dopracowaniach udało mi się uruchomić proponowane rozwiązanie w TurtleSim. Pokazy jego działania, wraz z kilkoma fajnymi ścieżkami do współrzędnych celu, można znaleźć na końcu tego wpisu na blogu. Kod do tego wszystkiego można zobaczyć TUTAJ.

Wnioski

Podsumowując, dzięki temu projektowi żółw TurtleSim skutecznie porusza się do zadanych współrzędnych przy użyciu systemu zamkniętej pętli, znacznie przewyższając mój początkowy system otwartej pętli z 2020 roku. Po pomyślnym przetestowaniu zaimplementowałem to rozwiązanie w robotach Triton. Jednak szum ze świata rzeczywistego wprowadził nieoczekiwane problemy, wymagające tygodni debugowania i testów w rzeczywistym świecie. Po tych tygodniach ostatecznie udało mi się doprowadzić Tritona do osiągania określonych współrzędnych w rzeczywistym świecie, stosując ostatecznie rozwiązanie, które przetestowałem w TurtleSim. Oto demonstracja poruszania się Tritona do określonych rzeczywistych współrzędnych przy użyciu metody przetestowanej w TurtleSim:

Z perspektywy czasu żałuję, że nie użyłem symulacji, która lepiej odwzorowywała fizykę świata rzeczywistego. TurtleSim świetnie nadaje się do nauki, ale nie jest zbyt dobry do testowania pomysłów z robotyki w świecie rzeczywistym. W tamtym czasie uważałem, że będzie wystarczający dla mojego prostego zastosowania, ale później okazało się, że tak nie było.

Głównym celem tego wpisu na blogu było opisanie stworzenia systemu zamkniętej pętli umożliwiającego żółwiowi w TurtleSim, a następnie robotom Triton, precyzyjne nawigowanie do określonych pozycji celu (X, Y). Uważam, że cel ten został osiągnięty, ale mogło to wzbudzić pewne zainteresowanie moją pracą w laboratorium HCR. Wiedząc to, planuję przygotować więcej wpisów na blogu o moim doświadczeniu zawodowym w laboratorium HCR…

Demosy Moving Turtle (TurtleSim)

Ten film zawiera następujące demo:

  • Cayley Nodal: Prosty projekt powierzchni węzłowej Cayleya
  • Circles: Projekt przypominający pajęczą sieć
  • Inner Circles: Projekt „okręgów wewnątrz okręgów”
  • M: Bardzo prosty projekt litery M