VIII. Autonóm járművek, formácó rányítás 1. Autonóm robotok rányításánál alkalmazott nemlneárs rányítás módszerek áttekntése. A bemenet/kmenet lnearzálás, a backsteppng és a mozgó horzontú predktív rányítás elve. Állapotbecslés kterjesztett Kalman-szűrővel. Nemlneárs rányítás módszerek: Bemenet/kmenet lnearzálás relatív fokszám, statkus vsszacsatolás Bronovsky lnears alak + zéró-dnamka Dfferencáls smaság (flatness) elvű Dnamkus állapot-vsszacsatolással + koordnáta transzformácóval lnearzálható Vsszalépéses stablzálás (backsteppng) szgorú vsszacsatolásos alak, Ljapunov technka Mozgó horzontú predktív rányítás (RHC) LTI /LTV lnearzálás + optmalzálás felny. körben első beavatkozó jel kadása zárt körben MIMO bemenet/kmenet lnearzált rendszer blokkvázlata: Szgorúan vsszacsatolásos alakban adott n relatív fokszámú rendszer: x 1 =G 1 ( x 1 ) x 2 + f 1 ( x 1 )+W 1 (x 1 )δ 1 (t) ẋ =G ( x ) x +1 + f ( x )+W ( x )δ (t) x n =G n ( x n )u+ f n ( x n )+W n (x n )δ n (t ), x =(x T 1 x T 2.. x T ) T y=h(x 1 ), h x 1 (x1 ), G ( x ) nvertálható, δ (t) korlátos Állapotbecslés kterjesztett Kalman-szűrővel (EKF): Sorfejtés az előző becslés helyén
2. Az automatkus akadályelkerülés feladat megfogalmazása föld jármű esetén. Pályatervezés az elasztkus szalag elve alapján, útszegély, statkus és mozgó akadályok fgyelembevétele. Smítás splne, ppval, unmkpp, mkpp MATLAB eszközökkel, a referenca pálya meghatározása. Automatkus akadályelkerülés feladat: Adott: álló és mozgó akadályok Pályatervezés az elasztkus szalag elve alapján Irányítás: Le-algebra (DGA), predktív (RHC) 2-antennás dff. GPS+IMU, 2-szntű Kalman-szűrő Irányítás gyors prototípus tervezése Automatkus ütközéselkerülő pályatervezés: Elasztkus szalag: Belső és külső erők: N 1 Csomópontok: belső potencál V nt = =0 N V nt = 1 1 =0 2 k ( r +1 r l 0 ) 2, az erők a negatív gradensek B Útszegélyek: r r q legközelebb az útszegély mentén, q { l,r : B F q=m B exp( 1 B 2 ( r r B q /σ B 2 r q ) ) r q B r r q, σ B q =k B q/ 2 ln (M B /m B ), k B l / k B r=3, M B =2, m B =0.05 Statkus akadályok: lassan változó taszító erők => megfelelő pályagörbület O F j, stat =k O d O j j / 2 r r O j r r j O r r j, O k O j =3 Mozgó akadályok: r reached at t movng obsacle s n r O j (t ) O F j, mov =k O j exp( ( r r O j (t ) d 2 j 2 ) ) r r O j (t ) r r O j (t ) Erőegyensúly: M F sum = F nt B + F l+ B F r+ O F j=0 *, x=(r T 1, r T 2,...,r T N ), j=1 f (x)=((f sum 1 ) T,(F sum 2 ) T,...,( F sum N ) T ) Matlab fsolve: f(x) = 0, Jacoban (dervatve) of f(x) 1s számítás dő Derváltak: elasztkus szalag törött vonal (x,y) (t,x), (t,y) sorozatok smítás: MATLAB: splne, ppval, unmkpp, mkpp Referenca pálya: β=0 X, Ẋ, Ẍ,Y,Ẏ,Ÿ,v, v,κ,ψ, ψ, ψ (kívánt)
3. Föld jármű modelljének bemenet-affn approxmácója. A modell nemlneárs szétcsatolása dfferencálgeometra (DGA) módszerrel. A paraméterek megválasztása és a zárt szabályozás kör alakja. Approxmált nemlneárs (bemenet-affn) járműmodell: Bement-affn modell nemlneárs szétcsatolása: Jelölések: y 1 =λ 1 w 1 α 01 x 5 α 11 x 4 C 12, y 2 =λ 2 w 2 α 02 x 6 α 12 x 4 S 12 Paraméterek megválasztása: λ 1 =λ 2 :=λ, α 01 =α 02 :=λ, α 11 =α 12 =2 λ Aperodkus határeset: s 2 +2 λ s+λ=0 Irányítás (DGA): w :=w d + 1 λ (α 1 w d + w d ), w 1d (t)=x d (t), w 2d (t )=Y d (t) u 1 = S h +[(C 12 x 1 S 12 ) y 1 +(S 12 x 1 +C 12 ) y 2 ]m v, u 1 = S h +[(C 12 x 1 S 12 ) y 1 +(S 12 x 1 +C 12 ) y 2 ]m v δ w = S v +β+ l ψ F c F v G Zárt szabályozás kör: ( w d ÿ )+α 1 ( w d ẏ )+λ(w d y )=0 4. A nemlneárs mozgó horzontú predktív rányítás algortmus (RHC ). Lnearzálás a horzont kezdetén, LTV állapotegyenlet a perturbácókra. A célfüggvény alakja, korlátozás a végállapotra, a megoldás elve. Az RHC algortmus lépése. LTV lnearzálás mnden horzont kezdetén: Állapot sorozat (nomnáls): { x 0, x 1,..., x N Irányítás sorozat (nomnáls): {u 0, u 1,...,u N 1 Kmenet sorozat (nomnáls): { y 0 =C x 0, y 1 =c x 1,..., y N =C x N Előírt kmenet sorozat: { y d0, y d1,..., y dn Hba sorozat (nomnáls): {e 0 = y d0 y 0, e 1 = y d1 y 1,...,e N = y dn y N Állapot perturbácók: {δ x 0 = x 0 x 0, δ x 1,..., δ x N Irányítás perturbácók: {δ u 0, δ u 1,..., δ u N 1 LTV perturbált rendszer: δ x +1, A δ x +B δ u
Kmenet hbák (reáls): y d C ( x +δ x )=e δ y N 1 e δ y 2 + 1 N 1 2 λ δ u 2 Költségfüggvény: J = 1 2 =1 =0 LTV állapotegyenlet a perturbácókra: N 1 Jelölések: m= h e, L 1 :=H T 1 H 1 +λ I, L μ := H 2 L 1 T 1 H 2 =1 Megoldás Lagrange-multplkátor szabállyal: δ U =L 1 { H T 2 L 1 u e N +(I H T 2 L 1 μ H 2 L 1 1 )m [ H T 1 P 1 +H T 2 L 1 u (P 2 H 2 L 1 1 H T 1 P 1 )]δ x 0 Irányítás zárt körben: u 0 +δ u 0, u 0 : Nomnáls rányítás (l. algortmus), δu 0 : Első eleme a felnytott körben optmáls δu rányítás sorozatnak
RHC algortmus: 1. Nomnáls állapot sorozat számítása a kezdet állapotból és a nomnáls rányítás sorozatból. Nomnáls kmenet és hba sorozatok számítása. 2. Dszkrétdejű LTV modell számítása a perturbácók számára Eulerformulával. 3. Optmáls rányítás sorozat számítása felnytott körben (változás és teljes). Első elem kadása zárt körben. 4. Következő rányítás számítása a horzont vége után (DGA, LS, smétlés). A bővített rányítás kell az eltoláshoz. 5. Nomnáls rányítás számítása a következő horzont számára a bővített rányítás sorozat balra tolásával. 5. Állapotbecslés GPS/IMU érzékelőkkel föld robot esetén. 2-antennás dfferencáls GPS. Kalman-szűrő az első sznten, szögsebesség becslés. Kalman-szűrő a másodk sznten. Az állapotbecslés mplementálása, sebesség és pozícó becslés. Érzékelő: 2 antennás dfferencáls GPS, 3D gyorsulásérzékelők és groszkópok GPS V m sebesség a GPS koordnáta-rendszerben GPS Ψ m orentácó (yaw) a GPS koordnáta-rendszerben a x, m longtudnáls gyorsulás az autó koordnáta-rendszerben a y, m transzverzáls gyorsulás az autó koordnáta-rendszerben r m szögsebesség az autó koordnáta-rendszerben Csúszás szög: γ=atan2(v GPS 2, V GPS 1 ) β GPS GPS =γ ψ m u GPS GPS x,m = V m cos(β GPS )+nose, u GPS GPS y, m = V m sn(β GPS )+nose
Állapotbecslés mplementálása: Tme update: x_=(t+1)= A d x +(t ) +B d u(t), P_ (t+1)= A d P +(t ) A T d +Q Measurement update: x + (t )=x_(t)+k [ y(t) C x_(t)], K =P_ (t)c T [C P_ (t)c T +R] 1, P + (t)=[ I KC ]P_ (t) Mntavétel dők: T INS = T= 0.01s (100Hz), T GPS,vel = 0.1s (10Hz), T GPS,att = 0.2 s (5Hz) Tovább becslések: v G = û 2 2 x +û y, β=atan2(ûx,û y ) X := X +T v G cos( ψ+ β), Ŷ :=Ŷ +T v G sn( ψ+ β) 6. Négyrotoros autonóm beltér helkopter rányítása. Dnamkus modell, emelőerő és forgató nyomatékok, mozgásegyenletek. Backsteppng rányítás az approxmált modell alapján. A kétszntű backsteppng rányítás blokkvázlata, a benne szereplő jelek értelmezése. Dnamkus modell:
Emelő erő és forgatónyomatékok: lb(ω 4 4 2 u= f 1 + f 2 + f 3 + f 4 =b Ω, =1 τ=( 2 Ω 2 2 ) )) lb(ω 2 3 Ω 2 1 ) d (Ω 2 2 +Ω 2 4 Ω 2 1 Ω 2 3 Mozgásegyenletek: m ξ=af ext +F g, I ω+ω ( I ω)=τ ext Approxmált dfferencálegyenlete: Backsteppng rányítás elve egy változó esetén: Irányítás törvény egy változóra: Kétszntű backsteppng rányítás:
7. Kétszntű állapotbecslés képfeldolgozás és IMU bevonásával. A kétszntű állapotbecslés blokkvázlata, a benne szereplő jelek értelmezése. Gyors prototípus tervezés, hardware-n-the-loop tesztelés. A tesztelés struktúra blokkvázlata, kommunkácó és sznkronzálás. Kétszntű állapotbecslés: ξ=( x, y, z) T, η=(φ,θ,φ) T Gyors prototípus tervezés: Szabályozó, helkopter és szenzorok MATLAB/Smulnk eszközökkel defnálva Tesztelt megoldások konverzója RTW és Target Compler bevonásával MPC555 és dspace eszközökre MPC555 C compler (Code Warror) DS1103 Control Desk alatt fut Hardware-n-the-loop test: Irányítás rendszer valós dőben fut, Ts=30ms mntavétel dő Irányítás algortmus Freescale MPC555 mkroprocomputer-en mplementálva Helkopter és szenzor rendszer DS1103 fejlesztőkártyán szmulálva valós dőben Kommunkácó CAN-buszon valós dőben Tesztelés struktúra: Kommunkácó sznkronzálás + Indítás feltételek
8. Formácóban haladó autonóm járművek rányítása. Pályaparaméterezés egyetlen skalárs változóval. Kommunkácós kapcsolat, dnamkusan változó csoportok, kommunkácós gráf. A backsteppng technkán és a passzvtás elméleten alapuló herarchkus formácó rányítás elve. A sznkronzált pályakövetés blokkvázlata, a blokkvázlat részenek feladata. Formácóban haladó autonóm járművek rányítása: Vízszntes síkban mozgó járművek Járművek előírt pályája geometralag egy skalár változóban paraméterezett Konstans közös haladás sebesség (v) Sznkronzácós hbák decentralzáltan szabályozandók (szűrt gradens, SPR Csoportba szerveződött járművek, korlátozott kommunkácó Herarchkus formácó rányítás elve: Backsteppng technkával szabályozott járművek + kegészítő sznkronzácó szabályozás A kommunkácós kapcsolat csak a csoporton belül járművek sznkronzácós hbájára terjed k (v-w) A kommunkácós topológát egy D mátrx írja le Az rányítás herarchkus Elmélet alap: passzvtás elmélet, Ljapunov drekt módszere, szgorúan poztív valós rendszerek (SPR) Sznkronzált pályakövetés blokkvázlata: