Planowanie przejazdu przez zbiór punktów. zadania zrobotyzowanej inspekcji

Transkrypt

1 dla zadania zrobotyzowanej inspekcji Katedra Sterowania i Inżynierii Systemów, Politechnika Poznańska 3 lipca 2014

2 Plan prezentacji 1 Wprowadzenie 2 3 4

3 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 *...

4 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.

5 Plan prezentacji 1 Wprowadzenie 2 3 4

6 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.

7 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

8 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

9 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.

10 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 rozdz. 2.2).

11 Plan prezentacji 1 Wprowadzenie 2 3 4

12 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} y[m] x[m] q 0 q d µ = 0.5 µ = 0.6 µ = 0.8 µ = 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

13 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.

14 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.

15 Plan prezentacji 1 Wprowadzenie 2 3 4

16 Wyniki planowania i realizacji ruchu S S S4 S5 4 2 y[m] S2 S3 0 0 x[m] y[m] 0 0 x[m] Trójkąty oznaczają konfiguracje zadane w punktach referencyjnych.

17 Wpływ parametrów na generowane plany ϕ= 0.2 m ϕ= 0.29 m ϕ= 0.08 m k f = 1 k f = 5 k f = k s = 3 2 y[m] k s = 7 Trójkąty oznaczają konfiguracje zadane w punktach referencyjnych x[m]

18 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