Sterowanie sześcionożnym robotem kroczącym w

Transkrypt

POLITECHNIKA WROCŁAWSKA
WYDZIAŁ ELEKTRONIKI
Kierunek:
Specjalność:
Automatyka i Robotyka (AIR)
Robotyka (ARR)
PRACA DYPLOMOWA
MAGISTERSKA
Sterowanie sześcionożnym robotem kroczącym
w nierównym terenie
Hexapod walking control over uneven terrain
Autor:
Maciej Patro
Prowadzący pracę:
dr inż. Robert Muszyński
Ocena pracy:
WROCŁAW 2014
Spis treści
1 Wstęp
3
2 Sterowanie
2.1 Chody sześcionogów . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.1 Chód pełzający . . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.2 Chód gąsienicowy . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.3 Chód trójpodporowy . . . . . . . . . . . . . . . . . . . . . . .
2.2 Sterowanie z wykorzystaniem krzywizny i prędkości . . . . . . . . . .
2.3 Generator wykorzystujący kratę Tody-Rayleigha . . . . . . . . . . . .
2.4 Połączenie kraty i sterowania z wykorzystaniem krzywizny i prędkości
2.5 Proste i odwrotne zadanie kinematyki . . . . . . . . . . . . . . . . . .
2.5.1 Proste zadanie kinematyki . . . . . . . . . . . . . . . . . . . .
2.5.2 Odwrotne zadanie kinematyki . . . . . . . . . . . . . . . . . .
2.6 Nawigacja w nierównym terenie . . . . . . . . . . . . . . . . . . . . .
3 Konstrukcja robota
3.1 Konstrukcja mechaniczna . . . . . . . . . . .
3.1.1 Korpus robota . . . . . . . . . . . . .
3.1.2 Projekt kończyny . . . . . . . . . . . .
3.1.3 Sensory . . . . . . . . . . . . . . . . .
3.2 Układy elektroniczne . . . . . . . . . . . . . .
3.2.1 Zasilanie . . . . . . . . . . . . . . . . .
3.2.2 Komunikacja MCU-Serwo . . . . . . .
3.2.3 Moduły Bluetooth i układ kamery . .
3.2.4 Mikrokontroler oraz pozostałe układy .
4 Oprogramowanie
4.1 Aplikacja użytkownika . . . . . . . . . . . .
4.1.1 Komunikacja z robotem . . . . . . .
4.1.2 Interfejs użytkownika . . . . . . . . .
4.1.3 Wizualizacja danych . . . . . . . . .
4.2 Inicjacja dodatkowych urządzeń . . . . . . .
4.2.1 Konfiguracja serwomechanizmów . .
4.2.2 Moduł Bluetooth . . . . . . . . . . .
4.3 Oprogramowanie mikrokontrolera . . . . . .
4.3.1 Komunikacja z serwomechanizmami
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
7
8
8
10
13
15
16
17
20
21
.
.
.
.
.
.
.
.
.
23
23
23
24
27
29
32
32
33
33
.
.
.
.
.
.
.
.
.
35
35
36
37
38
38
38
41
41
43
2
SPIS TREŚCI
4.3.2
4.3.3
4.3.4
4.3.5
Obsługa modułu MinIMU-9 [29] . . .
Odczyt danych z dalmierza IR . . . . .
Przerwania od czujników dotykowych .
Główne zadanie systemu — sterowanie
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
43
45
45
46
5 Badania
5.1 Symulacje w środowisku Simulink . . . . . . . . . . . . . .
5.1.1 Badania najniższej warstwy systemu . . . . . . . .
5.1.2 Warstwa zmian trybu chodu . . . . . . . . . . . . .
5.2 Badania rzeczywistego obiektu . . . . . . . . . . . . . . . .
5.2.1 Generator chodu . . . . . . . . . . . . . . . . . . .
5.2.2 Sterowanie z wykorzystaniem krzywizny i prędkości
5.2.3 Nawigacja w nierównym terenie . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
49
49
49
56
56
59
59
63
6 Podsumowanie
69
Dodatek A Płyta CD
71
Dodatek B Schematy elektroniczne
73
Bibilografia
82
Rozdział 1
Wstęp
Roboty w dzisiejszych czasach odgrywają coraz większą rolę w życiu człowieka. Jest
wiele specjalizowanych klas tych urządzeń, przeznaczonych do realizacji konkretnych
zadań. Jedną z takich grup są roboty mobilne. Ich podstawowym zadaniem jest przemieszczanie się w zadanej przestrzeni. Głównymi przedstawicielami tej grupy są maszyny kołowe oraz kroczące. Wprawdzie roboty kroczące mają w stosunku do kołowych
bardziej skomplikowaną konstrukcję, sterowanie oraz znaczne zużycie energii, jednakże
ich zaletami są większe możliwości poruszania się w przestrzeni, łatwość adaptacji do
otoczenia oraz możliwość poruszania w nierównym terenie, pokonywania przeszkód, czy
też manipulowania obiektami. Jak wiadomo człowiek bardzo często w swoich działaniach stara się naśladować rozwiązania zaobserwowane w naturze, dlatego szczególnymi
przedstawicielami maszyn mobilnych są urządzenia kroczące inspirowane zwierzętami
oraz ich sposobami poruszania się. Fakty pokazują, że człowiek interesował się takimi
urządzeniami już od tysięcy lat. Archeolodzy podczas swoich poszukiwań odkryli mechanicznego ”psa”, którego powstanie datowane jest na okres około 2 tysięcy lat przed
naszą erą [41], legendy hinduskie zawierają opisy mechanicznych słoni. W Chinach
w 231 roku naszej ery powstała maszyna krocząca MU NIU LIU MA, wykorzystywana
do transportu [6]. Mogła ona przewozić ładunki 200-250 kg pokonując około 10 kilometrów ścieżek górskich dziennie. Szkice projektowe tej maszyny zostały odnalezione
i na ich podstawie wykonano współczesną kopię.
Jedną z bardziej widocznych różnic między zwierzętami, jak i robotami kroczącymi,
jest posiadana liczba nóg. Jest ona różna ze względu na przystosowanie do środowiska
oraz zadania i cele. Im więcej kończyn posiada zwierzę czy maszyna, tym łatwiej utrzymać mu równowagę i stabilnie poruszać się w otoczeniu. Niestety konsekwencją dużej
liczby nóg jest znaczne utrudnienie sterowania. Dużo trudniej przestawiać w odpowiedni sposób osiem lub więcej nóg niż tylko dwie. Niniejsza praca porusza zagadnienia dotyczące robotów sześcionożnych ze względu na ich uniwersalność. Maszyna tego
typu ma możliwość pokonywania przeszkód, stabilnego kroczenia po nierównościach,
ponadto nie przewraca się po zatrzymaniu oraz może korzystać z wielu rodzajów chodu.
Z tych też powodów lokomocja na sześciu nogach jest popularna w naturze.
Jest wiele istniejących konstrukcji sześcionożnych robotów kroczących. Jednak zazwyczaj ich twórcy ograniczają się do prostych algorytmów sterowanie dających małe
możliwości lokomocji. Jedną z konstrukcji posiadających duże możliwości poruszania
się w różnym terenie jest robot RHex [26] charakteryzujący się prostą budową i wyko-
4
1. Wstęp
rzystaniem jedynie sześciu silników. Przykładem bardziej zaawansowanej konstrukcji
o dużym potencjale jest robot opisany w pracy [24]. Najciekawszy w tej konstrukcji
jest sposób generowania trajektorii końcówek nóg robota umożliwiający płynne rozpoczynanie ruchu oraz zatrzymywanie. Co więcej przechodzenie między różnymi trybami
chodu w tym rozwiązaniu jest bardzo intuicyjne oraz proste do realizacji.
Celem pracy jest zaprojektowanie, wykonanie konstrukcji mechanicznej oraz elektronicznej robota kroczącego, opracowanie algorytmów związanych z generacją chodu
oraz poruszaniem się w nierównym terenie, implementacja ich na jednostce centralnej
maszyny. Celem dodatkowym jest zbadanie działania wybranych rozwiązań w testowym środowisku. Konstrukcja mechaniczna robota musi cechować się dużym zakresem ruchów oraz funkcjonalnością. Rozwiązania związane z układami elektronicznymi
robota muszą zapewniać bezpieczeństwo dla użytkownika oraz robota, prostą rozszerzalność (możliwość dołączania dodatkowych sensorów lub funkcjonalności). Oprogramowanie opracowane w trakcie realizacji projektu musi przechowywać informacje
w odpowiednich strukturach danych tak, aby powstające w przyszłości moduły posiadały dostęp do informacji już przechowywanych w jednostce. Wykorzystane algorytmy
sterowania muszą zapewniać możliwie płynny ruch robota.
Praca podzielona jest na cztery główne rozdziały. Rozdział 2 poświęcony jest sterowaniu robota. Opisano w nim podstawy dotyczące chodu robotów, algorytmy generowania trajektorii dla końcówek nóg robota, sposób zadawania trajektorii maszynie, proste i odwrotne zadanie kinematyki oraz algorytmy związane z nawigacją w nierównym
terenie. W rozdziale 3 opisano konstrukcję mechaniczną robota, budowę korpusu, kończyn oraz zamocowane czujniki. Dodatkowo przedstawiono rozwiązania elektroniczne
zastosowane w robocie. Następnie rozdział 4 traktuje o oprogramowaniu robota, wykonanej aplikacji użytkownika oraz pracach związanych z odpowiednim ustawieniem
serwomechanizmów oraz modułu Bluetooth. Rozdział 5 pokazuje wyniki badań symulacyjnych oraz testy rzeczywistej konstrukcji robota. Ostatni rozdział podsumowuje
wykonane prace.
Rozdział 2
Sterowanie
Jest wiele różnych koncepcji oraz metod sterowania wielonożnymi robotami kroczącymi. Najważniejszym zadaniem sterowania robotem jest odpowiednie generowanie
chodu — formy lokomocji polegającej na synchronicznym przestawianiu odnóży powodującej przemieszanie. Często naukowcy czerpią inspirację z innych dziedzin nauki,
adaptując stworzone rozwiązania do własnej przestrzeni problemów. Jednym z takich
rozwiązań jest adaptacja kraty Tody-Rayleigha [19] opisana w pracy [24]. Z jej wykorzystaniem można generować chód dodając do niego funkcjonalności związane z poruszaniem się w nierównym terenie oraz wykonywaniem skomplikowanych manewrów.
W bieżącym rozdziale w sekcji 2.1 opisano podstawy związane z chodami sześcionożnych robotów kroczących. Następnie podrozdział 2.2 obrazuje sposób zadawanie
trajektorii maszynie poprzez parametry krzywizny i prędkości. Podrozdział 2.3 opisuje
generator chodu z wykorzystaniem kraty Tody-Rayleigha. Kolejna sekcja 2.4 łączy
zadane sterowanie poprzez krzywiznę i prędkość z generatorem chodu, aby umożliwić
śledzenie trajektorii. Następnie w podrozdziale 2.5 przedstawiono proste i odwrotne
zadanie kinematyki umożliwiające zamianę wartości współrzędnych kartezjańskich na
bezpośrednie wartości kątów ustawianych na silnikach. Ostatni podrozdział 2.6 poświęcony jest zagadnieniu modyfikacji otrzymywanych trajektorii nóg z generatora chodu
tak, aby umożliwić nawigację w nierównym terenie.
2.1 Chody sześcionogów
Podczas chodu każda z nóg robota jest kolejno podnoszona do góry, przesuwana wzdłuż
wektora ruchu, stawiana, a następnie przesuwana wzdłuż wektora o przeciwnym zwrocie do wektora ruchu. Na rysunku 2.1 przedstawiono przykładową trajektorię zakreślaną przez punkt końca kończyny podczas przestawiania. Końcówki nóg robota
zazwyczaj poruszają się po trajektorii przypominającej elipsę. Fragment ruchu nogi
w powietrzu jest nazywany fazą podnoszenia (protrakcji), natomiast faza podparcia
(retrakcji) następuje, gdy noga oparta jest o podłoże. Faza retrakcji powoduje ruch
robota w zadanym kierunku. Ponadto wyróżnia się dwie długości: wykrok — odcinek
na jaki noga wysuwa się do przodu — oraz zakrok — odcinek na jaki noga cofa się.
Suma wartości tych dwóch odcinków definiuje pełną długość kroku robota.
Przyjmuje się, że pozycja robota jest stabilna, gdy rzut jego środka ciężkości na
podłoże w kierunku zgodnym z siłą grawitacji znajduje się w środku figury geome-
6
2. Sterowanie
długość kroku
zakrok
wykrok
faza retrakcji
wysokość kroku
faza protrakcji
kierunek ruchu
Rysunek 2.1 Proces przestawiania nogi z parametrami
trycznej wyznaczonej przez punkty kontaktu nóg z podłożem. Figura ta nazywana
jest wielokątem podparcia. Przykład pozycji stabilnej sześcionoga można zobaczyć
na rysunku 2.2. Algorytmy chodu sześcionożnych robotów kroczących są inspirowane
między innymi sposobem poruszania się insektów [12]. Owady oraz roboty sześcionożne w opozycji do istot o mniejszej liczbie kończyn poruszają się w sposób stabilnie
statyczny. Maszyny kroczące statycznie stabilne charakteryzują się tym, że w każdej
fazie ruchu ich pozycja jest stała — zatrzymanie pracy silników w dowolnym momencie poruszania się nie spowoduje utraty stabilności. Dodatkowo ich ruch opisany jest
wyłącznie z wykorzystaniem metod kinematyki, a trajektorie zadawane są precyzyjnie
i ich realizacja powinna być dokładna [42]. Naukowcy zajmujący się entomologią podczas swoich obserwacji chodu owadów wprowadzili sześć prawidłowości dotyczących
algorytmów chodu [40, 42]:
• fale przemieszczeń nóg przenoszą się od tyłu do przodu ciała i żadna następna
noga nie zostanie podniesiona, zanim noga znajdująca się za nią nie zostanie
postawiona,
• naprzeciwległe nogi znajdujące się w tym samym segmencie ciała są przesunięte
w fazie ruchu (nie są przemieszczane równocześnie),
• czas przenoszenia jest stałą dla wszystkich nóg, niezależnie od prędkości ruchu,
• czas podparcia (styku z podłożem) maleje w miarę wzrostu prędkości ruchu,
rośnie więc częstość kroczenia (częstość przestawień nóg),
• odstęp czasu między podniesieniem nogi tylnej, a środkowej oraz między podniesieniem nogi środkowej i przedniej są porównywalne, podczas gdy taka różnica czasu
pomiędzy nogą tylną, a przednią zmienia się proporcjonalnie do częstości kroczenia,
• noga przednia (lub środkowa) jest podnoszona tylko wtedy, gdy noga poprzedzająca ją w sekwencji zostanie postawiona na podłoże.
2.1. Chody sześcionogów
7
b
b
b
b
b
środek masy
b
noga podpierająca
Rysunek 2.2 Wielokąt podparcia zaznaczony dla ruchu trójpodporowego
Korzystając z zaobserwowanych własności można zdefiniować kolejność kroczenia
nóg dla kilku podstawowych rodzajów chodu sześcionożnych maszyn kroczących. W tym
dla chodów opisanych w tej pracy: pełzającego 2.1.1, gąsienicowego 2.1.2 oraz trójpodporowego 2.1.3. Oznaczenia związane z numeracją nóg dla chodów pokrywają się
z oznaczeniami z rysunku 2.11.
Aby poprawnie opisać chód maszyny kroczącej niezbędne jest zdefiniowanie kilku
dodatkowych pojęć [42]:
• Okres chodu — jest to czas, w którym realizowane są pojedyńczo wszystkie fazy
ruchu (pełna sekwencja przestawiania każdej kończyny).
• Współczynnik obciążenia — stosunek czasu, w którym noga jest w powietrzu, do
okresu chodu.
• Zapas stabilności — określany jest jako odległość rzutu środka ciężkości od krawędzi wielokąta podparcia. Wielkość ta mierzona jest wzdłuż aktualnego wektora
ruchu środka ciężkości.
2.1.1
Chód pełzający
Najwolniejszym sposobem kroczenia sześcionogów jest chód pełzający (metachroniczny) [24]. Polega on na przestawianiu nóg jedna po drugiej. Współczynnik obciążenia
dla tego chodu jest równy 16 , ponieważ w każdej fazie ruchu tylko jedna noga znajduje
8
2. Sterowanie
Rysunek 2.3 Trajektorie poszczególnych nóg w chodzie metachronicznym
się w górze, pozostałe podtrzymują korpus sześcionoga. Oznacza to, że prędkość w fazie
retrakcji jest pięć razy mniejsza niż w fazie przenoszenia. Rysunek 2.3 pokazuje trajektorie wszystkich sześciu nóg podczas przemieszczania się chodem pełzającym. Na
rysunku 2.4 można zaobserwować wszystkie fazy chodu pełzającego wraz z wielokątem podparcia. Zapasy stabilności w tym rodzaju kroczenia są duże, co powoduje że
nawet w przypadku nieoczekiwanej przeszkody robot prawdopodobnie zachowa swoją
stabilność. Chód ten można określić jako zachowawczy.
2.1.2
Chód gąsienicowy
Chód gąsienicowy (czteropodporowy) nazwany w ten sposób ze względu na podobieństwo do charakterystycznego sposobu poruszania się gąsienic. Składa się on z trzech faz,
w trakcie których kolejno przestawiane są nogi tylne, środkowe i przednie, co przedstawiono na wykresie 2.5. Współczynnik obciążenia poszczególnych nóg wynosi 13 .
Rysunek 2.6 obrazuje sekwencje kroków robota oraz zmiany wielokąta podparcia podczas poszczególnych faz. Taki sposób poruszania się posiada niewielki zapas stabilności
dlatego wielkość zakroku i wykroku powinny być odpowiednie dobrane. Przy dużych
zakłóceniach (w postaci przeszkód) istnieje możliwość utraty stabilności. W teorii ten
rodzaj chodu przystosowany jest do pokonywania przeszkód o regularnym kształcie
(np. schodów) [42]. Nie spełnia on jednak drugiej prawidłowości opisanej wcześniej dla
sześcionogów. Prędkość ruchu nogi w fazie retrakcji jest dwa razy mniejsza niż w fazie
protrakcji.
2.1.3
Chód trójpodporowy
Chód trójpoporowy jest najszybszym stabilnym chodem jaki może być realizowany
przez roboty oraz organizmy sześcionożne. Trajektorie poszczególnych nóg podczas
2.1. Chody sześcionogów
Rysunek 2.4 Fazy chodu z wielokątami podparcia dla chodu pełzającego
Rysunek 2.5 Trajektorie poszczególnych nóg w chodzie gąsienicowym
9
10
2. Sterowanie
Rysunek 2.6 Fazy chodu z wielokątami podparcia dla chodu gąsienicowego
dwóch faz pokazano na rysunku 2.7. Współczynnik obciążenia dla tego chodu wynosi 12 . Na rysunku 2.8 pokazane są obie fazy ruchu wraz z wielokątami podparcia.
Nogi w tym ruchu przestawiane są trójkami — z tego powodu rozróżnialne są tylko
dwie fazy ruchu, gdyż pozostałe nakładają się na nie. Charakterystyczne dla tego ruchu
jest, że prędkość nóg w trakcie retrakcji i protrakcji jest równa. Chód trójpodporowy
jest chodem najczęściej wykorzystywanym do poruszania się w środowisku. Specjalny
rozstaw nóg u owadów pozwala na zwiększenie zapasu stabilności podczas przemieszczania się. Jest to ruch wykorzystywany często do przemieszczania się po płaskich
powierzchniach.
2.2
Sterowanie z wykorzystaniem krzywizny i prędkości
Sposób przenoszenia nóg robota zależy nie tylko od wybranego algorytmu chodu, ale
również od kształtu trajektorii, po której porusza się środek maszyny kroczącej. Trajektoria zadawana jest przez użytkownika i musi ona być jednoznaczna, aby uniemożliwić
maszynie ruch niezgodny z intencjami operatora. Z tego powodu do opisu trajektorii ruchu korpusu robota można wykorzystać pojęcie krzywizny, prędkości ruchu oraz
skręcenia1 [8]. Jednakże w tej pracy w miejsce skręcenia zastosowano wysokość utrzymywania korpusu nad powierzchnią ruchu, oznaczoną jako Z. Idąc za przykładem
zachowania żywych organizmów jako punkt odniesienia przyjmowany jest środek masy
1
W trakcie realizacji projektu korpus robota traktowany jest jako punkt, dlatego pomijane są
zagadnienia związane z jego orientacją. Ponieważ inspiracją dla projektu są owady i ich sposób
poruszania się, korpus robota jest domyślnie zorientowany równolegle do trajektorii ruchu
2.2. Sterowanie z wykorzystaniem krzywizny i prędkości
Rysunek 2.7 Trajektorie poszczególnych nóg w chodzie trójpodporowym
Rysunek 2.8 Fazy chodu z wielokątami podparcia dla chodu gąsienicowego
11
12
2. Sterowanie
Y0
Vi
Xsi
Ri
Ysi
ω
γ
V0
ω
S0
X0
R
Rysunek 2.9 Robot podczas kroczenia po łuku
robota.
Do odtworzenia zadanej trajektorii na płaszczyźnie X0Y , można wykorzystywać
fakt, iż funkcje x(t), y(t) kreślą lokalnie trajektorię charakteryzującą się daną krzywizną
K(t) lub promieniem R(t). Zależność między nimi jest następująca
R(t) =
1
,
K(t)
natomiast prędkość ruchu można opisać poprzez
√
v(t) = ẋ(t)2 + ẏ(t)2 .
(2.1)
(2.2)
Stąd prędkość kątowa ruchu jest opisywana wzorem
ω(t) =
v(t)
.
R(t)
(2.3)
Następnie można wyprowadzić wzór na promień krzywej, po której porusza się końcówka i-tej nogi robota
√
2
(t)
(2.4)
Ri (t) = (R(t) − |xis (t)|2 ) + yis
oraz jej prędkość
vi (t) = Ri (t)ω(t).
(2.5)
2.3. Generator wykorzystujący kratę Tody-Rayleigha
13
Aby koniec kończyny robota w trakcie przemieszczania się maszyny nie zmieniał położenia względem podłoża w fazie retrakcji, należy sterować nogą w taki sposób, by jej
prędkość viw (t) względem korpusu była równa
2
viw
(t) = v(t)2 + vi (t)2 − 2|v(t)||vi (t)| cos ψ,
(2.6)
gdzie
cos (ψ(t)) =
R(t) − xis (t)
.
Ri (t)
(2.7)
Uzyskana w ten sposób prędkość końca nogi jednoznacznie określa prędkości poszczególnych przegubów nogi robota zapewniając niezmienność punktów kontaktu z podłożem w trakcie zmiany pozycji. Prędkości te można obliczyć wykorzystując odwrotne
zadanie kinematyki opisane w podrozdziale 2.5.
2.3 Generator wykorzystujący kratę
Tody-Rayleigha
Bez możliwości generowania odpowiednich chodów dla robota, trudne jest stworzenie
poprawnie funkcjonującej konstrukcji. W trakcie prac nad tym projektem postanowiono posłużyć się kratą Tody-Rayleigha opisaną w pracy [24].
Kratą Tody-Rayleigha nazywa się sieć Tody [39], w której kolejne elementy sieci
to oscylatory Rayleigha [25]. Krata Tody (Toda lattice) jest to prosty model jednowymiarowego kryształu stosowany w fizyce ciała stałego opisujący nieskończony łańcuch
sąsiadujących elementów oraz ich wzajemnych relacji danych równaniem
{
ṗn = eyn−1 −yn − eyn −yn+1
,
(2.8)
ẏn = pn
gdzie yn reprezentuje przesunięcie n-tego elementu w sieci, natomiast pn to pęd tego
elementu, przy założeniu, że masa poszczególnych elementów kraty jest równa 1. Sieć
Tody [24] można modelować, jako układ mas połączonych ze sobą przy pomocy sprężyn. Sprężyny łączące kolejne elementy posiadają nieliniową charakterystykę sprężystości [24]. Gdy energia poszczególnych ogniw jest równa — układ jest stabilny. Jednak,
gdy do pewnego elementu łańcucha zostanie doprowadzone pobudzenie z zewnątrz,
zostanie ona rozpropagowana w postaci fali na sąsiednie elementy poprzez sprężyny.
Uzyskiwanym w ten sposób efektem są niegasnące fale propagowane przez kolejne masy
układu. Można również założyć, że sieć jest skończona i składa się z N elementów.
W ten sposób powstaje sieć w postaci pierścienia spełniająca zależność
yn+N = yn .
(2.9)
Konstruowany przez autora robot kroczący jest sześcionogiem, dlatego dla zastosowań
związanych z generacją chodów przyjmowane jest N = 6. Widok pierścienia reprezenstującego taką sieć znajduje się na rysunku 2.10.
14
2. Sterowanie
m1
m6
m2
m5
m3
m4
Rysunek 2.10 Sześcioelementowy pierścień sieci Tody
W artykule [20] po raz pierwszy zastosowano połączenie kraty Tody z nieliniowymi
oscylatorami Rayleigha opisywane równaniem
{
ṗn = ω02 (eyn−1 −yn − eyn −yn+1 ) + (µ − p2n )pn
,
(2.10)
ẏn = pn
gdzie ω0 oraz µ to współczynniki związane z oscylatorem Rayleigha oznaczające kolejno:
ω0 — współczynnik regulujący częstość oscylacji w układzie, µ — współczynnik wpływający na kształt cyklu granicznego. Równanie to charakteryzuje również wymianę
energii w sieci, jednak uogólnia ono to zjawisko dodając elementy odpowiadające za
wytracanie i pompowanie energii. Następnie w pracy [19] zaproponowano modyfikację
kraty Tody-Rayleigha,
{
ÿn = ω02 (eyn−1 −yn − eyn −yn+1 ) + f (y˙n − p0 ) − ωR2 yn
(2.11)
.
f (x) = (µ − x2 )x
Dodanie ujemnego sprzężenia zwrotnego w postaci członu ωR2 yn ma na celu stabilizację
oraz zapobieganie dryftowi niektórych rozwiązań cyklicznych. Dodatkowo uwzględniono przesunięcie rozwiązań w przestrzeni stanu o wartość p0 , które ma pełnić rolę
sygnału sterującego, przesuwając punkt wokół, którego tworzy się cykl graniczny. Takie podejście znacznie upraszcza przechodzenie między sąsiadującymi rozwiązaniami
cyklicznymi. Reprezentacja (2.16) umożliwia generację ruchu kolejnych chodów: pełzającego, gąsienicowego oraz trójpodporowego. Kolejne węzły kraty odpowiadają za
generację trajektorii ruchu dla odpowiednich końcówek nóg robota. Trajektoria otrzymywana z równania posiada jedynie współrzędne Y oraz Z nogi (według oznaczeń
z rysunku 2.12). Wartość współrzędnej X uznawana jest za stałą charakterystyczną
dla danego robota. Generowanie chodu zasadniczo posiada trzy fazy: fazę rozruchu,
fazę chodu właściwego oraz fazę zatrzymywania.
Faza rozruchu jest odpowiedzialna za zainicjowanie zadanego przez użytkownika
rodzaju chodu. W tym celu należy dobrać eksperymentalnie odpowiednie wartości
początkowe charakterystyczne dla tego chodu. Ważne jest, aby położyć nacisk na to,
by współrzędne osi Y w fazie rozruchu były równe zeru dla każdej z nóg. Gdy warunek
ten nie jest spełniony, występuje ”szuranie” nogi o podłoże, co powoduje niepotrzebne
straty energii oraz może powodować nienaturalny ruch robota.
2.4. Połączenie kraty i sterowania z wykorzystaniem krzywizny i prędkości
15
Faza chodu właściwego jest fazą naturalnie uzyskiwaną po rozruchu. Układ opisywany równaniem (2.16) po pewnym czasie stabilizuje swoje rozwiązanie oscylując
z daną częstotliwością, co odpowiada generacji trajektorii chodu właściwego dla wszystkich nóg robota.
Ważnym zagadnieniem jest również odpowiedni sposób zatrzymywania się maszyny
kroczącej. Ponieważ, jej zadaniem jest naśladowanie ruchu insektów, sposób poruszania
się jej powinien być płynny. Dlatego dynamika oscylatorów na czas fazy zatrzymania
jest zamieniana na równania oscylatorów harmonicznych tłumionych powodujące wytłumienie oscylacji do wartości końcowych, czyli płynne przesunięcie końcówek nóg
robota sześcionożnego do zadanej pozycji bazowej. Stąd w chwili rozpoczęcia fazy
zatrzymania równanie (2.16) przyjmuje postać
2
yn − Cn .
ÿn = −µy˙n − ωosc
(2.12)
Zaletą takiego rozwiązania jest możliwość zmiany zadanej pozycji bazowej Cn na jakiej robot ma się zatrzymać. Wykorzystując taką możliwość można w sposób płynny
przechodzić między różnymi sposobami chodu maszyny (np. gładkie przejście między
ruchem trójpodporowym, a czteropodporowym). Aby dokonać takiego przejścia należy
zrealizować następujące fazy:
• robot jest w fazie chodu właściwego, generator wykorzystuje dynamikę kraty
Tody-Rayleigha, rozwiązanie cykliczne odpowiada chodowi X,
• zadana jest zmiana trybu chodu z X na Y,
• wybierany jest odpowiedni punkt równowagi Cn dla chodu Y oraz realizowana
jest dynamika oscylatora tłumionego,
• gdy wartości zmiennych stanu yn osiągną zadane położenia Cn następuje przełączenie pracy na kratę Tody-Rayleigha ze stanu Cn ,
• układ zaczyna wykonywać oscylacje i po płynnym rozruchu rozpoczyna chód Y.
2.4 Połączenie kraty i sterowania z wykorzystaniem
krzywizny i prędkości
Sterowanie z wykorzystaniem krzywizny i prędkości należy w intuicyjny sposób połączyć z generatorem chodu wykorzystującym kratę Tody-Rayleigha. Krata TodyRayleigha generuje ruch jedynie w kierunku współrzędnych Y oraz Z, dlatego głównym
zadaniem połączenia sterowania jest umożliwienie zmiany pozycji nogi również wzdłuż
osi X.
Postanowiono rozwiązać ten problem dodając współczynniki pxi (t) oraz pyi (t) dla
i-tej końcówki nogi. Ich zadaniem jest modyfikacja wartości otrzymywanych z kraty
Tody-Rayleigha w taki sposób, by środek robota poruszał się zgodnie ze sterowaniem
wykorzystującym prędkość i krzywiznę. W ten sposób współrzędne (xwi (t) oraz ywi (t)
i-tej nogi w trakcie ruchu dane są równaniem
{
xwi (t) = pxi (t)Ytri (t)
(2.13)
,
ywi (t) = pyi (t)Ytri (t)
16
2. Sterowanie
gdzie Ytri (t) to współrzędna Y generowana przez kratę Tody-Rayleigha. Określenie
wartości współczynników pxi (t) oraz pyi (t) jest związane ze zmianą pozycji końca każdej z nóg względem osi X oraz Y w trakcie wykonywania pojedynczego kroku. W tym
celu należy określić pozycję początkową oraz końcową i-tej nogi w trakcie okresu jednego kroku. Pozycja początkowa jest znana, natomiast do obliczenia pozycji końcowej
niezbędne jest wyznaczenie kąta γ(t) widocznego na rysunku 2.9
γ(t) = atan2(yi , xi ∓ Ri (t)),
(2.14)
gdzie Ri (t) to promień ruchu i-tej nogi dany równaniem (2.4), natomiast (xi , yi ) to
współrzędne końca i-tej nogi robota w trakcie spoczynku. Znak przy promieniu określa,
w którą stronę ma skręcać robot, dodatni gdy skręt wykonywany jest w prawo, ujemny
gdy w przeciwną stronę. Aby obliczyć pozycję końcową nogi należy zwiększyć kąt
γ(t) o kąt przebyty w danej chwili równy co do wartości ω(t) (2.3).Przy danym kącie
określenie końcowych współrzędnych (xik (t), yik (t)) dane jest wzorem
{
xik (t) = Ri (t) cos(γ(t) + ω(t))
.
(2.15)
yik (t) = Ri (t) sin(γ(t) + ω(t))
Następnie wyznaczamy współczynniki pxi (t) oraz pyi (t) jako
{
pxi (t) = xik (t) − xi
.
pyi (t) = yik (t) − yi
(2.16)
Niestety tak uzyskane współczynniki mogą powodować, że generowana trajektoria będzie niemożliwa do realizacji przez rzeczywisty obiekt. Zwiększanie wartości krzywizny
powoduje zwiększanie również współczynników pxi (t) oraz pyi (t) dla stałej prędkości.
Dlatego niezbędne jest ograniczenie ich wartości. Mając ustaloną stałą pmax maksymalnego dystansu dla osi Y oraz X możliwego do pokonania w jednym ruchu należy
sprawdzić warunek
max|pi (t)| ≤ pmax ,
i
(2.17)
gdzie pi to współrzędne pxi (t) oraz pyi (t) i-tej nogi. Gdy warunek jest spełniony współczynniki pozostają bez zmian. W przeciwnym przypadku należy dokonać modyfikacji
wartości współczynników
{ k
pxi (t) = pxi (t)pmax (max|pi (t)|)−1
i
,
(2.18)
pkyi (t) = pyi (t)pmax (max|pi (t)|)−1
i
gdzie pkxi (t) i pkyi (t) to ostateczne wartości współczynników.
2.5
Proste i odwrotne zadanie kinematyki
Do zadawania odpowiedniej pozycji końca nogi robota niezbędne jest rozwiązanie prostego i odwrotnego zadania kinematyki dla kończyn. Zadanie proste kinematyki pozwala na wyznaczenie położenia końca nogi robota, jako funkcji zależności od nastaw
2.5. Proste i odwrotne zadanie kinematyki
17
poszczególnych przegubów kończyny. Jest ono wykorzystywane głównie do doboru odpowiednich pozycji bazowych ustawienia robota. Do realizacji chodu przydatne jest
również rozwiązanie odwrotnego zadania kinematyki, którego celem jest określenie odpowiednich nastaw przegubów nogi dla zadanego położenia jej końca.
Projektując i budując nogi robota kroczącego w sposób naturalny wykorzystywane
są przeguby obrotowe, ponieważ symulują one ruchy stawów organizmów biologicznych.
Kolejnym argumentem za takim wyborem są względy praktyczne: tego typu przeguby
redukują koszty i pozwalają na prostszą budowę mechaniczną. Jednak z tego powodu
proste i odwrotne zadanie kinematyki wymaga wyliczania funkcji trygonometrycznych,
co zwłaszcza w przypadku mniejszych mikrokontrolerów może być poważnym problemem ze względu na narzut obliczeniowy. Roboty kroczące zazwyczaj mają być systemami autonomicznymi, co powoduje, że z jednej strony należy dążyć do zastosowania
jak najprostszych i możliwie energooszczędnych rozwiązań elektronicznych, a z drugiej
strony należy pamiętać o zapewnieniu odpowiednio dużej mocy obliczeniowej dla projektowanego algorytmu. Dlatego należy możliwie zoptymalizować rozwiązanie odwrotnego zadania kinematyki względem liczby operacji trygonometrycznych wykorzystywanych w trakcie obliczeń. Ponadto podczas obliczania współrzędnych nogi robota na
podstawie położenia końcówki można natrafić na rozwiązania niejednoznaczne. Oznacza to, że zadane położenie końcówki nogi robota w przestrzeni kartezjańskiej można
uzyskać przez różne nastawy na poszczególnych przegubach kończyny. W tej pracy
większość niejednoznaczności została wyeliminowana ze względu na fizyczne ograniczenia ruchu przegubów.
Ponieważ, wszystkie nogi robota posiadają identyczną konstrukcję, obliczenia można
wykonać dla jednej nogi względem układu współrzędnych związanego z punktem jej
zamocowania, następnie w razie potrzeby dokonywać przekształcenia do współrzędnych związanych z środkiem masy robota. Na rysunku 2.11 przedstawiono układy
współrzędnych związane z poszczególnymi kończynami Xi Yi Zi oraz środkiem maszyny
X0 Y0 Z0 . Transformacje między układem nadrzędnym, a układami lokalnymi nóg mają
postać
Ti0 = T r(X, xi )T r(Y, yi )Rot(Z, αi ),
(2.19)
gdzie xi , yi to przesunięcie układu i-tej nogi względem układu nadrzędnego, natomiast
{
π
dla nóg 1, 2, 3
αi =
.
(2.20)
0
dla nóg 4, 5, 6
Macierz tego przekształcenia dana jest wzorem

cos αi − sin αi

sin αi cos αi
Ti0 = 
 0
0
0
0
2.5.1

0 xi
0 yi 
.
1 0
0 1
(2.21)
Proste zadanie kinematyki
Nogę wykonanej konstrukcji robota można przedstawić jako manipulator o trzech stopniach swobody z trzema przegubami rotacyjnymi. Rysunek 2.12 pokazuje nogą w tak
18
2. Sterowanie
Y1
X4
Y4
X1
Y0
Y2
X5
X2
X0
Y5
Y3
X6
X3
Y6
Rysunek 2.11 Robot z zaznaczonymi poszczególnymi układami współrzędnych
2.5. Proste i odwrotne zadanie kinematyki
Z0
19
Z2
Y0
X2
q3
Z1
q2
q1
X1
Z3
X0
X3
Rysunek 2.12 Kinematyka nogi robota
zwanej pozycji bazowej. Jest to pozycja wyjściowa, przyjmowana gdy robot stoi. Od
tej pozycji rozpoczyna się chód i w tej pozycji powinien się on zakończyć. Przyjęte
wartości położenia kątowego w poszczególnych przegubach w pozycji bazowej nogi wynoszą odpowiednio:
q1 = 0,
π
q2 = 12
,
7π
q3 = 12 .
(2.22)
Kinematyka pojedynczej nogi jest opisana jako transformacja T03 pomiędzy układem związanym z punktem zamocowania kończyny, a układem końca nogi. Poszczególne transformację między kolejnymi układami uzyskiwane są przy pomocy algorytmu
Denavita-Hartenberga [37] i opisane są zależnościami
T01 = Rot(Z, q1 )T r(X, l1 )Rot(X, − π2 ),
T12 = Rot(Z, q2 )T r(X, l2 ),
T23 = Rot(Z, q3 )T r(X, l3 ).
(2.23)
Stąd, na podstawie wzoru (2.23), wyliczamy przekształcenie T03 jako


c1 c23 −c1 s23 −s1 c1 (c23 l3 + c2 l2 + l1 )
s1 c23 −s1 s23 c1 s1 (c23 l3 + c2 l2 + l1 )
,
T03 = 
−s23 −c23
0
−(s23 l3 + s2 l2 ) 
0
0
0
1
(2.24)
gdzie ci = cos qi , si = sin qi , cij = cos(qi + qj ), sij = sin(qi + qj ). Elementy ostatniej
kolumny otrzymanej macierzy określają współrzędne (px , py , pz )T końca nogi maszyny
20
2. Sterowanie
Z
q3
α
l2
l3
l1
q2
β
X
γ
pz
r
d
Rysunek 2.13 Rzut nogi na płaszczyznę 0XZ dla q1 = 0
względem układu współrzędnych związanego z miejscem jej zamocowania:

 px = c1 (c23 l3 + c2 l2 + l1 )
py = s1 (c23 l3 + c2 l2 + l1 ) .

pz = −(s23 l3 + s2 l2 )
(2.25)
Długości członów w rzeczywistym obiekcie wynoszą: l1 = 43.5mm, l2 = 70.0mm,
l3 = 144.5mm, zaś zakresy ruchu przegubów ograniczono do q1 ∈ (− π2 , π2 ), q2 ∈ (− π2 , π2 )
oraz q3 ∈ (0, π).
2.5.2
Odwrotne zadanie kinematyki
Do obliczenia odwrotnego zadania kinematyki dla nogi robota postanowiono wykorzystać metodę algebraiczną, opisaną w [42]. Wartość kąta pierwszego przegubu rotacyjnego q1 można w prosty sposób obliczyć z zależności
q1 = atan2(py , px )
(2.26)
Korzystając z rysunku
√ 2.13 łatwo zauważyć, że długość nogi zrzutowanej na płaszczyznę 0XY wynosi p2x + p2y . Korzystając z twierdzenia cosinusów można dzięki temu
obliczyć kąt α, dla którego prawdziwa jest zależność
l22 + l32 − 2l2 l3 cos α = r2 + p2z ,
(2.27)
gdzie r to
r=
dla d =
√
√
d2 + p2z ,
(2.28)
p2x + p2y − l1 , co po uproszczeniu można zapisać jako
cos α =
l22 + l32 − d2 − p2z
.
2l2 l3
(2.29)
2.6. Nawigacja w nierównym terenie
21
Z rysunku 2.13 można wywnioskować, iż kąt trzeciego przegubu q3 jest równy q3 = π−α,
stąd można stwierdzić, iż
cos q3 =
d2 + p2z − l22 − l32
.
2l2 l3
(2.30)
Odpowiednią wartość kąta q2 można wyznaczyć z faktu, iż
q2 = β − γ.
(2.31)
Kąt β można uzyskać z twierdzenia cosinusów w analogiczny sposób, jak kąt α. Po
przeprowadzeniu obliczeń uzyskujemy:
l2 +r2 −l2
cos β = 2 2l2 r 3 ,
√
sin β = 1 − cos2 β.
(2.32)
Wartość kąta γ można wyliczyć, jako nachylenie prostej przechodzącej przez odcinek
r do osi 0X pomnożony przez −1. W ten sposób otrzymujemy wzór na kąt q2 w postaci
q2 = atan2(pz , d) + atan2(sin β, cos β).
(2.33)
2.6 Nawigacja w nierównym terenie
Ponieważ jednym z ważniejszych zadań stawianych przed robotem jest lokomocja w zróżnicowanym otoczeniu należy stworzyć odpowiedni sposób reagowania na nierówności
występujące w terenie. Do tego celu wykorzystywane są informacje z czujników dotykowych przymocowanych na końcach nóg robota. Z tego powodu zdefiniowano współczynniki λi oznaczające stan sensora znajdującego się na i-tej kończynie maszyny. Gdy
λi jest równa zero oznacza to, że dana noga nie ma kontaktu z podłożem. Wartość
parametrów λi równa 1 informuje o kontakcie i-tej nogi z podłożem.
Sterowanie w nierównym terenie jest złożonym procesem i nie zawsze można przewidzieć wszystkie możliwe sytuacje, które mogą zaistnieć w przestrzeni pracy robota.
Z tego powodu autor skupił się na możliwie prostym obliczeniowo rozwiązaniu problemu
zapewniającym robotowi możliwość poruszania się w wielu różnych środowiskach.
Mając zadaną trajektorię poruszania się końca i-tej nogi robota, należy modyfikować ją w taki sposób, by w momencie zaistnienia kontaktu z podłożem, dalszy ruch
wzdłuż współrzędnej Z obniżający pozycję nogi został zatrzymany. Do wykonania takiego rozwiązania modyfikowana jest wartość pzi (t) współrzędnej Z ustawianej na i-tej
nodze poprzez wzór
{
Ztri (t)
gdy λi = 0
(2.34)
pzi (t) =
,
max(Ztri (t), pzi (t − 1)) gdy λi = 1
gdzie Ztri (t) to współrzędna Z uzyskana z kraty Tody-Rayleigha, natomiast pzi (t − 1)
oznacza wartość pzi w poprzednim kwancie czasu. Takie rozwiązanie powoduje, że
robot przestaje naciskać na podłoże, gdy już na nim stoi, natomiast podnosi nogę gdy
należy to zrobić.
22
2. Sterowanie
Oprócz takiej modyfikacji podczas chodzenia, niezbędne jest zatrzymywanie robota
gdy przeszkoda, która pokonać ma maszyna jest zbyt głęboka. Należy zatrzymać
robota gdy minimum dla osi Z podczas cyklu chodu pzi (t) = pzmin robot nie natrafia
na podłoże λi = 0. W tym momencie wiadomo, iż robot opuszczając maksymalnie
nogę nie natrafia na stabilny punkt podparcia.
Rozwiązanie zaproponowane w równaniu (2.35) posiada pewną wadę. Gdy robot
natrafia na przeszkodę blisko maksymalnej pozycji wychylenia w osi Z, nie może podnieść nogi wystarczająco wysoko by zrobić kolejny krok. Dlatego dodano następujący
warunek

 max(Ztri (t), pzi (t − 1)) gdy λi = 1, Ztri (t) ≤ pzmax − 1[cm]
Ztri (t)
gdy λi = 1, Ztri (t) > pzmax − 1[cm] ,
pzi (t) =
(2.35)

Ztri (t)
gdy λi = 0,
umożliwiający dalsze kroczenie w takiej sytuacji.
Rozdział 3
Konstrukcja robota
Do przetestowania metod sterowania robota kroczącego w rzeczywistych warunkach
niezbędne jest przygotowanie konstrukcji robota będącej w stanie wykonywać konieczne
obliczenia oraz realizować zaimplementowane algorytmy sterowania. W tym celu powstała maszyna sześcionożna, której konstrukcja opisana jest w tym rozdziale. Poniżej
w sekcji 3.1 przedstawiono opis konstrukcji mechanicznej robota z wyszczególnionymi
założeniami dotyczącymi korpusu, nóg oraz przymocowania sensorów. Kolejna sekcja 3.2 poświęcona jest rozwiązaniom elektronicznym wykorzystanym w maszynie, opisuje wybrane układy elektryczne, zasilanie i podział na moduły.
3.1 Konstrukcja mechaniczna
Jednym z najbardziej podstawowych zadań podczas tworzenia maszyny kroczącej jest
wypracowanie przemyślanej konstrukcji umożliwiającej robotowi realizację zadań mu
przydzielanych. Dobrą metodą projektowania mechanizmów jest obserwacja rozwiązań
wypróbowanych w analogicznych warunkach przez naturę oraz próba ich naśladowania.
Autor pracy postanowił wzorować się na na żuku tygrysim, widocznym na rysunku 3.1.
Zaprojektowane elementy przypominają kształtem oraz proporcjami budowę żuka. Całość konstrukcji została zaprojektowana przy wykorzystaniu oprogramowania Autodesk
Inventor 2013 [3]. Złożony projekt oraz efekt końcowy realizacji widoczne są na rysunkach 3.10 i 3.11. Pliki z projektami poszczególnych elementów oraz złożeniami
konstrukcji dostępne są w dodatku A.
3.1.1
Korpus robota
Korpusy wielonożnych robotów, jak i insektów, posiadają odmienną budowę, zależną od
warunków, w których funkcjonują. Gdy przestrzeń pracy jest wypełniona przeszkodami
ich korpus tworzony jest na planie okręgu. Takie rozwiązanie zapewnia dużą zwrotność,
kosztem prędkości poruszania się. Gdy teren jest dość równy, insekty mogą poruszać
się szybciej, dlatego ich korpusy tworzone są na planie prostokątów, co zapewnia możliwość szybkiego przemieszczania się w wyznaczonym kierunku. Wadą tego rozwiązania
jest mała zwrotność. Żuki tygrysie funkcjonują w otoczeniu zróżnicowanym, przez co
ich przystosowanie jest kompromisem między dwoma poprzednimi strategiami. Korpus
tego insekta zbudowany jest na planie owalu, przez co umożliwia mu większą szybkość
24
3. Konstrukcja robota
Rysunek 3.1 Cicindela formosa generosa[17]
poruszania się, niż u stworzeń z tułowiem opartym o koło. Co więcej, takie rozwiązanie
daje możliwość efektywniejszego manewrowania, niż u insektów z korpusami na planie
prostokąta. Korpus robota, widoczny na rysunku 3.2, musi nie tylko zapewniać pożądane rozłożenie nóg, ale także w jak najmniejszym stopniu ograniczać ich możliwość
poruszania się. W tym celu podstawy pomiędzy kolejnymi kończynami są wklęsłe.
Do wykonania podstaw wykorzystano szkło akrylowe (pleksi), ponieważ przy zachowaniu odpowiedniej sztywności jest ono lekkie, a ponadto jest izolatorem, co zapobiega
przed przypadkowymi zwarciami. Dolna podstawa podtrzymuje cały ciężar nóg oraz
elektroniki, dlatego wykorzystano pleksi o grubości 5mm. Głównym zadaniem górnej
części jest zapewnienie sztywności w osiach obrotu nóg robota oraz zamknięcie bryły
konstrukcji. Do tego zadania wystarczy odchudzony materiał o przekroju 3mm. Podstawy między sobą połączone są kołkami dystansowymi.
3.1.2
Projekt kończyny
Jednym z ważniejszych elementów części mechanicznej robota są jego nogi. Główne
ograniczenie w projekcie nogi maszyny wprowadzają zastosowane serwomechanizmy.
Muszą one być wstanie działać poprawnie pod obciążeniem całej konstrukcji. Wymagany moment serw determinuje ich rozmiar i pośrednio projekt całej nogi. Standardowe serwomechanizmy (rysunek 3.3(a)) mają jedną oś napędu wyprowadzoną z swojej obudowy. Konstrukcja nogi posiadająca połączenie z taką osią napędu nie jest
wystarczająco sztywna do poprawnego funkcjonowania. Przez to projektant musi zapewnić sztuczną oś z drugiej strony silnika, by poprawić stabilność konstrukcji. Samodzielne dodanie takiej osi jest trudne, dlatego autor postanowił zastosować silniki
Hitec HSR8498HB [38] (rysunek 3.3(b)) posiadające obudowy z sztuczną osią po drugiej
stronie mechanizmu. Rozwiązanie takie znacznie upraszcza konstrukcję oraz sprawia,
3.1. Konstrukcja mechaniczna
25
(a) Widok 3D połączonych podstaw
(b) Widok z góry
Rysunek 3.2 Korpus robota
(a) Standardowe serwo [13]
(b) Serwo Hitec HSR8498HB [38]
Rysunek 3.3 Porównanie standardowych i robotycznych serwomechanizmów
że konstrukcja jest symetryczna — do obu stron serwa przymocowane są identyczne
orczyki — przez co projekt wymaga mniej różnych elementów.
Zaprojektowane kończyny podobnie jak u żuka tygrysiego powinny posiadać trzy
stopnie swobody (rysunek 3.4). Długość odcinka biodrowego (coxa), podobnie jak
u żuków tygrysich powinna być niewielka. Udo (femur) powinno być około dwukrotnie krótsze od połączonych piszczelu (tibia) i stawu skokowego (tarsus). Powierzchnia
końcówki nogi powinna być możliwie mała, jednocześnie zapewniając miejsce dla mocowania czujnika nacisku osłoniętego gumową oprawą w celu ochrony sensora oraz
zwiększenia tarcia. Podobne przystosowanie jest obserwowalne u żuków. Końcówki
odnóży zaopatrzone są w szpony zwiększające przyczepność insektów. Materiałem wybranym do wykonania nogi jest blacha aluminiowa o grubości 1,5mm. Jest ona bardzo
lekka oraz zapewnia odpowiednią sztywność. Kolejnym argumentem przemawiającym
na korzyść tego wyboru jest prosty sposób kształtowania materiału — wykonanie nacięć na linii gięcia umożliwia otrzymywanie odpowiednich zagięć przy użyciu imadła.
Model nogi oraz efekt końcowy widoczne są na rysunkach 3.5 i 3.6.
26
Rysunek 3.4 Jedno z odnóż Cicindela formosa generosa[17] z zaznaczonymi odcinkami
Rysunek 3.5 Projekt nogi robota w Autodesk Inventor 2013
3.1. Konstrukcja mechaniczna
27
Rysunek 3.6 Rzeczywista realizacja nogi maszyny kroczącej
3.1.3
Sensory
Każdy robot autonomiczny powinien posiadać sensory, aby móc rozpoznawać otoczenie.
Wykonana konstrukcja została zaopatrzona w szereg sensorów umożliwiających odbiór
informacji z otoczenia. Poniżej opisano poszczególne czujniki umieszczone na robocie
oraz sposób ich przymocowania do konstrukcji.
Czujniki nacisku
Jedną z najbardziej podstawowych informacji, które robot poruszający się w nierównym terenie powinien posiadać, to wiedza czy noga podczas stawiania dotyka podłoża.
Do uzyskiwania tych informacji robot posługuje się zamontowanymi na końcówkach
nóg przyciskami rozwiernymi PBS-10B [14]. Sposób podłączenia czujników widoczny
jest na rysunku 3.7. Sygnał otrzymywany z czujników zmienia wartość już przy ich
delikatnym wciśnięciu, przez co układ sterowania ma czas na reakcję.
Kamera oraz Dalmierz IR
Praktycznie każde stworzenie posiada głowę wyposażoną w oczy — system wizyjny.
Analogicznie jest w przypadku żuka tygrysiego. Aby upodobnić konstrukcję robota
oraz wyposażyć go w możliwość pobierania informacji z otoczenia poprzez system wizyjny stworzono moduł widoczny na rysunkach 3.8(a) i 3.8(b). Wyposażony jest on
w dwa serwomechanizmy typu mikro TowerPro SG-90 [31]. Dwa stopnie swobody
umożliwiają obrót układu względem osi X oraz Z w zakresach ±45o . Moduł kamery
(LinkSprite JPEG Color Camera [15]) i dalmierz IR (Sharp GP2Y0A21YKOF [32])
przymocowane są do serw za pomocą elementów wykonanych z 1,5mm grubości blachy
aluminiowej. Z identycznego materiału wykonano połączenie między orczykiem serwa
przymocowanego do podstawy a silnikiem podtrzymującym resztę układu głowy.
28
Rysunek 3.7 Sposób podłączenia przycisków rozwiernych
(a) Projekt w Autodesk Inventor 2013
(b) Rzeczywista realizacja
Rysunek 3.8 Moduł kamery oraz dalmierza
3.2. Układy elektroniczne
29
Rysunek 3.9 Sposób przymocowania modułu MinIMU-9
Moduł odczytu orientacji
Ważną informacją dla robota jest jego orientacja w stosunku do wektora układu odniesienia ziemi. Informacje, czy maszyna porusza się po równi pochyłej, w nierównym
terenie czy też powierzchnia jest płaska, mają duże znaczenie dla sposobu sterowania
robotem. W celu zdobywania wiedzy na temat własnej orientacji maszyna wyposażona jest w moduł MinIMU-9 [29], zawierający czujniki: akcelerometr, żyroskop oraz
magnetometr. Moduł, widoczny na rysunku 3.9, przymocowany jest na środku górnej
podstawy korpusu robota. Wybrano to miejsce do mocowania układów, aby zwiększyć odległość modułu od silników, które mogą zakłócać jego pracę. Ponadto moduł
ustawiony jest na gąbce z tworzywa sztucznego, znacznie zmniejszającej jego drgania
w trakcie ruchu, co minimalizuje błędy związane z tym zjawiskiem.
Odczyt pozycji serwomechanizmów
Sprzężenie zwrotne od położenia zadanego serwomechanizmom jest jednym z najbardziej podstawowych wymagań przy implementowaniu algorytmów sterowania. Wykorzystane w projekcie silniki [38] posiadają możliwość odczytu pomiaru wartości położenia serwomechanizmów poprzez specjalny protokół, dzięki czemu w projekcie nie
uwzględniano dodatkowych sensorów związanych z odczytem pozycji silników.
3.2 Układy elektroniczne
Konstrukcja mechaniczna robota jest bezużyteczna bez odpowiednich układów elektronicznych umożliwiających zasilanie oraz sterowanie maszyną. Zastosowane w robocie
rozwiązania elektroniczne zostały zgrupowane w dwóch oddzielnych układach: pierwszym — zawierającym mikrokontroler oraz zapewniającym odpowiednie wyjścia i wejścia do sterowania, oprogramowywania układu oraz przesyłania informacji, drugim —
odpowiedzialnym za zasilanie układów, kontrolę poziomów sygnałów i komunikację
30
Rysunek 3.10 Model robota w Autodesk Inventor 2013
31
Rysunek 3.11 Rzeczywista realizacja konstrukcji projektu
32
Rysunek 3.12 Schemat zasilania pojedynczej nogi robota
z serwomechanizmami. Schematy elektroniczne oraz widoki płytek PCB zaprojektowanych w ramach projektu zebrane zostały w dodatku B. Poniżej opisano rozwiązania
elektroniczne zawierające system zasilania, kontrolę serwomechanizmów, podział układów na odpowiednie moduły oraz połączenia między nimi.
3.2.1
Zasilanie
Do zasilania całej konstrukcji robota wykorzystywana jest bateria litowo-polimerowa
o napięciu 7, 4V , pojemności 3, 2Ah i maksymalnym prądzie rozładowania 20C (64A).
Serwomechanizmy wykorzystane w projekcie [38] wymagają napięcia zasilania 6V oraz
mogą pobierać prąd do 0, 5A. Dlatego w celu poprawnego ich funkcjonowania autor
postanowił pogrupować je po 3 (każda noga oddzielnie) oraz zasilać je poprzez osobne
dla każdej grupy stabilizatory MIC29502 [22]. Do ustawienia odpowiedniego napięcia
wykorzystano dzielnik napięcia złożony z rezystorów 100Ω i 390Ω. Ponieważ zastosowane stabilizatory są typu low-drop1 , baterię zasilającą robota można przeładowywać
do napięcia 8, 4V oraz rozładowywać do napięcia 7V , co przy poborze prądu serw pozwala na pracę ciągłą całego układu przez około 30 — 35 minut. Układ zasilania dla
pojedynczej nogi widoczny jest na rysunku 3.12.
Oprócz zasilania silników w nogach robota, niezbędne jest zapewnienie poprawnego
zasilania pozostałych układów robota. Napięcie 5V potrzebne do zasilania mikro serw,
dalmierza IR oraz modułu kamery uzyskiwane jest przy pomocy stabilizatora low-drop
LM1085R-5.0 [11]. Natomiast układ mikrokontrolera zasilany jest napięciem 3, 3V
wykorzystując LM2937IMP-3.3.
3.2.2
Komunikacja MCU-Serwo
Standardowe silniki mają możliwość sterowania pozycją za pomocą sygnału PWM
o okresie 20ms i szerokości 1 − 2ms. Oprócz standardowego sterowania serwomechanizmy użyte w nogach robota posiadają własny protokół HMI (opisany dokładniej w rozdziale 4). Protokół ten umożliwia dwustronną komunikację między serwem
a mikrokontrolerem, jednak wymaga specjalnego podłączenia widocznego na schemacie
z rysunku 3.13. Serwa wykorzystywane w module kamery i dalmierza są standardowe
— podłączone są poprzez translator poziomów ST2378E [35] do wyjść mikrokontrolera.
1
funkcjonujące poprawnie także przy małej różnicy pomiędzy napięciem wyjściowym a wejściowym
33
Rysunek 3.13 Schemat komunikacji między serwem a mikrokontrolerem
3.2.3
Moduły Bluetooth i układ kamery
W projekcie wykorzystywane są dwa moduły bluetooth. Pierwszy z nich niezbędny jest
do nawiązywania komunikacji między aplikacją komputerową, a jednostką na robocie.
Połączenie między modułem KAmodBTM222 [28] a mikrokontrolerem jest wykonane
poprzez jedno złącze 4x1 z sygnałami: GND, 3, 3V , TX (transmisja UART) oraz RX
(odbiór UART). Drugi moduł jest całkowicie niezależny od systemu na maszynie i jego
zadaniem jest przesyłanie obrazu z kamery do komputera. Zdecydowano się na takie rozwiązanie w celu odciążenia jednostki centralnej oraz umożliwienia przyszłego
rozwoju systemu wizyjnego, z większymi możliwościami obliczeniowymi na komputerze. Układ kamery jest zasilany napięciem 5V . Połączenie modułu bluetooth i układu
kamery realizowane jest przez transmisję UART.
3.2.4
Mikrokontroler oraz pozostałe układy
Do projektu wykorzystano mikrokontroler STM32F407VGT6 [36]. Wybór ten związany
jest z 32-bitową architekturą jednostki oraz sprzętowym wsparciem obliczeń zmiennoprzecinkowych pojedynczej precyzji niezbędnym przy tego typu projekcie. Schemat
poprawnego podłączenia procesora do układu pokazano na rysunku 3.14. Procesor
taktowany jest zewnętrznym kwarcem o częstotliwości 8M Hz. W celu umożliwienia
zaawansowanego programowania oraz debugowania kodu do układu dodano złącze 6x1
pozwalająca na łączenie modułu z programatorem na płytce testowej STM32F4 Discovery Board [34]. Dzięki takiemu rozwiązaniu można wykorzystywać SWD do prac
testowych na mikrokontrolerze.
34
Rysunek 3.14 Schemat podłączenia mikrokontrolera STM32F407VGT6
Dalmierz IR oraz napięcie z baterii podłączone są do mikrokontrolera poprzez dzielniki napięcia, tak by napięcia otrzymywane podczas pomiarów były mniejsze od napięcia odniesienia. Ponadto układ wyposażono w 5 diod LED służących do wyświetlania
informacji dla użytkownika. Diody łączone są z wyjściami jednostki za pomocą złącza
2x5, które umożliwia wykorzystanie tych pinów również do innych celów lub poprzez
zworki do łączenia z wcześniej wymienionymi diodami. Kolejnym ważnym elementem
układu jest złącze do komunikacji po magistrali I 2 C z modułem żyroskopu, akcelerometru i magnetometru. Sygnały przesyłane przez złącze 4x1 to: SDA, SCL, 3, 3V oraz
GND. Dodatkowo na płytce mikrokontrolera zamieszczono układ FT232RL [7] połączony ze złączem micro USB 2.0A żeńskim. Takie rozwiązanie umożliwia przesyłanie
dodatkowych danych do komputera podczas testów jednostki.
Rozdział 4
Oprogramowanie
Szkielet robota nie jest w stanie poruszać się samodzielnie bez odpowiedniego oprogramowania. Ważne jest, by zastosowana architektura oprogramowania była odpowiednio
przemyślana, tak by stworzone rozwiązania zapewniały nie tylko pełną realizację powierzonych zadań, ale również były łatwo rozszerzalne. Oprogramowanie robota jest
podzielone na dwie oddzielne aplikacje: aplikację użytkownika oraz oprogramowanie
systemu wbudowanego. Celem aplikacji użytkownika, opisanej dokładniej w podrozdziale 4.1, jest zadawanie sterowania maszynie kroczącej z wykorzystaniem krzywizny i prędkości. Oprogramowanie osadzone na mikrokontrolerze, opisane w podrozdziale 4.3, jest odpowiedzialne za obsługę urządzeń peryferyjnych robota oraz realizację zadań związanych z chodem. Dodatkowo, w podrozdziale 4.2 opisano sposób
konfiguracji serwomechanizmów i modułu Bluetooth w odpowiednich trybach pracy.
4.1 Aplikacja użytkownika
Aplikacja użytkownika ma za zadanie dostarczać prosty interfejs graficzny dla operatora robota, umożliwiający komunikację z robotem poprzez Bluetooth oraz wizualizację
danych pochodzących z czujników. Musi ona zapewniać możliwość prostego i szybkiego zadawania odpowiednich wartości do sterowania maszyny. Dodatkowo powinna
być skonstruowana w taki sposób, by dodawanie nowy funkcjonalności nie stanowiło
wyzwania.
Program wykonano przy użyciu języka C++ wykorzystując biblioteki Qt [30]. Wybór tych bibliotek związany jest z możliwością przenoszenia napisanych z ich wykorzystaniem aplikacji między różnymi systemami operacyjnymi. Do tworzenia interfejsu
graficznego użytkownika postanowiono wykorzystać system zakładek, przez co dodawanie kolejnych funkcjonalności polega na zwiększaniu liczby zakładek, dzięki czemu
gotowe elementy oprogramowania pozostają bez zmian.
Aplikacja użytkownika realizuje trzy główne zadania opisane dokładniej w kolejnych
podrozdziałach pracy. Pierwszym zadaniem jest uzyskiwanie informacji od użytkownika o sposobie poruszania się robota. Kolejne zadanie to nawiązywanie komunikacji
oraz wymiana informacji pomiędzy maszyną kroczącą a programem komputerowym.
Ostatnie zadanie to wizualizacja części danych sensorycznych uzyskiwanych przez robota. Połączenie funkcjonalności wykonujących te zadania jest realizowane przy pomocy mechanizmu rozsyłania zdarzeń jako sygnałów (signals) do gniazd (slots).
36
4. Oprogramowanie
(a) telegram
(b) potwierdzenie ack
Rysunek 4.1 Schemat ramki Profibus [5] typu telegram dla stałej długości danych
Oznaczenie Objaśnienie znaczenia
SD
Znacznik początku ramki — wartość 0xA2
DA
Adres docelowy — urządzenia, do którego wiadomość zostanie wyłana
SA
Adres nadawcy — urządzenia, z którego wysyłana jest ramka danych
FC
Suma kontrolna — obliczana jako suma danych DU modulo 256
Data
8 bajtów danych przesyłanych
FCS
Powtórzenie sumy kontrolnej
ED
Znak końca ramki — wartość 0x16
Tabela. 4.1 Oznaczenia ramki Profibus
4.1.1
Komunikacja z robotem
Komunikacja pomiędzy komputerem, a znajdującym się wewnątrz robota mikrokontrolerem, jest bezprzewodowa. Połączenie między urządzeniami realizowane jest przez
odpowiednie moduły wykorzystujące technologię Bluetooth [27]. Wymiana informacji
z punktu widzenia obu jednostek prowadzona jest poprzez interfejs szeregowy. Parametry połączenia posiadają następujące wartości:
• prędkość transmisji sygnału — 115200 bitów na sekundę,
• długość pojedyńczego słowa — 8 bitów,
• liczba bitów stopu — 1,
• brak kontroli parzystości,
• brak kontroli sprzętowej.
Wprawdzie zastosowana technologia Bluetooth zapewnia bezstratną komunikację,
jednakże możliwa jest utrata części danych podczas przesyłu pomiędzy mikrokontrolerem a modułem na robocie oraz między urządzeniem Bluetooth a jednostką centralną
komputera. Z tego powodu, do kontroli transmisji postanowiono wykorzystać pakiety
zgodne z ramką Profibus [5] dla telegramów o stałej długości. Format ramki oraz potwierdzenia odbioru (ack) można zobaczyć na rysunku 4.1, objaśnienia do oznaczeń
zamieszczone są w tabeli 4.1.
4.1. Aplikacja użytkownika
trybhtrójpodporowyh[3]
37
trybhczteropodporowyh[4]
trybhpięciopodporowyh[5]
inicjacjahpołączeniah
zhportemhszeregowym
wysyłaniehramkih[Enter]
trybhkomunikacjihciągłej
ustawieniehprędkościh
[W]h-hzwiększeniehprędkościh
[S]h-hzmniejszeniehprędkości
h
ustawieniehkrzywizny
[D]h-hzwiększeniehkrzywizny
[A]h-hzmniejszeniehkrzywizny
ustawieniehprędkościh
serwomechanizmówh[0-100F]h
[R]h-hzwiększenieh
[F]h-hredukcjah
Rysunek 4.2 Interfejs użytkownika do sterowania robotem
4.1.2
Interfejs użytkownika
Zadawanie sterowania maszynie jest niezbędne do jej poprawnego funkcjonowania, dlatego należy wykonać odpowiedni interfejs umożliwiający proste i szybkie zadawanie
sterowania w czasie rzeczywistym. Główny nacisk w tej części projektu położono na
intuicyjność obsługi aplikacji. Autorowi zależało na tym, by użytkownik pierwszy raz
korzystający z interfejsu był w stanie kontrolować robota. Poza sterowaniem za pomocą myszki interfejs pozwala na wykorzystanie skrótów klawiszowych analogicznych
do stosowanych powszechnie w grach komputerowych. Na rysunku 4.2 przedstawiono
panel sterowania wraz z opisem skrótów klawiaturowych wykorzystywanych do sterowania. Ustawienia umożliwiają wybór jednego z trzech trybów chodu oraz przesyłu
danych w sposób cykliczny (co okres 100 milisekund) lub zdarzeniowo (po wciśnięciu
przycisku Send albo klawisza Enter na klawiaturze). Najistotniejsze dla użytkownika
jest sterowanie krzywizną oraz prędkością. Możliwe jest to poprzez korzystanie z przycisków WSAD. Aby zwiększyć prędkości maszyny należy wcisnąć klawisz W, natomiast
zmniejszanie oraz ruch wstecz uzyskujemy poprzez klawisz S. Jednostką prędkości za, gdzie T to okres pełnego cyklu chodu. Aby robot mógł
dawaną w aplikacji jest cm
T
skręcać niezbędne jest zadanie mu odpowiedniej krzywizny. Wartość tego parametru
interpretowana jest w jednostkach m1 , jej odwrotność określa współrzędną X środka
okręgu leżącego na prostej y = 0, po którego obwodzie powinna poruszać się maszyna.
Klawisz D odpowiedzialny jest za zwiększanie krzywizny, natomiast przeciwną rolę
pełni wykorzystywanie przycisku A. Dodatkowo możliwe jest sterowanie prędkością samych serwomechanizmów użytych w projekcie. Zmiana tego parametru możliwa jest
w zakresie od 0 do 100% prędkości bazowej.
38
4. Oprogramowanie
Rysunek 4.3 Wizualizacja danych otrzymywanych z modułu MinIMU-9
4.1.3
Wizualizacja danych
Sensoryka robota pozwala na gromadzenie wielu danych dotyczących stanu robota.
Większość z nich jest istotna dla określania sposobu poruszania się robota w nierównym
terenie. Jednym z ważniejszych aspektów dotyczących tego zagadnienia jest określenie
orientacji robota względem ziemi. Informacje te uzyskiwane są z jednostki centralnej robota w postaci kwaternionów, czyli struktury algebraicznej wyprowadzonej przez
Williama Hamiltona do opisu mechaniki w przestrzeni trójwymiarowej [9]. Ponieważ
taki zapis jest abstrakcyjny, trudno wyobrazić sobie orientację robota w przestrzeni na
podstawie tego zapisu. Z tego powodu wykonano trójwymiarową kostkę (rysunek 4.3),
wizualizującą orientację przekazywaną przez robota. Do tego celu wykorzystano bibliotekę OpenGL [4] doskonale współpracującą z Qt oraz przykłady umieszczone na
stronie projektu [30].
4.2
Inicjacja dodatkowych urządzeń
Oprócz wykonania oprogramowania mikrokontrolera do obsługi systemu niezbędne jest
odpowiednie ustawienie serwomechanizmów oraz modułu Bluetooth, tak by można
wykorzystać ich cały potencjał podczas sterowania robotem kroczącym.
4.2.1
Konfiguracja serwomechanizmów
W projekcie wykorzystano serwomechanizmy firmy Hitec typu HSR8498HB [38]. Silniki
te posiadają możliwość komunikacji na kilka sposobów:
• komunikacji za pomocą sygnału PWM o okresie 20ms i wypełnieniu 1-2ms, jak
w standardowych serwomechanizmach,
• rozszerzonej komunikacji za pomocą protokołu wykorzystującego sygnał PWM,
dostępnej dla niektórych serw cyfrowych,
4.2. Inicjacja dodatkowych urządzeń
39
Rysunek 4.4 Schemat ramki i odpowiedzi dla protokołu HMI serwomechanizmów
• protokołu HMI umożliwiającego komunikację po UART ze znajdującym się wewnątrz serwomechanizmu mikrokontrolerem.
Niestety zastosowanie pierwszej z wymienionych metod komunikacji z serwami zapewnia jedynie kontrolę ich pozycji, natomiast sterowanie rozszerzonym protokołem wykorzystującym PWM nie spełnia ograniczeń czasowych nałożonych na konstrukcję —
niemożliwa jest odpowiednia kontrola robota ze sterowaniem o częstotliwości 2-3 Hz.
Z tych powodów w robocie wykorzystano protokół HMI. Jednakże zastosowanie tego
protokołu jest utrudnione z powodu braku dokumentacji producenta i konieczności bazowania na niepełnych informacjach ze źródeł niezależnych [21]. Autor pracy opierając
się na tych danych opracował prostą aplikację umożliwiającą sterowanie oraz programowanie serw.
Parametry komunikacji szeregowej są następujące:
• prędkość transmisji sygnału — 19200 bitów na sekundę,
• długość pojedyńczego słowa — 8 bitów,
• liczba bitów stopu — 2,
• brak kontroli parzystości,
• brak kontroli sprzętowej.
Każda przesyłana ramka danych składa się z siedmiu bajtów danych: początku
ramki (0x80h), komendy, dwóch bajtów parametrów, sumy kontrolnej oraz dwóch bajtów końcowych. Bajty te przyjmują wartość (0x00h) gdy są nadawane, serwomechanizm odpowiada analogiczną ramką modyfikując dwa ostatnie bajty jako odpowiedź
do systemu. Schemat ramki widoczny jest na rysunku 4.4. Suma kontrolna ramki dana
jest wzorem
Checksum = 256 − (0x80 + CM D + P 1 + P 2) mod 256,
(4.1)
gdzie CM D to numer danej komendy, a P 1 i P 2 to parametry komendy.
Aby zapewnić poprawną pracę serwomechanizmu dla wymagań robota niezbędne
jest nadanie mu odpowiedniego numeru ID, tak by każde serwo mogło być sterowane
niezależnie od reszty. W tym celu należy odczytać aktualny numer ID (domyślnie
wartość: 0), wpisać nowy wybrany numer ID oraz odczytać i nadpisać komórkę w pamięci EEPROM o adresie 44 reprezentującą sumę kontrolną. Krok ten jest niezbędny,
ponieważ sama zmiana ID nie wystarcza do zapewnienia serwu poprawnego funkcjonowania. Fragment kodu 4.1 obrazuje przykład takiej sekwencji komunikacji pozwalającej
na nadanie poprawnego adresu ID.
40
1
2
3
4
5
6
7
8
4. Oprogramowanie
SEND
: 0x80 0xE1
RECEIVE : 0x80 0xE1
SEND
: 0x80 0xE2
RECEIVE : 0x80 0xE2
SEND
: 0x80 0xE1
checksum
RECEIVE : 0x80 0xE1
SEND
: 0x80 0xE2
checksum to 0x98
RECEIVE : 0x80 0xE2
0x29
0x29
0x29
0x29
0x44
0x00
0x00
0x0A
0x0A
0x00
0x8A
0x8A
0x95
0x95
0xA6
0x00
0x00
0x00
0x03
0x00
0x00
0x03
0x00
0x03
0x00
// Get ID
// ID 0
// Set ID to 10
// ACK
// Get servo EEPROM
0x44 0x00 0xA6 0xA2 0x03 // Servo checksum A2
0x44 0x98 0x3e 0x00 0x00 // Set servo EEPROM
0x44 0x98 0x3e 0x03 0x03 // ACK
Fragment kodu 4.1 Zmiana ID serwomechanizmu
Oprócz rozróżniania serw poprzez ID niezbędne jest ustawienie odpowiedniej pozycji bazowej silnika, dzięki czemu wszystkie serwa po zadaniu jednakowego sterowania
będą osiągały identyczną pozycję. Ustawienia fabrycznie nowych serwomechanizmów
mogą nieznacznie różnić się od siebie, dlatego warto programowo skorygować pozycję, tak by minimalizować różnice między ustawieniem poszczególnych mechanizmów
w trakcie ich pracy. Pozycja początkowa w pamięci mikrokontrolera serwomechanizmu
jest zapisana w postaci liczby typu int16_t z zakresu (−1500, 1500). Adresy komórek
pamięci przechowujących te dane to 7 oraz 8. Aby poprawnie zmienić pozycję początkową należy postąpić analogicznie do ustawiania ID, odczytać stare wartości, zmienić
na nowe, odczytać sumę kontrolną oraz zmienić ją o ich różnicę. Obrazuje to fragment
kodu 4.2 dla nowej pozycji zerowej przesuniętej o 300.
1
2
3
4
5
6
7
8
9
10
11
12
SEND
: 0x80 0xE1 0x07 0x00
position
RECEIVE : 0x80 0xE1 0x07 0x00
SEND
: 0x80 0xE1 0x08 0x00
position
SEND
: 0x80 0xE2 0x07 0x01
position to 0x01h
SEND
: 0x80 0xE2 0x08 0x2C
position 0x2Ch
RECEIVE : 0x80 0xE2 0x08 0x2C
SEND
: 0x80 0xE1 0x44 0x00
checksum
SEND
: 0x80 0xE2 0x44 0xCE
checksum to (0 xA2+ 0x12C) %
RECEIVE : 0x80 0xE2 0x44 0xCE
0x68 0x00 0x00 // Get higher byte of
0x68 0x00 0x03 // Higher byte 0
0x69 0x00 0x00 // Get lower byte of
0x69 0x00 0x03 // Lower byte 0
0x6A 0x00 0x00 // Set higher byte of
0x6A 0x03 0x03 // ACK
0x96 0x00 0x00 // Set lower byte of
0x06 0x03 0x03 // ACK
0xA6 0x00 0x00 // Get servo EEPROM
0xA6 0xA2 0x03 // Servo checksum A2
0x74 0x00 0x00 // Set servo EEPROM
0x100 = 0xCE
0x74 0x03 0x03 // ACK
Fragment kodu 4.2 Ustawienie pozycji bazowej serwomechanizmu
4.3. Oprogramowanie mikrokontrolera
4.2.2
41
Moduł Bluetooth
W projekcie wykorzystany został moduł KAmodBTM222 [28]. Należy go odpowiednio
skonfigurować do ustawień zgodnych z wymaganiami ramki Profibus oraz aby uprościć nawiązywanie komunikacji między urządzeniami. W tym celu podłączono moduł
do komputera poprzez port szeregowy oraz przy wykorzystaniu programu Hercules [2]
nawiązano z nim połączenie z modułem oraz dokonano inicjalizacji przy użyciu następujących komend:
• ATN=Hexapod_Mgr — ustawienie nazwy urządzenia,
• ATR1 — tryb Slave,
• ATC0 — wyłączenie sprzętowej kontroli przepływu (linii CTS/RTS),
• ATM0 — brak bitu kontroli parzystości,
• ATK0 — 1 bit stopu,
• ATP=1234 — ustawienie pinu na 1234,
• ATH1 — ustawienie trybu urządzenia jako widocznego dla innych,
• ATE0 - wyłączenie echa komend AT
• ATQ1 - wyłączenie informacji zwrotnych z modułu
• ATL9 — zmiana szybkości transmisji na 115200.
Tak skonfigurowane urządzenie przy ponownym włączeniu do prądu jest gotowe do
komunikacji z wykorzystaniem ramki Profibus.
4.3 Oprogramowanie mikrokontrolera
Oprogramowanie systemu umieszczonego na maszynie jest jednym z największych wyzwań podczas tworzenia robota kroczącego. Należy zapewnić obsługę niezbędnych
peryferiów, komunikację między robotem a otoczeniem, algorytmy związane z poruszaniem się maszyny oraz przetwarzaniem danych uzyskiwanych z sensorów. Najważniejsze w tego typu przedsięwzięciu jest zamknięcie całego oprogramowania w ramy
czasu rzeczywistego. Z tego powodu postanowiono wykorzystać system czasu rzeczywistego FreeRTOS [1].
Zdecydowano się na wybór tego systemu czasu rzeczywistego z powodu statycznej
architektury systemu i sposobu planowania procesów za pomocą strategii planowania
priorytetowego z wywłaszczaniem [23]. Co więcej system ten jest jednym z najbardziej popularnych rozwiązań, co powoduje, iż łatwo znaleźć pomoc lub rozwiazanie
napotkanych problemów. Ponadto wiele projektów tworzonych jest z myślą o systemie FreeRTOS, przez co dostępnych jest wiele bibliotek wspierających programistę
w trakcie tworzenia projektu. Posiada on wiele wbudowanych funkcji bazowych takich jak: semafory, kolejki, blokady, liczniki oraz zadania. Niezależność tego systemu
42
4. Oprogramowanie
SERVOlCONTROL
gSETlpositionlandlservoSpeed
gGETlposition
speed
UARTl(gPlservos
UARTlWg()lservos
UARTl(4g(xlservos
float[(x]lpositionINlllluintx_tlspeedINllllfloat[(x]lpositionOUT
position
llllllLEGlSENSORlTASK
gGETlinformationlwhetherllegl
lllislsetlonlthelgroundl3EXTIB
velocityylgaitylcurvature
maxDistance
minDistance
headldirection
gsendlack
floatlvelocityOUTlllllllllllllllllllfloatlcurvatureOUT
uintx_tlgaitOUT
MAINlTASK
bool[P]llegIsTouchingGround
BLUETOOTHlCOMMUNICATION
USINGlUART
greceiveldatalfromlapplication
l
orientation
HEADlCONTROL
lllllORIENTATIONlTASK
gGETldatalfromlgyroscopey
lllaccelerometerycompassl3I)CB
gcalculatelorientationlusinglAHRS
float[Q]lorientationQOUT
gdirectionlcontrol
gdistancelmeasurement
ADCldistance
measurement
PWMlgl)lservosl
control
floatlmaxDistOUTllllfloatlminDistOUTlllluint(P_t[)]lservoPosINllll
Rysunek 4.5 Schemat systemu znajdującego się na mikrokontrolerze
od mikrokontrolera umożliwia również proste łączenie kodu generowanego przez różne
programy, jak na przykład pakiet Embedded Coder z środowiska Matlab. Takie rozwiązanie daje możliwość rozwijania projektu oraz tworzenia algorytmów dla systemu
z wykorzystaniem modeli skonstruowanych w Simulink środowiska Matlab.
Stworzenie odpowiedniej architektury dla systemu jest niezbędne do poprawnego
oraz szybkiego funkcjonowania robota z wykorzystaniem systemu czasu rzeczywistego [33, 16]. Większość zadań wykonywanych w systemie jest niezależna od pozostałych
elementów systemu. Z tego powodu zostały one podzielone na zadania (tasks) systemu
stanowiące autonomiczne części udostępniające jedynie informacje w postaci zmiennych
globalnych widocznych dla pozostałych elementów systemu, które chce się do nich odwołać. Dostęp do zmiennych jest zależny od charakteru zadań. Gdy zadanie generujące
wartości danej zmiennej jest przerwaniem o wysokim priorytecie (np. zdobywanie informacji o styczności nogi z podłożem) dostęp jest wolny. Natomiast w przypadku gdy
istnieje ryzyko, iż zmienne są w tle przetwarzane (np. dane o orientacji robota), dostęp
do zmiennych może być uzyskiwany tylko z wykorzystaniem zabezpieczeń komunikacji
między wątkami w postaci mutexów lub semaforów. Schemat architektury systemu
widoczny jest na rysunku 4.5. Zmienne opisane w poszczególnych zadaniach są globalnie dostępne oraz zabezpieczone przed błędami związanymi z dostępem do nich.
Nowe zadania dodawane w przyszłości mogą z nich korzystać bez przeszkód. Zadania
związane z obsługą serwomechanizmów zostały połączone w jedną grupę, ponieważ
realizują te same zadania dla różnych silników. Obsługa głowy robota polega na ustawianiu pozycji 2 serw sterujących nią oraz odbieraniu informacji na temat maksymalnej
43
i minimalnej odległości od czujnika w trakcie pomiarów (100 ms). W kolejnych podrozdziałach przedstawiono w bardziej szczegółowy sposób część zadań wykonywanych
przez system.
4.3.1
Komunikacja z serwomechanizmami
Jednym z głównych zadań stawianych przed systemem jest sterowanie osiemnastoma
serwomechanizmami. Serwomechanizm reaguje na komendę zmiany pozycji, podobnie
jak standardowe serwo co 20ms, dlatego należy przesyłać zadane sterowanie w tych
ramach czasowych. Sterowanie składa się z dwóch komend: zadawania położenia oraz
ustawiania prędkości serwomechanizmu. Komenda sterująca prędkością jako odpowiedź zwraca aktualną pozycję danego serwa. Aby zmieścić się w ograniczeniu czasowym, wykorzystując jeden UART, można sterować maksymalnie sześcioma silnikami.
Z tego powodu serwa pogrupowano po 6 (2 nogi) i każda grupa posiada oddzielne
sterowanie po innym kanale UART. Aby ograniczyć wykorzystywanie jednostki obliczeniowej do przesyłu danych wykorzystywany jest moduł DMA (bezpośredni dostęp
do pamięci), co umożliwia wykonywanie innych zadań w trakcie przesyłu danych. Komunikacja inicjowana jest przez wysłanie pierwszej ramki danych (pozycji pierwszego
z sześciu serwomechanizmów obsługiwanych przez dany kanał), następnie po uzyskaniu
odpowiedzi DMA wystawia przerwanie od odbioru, przekazywana jest kolejna ramka
danych (ustawienie prędkości) do bufora pamięci oraz uwalniany jest semafor inicjujący kolejne wysyłanie danych. Proces ten powtarzany jest ciągle dla kolejnych serw.
Z odpowiedzi na ramkę związaną ze sterowaniem prędkością odczytywana jest wartość
aktualnego położenia.
4.3.2
Obsługa modułu MinIMU-9 [29]
Sensoryka robota pozwala na gromadzenie wielu danych dotyczących stanu robota.
Większość z nich jest istotna dla określania sposobu poruszania się robota w nierównym
terenie. Jednym z ważniejszych aspektów dotyczących tego zagadnienia jest określenie
orientacji robota względem ziemi. Do tego celu wykorzystano moduł MinIMU-9 [29],
który pozwala uzyskać informacje na temat przyspieszenia, prędkości kątowej oraz pola
magnetycznego.
Odpowiednie ustawienie czujników oraz zapewnienie poprawnego okresowego odczytu danych jest jednym z ważniejszych zadań układu związanych z sensorami. Do
komunikacji z czujnikami wykorzystano magistralę I 2 C z ustawioną prędkością 400kHz.
Do poprawnego zainicjowania komunikacji poprzez tą magistralę należy wywołać funkcję widoczną we fragmencie kodu 4.3.
1
void init_I2C1 (void) {
2
3
4
GPIO_InitTypeDef GPIO_InitStruct ;
I2C_InitTypeDef I2C_InitStruct ;
5
6
7
8
// enable APB1 peripheral clock for I2C1
RCC_APB1PeriphClockCmd ( RCC_APB1Periph_I2C1 , ENABLE );
RCC_APB1PeriphResetCmd ( RCC_APB1Periph_I2C1 , ENABLE );
44
4. Oprogramowanie
RCC_APB1PeriphResetCmd ( RCC_APB1Periph_I2C1 , DISABLE );
// enable clock for SCL and SDA pins
RCC_AHB1PeriphClockCmd ( RCC_AHB1Periph_GPIOB , ENABLE );
9
10
11
12
// setup SCL and SDA pins SCL on PB6 and SDA on PB7
GPIO_InitStruct . GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 ;
GPIO_InitStruct . GPIO_Mode = GPIO_Mode_AF ; // set pins to alternate
function
GPIO_InitStruct . GPIO_Speed = GPIO_Speed_100MHz ; // set GPIO speed
GPIO_InitStruct . GPIO_OType = GPIO_OType_OD ; // set output to output
drain
GPIO_InitStruct . GPIO_PuPd = GPIO_PuPd_UP ; // enable pull up
resistors
GPIO_Init (GPIOB , & GPIO_InitStruct ); // init GPIOB
13
14
15
16
17
18
19
20
// Connect I2C1 pins to AF
GPIO_PinAFConfig (GPIOB , GPIO_PinSource6 , GPIO_AF_I2C1 ); // SCL
GPIO_PinAFConfig (GPIOB , GPIO_PinSource7 , GPIO_AF_I2C1 ); // SDA
21
22
23
24
// configure I2C1
I2C_InitStruct . I2C_ClockSpeed = 400000; // 400 kHz
I2C_InitStruct . I2C_Mode = I2C_Mode_I2C ; // I2C mode
I2C_InitStruct . I2C_DutyCycle = I2C_DutyCycle_2 ; // 50% duty cycle
--> standard
I2C_InitStruct . I2C_OwnAddress1 = 0x00; // own address , not relevant
in master mode
I2C_InitStruct . I2C_Ack = I2C_Ack_Enable ; // enable acknowledge when
reading
I2C_InitStruct . I2C_AcknowledgedAddress =
I2C_AcknowledgedAddress_7bit ; // set address length to 7 bit
addresses
I2C_Init (I2C1 , & I2C_InitStruct ); // init I2C1
25
26
27
28
29
30
31
32
33
// enable I2C1
I2C_Cmd (I2C1 , ENABLE );
34
35
36
}
Fragment kodu 4.3 Inicjacja magistrali I 2 C
Fragment kodu (4.3) podłącza zegar do odpowiednich peryferiów związanych z I 2 C
oraz magistralą portu B, gdzie znajdują się piny wykorzystywane do komunikacji. Następnie ustawiany jest odpowiedni tryb dla pinów oraz konfigurowany jest moduł I 2 C.
Ustawiono częstotliwość transmisji 400 kHz, tryb Master, 7 bitowe adresy oraz wysyłanie ACK (potwierdzenie).
Kolejnym krokiem wykonanym podczas realizacji odczytu danych jest odpowiednie ustawienie sensorów zgodnie z notami katalogowymi. L3G4200D ustawiono tak,
aby uzyskać częstotliwość próbkowania równą 800Hz oraz otrzymywać dane z zakresu
±1000o /s. Podobnie ustawiono LSM303DLH na częstotliwość próbkowania równą
1000Hz oraz zakres ±4g/s. W obu czujnikach ustawiono tryb wysyłania danych w postaci Little Endian.
Dane odbierane są przez mikrokontroler w przerwaniu od zegara. Częstotliwość
przerwań to 800Hz. Aby poprawić jakość uzyskiwanych pomiarów dokonywane jest
45
filtrowanie górnoprzepustowe usuwające zbyt małe wartości próbek, które według not
katalogowych można uznać za szumy. Kolejnym usprawnieniem jest uśrednianie 16
kolejnych próbek, aby zminimalizować wpływ błędów grubych (spowodowanych poruszeniem kablami lub innymi losowymi zdarzeniami). Dodatkowo dla żyroskopu przed
filtrowaniem dokonywana jest jeszcze kalibracja zera, czyli od pomiarów odejmowana
jest wartość początkowa (uśredniona wartość z 4096 pomiarów w stanie nieruchomym).
Ostatnim etapem obróbki pomiarów jest przeliczenie wartości liczbowych na rzeczywiste wartości fizyczne.
Oprócz obróbki niezbędna jest fuzja danych, tak aby wykorzystując pomiary przyspieszenia, prędkości kątowej oraz pola magnetycznego, uzyskiwać informację na temat
orientacji robota względem układu odniesienia związanego z podłożem. Z powodu,
iż problem ten nie jest głównym aspektem pracy autor zdecydował się na wykorzystanie istniejących rozwiązań. W projekcie tym wykorzystano filtr MARG oraz jego
implementację opisaną w pracy [18]. Wykorzystana implementacja wymaga od użytkownika jedynie dobrania stałych dla członów proporcjonalnego i całkującego, tak by
uzyskiwane rozwiązania były odpowiednio filtrowane w zależności od warunków pracy
modułu. Uzyskiwane wartości orientacji zapisywane są w postaci kwaternionów [9].
4.3.3
Odczyt danych z dalmierza IR
Jednym z założeń sterowania robotem jest założenie, iż układ współrzędnych związany
z robotem porusza się zawsze stycznie do krzywizny ruchu. Z tego względu ważnym
aspektem jest posiadanie czujników z przodu konstrukcji umożliwiających wykrywanie
przeszkód i zmian w terenie. Do tego celu w projektowanej konstrukcji robota wykorzystywany jest dalmierz IR Sharp GP2Y0A21YKOF [32] o zakresie pracy (4, 30) cm.
Jest to sensor analogowy, dlatego do jego obsługi wykorzystywany jest przetwornik
ADC3 z wsparciem DMA (podobnie jak w przypadku komunikacji z serwomechanizmami). Z powodu, że charakterystyka zależności odległości od napięcia tego czujnika
przedstawiona w dokumentacji jest niedokładna niezbędna jest jej identyfikacja. Do
uzyskania rzeczywistych wartości odległości od czujnika wykonano serię pomiarów. Ponieważ charakterystyka zależności odległości od napięcia jest nieliniowa, postanowiono
dokonać jej interpolacji (rysunek 4.6) wykorzystując wcześniej uzyskane i uśrednione
pomiary. System robota ma postawione wiele zadań, które należy wykonać w danej
jednostce czasu, dlatego postanowiono stablicować uzyskane w procesie interpolacji
wartości co 10mV i dokonywać jedynie przybliżenia prostą na małym odcinku pomiędzy
wyznaczonymi wartościami. Ten sposób przybliżenia przy takiej liczbie punktów jest
wystarczająco dokładny, natomiast zysk czasowy wynikający z odczytywania wartości
z tablicy, zamiast obliczania wartości według skomplikowanego wzoru matematycznego
jest znaczny.
4.3.4
Przerwania od czujników dotykowych
Najbardziej podstawową informacją wykorzystywaną przy nawigacji w nierównym terenie jest informacja o tym, czy dana noga jest postawiona na podłożu, czy porusza się
ponad powierzchnią, po której wykonywany jest ruch robota. Do uzyskiwania tej informacji wykorzystywane są czujniki rozwierne zamontowane na końcówkach nóg. Ich
46
4. Oprogramowanie
Rysunek 4.6 Interpolacja pomiarów uzyskiwanych z dalmierza IR wraz z naniesionymi
punktami pomiarowymi
obsługa przez mikrokontroler jest oprogramowana w prosty sposób. Ponieważ zmiana
stanu czujnika występuje stosunkowo rzadko, a informacja ta powinna jak najszybciej
trafić do systemu, postanowiono obsługiwać zdarzenia te w postaci przerwań o wysokim
priorytecie. W ten sposób informacje na temat stanu poszczególnych nóg zapisywane
są w zmiennych globalnych w sposób niezależny od całego systemu oraz ich zmiana nie
powoduje problemów związanych z dostępem do pamięci przez więcej niż jedno zadanie w danym czasie. Przyciski podłączone są do wyjść procesora na różnych liniach
przez to przerwanie dla każdej nogi ma bardzo krótki czas realizacji. Przykład obsługi
przerwania widoczny jest we fragmencie kodu 4.4.
1
2
3
4
5
6
7
8
void EXTI4_IRQHandler (void)
{
if( EXTI_GetITStatus ( EXTI_Line4 ) != RESET )
{
h_push_leg_RF = (GPIOE ->IDR & 0x10) == 0 ? 0 : 1;
EXTI_ClearITPendingBit ( EXTI_Line4 );
}
}
Fragment kodu 4.4 Przerwanie od zewnętrznego zdarzenia na linii 4
4.3.5
Główne zadanie systemu — sterowanie
Głównym zadaniem systemu jest realizacja sterowania robotem kroczącym implementując wybrane algorytmy. Przyjęte rozwiązania w sposób hierarchiczny oddziaływają
SensorSdata
47
UnevenSterrainSnavigation
Velocity
Curvature
Gait
Velocity&CurvatureScontrol
Gait&PhaseScontroller
Toda-RayleighSlatticeS
DampingSoscillator
InverseSkinematics servoSvalues
Rysunek 4.7 Schemat warstw sterowania robotem kroczącym
na siebie, z tego powodu zadanie sterowania postanowiono zaimplementować z wykorzystaniem rozwiązań warstwowych [10]. Schemat obrazujący działanie tego zadania
widoczny jest na rysunku 4.7.
Najniższą warstwą jest warstwa generująca trajektorie ruchu dla poszczególnych
nóg z wykorzystaniem kraty Tody-Rayleigha lub oscylatorów tłumionych. Ich implementacja wymaga numerycznego całkowania kolejnych kroków w czasie. Częstotliwość
całkowania została ustawiona na 1kHz. Dzięki temu prosta metoda całkowania metodą
prostokątów jest wystarczająca, ponieważ zapewnia większą dokładność, niż rozdzielczość ustawiania pozycji nogi.
Kolejną warstwą jest warstwa decyzyjna, ustawiająca odpowiednie parametry związane z trybem chodu oraz decydująca o przejściu między kratą a oscylatorem, w zależności od informacji uzyskiwanych z warstwy wyższej.
Następną warstwą jest warstwa sterująca. Otrzymuje ona informacje odnośnie sterowania z wykorzystaniem krzywizny i prędkości. Posiadając te informacje warstwa ta
modyfikuje w odpowiedni sposób mnożniki dla odpowiednich wartości uzyskiwanych
z najniższej warstwy, w taki sposób, by robot podążał po wyznaczonej trajektorii.
Najwyższa warstwa wykorzystuje informacje z otoczenia do poruszania się w nierównym terenie. Jej zadaniem jest określanie sposobu sterowania robotem uwzględniającego informacje sensoryczne uzyskiwane przez robota w trakcie pracy.
Rozdział 5
Badania
Rozdział ten poświęcony jest testowaniu oraz określaniu przydatności rozwiązań wykorzystanych podczas konstrukcji oraz sterowania robota kroczącego. Niezbędne jest
stwierdzenie, czy dane rozwiązania spełniają swoje funkcje oraz czy uzasadnione jest
wykorzystywanie ich w innych projektach realizujących podobne zadania. Pierwszym
etapem badań są symulacje zachowania kraty Tody-Rayleigha opisane w podrozdziale 5.1. Następnie w podrozdziale 5.2 badane jest zachowanie rzeczywistego robota
w testowym środowisku.
5.1 Symulacje w środowisku Simulink
W celu przeprowadzenia badań stworzono w środowisku Simulink odpowiednie modele
(dostępne w dodatku A na płycie CD) implementujące działanie poszczególnych części
systemu. Pierwszym elementem wykonanym było zamodelowanie połączenia dwóch
najniższych warstw czyli kraty Tody-Rayleigha oraz warstwy decyzyjnej, a następnie przebadanie działania modelu w celu dobrania odpowiednich parametrów modelu
oraz wyznaczenia wartości początkowych do efektywnego rozpoczynania pracy różnych
rodzajów chodu w fazie rozruchu robota. Schemat finalnego modelu (bez bloków wizualizacji) do testów przedstawiono na rysunku 5.1.
5.1.1
Badania najniższej warstwy systemu
Pierwszym zadaniem było zbadanie poprawności funkcjonowania kraty Tody-Rayleiha.
W tym celu niezbędne jest określenie wartości współczynników ω02 , ωR2 oraz µ z wzoru (2.16). Uzyskiwana trajektoria dla wybranych parametrów powinna być ograniczona
zakresem wartości (−5, 5), natomiast amplitudy na osi Z winny być odpowiednio duże
by robot mógł swobodnie kroczyć. Po dokonaniu serii testów ustalono zadane parametry na wartości:
ω02 = 3.3,
ωR2 = 1.8,
µ = 4.
(5.1)
Kolejnym etap prac było ustalenie odpowiednich współrzędnych początkowych umożliwiających inicjowanie następnych kroków oraz spełniających założenie, że współ-
50
5. Badania
Rysunek 5.1 Schemat dwóch najniższych warstw systemu sterowania w środowisku
Simulink
rzędne osi Y dla każdej z nóg są równe 0 by nie występowało zjawisko ”szurania”
nogi o podłoże.
Chód trójpodporowy
Podczas poszukiwania rozwiązań natrafiono na prawidłowość polegające na tym, że
ustawiając przeciwne wartości współrzędnych dla nóg 1,3,5 oraz 2,4,6 (zgodnie z numeracją z rysunku 2.11) zwykle można uzyskać chód trójpodporowy. Dodatkowo przeprowadzono serię testów dla różnych wartości współrzędnych w celu uzyskania płynnej
fazy rozruchu wprowadzającej jak najmniej zaburzeń. Wyniki badań przedstawiono na
rysunku 5.2. Można z nich wnioskować, iż niezależnie od współrzędnych początkowych
Z0 układ stabilizuje się na tym samym rozwiązaniu. Widać również, że wartości powyżej pewnej granicy (rysunki 5.2(a) i 5.2(b)) od razu zostają ograniczone. Ponieważ
zadaniem robota jest naśladowanie poruszania się insektów, rozpoczęcie ruchu musi
być łagodne i dopiero kolejne fazy (kroki) powinny osiągać maksymalną amplitudę.
Dodatkowo warto wystrzegać się przesterowań dlatego zdecydowano się na warunki
początkowe Z0 = 10 · (1, −1, 1, −1, 1, −1)T . Dla takiego rozwiązania ślad nogi poruszającej się po linii prostej obrazuje rysunek 5.3.
Chód gąsienicowy
Podobnie jak w przypadku chodu trójpodporowego najprostszym sposobem znalezienia chodu czteropodporowego jest ustawienie wartości początkowych w sposób taki,
5.1. Symulacje w środowisku Simulink
51
0.5
2.5
X1 , X3 , X5
X2 , X4 , X6
0.4
0.3
1.5
1
Coordinate Z [cm]
0.2
Coordinate X [cm]
Z1 , Z3 , Z5
Z2 , Z4 , Z6
2
0.1
0
−0.1
0.5
0
−0.5
−0.2
−1
−0.3
−1.5
−0.4
−2
−0.5
−2.5
0
1
2
3
4
5
0
1
2
Time t [s]
3
4
5
Time t [s]
(a) X = f (t) dla Z0 = 10 · (1, −1, 1, −1, 1, −1)T (b) Z = f (t) dla Z0 = 10 · (1, −1, 1, −1, 1, −1)T
0.6
4
X1 , X3 , X5
X2 , X4 , X6
Z1 , Z3 , Z5
Z2 , Z4 , Z6
3
0.4
2
Coordinate Z [cm]
Coordinate X [cm]
0.2
0
−0.2
−0.4
0
−1
−2
−0.6
−0.8
1
−3
0
1
2
3
4
−4
5
0
1
2
Time t [s]
(c) X = f (t) dla Z0 = (5, −5, 5, −5, 5, −5)T
4
5
(d) Z = f (t) dla Z0 = (5, −5, 5, −5, 5, −5)T
0.5
2.5
X1 , X3 , X5
X2 , X4 , X6
0.4
Z1 , Z3 , Z5
Z2 , Z4 , Z6
2
0.3
1.5
1
Coordinate Z [cm]
0.2
Coordinate X [cm]
3
Time t [s]
0.1
0
−0.1
0.5
0
−0.5
−0.2
−1
−0.3
−1.5
−0.4
−2
−0.5
−2.5
0
1
2
3
4
5
Time t [s]
(e) X = f (t) dla Z0 = ( 12 , − 12 , 12 , − 12 , 12 , − 12 )T
0
1
2
3
4
5
Time t [s]
(f) Z = f (t) dla Z0 = ( 12 , − 12 , 21 , − 12 , 12 , − 21 )T
Rysunek 5.2 Chód trójpodporowy — rozruch kraty Tody-Rayleigha
52
5. Badania
2.5
2
1.5
Coordinate Z [cm]
1
0.5
0
−0.5
−1
−1.5
−2
−2.5
−0.5
0
Coordinate X [cm]
0.5
Rysunek 5.3 Ślad nogi dla wybranych wartości początkowych chodu trójpodporowego
z fazą rozruchu
by pogrupować nogi w pary: (1,4), (2,5) oraz (3,6), a następnie zadawać różne współrzędne początkowe osi Z kolejnym parom. Podczas prowadzenia badań zauważono
jednak, iż znacznie trudniej uzyskać cykl odpowiadający temu sposobowi kroczenia.
Na rysunku 5.4 przedstawiono wyniki badań dla chodu gąsienicowego. Obserwacja zachowania układu pokazuje, iż różnice między wartościami kolejnych par nie mogą być
zbyt duże oraz jedna para musi posiadać przeciwny znak w stosunku do pozostałych.
Gdy niespełniane są te założenia układ stabilizuje się na innym rozwiązaniu cyklicznym, który nie reprezentuje znanego chodu insektów. Rysunek 5.4 pokazuje również,
że przy niewielkich różnicach warunków początkowych uzyskiwane są jakościowo różne
rozwiązania. Układ może tworzyć przesterowania, które utrudniałyby rzeczywistemu
obiektowi poprawne rozpoczęcie ruchu (rysunki 5.4(e) i 5.4(f)). Z tego powodu wybrano do fazy rozruchu chodu czteropodporowego współrzędne początkowe dla osi Z
równe Z0 = (1.4, −0.7, 0.2, 1.4, −0.7, 0.2)T . Ślad ruchu pojedynczej kończyny robota
dla tych wartości obrazuje rysunek 5.5.
Chód pełzający
Chód pełzający jest najtrudniejszym do uzyskania sposobem poruszania się robota
przy pomocy kraty Tody-Rayleigha. Podczas eksperymentów z kratą nie udało się autorowi pracy znaleźć rozwiązania cyklicznego odpowiadającego temu rodzajowi chodu
spełniającego założenie X0 = 0. Zmiany wartości początkowych dla współrzędnej Z
nie pozwalały na uzyskanie odpowiednich wyników, dlatego zdecydowano się na próby
odszukania rozwiązań cyklicznych modyfikując współrzędne początkowe osi X przy
Z0 = 0. Po dokonaniu serii prób uzyskano rozwiązania widoczne na rysunku 5.6.
Warunki początkowe mają podobny wpływ na fazę rozruchu, jak w przypadku chodu
gąsienicowego. Oddziaływają one na wielkość przesterowań oraz długość fazy rozpo-
53
1.2
3
1
2
Coordinate Z [cm]
Coordinate X [cm]
0.8
0.6
0.4
0.2
1
0
−1
0
−0.4
−2
X1 , X4
X2 , X5
X3 , X6
−0.2
0
2
4
6
8
−3
10
Z1 , Z4
Z2 , Z5
Z3 , Z6
0
2
4
Time t [s]
(a) X
=
f (t)
dla
(1.4, −0.7, 0.2, 1.4, −0.7, 0.2)T
8
= (b) Z
=
f (t)
dla
(1.4, −0.7, 0.2, 1.4, −0.7, 0.2)T
Z0
1.6
10
Z0
=
3
X1 , X4
X2 , X5
X3 , X6
1.4
Z1 , Z4
Z2 , Z5
Z3 , Z6
2
1.2
Coordinate Z [cm]
1
Coordinate X [cm]
6
Time t [s]
0.8
0.6
0.4
0.2
0
1
0
−1
−2
−0.2
−0.4
0
2
4
6
8
−3
10
0
2
4
Time t [s]
6
8
(c) X
=
f (t)
(2, −0.7, 1.2, 2, −0.7, 1.2)T
dla
= (d) Z
=
f (t)
(2, −0.7, 1.2, 2, −0.7, 1.2)T
Z0
1.5
dla
Z0
Z1 , Z4
Z2 , Z5
Z3 , Z6
1
2
0.5
1
Coordinate Z [cm]
Coordinate X [cm]
=
3
X1 , X4
X2 , X5
X3 , X6
0
−0.5
−1
−1.5
10
Time t [s]
0
−1
−2
0
2
4
6
8
10
Time t [s]
(e) X
=
f (t)
dla
(−2, 0.8, −0.5, −2, 0.8, −0.5)T
−3
0
2
4
6
8
10
Time t [s]
Z0
= (f) Z
=
f (t)
dla
(−2, 0.8, −0.5, −2, 0.8, −0.5)T
Z0
Rysunek 5.4 Chód czteropodporowy — rozruch kraty Tody-Rayleigha
=
54
5. Badania
3
Coordinate Z [cm]
2
1
0
−1
−2
−3
−0.2
0
0.2
0.4
0.6
Coordinate X [cm]
0.8
1
1.2
Rysunek 5.5 Ślad nogi dla wybranych wartości początkowych chodu czteropodporowego z fazą rozruchu
czynającej kroczenie robota. Wadą rozwiązań uzyskanych podczas testów jest fakt,
iż punkt startowy leży poza cyklicznym rozwiązaniem. Fakt ten widoczny jest na
rysunku 5.7 obrazującym ślad ruchu dla współrzędnych X0 = (−1, 1, −1, 1, 0.4, 2)T .
Z powodu znacznego ”szurania” występującego pomiędzy nogą fizycznego obiektu
a podłożem wynikającego z X0 ̸= 0 rozwiązanie to nie jest zaimplementowane w rzeczywistym obiekcie.
Zatrzymywanie robota
Gdy robot dostaje komendę zatrzymania włączany jest oscylator tłumiony, którego
zadaniem jest zatrzymanie w płynny sposób kończyny robota na zadanej wartości koń2
cowej (Xk , 0). Głównym zadaniem jest dobranie odpowiedniego współczynnika ωosc
występującego w równaniu oscylatora (2.12). Załóżmy, że współczynnik ten powinien
zapewnić stabilizację układu na zadanej wartości w czasie około 3 sekund. Zdecydowano się na taki okres fazy zatrzymywania z powodu, iż każdy żywy organizm potrzebuje trochę czasu na wyhamowanie i zatrzymanie się. Gdyby konstrukcja robota
zatrzymywała się szybciej, wyglądałoby to nienaturalnie, a jednym z celów pracy jest
naśladowanie zachowań żywego organizmu. Podczas testów dokonano też obserwacji,
iż aby uzyskać oczekiwaną wartość współrzędnej Xk przy zakończeniu ruchu niezbędne
2
. W przeciwnym wypadku
jest przemnożenie tej wartości przez współczynnik −ωosc
układ stabilizuje się, na innej wartości, niż zadana. Po serii testów stwierdzono, że
2
= 10 daje satysfakcjonujące rezultaty fazy zatrzymywania dla różnych rodzajów
ωosc
chodu.
55
3.5
3
1
2.5
0.5
2
Coordinate Z [cm]
Coordinate X [cm]
2
1.5
0
−0.5
−1
1.5
1
0.5
−1.5
0
−2
−0.5
−2.5
−1
−3
−1.5
0
5
X1
X2
10
Time t [s]
X3
X4
15
X5
X6
0
5
Z1
Z2
10
Time t [s]
Z3
Z4
15
Z5
Z6
3
4
2
3
1
2
Coordinate Z [cm]
Coordinate X [cm]
(a) X = f (t) dla X0 = (1.25, −1.25, −0.5, 0, 0)T (b) Z = f (t) dla X0 = (1.25, −1.25, −0.5, 0, 0)T
0
−1
−2
−3
1
0
−1
0
5
X1
X2
10
Time t [s]
X3
X4
−2
15
X5
0
X6
5
Z1
Z2
10
Time t [s]
Z3
Z4
15
Z5
Z6
(c) X = f (t) dla X0 = (2, −1.25, −0.5, 1, 0, 0)T (d) Z = f (t) dla X0 = (2, −1.25, −0.5, 1, 0, 0)T
4
4
3
3
Coordinate Z [cm]
Coordinate X [cm]
2
1
0
2
1
0
−1
−1
−2
−3
0
5
X1
X2
10
Time t [s]
X3
X4
15
X5
X6
(e) X = f (t) dla X0 = (−1, 1, −2, 1, 0.4, 2)T
−2
0
5
Z1
Z2
10
Time t [s]
Z3
Z4
15
Z5
Z6
(f) Z = f (t) dla X0 = (−1, 1, −2, 1, 0.4, 2)T
Rysunek 5.6 Chód pięciopodporowy — rozruch kraty Tody-Rayleigha
56
5. Badania
4
Coordinate Z [cm]
3
2
1
0
−1
−2
−3
−2
−1
0
Coordinate X [cm]
1
2
Rysunek 5.7 Ślad nogi dla wybranych wartości początkowych chodu pięciopodporowego
z fazą rozruchu
5.1.2
Warstwa zmian trybu chodu
Gdy stwierdzono, że najniższa warstwa działa poprawnie kolejnym etapem było sprawdzenie funkcjonowania warstwy decydującej o trybie kroczenia. Jedną z zalet wykorzystania do generacji trajektorii poszczególnych nóg robota kraty Tody-Rayleigha jest
fakt, iż umożliwia ona płynne przechodzenie między różnymi rodzajami chodu. Przejście takie wymaga, zastosowania odpowiedniej sekwencji (opisanej dokładniej w podrozdziale 2.3).
Postanowiono przeprowadzić testy działania zmiany trybu chodu. Rysunek 5.8
przedstawia wizualizację takiego przejścia zarówno dla wartości poszczególnych współrzędnych X oraz Z, jak i śladu pojedynczej nogi robota. Test polegał na rozpoczęciu
ruchu trójpodporowego oraz kroczenia w fazie właściwej przez okres 7 sekund, następnie zasymulowano zdarzenie zmiany sposobu kroczenia, co spowodowało wymuszenie
fazy zatrzymywania trwającej około 3 sekundy oraz następnie rozpoczęcie rozruchu
dla chodu czteropodporowego przez następne 5 sekund. Podobny test przeprowadzono
dla odwrotnej sytuacji przejścia z fazy właściwej chodu czteropodporowego do chodu
trójpodporowego. Jego rezultaty widoczne są na rysunku 5.9.
Porównując oba przejścia widać, że zmiana trybu chodu przechodzi poprawnie oraz
jest ona odwracalna. Widać również, że różnice występują w fazie zatrzymywania
w zależności od momentu, w którym nastąpi zmiana trybu kroczenia.
5.2
Badania rzeczywistego obiektu
Ważnym etapem prac nad projektem jest badanie funkcjonowania rzeczywistej konstrukcji robota. Pozwala ono określić przydatność rozwiązań zastosowanych w trakcie
5.2. Badania rzeczywistego obiektu
57
1.2
3
1
2
0.6
Coordinate Z [cm]
Coordinate X [cm]
0.8
0.4
0.2
0
1
0
−1
−0.2
−2
−0.4
−0.6
0
5
10
15
−3
0
5
10
Time t [s]
15
Time t [s]
(a) X = f (t)
(b) Z = f (t)
3
tripod gait
damping
quadropod gait
Coordinate Z [cm]
2
1
0
−1
−2
−3
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
Coordinate X [cm]
0.8
1
1.2
(c) Ślad końcówki nogi przy zmianie chodu
Rysunek 5.8 Przejście pomiędzy chodem trójpodporowym, a czteropodporowym
58
5. Badania
1.2
3
1
2
0.6
Coordinate Z [cm]
Coordinate X [cm]
0.8
0.4
0.2
0
1
0
−1
−0.2
−2
−0.4
−0.6
0
5
10
15
−3
0
5
10
Time t [s]
15
Time t [s]
(a) X = f (t)
(b) Z = f (t)
3
tripod gait
damping
quadropod gait
Coordinate Z [cm]
2
1
0
−1
−2
−3
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
Coordinate X [cm]
0.8
1
1.2
(c) Ślad końcówki nogi przy zmianie chodu
Rysunek 5.9 Przejście pomiędzy chodem czteropodporowym, a trójpodporowym
59
jego realizacji oraz dalsze możliwości rozwoju a także elementy, które należy zmodyfikować w celu usprawnienia działania. Badania swoim zakresem obejmowały: sprawdzenie i porównanie działania symulacji kraty Tody-Rayleigha a implementacji na robocie,
określenie poprawności działania sterowania z wykorzystaniem prędkości i krzywizny
oraz porównanie poruszania się robota w nierównym terenie sterowanego algorytmem
nieuwzględniającym i uwzględniającym sygnały z czujników dotykowych. Filmy z przeprowadzonych badań zostały zamieszczone w dodatku A na płycie CD.
5.2.1
Generator chodu
Pierwszym etapem testów wykonanego robota kroczącego było sprawdzenie poprawności funkcjonowania kraty Tody-Rayleigha. W tym celu przeprowadzono najpierw testy,
w których obserwowano zachowanie pojedynczej nogi robota podczas generowania trajektorii przez kratę. Na rysunku 5.10 przedstawiono zdjęcia poklatkowe trajektorii
poruszania się końcówki jednej z nóg robota. Porównując ślad ruchu końcówki nogi
na zdjęciach z naniesionym na nie śladem z symulacji w programie Simulink można
stwierdzić, iż działanie dyskretnej implementacji kraty Tody-Rayleigha dla pojedynczej kończyny jest poprawne.
Kolejnym testem było ustawienie chodu trójpodporowego oraz sprawdzenie kroczenia robota. Maszyna wykonywała ruchy w sposób prawidłowy zmieniając naprzemiennie pozycję dla trzech nóg. Dodatkowo stwierdzono, że konstrukcja robota porusza się
po linii prostej na odcinku około 10m. Pozwala to przyjąć, iż generowany w ten sposób
chód trójpodporowy jest poprawny oraz może być wykorzystywany do efektywnego
poruszania się robotem po płaskim podłożu.
Następnie przeprowadzono podobne testy dla ruchu gąsienicowego. Ich wynik potwierdził poprawność działania kraty Tody-Rayleigha również w tym zakresie. Ruch
ten zgodnie z teorią jest na granicy stabilności. W trakcie badań zauważono, że robot
ma tendencję do przechylania się w stronę kierunku ruchu. Powodem tego jest zbyt
wysoko umieszczony środek ciężkości konstrukcji, którego przemieszczenie w różnych
fazach ruchu powoduje przekroczenie granicy stabilności.
Podczas wykonanych testów dokonano jeszcze jednej ważnej obserwacji. Robot
jest bardzo czuły na zmianę chropowatości powierzchni, po której się porusza. Gdy
krocząc po płaszczyźnie natrafia na zmianę ośrodka na przykład przechodzi z parkietu
na dywan robot ma tendencję do zmiany kierunku ruchu. Zachowanie to może być
problematyczne gdy zadaniem robota będzie autonomiczne poruszanie się w terenie.
W takich wypadkach konstrukcją musi zostać wyposażona w możliwość rozpoznawania
takich sytuacji i niwelowania ich skutków.
5.2.2
Sterowanie z wykorzystaniem krzywizny i prędkości
Gdy stwierdzono, że podstawowe sposoby poruszania się robota kroczącego funkcjonują zgodnie z założeniami rozpoczęto badanie sterowania zadawanego przez operatora
z komputera.
Pierwszym etapem było sprawdzenie wpływu prędkości na maszynę. Aby sprawdzić poprawność działania ustawiono robota w pozycji początkowej, a następnie nakazywano mu chód trójpodporowy z zadaną prędkością przez czas 5 sekund. Gdy robot
5. Badania
60
Rysunek 5.10 Zdjęcia poklatkowe pokazujące trajektorię pojedynczej nogi
61
ID
Tryb chodu
Prędkość [ cm
] Odległość zmierzona [cm]
T
1
trójpodoporowy
1.0
8.75
2
trójpodoporowy
1.0
8.75
3
trójpodoporowy
1.0
9.00
4
trójpodoporowy
2.5
22.00
5
trójpodoporowy
2.5
21.50
6
trójpodoporowy
2.5
21.75
7
trójpodoporowy
5.0
43.50
8
trójpodoporowy
5.0
44.00
9
trójpodoporowy
5.0
44.25
10
czteropodporowy 1.0
6.50
11
czteropodporowy 1.0
6.25
12
czteropodporowy 1.0
6.50
13
czteropodporowy 2.5
16.25
14
czteropodporowy 2.5
16.75
15
czteropodporowy 2.5
17.00
16
czteropodporowy 5.0
33.00
17
czteropodporowy 5.0
34.00
18
czteropodporowy 5.0
34.25
Tabela. 5.1 Wyniki badań sterowania prędkością robota dla różnych trybów chodu
kończył wykonywanie ruchu mierzono jego odległość od punktu początkowego. Test
ten powtórzono trzykrotnie, aby zbadać powtarzalność ruchu. Gdy stwierdzono, że
ruch jest powtarzalny wykonano analogiczne testy dla różnych prędkości w celu sprawdzenia, zachowania proporcji ruchu. Testy potwierdziły poprawne funkcjonowanie sterowania za pomocą prędkości. Dwukrotne zwiększenie prędkości powoduje dwukrotne
zwiększenie pokonanego dystansu w zadanym czasie. Wyniki testów przedstawiono
w tabeli 5.1. Uzyskane dane pokazują również, iż niezbędne jest odpowiednie przeskalowanie współczynników długości kroku dla poszczególnych chodów w taki sposób, by
uzyskiwać jednakowe odległości przy danej prędkości dla każdego trybu chodu.
Następnie, sprawdzono poprawność wpływu krzywizny na sterowanie robotem. Do
tego celu przeprowadzono kilka testów. Pierwszym podstawowym testem było sprawdzenie, czy robot krocząc po zadanym promieniu ze stałą prędkością pokonuje w danych
odcinkach czasu te same odległości po okręgu o zadanym promieniu. Wyniki testów
przedstawiono na rysunku 5.11. Na ich podstawie można stwierdzić, że robot porusza się poprawnie w takich warunkach. Kolejnym testem był wpływ zmiany prędkości
na sposób kroczenia po zadanej stałej krzywiźnie. Testowano zachowanie to sprawdzając czy robot zmieniając prędkość podczas kroczenia po okręgu stałym promieniu
5. Badania
62
Rysunek 5.11 Zdjęcia poklatkowe pokazujące sposób kroczenia po okręgu
63
będzie w pewnym momencie ruchu przecinał punkt początkowy z odpowiednią orientacją. Badania te pokazały, że robot z każdym okrążeniem lekko zbacza wybranej trasy.
Przy pierwszym okrążeniu dla okręgu o promieniu jednego metra jest to praktycznie
niezauważalne. Jednak kolejne okrążenia pokazują, że maszyna zbacza coraz mocniej
z zadanej trasy. Zachowanie takie jest prawdopodobnie związane z niedokładnościami
wynikającymi z konstrukcji mechanicznej robota oraz błędami związanymi wpływem
współczynników wykorzystywanych do sterowania na dyskretną implementację karty
Tody-Rayleigha.
5.2.3
Nawigacja w nierównym terenie
Ostatnim etapem badań było porównanie zachowania rzeczywistego obiektu w nierównym terenie sterowanego algorytmami nieuwzględniającymi i uwzględniającymi informację z czujników dotykowych na końcach nóg robota. Do badań wykorzystano różne
otoczenia: płaski teren z pojedynczymi oraz wieloma przeszkodami o kształcie wielościanów, teren pokryty trawą oraz teren kamienisty. Filmy z testów przeprowadzonych
dostępne są w dodatku A na płycie CD.
Pierwszym porównaniem dokonanym w trakcie badań był wpływ uwzględniania
informacji z czujników dotykowych na trajektorię końca nogi stawianej na danej powierzchni. Na rysunku 5.12 przedstawiono wyniki testów zachowania robota stawiającego nogi na przeszkodach. Widać, że napotkanie przeszkody powoduje zatrzymanie
ruchu nogi w kierunku osi Z do momentu, w którym noga jest znów podnoszona.
Zmiana ta w stosunku do trajektorii generowanej z pominięciem informacji z czujników na końcach nóg robota (rysunek 5.10) jest wyraźna. Rozwiązanie wykorzystujące
sensory umożliwia lepszą kontrolę orientacji robota względem podłoża, dzięki czemu
ruch wykorzystujący to podejście wizualnie wygląda płynniej.
Kolejnym etapem testów było porównanie działania robota w przestrzeni płaskiej,
gdzie przeszkody to niewielkie wielościany rzadko występujące. Taki teren widoczny
jest na rysunku 5.13. Zarówno w przypadku, gdy robot poruszał się wykorzystując
informacje z czujników dotyku, jak i bez tych informacji, był wstanie pokonać bez
większych problemów tego typu przeszkody. Widoczna różnica następowała w orientacji robota, nie posiadając informacji robot miał tendencje do przechylania się na
boki. W trakcie testów zauważono również, że gdy jedna z kończyn maszyny ślizga się
przy wchodzeniu na przeszkodę, może powodować to zmianę kierunku ruchu robota.
Nie jest to większym problemem, gdy robot sterowany jest przez operatora. Jednak
w przypadku tworzenia konstrukcji do zadań związanych z autonomicznym przemieszczaniem się w danym środowisku, należy rozpoznawać takie sytuacje oraz korygować
kierunek ruchu.
Następnie przeprowadzono badania w płaskim terenie z dużą ilością zróżnicowanych przeszkód w kształcie wielościanów. Robota w trakcie pokonywania przeszkód
tego typu przedstawiono na rysunku 5.14. Poruszanie w tego typu terenie jest znacznie bardziej wymagające i robot bez wsparcia czujników nie potrafi pokonywać tego
typu przeszkód. Natomiast wykorzystując informacje z sensorów dotykowych robot pokonuje tego typu przeszkody. Spowodowane jest to dużymi zmianami w poziomie na
którym znajduje się część robota w danej chwili. Maszyna wchodząc po równi pochyłej
ma przód podniesiony do góry, natomiast tył robota skierowany jest w dół, ponieważ
5. Badania
64
Rysunek 5.12 Zdjęcia poklatkowe pokazujące wpływ korzystania z czujników dotyku na trajektorię końca nogi
65
Rysunek 5.13 Robot podczas pokonywania płaskiego terenu z niewielkimi przeszkodami
Rysunek 5.14 Robot w trakcie pokonywania płaskiego terenu ze zróżnicowanymi przeszkodami
66
5. Badania
Rysunek 5.15 Maszyna krocząca w trawiastym środowisku
nie ma żadnej kontroli orientacji robot przy wykroku podnosi przednie kończyny zbyt
wysoko i traci przyczepność stawiając je. To powoduje losowe zmiany kierunku ruchu,
a przy odpowiednio dużym nachyleniu podejścia zsuwanie się konstrukcji. Robot wykorzystujący czujniki przesuwa swój środek ciężkości w stronę przodu konstrukcji co
powoduje większą stabilność podczas pokonywania tego typu przeszkody i umożliwia
mu wejście na nią.
Oprócz poruszania się w sztucznych środowiskach z prosto zdefiniowanymi przeszkodami robot musi być wstanie poruszać się w naturalnym otoczeniu, dlatego kolejne
testy polegały na ocenie funkcjonowania robota w rzeczywistych warunkach. Pierwsze badania przeprowadzono na trawie (rysunek 5.15). Przestrzeń pracy tego typu
nie stanowi dużego wyzwania. Przeszkody są znacznie mniejsze od konstrukcji robota
i pokonywanie ich zarówna bez informacji z czujników jak również posiadając je jest łatwe. Jedyny problem, który konstrukcja robota może natrafić w takich warunkach jest
zahaczenie się o trawę i zablokowanie nogi. Jednakże rozwiązanie tego problemu jest
proste — wystarczy zabezpieczyć konstrukcję kończyn przed możliwością zaistnienia
takiej sytuacji poprzez założenie koszulek ochronnych lub oklejenie taśmą izolacyjną.
Najtrudniejszą przestrzenią pracy, w której wykonywano testy jest kamieniste podłoże. Robot chodzący po takiej powierzchni widoczny jest na rysunku 5.16. Kroczenie
po tego typu terenie jest możliwe przy wykorzystaniu czujników dotykowych na końcach nóg robota. Jednak podczas poruszania się w takim terenie trudne jest zachowanie
odpowiedniego kierunku ruchu przez robota, jako że często jego nogi ”ślizgają” się na
kamieniach. Bez odpowiedniej kontroli nóg wykorzystującej informację z czujników
robot ma tendencję do klinowania swoich kończyn pomiędzy kamieniami co powoduje
niemożliwość dalszego poruszania się. Lokomocja w bardziej zróżnicowanych środowiskach wymaga od robota uwzględniania większej ilości informacji o otoczeniu podczas
kroczenia.
Rysunek 5.16 Robot kroczący po kamienistym terenie
67
Rozdział 6
Podsumowanie
Główny cel pracy, czyli wykonanie sześcionożnego robota kroczącego posiadającego
umiejętność przemieszczania się w nierównym terenie, został osiągnięty. Zaprojektowano konstrukcję mechaniczną robota kroczącego oraz dobrano odpowiednie układy
elektroniczne i wykonano płytki PCB. Następnie oprogramowano robota oraz wykonano aplikację sterującą maszyną. Ostatnim etapem prac było przebadanie działania
zaproponowanej konstrukcji oraz algorytmów związanych z generacją chodu, sterowaniem z wykorzystaniem krzywizny i prędkości oraz poruszania się w nierównym terenie.
Zarówno systemy elektroniczne, jak i oprogramowanie zostały zaprojektowane w taki
sposób, by ich dalszy rozwój był możliwie prosty.
Badania przeprowadzono na dwa sposoby: najpierw symulacyjnie sprawdzono poprawność działania niższych warstw robota, następnie dokonywano testów na rzeczywistym obiekcie. Wyniki badań pozwalają stwierdzić, iż krata Tody-Rayleigha jest
dobrym rozwiązaniem do implementacji generatora chodu robotów. Dodatkowo, sterowanie z wykorzystaniem krzywizny i prędkości umożliwia intuicyjne i jednoznaczne
sterowanie obiektem. Ostatni etap badań pokazał, że algorytm wykorzystujący informacje z czujników dotyku na końcach nóg znacznie zwiększa możliwości lokomocyjne
robota w nierównym terenie.
Zaproponowana konstrukcja może być rozwijana na wiele sposobów. Jednym z nich
jest rozszerzanie oprogramowania robota w stronę większej autonomii konstrukcji umożliwiającej eksplorację nieznanego terenu. W tym wypadku dobrym pomysłem jest wyposażenie jednostki w dodatkowe sensory pozwalające na tworzenie map otoczenia,
które znacznie upraszczają zadanie poruszania się w nierównym terenie — każdy organizm porusza się pewniej w znanym otoczeniu, niż w warunkach, gdy nie posiada
wystarczających informacji na temat jego.
Kolejną możliwością rozwijania robota jest tworzenie jednostki zwiadowczej lub
saperskiej prowadzonej przez operatora. Do tego celu niezbędne jest rozwinięcie interfejsu kontrolnego oraz wyposażenie robota w kamerę z tyłu konstrukcji. Dodatkowo
w zależności od zadań stawianych przed robotem, konstrukcja powinna być rozszerzona
o specjalistyczne sprzęty niezbędne do wykonywania tych zadań. W przypadku kroczącego robota zwiadowczego, jego przewaga nad robotami kołowymi polega na większych
możliwościach poruszania się w trudnym terenie. Wyposażenie konstrukcji w sensory
określające skład powietrza może być przydatne dla robotów pracujących w kopalniach.
Czujniki wykrywania ruchu lub kamery termowizyjne mogą być wykorzystywane w ra-
70
6. Podsumowanie
zie trzęsień ziemi. Robot zwiadowczy może dojść w miejsca zbyt wąskie i trudne do
poruszania się dla człowieka, może również transportować do ludzi uwięzionych pod
ziemią żywność, wodę oraz środki medyczne. Zastosowanie saperskie wymaga wykonania robota w większej skali oraz przystosowania go do korzystania z manipulatora do
przenoszenia różnych obiektów. W stosunku do robotów kołowych wykonujących tego
typu zadania dużą zaletą takiej maszyny może być fakt, iż przednie nogi mogą również
być wykorzystywane do manipulacji obiektami.
Trudno porównywać poruszanie się żywych insektów w stosunku do maszyny kroczącej, gdyż różnice w rozwiązaniach występujące w dziedzinie materiałów, napędu oraz
miniaturyzacji pozostają niedoścignione. Nie powinno to jednak zniechęcać badaczy do
dalszego rozwoju. Obserwacje działania wzorców biologicznych są zwykle najlepszymi
wzorcami do budowy konstrukcji maszyn oraz tworzenia nowych algorytmów ruchu
gwarantujących zadawalające efekty kroczenia, a często najlepsze z możliwych. Wraz
z rozwojem materiałów oraz mechanizmów, możliwe jest tworzenie coraz to bardziej
zaawansowanych konstrukcji posiadających większe możliwości lokomocji.
Autor pracy planuje dalszy rozwój projektu mając na celu stworzenie konstrukcji
robota zwiadowczego, posiadającego możliwość poruszania się w opuszczonych budynkach z możliwością wspinania się po schodach. Jest to zadanie rozległe oraz otwarte
dla przyszłych badań.
Dodatek A
Płyta CD
Płyta CD zawiera:
• Elektroniczną wersję pracy magisterskiej,
• Projekt aplikacji komputerowej dla środowiska QtCreator,
• Projekt oprogramowania mikrokontrolera dla środowiska Keil µVision,
• Projekty poszczególnych elementów robota oraz rysunki złożeniowe dla Autodesk
Inventor,
• Schematy elektroniczne oraz płytek PCB wykonane w programie Eagle,
• Filmy nakręcone podczas badań funkcjonowania robota,
• Noty katalogowe elementów wykorzystanych w projekcie.
Dodatek B
Schematy elektroniczne
W dodatku zamieszczono schematy elektroczniczne oraz schematy płytek PCB:
• Schemat elektroniczny płytki z zasilaniem,
• Schemat PCB płytcki z zasilaniem,
• Schemat elektroniczny płytki mikrokontrolera,
• Schemat PCB płytcki mikrokontrolera,
Fragmenty kodu zawarte w pracy :
4.1
4.2
4.3
4.4
Zmiana ID serwomechanizmu . . . . . . . . . .
Ustawienie pozycji bazowej serwomechanizmu .
Inicjacja magistrali I 2 C . . . . . . . . . . . . .
Przerwanie od zewnętrznego zdarzenia na linii 4
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
40
40
43
46
Spis rysunków
2.1
2.2
2.3
2.4
2.5
2.6
2.7
2.8
2.9
2.10
2.11
2.12
2.13
Proces przestawiania nogi z parametrami . . . . . . . . . . . . .
Wielokąt podparcia zaznaczony dla ruchu trójpodporowego . . .
Trajektorie poszczególnych nóg w chodzie metachronicznym . .
Fazy chodu z wielokątami podparcia dla chodu pełzającego . . .
Trajektorie poszczególnych nóg w chodzie gąsienicowym . . . .
Fazy chodu z wielokątami podparcia dla chodu gąsienicowego .
Trajektorie poszczególnych nóg w chodzie trójpodporowym . . .
Fazy chodu z wielokątami podparcia dla chodu gąsienicowego .
Robot podczas kroczenia po łuku . . . . . . . . . . . . . . . . .
Sześcioelementowy pierścień sieci Tody . . . . . . . . . . . . . .
Robot z zaznaczonymi poszczególnymi układami współrzędnych
Kinematyka nogi robota . . . . . . . . . . . . . . . . . . . . . .
Rzut nogi na płaszczyznę 0XZ dla q1 = 0 . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6
7
8
9
9
10
11
11
12
14
18
19
20
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
3.13
3.14
Cicindela formosa generosa[17] . . . . . . . . . . . . . . . . . . . . . .
Korpus robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Porównanie standardowych i robotycznych serwomechanizmów . . . . .
Jedno z odnóż Cicindela formosa generosa[17] z zaznaczonymi odcinkami
Projekt nogi robota w Autodesk Inventor 2013 . . . . . . . . . . . . . .
Rzeczywista realizacja nogi maszyny kroczącej . . . . . . . . . . . . . .
Sposób podłączenia przycisków rozwiernych . . . . . . . . . . . . . . .
Moduł kamery oraz dalmierza . . . . . . . . . . . . . . . . . . . . . . .
Sposób przymocowania modułu MinIMU-9 . . . . . . . . . . . . . . . .
Model robota w Autodesk Inventor 2013 . . . . . . . . . . . . . . . . .
Rzeczywista realizacja konstrukcji projektu . . . . . . . . . . . . . . . .
Schemat zasilania pojedynczej nogi robota . . . . . . . . . . . . . . . .
Schemat komunikacji między serwem a mikrokontrolerem . . . . . . . .
Schemat podłączenia mikrokontrolera STM32F407VGT6 . . . . . . . .
24
25
25
26
26
27
28
28
29
30
31
32
33
34
4.1
4.2
4.3
4.4
4.5
4.6
Schemat ramki Profibus [5] typu telegram dla stałej długości danych . .
Interfejs użytkownika do sterowania robotem . . . . . . . . . . . . . . .
Wizualizacja danych otrzymywanych z modułu MinIMU-9 . . . . . . .
Schemat ramki i odpowiedzi dla protokołu HMI serwomechanizmów . .
Schemat systemu znajdującego się na mikrokontrolerze . . . . . . . . .
Interpolacja pomiarów uzyskiwanych z dalmierza IR wraz z naniesionymi punktami pomiarowymi . . . . . . . . . . . . . . . . . . . . . . .
36
37
38
39
42
46
82
SPIS RYSUNKÓW
4.7
Schemat warstw sterowania robotem kroczącym . . . . . . . . . . . . .
5.1
Schemat dwóch najniższych warstw systemu sterowania w środowisku
Simulink . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Chód trójpodporowy — rozruch kraty Tody-Rayleigha . . . . . . . . .
Ślad nogi dla wybranych wartości początkowych chodu trójpodporowego
z fazą rozruchu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Chód czteropodporowy — rozruch kraty Tody-Rayleigha . . . . . . . .
Ślad nogi dla wybranych wartości początkowych chodu czteropodporowego z fazą rozruchu . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Chód pięciopodporowy — rozruch kraty Tody-Rayleigha . . . . . . . .
Ślad nogi dla wybranych wartości początkowych chodu pięciopodporowego z fazą rozruchu . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Przejście pomiędzy chodem trójpodporowym, a czteropodporowym . .
Przejście pomiędzy chodem czteropodporowym, a trójpodporowym . .
Zdjęcia poklatkowe pokazujące trajektorię pojedynczej nogi . . . . . . .
Zdjęcia poklatkowe pokazujące sposób kroczenia po okręgu . . . . . . .
Zdjęcia poklatkowe pokazujące wpływ korzystania z czujników dotyku
na trajektorię końca nogi . . . . . . . . . . . . . . . . . . . . . . . . . .
Robot podczas pokonywania płaskiego terenu z niewielkimi przeszkodami
Robot w trakcie pokonywania płaskiego terenu ze zróżnicowanymi przeszkodami . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Maszyna krocząca w trawiastym środowisku . . . . . . . . . . . . . . .
Robot kroczący po kamienistym terenie . . . . . . . . . . . . . . . . .
5.2
5.3
5.4
5.5
5.6
5.7
5.8
5.9
5.10
5.11
5.12
5.13
5.14
5.15
5.16
47
50
51
52
53
54
55
56
57
58
60
62
64
65
65
66
67
Bibliografia
[1] FreeRTOS – market leading RTOS. http://www.freertos.org/, 2014.
[2] RS232 terminal, serial port terminal, udp terminal, tcp/ip client server - hercules.
http://www.hw-group.com/products/hercules/index_en.html, 2014.
[3] 3D CAD Software | Inventor 3D CAD | Autodesk. http://www.autodesk.com/
products/autodesk-inventor-family/overview#, 2014.
[4] OpenGL – The Industry Standard forHigh Performance Graphics. http://www.
opengl.org/, 2014.
[5] Acromag Incorporated. Introduction to Profibus DP, 2002.
[6] S. Bai. The Kinematics of the Terrain-adaptive Locomotion of Walking Machines.
Praca doktorska, Aalborg University Singapore, 2000.
[7] FTDI Chip. FT232R USB UART IC, 2010.
[8] R. Gierczak. Laboratoryjny robot kroczący. Praca magisterska, Politechnika
Wrocławska, 2012.
[9] W. R. Hamilton, W. E. Hamilton. Elements of quaternions. London: Longmans,
Green, & Company, 1866.
[10] D. Hatley, P. Hruschka, I. Pirbhai. Process for system architecture and requirements engineering. Addison-Wesley, 2013.
[11] HTC Korea TAEJIN Technology Co. 3A L.D.O. VOLTAGE REGULATOR (Adjustable & Fixed) LM1085, 2010.
[12] K. Inagaki. Study of leg arrangement for hexapod walking robot with gait seek
algorithm. Mobile Robotics. Solutions and Challenges. World Scientific Pub Co
Inc, 2009.
[13] HS-5585MH economical, high voltage, high torque, coreless, metal gear digital sport servo | HITEC RCD USA.
http://
hitecrcd.com/products/servos/sport-servos/digital-sport-servos/
hs-5585mh-economical-high-torque-digital-coreless-servo/product,
2014.
84
BIBLIOGRAFIA
[14] Jameco Electronics. Pushbutton Switches 104-0010-EVX, 104-0011-EVX, 1040012-EVX, 104-0013-EVX, 2005.
[15] LinkSprite. LinkSprite JPEG Color Camera Serial UART Interface, 2010.
[16] Z. Liu, L. Wang, C. P. Chen, X. Zeng, Y. Zhang, Y. Wang. Energy-efficiencybased gait control system architecture and algorithm for biped robots. Systems,
Man, and Cybernetics, Part C: Applications and Reviews, IEEE Transactions on,
42(6):926–933, 2012.
[17] T. MacRae. Cicindela formosa generosa (eastern sand tiger beetle) | flickr - photo
sharing! https://www.flickr.com/photos/49679700@N07/5720649177/, 2014.
[18] S. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor
arrays. Report x-io and University of Bristol (UK), 2010.
[19] V. A. Makarov, E. Del Rio, W. Ebeling, M. G. Velarde. Dissipative toda-rayleigh
lattice and its oscillatory modes. Physical review – series E, 64(3; PART 2), 2001.
[20] V. A. Makarov, W. Ebeling, M. G. Velarde. Soliton-like waves on dissipative toda
lattice. International Journal of Bifurcation and Chaos, 10(5):1075–1089, 2000.
[21] S. Messlinger. Hitec Robot Servos. http://www.staff.uni-bayreuth.de/
~bt150361/cmt2007/geraete/hitec_hsr/HSR_Serial_communication.pdf,
2007.
[22] Micrel. MIC29150/29300/29500/29750 High-Current Low-Dropout Regulators,
2005.
[23] M. Patro. Analiza porównawcza systemów czasu rzeczywistego dedykowanych pod
mikrokontrolery z rdzeniem Cortex-M4, 2014. Projekt inżynierski.
[24] M. Piątek. Problemy sterowania robotami kroczącymi — generatory chodu hexapoda. Praca doktorska, Akademia Górniczo-Hutnicza Kraków, 2012.
[25] J. W. S. Rayleigh. The theory of sound. Macmillan and co., 1877.
[26] U. Saranli, M. Buehler, D. E. Koditschek. Rhex: A simple and highly mobile
hexapod robot. The International Journal of Robotics Research, 20(7):616–631,
2001.
[27] Bluetooth – Wikipedia, the free encyclopedia. http://en.wikipedia.org/wiki/
Bluetooth, 2014.
[28] KAmodBTM222 � Kamami.pl - sklep internetowy dla elektroników. http://www.
kamami.pl/index.php?productID=137699, 2014.
[29] Pololu – MinIMU-9 gyro, accelerometer, and compass (L3G4200D and
LSM303DLH carrier). http://www.pololu.com/product/1264, 2014.
[30] Qt Project. http://qt-project.org/, 2014.
BIBLIOGRAFIA
85
[31] TowerPro SG90 servo specifications and reviews. http://www.servodatabase.
com/servo/towerpro/sg90, 2014.
[32] Sharp Corporation. GP2D12 Optoelectronic Device, 2005.
[33] A. Silberschatz, P. B. Galvin, Z. Płoski, J. L. Peterson. Podstawy systemów operacyjnych. Wydawnictwa Naukowo-Techniczne, 1996.
[34] STMicroelectronics. Getting started with software and firmware environments for
the STM32F4DISCOVERY Kit, 2011.
[35] STMicroelectronics. ST2378E 8-bit dual supply 1.71 to 5.5 V level translator with
I/O VCC ± 15 KV ESD protection, 2012.
[36] STMicroelectronics. STM32F405xx/07xx, STM32F415xx/17xx, STM32F42xxx
and STM32F43xxx advanced ARM®-based 32-bit MCUs, 2014.
[37] K. Tchoń, A. Mazur, I. Dulęba, R. Hossa, R. Muszyński. Manipulatory i roboty
mobilne. Akademicka Oficyna Wydawnicza PLJ, 2000.
[38] HSR-8498HB HMI standard robot servo | HITEC RCD USA.
http://
hitecrcd.com/products/servos/discontinued-servos-servo-accessories/
hsr-8498hb-hmi-standard-robot-servo/product, 2014.
[39] M. Toda. Theory of Nonlinear Lattices. Springer, wydanie 2, 1989.
[40] D. M. Wilson. Insect walking. Annual Rev. Entomology, strony 103–122, 1996.
[41] W. A. Wolovich. Robotics: Basic Analysis and Design. Oxford University Press,
1995.
[42] T. Zielińska. Maszyny kroczące. Wydawnictwo Naukowe PWN, 2013.

Sterowanie sześcionożnym robotem kroczącym w

Transkrypt

Podobne dokumenty

neato - Rackcdn.com