Politechnika Warszawska Mechatronika
Heurystyki dla planowania trajektorii
Izabela Bryk
Spis treści
Spis treści ... 1
1. Wstęp ... 2
2. Planowanie ścieżki robota ... 2
3. Algorytm A* ... 3
3.1 Przykładowe funkcje heurystyczne ... 5
3.2 Iteracyjne pogłębianie ... 6
4. Problemy związane z przeszukiwaniem ... 6
5. Literatura: ... 7
1. Wstęp
Planowanie ścieżki robota, jak wiele innych zagadnień inżynierskich, jest zadaniem optymalizacyjnym. W zadaniach takich istnieje zbiór rozwiązań i funkcja ich oceny, która każdemu rozwiązaniu przypisuje wartość, będącą miarą jego jakości. Zadanie optymalizacyjne polega na znalezieniu rozwiązania najlepszego pod względem jakości. W zależności od tego jak się określi jakość, zadanie sprowadza się do znalezienia rozwiązania o minimalnej albo maksymalnej wartości funkcji oceny. Planowanie trajektorii polega na minimalizacji funkcji kosztu.
Istnieją metody algorytmiczne do efektywnego rozwiązywania zadań optymalizacyjnych. Dla wielu zadań optymalizacyjnych nie znaleziono jednak żadnych efektywnych algorytmów, a perspektywa ich odkrycia w przyszłości wydaję się bardzo wątpliwa. Grupę takich problemów w wersji decyzyjnej nazywamy problemami NP-zupełnymi.
W przypadku dużej ilości rozwiązań, ważne jest wczesne odrzucenie nieobiecujących kierunków poszukiwania. Zapewnia to oszczędności na kosztach obliczeniowych, a w rezultacie przyspiesza znalezienie rozwiązania.
Alternatywą dla tradycyjnych algorytmów rozwiązujących zadania optymalizacyjne są metody heurystyczne. Heurystyka to zbiór specyficznych dla danego zadania reguł, które mogą pomóc w znalezieniu najlepszego rozwiązania. Tradycyjne sposoby przeszukiwania zbioru rozwiązań zależały jedynie od informacji dostarczanej przez dotychczas zbadane elementy tego zbioru. Można jednak znaleźć szybsze rozwiązanie dzięki odpowiedniej heurystyce, która pozwala uwzględnić informacje o niezbadanej jeszcze części przestrzeni rozwiązań. Skuteczności kroków heurystycznych nie można udowodnić teoretycznie, można jedynie pokazać doświadczalnie ich trafność. Heurystyki stosuje się gdy pełny algorytm jest zprzyczyn technicznych zbyt kosztowny lub nieznany. czyli metody znajdowania rozwiązań, dla których nie ma gwarancji znalezienia rozwiązania optymalnego, a często nawet prawidłowego. Metody używa się też często do znajdowania rozwiązań przybliżonych, na podstawie, których później wylicza się ostateczny rezultat pełnym algorytmem. Heurystyka jest wykorzystywana do nakierowywania pełnego algorytmu ku optymalnemu rozwiązaniu, aby zmniejszyć czas działania programu w typowym przypadku bez poświęcania, jakości rozwiązania.
Metody heurystyczne można stosować między innymi do planowania trajektorii robotów.
2. Planowanie ścieżki robota
Zaplanowanie trajektorii robota polega na wyznaczeniu krzywej łączącej bezkolizyjnie konfigurację początkową z zadaną konfiguracją końcową uwzględniając ograniczenia wynikające z wielkości i kształtu przestrzeni zajętej przez przeszkody robota oraz kinematyki i dynamiki łańcucha kinematycznego robota. Planowania trajektorii robota sprowadza się do wyznaczenia funkcji interpolującej punkty węzłowe, czyli pośrednie położenia punktu charakterystycznego pozycjonowania robota miedzy pozycją wyjściową a zadaną. Ze względu na konieczność omijania przeszkód, ruch robota odbywa się etapami, więc jego trajektorią jest łamana określana kolejnymi punktami węzłowymi.
3. Algorytm A*
Algorytm A* jest heurystycznym algorytmem służącym do znajdowania najkrótszej ścieżki w grafie, czyli najbardziej optymalnej drogi od punktu startu do punktu celu poprzez analizę przestrzeni przeszukiwań składającej się z punktów węzłowych.
Planowanie ścieżki robota odbywa się w następujących krokach:
1. Analiza otoczenia robota.
Rysunek 1 przestrzeń robota. źródło [3]
2. Powiększenie przeszkód o promień robota w celu uniknięcia kolizji.
Rysunek 2 powiększenie przeszkód. źródło [3]
3. Sprowadzenie wymiarów robota do punktu
4. Nałożenie siatki
Rysunek 3 Nałożenie siatki. Źródło [3]
5. Wyznaczenie punktów węzłowych
Rysunek 4wyznaczenie punktów węzłowych. Źródło [3]
6. Wyznaczenie ścieżki robota
Rysunek 5 wyznaczenie ścieżki robota. Źródło [3]
Wybór punktów węzłowych wyznaczających trajektorię determinuje równanie:
𝑓(𝑥) = 𝑔(𝑥) + ℎ(𝑥) Gdzie:
f(x) – funkcja celu
g(x) – funkcjakosztu najlepszej ścieżki znanej od początkowego węzła do węzła x h(x) – przewidywana przez heurystykę droga od x do wierzchołka docelowego
W każdym kroku algorytm A* przedłuża już utworzoną ścieżkę o kolejny wierzchołek grafu,wybierając taki, w którym wartość funkcji będzie najmniejsza.Funkcja g(x) określa rzeczywisty koszt dojścia do punktu x.Funkcja h(x) heurystyczna oszacowuje (zawsze optymistycznie) koszt dotarcia od punktu do wierzchołka docelowego.
Algorytm A*klasyfikuje węzły są do trzech grup(list):
Otwarte- węzły oczekujące na sprawdzenie Zamknięte- węzły które zostały już sprawdzone
Rodzic - węzeł z którego przeszliśmy do aktualnego węzła. Informacja który węzeł jest rodzicem danego węzła jest ważna do określenia ścieżki po zakończeniu sprawdzania
Algorytm przeszukiwania węzłów:
Początkowo lista węzłów otwartych zawiera punkt startowy, a lista węzłów zamkniętych jest pusta.
Szukanie dostępnych przyległych pól
zakwalifikowanie pola startowego do Listy zamkniętych
Wybór spośród wszystkich dostępnych punktów ten o najmniejszym współczynniku F.
Rodzic dodawany do Listy zamkniętych.
Kontynuacja poszukiwania, postępując wg. algorytmu.
Jeżeli węzeł w otoczeniu rodzica jest już na Liście otwartych, to sprawdzenie czy z aktualnego węzła nie malepszej (niższy koszt) ścieżki dla niego.
Algorytm kończy działanie w momencie kiedy punkt końcowy zostanie dodany do Listy zamkniętych, lub kiedy punkt końcowy zostanie osiągnięty
Ścieżka zostaje wyznaczona poprzez ruch od punktu końcowego, po kolejnych rodzicach, aż do punktu początkowego.
3.1 Przykładowe funkcje heurystyczne
Manhattan: 𝐻 = 𝐷 ∗ |𝑝1. 𝑥 – 𝑝2. 𝑥| + |𝑝1. 𝑦 – 𝑝2. 𝑦| prosta funkcja, nazwę wzięła od mierzenia odległości między przecznicami na Manhattanie. Poruszanie się tylko po liniach pionowych i poziomych
Diagonal: 𝐻 = 𝐷 ∗ 𝑚𝑎𝑥(|𝑝1. 𝑥 – 𝑝2. 𝑥|, |𝑝1. 𝑦 – 𝑝2. 𝑦|)podobna funkcja, lecz nieco o gorszych właściwościach. Działa dobrze w szczególnychprzypadkach i jest szybko obliczeniowa
Euklides:
: 𝐻 = 𝐷 ∗ (𝑝1. 𝑥 − 𝑝2. 𝑥) + (𝑝1. 𝑦 − 𝑝2. 𝑦)
Funkcja Euklides potrzebuje dwukrotnego podniesienia do kwadratu oraz wyciągnięcia pierwiastka, natomiast jest dopuszczalna. Manhattan ma prostszy wzór, jednak lekko
przeszacowuje. Diagonal została dodana ze względów eksperymentalnych. Jest gorsza od pozostałych funkcji.
3.2 Iteracyjne pogłębianie
IDA* - Interativedeepening A* - iteracyjne pogłębianie. Jest modyfikacją polegającą na tym że dołożony jest dodatkowy próg T na funkcję f(n):
Jeśli f (n) > T cofaj się; T jest tu zmiennym progiem
Jeśli nie znaleziono rozwiązania zwiększ T i powtarzaj.
Poprzednie limity T nie są zapamiętywane
Taka modyfikacja powtarza niektóre ścieżki, ale pochłania mniej pamięci.
4. Problemy związane z przeszukiwaniem
Właściwy algorytm musi zagwarantować znajdowanie najkrótszej ścieżki do celu. W tym celu musi być spełniony warunek dopuszczalności heurystyki (ang. admissibility). Funkcja heurystyczna jest dopuszczalna jeżeli algorytm wykorzystuje funkcję heurystyczna taką, że h(n) jest mniejsze lub równe kosztowi minimalnej rzeczywistejścieżki od wierzchołka n do celu:
ℎ(𝑛) ≤ ℎ∗(𝑛) ℎ∗(𝑛) – rzeczywisty koszt ścieżki od stanu n do celu.
Warunek dopuszczalności to nadmierny optymizm. Funkcja heurystyczna niedoszacowuje gdy minimalizujemy koszt, a przeszacowuje gdy maksymalizujemy zysk.
Drugim warunek jaki musi spełniać funkcja heurystyczna to osiąganie stanu w przestrzeni przy najniższym koszcie, innymi słowy, funkcja heurystyczna musi być monotoniczna
(ang.
monotonicity), czyli
ℎ(𝑛) ≤ 𝑐(𝑛, 𝑛 ) + ℎ(𝑛′)
𝑐(𝑛, 𝑛 ) - jest rzeczywistym kosztem przejścia od stanu n do n’,
Warunek monotoniczności mówi o tym, że im bardziej przybliżamy się do rozwiązania, tym oszacowanie musi być coraz mniej optymistyczne. Błąd oszacowania maleje wraz ze zbliżaniem się do celu. Monotoniczność oznacza, że heurystyka jestspójna w całej przeszukiwanej przestrzeni.
Trzecim problemem przy osiąganiu węzła docelowego jest dobór odpowiedniej heurystyki. Dla dwóch heurystyk h1 i h2, jeżeli h1(n) ≤ h2(n), dla dowolnego stanu n w przeszukiwanej przestrzeni, to h2 zawiera więcej informacji niż h1 (jest lepiej poinformowana).
5. Literatura:
1. Planowanie bezkolizyjnych ścieżek manipulacyjnych i stacjonarnych robotów przemysłowych oparte na procesach decyzyjnych Markowa i funkcji ocen Gabriel G. KOST Gliwice 2004 2. Dynamiczne planowanie trajektorii robotów mobilnych w zmiennej przestrzeni roboczej
Leszek Podsędkowski Łódź 1998 3. https://forbot.pl/blog/wp
4. Manipulatory i roboty mobilne. Modele, planowanie ruchu, sterowanie Krzysztof Tchoń, Alicja Mazur, Ignacy Dulęba, Robert Hossa, Robert Muszyński Warszawa 2000
5. Modelowanie kinematyki mobilnego robota transportowego Marek Stania, Ralf Stetter, Bogdan Posiadała
6. A Guide to Heuristic-based Path Planning Dave Ferguson, Maxim Likhachev, and Anthony Stentz School of Computer ScienceCarnegie Mellon University Pittsburgh, PA, US
7. https://www.aaai.org/Papers/JAIR/Vol28/JAIR-2808.pdf
