Warszawa,2018 Wykład3-Planowanieruchudrinż.JakubMożaryn,mgrinż.JanKlimaszewski Sterowaniemechanizmówwieloczłonowych

Sterowanie mechanizmów wieloczłonowych

Wykład 3 - Planowanie ruchu

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Instytut Automatyki i Robotyki

Warszawa, 2018

Przykład planowania

1 2 3 4 5

A B C D E

h 0

1 2 2 3 3

1 2 3

2 3

cel robot

g g - koszt od węzła do celu

h - oszacowany koszt z węzła do robota (heurystyka)

węzeł otwarty (w kolejce do rozwinięcia) węzeł zamknięty

węzeł v jest rodzicem u (następny na drodze do celu)

u v

Rysunek:przykład planowania

Podstawowe składniki planowania

Stan

Stan - Zestaw współrzędnych opisujących całkowicie rozważany problem.

Przestrzeń stanu to zbiór wszystkich możliwych stanów. W zakresie MW stan można zazwyczaj opisać za pomocą współrzędnych uogólnionych.

Stan początkowy i cel

Stan początkowy i cel - W problemie planowania zazwyczaj znany jest początkowy stan układu. Stosując odpowiednie akcje rozwiązanie problemu powinno pozwolić na doprowadzenie układu do stanu docelowego układu.

Podstawowe składniki planowania

Czas

Czas - W problemach planowania czas może być elementem przestrzeni stanu (time-state space). Czas można też rozumieć jako sekwencyjną kolejność czynności koniecznych do wykonania w celu rozwiązania problemu - np. kolejne ruchy konieczne do ułożenia kostki rubika.

Akcja

Akcja - Stan może ulec zmianie w wyniku akcji. Akcja jest to funkcja, która zmienia stan. Np. akcja ”jedź do przodu”powoduje zmianę położenia robota.

Koszt akcji

Koszt akcji - Koszt związany z wykonaniem akcji. Np. czas trwania akcji.

Algorytmy, plannery i plany

Plan

Plan - Jeśli stan i akcje są deternimistyczne - plan to sekwencja akcji do wykonania. Jeśli przyszłych stanów nie da się łatwo przewidzieć plan może np. definiować akcje jako funkcję aktualnego stanu. Pierwszy przypadek można rozumieć jako ”otwartą pętlę sprzężenia”, drugi przypadek umożliwia ”zamknięcie pętli”.

Planer

Planer - Konstruktor planu. Jeśli planer jest automatyczny, planer to algorytm opracowujący plan.

Ścieżka

Ścieżka - Sekwencja stanów.

Zupełność, wykonalność i optymalność

Wykonalność planu

Wykonalność planu - Plan pozwalający na osiągnięcie przez układ stanu docelowego.

Optymalność planu

Optymalność planu - Plan wykonalny, który optymalizuje pewien ściśle określony wskaźnik związany z wykonaniem planu - np. minimalizuje drogę do celu.

Zupełność algorytmu

Zupełność algorytmu - Planer znajdzie rozwiązanie, jeśli ono istnieje.

Zupełność probabilistyczna algorytmu

Zupełność probabilistyczna algorytmu - Prawdopodobieństwo znalezienia rozwiązania przez planer dąży do 1, gdy czas działania algorytmu dąży do nieskończoności.

Podział algorytmów planowania

Planowanie w środowisku deterministycznym.

Planowanie w obecności niepewności.

Planowanie z uwzględnieniem więzów różniczkowych.

Metody pozostałe.

Podział algorytmów planowania - wiedza o środowisku

Wiedza pełna

wystarczy planowanie globalne (offline)

planowanie optymalne (planowanie ścieżki o najniższym koszcie, np.

A*)

planowanie szybsze ale suboptymalne (anytime, np. ARA*)

Wiedza niepełna

konieczna eksploracja i aktualizacja mapy oraz planu (online) nie da się zapewnić planu optymalnego w skali całej misji (jedynie dla aktualnej wiedzy o świecie)

planowanie globalne przyrostowe (wykorzystanie wiedzy z wcześniejszego planowania)

plan optymalny dla aktualnej wiedzy (np. D*, D* Lite) plan suboptymalny dla aktualnej wiedzy (anytime, np. AD*) planowanie lokalne (uczące się, np. LRTA*)

planowanie w czasie rzeczywistym

Planowanie w środowisku deterministycznym

Istotne aspekty:

więzy są holonomiczne postaci f (x , y , z, t) ­ 0 lub f (x , y , z, t) = 0.

Wybrane metody:

planowanie bez dyskretyzacji przestrzeni konfiguracyjnej,

planowanie bazujące na próbkowaniu (dyskretyzacja przestrzeni w trakcie działania planera),

planowanie ze sprzężeniem zwrotnym.

Planowanie w obecności niepewności

Istotne aspekty:

niepewność wykonania akcji, niepewność stanu,

modelowanie niepewności.

Wybrane metody:

planowanie w obliczu niepewności wykonania akcji, planowanie w obliczu niepewności stanu.

Planowanie z uwzględnieniem więzów różniczkowych

Istotne aspekty:

konstrukcja planu uwzględnia np. więzy postaci f (x , y , z, ˙x , ˙y , ˙z, t) ­ 0 lub f (x , y , z, ˙x , ˙y , ˙z, t) = 0,

może uwzględniać kryteria stabilności i optymalności (równanie Hamiltona-Jacobiego-Bellmana).

Wybrane metody:

metody bazujące na próbkowaniu, metody ze sprzężeniem zwrotnym,

metody odsprzęgające do planowania trajektorii.

Planowanie dla potrzeb MW

Istnieje wiele podejść do planowania. W najprostrzym przypadku, aby otrzymać trajektorię dla MW konieczne będzie:

1 ustalenie przestrzeni konfiguracyjnej (C-space),

2 dyskretyzacja przestrzeni konfiguracyjnej,

3 planowanie w przestrzeni dyskretnej,

4 wyznaczenie planu ciągłego:

generowanie ścieżki (bez uwzględnienia modelu dynamiki i więzów różniczkowych) w postaci sekwencji stanów jako wyniki planowania, generowanie trajektorii (uwzględniając model dynamiki i więzy różniczkowe) na podstawie sekwencji stanów.

Przestrzeń konfiguracyjna MW

Przestrzeń konfiguracyjną C można rozumieć jako uogólnienie S do przestrzeni ciągłej. Planowanie może odbyć się w przestrzeni:

zewnętrznej (kartezjańskiej), zmiennych uogólnionych.

Przestrzeń konfiguracyjna robot 2-DOF

Definicja problemu (1)

W uproszczeniu problem planowania możemy zdefiniować następująco:

Stan Stan układu opisany jest przez wektor współrzędnych konfiguracyjnych:

S =



 q1

q2

. . .



, S ∈ S (1)

S jest zbiorem policzalnym,

S = {S1, S2, . . . , Sn} – zbiór stanów

Stan początkowy i cel Opis problemu planowania powinien definiować cel jako Sgoal ∈ S oraz stan początkowy Sstart ∈ S.

Definicja problemu (2)

Akcja Zastosowanie akcji powoduje zmianę stanu:

S⁰= f (S , U) (2)

, gdzie U - skończony zestaw możliwych akcji.

Przestrzeń akcji Skończona przestrzeń akcji U(S ) definiuje jake akcje są możliwe do wykonania dla stanu S .

Zbiór wszystkich możliwych akcji dla wszystkich stanów można zdefiniować jako:

U = [

S∈S

U(S ) (3)

Planowanie Celem planowania jest znalezienie planu, czyli sekwencji akcji transformujących stan początkowy S_start do stanu docelowego Sgoal.

Wybrane metody dyskretyzacji przestrzeni konfiguracyjnej

Mapa drogowa Regularna siatka Diagram Voronoi Graf widoczności Drzewa przeszukujące

Dyskretyzacja przestrzeni konfiguracyjnej

Mapa drogowa Regularna siatka

Dyskretyzacja przestrzeni konfiguracyjnej

Diagram Voronoi Graf widoczności

qstart

qgoal

Losowe generowanie mapy drogowej

Algorytm PRM (Probabilistic Roadmap) 1:function constructRoadmap 2: V = ∅

3: E = ∅

4: while not construction time elapsed do 5: S_rand= pickAtRandom(Cfree) 6: N = getNeighbourhood(Srand, V ) 7: N = sortByIncreasingDistance(N, Srand) 8: V = V ∪ S_rand

9: for each configuration S_n∈ N do

10: if not sameComponent(qrand, S_n, G ) and connect(Srand, S_n) then 11: E = E ∪ edge(S_rand, S_n)

Dyskretyzacja za pomocą regularnej siatki

Przestrzeń przeszukiwania podzielona na równe komórki Zbiór akcji stanowią ruchy do sąsiednich komórek

8 najbliższych 16 najbliższych

dalszych (akcje zakładają przejście przez kilka komórek)

Planowanie w przestrzeni przybliżonej - zupełność przy zadanej rozdzielczości (dyskretyzacji przestrzeni poszukiwań).

Wybrane metody przeszukiwania grafu

Metody ogólne przeszukiwania grafu w przód

wstecz wszerz w głąb Algorytm Dijkstry Algorytm A*

Algorytm Bellmana-Forda

Szukanie planów wykonalnych - przeszukiwanie w przód vs wstecz

Algorithm 1 Forward search.

1:function Forward-search(Sstart, Sgoal) 2: umieść Sstartna liście węzłów OPEN ← Sstart

3: pobierz z OPEN węzeł u 4: if u = Sgoal then

5: zakończ

6: for all v ∈ neighbours(u) do 7: if g (v ) > g (u) + cost(u, v ) then 8: ustaw g (v ) = g (u) + cost(u, v ) 9: ustaw parent(v ) = u

10: umieść v na liście OPEN 11: idź do linijki 3

Algorithm 2 Backward search.

1:function Backward-search(Sstart, Sgoal) 2: umieść Sgoalna liście węzłów OPEN ← Sgoal

3: pobierz z OPEN węzeł u 4: if u = Sstart then

5: zakończ

6: for all v ∈ neighbours(u) do 7: if g (v ) > g (u) + cost(v , u) then 8: ustaw g (v ) = g (u) + cost(v , u) 9: ustaw parent(v ) = u

10: umieść v na liście OPEN 11: idź do linijki 3

cost(u, v ) – koszt akcji zmiany stanu z u do v

Szukanie planów wykonalnych - algorytm A* (Hart, Nilsson, Raphael 1968)

Algorithm 3 A* search.

1:function A-star-search(Sstart, S_goal) 2: umieść S_startna liście węzłów OPEN ← S_start 3: pobierz z OPEN węzeł u o najmniejszej wartości

f (u) = g (u) + h(u, S_goal) 4: if u = S_goal then

5: zakończ

6: for all v ∈ neighbours(u) do 7: if g (v ) > g (u) + cost(u, v ) then 8: ustaw g (v ) = g (u) + cost(u, v ) 9: ustaw parent(v ) = u

10: umieść v na liście OPEN 11: idź do linijki 3

Algorithm 4 Get plan.

1:function getPlan(Sstart) 2: Π = hi

3: v = S_start 4: while v 6= NULL do 5: Π = hΠ, v i 6: v = parent(v ) 7: return Π

h(u, v ) – heurystyczne oszacowanie odległości między u i v

Losowe drzewa przeszukujące

Algorytm RRT (Rapidly Exploring Random Trees) function KinodynamicRRT(Sstart, Sgoal, K , ∆t)

V = S_start E = ∅

for i = 1 to K do

S_rand= pickAtRandom(Cfree) S_near= nearest(Srand, T )

(S_new, u_new) = selectControl(Snear, S_rand, ∆t) if S_new∈ C/ obsthen

V = V ∪ S_new

E = E ∪ edge(S_near, S_new, u_new) if kS_new− Sgoalk <  then

return success

Losowe drzewa przeszukujące

Algorytm RRT (Rapidly Exploring Random Trees)

Generowanie planu ciągłego dla MW

Generowanie ścieżki (bez uwzględnienia modelu dynamiki i więzów różniczkowych) w postaci sekwencji stanów jako wyniki planowania, generowanie trajektorii (uwzględniając model dynamiki i więzy różniczkowe) na podstawie sekwencji stanów.

Trajektoria

Trajektoria jest to funkcja opisująca stan układu w czasie.

Prędkosć i przyspieszenie układu można wyznaczyć poprzez różniczkowanie trajektorii po czasie.

Dla trajektorii gładkiej prędkosć muci być ciągła, inaczej realizacja trajektorii wymagałaby nieskończonych przyspieszeń.

Wyznaczanie trajektorii Q(t) na podstawie zestawu punktów S (efekt planowania w przestrzeni dyskretnej) można wykonać wieloma metodami.

Najprościej określić sposób wyznaczania trajektorii pomiędzy dwoma punktami, a następnie zastosować to podejście do wszystkich punktów planu.

Metody wyznacznia trajektorii między dwoma punktami

Cubic Polynomial Trajectories Quintic Polynomial Trajectories.

Linear Segments with Parabolic Blends (LSPB).

Wielomian trzeciego stopnia między dwoma punktami - cechy

Przyspieszenie zmienia się liniowo z czasem i jest ciągłe.

Dwa ograniczenia na stan początkowy i końcowy.

Dwa ograniczenia na początkową i końcową prędkość.

Do ustalenia większej liczby ograniczeń (np. przyspieszenia) potrzebne są wielomiany wyższego stopnia.

Wielomian trzeciego stopnia między dwoma punktami - równiania

Definicja trajektorii pomiędzy dwoma stanami Q(t₀) i Q(t_f) za pomocą wielomianu trzeciego stopnia:

Q(t) =



 q1(t) q2(t) . . .



 (4)

qi(t) = ai 0+ ai 1· t + ai 2· t²+ ai 3· t³ (5) , gdzie ai 0, ai 1, ai 2, ai 3 - parametry wielomianu dla qi(t), dalej zapis uproszczony:q(t) oraz a0, a1, a2, a3.

Różniczkując po czasie otrzymujemy prędkość i przyspieszenie:

˙

q(t) = v (t) = a + 2a · t + 3a · t² (6)

Wielomian trzeciego stopnia między dwoma punktami - ograniczenia

Potrzebne są cztery ograniczenia w celu znalezienia parametrów a0, a1, a2, a3.

Dwa ograniczenia na stan początkowy i końcowy:

q(t0) = a0+ a1· t0+ a2· t₀²+ a3· t₀³ (8) q(t_f) = a₀+ a₁· tf + a₂· t_f²+ a₃· t_f³ (9) Dwa ograniczenia na początkową i końcową prędkość:

v0= v (t0) = a1+ 2a2· t0+ 3a3· t₀² (10) vf = v (tf) = a1+ 2a2· tf + 3a3· t_f² (11)

Generowanie trajektorii z wielu punktów

Generowanie trajektorii dla sekwencji stanów S należy wykonać:

wyznaczając parametry wielomianu pomiędzy dwoma kolejnymi stanami,

dla każdej współrzędnej stanu,

przyjmując założenia o ciągłości prędkości w kolejnych punktach.