Docstoc

Submarine Automatic Control

Document Sample
Submarine Automatic Control Powered By Docstoc
					                                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 flume         the dynamic open and closed loop behavior of the sub-
                                                           marine. The submarine is immersed in a flume 585mm
                                                           wide through which water flows 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]       flume. 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 first principles calculations of force/moment co-    Linearised Transfer Functions
efficients, this non-linear model is reduced to a set of
uncoupled, transfer functions which describe the yaw,      The following non-linear differential 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 flume 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 coefficients,
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 identified 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 figure 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
                                                                                        different 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 effect 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 first 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 differential 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 first order, we get the following linear equation.
actual system.
  The controller has a saturation limit imposed which                                                                                                                                      ˙
                                                                                                                                                                                           z = −V θ                                                          (9)
prevents the fin 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.
coefficients are identical. The only difference 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 effects, 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 effect 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 staff 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 field 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