Robot mobily o zmieym sposobie lokomocji system sterowaia Tomasz Wiiarski, Dawid Seredyński Istytut Automatyki i Iformatyki Stosowaej, Politechika Warszawska Streszczeie: W artykule przedstawioo opis systemu sterowaia robota mobilego o dwóch współosiowych kołach, mogącego poruszać się w dwóch trybach lokomocji: dyamiczie stabilym oraz statyczie stabilym. Robot może zmieiać tryb ruchu przez automatyczy maewr wstawaia do piou. System sterowaia staowią kaskady ów PID. Słowa kluczowe: robot mobily, odwrócoe wahadło, kaskada PID W spólą cechą większości zrobotyzowaych platform mobilych jest apęd różicowy, którego podstawowymi zaletami są prosta realizacja i zapewieie wystarczającej maewrowości robota w dedykowaych zastosowaiach. Oparte a im roboty saperskie o apędzie gąsieicowym [1] wspierają człowieka w iebezpieczych zadaiach (p. w obszarze zagrożoym wybuchem ładuków albo działaiem terrorystów). Przemieszczają się w trudym tereie, mogą przepychać przeszkody i są stabilą bazą dla maipulatorów. Z drugiej stroy ich ruch pochłaia zacze ilości eergii, dodatkowo ze względu a poślizgi samolokalizacja robota jest utrudioa, gdyż przy takiej kofiguracji iformacja z ekoderów zamocowaych a silikach jest zdecydowaie iewystarczająca. Wychodząc a przeciw powyższym problemom stworzoo całą gamę robotów o dwukołowym apędzie różicowym, które przy odpowiediej kostrukcji mogą poruszać się w trybie odwrócoego wahadła. Tego typu roboty wprawdzie ie posiadają takich dobrych właściwości do przemieszczaia się w tereie w obecości przeszkód jak roboty saperskie, ale za to w praktyczych realizacjach z powodzeiem przewożą ludzi [2] zużywając przy tym relatywie mało eergii. Co więcej stojąc w pioie w swojej omialej pozycji zajmują mało miejsca a powierzchi co pozwala im a poruszaie się w ograiczoej przestrzei. Na bazie odwrócoego wahadła budowao też roboty wyposażoe w ręce zdole do werbalej i iewerbalej komuikacji z ludźmi [3]. Roboty mobile działające a zasadzie odwrócoego wahadła są także z powodzeiem wykorzystywae jako maipulatory humaoidale [4] oraz pojazdy trasportowe [5]. Sterowaie balasującym robotem mobilym wymaga, ze względu a iestabilą aturę, szybkiej odpowiedzi układu sterowaia a zmiay orietacji i stau robota. Koiecze jest zapewieie dokładego i pozbawioego opóźień pomiaru kąta odchyleia robota od piou [6]. Do pomiaru kąta pochyleia robota wykorzystuje się zwykle pomiary z kilku czujików, filtrowae za pomocą filtru Kalmaa [7] lub filtru komplemetarego [8]. Z drugiej stroy koiecza jest syteza odpowiediego układu regulacji. W liczych pracach zastosowao róże rozwiązaia, m. logikę rozmytą [9], y PID [10], sterowaie z wyprzedzeiem (feed-forward) [11]. Ostatie osiągięcia aukowców [12, 13] pozwalają stwierdzić, że budowa robotów o zmieej kofiguracji mechaiczej będąca jak dotąd domeą fatastyki aukowej staje się rzeczywistością. Celem iiejszej pracy jest przeprowadzeie studium wykoalości robota, a bazie doświadczeń z prac [14, 15], mogącego poruszać się w dwóch trybach jazdy: stabilym statyczie (poziomym) oraz stabilym dyamiczie (pioowym) z możliwością automatyczej zmiay trybu ruchu, łącząc zalety obydwu trybów ruchu. System sterowaia staowią algorytmy i y, dzięki którym robot może samodzielie utrzymywać rówowagę w zależości od zadaego sterowaia oraz od waruków zewętrzych, takich jak pochyleie i ierówości podłoża oraz kolizje z przeszkodami. Artykuł kocetruje się a systemie sterowaia, tym iiej dla kompletości, a wstępie zostaą pokrótce sformułowae wymagaia (sekcja 1) oraz kostrukcja mechaicza i elektroicza robota (sekcja 2). Postawioe wymagaia i kostrukcja robota były podstawą do sytezy a kaskadowego (sekcja 3) sterującego silikami. Struktura a implikowała sposób dekompozycji sterowika sprzętoworogramowego oraz jego implemecję a rzeczywistym sprzęcie (sekcja 4). Pracę kończy podsumowaie (sekcja 5). 1. Wymagaia Celem projektu było stworzeie taiego robota zdolego do poruszaia się w dwóch trybach ruchu: poziomym, czyli stabilym statyczie, w którym robot ma trzy pukty kotaktu z podłożem: dwa koła oraz zderzak. W tym trybie możliwe jest pchaie przeszkód za pomocą zderzaka jak rówież jazda w trudym tereie, pioowym, czyli stabilym dyamiczie, w którym robot utrzymuje rówowagę w pozycji zderzakiem do góry. Z założeia te tryb ma zapewiać zdolość do poruszaia się w wąskich przejazdach dzięki małej powierzchi przekroju poziomego, iskie zużycie eergii z powodu braku tarcia zderzaka o powierzchię ziemi, a także wysoką jakość samolokalizacji a bazie odczytów z odometrii. W obydwu trybach zakładamy zadawaie prędkości postępowej i obrotowej całego robota, a także możliwość automa- Pomiary Automatyka Robotyka r 5/2013 93
NAUKA tyczego wstawaia oraz zatrzymaia silików w dowolym trybie ruchu, co ilustruje diagram staów a rys. 1. utrzymywaie rówowagi zatrzymaie silików jazda z podparciem wstawaie do piou Rys. 1. Diagram staów, w których może zajdować się robot oraz przejść międzymi Fig. 1. Robot s state diagram 2. Kostrukcja bazy jezdej Kostrukcja mechaicza (rys. 2) [16] robota awiązuje do klasy rzucaych robotów zwiadowczych (p. [17]) i składa się z dwóch współosiowych kół zamotowaych a długiej osi, połączoej z ramą. Rozstaw kół jest zbliżoy do ich średicy i wyosi 50 cm. Przeiesieie apędu zostało podobie rozwiązae jak w rowerze, koła mogą obracać się swobodie a ieruchomej osi, a apęd jest przeoszoy za pomocą zębatek i łańcucha. Do tylej części ramy przyczepioy jest miękki, owaly zderzak o wysokości zbliżoej do średicy kół. Elemet te staowi trzeci pukt podparcia, dzięki któremu robot może poruszać się w trybie stabilym statyczie. Zderzak jest jedyym elemetem kostrukcji który wystaje poza obrys kół. Środek ciężkości robota zajduje się w iewielkiej odległości od osi kół, dzięki czemu robot może samodzielie wstawać do pozycji pioowej ( zderzakiem do góry ) i utrzymywać rówowagę poprzez odpowiedie sterowaie. Kostrukcja mechaicza jest symetrycza w dwóch płaszczyzach: prostopadłej do osi kół (symetria lewo prawo), zawierającej oś kół oraz geometryczy środek zderzaka (symetria góra dół). przekładiami plaetarymi i sprzęgłami. Sterowae są zgodie z metodologią opisaą w pracach [18, 19] poprzez mostek H z dolymi trazystorami N-MOSFET i górymi trazystorami P-MOSFET. Średi prąd płyący przez każdy z silików mierzoy jest pośredio, poprzez pomiar spadku apięcia a rezystorze o iewielkiej rezystacji. Mostek H staowi część sterowika silika, który dodatkowo jest odpowiedzialy za pomiar prędkości obrotowej silika a podstawie daych z ekoderów, oraz za komuikację z modułem główym robota. W skład systemu sterowaia wchodzą dwa sterowiki silika. Za określaie orietacji robota w przestrzei odpowiada jedostka iercyja, zawierająca 3-osiowy żyroskop oraz 3-osiowy akcelerometr. Jedostka iercyja, podobie jak sterowik silika, komuikuje się z modułem główym robota. Moduł główy to iewielki etbook o stosukowo dużej mocy obliczeiowej. Komuikuje się ze wszystkimi podzespołami robota za pomocą magistrali RS-485 oraz RS-232. 3. Struktura a Do obsługi trybów ruchu z rys. 1 zdecydowao się wykorzystać kaskadowy. Struktura a sterującego ruchem robota jest zależa od aktualego stau, w którym zajduje się robot. Jako pętlę ajbliższą sprzętowi i zarazem ajszybszą przyjęto prądowy, który w aturaly sposób zapewia ograiczeie prądu płyącego przez silik, jak rówież pozwala a pośredie zadawaie mometu a siliki DC, a więc w pewym przybliżeiu regulację przyspieszeia robota. Jest to całkujący, którego wejście staowi uchyb prądu zmierzoego względem zadaego, a wyjściem jest wypełieie sygału. Regulator te jest wspólą częścią wszystkich ów kaskadowych opisaych dalej. Z drugiej stroy w ajbardziej zewętrzej pętli zadawaa jest prędkość postępowa i obrotowa robota. 3.1. Tryb poziomy W poziomym trybie ruchu prędkość postępowa i obrotowa robota jest przeliczaa, za pomocą prostych przekształceń, a prędkości obrotowe silików, które z kolei są wejściem dwóch ów kaskadowych, prędkościowo prądowych (po jedej kaskadzie a każdy silik rys. 3). Regulator kaskadowy w trybie poziomym składa się z a całkującego prądowego w wewętrzej pętli oraz z a proporcjoalego prędkości obrotowej w zewętrzej pętli. zmierzoa silika zmierzoa silika silika silika + - postêpowa obrotowa Rys. 2. Baza jezda robota [16] Fig. 2. Robot s mobile base [16] Napęd robota staowią dwa siliki szczotkowe prądu stałego z wkrętarek akumulatorowych, wraz z orygialymi Rys. 3. Kaskada ów dla trybu poziomego Fig. 3. Cascade of s used i horizotal mode Regulator prędkości pozwala a precyzyją kotrolę trajektorii robota oraz elimiuje wpływ iewielkiej, trudej 94
do uikięcia asymetrii robota, związaej z różicami we właściwościach fizyczych silików oraz z różymi siłami oporu przekładi. 3.2. Tryb pioowy W trybie pioowym, kaskadowy ma bardziej złożoą strukturę (rys. 4). Wewętrze pętle dla każdego z silików staowią y prądu. Nad imi działają iezależie od siebie dwa y: prędkości obrotowej oraz kaskadowy prędkości postępowej i kąta pochyleia. + - zmierzoy a postêpowa zmierzoa postêpowej obrotowej obrotowa zmierzoa postêpowa obrotowa Rys. 4. Kaskada ów dla trybu pioowego Fig. 4. Cascade of s used i dyamically stable mode Kaskada ów kąta pochyleia i prędkości postępowej odpowiada za utrzymywaie rówowagi oraz za utrzymywaie i zmiaę pozycji robota w przestrzei. Regulator prędkości postępowej to proporcjoalo całkującoóżiczkującpid). Jego wejściem jest uchyb prędkości postępowej zmierzoej wzgędem zadaej. Prędkość postępowa robota została określoa jako średia prędkość obrotowa obu kół. Wyjściem tego a jest kąt dla a kąta pochyleia. Kąt te jest, dla kątów bliskich kątowi rówowagi, wprost proporcjoaly do przyspieszeia robota. Ozacza to, że prędkości postępowej zadając kąt pochyleia dla a kąta, zadaje przyszpieszeie calego robota. Czło różiczkujący pozwala a szybką odpowiedź a zmiaę prędkości zadaej, a dzięki człoowi całkującemu, robot jest w staie wjechać a rówię pochyłą, achylając się w stroę rówi. Regulator kąta to proporcjoaloałkujący. Jego zadaiem jest utrzymywaie zadaego kąta odchyleia robota od piou, wyzaczoego przez prędkości postępowej. Czło całkujący iweluje uchyb statyczy, który był obserwoway podczas doświadczeń i który był związay z działaiem siły grawitacji a kostrukcję robota. Wyjściem a jest średi prąd dla obu silików. Niezależie od a kaskadowego prędkości postępowej i kąta pochyleia, działa proporcjoalo całkujący prędkości obrotowej robota. Dzięki iemu robot może skręcać oraz utrzymywać kieruek ruchu. Wejściem tego a jest uchyb prędkości obrotowej robota, a wyjściem jest różicowa wartość prądu dodaa do prądu zadaego dla jedego silika i odjęta od zadaego prądu dla drugiego silika. 3.3. Wstawaie do piou Podczas wstawaia do piou robot wykouje określoą sekwecję ruchów. W pierwszej fazie maewru robot rozpędza się. Siliki są sterowae prądowo, prądem o maksymalym dozwoloym atężeiu. Dla tej fazy maewru przedstawioo a rys. 5. I max Rys. 5. Regulator dla pierwszej fazy maewru wstawaia do piou Fig. 5. Regulator for the first phase of gettig-up maeuver Po upływie określoego czasu, rozpoczya się druga faza maewru. Polaryzacja prądu zmieia się a przeciwą i zostaje włączoy prosty proporcjoaly, który zadaje prąd proporcjoaly do kąta odchyleia od piou (rys. 6). Druga faza trwa dopóki kąt odchyleia od piou ie zajdzie się w zdefiiowaym przedziale robot przechodzi wtedy do stau balasowaia (trybu pioowego). zmierzoy a Rys. 6. Regulator dla drugiej fazy maewru wstawaia do piou Fig. 6. Regulator for the secod phase of gettig-up maeuver 4. Dekompozycja i implemetacja ika Sterowik programowy robota tworzy system rozproszoy (rys. 7), który podzieloy został a oprogramowaie iskopoziomowe (sekcja 4.1), działające a sterowikach silików i a jedostce iercyjej, oraz oprogramowaie wysokopoziomowe (sekcja 4.2) działające a etbooku. silik B ekoder B sterowik silika B rs-485 Rys. 7. Moduły systemu Fig. 7. Modules of the system silik A ekoder A sterowik silika A etbook 0 jedostka iercyja rs-232 Na sterowikach silików zaimplemetowao pracujące z dużą częstotliwością y prądu i prędkości obrotowej silików. Na etbooku działa maszya staów, która odpowiada za sterowaie całym robotem, oraz y wyższego poziomu: kąta pochyleia, prędkości postępowej i prędkości obrotowej robota. 4.1. Oprogramowaie iskopoziomowe W sterowiku iskopoziomowych zaimplemetowao zadaia wymagające częstego odwoływaia się do sprzętu, z drugiej stroy ie wymagające skomplikowaych obliczeń: komuikację z komputerem PC, Pomiary Automatyka Robotyka r 5/2013 95
NAUKA pomiar czasu, sterowaie mostkiem H zasilającym silik, pomiar prądu, regulację i ograiczeie prądu, pomiar prędkości obrotowej silika oraz jej regulacja, watchdog. Sterowiki silika zostały zaimplemetowae a mikrokotrolerze AVR ATmega8, który został wybray ze względu a iską ceę, prostą obsługą i wystarczająco dobre parametry. Moc obliczeiowa procesora zdetermiowała apisaie aplikacji odwołującej się bezpośredio do sprzętu bez wykorzystaia systemu operacyjego. Aplikacja została zaprojektowaa w taki sposób, aby zadaia, wyko a jedordzeiowym procesorze, ie blokowały się awzajem. Ze względu a iewielką liczbę zadań oraz iedużą ich złożoość, możliwe było zastosowaie podejścia zapewiającego względie częste wykoywaie każdego z zadań. Główa pętla wykoywaa jest z dużą częstotliwością, przy- z ając każdemu zadaiu czas w każdej i. Oprogramowaie (rys. 8) zostało apisae zgodie z astępującymi założeiami: żade zadaie ie stosuje aktywego oczekiwaia i jeśli ie ma w daej chwili iczego do zrobieia, zostaje wstrzymae, zadaia wykoują jedorazowo iewielką liczbę czyości. W praktyce ozacza to brak pętli o dużej liczbie iteracji, sta zadaia może zostać zapamiętay a czas wykoywaia pozostałych zadań. Program g³ówy Iicjalizacja Zmierzeie czasu jaki up³y¹³ od poprzediej iteracji Zadaie 1 Zadaie 2... Komuikacja (buforowae We/Wy) Rys. 8. Struktura sterowika silika Fig. 8. Diagram of motor cotroller Przerwaia Odebrao bajt dodaj do bufora Dzięki takiej strukturze aplikacji każde zadaie może wykoywa z dużą częstotliwością i możliwe jest: szybkie pobieraie daych z bufora, aby ie przepełił się, regulare i precyzyje uruchamiaie pewych procedur, które powiy wykoywać się co stały iterwał czasu, p. a prędkości, zapamiętaie wyiku kowersji aalogowo cyfrowej tuż po jej zakończeiu i atychmiastowe poowe uruchomieie kolejej kowersji, ograiczeie liczby przerwań do iezbędego miimum. Dodatkowo, zadaia mogą działać od siebie iezależie, co zaczie uprościło aplikację, zmiejszając ryzyko wystąpieia błędów. Nie jest potrzeba sychroizacja między zadaiami, gdyż ie wykoują się oe rówolegle. Przerwaia w większości zastąpioo sprawdzaiem, w każdej iteracji pętli główej, czy już wystąpiło dae zdarzeie (p. upłyięcie określoego czasu, lub zakończeie kowersji aalogowoyfrowej). Poiżej zostaą opisae szczegółowo ajistotiejsze zadaia sterowika iskopoziomowego. 4.1.1. Regulacja i ograiczeie prądu Regulacja prądu oparta jest a dyskretym ze PID, którego wejściem jest uchyb prądu zmierzoego względem zadaego, a wyjściem jest wartość wypełieia sygału. Koleje iteracje a uruchamiae są, z częstotliwością 7 2 khz, w procedurze obsługi przerwaia zegara geerującego sygał. Dzięki temu, w każdej iteracji, wartość a wyjściu a zostaje użyta do sterowaia silikiem. Zastosowao także ograiczeie całkowaia, gdy wartość a wyjściu jest zbyt duża. Dodatkowo, moża w każdej chwili zmieić wartość jego astawów, wysyłając odpowiedie dae z komputera PC do sterowika silików. Regulator prądowy oprócz swojej główej fukcji, jaką jest sterowaie prądem płyącym przez silik, zabezpiecza przed spaleiem mostka. W przypadku, gdy astąpi zwarcie przez p. iepoprawe podłączeie silika, ograiczy prąd zwarciowy i zaczie zmiejszy wzrost temperatury trazystorów. 4.1.2. Pomiar prędkości obrotowej silika gulacja Dla trybu jazdy w poziomie y prędkości kół zostały umieszczoe w sterowiku iskopoziomowym, w przeciwieństwie do trybu jazdy w pioie, w którym y prędkości umieszczoe zostały w sterowiku wysokopoziomowym. Różica ta wyika ze struktury ów kaskadowych w obu trybach jazdy. W trybie pioowym prędkości działa ad em kąta, a etbooku. Dodatkowo, prędkości postępowej steruje średią prędkością kół robota, zo szczoy w jedostce cetralej, sterującej całym W trybie poziomym y prędkości sterują ciami obrotowymi kół (poprzez regulację prędko ków) i mogą działać iezależie od siebie. Dlatego e w sterowikach iskopoziomowych, co datkowo pozwala a uikięcie iewielkiego opóźieia w sterowaiu wyikającego z trasmisji daych: prędkości zmierzoej i prądu zadaego. Regulator PID prędkości obrotowej silika został zaimplemetoway podobie jak prądu. Posiada ograiczeie całkowaia oraz możliwość zmiay astaw przez komputer PC. Dodatkowo może zostać w każdej chwili wyłączoy, a sterowik silika przechodzi wtedy w tryb sterowaia prądem. Regulator prędkości działa z częstotliwością około 55 Hz. Tak iska częstotliwość jest skutkiem kompromisu, jaki ależało przyjąć wybierając częstotliwość regulacji i dokładość pomiaru. Ekodery geerują 1056 impulsów a peły obrót koła, co przy maksymalej prędkości robota daje maksymalie ok. 2000 impulsów a sekudę. Przy 50 pomiarach prędkości a sekudę i maksymalej prędkości 96
robota, zmierzoych zostaie maksymalie około 40 impulsów. Skutkiem tego jest dość dobra regulacja prędkości robota w zakresie dużych prędkości, zaś ieregulara praca i szarpaie przy prędkości bliskiej zeru. Momet, w którym zostaje uruchomioa koleja iteracja a prędkości jest określay dzięki zadaiu pomiaru czasu. W kolejych iteracjach zliczaa jest liczba impulsów ekodera (z uwzględieiem kieruku obrotu) i a tej podstawie obliczaa jest prędkość obrotowa silika. Wejściem a prędkości jest uchyb prędkości zmierzoej względem zadaej, a wyjściem jest wartość prądu, która podaa zostaje a wejście a prądu. W trybie jazdy z podparciem (bez utrzymywaia rówowagi) oba y są włączoe tworząc kaskadę. 4.1.3. Watchdog Współczese roboty powiy być kostruowae zgodie z wymogami bezpieczeństwa [20]. Na poziomie mikrokotrolerów mechaizm watchdog pozwala a uikięcie sytuacji, w której mikrokotroler wykouje bez końca program w pętli (w sposób ieprzewidziay), tj. zawiesi się. Watchdog zastosoway w programie obsługującym mostek H powoduje reset mikrokotrolera po 0 26 s od czasu ostatiego wyzerowaia wewętrzego timera. Zerowaie timera watchdoga astępuje tylko po otrzymaiu poprawej ramki daych adresowaej do daego urządzeia. Ozacza to, że reset wywołay przez watchdog astąpi po zawieszeiu się programu, jak rówież przy przerwie w trasmisji daych dłuższej iż 0 26 s. Zabezpieczeie to zostało przetestowae, kiedy w programie pojawił się błąd, przez który co pewie czas jedo z urządzeń ie przełączało swojego końca magistrali RS-485 w tryb odczytu. Skutkiem tego było zatrzymaie trasmisji daych i reset urządzeń wywołay przez watchdoga. Pomimo tego, że zablokowaie magistrali astępowało średio co kilka sekud, możliwe było działaie robota bez większych utrudień. 4.2. Oprogramowaie wysokopoziomowe Jedostka cetrala to urządzeie, a którym dokouje się większość obliczeń, gdyż ma ajwiększe zasoby ze wszystkich urządzeń w robocie. Jest to etbook z procesorem Itel Atom, 1 GB pamięci RAM oraz dyskiem twardym SSD o pojemości 16 GB. Działa a im system operacyjy Ubutu oraz system ROS, stosowae dotychczas z powodzeiem m.i. w systemach awigacji robotów mobilych [21]. Gdy robot jest uruchomioy, a jedostce cetralej działa ROS i jego dwa moduły. Jede moduł obsługuje zadajik i przesyła dae dotyczące ruchu zadaego przez operatora do drugiego modułu, który steruje całym robotem, realizując dwie ajważiejsze fukcje: zbieraie i filtrację daych z jedostki iercyjej, zależe od trybu ruchu sterowaie robota a podstawie poleceń od operatora 4.2.1. Zbieraie i filtracja daych z jedostki iercyjej Moduł główy zawiera y z zamkiętą pętlą sprzężeia zwrotego i dlatego wymaga ciągłego dostarczaia aktualych daych o staie fizyczym całego systemu. Wielkości fizycze są mierzoe i zamieiae a postać cyfrową przez urządzeia wchodzące w skład systemu rozproszoego. Następie są przesyłae do modułu główego za pomocą iterfejsów komuikacyjych i tam są ostateczie przetwarzae. Sygały aalogowe zamieioe a postać cyfrową wymagają dodatkowego przetwarzaia przed zastosowaiem ich w systemach regulacji, gdyż zawierają oe szumy i błędy pomiarowe, które trzeba wyelimiować lub zmiejszyć. Dodatkowo, w przypadku pomiaru kąta koiecze jest zastosowaie filtru komplemetarego, łączącego pomiary z co ajmiej dwóch czujików. Na rys. 9 przedstawioo schemat algorytmu filtru komplemetarego, który został zaimplemetoway w systemie. Filtr te zawiera dwa wejścia: ω i θ oraz dwa wyjścia ω i θ. Zmiee ω i θ to prędkość obrotowa i kąt pochyleia. W wyiku działaia filtru otrzymujemy dwa sygały wyjściowe o podobej iterpretacji co sygały wejściowe, lecz o zaczie większej dokładości. akcelerometr yroskop owa przyspieszeie w osi X przyspieszeie w osi Y pochyleia filtr komplemetary owa ) (1 x) 4 1 2 3 ( 1 1 x pochyleia arctg Rys. 9. Filtr komplemetary, x to liczba z przedziału (0; 1) Fig. 9. Complemetary filter, x is a real umber i (0; 1) Filtr te jest dyskrety w dziedziie czasu, tz. wykoyway jest w iteracjach w stałych przedziałach czasu. Jest poday wzorem rekurecyjym a rys. 9. Wewętrzy sta filtru jest przechowyway, między kolejymi iteracjami, za pomocą zmieych θ 1, θ 1, θ 2 oraz θ 3. Liczba x to stała z przedziału (0; 1), która określa wagę wejść. Wartość stałej x bliska 0 ozacza, że wartości a wyjściach będą wyliczae główie a podstawie całki prędkości kątowej. Wartość x bliska 1 ozacza, że wartości a wyjściu będą bardziej wrażliwe a pomiar kąta z akcelerometru. Im miejszy dryf żyroskopu, tym miejsza może być wartość stałej x i tym większa dokładość pomiaru kąta. W przedstawioej tutaj implemetacji dobrao wartość x = 0.03, przy której dryf żyroskopu ie wprowadza błędu. Otrzymaa a wyjściu wartość kąta pochyleia jest zaczie dokładiejsza i bliższa rzeczywistej wartości kąta. Wyiki badaia działaia filtru komplemetarego zostaą przedstawioe w kolejej części artykułu. Uzyskaa wartość kąta pochyleia robota jest wykorzystywaa w pętli sprzężeia zwrotego a kąta. Pomiary Automatyka Robotyka r 5/2013 97
NAUKA 4.2.2. Poziomy tryb ruchu W trybie jazdy w poziomie, moduł główy przelicza dae z zadajika a prędkości obrotowe kół, które astępie przesyła do kaskad ów w sterowikach silików. W trybie tym, w module główym ie działa żade. 4.2.3. Pioowy tryb ruchu W trybie jazdy w pioie moduł główy zawiera część kaskady ów: kąta odchyleia od piou oraz y prędkości postępowej i obrotowej robota. Regulator kąta jest dyskretym em PI, a prędkości postępowej jest dyskretym em PID. Oba y tworzą kaskadę i działają z częstotliwością 100 Hz. Zaimplemetowae ograiczeie całkowaia oraz ograiczeie wartości a wyjściu zabezpieczają system przed zbyt gwałtową i zbyt silą odpowiedzią a sytuacje wyjątkowe, jak p. agła utrata stabilości. Regulatory te są uruchomioe tylko w trybie utrzymywaia rówowagi w pozycji pioowej i są resetowae przy każdym przejściu do tego trybu. Reset polega a wyzerowaiu całki, aby w początkowych iteracjach regulacji ie była braa pod uwagę jej wartość z poprzediej fazy jazdy w trybie utrzymywaia rówowagi. Oprócz sterowaia prędkością postępową robota koiecze jest także sterowaie prędkością obrotową robota w trybie balasowaia. W tego typu kostrukcjach stosuje się apęd różicowy, w którym sterowaie prędkością obrotową robota polega a zadawaiu różych prędkości dla każdego z kół. W przypadku opisaego w iiejszym artykule robota, będącego w trybie jazdy w pozycji pioowej, ie moża bezpośredio i precyzyjie kotrolować różicy w prędkości obrotowej kół, gdyż prędkości postępowej jest wspóly dla obu kół. Z kolei kąta zadaje prąd płyący przez oba siliki, co przy awet iewielkiej asymetrii w elemetach mechaiczych lub elektroiczych skutkuje w iezaczych różicach w prędkości obrotowej silików i iekotrolowaym skręcaiu robota. Aby poprawić kotrolę ad trajektorią robota, dodao jeszcze jede PI, który steruje jego prędkością obrotową podczas ruchu w pozycji pioowej. Jego wyjście staowi różicowa wartość prądu, która jest dodawaa do wartości prądu wyzaczoej przez kąta dla jedego silika i odejmowaa od tejże wartości dla drugiego silika. 4.2.4. Maewr wstawaia do piou Poszczególy fazy maewru są zlecae ze sterowika wysokopoziomowego. Pierwsza, w której robot się rozpędza w pozycji poziomej trwa 0 7 s. Siliki są sterowae prądowo, prądem o maksymalym dozwoloym atężeiu. Następie, po czasie 0 7 s, rozpoczya się druga faza, gdzie polaryzacja prądu zmieia się a przeciwą i zostaje włączoy prosty proporcjoaly, który zadaje prąd proporcjoalie do kąta odchyleia od piou. Rówież w tej fazie zadae atężeie prądu podlega ograiczeiu. Druga faza trwa dopóki kąt odchyleia od piou ie zajdzie się w przedziale ustaloym eksperymetalie ( 10 +10. Jeżeli po czasie 2 0 s od rozpoczęcia maewru robot ie osiągie trybu pioowego ( ie osiągie założoego kąta), zostaje włączoy sta awaryjego zatrzymaia silików. Czasy trwaia obu faz maewru zostały dobrae doświadczalie. Zachowaie robota podczas maewru wstawaia do piou zostaie szczegółowo opisae w kolejej części artykułu. 5. Podsumowaie Zaprojektoway i wykoay w ramach projektu robot może poruszać się w dwóch trybach jazdy, automatyczie zmieiać tryb ruchu, wjeżdżać a rówię pochyłą, pchać obiekty, utrzymywać rówowagę po zderzeiu z przeszkodą oraz zachowuje się w sposób przewidywaly i bezpieczy. Należy zwrócić szczególą uwagę a iezawodość robota, który podczas testów ie ulegał awariom oraz ie staowił zagrożeia dla otoczeia. Jego system sterowaia został grutowie przetestoway wróżych sceariuszach. czasie testów zbierao i aalizowao dae szczegóło opisujące sta robota. Przeprowadzoe doświadczeia, z przebiegami czasowymi zmieych stau robota, zo przedstawioe w kolej artyku. Z kolei badaia filtracji daych z jedostki iercyjej będą podstawą do rozwiięcia metod estymacji siły bezwładości [22]. Podziękowaia Tomasz Wiiarski dziękuje za wsparcie otrzymae w ramach stypedium współfiasowaego przez Uię Europejską w ramach Europejskiego Fuduszu Społeczego, które przyzawae jest przez Cetrum Studiów Zaawasowaych Politechiki Warszawskiej w ramach projektu Program Rozwojowy Politechiki Warszawskiej. Projekt był fiasoway ze środków Narodowego Cetrum Nauki przyzaych a podstawie decyzji umer DEC-2012/05/D/ST6/03097. Bibliografia 1. Klimasara W., Kocepcja, projekt oraz kostrukcja mechaicza mobilego robota iterwecyjoispekcyjego sr-10 ispector, Krajowa Kofrecja Robotyki, VII KKR, Prace Naukowe ICT PWr. Proceedigs Red. K. Tcho, Ladek Zdrój 2001, 139 148. 2. Nguye H. G., Morrell J., Mulles K., Burmeister A., Miles S., Farrigto N., Thomas K., Cage D. W. (2004): Segway robotic mobility platform. Defese Techical Iformatio Ceter. 3. Kędzierski J., Małęk L., Oleksy A., Budowa robota społeczego FLASH, [i:] XII Krajowa Koferecja Robotyki Postępy Robotyki, Oficya Wydawicza Politechiki Warszawskiej, 2012, 681 694. 4. Stilma M., Olso J., Gloss W. (May, 2010): Golem Krag: Dyamically Stable Humaoid Robot for Mobi William Gloss, [i:] IEEE Iteratioal Coferece o Robotics ad Automatio ICRA 10, IEEE. 5. Cazzolato B., Harvey J., Dyer C., Fulto K., Schuma E., Zhu T., Prime Z., Cotrol of a electric diwheel, 2009. 6. McComb G. (2001): The robot builder s boaza. McGraw-Hill/TAB Electroics. 7. Ooi R. C., Balacig a two-wheeled autoomous robot, Uiversity of Wester Australia 2003. 8. Kim Y., Kim S. H., Kwak Y. K., Dyamic aalysis of a oholoomic two-wheeled iverted pedulum robot, Joural of Itelliget & Robotic Systems 1/2005, 25 46. 98
Zobacz więcej Pobierz bezpłatą aplikację PAR+ App Store Google Play Pomiary Automatyka Robotyka r 5/2013 99