GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWAT...
14th World Congress ofIFAC
Copyr ight © 1999 IFAC 14th Triennial World Congress, Bcijing, P.R. China .
Q8fO:;!5
GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWATER VEHICLES M RKatebi Industrial Control Celltre, University of Strathclyde, UK email: re~icu.strath.ac . uk
Abstract: TIlis paper is concerned with the development of the guidance controller for Autonomous Undenvater Vehicles (AUVs). A threelayer controller architecture is proposed to enhance the autonomy of the vehicle. At the bottom layer, the local feedback controllers arc designed using H", !H2 control design teChnique to directly drive the actuators. A 4degrccoffrcedom controller is used to incorporate the sensor and actuator faults and to account for tllC interaction between the regulation and the diagnostic module. Predictive Control (PC) dcsign technique is employed at the middle layer (guidance) to optimally manocuvre the AUV along a desired trajectory. Simulation results arc presented to demonstrate the performance of the proposed technique. C opyright © 1999lFAC
Keywords: predictive control, robust stability, AUV control, integrated control, R.JH2 control, Fault MonitOring, Reconfiguration
1.
INTRODUCTION
For the execution of precision tasks, it is essential that remotely operated Autonomous Undenvater Vehicles (AUV) fo11o\,' a precise reference trajectory. Advances in computing technologies, sensing control and communications have made it possible to develop AUVs capable of operating in harsh environments. The control architecture for an AUV should always provide autonomous tasks of sensor integrdtion, vehicle status monitoring, mission and command execution. Furthermore, the highly nonlinear dynanlics of the vehicle, the stochastic nature of ocean envjrorunent and distlllbances increases the complexity of AUV control design. Valavanis, et al (1997) have identified four major control architectures, namely, hieratchical, heterarchical (multilayered), subsumption (the upper layers can take over and subsumes the lower layers) and hybrid (mixed) architectures in a study of 25 AUVs. A control architecture comprising three levels, namely, guidance, navigation and regulation are llsnally employed to design the control system. The navigation system provides the estimates of linear and angular positions and velocilies with respect to
axis for the velocities and attitude control loops. The control systems generate the thruster, propeller and other actuator signals to stabilise the vehicle around tlle set points (Fryxe11, cl all, 1995, Smestad, 1995, Bartolini, ct al. 1995). The different types of control architectures differ mainly in the way that lilC controllers, sensors and actuators communicate and interact with each other. Traditionally, the three layers of control are designed independently using wellestablished linear/nonlinear control design techniques for the local controllers and simple intuitive strategies such as Line Of Sight (LOS) for guidance. The diagnostic control system is often designed for the uncontrolled (open loop) process and hence the effect of the feedback loop is ignored. This will lead to poor perfomlance when used with the controlled (closedloop) process. The controller responds to faults in the system and it will thus hide the information from the diagnostic control system. Since the three systems are effectively coupled this approach can lead to poor stability and possible trajectory tracking error. Attempts to include a model of the trajectory in the control design are reported by FryxelL et al, (1995). The trajectory is not, however, always known, as for example in the case of tracking seabed. Moreover, the dynamics of undenvater vehicles are highly non
8462
Copyright 1999 IF AC
ISBN: 0 08 043248 4
GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWAT...
linear, due to inertia, buoyancy and hydrodynamics effects. These vehicles do not generally have a streamline shape and their dynamics can significantly change at different operating conditions. It is useful to introduce several layers of control actions to compensate the effect of nonlinear dynamics, interactions and disturbances. Such a control architecture can improve the AUV performance and make the system more autonomous. The contribution of tlris paper is a novel integrated control design approach for the regulation, and control guidance, and fault dctection reconfiguration of the AUVs. The proposed approach overcomes the problems of instability and large overshoot encountered in some decoupled control design approaches. It has the further advantage of hflving 3n implicit diagnosis module, which can be used for actuator and sensor fault detection and possible reconfiguration of the control systems. The scheme should also enhance the autonomy of AUV control systems. The inteb,'Tatc
AUVMODEL
TIle 6 DOF noruinear model of AUVs have been discussed and detailed by many researchers (see for
14th World Congress oflFAC
example, Fossen and Fjell stad, (1995)). The model used in tlus study is described in detail in Healey and Lienard, 1993. The Matiab implementation of the model can be found in Lauvdal and Fosse~ 1994. The vehicle may be described by the follovving set of nonlinear differential equations: M(t) dx(t)
dl
=;
f[x(t),z(t),c(t)] + g[x(i),z(t)]u(t) (1)
dz(t) = h[z(t),X(I),u ] dt C
where MCt) is the coupled mass matrix representing both mechanical and hydrodynamic added mass. The functions f(.) and g(.) are mappings of the vehicle motions into forces including coriolis, gravitational and centrifugal forces, e(t) representing the hydrostatic and hydrodynamic forces and moments acting on the vehicle in a body fixed coordinate frame. The function It includes the kinematic relationships, u., is the constant ocean current; x is the vector of surge (n), sway Cv), heave (w), roll (p), pitch (q) and yaw (r ) velocities, z is the vector of x, y and z positions and roll (cp), pitch (9) and yaw (q» angles, u is the vector of control inputs, i.e. rudder angle, port and starboard stern plane, top and bottom bow plane, port bow plane, starboard bow plane and propeller shaft speed. 2.1. Linear Model For a specific operating point, model may be derived as : x(t) = Ax(t) + Bu(t)
a
linear statespace
+ ;(t)
(2)
yet) = Cx(t) z(t) =; Hx(t) + ,;(t) where x(t) represents the vector of positions and attitude and the translational and rotational velocities with respect to the body axis coordinates. The signals ~ and ~are zeromean white noise sequences, which represent the disturbance and measurement noises, respectively. The signal yet) denotes the variables to be controlled. The signal Z(t) represents tlle measured variables, which usually involve a subset of tile states x(t). The u(t) is the vector of all the actuator inputs. TIH~ linear model given in equation (1) can be combined with the model of the faults (variables f and a) and partitioned according to the controlled and uncontrolled inputs to give (see Fig. 2):
[~]=(;:: ;~:)[:] where w is described by:
(3)
w = [f T
."t:T
rT}T :>
and f
rcpresents dIe fault signals. The fourdegree of freedom controller operates on tile reference signal r as well as the measurement Z, 811d returns the actuator input and the diagnostic alarm signal, a, i.e.
[a] (CIl U
=
C lI
ec22 J[r] z 12
(4)
The standard system representation is shown in Fig. 2a. Augmenting tlle reference signal and the diagnostic signal to the plant model results in Fig. 2.b. TIlis system has tlle fol1o~ing representation (Tyler and Morari, 1994):
8463
Copyright 1999 IFAC
ISBN: 0 08 043248 4
GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWA T...
o 10
requirement for the stability of the transfer function e12 N .
fi.2j 0
J
0
0
o
0
PJ2
(5)
3.1. StabiJity
The generalised standard system can be used to calculate the local controller, using standard Hoc 1H2 control design technique. The diagnostic state, a, can be used to detect sensors and actuator fuults using a threshold logiC control system. In general, for uncertain plants, the closedloop performance and the diagnostic performance should be traded off against each other (Nett, et al, 1988). For the case of an H2 controller, Tyler and Morari, 1994, have shown that the control and diagnostic controllers can be designed sequentially when the cost function does not lnelude 3ny cross coupling tenn benveen the diagnostic and control states. This implies that an optimal controller is constructed and then an optimal diagnostic controller is calculated. This two step scheme does not, however, give an acceptable diagnostic controller when plant uncertainty is large. The two controllers should then be designed simultaneously. 3.
LOCAL CONTROL DESIGN
[V~ ~] = N
M
[
M N
~]l 51 V
The stability of the closed loop system depends only on C22. Once the nominal stability is established, the stability robustness of the integrated controller is equivalent to a standard feedback system. This fact will be used later to analyse the stability of the guidance controller. 3.2. The Closedloop System The controller may be written in standard statespace form as: xJt) = AexJt) + Bez(t) u(t) = CeXe (t) + Dcz(t) (8) This controller model is combined with the AUV model to find the closedloop system: Xc! (t) == Aa/Xci (t) + Bc/r(t) + Dc/v(t) where _[
X 
(6)
The controller C can be par.1meterised in lenns of the factorisation ofP as:
T
1'J
_(ABD,H BC B H A ' c)
x x. Ad 
c
Bo!
To paramcterise a stabilising controller, a doubly coprime factorisation can be used (Vidyasagar,1985) for the first subsystem in (3). 1 ~ 1G 22 =: NA1 = A1 N where
14th World Congress ofTFAC
c
=(B:C).Dcl =(~ ~:cJ
(9)
The closedloop model is usually of high order. The fast modes are assumed to be regulated by the local
controller and hence can be removed from the closed loop system model. The process noise, of the closedloop system, has the covariance:
l'
Q+ BDc RD[ BT BcRD'{ BBT
BDcRB'[)
BeRB!
where E{ssT}=QandE{~~T}=R Note that the measurement noise and the process noise are also correlated for the closedloop system. 4.
(
QJl  Q12 iJW ~ Qn,V)1 Q21
CV +Q22 N )1 Q21
(7)
where Q is any stable transfer function (see Youla, et aI, 1976).
The design of the local controller is a tradeoff between the plant controller en and a diagnostic controller C ll , considering the controller interoctions C 12 and C21 . A number of limitations and tradeoffs exists which should be dealt with dwing the design stage. These are discussed in details in Nett et ai, 1988 and are summarised here: • Simultaneous sensor fault diagnosis and disturbance rejection requires appropriatc choice of C 12 S, where S is the sensitivity function, • Sensor diagnostic perfonnance and the actuator diagnostic performance must be tradedoff against one another. • The achievable sensor diagnostic performance is limited by the stability requirement far the transfer function CuM. • The achievable actuator diagnostic performance for nOllminimum phase system is limited by the
GUIDANCE CONTROLLER
Vehicle autonomous guidance is traditionally accomplished by manipulating the heading reference to the autopilot (Healey and Lienard, 1993). The controller will then drive the vehicle to the Line Of Sight (LOS) between the present pOSition and the way point to be reached. The LOS is defined by Nett et al, 1993 as:
t/Jr
=tanl[(~ Y(t»)/(X, X(/»)]
(10)
where [Xr Yrj are way points and X and Y are the po!;itions of the vehicle. The criterion to determine that the vehicle has reached the waypoint is based on the square of the position errors: J = (X,  XCt))2 +(I~  YCt)? + (Zr  Z(t))2 (11) If a desired time is requested for each leg of the mission, then tJ1C speed references should also be supplied to the controller. The results presented by Nett et al, 1993 show that the vehicle experiences a large overshoot when the path dimensions arc not large enough compared to the vehicle length. Moreover this operation is not always stable since the vehicle response may be slow
8464
Copyright 1999 IF AC
ISBN: 008 0432484
GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWA T ...
compared to the rate of change of the reference signal. When
le
22 ~
2 = tiT (k+nl) ... i T (k+n2)]
2:: c, where c is the actuator rate limits,
U == [u(kj) .... u(k + nu 1)]
dIe vehicle may become unstable due to rate saturation. Fryxell et al, 1996 and Silvestre et a!, 1995 proposed a technique based on the assumptions that the AUV trajectory is helices and the vehicle has a linear timeinvariant dynamic around any operating point on this trajectory. The problem of the guidance control is then solved using a gain scheduled control system 10 recover the controller performance at each trimming trajectory. Although this approach resolves the problem of overshoot, but it still has the potential of instability when dIe model uncertainty is large. An optimal and integrated guidance and motion control is proposed in this section based predictive control design tecImique, which overcomes the problem of instability and large overshoot. The guidance controller is designed based on fue dosedloop model of the AUV described in previous section. The advantage of using PC is that the actuator rate constraints and a desired limit for the overshoot can be explicitly incorporated into fue control design formulation. For simplicity it is assumed thM the mOeasurements and outputs for PC control design are similar to fuose used by the local controllers. The quantities nj and n2 are termed tb.e minimrnn and maximum output prediction horizons and tlle cost function is defined as: J =
t {~[r
j(t)[y _r]2 (t
+ j)]+
Using the predictor lo stack the variables for nl
f
H= [
4.1. Predictive Control Law
Based on fue PC quadratic cost function, the control algorithms may be derived. Let assume the weighting functions arc diagonal: r = diag{y,} and 1\ == diag{A,}. If the future values of the trajectory r(k+j) is not known, the estimated values may be used. The control input u(t) is assumed to be constant for J > nu. The cost function can be represented in vector form as: J == (W  2)TnW 2)+ UT AU (14) where
W
= fw T (k +n] ) ... w T (k+11 2 )]
=:
[(CAR,)T ... (CA n ,
It
. hn,_l·
where h·· J1
h"l"u
= {c cl ATB d d 0
J">i j
The task is now to minimise the cost function with respect to U. The insertion of the above relationships in the cost function gives:
In practice, fue control signal is constrained to some physical limits Le. IV I < c. When fue input is not constrained, the optimal solution can be found by putting the derivative of J with respect to U to zero to obtain: r(t) '= Kgpc (W(t)  JCt) (15)
where the PC control gain is: Kgpc = (1 0 . .)(J[TrH +A)lrHT
(16)
Since only 'he current control input is applied as fue setpoint to the local controller. 5.
SIMULATION STUDIES
The linearised model for fue steering autopilot design is given by Healey and Lienard, (1993) as follows: mj);(t) + ~r(t) = Yju(t)v(t) + Y2 u(t)r(t) + 13or(t) m3~·(t)+m41;(t) = N\u(t)v(t) + Nzu(t)r(t) + N 3g r (t) I,ir(t) == r(t) The linear mode for u=1.832 mlsec can be calculated as:
xAk+I)= Aclxul +BJ+K((z(k)Cxcl) (13) where K f is the Kalman filter gain. For timeinvariant svstem, Kf can be calculated offline by solving Riccati· equation. For timevarying system, Kr should be ca1culated onlinc.
= Fx; F . h",_l
~A,i(t)U/} (J 2)
where y(.) and u(.) are the controlled outputs and the control inputs offue AUV as described in Section 2. The first term of tlle cost function involves a summation on a prediction horizon from ill to n2. The upper limit in the second term represents the control horizon. The ?c;(t) is the control weighting factor for loop i and can be variable but for simplicity a constant, i.e. ~"I(t) = Ai is used here. Similarly, [l(t) the weighting factor for the output errors, is also taken to be a constant. To derive a jstep predictor, the states are first estimated using a Kalman filter (Fossen, et al, 1985).
14th World Congress ofTFAC
[~;;;l [_~0~~:5 ~ ~~:: ~][~;l+[~·~~~olgr(t) ..
=
lP(t)
0
0
1 t/J(t)
0
An Hz controller is designed based on this linear model. It is assumed that yaw :rnte and yaw angle are measured. The dynamic weightings are selected as: 0.001s+1 s+O.OOI Wsen(s) = O. 1 Cscn(s) 0.00Is+1 s+0.005 The controller was calculated using Matlab Mu Toolbox. The closed loop model of the system was then found and used to design the predictive controller. The tuning parameters used were: Y= 1,/1.= 103 ,n l = l,n2 = 25,nu = 1 For the purpose of this study, it was assumed that all the sates are available. A PID controller was also used to control the Z position.
8465
Copyright 1999 IF AC
ISBN: 008 0432484
GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWAT...
14th World Congress ofIFAC
,
The controllers were then implemcnted on the nOfllinear model (see Lauvdal, 1997 for MATLAB Version) and a sample time of 0.25 s was used to discretised the conrroller. The response of the vehicle to a step change of 30 degrees in yaw angle is shown in Fig. 3 for thc cases where dIe predictive controller is used as the guidance controller and where only the H2 controller is employed to maintain the heading. This comparison demonstrates that the proposed scheme removes overshoot and optimises the trajectory tracking performance. Furtllcnnore, it reduces the rudder activity as indicated by the rudder command response shovm in Fig. 3. The trajectory tracking of the proposed controllcr is shown in Fig. 4 for both Hz and H 2 GPC The yaw angle is changed from 0 to 360" over the time horizon of 250 s. The rudder response is also shown in Fig. 4 for comparison. TIle results shO\v that the improved tracking performance can be achieved with a lower rudder activity. 5.1. The Current Disturbance In gcneral currents are considered to remain constant over long periods, as the density of water is so high that a change of current magnitude and direction requires a large force. Thus it is possible to consider the current disturbances on ROVs as a constant offset oftlle relative velocity between the. sea and the vehicle, i.e. U::;:;;Ur 
Uc
v~v,.vc
The vertical component of the current can usually be neglected. Fig.5 shows the performance of the controller to a eurrent of magnitude 0.5 m/s and angle of 45°. The vehicle response is oscillatory, bllt it settles down to zero.
5.2. The Fault Diagnosis and Recontiguration An example of the fault diagnosis and control reconfiguration for the steering autopilot is demonstrated in Fig. 6. TIle vehicle is initially stabilised around zero degrec yaw angle using tlle measurements from a yaw angle sensor and a yaw rate sensor. The steering system develops a fault at t = 15 seconds and tllc gyro also fails simultaneously. The faults were simulated by adding a constant bias to the rudder command and setting the yaw angle measurement to zero. The reconfiguration controller is activated as soon as tJlc yaw angle sensor fails and the heading error goes outside the ±1°. A new controller is switched in which uses only the yaw rate measurements. The new controller stabilises the system the original operdting point aftcr about 30 seconds.
6.
CONCLUSION
An integrated control system was proposed for the navigation, guidance and control of AUV. Unlike the cOllventional scheme, which may lead to poor stabilily and large ttacki11.g error, the proposed scheme guarantees stability and low tracking error. TIIC controller has the capability to detect faults and can be reconfigured to compensate the effect of any major change in the vehicle dynamics. The Simulation
results presented showed the advantage of the proposed scheme over the conventional ROV controller.
REFERENCES Bartolini, G, G Cannata, G Casalino and A Ferrara, 1995, ' A hierarchical control architecture for the control of undelWater robots'., Proe. Of 3cd IF AC Wmkshop on CAMS, Trondhejm., 6065. Fossen, T I and 0 E Fjcllstad, 1995, 'Robust adaptive control of Underwater Vehicles: A comparitive study', Proc. Of 3r<1 IFAC Workshop on CAMS, Trondheim, 4348. FI)'xell D, P Oliveria, A Paseoal,C Silvestre and I Kaminar, 1995, 'Navigation, guidance and control of AUVs, An ~plication to the MARlUS vehicle', Proc. Of 3 IFAC Workshop on CAMS, Trond.heim. 3542. Katebi, M Rand Y Zhang, 1995, 'Boo control analysis and design for nonIincar systems', 1nt. J Control, Vol. 61, No. 2,459474. Smestad E, 1995, 'Minesniper', Proc. Of 3Td IFAC Workshop on CAMS, Trondheim, 4348. Silvestre, C, A Pascoal, I Kaminer and E Hallberg, 'Trajectory trackning for AUVs: An integrated approach to guidance and control, 13 th IFAC World Congress, San Francisco, 1996. 345351 Cowling, D , , Full range autopilot design for an unmanned undenvater vehicle, 13m IFAC World Congress, San Francisco, 1996., 339345 Robcrts G N and R Sutton, , Artifical Neural Network Control of a UUV in the Terrain Following Mode, 13~1 IFAC World Congress, San Francisco, 1996., 315320 TrIer, L M and M Morari, 'Optimal Control Design of Integrated Control and Diagnostic Modules, ACe, Baltimore, June 1994., pp 20602064 Nett C N , C A Jacobson and A T Miller, ' An integrated Approach to Conrols and Diagnostics: The 4Parameter Controller, ACe, Atlanta, June 1988., 814835 Healey A and D Lienarci, Multivariable Sliding Mode Control for Autonomous Diving and Streeting of Unll1anned Undenvater Vehicles, IEEE 1. Ocean Engineering, Vcl 18 No 3 , 1993, 327338. Lauvdal, T, 1997, http://www.itk.unit.no/SIMULATORIMATLABI. Ranch, H.E., 1995, Autonomous control reconfiguration, IEEE Control Systems Magazine, December 1995, pp. 3748. Valavanis K, C Gracanin, M Matijasevic, R Kolluru and G A Demetriou, Control Architectues for Autonomous Undenvatre Vehicles, IEEE Control Systems, Vol 17 No 6, 1997, pp4864. Youlad D.e., H A Jaber and J J Bongiorno, 1976, Modem WienerHopf design of optimal controllers, IEEE trans. on AC, 21, 319338.
8466
Copyright 1999 IF AC
ISBN: 0 08 043248 4
GUIDANCE CONTROLLER DESIGN FOR AUTONOMOUS UNDERWA T...
14th World Congress ofTFAC
Other data
Navigation System Position and Attitude Predictors
[~]
[q] ~
[;]
~
y
~+l@
Logic Ther;;ho1d
uiBz H", controller Regulation ( C 21
e l2
[:]
[:]
Fig.2a and 2b Generalised system )
Diagnosis
Fig. 1 The AUV proposed control scheme
The effect of current disturbance
10
o
50
200
150
100
250
Times The effect of current dislumance
15r,j':~,,
.x: .
Times
A comparison of H2 and H2GPC Response5 5
~~'!
i
o==::~
'11'/' :
c/'
::Jo
50
~l
10
 ~~===~et"" ~". WO"
==t..",..~
_=1
5
j
0
r
100
150
R"dder A~gl~ (deg)
,
~~= ~t
_
j
 ·t
I ~
5
0
1
7)
50
l~O
100
200
250
Times 200
25
Times.
Fig. 5 The effect of the current
Fig. 3 A comparison ofH2 and H 2 GPC 15~,__,r,
A Cir.c le test at U'" 1.2 mfs
0_
_O~~~~JJ
o
50
150
100
An Example of Sensor
200
25
I~mctuator Failure and
D_fr,r,,,,
o
:~f~  ~\~!~~_~~~1dd,,r.com~rnand.(d~ek) ~=. 4 . 

r:1 \Y 
0_
0
O·~O~501~O~O~~~2~5
r  ..
Time
.6L~~~=~~~~~=~ o 10D 200 300 400 50D 60D 7DO
Fig. 6 The fault diagnosis and controller re configuration
Times
Fig. 4 The trdjectory tracking
8467
Copyright 1999 IF AC
ISBN: 008 0432484