0 M. Z w Z q w M w M q q. M D G b 1 s

Transkript

1 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

2 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

3 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

4 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 System: LinearSys I/O: Input Point to Output Point Time (sec): 995 Amplitude: Time (sec) Step på forward speed modell Avleste parametre s K 1 som kan modelleres som et første ordens system h speed s K st s 1 Dette gir en knekkfrekvens på c.1rad/s Bodeplott av forward speed 4

5 Hastighet U [m/s] Phase (deg) Magnitude (db) TTK 419 Guidance, Navigation & Control - Computer problem 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.11m/s : 4.5 DSRV u d = 4.11m/s DSRV forward speed 5

6 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 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 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

7 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

8 Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 Plott av hastighet U og estimert Û med K 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 8

9 Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 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 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 9

10 Hastighet U [m/s] Hastighet U [m/s] TTK 419 Guidance, Navigation & Control - Computer problem 1 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.1 Resultat: Når vi øker forsterkningen K 1, minker svingefrekvensen og estimert feil blir mindre. 1

11 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

12 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 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 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

13 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) θ(t) 5 θ(t) [grader] Optimalisert trajektor med PID-regulering Plott av dybdeprofil, xz-plott. Plotter med negativ z-akse 13

14 z-posisjon [m] TTK 419 Guidance, Navigation & Control - Computer problem 1-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 14

15 TTK 419 Guidance, Navigation & Control - Computer problem 1 3 Plott av akterror δ s δ s (t) 2 1 δ s (t) [grader] 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

16 TTK 419 Guidance, Navigation & Control - Computer problem 1 E e , 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

17 TTK 419 Guidance, Navigation & Control - Computer problem 1 Simulinkmodell med Kalmanfilter Plott av estimert og virkelig vinkelhastighet til pitch 17

18 z(t) [m] q(t) [rad/s] TTK 419 Guidance, Navigation & Control - Computer problem 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 18

19 z-posisjon [m] TTK 419 Guidance, Navigation & Control - Computer problem 1 Plott av dybdeprofil Plott av dybdeprofil x og -z med kalmanfilter xz dybdeprofil x-posisjon [m] Dybdeprofil med Kalmanfilter Plott av akterror 3 Plott av akterror δ s med kalmanfilter δ s (t) 2 1 δ s (t) [grader] Akterror med Kalmanfilter 19

20 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