Freestyle Dwukołowy robot balansujący

[asdpii0]

Witam, przedstawiam robota, z którym męczę się już od jakiegoś czasu... Były próby na silnikach DC ale ze względu na duże luzy na przekładniach przerzuciłem się na silniki skokowe.

1. Wstęp

Określenie robot balansujący odnosi się do robotów działających na zasadzie odwróconego wahadła. Problem odwróconego wahadła jest popularnym problemem w teorii sterowania i jest on częstym tematem prac oraz badań naukowych ze względu na niestabilność układu, który wymaga odpowiedniego sterowania, aby utrzymać punkt równowagi. Do grupy robotów balansujących możemy zaliczyć wiele różnych konstrukcji. Zaczynając od dwukołowych robotów, których środek ciężkości znajduję się powyżej osi obrotu, przez roboty utrzymujące równowagę na piłce, aż do robotów humanoidalnych. Oprócz robotów możemy zaliczyć również niektóre nowoczesne pojazdy transportu.

Rysunek 1.1. Rożne konstrukcje robotow balansujących: a) robot balansujący na kuli Razero, b) dwukołowy robot nBot, c) humanoidalny robot ASIMO

Rysunek 1.2.Przykładowe pojazdy transportu: a) dwukołowy pojazd Segway HT model i170, b) jednokołowy pojazd Solowheel

Ze względu na niestabilność robotów balansujących są one ciekawym obiektem badań. Pozwalają one na zastosowanie wielu metod sterowania, przetwarzania sygnałów oraz wymagają uwzględnienia ograniczeń czasowych wynikających z pracy w trybie rzeczywistym. Zaletą robotów balansujących w porównaniu do innych układów wahadeł odwróconych, przy których niezbędna jest jednostka ruchu liniowego bądź w przypadku robotów poruszających się po okręgu - podstawa, jest ograniczenie zajmowanego miejsca, łatwość transportu oraz możliwość zmiany kierunku ruchu w miejscu. Głównym problemem jest trudność z poruszaniem się po nierównym podłożu, ale również złożoność systemu sterowania w celu utrzymanie równowagi, który komplikuje się jeszcze bardziej przy dodatkowej funkcjonalności robota.

2. Pomiar kąta wychylenia robota

Pomiar kąta wychylenia wahadła od pionu może być realizowany na co najmniej dwa sposoby w robotach balansujących. Pierwszym, rzadziej stosowanym sposobem jest pomiar odległości między czujnikami zamontowanymi w górnej części robota, a podłożem z obydwu stron. W przypadku, gdy robot jest w równowadze odległości są sobie równe, natomiast gdy robot się przechyla powstaje różnica między tymi odległościami. Znając różnice można wyznaczyć kąt wychylenia oraz jego kierunek. Drugim, częściej stosowanym sposobem jest zastosowanie specjalnych czujników jakimi są akcelerometr, mierzący przyśpieszenie liniowe w swoich osiach, oraz żyroskop, mierzący prędkość kątową w swoich osiach.

2.1. Akcelerometr

Akcelerometr mierzy przyśpieszenie liniowe w swoich osiach. Na ich podstawie, z założeniem, że jedyne przyśpieszenie jakie działa na układ jest to przyśpieszenie grawitacyjne, stosując funkcje trygonometryczne możemy wyznaczyć kąt wychylenia wahadła na podstawie dwóch wartości:

Rysunek 2.1. Pomiar kąta wychylenia na podstawie dwóch wartości przyśpieszeń z akcelerometru

2.2. Żyroskop

Żyroskop mierzy prędkość kątową w swoich osiach. Aby otrzymać kąt na podstawie prędkości kątowej wystarczy scałkować odczyt z żyroskopu.

2.3. Filtr komplementarny

Akcelerometr reaguje na zmianę przyśpieszenia przez co charakteryzuje się częstymi i krótkotrwałymi zakłóceniami. Pomiar kąta na podstawie odczytu żyroskopu wiąże się z błędem wynikającym z całkowania chwilowych błędów, przez co błąd całkowity wraz z upływem czasu zwiększa się.

Rysunek 2.2. Schemat ideowy filtru komplementarnego

W celu eliminacji błędów chwilowych kąta mierzonego na podstawie pomiarów z akcelerometru stosuje się filtr dolnoprzepustowy, lecz skutkiem takiego zabiegu są opóźnienia względem rzeczywistej zmiany orientacji. W przypadku pomiaru kąta poprzez całkowanie odczytu z żyroskopu stosuje się filtr górnoprzepustowy, lecz w wyniku tego otrzymamy pomiar niewrażliwy na powolne zmiany kąta wychylenia. Filtr komplementarny powstaje poprzez zsumowanie obydwu tych kątów, dzięki czemu otrzymujemy kąt niewrażliwy na zakłócenia chwilowe wynikające z pomiarów z akcelerometru oraz na sumaryczny błąd całkowania pomiaru z żyroskopu. Wzór filru komplementarnego:

Rysunek 2.3. Porównanie działanie filtru komplementarnego dla różnich współczynników a oraz dodatkowego uśrednienia pomiarów z akcelerometru

3. Układ regulacji

Odwrócone wahadło jest układem niestabilnym. Wymaga on specjalnego sterowania, aby

doprowadzić do jego stabilności. Stosowane są różne metody sterowania wahadłem odwróconym takie jak: regulator PID, regulator liniowo-kwadratowy ( LQR – ang. linear-quadratic regulator), model sztucznych sieci neuronowych oraz regulator z logiką rozmytą. W pracy został zastosowany regulator PID.

3.1. Regulator PID

Regulator proporcjonalno-całkująco-różniczkujący (PID) składa się z trzech członów, których celem jest uzyskanie wartości zadanej. Regulator realizuje algorytm:

Rysunek 3.1. Schemat regulatora PID

Dobór parametrów regulatora kąta wychylenia (stabilności) można zilustrować jako robota przytwierdzonego poprzez sprężynę do ściany (człon proporcjonalny) oraz tłumik (człon różniczkujący). Zaczynając od doboru wzmocnienia Kp przy zerowych pozostałych wzmocnieniach, zwiększa się go aż do wystąpienia oscylacji. Następnie zwiększa się wzmocnienie Kd aż oscylacje zostaną wygładzone.

Rysunek 3.2. Analogia doboru parametrów regulatora PD do wahadła przymocowanego przy pomocy sprężyny oraz tłumika

3.2. Kaskada regulatorów PID

Niestety jeden regulator nie sprawdza się do końca. Widać niedokładne zamontowanie czujników przez co robot mimo że utrzymuje równowagę to ciągle przemieszcza się w jednym kierunku. Aby temu zapobiec niezbędny jest drugi regulator ze sprzężeniem zwrotnym z prędkością. Wyjściem z regulatora prędkości jest zadany kąt który wchodzi do wcześniej wyznaczonego regulatora kąta wychylenia. Dzięki temu zabiegowi nie tylko robot stoi w miejscu ale również dodaje to możliwość sterowania robotem poprzez zadawanie prędkości. Parametry regulatora prędkości zostały dobrane metodą prób i błędów.

4. Konstrukcja

4.1. Część mechaniczna

Jeżeli chodzi o konstrukcje mechaniczną to wzorowałem się na robociku, który nosi nazwę B-robot. Całość zaprojektowana została w programie Autodesk Inventor, a drukował kolega z forum mosi2.

Rysunek 4.1. Konstrukcja mechaniczna robota balansującego

4.2. Część elektroniczna

Zastosowane elementy w większość są to wybory typu „byle tańszy”, więc są one średniej jakości. Jeśli chodzi o akcelerometr i żyroskop to użyłem popularny moduł MPU6050 (chiński tani moduł) przez co filtrowanie wymagało poza zastosowaniem filtru komplementarnego to jeszcze dodatkowego uśredniania odczytu a akcelerometru. Do komunikacji użyłem modułu bluetooth HC-05. Silniki krokowe 42HS48-1684 (1.68A na cewkę; 200 kroków na obrót; 0,44Nm; rozmiar NEMA 17). Jako sterowniki posłużyły mi moduły Pololu A4988, wcześniej również używałem tanich chińskich odpowiedników... ale mają one ograniczony prąd i nie dało się ustawić go powyżej 1.2A na cewkę przez co brakowało momentu obrotowego i trzeba było zainwestować w oryginalne moduły. Jako zasilania użyłem LiPol Redox 1300mAh 11.1V.

Zostały wykonane dwa obwody drukowane metodą termotransferu. Pierwsza jako mózg robota, gdzie znajdują się sterowniki, bluetooth, miejsce na montaż serwa modelarskiego. Natomiast druga służy jako pomiar napięcia na poszczególnych ogniwach oraz sygnalizowania stanu baterii poprzez krótką linijkę LED (dodatkowo gdy napięcie na którymś ogniwie spadnie poniżej 3.1V wysyłany jest sygnał do „mózgu”, żeby nie załączać silników.

Jako mikrokontrolery zostały zastosowane 16-bitowe dsPIC33FJ128GP802 ze względu na możliwość pracy na 80MHz (40MIPS) oraz ze względu, że firma Microchip wysyła darmowe próbki mikrokontrolerów. W przypadku użycia tego mikrokontrolera w układzie monitorującym stan baterii jest to troszeczkę użycie armaty przeciwko komarowi ale ze względu, że wstępnie miało wszystko znajdować się na jednym obwodzie drukowanym to postanowiłem zastosować taki sam (program był praktycznie gotowy).

4.3. Oprogramowanie

„Mózg” z częstotliwością 250Hz (co 4ms) odczytuje nowe wartości z akcelerometru i żyroskopu, przelicza je na kąt, przepuszcza pomiary przez regulatory oraz wystawia nowe prędkości silników. W między czasie sprawdza czy nie zostały odebrane nowe dane oraz czy stan baterii nie wymusza wyłączenia silników.

W LabView został napisany prosty program, w którym możliwe są zmiany wzmocnień regulatorów w trybie on-line, odczyt danych pomiarowych takich jak uchyb, kąt i prędkość silników oraz włączanie i wyłączanie silników robota.

Rysunek 4.2. Program napisany w LabView

Program na urządzenia mobilne z systemem Android został napisany w AppInventor2. Niestety posiada on dużo ograniczeń - głównym jest brak wsparcia dla multitouch'a co ogranicza możliwość jazdy prosto oraz skręcania w tym samym czasie.

Rysunek 4.3. Program napisany w AppInventorze do sterowania robotem

5. Podsumowanie

Jako podsumowanie odsyłam do krótkiego filmiku robota. W przyszłości chciałbym jeszcze dodać kolejny regulator w kaskadzie, który regulował by położenie robota oraz implementacja czegoś w rodzaju prostych G kodów, dzięki czemu robot mógłby posłużyć jako nauka programowania dla dzieci (nieco zabawniejszy „żółwik”). Kolejną rzeczą jaka chciałbym poprawić to wyznaczyć parametry regulatorów symulacyjnie w Matlab'ie ale póki co został wyznaczony dopiero model matematyczny. Dodatkowo wspomniałem już o ograniczeniach AppInventora, więc oczywiście przewidziany jest nowy program napisany na Androida, który nie będzie posiadał tych wad.

[klimek4765]

Mega kawal dobrej roboty^^!

Czy bylo to wykonywane w ramach pracy inz/mgr?

w jakim jezyku bylo pisane oprogramowanie mikrokontrolera?

Czy kolega bylby taki mily i udostepnil tu badz na privie biblioteke obslugujaca modul MPU 6050?

Sam mecze sie z tym modulem od jakiegos czasu ale porzadnych bibliotek w C w internecie jak na lekarstwo a przerabianie bibliotek z arduino po 3000 linijek kodu troche mnie jeszcze przerasta.

[pawel]

Ciekawy robot, estetycznie wykonany. Podoba mi się pomysł z zmianą parametrów do PID poprzez aplikacje na komputerze, na pewno bardzo to przyśpieszyło pracę. Kiedyś jak robiłem wahadło odwrócone to można było się zamęczyć przeprogramowując ciągle układ z nowymi parametrami.

[asdpii0]

Dzięki bardzo za miłe słowa 😃

Wstępnie na silnikach DC był zrobiony hobbistycznie, a teraz ten jako praca inżynierska jest robiony 🙂

Program w języku C (kompilator XC16 dla PICów 16bitowych)

W załączniku obsługa modułu I2C i biblioteki do MPU6050 (przerobione gotowe, które kiedyś znalazłem chyba do jakiegoś PIC18F ale źródła już nie pamietam niestety).

Jeśli chodzi o ten program na PC to jest on właśnie wynikiem poirytowania ciągłym przeprogramowywaniem xd i powiem szczerze, że warto było napisać lepszą warstwe aplikacyjna komunikacji, bo zaoszczedziło to bardo dużo czasu i teraz przy chęci dodania opcji jak sterowanie to było w zasadzie dopisanie kilku linijek do case'a 🙂

i2c_MPU6050.rar

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[adam19_91]

Bardzo fajny robocik gratuluję wykonania i powodzenia na obronie. Zastanawia mnie natomiast inna rzecz jak rozwiązałeś sprawę sterowania silnikami krokowymi? Chodzi mi konkretnie o przełożenie wartości jaka wychodzi z PIDa na pracę silników? Jak to przeliczasz bo model matematyczny (nie wiem czy taki wykonałeś) zakłada, że sterowaniem jest moment obrotowy w Nm lub jeśli rozpiszesz również dynamikę silników to sterowanie jest napięciem w V a z tego co się orientuję obroty silników krokowych a co za tym idzie moment i przykładane napięcie zależą od odstępu czasowego między krokami. Powtórzę się jak to przeliczasz? I jeszcze czy zastosowałeś sterowanie mikrokrokowe bo na małych obrotach silniki takie mają tendencję do drgań co w tego typu konstrukcji jest niedopuszczalne.

Pozdrawiam

[asdpii0]

dzięki 🙂

Model wyprowadzilem dla silników DC na poczatku ze względu, że pełno przykładów tego jest. Przy silnikach krokowych miałem tygodniowy przestuj, bo jak? Problemem jest skomplikowany model silników krokowych oraz jego nieliniowość (najgorzej jeśli chodzi o moment w funkcji wychylenia wału), więc przyjąłem MAŁE uproszczenie... założyłem, że silniki bez żadnych opóźnień przyjmują zadane położenie (kroki), więc steruje bezpośrednio wałem silnika. Model wychodzi dzięki temu prościutki... ale mam z nim problemy (albo z parametrami, bo ciężko wyznaczyć moment bezwładności albo środek cięzkości skoro silniki to 90% całej masy xd). Podsumowując to co wychodzi z mojego wewnetrzego regulatora jest to poprostu przyśpieszenie wału silnika (kół) w obr/s^2. Tutaj wtrące, że używam sterowania mikrokrokowego z podziałem 1/8 (zadowalająco wygładzone jest już mi się wydaje, a nie chciałbym zwiększać jeszcze bardziej prądu silników), więc mając obr/s prosty iloczyn:

wyjście z regulatora * liczba kroków silnika na obrót * podział kroku = liczba kroków /s

W moim przypadku kod wygląda tak:

acceleration = (float)Angle.WartoscWyjsciowa*1600;
velocity += acceleration*Tp;

(małe uproszczenie jest cholernie duże dlatego dużymi literami i pogrubione 😋)

Pozdrawiam

[bartek1333]

Fajna konstrukcja 🙂

Mam pytanie.

Czy w takich konstrukcjach jest tak, że łatwiej steruje się robotem jeśli ma wysoko położony środek ciężkości?

[cassius17]

Tak, łatwiej jest sterować takim robotem, kiedy środek ciężkości jest jak najdalej od osi obrotu kół.

[mosi2]

Miło widzieć to po zmontowaniu. Całkiem zgrabnie Ci wyszło 😃

[klimek4765]

asdpii0, Czy mozesz wrzucic obsluge i2c tego zyroskopu? mam caly czas problem z odczytami, ciagle odczytuje ze zyro daje 0 we wszystkich osiach, accel 26728 i ze niby w kazdym rejestrze jest wartosc 104 :| o co tu moze chodzic? pomozesz?