Programowanie Arduino w Simulinku – balansujący motocykl

Mechatronika to interdyscyplinarna dziedzina nauki, którą w najprostszy sposób można określić jako dodawanie rozwiązań elektronicznych do urządzeń mechanicznych. Często utożsamiana jest ona z automatyzacją (urządzenie wcześniej nastawiane ręcznie teraz sterowane jest w oparciu o układ elektroniczny – sterownik). Mechatronika łączy w sobie zagadnienia z wielu obszarów techniki, m.in. mechaniki, elektroniki czy informatyki.

Można by pomyśleć, że ciężko rozpocząć swoją przygodę z tak trudną dziedziną wiedzy. Nic bardziej mylnego. Artykuł ten przedstawia, jak za pomocą prostych narzędzi z Arduino Engineering Kit oraz oprogramowania MATLAB i Simulink jesteśmy w stanie zrealizować od podstaw projekt rozwiązania mechatronicznego – samobalansujący motocykl.

Czym jest Arduino Engineering Kit?

Arduino Engineering Kit to zestaw narzędzi programowych oraz sprzętowych stworzony z myślą o osobach zainteresowanych tematyką mikrokontrolerów, robotyką i tworzeniem czegoś „rzeczywistego”, tj. wykonującego jakieś praktyczne zadanie w świecie rzeczywistym. Zawiera on komplet elementów pozwalających na zbudowanie trzech różnych robotów:

1. samobalansującego motocykla,
2. robota mobilnego wykorzystującego rozpoznawanie obrazów,
3. robota plotera komunikującego się z komputerem i rysującego wybrane zdjęcia.

Część sprzętowa zestawu bazuje na Arduino, dzięki czemu mamy łatwy dostęp do różnego rodzaju płytek rozszerzeń, gotowych schematów i bibliotek oraz elementów typu czujniki, serwonapędy i silniki. Część programowa będzie wykorzystywała oprogramowanie MATLAB i Simulink, a także ich możliwości w dziedzinie automatycznej generacji kodu, dzięki czemu również osoby niekoniecznie będące asami programowania bez problemu poradzą sobie z realizacją swoich pomysłów.

Oczywiście, aby zdobyte doświadczenie wykorzystać we własnych rozwiązaniach, każdy z projektów dostarczono w otoczce wiedzy teoretycznej, wyjaśniającej nie tylko, jak to zostało zrobione, ale również – w jaki sposób działa. Takie połączenie działań praktycznych wraz z przystępną dawką teorii nosi miano project-based learningu. 

 

Zawartość zestawu Arduino Engineering Kit

W zestawie znajdziemy niezbędne elementy mechaniczne (w tym śruby, nakrętki, podkładki i zębatki) oraz dedykowane elementy konstrukcyjne wykonane z tworzywa sztucznego. Konstrukcje zostaną przez nas wprawione w życie dzięki dołączonym serwonapędom i silnikom prądu stałego. Oprócz tego w zestawie znajdziemy też potrzebną elektronikę Arduino MKR1000, czujniki, markery oraz kamerę USB.

Wraz z zestawem dostarczana jest „paczka wiedzy” – kod dostępowy do portalu Arduino z materiałami dotyczącymi poszczególnych projektów w postaci dedykowanej platformy e-learningowej. Na niej znajdziemy szczegółowe informacje na temat każdego z trzech robotów, wykaz elementów oraz instrukcje składania. Pojawi się tam też pełen spis zagadnień, jakie zostaną poruszone w danym projekcie (składanym robocie), poradniki na temat zastosowanego oprogramowania oraz instrukcje do każdego z poszczególnych robotów w postaci interaktywnych ćwiczeń.

Ostatnią, ale równie ważną rzeczą wchodzącą w skład zestawu Arduino Engineering Kit jest licencja na oprogramowanie firmy MathWorks, MATLAB i Simulink, wraz z zestawem tzw. toolboxów, swego rodzaju bibliotek zawierających zaawansowane funkcje pozwalające m.in. na projektowanie systemów sterowania, algorytmów wizyjnych i przetwarzania sygnałów (Simscape, Simscape Multibody, Control System Toolbox, Curve Fitting Toolbox, DSP System Toolbox, Image Processing Toolbox, Instrument Control Toolbox, Optimization Toolbox, Signal Processing Toolbox i Symbolic Math Toolbox).

Jak zbudować samobalansujący motocykl?

Jak wiadomo, dwukołowy motocykl bez podpórki nie może stać w jednym miejscu, bo szybko przewróci się na jeden lub drugi bok. Celem projektu jest stworzenie systemu, który sprawi, że pojazd będzie stabilnie stał w miejscu. Wykorzystamy do tego napędzane silnikiem małe koło zamachowe, które będzie generowało moment siły zdolny do ustabilizowania pojazdu.

Oczywiście, aby wykonać to w sposób precyzyjny, będzie trzeba posiadać ciągłą informację o stanie motocykla i dokładnie dobierać prędkości koła zamachowego, tak aby przypadkiem nie przechylić go na drugą stronę. Tym zajmie się dedykowany algorytm sterowania, który zaprojektujemy w środowisku Simulink i automatycznie przekształcimy go na kod C, którym później zaprogramujemy Arduino.

Modelowanie obiektu sterowania

MATLAB i Simulink słyną ze swoich możliwości symulacyjnych – poprzez modelowanie jakiegoś obiektu (np. motocykla) jesteśmy w stanie testować jego zachowanie w komputerze, bez potrzeby tworzenia rzeczywistego prototypu.

 

 

W klasycznym podejściu do modelowania wymagana jest znajomość zjawisk fizycznych stojących za działaniem obiektu (najczęściej równania różniczkowe), co nie zawsze jest możliwe z uwagi na złożoność systemu lub brak informacji (np. silniki bez oznaczeń i not katalogowych). Na szczęście narzędzie Simscape, służące do modelowania fizycznego, pozwala nam na utworzenie modelu obiektu w Simulinku przy znajomości tylko jego struktury fizycznej i głównych parametrów charakterystycznych, bez potrzeby znajomości wszystkich równań opisujących jego dynamikę.

W naszym przypadku zastosowane zostało Simscape Multibody, w którym mając model 3D motocykla, możemy zaimportować złożoną geometrię do środowiska Simulink wraz z informacją na temat mas oraz momentów bezwładności i automatycznie przekształcić je w symulacyjny model fizyczny – w modelu tym możemy testować nasze algorytmy (jego stworzenie możliwe jest przy użyciu oprogramowania Autodesk Inventor, Solidworks i Creo Parametric).

Po zaimportowaniu geometrii 3D motocykla (standardowe pliki STL) powstaje schemat mechaniczny obiektu, tj. poszczególne elementy motocykla w postaci brył sztywnych o danej masie oraz danym momencie bezwładności połączone ze sobą poprzez węzły (np. obrotowe). To w nich właśnie będziemy mierzyli (jeszcze wirtualnie) sygnały takie jak np. położenie, prędkość czy przyspieszenie kątowe poszczególnych elementów. Na ich podstawie określimy stan, w jakim znajduje się motocykl, a przede wszystkim kąt wychylenia motocykla θ – opierając się na nim, będziemy wnioskować, czy motocykl stoi pionowo, czy też przewraca się.

Projektowanie układu sterowania

Mając do dyspozycji model obiektu, nic nie stoi na przeszkodzie, aby zacząć projektować odpowiedni algorytm regulacji, który na podstawie zmierzonych danych (położenia kątowego) wysteruje koło zamachowe, tak aby ustawić motocykl do pionu. Zastosujemy tutaj klasyczny regulator PID, który powszechnie używany jest w automatyce. Nasz obiekt regulacji (motocykl) umieścimy w pętli sprzężenia zwrotnego, tworząc URA (układ regulacji automatycznej).

Teorię na temat regulacji PID znajdziemy w większości podręczników z teorii sterowania. Opisując ją jednak w skrócie: mamy wielkość, którą chcemy sterować – niech będzie to wychylenie motocykla y(t), które jest mierzone przez czujniki i prezentowane w postaci sygnału v(t). Sygnał ten jest porównywany z wielkością zadaną w(t) w węźle sumacyjnym, a rezultatem jest sygnał e(t), który określamy mianem błędu regulacji. Ten sygnał wprowadzony zostaje do algorytmu regulacji PID, który przetwarza go, poddając operacjom w czasie: mnożenie (P), całkowanie (I), różniczkowanie (D).

Każdy z tych członów ma przypisane tzw. wzmocnienie (przemnożenie przez stałą), które określa, z jaką intensywnością oddziałują one na sygnał e(t) i w rezultacie zmieniają jego dynamikę (np. jeżeli wzmocnienie całkowania wynosi 0, to człon całkowania zostaje całkowicie wyłączony).

Po wykonaniu wszystkich operacji otrzymujemy sygnał sterujący u(t), który pośrednio posłuży do wysterowania elementu wykonawczego, np. poprzez mostek H wysteruje silnik prądu stałego z kołem zamachowym, co w rezultacie wpłynie na stan całego obiektu regulacji (motocykla), zmieniając jego położenie kątowe y(t). Cały układ sterowania, realizowany przez komputer lub mikrokontroler, będzie działał nieustannie, a dynamicznie zmieniające się sygnały y(t) oraz v(t) sprawią, że regulator automatycznie będzie dążył do zminimalizowania błędu regulacji e(t), czyli sytuacji idealnej, gdzie wartość mierzona v(t) jest dokładnie taka sama jak w(t).

 

 

Całą sztuką jest takie dobranie poszczególnych nastaw, aby uzyskać oczekiwane efekty bez pogarszania charakterystyk dynamicznych. Na przykład zwiększenie wzmocnienia P sprawia, że nasz regulator działa „szybciej” na zmianę błędu regulacji, ale jednocześnie zwiększa ryzyko, że nastąpi zjawisko przeregulowania, tzn. gdy chcemy uzyskać wychylenie na poziomie 0 stopni, na samym początku działania regulatora może nastąpić „przestrzelenie” wartości, np. motocykl odchyli się aż do 5 stopni i dopiero potem zacznie wracać bliżej 0 stopni (analogicznie brak przeregulowania oznacza, że wartość zadana nie zostanie przekroczona).

Zwiększanie wzmocnienia całkującego I polepsza tzw. uchyb ustalony, tzn. gdy wartość zadana wynosi 0 stopni, to nieważne, jak długo działa nasz regulator – może się okazać, że ostateczny kąt wychylenia wynosi 5 stopni. Różnica między wartością zadaną a wartością finalną to właśnie uchyb ustalony. Jednocześnie zwiększenie wzmocnienia całkującego pogarsza stabilność układu, czyli może dojść do oscylacji, w której nigdy nie uzyskamy wychylenia dokładnie 0 stopni, lecz będzie się ono mieściło w zakresie od np. −5 do 5 stopni.

Jak widzimy, zmiana pojedynczego parametru niesie ze sobą zarówno pozytywne, jak i negatywne skutki, dodatkowo niektóre parametry wpływają na siebie nawzajem. Próba strojenia regulatora „na oko”, owszem, jest możliwa, istnieją również teorie opisujące metody strojenia, np. Zieglera–Nicholsa.

Samo środowisko MATLAB wraz z toolboxem Control System Toolbox posiada pełne zaplecze narzędzi, nie tylko do analizy działania obiektu regulacji czy też badania charakterystyk obiektu, ale również do automatycznego strojenia regulatorów PID i innych, czyli praktycznie potrafi wykonać za nas najcięższą pracę. Dodatkowo, mając do dyspozycji pełen model naszego obiektu, wszelkie próby dobierania nastaw możemy zweryfikować natychmiast poprzez symulację, praktycznie bezwysiłkowo, jednocześnie wspomagając się możliwością monitorowania i wizualizacją każdego z przebiegów w modelu, co nie zawsze byłoby możliwe przy pracy na rzeczywistym prototypie. Możemy sobie zatem odpowiedzieć na pytanie, który z członów regulatora PID ma największy wpływ na zachowanie motocykla.

Realizacja regulatora PID w środowisku Simulink sprowadza się do dodania dedykowanego bloczka, w którym konfigurujemy algorytm regulacji, podając nie tylko wartości wzmocnień, ale również określając, czy chcemy, aby regulator był ciągły czy dyskretny, a następnie korzystamy z zaawansowanych ustawień, jak np. saturacja, anty-windup, forma regulatora i metody całkowania numerycznego.

Jednocześnie zaczynamy porządkować model, zamykając części funkcjonalne w podsystemy, tak aby zwiększyć jego czytelność. W tym miejscu warto również zwrócić uwagę, że na drodze eksperymentów symulacyjnych może się okazać, że nie wszystkie człony regulatora PID będą potrzebne. W rezultacie w dalszej części przykładu stosowany będzie regulator PD.

Implementacja regulatora PID w praktyce

Docelowo chcemy, aby cały układ regulacji został przeniesiony na nasze Arduino i zaczął sterować rzeczywistym motocyklem. Mając przygotowany algorytm regulacji przetestowany na naszym wirtualnym prototypie, możemy teraz bezpiecznie przejść do fazy implementacji. Wcześniej jednak należy przebudować model, uwzględniając rzeczywisty obiekt oraz mikrokontroler. Do tej pory pomiar kąta położenia był realizowany wirtualnie, a od teraz będzie zajmował się tym dedykowany czujnik podłączony do interfejsu I2C mikrokontrolera. Tak samo ze sterowaniem silnika – wcześniej wirtualny sygnał, teraz wyjście PWM Arduino.

W tym momencie możecie zadać pytanie: skąd w Simulinku weźmiemy blok, np. wyjścia, PWM Arduino? Otóż w tym z pomocą przyjdzie nam tzw. pakiet wsparcia sprzętowego dla Arduino, który dostarcza do środowiska Simulink bloki niezbędne do obsługi peryferii naszych mikrokontrolerów. Dzięki nim tam, gdzie wcześniej np. źródłem jakiegoś sygnału były stała lub wirtualny generator, teraz może to być rzeczywisty przetwornik analogowo-cyfrowy. Tak samo z wyjściami cyfrowymi bądź PWM.

Dodatkowo do dyspozycji mamy zaawansowane funkcje, jak np. komunikacja I2C, SPI, UART, obsługa przerwań, a także obsługę czujników (odległości, przyspieszeń itp.) oraz – w zależności od mikrokontrolera i dołączonych shieldów – komunikację Ethernet lub Wi-Fi (TCP/IP i UDP), jak również obsługę elementów wykonawczych (np. silników DC, serwonapędów).

Środowisko Simulink jest w stanie w sposób automatyczny wygenerować kod C na urządzenia wbudowane ze swoich bloków oraz funkcji. Jednocześnie bloki do Arduino z pakietu wsparcia to tak naprawdę fragmenty gotowego kodu C zaczerpnięte z bibliotek IDE Arduino.

Łącząc te dwie rzeczy razem, jesteśmy w stanie zaprojektować pełne rozwiązanie w sposób przystępny za pomocą schematów blokowych i interfejsu graficznego, a następnie z całości automatycznie wygenerować kod w języku C oraz wgrać go na mikrokontroler, nie pisząc przy tym nawet jednej linijki kodu. Coś takiego nie tylko umożliwia początkującym programistom łatwiejszy start w świecie mikrokontrolerów, ale również znacząco przyśpiesza prototypowanie nowych rozwiązań i jednocześnie pozwala uniknąć wielu typowych błędów pojawiających się podczas ręcznego pisania kodu, m.in. poprzez symulacyjne testowanie naszego algorytmu.

Implementacja czujników i napędów

Aby czymś sterować, najpierw trzeba to zmierzyć. To, co nas interesuje w samobalansującym motocyklu, to jego pochylenie względem ziemi, a zmierzymy je za pomocą zespołu czujników inercyjnych. Na rynku możemy znaleźć wiele tzw. jednostek IMU. Są to kompleksowe rozwiązania pomiarowe do tzw. nawigacji inercyjnej, łączące w sobie szereg czujników, jak np. żyroskop, przyspieszeniomierz, magnetometr. Pakiet wsparcia sprzętowego dla Arduino posiada m.in. dedykowany blok do obsługi popularnego czujnika BNO055.

 

 

Konfiguracja czujnika sprowadza się do nadania adresu I2C oraz wskazania interesujących wielkości, w tym przypadku przyśpieszenia kątowego i położenia kątowego. Dodatkowo podajemy czas próbkowania czujnika, czyli to, jak często otrzymamy nowe informacje. Pracując z czujnikami inercyjnymi, niezwykle istotna jest kalibracja.

Jeżeli chodzi o sam model obsługujący czujnik, przyśpieszenia kątowe zostały wyprowadzone na blok wyświetlacza Display, tak samo jak położenia kątowe (Euler Angles). Z uwagi na to, że mamy trójosiowy czujnik i sygnał jest wektorem wielkości, blok Selector wybiera tylko to, które nas interesuje, tj. położenie kątowe w pionie. Kolejno blok sumacyjny pozwala wprowadzić poprawkę położenia, np. gdy w odczycie z czujnika pojawia się stały uchyb, możemy go tu zredukować.

Sterowanie silnikiem i jednocześnie pomiar prędkości koła zamachowego zrealizowane zostały za pomocą dedykowanych bloków sterowania silnikiem DC, które współpracują bezpośrednio z Arduino poprzez dedykowany MKR Motor Carrier Shield ze zintegrowanymi mostkami H. Dzięki temu cała konfiguracja to obranie portu, do którego podpięty został silnik, i zapamiętanie zakresu, w jakim sterujemy silnikiem (od −255 dla całej wstecz do 255 dla całej naprzód). Stąd bezpośrednio przed blokiem silnika pojawiło się mnożenie przez −255.

Pomiar prędkości koła zamachowego realizowany jest za pomocą prostego tachometru bazującego na czujniku Halla i dwóch magnesach umieszczonych w kole zamachowym. Znając liczbę impulsów, które zostały zliczone w skończonym odcinku czasu, wiemy, jaką drogę kątową wykonało koło, i w taki sposób możemy oszacować prędkość kątową koła w stopniach na sekundę.

Poza samym algorytmem sterowania warto pomyśleć o bezpieczeństwie naszego motocykla, np. o ograniczeniu prędkości obrotowej koła zamachowego, tak aby nie przegrzać silnika, lub o sytuacji, gdy bateria będzie blisko rozładowania. To zostało zrealizowane za pomocą bloków Simulink oraz bloków Arduino. Numerem 1 oznaczona jest sekcja kontroli napięcia baterii. Pomiar wykonywany jest przez 16-bitowy przetwornik ADC, więc wartość wyjściowa musi zostać odpowiednio przeskalowana do napięcia, gdzie następnie logika decyzyjna sprawdza, czy napięcie jest większe niż 10,4 V. Dzięki blokowi z pamięcią Unit Delay, nawet gdy napięcie spadnie na moment, np. pod dużym obciążeniem, układ zapamięta, że doszło do takiej sytuacji, i zablokuje motocykl do momentu naładowania baterii.

Znajdziemy tam również bloczek (oznaczony numerem 2) Interval Test, który sprawdza, czy motocykl się przewrócił (wartość kąta wychylenia większa/mniejsza niż 6/−6 stopni). Wykrycie takiej sytuacji zablokuje koło zamachowe, tak aby nie uszkodzić silnika. Idąc dalej w górę schematu, znajdziemy podsystem, w którym również kontrolujemy prędkość obrotową koła zamachowego, i gdy jest ona zbyt wysoka (np. motocykl napotkał przeszkodę i sam moment obrotowy koła nie wyprowadzi go z tego położenia), obroty zostaną zablokowane. Ostatecznie górny schemat blokowy, funkcja logiczna OR oraz element z pamięcią służą do tymczasowego wyłączenia motocykla, gdy jednostka IMU straci kalibrację. Te wszystkie flagi zatrzymania wspólnie tworzą jeden sygnał decydujący o blokadzie motocykla w momencie, gdy nastąpi co najmniej jedna niepożądana sytuacja.

Implementacja oraz symulacja w trybie External

Mając już gotowy model do implementacji, rozbudowany o sterowniki sprzętowe i zabezpieczenia programowe, możemy nareszcie wgrać nasz program na Arduino i uruchomić motocykl. Zrobimy to jednak w taki sposób, aby móc monitorować pracę naszego układu.

Standardowym podejściem byłoby wyposażenie Arduino w wyświetlacz LCD i prezentacja na nim jakichś danych (co wymagałoby dodatkowych elementów elektronicznych) lub wykorzystanie połączeń z PC. Mając do dyspozycji środowisko MATLAB oraz Simulink, wykorzystamy tzw. symulację w trybie External. Na czym ona polega? Otóż nadal mamy do czynienia z generacją kodu – z naszego modelu, algorytmu sterowania i bloków Arduino utworzony zostanie kod C, następnie skompilowany i wgrany na Arduino, jednak dodatkowo zostaną do niego dołączone funkcje i procedury umożliwiające komunikację poprzez wybrany interfejs sprzętowy, nie tylko z naszym komputerem, ale z całym środowiskiem MATLAB i Simulink oraz wcześniej utworzonym modelem. Dzięki temu będziemy mogli wykorzystać wirtualne wykresy Scope i wyświetlacze Display do wizualizacji i monitorowania dowolnych sygnałów w naszym programie działającym już na mikrokontrolerze z rzeczywistym obiektem.

Jednak to nie wszystko. Komunikacja jest obustronna – nie tylko mikrokontroler będzie wysyłał dane do Simulinka i MATLAB-a, ale my też możemy wysyłać informacje do Arduino, mianowicie możemy „na żywo” modyfikować parametry występujące w naszym algorytmie (np. modyfikacja nastaw PID, aby dostroić je precyzyjnie już do rzeczywistego motocykla) lub też utworzyć wirtualny panel kontrolny (z przyciskami, przełącznikami, suwakami itp.) do interakcji z naszym żywym, działającym programem.

Uruchomienie symulacji w trybie External odbywa się poprzez przestawienie trybu symulacji z Normal na External oraz wprowadzenie odpowiedniej konfiguracji. Wśród ustawień modelu niezbędnych do generacji kodu mamy m.in. model Arduino, rodzaj połączenia trybu External (Wi-Fi, UART i/lub protokół XCP), konfigurację interfejsów (prędkości magistrali I2C, SPI itp.) lub ustawienia poziomu napięcia odniesienia dla przetworników ADC.

Następnie przyciskiem „Run” uruchamiamy tryb External i tu już zamiast standardowej symulacji modelu rozpoczną się procesy generacji kodu C, wywołania oprogramowania do kompilacji kodu, a potem programatora, który wgra całość na nasz procesor.

Oczywiście nie jest to jedyna możliwość generacji kodu. Do dyspozycji jest również funkcja Deploy to Hardware, która w prosty sposób zaprogramuje Arduino do niezależnej i samodzielnej pracy bez potrzeby dalszego angażowania oprogramowania MATLAB i Simulink  (oczywiście przy utracie możliwości monitorowania sygnałów i modyfikacji parametrów).

Pozostałe projekty

Przedstawiony samobalansujący motocykl to jeden z trzech robotów, które możemy zbudować za pomocą Arduino Engineering Kit oraz zaprogramować, używając narzędzi MATLAB i Simulink. Pozostałe dwa to robot ploter, który komunikując się z komputerem oraz MATLAB-em, jest w stanie odrysować na tablicy zdjęcia i grafiki (wskazane i odpowiednio przetworzone) oraz robot mobilny, który stosując rozpoznawanie obrazów i SLAM, odnajdzie swoją pozycję względem celu (plastikowego klocka), a następnie podjedzie do niego i przeniesie w inne miejsce.

Więcej na ich temat znaleźć można w poniższej serii filmów:

Podsumowanie

Jak mogliśmy się przekonać, oprogramowanie MATLAB i Simulink w przystępny sposób pozwala nie tylko na wirtualne symulowanie, testowanie i prototypowanie rozwiązań, nawet bez znajomości teorii modelowania oraz równań różniczkowych, ale również na bezproblemowe zaprogramowanie mikrokontrolera za pomocą algorytmu, który w środowisku Simulink tworzony jest w sposób intuicyjny, przez osoby niekoniecznie biegłe w kodowaniu – poprzez narzędzia do automatycznej generacji kodu.

Na zakończenie warto dodać, że Arduino to nie jedyna rodzina modułów, które można programować ze środowiska Simulink. Odpowiednie pakiety wsparcia pozwalają na pracę z Raspberry Pi, STM Nucleo i Discovery, Texas Instruments C2000 oraz systemem ROS. Więcej na temat Arduino Engineering Kit możemy znaleźć na stronie jego producenta, a informacje oraz dodatkowe przykłady na temat współpracy środowisk MATLAB oraz Simulink z Arduino i innymi platformami sprzętowymi na stronach firmy MathWorks. Łączyliście już Arduino lub Raspberry Pi z MATLAB-em?

Autor: Konrad Kolski

https://forbot.pl/blog/programowanie-arduino-w-simulinku-balansujacy-motocykl-id39730