Pomiar odchylenia od pionu korpusu robota balansujacego. Marek Wnuk

Transkrypt

1 INSTYTUT INFORMATYKI AUTOMATYKI I ROBOTYKI POLITECHNIKI WROCŁAWSKIEJ Raport serii PRE nr 28/28 Na prawach r ekopisu Pomiar odchylenia od pionu korpusu robota balansujacego Marek Wnuk Słowa kluczowe: robot balansujacy, sterownik, żyroskop, akcelerometr, czujnik odległości, mikrokontroler. Wrocław 28

2 Spis treści 1 WPROWADZENIE 2 2 KONCEPCJA POMIARU NACHYLENIA ROBOTA Rozważania geometryczne Warunki doboru dalmierzy Charakterystyki i kalibracja dalmierzy Charakterystyki kat napi ecie Ograniczenia czasowe REALIZACJA PRAKTYCZNA Stanowisko badawcze z mikrokontrolerem MC Rozbudowa robota RoBik Zastosowanie w robocie Kosmos PODSUMOWANIE 14

3 1 WPROWADZENIE W dotychczasowych pracach zwiazanych z dwukołowymi robotami balansujacymi (posiadajacymi korpus w postaci wahadła, którego wychylenie zapewnia nap ed) wyst epował problem pomiaru chwilowej wartości kata wychylenia korpusu od pionu. Stosowane rozwiazania opierały si e na czujnikach inercyjnych (akcelerometrach i żyroskopach) i wymagały złożonej obróbki sygnałów. Korzystajac z dwuosiowego akcelerometru do pomiaru składowych wektora grawitacji nie udało si e zapewnić jednoznacznego pomiaru kata odchylenia korpusu [1]. Dodatkowa informacja z żyroskopu, pozwalajaca określić znak przyspieszenia w ruchu post epowym robota, pozwalała uzyskać informacj e o kacie [2]. Metoda oparta na całkowaniu pr edkości katowej mierzonej żyroskopem działa dobrze tylko na krótkom odcinku czasu, ze wzgl edu na niestabilność punktu zerowego żyroskopu i wprowadzany w ten sposób dryf do układu pomiaru kata [5]. Pewna popraw e dało zastosowanie filtru Kalmana do fuzji sygnałów z sensorów inercyjnych (trzyosiowy akcelerometr i jednoosiowy żyroskop) [3] wprowadzone w robocie balansujacym Kosmos. Roboty Balansujace RoBik i Kosmos zostały pokazane na rysunku 1. Rysunek 1: Roboty balansujace RoBik [5] i Kosmos [3] Zastosowanie dobrze skompensowanych termicznie i długoterminowo czujników inercyjnych powinno wprowadzić dalsza popraw e jakości mierzonych parametrów ruchu korpusu. Przykładem takiego czujnika, zawierajacego w obudowie o rozmiarach ok. 2mm 2mm 2mm trzyosiowy akcelerometr i trzy żyroskopy wraz z układami filtracji i korekcji pomiarów, jest ADIS16355 firmy Analog Devices [16]. Mimo ciagłej poprawy jakości czujników inercyjnych istnieje nadal potrzeba posiadania bezpośredniego pomiaru wychylenia korpusu robota balansujacego 1. 1 Przez analogi e do kompasu magnetycznego zapewniajacego bezpośredni pomiar orientacji. 2

4 2 KONCEPCJA POMIARU NACHYLENIA ROBOTA Przy założeniu, że robot balansujacy porusza si e po poziomej płaszczyźnie (które jest zazwyczaj spełnione), możemy odnieść pomiar do poziomu, zamiast do pionu. Proponowany w niniejszej pracy system do pomiaru pochylenia robota opiera si e na pomiarze odległości od podłoża przy pomocy prostych dalmierzy (np. optycznych). 2.1 Rozważania geometryczne Na rysunku 2 pokazano ide e układu pomiaru kata α pomi edzy korpusem robota a normalna do podłoża. Dwa czujniki odległości (dalmierze) sa umieszczone symetrycznie na korpusie robota (po przeciwnych stronach osi jego kół)i skierowane równolegle do osi korpusu, w kierunku podłoża. Odległość od podłoża w położeniu pionowym jest dobrana tak, by możliwy był pomiar odległości orzy odchylaniu korpusu robota od pionu w żadanym zakresie katów (w pracy przyj eto maksymalne odchylenie równe 45 o ). Rysunek 2: Dwa przypadki umieszczenia czujników wzgl edem osi Można wyróżnić dwa przypadki, charakterystyczne dla dwóch przykładowych robotów (RoBik [5], [4] i Kosmos [3]) przedstawione na rysunku 2. RoBik ma koła o dużym promieniu, wi ec czujniki sa umieszczone poniżej osi (odległość czujników od podłoża przy pionowym położeniu korpusu jest mniejsza od promienia kół). Mały promień kół robota Kosmos skłania do umieszczenia czujników powyżej osi kól (w położeniu pionowym ich odległość od podłoża jest wi eksza od promienia kół). Przez R oznaczono promień kół, przez l wysokość umieszczenia czujników ponad osia 2, przez b połow e rozstawu czujników w stosunku do płaszczyzny symetrii korpusu, przez d 1 i d 2 pomiary odległości z obu dalmierzy. 2 Gdy czujniki sa umieszczone poniżej osi, l < d 1 d 2 = R cosα + l btanα = R cosα + l + btanα (1) 3

5 Różnica obu pomiarów odległości pozwala wyznaczyć kat nachylenia: d = d 2 d 1 = 2btanα (2) α = arctan d (3) 2b Warto zwrócić uwag e, że w arbitralnie przyj etym zakresie odchyleń ( α α max = 45 o ) 3 zależność różnicy mierzonych odległości d od kata α jest dobrze uwarunkowana. Charakterystyki d i i d przedstawiono na rysunku d1(alpha) d2(alpha) d2(alpha)-d1(alpha) 2 18 d1(alpha) d2(alpha) d2(alpha)-d1(alpha) Rysunek 3: Zależność mierzonych odległości od kata pochylenia dla RoBik i Kosmos Ważnym parametrem geometrii układu pomiarowego jest odległość bazowa b pomi edzy czujnikiem a płaszczyzna symetrii korpusu (odległość pomi edzy dalmierzami wynosi 2b). Od wielkości bazy zależy rozdzielczość układu (różnica mierzonych odległości d przy danym kacie α jest proporcjonalna do b). 2.2 Warunki doboru dalmierzy Odległość mierzona przez wyżej położony z dalmierzy (d 2 ) rośnie monotonicznie. W zależności od wielkości bazy b w stosunku do promienia kół R odległość mierzona przez niżej położony dalmierz (d 1 ) maleje monotonicznie (gdy R b) lub osiaga w roboczym zakresie kata α minimum: d dα d 1 = Rsinα b cos 2 α α min = arcsin b R (4) (5) Dalmierze maja zakresy pomiarowe ograniczone zarówno od dołu (d min ), jak i od góry (d max ). Przy maksymalnym kacie wychylenia korpusu musi być spełniony warunek, by odległość d 2 mieściła si e w zakresie pracy dalmierza: 3 Ze wzgl edu na symetri e układu rozważono jeden kierunek wychylenia korpusu. d max d 2 (α max ) (6) 4

6 Drugi warunek opiera si e na d 1 i dotyczy dolnego ograniczenia zakresu pomiarowego dalmierza: d 1 (α max ) dla R b d min (7) d 1 (α min ) dla R > b Ponieważ promień kół R i odległość l nie wpływaja na różnic e odległości d (2), odpowiednio dobierajac l przy możliwie dużym b możemy dostosować geometri e układu do posiadanych dalmierzy, by jak najlepiej wykorzystać ich możliwości. Obszar dopuszczalnych wielkości l i b przy założonym dopuszczalnym kacie odchylenia α max jest określony przez układ nierówności: l d max cosα R max btanα max d min cosα R max + btanα max dla R b (8) l d min R 2 b 2 dla R > b Przyjmijmy przykładowo dalmierz o zakresie pomiarowym d min = 5mm, d max = 2mm i maksymalny kat odchylenia α max = 45 o. Dopuszczalne rozwiazania dla robotów RoBik (R = 25) i Kosmos (R = 6) pokazano na rysunku l(b) k1(b) l1(b) 12 1 l(b) k1(b) l1(b) Rysunek 4: Dobór parametrów umieszczenia czujników w robotach RoBik i Kosmos 2.3 Charakterystyki i kalibracja dalmierzy Do przeprowadzenia prób wykorzystano dost epne i tanie dalmierze firmy SHARP. Dalmierz GP2D12 ma silnie nieliniowa zależność napi ecia wyjściowego od mierzonej odległości, podawana przez producenta w formie wykresu. W tabeli 1 przedstawiono punkty odczytane z wykresu fabrycznego [1]. W literaturze i publikacjach internetowych cz esto spotyka si e próby aproksymowania jej wielomianami stosunkowo wysokich stopni, z miernymi rezultatami. Zaobserwowano, że charakterystyka w zakresie użytecznych odległości przypomina hiperbol e. Okazało si e, że bardzo dobre wyniki daje zastosowanie modelu w postaci funkcji wymiernej z trzema parametrami (9). Jej dodatkowa zaleta jest łatwość uzyskania charakterystyki odwrotnej w tej samej postaci (11). 5

7 odległość [mm] napi ecie [mv] Tablica 1: Charakterystyka fabryczna dalmierza GP2D12 v(d) = a + a 1 (9) d + a 2 Minimalizujac bład średniokwadratowy znaleziono nast epuj ace parametry: a = a 1 = a 2 = (1) Całkowity bład dla 12 próbek wyniósł: , wartość średnia bł edu: 3.552E 15, odchylenie standardowe: Na rysunku 5 przedstawiono punkty z charakterystyki fabrycznej (tablica 1) oraz jej aproksymacj e według (9). 3 v(d) gp2d12f.xy 25 2 voltage distance Rysunek 5: Charakterystyka fabryczna GP2D12 i jej aproksymacja Do przeliczania mierzonych napi eć na odległości należy skorzystać z łatwej do wyliczenia charakterystyki odwrotnej: 2.4 Charakterystyki kat napi ecie d(v) = a 2 + a 1 v a (11) Złożenie zależności geometrycznych (rysunek 3) i charakterystyki czujnika (rysunek 5) może zostać wykorzystane do uproszczenia przeliczania pomiarów tylko w przypadku gdy zależność obu 6

8 odległości (d 1,d 2 ) od kata α jest monotoniczna. v 1 (α) = v(d 1 (α)) = a + v 2 (α) = v(d 2 (α)) = a + a 1 R cosα +l btanα+a 2 (12) a 1 R cosα +l +btanα+a 2 Odpowiednie charakterystyki wypadkowe dla robotów: RoBik (czujniki w rozstawie mniejszym od średnicy koła: R = 25, b = 7, l = 2) i Kosmos (czujniki w rozstawie wi ekszym od średnicy koła: R = 6, b = 7,l = 5) przedstawiono na rysunku 6. 3 v1(alpha) v2(alpha) v1(alpha)-v2(alpha) 25 v1(alpha) v2(alpha) v1(alpha)-v2(alpha) Rysunek 6: Zależność napi ecia od kata dla RoBik i Kosmos Jak widać, zależność różnicy napi eć od kata jest niejednoznaczna dla robota RoBik, a jednoznaczna dla robota Kosmos. 2.5 Ograniczenia czasowe Proste dalmierze trangulacyjne z diodami podczerwonymi LED i elementami PSD (ang. Position Sensitive Device), jakimi sa GP2D12 firmy SHARP, maja nast epuj ace wady: Okres pomiarowy wynosi około 4ms (cz estotliwość pomiarów ok. 25Hz). Nie ma możliwości zsynchronizowania ich pracy (wyzwalania pomiarów). Konieczna jest filtracja pomiarów i eliminacja bł edów spowodowanych brakiem synchronizacji. Aby uzyskać lepsze wyniki należy zastosować np. dalmierze laserowe z synchronicznym wyzwalaniem lub znacznie wi eksz a szybkościa działania (czasem odpowiedzi 1ms). Wybrane dalmierze laserowe zestawiono w tablicy 2. Spośród nich do naszych celów najlepiej nadaje si e OADM 13U6745/S35A. Ma on wiele dodatkowych zalet: napi eciowy sygnał wyjściowy (..1V), mała średnic e plamki (1mm), szeroki zakres napi eć zasilania (12..28V), dobra rozdzielczość (.1...4mm). Jego istotna wada jest jednak wysoka cena ( $13). 7

9 typ producent odl. min. odl. max. czas reakcji wymiary mm mm ms mm OD5-5W2 SICK [2] x76.5x27 OD5-35W1 SICK x76.5x27 DT2-N254B SICK 5 15? 72.4x54.8x24.3 OADM 13U6745/S35A BAUMER [17] x4x13.4 MX1C IDEC [18] x5x21 Tablica 2: Wybrane dalmierze laserowe i ich parametry 3 REALIZACJA PRAKTYCZNA W celu zbadania słuszności opisanej koncepcji wykonano badania praktyczne z dalmierzami GP2D12 na stanowisku laboratoryjnym oraz na robocie RoBik. Rozważono też zagadnienia techniczne zwiazane z implementacja opisanej metody w robocie Kosmos. 3.1 Stanowisko badawcze z mikrokontrolerem MC68332 Wst epne badania dalmierzy i zdejmowanie ich charakterystyk przeprowadzono na stanowisku wyposażonym w mikrokontroler MC68332 [7] oraz 12-bitowy przetwornik A/C MAX144 [19] przyła- czony przez magistral e QSPI [14]. Oprogramowanie przygotowano w środowisku HIWARE TOOLS w j ezyku C i uruchomiono korzystajac z interfejsu ICD [6]. Zaobserwowana przy pomocy kamery CCD plamka świetlna dalmierza ma w użytecznym zakresie odległości średnic e ok. 7mm, jednak wraz ze wzrostem odległości rośnie rozmycie jej brzegów. Widoki plamki na tle siatki o boku 5mm, dla odległości 5mm, 13mm i 22mm pokazano na rysunku 7. Rysunek 7: Plamka świetlna GP2D12 w odległościach 5, 13 i 22mm (siatka o boku 5mm) Zmierzono indywidualne charakterystyki dla dwóch dalmierzy GP2D12 (tablica 3). Wyznaczone na ich podstawie parametry modelu w formie (9) przedstawiono w tablicy 4. Odpowiednie wykresy przedstawiono na rysunku 8. Korzystajac z wyników kalibracji dalmierzy wykonano próby statyczne pomiaru kata na makiecie symulujacej prac e układu pomiarowego według rysunku 2. Stanowisko przedstawiono na rysunku 9. Na pionowym stojaku, na wysokości R od podłoża, zamocowano poziomo koder impusowo-obrotowy MOZ3. Na jego osi zamocowano głowic e pomiarowa wyposażona w dwa dalmierze GP2D12 umieszczone równolegle do siebie w odległości 2b. Odległość bazy dalmierzy od osi obrotu głowicy wynosi l (por. rysunek 2). 8

10 odległość napi ecie1 napi ecie2 [mm] [mv] [mv] Tablica 3: Wyniki pomiarów dalmierzy GP2D12 użytych w badaniach parametr dalmierz 1 dalmierz 2 a a a bład całkowity bład średni E E-14 odchylenie standardowe Tablica 4: Parametry dalmierzy wyliczone dla 18 próbek z tablicy 3 35 v(d) dal1.xy 35 v(d) dal2.xy Rysunek 8: Wyliczone charakterystyki użytych dalmierzy GP2D12 z naniesionymi punktami pomiarowymi 9

11 Rysunek 9: Stanowisko do pomiaru kata wzgl edem podłoża przy pomocy dalmierzy Zastosowany koder obrotowy zapewnia rozdzielczośc 496 działek na obrót. Sygnały kwadraturowe sa mierzone przez funkcj e QDEC [13] w TPU [15] mikrokontrolera MC Dokładność pomiaru kata α O obrotu głowicy z czujnikami wzgl edem osi poziomej wynosi.8789 o. Wymiary stanowiska (R = 6, b = 7, l = 5) wybrano na podstawie wymiarów robota Kosmos. Eksperymenty przeprowadzono przez r eczne poruszanie głowica pomiarowa. Oprogramowanie mikrokontrolera zostało przygotowane tak, by rejestrowć pomiary odległości z dalmierzy przy wybranych wartościach kata mierzonego koderem obrotowo-impulsowym. Wyniki przedstawiono w tablicy 5. Wyniki pomiarów okazały si e nadspodziewanie dobre (bład nie przekracza.5 o ). 3.2 Rozbudowa robota RoBik Robot RoBik [5] został wyposażony w dalmierze GP2D12 zgodnie z koncepcja przedstawiona w pracy. Czujniki zamontowano w dolnej cz eści korpusu robota, symetrycznie po obu stronach (patrz rysunek 11). Rzeczywiste wymiary uzyskanego w ten sposób układu pomiarowego to: R = 245, b = 65, l = 176,4. Dalmierze przyłaczono, za pośrednictwem gniazda dodanego na płytce STER sterownika [5], do niewykorzystanych wejść (AIN1/CH3 i AIN2/CH4) przetwornika A/C MAX186 [19] (U7 na rysunku 1 w [5]). Zasilanie dla czujników (+5V ) zostało doprowadzone przez filtry LC. Schemat rozszerzenia sterownika i przyłaczenia czujników przedstawiono na rysunku 1. Napi ecie odniesienia dla przetwornika wynosi 4.96V, rozdzielczość - 12 bitów (1mV /bit). Przy zakresie napi eć wyjściowych dalmierzy (.5V 3.V ) wykorzystany zakres pomiarowy przetwornika A/C wynosi 5 3. Jednostka centralna robota RoBik to mikrokontroler MPC555 (moduł phycore-mpc555 [12]). Oprogramowanie do prób przygotowano w środowisku CodeWarrior [9] w j ezyku C i uruchomiono korzystajac z interfejsu BDM typu Wiggler [5]. 1

12 α O [ o ] d 1 [mm] d 2 [mm] α [ o ] α O α [ o ] 45, 27, 99 67, 48 45, 1, 1 39, , 3 72, 22 4, 32, 33 34, , 99 76, 55 35, 39, 41 29, , 4 81, 61 29, 88, 9 24, , 64 86, 41 25, 32, 36 2, 4 142, 5 9, 87 2, 8, 4 15, 3 133, 68 95, 91 15, 1, 7 1, 2 128, 1 12, 55 1, 34, 32 5, 1 121, 12 17, 96 5, 37, 36, 114,41 114,39,1,1 5,1 17,76 12,14 5,5,4 1,2 1,98 125,5 9,93,9 15, 3 95, , 33 15, 17, 14 2, 4 89, , 47 2, 49, 45 24,96 84,43 149,38 24,89,7 29, 97 78, 91 16, 76 3, 31, 34 34,98 73,94 171,7 34,93,5 39,99 68,78 185,85 39,9,9 45, 63, 97 24, 69 45, 15, 15 Tablica 5: Wyniki statycznych pomiarów kata na r ecznej obrotnicy Rysunek 1: Schemat przyłaczenia dalmierzy w robocie RoBik 11

13 Rysunek 11: Modyfikacja robota RoBik i płytki sterownika STER Na rysunku 12 przedstawiono pomiary odległości d 1 i d 2 w funkcji kata α (por. rysunek 3) alpha using 1:3 alpha using 1: Rysunek 12: Zależność pomiarów odległości od kata Uzyskane z czujników pomiary porównano z odczytem z koderów mierzacych położenie katowe silników przy unieruchomionych kołach i korpusie poruszajacym si e wahadłowo. Wyniki porównania (wykres α(α )) przedstawiono na rysunku 13. Obok pokazano przebiegi α i α w funkcji czasu (okres próbkowania ok. 1ms). Zależność jest liniowa, rozbieżności niewielkie w zakresie katów do ok. 3 o. Przy szybszych ruchach korpusu robota okazuje si e, że opóźnienia pomiaru odległości przy pomocy 12

14 45 4 slow 45 4 slow using :1 slow using : Rysunek 13: Wyniki pomiarów dla wolnych ruchów korpusu GP2D12 (por. rozdział 2.5) sa zbyt duże. Dobrze ilustruje to rysunek fast 45 4 fast using :1 fast using : Rysunek 14: Wyniki pomiarów dla szybkich ruchów korpusu Najlepszym sposobem usuni ecia tej niedokładności byłoby użycie rozważanych w 2.5 dalmierzy laserowych. Istotne jest, że opisany sposób pomiaru dobrze nadaje si e do kompensowania dryfu zakłócajacego pomiar kata przez całkowanie pr edkości kataowej mierzonej żyroskopem. 3.3 Zastosowanie w robocie Kosmos Jednostka sterujaca w robocie Kosmos jest oparta na takim samym procesorze (i module), jak w robocie RoBik. Sygnały z dalmierzy można doprowadzić do niewykorzystanych wejść przetwornika A/C (TLV2556 [8]) w module INS (cyt. [3]), podłaczonego przez interfejs QSPI do MPC-555. Podstawowe parametry tego przetwornika (napi ecie odniesienia i rozdzielczość) sa analogiczne do MAX186 [19], użytego w robocie RoBik. Czujniki można zamocować przy pomocy odpowiednich katowników do ukośnych elementów bocznej ramy robota po stronie modułu INS. Oprogramowanie pozwalajace obliczać kat pochylenia korpusu robota na podstawie pomiaru różnicy odległości czujników od podłoża da si e niemal bezpośrednio przenieść z robota RoBik, ze wzgl edu 13

15 na identyczna jednostk e centralna i podobny sposób obsługi wejść analogowych. Po wykonaniu odpowiednich przeróbek sprz etu i oprogramowania należy przeprowadzić kalibracj e dalmierzy i testy podobne do opisanych powyżej. 4 PODSUMOWANIE Zaproponowany sposób pomiaru pochylenia robota wzgl edem podłoża może zastapić lub wspomagać inercyjne metody pomiaru pod warunkiem, że robot porusza si e po poziomej płaszczyźnie. W przypadku zastosowań we wn etrzach (ang. indoor) warunek ten jest spełniony. W każdym przypadku można t e metod e polecić do określania pochylenia robota wzgl edem podłoża, po którym si e on porusza. Literatura [1] Kabała M., Tchoń K., Wnuk M., Robot mobilny nap edzany w układzie wewn etrznym, VII KKR, Ladek Zdrój, Prace Naukowe ICT PWr., Konferencje 46, t.1, ss [2] Kabała M., Wnuk M., Konstrukcja i oprogramowanie dwukołowego robota mobilnego, Raport serii SPR nr 2/22, Inst. Cyb. Techn. PWr, Wrocław 22. [3] K edzierski J., Robot balansujacy - model i sterowanie, Praca magisterska pod kierunkiem dr inż. A. Mazur, IIAR, PWr., Wrocław, 28. [4] Szlawski R., Implementacja i badanie algorytmow sterowania robotem dwukołowym, Praca magisterska pod kierunkiem dr. inż. M. Wnuka, ICT PWr., Wrocław, 24. z [5] Szlawski R., Wnuk M., Konstrukcja dwukołowego robota mobilnego RoBik, Raport serii SPR nr 12/24, Inst. Cyb. Techn., PWr, 24. [6] Wnuk M., ICD - Interfejs BDM dla CPU32, Raport serii SPR nr 8/24, Inst. Cyb. Techn., PWr, 24. [7] Wnuk M., Moduł z mikrokontrolerem mc Raport serii SPR nr 7/24, Inst. Cyb. Techn., PWr, 24. [8] 12-bit, 2-ksps, 11 channel, low power, serial ADC with internal reference, Texas Instruments, December 21. [9] CodeWarrior Development Tools IDE 5.1 Users Guide. [1] GP2D12 Optoelectronic Device, Data Sheet, SHARP, ref. no. SMA68, 26. [11] MC68332 User s Manual, MC68332UM/AD, Motorola Inc., [12] phycore-mpc555 Hardware Manual, Edition July 25. [13] Quadrature Decode TPU Function (QDEC), TPUPN2/D, Motorola Inc., [14] Queued Serial Module Reference Manual, QSMRMAD, Motorola Inc., [15] Time Processor Unit Reference Manual, TPURMAD, Rev.3, Mototrola Inc.,

16 [16] [17] [18] [19] [2] 15

17 dr inż. Marek Wnuk Instytut Informatyki Automatyki i Robotyki Politechniki Wrocławskiej ul. Janiszewskiego 11/ Wrocław Niniejszy raport otrzymuja: 1. OINT egz. 2. Autor egz. Razem : 6 egz. Raport wpłynał do redakcji I-6 w październiku 28 roku. 16