You are on page 1of 9

NAVIGATION, GUIDANCE AND CONTROL OF AUVS: AN

APPLICATION TO THE MARIUS VEHICLE


,1
D. Fryxell P. Oliveira ,1 A. Pascoal ,1
C.Silvestre ,1

I. Kaminer ,2


Institute for Systems and Robotics and Department of Electrical Engineering,
Instituto Superior Tecnico, Av. Rovisco Pais, 1096 Lisboa Codex, Portugal

Department of Aeronautics and Astronautics, Naval Postgraduate School,
Monterey, California 93943, USA

Abstract. This paper addresses the problem of designing guidance, control and
navigation systems for autonomous underwater vehicles (AUVs). Its contribution is
twofold: i) it introduces a new methodology for the integrated design of guidance and
control, and ii) it describes a novel approach to the analysis and design of multi-rate
complementary filters for navigation. The methodologies proposed lead to an efficient
procedure for the design of controllers for AUVs, to accurately track reference trajec-
tories defined in an inertial reference frame. The paper illustrates the application of
that procedure to the design of a tracking controller for the MARIUS AUV. The de-
sign phase is summarized, and the performance of the resulting controller is assessed
in simulations, using dynamic models of the vehicle and its sensor suite.

Keywords. Autonomous vehicles, Tracking systems, Guidance, Navigation,


H-infinity Control.

1. INTRODUCTION. GUIDANCE, CONTROL as line of sight (LOS) for guidance, see (Healey
AND NAVIGATION and Lienard, 1993) and the references therein.
During the design phase, the control system is
In a great number of envisioned mission scenar- usually designed with sufficiently large bandwidth
ios, autonomous underwater vehicles (AUVs) will to track the commands that are expected from
be required to follow inertial reference trajectories the guidance system. However, since the two sys-
accurately (Pascoal, 1994). To achieve that goal, tems are effectively coupled, the stability and ade-
the following systems must be designed and im- quate performance of the combined system about
plemented on board AUVs: i) navigation, to pro- nominal trajectories are not guaranteed. In prac-
vide estimates of linear and angular positions and tice, this problem can be resolved by the judicious
velocities of the vehicle, ii) guidance, to process choice of guidance-law parameters (such as the so-
navigation/inertial reference trajectory data and called visibility distance in the LOS strategy),
output set-points for the vehicles (body) velocity based on extensive computer simulations. Even
and attitude, and iii) control, to generate the ac- when stability is obtained, however, the resulting
tuator signals that are required to drive the actual strategy leads to finite trajectory-tracking errors,
velocity and attitude of the vehicle to the values the magnitude of which depends on the type of
commanded by the guidance scheme. trajectory to be tracked (radius of curvature, ve-
Traditionally, control and guidance systems are hicles desired speed, etc.).
designed separately, using well-established design The first part of the paper proposes a new method-
methods for control, and simple strategies such ology for the design of guidance and control sys-
tems for AUVs, whereby the two systems are ef-
fectively designed simultaneously. The key idea is
1 Work supported by the Commission of the European to realize that for these types of vehicles the equi-
Communities under contract MAS2-CT92-0021 of the Ma-
rine Science and Technology programme (MAST-II).
librium (also known as trimming) trajectories are
tion and velocity estimation, however, the charac-
teristics of the sound channel imply that the po-
sition measurements (obtained from a long base-
line system) are available at a rate that is lower
than that of the remaining sensors. This prob-
lem has been tackled in (Oliveira et al., 1994),
where a new approach to the design and anal-
ysis of multi-rate complementary filters was in-
troduced. Interestingly enough, these filters can
be viewed as input-output operators that exhibit
frequency-like properties, that are the natural
generalization of those obtained for the single-rate
case.
Fig. 1. The vehicle MARIUS
The paper is organized as follows: Section 2 in-
more, using a convenient coordinate transforma- troduces the model of the MARIUS AUV, and
tion, the linearization of the vehicle error dynam- derives its linearized equations of motion about
ics and kinematics about any trimming trajectory trimming trajectories. Section 3 introduces the
can be shown to be time-invariant (Silvestre et al., structure of a gain-scheduled trajectory-tracking
1994). Thus, the problem of designing integrated controller for the vehicle. Section 4 describes the
guidance/control systems to track inertial trajec- multirate navigation system of the AUV. Finally,
tories that consist of the piecewise union of trim- Section 5 assesses the performance of combined
ming trajectories, falls within the scope of gain- navigation, guidance and control in simulation.
scheduled control theory (Kaminer et al., 1995).
Using this approach, the vehicles body-axis ve-
locity, yaw rate, and flight-path angle play the 2. VEHICLE DYNAMICS.
role of scheduling variables that interpolate the
parameters of linear controllers designed for a fi- This section describes the dynamic model of the
nite number of representative trimming trajecto- MARIUS AUV, depicted in Fig. 1. A complete
ries. The results reported in (Kaminer et al., 1995) study of the AUVs dynamics, based on hydrody-
on so-called D-implementation of gain-scheduled namic tank tests with a Planar Motion Mecha-
controllers can then be used to obtain a combined nism (PMM), can be found in (Fryxell and Pas-
guidance/control system such that the properties coal, 1994). In what follows, {I} denotes a univer-
of the linear designs are recovered locally, about sal reference frame, and {B} denotes a body-fixed
each trimming trajectory. This new approach guar- coordinate frame that moves with the AUV. The
antees that the steady-state tracking error about following notation is required:
any trimming trajectory condition is zero. More-
p = [x, y, z]0 - position of the origin of {B}
over, the D-implementation method leads natu-
expressed in {I};
rally to a structure where the only exogenous com-
v = [u, v, w]0 -linear velocity of the origin
mands required are the desired linear inertial po-
of {B} relative to {I}, expressed in {B};
sition and the yaw rate, thus avoiding the need to
= [, , ]0 - vector of Euler angles which
feedforward the trimming conditions for the re-
describe the orientation of frame {B} with
maining state variables. Due to space limitations,
respect to {I};
the methodology used for the design of combined
=[p, q, r]0 - angular velocity of {B} rela-
guidance and control systems for the MARIUS
tive to {I}, expressed in {B};
AUV is only briefly summarized here. For com-
R = R() - rotation matrix from {B} to
plete details, see (Silvestre et al., 1994).
{I};
The second part of the paper describes the design Q = Q() - matrix that relates to and
of the navigation system for the MARIUS AUV, satisfies = Q().
using a conceptually simple framework for filter-
ing that is rooted in the kinematic equations of The symbol := [a,c , a,d , e , r ]0 denotes the
motion of the vehicle. This approach borrows from vector whose entries correspond to deflections of
complementary filtering theory, see (Lin, 1991) for the ailerons (common and differential), elevator,
an introduction to the subject and for interest- and rudder respectively, and the symbol n de-
ing applications to aircraft navigation. The set- notes the rotational rate of the propellers. With
up adopted leads naturally to the design of linear the above notation, the dynamics of the AUV can
Kalman filters, whereby the covariances of pro- be written in compact form as
cess and observation noises are viewed as tuning MRB q
+ CRB (q)
q = (
q, q,
, , n), (1)
knobs to shape the characteristics of the opera-
tors that map measured into estimated variables.
In the case of attitude estimation, all sensors are where denotes the vector of external forces and
0
sampled at the same rate, and the corresponding moments, q = [v0 , 0 ] , and MRB and CRB denote
operators are linear time-invariant. This leads to the rigid body inertia matrix and the matrix of
an interpretation of the filters in the frequency Coriolis and centrifugal terms, respectively. The
domain that is fruitful in analyzing the stabil- vector can be further decomposed as
+ surf (q,
) + visc (q,
) + prop (q,
n), Let Pc (c ) be a trimming trajectory for the vehi-
cle, and define the variables
where rest denotes the (restoring) forces and mo-
ments caused by gravity and buoyancy, and add

vE = v vC
is the added mass term. The term surf captures
E = C
the effects of the deflecting surfaces, visc consists (7)
of the hydrodynamic forces and moments exerted
p = R1 (p pC )
E
on the vehicles body (including the skin friction E = Q ( C ),
1

terms), and prop represents the forces and mo-


ments generated by the main propellers. Using which can be interpreted as the generalized error
equation (1) and the associated kinematic rela- vector between the vehicle state and the trajec-
tionships, the state-space model for the AUV can tory Pc (c ). Let uE = u uC . By noticing that
be written as vC and C are constant along the trimming tra-
jectories, straightforward computations show that
q = F (q, ) + G(q)H(
q,
u)
G = p = R v (3)
qE = F(q E , E ) + G(q E )H(q E , uE )
= Q ,
p E = vE + vC R1 C RvC S( E + C )pE (8)
1 QE ,
where F , G and H are continuously differentiable E = E + C Q1 QC C + Q
functions, v, , p and are state space variables,
and u = [ 0 , n]0 is the vector of control inputs. where
An equilibrium or trimming trajectory of (3) is
F(q E , E ) = F (q E + q C , Q E + C )
defined as a path Pc = [q 0C , p0C , C 0 ]0 such that
G(q E ) = G(q E + q C )
F (q C , C ) + G(q C )H(q C , uC ) = 0 (4) H(q E , u) = H(q E + q C , uE + uC ),

for some constant vector uC . Notice that for sim- and S() is the skew-symmetric matrix defined by
plicity, the equations do not show the explicit de- S() = . It is now possible to prove that the
pendence on time. linearization of (8) about the vector [00121 0051 ]0
From equation (1), it can be concluded that the is time-invariant and can be written in the form
only possible trimming trajectories, Pc , correspond
to helices defined by
qE = AqE (c ) q E + AE (c )E + B(c )uE
p E = vE S( C )pE S(vC )E (9)
0 VTc cos(C ) cos( C t)
E = E S( C )E ,
C = 0 , p C = VTc cos(C ) sin( C t) , (5)
C VTc sin(C ) where the matrices

where C is yaw rate, VTc = ||vC || is the linear Ax = [F(x, y) + G(x)H(x, z)],
body speed, and C is so-called flight path an- x
gle. Thus, the trimming trajectories can be pa-
B = [G(x, y)H(x, z)]
rameterized by the vector c = [VTc , C , C ] z
R3 .
are computed at equilibrium values. Throughout
Given c , the corresponding trimming values for
the rest of the paper, the symbol Gl (c ) denotes
the state variables of (3) can be determined from
the linearized time-invariant system with realiza-
analytical and numerical computations, as follows.
tion (9) determined by the parameter c .
Let G (q)
be the orthogonal complement of G(q)
satisfying G (q)G(
q)
= 0. Multiplying the first
equation of (3) by G (q),
it follows that
3. GUIDANCE/CONTROL SYSTEM
0 = G (q C )F (q C , C )
p C = RC vC (6) Suppose that, associated with each linearized sys-

C = QC C tem Gl (c ), there is a linear time-invariant con-
troller K(c ) that stabilizes and achieves adequate
along a trimming trajectory, thus eliminating the performance for the closed-loop system, as eval-
control input uC . By requiring that a,c = 0 at uated by some performance criterion. Theoreti-
trimming, it can be shown that (6), together with cally, it is then possible to define a gain-scheduled
(5), provides a set of equations that can be solved controller C that selects the appropriate linear
numerically to give v, , , as functions of VTc , controller K(), based on the measured value
of the parameter c .
C , C , thus concluding the computation of all
relevant state variables at trimming. For complete In practice, C is obtained by designing a family
details, the reader is referred to (Silvestre et al., of linear controllers for a finite number of sys-
linearized plants in the regimes where the vehi- The outputs of W1 , W2 , and W3 constitute the
cle is expected to operate. During real-time op- vector z. Since zero steady-state error in track-
eration, the controller parameters are updated as ing the step command for all variables in x1 was
functions of the scheduling variable required, the weighting function W1 was chosen
as a diagonal of integrators. The integrator gains
]0
= [VT , , were adjusted to get the desired command re-
sponse bandwidths. The weights W3 , W4 do not
include any dynamics. In order to drive a,c to
zero in the steady state, an integrator was in-
3.1 Linear Controller Design cluded in W2 .The signal y includes all the states
of the plant Gl (c ), together with the appended
The methodology selected for linear control sys- integrator states that correspond to integrators.
tem design was H (Doyle et al., 1989). This
method rests on a firm theoretical basis, and leads Given a design model, suppose that the feedback
naturally to an interpretation of control design system is well posed and let Tzw denote the closed-
specifications in the frequency domain. Further- loop transfer matrix from w to z. The H syn-
more, it provides clear guidelines for the design of thesis problem consists of finding, among all the
controllers so as to achieve robust performance in controllers that yield a stable closed-loop system,
the presence of plant uncertainty. a sub-optimal controller K(c ) that makes the
maximum energy amplification of the closed-loop
J (c ) operator Tzw , denoted ||Tzw || , arbitrarily close
w1 to its infimum. This problem was solved using the
6 methodology explained in (Doyle et al., 1989), see
x- ?e (Silvestre et al., 1994).
+k - W1 -6
1
w

w u
? 2 - W4 - - W2 - z
Gl (c )
x2
3.2 Non-linear Controller Implementation
- W3 -
?
- A set of controllers was determined for a finite
- I combination of values of VT , and , and their
- s parameters interpolated according to the schedul-
u y ing vector in a given bounded domain, see (Sil-
vestre et al., 1994). The resulting non-linear gain-
scheduled controller was implemented by extend-
K(c )
ing the D-methodology described in (Kaminer et
al., 1995), which guarantees the following funda-
Fig. 2. Synthesis model. mental linearization property: the linearization of
the nonlinear feedback control system about each
The first step in the controller-design procedure is equilibrium trajectory preserves the internal as
the development of a synthesis model which can well as the input-output properties of the corre-
serve as an interface between the designer and the sponding linear closed-loop designs.
H controller synthesis algorithm. Consider the Surprisingly, this property is often not satisfied
feedback system shown in Figure 2, where Gl (c ) by the gain-scheduled controllers proposed in the
is obtained from the linearized model of the AUV, literature, see (Kaminer et al., 1995) and the ref-
and K(c ) is the controller to be designed. The erences therein. In practice, violation of that prop-
correspondence between the standard notation of erty may lead to a degradation in performance, or
Fig. 2 (Doyle et al., 1992) and that introduced in even instability, of the closed-loop system.
Section 2 for incremental variables will be clear
from the context. The block J (c ) within the The D-methodology is based on the key obser-
dashed line is the synthesis model, which is de- vation that linear controllers are designed to op-
rived from the linearized model of the plant by erate on the perturbations of the plants inputs
appending the depicted weights. In practice, the and outputs about the equilibrium points. Proper
weights serve as tuning knobs which the de- blending of the different controllers requires that
signer can adjust to meet the desired performance they have access to such perturbations, locally.
specifications. This is achieved by differentiating some of the
measured outputs before they are fed back to the
The signal w1 corresponds to the vector of input gain-scheduled controller. In order to preserve the
commands that must be tracked. In this design ex- input-output behaviour of the feedback system,
ample, it includes linear positions. The signal w2 integral action is provided at the input to the
represents the noise inputs to each of the sensors, plant.
and disturbance inputs to the states of the plant.
The signal u corresponds to the control inputs The gain-scheduled controller implementation is
to the system. The signal x1 represents the com- depicted in Fig. 3, where K denotes the interpo-
ponents of the state vector that must track the lation of the linear controllers obtained in Section
The simplified design model is depicted in Fig.
q - -
d - 4, where denotes the time derivative of , and
pC - dt
R m is the measured value of .
The measurements
p- ?

i
- R1 - K() - (.) -
m and m are corrupted by observation noise
C - Q1 -
i
- and process noise respectively, where the latter
! 6 is obtained by driving an integrator with a noise
source . Clearly, this simple model includes the
Fig. 3. Tracking controller implementation case where the measurement m of exhibits an
unknown bias term that must be rejected.
which are easily available from the trajectory gen-
erator. .m
It is important to stress that the D-method pre-
sented above requires differentiation of some of 1 - ?
i
?
1 - i- m
the plants measured outputs. Except for the case
s s
where some of the derivatives are available from -
dedicated sensors, this cannot be done in prac- m .

tice. In this case, the differentiation operator may 1 - ? 1. -


m - i k1 i
simply be replaced by a causal system with trans- s s
6 6
fer function s s+ 1 , or by a simple finite differ-
ence operator for discrete-time implementation, k2
.
see (Kaminer et al., 1995). It is also important
to remark that the D-methodology would require Fig. 4. Complementary Filter - Design model and
that the time-derivative of E be computed on- Implementation
line. However, from the relations q E = q and
E = Q1 C , it follows that the derivative The design model admits the description
is simply computed as depicted in Fig. 3. Thus,
the method proposed avoids the need to feedfor- x = F x + Gu u + G ,
ward trimming conditions for the state variables
z = Hx + ,
and inputs, except pC and C .
where x = [, ]0 is the state vector, u = m and
z = m are input and output variables, respec-
4. NAVIGATION SYSTEM DESIGN tively, and F, Gu , G and H are matrices of com-
patible dimensions. Following the standard ap-
This section describes the basic framework used in proach in the design of Kalman filters, it is as-
the design of the navigation system for the MAR- sumed that and are zero-mean, Gaussian, un-
IUS AUV. The objective of this system is to ob- correlated stochastic processes with covariances
tain accurate estimates of the position and atti-
tude of the vehicle, based on measurements avail- E[(t)( )] = Q(t ), E[(t)( )] = R(t ),
able from a motion sensor package installed on
board. The estimates are input to the integrated where (.) is the Kronecker delta operator. Under
control and guidance systems described in Section some generic technical assumptions, the station-
3. ary filter that minimizes the mean-square error
estimation of x based on the observations z, is
This paper adopts a conceptually simple frame- asymptotically stable, and given by the Kalman
work for filtering, that is rooted in the kinematic filter structure
equations of (3) . This approach is based on the
theory of complementary filtering, see (Lin, 1991) = F x
x + Gu u + K(z H x
),
for an introduction to the subject and (Oliveira et
al., 1994) for a complete study of the multi-rate denotes the best estimate of x, and
where x
case.
K = [k1 , k2 ]0 = P H T R1

4.1 Attitude Estimation is obtained from the positive semidefinite solution


P to the algebraic Riccati equation
The motion sensor package of MARIUS includes F P P F T P H T R1 HP + G QGT = 0.
two pendulums and one fluxgate that provide - in-
directly - measurements m of , and three rate The resulting filter is depicted in Fig. 4. Let T,d

gyroscopes that provide measurements m of an-
gular body rates . and T denote the operators from m to and
,i

For the sake of completeness, the synthesis of a from the integral of m to ,


respectively. Straight-
continuous-time filter to estimate pitch on the ba- forward computations show that the correspond-
sis of measurements of pitch and pitch rate is ing - stable - transfer functions are given by
briefly described below. This illustrative example
and that high frequency, the estimator relies essentially on
the integral of the measured pitch rate. The gen-
T,d
(s) + T,i
(s) = I. (10) eral case (where corrected estimates of roll, pitch
and yaw angles and body rates are sought) can
Equation (10) admits a simple interpretation: in be dealt with by an obvious generalization of the
above procedure (Oliveira et al., 1994).
order to compute estimates of angular position ,
the filter complements the information m avail-
(1) (2)
able directly from the pendulum at low frequency
with that obtained by integrating the information
from the rate gyro at high-frequency. Thus, the fil-
ter is convenient to use when high-frequency sen-

Gain [dB]
sor data of good quality are available. Notice also
that with the above filter structure, any bias in
the rate information will be rejected at the out-
put.
From a purely formal point of view, if the vari-
0.001 0.01 0.1 1 10 100 1000
able m in Fig. 4 is redefined as the integral of [rad/s]
the input u = m , then the overall filter defines
an input-output operator from u to [, ]
0 that Fig. 5. Discrete Bode diagrams corresponding to
I 0
is equal to [I, s ] , thus capturing the underlying the operators: (1) - T,d
, (2) - T,i
F.
physical constraint between angular velocity and
position. This seemingly trivial property plays an
important role in the stability analysis of the com-
bined controller and navigation systems, since in 4.2 Position/Velocity Estimation
theory no extra dynamics are introduced in the
overall loop by the complementary filter. In prac- The following sensor units are used to provide
tice, high-frequency dynamics are bound to be in- measurements of the linear position and velocity
troduced in that loop, as one must use a high- of the vehicle: a long baseline positioning system
bandwidth, low-pass filter to process the informa- (LBL) and a depth cell that provide measurements
tion obtained from the rate gyro. However, well- pm of p, and a Doppler sonar that provides mea-
known results from robust stability theory indi- surements of the velocity of the vehicle with re-
cate that stability will not be compromised if one spect to the water (i.e., of v R1 vw , where vw
restricts the bandwidth of the control loop to be is the inertial sea current velocity).
well below that of the corresponding complemen- Conceptually, the basic framework described in
tary filter, see (Doyle et al., 1992) for the general Section 4.1 could be used to design a filter that
theory and (Oliveira et al., 1994) for an applica- would provide corrected estimates of the position
tion to the vehicle under study. and velocity of the vehicle with respect to the wa-
The following requirements were specified in the ter and to the seabed. In fact, the time-derivative
design example reported here: of p can be estimated from v using the kinematic
equations. A time-invariant, multivariable com-
i) pitch and pitch rate estimation errors should be plementary filter could then be obtained, adopting
driven asymptotically to zero when the vehicle is a design model similar to that in Fig. 4.
following a trimming trajectory (in particular, the
filter should reject constant rate gyro bias terms); Notice, however, that due to the characteristics
of the acoustic channel, the measurements from
ii) the filter bandwidth corresponding to the trans- the LBL system are available at a rate that is
fer function from pitch measurement to the cor- much smaller than that of the remaining sensors.
responding estimate should be of the order of 0.3 Thus, the resulting filter must exhibit a multi-
rad/s (this requirement is dictated by the low- rate structure. This problem has been tackled and
pass sensor characteristics); solved in (Oliveira et al., 1994) exploring the re-
iii) the overall bandwidth of the filter after inser- lationship between multi-rate and periodic sys-
tion of a low-pass system at the output of the rate tems, and using some algebraic and analytical re-
gyro should be of the order of 30 rad/s, that is, sults on the equivalence between (discrete-time)
much larger than that of the corresponding con- periodic and time-invariant systems (Bittanti et
trol loops. al., 1990; Khargonekar et al., 1985; Souza, 1991).
Using the formalism given above in a discrete-time The set-up adopted here is best explained by con-
setting, the covariances Q and R were manipu- sidering the simplified case where only naviga-
lated to shape the transfer functions between the tional data along the inertial x axis are sought.
variables z and u and the estimates x of x. The The corresponding filter design model is depicted
resulting complementary filter, sampled at 50 Hz, in Fig. 6, where x denotes the time derivative of x
exhibits the Bode diagram of Fig. 5. A low-pass and x m is the measured value of x that is derived
filter F with a bandwidth of 30 rad/s has been from the Doppler log output. The measurements
inserted at the output of the rate gyro. Notice xm and x m are corrupted by observation noise
noise source . The state variable captures the at high frequency. Furthermore, the filter rejects
evolution of the water current in the x direction. any possible biases caused by the non-zero veloc-
ity of the water with respect to the seabed.
x. m
It is important to point out that numerically effi-
cient methods to solve the periodic Ricatti equa-
1 - 1 - ?
j
?
1 - j- xm tion (14) are available. A good reference is the

s s x s x work of (Bittanti et al., 1990), which explores the
equivalence between the class of periodic systems
Fig. 6. Complementary Filter - Design model and a sub-class of invariant systems using a cer-
tain lift operator. The reader will find in (Oliveira
Suppose that measurements xm and x m are avail- et al., 1994) the application of this circle of ideas
able at rates M1T and T1 respectively, where T1 is to the design of a multi-rate filter for the MAR-
the fastest sampling rate and M is an integer. By IUS AUV, based on information provided by the
discretizing the above design model at the sam- LBL system and the Doppler sonar.
pling frequency of T1 , an M-periodic, discrete-time

design model is obtained which is described by 3 x m
1/T -x
T 2 /2 T
x(k + 1) = F (k)x(k) + Gu (k)u(k) + G (k)(k) x-
m c - c. z1 - c. z1 @R?
@
- c - c z1 -x
k1 (k). T. T

z(k) = H(k)x(k) + (k), (11) 6 6


6
6
k2 (k)
0
where x = [, , x] is the state vector, u = x m
k3 (k)
and z = xm are input and output variables, and
F , Gu and G are matrices of compatible dimen-
sions. The matrix H(k) consists of a string of zeros Fig. 7. Multi-rate Complementary Filter
and ones, the pattern of which mirrors the posi-
tion interrogation strategy. It is easy to check that In the case where navigational data along the x,
H(k +M ) = H(k). Assume that the state and ob- y and z axes are required, a simple extension of
servations are corrupted by zero-mean, Gaussian, the above design procedure leads to the general
white-noise processes with covariance matrices navigation system of Fig. 8, where M consists of
three multi-rate complementary filters with the
structure shown in Fig. 7. For simplicity of pre-
E[(k)(j)T ] = Q(k)((k j)modM ) sentation, it was assumed that vw = 0.
E[(k)(j)T ] = R(k)((k j)modM ), (12) Interestingly enough, the multi-rate filters descri-
where (.) is the Kronecker delta operator, Q(k) bed here exhibit properties that are the general-
0 and R(k) > 0. ization of those obtained for the single-rate case,
as explained briefly below.
Associated with the periodic design model, con-
sider the Kalman filter structure described by
vm .... aa [k] [k]
v
....
R(.) - a R1 (.). -
1/T c (velocity
(k + 1) = F (k)
x x(k) + Gu (k)u(k) M estimate)
pm .
ba .... [M k] [k]
p
+K(k) [z(k) H(k)
x(k)] , (13) ...
.... . - a -
F 1/T 1/M T d (position
where the Kalman gain K(k) is given by estimate)

M - multi-rate filter; [k] - time mark;


1
K(k) = P (k)H T (k) H(k)P (k)H T (k) + R(k) , M - integer; F - fictitious sampler.

and the matrix P (k) is the unique periodic, sym- Fig. 8. Multi-rate Navigation System.
metric, positive-semidefinite stabilizing solution to
the periodic Riccati equation Let G be the time-varying operator from a to c
that is obtained by forcing b to be equal to the
discrete-time integral of a. It has been checked
P (k + 1) = F (k)P (k)F T (k) + Gu (k)Q(k)GTu (k) computationally in (Oliveira et al., 1994) that,

F (k)P (k)H T (k) H(k)P (k)H T (k) for certain combinations of sampling frequencies,
1 it is possible to select the process and noise co-
+R(k) ] H(k)P (k)F T (k). (14) variances in (12) such that the operator can be
The technical conditions under which such a peri- written in the form G = I + , where the in-
odic solution exists can be found in (Souza, 1991). duced norm (Doyle et al., 1992) of the operator
The resulting multi-rate complementary filter is is small. Mathematically, this means that G is
depicted in Fig. 7. The filter complements the in- close to the desired identity operator. As in the
formation obtained from the LBL system at low time-invariant case, this property plays a key role
frequency, and that obtained from the Doppler log in analyzing the stability of combined navigation,
guidance and control systems, and suggests im-
portant rules of thumb for the choice of the co-
the area of robust stability of time-varying sys- The desired and observed trajectories are depicted
tems. The algorithms for computing the induced in Fig. 9. The activity of some relevant state vari-
norm of are described in (Oliveira et al., 1994). ables are condensed in Fig. 10. In this simulation,
the LBL system uses four transponders located in
positions {40, 0, 160}, {130, 0, 150}, {40, 190,
170} and {140, 190, 135}, specified in meters. At
5. INTEGRATED NAVIGATION, GUIDANCE the beginning of the maneuver, the actuation vari-
AND CONTROL. ables are essentially constant during the first 25s.
Upon entering the circular path, the rudder de-
The combined performance of guidance, naviga- flects to create a torque that will impart the de-
tion and control was evaluated in simulation with sired rotational speed to the vehicle. Once the de-
a nonlinear model of the vehicle. The simulation sired speed is reached, the rudder deflects slightly
included physically based models of the sensor in the opposite direction to stabilize the rotation.
units described in Section 4. In the simulations, This maneuver is characteristic of vehicles that
the control and navigation systems were discre- are unstable in yaw.
tized using the following sampling rates:
Navigation (Attitude and attitude rate): 50 30

Hz, that is, larger than twice the desired 25


T/10
bandwidth of the corresponding complemen- 20
tary filters. 15
Navigation (Linear position and velocity) - 10
[deg],[N]
a,c
LBL system: 0.2 Hz, Doppler sonar: 1 Hz. 5
a,d
These frequencies are mission dependent, and 0
reflect the compromise among such factors as -5
range of operation required, precision sought -10
and (acoustic) energy minimization. -15
Integrated Control and Guidance - 10 Hz, 0 25 50 75 100 125 150 175 200
that is, much larger than the desired band- Time [s]
width of the integrated control and guidance 20
e
system. 15

10
-20
r
0 5
[deg]

Beginning
20 0
40
-5
60
y [m]

80 -10

100 -15
120 0 25 50 75 100 125 150 175 200

140 Time [s]


End
160
Fig. 10. Control activity: Rudder (r ), Ailerons
x [m] (a,c and a,d ), Elevator (e ) and Thruster
-5
Beginning
(T ).
0

10 At the middle of the first turn, the vehicle shows


a pronounced rotation in pitch in order to con-
z [m]

15

20 verge rapidly to the desired vertical inclination


25
of 10 deg. This rotation is achieved by deflect-
ing the common aileron a,c and the elevator e
30
End in opposite directions, so as to generate a pure
35
-40 -20 0 20 40 60 80 100
torque. When the vehicle reaches the desired ori-
x [m] entation, a,c and e decrease. However, their val-
ues do not tend to zero, since they must coun-
Fig. 9. Reference and observed trajectory - hori- teract the restoring torque due to the combined
zontal and vertical planes. effects of buoyancy and gravity.

The reference for linear position in the xy plane When the vehicle reaches the end of the first turn,
is an S - shaped trajectory consisting of three there is a strong deflection in the rudder to drive
straight lines, each 50m long, and two semicir- the velocity of rotation to zero. Similar comments
cumferences with radii of 38m. The reference tra- apply to the remaining part of the trajectory.
jectory in the vertical plane descends smoothly It is important to remark that the thrust activity
along the depth coordinate z with a slope of 10 rises during maneuvers that require large deflec-
deg. In order to simplify the interpretation of the tion of the control surfaces. This is required to
6. REFERENCES Khargonekar, P., Poola, K. and Tannenbaum,
A. (1985). Robust Control of Linear Time-
Bittanti, P., Colaneri,P. and Nicolau, G. (1990). Invariant Plants using Periodic Compensa-
An Algebraic Riccati Equation for the Peri- tion. IEEE Transactions on Automatic Con-
odic Prediction Problem. Systems and Con- trol. Vol. AC-30, pp.10881096.
trol Letters. Vol. 14, pp. 7178. Lin, C. (1991). Modern Navigation, Guidance,
Doyle, J., Glover, K., Khargonekar, P. and Fran- and Control Processing. Prentice-Hall.
cis, B. (1989). State space solutions to stan- Oliveira, P., Pascoal, A. and Silvestre, C. (1994).
dard H2 and H control problems. IEEE Guidelines for the Design of Advanced Nav-
Transactions on Automatic Control. Vol. igation Systems for AUVs. SOUV-Technical
AC- 34(8), pp. 831847. Report. Institute for Systems and Robotics.
Doyle, J., Francis, B. and Tannenbaum, A. (1992). Pascoal, A. (1994).The AUV MARIUS: Mission
Feedback Control Theory. MacMillan Pub- Scenarios, Vehicle Design, Construction and
lishing Company. Testing. Proceedings of the 2nd Workshop
Fryxell, D. and Pascoal, A. (1994). Modeling, on Mobile Robots for Subsea Environments.
Identification and Control of the AUV MAR- Monterey Bay Aquarium, Monterey, Califor-
IUS. SOUV-Technical Report. Institute for nia, USA.
Systems and Robotics. Silvestre, C., Pascoal, A. and Kaminer, I. (1994).
Healey, A. and Lienard, D. (1993). Multivariable Design of Integrated Guidance and Control
sliding mode control for autonomous diving Systems for AUVs. SOUV-Technical Report.
and steering of unmanned underwater vehi- Institute for Systems and Robotics.
cles. IEEE Journal of Oceanic Engineering. Souza, C. (1991). Periodic strong solution for the
Vol. 18., pp. 327-339. optimal filtering problem of linear discrete-
Kaminer, I., Pascoal, A., Khargonekar, P. and time periodic systems. IEEE Transactions on
Coleman, E. (1995). A Velocity Algorithm for Automatic Control. Vol. AC-36(3), pp. 333
the Implementation of Gain-Scheduled Con- 337.
trollers. Automatica. Vol. 31, pp. 11851191.