Laoratorium Podstaw Rootyki Politechnika Poznańska Katedra Sterowania i Inżynierii Systemów ĆWICZENIE 3 Kinematyka i lokalizacja dwukołowego roota moilnego Celem ćwiczenia jest wyprowadzenie modelu kinematyki dwukołowego roota moilnego oraz poznanie udowy i zasady działania systemu pomiarowego odometrii inkrementalnej służącego do określania ieżącej pozycji i orientacji platformy roota w gloalnym układzie współrzędnych. 1 Kinematyka dwukołowego roota moilnego Model kinematyki dowolnego systemu mechanicznego wynika z matematycznych relacji wiążących wszystkie prędkości występujące w rozważanym systemie. Uwaga nasza skupiona zostanie na prostym modelu roota dwukołowego przedstawionego na rys. 1. Na rysunku tym zaznaczono lokalny {x L, y L } oraz gloalny {x G, y G } układ współrzędnych. Środek układu lokalnego przywiązano na osi kół i w połowie odległości pomiędzy kołami. Pozycję oraz orientację platformy wyrażoną w układzie gloalnym oznaczono odpowiednio jako x, y oraz ϕ (zwrot strzałki oznacza dodatni przyrost kąta orientacji). Zaznaczono także prędkości kątowe kół: prawego ω P i lewego ω L oraz prędkości związane z całą platformą: prędkość postępową v początku układu lokalnego (znak + oznacza dodatni zwrot prędkości) oraz prędkość kątową ω (znak + oznacza dodatni kierunek orotu platformy). Przy założeniu raku poślizgu kół (zarówno poślizgu wzdłużnego jak i poprzecznego), relacje wiążące prędkości kół z prędkościami postępową v i kątową ω całej platformy są następujące [1]: v(t) = v P (t) + v L (t) ω(t) = v P (t) v L (t) = [ω P (t) + ω L (t)]r, (1) = [ω P (t) ω L (t)]r, () gdzie v L i v P oznaczają liniowe prędkości związane z owodem odpowiednio lewego i prawego koła, R jest długością promienia koła, a wartość oznacza rozstaw między kołami (por. rys. 1). W oparciu o prostą interpretację geometryczną i korzystając z rysunku 1 można zapisać następujące relacje różniczkowe []: ϕ = ω, (3) ẋ = v cos ϕ, (4) ẏ = v sin ϕ, (5) gdzie ẋ v x i ẏ v y oznaczają składowe prędkości postępowej platformy wyznaczone jako ortogonalne rzuty na poszczególne osie układu azowego {x G, y G }. 1
Laoratorium Podstaw Rootyki 3 Rysunek 1: Dwukołowy root moilny w gloalnym układzie współrzędnych. 1.1 Korzystając z zależności (3)-(5) wyznaczyć macierzowe równanie stanu roota dwukołowego przyjmując wektor stanu q [ϕ x y] T oraz sterowanie u [ω v] T. 1. Korzystając z zależności (1)-(5) wyznaczyć macierzowe równanie stanu roota dwukołowego przyjmując wektor stanu q [ϕ x y] T oraz sterowanie u [ω P ω L ] T. Lokalizacja oliczanie pozycji i orientacji roota Ay root mógł ezpiecznie poruszać się w danym otoczeniu, musi on mieć możliwość określenia swojego położenia względem otaczających go przeszkód. Zwykle zadanie to realizowane jest poprzez udowę mapy otoczenia i okresowe wyznaczanie współrzędnych pozycji oraz orientacji roota w gloalnym układzie współrzędnych (wyznaczanie lokalizacji roota). Do wyznaczenia położenia i orientacji konieczna jest znajomość modelu kinematyki platformy. Powszechnie stosowaną techniką służącą do określania lokalizacji kołowych rootów moilnych jest odometria, która polega na wyznaczaniu położenia i orientacji roota na podstawie pomiarów orotu kół za pomocą przetworników orotowo-impulsowych (enkoderów) sprzężonych mechanicznie z kołami roota [3]. Na podstawie ciągu impulsów wyznaczana jest zmiana pozycji kątowej każdego z kół w przyjętej jednostce czasu, pozwalając na oliczenie prędkości kół: ω P, ω L. Prędkości te z kolei pozwalają na oliczenie prędkości platformy roota zgodnie z zależnościami (1) oraz (). Układ odometrii inkrementalnej olicza ieżące wartości pozycji i orientacji platformy w następujący sposó: ϕ(t) = ϕ() + x(t) = x() + y(t) = y() + ω(τ)dτ = ϕ() + v x (τ)dτ = x() + v y (τ)dτ = y() + [ω P (τ) ω L (τ)] R dτ, [ω P (τ) + ω L (τ)] R [ω P (τ) + ω L (τ)] R cos ϕ(τ)dτ, (6) sin ϕ(τ)dτ, gdzie x(), y() i ϕ() oznaczają wartości początkowe współrzędnych pozycji i orientacji platformy.
Laoratorium Podstaw Rootyki 3 3 Należy pamiętać, iż odometria najczęściej jest implementowana w postaci dyskretnej i realizowana za pomocą układów cyfrowych. W związku z tym równania (3)-(5) należy przedstawić w postaci równań różnicowych stosując jedną ze znanych metod dyskretyzacji. Dla przykładu, równania (6) wynikające z dyskretyzacji modelu (3)-(5) metodą Eulera wstecz mają następującą postać [3]: ϕ(n) = ϕ(n 1) + T p ω(n), x(n) = x(n 1) + T p v x (n), (7) y(n) = y(n 1) + T p v y (n), gdzie: ω(n) = [ω P (n) ω L (n)]r, v x (n) = [ω P (n)+ω L (n)]r cos ϕ(n) oraz v y (n) = [ω P (n)+ω L (n)]r sin ϕ(n), a T p > jest przyjętym okresem prókowania..1 W środowisku Matla-Simulink utworzyć schemat lokowy realizujący oliczenia lokalizacji platformy w oparciu o sygnały prędkości kątowych ω P oraz ω L. Sprawdzenie algorytmu należy wykonać dla stałych i sinusoidalnie zmiennych prędkości ω P, ω L. Przed realizacją ćwiczenia należy utworzyć własny podkatalog w katalogu D:\Laoratorium\Rootyka\cw3, w którym należy zapisywać wyniki.. Powyższy schemat lokowy zmodyfikować tak, ay sygnały prędkości kół roota ω P oraz ω L yły rzeczywistymi sygnałami wózka dwukołowego połączonego z kartą RT- DAC4/PCI D (rys. ). Rysunek : Schemat realizujący oliczenia lokalizacji platformy roota w oparciu o sygnały z enkoderów Sposó realizacji loku Enkodery z rysunku przedstawia rysunek 3. Rysunek 3: Schemat realizacji odczytu z enkoderów w czasie rzeczywistym Ay powyższy schemat lokowy przygotować do współpracy z kartą RT, należy skorzystać z ilioteki Simulink a RTCON (rys. 4). Uruchomienie ilioteki następuje poprzez wpisanie w Command Window programu MATLAB polecenia rtcon.
4 Laoratorium Podstaw Rootyki 3 Rysunek 4: Widok głównego okna pakietu RT-CON W zmodyfikowanym schemacie lokowym sygnały prędkości kątowych ω P, ω L należy skojarzyć z kartą RT poprzez lok RT-DAC PCI D Encoder, który jest programowym licznikiem impulsów zierającym impulsy z dwóch linii przetworników dla koła prawego i lewego. Na wyjściach tego loku otrzymuje się zarejestrowaną w jednostce czasu T p = const liczę impulsów enkoderów ou kół platformy. Wartość tego czasu oraz licza impulsów stanowią podstawę do wyznaczenia chwilowych prędkości kół roota. Do oliczenia prędkości ω L i ω P konieczne jest przeskalowanie otrzymanych wartości wyjściowych loku RT-DAC PCI D Encoder stosując następujący współczynnik skalujący: K = π N, (8) gdzie N = 648 oznacza liczę impulsów przetwornika przypadającą na jeden orót koła platformy moilnej, a T p =.1[s] jest przyjętym (stałym) okresem prókowania. Wartości parametrów R i występujące w równaniach (6) muszą odpowiadać geometrycznym wymiarom rzeczywistej platformy moilnej: R =.1[m], =.73[m]. (9).3 W celu poprawnego odczytu impulsów z enkoderów należy określić w loku RT- DAC PCI D Encoder jego adres oraz kanał. Odczyt następuje na kanale dla lewego koła, oraz 1 dla koła prawego. Konfigurację enkodera dla kanałów oraz 1 przedstawia rysunek 5, gdzie w miejsce Base Address należy wpisać wartość 5788 ędącą adresem konkretnego wejścia z enkodera na karcie RT. Rysunek 5: Konfiguracja enkodera
Laoratorium Podstaw Rootyki 3 5.4 W oknie, w którym tworzymy schemat lokowy dokonać ustawień następujących parametrów symulacji: Simulation Configuration Parameters Solver: Stop time 9999999, Type-FixedStep ode1, FixedStepSize.1, Tasking Mode SingleTask. Simulation Configuration Parameters Code Generation: System target file rtcon.tlc (Browse...), Language C, Description RT-CON (Visual C/C++). Simulation Configuration Parameters Code Generation Interface: Data exchange Interface: External Mode, Host/Target interface Transport layer: RT-CON tcpip. Simulation Configuration Parameters Code Generation RT-CON options: Display start-up message TAK, Append RT-DAC/USB drivers NIE, Append RT-DAC/PCI drivers TAK..5 Po ustawieniu parametrów symulacji uruchomić kompilację modelu : Simulation Configuration Parameters Code Generation BUILD:.6 W wyniku poprawnego procesu kompilacji w oknie poleceń MATLABa zostaną wyświetlone następujące komunikaty: ### Created RT-CON executale: NAZWA PLIKU.dll ### Successful completion of uild procedure for model: NAZWA MODELU Ay uruchomić try pracy real-time należy ustawić w głównym oknie symulacji try pracy External (zamiast Normal), a nastepnie uruchomić Simulation Connect to target. Zatwierdzić OK w oknie informacyjnym kompilacji oraz uruchomić try Start real-time code. Literatura [1] K. Kozłowski and J. Majchrzak, Nowe algorytmy sterowania nieholonomicznym rootem moilnym, in XIV Krajowa Konferencja Automatyki, vol., (Zielona Góra), pp. 65 63,. [] K. Tchoń, A. Mazur, I. Dulęa, R. Hossa, and R. Muszyński, Manipulatory i rooty moilne. Modele, planowanie ruchu, sterowanie. Warszawa: Akademicka Oficyna Wydawnicza PLJ,. [3] T. Jedwany, M. Kowalski, J. Majchrzak, and G. Wiczyński, Przykład wielosensorycznego systemu pozycjonowania nieholonomicznego roota moilnego, in XIV Krajowa Konferencja Automatyki, vol., (Zielona Góra), pp. 641 646,. 1/5/13