Document Sample

Submarine Automatic Control Dr Peter Ridley Julien Fontan and Dr Peter Corke School of Mechanical Engineering, CSIRO Manufacturing Science and Technology, Queensland University of Technology, QCAT PO Box 883, Kenmore 4069, GPO Box 2434, Brisbane 4001, Australia, Australia, p.ridley@qut.edu.au peter.corke@csiro.au Abstract This paper investigates the automatic atti- Pivot tude and depth control of a torpedo shaped submarine. Both experimental results and Water flow dynamic simulations are used to tune feed- back control loops in order to obtain stable control of yaw, pitch and roll of the craft. Ysub Xsub Channel Zsub Figure 2: Experimental setup transfer function and to determine suitable controller gains to tune the pitch control loop. A depth control loop is also constructed and its controller gains tuned by simulation. Feedback control of yaw and roll axes is investigated, and appropriate controller gains are esti- mated using simulation. Experimental setup Figure 2 shows the experimental setup used to analyse Figure 1: Full size submarine pivoted in the ﬂume the dynamic open and closed loop behavior of the sub- marine. The submarine is immersed in a ﬂume 585mm wide through which water ﬂows at rates up to 200 Introduction litres/sec. Water depth, which determines the water This paper takes a non-linear multi degree of freedom speed (V ) is controlled by a weir at the end of the mathematical model from [Ridley, Fontan, Corke 2003] ﬂume. In this setup the submarine is restrained in a and applies it to the implementation of automatic con- cradle, which allows it to pivot about a horizontal axis trol loops which regulate the attitude (pitch, roll and through its centre of gravity. yaw) of the submarine shown in Figure 1. On the ba- sis of ﬁrst principles calculations of force/moment co- Linearised Transfer Functions eﬃcients, this non-linear model is reduced to a set of uncoupled, transfer functions which describe the yaw, The following non-linear diﬀerential equations, calcu- pitch and roll dynamics of the submarine. Experimen- lated in a body centred coordinate frame, describe tal data obtained from the full-size submarine, horizon- change of attitude of a submarine (mass m, inertia tally pivoted, in a ﬂume is used to validate the pitch [Ixx , Iyy , Izz ] ) whose velocity is V = [u, v, w]T . Root locii: Yaw, Pitch and Roll 0<V<1 m/s 2.5 2 Yaw 1.5 Roll 1 Pitch 0.5 0 −0.5 −1 −1.5 −2 −2.5 −0.7 −0.6 −0.5 −0.4 −0.3 −0.2 −0.1 0 Figure 4: Root locus: Yaw, pitch and roll poles vs submarine speed. Squares indicate pole positions for Figure 3: Submarine control surfaces. V=0.5 m/s. ˙ ˙ ˙ Ixx p + (Izz − Iyy )qr − m[yG (w − uq + vp) − zG (v − wp + ur)] 2Nuuδr = Kext ψ(s) Nuv = (4) δr (s) Izz −Nr˙ s2 + −mXg −Nur Vs+1 ˙ ˙ ˙ Iyy q + (Ixx − Izz )rp − m[zG (u − vr + wq) − xG (w − uq + vp)] Nuv V 2 Nuv V 2 = Mext Izz r + (Iyy − Ixx )pq − m[xG (v − wp + ur) − yG (u − vr + wq)] ˙ ˙ ˙ 2Muuδs V 2 = Next θ(s) Zg W −Muw V 2 = (1) δs (s) Iyy −Mq ˙ s2 + mXg −Muq Vs+1 Zg W −Muw V 2 Zg W −Muw V 2 (5) Nett external moments ( Kext , Mext , Next ) acting on the submarine are: 4Kuuδa φ(s) Zg W = (6) Kext = KHS + Kp p + Kuuδr −δrtop + δrbottom + δa (s) Ixx −Kp ˙ s2 + 1 ˙ ˙ Zg W Kuuδs −δsright + δslef t + Kprop where: 2 Mext = MHS + Muuδs u δs + Muw uw + Muq uq+ • (δr ,δs ,δa ) are the rudder,stern plane,aileron an- ˙ ˙ ˙ ˙ Mvp vp + Mw w + Mq q + Mrp rp gles, Next = NHS + Nuuδr u2 δr + Nur ur + Nuv uv+ • Nuuδ r ,Muuδ s ,Kuuδ a rudder,stern plane,aileron ef- Nv v + Nwp wp + Npq pq + Nr r ˙ ˙ ˙˙ fectiveness, (2) • Nur ,Muw (body moment), and the resulting angular velocity is ω = [p, q, r]T . • Nr , Mq , Kp (added mass), ˙ ˙ ˙ Assuming that changes of attitude, measured in Eu- • Nur ,Muq (added mass cross term) are hydrody- ler angles for roll, pitch and yaw (φ, θ, ψ), are small, the namic coeﬃcients, hydrostatic moments acting on the submarine, about • Xg ,Zg are the coordinates of the CG relative to its centre of bouyancy, are: the centre of buoyancy. KHS = −yG W − zG W φ Numerical estimates of these are tabulated in the Ap- MHS = −zG W θ − xG W (3) pendix of this paper. NHS = −yG W φ − zG W θ These transfer functions are quadratic lags of the form: If these small angle approximations are substituted θ(s) K ˙ ˙ into the combined equations 1 and 2, writing φ = p, θ = = s2 2ξ , (7) ˙ = r and ignoring negligably small quantities, the δs (s) ω 2 + ωn s + 1 q, ψ n linearised transfer functions for yaw, pitch and roll are Figure 4 shows the root locii of the poles of these : transfer functions as the submarine speed V increases. Variation of natural frequency 0<V<1.0 m/s Dynamics for yaw and pitch both depend on speed 2.2 whereas the roll dynamics are insensitive to speed. Roll 2 dynamics exhibit marginally stable poles. Open loop yaw response and pitch responses are both oscillatory 1.8 but stable. 1.6 DC gain K of the yaw and roll transfer functions are 1.4 invariant with speed, whereas the DC gain of the pitch rad./sec. transfer function varies with speed as shown in Figure 1.2 5. 1 Natural frequency and damping ratio of the pitch poles is plotted in Figure 6. Both yaw and pitch re- 0.8 sponses exhibit natural frequencies which are essen- 0.6 tially directly proportional to the waterspeed. 0.4 Positioning of the centre of gravity below the cen- tre of buoyancy (Zg positive) gives the pitch response 0.2 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 a little more damping than the yaw response. It also m/s causes the damping ratio of the pitch response to in- crease asymptotically toward a constant value (Figure Figure 6: Open loop pitch response:Natural frequency 7) whereas the damping ratio of the yaw response is ωn vs submarine speed invariant with speed. Damping ratio of the yaw and pitch modes is very 0.35 Variation of damping ratio 0<V<1.0 m/s sensitive to the estimate of Xg , the position of the cen- tre of gravity relative to the centre of buoyancy. 0.3 Experimentally measured open-loop, pitch angle re- sponses to a stern plane, step input (Figure 8) exhibit 0.25 overshoot and transient decay to a steady state, which are characteristic of a second order, underdamped re- 0.2 sponse identiﬁed in equations 5 and 7. When numer- ical values are substited for the parameters contained 0.15 in equation 5, a theoretical estimate of the natural fre- quency of 1.1 rad./sec. obtained. This ﬁgure is smaller 0.1 than the experimentally measured value of 2.8 rad/sec, observed in Figure 8. 0.05 Variation of DC gain 0<V<1.0 m/s 0.5 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 m/s 0.45 0.4 Figure 7: Open loop pitch response: Damping ratio ξ 0.35 vs submarine speed 0.3 0.25 tain automatic control of pitch angle. Initially the con- trol loop was tuned with proportional gain alone. The 0.2 pitch angle response of the submarine to step inputs 0.15 of the command input to the loop are shown at two 0.1 diﬀerent water speeds in Figures 9 and 10. As the speed increases, the response shows a marked reduc- 0.05 tion in steady state error and a less damped transient 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 response. These changes are a direct eﬀect of the in- m/s creasing DC gain of the transfer function noted in Fig- ure 5. Figure 5: Open loop pitch response: DC Gain K vs Integral and derivative gains were added to improve submarine speed the steady state error characteristic exhibited when proportional control alone is used. The root locus diagram, shown in Figure 11, shows that the system Pitch angle control is unconditionally stable. Three active poles dictate Incorporating an inclinometer into a feedback loop and the loop dynamics. A dominant ﬁrst order pole, close tuning the loop with a PID controller allows us to ob- to the origin, causes a slow drift which removes the Open loop response to a step disturbance @ V = 0.52 m/s 0.15 measured pitch 0.03 reference pitch 0.1 0.02 0.05 0.01 pitch (radians) 0 pitch (radians) 0 −0.05 −0.01 −0.1 −0.02 −0.15 −0.03 −0.2 0 50 100 150 200 250 300 350 400 450 35 40 45 50 time (s) time (s) Figure 10: Closed loop pitch command response: Figure 8: Open loop pitch disturbance response: V=0.743 m/sec. kp=11 ωn =2.8 r/sec. and ξ = 0.25 Root Locus 6 0.1 0.91 0.84 0.74 0.6 0.42 0.22 0.08 0.96 4 0.06 0.99 Closed loop poles: 2 0.04 V=0.5 m/s Imaginary Axis 0.02 pitch (radians) 14 12 10 8 6 4 2 0 0 −0.02 −2 0.99 −0.04 −4 0.96 −0.06 measured pitch reference pitch 0.91 0.84 0.74 0.6 0.42 0.22 −0.08 −6 −14 −12 −10 −8 −6 −4 −2 0 Real Axis −0.1 0 100 200 300 400 500 600 700 800 time (s) Figure 11: Pitch loop root locus as waterspeed varies: Figure 9: Closed loop pitch command response: kp=12, kd=2, ki=0.2, Design point V=0.5 m/sec. V=0.618 m/sec. kp=11 Depth control The pitch control loop is nested inside a depth steady state error. The pair of quadratic poles provide loop,using a pressure transducer as the feedback el- an oscillatory component of response superimposed on ement. A proportional plus diﬀerential controller is top of this drift. Theoretical pitch command step re- used to stabilise the loop. In order to get the depth sponse, based on a linear model, is shown in Figure transfer function we linearise the depth equation: 12 and can be compared with the experimentally mea- sured responses plotted in Figure 13. Figure 13 shows ˙ z = − sin θu + cos θ sin φv + cos θ cos φw (8) that in the actual response, steady state error is more Assuming small vehicle perturbations about θ =0, φ quickly eliminated than is theoretically predicted. This =0, u=V , v=0,w=0 and dropping any term higher is possibly due to the unmodelled non-linearities in the than ﬁrst order, we get the following linear equation. actual system. The controller has a saturation limit imposed which ˙ z = −V θ (9) prevents the ﬁn exceeding its stall angle of 14o . Sat- Taking the Laplace transform, we arrive at the desired uration dictates the upper useful limit to which the open loop transfer proportional gain can be increased.Measured response of the submarine pitch angle to impulse disturbance z(s) V inputs is shown in Figure 14. Gz (s) = =− (10) θ(s) s Command Step Response: Kp=12, Kd=2, Ki=0.2 Pitch Disturbance Response V=0.535 m/s : kp=12, kd=2, ki=0.2 1.5 0.1 0.05 0 −0.05 1 −0.1 −0.15 −0.2 210 215 220 225 230 235 240 245 250 Controller Output V=0.535 m/s: kp=12, kd=2, ki=0.2 1 0.5 0.5 0 −0.5 0 0 1 2 3 4 5 6 7 8 9 10 sec −1 210 215 220 225 230 235 240 245 250 sec. Figure 12: Command pitch step response: kp=12, kd=2, ki=0.2, Design point V=0.5 m/sec. Figure 14: Closed loop pitch disturbance response: V=0.535 m/sec. kp=12, kd=2, ki=0.2 Pitch Command Response: V=0.535, kp=12, kd=2, ki=0.2 Command Depth Response V=0.5 m/sec. (Kp=0.75, Kd=1) 0.1 0.1 0.05 0 0 −0.1 depth: [m] −0.05 −0.2 −0.1 −0.3 −0.4 −0.15 −0.5 −0.2 0 5 10 15 20 25 30 35 40 45 50 −0.6 0 10 20 30 40 50 60 70 80 90 100 1 1 0.5 pitch command: [rad] 0.5 0 0 −0.5 −0.5 −1 −1 0 5 10 15 20 25 30 35 40 45 50 0 10 20 30 40 50 60 70 80 90 100 sec. sec Figure 13: Closed loop pitch command response: Figure 15: Command depth step response: kp=0.75, V=0.535 m/sec. kp=12, kd=2, ki=0.2 kd=1, ki=0, Design point V=0.5 m/sec. Figure 15 shows the simulated response to a depth provide a stable yaw (heading angle) response. command step input. In this simulation, saturation limits were placed on both the stern plane angle de- Roll control mand (0.23 rad.) and the pitch command (0.46 rad.). The roll dynamics, as predicted by Equation 6, are independent of the water speed. Figure 16 shows the simulated roll response to a command step input. Heading control Heading control is achieved using a magnetometer to Conclusions provide directional feedback. Symmetry of the sub- This paper has developed control loops which individu- marine dictates that the pitch and yaw force/moment ally stabilise yaw, pitch and roll axes of the submarine. coeﬃcients are identical. The only diﬀerence which It is clear, however, from the original model that the arises in the transfer functions (Equations 1 and 2) dynamics between these axes is coupled. We predict is through the eﬀects, in the pitch transfer function, that separate control of each axis will be adequate for caused by the relative positioning between the centres small perturbations about straight and level cruising of buoyancy and gravity. This has very little eﬀect on conditions. It remains to be seen whether coordinated tuning the controller. A PID control loop, with simi- turns, where rotations about all three axes occur si- lar gains to the pitch control loop may be applied to multaneously, are achievable. Command Roll Response (Kp=1.0,Ki=1, Kd=4) 0.1 References 0.05 Ridley P., Fontan J., Corke P. [2003], Submarine dy- namic modeling, Proceedings Australian Conference roll: [rad] 0 on Robotics and Automation, 2nd - 4th December, −0.05 2003, Brisbane pp?-? −0.1 0 2 4 6 8 10 12 14 16 18 20 Appendix 0.3 Modeling Parameters 0.2 Symbol Magnitude Units roll command: [rad] 0.1 0 m 18.826 kg −0.1 W 184.7 N −0.2 Ixx 1.77 kg.m2 −0.3 −0.4 Iyy 1.77 kg.m2 0 2 4 6 8 10 sec 12 14 16 18 20 Izz 0.0727 kgm2 Xg 0.003 m Figure 16: Command roll step response: kp=1.0, Zg 0.0048 m kd=4.0, ki=1.0. Nuuδr -6.08 kg.rad.−1 Muuδr -6.08 kg.rad.−1 Kuuδr 4.48 kg.rad.−1 Acknowledgements Nr˙ -4.34 kg.rad−2 The authors wish to acknowledge the support of Mr ˙ -4.34 kg.rad−2 Queensland University of Technology, Schools of Me- Kr˙ -0.041 kg.rad−2 chanical and Civil Engineering who manufactured the Nuv 24 kg submarine and provided the experimental test facili- Muw -24 kg ties. Design of the submarine and experimental fa- Muq -4.93 kg.m.rad−1 . cilities and laboratory measurements were by QUT Nur -4.93 kg.m.rad−1 . undergraduate students Sam Reid and Simon Cham- bers. The staﬀ of the CSIRO, Automation Group of Automation Group at CMST, designed and man- ufactured the submarine computing and control elec- tronics and software and also undertook ﬁeld trials of the submarine. CSIRO sponsored Julien Fontan, dur- ing 2002,as a visiting scholar from Ecole Centrale de Nantes (France).

DOCUMENT INFO

Shared By:

Categories:

Tags:

Stats:

views: | 35 |

posted: | 9/19/2011 |

language: | English |

pages: | 6 |

OTHER DOCS BY wulinqing

How are you planning on using Docstoc?
BUSINESS
PERSONAL

Feel free to Contact Us with any questions you might have.