Cwiczenie 4 - Politechnika Poznańska

Laboratorium Podstaw Robotyki
Politechnika Poznańska
Katedra Sterowania i Inżynierii Systemów
ĆWICZENIE 4
System sterowania robotem mobilnym MiniTracker V3
Celem ćwiczenia jest poznanie sposobów sterowania i komunikacji z nieholonomicznym, dwukołowym robotem mobilnym MiniTracker V3. Podczas ćwiczenia poruszane będą zagadnienia związane
z planowaniem trajektorii w przestrzeni zadania oraz jej realizacją w torze otwartym.
1
Model kinematyczny robota MiniTracker V3
Autonomiczny pojazd kołowy posiadający zdolność zautomatyzowanego ruchu związanego z realizacją postawionego mu zadania nazywany jest kołowym robotem mobilnym. Realizacja zadania
związana jest ścisle z przestrzenią, w której zadanie to zostało zdefiniowane. W przypadku kołowych
robotów mobilnych naturalną przestrzenią zadania jest trójwymiarowa podprzestrzeń kartezjańska,
w której robot się porusza. W tym ćwiczeniu rozważania zostaną ograniczone do ruchu robota na
płaszczyźnie zorientowanej ortogonalnie do wektora przyspieszenia grawitacyjnego.
Do planowania i realizacji trajektorii w przestrzeni zadania niezbędna jest znajomość modelu kinematyki rozważanego robota mobilnego. Uwaga nasza zostanie skupiona na modelu dwukołowego
robota mobilnego MiniTracker V3 przedstawionego na rys. 1. Schemat kinematyczny robota MiniTracker V3 (w skrócie: MTV3) przedstawia rys. 2. Na rysunku tym zaznaczono lokalny {xL , yL }
oraz globalny {xG , yG } 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
globalnym 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 obrotu platformy).
Przy założeniu braku poślizgu kół (zarówno poślizgu wzdłużnego jak i poprzecznego), zależności
między prędkościami liniową v i kątową ω platformy robota a prędkościami kątowymi koła lewego
ωl i prawego ωp są następujące [1]:
ωp + ωl
ωp − ωl
R,
ω =
R,
(1)
2
b
gdzie R jest promieniem kół, a b jest rozstawem między punktami styku kół z podłożem (patrz
rys. 2). Przekształcając równania (1) otrzymuje się wyrażenia opisujące prędkości kół robota jako
funkcje prędkości liniowej v i kątowej ω całej platformy:
v =
v + 2b ω
v − 2b ω
,
ωl (v, ω) =
.
(2)
R
R
Zależności funkcyjne ωp = ωp (v, ω) oraz ωl = ωl (v, ω) mają kluczowe znaczenie z punktu widzenia
realizacji trajektorii zdefiniowanej w przestrzeni zadania. Pozwalają one na przeliczenie prędkości platformy do przestrzeni prędkości napędów, które ostatecznie mogą zostać zrealizowane przez
pokładowy system sterowania.
ωp (v, ω) =
1
2
Laboratorium Podstaw Robotyki – 4
Rysunek 1: Dwukołowy robot mobilny MiniTracker V3.
Rysunek 2: Schemat kinematyczny dwukołowego robota mobilnego w globalnym układzie współrzędnych.
Model kinematyki w przestrzeni zadania dla dwukołowej platformy mobilnej z rysunku 2 uwzględniający założenie o braku poślizgu kół względem podłoża ma następującą postać [3]:
 
 


ϕ̇
1
0
 ẋ  = 0 ω + cos ϕ v,
(3)
ẏ
0
sin ϕ
∆
∆
gdzie q = [ϕ x y]T jest wektorem stanu platformy moblinej, a u = [ω v]T jest wektorem sterowań
w przestrzeni zadania.
Korzystając z rysunku 2 można zauważyć kilka użytecznych relacji wiążących poszczególne
sygnały związane z kinematyką platformy robota [2]:
q
ẋ ≡ vx = v cos ϕ
(4)
⇒
|v| = vx2 + vy2 ,
ẏ ≡ vy = v sin ϕ
ẏ
⇒
ϕ = Atan2 (sgn(v)ẏ, sgn(v)ẋ) ,
(5)
tan ϕ =
ẋ
Z t
dϕ
ω=
ω(τ )dτ,
(6)
⇒
ϕ(t) = ϕ(0) +
dt
0
przy czym Atan2 (·, ·) jest czteroćwiartkową funkcją arctan.
Parametry geometryczne robota MTV3 występujące w równaniach wiążących prędkości platformy z prędkościami napędów są następujące:
R = 0.026[m],
2
b = 0.066[m].
(7)
Komunikacja i sterowanie robotem MTV3
Komunikacja nadrzędnego układu sterowania z systemem pokładowym sterownika robota możliwa
jest poprzez standardowy interfejs szeregowy RS232 oraz poprzez łącze radiowe pracujące na częstotliwości nośnej 433MHz. Łącze komunikacyjne umożliwia półdupleksową wymianę danych z prędkością 115200[b/s] i służy do przekazywania sterownikowi pokładowemu wartości zadanych prędkości
3
Laboratorium Podstaw Robotyki – 4
ωpd , ωld obu kół robota, parametrów pracy pokładowego systemu sterowania oraz służy do odbioru
danych pomiarowych. Większość wysyłanych do robota rozkazów powoduje zwrócenie odpowiedzi
potwierdzającej. Komunikację można uznać za poprawną dopiero wtedy, gdy otrzymana od robota
odpowiedź będzie prawidłowa. Pełną listę rozkazów oraz systemu komunikacyjnego można znaleźć
w instrukcji robota [4].
Przekazanie wartości zadanych prędkości poszczególnych kół robota zapewnia rozkaz SetWheelsVelocities, który jednocześnie umożliwia otrzymanie w odpowiedzi następujących danych: aktualnej
wartości znacznika czasu pokładowego, aktualnych pozycji kątowych kół, bieżących wartości prądów silników, bieżących wartości prędkości kół oraz aktualnej pozycji i orientacji platformy robota.
Prędkości zadane kół ωpd , ωld przesyła się w jednostkach wewnętrznych sterownika, które są interpretowane jako zadana ilość impulsów enkodera przypadająca na jedną sekundę ([imp/s]). Na
pełen
obrót koła przypadają 384 impulsy enkodera. Maksymalna prędkość kątowa kół wynosi około
ωp/l ≈ 80[rad/s].
max
3
Generator trajektorii zadanej
Zadaniem generatora trajektorii jest obliczanie chwilowych wartości sygnałów pozycji xd (t), yd (t)
i orientacji ϕd (t) platformy, które mają zostać zrealizowane w układzie: system sterowania robota
↔ robot. Ze względu na konstrukcję samego robota oraz ze względu na strukturę pokładowego sys-
Rysunek 3: Schemat ilustrujący sposób przygotowania sygnałów zadanych do realizacji w systemie sterownia robotem MTV3. Przyjęto oznaczenia: GTZ – generator trajektorii zadanej, qd (t) =
[ϕd (t) xd (t) yd (t)]T , Q/V – blok konwersji trajektoria/prędkości platformy, V/Ω – blok konwersji
prędkości platformy/prędkości kół robota.
temu sterowania, realizacja trajektorii zadanej wymaga dodatkowo obliczania chwilowych wartości
zadanych prędkości platformy vd (t), ωd (t) (wynikających bezpośrednio z zadanej trajektorii) i przeliczania ich na chwilowe prędkości poszczególnych kół robota ωpd (t), ωld (t). Dopiero wartości zadane
prędkości kół mogą zostać przesłane na pokład robota w celu ich realizacji za pomocą pokładowych
prędkościowych pętli regulacyjnych typu PI+F (patrz rys. 3).
Aby prześledzić sposób generowania sygnałów zadanych, rozważmy równania generatora trajek∆
torii zadanej qd (t) = [ϕd (t) xd (t) yd (t)]T w przestrzeni zadania na przykładzie trajektorii o kształcie
zbliżonym do sinusoidy kreślonej na płaszczyźnie (x, y). Niech zadane współrzędne pozycji platformy
opisane będą następującymi równaniami:
∆
xd (t)
= At,
yd (t)
= B sin(2πfd t),
∆
(8)
gdzie A, B > 0 są parametrami projektowymi, a fd jest żądaną częstotliwością zadanej sinusoidy.
Aby określić przebieg zadanej orientacji platformy robota ϕd (t) wzdłuż trajektorii pozycji (8), należy
4
Laboratorium Podstaw Robotyki – 4
określić przebiegi składowych prędkości vxd (t) oraz vyd (t) wzdłuż tej trajektorii. Różniczkowanie
równań (8) względem czasu pozwala zapisać:
ẋd (t) = vxd (t)
ẏd (t) = vyd (t)
= A = const,
= 2πfd B cos(2πfd t).
(9)
Na podstawie powyższych składowych prędkości można wyznaczyć przebieg orientacji zadanej ϕd (t)
zgodnie z zależnością (5):
ϕd (t) = Atan2 (sgn(vd )ẏd , sgn(vd )ẋd ) ∈ [−π, π),
(10)
q
2 (t) + v 2 (t),
vd (t) = ± vxd
yd
(11)
gdzie
a znak prędkości vd (t) determinuje wybór strategii ruchu robota (ruch przodem/ruch tyłem), którą
określa projektant trajektorii. Zadana prędkość kątowa platformy wynika z różniczkowania równania
(10):
ayd (t)vxd (t) − axd (t)vyd (t)
,
(12)
ωd (t) =
vd2 (t)
gdzie axd (t) oraz ayd (t) są składowymi sygnałami przyspieszenia zadanego i wynikają z różniczkowania prędkości (9):
axd (t) ≡ 0,
(13)
ayd (t) = −(2πfd )2 B sin(2πfd t).
Korzystając z faktu, że zachodzi ωd (t) =
następująco:
dϕd (t)
dt ,
ϕd (t) = ϕd (0) +
przebieg orientacji zadanej możemy także obliczać
Z
t
ωd (τ )dτ ∈ R,
(14)
0
gdzie ωd (t) wynika z (12). Należy zauważyć istotną różnicę między definicjami (10) oraz (14) wynikającą z dziedzin, do których należy sygnał ϕd (t).
Na podstawie wyznaczonych zadanych prędkości: liniowej vd (t) i kątowej ωd (t) platformy i korzystając z zależności (2), można teraz określić funkcje zadanych prędkości ωpd (t) i ωld (t) dla poszczególnych kół robota, które mogą zostać fizycznie zrealizowane przez pokładowy system sterowania
robota.
Poza analitycznym wyznaczeniem prędkości (11) oraz (12) istnieje także możliwość numerycznego obliczania tych prędkości w trybie on-line poprzez zastąpienie opercji różniczkowania operacją
różnicy skończonej. Dla różnicy wstecz i wybranego elementarnego przyrostu czasu ∆t otrzymujemy
następujące wyrażenia:
q
2 (n) + v 2 (n),
vd (n) = ± vxd
yd
oraz
ωd (n) =
gdzie
vxd (n)
=
vyd (n)
=
ϕd (n) − ϕd (n − 1)
.
∆t
xd (n) − xd (n − 1)
,
∆t
yd (n) − yd (n − 1)
∆t
(15)
(16)
5
Laboratorium Podstaw Robotyki – 4
3.1 Do własnego katalogu roboczego w katalogu C:\laboratorium\Robotyka\cw4 skopiować zawartość archiwum z pliku MTV3ControllerVisual2010.zip. Plik archiwum znajduje się w
katalogu C:\laboratorium\Robotyka\cw4. W środowisku Microsoft Visual C++ uruchomić
przygotowany projekt aplikacji MTV3Controller do komunikacji i sterowania robotem
(plik MTV3Controller.sln w podkatalogu \MTVControllerVisual2010). Otworzyć plik źródłowy
MTV3ControllerDlg.cpp i odszukać procedurę obsługi timera o nazwie OnTimer. W procedurze
obsługi timera (wywoływanej teoretycznie co 40[ms]) przedstawiono sposób wykorzystania
funkcji do komunikacji z robotem i odczytu danych z pokładu, miejsce gdzie powinien się znaleźć generator trajektorii zadanej oraz sposób zapisu danych do plików. Wartości sygnałów
z generatora trajektorii zapisywane są w pliku generator.txt, natomiast dane odczytywane z
pokładu robota w pliku robot.txt a . Kolejność zapisu poszczególnych danych jest następująca:
– generator.txt:
– robot.txt:
t, xd , yd , ϕd , ωld , ωpd ,
t, x, y, ϕ, ωl , ωp .
Do parametyzacji generatora można wykorzystać zmienne pól edycyjnych okna dialogowego
aplikacji (zmienne m dP1 . . . m dP6).
3.2 Zmodyfikować procedurę obsługi timera tak, aby możliwe było zrealizowanie następującej
sekwencji sygnałów zadanych prędkości kół robota:

 ωpd = ωld = +20[rad/s], dla t ∈ [0, 2),
ωpd = ωld = −20[rad/s], dla t ∈ [2, 4),
(17)

ωpd = ωld = 0[rad/s],
dla t ∈ [4, ∞)
przy czym czas wyrażono w [s]. Powyższe przebiegi prędkości kół odpowiadają trajektorii
w postaci odcinka prostoliniowego na płaszczyźnie (x, y) (ruch do przodu/do tyłu ze stałą
orientacją platformy). Wykreślić i porównać przebiegi sygnałów zadanych ωpd (t), ωld (t) oraz
sygnałów rzeczywistych ωp (t), ωl (t) odczytanych z pokładu robota (wykorzystać środowisko
MATLAB lub Excel). Doświadczenie wykonać dla kół obracających się bez kontaktu z podłożem oraz dla robota umieszczonego na płycie boiska. Badania można powtórzyć dla innych
poziomów prędkości ωpd (t), ωld (t) rejestrując kilka okresów sygnałów zadanych. Wykreślić
przebieg odczytanej orientacji ϕ(t) robota.
3.3 Powtórzyć powyższe badania dla zadanych prędkości kół w postaci sygnałów sinusoidalnych:
∆
ωpd (t) = ωld (t) = 10 sin 0.5t.
(18)
Wykreślić i porównać przebiegi zadanych oraz rzeczywistych prędkości kół robota. Wyznaczyć przebieg orientacji ϕ(t) platformy i porównać go z przebiegiem orientacji otrzymanym
dla prędkości z równań (17).
• Czy rzeczywista orientacja robota w obu przypadkach jest stała? Jakie zjawisko można
zaobserwować w przypadku sygnałów (18) i jak można je uzasadnić?
• Co może być przyczyną dryfu orientacji przy zmianie wartości zadanych prędkości kół
robota?
a Dane
odczytane z robota są zapisywane do pliku tylko wtedy, gdy odebrana ramka jest prawidłowa.
Laboratorium Podstaw Robotyki – 4
6
3.4 W środowisku MATLAB zaprojektować (w postaci skryptu) generator zadanej trajektorii
kołowej o promieniu rk = 0.4[m]. Przyjąć krok czasowy generowania trajektorii ∆t = 40[ms].
Na płaszczyźnie (x, y) wykreślić otrzymaną trajektorię zadaną jako krzywąa yd (t) = xd (t).
Wykreślić także czasowe przebiegi sygnałów prędkości ωpd (t) i ωld (t). Doświadczalnie dobrać
parametry trajektorii tak, aby prędkości kątowe kół nie przekraczały wartości 10[rad/s].
Obliczenia generatora wykonywać w jednostkach SI.
3.5 Zasymulowany w środowisku MATLAB generator zaimplementować w aplikacji sterującej
robotem. Obliczenia generatora wykonywać w jednostkach SI. Na wspólnych wykresach zamieścić i porównać sygnały zadane z przebiegami otrzymanymi z pokładu robota.
• Jakie zjawisko można zaobserwować podczas próby realizacji zadanej trajektorii?
• W jaki sposób można wyeliminować wpływ dryfu orientacji na realizację zadanej trajektorii?
3.6 Powtórzyć poprzednie polecenia implementując (do wyboru):
– generator trajektorii zadanej w kształcie ósemki (mieszczącej się w prostokącie o bokach
1.2 × 1[m] lub innej mieszczącej się w wymiarach boiska),
– generator trajektorii zadanej w postaci sinusoidy o amplitudzie B = 0.2[m] i okresie
T = 2π[s], położonej wzdłuż osi x układu globalnego.
Zarejestrować wyniki pomiarów i przeanalizować jakość realizacji zadanej trajektorii w przestrzeni zadania.
a Krzywa taka nazywana jest zadaną ścieżką geometryczną, gdyż parametr czasowy w tej sytuacji nie
występuje w sposób jawny.
Literatura
[1] K. Kozłowski, J. Majchrzak. Nowe algorytmy sterowania nieholonomicznym robotem mobilnym.
XIV Krajowa Konferencja Automatyki, strony 625–632, Zielona Góra, 2002.
[2] M. Michałek, K. Kozłowski. Sterowanie nieholonomicznym robotem mobilnym metodą orientowania pola wektorowego. K. Tchoń, redaktor, Postępy Robotyki, strony 235–246. Wydawnictwa
Komunikacji i Łączności, Warszawa, 2005.
[3] K. Tchoń, A. Mazur, I. Dulęba, R. Hossa, R. Muszyński. Manipulatory i roboty mobilne. Modele,
planowanie ruchu, sterowanie. Akademicka Oficyna Wydawnicza PLJ, Warszawa, 2000.
[4] Praca zbiorowa. Dwukołowy badawczy robot mobilny Mini-TrackerV3.0. Instrukcja obsługi systemu. Instytut Sterowania i Inżynierii Systemów, Zakład Sterowania i Robotyki, Politechnika
Poznańska, Poznań, 2003.