VIII. Autonóm járművek, formáció irányítás 1. Autonóm robotok irányításánál alkalmazott nemlineáris irányítási módszerek áttekintése. A bemenet/kimenet linearizálás, a backstepping és a mozgó horizontú prediktív irányítás elve. Állapotbecslés kiterjesztett Kalman-szűrővel. Nemlineáris irányítási módszerek: – Bemenet/kimenet linearizálás relatív fokszám, statikus visszacsatolás Bronovsky linearis alak + zéró-dinamika – Differenciális simaság (flatness) elvű Dinamikus állapot-visszacsatolással + koordináta transzformációval linearizálható – Visszalépéses stabilizálás (backstepping) szigorú visszacsatolásos alak, Ljapunov technika – Mozgó horizontú prediktív irányítás (RHC) LTI /LTV linearizálás + optimalizálás felny. körben első beavatkozó jel kiadása zárt körben MIMO bemenet/kimenet linearizált rendszer blokkvázlata:
Szigorúan visszacsatolásos alakban adott n relatív fokszámú rendszer: x˙1=G 1( x 1) x 2+ f 1 ( x1 )+W 1 (x 1 )δ 1( t) … x˙ i=G i ( xi ) x i+1+ f i ( xi )+W i ( x i )δi ( t) … x˙n =Gn ( x n)u+ f n ( x n)+W n ( x n) δn (t ) , x i=( x T1 x T2 .. x Ti )T y=h(x 1 ) , h x ( x 1 ), Gi ( x i) invertálható, δi ( t) korlátos 1
Állapotbecslés kiterjesztett Kalman-szűrővel (EKF): Sorfejtés az előző becslés helyén
2. Az automatikus akadályelkerülési feladat megfogalmazása földi jármű esetén. Pályatervezés az elasztikus szalag elve alapján, útszegély, statikus és mozgó akadályok figyelembevétele. Simítás spline, ppval, unmkpp, mkpp MATLAB eszközökkel, a referencia pálya meghatározása. Automatikus akadályelkerülési feladat: – Adott: álló és mozgó akadályok – Pályatervezés az elasztikus szalag elve alapján – Irányítás: Lie-algebrai (DGA), prediktív (RHC) – 2-antennás diff. GPS+IMU, 2-szintű Kalman-szűrő – Irányítás gyors prototípus tervezése Automatikus ütközéselkerülő pályatervezés: Elasztikus szalag: Belső és külső erők: –
N −1
int i
i=0
–
N −1
Csomópontok: belső potenciál V = ∑ V = ∑ int
i=0
2 1 k i (∣r i+1−r i∣−l 0i ) , az erők a 2
negatív gradiensek B Útszegélyek: r i → r i legközelebbi az útszegély mentén, q ∈{ l , r } : B 1 B B B B 2 r i−r i F i =M exp(− (∣r i−r i ∣/σ ) ) , σ B =k B / √ 2 ln (M B /mB ) , k B / k B =3 , B 2 r −r ∣i i∣ q
q
q
q
q
q
q
l
r
q
B
–
B
M =2 , m =0.05 Statikus akadályok: lassan változó taszító erők => megfelelő pályagörbület O O d / 2 r i−r F Oi , stat =k O , k O =3 ∣r i−r O ∣ ∣r i−r O ∣ O Mozgó akadályok: ri reached at ti ⟶ moving obsacle is in r (t i ) d j 2 r i−r O (t i) O O O F i , mov =k exp (−(∣r i−r (t i)∣− ) ) 2 ∣r −r O (t )∣ j
j
j
j
j
j
–
j
j
j
j
j
j
j
i
i
M
–
Erőegyensúly:
int B B O F sum i = F i + F i + F i +∑ F i* =0 , l
r
j
x=(r T1 , r T2 ,... , r TN ) ,
j=1
– – –
T sum T sum T f ( x )=((F sum 1 ) ,( F 2 ) , ... ,( F N ) ) Matlab fsolve: f(x) = 0, Jacobian (derivative) of f(x) ⟶ 1s számítási idő Deriváltak: elasztikus szalag ⟶ törött vonal (x,y) ⟶(t,x), (t,y) sorozatok simítás: MATLAB: spline, ppval, unmkpp, mkpp Referencia pálya: β=0 ⇒ X , X˙ , X¨ ,Y , Y˙ , Y¨ , v , v˙ , κ , ψ , ψ˙ , ψ¨ (kívánt)
3. Földi jármű modelljének bemenet-affin approximációja. A modell nemlineáris szétcsatolása differenciálgeometriai (DGA) módszerrel. A paraméterek megválasztása és a zárt szabályozási kör alakja. Approximált nemlineáris (bemenet-affin) járműmodell:
Bement-affin modell nemlineáris szétcsatolása: Jelölések: y 1=λ 1 w1−α01 x5 −α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 √ λ Aperiodikus határeset: s 2+2 √ λ s+λ=0 1 Irányítás (DGA): w i :=wid + ( α1i w˙id + w¨id ) , 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 ] mv , u 1=−S h+[(C 12 x 1−S 12) y 1+(S 12 x 1+C 12 ) y 2 ]mv ˙ S l ψ δ w = v +β+ F cF vG Zárt szabályozási kör: ( w¨id − y¨ i)+α1i ( w˙id − y˙ i )+λ( wid − y i )=0 4. A nemlineáris mozgó horizontú prediktív irányítási algoritmus (RHC ). Linearizálás a horizont kezdetén, LTV állapotegyenlet a perturbációkra. A célfüggvény alakja, korlátozás a végállapotra, a megoldás elve. Az RHC algoritmus lépései. LTV linearizálás minden horizont kezdetén: – Állapot sorozat (nominális): { x 0, x1, ... , x N } – Irányítás sorozat (nominális): {u 0, u 1, ... , u N −1 } – Kimenet sorozat (nominális): { y 0=C x0, y 1=c x 1, ... , y N =C x N } – Előírt kimenet sorozat: { y d0 , y d1 ,... , y dN } – Hiba sorozat (nominális): {e 0= y d0− y 0, e 1= y d1− y 1, ... , e N = y dN − y N } – Állapot perturbációk: {δ x 0 = x̂0− x 0, δ x1, ... , δ x N } – Irányítás perturbációk: {δ u 0, δ u 1, ... , δ u N −1 } – LTV perturbált rendszer: δ x i+1 , Ai δ x i +Bi δ u i
– –
Kimenet hibák (reális): y di −C ( x i+δ x i )=ei −δ y i N −1 N −1 2 2 1 1 Költségfüggvény: J = ∑ ∥e i−δ y i∥ + λ ∑ ∥δ u i∥ 2 i =1 2 i =0
LTV állapotegyenlet a perturbációkra:
N −1
Jelölések:
T m= ∑ h i ei , L1 :=H T1 H 1+λ I , Lμ := H 2 L−1 1 H2
i =1
Megoldás Lagrange-multiplikátor szabállyal: −1 T −1 T −1 −1 T T −1 −1 T δ U =L { H 2 Lu e N +( I −H 2 Lμ H 2 L1 )m−[ H 1 P 1+H 2 Lu (P 2−H 2 L1 H 1 P 1)] δ x 0 } Irányítás zárt körben: u 0+δ u 0 , u0: Nominális irányítás (l. algoritmus), δu0: Első eleme a felnyitott körben optimális δU irányítási sorozatnak
RHC algoritmus: 1. Nominális állapot sorozat számítása a kezdeti állapotból és a nominális irányítás sorozatból. Nominális kimenet és hiba sorozatok számítása. 2. Diszkrétidejű LTV modell számítása a perturbációk számára Eulerformulával. 3. Optimális irányítás sorozat számítása felnyitott körben (változás és teljes). Első elem kiadása zárt körben. 4. Következő irányítás számítása a horizont vége után (DGA, LS, ismétlés). A bővített irányítás kell az eltoláshoz. 5. Nominális irányítás számítása a következő horizont számára a bővített irányítási sorozat balra tolásával.
5. Állapotbecslés GPS/IMU érzékelőkkel földi robot esetén. 2-antennás differenciális GPS. Kalman-szűrő az első szinten, szögsebesség becslés. Kalman-szűrő a második szinten. Az állapotbecslés implementálása, sebesség és pozíció becslés. Érzékelő: 2 antennás differenciális GPS, 3D gyorsulásérzékelők és giroszkópok GPS V m sebesség a GPS koordináta-rendszerben – GPS Ψm orientáció (yaw) a GPS koordináta-rendszerben – a x , m longitudinális gyorsulás az autó koordináta-rendszerben – a y , m transzverzális gyorsulás az autó koordináta-rendszerben – r m szögsebesség az autó koordináta-rendszerben – GPS GPS GPS GPS – Csúszási szög: γ=atan2(V 2 , V 1 )→ β =γ−ψm GPS GPS GPS GPS u GPS )+noise , u GPS )+noise x ,m =∥V m ∥cos (β y , m=∥V m ∥sin(β
Állapotbecslés implementálása: T Time update: x_ =(t+1)= Ad x+(t )+B d u( t) , P_ (t+1)= Ad P+(t ) Ad +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) Mintavételi idők: TINS= T= 0.01s (100Hz), TGPS ,vel = 0.1s (10Hz), TGPS ,att = 0.2 s (5Hz) ̂ û x , û y ) További becslések: v̂ G =√ û 2x + û 2y , β=atan2( ̂ , Ŷ :=Ŷ +T v̂ G sin( ψ+ ̂ ̂ β) ̂ β) X̂ := X̂ +T v̂ G cos ( ψ+ 6. Négyrotoros autonóm beltéri helikopter irányítása. Dinamikus modell, emelőerő és forgató nyomatékok, mozgásegyenletek. Backstepping irányítás az approximált modell alapján. A kétszintű backstepping irányítás blokkvázlata, a benne szereplő jelek értelmezése. Dinamikus modell:
Emelő erő és forgatónyomatékok: 4
(
u= f 1+ f 2 + f 3+ f 4=b ∑ Ωi2 , τ= i=1
lb (Ω24−Ω22) lb (Ω32−Ω21) 2
2
2
2
d (Ω2+Ω4 −Ω1 −Ω3)
)
Mozgásegyenletek: ¨ m ξ=AF I ω+ω×( ˙ I ω)=τ ext ext +F g , Approximált differenciálegyenlete:
Backstepping irányítás elve egy változó esetén:
Irányítási törvény egy változóra:
Kétszintű backstepping irányítás:
7. Kétszintű állapotbecslés képfeldolgozás és IMU bevonásával. A kétszintű állapotbecslés blokkvázlata, a benne szereplő jelek értelmezése. Gyors prototípus tervezés, hardware-in-the-loop tesztelés. A tesztelési struktúra blokkvázlata, kommunikáció és szinkronizálás. Kétszintű állapotbecslés:
ξ=( x , y , z )T ,
η=( Φ ,Θ , Φ)T
Gyors prototípus tervezés: – Szabályozó, helikopter és szenzorok MATLAB/Simulink eszközökkel definiálva – Tesztelt megoldások konverziója RTW és Target Compiler bevonásával MPC555 és dSPACE eszközökre – MPC555 C compiler (Code Warrior) – DS1103 Control Desk alatt fut Hardware-in-the-loop test: – Irányítási rendszer valós időben fut, Ts=30ms mintavételi idő – Irányítási algoritmus Freescale MPC555 mikroprocomputer-en implementálva – Helikopter és szenzor rendszer DS1103 fejlesztőkártyán szimulálva valós időben – Kommunikáció CAN-buszon valós időben Tesztelési struktúra:
Kommunikáció szinkronizálás + Indítási feltételek
8. Formációban haladó autonóm járművek irányítása. Pályaparaméterezés egyetlen skaláris változóval. Kommunikációs kapcsolat, dinamikusan változó csoportok, kommunikációs gráf. A backstepping technikán és a passzivitás elméleten alapuló hierarchikus formáció irányítás elve. A szinkronizált pályakövetés blokkvázlata, a blokkvázlat részeinek feladatai. Formációban haladó autonóm járművek irányítása: – Vízszintes síkban mozgó járművek – Járművek előírt pályája geometriailag egy skalár változóban paraméterezett – Konstans közös haladási sebesség (v) – Szinkronizációs hibák decentralizáltan szabályozandók (szűrt gradiens, SPR – Csoportba szerveződött járművek, korlátozott kommunikáció Hierarchikus formáció irányítás elve: – Backstepping technikával szabályozott járművek + kiegészítő szinkronizáció szabályozás – A kommunikációs kapcsolat csak a csoporton belüli járművek szinkronizációs hibájára terjed ki (v-w) – A kommunikációs topológiát egy D mátrix írja le – Az irányítás hierarchikus – Elméleti alap: passzivitás elmélet, Ljapunov direkt módszere, szigorúan pozitív valós rendszerek (SPR) Szinkronizált pályakövetés blokkvázlata:
