Computer Problem 1 TTK 4190 NavFart

Transkript

1 Computer Problem 1 TTK 419 NavFart Frode Efteland efteland@stud.ntnu.no 3 mars 24

2 Innhold 1 Oppgave 1 - DSRV a)forwardspeedmodell Simulinkmodell Matlabplott Resultat b)forwardspeedmodell-tilstandsestimering Simulinkmodell Matlabplott Resultat: Oppgave Dybdemanøvrering Simulinkmodell Matlabplott Resultat Oppgave Kalmanfilter Simulinkmodell Matlabplott Resultat

3 US Navy s Deep Submergence Rescue Vehicle 3

4 Kapittel 1 Oppgave 1 - DSRV DSRV kinematisk bevegelseslikninger ẋ = u cos θ + w sin θ (1.1) ż = u sin θ + w cos θ θ = q Dynamiske likninger for heave og pitch m Zẇ Z q Mẇ I y M q {z } M ẇ q Zw Z + q M w M q {z } D w q + M θ U 2 z θ = {z } G M ν + Dν + Gη = b 1 δ s ZδS (1.2) δ M S δs {z } b 1 hvor U = u 2 + w 2 og linearisert om kontant cruisespeed U = U = 4.11 m/ s. Det impliserer at w =. M er hydrodynamisk treghetsmatrise pga tilleggsmasse, D er hydrodynamisk demping, G beskriver tyngdekraft/oppdrift og τ = b 1 δ s er pådrag. Matrise M: m er masse ubåt, Zẇ tilleggskraft (added mass force) i z-retning pga en akselerasjon i z-retning, Z q tilleggskraft i z-retning pga aks i pitch, Mẇ er tilleggsmoment pga akselerasjon i z-retning, I y er treghetsmoment om y-akse (pitch), M q tilleggsmoment pga aks om pitchakse. Matrise D: Z w og Z q er tilleggskraft i z-retning pga hastighet i heave og pitch, M w og M q er tilleggsmoment pga hastighet i heave og pitch Matrise G: M θ er moment om pitchaksen Matrise τ: Pådragsmatrise τ = b 1 δ s.herharvikunetrorsombeskriver kraft og moment som virker på roret. 4

5 Tilstander og dimensjoner for systemet er " # pe [x, /y, z] T η = = Θ [ /φ, θ, /ψ] T = x posisjon i x-retning (surge) z posisjon i z-retning (heave) θ rotasjon om y-akse (pitch) " # νb ν = o [u, /v, w] T ω b = nb [ /p, q, /r] T = u lineær hastighet i x-retning (surge) w lineær hastighet i z-retning (heave) q vinkelhastighet om y-akse (pitchrate) " # f b τ = o [X, /Y,Z] T m b = o [ /K,M, /N] T = X kraft dekomponert i x-retning (surge) Z kraft dekomponert i z-retning (heave) M moment om y-akse (pitch) Systemet er et 3 DOF system (DOF 1,3,5 - surge,heave,pitch). Siden ubåten har konstant hastighet U = u 2 + w 2 u, så kan modellen dekoples i en forward speed modell (1.3 ) og heave-pitch subsystem (?? ) for manøvrering. Forward speed modell (surge) (m X u ) U ρc da (U u c ) U u c = τ (1.3) u c = white noise hvor (m X u ) = 1.kg, 1 2 ρc da = 1 kg/ s 1.1 a) Forward speed modell Vi skal utvikle en PID-regulator for forward speed modellen. Modellen er linearisert rundt en constant cruise-speed u d =4.11 m/ s. u c er havstrøm. Den modelleres ved å integrere opp hvit støy gjennom et lavpassfilter med knekkfrekvens på ω c =.1rad/ s Havstrøm med lavpassfilter parametrene for hvit støy er noise power =.1 5

6 1.1.1 Simulinkmodell Forward speed modell PID regulator: PID regulator Forward speed modell: 6

7 Amplitude Forward speed modell Matlabplott For å finne parametrene til modellen brukes sprangrespons-metoden Step Response 1 From: Input Point To: Output Point System: LinearSys I/O: Input Point to Output Point Time (sec): 995 Amplitude: Time (sec) Step på forward speed modell Avleste parametre τ 63 = 995 s K = 1 som kan modelleres som et første ordens system h speed (s) = K st +1 = 1 995s +1 7

8 Phase (deg) Magnitude (db) Dette gir en knekkfrekvens på ω c =.1 rad/ s Bodeplott av forward speed -5-1 Bode Diagram System: sys Frequency (rad/sec):.1 Magnitude (db): Frequency (rad/sec) Bodeplott av Forward Speed modell Parametre for PID-regulator velges til K p = 5 K i = 2 K d = 2 som gir en PID-regulator Plott av konstant hastighet med u d =4.11 m/ s: 8

9 Hastighet U [m/s] Hastighet U [m/s] 4.5 DSRV u d = 4.11m/s DSRV forward speed Plott av havstrøm u c : 1.2 Havstrøm u c Havstrøm Uc Plott av hastighet U med sprang fra 3 5m/ s: 9

10 Hastighet U [m/s] 6 DSRV med sprang fra u d = 3 til 5 m/s DSRV med sprang Resultat Av plottene ser vi at DSRV en ikke klarer å følge en konstant hastighet med veldig stor nøyaktighet. I det praksis vil det likevel være godt nok. Regulatoren er en PID-regulator. 1.2 b) Forward speed modell - tilstandsestimering Fixed gain nonlinear state estimator (m X u ) Û ρc da (U û c ) U û c = τ + K 1 ³U Û û c = K 2 ³U Û For å estimere Û og û c, må tilstandene være observerbar og styrbare Simulinkmodell 1

11 Detaljert estimatormmodell Simulink med estimator Estimatorblokk i Simulink Matlabplott Plott av hastighet U og estimert Û med K 1 =1 11

12 Hastighet U [m/s] Hastighet U [m/s] 4.5 DSRV u d = 4.11m/s estimatormodell speed U estimert speed U Estimert og virkelig hastighet U Forstørret plott av hastighet U og estimert Û med K 1 =1 DSRV u d = 4.11m/s estimatormodell speed U estimert speed U Hastighet U med K1 = 1 Plott av hastighet U og estimert Û med K 1 =1 12

13 Hastighet U [m/s] Hastighet U [m/s] DSRV u d = 4.11m/s estimatormodell speed U estimert speed U Hastighet U med K1 = 1 Plott av havstrøm u c og estimert strøm û c med forsterkning K 2 =1.83 DSRV estimatormodell havstrøm u c strøm u c estimert strøm u c Havstrøm uc med K2 = 1 Plott av havstrøm u c og estimert strøm û c med forsterkning K 2 =1 13

14 Hastighet U [m/s] Hastighet U [m/s] DSRV estimatormodell havstrøm u c strøm u c estimert strøm u c Havstrøm uc med K2 = 1 Plott av havstrøm u c og estimert strøm û c med forsterkning K 2 =.1 DSRV estimatormodell havstrøm u c strøm u c estimert strøm u c Havstrøm uc med K2 = Resultat: NårviøkerforsterkningenK 1, minker svingefrekvensen og estimert feil blir mindre. NårviøkerforsterkningenK 2, øker svingefrekvensen og estimert feil blir mindre. 14

15 Estimeringen av Û,û c er ganske bra til praktisk bruk. Pådraget τ er glatt og pent uten små svingninger. 15

16 Kapittel 2 Oppgave Dybdemanøvrering Vi skal designe en PID-basert regulator for dybdemanøvrering av DSRVen. Ubåten skal foreta et dykk fra 1m under havflaten til 1m. Alle starttilstander er. De kinematiske likningene (1.1 ) for dykking, kan lineariseres rundt stasjonærverdien. Det gir ż = u sin θ + w cos θ uθ+ w θ = q som i tilstandsrom svarer til ż 1 w = θ 1 q η = Iν+ A 1 η Vi definerer en ny vektor x + U z θ x := η T ν T T =[z,θ,w,q] T som gir ẋ = Ax + Bδ s η ν A = 1 M 1 G I η M 1 + D ν M 1 δ b s 1 Referansedybde er z r, som gir η r = z r T Vi definerer feilen som η = η η r. Dette gir η = η = ν + A 1 η = ν + A 1 (η η r ) 16

17 siden A 1 η r =gir dette η = ν + A 1 η Vi legger til integraleffekt, og definerer en integratortilstand ė = z = z z r e (t) = ė = 1 η = J η Z t z (τ) dτ Tilstander er nå Da har vi Ẋ = X = e η T ν T T J A 1 I e η T + δ s (2.1) M 1 G M 1 D ν T M 1 b 1 Ẋ = A e X + B e δ s Y = I 3x3 = C e X Vi lager nå en tilstandstilbakekobling som minimaliserer objektfunksjonen Z h J = q e e (t) 2 + q z z (t) 2 + q θ θ (t) 2 + rδ s (t) 2i dt Z h = X T (t) Ce T QC e X (t)+rδ s (t) 2i dt hvor Q = diag (q e,q z,q θ ). Forsterkningen til PID-regulatoren finnes ved å bruke lqr.m i matlab. K pid =[ ] T Det gir egenverdiene λ i = i i T Systemet må være observerbart og styrbart Obs = rank B e A e B e A 2 eb e A 3 eb e A 4 eb e = 5, dvs full rang observerbart ³ Ce Contr = rank C e A e C e A 2 e C e A 3 e C e A 4 T e = 5, dvs full rang styrbart Simulinkmodell Simulinkdiagram 17

18 z(t) [m] Simulink modell av optimalisert banefølging Matlabplott Plott av dybde z og pitchvinkel θ med et sprang i z r fra 1 til 1m Plott av dybde z og pitchvinkel θ. Sprang fra 1 til 1m 1 z(t) θ(t) 5 θ(t) [grader] Optimalisert trajektor med PID-regulering Plott av dybdeprofil, xz-plott. Plotter med negativ z-akse 18

19 z-posisjon [m] -1 Plott av dybdeprofil x og -z xz dybdeprofil x-posisjon [m] Dybdeprofil xz Vi ser av plottet over at for å dykke 9m dypere, så må vi kjøre over 11m i x-retning. Plott av akterror δ s 3 Plott av akterror δ s δ s (t) 2 1 δ s (t) [grader] Plott av akterror Resultat Vi får meget pene og glatte kurver med optimaliseringen. Det er fordi vi ikke har tatt med hverken prosess- eller målestøy. 19

20 Fra dsrv.m fila, ser man at max vinkel på roret er ±3.Detvilsi δ s 3. Det er tydelig at man er godt innenfor denne grensa, selv med et sprang på 9m. 2

21 Kapittel 3 Oppgave Kalmanfilter Nå antar vi at vi kun kan måle tilstandene posisjon x, z og pitchvinkel θ mens pitchhastigheten q er ukjent. Dermed må vi estimere q med et stasjonært kalmanfilter. Utvidet systemmatrise (2.1 ) Ẋ = A e X + B e δ s + E e w (t) Y = C e X + v (t) hvor w (t) er prosesstøy, og v (t) er målestøy. Begger er hvit støy. 1 ω z E e = π 18 1, w = ω θ ω w, v = v e v z v π ω θ q 18 Når vi designer estimatoren, kopierer vi den virkelige modellen og legger til avviket mellom esimert og virkelig y ˆX = A e ˆX + Be δ s + L (y ŷ) ŷ = C e ˆX Den estimerte feilen er Z = X ˆX, som gir Ż = ˆX Ẋ = A e X + B e δ s + E e w (t) ³ A e ˆX + Be δ s + L (y ŷ) = A e X + B e δ s + E e w (t) A e ˆX Be δ s L (y ŷ) = A e ³X ˆX ³ L C e X + v (t) C e ˆX + E e w (t) = A e ³X ˆX ³ LC e X ˆX Lv (t)+e e w (t) = (A e LC e ) Z Lv (t)+e e w (t) 21

22 Forsterkningsmatrisen L bereknes vha Riccati-likningen hvor L = P o (t) C T e R 1 v P o = A e P o + P o A T e P o Ce T Rv 1 C e P o + E e Q w Ee T ³ P () = E X () ˆX ³ () X () ˆX T () og P o er løsningen av den algebraiske Riccati-likningen Simulinkmodell A e P o + P o A T e P oc T e R 1 v C ep o + E e Q w E T e = Simulinkmodell med Kalmanfilter Matlabplott Plott av estimert og virkelig vinkelhastighet til pitch 22

23 z(t) [m] q(t) [rad/s] 4 2 Plott av virkelig og estimert pitchhastighet q(t) q(t) q est (t) Vinkelhastighet for pitch med Kalmanfilter Plott av dybde og pitchvinkel Plott av dybde z og pitchvinkel θ. Sprang fra 1 til 1m z(t) z est (t) θ(t) θ est (t) θ(t) [grader] Dybde og pitchvinkel med Kalmanfilter Plott av dybdeprofil 23

24 z-posisjon [m] Plott av dybdeprofil x og -z med kalmanfilter xz dybdeprofil x-posisjon [m] Dybdeprofil medkalmanfilter Plott av akterror 3 Plott av akterror δ s med kalmanfilter δ s (t) 2 1 δ s (t) [grader] Akterror med Kalmanfilter Resultat Vi ser at estimatene blir ganske bra. Estimerte tilstander stemmer veldig godt overens med virkelige målinger. Når spranget fra 1 til 9m kommer etter 3 sek, så utløser det en peak i de fleste estimerte tilstander. Pga støy i prossessen (noisepower =.1) og målestøy(noisepower =.5), blir plottene ganske ruglete. 24