dla zadania zrobotyzowanej inspekcji Katedra Sterowania i Inżynierii Systemów, Politechnika Poznańska 3 lipca 2014
Plan prezentacji 1 Wprowadzenie 2 3 4
Postawienie problemu Założenia: Rozpatrujemy kinematykę monocykla: 1 0 [ ] θ [ ] q = 0 cos θ u1, q x θ = u 0 sin θ 2 q y Znana a priori siatka zajętości Realizacja ruchu sterownikiem VFO y y G yl a r q * 0 x x L θ b r q * dn x G Robot zajmuje prostokąt a r b r Zadanie: Wybrać uporządkowany zbiór punktów referencyjnych umożliwiający bezkolizyjny przejazd z q 0 do q dn.? q * d1? q * d2? q *...
Proponowana strategia planowania Proces planowania: 1 Wyszukać ścieżkę w grafie uzyskanym z siatki zajętości (znajdujemy bezkolizyjną łamaną zmodyfikowanym algorytmem A*) 2 Wybrać pozycje punktów referencyjnych (wierzchołki łamanej stanowią pozycje większości punktów przejazdowych) 3 Zaplanować orientacje zadane w punktach referencyjnych (analityczny, stałoczasowy algorytm wykorzystujący własności VFO) Ograniczenia nieholonomiczne uwzględniane są tylko w ostatnim etapie.
Plan prezentacji 1 Wprowadzenie 2 3 4
Graf ze zredukowanym sąsiedztwem Wybór sąsiedztwa: Sąsiedztwo zależy od kierunku przetwarzania c f f Nie rozpatrujemy kierunków prostopadłych 2 Wybór sąsiada oznacza wybór strategii P/T 1 Skutki powyższego wyboru: Redukcja kosztu obliczeniowego d 2 Minimalizacja ilości wierzchołków w łamanych d 1 f l f fff Globalne planowanie strategii P/T Brak redundantnych zmian strategii P/T b l p c d p b p f p d 4 d 3 1 P/T oznacza jazdę przodem lub tyłem. 2 D. Harabor, A. Grastien. Online graph pruning for pathfinding on grid maps. Proc. 25th Conference on Artificial Intelligence (AAAI-11), 2011.
Wybór strategii P/T siatka dla jazdy przodem Wykorzystujemy dwie połączone siatki zajętości. f l c f f f p Zmiana strategii P/T powoduje przeskok do drugiej siatki Rozróżniamy 2 zestawy kosztów ruchu dla każdej komórki siatki W pamięci przechowana jest tylko jedna siatka b l c b p siatka dla jazdy tyłem
Wstępne uwzględnienie orientacji brzegowych Początkowy kierunek przetwarzania wynika z orientacji początkowej θ 0 Odrzucamy sąsiadów punktu końcowego spełniających relację: b l f l c f f f p q * dn arg(σd p ) θ dn > π 4 p d p b p θ dn
Funkcja kosztu ruchu f g + h, h g(c) ˆD min(d) Q dn λ(c) 1, ( ) m c k s g m c + [g(p) + λ(c) λ(p) ] φ exp(min(d)) d 1 d 2 p d p c f f d 3 d 4 Składniki: Zbiór D {d 1, d 2, d 3, d 4} odległości od przeszkód wzdłuż kluczowych kierunków Współczynnik m c rosnący przy zmianie kierunku ścieżki Ziarno przetwarzania φ Odległość komórki od punktu końcowego: Q dn λ(c) Odległość od komórki poprzednio przetwarzanej: λ(c) λ(p) 1 ˆD oznacza średnią arytmetyczną z elementów D.
Własności funkcji kosztu ruchu Zarówno g, jak i h są nieliniowymi funkcjami odległości od przeszkód h jest niedopuszczalna, ale algorytm pozostaje kompletny 1 Dla równych D różnice w kosztach siąsiadujących komórek są podobne klasycznego A* k s pozwala na strojenie wpływu długości ścieżki na funkcje kosztu W odróżnieniu od innych rozwiązań, uzyskany efekt jest globalny 1 Wynika to z systematyczności wyszukiwania (patrz S.M. LaValle Planning Algorithms. Cambridge University Press 2006. rozdz. 2.2).
Plan prezentacji 1 Wprowadzenie 2 3 4
Prawo sterowania VFO Dla każdego punktu śledzić pole zbieżności 2 : θ a Atan2c(σh y, σh x ), [ ] h hx = k h p e + v, k p > 0, y [ ] v σk p µ e cos θ, sin θ u 1 k a (θ a θ), k a > 0, u 2 σ ρ cos (θ a θ), µ (0, 1), ρ 0, σ { 1, 1} 5 4.5 4 3.5 3 2.5 2 1.5 1 0.5 0 y[m] x[m] 2 1.5 1 0.5 0 0.5 1 1.5 2 q 0 q d µ = 0.5 µ = 0.6 µ = 0.8 µ = 0.9 1 Sterownik pokazano w: M. M. Michałek, K. Kozłowski. Motion planning and feedback control for a unicycle in a way point following task: The VFO approach. Int. J. Appl. Math. Comput. Sci., 2009, wol. 19, nr 4
Algorytm planowania orientacji Dla i {N, N 1,..., 1}: 1 Zaplanować współczynnik µ i kształtujący ścieżkę dojazdu do q di 2 Wyznaczyć orientację zadaną w q di 1, aby zagwarantować ciągłość orientacji: θ di 1 (t i ) := θ ai (t i ), θ ai = Atan2c (σ i h yi (t i ), σ i h xi (t i )) W chwili t i następuje przełączenie na i-ty punkt referencyjny. Uwaga Algorytm planowania orientacji ma złożoność O(n) względem ilości punktów referencyjnych.
Planowanie względnego współczynnika naprowadzania µ Jak odtworzyć łamaną nie zatrzymując monocykla? µ min maksymalizuje dopasowanie do i-tego segmentu łamanej µ ai maksymalizuje dopasowanie do i 1-szego segmentu łamanej Proponujemy kompromis w postaci średniej ważonej: µ i k f q di q di 1 µmin + q di 1 q di 2 µai k f q di q di 1 + q di 1 q di 2, µ min (0, µ max), µ ai (µ min, µ max) y G q* di-2 q* di-1 μ ai μ μ i min q di v * ai θ di x G Uwaga k f pozwala na płynne strojenie dopasowania do łamanej.
Plan prezentacji 1 Wprowadzenie 2 3 4
Wyniki planowania i realizacji ruchu 12 20 10 18 16 S6 8 14 12 6 S1 10 8 S4 S5 4 2 y[m] S2 S3 0 0 x[m] 2 4 6 8 10 12 6 4 2 y[m] 0 0 x[m] 2 4 6 8 10 12 14 16 18 20 Trójkąty oznaczają konfiguracje zadane w punktach referencyjnych.
Wpływ parametrów na generowane plany ϕ= 0.2 m ϕ= 0.29 m ϕ= 0.08 m 12 10 8 k f = 1 k f = 5 k f = 10 6 4 k s = 3 2 y[m] k s = 7 Trójkąty oznaczają konfiguracje zadane w punktach referencyjnych. 0 0 2 4 6 8 10 12 x[m]
Podsumowanie Planer korzysta z wiedzy o sterowniku VFO Prostota podejścia do problemu Można wpływać na bezpieczeństwo i gładkość generowanych planów Zredukowano wymiar przestrzeni wyszukiwania do R 2 W prosty sposób uwzględniono strategię P/T Bezpieczeństwo ścieżki wpływa globalnie na wyniki planowania Reprezentacja planu jest oszczędna i łatwa w obróbce Prezentowane modyfikacje algorytmu A* są niezależne od sterownika Proponowany algorytm: jest kompletny przy odpowiednim wyborze φ i k f jest deterministyczny może być strojony w łatwy sposób