0 Formation Guidance of AUVs Using Decentralized Control Functions

Autonomous Underwater Vehicles (AUVs) are the most complex type of unattended marine systems, being mobile, with challenging dynamics and non-holonomic kinematics. They are increasingly being recognized as a keystone technology for projecting human scientific and economical interests into the deep Ocean (Papoulias et al., 1989). A recent report by Bildberg (2009) delivers the verdict of several key researchers that the AUVs are rapidly moving towards maturity. The autonomy of AUVs is their key capability. They autonomously explore Ocean phenomena relevant to human scientific and economic interests. Well engineered autonomous control allows them to act robustly and predictably with regards to waves, currents, wind, sea-state and numerous other disturbances and operational conditions in nature. As a consequence, they are today being cast in the leading role in projecting human presence and human interests in the Ocean, in an increasingly diverse gamut of topics:


Introduction
Autonomous Underwater Vehicles (AUVs) are the most complex type of unattended marine systems, being mobile, with challenging dynamics and non-holonomic kinematics.They are increasingly being recognized as a keystone technology for projecting human scientific and economical interests into the deep Ocean (Papoulias et al., 1989).A recent report by Bildberg (2009) delivers the verdict of several key researchers that the AUVs are rapidly moving towards maturity.The autonomy of AUVs is their key capability.They autonomously explore Ocean phenomena relevant to human scientific and economic interests.Well engineered autonomous control allows them to act robustly and predictably with regards to waves, currents, wind, sea-state and numerous other disturbances and operational conditions in nature.As a consequence, they are today being cast in the leading role in projecting human presence and human interests in the Ocean, in an increasingly diverse gamut of topics: • Physical oceanography (Plueddemann et al., 2008;Tuohy, 1994), • Marine biology, conservationist biology, marine ecology management, biological oceanography (Farrell et al., 2005;Pang, 2006;Pang et al., 2003), • Geology, petrology, seismology, hydrography (for the benefit of e.g. the oil and gas industry, maritime civil engineering etc.), • Maritime and naval archaeology, submerged cultural heritage protection and management, • Marine traffic management, search and rescue, hazardous material and waste management, emergencies and catastrophes management and first responding (Carder et al., 2001;Pang, 2006) • Maritime security, customs enforcement, border protection and defense (Allen et al., 1997;2004;Clegg & Peterson, 2003;Curtin et al., 1993;Eisman, 2003;US Navy, 2004).
To increase the effectiveness, safety, availability, economics and applicability of AUVs to these and other topics of interest, this chapter proposes a decentralized cooperative cross-layer formation-control paradigm for entire groups of AUVs collaborating in exploration tasks.The AUVs are assumed to navigate on a common "flight ceiling" by using robust altitude controllers, based on altimeter echosounder measurements.The proposed virtual potential framework allows for the 2D organization of individual trajectories on such a "flight ceiling".
The goal is to provide decentralized consensus-building resulting in synoptical situational awareness of, and coordinated manoeuvring in the navigated waterspace.The paradigm is formally developed and tested in a hardware-in-the-loop simulation (HILS) setting, utilizing a full-state hydrodynamical rigid-body dynamic model of a large, sea-capable, long-endurance Ocean-going vehicle.Existence of realistic, technically feasible sensors measuring proxy variables or directly the individual kinematic or dynamic states is also simulated, as is the presence of realistic, non-stationary plant and measurement noise.

The cooperative paradigm
Since 1970s, robotics and control engineers have studied the cooperative paradigm.
Cooperative control is a set of complete, halting algorithms and machine-realized strategies allowing multiple individual agents to complete a given task in a certain optimal way.This optimality results from the agents' leveraging each other's resources (e.g.manoeuvring abilities) to more effectively minimize some cost function that measures a "budget" of the entire task, in comparison to what each agent would would be capable of on their own (without the benefit of the group).
In the marine environment, such "social" leveraging is beneficial in several ways.Firstly, deployment of more AUVs significantly reduces the time needed to survey a given theater of operations.This has enormous economic repercussion in terms of conserved hours or days of usually prohibitively expensive ship-time (for the vessel that is rendering operational support to the AUV fleet).Secondly, deployment of a larger number of AUVs diversifies the risk to operations.In a group scenario, loss of a (small) number of AUVs doesn't necessarily preclude the achievement of mission goals.Lastly, if each of the group AUVs are furnished with adaptive-sampling algorithms, such as in the chemical plume-tracing applications (Farrell et al., 2005;Pang, 2006;Pang et al., 2003), deployment of multiple vehicles guarantees much faster convergence to the points of interest.
Cooperative control frameworks are split into centralized and decentralized strategies.A centralized cooperative control system's task is to determine the actions of each agent based on a perfectly (or as near perfectly as possible) known full data-set of the problem, which consists of the state vectors of every agent for which the problem is stated.The centralized system instantiates a globally optimal solution based on the assessment of momentary resource-disposition of the entire ensemble, as well as based on the total, if possibly non-ideal knowledge of the environment.The state data are usually collected by polling all agents through a communication network.After the polling cycle, the centralized system communicates the low-level guidance commands back to individual agents.This approach allows for the emergence of a global optimum in decision-making on grounds of all obtainable information, but heavily depends on fault-intolerant, quality-assured, high-bandwidth communication.
In a decentralized approach, such as we have chosen to present in this chapter, each agent possesses imperfect state and perception data of every other agent and of the observable portion of the environment, and locally decides its own course of action.The greatest issue in decentralized cooperative control is the achievement of a consensus between separately reasoning autonomous agents.

The virtual potentials framework
To address the issue of reactive formation guidance of a number of AUVs navigating in a waterspace, a method based on virtual or artificial potentials is hereby proposed.The virtual potentials alleviate some of the most distinct problems encountered by competing reactive formation guidance strategies, which are prone to the following problems: • Reliance on the perfect knowledge of a map of the waterspace, • Lack of reaction to the decentralized, agent-local process of accumulating or perfecting knowledge of the environment on top of the initially imperfect situational awareness of each individual agent, • Trajectory planning that is sub-optimal, or optimal based on a hard-coded criterion, without possibility of adjusting or restating that criterion at run-time, because the cost function is implicit in the choice of mathematical tools (such as a distinct set of curve formulations used for trajectories etc.).
Stemming from these considerations, we propose a scheme where each AUV in a 2D formation imbedded in the "flight ceiling" plane as previously discussed maintains a local imperfect map of the environment.Every possible map only ever consists of a finite number of instantiations of any of the three types of features: 1.A way-point that is commanded for the entire formation, w ∈ R 2   2. Obstacles which need to be circumnavigated in a safe and efficient manner, 3. Vertices of the characteristic cell of the chosen formation geometry, covered in more detail in sec.2.3.3 and 3.
With this in mind, let the virtual potential be a real, single valued function P : R 2 → R, mapping almost every attainable position of an AUV on the "flight ceiling" to a real.Let P-s total differential exists almost wherever the function itself is defined.P can be said to live on the subspace of the full-rank state-space of the AUVs, C = R 6 × SE 3 .The state-space of the AUV is composed of the Euclidean 6-space R 6 spanned by the angular and linear velocities, { v T ω T T } = { u v w | p q r T } ≡ R 6 and a full 3D, 6DOF configuration-space T } which possesses the topology of the Special Euclidean group of rank 3, SE 3 .Function P therefore maps to a real scalar field over that same C. Furthermore, this framework will be restricted to only those P that can be expressed in terms of a sum of finitely many terms: Where P i is of one of a small variety of considered function forms.Precisely, we restrict our attention to three function forms with each one characteristic of each of the three mentioned types of features (way-point, obstacle, vertices of formation cells).The critical issue in the guidance problem at hand is Euclidean 2D distance (within the "flight ceiling") between pairs of AUVs in the formation, and each AUV and all obstacles.Therefore, our attention is further restricted to only such {P i } ⊂ L(C → R) with L being the space of all functions mapping C to R whose total differential exists almost wherever each of the functions is defined on C, which can be represented as the composition P i ≡ p i • d i , p i : R + 0 → R, and d i : C → R + 0 a Euclidean 2D metric across the "flight ceiling".Consequently, P i is completely defined by the choice of p i (d), the isotropic potential contour generator.Choices and design of p i (d)-s will be discussed in sec.2.3.With all of the above stated, a decentralized total control function f : Z → R 2 is then defined as a sampling, repeated at sample times k ∈ Z 0 , of the 2D vector field E : Where contains all of R 2 to the exclusion of closed connected subsets of R 2 that represent interiors of obstacles, {O i } and those that represent safety areas around all the j-th AUVs (j = i) other than the i-th one considered.W i is an open, connected subset of R 2 , the "flight ceiling", inheriting its Euclidean-metric-generated topology and always containing the way-point w.Sampling E at the specific x i (k) ∈ W i , the location of the i-th AUV, results in f i (k), the total decentralized control function for the i-th AUV at time k and location x i .

Passivity
The decentralized total control function f is used as the forcing signal of an idealized dimensionless charged particle of unit mass, modeled by a holonomic 2D double integrator.If any AUV were able to behave in this manner, the AUV would follow an ideal conservative trajectory given by: This ideal conservative trajectory, while stable in the BIBO sense, is in general not asymptotically stable, nor convergent by construction.The simplest case when this doesn't hold is when E(x) is an irrotational 1 vector field whose norm is affine in the x − w 2D Euclidean distance: And whose direction is always towards w: In that case (3) can be regarded as a linear second or third order system with two of the poles in ±i.Such a system exhibits borderline-stable oscillation -a hallmark of its conservativeness.
An example of such BIBO-stable non-convergent oscillation is given in figure 1.
Note that this analysis is irrespective of the initial condition ẋ0 as long as (4, 5) approximate E(x) sufficiently well in some open ε-ball centered on w.However, AUVs are in general not able to actuate as ideal holonomic 2D double integrators.The introduction of any finite non-zero lag in the above discussion, which is sure to exist from first physical principles in a real AUV, is sufficient to cause dissipation and as a consequence passivity and convergence to w.

Local minima
In addition to the problem of passivity, the virtual potential approach suffers from the existence of local minima.i, and the enumeration of the distinct points of convergence other than the way-point by j omitted for clarity) that do not converge to the way-point w or a finitely large orbit around it, but rather to another point x (j) ∞ (or a finitely large orbit around it).Therefore, for each of these uncountably many "nearby" trajectories (to be visualized as a "sheaf" of trajectories emanating from a distinct, well defined neigbourhood in W i for some range of initial linear and angular velocities) there exists a lower bound t l after which x i (t ) − w almost always.The set {t l } is also dense and connected.
Furthermore, there is no prejudice as to the number of such C (j) i 0 -s, i.e. there exists There may be multiple disjoint dense, connected, closed sets of initial conditions of the trajectory of the i-th AUV which all terminate in the same, or distinct local minima.The enumerator j may even come from R (i.e.there may be uncountably many distinct local minima, perhaps arranged in dense, connected sets -like curves or areas in R 2 ).An example of an occurrence of a local minimum is depicted in figure 2. In order to resolve local minima, an intervention is required that will ensure that either one of the following conditions is fulfilled: 1.The set C Σ i 0 is empty by construction.2. A halting P-complete algorithm is introduced that for every x 0 ∈ C Σ i 0 , triggering at t 0 with ε(t 0 ) = sup x(t > t 0 ) − x ∞ | x 0 characterizing a ε-ball centered on the particular x ∞ and containing all x(t > t 0 ), to intervene in E(W i ) guaranteeing that this entire ball is outside (a possibly existing) new C Σ i 0 (with x 0 ← x(t 0 )).Out of the two listed strategies for dealing with local minima, the authors have published extensively on strategy 2 (Barisic et al., 2007a), (Barisic et al., 2007b).However, strategy 1 represents a much more robust and general approach.A method guaranteeing C Σ i 0 id = ∅ by designing in rotors will be described in sec.2.4.

Potential contour generators and decentralized control functions
As for the potential contour generators p i (d) : R + 0 → R, their definition follows from the global goals of guidance for the formation of AUVs.Bearing those in mind, the potential contour generators of each feature type, p (o,w,c) (for obstacle, way-point and formation cell vertex, accordingly) are specified below.

Obstacles
Fig. 3.The potential contour generator of an obstacle, p o (d(x)). Where: -p o (d) : R + → R is the potential contour generator of obstacles, a strictly monotonously decreasing smooth single-valued Lebesgue-integrable function mapping a non-negative real to a real, -A + ∈ R + \ {0} is a positive real independent parameter dictating the scale of the acceleration away from the obstacle.

Way-points
Where: -p w (d) : R + → R is the potential contour generator of way-points, a strictly monotonously increasing smooth single-valued Lebesgue-integrable function mapping a non-negative real to a real, -A − p ∈ R + \ {0} is a positive real independent parameter dictating the scale of acceleration towards the way-point in the area of proportional attraction, -A − 0 ∈ R + \ {0} is a positive real independent parameter dictating the constant acceleration towards the way-point outside the area of proportional attraction, -d p ∈ R + \ {0} is a positive real independent parameter dictating the radius of the open ball centered on the way-point that constitutes the area of proportional acceleration.105 Formation Guidance of AUVs Using Decentralized Control Functions

Cell vertices
A good candidate potential contour generator of formation cell vertices, which behaves similar to a function with local support, is the normal distribution curve, adjusted for attractiveness (i.e. of inverted sign). Where: -p c (d) : R + → R is the potential contour generator of cell vertices of characteristic cells of a formation, a strictly monotonously increasing smooth single-valued Lebesgue-integrable function mapping a non-negative real to a real, -A − c ∈ R + \ {0} is a positive real independent parameter dictating the scale of acceleration towards the cell vertex at the distance of maximum acceleration towards the vertex (equivalent to the valuation of A − c • N (±σ) on a Gaussian normal distribution curve), -d c ∈ R + \ {0} is a positive real independent parameter dictating the radius of a sphere at which the inflection in the potential contour generator occurs, i.e. the distance at which maximum acceleration towards the vertex occurs (taking the place of σ in (12), which is analogous to a Gaussian normal distribution curve).The potential of a square formation cell surrounding an agent that figures as an obstacle is represented in figure 5.

Reformulation in terms of decentralized control functions
The monotonicity of (6,8,11) ensures that the direction of the gradient of the potential, ) can be solved analytically for any finite sum of terms of the form specified by (6,8,11) up to the values of the independent parameters (A ).The procedure follows: Equation ( 18) can be summarized by designating the terms in (7, 10, 12) as a (o,w,c) respectively.

The terms a
(o,w,c) i • n i , can likewise be denoted a potential-based decentralized control functions due to the i-th feature.

Rotor modification
As mentioned in sec.2.2, the virtual potential approach to guidance is extremely susceptible to the appearance of local minima.A robust and simple approach is needed to assure local minima avoidance.In terms of the vector field introduced by ( 2), the analytical solution of which is presented in (20 -21), stable local minima occur due to the irrotationality of the field, rot E(x) id = 0.In order to avoid irrotationality, and thereby local minima, decentralized control functions proposed in (7,10,12) are redesigned, adding a rotor component: Where: a i is the redefined total decentralized control function due to the i-th feature (the dash will hereafter be omitted), a (s) i is the stator decentralized control function as introduced in the preceding section, 107 Formation Guidance of AUVs Using Decentralized Control Functions denoted with the superscript (s) to contrast it with the newly introduced a i is the rotor decentralized control function, all of which are continuous real 2D vector fields over the Euclidean 2-space (mapping R 2 to itself) such that they Jacobians exist wherever each of them is defined.The introduction of a (r) i establishes a non-zero rot(E) by design, as follows: With respect to the way-point, its potential influence on an AUV in this framework must not be prejudiced in terms of the direction of approach.If a decentralized control function of a way-point were augmented with a rotor part, the direction of a w would deviate from line-of-sight.The same is true of formation cell vertices.Therefore, the only non-zero rotor decentralized control functions are those of obstacles.As a result, ( 23) can be further simplified to: An individual obstacle rotor decentralized control function is defined below: Where: - is a positive real independent parameter dictating the scale of acceleration perpendicular to the direction of fastest flight from the obstacle, -a (r) i ∈ R + is the magnitude of the rotor decentralized control function, -â(r) i ∈ SO 2 is the direction of the rotor decentralized control function, x i ∈ R 2 is the center of the i-th obstacle, n i (x) ∈ SO 2 is the unit vector in the direction of fastest flight from the i-th obstacle, r i (x) is the unit rotor direction generator, such that â(r) 2 is the current true over-ground velocity of the AUV (including possible sideslip) projected onto the "flight ceiling".The rotor decentralized control function and the total decentralized control function consisting of the superposition of the rotor and stator parts, are displayed in figure 6.

Potential framework of formations
The formation introduced by the proposed framework is the line graph occurring at the tile interfaces of the square tessellation of R 2 , represented in figure 7. Due to a non-collocated nature of AUV motion planning, an important feature of candidate tessellations is that they be periodic and regular, which the square tessellation is.Each AUV whose states are being estimated by the current, i-th AUV, meaning j-th AUV, j = i) is considered to be a center of a formation cell.The function of the presented framework for potential-based formation keeping is depicted in figure 8.In an unstructured motion of the cooperative group, only a small number of cell vertices attached to j-th AUVs ∀j = i, if any, 109 Formation Guidance of AUVs Using Decentralized Control Functions

The platform -A large Aries-precursor AUV
The vehicle whose dynamic model will be used to demonstrate the developed virtual potential framework is an early design of the NPS2 Aries autonomous underwater vehicle which was resized during deliberations preceding actual fabrication and outfitting.The resulting, smaller Aries vehicle has been used in multiple venues of research, most notably (An et al., 1997;Marco & Healey, 2000;2001).As Marco & Healey (2001) describe, the vehicle whose model dynamics are used has the general body plan of the Aries, displayed in figure 9, albeit scaled up.The body plan is that of a chamfered cuboid-shaped fuselage with the bow fined using a nose-cone.The modeled Aries-precursor vehicle, the same as the Aries itself, as demonstrated in the figure, combines the use of two stern-mounted main horizontal thrusters with a pair of bow-and stern-mounted rudders (four hydrofoil surfaces in total, with dorsal and ventral pairs mechanically coupled), and bow-and stern-mounted elevators.Healey & Lienard (1993) have designed sliding mode controllers for the Aries-precursor vehicle, considering it as a full-rank system with states x = [v T ω T x T Θ] = [u v w|p q r|x y z|ϕ ϑ ψ] T , relying on the actuators: Image from the public domain. Where: δ r (t) is the stern rudder deflection command in radians, -δ s (t) is the stern elevator planes' command in radians, -n(t) is the main propellers' revolution rate in rad/s.

Model dynamics of the vehicle
The dynamics published by Healey and Lienard are used in the HILS 3 in the ensuing sections, and were developed on the grounds of hydrodynamic modelling theory (Abkowitz, 1969;Gertler & Hagen, 1967), exploited to great effect by Boncal (1987).The equations for the six degrees of freedom of full-state rigid-body dynamics for a cuboid-shaped object immersed in a viscous fluid follow, with the parameters expressed in Table 1.

Surge
3 Hardware-in-the-loop simulation. 111 Formation Guidance of AUVs Using Decentralized Control Functions

Control
The Aries-precursor's low-level control encompasses three separate, distinctly designed decoupled control loops: 1. Forward speed control by the main propeller rate of revolution, 2. Heading control by the deflection of the stern rudder, 3. Combined control of the pitch and depth by the deflection of the stern elevator plates.
All of the controllers are sliding mode controllers, and the precise design procedure is presented in (Healey & Lienard, 1993).In the interest of brevity, final controller forms will be presented in the ensuing subsections.

Forward speed
The forward speed sliding mode controller is given in terms of a signed squared term for the propeller revolution signal, with parameters (α, β) dependent on the nominal operational parameters of the vehicle, and the coefficients presented in table 1: x G = 0.0 m

Parameters of the Model Dynamics
It is apparent from the above that the propeller rate of revolution command comprises a term that accelerates the vehicle in the desired measure ( uc (t)), overcomes the linear drag (u(t)|u(t)|), and attenuates the perturbations due to disturbances and process noise ( σu (t)).

Heading
The sliding surface for the subset of states governing the vehicle's heading is given below, in (43).The resulting sliding mode controller is contained in (44).σ r = −0.074ṽ(t) + 0.816r(t) + 0.573 φ(t) (43) δ r = 0.033v(t) + 0.1112r(t) + 2.58 tanh 0.074 ṽ(r) + 0.816r(t) + 0.573 φ(t) 0.1 It should be noted that ṽ(r) seems to imply the possibility of defining some v c (t) for the vehicle to track.This is impractical.The Aries-precursor's thrust allocation and kinematics, nonholonomic in sway, would lead to severe degradation of this sliding mode controller's performance in its main objective -tracking the heading.Lienard (1990) provides a further detailed discussion of this and similar sliding mode controllers.

Pitch and depth
The main objective of the third of the three controllers onboard the Aries-precursor HIL simulator, that for the combination of pitch and depth, is to control depth.For a vehicle with the holonomic constraints and kinematics of the model used here, this is only possible by using the stern elevators δ s to pitch the vehicle down and dive.Accordingly, the sliding surface is designed in (44), and the controller in (45).

Obstacle classification, state estimation and conditioning the control signals
In this section, the issues of obstacle classification will be addressed, giving the expressions for (d i , n i ) of every type of obstacle considered, which are functions prerequisite to obtaining P i -s through composition with one of (6,8,11).Also, full-state estimation of the AUV (modeled after the NPS Aries-precursor vehicle described in the preceding section), x = [ û v ŵ p q r x ŷ ẑ φ θ ψ ] T will be explored.Realistic plant and measurement noise ( ñ, ỹ), which can be expected when transposing this control system from HILS to a real application will be discussed and a scheme for the generation of non-stationary stochastic noise given.
Finally, the section will address a scheme for conditioning / clamping the low-level control signals to values and dynamic ranges realizable by the AUV with the Aries body-plan.The conditioning adjusts the values in the low-level command vector c = [a c u c r c ψ c ] T to prevent unfeasible commands which can cause saturation in the actuators and temporary break-down of feedback.

Obstacle classification
The problem of classification in a 2D waterspace represented by R 2 is a well studied topic.
We have adopted an approach based on modeling real-world features after a sparse set of geometrical primitives -circles, rectangles and ellipses.
In the ensuing expressions, {x int } will be used for the closed, connected set comprising the interior of the obstacle being described.T i shall be a homogeneous, isomorphic coordinate transform from the global reference coordinate system to the coordinate system attached to the obstacle, affixed to the centroid of the respective obstacle with a possible rotation by some ψ i if applicable.

Circles
Circles are the simplest convex obstacles to formulate mathematically.The distance and normal vector to a circle defined by (x i ∈ R 2 , r i ∈ R + ), its center and radius respectively, are given below: Robust and fast techniques of classifying 2D point-clouds as circular features are very well understood both in theory and control engineering practice.It is easy to find solid algorithms applicable to hard-real time implementation.Good coverage of the theoretic and practical aspects of the classification problem, solved by making use of the circular Hough transform is given in (Haule & Malowany, 1989;Illingworth & Kittler, 1987;Maitre, 1986;Rizon et al., 2007).

Rectangles
The functions for the distance and normal vector (d i (x), n i (x)), of a point with respect to a rectangle in Euclidean 2-space defined by ), the center of the rectangle, the half-length and half-breadth and the angle of rotation of the rectangle's long side w.r.t. the global coordinate system, respectively, are given below: There is a large amount of published work dedicated to the extraction of the features of rectangles from sensed 2D point-clouds.Most of these rely on Hough space techniques (Hough & Powell, 1960) and (Duda & Hart, 1972) to extract the features of distinct lines in an image and determine whether intersections of detected lines are present in the image (He & Li, 2008;Jung & Schramm, 2004;Nguyen et al., 2009).

Ellipses
The method of solving for a distance of a point to an ellipse involves finding the roots of the quartic (57).Therefore, it is challenging to find explicit analytical solutions, although some options include Ferrari's method (Stewart, 2003) or algebraic geometry (Faucette, 1996).A computer-based control system can, however, employ a good, numerically stable algorithm to obtain a precise enough solution.The rudimentary part of analytic geometry that formulates the quartic to be solved is given below in (50 -57).
The equation of the ellipse with the center in the origin and axes aligned with the axes of the coordinate system is: The locus of its solutions is the ellipse, {x e = x e y e T }.The analysis proceeds by considering those x = x y T ∈ R 2 for which x − x e is normal to the ellipse.The equation of such a normal is: Where τ ∈ R is an independent parameter, the degree of freedom along the line and k is the direction vector of the line, given below: Substituting the right-hand side of ( 54) into (50), the quartic discussed is obtained as: The greatest root of (57), t, allows for the calculation of (d i (x), n i (x)) in (51, 54), as given below: 59) With the advent of cheap solid-state perception sensors in service robotics and aerial photography in the last decade, publication on fast and robust ellipse-fitting of 2D point clouds has intensified - (Ahn et al., 1999;Jiang et al., 2007;Pilu et al., 1996).

State estimation
The full state x = v T ω T x T Θ T T = u v w|p q r|x y z|ϕ ϑ ψ T of the AUV will be estimated using the Scaled Unscented Transform Sigma-Point Kalman Filter (SP-UKF) introduced by van der Merwe (2004).
The Extended Kalman Filter formulations that feature more prominently in marine control engineering state-of-the-art are capable of estimating the states of nonlinear model dynamics by taking into account only first-order statistics of the states (with possible addition of plant / process noise.EKFs use Jacobians of the nonlinear operator(s) evaluated at the current state estimate.The Unscented Kalman Filters (UKF), in contrast, use the original non-linear model dynamics to propagate samples -called sigma-points, which are characteristic of the current estimate of the statistical distribution of the states, influenced by process and measurement noise.The Kalman gain is evaluated based on the covariance of state hypotheses thus propagated vs. the covariance of the samples characteristic of the current estimate of the statistical distribution of the measurements.The Kalman gain will award a higher gain to those measurements for which a significant correlation is discovered between the state and measurement hypotheses and for which the covariance of the measurement hypotheses themselves is relatively small.The algorithm is listed in table 2.

Measurement and process noise
The AUV is assumed to carry a 4-beam DVL4 which it can use to record the true 3D speed-over-ground measurement v = u v w T .Furthermore, the AUV carries a 3-axial rate gyro package capable of measuring the body-fixed angular velocities ω = p q r T .A Set κ, the scaling parameter of the set of sigma-point drawn from the underlying distribution 0.4.L = 12, the number of states 0.5.
-the initial covariance matrix of the estimates 1.3.
Set R f -the process noise covariance 1.4.
Set R n -the measurement noise covariance 2. Iteration for k = 1 . . .∞ 2.1.Sigma-points and hypotheses of the states 2.1.1.X − (k) = {x − } = x(k|k − 1) x(k|k − 1) 2.4.Re-draw the hypotheses taking into account the process noise covariance low-grade commercial USBL 5 system is assumed to provide estimates of x y T .A fusion of the USBL estimate and the pressure gauge prior to the SP-UKF entry point is assumed to provide a relatively good-quality depth reading of z.A 3-axial middle-market strap-down AHRS 6 is assumed to provide the Tait-Bryan angle readings, Θ = ϕ ϑ ψ T .
In the proposed HILS framework, measurement noises should mimic the actual experience during AUV fieldwork operations.Therefore, a noise generator which can produce non-stationary, varying noises is required.It is intended that these noises include errors in the sensor readings whose sources cannot be simply identified by recourse to first-order statistics, and which therefore cannot be easily calibrated (de-biased) for.Additionally, we wish to be able to generate sporadic irrecoverable faults i.e. events during which a sensor reading cannot be relied on in any meaningful way.
For this reason, we propose the use of a bank of Gaussian Markov models -GMMs, for generating the additive measurement noise.Markov models are stochastic state-machines whose state-switching is governed by random number generators.GMMs ultimately output a normally distributed random number with the statistics dependent on the current state.
Means and standard deviations (μ i , σ i ) of each state i are designed into the GMM.In this chapter, a bank of 12n AUV Gaussian Markov models, one for each state of each of the n AUV AUVs is used.All of the GMM states contain separate univariate rate-limited white noise generators parameterized by (μ i , σ i , n i ), where n i is the rate limit of the additive measurement noise in the i-th channel.
Relying on the MATLAB normally distributed random number generator invoked by the randn command, each state generates a number according to: Where T is the sampling time.
To optimize between a realistic nature of the measurement noises and HILS complexity, each of the Markov models in the employed bank contains 6 states, {nominal, +reliable, −reliable + unreliable, −unreliable, f ault}.The 6-state Gaussian Markov models are initialized by a 6-tuple M = ((μ 1 , σ 1 , n 1 ), • • • , (μ 6 , σ 6 , n 6 )) and a 6 × 6 transition matrix T = [t ij ] with t ij being a priori probabilities of switching from state i to state j.The actual parameters used in the HILS simulation are presented in (65 -74).Before adding them to idealized state measurements, the noise channels are mixed together (correlated) as y ← S y y using the matrix S y in (75), to mimic the physics of the relevant sensors' interdependence of measurements.Notice that S y has a pronounced block-diagonal structure, indicative of the fact that the mentioned sensors (DVL, USBL, AHRS, gyro-compass and rate gyros) output several state measurements each.The correlation between the states measured by a single instrument is more pronounced than the one between measurements of mutually dislocated sensors operating along different physical principles.
5 Ultra-short baseline hydroacoustic localization. 6Attitude and heading reference system.

Sensor fault simulation
The last mentioned state, "fault" doesn't generate random additive measurement noises, but outputs NaN 7 values, which are ignored by the SP-UKF.On detecting the NaN value in a measurement channel, the SP-UKF sets the corresponding, i-th row of the column vector of measurements y i to the value of ŷi .This results in the corresponding rows and columns of the P y covariance matrix being zero, an update to the corresponding row of the estimate column vector x(k|k) being omitted, and the corresponding rows and columns of P x (k) growing rather than falling.The latter signifies a decrease in the trustworthiness of the estimate, and compromises the stability of the SP-UKF should a string of faulty readings continue overlong.

Outlier rejection
In order for the SP-UKF to remain stable and deliver trustworthy state estimates to the feedback of the relevant controllers, outlier measurements are rejected.Rejection of an outlier is dealt with identically to a faulty measurement, the appropriate row of the measurement column vector y being over-written with NaN as if there were a sensor fault occurring in the outlier measurement channel.Outlier rows of y are considered those for which any values, inspected column-wise, fulfill: Where P x (k) is the covariance matrix of the estimate of the full-state vector rendered by the SP-UKF.

Process noise
The additive process noise is assumed to be multivariate rate-limited white noise without bias.
The used covariance matrix is given in (79) and the rate limits in a vector in (80).

122
Autonomous Underwater Vehicles

Control signals conditioning
The solution of f i (k) ← E(x AUV (k)), obtained by evaluating (2), or more precisely (29), at the estimate xAUV (k) rendered by the SP-UKF, will be used for the formation of commands accepted by the forward speed and heading controllers in (41, 44), c = [ uc u c ψc ψ c ].
The task of the low-level control system of the AUV is to try to recreate a motion that would result from applying f (k) to an unconstrained point unit mass, i.e. a holonomic 2D double integrator model, up to the thrust allocation and kinematic and dynamic constraints of the actual vehicle.In the following equations, the sampling with time T of the integration over the R 2 space is assumed in the form of the Euler backwards formula.So naively: Where: -f id = f is the norm of the total controlling force, admitting decomposition into f f ⊥ T , the components parallel and perpendicular to the direction of heading of the AUV (given by ψ(k − 1), notwithstanding possible sideslip resulting from At this point, it is assumed that an AUV has a specified performance envelope of (u, u, ψ, ψ).With these as given independent parameters, the manipulation of (81 -84) results in the constraints that dictate the admissible ranges to which f needs to be clamped to avoid forcing the low-level controllers of the AUV beyond their normal operating range.

The constraint inequalities
The locus of solutions of (81) for f in easier to visualize in an AUV-fixed coordinate system (one with the origin in x i , with the x-axis aligned with u(k − 1)∠ψ(k − 1)).In it, the admissible solution locus is a disc offset along the x-axis by u(k − 1)/T, given by the implicit expression: The locus of solutions of ( 82) is, similarly to the preceding case, an offset annulus concentric to the disc described by (85): 123 Formation Guidance of AUVs Using Decentralized Control Functions The locus of solutions of ( 83) is an angular sector of an infinite disk (a 1-cone) concentric to the previous two loci, expressed in terms of: The locus of solutions of ( 84) is likewise an angular sector of an infinite disk (a 1-cone) concentric to all the other loci, which satisfies the inequality: A solution for f (k) is legal if it meets all of the criteria stated in (85-88), i.e. if it belongs to a subset of R 2 shaped as an annular sector.

Clamping the total controlling force
If (85 -88) are not fulfilled, a non-linear procedure for the clamping f to an admissible range is employed.Consequently, the low-level controllers' operating point(s) remain within a quasi-linear vicinity of the sliding surfaces.The procedure is pseudo-coded in Algorithm 3.After clamping by the presented algorithm, (81 -84) are used to form the commands c = a c u c r c ψ c T for the low-level controllers 41, 44).

Simulation results
Combining the presented virtual potentials framework and the HILS presented in the previous chapters, a full simulation is presented for a group of 4 simulated Aries-precursor AUVs cruising in formation.

Simulation 1
The first simulation presents a cruise in formation down an unobstructed channel in between two obstacles towards the way-point.Figure 10 presents the actual paths traveled by the AUVs. Figure 11 presents the speeds of all four vehicles.The initial dips in the path occur due to the non-holonomic nature of the vehicles' kinematics, due to which they cannot initialize the manoeuvres from zero starting speed that would preserve the initial formation perfectly and still commence navigation to the way-point.This is especially exacerbated by the fact that at near-zero speeds, the control surfaces (δ r , δ s ) are terribly ineffective.The final dips in the area of the paths around the way-point occur after the AUVs have parked in the stable formation configuration.Near the waypoint and at low speeds, the drift in the state estimates is accentuated by a lack of passive stability provided by AUVs' streamlining at higher speeds.This, in hand with non-holonomic kinematics, causes the vehicles to momentarily break formation.It is only after accumulating enough speed that vehicles can turn within a small enough radius to re-establish the formation.Dips in the path correspond to the dips in the speed graphs for the vehicles, as their commanded speed shoots up again in order to re-establish the formation.if not (85) or (86) if f > 0 -this indicates that the required dominant behaviour produced by the virtual potentials framework is to accelerate the AUV further in the forward direction; Trying to follow through on this behaviour, having in mind the operational wisdom concerning real-world missions and experiences, it is much safer to give priority to the component that manoeuvres the AUV to the side, in order for the AUV to be able to circumnavigate the obstacles it might be heading towards.

Simulation 2
The second simulation presents a cruise in formation down a heavily cluttered corridor defined by two larger rectangular obstacles.Figure 12 presents the actual paths traveled.Figure 13 presents the speeds of the four vehicles.

Cruise phase 1
A cruise is started in a formation.However, very soon the formation encounters the first obstacle.As the leading formation-members are momentarily slowed down before they circumnavigate to either side of the obstacle, the trailing members "pile up" in front of this artificial potential barrier (especially the AUV closest to the origin).This is evident in the dips and temporary confusion before the first, circular obstacle.However, the operational safety approach that is implicitly encapsulated by the cross-layer design is preserved.The vehicles break formation, so that one of the vehicle circumnavigates the first obstacle on the left, and the others on the right.Since this produces a significantly different trajectory from the rest of the group, vehicle 1 isn't able to rejoin the formation until much later.

Cruise phase 2
The other 3 vehicles (2, 3 and 4), before being able to restore a formation encounter the first of the two large rectangular obstacles.Note that vehicles 2 and 3 remain in the leader-follower arrangement as evidenced by their closely matching trajectories in this phase of the cruise.
The "outrigger" vehicle 4, trying to keep in formation with 2 and 3, encounters the large rectangular obstacle at a bearing much closer to head-on.Therefore, it executes a significant course change manoeuvre, during which it cannot satisfactorily compromise between safe avoidance of the obstacle and staying in formation with 2 and 3.As vehicles 2 and 3 navigate in formation through the strait in between the circular and the first large rectangular obstacle, the leader vehicle 3 starts to manoeuvre to starboard towards the way-point.This manoeuvre causes the formation cell vertex trailing behind vehicle 3 that represents the dominant navigation goal for vehicle 2 to start accumulating speed in excess of what 2 is able to match.This is due to the fact that as 3 swings to starboard, the formation cell vertex "sweeps" through water with a velocity that consists of the sum of linear velocity of vehicle 3 and the tangential velocity contributed by the "arm" of the formation cell f .Therefore, the formation is temporarily completely broken.

Cruise phase 3
However, the breaking of formation between 2 and 3 occurs at such a time that 1 catches up with 2 before 2 gets much farther afield, presenting its trailing cell vertex as a local navigation goal to 2. That is why 2 exhibits a hard break to starboard, trying to form itself up as a follower of vehicle 1.However, just as 2 is completing its formation, vehicle 1, manoeuvres around the final obstacle -the small diagonally presented rectangle.As the trailing cell vertex of 1 is, from 2's viewpoint, shadowed by the obstacle's repulsion, it reorients towards what until then is a secondary navigation goal in its vicinity -the cell vertex of the "latecomer" of Phase 2, vehicle 4.This reorientation is what contributes to 2's "decision" to circumnavigate the diagonal rectangle to starboard, rather than to port, as would be optimal if no formation influences were present.Phase 3 finishes as vehicle 2 is trying to pursue vehicle 4, and vehicle 4 corners the diagonal rectangle, getting away from vehicle 2.

Cruise phase 4
Phase 4 is entered into without formations.This phase is characterized by converging on the way-point, which all the vehicles reach independently, followed by re-establishing the formation.However, an ideal formation is impossible due to operational safety, as no vehicle is "willing" to approach the second large rectangle (towards the top of the figure).This is exacerbated by reduced manoeuvring capabilities, as all vehicles reduce speed in the vicinity of the way-point.

Conclusion
The chapter has presented a virtual potentials-based decentralized formation guidance framework that operates in 2D.The framework guarantees the stability of trajectories, convergence to the way-point which is the global navigation goal, and avoidance of salient, hazardous obstacles.Additionally, the framework offers a cross-layer approach to subsuming two competing behaviours that AUVs in a formation guidance framework need to combine -a priority of formation maintenance, opposed by operational safety in avoiding obstacles while cruising amidst clutter.
Additionally to the theoretical contribution, a well-rounded functional hardware-in-the-loop system (HILS) for realistic simulative analysis was presented.Multiple layers of realistic dynamic behaviour are featured in the system: 1.A full-state coupled model dynamics of a seaworthy, long-autonomy AUV model based on rigid-body physics and hydrodynamics of viscous fluids like water, 2. An unbiased rate-limited white noise model of the process noise, 3. A non-stationary generator of measurement noise based on Gaussian Markov models with an explicitly included fault-mode, 4. An outlier-elimination scheme based on the evaluation of the state estimate covariance returned by the employed estimator, 5.A Scaled Unscented Transform Sigma-Point Kalman Filter (SP-UKF) that can work either in the filtering mode, or a combination of filtering and pure-prediction mode when faulty measurements are present, utilizing a full-state non-linear coupled AUV model dynamics, 6.A command signal adaptation mechanism that accents operational safety concerns by prioritizing turning manoeuvres while accelerating, and "pure" braking / shedding forward speed when decelerating.

Further work
Several distinct areas of research, based on the developed HILS framework, remain to ascertain the quality of the presented virtual potential-based decentralized cooperative framework.These are necessary in order to clear the framework for application in costly and logistically demanding operations in the real Ocean environment.

Fig. 1 .
Fig. 1.An example of an oscillatory trajectory due to the conservativeness of the virtual potential system.

Fig. 2 .
Fig. 2. Example of a local minimum occurring in virtual potential guidance in a 2D waterspace.
Fig. 4. The potential contour generator of a way-point, p w (d(x)).

Fig. 5 .
Fig. 5.The potential contour generator of a formation agent.

Fig. 6 .
Fig. 6.Direction of the rotor decentralized control function a (r) i and the two-term a i = a (s) i + a (r) i decentralized control function.

Fig. 8 .
Fig. 8.The potential masking of agents out of and in formation.arepartially masked by nodes.The i-th AUV is attracted strongest to the closest cell vertex, in line with how attractiveness of a node varies with distance expressed in (11).In the structured case in 8.b), presenting an ideal, undisturbed, non-agitated and stationary formation, all the j-th AUVs in formation are masking each the attractiveness (w.r.t. the i-th AUV) of the vertex they already occupy.At the same time they reinforce the attractiveness of certain unoccupied vertices at the perimeter of the formation.The vertices that attract the i-th AUV the strongest thus become those that result in the most compact formation.Notice in figure8.b)how certain vertices are colored a deeper shade of blue than others, signifying the lowest potential.The square formation cell is a cross figure appearing at the interstice of four squares in the tessellation, comprised of the j-th AUV and the four cell vertices attached to it, in the sense that their position is completely determined based on the i-th AUV's local estimation of j-th AUV's position, (x (i) j ), as in figure7.The cell vertices are uniquely determined by x(i) j and an independent positive real scaling parameter f .

Fig. 9 .
Fig. 9.The Aries, demonstrating the body-plan and general type of the model dynamics.

Fig. 10 .
Fig. 10.Paths of the 4 HILS models of AUVs based on the precursor to the NPS Aries vehicle cruising in an uncluttered environment.

Fig. 11 .
Fig. 11.Speeds of the 4 HILS models of AUVs based on the precursor to the NPS Aries vehicle cruising in an uncluttered environment.

Fig. 12 .Fig. 13 .
Fig. 12. Paths of the 4 HILS models of AUVs based on the precursor to the NPS Aries vehicle cruising in a cluttered environment.

1.
Realistically model the representation of knowledge of the other AUVs aboard each AUV locally.This can be approached on several fronts: (a) Exploring the realistic statistics of the sensing process when applied to sensing other AUVs as opposed to salient obstacles in the waterspace.Exploring and modeling the beam-forming issues arising with mechanically scanning sonars vs. more complex and costlier multi-beam imaging sonars, (b) Exploring the increases in complexity (and computer resource management), numerical robustness and stability issues of AUV-local estimation of other AUVs in the formation, 129 Formation Guidance of AUVs Using Decentralized Control Functions

Table 2 .
The Scaled Unscented Transform Sigma-Point Kalman Filter Algorithm , require the f ⊥ component to be as large as admissible, and adjust the f accordingly else -otherwise the required dominant behaviour produced by the virtual potentials framework is to decelerate or break the AUV; Trying to follow through on this behaviour, it is much safer to give priority to the component that decreases the forward speed of the AUV, in order for the AUV to avoid colliding with possible obstacles it is heading towards.