Kaskadowa metoda sterowania standardowym pojazdem N-przyczepowym z zastosowaniem algorytmu VFO

Kaskadowa metoda sterowania standardowym pojazdem N-przyczepowym z zastosowaniem algorytmu VFO Seminarium naukowe KSIS Poznań, kwiecień 2 Praca naukowa finansowana ze środków na naukę w latach 2-22 jako projekt badawczy nr N N54 8738 Maciej Michałek Katedra Sterowania i Inżynierii Systemów Politechnika Poznańska M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 2 / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 3 / 39

Dwa sposoby pasywnego mocowania segmentów pojazdu segment i- segment i- L hi i i segment i segment i L i i-ty przegub pasywny L i i-ty przegub pasywny L hi > mocowanie pozaosiowe (po) (ang. off-axle) L hi mocowanie osiowe (O) (ang. on-axle) Konfiguracje pojazdów przegubowych N-przyczepowych: Ogólny Pojazd N-Przyczepowy (OPNP): ciagnik (monocykl) + po/o +... + po +... + po/o Standardowy Pojazd N-Przyczepowy (SPNP): ciagnik (monocykl) + O + O +... + O M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 4 / 39

Sterowanie pojazdami przegubowymi motywacja Argumenty teoretyczne SPNP i OPNP trudnymi obiektami sterowania (nieliniowe, nieholonomiczne, niestabilne w ruchu tyłem) dla SPNP większość rozwiazań wynika transformacji modelu do postaci łańcuchowej (lokalność metody, potencjalne trudności w strojeniu sterownika problem jakości sterowania w przestrzeni oryginalnej) szczególne trudności dla modelu OPNP (model nie jest linearyzowalny sprzężeniem zwrotnym dla N 2, nieminimalnofazowość w ruchu przodem) otwarte problemy (sterowanie z ograniczeniami na stan, odporność, uniwersalność sterowników, sterowanie OPNP dla N... ) 2 Argumenty praktyczne liczne zastosowania pojazdów przegubowych (w tym z przyczepami) manewry pojazdami przegubowymi częste i uciażliwe w praktyce nieintuicyjne i złożone manewry pojazdami wielosegmentowymi SPNP: Standardowy Pojazd N-Przyczepowy OPNP: Ogólny Pojazd N-Przyczepowy M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 5 / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 6 / 39

Standardowy pojazd N-przyczepowy: podstawowe definicje Y y tractor (segment ) v Wektor konfiguracji pojazdu q = [β... β N θ N x N y N ] T R N+3 () v 2 L Wektor konfiguracji ostatniego segmentu y y 2 trailer N (segment N+) y N P guidance point v N N N L N L 2 2 v 2 2 3 x N x 2 x trailer 2 (segment 3) trailer (segment 2) X q = [θ N x N y N ] T R 3 (2) Wektor wejść sterujacych u = [ω v ] T R 2 (3) Punkt wyróżniony P (ang. guidance point) P = (x N, y N ) (4) x Współrzędne punktu P sa wyjściami płaskimi modelu kinematyki L i >, i =,..., N długości przyczep Wszystkie przyczepy mocowane osiowo (ang. on-axle hitching) standardowy pojazd/robot N-przyczepowy M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 7 / 39

Rekurencyjna (kaskadowa) postać modelu SPNP a) Y y tractor (segment ) v Równania kinematyki w postaci rekurencyjnej: ω v b) θ β θ ω = = ẋ =v c ẏ =v s tractor + - + - Eqs. (8,9) y y 2 y N v trailer N (segment N+) P guidance point β β 2 β N ẋ =v c ẏ =v s trailer v N Eqs. (8,9) N N L N β 2 L 2......... ω N- v N- 2 v 2 3 v x N x 2 x 2 2 θ N- N = N trailer 2 (segment 3) x ẋ N =v N c N ẏ N =v N s N trailer N- L trailer (segment 2) X + - Eqs. (8,9) β N ω N v N N = N ẋ N =v N c N ẏ N =v N s N trailer N standard N-trailer vehicle a) szkielet kinematyczny pojazdu/robota, b) schemat modelu w postaci kaskadowej θ N x N y N θ i = ω i, (5) ẋ i = v i cos θ i, (6) ẏ i = v i sin θ i, (7) ω i = v i sin β i, L i (8) v i = v i cos β i (9) β i = θ i θ i. () ω i, v i wirtualne sterowania i-tego segmentu M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 8 / 39

Model SPNP w postaci zwartej sin β L β sin β L sin β L 2 cos β 2 β 2. (. i 2 ) ( ). β i = j= cos β j sin β L i sin β i L i cos β i i ω + v ().. (. N ) θ N j= cos β j sin β L N N ẋ N ( N ) ẏ N j= cos β j cos θ N ( N ) j= cos β j sin θ N q = g ω + g 2 (q)v = S(q)u (2) S(q) = [ g g 2 (q) ], u = [ω v ] T M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 9 / 39

Cechy modelu kinematyki SPNP sterowalny w krótkim czasie* (spełnia LARC) Y y tractor (segment ) v silnie nieliniowy nieholonomiczny: ẋ i sin θ i ẏ i cos θ i =, i [,N] v 2 L DOF = dim(q) (N + ) = N + 3 N = 2 y y 2 y N trailer N (segment N+) P guidance point v N N N L N L 2 2 v 2 3 x N x 2 x 2 trailer 2 (segment 3) x trailer (segment 2) X ubogi w sterowanie: dim(q) dim(u ) = n m = N + 3 2 = N + osobliwy**, gdy i [;N ] : β i = π/2 (zmiana stopnia nieholonomiczności) strukturalnie niestabilny dla v < (efekt składania się pojazdu oraz efekt scyzoryka ang. jacknifing phenomenon) różniczkowo płaski***: wyścia płaskie x N i y N (linearyzowalny sprzężeniem zwrotnym) *J.-P. Laumond: Controllability of a multibody mobile robot, IEEE Trans. Robotics and Automation, 9(6), 993 **F. Jean: The car with N trailers: characterization of the singular configurations, ESAIM: Control, Optim. Calc. Variations, Vol., 996 *** P. Rouchon et al.: Flatness, motion planning and trailer systems, Proc. of 32nd CDC Conf., San Antonio USA, 993 M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym / 39

Definicje zwiazane z zadaniem sterowania Wektor konfiguracji referencyjnej: [ ] qβt q t = = [β t... β tn θ tn x tn y tn ] T R N+3 q t Wektor błędu konfiguracji: [ ] eβ e = = [e β ē... e βn e θ e x e y] T q t q (3) Wektor błędów katów w przegubach: e β [β t β... β tn β N ] T R N (4) Wektor błędu położenia (orientacji i pozycji) ostatniego segmentu: e θ θ tn θ N ē = e x qt q = x tn x N R 3 e y y tn y N (5) M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 2 / 39

Zadanie sterowania Śledzenie trajektorii Dane: q t (τ) C d Założenia: At. At2. At3. trajektoria q t (τ) jest dopuszczalna i ustawicznie pobudzajaca: τ ẋ 2 tn (τ) + ẏ2 tn (τ) wszystkie elementy wektora q mierzalne wartości parametrów L i dokładnie znane Sterowanie do punktu Dane: q t = const, β ti, i =,..., N Założenia: Ap. dla ostatniego segmentu: ē() Ap2. Ap3. wszystkie elementy wektora q mierzalne wartości parametrów L i dokładnie znane Zadanie sterowania polega na zaprojektowaniu reguły sterowania ze sprzężeniem zwrotnym u = u (q t, q, ), która zastosowana do kinematyki robota reprezentowanej równaniami (5)-() zapewnia zbieżność błędu (3) w takim sensie, że: Śledzenie trajektorii lim ē(τ) δ oraz lim eβi (τ) δ2, τ τ Sterowanie do punktu lim ē(τ) δ oraz lim e βi (τ) = ±kπ τ τ przy czym: δ, δ 2 to założone otoczenia zera w przestrzeni uchybu oraz k {,, 2,...}. M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 3 / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 4 / 39

Propagacja prędkości wzdłuż łańcucha kinematycznego SPNP Propagacja prędkości w kierunku ciagnik przyczepy (na podstawie kinematyki (5)-()): ω i = L i v i sin β i (6) oraz v i = v i cos β i (7) β i = θ i θ i (8) β i = θ i θ i = ω i ω i (9) Propagacja prędkości w kierunku przyczepy ciagnik (na podstawie (6)-(9)): v i = L i ω i sin β i + v i cos β i (2) ω i = ω i + β i (2) β i = Atan2c (L i ω i v i, v i v i ) R (22) Atan2c (, ) : R R R M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 5 / 39

Wyprowadzenie sterowania kaskadowego pętla zewnętrzna Eksperyment myślowy: uwalniamy ostatnia przyczepę i traktujemy ja jak pojazd typu monocykl o wejściach sterujacych ω N i v N. Zakładamy, że dane sa funkcje sterujace Φ ω(ē, ), Φ v(ē, ), które zapewniaja asymptotyczne śledzenie trajektorii lub asymptotyczna stabilizację w punkcie dla pojazdu typu monocykl: LTPT: ω N = Φ ω(ē, ), v N = Φ v(ē, ) lim ē(τ) = qt(τ) q(τ) = τ LTPS: ω N = Φ ω(ē, ), v N = Φ v(ē, ) lim ē(τ) = qt q(τ) = τ LTPT: Last-Trailer Posture Tracker, LTPS: Last-Trailer Posture Stabilizer M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 6 / 39

Wyprowadzenie sterowania kaskadowego pętla zewnętrzna Skoro w SPNP nie możemy bezpośrednio wymusić spełnienia równań ω N = Φ ω(ē, ) oraz v N = Φ v(ē, ), zatem proponujemy przyjać: ω dn := Φ ω(ē, ), v dn := Φ v(ē, ) (23) q t q LTPT / LTPS ω dn := Φ ω ( e,.) v dn := Φ v ( e,.)...?... ω v kinematyka SPNP q pętla zewnętrzna Jak zapewnić, aby (ω dn ω N ) oraz (v dn v N )? Realizacja (ω dn ω N ) oraz (v dn v N ) możliwa poprzez odpowiedni ruch segmentu N, który wynika z ruchu segmentu N 2..., który ostatecznie wynika z ruchu modułu czyli modułu ciagnika (aktywny segment pojazdu SPNP). M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 7 / 39

Wyprowadzenie sterowania kaskadowego pętle wewnętrzne Propozycja procedury projektowej K. propagacja prędkości zadanych oraz kat zadany w złaczu (na podstawie (2)-(22)): v di L i ω di sin [ β i + v di cos β i (24) ] ω di ω di + βi (25) d β di Atan2c (L i ω di v di, v di v di ) R (26) K2. konstrukcja wewnętrznej pętli sprzężenia zwrotnego: [ βi ] d k i(β di β i ) + β di = k i e di + β di, k i > (27) ω di d/dτ ω di v di (24) v di- β di e di (26) - k i ω di- v di- SCM i Single Control Module β i M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 8 / 39

Wynikowy schemat układu sterowania kaskadowego SPNP Równania sterownika: v dn := Φ v(ē, ) (28) ω dn := Φ ω(ē, ) (29) v dn := L N ω dn sin β N + v dn cos β N (3) ) β dn := Atan2c (L N ω dn v dn, v dn v dn (3) ω dn := k N (β dn β N ) + β dn + ω dn (32) q wyjście (odpowiedź) obiektu sterowania β i wyjścia pomocnicze. v d := L 2 ω d2 sin β 2 + v d2 cos β 2 (33) β d2 := Atan2c (L 2 ω d2 v d, v d2 v d ) (34) ω d := k 2 (β d2 β 2 ) + β d2 + ω d2 (35) v d := L ω d sin β + v d cos β (36) β d := Atan2c (L ω d v d, v d v d ) (37) ω d := k (β d β ) + β d + ω d (38) Pozostaja do określenia jawne postaci funkcji sterujacych Φ v(ē, ) oraz Φ ω(ē, ) M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 9 / 39

Efekt składania się łańcucha kinematycznego pojazdu A R d 2 R D tractor (segment ) trailer (segment 2) tractor (segment ) trailer 2 (segment 3) B R 2 Q R tractor (segment ) trailer (segment 2) trailer 2 (segment 3) A: kostrukcja mechanicznie dopuszczajaca składanie się pojazdu (β, β 2 R) B: kostrukcja z mechanicznym ograniczeniem drugiego kata skręcenia (β 2 Q β R) M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 2 / 39

Efekt składania się łańcucha kinematycznego pojazdu Przedstawiona postać równań dopuszcza występowanie efektu składania pojazdu, ponieważ: β di Atan2c Li ω di v di, v di v di R }{{}}{{} R R Propozycja unikania efektu składania: wówczas v di σ L i ω di sin β i + v di cos β i, σ {, +} (39) β di Atan2c L i ω di v di, v di v di }{{}}{{} R R + gdy sgn(v di )=σ? Skuteczność propozycji (39) zależy od własności sterownika pętli zewnętrznej (LTPT/LTPS). Poza modyfikacja (39) pozostałe równania sterownika nie ulegaja zmianie. σ to zmienna decyzyjna, której wartość wybierana jest w odniesieniu do własności sterownika pętli zewnętrznej (LTPT/LTPS) M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 2 / 39

Kwestie implementacyjne algorytmu sterowania Nieciagłość algorytmu Definicja β di (τ I ) = Atan2c (A(τ I ), B(τ I )) nieokreślona, gdy A(τ I ) = B(τ I ) =. Propozycja: w chwilach τ I przyjać β di (τ I ) := β di = lim τ τ β di (τ). Alternatywnie: I IF (A 2 + B 2 ɛ) AND ( ē ɛ e) THEN β di := β di 2 Realizacja sprzężeń wyprzedzajacych β di analityczna wynikajaca z formalnego zróżniczkowania β di (wymaga pochodnych ω di oraz v di ) numeryczna z zastosowaniem dokładnych odpornych różniczkowatorów (ang. exact robust differentiators*) numeryczna aproksymacja poprzez filtrację: β di β dif = L {sβ di (s)/( + st F )} (aproksymacja zwykle pogarsza dokładność śledzenia w stanie ustalonym) 3 Zasada syntezy pętli wewnętrznych reguła heurystyczna: k i > k i+ (4) Wybór wartości k i > jako kompromis między sztywnościa pętli wewnętrznych a wrażliwościa układu zamkniętego na szumy pomiarowe oraz tolerancja ograniczeń sygnałów sterujacych (prędkości kół ciagnika). * A. Levant: Robust exact differentiation via sliding mode technique, Automatica, 34(3), 998 M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 22 / 39

Kwestie implementacyjne ograniczenia sterowania ω k max maksymalna dopuszczalna prędkość katowa koła ciagnika (kinematyka (2,)) przestrzeń prędkości kół ciągnika L kmax s u 2 przestrzeń prędkości platformy ciągnika u 2dop u 2max D P kinematyka u kmax kmax (2,) u max u dop u max Ω = kmax u 2max [ ] b/(2r) /r u b/(2r) /r, u = [u u 2 ] T = [ω v ] T, Ω = [ω P ω L ] T Procedura skalowania prędkości: u s = s u, { s max ; ω P ω k max ; } ω L (4) ω k max Składowe przeskalowanego sterowania u s sa już realizowalne fizycznie. M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 23 / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 24 / 39

Sterownik VFO jako LTPT lub LTPS - równania Równania sterownika VFO zapisane dla ostatniej (N-tej) przyczepy: Φ ω k ae a + θ a (42) Φ v h x cos θ N + h y sin θ N (43) Śledzenie trajektorii (LTPT) Sterowanie do punktu (LTPS) h x = k pe x + v x, v x = ẋ t (44) h y = k pe y + v y, v y = ẏ t (45) e a = θ a θ N (46) θ a = Atan2c (σ h y, σ h x) (47) θ a = (ḣyhx hyḣx)/(h2 x + h 2 y) (48) σ sgn(u 2t (τ)) (49) h x = k pe x + v x, v x = ησ ē cos θ tn (5) h y = k pe y + v y, v y = ησ ē sin θ tn (5) e a = θ a θ N (52) θ a = Atan2c (σ h y, σ h x) (53) θ a = (ḣyhx hyḣx)/(h2 x + h 2 y) (54) σ sgn(e x cos θ tn + e y sin θ tn ) (55) Współczynniki projektowe: k a, k p >, η (, k p) Postaci zmiennej decyzyjnej σ zdefiniowane w (49) i (55) sa wykorzystywane w ramach modyfikacji (39) algorytmu sterowania kaskadowego. M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 25 / 39

Sterownik VFO jako LTPT lub LTPS - interpretacje geometryczne śledzenie trajektorii k p = sterowanie do punktu k p = y tn y N y G wirtualna N-ta przyczepa referencyjna P x N e * e a q * t q * t h x tn * q * trajektoria referencyjna a N x G tn P x N h q * * ea a N e v * * y G y N 2ε * g 2t wirtualna N-ta przyczepa referencyjna tn q * t = x G przyjęte otoczenie pozycji referencyjnej dla N-tej przyczepy h = [h x h y] T, ē = [e x e y] T, v = [v x v y] T, q = [ẋ N ẏ N ] T, ḡ 2t = [cos θ tn sin θ tn ] T M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 26 / 39

Sterownik VFO jako LTPT lub LTPS - motywacja Gwarancja ruchu nieoscylacyjnego, prawie bez zmiany znaku prędkości postępowej (możliwa co najwyżej jedna zmiana znaku w stanie przejściowym skuteczność propozycji (39)) Efekt naprowadzania N-tej przyczepy przydatny w prostowaniu łańcucha SPNP Prostota strojenia sterownika VFO: K. k p (, 5] jako kompromis między szybkościa zbieżności a wrażliwościa na szumy pomiarowe i opóźnienia w pętli sprzężenia K2. k a := 2k p ponieważ proces orientowania jest kluczowy i istotniejszy od procesu popychania K3. η (, k p) zgodnie z żadan a intensywnościa efektu naprowadzania (tym większa intensywność im mniejsze k p η) Gwarancja asymptotycznej zbieżności błędów do zera dla zadania śledzenia oraz sterowania do punktu* *M. Michałek, K. Kozłowski: Vector-Field-Orientation feedback control method for a differentially driven vehicle, IEEE Trans. Control Sys. Techn., 8(), 2 M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 27 / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 28 / 39

Ilustracja wyników legenda.9.8.7.6.5.4.3 q t.2. q() q(τ)..2.2.4.6.8.2 q(): konfiguracja poczatkowa pojazdu, q t : konfiguracja referencyjna pojazdu, q(τ): bieżaca konfiguracja pojazdu M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 29 / 39

Sim: śledzenie trajektorii kołowej (ruch tyłem) TRAJECTORY TRACKING + VFO for LTPT Warunki symulacji.5 q() = [ π ] T q t () = [ π/2 ] T ω 3t =.5 rad/s, v 3t =.2 m/s β d := β df, T F =.5 s βd2,3 :=.5 Wartości parametrów L = L 2 = L 3 =.25 m k = 5, k 2 = 2, k 3 = 5, ɛ =. k a = 2, k p =, ɛ h =. ω k max = 8π rad/s, b =.7 m, r =.25 m y G [m].5.5 q t () Animacja: SRMPMovie.gif.5.5.5.5 2 x G [m] e θ [rad] e x [m] e y [m] 2 β [rad] β 2 [rad] β 3 [rad] 5 ω [rad/s] v [m/s] 5 2 3 4 time [s] 2 3 4 time [s] 2 3 4 time [s] M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 3 / 39

Sim2: śledzenie trajektorii złożonej (ruch tyłem) TRAJECTORY TRACKING + VFO for LTPT Warunki symulacji 2 q() = [ π/2.3] T q t () = [ π/2 ] T ω 3t =.5( + sin.5τ) rad/s, v 3t =.2 m/s β d := β df, T F =.5 s βd2,3 :=.5 Wartości parametrów L = L 2 = L 3 =.25 m k = 5, k 2 = 2, k 3 = 5, ɛ =. k a = 2, k p =, ɛ h =. ω k max = 8π rad/s, b =.7 m, r =.25 m y G [m].5.5 q t () Animacja: S2RMPMovie.gif.5.5.5.5 2 x G [m] e θ [rad] e x [m] e y [m] 2 β [rad] β 2 [rad] β 3 [rad] 5 ω [rad/s] v [m/s] 5 2 3 4 time [s] 2 3 4 2 3 4 time [s] time [s] M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 3 / 39 5

Sim3: sterowanie do punktu (parkowanie równoległe tyłem) Warunki symulacji q() = [ π/2 ] T q t = [ π/2 ] T ω 3t = v 3t β d := β df, T F =.5 s βd2,3 :=.5 SET POINT REGULATION + VFO for LTPS Wartości parametrów L = L 2 = L 3 =.25 m k = 5, k 2 = 2, k 3 = 5, ɛ =. k a = 2, k p =, η =.8, ɛ h =. ω k max = 8π rad/s, b =.7 m, r =.25 m y G [m].5 q t.5 Animacja: S3RMPMovie.gif S3FoldRMPMovie.gif.5.5.5 x G [m] 3 2 e θ [rad] e x [m] e y [m] 2 2 3 4 time [s] 2 β [rad] β 2 [rad] β 3 [rad] 2 3 4 time [s] 5 5 ω [rad/s] v [m/s] 2 3 4 time [s] M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 32 / 39

Sim4: sterowanie do punktu (parkowanie równoległe przodem) Warunki symulacji q() = [ π/2 ] T q t = [ π/2 ] T ω 3t = v 3t β d := β df, T F =.5 s βd2,3 :=.5 SET POINT REGULATION + VFO for LTPS Wartości parametrów L = L 2 = L 3 =.25 m k = 5, k 2 = 2, k 3 = 5, ɛ =. k a = 2, k p =, η =.8, ɛ h =. ω k max = 8π rad/s, b =.7 m, r =.25 m y G [m].5 q t.5 Animacja: S4RMPMovie.gif.5.5.5 x G [m] 3 2 2 e θ [rad] e x [m] e y [m] 2 3 time [s] 3 2 β [rad] β 2 [rad] β 3 [rad] 2 3 2 3 time [s] time [s] 5 5 ω [rad/s] v [m/s] M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 33 / 39

Sterowanie do punktu wpływ efektu naprowadzania SET POINT REGULATION + VFO: k p =., η=. SET POINT REGULATION + VFO: k p =., η=.3 SET POINT REGULATION + VFO: k p =., η=.4.5.5.5 y G [m].5 y G [m].5 y G [m].5.5.5.5.5.5.5.5.5.5.5.5.5 x G [m] x G [m] x G [m] SET POINT REGULATION + VFO: k p =., η=.6 SET POINT REGULATION + VFO: k p =., η=.8 SET POINT REGULATION + VFO: k p =., η=.9.5.5.5 y G [m].5 y G [m].5 y G [m].5.5.5.5.5.5.5.5.5.5.5.5.5 x G [m] x G [m] x G [m] Czasy trwania poszczególnych symulacji: τ h 29 s dla η =., η =.3, η =.4 oraz τ h = 4 s dla η =.6, η =.8, η =.9. M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 34 / 39

Plan Wprowadzenie 2 Model kinematyki SPNP 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania 5 Zastosowanie metody VFO 6 Weryfikacja numeryczna 7 Uwagi końcowe M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 35 / 39

Uwagi na temat stabilności układu zamkniętego Można pokazać, że dynamika układu zamkniętego daje się zapisać w następujacej postaci: gdzie: ē = f(ē, τ) + f (ē, e d, τ), ė d = Ae d + f 2 (ē, e d, τ) e d = [e d... e dn ] T = [β d β... β dn β N ] T A = diag{ k, k 2,..., k N } f (ē, e d, τ) = G( q t ē)γe ωv(ē, e d, τ), G( ) jest macierza kinematyki monocykla f 2 (ē, e d, τ) = He ωv(ē, e d, τ) e ωv = [e T ω e T v ] T, e ω = [ω d ω... ω dn ω N ] T, e v = [v d v... v dn v N ] T Γ, H sa macierzami o stałych elementach równych, lub + Dodatkowo zachodzi: f 2 (e d =, ) = oraz f (e d =, ) =, a nominalna (niezaburzona) dynamika ē = f(ē, τ) jest asymptotycznie stabilna (dla q t = const mamy ē = f(ē)). Wykazujac stosowne cechy funkcji f ( ) oraz f 2 ( ) planuje się skorzystać z twierdzeń o stabilności systemów wzajemnie połaczonych (ang. interconnected systems)*. * H. Khalil: Nonlinear systems. 3rd Edition, Prentice-Hall, 22 A. Isidori: Nonlinear control systems II, Springer 999 M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 36 / 39

Planowane dalsze prace Ograniczenie skutków znacznej wrażliwości układu zamkniętego w otoczeniu punktu q t 2 Badania w zakresie odporności układu zamkniętego (niepewność parametryczna: L si L i oraz L hi, szumy pomiarowe w sprzężeniu zwrotnym: q n = q + n) 3 Przeprowadzenie formalnej analizy stabilności i zbieżności 4 Eksperymentalna weryfikacja metody (stanowisko laboratoryjne RMP-SW) 5 Rozszerzenie koncepcji sterowania kaskadowego na pojazd OPNP Ad 4: Pomimo trudniejszej kinematyki OPNP propagacja prędkości jest tu prostsza (!): [ ] [ ] [ ] [ ] ωi ωi ωi = J(β v i ) = J ωi (β i v i v i ) i v i gdzie [ ] (Lhi /L J(β i ) = i ) cos β i (/L i ) sin β i, det(j(β L hi sin β i cos β i )) = (L hi /L i ) i M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 37 / 39

Pojazd laboratoryjny RMP (Robot Mobilny Przegubowy) M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 38 / 39

... Dziękuję za uwagę! M. Michałek (KSIS-PP) Sterowanie pojazdem N-przyczepowym 39 / 39