Uniwersytet Zielonogórski Instytut Sterowania i Systemów Informatycznych Laoratorium Sterowania Rootów Lokalizacja roota Lego Mindstorms NXT przy użyciu odometrii Uwagi wstępne 1. Wszystkie przykłady i zadania wykonujemy w środowisku Matla z użyciem skrzynki narzędziowej RWTHMindstormsNXT.. Komunikację z rootem rozpoczynamy od podłączenia poprzez kael USB. Następnie wykorzystujemy połączenie poprzez Bluetooth, zgodnie z zaleceniami prowadzącego. 3. Poniższy opis został przygotowany na podstawie informacji umieszczonych na stronach: (a) http://www.mindstorms.rwth-aachen.de/. () http://www.inpharmix.com/jps/pid Controller For Lego Mindstorms Roots.html Budowa i oprogramowanie roota 1. Celem ćwiczenia jest określenie pozycji roota lu zmian jego pozycji, czyli współrzędnych x, y oraz orientacji (kąt θ) w odniesieniu do czasu. Najważniejsze ędzie jednak określenie pozycji i orientacji roota na podstawie znanych prędkości kół.. Wszystkie poniższe równania matematyczne zawierają poniższe zmienne x, y - współrzędne początkowe roota (pozycja początkowa), x(t), y(t) - współrzędne roota jako funkcje czasu, θ, θ(t) - początkowa orientacja roota i orientacja jako funkcja czasu, v R, v L - prędkość odpowiednio prawego i lewego koła, s R, s L - odległość pokonana odpowiednio przez kolo prawe i lewe (np.: oliczona na podstawie odczytu z enkoderów i danego owodu koła), - odległość pomiędzy kołami (ich środkami), r - promień skrętu roota, t - czas w sekundach, 3. W każdej chwili czasu, współrzędne roota zmieniają się w zależności od prędkości roota i jego orientacji. 4. Przy poruszaniu się do przodu z pozycji (x, y, θ) o dystans d, nowe współrzędne roota ędą opisane równaniem x nowy x + d cos(θ) y nowy θ nowy = y + d sin(θ) θ 5. Przy orocie roota z pozycji (x, y, θ) o kąt α, nowe współrzędne roota ędą opisane równaniem x nowy x y nowy = y θ nowy θ + α 6. Ponieważ używamy dwóch niezależnie sterowanych kół napędowych to przy wykonaniu skrętu o promieniu r każde z kół pokona następujące odległości s R =(r + )θ s L =rθ Powyższe wyrażenia odnoszą się do sytuacji przedstawionej na rysunku (czyli skrętu w lewą stronę) 1
7. W celu wyznaczenia trajektorii roota na podstawie znajomości prędkości jego kół, możemy posłużyć się następującymi równaniami gdzie v(t) jest funkcją prędkości roota. =v(t) cos(θ(t)) dt dy =v(t) sin(θ(t)) dt 8. Zmiana orientacji roota, gdy mamy różne ale stałe prędkości kół, jest opisana następującym równaniem różniczkowym dθ dt = (v R v L ) Całkując powyższe równanie i iorąc pod uwagę początkową orientację roota θ() = θ otrzymujemy równanie do oliczania pozycji roota θ(t) = (v R v L )t Opisana tutaj sytuacja jest przedstawiona na poniższym rysunku (1) + θ () 9. Znając orientację roota (czyli wzór ()), możemy wyznaczyć jego pozycję na podstawie poniższych równań różniczkowych (przyjmujemy, że punkt odniesienia znajduje się w połowie odległości pomiędzy kołami, dlatego jego prędkość to (v R+v L ) ) dt =(v R + v L ) cos(θ(t)) dt =(v R + v L ) sin(θ(t)) 1. Równanie (3) ma taką samą postać jak (1). Wykonując teraz operację całkowania i przyjmując, że początkowa pozycja roota to x() = x i y() = y otrzymujemy x(t) =x + (v ( ( ) ) R + v L ) (vr v L )t sin + θ sin(θ ) (v R v L ) y(t) =y (v ( ( ) ) (4) R + v L ) (vr v L )t cos + θ cos(θ ) (v R v L ) 11. Korzystając z równania (4) mamy możliwość określenia aktualnej pozycji roota. Podstawiając za v R i v L odpowiednio s R i s L oraz opuszczając zmienną t (czyli czas, gdyż s R i s L przechowują aktualnie pokonany dystans przez koła a nie ich prędkości) możemy otrzymać dore przyliżenie pozycji roota. 1. W programie komputerowym symulującym równania (4) musimy oddzielnie zaimplementować przypadek gdy v R = v L lu s R = s L (czyli gdy mianownik dąży do zera). W takim przypadku root musi poruszać się po linii prostej. (3)
13. Ponieważ w wyranych aplikacjach nie mamy dostępu do oliczeń zmiennoprzecinkowych lu moc oliczeniowa pokładowego sterownika jest ograniczona to wtedy do oliczeń możemy używać uproszczonych wyrażeń. Jednym z najczęściej stosowanych przyliżeń są wzory przedstawione poniżej s = (s R+s L ) θ = (s R s L ) +θ x =s cos(θ)+x (5) y =s sin(θ)+y 14. W powyższych rozważaniach przyjmowaliśmy, iż prędkości kół są stałe alo wolno zmienne. Jeśli jednak taka sytuacja nie ma miejsca, musimy dodatkowo rozważyć wpływ przyspieszenia. W najprostszym przypadku prędkości poszczególnych kół mozemy zapisać jako v R (t) =α R t + w R v L (t) =α L t + w L (6) gdzie α R, α L są stałymi przyspieszeniami odpowiednio koła prawego i lewego, a w R i w L są wartościami początkowych prędkości. 15. Podstawiając wzór (6) w odpowiednie miejsca otrzymujemy A = (α R + α L ) B = (w R + w L ) C = (α R α L ) D = (w R w L ) Teraz, wzory na połozenie i orientację roota ędą miały następującą postać θ(t) =Ct + Dt + θ dt =(At + B) cos(ct + Dt + θ ) dy dt =(At + B) sin(ct + Dt + θ ) Oczywistym jest, że łatwo jest wyznaczyć orientację roota. Jednak analityczne rozwiązanie równań różniczkowych pozycji roota jest ardzo skomplikowane. Dlatego musimy rozwiązać je w sposó numeryczny. W programie komputerowym skorzystamy metod całkownia numerycznego (np. metody Simpsona). Zadania do wykonania 1. Zudować roota według wskazówek umieszczonych w instrukcji oraz przekazanych przez prowadzącego.. Napisać program do pomiaru charakterystyki silnika. Istotne jest ay zmierzyć prędkość orotową silnika (licza orotów na minutę) w zależności do zadanej mocy silnika. Następnie wyznaczyć odległość pokonywaną przez roota w zadanej jednostce czasu (przyjąć, że średnica koła napędowego to 56 [mm]). 3. Sprawdzić czy dla zadanych, stałych prędkości kół, gdzie prędkość jednego koła różni się od prędkości drugiego koła, root ędzie poruszał się po okręgu o promieniu (/) (v R+v L ) v R v L. 4. Napisać program do sterowania roorem tak ay poruszał się według danej trajektorii położenia lu prędkości.przyjąć, że root ma się poruszać po trajektorii której segmenty są tylko liniami prostymi i wycinkami koła. 5. Napisać program umożliwający wizualizację aktualnej pozycji roota na ekranie komputera dla zadanych trajektorii prędkości kół. 6. Porównać wyniki określania pozycji roota na podstawie wzorów (4) i (5). (7) 3
7. Podstawiając v = v L+v R oraz ω = v R v L model kinematyki roota o napędzie różnicowym można zapisać jako: ẋ cos(θ) [ ẏ = sin(θ) v (8) ω] θ 1 Przy założeniu, że v oraz ω są stałe, root powinien poruszać się po okręgu o promieniu R = v/ω z okresem T = π/ω. Korzystając ze środowiska Simulink zaimplementować model kinematyki roota i dorać sterowania (v, ω) w taki sposó, żey w okresie T = 5 s root wykreślił okrąg o promieniu R =, 5 m. Następnie dokonać korekt sterowań tak ay root poruszał się po: (a) spirali Archimedesa o równaniu (współrzędne iegunowe) θ = a/r,, r = x + y, a = const (rys. 1(a)), () spirali hiperolicznej o równaniu θ = a r (rys. 1()), (c) spirali logarytmicznej o równaniu θ = ln(r/a) (rys. 1(c)). Częstotliwość ojazdu ieguna spirali w każdym przypadku powinna wynosić, Hz. Następnie zaimplementować wyrany przypadek spirali (a) (c) na roocie LEGO i sprawdzić praktycznie realizację zaplanowanej trajektorii..5 (a) () (c).5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5 Rysunek 1: Trajektorie do zadania 8. 8. (*) Droga roota zakręca o 9 o po łuku okręgu. Promienie wewnętrznej i zewnętrznej krawędzi jezdni wynoszą odpowiednio 8 i 1. Korzystając z modelu kinematyki z poprzedniego zadania, zaprojektować sterownik, który dokona przejazdu pojazdu po trajektorii minimalizującej siłę odśrodkową w trakcie jazdy po zakręcie. Wskazówka 1 (dla dociekliwych): przy założeniu stałej prędkości jazdy maksymalna siła odśrodkowa występuje w szczycie zakrętu, Wskazówka (dla mniej dociekliwych): minimalizacja maksymalnej siły odśrodkowej prowadzi do trajektorii hiperolicznej, przechodzącej przez szczyt zakrętu. 9. W klasycznym teście Borensteina na łąd lokalizacji w odometrii root wykonuje zaprogramowany przejazd po kwadracie o ustalonym promieniu (patrz rys. ), przy czym mierzy się łędy odległości i kąta w każdym wierzchołku kwadratu. Błąd ma źródła deterministyczne (np. niedokładne wymiary roota) oraz stochastyczne (np. łędy pomiaru enkoderów roota). Teoretycznie kluczowy wpływ w odometrii ma pomiar azymutu roota, który wpływa nieliniowo na pomiar odległości. Sprawdzić czy i na ile użycie żyroskopu do dokładniejszego ezpośredniego pomiaru zmian kąta (zamiast pomiaru pośredniego z enkoderów, np. we wzorze 5) poprawi dokładność jazdy po zadanej trajektorii kwadratu. Bok kwadratu ustalić jako 1m, a łędy lokalizacji (czysta odometria vs odometria+żyroskop) przedstawić na wykresach. 4
Rysunek : Test łędu odometrii 5