Zastosowanie metody VFO do sterowania pojazdami N-przyczepowymi

Transkrypt

1 Zastosowanie metody VFO do sterowania pojazdami N-przyczepowymi Naukowe seminarium wyjazdowe Szklarska Poręba, 5-8 V 211 Praca naukowa finansowana ze środków na naukę w latach jako projekt badawczy nr N N Maciej Michałek Katedra Sterowania i Inżynierii Systemów Politechnika Poznańska M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 1 / 42

2 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 2 / 42

3 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 3 / 42

4 Zasadnicze rozróżnienie dwa sposoby mocowania przyczep segment i-1 segment i-1 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) Rodzaje pojazdów 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) Metoda VFO dla pojazdów N-przyczepowych 4 / 42

5 Sterowanie pojazdami N-przyczepowymi motywacja 1 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 1... ) 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 (ang. standard N-trailer vehicle) OPNP: Ogólny Pojazd N-Przyczepowy (ang. general N-trailer vehicle) M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 5 / 42

6 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 6 / 42

7 Ogólny pojazd N-przyczepowy (OPNP): podstawowe definicje v Y y tractor (segment 1) 1 L h1 Wektor konfiguracji pojazdu q = [β 1... β N θ N x N y N ] T R N+3 (1) 1 y 1 trailer 1 (segment 2) v L 1 Wektor konfiguracji ostatniego segmentu q = [θ N x N y N ] T R 3 (2) L h2 y 2 trailer N (segment N+1) v N trailer 2 (segment 3) N 2 v L 2 Wektor wejść sterujacych u = [ω v ] T R 2 (3) Punkt wyróżniony P (ang. guidance point) y N P guidance point N L N L h3 X P = (x N, y N ) (4) x N x 2 x 1 L i > długość i-tej przyczepy L hi odsunięcie i-tego przegubu x OPNP L hi= i SPNP M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 7 / 42

8 Rekurencyjna (kaskadowa) postać modelu OPNP Y tractor (segment 1) v 1 Równania kinematyki OPNP: y L h1 θ i = ω i (5) y 1 trailer 1 (segment 2) 1 v L 1 ẋ i = v i cos θ i (6) ẏ i = v i sin θ i (7) y 2 y N trailer N (segment N+1) P guidance point v N trailer 2 (segment 3) N N L N 2 v 2 3 x N x 2 x 1 2 L h3 L 2 x L h2 X [ ] ωi = v i ω i, v i wirtualne sterowania i-tego segmentu [ ] L hi 1 cos β L i sin β i L i i L hi sin β i cos β i }{{} J i (β i ) odwracalna β i [ ] ωi 1 v i 1 (8) β i = θ i 1 θ i (9) β 1 β 2 β N θ + - θ θ N ω v = ẋ =v c ẏ =v s tractor β 1 J 1 (β 1 ) ω 1 v 1 1 = 1 ẋ 1 =v 1 c 1 ẏ 1 =v 1 s 1 trailer 1 β 2 J 2 (β 2 ) ω N-1 v N-1 N 1 = N 1 ẋ N 1 =v N 1 c N 1 ẏ N 1 =v N 1 s N 1 trailer N-1 β N J N (β N ) ω N v N N = N ẋ N =v N c N ẏ N =v N s N trailer N θ N x N y N general N-trailer vehicle M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 8 / 42

9 Rekurencyjna (kaskadowa) postać modelu SPNP Y y y 1 y 2 y N trailer N (segment N+1) P guidance point v N N N L N L 2 tractor (segment 1) 2 v 2 3 v 1 v 1 x N x 2 x trailer 2 (segment 3) x L 1 trailer 1 (segment 2) X Równania kinematyki SPNP: θ i = ω i (1) ẋ i = v i cos θ i (11) ẏ i = v i sin θ i (12) ω i, v i wirtualne sterowania i-tego segmentu [ ] ωi = v i [ ] 1 sin β L i i cos β }{{} J i (β i ) osobliwa β i [ ] ωi 1 v i 1 (13) β i = θ i 1 θ i (14) ω v = 1 = 1 ẋ =v c ω 1 ẋ 1 =v 1 c 1 ẏ J 1 (β 1 ) =v s J 2 (β 2 ) v ẏ 1 =v 1 s 1 1 tractor β 1 β 2 β N θ θ 1 β 1 trailer 1 β ω N-1 v N-1 θ N-1 N 1 = N 1 ẋ N 1 =v N 1 c N 1 ẏ N 1 =v N 1 s N 1 trailer N β N J N (β N ) ω N v N N = N ẋ N =v N c N ẏ N =v N s N trailer N θ N x N y N standard N-trailer vehicle M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 9 / 42

10 SPNP cechy modelu kinematyki sterowalny w krótkim czasie* (spełnia LARC) Y y tractor (segment 1) v 1 1 silnie nieliniowy nieholonomiczny: ẋ i sin θ i ẏ i cos θ i =, i [,N] 1 v 2 1 L 1 DOF = dim(q) (N + 1) = N + 3 N 1 = 2 y 1 y 2 y N trailer N (segment N+1) P guidance point v N N N L N L 2 2 v 2 3 x N x 2 x 1 2 trailer 2 (segment 3) x trailer 1 (segment 2) X ubogi w sterowanie: dim(q) dim(u ) = n m = N = N + 1 osobliwy**, gdy i [1;N 1] : β 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), 1993 **F. Jean: The car with N trailers: characterization of the singular configurations, ESAIM: Control, Optim. Calc. Variations, Vol.1, 1996 *** P. Rouchon et al.: Flatness, motion planning and trailer systems, Proc. of 32nd CDC Conf., San Antonio USA, 1993 M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 1 / 42

11 OPNP specyficzne cechy modelu kinematyki Y y tractor (segment 1) v 1 L h1 osobliwy*, gdy: i [1;N 1] : β i = π/2 lub j : γ j = π/2, gdzie: γ i = θ i θ i+1 + arc tg( θ i L hi /v i ) segment i 1 v i i y 1 trailer 1 (segment 2) v L 1 segment i+1 L i i L hi y 2 y N trailer N (segment N+1) P guidance point v N trailer 2 (segment 3) N N L N 2 v 2 3 x N x 2 x 1 2 L h3 L 2 x L h2 X (zmiana stopnia nieholonomiczności) i 1 wirtualne koło skrętne dynamika w torze ω i 1 θ i nieminimalnofazowa**: G(s) θ i(s) ω i 1 (s) v i 1 :=V > nie jest różniczkowo płaski*** dla N 2 (nie jest linearyzowalny sprzężeniem zwrotnym, gdy N 2) = 1 L hi V s s(1 + L i V s) *C. Altafini: Some properties of the general n-trailer, Int. Journal of Control 74(4), 21 ** M. Michałek, K. Kozłowski: VFO tracking and set-point control for an articulated vehicle with off-axle hitched trailer, Proc. of ECC, Budapeszt, 29 *** P. Rouchon et al.: Flatness, motion planning and trailer systems, Proc. of 32nd CDC Conf., San Antonio USA, 1993 M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 11 / 42

12 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 12 / 42

13 Definicje zwiazane z zadaniem sterowania Wektor konfiguracji referencyjnej: [ ] qβt q t = = [β t1... β tn θ tn x tn y tn ] T R N+3 q t Wektor błędu konfiguracji: [ ] eβ e = = [e β1 ē... e βn e θ e x e y] T q t q (15) Wektor błędów katów w przegubach: e β [β t1 β 1... β tn β N ] T R N (16) 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 (17) M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 13 / 42

14 Zadanie sterowania Śledzenie trajektorii Dane: q t (τ) C d (dopuszczalna) Założenia: At1. At2. At3. trajektoria q t (τ) jest ustawicznie pobudzajaca: τ ẋ 2 tn (τ) + ẏ2 tn (τ) wszystkie elementy wektora q mierzalne wartości parametrów L i, L hi dokładnie znane Sterowanie do punktu Dane: q t = [... q T t ]T = const Założenia: Ap1. dla ostatniego segmentu: ē() Ap2. Ap3. wszystkie elementy wektora q mierzalne wartości parametrów L i, L hi 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 (1)-(14) zapewnia zbieżność błędu e = [e T β ēt ] T w takim sensie, że: Śledzenie trajektorii lim ē(τ) δ 1 oraz lim eβi (τ) δ2, τ τ Sterowanie do punktu lim ē(τ) δ 1 oraz lim eβi (τ) = kπ τ τ przy czym: δ 1, δ 2 to założone otoczenia zera w przestrzeni uchybu oraz k {, 1, 2,...}. zadanie = stabilizacja N-tej przyczepy + układanie/prostowanie łańcucha M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 14 / 42

15 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 15 / 42

16 SPNP: propagacja prędkości wzdłuż łańcucha kinematycznego Propagacja prędkości w kierunku ciagnik przyczepy (na podstawie kinematyki (1)-(14)): oraz ω i = (1/L i )v i 1 sin β i (18) v i = v i 1 cos β i (19) β i = θ i 1 θ i βi = θ i 1 θ i = ω i 1 ω i (2) Propagacja prędkości w kierunku przyczepy ciagnik (na podstawie (18)-(2)): oraz ω i 1 = ω i + β i (21) v i 1 = L i ω i sin β i + v i cos β i (22) β i = Atan2c (L i ω i v i 1, v i v i 1 ) R (23) Atan2c (, ) : R R R M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 16 / 42

17 SPNP: wyprowadzenie sterownika 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(τ) = τ Y y N trailer N (segment N+1) P v N = v N N = X x N LTPT: Last-Trailer Posture Tracker, LTPS: Last-Trailer Posture Stabilizer M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 17 / 42

18 SPNP: wyprowadzenie sterownika 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(ē, ) (24) 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 1, 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) Metoda VFO dla pojazdów N-przyczepowych 18 / 42

19 SPNP: wyprowadzenie sterownika kaskadowego (pętle wewnętrzne) Propozycja procedury projektowej K1. propagacja prędkości zadanych oraz k [ at zadany w złaczu (na podstawie (21)-(23)): ] ω di 1 ω di + βi d (25) v di 1 L i ω di sin β i + v di cos β i (26) β di Atan2c (L i ω di v di 1, v di v di 1 ) R (27) K2. konstrukcja i-tej wewnętrznej pętli sprzężenia zwrotnego: [ βi ] d k i(β di β i ) + β di = k i e di + β di, k i > (28) ω di v di (26) v di-1 β di e di (27) - d/dτ k i ω di ω di-1 v di-1 JCM i Joint Control Module β i k i > parametr projektowy M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 19 / 42

20 SPNP: schemat układu sterowania kaskadowego Równania sterownika: q t q LTPT / LTPS ω dn v dn JCM N... ω d2... v d2 JCM 2 ω d1 v d1 JCM 1 ω d v d kinematyka SPNP β N q wyjście systemu β i wyjścia pomocnicze... β 2 pętle wewnętrzne pętla zewnętrzna β 1 q v dn := Φ v(ē, ) (29) ω dn := Φ ω(ē, ) (3) v dn 1 := L N ω dn sin β N + v dn cos β N (31) ) β dn := Atan2c (L N ω dn v dn 1, v dn v dn 1 (32) ω dn 1 := k N (β dn β N ) + β dn + ω dn (33). v d1 := L 2 ω d2 sin β 2 + v d2 cos β 2 (34) β d2 := Atan2c (L 2 ω d2 v d1, v d2 v d1 ) (35) ω d1 := k 2 (β d2 β 2 ) + β d2 + ω d2 (36) v d := L 1 ω d1 sin β 1 + v d1 cos β 1 (37) β d1 := Atan2c (L 1 ω d1 v d, v d1 v d ) (38) ω d := k 1 (β d1 β 1 ) + β d1 + ω d1 (39) Pozostaja do określenia jawne postaci funkcji sterujacych Φ v(ē, ) oraz Φ ω(ē, )... M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 2 / 42

21 Efekt składania łańcucha kinematycznego pojazdu SPNP A 1 R d 2 R D tractor (segment 1) trailer 1 (segment 2) tractor (segment 1) trailer 2 (segment 3) B 1 R 2 Q R tractor (segment 1) trailer 1 (segment 2) trailer 2 (segment 3) A: kostrukcja mechanicznie dopuszczajaca składanie się pojazdu (β 1, β 2 R) B: kostrukcja z mechanicznym ograniczeniem drugiego kata skręcenia (β 2 Q β R) M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 21 / 42

22 Efekt składania łańcucha kinematycznego pojazdu SPNP Przedstawiona postać równań dopuszcza występowanie efektu składania pojazdu, ponieważ: β di Atan2c Li ω di v di 1, v di v di 1 R }{{}}{{} R R Propozycja unikania efektu składania: wówczas v di 1 σ L i ω di sin β i + v di cos β i, σ { 1, +1} (4) β di Atan2c L i ω di v di 1, v di v di 1 }{{}}{{} R R + gdy sgn(v di )=σ? Skuteczność propozycji (4) zależy od własności sterownika pętli zewnętrznej (LTPT/LTPS). Poza modyfikacja (4) 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) Metoda VFO dla pojazdów N-przyczepowych 22 / 42

23 Kwestie implementacyjne 1 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 1 {sβ di (s)/(1 + 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+1 (41) 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), 1998 M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 23 / 42

24 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 1 kmax kmax (2,) u 1max u 1dop u 1max Ω = kmax u 2max [ ] b/(2r) 1/r u b/(2r) 1/r, u = [u 1 u 2 ] T = [ω v ] T, Ω = [ω P ω L ] T b: rozstaw kół ciagnika, r: promień koła ciagnika Procedura skalowania prędkości: u s = 1 s u, { s max 1; ω P ω k max ; } ω L 1 (42) ω k max Składowe przeskalowanego sterowania u s sa już realizowalne fizycznie. M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 24 / 42

25 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 25 / 42

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

27 Sterownik VFO jako LTPT lub LTPS - interpretacje geometryczne śledzenie trajektorii k p =1 sterowanie do punktu k p =1 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 * v e * y G wirtualna N-ta przyczepa referencyjna x G tn * g 2t przyjęte 2ε otoczenie pozycji y referencyjnej dla N N-tej przyczepy q * t = 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) Metoda VFO dla pojazdów N-przyczepowych 27 / 42

28 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 (4)) Efekt naprowadzania N-tej przyczepy przydatny w prostowaniu łańcucha SPNP Prostota strojenia sterownika VFO: K1. 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., 18(1), 21 M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 28 / 42

29 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 29 / 42

30 SPNP: śledzenie trajektorii złożonej (ruch tyłem) TRAJECTORY TRACKING + VFO for LTPT Warunki symulacji 2 q() = [ π/2.3] T q t () = [ π/2 1 ] T ω 3t =.15(1 + sin.5τ) rad/s, v 3t =.2 m/s β d1 := β d1f, T F =.5 s βd2,3 := Wartości parametrów L 1 = L 2 = L 3 =.25 m k 1 = 5, k 2 = 2, k 3 = 5, ɛ =.1 k a = 2, k p = 1, ɛ h =.1 ω k max = 8π rad/s, b =.17 m, r =.25 m y G [m] q t () Animacje: S2RMPMovie.gif S2FoldRMPMovie.gif x G [m] 1 e θ [rad] e x [m] e y [m] 2 1 β 1 [rad] β 2 [rad] β 3 [rad] 1 5 ω [rad/s] v [m/s] time [s] time [s] time [s] M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 3 / 42 5

31 SPNP: sterowanie do punktu (parkowanie równoległe tyłem) Warunki symulacji 1.5 SET POINT REGULATION + VFO for LTPS q() = [ π/2 ] T q t = [ π/2 1 ] T ω 3t = v 3t β d1 := β d1f, T F =.5 s βd2,3 := 1 Wartości parametrów y G [m].5 L 1 = L 2 = L 3 =.25 m k 1 = 5, k 2 = 2, k 3 = 5, ɛ =.1 k a = 2, k p = 1, η =.8, ɛ h =. ω k max = 8π rad/s, b =.17 m, r =.25 m q t.5 Animacje: S3RMPMovie.gif S3FoldRMPMovie.gif x G [m] e θ [rad] e x [m] e y [m] 2 1 β 1 [rad] β 2 [rad] β 3 [rad] 5 ω [rad/s] v [m/s] time [s] time [s] time [s] M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 31 / 42

32 SPNP: wpływ efektu naprowadzania na prostowanie pojazdu SET POINT REGULATION + VFO: k p =1., η=.1 SET POINT REGULATION + VFO: k p =1., η=.3 SET POINT REGULATION + VFO: k p =1., η= y G [m].5 y G [m].5 y G [m] x G [m] x G [m] x G [m] SET POINT REGULATION + VFO: k p =1., η=.6 SET POINT REGULATION + VFO: k p =1., η=.8 SET POINT REGULATION + VFO: k p =1., η= y G [m].5 y G [m].5 y G [m] x G [m] x G [m] x G [m] Czasy trwania poszczególnych symulacji: τ h 29 s dla η =.1, η =.3, η =.4 oraz τ h = 4 s dla η =.6, η =.8, η =.9. M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 32 / 42

33 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 33 / 42

34 OPNP: propagacja prędkości wdłuż łańcucha kinematycznego Propagacja prędkości w kierunku ciagnik przyczepy (na podstawie kinematyki (8)): [ ] ωi = v i ] [ L hi cos β L i i 1 sin β L i i } L hi sin β i {{ cos β i } J i (β i ) [ ] ωi 1 v i 1 (59) Propagacja prędkości w kierunku przyczepy ciagnik (na podstawie (59)): [ ] [ ] ωi 1 L i 1 cos β = L i sin β hi L i hi v i 1 L i sin β i cos β i }{{} J 1 (β i i ) [ ] ωi v i (6) det(j i (β i )) = L hi /L i M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 34 / 42

35 OPNP: modyfikacja pętli wewnętrznych dla przegubów typu po Transformacja prędkości zadanych (na podstawie (6)): [ ] [ ] ωdi 1 L i 1 [ωdi ] [ ] cos β = L i sin β hi L i hi = J 1 ωdi v di 1 L i sin β i cos β i v i (β i ) di v di (61) ω di v di J i -1 (β i ) ω di-1 v di-1 JTM i Joint Transformation Module β i JTM i odwzorowanie algebraiczne (brak dynamiki) M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 35 / 42

36 OPNP: schemat układu sterowania w przypadku N po Równania sterownika: q t q LTPT / LTPS ω dn v dn JTM N J N -1 (β N ) β N q wyjście systemu β i wyjścia pomocnicze ω d2 JTM v 2 d2-1 J 2 (β 2 ) β 2 ω d1 v d1 pętle wewnętrzne pętla zewnętrzna JTM 1 J 1-1 (β 1 ) β 1 ω d v d kinematyka W tym przypadku obwody podrzędne nie wprowadzaja dodatkowej dynamiki nieliniowe odwzorowanie statyczne zależne od bieżacej konfiguracji. OPNP q v dn := Φ v(ē, ) (62) ω dn := Φ ω(ē, ) (63) [ ] [ ] ω dn 1 := J 1 v dn 1 N (β ω N ) dn v dn. [ ] ω d := J 1 v 1 (β 1) d [ ] ω d1 v d1 ( [ ] N [ ] ω d = J 1 Φ (β v i i )) ω(ē, ) d Φ v(ē, ) i=1 (64) (65) (66) M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 36 / 42

37 OPNP (SPNP): schemat układu sterowania w przypadku ogólnym q t q LTPT / LTPS ω dn v dn JTM N / JCM N β N ω d2 v d2 JTM 2 / JCM 2 β 2 ω d1 v d1 JTM 1 / JCM 1 β 1 ω d v d kinematyka OPNP (SPNP) q pętle wewnętrzne pętla zewnętrzna JTM i gdy i-ty przegub jest typu po JCM i gdy i-ty przegub jest typu O M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 37 / 42

38 OPNP: śledzenie trajektorii złożonej (ruch tyłem dla N po) TRAJECTORY TRACKING + VFO for LTPT Warunki symulacji q() = [ 2π 1.5.5] T q t () = [ π/2 1 ] T ω 3t =.15(1 + sin.5τ) rad/s, v 3t =.2 m/s Wartości parametrów L 1 =.35 m, L 2 = L 3 =.25 m L h1 =.1 m, L h2 =.2 m, L h3 =.1 m k a = 2, k p = 1, ɛ h =.1 ω k max = 8π rad/s, b =.17 m, r =.25 m y G [m].5.5 q t () Animacje: S6RMPMovie.gif S6FoldRMPMovie.gif* * ruch przodem przyczepy referencyjnej (v 3t = +.2 m/s) x G [m] e θ [rad] e x [m] e y [m] time [s] time [s] β 1 [rad] β 2 [rad] β 3 [rad] ω [rad/s] v [m/s] time [s] M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 38 / 42

39 OPNP: sterowanie do punktu (parkowanie prostopadłe tyłem dla N po) Warunki symulacji q() = [ π 2 ] T q t = [ π/2 1 ] T ω 3t = v 3t SET POINT REGULATION + VFO for LTPS Wartości parametrów L 1 =.35 m, L 2 = L 3 =.25 m L h1 =.1 m, L h2 =.2 m, L h3 =.1 m k a = 2, k p = 1, η =.8, ɛ h =. ω k max = 8π rad/s, b =.17 m, r =.25 m y G [m] Animacje: S7RMPMovie.gif S7FoldRMPMovie.gif* * żadany ruch przodem dla ostatniej przyczepy (σ := +1) q t x G [m] e θ [rad] e x [m] e y [m] β 1 [rad] β 2 [rad] β 3 [rad] 5 5 ω [rad/s] v [m/s] time [s] time [s] time [s] M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 39 / 42

40 Plan 1 Wprowadzenie 2 Modele kinematyki pojazdów N-przyczepowych 3 Definicja zadania sterowania 4 Kaskadowa struktura sterowania dla SPNP 5 Wykorzystanie metody VFO 6 Wyniki symulacji dla SPNP 7 Zastosowanie metody dla OPNP 8 Uwagi końcowe M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 4 / 42

41 Problemy otwarte i przyszłe prace 1 Ograniczenie skutków wrażliwości układu zamkniętego w otoczeniu punktu q t (skutek nieciagłości algorytmu) 2 Modyfikacja sterownika dla OPNP w ruchu przodem (skutek nieminimalnofazowości w ruchu przodem dla L hi > ) 3 Badania w zakresie odporności układu zamkniętego (niepewność parametryczna: L si L i oraz L shi L hi, szumy pomiarowe w sprzężeniu zwrotnym: q n = q + n) 4 Przeprowadzenie formalnej analizy stabilności i zbieżności (twierdzenie o stabilności systemów wzajemnie połaczonych interconnected systems) 5 Sterowanie z ograniczeniami na stan (ograniczone katy w złaczach) 6 Eksperymentalna weryfikacja metody (stanowisko laboratoryjne RMP-SW) 7 Wykorzystanie metody do realizacji systemu wsparcia kierowców M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 41 / 42

42 ... Dziękuję za uwagę! M. Michałek (KSIS-PP) Metoda VFO dla pojazdów N-przyczepowych 42 / 42