Wstępny samouczek programowania robotów

Spójrzmy prawdzie w oczy, roboty są fajne. Pewnego dnia będą też rządzić światem i miejmy nadzieję, że w tym czasie zlitują się nad swoimi biednymi, miękkimi, mięsistymi twórcami (znanymi również jako twórcy robotyki) i pomogą nam zbudować kosmiczną utopię wypełnioną mnóstwem. Żartuję oczywiście, ale tylko trochę.

W mojej ambicji, by mieć trochę wpływu na tę sprawę, w zeszłym roku wziąłem udział w kursie teorii sterowania robotami autonomicznymi, którego kulminacją było zbudowanie opartego na Pythonie symulatora robota, który pozwolił mi ćwiczyć teorię sterowania na prostym, mobilnym, programowalnym robocie .

W tym artykule pokażę, jak wykorzystać framework robota w Pythonie do opracowania oprogramowania sterującego, opiszę schemat sterowania, który opracowałem dla mojego symulowanego robota, zilustruję, w jaki sposób wchodzi on w interakcję z otoczeniem i osiąga swoje cele, oraz omówię niektóre z podstawowe wyzwania programowania robotów, które napotkałem po drodze.

Aby skorzystać z tego samouczka dotyczącego programowania robotyki dla początkujących, powinieneś mieć podstawową wiedzę na temat dwóch rzeczy:

• Matematyka — użyjemy kilku funkcji trygonometrycznych i wektorów
• Python — ponieważ Python jest jednym z bardziej popularnych podstawowych języków programowania robotów — będziemy korzystać z podstawowych bibliotek i funkcji Pythona

Pokazane tutaj fragmenty kodu to tylko część całego symulatora, który opiera się na klasach i interfejsach, więc aby bezpośrednio odczytać kod, możesz potrzebować trochę doświadczenia w Pythonie i programowaniu obiektowym.

Wreszcie, opcjonalne tematy, które pomogą ci lepiej śledzić ten samouczek, to wiedza, czym jest maszyna stanu i jak działają czujniki zasięgu i enkodery.

Wyzwanie programowalnego robota: percepcja a rzeczywistość i kruchość kontroli

Podstawowym wyzwaniem dla całej robotyki jest to: niemożliwe jest poznanie prawdziwego stanu środowiska. Oprogramowanie sterujące robotem może jedynie odgadywać stan rzeczywistego świata na podstawie pomiarów zwracanych przez jego czujniki. Może jedynie próbować zmienić stan realnego świata poprzez generowanie sygnałów sterujących.

Tak więc jednym z pierwszych kroków w projektowaniu sterowania jest wymyślenie abstrakcji rzeczywistego świata, zwanego modelem , za pomocą którego można interpretować odczyty naszych czujników i podejmować decyzje. Dopóki świat rzeczywisty zachowuje się zgodnie z założeniami modelu, możemy dobrze zgadywać i sprawować kontrolę. Jednak gdy tylko rzeczywisty świat odbiegnie od tych założeń, nie będziemy już w stanie dobrze zgadywać, a kontrola zostanie utracona. Często, gdy utraci się kontrolę, nigdy nie można jej odzyskać. (Chyba że jakaś życzliwa siła zewnętrzna go przywróci.)

To jeden z głównych powodów, dla których programowanie robotyki jest tak trudne. Często oglądamy filmy przedstawiające najnowszego robota badawczego w laboratorium, wykonującego fantastyczne wyczyny zręcznościowe, nawigacyjne lub zespołowe, i mamy ochotę zapytać: „Dlaczego nie używa się tego w prawdziwym świecie?” Cóż, następnym razem, gdy zobaczysz taki film, spójrz, jak wysoce kontrolowane jest środowisko laboratoryjne. W większości przypadków roboty te są w stanie wykonywać te imponujące zadania tylko wtedy, gdy warunki środowiskowe pozostają w wąskich granicach ich modelu wewnętrznego. Tak więc jednym z kluczy do postępu robotyki jest opracowanie bardziej złożonych, elastycznych i solidnych modeli – a wspomniany postęp podlega ograniczeniom dostępnych zasobów obliczeniowych.

[Uwaga dodatkowa: Filozofowie i psychologowie zauważą, że żywe stworzenia również cierpią z powodu zależności od własnej wewnętrznej percepcji tego, co mówią im zmysły. Wiele postępów w robotyce pochodzi z obserwacji żywych stworzeń i obserwowania, jak reagują na nieoczekiwane bodźce. Pomyśl o tym. Jaki jest twój wewnętrzny model świata? Różni się od mrówki i od ryby? (Mam nadzieję.) Jednak, podobnie jak mrówka i ryba, prawdopodobnie zbytnio uprości niektóre realia świata. Kiedy twoje założenia dotyczące świata nie są poprawne, może to narazić cię na utratę kontroli nad rzeczami. Czasami nazywamy to „niebezpieczeństwem”. W ten sam sposób, w jaki nasz mały robot walczy o przetrwanie w nieznanym wszechświecie, tak samo my wszyscy. To potężny wgląd dla robotyków.]

Programowalny symulator robota

Symulator, który zbudowałem, jest napisany w Pythonie i bardzo sprytnie nazwany Sobot Rimulator . Możesz znaleźć v1.0.0 na GitHub. Nie ma wielu dzwonków i gwizdków, ale jest zbudowany, aby robić jedną rzecz bardzo dobrze: zapewnić dokładną symulację robota mobilnego i dać początkującemu robotykowi proste ramy do ćwiczenia programowania oprogramowania robota. Chociaż zawsze lepiej jest mieć prawdziwego robota do zabawy, dobry symulator robota Pythona jest znacznie bardziej dostępny i jest świetnym miejscem do rozpoczęcia.

W robotach w świecie rzeczywistym oprogramowanie generujące sygnały sterujące („kontroler”) musi działać z bardzo dużą prędkością i wykonywać złożone obliczenia. Ma to wpływ na wybór najlepszego języka programowania robotów: zwykle w tego rodzaju scenariuszach używany jest C++, ale w prostszych aplikacjach robotyki Python jest bardzo dobrym kompromisem między szybkością wykonywania a łatwością programowania i testowania.

Oprogramowanie, które napisałem, symuluje prawdziwego robota badawczego o nazwie Khepera, ale można je dostosować do szeregu robotów mobilnych o różnych wymiarach i czujnikach. Ponieważ próbowałem zaprogramować symulator jak najbardziej zbliżony do możliwości prawdziwego robota, logikę sterowania można załadować do prawdziwego robota Khepera przy minimalnej refaktoryzacji i będzie on działał tak samo, jak symulowany robot. Zaimplementowane konkretne funkcje nawiązują do Khepery III, ale można je łatwo dostosować do nowej Khepera IV.

Innymi słowy, programowanie symulowanego robota jest analogiczne do programowania prawdziwego robota. Ma to kluczowe znaczenie, jeśli symulator ma być przydatny do opracowywania i oceny różnych podejść do oprogramowania sterującego.

W tym samouczku opiszę architekturę oprogramowania sterującego robotem, która jest dostarczana z wersją 1.0.0 programu Sobot Rimulator , oraz udostępnię fragmenty kodu źródłowego Pythona (z niewielkimi modyfikacjami dla jasności). Zachęcam jednak do zagłębienia się w źródło i bałaganu. Symulator został rozwidlony i służy do sterowania różnymi robotami mobilnymi, w tym Roomba2 firmy iRobot. Podobnie, możesz rozwidlić projekt i go ulepszyć.

Logika sterowania robota jest ograniczona do tych klas/plików Pythona:

• models/supervisor.py — ta klasa odpowiada za interakcję między symulowanym światem wokół robota a samym robotem. Rozwija naszą maszynę stanu robota i uruchamia kontrolery do obliczania pożądanego zachowania.
• models/supervisor_state_machine.py — ta klasa reprezentuje różne stany, w których może znajdować się robot, w zależności od interpretacji czujników.
• Pliki w katalogu models/controllers — te klasy implementują różne zachowania robota w znanym stanie środowiska. W szczególności dobierany jest określony sterownik w zależności od maszyny stanowej.

Cel

Roboty, podobnie jak ludzie, potrzebują celu w życiu. Cel naszego oprogramowania sterującego tym robotem będzie bardzo prosty: będzie próbował dotrzeć do z góry określonego punktu docelowego. To zazwyczaj podstawowa cecha, jaką powinien posiadać każdy robot mobilny, od autonomicznych samochodów po zrobotyzowane odkurzacze. Współrzędne celu są programowane w oprogramowaniu sterującym przed aktywacją robota, ale mogą zostać wygenerowane z dodatkowej aplikacji Pythona, która nadzoruje ruchy robota. Pomyśl na przykład o przejeżdżaniu przez wiele punktów orientacyjnych.

Jednak, aby skomplikować sprawy, otoczenie robota może być usiane przeszkodami. Robot NIE MOŻE zderzyć się z przeszkodą w drodze do celu. Dlatego też, jeśli robot napotka przeszkodę, będzie musiał znaleźć drogę, aby móc kontynuować drogę do celu.

Programowalny robot

Każdy robot ma inne możliwości i problemy związane ze sterowaniem. Zapoznajmy się z naszym symulowanym robotem programowalnym.

Pierwszą rzeczą, na którą należy zwrócić uwagę, jest to, że w tym przewodniku nasz robot będzie autonomicznym robotem mobilnym . Oznacza to, że będzie się swobodnie poruszać w przestrzeni i będzie to robić pod własną kontrolą. Jest to w przeciwieństwie do, powiedzmy, zdalnie sterowanego robota (który nie jest autonomiczny) lub fabrycznego ramienia robota (które nie jest ruchome). Nasz robot musi sam wymyślić, jak osiągnąć swoje cele i przetrwać w swoim otoczeniu. Okazuje się to zaskakująco trudnym wyzwaniem dla początkujących programistów robotyki.

Wejścia sterujące: czujniki

Istnieje wiele różnych sposobów, w jakie robot może być wyposażony w monitorowanie otoczenia. Mogą to być czujniki zbliżeniowe, czujniki światła, zderzaki, kamery i tak dalej. Ponadto roboty mogą komunikować się z zewnętrznymi czujnikami, które przekazują im informacje, których same nie mogą bezpośrednio obserwować.

Nasz robot referencyjny jest wyposażony w dziewięć czujników podczerwieni — nowszy model ma osiem czujników podczerwieni i pięć czujników ultradźwiękowych — rozmieszczonych w „spódnicy” we wszystkich kierunkach. Jest więcej czujników skierowanych z przodu robota niż z tyłu, ponieważ zwykle ważniejsze jest, aby robot wiedział, co jest przed robotem, niż co jest za nim.

Oprócz czujników zbliżeniowych robot posiada parę czujników koła , które śledzą ruch koła. Pozwalają one śledzić, ile obrotów wykonuje każde koło, przy czym jeden pełny obrót koła do przodu to 2765 tyknięć. Skręty w przeciwnym kierunku liczą się do tyłu, zmniejszając liczbę tików zamiast ją zwiększać. Nie musisz się martwić o konkretne liczby w tym samouczku, ponieważ oprogramowanie, które napiszemy, wykorzystuje przebytą odległość wyrażoną w metrach. Później pokażę, jak obliczyć to z tików za pomocą prostej funkcji Pythona.

Wyjścia sterujące: Mobilność

Niektóre roboty poruszają się na nogach. Niektóre toczą się jak piłka. Niektórzy nawet ślizgają się jak wąż.

Nasz robot to robot z napędem różnicowym, co oznacza, że ​​porusza się na dwóch kołach. Gdy oba koła obracają się z tą samą prędkością, robot porusza się w linii prostej. Gdy koła poruszają się z różnymi prędkościami, robot się obraca. W ten sposób kontrolowanie ruchu tego robota sprowadza się do odpowiedniego kontrolowania szybkości, z jaką każde z tych dwóch kół się obraca.

API

W Sobot Rimulator oddzielenie „komputera” robota od (symulowanego) świata fizycznego urzeczywistnia plik robot_supervisor_interface.py , który definiuje całe API do interakcji z czujnikami i silnikami „prawdziwego robota”:

• read_proximity_sensors() zwraca tablicę dziewięciu wartości w natywnym formacie czujników
• read_wheel_encoders() zwraca tablicę dwóch wartości wskazujących całkowitą liczbę ticków od początku
• set_wheel_drive_rates( v_l, v_r ) przyjmuje dwie wartości (w radianach na sekundę) i ustawia lewą i prawą prędkość kół na te dwie wartości

Ten interfejs wewnętrznie wykorzystuje obiekt robota, który dostarcza dane z czujników oraz możliwość poruszania silnikami lub kołami. Jeśli chcesz stworzyć innego robota, po prostu musisz dostarczyć inną klasę robota Pythona, która może być używana przez ten sam interfejs, a reszta kodu (kontrolery, nadzorca i symulator) zadziała po wyjęciu z pudełka!

Symulator

Ponieważ używałbyś prawdziwego robota w prawdziwym świecie bez zwracania zbytniej uwagi na prawa fizyki, możesz zignorować sposób, w jaki robot jest symulowany i po prostu przejść bezpośrednio do programowania oprogramowania kontrolera, ponieważ będzie to prawie to samo między światem rzeczywistym a symulacją. Ale jeśli jesteś ciekawy, krótko to przedstawię.

Plik world.py to klasa Pythona, która reprezentuje symulowany świat z robotami i przeszkodami w środku. Funkcja step w tej klasie dba o ewolucję naszego prostego świata poprzez:

• Stosowanie zasad fizyki do ruchów robota
• Uwzględnienie kolizji z przeszkodami
• Dostarczanie nowych wartości dla czujników robota

W końcu dzwoni do nadzorców robotów odpowiedzialnych za wykonanie oprogramowania mózgu robota.

Funkcja step jest wykonywana w pętli, dzięki czemu robot.step_motion() przesuwa robota przy użyciu prędkości koła obliczonej przez przełożonego w poprzednim kroku symulacji.

 # step the simulation through one time interval def step( self ): dt = self.dt # step all the robots for robot in self.robots: # step robot motion robot.step_motion( dt ) # apply physics interactions self.physics.apply_physics() # NOTE: The supervisors must run last to ensure they are observing the "current" world # step all of the supervisors for supervisor in self.supervisors: supervisor.step( dt ) # increment world time self.world_time += dt

Funkcja apply_physics() wewnętrznie aktualizuje wartości czujników zbliżeniowych robota, dzięki czemu osoba nadzorująca może oszacować środowisko na bieżącym etapie symulacji. Te same koncepcje dotyczą enkoderów.

Prosty model

Po pierwsze nasz robot będzie miał bardzo prosty model. Zrobi wiele założeń na temat świata. Niektóre z najważniejszych to:

• Teren jest zawsze płaski i równy
• Przeszkody nigdy nie są okrągłe
• Koła nigdy się nie ślizgają
• Nic nigdy nie będzie pchać robota dookoła
• Czujniki nigdy nie zawodzą ani nie dają fałszywych odczytów
• Koła obracają się zawsze, gdy im każą

Chociaż większość z tych założeń jest rozsądna w środowisku przypominającym dom, mogą pojawić się okrągłe przeszkody. Nasze oprogramowanie do unikania przeszkód ma prostą implementację i podąża za granicą przeszkód, aby je ominąć. Podpowiemy czytelnikom, jak ulepszyć ramy sterowania naszego robota z dodatkowym sprawdzianem, aby omijać koliste przeszkody.

Pętla kontrolna

Wejdziemy teraz w rdzeń naszego oprogramowania sterującego i wyjaśnimy zachowania, które chcemy zaprogramować wewnątrz robota. Do tego frameworka można dodać dodatkowe zachowania, a po przeczytaniu powinieneś wypróbować własne pomysły! Oprogramowanie do robotyki opartej na zachowaniu zostało zaproponowane ponad 20 lat temu i nadal jest potężnym narzędziem dla robotyki mobilnej. Na przykład w 2007 roku zestaw zachowań został wykorzystany w DARPA Urban Challenge — pierwszym konkursie autonomicznych samochodów!

Robot to dynamiczny system. Stan robota, odczyty jego czujników i skutki jego sygnałów sterujących podlegają ciągłym zmianom. Kontrolowanie przebiegu wydarzeń obejmuje następujące trzy kroki:

1. Zastosuj sygnały sterujące.
2. Zmierz wyniki.
3. Generuj nowe sygnały sterujące, obliczone, aby przybliżyć nas do celu.

Te kroki są powtarzane w kółko, aż osiągniemy nasz cel. Im więcej razy możemy to zrobić na sekundę, tym lepszą kontrolę będziemy mieć nad systemem. Robot Sobot Rimulator powtarza te kroki 20 razy na sekundę (20 Hz), ale wiele robotów musi to robić tysiące lub miliony razy na sekundę, aby mieć odpowiednią kontrolę. Zapamiętaj nasze poprzednie wprowadzenie dotyczące różnych języków programowania robotów dla różnych systemów robotyki i wymagań dotyczących prędkości.

Ogólnie rzecz biorąc, za każdym razem, gdy nasz robot wykonuje pomiary za pomocą swoich czujników, wykorzystuje te pomiary do aktualizacji wewnętrznej oceny stanu świata — na przykład odległości od celu. Porównuje ten stan z wartością odniesienia , jaką chce , aby stan był (dla odległości chce, aby był równy zero) i oblicza błąd między stanem pożądanym a stanem rzeczywistym. Znając te informacje, generowanie nowych sygnałów sterujących można zredukować do problemu minimalizacji błędu , który ostatecznie przesunie robota w kierunku celu.

Sprytna sztuczka: uproszczenie modelu

Aby sterować robotem, którego chcemy zaprogramować, musimy wysłać sygnał do lewego koła informujący, jak szybko ma się skręcać, a osobny sygnał do prawego koła, informujący , jak szybko ma się skręcać. Nazwijmy te sygnały v L i v R . Jednak ciągłe myślenie w kategoriach vL i vR jest bardzo kłopotliwe. Zamiast pytać: „Jak szybko ma się obracać lewe koło, a jak szybko ma się obracać prawe?” bardziej naturalne jest pytanie: „Jak szybko chcemy, aby robot poruszał się do przodu i jak szybko ma się obracać lub zmieniać kierunek?” Nazwijmy te parametry prędkością v i prędkością kątową (obrotową) ω (czytaj „omega”). Okazuje się, że możemy oprzeć cały nasz model na v i ω zamiast v L i v R , i dopiero po ustaleniu, w jaki sposób chcemy, aby nasz zaprogramowany robot się poruszał, matematycznie przekształcić te dwie wartości w potrzebne nam v L i v R faktycznie kontrolować koła robota. Jest to znane jako jednokołowy model sterowania.

Oto kod Pythona, który implementuje ostateczną transformację w supervisor.py . Zwróć uwagę, że jeśli ω wynosi 0, oba koła będą się obracać z tą samą prędkością:

 # generate and send the correct commands to the robot def _send_robot_commands( self ): # ... v_l, v_r = self._uni_to_diff( v, omega ) self.robot.set_wheel_drive_rates( v_l, v_r ) def _uni_to_diff( self, v, omega ): # v = translational velocity (m/s) # omega = angular velocity (rad/s) R = self.robot_wheel_radius L = self.robot_wheel_base_length v_l = ( (2.0 * v) - (omega*L) ) / (2.0 * R) v_r = ( (2.0 * v) + (omega*L) ) / (2.0 * R) return v_l, v_r

Stan szacowania: Robot, poznaj siebie

Używając swoich czujników, robot musi starać się oszacować stan środowiska, a także swój stan. Te oszacowania nigdy nie będą doskonałe, ale muszą być całkiem dobre, ponieważ robot będzie opierał wszystkie swoje decyzje na tych szacunkach. Korzystając wyłącznie z czujników zbliżeniowych i pasków kół, musi spróbować odgadnąć następujące rzeczy:

• Kierunek do przeszkód
• Odległość od przeszkód
• Pozycja robota
• Kierunek robota

Pierwsze dwie właściwości są określane przez odczyty czujnika zbliżeniowego i są dość proste. Funkcja API read_proximity_sensors() zwraca tablicę dziewięciu wartości, po jednej dla każdego czujnika. Wiemy z wyprzedzeniem, że na przykład siódmy odczyt odpowiada czujnikowi, który wskazuje 75 stopni na prawo od robota.

Tak więc, jeśli ta wartość pokazuje odczyt odpowiadający odległości 0,1 metra, wiemy, że znajduje się przeszkoda w odległości 0,1 metra, 75 stopni w lewo. Jeśli nie ma przeszkody, czujnik zwróci odczyt swojego maksymalnego zasięgu 0,2 metra. Tak więc, jeśli odczytamy 0,2 metra na czujniku siódmym, założymy, że w tym kierunku nie ma żadnej przeszkody.

Ze względu na sposób działania czujników podczerwieni (pomiar odbicia podczerwieni), zwracane przez nie liczby są nieliniową transformacją rzeczywistej wykrytej odległości. W związku z tym funkcja Pythona do określania wskazanej odległości musi przekonwertować te odczyty na metry. Odbywa się to w supervisor.py w następujący sposób:

 # update the distances indicated by the proximity sensors def _update_proximity_sensor_distances( self ): self.proximity_sensor_distances = [ 0.02-( log(readval/3960.0) )/30.0 for readval in self.robot.read_proximity_sensors() ]

Ponownie, w tej strukturze robota Pythona mamy konkretny model czujnika, podczas gdy w prawdziwym świecie czujniki są dostarczane z towarzyszącym oprogramowaniem, które powinno zapewniać podobne funkcje konwersji z wartości nieliniowych na metry.

Ustalenie pozycji i kierunku robota (łącznie znane jako pozy w programowaniu robotyki) jest nieco trudniejsze. Nasz robot wykorzystuje odometrię do oceny swojej pozy. Tutaj wkraczają paski kół. Mierząc, o ile obróciło się każde koło od ostatniej iteracji pętli sterowania, można dobrze oszacować, jak zmieniła się pozycja robota — ale tylko wtedy, gdy zmiana jest niewielka .

Jest to jeden z powodów, dla których ważne jest, aby bardzo często iterować pętlę sterowania w rzeczywistym robocie, w którym silniki poruszające kołami mogą nie być idealne. Gdybyśmy zbyt długo czekali z pomiarem tickerów, oba koła mogłyby zdziałać całkiem sporo i nie da się oszacować, gdzie trafiliśmy.

Biorąc pod uwagę nasz obecny symulator oprogramowania, możemy sobie pozwolić na wykonywanie obliczeń odometrycznych z częstotliwością 20 Hz — taką samą częstotliwością jak sterowniki. Ale dobrym pomysłem może być posiadanie oddzielnego wątku Pythona działającego szybciej, aby wyłapać mniejsze ruchy pasków.

Poniżej znajduje się pełna funkcja odometrii w module supervisor.py , która aktualizuje oszacowanie pozycji robota. Zauważ, że pozycja robota składa się ze współrzędnych x i y oraz kierunku theta mierzonego w radianach od dodatniej osi X. Dodatnie x jest na wschodzie, a dodatnie y na północy. Zatem kurs równy 0 wskazuje, że robot jest skierowany bezpośrednio na wschód. Robot zawsze przyjmuje, że jego pozycja początkowa to (0, 0), 0 .

 # update the estimated position of the robot using it's wheel encoder readings def _update_odometry( self ): R = self.robot_wheel_radius N = float( self.wheel_encoder_ticks_per_revolution ) # read the wheel encoder values ticks_left, ticks_right = self.robot.read_wheel_encoders() # get the difference in ticks since the last iteration d_ticks_left = ticks_left - self.prev_ticks_left d_ticks_right = ticks_right - self.prev_ticks_right # estimate the wheel movements d_left_wheel = 2*pi*R*( d_ticks_left / N ) d_right_wheel = 2*pi*R*( d_ticks_right / N ) d_center = 0.5 * ( d_left_wheel + d_right_wheel ) # calculate the new pose prev_x, prev_y, prev_theta = self.estimated_pose.scalar_unpack() new_x = prev_x + ( d_center * cos( prev_theta ) ) new_y = prev_y + ( d_center * sin( prev_theta ) ) new_theta = prev_theta + ( ( d_right_wheel - d_left_wheel ) / self.robot_wheel_base_length ) # update the pose estimate with the new values self.estimated_pose.scalar_update( new_x, new_y, new_theta ) # save the current tick count for the next iteration self.prev_ticks_left = ticks_left self.prev_ticks_right = ticks_right

Teraz, gdy nasz robot jest w stanie dobrze oszacować rzeczywisty świat, wykorzystajmy te informacje do osiągnięcia naszych celów.

Metody programowania robotów w Pythonie: zachowanie na drodze do celu

Nadrzędnym celem istnienia naszego małego robota w tym samouczku programowania jest dotarcie do celu. Jak więc sprawić, by koła się skręcały, żeby to tam dotarły? Zacznijmy od pewnego uproszczenia naszego światopoglądu i załóżmy, że nie ma na drodze żadnych przeszkód.

To staje się wtedy prostym zadaniem i można je łatwo zaprogramować w Pythonie. Jeśli pójdziemy do przodu, mierząc się z celem, dotrzemy tam. Dzięki naszej odometrii wiemy, jakie są nasze aktualne współrzędne i kierunek. Wiemy też, jakie są współrzędne celu, ponieważ zostały wcześniej zaprogramowane. Dlatego używając małej algebry liniowej, możemy określić wektor od naszej lokalizacji do celu, tak jak w go_to_goal_controller.py :

 # return a go-to-goal heading vector in the robot's reference frame def calculate_gtg_heading_vector( self ): # get the inverse of the robot's pose robot_inv_pos, robot_inv_theta = self.supervisor.estimated_pose().inverse().vector_unpack() # calculate the goal vector in the robot's reference frame goal = self.supervisor.goal() goal = linalg.rotate_and_translate_vector( goal, robot_inv_theta, robot_inv_pos ) return goal

Zwróć uwagę, że dostajemy wektor do celu w układzie odniesienia robota , a NIE we współrzędnych światowych. Jeśli cel znajduje się na osi X w ramie odniesienia robota, oznacza to, że znajduje się bezpośrednio przed robotem. Zatem kąt tego wektora od osi X jest różnicą między naszym kursem a kursem, na którym chcemy się znaleźć. Innymi słowy, jest to błąd między naszym obecnym stanem a tym, jaki chcemy, aby był nasz obecny stan. W związku z tym chcemy wyregulować naszą prędkość skrętu ω tak, aby kąt między naszym kursem a celem zmieniał się w kierunku 0. Chcemy zminimalizować błąd:

 # calculate the error terms theta_d = atan2( self.gtg_heading_vector[1], self.gtg_heading_vector[0] ) # calculate angular velocity omega = self.kP * theta_d

self.kP w powyższym fragmencie kontrolera Implementacja Pythona to wzmocnienie kontroli. Jest to współczynnik, który określa, jak szybko skręcamy, proporcjonalnie do tego, jak daleko od celu, przed którym stoimy. Jeśli błąd w naszym nagłówku wynosi 0 , to prędkość skrętu również wynosi 0 . W prawdziwej funkcji Pythona w pliku go_to_goal_controller.py zobaczysz więcej podobnych korzyści, ponieważ użyliśmy kontrolera PID zamiast prostego współczynnika proporcjonalnego.

Teraz, gdy mamy już naszą prędkość kątową ω , jak wyznaczymy naszą prędkość do przodu v ? Dobrą ogólną zasadą jest taka, którą prawdopodobnie znasz instynktownie: jeśli nie skręcamy, możemy jechać do przodu z pełną prędkością, a im szybciej skręcamy, tym bardziej powinniśmy zwalniać. To generalnie pomaga nam utrzymać stabilność naszego systemu i działanie w granicach naszego modelu. Zatem v jest funkcją ω . W go_to_goal_controller.py równanie to:

 # calculate translational velocity # velocity is v_max when omega is 0, # drops rapidly to zero as |omega| rises v = self.supervisor.v_max() / ( abs( omega ) + 1 )**0.5

Propozycja rozwinięcia tej formuły polega na rozważeniu, że zwykle zwalniamy, gdy zbliżamy się do celu, aby osiągnąć go z zerową prędkością. Jak zmieniłaby się ta formuła? Musi zawierać jakoś zastąpienie v_max() czymś proporcjonalnym do odległości. OK, prawie ukończyliśmy jedną pętlę sterowania. Jedyne, co pozostało do zrobienia, to przekształcenie tych dwóch parametrów modelu monocykla na różnicowe prędkości kół i wysłanie sygnałów do kół. Oto przykład trajektorii robota pod kontrolerem go-to-goal, bez przeszkód:

Jak widzimy, wektor do celu jest dla nas efektywnym punktem odniesienia, na którym możemy oprzeć nasze obliczenia kontrolne. Jest to wewnętrzna reprezentacja „dokąd chcemy iść”. Jak zobaczymy, jedyną istotną różnicą między dążenie do celu a innymi zachowaniami jest to, że czasami dążenie do celu to zły pomysł, więc musimy obliczyć inny wektor odniesienia.

Metody programowania robotów w Pythonie: zachowanie omijania przeszkód

Dobrym przykładem jest dążenie do celu, gdy w tym kierunku jest przeszkoda. Zamiast wpadać w różne rzeczy na naszej drodze, spróbujmy zaprogramować prawo kontroli, które sprawi, że robot będzie ich unikał.

Aby uprościć scenariusz, zapomnijmy teraz całkowicie o punkcie docelowym i po prostu postawmy sobie następujący cel: Kiedy nie ma przed nami żadnych przeszkód, idź do przodu. Kiedy napotkasz przeszkodę, odwróć się od niej, aż nie będzie już przed nami.

W związku z tym, gdy nie ma przed nami żadnej przeszkody, chcemy, aby nasz wektor odniesienia po prostu wskazywał do przodu. Wtedy ω będzie wynosić zero, a v będzie maksymalną prędkością. Jednak gdy tylko wykryjemy przeszkodę za pomocą naszych czujników zbliżeniowych, chcemy, aby wektor odniesienia wskazywał kierunek, w którym znajduje się od przeszkody. Spowoduje to, że ω wystrzeli w górę, aby odwrócić nas od przeszkody i spowoduje, że v opadnie, aby upewnić się, że przypadkowo nie natkniemy się na przeszkodę podczas tego procesu.

Zgrabnym sposobem na wygenerowanie pożądanego wektora odniesienia jest przekształcenie dziewięciu odczytów zbliżeniowych w wektory i pobranie sumy ważonej. Gdy nie zostaną wykryte żadne przeszkody, wektory sumują się symetrycznie, dając wektor odniesienia, który będzie wskazywał na wprost zgodnie z potrzebami. Ale jeśli czujnik, powiedzmy, po prawej stronie wykryje przeszkodę, doda do sumy mniejszy wektor, a wynikiem będzie wektor odniesienia przesunięty w lewo.

W przypadku robota ogólnego z innym rozmieszczeniem czujników można zastosować ten sam pomysł, ale może wymagać zmiany masy i/lub dodatkowej ostrożności, gdy czujniki są symetryczne z przodu i z tyłu robota, ponieważ suma ważona może wynosić zero .

Oto kod, który to robi w avoid_obstacles_controller.py :

 # sensor gains (weights) self.sensor_gains = [ 1.0+( (0.4*abs(p.theta)) / pi ) for p in supervisor.proximity_sensor_placements() ] # ... # return an obstacle avoidance vector in the robot's reference frame # also returns vectors to detected obstacles in the robot's reference frame def calculate_ao_heading_vector( self ): # initialize vector obstacle_vectors = [ [ 0.0, 0.0 ] ] * len( self.proximity_sensor_placements ) ao_heading_vector = [ 0.0, 0.0 ] # get the distances indicated by the robot's sensor readings sensor_distances = self.supervisor.proximity_sensor_distances() # calculate the position of detected obstacles and find an avoidance vector robot_pos, robot_theta = self.supervisor.estimated_pose().vector_unpack() for i in range( len( sensor_distances ) ): # calculate the position of the obstacle sensor_pos, sensor_theta = self.proximity_sensor_placements[i].vector_unpack() vector = [ sensor_distances[i], 0.0 ] vector = linalg.rotate_and_translate_vector( vector, sensor_theta, sensor_pos ) obstacle_vectors[i] = vector # store the obstacle vectors in the robot's reference frame # accumulate the heading vector within the robot's reference frame ao_heading_vector = linalg.add( ao_heading_vector, linalg.scale( vector, self.sensor_gains[i] ) ) return ao_heading_vector, obstacle_vectors

Używając wynikowego ao_heading_vector jako naszego odniesienia dla robota, aby spróbować dopasować, oto wyniki uruchomienia oprogramowania robota w symulacji przy użyciu tylko kontrolera omijania przeszkód, całkowicie ignorując punkt docelowy. Robot podskakuje bez celu, ale nigdy nie zderza się z przeszkodą, a nawet radzi sobie w bardzo ciasnych przestrzeniach:

Metody programowania robotów w Pythonie: Automaty hybrydowe (maszyna stanu zachowania)

Do tej pory opisaliśmy dwa zachowania — pójście do celu i unikanie przeszkód — w izolacji. Oba pełnią swoją funkcję znakomicie, ale aby skutecznie osiągnąć cel w środowisku pełnym przeszkód, musimy je połączyć.

Rozwiązanie, które opracujemy, tkwi w klasie maszyn, które noszą niezwykle fajnie brzmiące oznaczenie automatów hybrydowych . Automat hybrydowy jest zaprogramowany z kilkoma różnymi zachowaniami lub trybami, a także nadzorującą maszynę stanów. Nadzorująca maszyna stanu przełącza się z jednego trybu do drugiego w dyskretnych momentach (gdy cele są osiągnięte lub otoczenie nagle zmieniło się za bardzo), podczas gdy każde zachowanie wykorzystuje czujniki i koła do ciągłego reagowania na zmiany otoczenia. Rozwiązanie nazwano hybrydą , ponieważ ewoluuje zarówno w sposób dyskretny, jak i ciągły.

Nasz framework robota w Pythonie implementuje maszynę stanów w pliku supervisor_state_machine.py .

Wyposażony w nasze dwa przydatne zachowania, nasuwa się prosta logika: gdy nie zostanie wykryta żadna przeszkoda, użyj zachowania idź do celu. Po wykryciu przeszkody przełącz się na zachowanie omijania przeszkód, aż przeszkoda nie będzie już wykrywana.

Jak się jednak okazuje, ta logika przysporzy sporo problemów. To, co ten system ma tendencję do robienia, gdy napotka przeszkodę, to odwracanie się od niej, a następnie, gdy tylko się od niej oddala, zawraca w prawo i ponownie na nią wbiega. Rezultatem jest niekończąca się pętla szybkiego przełączania, która czyni robota bezużytecznym. In the worst case, the robot may switch between behaviors with every iteration of the control loop—a state known as a Zeno condition .

There are multiple solutions to this problem, and readers that are looking for deeper knowledge should check, for example, the DAMN software architecture.

What we need for our simple simulated robot is an easier solution: One more behavior specialized with the task of getting around an obstacle and reaching the other side.

Python Robot Programming Methods: Follow-Wall Behavior

Here's the idea: When we encounter an obstacle, take the two sensor readings that are closest to the obstacle and use them to estimate the surface of the obstacle. Then, simply set our reference vector to be parallel to this surface. Keep following this wall until A) the obstacle is no longer between us and the goal, and B) we are closer to the goal than we were when we started. Then we can be certain we have navigated the obstacle properly.

With our limited information, we can't say for certain whether it will be faster to go around the obstacle to the left or to the right. To make up our minds, we select the direction that will move us closer to the goal immediately. To figure out which way that is, we need to know the reference vectors of the go-to-goal behavior and the avoid-obstacle behavior, as well as both of the possible follow-wall reference vectors. Here is an illustration of how the final decision is made (in this case, the robot will choose to go left):

Determining the follow-wall reference vectors turns out to be a bit more involved than either the avoid-obstacle or go-to-goal reference vectors. Take a look at the Python code in follow_wall_controller.py to see how it's done.

Final Control Design

The final control design uses the follow-wall behavior for almost all encounters with obstacles. However, if the robot finds itself in a tight spot, dangerously close to a collision, it will switch to pure avoid-obstacles mode until it is a safer distance away, and then return to follow-wall. Once obstacles have been successfully negotiated, the robot switches to go-to-goal. Here is the final state diagram, which is programmed inside the supervisor_state_machine.py :

Here is the robot successfully navigating a crowded environment using this control scheme:

An additional feature of the state machine that you can try to implement is a way to avoid circular obstacles by switching to go-to-goal as soon as possible instead of following the obstacle border until the end (which does not exist for circular objects!)

Tweak, Tweak, Tweak: Trial and Error

The control scheme that comes with Sobot Rimulator is very finely tuned. It took many hours of tweaking one little variable here, and another equation there, to get it to work in a way I was satisfied with. Robotics programming often involves a great deal of plain old trial-and-error. Robots are very complex and there are few shortcuts to getting them to behave optimally in a robot simulator environment…at least, not much short of outright machine learning, but that's a whole other can of worms.

I encourage you to play with the control variables in Sobot Rimulator and observe and attempt to interpret the results. Changes to the following all have profound effects on the simulated robot's behavior:

• The error gain kP in each controller
• The sensor gains used by the avoid-obstacles controller
• The calculation of v as a function of ω in each controller
• The obstacle standoff distance used by the follow-wall controller
• The switching conditions used by supervisor_state_machine.py
• Pretty much anything else

When Programmable Robots Fail

We've done a lot of work to get to this point, and this robot seems pretty clever. Yet, if you run Sobot Rimulator through several randomized maps, it won't be long before you find one that this robot can't deal with. Sometimes it drives itself directly into tight corners and collides. Sometimes it just oscillates back and forth endlessly on the wrong side of an obstacle. Occasionally it is legitimately imprisoned with no possible path to the goal. After all of our testing and tweaking, sometimes we must come to the conclusion that the model we are working with just isn't up to the job, and we have to change the design or add functionality.

In the mobile robot universe, our little robot's “brain” is on the simpler end of the spectrum. Many of the failure cases it encounters could be overcome by adding some more advanced software to the mix. More advanced robots make use of techniques such as mapping , to remember where it's been and avoid trying the same things over and over; heuristics , to generate acceptable decisions when there is no perfect decision to be found; and machine learning , to more perfectly tune the various control parameters governing the robot's behavior.

A Sample of What's to Come

Robots are already doing so much for us, and they are only going to be doing more in the future. While even basic robotics programming is a tough field of study requiring great patience, it is also a fascinating and immensely rewarding one.

In this tutorial, we learned how to develop reactive control software for a robot using the high-level programming language Python. But there are many more advanced concepts that can be learned and tested quickly with a Python robot framework similar to the one we prototyped here. I hope you will consider getting involved in the shaping of things to come!

Acknowledgement: I would like to thank Dr. Magnus Egerstedt and Jean-Pierre de la Croix of the Georgia Institute of Technology for teaching me all this stuff, and for their enthusiasm for my work on Sobot Rimulator.