US Navy s Deep Submergence Rescue Vehicle Oppgave 1 - DSRV DSRV kinematisk bevegelseslikninger x ucos wsin ż usin wcos q Dynamiske likninger for heave og pitch # m Z w Z q w M w I y M q q Z w Z q w M w M q q M U 2 z Z M M D G b hvor U u 2 w 2 og linearisert om kontant cruisespeed U U 4.11m/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 w tilleggskraft (added mass force) i z-retning pga en akselerasjon i z-retning, Z q tilleggskraft i z-retning pga aks i pitch, M w er M D G b 1 s
TTK 419 Guidance, Navigation & Control - Computer problem 1 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. Her har vi kun et ror som beskriver kraft og moment som virker på roret. Tilstander og dimensjoner for systemet er p e x,y,z T,, T x z posisjon i x-retning (surge) posisjon i z-retning (heave) rotasjon om y-akse (pitch) o b b nb u,v,w T p,q,r T u w q lineær hastighet i x-retning (surge) lineær hastighet i z-retning (heave) vinkelhastighet om y-akse (pitchrate) f o b m o b X,Y,Z T K,M,N T X Z M kraft dekomponert i x-retning (surge) kraft dekomponert i z-retning (heave) 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 # og heave-pitch subsystem # for manøvrering. Forward speed modell (surge) m X u U 1 2 C dau u c U u c # hvor m X u 1.kg, 1 2 C da 1kg/s u c white noise 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.11m/s. u c er havstrøm. Den modelleres ved å integrere opp hvit støy gjennom et lavpassfilter med knekkfrekvens på 2
TTK 419 Guidance, Navigation & Control - Computer problem 1 c.1rad/s Havstrøm med lavpassfilter parametrene for hvit støy er noise power.1 Simulinkmodell: PID regulator: Forward speed modell Forward speed modell: PID regulator 3
Amplitude TTK 419 Guidance, Navigation & Control - Computer problem 1 Forward speed modell 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 995s K 1 som kan modelleres som et første ordens system h speed s K st 1 1 995s 1 Dette gir en knekkfrekvens på c.1rad/s Bodeplott av forward speed 4
Hastighet U [m/s] Phase (deg) Magnitude (db) TTK 419 Guidance, Navigation & Control - Computer problem 1-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.11m/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 5
Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 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 : 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 Resultat DSRV med sprang 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. 6
TTK 419 Guidance, Navigation & Control - Computer problem 1 b) Tilstandsestimering av u c Fixed gain nonlinear state estimator m X u Û 1 2 C dau û c U û c K 1 û c K 2 U Û U Û For å estimere Û og û c, må tilstandene være observerbar og styrbare. Simulinkmodell Detaljert estimatormmodell Simulink med estimator Estimatorblokk i Simulink 7
Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 Plott av hastighet U og estimert Û med K 1 1 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 8
Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 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 9
Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 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 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 Resultat: Når vi øker forsterkningen K 1, minker svingefrekvensen og estimert feil blir mindre. 1
TTK 419 Guidance, Navigation & Control - Computer problem 1 Når vi øker forsterkningen K 2, øker svingefrekvensen og estimert feil blir mindre. Estimeringen av Û,û c er ganske bra til praktisk bruk. Pådraget er glatt og pent uten små svingninger. Oppgave 2 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 (#) for dykking, kan lineariseres rundt stasjonærverdien. Det gir ż usin wcos uw q som i tilstandsrom svarer til ż 1 1 w q U z I A 1 Vi definerer en ny vektor x x : T T T z,,w,q T som gir x Ax B s A 1 M 1 G I M 1 D M 1 b 1 s Referansedybde er z r, som gir r z r T Vi definerer feilen som r. Dette gir A 1 A 1 r siden A 1 r gir dette A 1 Vi legger til integraleffekt, og definerer en integratortilstand 11
TTK 419 Guidance, Navigation & Control - Computer problem 1 ė z z z r et t zd ė 1 J Tilstander er nå X e T T T Da har vi J e X A 1 I T s # M 1 G M 1 D T M 1 b 1 X A e X B e s Y I 3x3 C e X Vi lager nå en tilstandstilbakekobling som minimaliserer objektfunksjonen J qe et 2 q z zt 2 q t 2 r s t 2 dt X T tc T e QC e Xt r s t 2 dt hvor Q diagq 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 e B e A 3 e B e A 4 e B e 5,dvs full rang observerbart Contr rank 2 3 C e C e A e C e A e C e A e C e A e4 T 5,dvs full rang styrbart Simulinkdiagram 12
z(t) [m] TTK 419 Guidance, Navigation & Control - Computer problem 1 Simulink modell av optimalisert banefølging Plott av dybde z og pitchvinkel med et sprang i z r fra 1 til 1m 1 8 Plott av dybde z og pitchvinkel θ. Sprang fra 1 til 1m z(t) 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 13
z-posisjon [m] TTK 419 Guidance, Navigation & Control - Computer problem 1-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 14
TTK 419 Guidance, Navigation & Control - Computer problem 1 3 Plott av akterror δ s δ s (t) 2 1 δ s (t) [grader] -1-2 -3 5 1 15 2 25 3 35 4 Resultat Plott av akterror Vi får meget pene og glatte kurver med optimaliseringen. Det er fordi vi ikke har tatt med hverken prosess- eller målestøy. Fra dsrv.m fila, ser man at max vinkel på roret er 3. Det vil si s 3. Det er tydelig at man er godt innenfor denne grensa, selv med et sprang på 9m. Oppgave 3 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 # X A e X B e s E e wt Y C e X vt hvor wt er prosesstøy, og vt er målestøy. Begger er hvit støy. 15
TTK 419 Guidance, Navigation & Control - Computer problem 1 E e 1 18 1 18, w z w q, v Når vi designer estimatoren, kopierer vi den virkelige modellen og legger til avviket mellom esimert og virkelig y X A e X B e s Ly ŷ ŷ C e X Den estimerte feilen er Z X X, som gir Ż X X A e X B e s E e wt A e X B e s Ly ŷ A e X B e s E e wt A e X B e s Ly ŷ A e X X L C e X vt C e X E e wt A e X X LC e X X Lvt E e wt A e LC e Z Lvt E e wt Forsterkningsmatrisen L bereknes vha Riccati-likningen L P o tc T 1 e R v hvor P o A e P o P o A e T P o C e T R v 1 C e P o E e Q w E e T P E X X X X T og P o er løsningen av den algebraiske Riccati-likningen A e P o P o A T e P o C T e R 1 v C e P o E e Q w E T e Simulinkmodell v e v z v 16
TTK 419 Guidance, Navigation & Control - Computer problem 1 Simulinkmodell med Kalmanfilter Plott av estimert og virkelig vinkelhastighet til pitch 17
z(t) [m] q(t) [rad/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 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 18
z-posisjon [m] TTK 419 Guidance, Navigation & Control - Computer problem 1 Plott av dybdeprofil 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 med Kalmanfilter 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 19
TTK 419 Guidance, Navigation & Control - Computer problem 1 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. 2