A LGORYTMIZACJA RUCHU ROBOTÓW

Politechnika Pozna ´nska

Wydział Informatyki

I NSTYTUT A UTOMATYKI I R OBOTYKI

R OZPRAWA DOKTORSKA T ^OMASZ G ^AWRON

A LGORYTMIZACJA RUCHU ROBOTÓW

MOBILNYCH Z OGRANICZENIAMI STANU I WEJ ´S ´C STERUJ ˛ ACYCH W KONTEK ´SCIE METODYKI VFO

P ROMOTOR :

dr hab. in˙z. Maciej Marcin Michałek

Pozna´n 2019

Dzi˛ekuj˛e Promotorowi za opiek˛e merytoryczn ˛ a i cenne uwagi. Dzi˛ekuj˛e

równie˙z wszystkim pozostałym osobom, które wspierały mnie i motywo-

wały do intensywnej pracy naukowej.

Spis tre ´sci

Spis rysunków ... vi

Spis lematów, wniosków, własno´sci i uwag ... x

Spis algorytmów ... xii

1. Streszczenie ... 1

2. Wprowadzenie ... 4

2.1. Notacja ... 4

2.2. Motywacja i postawienie problemu... 5

2.3. Zakres, wkład i struktura rozprawy ... 14

2.4. Przegl ˛ad istniej ˛acych rozwi ˛aza´n... 19

2.4.1. Wst˛ep... 19

2.4.2. Planowanie sekwencji konfiguracji przejazdowych ... 20

2.4.3. Realizacja przejazdu przez zbiór punktów i wyznaczanie tuneli zbie˙zno´sci 25 2.4.4. Planowanie ´scie˙zki ... 26

2.4.5. Odtwarzanie ´scie˙zki i wyznaczanie tuneli zbie˙zno´sci wokół ´scie˙zki re- ferencyjnej ... 29

3. Metodyka algorytmizacji ruchu ... 31

3.1. Generyczny model kinematyczny kołowych robotów mobilnych ... 31

3.2. Koncepcja zastosowania przyj˛etej metodyki dla samochodu kinematycznego... 33

3.3. Modele ´srodowiska ruchu ... 35

3.3.1. Wst˛ep... 35

3.3.2. Model wielok ˛atowy ... 36

3.3.3. Model wypukły... 37

3.3.4. Model poziomicowy ... 38

3.4. Rozpatrywane zadania ruchu w kontek´scie problemów 1 i 2 ... 39

4. Analiza prawa sterowania VFO dla zadania przejazdu przez zbiór punktów... 41

4.1. Prawo sterowania VFO ... 41

iv

v

4.2. Tunele zbie˙zno´sci nominalnej ... 44

4.3. Zmodyfikowane prawo sterowania VFO dla przejazdu przez zbiór punktów... 54

4.4. Tunele zbie˙zno´sci niemonotonicznej... 57

4.5. Tunele zbie˙zno´sci monotonicznej... 60

5. Planowanie sekwencji punktów przejazdowych ... 65

5.1. Planowanie jako nieliniowy dynamiczny problem optymalizacyjny ... 65

5.2. Metody weryfikacji ogranicze´n stanu w tunelach zbie˙zno´sci ... 68

5.3. Suboptymalny zachłanny algorytm planowania ... 73

5.4. Algorytm planowania wykorzystuj ˛acy optymalizacj˛e liniow ˛a... 75

5.4.1. Dyskretyzacja i przygotowanie siatki orientacji ... 76

5.4.2. Strategia planowania... 79

5.4.3. Planowanie w nieograniczonej przestrzeni pozycji ... 83

5.4.4. Planowanie w pojedynczym wielok ˛acie wypukłym ... 86

5.4.5. Planowanie w pełnym modelu wypukłym... 88

5.4.6. Wyniki obliczeniowe i zastosowania... 89

5.5. Próbkuj ˛ace algorytmy planowania ... 97

5.6. Algorytm planowania wykorzystuj ˛acy obliczenia równoległe... 103

6. Realizacja przejazdu przez zbiór punktów w układzie zamkni˛etym - strategia sterowania VFO... 109

6.1. Zapewnienie gładkich przej´s´c mi˛edzy segmentami ruchu ... 109

6.2. Zapewnienie niezmienniczo´sci tuneli zbie˙zno´sci niemonotonicznej ... 113

6.3. Wyniki bada´n symulacyjnych... 117

7. Algorytmizacja ruchu z wykorzystaniem G ³ -ci ˛ agłej ´scie˙zki referencyjnej ... 123

7.1. Sterownik VFO dla zadania odtwarzania ´scie˙zki nieparametryzowanej... 123

7.2. Struktura ´scie˙zek G ³ -ci ˛agłych z ograniczon ˛a krzywizn ˛a... 126

7.2.1. Krzywe wielomianowe ... 126

7.2.2. Uci ˛aglone ´scie˙zki Reedsa-Sheppa ... 128

7.3. Planowanie ´scie˙zek G ³ -ci ˛agłych z ograniczon ˛a krzywizn ˛a ... 133

7.4. Wyznaczanie tuneli zbie˙zno´sci dla ´scie˙zek wielomianowych... 141

7.4.1. Warunki dodatniej niezmienniczo´sci tunelu zbie˙zno´sci ... 141

7.4.2. Algorytm wyznaczania tuneli... 144

7.4.3. Wyniki bada´n symulacyjnych... 149

8. Wyniki bada´n eksperymentalnych ... 152

8.1. Opis stanowiska laboratoryjnego... 152

8.2. Wyniki dla zadania przejazdu przez zbiór punktów ... 155

8.3. Wyniki dla zadania odtwarzania ´scie˙zki... 164

9. Podsumowanie ... 168

Spis rysunków

2.1 Monocykl kinematyczny. . . . . 7

2.2 Samochód kinematyczny z nap˛edem na o´s tyln ˛a. Sylwetka robota zajmuje w przestrzeni pozycji prostok ˛at o wymiarach (a+c)×b definiuj ˛acy zbiór R. Loka- lizacja punktu p _o wykorzystywanego przy sprawdzaniu bezkolizyjno´sci ´scie˙zki jest zale˙zna od chwilowej krzywizny ruchu platformy. . . . . 8

2.3 Schemat ideowy prezentuj ˛acy opracowane metody algorytmizacji ruchu oraz wynikaj ˛ac ˛a z nich struktur˛e rozprawy. . . . . 16

3.1 Przykładowe ´srodowisko ruchu o modelu wielok ˛atowym. . . . . 37

3.2 Przykładowe ´srodowisko ruchu o modelu wypukłym. . . . . 38

3.3 Przykładowe ´srodowisko ruchu o modelu poziomicowym. . . . . 39

4.1 ´Scie˙zki kre´slone przez monocykl przy zastosowaniu sterowania VFO w i-tym segmencie ruchu dla 2 warto´sci parametru µ _i i k _p = 1 przy e ai ≡ 0. . . . . 43

4.2 Krzywe całkowe (4.15) przy zało˙zeniu ¯ q _di = 0 dla jazdy tyłem (strona lewa) i jazdy przodem (strona prawa). Niebieskie i fioletowe krzywe maj ˛a we wszyst- kich punktach warto´sci bezwzgl˛edne krzywizny ni˙zsze ni˙z κ _b . Zatem w warun- kach nominalnych, je´sli pozycja pocz ˛atkowa robota ˜ q ⁱ⁺¹ (t i ) = ˜ q _di ⁱ⁺¹ znajduje si˛e w niebieskim lub fioletowym regionie, to ograniczenie (2.3) jest spełnione w i-tym segmencie ruchu. . . . . 51

4.3 Krzywizna ruchu (4.22) w funkcji pozycji ˜ q ⁱ⁺¹ dla µ _i = 0.75. . . . . 51

4.4 Przedziały θ _{di min} ⁱ⁺¹ , θ ⁱ⁺¹ _{di max}  odpowiadaj ˛ace dopuszczalnym warto´sciom θ ⁱ⁺¹ _di tworz ˛ace wizualizacj˛e tuby ¯ T _i dla przypadku zbie˙zno´sci nominalnej. Tuba jest symetryczna wzgl˛edem osi x ⁱ⁺¹ _di , przy czym jej zbiór pozycji jest ograniczony granicznymi krzywymi całkowymi z ±p bi ze wzgl˛edu na umieszczenie tuby w przestrzeni bezkolizyjnej. . . . . 53

vi

SPIS RYSUNKÓW vii

4.5 Gładka funkcja nasycenia ¯b(z, l) ∈ [−l, l] (tutaj l = 1). . . . . 55 4.6 Tuba zbie˙zno´sci niemonotonicznej pokazana jako funkcja e _ai = f ( ˜ q ⁱ⁺¹ ) dla

µ i = 0.65 i κ b = 2m ⁻¹ w ´srodowisku wolnym od przeszkód. . . . . 60 4.7 Tuba zbie˙zno´sci monotonicznej pokazana jako funkcja e ai = f ( ˜ q ⁱ⁺¹ ) dla µ i =

0.65, κ b = 2 m ⁻¹ , k a = 4 i v d = φ ₂ = 1 m/s, ǫ = 0.1 m w ´srodowisku wolnym od przeszkód. . . . . 63 5.1 Relacja mi˛edzy granicznymi krzywymi całkowymi (tj. krzywymi z parametrem

p bi ) a lini ˛a okre´slaj ˛ac ˛a pozycje punktów przejazdowych o stałej orientacji refe- rencyjnej. Je´sli i-ty punkt przejazdowy le˙zy na tej linii i spełnia 

 x ⁱ⁺¹ _di 

 ≥ x bi , to zagwarantowane jest spełnienie ograniczenia (2.3) w warunkach nominalnych. . 70 5.2 Punkt krytyczny ¯ s _pi dla p-tej kraw˛edzi przeszkody, który wykorzystywany jest

do efektywnego sprawdzania ogranicze´n stanu (tj. kolizji z przeszkod ˛a). W przedstawionej sytuacji i-ty segment ruchu jest niedopuszczalny, gdy˙z krzywa całkowa przecina badan ˛a kraw˛ed´z, co oznacza kolizyjno´s´c zaplanowanego ruchu. 71 5.3 Asymetryczna tuba zbie˙zno´sci niemonotonicznej zawarta w dwóch wielok ˛atach

b˛ed ˛acych cz˛e´sci ˛a ´srodowiska o modelu wypukłym z rozdziału 3.3. Punkty kry- tyczne oznaczono na niebiesko. . . . . 72 5.4 Wyniki planowania sekwencji W w pojedynczym wielok ˛acie wypukłym z wy-

korzystaniem suboptymalnego algorytmu zachłannego. Konfiguracja pocz ˛at- kowa ¯ q d0 została oznaczona kolorem zielonym, a konfiguracja ko´ncowa ¯ q df zo- stała oznaczona kolorem czerwonym. . . . . 75 5.5 Przykładowa dyskretna siatka orientacji i plan orientacji dla θ _d0 = 0.5 rad,

θ dN = 1.3 rad, N L = 1, L θ = 1.6 rad i ∆θ = 0.4 rad. Warto´s´c parametru N p wyznaczono ze wzoru (5.15). Plan orientacji jest generowany z siatki orien- tacji na podstawie denegeneracji automatycznie wybranych segmentów ruchu. . 78 5.6 Przykładowa dyskretna siatka orientacji dla θ _d0 = 0.5 rad, θ dN = −1.5 rad,

N L = 2, L θ = 1.6 rad, and ∆θ = 0.4 rad. Warto´s´c parametru N p wyznaczono ze wzoru (5.15). . . . . 79 5.7 Wyniki scenariusza symulacyjnego SA1, pokazuj ˛acego wpływ parametru w _N

na plany W generowane przez Algorytm 1 i proces ich realizacji. Plan widoczny po lewej stronie wygenerowano z w _N = 0.5, natomiast plan widoczny po prawej stronie wygenerowano przy w N = 0. Zgodnie z przewidywaniami, plan dla w N = 0 stanowi dobre przybli˙zenie najkrótszych ´scie˙zek Dubinsa osi ˛ agni˛ete kosztem du˙zej liczby punktów przejazdowych. . . . . 92 5.8 ´Scie˙zka robota i przebiegi krzywizny podczas symulacyjnej (strona lewa) i eks-

perymentalnej (strona prawa) realizacji scenariusza SA2. Podczas planowania

przyj˛eto parametr ψκ b = 1.6m ⁻¹ . . . . . 93

SPIS RYSUNKÓW viii

5.9 ´Scie˙zka robota i przebiegi krzywizny podczas symulacyjnej (strona lewa) i eks- perymentalnej (strona prawa) realizacji scenariusza SA3. Podczas planowania przyj˛eto parametr ψκ b = 1.6m ⁻¹ . . . . . 94 5.10 Wyniki planowania dla scenariusza SA4. Plan widoczny na górze był pierw-

szym dopuszczalnym rozwi ˛azaniem wyznaczonym przez algortym, a plan wi- doczny na dole jest rozwi ˛azaniem optymalnym dla w N = 0.5. Wybrano para- metry µ i = 0.65 i ψκ b = 2 m ⁻¹ . . . . . 95 5.11 Wyniki planowania dla scenariusza SA5 z planem optymalnym dla w _N = 0.5.

Wybrano parametry µ _i = 0.65 i ψκ b = 2 m ⁻¹ . . . . . 95 5.12 Wyniki planowania i realizacji scenariusza SB1. . . . 102 5.13 Wyniki planowania i realizacji scenariusza SB2. . . . 102 5.14 Symulacja SC1 przejazdu samochodu kinematycznego przez tuby zbie˙zno´sci

niemonotonicznej w ´srodowisku kolizyjnym. Wybrano parametry projektowe:

v _d = 0.2 m/s, k _a = 0.8, k _d = 6, k _p = 1, k _s = 5, k _v = 10, k _w = 20, ζ = 0.5, δ = 0.95, L = 0.5 m, β m = π/3 rad, ǫ = 0.2 m. . . . 108 6.1 Funkcja ˜b(z). . . . 110 6.2 Wizualizacja zbioru dopuszczalnych sterowa´n V dla monocykla. . . . 112 6.3 Automat sko´nczony A wykorzystywany sterowaniu Φ _n z (6.9). Zmienne przy-

pisywane s ˛a w kolejno´sci od lewej do prawej. Czasy ¯t 1 and ¯t 2 oznaczaj ˛a chwile wej´scia automatu w stany S ₁ i S ₂ . Przyjmujemy σ ₋₁ ≡ σ ₀ w definicji stanów S ₁ i S ₂ . . . . 114 6.4 Symulacja SC1 przejazdu samochodu kinematycznego przez tuby zbie˙zno´sci

niemonotonicznej w ´srodowisku kolizyjnym. Wybrano parametry projektowe:

v d = 0.2 m/s, k a = 0.8, k d = 6, k p = 1, k s = 5, k v = 10, k w = 20, ζ = 0.5, δ = 0.95, L = 0.5 m, β _m = π/3 rad, ǫ = 0.2 m. . . . 119 6.5 Symulacja SC2 pokazuj ˛aca działanie automatu A w celu utrzymania samo-

chodu kinematycznego w tubie zbie˙zno´sci przy celowo obni˙zonym wzmocnie- niu k a . Wybrano parametry projektowe: v d = 0.2 m/s, k a = 0.3, k d = 6, k p = 1, k s = 5, k v = 10, k w = 20, ζ = 0.5, δ = 0.95, L = 0.5 m, β m = π/3 rad, ǫ = 0.2 m. . . . 120 6.6 Symulacja SC5 pokazuj ˛aca działanie automatu A w celu utrzymania monocykla

w tubie zbie˙zno´sci przy celowo obni˙zonym wzmocnieniu k a . Wybrano parame- try projektowe: µ _i = 0.65, κ _b = 4 m ⁻¹ , k _s = 5, k _v = 5, k _w = 1, k _a = 0.2, k _p = 0.1, v _d = 0.2 m/s, ǫ = 0.2m, ψ = 0.5. . . . 121 6.7 Symulacja SC6 przejazdu monocykla przez tuby zbie˙zno´sci niemonotonicznej

w ´srodowisku kolizyjnym. Wybrano parametry projektowe: κ b = 4 m ⁻¹ , k s =

5, k v = 5, k w = 1, k a = 0.8, k p = 0.4, v d = 0.4 m/s, ǫ = 0.2m. . . . 122

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

SPIS RYSUNKÓW ix

7.1 Powierzchnia przykładowej funkcji F ( ˜ q) z oznaczonym na czerwono zbiorem Q ˜ d (dla którego spełniona jest zale˙zno´s´c F ( ˜ q) = 0) stanowi ˛ acym ´scie˙zk˛e refe- rencyjn ˛a w postaci krzywej poziomicowej. . . . 124 7.2 Wyja´snienie symboli wykorzystanych w definicji (7.10) dla i = 1 (strona lewa)

i powierzchnia wygładzaj ˛aca C _i (L i ) (strona prawa). . . . 128 7.3 Porównanie standardowej nieci ˛agłej reprezentacji funkcji F ( ˜ q) (strona lewa) i

proponowanej uci ˛aglonej reprezentacji (strona prawa). . . . 128 7.4 Struktura i parametry G ³ -ci ˛agłego prymitywu ´scie˙zki. . . . 130 7.5 G ³ -ci ˛agłe prymitywy ´scie˙zki dla wybranej warto´sci parametru µ (strona lewa)

wraz z ich profilami krzywizny (strona prawa). . . . 131 7.6 (a) Maksymalna warto´s´c pochodnej krzywizny κ _d (s) segmentu przej´sciowego

w funkcji parametru µ (strona lewa) i przebieg tej pochodnej dla wybranych warto´sci parametru µ (strona prawa). . . . 132 7.7 Wizualizacja procedury predykuj ˛acej wykorzystuj ˛acej G ³ -ci ˛agły prymityw

´scie˙zki. Uwolnione punkty ko´ncowe musz ˛a le˙ze´c na okr˛egach pomocniczych o promieniu R. Nale˙zy wyznaczy´c odpowiednie długo´sci łuków okr˛egów, które pozwol ˛a na poł ˛aczenie dwóch wolnych ko´nców segmentów (tj. przeciwnych ko´nców prymitywów wychodz ˛acych z konfiguracji pocz ˛atkowej i ko´ncowej) odcinkiem bez wprowadzania nieci ˛agło´sci w orientacji zadanej wzdłu˙z ´scie˙zki referencyjnej. . . . 134 7.8 Krzywizna κ o ruchu punktu zewn˛etrznego p 0 (por. (7.19)), który znajduje si˛e

najdalej od ´scie˙zki, wyra˙zona w funkcji krzywizny ´scie˙zki κ _d dla a = 5.9 m i b = 2.5 m. Czerwona linia przerywana odpowiada konserwatywnemu przybli-

˙zeniu tej zale˙zno´sci, które wykorzystywane jest przy sprawdzaniu bezkolizyj- no´sci ´scie˙zki. . . . 137 7.9 ´Scie˙zka referencyjna i profil krzywizny zaplanowane w scenariuszu SP1 odpo-

wiadaj ˛acym niemonotonicznemu manewrowi parkowania. . . . 139 7.10 ´Scie˙zka referencyjna i profil krzywizny zaplanowane w scenariuszu SP2 odpo-

wiadaj ˛acym niemonotonicznemu manewrowi parkowania. . . . 140 7.11 ´Scie˙zka referencyjna i profil krzywizny zaplanowane w scenariuszu SP3 odpo-

wiadaj ˛acym manewrowi zmiany pasa. . . . 140 7.12 Wizualizacja warunków bezkolizyjno´sci tunelu zbie˙zno´sci wokół ´scie˙zki refe-

rencyjnej (strona lewa) i przykładowa przeszkoda G opisana w modelu pozio- micowym przy pomocy uogólnionego owalu Cassiniego (strona prawa). . . . . 147 7.13 Wyniki symulacji SD1, w której robot o kinematyce samochodowej odtwarzał

´scie˙zk˛e referencyjn ˛a znajduj ˛ac si˛e przy tym w tunelu zbie˙zno´sci F(r a , r F ). . . 150

SPIS RYSUNKÓW x

7.14 Wyniki symulacji SD2, w której robot o kinematyce samochodowej odtwarzał

´scie˙zk˛e referencyjn ˛a znajduj ˛ac si˛e przy tym w tunelu zbie˙zno´sci F(r a , r F ). W tym przypadku wymuszono du˙z ˛a warto´s´c pocz ˛atkow ˛a bł˛edu k ˛ata skr˛etu |e d |,

aby pokaza´c działanie wst˛epnej pomocniczej strategii sterowania. . . . 151

8.1 Schemat funkcjonalny stanowiska badawczego, na którym przeprowadzono eksperymenty w celu walidacji proponowanych metod algorytmizacji. Skrót est. oznacza sygnały estymowane. . . . 153

8.2 Zdj˛ecia stanowiska badawczego podczas realizacji zadania odtwarzania ´scie˙zki. 154 8.3 Zdj˛ecia stanowiska badawczego podczas realizacji zadania przejazdu przez zbiór punktów. . . . 154

8.4 ´Scie˙zka robota i przebiegi krzywizny podczas symulacyjnej (strona lewa) i eks- perymentalnej (strona prawa) realizacji scenariusza SA2. Podczas planowania przyj˛eto parametr ψκ _b = 1.6m ⁻¹ . . . . 156

8.5 Przebiegi sygnałów steruj ˛acych i uchybów podczas symulacyjnej (strona lewa) i eksperymentalnej (strona prawa) realizacji scenariusza SA2. . . . 157

8.6 ´Scie˙zka robota i przebiegi krzywizny podczas symulacyjnej (strona lewa) i eks- perymentalnej (strona prawa) realizacji scenariusza SA3. Podczas planowania przyj˛eto parametr ψκ _b = 1.6m ⁻¹ . . . . 158

8.7 Przebiegi sygnałów steruj ˛acych i uchybów podczas symulacyjnej (strona lewa) i eksperymentalnej (strona prawa) realizacji scenariusza SA3. . . . 159

8.8 Wyniki planowania i realizacji scenariusza SB1. . . . 161

8.9 Wyniki planowania i realizacji scenariusza SB2. . . . 161

8.10 Wyniki planowania i realizacji scenariusza SC7. . . . 162

8.11 Przebiegi sygnałów steruj ˛acych, k ˛ata skr˛etu β i uchybów dla scenariusza SC7. . 163

8.12 Wyniki planowania i realizacji scenariusza SD3. . . . 165

8.13 Przebiegi sygnałów steruj ˛acych, k ˛ata skr˛etu β i bł˛edów dla scenariusza SD3. . . 166

8.14 Przebiegi sygnałów steruj ˛acych, k ˛ata skr˛etu β i bł˛edów dla scenariusza SD3 z trudniejszym warunkiem pocz ˛atkowym skutkuj ˛acym wy˙zszym bł˛edem e _a (t = 0). 167 9.1 Proponowane rozwi ˛azania poszczególnych wariantów problemów algorytmiza- cji. VFO-WF oznacza metody algorytmizacji wykorzystuj ˛ace zadanie przejazdu przez zbiór punktów, a VFO-PF metody algorytmizacji wykorzystuj ˛ace zadanie odtwarzania ´scie˙zki nieparametryzowanej. . . . 169

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

Spis lematów, wniosków, własno ´sci i uwag

1 Uwaga . . . . 8

1 Problem (Algorytmizacja ruchu monocykla kinematycznego) . . . . 9

2 Problem (Algorytmizacja ruchu samochodu kinematycznego) . . . . 10

2 Uwaga . . . . 11

Teza . . . . 12

3 Uwaga . . . . 13

4 Uwaga . . . . 13

5 Uwaga . . . . 13

6 Uwaga . . . . 13

7 Uwaga . . . . 35

8 Uwaga . . . . 36

1 Lemat . . . . 44

9 Uwaga . . . . 44

1 Własno´s´c . . . . 45

10 Uwaga . . . . 45

1 Wniosek . . . . 47

2 Własno´s´c . . . . 47

2 Wniosek . . . . 50

2 Lemat . . . . 55

3 Lemat . . . . 57

11 Uwaga . . . . 64

3 Wniosek . . . . 64

3 Własno´s´c . . . . 68

12 Uwaga . . . . 78

SPIS LEMATÓW, WNIOSKÓW, WŁASNO ´SCI I UWAG xii

13 Uwaga . . . . 78

14 Uwaga . . . . 78

15 Uwaga . . . . 90

16 Uwaga . . . 100

4 Lemat . . . 110

4 Wniosek . . . 113

5 Lemat . . . 115

6 Lemat . . . 116

5 Wniosek . . . 116

7 Lemat . . . 146

6 Wniosek . . . 148

17 Uwaga . . . 155

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

Spis algorytmów

1 Strategia planowania z wykorzystaniem optymalizacji liniowej . . . . 80

2 Przetwarzanie wst˛epne siatki orientacji . . . . 80

3 Zrównoleglony algorytm próbkuj ˛acy planuj ˛acy sekwencj˛e W i tuby zbie˙zno´sci. 106

4 Wyznaczanie tunelu zbie˙zno´sci ¯ F(r _a ^∗ , r ^∗ _F ). . . . 145

1

Streszczenie

W niniejszej rozprawie zaprezentowano wyniki teoretyczne, symulacyjne oraz eksperymentalne dotycz ˛ace nowych metod algorytmizacji (tj. planowania i realizacji) ruchu wybranych kołowych robotów mobilnych. W metodach tych wykorzystano techniki planowania ruchu oraz algorytmy sterowania w układzie ze sprz˛e˙zeniem zwrotnym, które s ˛a ze sob ˛a ´sci´sle poł ˛aczone w celu zapewnienia odpornej realizacji ruchu. Proponowane metody algorytmizacji opracowano dla generycznego modelu kinematycznego w postaci monocykla z dodatkowo narzuconym ograni- czeniem krzywizny ruchu. Rozpatrzono tak˙ze wyst˛epowanie ograniczonych amplitud sygnałów steruj ˛acych, ograniczenia monotoniczno´sci ruchu (tj. wymuszony znak pr˛edko´sci post˛epowej robota) oraz ograniczenia stanu wynikaj ˛ace z obecno´sci przeszkód w ´srodowisku ruchu robota i ograniczonej powierzchni tego ´srodowiska. Nast˛epnie rozszerzono wprowadzone metody, aby zapewni´c algorytmizacj˛e ruchu samochodu kinematycznego z ograniczonymi sygnałami steru- j ˛acymi i ograniczonym k ˛atem skr˛ecenia koła skr˛etnego.

W odró˙znieniu od wielu podej´s´c spotykanych w literaturze, zaproponowano metody algo- rytmizacji, w których techniki sterowania i planowania ruchu s ˛a ze sob ˛a ł ˛aczone ju˙z na etapie projektowania. To podej´scie zaowocowało nowymi algorytmami planowania, których działanie jest motywowane sterowaniem. Algorytmy te wykorzystuj ˛a informacje o dodatnio niezmienni- czych tunelach zbie˙zno´sci wyznaczonych dla układu zamkni˛etego z prawem sterowania VFO (z ang. Vector Field Orientation) stosowanym w proponowanych metodach algorytmizacji ru- chu. Podczas projektowania tych algorytmów planowania wykorzystano równie˙z szereg ko- rzystnych własno´sci układu zamkni˛etego z prawem sterowania VFO, które pozwalaj ˛a na we- ryfikacj˛e spełnienia ogranicze´n w ci ˛agłej dziedzinie czasu i w ci ˛agłej dziedzinie konfiguracji robota. Na potrzeby proponowanych metod algorytmizacji, dokonano tak˙ze modyfikacji w pra- wach sterowania VFO dla przejazdu przez zbiór punktów i odtwarzania ´scie˙zki. Modyfika- cje te zapewniaj ˛a spełnienie wspomnianych ogranicze´n stanu i sygnałów steruj ˛acych podczas procesu sterowania. Zmodyfikowane prawa sterowania VFO współpracuj ˛ace z dedykowanymi (motywowanymi procesem sterowania) algorytmami planowania składaj ˛a si˛e na metody algo- rytmizacji ruchu, które odporne s ˛a na wyst˛epowanie nienominalnych warunków ruchu dzi˛eki

1

2

wyznaczonym dodatnio niezmienniczym tunelom zbie˙zno´sci. W odró˙znieniu od wielu znanych podej´s´c, proponowane metody algorytmizacji gwarantuj ˛a spełnienie ogranicze´n stanu i wej´s´c steruj ˛acych w ci ˛agłej dziedzinie czasu oraz w ci ˛agłej dziedzinie konfiguracji robota.

Efektywno´s´c proponowanych metod algorytmizacji została zilustrowana wynikami bada´n

symulacyjnych oraz wynikami eksperymentalnymi uzyskanymi na laboratoryjnym robocie mo-

bilnym. Niski koszt obliczeniowy tych metod algorytmizacji ruchu został potwierdzony wyni-

kami symulacji, podczas których porównano proponowane metody z podej´sciami znanymi z

literatury.

Abstract

This dissertation describes the theoretical, simulation-based, and experimental results concer- ning the novel motion algorithmization methods (i.e., motion planning and robust motion exe- cution via feedback control) for selected wheeled mobile robots. The proposed motion algorith- mization methods comprise tightly integrated motion planning and feedback control techniques.

They have been developed for a generic vehicle-body kinematics in the form of a unicycle with an additionally imposed motion curvature constraint. The presence of limited control inputs and state constraints arising from the presence of obstacles in the bounded motion environment has been also considered. The generic motion algorithmization methods have been subsequently extended to handle car-like kinematics with limited control inputs and limited steering angle.

Contrary to numerous approaches found in the literature, motion algorithmization methods proposed in this dissertation combine planning and feedback control algorithms at the core of the design process. Such an approach resulted in new controller-driven planning algorithms benefitting from information about evolution of the closed-loop system with the VFO (Vector Field Orientation) control law, which comes in form of positively invariant funnels and specific properties useful in verification of constraints. The aforementioned state and input constraints are guaranteed to be satisfied in continuous time and configuration domains, contrary to many methods known from the literature. For the purposes of motion algorithmization in the presence of state and input constraints, modifications of the VFO control laws for waypoint following and path following have been introduced. Modified VFO control laws combined with dedica- ted (controller-driven) planning algorithms create motion algorithmization methods, which are robust to nonnominal motion conditions and guarantee satisfaction of constraints in continuous time and configuration domains.

Effectiveness of the proposed motion algorithmization methods has been illustrated through simulations and experimental results obtained using a laboratory-scale wheeled mobile robot.

The low computational cost of the proposed motion algorithmization methods has been verified by the results of simulations, during which the proposed motion algorithmization methods have been compared with state of the art.

3

2

Wprowadzenie

2.1. Notacja

W niniejszej pracy przyj˛eto nast˛epuj ˛ace zało˙zenia dotycz ˛ace notacji:

– R, R ₊ oznaczaj ˛a odpowiednio zbiór liczb rzeczywistych i zbiór dodatnich liczb rzeczy- wistych wraz z zerem,

– R ⁿ oznacza n-wymiarow ˛a przestrze´n Euklidesow ˛a,

– wielko´sci wektorowe oznaczono małymi literami i pismem pogrubionym, np. v ∈ R ⁿ , – macierze oznaczono du˙zymi literami i pismem pogrubionym, np. A ^m×n ,

– symbol , oznacza równo´s´c z definicji, natomiast := oznacza przypisanie,

– zbiory i przestrzenie wektorowe oznaczono du˙zymi literami kaligrafowanymi, np. U, – f(·) oznacza funkcj˛e f o niezdefiniowanych argumentach, a f(x, ·) oznacza funkcj˛e o

cz˛e´sciowo zdefiniowanych argumentach,

– przedziały zamkni˛ete oznaczono nawiasami kwadratowymi, natomiast przedziały otwarte nawiasami okr ˛agłymi,

– zmienn ˛a czasu rzeczywistego oznaczono przez t, a parametr długo´sci krzywoliniowej

´scie˙zki przez s,

– warto´sci zadane oznaczane s ˛a indeksem dolnym d, np. q d ,

– konfiguracja robota o kinematyce samochodowej oznaczona jest wektorem q = h

β ¯ q ^⊤ i ⊤

= h β θ ˜ q ^⊤

i ⊤

= h

β θ x y i ⊤

∈ R ⁴ , gdzie β jest k ˛atem skr˛ecenia koła przed-

niego (koła wirtualnego), θ jest k ˛atem orientacji platformy, a x i y to współrz˛edne punktu

2.2. Motywacja i postawienie problemu 5

prowadzenia robota. Wektor ¯ q stanowi konfiguracj˛e platformy robota o kinematyce sa- mochodowej i tym samym konfiguracj˛e monocykla. Wektorem ˜ q oznaczono natomiast pozycj˛e platformy.

– poza szczególnymi przypadkami, poszczególne punkty przejazdowe, b ˛ad´z segmenty

´scie˙zki oznaczane s ˛a indeksem dolnym i, np. ¯ q _di ,

– indeksem górnym i+1 oznaczane s ˛awielko´sci wyra˙zone w układzie lokalnym i+1-szego punktu przejazdowego, np. ¯ q _di ⁱ⁺¹ ,

– stosuje si˛e skrócon ˛a form˛e zapisu niektórych funkcji trygonometrycznych, tj. cα ≡ cos α i sα ≡ sin α,

– k ·k oznacza norm˛e Euklidesow ˛a, – dla wektorów a = h

a 1 a 2 . . . a n

i ⊤

∈ R ⁿ , b h

b 1 b 2 . . . b n

i ⊤

∈ R ⁿ zapis a ≤ b oznacza układ n osobnych nierówno´sci: a ₁ ≤ b ₁ , a 2 ≤ b ₂ , . . . , a n ≤ b _n ,

– pochodne sygnałów liczone wzgl˛edem czasu t oznaczono symbolami kropek, tj. ^dx _dt =

˙x, ^d _dt

^x = ¨ x, ^d _dt

^x = ...

x,

– skrót p.o. spotykany przy definicjach problemów optymalizacyjnych oznacza „przy ogra- niczeniach",

– funkcj˛e znaku zdefiniowano nast˛epuj ˛aco

sgn (z) ,







−1 dla z < 0, 1 dla z ≥ 0, – funkcja saturacji zdefiniowana została nast˛epuj ˛aco:

sat(z, l) , sgn (z) min (|z| , l) ,

gdzie z stanowi wielko´s´c podlegaj ˛ac ˛a nasyceniu, a l > 0 oznacza próg nasycenia.

2.2. Motywacja i postawienie problemu

Niezawodna automatyzacja zwinnych, zło˙zonych manewrów wykonywanych przez kołowe

roboty mobilne w ´srodowisku kolizyjnym jest kluczowa w wa˙znych obecnie zastosowaniach

robotyki mobilnej takich jak zaawansowane systemy wspomagania kierowców, autonomiczne

pojazdy drogowe i maszyny rolnicze, czy te˙z roboty ratowniczo-eksploracyjne. Ze wzgl˛edu na

obecno´s´c przeszkód w ´srodowisku ruchu, specyficzne cechy konstrukcji mechanicznej pojaz-

dów i dodatkowe wymagania zale˙zne od konkretnych zastosowa´n, rozwi ˛azanie wspomnianego

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.2. Motywacja i postawienie problemu 6

problemu automatyzacji manewrów wi ˛a˙ze si˛e z konieczno´sci ˛a rozpatrywania systemu dyna- micznego z ograniczeniami stanu i wej´s´c steruj ˛acych. Badacze nadal intensywnie poszukuj ˛a (co potraktowano w rozdziale 2.4) rozwi ˛aza´n tego problemu charakteryzuj ˛acych si˛e formalnymi gwarancjami spełnienia ogranicze´n, niskim kosztem obliczeniowym, odporno´sci ˛ana zakłócenia i niepewno´sci pomiarowe oraz elastyczn ˛a (tj. łatw ˛a do pozyskania na podstawie znanych metod mapowania) reprezentacj ˛a ´srodowiska ruchu. Wspomniana zło˙zono´s´c manewrów powoduje,

˙ze ich uprzednie zaplanowanie jest niemal˙ze konieczne, gdy˙z zaprojektowanie praw sterowa- nia realizuj ˛acych te manewry bez ich uprzedniej dekompozycji na podzadania jest problemem niezmiernie trudnym. Z drugiej strony warto tak˙ze zauwa˙zy´c, ˙ze realizacja zaplanowanego ma- newru w p˛etli otwartej (tj. bez sterowania ze sprz˛e˙zeniem zwrotnym od stanu robota) sprawia,

˙ze wynikowe rozwi ˛azanie nie jest odporne na zakłócenia i niepewno´sci pomiarowe obecne w warunkach rzeczywistych. Zatem skuteczne rozwi ˛azanie problemu automatyzacji manewrów kołowych robotów mobilnych powinno składa´c si˛e z umiej˛etnego poł ˛aczenia algorytmów pla- nowania ruchu i metod sterowania ze sprz˛e˙zeniem zwrotnym słu˙z ˛acych do odpornej algorytmi- zacji ruchu.

W niniejszej rozprawie poł ˛aczenie algorytmów planowania ruchu i metod sterowania ze sprz˛e˙zeniem zwrotnym w celu automatyzacji manewrów okre´slane jest mianem algorytmizacji ruchu. Celem metod algorytmizacji ruchu jest wypracowanie sygnałów steruj ˛acych zapewnia- j ˛acych przeprowadzenie robota z konfiguracji pocz ˛atkowej do zadanej konfiguracji ko´ncowej przy jednoczesnym spełnieniu zadanych ogranicze´n. Przed sformalizowaniem definicji zada- nia algorytmizacji ruchu, nale˙zy poda´c rozpatrywane modele robotów mobilnych. Rozwa˙zone zostan ˛a dwie struktury kinematyczne robotów mobilnych. Pierwsz ˛a z nich jest monocykl z na- rzuconym ograniczeniem krzywizny ruchu, którego model kinematyczny mo˙zna przedstawi´c nast˛epuj ˛aco:

˙¯q =









1 0

0 cos θ 0 sin θ









"

v 1

v 2

#

= G( ¯ q)v, (2.1)

κ = v ₁ v 2

, (2.2)

|κ| ≤ κ b , (2.3)

v =

"

v 1

v 2

#

∈ V, V , {v : |κ| ≤ κ b ∧ |v 1 | ≤ v 1B ∧ |v 2 | ≤ v 2B } (2.4)

gdzie ¯ q = [θ x y] ^⊤ = [θ ˜ q ^⊤ ] ^⊤ ∈ ¯ Q stanowi wektor współrz˛ednych konfiguracyjnych monocy-

kla (nazywany dalej tak˙ze konfiguracj ˛a platformy) składaj ˛acy si˛e z jego orientacji θ oraz współ-

rz˛ednych pozycji x i y (patrz rys. 2.1), ¯ Q ≡ R ³ oznacza przestrze´n konfiguracyjn ˛a monocykla,

a v ∈ V stanowi wej´scie steruj ˛ace monocykla, którego składowymi s ˛a pr˛edko´s´c k ˛atowa plat-

formy v 1 i pr˛edko´s´c post˛epowa v 2 punktu prowadzenia platformy. Przestrze´n dopuszczalnych

2.2. Motywacja i postawienie problemu 7

Rysunek 2.1: Monocykl kinematyczny.

sterowa´n V jest ograniczona w taki sposób, aby spełni´c nierówno´s´c (2.3), w której κ oznacza krzywizn˛e ruchu monocykla, a κ b > 0, v 1B > 0 i v 2B > 0 oznaczaj ˛ a odpowiednio maksy- maln ˛a dopuszczaln ˛a krzywizn˛e ruchu, maksymaln ˛a dopuszczaln ˛a pr˛edko´s´c k ˛atow ˛a platformy i maksymaln ˛a dopuszczaln ˛a pr˛edko´s´c post˛epow ˛a platformy.

Pomimo swej prostoty, kinematyka (2.1) wykazuje fundamentalne trudno´sci w sterowaniu wynikaj ˛ace z ogranicze´n nieholonomicznych, które wystepuj ˛a w przypadku znakomitej wi˛ek- szo´sci kołowych robotów mobilnych. Dost˛epno´s´c jedynie ograniczonych sygnałów steruj ˛acych wyra˙zona ograniczeniem (2.4) wyst˛epuje w zastosowaniach praktycznych. Dodatkowe narzu- cone ograniczenie krzywizny (2.3) nie wynika ze specyfiki kinematyki monocykla, jest ono jed- nak przydatne w wielu zastosowaniach i kluczowe w proponowanej metodyce algorytmizacji ruchu. Wykorzystanie monocykla z ograniczon ˛a krzywizn ˛a jako generyczny model kinematyki robota mobilnego zostało opisane szerzej w rozdziale 3.1.

Drugim, bardziej zło˙zonym, modelem kinematycznym robota mobilnego rozwa˙zonym w niniejszej rozprawie jest samochód kinematyczny z nap˛edem na o´s tyln ˛a. Zgodnie z dekompo- zycj ˛a przedstawion ˛a w [64], jego model kinematyczny mo˙zna opisa´c równaniami:

β = u ˙ 1 , (2.5)

˙¯q = G( ¯ q)v, v := h

u

L tan β u ₂ i ⊤

∈ R ² , (2.6)

u =

"

u 1

u 2

#

∈ U, U , {u : |u 1 | ≤ u _1B ∧ |u ₂ | ≤ u _2B }, (2.7)

|β| ≤ β m < π/2, (2.8)

gdzie q = [β θ x y] ^⊤ = [β ¯ q ^⊤ ] ^⊤ = [β θ ˜ q ^⊤ ] ^⊤ ∈ Q = [−β _m , β _m ] × ¯ Q oznacza wektor

współrz˛ednych konfiguracyjnych, w którym β oznacza k ˛at skr˛ecenia koła przedniego, przy

czym β _m < π/2 stanowi maksymalny k ˛ at skr˛ecenia koła, a pozostałe składowe wektora q

maj ˛a znaczenie identyczne jak w przypadku monocykla. Wirtualne wej´scie steruj ˛ace v kinema-

tyki platformy (2.6) to˙zsamej z kinematyk ˛a monocykla, składa si˛e z kolei z pr˛edko´sci k ˛atowej

v 1 platformy i pr˛edko´sci post˛epowej v 2 = u 2 punktu prowadzenia ˜ q = [x y] ^⊤ widocznego na

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.2. Motywacja i postawienie problemu 8

Rysunek 2.2: Samochód kinematyczny z nap˛edem na o´s tyln ˛a. Sylwetka robota zajmuje w prze- strzeni pozycji prostok ˛at o wymiarach (a + c) × b definiuj ˛acy zbiór R. Lokalizacja punktu p _o wykorzystywanego przy sprawdzaniu bezkolizyjno´sci ´scie˙zki jest zale˙zna od chwilowej krzy- wizny ruchu platformy.

rys. 2.2. Macierz G zdefiniowano w równaniu (2.1). L > 0 jest natomiast odległo´sci ˛a pomi˛e- dzy osi ˛a tyln ˛a i osi ˛a przedni ˛a (patrz rys. 2.2). Wej´scie steruj ˛ace u samochodu kinematycznego składa si˛e natomiast z pr˛edko´sci k ˛atowej dla k ˛ata skr˛ecenia koła przedniego, któr ˛a oznaczono przez u ₁ oraz pr˛edko´sci post˛epowej punktu prowadzenia platformy u ₂ ≡ v 2 . Przestrze´n dopusz- czalnych sterowa´n U z definicji (2.7), w których u 1B > 0 i u 2B > 0 oznaczaj ˛ a odpowiednio maksymaln ˛a dopuszczaln ˛a warto´s´c sterowania u ₁ i maksymaln ˛a dopuszczaln ˛a pr˛edko´s´c post˛e- pow ˛aplatformy. Warto zwróci´c uwag˛e na to, ˙ze wyst˛epuje tutaj ograniczenie (2.8) wynikaj ˛ace z konstrukcji mechanicznej pojazdów spotykanych w praktyce, które jednocze´snie pozwala wy- ł ˛aczy´c z przestrzeni konfiguracyjnej Q konfiguracje osobliwe wyst˛epuj ˛ace dla β = ±π/2.

Uwaga 1. W przytoczonych modelach monocykla i samochodu kinematycznego k ˛ at orientacji θ jest wielko´sci ˛ a ci ˛ agł ˛ a, tj. θ ∈ R. Taka reprezentacja k ˛ ata orientacji została wybrana spo-

´sród kilku innych mo˙zliwo´sci ze wzgl˛edu na uproszczenie prowadzonej analizy i zachowanie spójno´sci z metodyk ˛ a VFO (z ang. Vector Field Orientation).

Zdefiniowane zostan ˛a teraz problemy algorytmizacji ruchu. Rozwa˙zmy kompaktowy, jed- nospójny (poł ˛aczony) zbiór

Q ¯ f ⊆ ¯ Q (2.9)

stanowi ˛acy podzbiór przestrzeni konfiguracyjnej platformy. Zbiorem ¯ Q f opisywa´c b˛edziemy

przestrze´n woln ˛a od przeszkód w ograniczonym ´srodowisku ruchu robota. Przez przeszkody

rozumie si˛e tutaj zarówno obszary fizycznie niedost˛epne jak i fizycznie wolne, lecz zabro-

nione przez u˙zytkownika z ró˙znych wzgl˛edów. Zbiór ¯ Q _f definiowany b˛edzie na ró˙zne sposoby,

których wybór zale˙zny jest od zastosowania i wybranego algorytmu planowania ruchu. Opis

wszystkich definicji zastosowanych w tej rozprawie wraz z dyskusj ˛a ich zastosowa´n przedsta-

wiono w rozdziale 3.3. Pomijaj ˛ac chwilowo szczegóły definicji zbioru ¯ Q f mo˙zemy zapisa´c roz-

patrywane problemy algorytmizacji w sposób ogólny. Oznaczmy konfiguracj˛e pocz ˛atkow ˛aplat-

2.2. Motywacja i postawienie problemu 9

formy przez

¯

q d0 = [θ d0 x d0 y d0 ] ^⊤ = [θ d0 q ˜ _d0 ^⊤ ] ^⊤ , a konfiguracj˛e ko´ncow ˛a platformy przez

¯

q df = [θ df x df y df ] ^⊤ = [θ df q ˜ ^⊤ _df ] ^⊤ .

Wprowad´zmy te˙z symbole ˜ǫ _d ≥ 0 i ǫ _θd ≥ 0 oznaczaj ˛ ace kolejno zadane dokładno´sci realizacji zadanej pozycji oraz orientacji. Algorytmizacja ruchu z tymi dokładno´sciami w horyzoncie czasowym T ∈ (0, ∞) oznacza, ˙ze zachodz ˛a relacje

˜ǫ(t) −→ ˜ǫ ^t→T d ∧ ǫ θ (t) −→ ǫ ^t→T θd ∧ (∀t ≥ 0 k ˜ e(t)k ≤ ˜ǫ(t) ∧ |e θ (t)| ≤ ǫ θ (t)) , (2.10) gdzie bł ˛ad pozycji ˜ e(t) i bł ˛ ad orientacji e _θ (t) zdefiniowane s ˛ a nast˛epuj ˛aco

˜

e(t) , ˜ q df − ˜ q(t), e θ (t) , (θ df − θ(t)) mod 2π.

Relacje (2.10) s ˛a wspólne dla obu zdefiniowanych poni˙zej problemów algorytmizacji. Podob- nie, wspólnym dla nich jest poj˛ecie tunelu zbie˙zno´sci. Tunel zbie˙zno´sci ¯ F ⊆ ¯ Q _f dla monocykla i tunel zbie˙zno´sci F ⊆ Q _f dla samochodu kinematycznego stanowi ˛a dodatnio niezmiennicze podzbiory dopuszczalnej przestrzeni konfiguracyjnej. Konfiguracja robota pozostaje utrzymana w tunelu zbie˙zno´sci w całym horyzoncie czasowym algorytmizacji ruchu, co pozwala na zapew- nienie bezkolizyjno´sci ruchu. Poj˛ecie to wykorzystywane jest w zdefiniowanych ni˙zej dwóch problemach algorytmizacji rozpatrywanych w tej rozprawie.

Problem 1 (Algorytmizacja ruchu monocykla kinematycznego).

Dla wybranego horyzontu czasowego realizacji ruchu T ∈ (0, ∞), zbioru strategii ruchu S, zadanej konfiguracji pocz ˛ atkowej q ¯ _d0 i zadanej konfiguracji ko´ncowej q ¯ _df nale˙zy znale´z´c (lub poinformowa´c o nieistnieniu takowej) strategi˛e sterowania ze sprz˛e˙zeniem zwrotnym

Φ = [φ 1 φ 2 ] ^⊤ i tunel zbie˙zno´sci ¯ F spełniaj ˛ acy

F ⊆ ¯ ¯ Q f , q ¯ d0 ∈ ¯ F, q ¯ df ∈ ¯ F , (2.11) które w przypadku spełnienia q(t = 0) ∈ ¯ ¯ F i przyj˛ecia v := Φ dla kinematyki monocykla (2.1) zagwarantuj ˛ a zachodzenie nast˛epuj ˛ acych relacji:

– dodatnia niezmienniczo´s´c tunelu zbie˙zno´sci i spełnienie ogranicze´n stanu

∀t ≥ 0 q(t) ∈ ¯ ¯ F ⊆ ¯ Q _f , (2.12) – spełnienie ogranicze´n wej´s´c steruj ˛ acych

∀t ≥ 0 v(t) ∈ V, (2.13)

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.2. Motywacja i postawienie problemu 10

– zbie˙zno´s´c konfiguracji do zadanej konfiguracji ko´ncowej zgodnie z (patrz (2.10))

˜ǫ(t) −→ ˜ǫ ^t→T _d ∧ ǫ _θ (t) −→ ǫ ^t→T _θd ∧ (∀t ≥ 0 k ˜ e(t)k ≤ ˜ǫ(t) ∧ |e θ (t)| ≤ ǫ θ (t)) ,

– spełnienie ogranicze´n monotoniczno´sci manewru

∀t ≥ 0 sgn (v ₂ (t)) ∈ S. (2.14)

przy jednoczesnej minimalizacji funkcjonału kosztu

¯ c(Φ) ,

Z T 0

f ¯ c (q(t), ˙ q(t), Φ(t))dt. (2.15) Problem 2 (Algorytmizacja ruchu samochodu kinematycznego).

Dla wybranego horyzontu czasowego realizacji ruchu T ∈ (0, ∞), zbioru strategii ruchu S, zadanej konfiguracji pocz ˛ atkowej

q _d0 = [β d0 q ¯ _d0 ^⊤ ] ^⊤ i zadanej konfiguracji ko´ncowej

q df = [β df q ¯ _df ^⊤ ] ^⊤

nale˙zy znale´z´c (lub poinformowa´c o nieistnieniu takowej) strategi˛e sterowania ze sprz˛e˙zeniem zwrotnym

Ψ = [ψ 1 ψ 2 ] ^⊤ i tunel zbie˙zno´sci F spełniaj ˛ acy

F ⊆ Q _f = [−β m , β m ] × ¯ Q _f , q ¯ d0 ∈ F , q ¯ df ∈ F , (2.16) które w przypadku spełnienia q(t = 0) ∈ F i przyj˛ecia u := Ψ dla kinematyki danej równa- niami (2.5) i (2.6) zagwarantuj ˛ a zachodzenie nast˛epuj ˛ acych relacji:

– dodatnia niezmienniczo´s´c tunelu zbie˙zno´sci i spełnienie ogranicze´n stanu

∀t ≥ 0 q(t) ∈ F ⊆ Q ¯ _f = [−β _m , β _m ] × ¯ Q _f , (2.17)

– spełnienie ogranicze´n wej´s´c steruj ˛ acych

∀t ≥ 0 u(t) ∈ U, (2.18)

– zbie˙zno´s´c konfiguracji platformy do zadanej konfiguracji ko´ncowej zgodnie z (patrz (2.10)),

˜ǫ(t) −→ ˜ǫ ^t→T d ∧ ǫ θ (t) −→ ǫ ^t→T θd ∧ (∀t ≥ 0 k ˜ e(t)k ≤ ˜ǫ(t) ∧ |e θ (t)| ≤ ǫ θ (t)) ,

2.2. Motywacja i postawienie problemu 11

– zbie˙zno´s´c k ˛ ata skr˛ecenia kół

ǫ β (t) −→ ˜ǫ ^t→T _βd , ∀t ≥ 0 |e _β (t)| ≤ ǫ β (t) (2.19) przy czym ǫ βd ≥ 0 oznacza dokładno´s´c, a bł ˛ ad k ˛ ata skr˛ecenia kół e β (t) zdefiniowano jako

e _β (t) , β df − β(t), (2.20)

– spełnienie ogranicze´n monotoniczno´sci manewru

∀t ≥ 0 sgn (v ₂ (t)) ∈ S, (2.21)

przy jednoczesnej minimalizacji funkcjonału kosztu c(Ψ) ,

Z T 0

f c (q(t), ˙ q(t), Ψ(t))dt. (2.22) Uwaga 2. Podczas wi˛ekszo´sci rozwa˙za´n przedstawionych w niniejszej rozprawie, przyjmuje si˛e,

˙ze β df = 0.

Ze wzgl˛edu na wybór zbioru S, horyzontu T i dokładno´sci w Problemie 1 i Problemie 2, wprowadzono nast˛epuj ˛ac ˛a klasyfikacj˛e automatyzowanych manewrów:

Manewry niemonotoniczne

S ˛a to takie manewry, podczas których strategia ruchu robota (jazda przodem/tyłem) mo˙ze zmienia´c si˛e podczas realizacji ruchu, tj. robot mo˙ze zatrzymywa´c si˛e i zmienia´c strategi˛e ruchu z jazdy przodem na jazd˛e tyłem i odwrotnie. Odpowiada im wybór S = {−1, 1}.

Manewry monotoniczne

S ˛a to takie manewry, podczas których strategia ruchu robota (jazda przodem/tyłem) jest z góry narzucona i stała podczas całego horyzontu realizacji manewru, tj. robot nie mo˙ze zatrzymywa´c si˛e i zmienia´c strategii ruchu z jazdy przodem na jazd˛e tyłem i odwrotnie.

Odpowiada im wybór S = {1} dla jazdy przodem i S = {−1} dla jazdy tyłem. W tym przypadku systemy sterowane podczas algorytmizacji nie s ˛asterowalne w krótkim czasie.

Manewry ze zbie˙zno´sci ˛a asymptotyczn ˛a

W przypadku wyboru niesko´nczonego horyzontu realizacji ruchu T = ∞ i zerowych do- kładno´sci ˜ǫ d , ǫ θd , ǫ βd , relacje (2.19) i (2.10) implikuj ˛a asymptotyczn ˛a zbie˙zno´s´c do konfi- guracji ko´ncowej.

Manewry ze zbie˙zno´sci ˛a praktyczn ˛a

W przypadku wyboru niezerowych dokładno´sci ˜ǫ _d , ǫ θd , ǫ βd , relacje (2.19) i (2.10) impli- kuj ˛apraktyczn ˛azbie˙zno´s´c do konfiguracji ko´ncowej, co oznacza ˙ze konfiguracja ko´ncowa nie b˛edzie osi ˛agni˛eta dokładnie, lecz z odpowiednio zało˙zonym bł˛edem.

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.2. Motywacja i postawienie problemu 12

Rozwi ˛azaniami Problemu 1 i Problemu 2 s ˛a strategie sterowania Φ( ¯ q) i Ψ(q) ze sprz˛e˙ze- niem od stanu, które gwarantuj ˛a lokaln ˛a zbie˙zno´s´c w sensie relacji (2.10) i (2.19) przy ogra- niczeniach wej´s´c steruj ˛acych (2.4) i (2.7) w obr˛ebie podzbiorów przestrzeni konfiguracyjnej (2.11) i (2.16) nazywanych tunelami zbie˙zno´sci. Zgodnie z definicjami (2.11) i (2.16) tunele zbie˙zno´sci s ˛a podzbiorami dopuszczalnej przestrzeni konfiguracyjnej ¯ Q _f (dla monocykla) i Q _f (dla samochodu kinematycznego), zatem postulowana w relacjach (2.12) i (2.17) dodatnia nie- zmienniczo´s´c tuneli zbie˙zno´sci zapewnia spełnienie ogranicze´n stanu i wej´s´c steruj ˛acych. Warto tak˙ze podkre´sli´c, ˙ze dzi˛eki tunelom zbie˙zno´sci, wyznaczone strategie sterowania Φ( ¯ q) i Ψ(q) rozwi ˛azuj ˛a Problem 1 i Problem 2 nie tylko w warunkach nominalnych, tj. gdy odpowiednio q(t = 0) = ¯ ¯ q _d0 i q(t = 0) = q _d0 , ale równie˙z w warunkach nienominalnych, tj. gdy zachodz ˛a jedynie relacje ¯ q(t = 0) ∈ ¯ F i q(t = 0) ∈ F . Rola tuneli zbie˙zno´sci w procesach planowania i realizacji ruchu opisana została szerzej w rozdziałach 4.2-4.5.

Strategie sterowania spełniaj ˛ace wymagania postawione w Problemie 1 i Problemie 2 mog ˛a nie istnie´c głównie ze wzgl˛edu na ograniczenia stanu. Przykładem takiej sytuacji s ˛a ´srodowiska ruchu, w których nie istnieje ˙zadna bezkolizyjna ´scie˙zka ł ˛acz ˛aca konfiguracj˛e pocz ˛atkow ˛a z za- dan ˛a konfiguracj ˛a ko´ncow ˛a. Struktura strategii sterowania oraz współdziałaj ˛acych z nimi algo- rytmów planowania jest silnie zale˙zna od typu manewru. Powoduje to konieczno´s´c traktowania poszczególnych wariantów Problemu 1 oraz Problemu 2 osobno, aby zaprojektowa´c specjalizo- wane dla nich metody algorytmizacji. Nale˙zy jednak zwróci´c uwag˛e na to, ˙ze powy˙zsza klasyfi- kacja manewrów podyktowana jest istotnymi przesłankami praktycznymi. Niektóre roboty mo- bilne (np. roboty ci ˛agn ˛ace za sob ˛a przewód) mog ˛a wykonywa´c tylko manewry monotoniczne.

Z kolei manewry niemonotoniczne s ˛a konieczne w przypadku parkowania w ´srodowiskach z w ˛askimi przej´sciami lub du˙z ˛a ilo´sci ˛a przeszkód. Podobnie, wymóg asymptotycznej zbie˙zno´sci do konfiguracji ko´ncowej jest uzasadniony w przypadku np. dokowania robota mobilnego do gniazda ładuj ˛acego. Z drugiej strony, wymóg asymptotycznej zbie˙zno´sci nakłada dodatkowe ograniczenia na metod˛e algorytmizacji i mo˙ze tym samym przyczyni´c si˛e np. do wyst ˛apienia niepo˙z ˛adanych stanów przej´sciowych, które mog ˛a by´c konieczne do uzyskania wspomnianej zbie˙zno´sci. Jest to widoczne szczególnie w przypadku manewrów monotonicznych, ze wzgl˛edu na to ˙ze zarówno monocykl z ograniczon ˛a krzywizn ˛a jak i samochód kinematyczny nie s ˛a sys- temami sterowalnymi w krótkim czasie przy zało˙zeniu stałego znaku pr˛edko´sci post˛epowej robota.

Celem prac zawartych w tej rozprawie jest rozwi ˛azanie wybranych wariantów Problemu 1 i Problemu 2 z wykorzystaniem algorytmów sterowania ze sprz˛e˙zeniem zwrotnym opracowa- nych w ramach metodyki VFO (z ang. Vector Field Orientation). Tez˛e pracy sformułowano zatem w nast˛epuj ˛acy sposób:

Teza. W ramach metodyki VFO mo˙zna opracowa´c metody algorytmizacji ruchu, które rozwi ˛ a-

zuj ˛ a wybrane warianty Problemu 1 i Problemu 2 przy jednoczesnym zapewnieniu:

2.2. Motywacja i postawienie problemu 13

– wysokiej wydajno´sci obliczeniowej w porównaniu ze znanymi metodami,

– i formalnych gwarancji spełnienia ogranicze´n w ci ˛ agłej dziedzinie czasu i konfiguracji robota.

Uwaga 3. Problem 1 i Problem 2 rozwi ˛ aza´c mo˙zna wykorzystuj ˛ ac ró˙zne pomocnicze zadania ruchu. Plan manewru mo˙ze przybra´c posta´c ci ˛ agłej ´scie˙zki referencyjnej przechodz ˛ acej przez konfiguracj˛e pocz ˛ atkow ˛ a i ko´ncow ˛ a, umo˙zliwiaj ˛ ac tym samym wykorzystanie sterowników dla zadania odtwarzania ´scie˙zki do realizacji manewru. Alternatywnie, mo˙zna ograniczy´c si˛e do zaplanowania jedynie sekwencji punktów przejazdowych, stanowi ˛ acych po´srednie punkty refe- rencyjne dla zastosowanego prawa sterowania. W tej rozprawie zastosowano oba podej´scia, o czym szerzej traktuje rozdział 3.4.

Uwaga 4. Wyniki zawarte w niniejszej rozprawie dotycz ˛ a samochodów kinematycznych z nap˛e- dem na o´s tyln ˛ a, mo˙zna je jednak w prosty sposób dostosowa´c do samochodów kinematycznych z nap˛edem na o´s przedni ˛ a stosuj ˛ ac metod˛e wprowadzon ˛ a w [64].

Uwaga 5. Rozwi ˛ azania Problemu 1 i Problemu 2 dla manewrów monotonicznych mog ˛ a by´c za- adaptowane do algorytmizacji ruchu innych robotów mobilnych ni˙z kołowe. Przykładem mog ˛ a by´c tutaj roboty lataj ˛ ace typu samolot, które utrzymywane s ˛ a na stałej wysoko´sci. Modeluje si˛e je cz˛esto w sposób przybli˙zony, stosuj ˛ ac model podobny do tego z równania (2.1). Przykład takiego podej´scia do modelowania robotów lataj ˛ acych zawiera praca [7].

Uwaga 6. Ze wzgl˛edu na to, ˙ze główne trudno´sci w sterowaniu kołowych robotów mobilnych wyst˛epuj ˛ a na poziomie kinematyki, w niniejszej rozprawie ograniczono si˛e wła´snie do struktur czysto kinematycznych, zaniedbuj ˛ ac kinetyk˛e robota. Zało˙zenie to jest zasadne w przypadku au- tomatyzacji manewrów odbywaj ˛ acych si˛e z mał ˛ a pr˛edko´sci ˛ a post˛epow ˛ a takich jak parkowanie.

Nale˙zy jednak zauwa˙zy´c, ˙ze w przypadku manewrów z pr˛edko´sci ˛ a post˛epow ˛ a, przy której efekty dynamiczne maj ˛ a znacz ˛ acy wpływ na ruchu robota, mo˙zna zastosowa´c znane z literatury me- tody kompensacji tych efektów. W pracy [44] przedstawiono metod˛e pozwalaj ˛ ac ˛ a na sterowanie samochodem kinematycznym z ograniczonym uwzgl˛ednieniem efektów wynikaj ˛ acych z kinetyki platformy bez uciekania si˛e do bezpo´sredniego wykorzystania modelu kinetycznego podczas projektowania sterowania. Osi ˛ agni˛eto to poprzez wykorzystanie ogranicze´n krzywizny ruchu oraz zmiennych w czasie warto´sci maksymalnych u 1B i u 2B w ograniczeniach (2.7). Zgodnie z definicjami Problemu 1 i Problemu 2, proponowane metody algorytmizacji ruchu mog ˛ a zo- sta´c zastosowane w poł ˛ aczeniu z metod ˛ a podan ˛ a w [44]. Problem modelowania kinetyki robota mobilnego jest zło˙zony. Potraktowano go dokładnie w [93].

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.3. Zakres, wkład i struktura rozprawy 14

2.3. Zakres, wkład i struktura rozprawy

W niniejszej rozprawie przedstawiono rozwi ˛azania 5 wariantów Problemu 1 i Pro- blemu 2, zwanymi dalej problemami algorytmizacji. Rozwi ˛azania te i zwi ˛azane z nimi wy- niki prezentowane w rozprawie powstały podczas prac w ramach grantu badawczego NCN 2016/21/B/ST7/02259 „Algorytmizacja sterowania bezdryfowymi systemami nieholonomicz- nymi z ograniczeniami stanu i wej´s´c steruj ˛acych w kontek´scie zło˙zonych zada´n ruchu robo- tów mobilnych". Proponowana metodyka rozwi ˛azywania tych problemów, czyli algorytmiza- cji ruchu wpisuje si˛e koncepcj˛e tzw. algorytmicznej teorii sterowania (z ang. algorithmic con- trol theory) opisanej w [55] i planowania ruchu z uwzgl˛ednieniem sterowania ze sprz˛e˙zeniem zwrotnym (z ang. feedback planning). Podej´scie to polega na ´scisłym ł ˛aczeniu dwóch, trady- cyjnie rozwijanych w separacji typów algorytmów. Pierwszym z nich s ˛a algorytmy sterowania ze sprz˛e˙zeniem zwrotnym i techniki formalne znane z nieliniowej teorii sterowania. Drugim s ˛a metody planowania ruchu. Wspomniane dwa typy algorytmów s ˛a niezb˛edne do algorytmi- zacji ruchu w zastosowaniach praktycznych. Wynikiem planowania s ˛a sygnały referencyjne dla metod sterowania ze sprz˛e˙zeniem zwrotnym, które zapewniaj ˛a odporn ˛a realizacj˛e planu w obliczu zakłóce´n i niepewno´sci pomiarowych. Dotychczas zajmowano si˛e zwykle zagad- nieniami planowania ruchu i sterowania osobno. Okazuje si˛e jednak, ˙ze ich poł ˛aczenie, czyli wspólne projektowanie metod planowania wraz z metodami sterowania ze sprz˛e˙zeniem zwrot- nym pozwala na osi ˛agni˛ecie skutecznych metod algorytmizacji ruchu, które mog ˛a by´c w pew- nych aspektach (np. koszt obliczeniowy, łatwo´s´c implementacji) lepsze od tradycyjnych metod znanych z literatury. Dzi˛eki temu poł ˛aczeniu, informacje o procesie sterowania ze sprz˛e˙zeniem zwrotnym mog ˛a by´c wykorzystane podczas planowania w taki sposób, aby zaplanowa´c sy- gnały referencyjne odpowiadaj ˛ace stosowanej metodzie sterowania ze sprz˛e˙zeniem zwrotnym i powoduj ˛ace stany przej´sciowe układu zamkni˛etego o po˙z ˛adanym charakterze. Takie poł ˛aczenie stwarza tak˙ze okazj˛e do adaptacji integralnego dla metod algorytmizacji ruchu procesu plano- wania w taki sposób, aby mo˙zliwie efektywnie wykorzysta´c struktur˛e problemu wynikaj ˛ac ˛a z zastosowanego algorytmu realizacji ruchu. W niniejszej rozprawie zastosowano wła´snie takie podej´scie, które okre´slane jest jako planowanie motywowane sterowaniem. Jego przykłady s ˛a coraz liczniejsze w literaturze o czym ´swiadcz ˛a prace przywołane w rozdziale 2.4.2. To wła´snie wspomniane poł ˛aczenie metod sterowania i planowania stanowi główn ˛a zalet˛e warto´s´c dodan ˛a wyników prezentowanych w rozprawie. Poł ˛aczenie to widoczne jest procesie projektowania metod algorytmizacji, w którym dostosowano algorytmy planowania do specyficznych metod sterowania ze sprz˛e˙zeniem zwrotnym poprzez wykorzystanie informacji o stanie przej´sciowym systemu sterowanego w p˛etli zamkni˛etej.

Kluczow ˛a cech ˛a proponowanych metod algorytmizacji jest zastosowanie sterowania VFO

(z ang. Vector Field Orientation). Zało˙zono, ˙ze realizacja ruchu odbywa´c si˛e b˛edzie z wyko-

2.3. Zakres, wkład i struktura rozprawy 15

rzystaniem sterownika VFO, co jak ju˙z wspomniano, dało mo˙zliwo´s´c zaprojektowania dedy- kowanych algorytmów planowania ruchu oraz wyznaczania tuneli zbie˙zno´sci. Takie podej´scie umo˙zliwiło wykorzystanie nowych wyników zwi ˛azanych z istotnymi w algorytmizacji ruchu własno´sciami praw sterowania VFO, które przedstawiono w rozprawie. Dzi˛eki temu, w prze- ciwie´nstwie do wielu rozwi ˛aza´n obecnych w literaturze, prezentowane dedykowane algorytmy daj ˛a gwarancj˛e spełnienia nieliniowych ogranicze´n w ci ˛agłych dziedzinach czasu i konfiguracji robota. Oznacza to brak dyskretyzacji ogranicze´n wyst˛epuj ˛acych w definicji problemów algo- rytmizacji i tym samym eliminacj˛e potencjalnych bł˛edów z niej wynikaj ˛acych. Jest to szcze- gólnie istotne w przypadku manewrów, podczas których priorytetowe s ˛a bezpiecze´nstwo (tj.

pewno´s´c bezkolizyjno´sci) i powtarzalno´s´c ruchu. Nale˙zy tak˙ze podkre´sli´c, ˙ze proponowane po- dej´scie charakteryzuje si˛e tak˙ze niskim kosztem obliczeniowym. Koszt ten jest na tyle niski,

˙ze w wielu zastosowaniach praktycznych system algorytmizacji ruchu mógłby z powodzeniem reagowa´c na zmiany w ´srodowisku ruchu (np. dynamiczne przeszkody) poprzez replanowanie, czyli wyznaczanie nowego planu ruchu i tuneli zbie˙zno´sci na bie˙z ˛aco podczas realizacji ruchu.

Skuteczno´s´c proponowanych metod algorytmizacji ruchu zweryfikowano zarówno w symula- cjach (patrz rozdział 6.3 i rozdział 7.4.3) jak i eksperymentalnie (patrz rozdział 8) dla robotów o kinematyce monocykla (2.1) i kinematyce samochodowej.

Problemy algorytmizacji ruchu zdefiniowane w rozdziale 2.2 ró˙zni ˛a si˛e od klasycznych problemów sterowania czy planowania ruchu ze wzgl˛edu na konieczno´s´c wzi˛ecia pod uwag˛e procesu realizacji ruchu podczas planowania i wyznaczania tuneli zbie˙zno´sci. Naturaln ˛a konse- kwencj ˛a tej trudno´sci jest nieco wi˛eksza zło˙zono´s´c metod rozwi ˛azuj ˛acych te problemy. Opisane w rozprawie algorytmy mog ˛a by´c ł ˛aczone ze sob ˛a na wiele sposobów, aby zbudowa´c ró˙zne systemy rozwi ˛azuj ˛ace problemy algorytmizacji ruchu o ró˙znych specyficznych własno´sciach.

Zró˙znicowanie to widoczne jest na schemacie z rys. 2.3, który prezentuje opracowane metody

algorytmizacji ruchu oraz struktur˛e logiczn ˛a rozprawy. Zgodnie z dwoma gał˛eziami rozpoczy-

naj ˛acymi si˛e na schemacie od zielonych bloków, problemy algorytmizacji ruchu rozpatrywane

s ˛a tutaj na dwa sposoby: jako problem planowania i przejazdu przez zbiór punktów referencyj-

nych (z ang. waypoint following) oraz jako problem planowania i odtwarzania ´scie˙zki niepara-

metrycznej (z ang. path-following). Wi˛ecej na temat ró˙znic mi˛edzy tymi podej´sciami znale´z´c

mo˙zna w rozdziale 3.4. W obu przypadkach, rozwa˙zania skupiaj ˛a si˛e głównie na rozwi ˛aza-

niu Problemu 1, a metoda algorytmizacji rozwi ˛azuj ˛aca Problem 2 uzyskiwana jest poprzez

odpowiednie rozszerzenie rozwi ˛azania Problemu 1. Ogólne podej´scie pozwalaj ˛ace na wyko-

rzystaniu rozwi ˛azania Problemu 1 do uzyskania rozwi ˛azania Problemu 2 poprzez zastosowanie

generycznego modelu kinematycznego opisano w rozdziale 3.1 i rozdziale 3.2. W przypadku

wyznaczania tuneli zbie˙zno´sci nale˙zy tutaj poczyni´c dodatkowe kroki zale˙zne od wybranego

zadania ruchu (przejazd przez zbiór punktów lub odtwarzanie ´scie˙zki) i (nie)monotoniczno´sci

manewru, aby uwzgl˛edni´c dodatkowy wymiar przestrzeni konfiguracyjnej. Kroki te uwzgl˛ed-

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.3. Zakres, wkład i struktura rozprawy 16

nione s ˛a w rozdziałach odpowiadaj ˛acych danym metodom realizacji ruchu. Podobnie, wybrany model ´srodowiska ruchu zale˙zny jest od wyboru algorytmu planowania i wyznaczania tuneli zbie˙zno´sci. Rozpatrywane modele ´srodowisk ruchu oraz sposoby ich uzyskania opisano w roz- dziale 3.3.

Rysunek 2.3: Schemat ideowy prezentuj ˛acy opracowane metody algorytmizacji ruchu oraz wy- nikaj ˛ac ˛a z nich struktur˛e rozprawy.

Przejd´zmy teraz do opisu dwóch gał˛ezi widocznych na schemacie. Ze wzgl˛edu metodyk˛e planowania motywowanego sterowaniem, znajomo´s´c jak najwi˛ekszej liczby korzystnych wła- sno´sci zastosowanych algorytmów sterowania jest kluczowa w procesie projektowania i ana- lizy algorytmów planowania. Z tego powodu, obie gał˛ezie zawieraj ˛ace rozwi ˛azania problemów algorytmizacji rozpoczynaj ˛a si˛e wprowadzeniem odpowiednich sterowników kinematycznych dla monocykla z ograniczon ˛a krzywizn ˛a ruchu wraz z ich analiz ˛a formaln ˛a nakierowan ˛a na wyznaczanie tuneli zbie˙zno´sci. W przypadku przejazdu przez zbiór punktów, wykorzystywane jest prawo sterowania VFO dla tego zadania (nazywane dalej sterownikiem VFO-WF), które przypomniano w rozdziale 4.1. Prawo to zostało oryginalnie opracowane dla monocykla bez ograniczenia krzywizny ruchu (2.3). Ograniczenie to pozostaje spełnione tylko w pewnym zbio- rze warunków pocz ˛atkowych, który w przypadku ruchu w wolnej przestrzeni konfiguracyjnej stanowi jednocze´snie tunel zbie˙zno´sci w sensie definicji z Problemu 1. Wyznaczono tunele zbie˙zno´sci dla trzech przypadków:

– tunele zbie˙zno´sci nominalnej (rozdział 4.2),

2.3. Zakres, wkład i struktura rozprawy 17

– tunele (nienominalnej) zbie˙zno´sci niemonotonicznej (rozdział 4.2), – tunele (nienominalnej) zbie˙zno´sci monotonicznej (rozdział 4.5).

Ponadto, w rozdziale 4.3 zaproponowano modyfikacj˛e sterownika VFO przyczyniaj ˛ac ˛a si˛e do powi˛ekszenia tuneli zbie˙zno´sci i poprawienia stanów przej´sciowych.

Planowanie sekwencji punktów przejazdowych dla sterownika VFO-WF stanowi trudny problem optymalizacyjny z ograniczeniami ró˙zniczkowymi i ograniczeniami pół- niesko´nczonymi (z ang. semi-infinite, tj. ograniczenia, które musz ˛a by´c spełnione w g˛estym zbiorze parametrów). W rozdziale 5.1 pokazano jak mo˙zna wykorzysta´c definicje tuneli zbie˙z- no´sci i uzyskane drog ˛a analizy własno´sci sterownika VFO do zapisania tego problemu jako równowa˙zny nieliniowy algebraiczny problem optymalizacji, który pozbawiony jest ograni- cze´n ró˙zniczkowych i pół-niesko´nczonych. Zgodnie ze schematem widocznym na rys. 2.3, dzi˛eki tej transformacji opracowano nowe algorytmy planowania. W rozdziale 5.4 przedsta- wiono nowy algorytm planowania sekwencji punktów przejazdowych wykorzystuj ˛acy opty- malizacj˛e liniow ˛a, własno´sci nominalnych tuneli zbie˙zno´sci i koncepcj˛e tzw. siatki orientacji referencyjnych. Kolejny algorytm planowania motywowany sterowaniem, który wykorzystuje tunele zbie˙zno´sci sterownika VFO oraz obliczenia równoległe prowadzone na grafie w postaci tzw. mapy drogowej zaproponowano w rozdziale 5.6. Rozdział 5.3 zawiera z kolei rozwa˙za- nia na temat prostego zachłannego algorytmu planowania, którego wyniki słu˙zy´c mog ˛a za wa- runek pocz ˛atkowy dla pozostałych algorytmów planowania. W rozdziale 5.5 zaproponowano procedur˛e predykuj ˛ac ˛a, która pozwala na zaplanowanie sekwencji punktów przejazdowych po- trzebnej do rozwi ˛azania Problemu 1 z wykorzystaniem niemal dowolnych (znanych z litera- tury) optymalizuj ˛acych algorytmów próbkuj ˛acych takich jak A* czy te˙z RRT* (z ang. Rapidly- exploring Random Trees). ´Scie˙zka zwi ˛azana z planowaniem sekwencji punktów przejazdowych zako´nczona jest prezentacj ˛a modyfikacji sterownika VFO-WF, których celem jest zapewnienie odpornej realizacji zaplanowanej sekwencji punktów przejazdowych. W rozdziale 6.2 wpro- wadzono mechanizm zapewniaj ˛acy niezmienniczo´s´c tuneli zbie˙zno´sci niemonotonicznej we wszystkich warunkach, na który składa si˛e automat sko´nczony wraz ze stowarzyszonymi rów- naniami dynamiki pomocniczej. rozdział 6.1 zawiera natomiast now ˛a metod˛e dynamicznego ł ˛aczenia praw sterowania zwi ˛azanych z poszczególnymi punktami przejazdowymi, która stoso- wana jest w celu osi ˛agni˛ecia wynikowego prawa sterowania rozwi ˛azuj ˛acego Problem 1 i Pro- blem 2. Metoda ta ma potencjalne zastosowanie równie˙z w przypadku sterowników innych, ni˙z te wykorzystuj ˛ace metodyk˛e VFO.

Rozwa˙zmy teraz gał ˛a´z umieszczon ˛a po prawej stronie schematy z rys. 2.3. Podobnie jak w poprzednim przypadku, rozpoczyna si˛e ona przytoczeniem sterownika VFO dla odtwarza- nia ´scie˙zki (zwanego dalej VFO-PF) w rozdziale 7.1 i wprowadzeniem jego odpowiednich modyfikacji zwi ˛azanych z ograniczon ˛a krzywizn ˛a ruchu opisanych wcze´sniej w rozdziale 4.3.

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO

2.3. Zakres, wkład i struktura rozprawy 18

rozdział 7.2 po´swi˛econy jest dwóm nowym strukturom ´scie˙zek. Charakteryzuj ˛a si˛e one m.in.

G ³ -ci ˛agło´sci ˛a, czyli ci ˛agło´sci ˛a pierwszej pochodnej krzywizny wzdłu˙z naturalnego parametru

´scie˙zki. W rozdziale 7.4 zaprezentowano algorytm wyznaczaj ˛acy tunele zbie˙zno´sci w ´srodowi- sku kolizyjnym z wykorzystaniem optymalizacji na sumach kwadratów (z ang. sum-of-squares programming). W rozdziale 7.3 wykorzystano natomiast wspomniane wcze´sniej próbkuj ˛ace al- gorytmy planowania do zbudowania kompletnego rozwi ˛azania Problemu 1 i Problemu 2.

Podsumowuj ˛ac, w rozprawie zawarto nast˛epuj ˛ace nowe, uzyskane przez autora, wyniki roz- szerzaj ˛ace aktualny stan wiedzy:

– modyfikacje sterowników VFO-WF i VFO-PF zapewniaj ˛ace niezmienniczo´s´c tuneli zbie˙zno´sci,

– analityczne wyznaczenie tuneli zbie˙zno´sci dla sterownika VFO-WF,

– metoda mieszania zapewniaj ˛aca gładkie przej´scia mi˛edzy tunelami zbie˙zno´sci dwóch ste- rowników VFO-WF podczas realizacji zaplanowanego manewru,

– zachłanny algorytm planowania punktów przejazdowych dla sterownika VFO-WF, – nowy algorytm planowania punktów przejazdowych dla sterownika VFO-WF wykorzy-

stuj ˛acy optymalizacj˛e liniow ˛a,

– procedura predykuj ˛aca umo˙zliwiaj ˛aca zastosowanie próbkuj ˛acych algorytmów planowa- nia do wyznaczenia sekwencji punktów przejazdowych dla sterownika VFO-WF,

– propozycja struktury ´scie˙zek G ³ -ci ˛agłych z ograniczon ˛akrzywizn ˛a, które mog ˛a dowolnie dokładnie aproksymowa´c ´scie˙zki Reedsa-Sheppa,

– metodyka wyznaczania tuneli zbie˙zno´sci sterownika VFO-PF niezale˙zna od struktury

´scie˙zki referencyjnej,

– algorytm wyznaczania tuneli zbie˙zno´sci sterownika VFO-PF dla pewnej podklasy ´scie˙zek referencyjnych,

przy czym nale˙zy wspomnie´c, ˙ze wszystkie powy˙zsze rezultaty odnosz ˛a si˛e zarówno do Pro-

blemu 1 jak i Problemu 2. Wi˛ekszo´s´c z tych wyników opublikowano w pracach [33], [28], [32],

[65], [29], [31], [30], [36], [34]. Prace zawieraj ˛ace wyniki, do których odnosi si˛e dany roz-

dział, przywoływane s ˛a zawsze na jego pocz ˛atku. W nast˛epnym rozdziale, przytoczone zostan ˛a

wybrane, znane z literatury metody rozwi ˛azuj ˛ace całkowicie lub cz˛e´sciowo problemy algoryt-

mizacji ruchu.

2.4. Przegl ˛ad istniej ˛ acych rozwi ˛aza´n 19

2.4. Przegl ˛ ad istniej ˛ acych rozwi ˛ aza ´n

2.4.1. Wst˛ep

W literaturze istnieje wiele metod rozwi ˛azuj ˛acych całkowicie lub cz˛e´sciowo zdefiniowane wcze´sniej problemy algorytmizacji. Wi˛ekszo´s´c z nich zaliczy´c mo˙zna do dwóch grup. Pierw- sz ˛a z nich stanowi ˛a metody polegaj ˛ace wył ˛acznie na sterowaniu ze sprz˛e˙zeniem zwrotnym, podczas opracowania których zało˙zono, ˙ze cała przestrze´n konfiguracyjna platformy jest wolna od przeszkód (tj. ¯ Q _f ≡ ¯ Q w zdefiniowanych problemach algorytmizacji) lub te˙z nie przewi- dziano odpowiednio zło˙zonych form tych˙ze ogranicze´n, które mogłyby modelowa´c ´srodowiska ruchu spotykane w zastosowaniach praktycznych. Do drugiej grupy zaliczaj ˛a si˛e metody pla- nowania ruchu, rezultatem działania których jest trajektoria referencyjna, ´scie˙zka referencyjna, b ˛ad´z te˙z przebieg sygnałów steruj ˛acych. Takie podej´scie zapewnia prawidłow ˛a realizacj˛e ruchu w warunkach nominalnych, których istnienie zało˙zono podczas planowania. W zastosowaniach praktycznych realizacja ruchu odbywa si˛e zawsze w warunkach nienominalnych ze wzgl˛edu na niedokładno´sci w modelowaniu obiektu sterowania, obecno´s´c szumów pomiarowych i zakłó- cenia w torze wej´scia. Odporno´s´c na wyst˛epowanie warunków nienominalnych uzyska´c mo˙zna przez zastosowanie sterowania ze sprz˛e˙zeniem zwrotnym podczas realizacji ruchu. Zatem me- toda algorytmizacji ruchu powinna składa´c si˛e zarówno z etapu planowania ruchu jak i algo- rytmu sterowania ze sprz˛e˙zeniem zwrotnym wykorzystywanego do realizacji planu. Algorytmy działaj ˛ace podczas tych dwóch etapów projektowano dot ˛ad w wi˛ekszo´sci przypadków osobno, wykorzystuj ˛ac ró˙zne narz˛edzia.

W ostatnich latach dostrze˙zono wiele zalet wynikaj ˛acych z umiej˛etnego ł ˛aczenia metod planowania ruchu i sterowania ju˙z na poziomie projektowym. Z tego powodu coraz cz˛e´sciej rezygnuje si˛e z tradycyjnego podziału metody algorytmizacji ruchu na odseparowane od siebie moduły planowania i sterowania na rzecz podej´s´c, w których algorytmy planowania i stero- wania s ˛a ze sob ˛a ´sci´sle powi ˛azane. Pozwala to stosowa´c algorytmy planowania dedykowane dla wybranych metod sterowania ze sprz˛e˙zeniem zwrotnym mog ˛a by´c zaprojektowane tak, aby bezpo´srednio wyznacza´c sygnały potrzebne do sterowania. Koncepcj˛e t ˛a opisano m.in. w pra- cach [15] i [55], gdzie nazwano j ˛a algorytmiczn ˛ a teori ˛ a sterowania (z ang. algorithmic control theory). Kluczow ˛a zalet ˛a takiego podej´scia jest tak˙ze mo˙zliwo´s´c planowania ruchu nie tylko dla przypadku nominalnego, ale równie˙z dla nienominalnych warunków ruchu poprzez bezpo-

´srednie rozpatrzenie układu z p˛etl ˛a sterowania ze sprz˛e˙zeniem zwrotnym podczas planowania

ruchu. Prowadzi to cz˛esto do rzadko spotykanych w klasycznym planowaniu ruchu proble-

mów optymalizacyjnych, których specyficzna struktura mo˙ze zosta´c wykorzystana na etapie

projektowania algorytmu planowania. Przykładem wykorzystania takiej struktury jest algorytm

zaproponowany w rozdziale 5.4. W tym paradygmacie algorytm planowania generuje ci ˛ag lo-

kalnych strategii sterowania, których sekwencyjne zastosowanie sprowadza robota do zadanej

T. Gawron Algorytmizacja ruchu robotów mobilnych z ograniczeniami stanu i wej´s´c steruj ˛ acych w

kontek´scie metodyki VFO