Computer Problem 1: SIE 39 - Guidance, Navigation, and Control Roger Skjetne March 12, 23 1 Problem 1 (DSRV) We have the model: m Zẇ Z q ẇ Mẇ I y M q q + ẋ U cos θ + w sin θ ż U sin θ + w cos θ θ q Zw Z q M w M q q w z + M θ θ No. 1a) The restoring moment in pitch can according to equation (3.124) of [1], be written ZδS M δs (1) δ S (2) This gives M BG z W {z } M θ sin θ M θ θ. M θ BG z W<. No. 1b) According to equation (3.275) of [1], assume ẇ w. Then the natural frequency in pitch is With U 4.11 (m/s) we get: s BG z W ω θ (I y M q ) s M θ (I y M q ) s.156276 U 2.1925 +.1573 6.684 U. ω θ 6.684 4.11 1.63 (rad/s). 1
1 8 w 6 4 2 2 4 6 8 1 U The pitch period for U 4.11 (m/s) becomes: T θ 2π 3.85 (s). ω θ For a neutrally buoyant vessel, there is no natural frequency in heave, since it cannot have a spring force. No. 1c) Firstwehavethat2ξω d m which gives that r k d 2ξmω 2ξm m 2ξ km. We further recall from linear control theory that for a damped system, the poles are in general the complex conjugate pair p 1,2 a ± jω such that (s + a + jω)(s + a jω) s 2 +2as + a 2 + ω 2 s 2 +2ξω s + ω 2. Hence, a ξω and a 2 + ω 2 ω 2 a2 + r 2 ω 2 such that a2 (1 r 2 )ω 2 and Together with the above equation, this gives We have d 2 1 r 2 km which gives: ξ a ω p 1 r 2. d 2 p 1 r 2 km. M q 2 p 1 r q M 2 θ (I y M q ) m Ã! 2 1 r 2 M q 2 p M θ (I y M q ) m r v Ã! u 2 t M q 1 2 p. M θ (I y M q ) Plugging in numbers from DSRV.m s µ 2.1131 r 1 2.13..93.3498 This means that the frequency of the damped system is 13% of the undamped resonans frequency ω θ. 2
No. 1d) The dynamical equation is m Zẇ Z q ẇ Mẇ I y M q q Zw Z + q w M w M q q z + M θ θ ZδS M δs δ S and can be written on the form M ν + Dν + Gη b 1 δ S. (Note: we use the approximation U U when defining M θ )Withu we get the stationary equation: ν ss D 1 Gη ss 1 Zw Z q zss M w M q M θ θ ss " # Z q Z wm q+z qm w M θ zss Z w. θ ss Z w M q +Z q M w M θ Therefore w ss Z q Z w M q + Z q M w M θ θ ss Z w q ss M θ θ ss. Z w M q + Z q M w Assuming small angles θ ss so that w ss q ss, the kinematic equations (1) for diving (pitch, heave) can be linearized around these stationary values: ż U sin θ + w cos θ U θ + w θ q or This gives the state-space model ż θ 1 1 w q U + z θ. or in traditional notation, letting x : [η > ν > ] > [z,θ,w,q] >, gives A ẋ Ax + Bδ S 1 I M 1 G M 1 x+ D The numerical values for the system is computed in the file (hydro.m): η ν + A 1 η M ν + Dν + Gη b 1 δ S (3) M 1 b 1 δ S. (4) 3
% Hydrodynamic computations for DSRV % % Thor I. Fossen - NTNU 18.4.1 U 4.11; Iy.1925; m.36391; Mqdot -.1573; Zqdot -.13; Mwdot -.146; Zwdot -.31545; Mq -.1131; Zq -.17455; Mw.11175; Zw -.43938; Mtheta -.156276/U^2; Mdelta -.12797; Zdelta.27695; M [m-zwdot -Zqdot; -Mwdot Iy-Mqdot]; D [-Zw -Zq; -Mw -Mq ]; G [ ; -Mtheta]; A1 [ -U; ]; B [Zdelta; Mdelta]; A [ A1 eye(2); -inv(m)*g -inv(m)*d ]; damp(a) %%%%%% Output: %%%%%% >> Eigenvalue Damping Freq. (rad/s).e+ -1.e+.e+ -4.15e-1 1.e+ 4.15e-1-1.73e+ + 1.6e+i 8.52e-1 2.3e+ -1.73e+ - 1.6e+i 8.52e-1 2.3e+ >> Notice that there is two complex conjugated poles and two real poles p 1 p 2.42 p 3,4 1.73 ± j 1.6 where the zero eigenvalue correspond to the z-mode. The vessel is therefore critically stable under diving. It is intuitive that the system is only critically stable in heave, since we earlier found that there is no resonans frequancy in this mode due to no spring force. If it was stable in this state, it would have to go to the surface, z(t) as t. We notice that with (w, θ) then ż and thus z(t) z(), t. The entire positive z-axis therefore becomes an equilibrium manifold for the linearized system. In a regulator design, on the other hand, we can easily introduce feedback from z to regulate the depth to any desired depth. Notice also that the resonans frequency for pitch in the multivariable system ω θ.415 (rad/s) which is higher than what you got in the assumed decoupled model in Problem 1b) and c), that is, 13% of 1.63 rad/s.22 rad/s. The reason for this is that the frequency is strongly affected by the coupling between heave and pitch. 4
The true pitch period is therefore: T θ 2π 15.14 (s)..415 We may therefore conclude that the assumption of a decoupled model is NOT valid, and hence a regulator design should be multivariable. 2 Problem 2 We will develop an LQR PID state space regulator in this problem. The model is given by the linear system (4). Let a reference depth be z r which gives η r [z r ] >. Let η η η r. This gives η η ν + A 1 η ν + A 1 η + A 1 η r ν + A 1 η since A 1 η r. To obtain integral action, define the integrator state Let now the full state be We then have ė z z z r e(t) ė [1 ] η J η. X e η > ν > >. Z t z(τ)dτ Ẋ J A 1 I X+ δ S A e X + B e δ S (5a) M 1 G M 1 D M 1 b 1 y I 3 3 C e X (5b) whereweviewthestatese and η astheoutput.nowwewanttofind the state feedback gain that minimizes the cost function Z J qe e(t) 2 + q z z(t) 2 + q θ θ(t) 2 + rδ S (t) 2 dt Z X > (t)c > e QC e X(t)+rδ S (t) 2 dt (6) where Q diag(q e,q z,q θ ). The optimal control is then given by and P c is the solution to the Algebraic Riccati Equation (ARE): δ s 1 r B> e P c X K pid X (7) P c A e +A > 1 e P c PB e r B> e P c +C > e QC e (8) whichissolvedbythecommandare.m. Recall that for a feasible solution to exist, then (A e, B e ) must be controllable and (C e, A e ) must be observable. We get the closed-loop: Ẋ (A e B e K pid ) X and it is your task to check that this system has negative eigenvalues (which is always true if (A e, B e ) is controllable and (C e, A e ) is observable). A simulation is performed using Simulink, as shown in Figure 1: Remember that the kontroller δ S K pid X is based on the linearized model. In the simulation, we use the true nonlinear plant with saturation on the rudder and nonlinear kinematics. Clearly, going from z 1m to z 1 m is a large step. However, using the weight in the cost function, r 1 and Q diag(.4, 1, 1) will give adequate performance. With initial conditions: x(), e(), and x() [1] > and a step in z :1m 1 m at t 15 s, we get the responses as shown in Figure 2: 5
Figure 1: Block diagram of the PID state feedback closed-loop system. We see that the rudder is NOT saturating, even for the BIG step. And the response is nice and smooth. The power of the method is that tuning is easy, since the weights r and Q affects the physical quantities δ S and (e, z,θ) directly in the time-domain. 6
z-pos (m) z(t) 1 Responses z(t) 5 5 1 15 2 25 3 35 4 2 θ(t) θ(t) -2 5 1 15 2 25 3 35 4 5 δ (t) s δ s (t) -5 5 1 15 2 25 3 35 4 1 x(t) vs. z(t) 5 2 4 6 8 1 12 14 16 18 x-pos (m) Figure 2: Output responses: a) z(t) b) θ(t) c) δ S (t) d) x(t) vs. z(t) 7
3 Problem 3 We will now develop a Kalman filter to estimate all the states of the plant, including the unmeasured states w and q. Assuming the noise covariance matrices are constant, we use a stationary solution to the Riccati equation in our Kalman filter. With the augmented integrator state, the linear plant is given by (9): Ẋ J A 1 I X+ δ S A e X + B e δ S + E e w(t) (9a) M 1 G M 1 D M 1 b 1 y I 3 3 C e X + v(t) (9b) where w is dynamic (process) disturbances and v is measurement noise (both assumed white noise) 1 ω z E e π 18 1, w ω θ ω w, v υ e υ z υ π ω θ q 18 Assume the noise sources are modeled by a Gauss distribution, i.e., w N (, Q w ) og v N (, R v ) where Q w and R v are two weight matrices reflecting the standard deviation of the disturbances. Note that R v are usually accurately given by the specifications of the measurement equipment. Q w on the other hand needs knowledge from environmental disturbance models and statistical data. We assume R v E vv >ª diag(r 1,r 2,r 3 ) and Q w diag(q 1,q 2,q 3,q 4 ). In the simulation of Problem 2, we have verified that this system is both controllable and observable. We design the observer states by copying the plant + an extra injection term Lỹ : The estimation error is Z X ˆX which gives ˆX A e ˆX + B e δ S + L[y ŷ] ŷ C e ˆX. Ż A e X + B e δ S + E e w(t) A e ˆX B e δ S L[y ŷ] A e Z L[C e X + v(t) C e ˆX]+E e w(t) (A e LC e ) Z Lv(t)+E e w(t). (1) Looking at the expected value of (1), we get o E nż E {(A e LC e ) Z Lv(t)+E e w(t)} d dt E {Z} (A e LC e ) E {Z} LE {v(t)} + E e E {w(t)} (A e LC e ) E {Z} since the expected value of white noise is zero (see [2] for details). Consequently, if A e LC e is a Hurwitz matrix (negative n oeigenvalues), then the expected value of the estimation error is regulated to zero. It follows that as t, E ˆX(t) E {X(t)}. In a Luenberger observer, we would choose the gain vector L by pole placement, where the poles of A e LC e should be about 5 1 times faster than the poles of the state feedback closed-loop A e B e K pid. In the Kalman filter, on the other hand, we calculate L from the Riccati equation where L P o (t) C > e R 1 v Ṗ o A e P o +P o A > e P o C > e R 1 v C e P o +E e Q w E > e (11) ³ ³ > P o () E X () ˆX () X () ˆX () 8
z-pos (m) z(t) As mentioned above, assuming a stationary solution for (11) we instead solve the algebraic Riccati equation: A e P o +P o A > e P o C > e R 1 v C e P o +E e Q w E > e (12) Notice that this equation is somewhat different than (8) with respect to transposes, etc. This gives the constant gain vector L P o C > e R 1 v that is used in the observer. We have already designed an optimal linear PID controller in Problem 2. Next, we extend that simulation with the observer we just have developed. See the corresponding simulink model for the entire system in file Comp1WithKF.mdl. Since the estimation introduces extra initial transients and the model now contains heavy disturbances, we have changed the settings of the PID controller to: r 1 8, Q diag(.1, 4, 2). Similarly, π we have for the Kalman filter: R v diag(1, 1, 18 ) and Q w.5i 4 4. To see the rest of the parameters, see the corresponding simulink model, and the m-file Comp1WithKF_run.m that runs the simulation. 15 1 5 Responses z(t) z est (t) θ(t) 5 1 15 2 25 3 35 4 5 θ(t) θ (t) est -5 5 1 15 2 25 3 35 4 5 δ (t) s δ s (t) -5 5 1 15 2 25 3 35 4 15 x(t) vs. z(t) 1 5 2 4 6 8 1 12 14 16 18 x-pos (m) Figure 3: The responses for the total system with a Kalman filter estimating all states. Simulation performed with noisy measurements and process disturbances. We clearly see in Figure 3 that the estimated states track the true states. Also we avoid saturation, even for the 9
q(t) w(t) large step z r :1m 1 m. 1.5 Speeds responses w(t) w est (t) -.5-1 -1.5 5 1 15 2 25 3 35 4 5 q(t) q est (t) -5-1 -15 5 1 15 2 25 3 35 4 Figure 4: Heave speed w(t) and pitch rate q(t) and their estimated values ŵ(t) and ˆq(t). In Figure 4 we see that the estimated speeds track the true speed pretty good, at least in the mean, which is the intention of the Kalman filter. 1 8 6 Estimation Errors e~ z~ θ~ w~ q~ 4 2-2 -4-6 -8-1 5 1 15 2 25 3 35 4 h Figure 5: Estimation errors for the estimated states ê(t), ẑ(t), ˆθ(t), i ŵ(t), ˆq(t) in the Kalman filter. A large transient happens when the set-point switches for z r, but are regulated fast back to a neighborhood of zero. Finally, we see in Figure 5 that the estimation errors are small, and zero in the mean. References [1] T.I.Fossen,Marine Control Systems: Guidance, Navigation, and Control of Ships, Rigs and Underwater Vehicles, Marine Cybernetics AS, Trondheim, Norway, 22. [2] F. Lewis and V. L. Syrmos, Optimal Control, John Wiley & Sons Ltd, 2 edition, 1995. R.S - March 12, 23 1