Sterowanie Mechanizmów Wieloczłonowych
Raport
Sposoby dyskretyzacji przestrzeni przeszukiwania przy planowaniu trajektorii
Wykonał
Michał Korczak
1. Generalne Planowanie Trajektorii
Planowanie trajektorii polega na automatycznym opracowaniu ruchu manipulatora tak aby uniknąć kolizji z przeszkodami. Aby rozwiązać zadanie planowania należy znając początkową i końcową pozycje manipulatora, a także geometryczny opis manipulatora oraz przeszkód, podać opis ruchu jaki należy podjąć aby osiągnąć pozycję końcową bez kolizji z przeszkodą. Alternatywnie ścieżkę jaką musi podążać robot można wyznaczyć w tzw. przestrzeni konfiguracyjnej.[1]
Rys 1. Przykład planowania trasy w 2 wymiarach.[1]
2. Przestrzeń konfiguracyjna
Przestrzeń konfiguracyjna to przestrzeń której punkty opisują wszystkie możliwe stany układu. Każde położenie obiektu w przestrzeni rzeczywistej można opisać unikalnym punktem w przestrzeni konfiguracyjnej.[1]
Dla świata W⊂ ℝn, przestrzeń konfiguracyjna jest tzw. osobliwością C ℝ⊂ m, gdzie m ≥ n.
Punkt q=(q1,q2, ... ,qn) , q ∈C nazywamy konfiguracją. Każda konfiguracja q jest zawarta w otwartym podzbiorze O ⊂ C dla którego istnieje ciągła bijekcja f : O → ℝn oraz ciągła funkcja odwrotna f -1.[1]
Przykład: dla świata 2-wymiarowego zdefiniujmy robota A ⊂ ℝ2 jako zbiór punktów ograniczony bokami wielokąta. Robota możemy przesuwać wzdłuż osi X o xt ℝ oraz wzdłuż osi Y o ∈ yt ℝ, a także obracać o kąt ∈ θ ∈ [0,2π). Wtedy zbiór wszystkich konfiguracji q=(xt, yt, θ) daje wszystkie możliwe położenia robota. Dodatkowo położenie robota dla każdej konfiguracji jest unikalne. Wtedy punkt (x,y) ∈A będzie w punkcie (x',y') ∈W, zgodnie z wzorem
( x ' y ' 1 ) = ( cosθ −sinθ sinθ 0 cosθ 0 y x 1
tt) ( x 1 y ) , (1)
który wykorzystuje standardową macierz transformacji.[1]
Warto zauważyć że wszystkie obroty θ ± 2πn są identyczne. Oznacza to że θ „zawija się” w 2π tworząc okrąg.
Trzeba to wziąć pod uwagę przy planowaniu trajektorii gdyż inaczej w przestrzeni powstanie sztuczna bariera. Obroty dla A ℝ⊂ 3 tworzą bardziej skomplikowaną przestrzeń konfiguracyjną. Mogą wystąpić punkty osobliwe, które
uniemożliwiają jednoznaczne wyznaczanie pozycji. Aby tego uniknąć stosuje się opis obrotów kwaternionami.[1]
3. Planowanie Trajektorii w przestrzeni konfiguracyjnej
Rys 2. Przykład planowania trasy w przestrzeni konfiguracyjnej.[1]
Zanim przystąpimy do planowania trasy należy rozważyć fragmenty C których nie można osiągnąć ze względu na kolizje. Niech A(q) ⊂W oznacza zamknięty zbiór punktów w świecie należących do robota A przy konfiguracji q, a O zbiór punktów w świecie należący do przeszkód. Konfiguracja q ∈C powoduje kolizje wtedy i tylko wtedy gdy A(q) ∩ O ≠ (robot i przeszkoda mają co najmniej jeden wspólny punkt w ∅ W). Podzbiór wszystkich konfiguracji niepowodujący kolizji nazwany jest wolną przestrzenią i zdefiniowany jest jako Cfree= {q ∈C | A(q) ∩ O ≠ }. ∅ Dopełnienie tego zbioru nazywa się regionem przeszkód: Cobs= C \ Cfree.[1]
Posiadając tak zdefiniowana przestrzeń konfiguracyjną można formalnie opisać problem planowania trajektorii jako:
• Mając zdefiniowany świat W który W= ℝ3 ∨ W= ℝ2.
• Przeszkody O ⊂W.
• Robota A ⊂W. Gdzie A może być opisany jako ciało sztywne lub zbiór członów
• Przestrzeń konfiguracyjną C określona na podstawie wszystkich możliwych położeń robota. Z tych danych można wyznaczyć Cobs oraz Cfree.
• Konfiguracja początkową qI ∈ Cfree.
• Konfiguracje końcową qG ∈ Cfree.
• Należy wyznaczyć ciągłą ścieżkę τ: [0,1] → Cfree, taką że τ(0)=qI i τ(1)=qG, lub poprawnie stwierdzić iż taka ścieżka nie istnieje.
Ścieżka τ jest zwykle łamaną. Ciągłość ścieżki w C powoduje stopniowy ruch robota w W.[1]
4. Dyskretyzacja przy tworzeniu przestrzeni konfiguracyjnej z współrzędnych uogólnionych.
Jak było wspomniane przestrzeń konfiguracyjną w W= ℝ3 można opisać dla ciała sztywnego za pomocą 3 translacji i 3 rotacji. W przypadku mechanizmów wieloczłonowych bardziej naturalny jest opis przy pomocy zmiennych uogólnionych. [2]
Ważne jest aby wybrać odpowiednio rozdzielczość współrzędnych uogólnionych. Zbyt mała oznacza bardzo dużą przestrzeń C i długie przeszukiwanie, zbyt duża może oznaczać że nie będzie można odnaleźć ścieżki.[2]
Najprostsza metoda to ustalenie jednolitej rozdzielczości dyskretyzacji Δqi takiej samej dla wszystkich członów. Na przykład ustalenie rozdzielczości Δq=(1°,1°,1°,1°,1°,1°) dla robota Puma 260 sprawia że przestrzeń C ma 2.13*1015 punktów.[2]
Rys 3. a) Jednolita dyskretyzacja ( Δqi= Δqj ) powoduje inne przesunięcie TCP (Δxi≠ Δxj ) przy ruchu różnych członów b) Optymalna dyskretyzacja powoduje takie samo przesunięcie TCP (Δxi=Δxj ) przy ruchu różnych członów.[2]
Możliwe jest też obliczenie optymalnej dyskretyzacji która zapewnia iż zmiana dowolnej współrzędnej uogólnionej o jeden krok powoduje takie samo przesunięciem TCP w najgorszej konfiguracji manipulatora. Dla robota Puma 260 optymalna dyskretyzacja w której maksymalne przesunięcie w jednym kroku wynosi 10mm wynosi
Δq=(0.96°,0.98°,1.40°,2.83°,2.84°,10.33°). Przestrzeń C ma w takim wypadku 1.88*1013 punktów, 2 rzędy wielkości mniej niż dyskretyzacja jednolita przy tych samych założeniach.[2]
Podobną metodą jest dyskretyzacja w której określone jest średnie przesunięcie TCP. [2]
Ostatnią jest idealna dyskretyzacja w której przesunięciem TCP jest stałe pomiędzy wszystkimi okolicznymi punktami C. Niestety przesunięcie zależy od konfiguracji więc niezbędne jest zastosowanie zmiennej rozdzielczości.
Z tego powodu aby uzyskać tego typu reprezentacje najlepiej wykorzystać zaawansowane techniki sztucznej inteligencji.[2]
rys 4. Rozmiar zdyskredytowanej przestrzeni C dla różnych metod dyskretyzacji w funkcji maksymalnego przemieszczenia TCP w jednym kroku w najgorszej konfiguracji[2]
5. Metody Kombinatoryczne
Aby zaplanować trajektorię w przestrzeni konfiguracyjnej można podzielić tę przestrzeń na rozłączne podzbiory. Następnie każdy taki podzbiór wyznaczyć jako wierzchołek grafu. Uzyskany graf jest dyskretyzacją całej przestrzeni konfiguracyjnej. Aby znaleźć ścieżkę wystarczy zastosować algorytm przeszukiwania grafu.[1]
Najlepiej pokazać działanie algorytmu na prostym przykładzie. Niech świat W= ℝ2 i zawiera punktowego robota, który nie może się obracać, wtedy C= ℝ2. Aby uzyskać graf należy:
• Podzielić Cfree na trapezoidy pionowymi liniami. Z każdego wierzchołka Cfree należy wystrzelić promienie do góry i w dół. Każdy taki promień może zostać natychmiast zablokowany albo kreślić krawędź trapezoidu dopóki nie zetknie się z regionem przeszkód.
• Umieścić wierzchołek grafu w wnętrzu każdego trapezoidu. Ponieważ nie jest istotnie gdzie więc dla wygody wybieramy środek.
• Umieścić wierzchołek grafu w każdej pionowej krawędzi.
• Połączyć każdy wierzchołek wewnątrz trapezoidu z wierzchołkami na jego krawędziach. Każde takie połączenie jest krawędzią grafu i, w tym wypadku, oznacza ruch robota w prostej linii.[1]
Warto zauważyć że podział nie zależy od punktów początkowego qI i końcowego qG. Gdy dla danej przestrzeni zostanie przeprowadzona dyskretyzacja można uzyskany graf stosować do różnych qI i qG. Aby do grafu dodać punkty qI i qG należy:
• Znaleźć trapezoidy zawierające qI i qG.
• Połączyć qI i qG do wierzchołków grafu wewnątrz zawierających je trapezoidów.[1]
Dla takiego przypadku możliwe są inne podziały, np. na trójkąty. Generalnie każda komórka powinna być prosta do przebycia (najlepiej wypukła), podział na komórki powinien być prosty do obliczenia, a także powinno być łatwo określić czy komórki ze sobą graniczą.[1]
Największym problemem w stosowaniu tego algorytmu jest tzw. kombinatorzyczna eksplozja. Niewiele bardziej skomplikowany przypadek w którym robot jest trójkątem który może się obracać daje reprezentację
przeszkody która ma ponad 70 powierzchni pokazaną na rysunku 5. Dla przypadków 3-wymiarowych złożoność rośnie jeszcze bardziej.[1]
Kolejnym problemem jest podział na komórki. Pierwszą metodą zdolną to zrobić dla wielowymiarowych przestrzeni jest metoda Schwartza i Sharira, niestety tworzy dużą ilość komórek przez co jest złożona obliczeniowo.
Opracowano efektywniejsze metody, w szczególności metodę Cannego która tworzy graf bez uprzedniego podziału przestrzeni.[1]
Metody kombinatoryczne dają rozwiązanie generalnego problemu planowania trajektorii, ale są rzadko stosowane że względu na dużą złożoność obliczeniową.[1]
Rys. 6. Ilustracja planowania kombinacyjnego. a) reprezentacja regionu przeszkód 2-wymiarowym wielokątem oraz punkt początkowy i końcowy. b) Podział przestrzeni na trapezy. c) Tworzenie grafu poprzez umieszczenie wierzchołka
na każdej pionowej krawędzi i w centrum każdego trapezoidu. d) połączenie punktów początkowego i końcowego do grafu i szukanie ścieżki.[1]
6. Metoda Próbkowania
Metody oparte na próbkowaniu są najbardziej popularne w rozwiązaniach przemysłowych. Ponieważ
Cobs składa się z dużej ilości powierzchni w metodach próbkujących nie definiuje się regionu przeszkód jawnie. Zamiast tego funkcja inkrementalnie sonduje i przeszukuje C aby znaleźć ścieżkę. Sprawdzenia dokonuje się algorytmem który sprawdza czy dana konfiguracja (i często jej okolica) znajduje się w regionie przeszkód Cobs. Tego typu podejście sprawia ze nie trzeba w całości definiować wysoko wymiarowej przestrzeni C.[1]
Przykładem stosowanego algorytmu jest RRT (rapidly exploring random trees). Algorytm tworzy drzewo zaczynając od początkowej konfiguracji q0. Następnie w każdej iteracji dodaje wierzchołki i krawędzie grafu łączące je z istniejącymi wierzchołkami. Każda krawędź grafu jest bezkolizyjną ścieżka pomiędzy dwoma konfiguracjami.
Algorytm na początku tworzy dalekie połączenia (odpowiednik autostrady) a potem tworzy krótsze gałęzie wokół tych punktów (drogi lokalne).[1]
Rys 7. Schemat działania RRT. Na początku agresywnie eksploruje przestrzeń, potem pokrywa ją jednorodną siecią.[1]
Aby zastosować algorytm w zadaniach znajdowania trasy najłatwiej wybrać „korzeń” drzewa q0=qI. Następnie, co np. 100 iteracji, wymusić aby drzewo spróbowało połączyć się z konfiguracja końcową qG. Istotne jest aby nie sprawdzać możliwości połączenia z qG zbyt często. W takiej sytuacji algorytm będzie tracił czas na próbę tworzenia krawędzi przez region przeszkód, zamiast eksplorować przestrzeń.[1]
Może się okazać iż prościej jest budować drzewo w drugą stronę, tzn. q0=qG. Z tego powodu stosuje się dwukierunkowe RRT. Algorytm buduje dwa drzewa, jedno z punktu początkowego drugie z końcowego i stara się połączyć najnowsze wierzchołki obu drzew ze sobą.[1]
Rys. 8 Przykłady przestrzeni które posiadają lokalne minima, lub można je przejść tylko w jedna stronę.[1]
Metody próbkujące dają ścieżki które często zmieniają kierunek, dlatego stosuje się wygładzanie ścieżki.
Prostą metodą jest wybieranie iteracyjne pary punktów na ścieżce i łączenie ich prostą linią. Jeśli wygenerowana linia przebiega przez Cfree zastępuje się oryginalny fragment ścieżki tą linią. Po przeprowadzaniu kilku iteracji uzyskuje się
Opisane do tej pory metody on-line znajdują pojedynczą ścieżkę od danego qI do qG. Możliwa jest dyskretyzacja przestrzeni przed przystąpieniem do szukania ścieżki np. poprzez wybranie dużej liczby losowych konfiguracji (np. 1000) i połączenie ich w jedno drzewo. Następnie można skorzystać z tak przygotowanego drzewa dodając tylko qI i qG. Takie podejście tworzy jednak drzewa o dużym rozmiarze, konieczne jest więc ich upraszczanie.[1]
Metody tego typu nie są zupełne. Jeśli rozwiązanie istnieje to znajdą je w skończonym czasie, ale jeśli go nie ma algorytm może wykonywać się bez końca.[1]
7. Podsumowanie
Problemy planowania trajektorii rozwiązuje się w wielowymiarowej przestrzeni konfiguracyjnej. Przy tworzeniu przestrzeni konfiguracyjnej istotne jest poprawne wybranie rozdzielczości dyskretyzacji, tak aby zapewnić wystarczającą dokładność przy możliwie małej wielkości przestrzeni konfiguracyjnej. Ponieważ przeszukiwanie całej przestrzeni konfiguracyjnej jest niepraktyczne, dzieli się ją na fragmenty przy użyciu grafu który łatwo przeszukiwać przy pomocy znanych algorytmów np. A*. Przestrzeń konfiguracyjną można podzielić na komórki i stworzyć graf połączeń pomiędzy tymi komórkami. Taki podział dobrze opisuje całą przestrzeń, ale jest skomplikowany w wielowymiarowej przestrzeni. Inną metodą jest opisanie przestrzeni losowym drzewem. Tego typu opis nie definiuje dobrze całej przestrzeni, ale jest na tyle szybki by planować trajektorie w czasie rzeczywistym.
Bibliografia
[1] S.M. LaValle, “Motion Planning: The Essentials”, IEEE Robotics & Automation Magazine, vol. 18, 2011, pp. 79-89.
[2] D. Henrich, C. Wurll, H. Worn, "Online path planning with optimal c-space discretization,", 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vol. 3, Victoria, British Columbia. Canada, October 1998, pp. 1479-1484.
[3] D. Atariah, G. Rote, „Configuration space visualization", 2012 Symposium on Computational Geometry, Chapel Hill, North Carolina, USA, June 2012, pp. 415-416.
