Hjemmeeksamen TTK 4190 NavFart. 1mars2004

Transkript

1 Hjemmeeksamen TTK 490 NavFart 6664 mars004

2 Contents Teori. Rotasjonsmatrise.... Invariansavenhetskaternionen Elementærrotasjon Egenskaptilrotasjonsmatrisen Kvarternionerpropagering... 6 Simulering 8. Hydrodynamikk Tilstandsrommodell Observerbarhet Lineærtilstandsrommodell Utvidetmålematrise....4 Kalmanfilter....5 Ulineært kalmanfilter Nomotomodell Autopilotdesign Integralvirkning Tilbakekobling fra ṙ... 6

3 Chapter Teori. Rotasjonsmatrise Verifiser at rotasjonsmatrisen til enhetskvarternionen er gitt ved: R n b (Θ) ε + ε 3 (ε ε ε 3 η) (ε ε 3 + ε η) (ε ε + ε 3 η) ε + ε 3 (ε ε 3 ε η) (ε ε 3 ε η) (ε ε 3 + ε η) ε + ε Property. sier at en rotasjonsmatrise må tilfredsstille RR T R T R I og det R som betyr at R er ortogonal. Da er den inverse rotasjonsmatrisen gitt ved R R T Kvarternionene tilfredstiller betingelsen q T q η + ε + ε + ε 3 Rotasjonsmatrisen for kvarternioner er definert som hvor R n b (q) :R η,ε I 3x3 +ηs (ε)+s (ε) ηs (ε) η 0 ε 3 ε ε 3 0 ε ε ε 0 S (ε) 0 ε 3 ε ε 3 0 ε 0 ε 3 ε ε 3 0 ε ε ε 0 ε ε 0 ε ε 3 ε ε ε ε 3 ε ε ε ε 3 ε ε 3 ε ε 3 ε ε 3 ε ε

4 Da blir rotasjonsmatrisen R n b (q) I 3x3 +ηs (ε)+s (ε) η 0 ε 3 ε ε 3 0 ε + ε ε 3 ε ε ε ε 3 ε ε ε ε 3 ε ε ε ε 0 ε ε 3 ε ε 3 ε ε ε ε 3 + ηε 3 +ε ε ηε +ε ε 3 ηε 3 +ε ε ε ε 3 + ηε +ε ε 3 ηε +ε ε 3 ηε +ε ε 3 ε ε + Som er det samme som den gitte rotasjonsmatrisen.. Invarians av enhetskaternionen Bevis at enhetskvarternionen er invariant med hensyn til på rotasjonsmatrisen eks. R (η, ε) ε R T (η, ε) ε ε En enhetskvarternion tilfredsstiller q T q.settetqer definert som n Q q q T q, q η,ε T o T, ε R 3 og η R For å bevise dette bruker man eks over 3

5 R (η, ε) ε R T (η, ε) ε ε + ε 3 (ε ε ε 3 η) (ε ε 3 + ε η) (ε ε + ε 3 η) ε + ε 3 (ε ε 3 ε η) (ε ε 3 ε η) (ε ε 3 + ε η) ε ε ε + ε ε 3 ε ( ηε 3 +ε ε )+ε 3 (ηε +ε ε 3 )+ε ε ε 3 + ε (ηε 3 +ε ε )+ε 3 ( ηε +ε ε 3 )+ε ε ε 3 + ε ( ηε +ε ε 3 )+ε (ηε +ε ε 3 )+ε 3 ε ε + ηε ε 3 +ε ε + ηε ε 3 +ε ε3 + ε ε ε ε 3 + ε ηε ε 3 +ε ε + ηε ε 3 +ε ε3 + ε ε ε ε 3 + ε ηε ε +ε ε 3 + ηε ε +ε ε 3 + ε ε 3 ε ε 3 + ε 3 ε ε ε 3 ε ε 3 + ηε 3 +ε ε ηε +ε ε 3 ηε 3 +ε ε ε ε 3 + ηε +ε ε 3 ε ε ηε +ε ε 3 ηε +ε ε 3 ε ε + ε 3 ε ( ηε 3 +ε ε )+ε 3 (ηε +ε ε 3 )+ε ε ε 3 + ε (ηε 3 +ε ε )+ε 3 ( ηε +ε ε 3 )+ε ε ε 3 + ε ( ηε +ε ε 3 )+ε (ηε +ε ε 3 )+ε 3 ε ε + ε ε ε 3 Dermed er det bevist at enhetskvartionene er invariante..3 Elementær rotasjon Bevis:.4 Egenskap til rotasjonsmatrisen Bevis likningen RS (ω) R T S (Rω) Beviser likningen ovenfor ved å rekne ut venstre side, så høyre side og deretter sammenlikne svarene Vi benytter en rotasjonsmatrise om x-aksen, R x,φ, for å slippe så store matriser. 4

6 Venstre side R cosφ sin φ,r T cosφ sin φ 0 sinφ cos φ 0 sin φ cos φ S(ω) 0 r q r 0 p q p 0 RS(ω) cosφ sin φ 0 r q r 0 p 0 sinφ cos φ q p 0 0 r q r cos φ + q sin φ p sin φ p cos φ q cos φ + r sin φ pcos φ p sin φ 0 r q RS(ω)R T r cos φ + q sin φ p sin φ p cos φ cosφ sin φ q cos φ + r sin φ pcos φ p sin φ 0 sin φ cos φ 0 r cos φ q sin φ qcos φ r sin φ r cos φ + q sin φ 0 p cos φ p sin φ q cos φ + r sin φ pcos φ + p sin φ 0 0 r cos φ q sin φ qcos φ r sin φ r cos φ + q sin φ 0 p q cos φ + r sin φ p 0 Høyre side S (Rω) S cosφ sin φ p q 0 sinφ cos φ r p S q cos φ r sin φ r cos φ + q sin φ 0 (r cos φ + q sin φ) q cos φ r sin φ r cos φ + q sin φ 0 p (q cos φ r sin φ) p 0 0 r cos φ q sin φ qcos φ r sin φ r cos φ + q sin φ 0 p q cos φ + r sin φ p 0 RS (ω) R T S (Rω) Vi ser at vi får samme svar på begge sider. Dermed er det bevist. 5

7 .5 Kvarternioner propagering Bevis likningen η εt ω ε (ηi S (ε)) ω Vi starter beviset med å substituere rotasjonsmatrisen Rb n inn i likningen for Ṙn b. Da får vi likningen på formen for kvarternioner q T q (q)ω b nb hvor og ε ε ε 3 T q (q) η ε 3 ε ε 3 η ε ε ε η T T q (q)t q (q) 4 I 3x3 Da har vi q T q (q)ω b nb ε ε ε 3 η ε 3 ε p ε 3 η ε q r ε ε η pε qε rε 3 pη qε 3 + rε qη + pε 3 rε rη pε + qε ε ε ε 3 η ε 3 ε ε 3 η ε (S(ε) ε +η ε ηε) ε ε η Ṙ n b (q) R n b (q)s ω b nb R n b (ω b nb)s (q) 6

8 Vi benytter oss av hintet ω S(ε) ε +η ε ηε 3 ε ε 3 0 ε ε +η ε η ε ε ε ε 0 ε 3 ε 3 ε η ε η ε ε 0 ε 3 ε 3 Nå kan vi benytte hint ε T ε ε T ε η η 7

9 Chapter Simulering Gitt en 3 DOF forsyningskip med modell ψ r M ν + Dν τ m m m 3 u v + d d d 3 u ub c v v b c τ τ 0 m 3 m 33 ṙ 0 d 3 d 33 r τ 3 Genrelle vektorer i DOF 6 pn η n e d φ θ ψ T posisjon og eulervinkler Θ vb ν o ω b u v w p q r T hastighet og rotasjonshastighet nb f b τ o X Y Z K M N T kraft og moment som virker på skip m b o Dette reduseres i DOF3 (,,6) til ψ Θ η l vb,ν o Ψ ω µ b u v f b,τ o nb m r b X Y o N. Hydrodynamikk Vi bruker en standard modell for horisontal bevegelse (surge, sway, yaw - u, v, r). Det betyr at vi neglisjerer heave, roll og pitch (w p q 0) Vi antar lav hastighet slik at coriolismatrisen C(ν)ν er neglisjerbar og at treghetsmomentene i xz planet er I xy I yz 0 8

10 Stivt-legeme dynamikk M RB m m mx g 0 mx g I z hvor x g er en posisjonsvektor fra O til CG dekomponert i surge retning i BODY. Hydrodynamikk - added mass M A X u Y v Yṙ 0 Yṙ Nṙ Total dynamikk slik at vi får M D M ν + Dν τ (M RB + M A ) ν + Dν τ m X u m Y v mx g Yṙ 0 mx g Yṙ I z Nṙ X u Y v Y r 0 N v N r hvor D er en lineær dempematrise og surge og sway er dekoblet. M ν + Dν τ m X u m Y v mx g Yṙ ν+ X u Y v Y r ν τ 0 mx g Yṙ I z Nṙ 0 N v N r Man kan også anta at N v Y r slik at D D T hvis U 0 f.eks til dynamisk posisjonering når båten står i ro. Men slik er det ikke her. Det kan vi se på den numeriske matrisen D. Fysisk forklaring: Y v kraft Y pga akselerasjon i sway (y-retning) Yṙ kraft Y pga akselerasjon i yaw Nṙ moment N pga akselerasjon i yaw I z treghetsmoment om z-aksen i BODY Y v kraft Y pga sway hastighet (y-retning) N v moment N pga sway hastighet (y-retning) 9

11 . Tilstandsrommodell Vi kan finne en lineær modell hvis vi antar at u b c 0 v c b 0 slik at x ψ, u, v, r, u b c,vc b T Vi må finne rotasjonsmatrisen om z-aksen cos ψ sin ψ 0 R n b (Θ) sin ψ cos ψ og Eulervinkel transformasjonen, som her blir T Θ (Θ) fordi θ 0og φ 0 Tilstandsrommodell blir da ẋ Ax + Bτ ψ ψ X u cos ψ sin ψ u Y v ṙ sin ψ cos ψ v r + 0 Z 0 K u b c u b v c b c 0 M vc b N {z } {z } {z } {z} {z } Ẋ hvor u b c V c cos (β c ψ) og vc b V c sin (β c ψ) A matrisen blir da cos ψ sin ψ A : sin ψ cos ψ A x B τ Denne modellen er gyldig når Eulervinklene θ og φ er små. Altså når roll og pitch er liten. Dette er en rimelig antakelse når været er fint som impliserer lite bølger. Vi legger merke til at det ikke er kritisk mhp "Titanic-vinkelen" θ ±90, slik at rotasjonsmatrisen kan bli singulær. Det er fordi den ikke eksisterer her. Videre ser vi at modellen er basert på at båten går med konstant hastighet eller er i ro pga lineariseringen. 0

12 .3 Observerbarhet Målematrisen er gitt ved z Hx + v.3. Lineær tilstandsrommodell Vi har bare en måling, vinkel yaw ψ z ψ + white noise Målematrisen blir nå z Hx + v z ψ u v r + v u b c vc b H matrisen er nå Observerbarhet H : Obs h H T A T H T A T H T A T 3 H T A T 4 H T 0 0 sinψ cos ψ cos ψ sin ψ cosψ cos ψ sin ψ cos ψ sin ψ sinψ sinψ cos ψ A T 5 H T i Vi ser med en gang at rangen til matrisen er 4. Matrisen har ikke full rang, og da er systemet ikke observerbart med kun en måling..3. Utvidet målematrise Nå har vi tre målinger slik at z ψ u + white noise v

13 Målematrisen blir nå z Hx + v ψ z u v r + v u b c vc b H matrisen er nå Observerbarhet H : Obs h H T A T H T A T H T A T 3 H T A T 4 i H T A T 5 H T cosψ +sinψ cos ψ sin ψ +cos ψ sin ψ +cosψsin ψ cos ψ sin ψ cos ψ sin ψ +cos cosψ sin ψ cos ψ sin ψ +sin ψ cos ψ sin 3 ψ +cosψsin ψ cos 0 cosψ +sinψ 0 cosψ +sinψ cos ψ sin ψ +cos ψ sin ψ cosψ +sinψ cos ψ sin ψ + 4 cos ψ sin ψ 4 cos 3ψ 4 sin 3ψ 8 cos 4 cosψ sin ψ cos ψ sin ψ cos ψ 3 4 sin ψ 4 cos 3ψ + 4 sin 3ψ 8 cos 4ψ 0 cosψ +sinψ 0 cosψ +sinψ cos ψ sin ψ + 4 cos ψ Fortsatt har matrisen rang 4, og er ikke observerbar. Magefølelsen sier meg at denne matrisen skal være observerbar. Hvis den ikke er observerbar kan vi ikke rekonstruere alle tilstandene og vi kan ikke bruke Kalmanfilter..4 Kalmanfilter kontinuerlig og stasjonær modell ẋ Ax + Bu + Ew y Hx + v ˆx Aˆx + Bu + K (y H ˆx) K P H T R

14 hvor kovariansmatrisen P P T > 0 positiv definitt Riccattilikningen som skal løses er AP + P A T + EQE T P H T R HP 0 Q 0*[eye(3)]; R 0*[eye(3)]; x0 [ ] ; x0_est [ 3 0 ] ; P [ ]*00; v 0.*randn()*[;;]; w v; E [zeros(,3); eye(3); zeros(,3)]; P0 (x0-x0_est)*(x0-x0_est) ; A [ ]; H [ 0 0 0]; [K,P] lqe(a,e,h,q,r);$\allowbreak $.5 Ulineært kalmanfilter Man kan bruke en passiv ulineær DP observer sammen med et kalmanfilter. For å sjekke passivitet kan man bruke Kalman-Yakubovich-Popov kriteriet PA+ A T P Q B T P C hvor P P T,Q Q T,A Hurwitz,ogA, B styrbar og A, C observerbar. 3

15 .6 Nomoto modell Finn forsterkningen og tidskonstanten i de to modellene ψ (s) τ 3 ψ (s) τ 3 K( + T 3 s) s( + T s)( + T s) K s( + Ts) Matlabkode: M [6.7644e e e e3.306e]; D [7.703e e e e.44e]; A -inv(m)*d; b [0; e;.44e]; b inv(m)*b; C [ ]; D 0; [n,d] sstf(a,b,c,d); sys tf(n,d) Transfer function: s s s^ s^ s +.9e-005 Transferfunksjonen plottes i et bodediagram 4

16 Amplitude Phase (deg) Magnitude (db) System: sys Frequency (rad/sec): Magnitude (db): 7.8 Bode Diagram System: sys Frequency (rad/sec): Magnitude (db): System: sys Frequency (rad/sec): Phase (deg): Frequency (rad/sec) Bodeplot Nomotomodell Kryssfrekvensen leses av til rad/ s ved 3dB. Fasemarginen 80 finnes ved rad/ s. Forsterkning er 7.8dB 0 log (K) 7.8 µ 7.8 K log Ved sprangrespons finnes den dominerende tidskonstanten 4000 Step Response System: sys Time (sec): 6 Amplitude:.6e Time (sec) Steprespons Nomotomodell 5

17 Tidskonstanten leses av til τ 6 s.7 Autopilot design Vi følger oppskriften fra side 3 i lærebok.. Spesifisert båndbredde: Vi velger ca 0x under kryssfreksvensen ω b rad/ s. Berekner naturlig frekvens med ζ.0: ω b 0.64ω n ω n ω b rad/ s Bestemmer designforsterkningen i tilbakekoblingen: K m > 0 velges til K m 4. Berekner P-ledd: K p (m + K m ) ω n T K + ω n Berekner D-ledd: K d ζω n (m + K m ) Berekner I-ledd: K i ω n 0. K p Simulering utgår herved fra oppgaven..8 Integralvirkning Vi må ha integraleffekt fordi de fjerner stasjonære feil pga strøm, bølgekrefter og feil i modellen..9 Tilbakekobling fra ṙ Ved å tilbakekoble ṙ kan vi oppnå å fjerne hydrodynamisk masse (added mass). Kalles gjerne for akselerasjonstilbakekobling. Å måle akselerasjon er ikke vanskelig. Prinsippet er å feste et lodd til en fjær og måle kraften fra fjæren under akselerasjonen. Men å måle yaw-akselerasjon krever veldig nøyaktig måleutstyr, slik at disse blir veldig dyre. En annen måte er å integrere opp posisjon eller fart gjennom et filter. 6