Computer Problem 1 TTK 419 NavFart Frode Efteland efteland@stud.ntnu.no 3 mars 24
Innhold 1 Oppgave 1 - DSRV 4 1.1 a)forwardspeedmodell... 5 1.1.1 Simulinkmodell... 6 1.1.2 Matlabplott... 7 1.1.3 Resultat... 1 1.2 b)forwardspeedmodell-tilstandsestimering... 1 1.2.1 Simulinkmodell... 1 1.2.2 Matlabplott... 11 1.2.3 Resultat:... 14 2 Oppgave 2 16 2.1 Dybdemanøvrering... 16 2.1.1 Simulinkmodell... 17 2.1.2 Matlabplott... 18 2.1.3 Resultat... 19 3 Oppgave 3 21 3.1 Kalmanfilter... 21 3.1.1 Simulinkmodell... 22 3.1.2 Matlabplott... 22 3.1.3 Resultat... 24 2
US Navy s Deep Submergence Rescue Vehicle 3
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
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 + 1 2 ρ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
1.1.1 Simulinkmodell Forward speed modell PID regulator: PID regulator Forward speed modell: 6
Amplitude Forward speed modell 1.1.2 Matlabplott For å finne parametrene til modellen brukes sprangrespons-metoden Step Response 1 From: Input Point To: Output Point.9.8.7 System: LinearSys I/O: Input Point to Output Point Time (sec): 995 Amplitude:.63.6.5.4.3.2.1 1 2 3 4 5 6 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
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): -2.99-15 -2-25 -3-35 -4-45 -9 1-5 1-4 1-3 1-2 1-1 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
Hastighet U [m/s] Hastighet U [m/s] 4.5 DSRV u d = 4.11m/s 4 3.5 3 2.5 2 1.5 1.5 2 4 6 8 1 12 14 16 18 2 DSRV forward speed Plott av havstrøm u c : 1.2 Havstrøm u c 1.8.6.4.2 -.2 2 4 6 8 1 12 14 16 18 2 Havstrøm Uc Plott av hastighet U med sprang fra 3 5m/ s: 9
Hastighet U [m/s] 6 DSRV med sprang fra u d = 3 til 5 m/s 5 4 3 2 1 2 4 6 8 1 12 14 16 18 2 DSRV med sprang 1.1.3 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 ) Û + 1 2 ρc da (U û c ) U û c = τ + K 1 ³U Û û c = K 2 ³U Û For å estimere Û og û c, må tilstandene være observerbar og styrbare. 1.2.1 Simulinkmodell 1
Detaljert estimatormmodell Simulink med estimator Estimatorblokk i Simulink 1.2.2 Matlabplott Plott av hastighet U og estimert Û med K 1 =1 11
Hastighet U [m/s] Hastighet U [m/s] 4.5 DSRV u d = 4.11m/s estimatormodell 4 3.5 3 2.5 2 1.5 1.5 speed U estimert speed U 2 4 6 8 1 12 14 16 18 2 Estimert og virkelig hastighet U Forstørret plott av hastighet U og estimert Û med K 1 =1 DSRV u d = 4.11m/s estimatormodell 4.7 4.6 4.5 4.4 4.3 4.2 speed U estimert speed U 93 94 95 96 97 98 99 1 11 12 Hastighet U med K1 = 1 Plott av hastighet U og estimert Û med K 1 =1 12
Hastighet U [m/s] Hastighet U [m/s] DSRV u d = 4.11m/s estimatormodell 4.5 4.4 4.3 4.2 4.1 4 speed U estimert speed U 93 94 95 96 97 98 99 1 11 12 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.825.82.815.81.85.8.795.79 strøm u c estimert strøm u c 43 44 45 46 47 48 49 Havstrøm uc med K2 = 1 Plott av havstrøm u c og estimert strøm û c med forsterkning K 2 =1 13
Hastighet U [m/s] Hastighet U [m/s] DSRV estimatormodell havstrøm u c.83.825.82.815.81.85.8.795.79 strøm u c estimert strøm u c 43 44 45 46 47 48 49 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.83.825.82.815.81.85.8.795.79 strøm u c estimert strøm u c 43 44 45 46 47 48 49 Havstrøm uc med K2 =.1 1.2.3 Resultat: NårviøkerforsterkningenK 1, minker svingefrekvensen og estimert feil blir mindre. NårviøkerforsterkningenK 2, øker svingefrekvensen og estimert feil blir mindre. 14
Estimeringen av Û,û c er ganske bra til praktisk bruk. Pådraget τ er glatt og pent uten små svingninger. 15
Kapittel 2 Oppgave 2 2.1 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
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 =[.6.341.4854.3334.1157] T Det gir egenverdiene λ i = 2.7884.2.2851 +.2839i.2851.2839i 1.519 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 2.1.1 Simulinkmodell Simulinkdiagram 17
z(t) [m] Simulink modell av optimalisert banefølging 2.1.2 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) 8 6 4 2 5 1 15 2 25 3 35 4 15 1 θ(t) 5 θ(t) [grader] -5-1 -15-2 5 1 15 2 25 3 35 4 Optimalisert trajektor med PID-regulering Plott av dybdeprofil, xz-plott. Plotter med negativ z-akse 18
z-posisjon [m] -1 Plott av dybdeprofil x og -z xz dybdeprofil -2-3 -4-5 -6-7 -8-9 -1 2 4 6 8 1 12 14 16 18 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] -1-2 -3 5 1 15 2 25 3 35 4 Plott av akterror 2.1.3 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
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
Kapittel 3 Oppgave 3 3.1 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
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 3.1.1 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 3.1.2 Matlabplott Plott av estimert og virkelig vinkelhastighet til pitch 22
z(t) [m] q(t) [rad/s] 4 2 Plott av virkelig og estimert pitchhastighet q(t) q(t) q est (t) -2-4 -6-8 -1-12 -14 5 1 15 2 25 3 35 4 Vinkelhastighet for pitch med Kalmanfilter Plott av dybde og pitchvinkel 12 1 8 Plott av dybde z og pitchvinkel θ. Sprang fra 1 til 1m z(t) z est (t) 6 4 2 5 1 15 2 25 3 35 4 4 2 θ(t) θ est (t) θ(t) [grader] -2-4 -6 5 1 15 2 25 3 35 4 Dybde og pitchvinkel med Kalmanfilter Plott av dybdeprofil 23
z-posisjon [m] Plott av dybdeprofil x og -z med kalmanfilter xz dybdeprofil -2-4 -6-8 -1-12 2 4 6 8 1 12 14 16 18 x-posisjon [m] Dybdeprofil medkalmanfilter Plott av akterror 3 Plott av akterror δ s med kalmanfilter δ s (t) 2 1 δ s (t) [grader] -1-2 -3 5 1 15 2 25 3 35 4 Akterror med Kalmanfilter 3.1.3 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