**3. 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.

Fig. 7. The square tiling of the plane.

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,

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

Formation Guidance of AUVs Using Decentralized Control Functions 111

The dynamics published by Healey and Lienard are used in the HILS<sup>3</sup> 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

<sup>2</sup>) + *yG*(*pq* <sup>−</sup> *<sup>r</sup>*˙) + *zG*(*pr* <sup>+</sup> *<sup>q</sup>*˙)

*uvδ<sup>r</sup>* <sup>+</sup> *uw*

<sup>−</sup> (*<sup>W</sup>* <sup>−</sup> *<sup>B</sup>*) sin *<sup>ϑ</sup>* <sup>+</sup> *<sup>ρ</sup>*

*Xu*˙ *<sup>u</sup>*˙ <sup>+</sup> *Xwqwq* <sup>+</sup> *Xvpvp* <sup>+</sup> *Xvrvr* <sup>+</sup> *uq*

 <sup>=</sup> *<sup>ρ</sup>* 2 *L*<sup>4</sup>

*Xw<sup>δ</sup><sup>s</sup>*

2

*L*2*u*2*Xprop* (31)

*Xpp p*<sup>2</sup> + *Xqqq*<sup>2</sup> + *Xrrr*

2 *L*<sup>2</sup>

*Xq<sup>δ</sup>sδ<sup>s</sup>* <sup>+</sup> *Xq<sup>δ</sup>b*/2*δbp* <sup>+</sup> *Xq<sup>δ</sup>b*/2*δbs*

*<sup>L</sup>*3*Xδsnuqδs�*(*n*) + *<sup>ρ</sup>*

*<sup>δ</sup><sup>s</sup>* <sup>+</sup> *Xw<sup>δ</sup>b*/2*δbs* <sup>+</sup> *Xw<sup>δ</sup>b*/2*δbp*

2

*Xwδsnuwδ<sup>s</sup>*

Image from the public domain.

**4.1 Model dynamics of the vehicle**

*<sup>u</sup>*˙ <sup>−</sup> *vr* <sup>+</sup> *wq* <sup>−</sup> *xG*(*q*<sup>2</sup> <sup>+</sup> *<sup>r</sup>*

*<sup>s</sup>* <sup>+</sup> *<sup>X</sup>δbδb*/2*δ*<sup>2</sup>

*�*(*n*) + *<sup>ρ</sup>* 2

+ *ρ* 2 *L*2 

> *s*

<sup>3</sup> Hardware-in-the-loop simulation.

*urδr* + *ρ* 2 *L*2 

*<sup>X</sup>δsδ<sup>s</sup> <sup>δ</sup>*<sup>2</sup>

+*Xδs<sup>δ</sup>snu*<sup>2</sup>*δ*<sup>2</sup>


a viscous fluid follow, with the parameters expressed in Table 1.

*Xvvv*<sup>2</sup> + *Xwww*<sup>2</sup> + *Xv<sup>δ</sup><sup>r</sup>*

*δ*2 *r* 

*<sup>b</sup>* + *Xδrδ<sup>r</sup>*

Where:

**4.1.1 Surge**

+*Xpr pr*

+*Xr<sup>δ</sup><sup>r</sup>*

+*u*2

*m* 

Fig. 8. The potential masking of agents out of and in formation.

are *partially* 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 figure 8.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*) *<sup>j</sup>* ), as in figure 7. The cell vertices are uniquely determined by ˆ*x* (*i*) *<sup>j</sup>* and an independent positive real scaling parameter *f* .

#### **4. 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 NPS<sup>2</sup> *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**<sup>T</sup> ω<sup>T</sup> *x*<sup>T</sup> **Θ**] = [*uvw*|*pqr*|*xyz*|*ϕϑψ*] T, relying on the actuators:

$$\mathbf{u}(t) = [\delta\_r(t)\,\delta\_s(t)\,\ n(t)]^\mathrm{T} \tag{30}$$

<sup>2</sup> Naval Postgraduate School, 700 Dyer Rd., Monterey, CA, USA.

Fig. 9. The *Aries*, demonstrating the body-plan and general type of the model dynamics. Image from the public domain.

### Where:

12 Will-be-set-by-IN-TECH

(a) Disordered arrangement (b) Formation arrangement

are *partially* 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 figure 8.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

*<sup>j</sup>* ), as in figure 7. The cell vertices are uniquely determined by ˆ*x*

The vehicle whose dynamic model will be used to demonstrate the developed virtual potential framework is an early design of the NPS<sup>2</sup> *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

Healey & Lienard (1993) have designed sliding mode controllers for the *Aries*-precursor vehicle, considering it as a full-rank system with states **x** = [**v**<sup>T</sup> ω<sup>T</sup> *x*<sup>T</sup> **Θ**] =

**u**(*t*)=[*δr*(*t*) *δs*(*t*) *n*(*t*)]<sup>T</sup> (30)

(*i*) *<sup>j</sup>* and an

Fig. 8. The potential masking of agents out of and in formation.

AUV's position, (*x*ˆ

[*uvw*|*pqr*|*xyz*|*ϕϑψ*]

(*i*)

independent positive real scaling parameter *f* .

**4. The platform – A large** *Aries***-precursor AUV**

pairs mechanically coupled), and bow- and stern-mounted elevators.

T, relying on the actuators:

<sup>2</sup> Naval Postgraduate School, 700 Dyer Rd., Monterey, CA, USA.


#### **4.1 Model dynamics of the vehicle**

The dynamics published by Healey and Lienard are used in the HILS<sup>3</sup> 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.

#### **4.1.1 Surge**

$$\begin{split} &m\left[\dot{u} - ur + w\eta - \mathbf{x}\_G(q^2 + r^2) + y\_G(pq - \dot{r}) + z\_G(pr + \dot{\eta})\right] = \frac{\rho}{2} \mathbf{L}^4 \left[\mathbf{X}\_{pp}p^2 + \mathbf{X}\_{q\dot{q}}q^2 + \mathbf{X}\_{rr}r^2\right] \\ &+ \mathbf{X}\_{pr}pr\left[ + \frac{\rho}{2} \mathbf{L}^2 \left[\mathbf{X}\_{\dot{u}\dot{l}}\dot{u} + \mathbf{X}\_{w\dot{q}}u\eta + \mathbf{X}\_{vp}vp + \mathbf{X}\_{\dot{v}\dot{r}}vr + u\eta \left(\mathbf{X}\_{q\dot{\delta}\_s\dot{s}}\dot{s}\_s + \mathbf{X}\_{q\dot{\delta}\_b\dot{s}}\dot{\delta}\_s\theta\_{bp} + \mathbf{X}\_{q\dot{\delta}\_s\dot{r}}\dot{\delta}\_s\theta\_{bs}\right) \right. \\ &+ \mathbf{X}\_{\delta\dot{s}}ur\delta\_{\dot{r}}\Big] + \frac{\rho}{2} \mathbf{L}^2 \left[\mathbf{X}\_{\ddot{w}\dot{r}}v^2 + \mathbf{X}\_{\ddot{w}\dot{w}}w^2 + \mathbf{X}\_{\ddot{u}\delta\_r}uv\delta\_r + uw\left(\mathbf{X}\_{w\dot{\delta}\_s\dot{s}}\delta\_s + \mathbf{X}\_{w\dot{\delta}\_b\dot{r}}\dot{\delta}\_s\theta\_{br} + \mathbf{X}\_{w\dot{\delta}\_s\dot{r}}\dot{\delta}\_s\theta\_{br}\right) \right. \\ &+ u^2 \left(\mathbf{X}\_{\ddot{\delta}\_s\dot{s}}\dot{\delta}\_s^2 + \mathbf{X}\_{\ddot{\delta}\_b\dot{s}}\delta\_s^2\dot{\delta}\_s + \mathbf{X}\_{\ddot{\delta}\_s\dot{s}}\delta\_r\theta\_r^2\right)$$

<sup>3</sup> Hardware-in-the-loop simulation.

**4.1.6 Yaw**

−*yG*(*u*˙ − *vr* + *wq*)

**4.1.7 Substitution terms**

· sin *<sup>ϑ</sup>* <sup>+</sup> *<sup>ρ</sup>*

**4.2 Control**

decoupled control loops:

**4.2.1 Forward speed**

 + *ρ* 2 *L*<sup>5</sup>

+*Nrur* + *Nvqvq* + *Nwpwp* + *Nwrwr*

·*h*(*x*)(*<sup>v</sup>* <sup>+</sup> *xr*)<sup>2</sup> <sup>+</sup> *Cdzb*(*x*)(*<sup>w</sup>* <sup>−</sup> *xq*)

*Uc f*(*x*) =

*�*(*n*) = <sup>−</sup><sup>1</sup> <sup>+</sup> sign(*n*)

1. Forward speed control by the main propeller rate of revolution,

parameters of the vehicle, and the coefficients presented in table 1:

*<sup>α</sup>* <sup>=</sup> *<sup>ρ</sup>L*<sup>2</sup>*Cd* 2*m* + *ρL*<sup>3</sup>*Xu*˙

; *<sup>n</sup>*<sup>0</sup> <sup>=</sup> 52.359 rad

*<sup>n</sup>*(*t*)|*n*(*t*)<sup>|</sup> = (*αβ*)−<sup>1</sup>

*<sup>β</sup>* <sup>=</sup> *<sup>n</sup>*<sup>0</sup> *u*0

*Ct* <sup>=</sup> 0.008 *<sup>L</sup>*2*η*|*η*<sup>|</sup>

2. Heading control by the deflection of the stern rudder,

be presented in the ensuing subsections.

*Izr*˙ + (*Iy* <sup>−</sup> *Ix*)*pq* <sup>−</sup> *Ixy*(*p*<sup>2</sup> <sup>−</sup> *<sup>q</sup>*2) <sup>−</sup> *Iyz*(*pr* <sup>+</sup> *<sup>q</sup>*˙) + *Ixz*(*qr* <sup>−</sup> *<sup>p</sup>*˙) + *<sup>m</sup>*

*Np*˙ *p*˙ + *Nr*˙*r*˙ + *Npqpq* + *Nqrqr*

Formation Guidance of AUVs Using Decentralized Control Functions 113

+ *ρ* 2 *L*<sup>3</sup>

<sup>2</sup> *<sup>w</sup>* + *xq Uc f*(*x*)

*Xprop* <sup>=</sup> *Cd*0(*<sup>η</sup>* <sup>|</sup>*η*<sup>|</sup> <sup>−</sup> <sup>1</sup>); *<sup>η</sup>* <sup>=</sup> 0.012 *<sup>n</sup>*

sign(*u*) ·

2.0 ; *Ct*<sup>1</sup> <sup>=</sup> 0.008 *<sup>L</sup>*<sup>2</sup>

The *Aries*-precursor's low-level control encompasses three separate, distinctly designed

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

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

*<sup>α</sup>u*(*t*)|*u*(*t*)<sup>|</sup> <sup>+</sup> *<sup>u</sup>*˙ *<sup>c</sup>*(*t*) <sup>−</sup> *<sup>η</sup><sup>u</sup>* tanh *<sup>u</sup>*˜(*t*)

; *<sup>u</sup>*<sup>0</sup> <sup>=</sup> 1.832 <sup>m</sup>

; *Cd* = 0.0034

s

*xG*(*v*˙ + *ur* − *wp*)

; *Cd*<sup>0</sup> = 0.00385 (38)

2.0 (40)

*φu*

s

(41)

(42)

 *xnose xtail Cdy* ·

*Nv*˙ *c*˙ + *Npup*

*u*2*δ<sup>r</sup>* − *ρ* 2

*x dx* + (*xGW* − *xBB*) cos *ϑ* sin *ϕ* + (*yGW* − *yBB*) ·

<sup>√</sup>*Ct*<sup>1</sup> <sup>+</sup> <sup>1</sup> <sup>−</sup> <sup>1</sup> (39)

+ *ρ* 2 *L*<sup>4</sup>

(*<sup>v</sup>* + *xr*)<sup>2</sup> + (*<sup>w</sup>* − *xq*)<sup>2</sup> (37)

*Nvuv* + *Nvwvw* + *Nδ<sup>r</sup>*

*u*

<sup>2</sup> *<sup>L</sup>*3*u*2*Nprop* (36)

<sup>√</sup>*Ct* <sup>+</sup> <sup>1</sup> <sup>−</sup> <sup>1</sup>

#### **4.1.2 Sway**

$$\begin{aligned} &m\left[\dot{\upsilon} + \iota r - wp + \mathbf{x}\_{\mathcal{G}}(pq + \dot{r}) - y\_{\mathcal{G}}(p^2 + r^2) + z\_{\mathcal{G}}(qr - \dot{p})\right] = \frac{\rho}{2} L^4 \left[Y\_{\dot{p}}\dot{p} + Y\_{\dot{r}}\dot{r} + Y\_{pq}pq\right] \\ &+ Y\_{\Psi}q\eta r\Big] + \frac{\rho}{2} L^3 \left[Y\_{\dot{v}}\dot{v} + Y\_{\Psi}\mu p + Y\_{\mathbf{r}}\iota r + Y\_{\mathbf{v}q}vq + Y\_{\mathbf{v}p}wp\right] + \frac{\rho}{2} L^2 \left[Y\_{\mathbf{v}}\mu\upsilon + Y\_{\mathbf{v}w}vw\right] \\ &+ Y\_{\delta\_r}\mu^2 \delta\_r\Big] - \frac{\rho}{2} \int\_{x\_{\text{max}}}^{x\_{\text{tail}}} \left[\mathcal{C}\_{dy}h(\mathbf{x})(\upsilon + \mathbf{x}r)^2 + \mathcal{C}\_{dz}b(\mathbf{x})(w - \mathbf{x}q)^2\right] \frac{\upsilon + \mathbf{x}r}{\mathcal{U}\_{cf}(\mathbf{x})}d\mathbf{x} + (W - B)\cos\theta\sin q\tau \end{aligned} \tag{32}$$

#### **4.1.3 Heave**

$$\begin{aligned} &m\left[\dot{w} - u\eta + vp + x\_G(pr - \dot{q}) + y\_G(qr + \dot{p}) - z\_G(p^2 + q^2)\right] = \frac{\rho}{2}\mathcal{L}^4 \left[Z\_{\dot{q}}\dot{q} + Z\_{pp}p^2 + Z\_{pr}pr + \dot{p}\right] \\ &+ Z\_{rr}r^2\Big] + \frac{\rho}{2}\mathcal{L}^3 \left[Z\_{\dot{w}}\dot{q} + Z\_{\dot{q}}u\eta + Z\_{vp}vp + Z\_{vv}vr\right] + \frac{\rho}{2}\mathcal{L}^2 \left[Z\_{\omega u}uw + Z\_{v\beta}v^2 + u^2\left(Z\_{\delta\_l}\delta\_s + Z\_{\delta\_l/2}\delta\_{\delta\_l}\right)\right] \\ &+ Z\_{\delta\_l/2}\mathcal{L}\delta\_{lp}\Big{)} + \frac{\rho}{2}\int\_{x\_{\delta dl}}^{x\_{\delta ms}} \left[\mathcal{L}\_{dy}h(\mathbf{x})(v+x\mathbf{r})^2 + \mathcal{L}\_{dx}b(\mathbf{x})(w-\mathbf{x}q)^2\right] \frac{w-\mathbf{x}q}{\mathcal{U}\_l(\mathbf{x})}d\mathbf{x} \\ &+ (W-B)\cos\theta\cos\rho + \frac{\rho}{2}\mathcal{L}^3 \mathcal{Z}\_{qul}u\eta\boldsymbol{\epsilon}(\mathbf{n}) + \frac{\rho}{2}\mathcal{L}^2 \left[Z\_{\omega u}uw + Z\_{\delta\_l u}u\delta\_s\right] \boldsymbol{\epsilon}(\mathbf{n}) \end{aligned} \tag{33}$$

#### **4.1.4 Roll**

$$\begin{aligned} &I\_y \dot{q} + (I\_x - I\_z)pr - I\_{xy}(qr + \dot{p}) + I\_{yz}(pq - \dot{r}) + I\_{xz}(p^2 - r^2) + m \left[ \mathbf{x}\_G(\dot{w} - uq + vp) \right. \\ &+ I\_z - (\dot{v} + ur - wp) \left] + \frac{\rho}{2} \mathbf{L}^5 \left[ \mathbf{K}\_{\dot{p}} \dot{p} + \mathbf{K}\_{\dot{r}} \dot{r} + \mathbf{K}\_{pq}pq + \mathbf{K}\_{qr}qr \right] + \frac{\rho}{2} \mathbf{L}^4 \left[ \mathbf{K}\_{\dot{v}\dot{r}} \dot{v} + \mathbf{K}\_p \dot{u}p + \mathbf{K}\_r \dot{u}r \right. \\ &+ \mathbf{K}\_{\dot{v}q} vq + \mathbf{K}\_{wp} wp + \mathbf{K}\_{wr} vwr \right] + \frac{\rho}{2} \mathbf{L}^3 \left[ \mathbf{K}\_{\dot{v}} uw + \mathbf{K}\_{\overline{v}w} vw + u^2 \left( \mathbf{K}\_{\delta\_0/2} \delta\_{bp} + \mathbf{K}\_{\delta\_0/2} \delta\_{bs} \right) \right] \\ &+ (y\_G W - y\_B B) \cos \theta \cos \varphi - (z\_G W - z\_B B) \cos \theta \sin \varphi + \frac{\rho}{2} \mathbf{L}^4 \mathbf{K}\_{p\underline{n}} up \varepsilon(n) \\ &+ \frac{\rho}{2} \mathbf{L}^3 u^2 \mathbf{K}\_{prop} \end{aligned} \tag{34}$$

#### **4.1.5 Pitch**

$$\begin{split} &I\_{1}\dot{p}+(I\_{2}-I\_{3})qr+I\_{3}(pr-\dot{q})-I\_{yz}(q^{2}-r^{2})-I\_{xz}(pq+\dot{r})+m\left[y\_{G}(\dot{w}-uq+vp)\right.\\ & \left. -z\_{G}(\dot{u}-vr+wq)\right]+\frac{\rho}{2}L^{5}\left[M\_{\dot{q}}\dot{q}+M\_{pp}p^{2}+M\_{pr}pr+M\_{rr}r^{2}\right]+\frac{\rho}{2}L^{4}\left[M\_{\ddot{w}}\dot{w}+M\_{\dot{q}}uq\right.\\ & \left. +M\_{\upsilon p}vp+M\_{\upsilon p}vr\right]+\frac{\rho}{2}L^{3}\left[M\_{\dot{u}w}uv+M\_{\upsilon v}v^{2}+u^{2}\left(M\_{\delta\_{s}\delta}\delta\_{s}+M\_{\delta\_{b}\delta}\delta\_{bs}+M\_{\delta\_{b}/2}\delta\_{bp}\right)\right] \\ & -\frac{\rho}{2}\int\_{x\_{\rm{nl}}}^{x\_{\rm{nuc}}}\left[C\_{dy}h(\mathbf{x})(v+\mathbf{x}r)^{2}+C\_{dz}b(\mathbf{x})(w-\mathbf{x}q)^{2}\right]\frac{w+\mathbf{x}q}{L\_{\upsilon f}(\mathbf{x})}d\mathbf{x}-(\mathbf{x}\_{\rm{G}}W-\mathbf{x}\_{\rm{B}}B)\cdot\\ & \left. \cos\theta\cos\phi\,-(\mathbf{z}\_{\rm{G}}W-\mathbf{z}\_{\rm{B}}B)\sin\theta+\frac{\rho}{2}L^{4}M\_{\dot{q}\dot{q}}\eta\boldsymbol{n\epsilon}(n)+\frac{\rho}{2}L^{3}\Big[M\_{\dot{u}\dot{m}}uw \\ & +M\_{\delta\_{s}\boldsymbol{u}}u^{2}\delta\_{s}\right]\boldsymbol{\varepsilon}(n) \end{split} \tag{35}$$

#### **4.1.6 Yaw**

14 Will-be-set-by-IN-TECH

*Yv*˙*v*˙ + *Ypup* + *Yrur* + *Yvqvq* + *Ywpwp* + *Ywrwr*

 <sup>=</sup> *<sup>ρ</sup>* <sup>2</sup> *<sup>L</sup>*<sup>4</sup>

 <sup>=</sup> *<sup>ρ</sup>* 2 *L*<sup>4</sup>

*Zwnuw* + *Zδsnuδ<sup>s</sup>*

<sup>2</sup> <sup>+</sup> *Cdzb*(*x*)(*<sup>w</sup>* <sup>−</sup> *xq*)<sup>2</sup> *<sup>v</sup>* <sup>+</sup> *xr*

+ *ρ* 2 *L*2 

*Cdyh*(*x*)(*<sup>v</sup>* <sup>+</sup> *xr*)<sup>2</sup> <sup>+</sup> *Cdxb*(*x*)(*<sup>w</sup>* <sup>−</sup> *xq*)

2 *L*<sup>2</sup>

*Kp*˙ *p*˙ + *Kr*˙*r*˙ + *Kpqpq* + *Kqrqr*

*Kvuv* + *Kvwvw* + *u*<sup>2</sup>

<sup>2</sup> *<sup>L</sup>*3*u*2*Kprop* (34)

*Mq*˙*q*˙ + *Mpp p*<sup>2</sup> + *Mpr pr* + *Mrrr*<sup>2</sup>

<sup>2</sup> <sup>+</sup> *Cdzb*(*x*)(*<sup>w</sup>* <sup>−</sup> *xq*)<sup>2</sup> *<sup>w</sup>* <sup>+</sup> *xq*

<sup>2</sup> *<sup>L</sup>*4*Mqnqn�*(*n*) + *<sup>ρ</sup>*

 *Mδ<sup>s</sup>*

*�*(*n*) (35)

*Uc f*(*x*)

2 *L*<sup>3</sup>

*Muwuw* + *Mvvv*<sup>2</sup> + *u*<sup>2</sup>

+ *ρ* 2 *L*<sup>2</sup>

*Uc f*(*x*)

*Zwuw* + *Zvvv*<sup>2</sup> + *u*2

 *w* − *xq Uc f*(*x*)

2

+ *ρ* 2 *L*<sup>4</sup>

2

*Yp*˙ *p*˙ + *Yr*˙*r*˙ + *Ypqpq*

*Yvuv* + *Yvwvw*

*Zq*˙*q*˙ + *Zpp p*<sup>2</sup> + *Zpr pr*

*Zδ<sup>s</sup>*

*�*(*n*) (33)

*xG*(*w*˙ − *uq* + *vp*)

*Kv*˙*v*˙ + *Kpup* + *Krur*

*yG*(*w*˙ − *uq* + *vp*)

*Mw*˙ *w*˙ + *Mquq*

*Kδb*/2*δbp* + *Kδb*/2*δbs*

*L*4*Kpnup�*(*n*)

*δ<sup>s</sup>* + *Mδb*/2*δbs* + *Mδb*/2*δbp*

*x dx* − (*xGW* − *xBB*) ·

+ *ρ* <sup>2</sup> *<sup>L</sup>*<sup>4</sup>

*Mwnuw*

*dx*

*δ<sup>s</sup>* + *Zδb*/2*δbs*

*dx* + (*W* − *B*) cos *ϑ* sin *ϕ*

(32)

*<sup>v</sup>*˙ <sup>+</sup> *ur* <sup>−</sup> *wp* <sup>+</sup> *xG*(*pq* <sup>+</sup> *<sup>r</sup>*˙) <sup>−</sup> *yG*(*p*<sup>2</sup> <sup>+</sup> *<sup>r</sup>*2) + *zG*(*qr* <sup>−</sup> *<sup>p</sup>*˙)

*Cdyh*(*x*)(*v* + *xr*)

*<sup>w</sup>*˙ <sup>−</sup> *uq* <sup>+</sup> *vp* <sup>+</sup> *xG*(*pr* <sup>−</sup> *<sup>q</sup>*˙) + *yG*(*qr* <sup>+</sup> *<sup>p</sup>*˙) <sup>−</sup> *zG*(*p*<sup>2</sup> <sup>+</sup> *<sup>q</sup>*2)

*Zw*˙ *q*˙ + *Zquq* + *Zvpvp* + *Zvrvr*

<sup>2</sup> *<sup>L</sup>*3*Zqnuq�*(*n*) + *<sup>ρ</sup>*

*Iyq*˙ + (*Ix* <sup>−</sup> *Iz*)*pr* <sup>−</sup> *Ixy*(*qr* <sup>+</sup> *<sup>p</sup>*˙) + *Iyz*(*pq* <sup>−</sup> *<sup>r</sup>*˙) + *Ixz*(*p*<sup>2</sup> <sup>−</sup> *<sup>r</sup>*2) + *<sup>m</sup>*

+ *ρ* 2 *L*3 

*Ix <sup>p</sup>*˙ + (*Iz* <sup>−</sup> *Iy*)*qr* <sup>+</sup> *Ixy*(*pr* <sup>−</sup> *<sup>q</sup>*˙) <sup>−</sup> *Iyz*(*q*<sup>2</sup> <sup>−</sup> *<sup>r</sup>*2) <sup>−</sup> *Ixz*(*pq* <sup>+</sup> *<sup>r</sup>*˙) + *<sup>m</sup>*

+(*yGW* <sup>−</sup> *yBB*) cos *<sup>ϑ</sup>* cos *<sup>ϕ</sup>* <sup>−</sup> (*zGW* <sup>−</sup> *zBB*) cos *<sup>ϑ</sup>* sin *<sup>ϕ</sup>* <sup>+</sup> *<sup>ρ</sup>*

**4.1.2 Sway**

+*Yqrqr*

**4.1.3 Heave**

+*Zrrr*<sup>2</sup>

+*Zδb*/2*δbp*

**4.1.4 Roll**

+ *ρ*

− *ρ* 2

**4.1.5 Pitch**

+ *ρ* <sup>2</sup> *<sup>L</sup>*<sup>3</sup>

> + *ρ* 2

+(*<sup>W</sup>* <sup>−</sup> *<sup>B</sup>*) cos *<sup>ϑ</sup>* cos *<sup>ϕ</sup>* <sup>+</sup> *<sup>ρ</sup>*

−*zG*(*v*˙ + *ur* − *wp*)

−*zG*(*u*˙ − *vr* + *wq*)

+*Mvpvp* + *Mvrvr*

 *xnose xtail* 

+*Mδsnu*<sup>2</sup>*δ<sup>s</sup>*

+*Kvqvq* + *Kwpwp* + *Kwrwr*

 *xnose xtail*

 + *ρ* <sup>2</sup> *<sup>L</sup>*<sup>5</sup>

 + *ρ* <sup>2</sup> *<sup>L</sup>*<sup>5</sup>

*Cdyh*(*x*)(*v* + *xr*)

· cos *<sup>ϑ</sup>* cos *<sup>ϕ</sup>* <sup>−</sup> (*zGW* <sup>−</sup> *zBB*) sin *<sup>ϑ</sup>* <sup>+</sup> *<sup>ρ</sup>*

+ *ρ* 2 *L*3 

*m*

+*Yδ<sup>r</sup> u*2*δ<sup>r</sup>* − *ρ* 2

+ *ρ* 2 *L*<sup>3</sup>

> *xtail xnose*

*m* 

*Izr*˙ + (*Iy* <sup>−</sup> *Ix*)*pq* <sup>−</sup> *Ixy*(*p*<sup>2</sup> <sup>−</sup> *<sup>q</sup>*2) <sup>−</sup> *Iyz*(*pr* <sup>+</sup> *<sup>q</sup>*˙) + *Ixz*(*qr* <sup>−</sup> *<sup>p</sup>*˙) + *<sup>m</sup> xG*(*v*˙ + *ur* − *wp*) −*yG*(*u*˙ − *vr* + *wq*) + *ρ* 2 *L*<sup>5</sup> *Np*˙ *p*˙ + *Nr*˙*r*˙ + *Npqpq* + *Nqrqr* + *ρ* 2 *L*<sup>4</sup> *Nv*˙ *c*˙ + *Npup* +*Nrur* + *Nvqvq* + *Nwpwp* + *Nwrwr* + *ρ* 2 *L*<sup>3</sup> *Nvuv* + *Nvwvw* + *Nδ<sup>r</sup> u*2*δ<sup>r</sup>* − *ρ* 2 *xnose xtail Cdy* · ·*h*(*x*)(*<sup>v</sup>* <sup>+</sup> *xr*)<sup>2</sup> <sup>+</sup> *Cdzb*(*x*)(*<sup>w</sup>* <sup>−</sup> *xq*) <sup>2</sup> *<sup>w</sup>* + *xq Uc f*(*x*) *x dx* + (*xGW* − *xBB*) cos *ϑ* sin *ϕ* + (*yGW* − *yBB*) · · sin *<sup>ϑ</sup>* <sup>+</sup> *<sup>ρ</sup>* 2 *L*3*u*2*Nprop* (36)

#### **4.1.7 Substitution terms**

$$\mathcal{U}\_{\varepsilon f}(\mathbf{x}) = \sqrt{(v + \mathbf{x}r)^2 + (w - \mathbf{x}q)^2} \tag{37}$$

$$X\_{prop} = \mathbb{C}\_{d0}(\eta \, |\, \eta \, | \, -1); \quad \underbrace{\eta = 0.012}\_{} \underbrace{\frac{n}{u}}\_{}; \quad \mathbb{C}\_{d0} = 0.00385\tag{38}$$

$$\varepsilon(n) = -1 + \frac{\text{sign}(n)}{\text{sign}(u)} \cdot \frac{\sqrt{\mathbb{C}\_{l} + 1} - 1}{\sqrt{\mathbb{C}\_{l1} + 1} - 1} \tag{39}$$

$$\mathbf{C}\_{t} = 0.008 \frac{L^{2} \eta \, |\eta|}{2.0}; \quad \mathbf{C}\_{t1} = 0.008 \frac{L^{2}}{2.0} \tag{40}$$

#### **4.2 Control**

The *Aries*-precursor's low-level control encompasses three separate, distinctly designed decoupled control loops:


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.

#### **4.2.1 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:

$$|n(t)|n(t)| = (a\beta)^{-1} \left[ a u(t)|u(t)| + \dot{u}\_c(t) - \eta\_u \tanh \frac{\tilde{u}(t)}{\Phi\_u} \right] \tag{41}$$

$$\alpha = \frac{\rho L^2 \mathbb{C}\_d}{2m + \rho L^3 X\_{\text{il}}}; \quad \mathbb{C}\_d = 0.0034$$

$$\beta = \frac{n\_0}{\mu\_0}; \quad n\_0 = 52.359 \,\frac{\text{rad}}{\text{s}}; \quad \mu\_0 = 1.832 \,\frac{\text{m}}{\text{s}}\tag{42}$$

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

Formation Guidance of AUVs Using Decentralized Control Functions 115

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

*<sup>δ</sup>s*(*t*) = <sup>−</sup>5.143*q*(*t*) + 1.070*ϑ*(*t*) + 4 tanh *<sup>σ</sup>z*(*t*)

<sup>+</sup>4 tanh *<sup>q</sup>*˜(*t*) + 0.520*ϑ*˜(*t*) <sup>−</sup> 0.011*z*˜(*t*)

= −5.143*q*(*t*) + 1.070*ϑ*(*t*)

**5. Obstacle classification, state estimation and conditioning the control signals** In this section, the issues of obstacle classification will be addressed, giving the expressions for (*di*, *ni*) of every type of obstacle considered, which are functions prerequisite to obtaining *Pi*-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** =

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

unfeasible commands which can cause saturation in the actuators and temporary break-down

The problem of classification in a 2D waterspace represented by **R**<sup>2</sup> is a well studied topic. We have adopted an approach based on modeling real-world features after a sparse set of

In the ensuing expressions, {*xint*} will be used for the closed, connected set comprising the interior of the obstacle being described. T*<sup>i</sup>* 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

Circles are the simplest convex obstacles to formulate mathematically. The distance and normal vector to a circle defined by (*x<sup>i</sup>* <sup>∈</sup> **<sup>R</sup>**2, *ri* <sup>∈</sup> **<sup>R</sup>**+), its center and radius respectively,

conditioning adjusts the values in the low-level command vector **c** = [*ac uc rc ψ<sup>c</sup>* ]

*<sup>σ</sup>z*(*t*) = *<sup>q</sup>*˜(*t*) + 0.520*ϑ*˜(*t*) <sup>−</sup> 0.011*z*˜(*t*) (44)

<sup>T</sup> will be explored. Realistic plant and measurement noise (**n**˜, ˜**y**), which

0.4

0.4 (45)

<sup>T</sup> to prevent

detailed discussion of this and similar sliding mode controllers.

surface is designed in (44), and the controller in (45).

**4.2.3 Pitch and depth**

[*u*ˆ *v*ˆ *w*ˆ *p*ˆ *q*ˆ*r*ˆ *x*ˆ *y*ˆ *z*ˆ *ϕ*ˆ *ϑ*ˆ *ψ*ˆ ]

**5.1 Obstacle classification**

geometrical primitives – circles, rectangles and ellipses.

of feedback.

*ψ<sup>i</sup>* if applicable.

**5.1.1 Circles**


Table 1. 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 (*u*˙ *<sup>c</sup>*(*t*)), overcomes the linear drag (*u*(*t*)|*u*(*t*)|), and attenuates the perturbations due to disturbances and process noise (*σ*˙ *<sup>u</sup>*(*t*)).

#### **4.2.2 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).

$$
\sigma\_r = -0.074\tilde{v}(t) + 0.816\tilde{v}(t) + 0.573\tilde{\phi}(t) \tag{43}
$$

$$
\delta\_r = 0.033v(t) + 0.1112r(t) + 2.58\tanh\frac{0.074\tilde{v}(r) + 0.816\tilde{v}(t) + 0.573\tilde{\phi}(t)}{0.1}
$$

It should be noted that *v*˜(*r*) seems to imply the possibility of defining some *vc* (*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.

#### **4.2.3 Pitch and depth**

16 Will-be-set-by-IN-TECH

*W* = 53.4 kN *B* = 53.4 kN *L* = 5.3 m *Ix* = 13587 Nms2 *Ixy* <sup>=</sup> <sup>−</sup>13.58 Nms2 *Iyz* <sup>=</sup> <sup>−</sup>13.58 Nms2 *Ixz* <sup>=</sup> <sup>−</sup>13.58 Nms2 *Iy* <sup>=</sup> 13587 Nms2 *Ix* = 2038 Nms2 *xG* = 0.0 m *xB* = 0.0 m *yG* = 0.0 m *yB* = 0.0 m *zG* = 0.061 m *zB* = 0.0 m *g* = 9.81m/s2

*Xp p* <sup>=</sup> 7.0 · <sup>10</sup>−<sup>3</sup> *Xqq* <sup>=</sup> <sup>−</sup>1.5 · <sup>10</sup>−<sup>2</sup> *Xrr* <sup>=</sup> 4.0 · <sup>10</sup>−<sup>3</sup> *Xpr* <sup>=</sup> 7.5 · <sup>10</sup>−<sup>4</sup> *Xu*˙ <sup>=</sup> <sup>−</sup>7.6 · <sup>10</sup>−<sup>3</sup> *Xwq* <sup>=</sup> <sup>−</sup>2.0 · <sup>10</sup>−<sup>1</sup> *Xv p* <sup>=</sup> <sup>−</sup>3.0 · <sup>10</sup>−<sup>3</sup> *Xvr* <sup>=</sup> 2.0 · <sup>10</sup>−<sup>2</sup> *Xq<sup>δ</sup><sup>s</sup>* <sup>=</sup> 2.5 · <sup>10</sup>−<sup>2</sup> *Xq<sup>δ</sup>b*/2 <sup>=</sup> <sup>−</sup>1.3 · <sup>10</sup>−<sup>3</sup> *Xr<sup>δ</sup><sup>r</sup>* <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>3</sup> *Xvv* <sup>=</sup> 5.3 · <sup>10</sup>−<sup>2</sup> *Xww* <sup>=</sup> 1.7 · <sup>10</sup>−<sup>1</sup> *Xv<sup>δ</sup><sup>r</sup>* <sup>=</sup> 1.7 · <sup>10</sup>−<sup>3</sup> *Xw<sup>δ</sup><sup>s</sup>* <sup>=</sup> 4.6 · <sup>10</sup>−<sup>2</sup> *Xw<sup>δ</sup>b*/2 <sup>=</sup> 0.5 · <sup>10</sup>−<sup>2</sup> *<sup>X</sup>δsδ<sup>s</sup>* <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>2</sup> *<sup>X</sup>δbδb*/2 <sup>=</sup> <sup>−</sup>4.0 · <sup>10</sup>−<sup>3</sup> *<sup>X</sup>δrδ<sup>r</sup>* <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>2</sup> *Xqδsn* <sup>=</sup> 2.0 · <sup>10</sup>−<sup>3</sup>

*Yp*˙ <sup>=</sup> 1.2 · <sup>10</sup>−<sup>4</sup> *Yr*˙ <sup>=</sup> 1.2 · <sup>10</sup>−<sup>3</sup> *Ypq* <sup>=</sup> 4.0 · <sup>10</sup>−<sup>3</sup> *Yqr* <sup>=</sup> <sup>−</sup>6.5 · <sup>10</sup>−<sup>3</sup> *Yv*˙ <sup>=</sup> <sup>−</sup>5.5 · <sup>10</sup>−<sup>2</sup> *Yp* <sup>=</sup> 3.0 · <sup>10</sup>−<sup>3</sup> *Yr* <sup>=</sup> 3.0 · <sup>10</sup>−<sup>2</sup> *Yvq* <sup>=</sup> 2.4 · <sup>10</sup>−<sup>2</sup> *Yw p* <sup>=</sup> 2.3 · <sup>10</sup>−<sup>1</sup> *Ywr* <sup>=</sup> <sup>−</sup>1.9 · <sup>10</sup>−<sup>2</sup> *Yv* <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>1</sup> *Yvw* <sup>=</sup> 6.8 · <sup>10</sup>−<sup>2</sup>

*Zq*˙ <sup>=</sup> <sup>−</sup>6.8 · <sup>10</sup>−<sup>3</sup> *Zp p* <sup>=</sup> 1.3 · <sup>10</sup>−<sup>4</sup> *Zpr* <sup>=</sup> 6.7 · <sup>10</sup>−<sup>3</sup> *Zrr* <sup>=</sup> <sup>−</sup>7.4 · <sup>10</sup>−<sup>3</sup> *Zw*˙ <sup>=</sup> <sup>−</sup>2.4 · <sup>10</sup>−<sup>1</sup> *Zq* <sup>=</sup> <sup>−</sup>1.4 · <sup>10</sup>−<sup>1</sup> *Zv p* <sup>=</sup> <sup>−</sup>4.8 · <sup>10</sup>−<sup>2</sup> *Zvr* <sup>=</sup> 4.5 · <sup>10</sup>−<sup>2</sup> *Zw* <sup>=</sup> <sup>−</sup>3.0 · <sup>10</sup>−<sup>1</sup> *Zvv* <sup>=</sup> <sup>−</sup>6.8 · <sup>10</sup>−<sup>2</sup> *<sup>Z</sup>δ<sup>s</sup>* <sup>=</sup> <sup>−</sup>7.3 · <sup>10</sup>−<sup>2</sup> *<sup>Z</sup>δb*/2 <sup>=</sup> <sup>−</sup>1.3 · <sup>10</sup>−<sup>2</sup>

*Kp*˙ <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>3</sup> *Kr*˙ <sup>=</sup> <sup>−</sup>3.4 · <sup>10</sup>−<sup>5</sup> *Kpq* <sup>=</sup> <sup>−</sup>6.9 · <sup>10</sup>−<sup>5</sup> *Kqr* <sup>=</sup> 1.7 · <sup>10</sup>−<sup>2</sup> *Kv*˙ <sup>=</sup> 1.3 · <sup>10</sup>−<sup>4</sup> *Kp* <sup>=</sup> <sup>−</sup>1.1 · <sup>10</sup>−<sup>2</sup> *Kr* <sup>=</sup> <sup>−</sup>8.4 · <sup>10</sup>−<sup>4</sup> *Kvq* <sup>=</sup> <sup>−</sup>5.1 · <sup>10</sup>−<sup>3</sup> *Kw p* <sup>=</sup> <sup>−</sup>1.3 · <sup>10</sup>−<sup>4</sup> *Kwr* <sup>=</sup> 1.4 · <sup>10</sup>−<sup>2</sup> *Kv* <sup>=</sup> 3.1 · <sup>10</sup>−<sup>3</sup> *Kvw* <sup>=</sup> <sup>−</sup>1.9 · <sup>10</sup>−<sup>1</sup>

*Mq*˙ <sup>=</sup> <sup>−</sup>1.7 · <sup>10</sup>−<sup>2</sup> *Mp p* <sup>=</sup> 5.3 · <sup>10</sup>−<sup>5</sup> *Mpr* <sup>=</sup> 5.0 · <sup>10</sup>−<sup>3</sup> *Mrr* <sup>=</sup> 2.9 · <sup>10</sup>−<sup>3</sup> *Mw*˙ <sup>=</sup> <sup>−</sup>6.8 · <sup>10</sup>−<sup>2</sup> *Muq* <sup>=</sup> <sup>−</sup>6.8 · <sup>10</sup>−<sup>2</sup> *Mv p* <sup>=</sup> 1.2 · <sup>10</sup>−<sup>3</sup> *Mvr* <sup>=</sup> 1.7 · <sup>10</sup>−<sup>2</sup> *Muw* <sup>=</sup> 1.0 · <sup>10</sup>−<sup>1</sup> *Mvv* <sup>=</sup> <sup>−</sup>2.6 · <sup>10</sup>−<sup>2</sup> *<sup>M</sup>δ<sup>s</sup>* <sup>=</sup> <sup>−</sup>4.1 · <sup>10</sup>−<sup>2</sup> *<sup>M</sup>δb*/2 <sup>=</sup> 3.5 · <sup>10</sup>−<sup>3</sup>

*Np*˙ <sup>=</sup> <sup>−</sup>3.4 · <sup>10</sup>−<sup>5</sup> *Nr*˙ <sup>=</sup> <sup>−</sup>3.4 · <sup>10</sup>−<sup>3</sup> *Npq* <sup>=</sup> <sup>−</sup>2.1 · <sup>10</sup>−<sup>2</sup> *Nqr* <sup>=</sup> 2.7 · <sup>10</sup>−<sup>3</sup> *Nv*˙ <sup>=</sup> 1.2 · <sup>10</sup>−<sup>3</sup> *Np* <sup>=</sup> <sup>−</sup>8.4 · <sup>10</sup>−<sup>4</sup> *Nr* <sup>=</sup> <sup>−</sup>1.6 · <sup>10</sup>−<sup>2</sup> *Nvq* <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>2</sup> *Nw p* <sup>=</sup> <sup>−</sup>1.7 · <sup>10</sup>−<sup>2</sup> *Nwr* <sup>=</sup> 7.4 · <sup>10</sup>−<sup>3</sup> *Nv* <sup>=</sup> <sup>−</sup>7.4 · <sup>10</sup>−<sup>3</sup> *Nvw* <sup>=</sup> <sup>−</sup>2.7 · <sup>10</sup>−<sup>2</sup>

It is apparent from the above that the propeller rate of revolution command comprises a term that accelerates the vehicle in the desired measure (*u*˙ *<sup>c</sup>*(*t*)), overcomes the linear drag (*u*(*t*)|*u*(*t*)|), and attenuates the perturbations due to disturbances and process noise (*σ*˙ *<sup>u</sup>*(*t*)).

The sliding surface for the subset of states governing the vehicle's heading is given below, in

*<sup>δ</sup><sup>r</sup>* <sup>=</sup> 0.033*v*(*t*) + 0.1112*r*(*t*) + 2.58 tanh 0.074*v*˜(*r*) + 0.816*r*˜(*t*) + 0.573*ϕ*˜(*t*)

It should be noted that *v*˜(*r*) seems to imply the possibility of defining some *vc* (*t*) for the vehicle to track. This is impractical. The *Aries*-precursor's thrust allocation and kinematics,

*σ<sup>r</sup>* = −0.074*v*˜(*t*) + 0.816*r*˜(*t*) + 0.573*ϕ*˜(*t*) (43)

0.1

*Zqn* <sup>=</sup> <sup>−</sup>2.9 · <sup>10</sup>−<sup>3</sup> *Zwn* <sup>=</sup> <sup>−</sup>5.1 · <sup>10</sup>−<sup>3</sup> *<sup>Z</sup>δsn* <sup>=</sup> <sup>−</sup>1.0 · <sup>10</sup>−<sup>2</sup>

*Mqn* <sup>=</sup> <sup>−</sup>1.6 · <sup>10</sup>−<sup>3</sup> *Mwn* <sup>=</sup> <sup>−</sup>2.9 · <sup>10</sup>−<sup>3</sup> *<sup>M</sup>δsn* <sup>=</sup> <sup>−</sup>5.2 · <sup>10</sup>−<sup>3</sup>

*<sup>K</sup>δb*/2 <sup>=</sup> 0.0 *Kpn* <sup>=</sup> <sup>−</sup>5.7 · <sup>10</sup>−<sup>4</sup> *Kprop* <sup>=</sup> 0.0

*<sup>N</sup>δ<sup>r</sup>* <sup>=</sup> <sup>−</sup>1.3 · <sup>10</sup>−<sup>2</sup> *Nprop* <sup>=</sup> 0.0

(43). The resulting sliding mode controller is contained in (44).

Table 1. Parameters of the Model Dynamics

**4.2.2 Heading**

*ρ* = 1000.0 kg/m<sup>2</sup> *m* = 5454.54 kg

*Xwδsn* <sup>=</sup> 3.5 · <sup>10</sup>−<sup>3</sup> *<sup>X</sup>δsδsn* <sup>=</sup> <sup>−</sup>1.6 · <sup>10</sup>−<sup>3</sup>

*<sup>Y</sup>δ<sup>r</sup>* <sup>=</sup> 2.7 · <sup>10</sup>−<sup>2</sup>

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).

$$
\sigma\_z(t) = \ddot{q}(t) + 0.520\dot{\theta}(t) - 0.011\ddot{z}(t) \tag{44}
$$

$$
\begin{aligned}
\delta\_s(t) &= -5.143q(t) + 1.070\theta(t) + 4\tanh\frac{\sigma\_z(t)}{0.4} \\ &= -5.143q(t) + 1.070\theta(t) \\ &+ 4\tanh\frac{\ddot{\theta}(t) + 0.520\dot{\theta}(t) - 0.011\ddot{z}(t)}{0.4} \end{aligned}
$$

#### **5. Obstacle classification, state estimation and conditioning the control signals**

In this section, the issues of obstacle classification will be addressed, giving the expressions for (*di*, *ni*) of every type of obstacle considered, which are functions prerequisite to obtaining *Pi*-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** = [*u*ˆ *v*ˆ *w*ˆ *p*ˆ *q*ˆ*r*ˆ *x*ˆ *y*ˆ *z*ˆ *ϕ*ˆ *ϑ*ˆ *ψ*ˆ ] <sup>T</sup> will be explored. Realistic plant and measurement noise (**n**˜, ˜**y**), 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** = [*ac uc rc ψ<sup>c</sup>* ] <sup>T</sup> to prevent unfeasible commands which can cause saturation in the actuators and temporary break-down of feedback.

#### **5.1 Obstacle classification**

The problem of classification in a 2D waterspace represented by **R**<sup>2</sup> 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, {*xint*} will be used for the closed, connected set comprising the interior of the obstacle being described. T*<sup>i</sup>* 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 *ψ<sup>i</sup>* if applicable.

#### **5.1.1 Circles**

Circles are the simplest convex obstacles to formulate mathematically. The distance and normal vector to a circle defined by (*x<sup>i</sup>* <sup>∈</sup> **<sup>R</sup>**2, *ri* <sup>∈</sup> **<sup>R</sup>**+), its center and radius respectively,

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

Formation Guidance of AUVs Using Decentralized Control Functions 117

The equation of the ellipse with the center in the origin and axes aligned with the axes of the

*xe ye* T

Where *τ* ∈ **R** is an independent parameter, the degree of freedom along the line and *k* is the

<sup>T</sup> = *txe a*2 *tye b*2 T

<sup>T</sup> =

<sup>2</sup> + *by t* + *b*<sup>2</sup>

The greatest root of (57), *t*, allows for the calculation of (*di*(*x*), *ni*(*x*)) in (51, 54), as given

*a* 0 *b* 0 <sup>T</sup> <sup>−</sup><sup>1</sup> *<sup>i</sup>* (*x*) < 1 

*<sup>e</sup>* + (*t* + *a*2)2*b*4*y*<sup>2</sup>

 *<sup>a</sup>*<sup>2</sup> *<sup>x</sup> t*+*a*<sup>2</sup>

*b*2*y t*+*b*<sup>2</sup> T

<sup>2</sup>

<sup>2</sup>*a*2*x*<sup>2</sup> + (*t* + *a*2)2*b*2*y*<sup>2</sup> = (*t* + *a*2)

*b*<sup>2</sup>*ye t*+*b*<sup>2</sup> T 

*e*

<sup>2</sup> + (*<sup>t</sup>* <sup>+</sup> *<sup>a</sup>*2)2*b*<sup>4</sup> [ˆ*j*T*i*(*x*))]<sup>2</sup>

(*<sup>t</sup>* <sup>+</sup> *<sup>a</sup>*2)2(*<sup>t</sup>* <sup>+</sup> *<sup>b</sup>*2)<sup>2</sup> (58)

(*<sup>t</sup>* <sup>+</sup> *<sup>a</sup>*2)2(*<sup>t</sup>* <sup>+</sup> *<sup>b</sup>*2)<sup>2</sup> (59)

*x y*<sup>T</sup> <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> for which *<sup>x</sup>* <sup>−</sup> *<sup>x</sup><sup>e</sup>* is normal to the ellipse. The equation of such a normal

= 1 (50)

}. The analysis proceeds by considering

<sup>T</sup> (52)

= 1 (55)

<sup>2</sup>(*t* + *b*2)

(53)

(54)

<sup>2</sup> (56)

*xn*(*τ*) = *kτ* + *x<sup>e</sup>* (51)

= *x*. Then, the following manipulation can be made:

<sup>2</sup>*a*2*x*<sup>2</sup> <sup>−</sup> (*<sup>t</sup>* <sup>+</sup> *<sup>a</sup>*2)2*b*2*y*<sup>2</sup> <sup>=</sup> <sup>0</sup> (57)

<sup>→</sup> **<sup>R</sup>**<sup>+</sup>

 *x a* 2 + *y b* 2

the quartic to be solved is given below in (50 - 57).

The locus of its solutions is the ellipse, {*x<sup>e</sup>* <sup>=</sup>

direction vector of the line, given below:

It follows that if *<sup>τ</sup>* <sup>=</sup> *<sup>t</sup>* <sup>=</sup> arg *<sup>x</sup>*, i.e. *<sup>x</sup>n*(*t*) id

(*t* + *a*2)

below:

<sup>2</sup>(*t* + *b*2)

*di* : **<sup>R</sup>**<sup>2</sup> \

= *t* 

= *t* 

*di*(*x*) = �*<sup>x</sup>* <sup>−</sup> *<sup>x</sup>e*� <sup>=</sup>

*k* = ∇

 *xe a* 2 + *ye b* 2 − 1 = *xe a*2 *ye b*2

*x* − *xe y* − *ye*

 *xe ye*

 *ax t* + *a*<sup>2</sup>

(*t* + *b*2)

<sup>2</sup> <sup>−</sup> (*<sup>t</sup>* <sup>+</sup> *<sup>b</sup>*2)

*<sup>x</sup>int* : *<sup>x</sup>*TT*<sup>i</sup>*

*kt* = *t a*<sup>2</sup> *xe t*+*a*<sup>2</sup>

(*<sup>t</sup>* + *<sup>b</sup>*2)2*a*<sup>4</sup> [*ı*ˆT*i*(*x*)]

(*t* + *b*2)2*a*4*x*<sup>2</sup>

Substituting the right-hand side of (54) into (50), the quartic discussed is obtained as:

coordinate system is:

those *x* =

is:

are given below:

$$\begin{aligned} d\_{\dot{l}} &: \quad \mathbb{R}^2 \backslash \left\{ \mathbf{x}^{int} : \left\| \mathbf{x}^{int} - \mathbf{x}\_{\dot{l}} \right\| < r\_{\dot{l}} \right\} \to \mathbb{R}^+ \\ d\_{\dot{l}}(\mathbf{x}) &= \left\| \mathbf{x} - \mathbf{x}\_{\dot{l}} \right\| - r\_{\dot{l}} \in \mathbb{R}^+ \\ n\_{\dot{l}} &: \quad \mathbb{R}^2 \backslash \left\{ \mathbf{x}^{int} : \left\| \mathbf{x}^{int} - \mathbf{x}\_{\dot{l}} \right\| < r\_{\dot{l}} \right\} \to \mathbb{SO}^2 \end{aligned} \tag{46}$$

$$n\_{\dot{l}}: \quad \mathbb{R}^2 \backslash \left\{ \mathbf{x}^{\text{int}} : \left\| \mathbf{x}^{\text{int}} - \mathbf{x}\_{\dot{l}} \right\| < r\_{\dot{l}} \right\} \to \mathbb{SO}^2$$
 
$$n\_{\dot{l}}(\mathbf{x}) = \frac{\mathbf{x} - \mathbf{x}\_{\dot{l}}}{||\mathbf{x} - \mathbf{x}\_{\dot{l}}||} \tag{47}$$

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).

#### **5.1.2 Rectangles**

The functions for the distance and normal vector (*di*(*x*), *ni*(*x*)), of a point with respect to a rectangle in Euclidean 2-space defined by (*x<sup>i</sup>* <sup>∈</sup> **<sup>R</sup>**2, *ai*, *bi* <sup>∈</sup> **<sup>R</sup>**+, *<sup>ψ</sup><sup>i</sup>* <sup>∈</sup> [−*π*, *<sup>π</sup>*)), 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:

*di* : **<sup>R</sup>**<sup>2</sup> \ � *xint* : � � � � � � *ai* 0 0 *bi* �−<sup>1</sup> · T*i*(*xint*) � � � � � ∞ < 1 � <sup>→</sup> **<sup>R</sup>**<sup>+</sup> *di*(*x*) = ⎧ ⎪⎪⎪⎨ ⎪⎪⎪⎩ |*ı*ˆ · T*i*(*x*)| < *ai* : |ˆ*j* · T*i*(*x*)| − *bi* |ˆ*j* · T*i*(*x*)| < *bi* : |ˆ*ı* · T*i*(*x*)| − *ai* otherwise : � � � � |T*i*(*x*)| − � *ai* 2 *bi* 2 �T � � � � (48) *<sup>n</sup><sup>i</sup>* : **<sup>R</sup>**<sup>2</sup> \ � *xint* : � � � � � � *ai* 0 0 *bi* �−<sup>1</sup> · T*i*(*xint*) � � � � � ∞ < 1 � <sup>→</sup> **SO**<sup>2</sup> *ni*(*x*) = ⎧ ⎪⎪⎪⎨ ⎪⎪⎪⎩ <sup>|</sup>*ı*<sup>ˆ</sup> · T*i*(*x*)<sup>|</sup> <sup>&</sup>lt; *ai* <sup>2</sup> : <sup>T</sup> <sup>−</sup><sup>1</sup> *<sup>i</sup>* {sign [ˆ*j* · T*i*(*x*)] ˆ*j*} <sup>|</sup>ˆ*<sup>j</sup>* · T*i*(*x*)<sup>|</sup> <sup>&</sup>lt; *bi* <sup>2</sup> : <sup>T</sup> <sup>−</sup><sup>1</sup> *<sup>i</sup>* {sign [ˆ*ı* · T*i*(*x*)] ˆ*ı*} otherwise : <sup>T</sup> <sup>−</sup><sup>1</sup> *i* � T*i*(*x*) − � *ai* sign [*ı*ˆ · T*i*(*x*)] *bi* sign [ˆ*j* · T*i*(*x*)] �T � (49)

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).

#### **5.1.3 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 18 Will-be-set-by-IN-TECH

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.,

The functions for the distance and normal vector (*di*(*x*), *ni*(*x*)), of a point with respect to a rectangle in Euclidean 2-space defined by (*x<sup>i</sup>* <sup>∈</sup> **<sup>R</sup>**2, *ai*, *bi* <sup>∈</sup> **<sup>R</sup>**+, *<sup>ψ</sup><sup>i</sup>* <sup>∈</sup> [−*π*, *<sup>π</sup>*)), the center of the rectangle, the half-length and half-breadth and the angle of rotation of the rectangle's long

> � � � � � ∞ < 1 �

� *ai* 2 *bi* 2 �T � � � �

� � � � � ∞ < 1 �

*<sup>i</sup>* {sign [ˆ*j* · T*i*(*x*)] ˆ*j*}

*<sup>i</sup>* {sign [ˆ*ı* · T*i*(*x*)] ˆ*ı*}

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,

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

�

T*i*(*x*) −

� � � <sup>&</sup>lt; *ri* �

� � � <sup>&</sup>lt; *ri* �

*di*(*x*) = �*<sup>x</sup>* <sup>−</sup> *<sup>x</sup>i*� <sup>−</sup> *ri* <sup>∈</sup> **<sup>R</sup>**<sup>+</sup> (46)

<sup>→</sup> **<sup>R</sup>**<sup>+</sup>

<sup>→</sup> **SO**<sup>2</sup>

*ai* sign [*ı*ˆ · T*i*(*x*)] *bi* sign [ˆ*j* · T*i*(*x*)]

(48)

�T

� (49)

<sup>→</sup> **<sup>R</sup>**<sup>+</sup>

<sup>→</sup> **SO**<sup>2</sup>

�*<sup>x</sup>* <sup>−</sup> *<sup>x</sup>i*� (47)

*di* : **<sup>R</sup>**<sup>2</sup> \

*<sup>n</sup><sup>i</sup>* : **<sup>R</sup>**<sup>2</sup> \

*<sup>n</sup>i*(*x*) = *<sup>x</sup>* <sup>−</sup> *<sup>x</sup><sup>i</sup>*

side w.r.t. the global coordinate system, respectively, are given below:

�−<sup>1</sup>

· T*i*(*xint*)


· T*i*(*xint*)

�

� *ai* 0 0 *bi*

� *ai* 0 0 *bi*

otherwise : <sup>T</sup> <sup>−</sup><sup>1</sup>


� � � �

�−<sup>1</sup>

<sup>2</sup> : <sup>T</sup> <sup>−</sup><sup>1</sup>

<sup>2</sup> : <sup>T</sup> <sup>−</sup><sup>1</sup>

*i*

� *xint* : � � � *<sup>x</sup>int* <sup>−</sup> *<sup>x</sup><sup>i</sup>*

� *xint* : � � � *<sup>x</sup>int* <sup>−</sup> *<sup>x</sup><sup>i</sup>*

are given below:

2007).

**5.1.2 Rectangles**

*di* : **<sup>R</sup>**<sup>2</sup> \

⎧ ⎪⎪⎪⎨

⎪⎪⎪⎩

⎧ ⎪⎪⎪⎨

⎪⎪⎪⎩

*<sup>n</sup><sup>i</sup>* : **<sup>R</sup>**<sup>2</sup> \

*di*(*x*) =

*ni*(*x*) =

**5.1.3 Ellipses**

� *xint* : � � � � �

otherwise :

<sup>|</sup>*ı*<sup>ˆ</sup> · T*i*(*x*)<sup>|</sup> <sup>&</sup>lt; *ai*

<sup>|</sup>ˆ*<sup>j</sup>* · T*i*(*x*)<sup>|</sup> <sup>&</sup>lt; *bi*

2008; Jung & Schramm, 2004; Nguyen et al., 2009).

� *xint* : � � � � � 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:

$$
\left(\frac{\chi}{a}\right)^2 + \left(\frac{\mathcal{Y}}{b}\right)^2 = 1\tag{50}
$$

The locus of its solutions is the ellipse, {*x<sup>e</sup>* <sup>=</sup> *xe ye* T }. The analysis proceeds by considering those *x* = *x y*<sup>T</sup> <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> for which *<sup>x</sup>* <sup>−</sup> *<sup>x</sup><sup>e</sup>* is normal to the ellipse. The equation of such a normal is:

$$\mathbf{x}\_{\rm ll}(\boldsymbol{\tau}) = \mathbf{k}\boldsymbol{\tau} + \mathbf{x}\_{\rm t} \tag{51}$$

Where *τ* ∈ **R** is an independent parameter, the degree of freedom along the line and *k* is the direction vector of the line, given below:

$$\mathbf{k} = \nabla \left\{ \left(\frac{\chi\_{\varepsilon}}{a}\right)^{2} + \left(\frac{y\_{\varepsilon}}{b}\right)^{2} - 1 \right\} = \left[\frac{\chi\_{\varepsilon}}{a^{2}} \frac{y\_{\varepsilon}}{b^{2}}\right]^{\mathbf{T}} \tag{52}$$

It follows that if *<sup>τ</sup>* <sup>=</sup> *<sup>t</sup>* <sup>=</sup> arg *<sup>x</sup>*, i.e. *<sup>x</sup>n*(*t*) id = *x*. Then, the following manipulation can be made:

$$\begin{bmatrix} \mathbf{x} - \mathbf{x}\_{\ell} \ \mathbf{y} - \mathbf{y}\_{\ell} \end{bmatrix}^{\mathrm{T}} = \begin{bmatrix} \frac{t \mathbf{x}\_{\ell}}{a^2} \ \frac{t \mathbf{y}\_{\ell}}{b^2} \end{bmatrix}^{\mathrm{T}} \tag{53}$$

$$\begin{bmatrix} \mathbf{x}\_{\ell} \ \mathbf{y}\_{\ell} \end{bmatrix}^{\mathrm{T}} = \begin{bmatrix} \frac{a^2 \mathbf{x}}{t + a^2} \ \frac{b^2 \mathbf{y}}{t + b^2} \end{bmatrix}^{\mathrm{T}} \tag{54}$$

Substituting the right-hand side of (54) into (50), the quartic discussed is obtained as:

$$\left(\frac{a\mathbf{x}}{t+a^2}\right)^2 + \left(\frac{by}{t+b^2}\right)^2 = 1\tag{55}$$

$$(t+b^2)^2a^2x^2 + (t+a^2)^2b^2y^2 = (t+a^2)^2(t+b^2)^2\tag{56}$$

$$(t+a^2)^2(t+b^2)^2 - (t+b^2)^2a^2x^2 - (t+a^2)^2b^2y^2 = 0\tag{57}$$

The greatest root of (57), *t*, allows for the calculation of (*di*(*x*), *ni*(*x*)) in (51, 54), as given below:

$$d\_{\bar{l}}: \quad \mathbb{R}^2 \left\{ \left\{ \mathbf{x}^{int} : \mathbf{x}^{\mathrm{T}} \mathcal{T}\_{\bar{l}} \left\{ \begin{bmatrix} a \ 0 \\ b \ 0 \end{bmatrix} \mathcal{T}\_{\bar{l}}^{-1}(\mathbf{x}) \right\} < 1 \right\} \to \mathbb{R}^+$$

$$d\_{\bar{l}}(\mathbf{x}) = ||\mathbf{x} - \mathbf{x}\_{\varepsilon}|| = ||k\overline{\mathbf{r}}|| = \overline{\mathfrak{l}} \left\| \begin{bmatrix} a^2 \mathbf{x}\_{\varepsilon} & b^2 \mathbf{y}\_{\varepsilon} \\ \overline{\mathfrak{l}} + a^2 \frac{b^2 \mathbf{y}\_{\varepsilon}}{\mathfrak{l} + b^2} \end{bmatrix}^{\mathrm{T}} \right\|$$

$$= \overline{\mathfrak{l}} \sqrt{\frac{(\overline{\mathfrak{l}} + b^2)^2 a^4 \mathbf{x}\_{\varepsilon}^2 + (\overline{\mathfrak{l}} + a^2)^2 b^4 y\_{\varepsilon}^2}} \tag{58}$$

$$= \overline{\mathfrak{l}} \sqrt{(\overline{\mathfrak{l}} + b^2)^2 \mathcal{A}^4 \left\{ \mathfrak{l} \mathcal{T}(\mathbf{x}) \right\}^2 + (\overline{\mathfrak{l}} + b^2)^2 \mathbf{A}^4 \left\{ \mathfrak{l} \mathcal{T}(\mathbf{x}) \right\}^2}$$

$$=\overline{t}\sqrt{\frac{(\overline{t}+b^2)^2a^4\left[t\mathcal{T}\_i(\mathbf{x})\right]^2+(\overline{t}+a^2)^2b^4\left[j\mathcal{T}\_i(\mathbf{x})\right])^2}{(\overline{t}+a^2)^2(\overline{t}+b^2)^2}}\tag{59}$$

Table 2. The Scaled Unscented Transform Sigma-Point Kalman Filter Algorithm

Formation Guidance of AUVs Using Decentralized Control Functions 119

sigma-point drawn from the underlying distribution

 ,

> ,

2(*L*+*λ*)

0.1. Set *α*, the scaling parameter for the Scaled

0.3. Set *κ*, the scaling parameter of the set of

*<sup>c</sup>* ··· *<sup>w</sup>*(2*L*) *<sup>c</sup>*

*<sup>m</sup>* ··· *<sup>w</sup>*(2*L*) *<sup>m</sup>*

(**<sup>x</sup>** <sup>−</sup> **<sup>x</sup>**)(**<sup>x</sup>** <sup>−</sup> **<sup>x</sup>**)<sup>T</sup> - the initial covariance matrix of the estimates

**x**(*k*|*k* − 1) **x**(*k*|*k* − 1) + *γ*

*<sup>i</sup>*=<sup>0</sup> *<sup>w</sup>*(*i*)

account the process noise covariance

**<sup>F</sup>**(*<sup>X</sup>* <sup>−</sup>(*k*))

*<sup>m</sup>* **x**− ∗ (*i*)

<sup>√</sup>**Rv <sup>x</sup>**ˆ<sup>−</sup> <sup>−</sup> *<sup>γ</sup>*

*<sup>Y</sup>*(*i*) <sup>−</sup> **<sup>y</sup>**<sup>ˆ</sup> <sup>−</sup>

**<sup>x</sup>** <sup>−</sup> **<sup>K</sup>**(*k*)**PyK**(*k*)<sup>T</sup>

*<sup>X</sup>*(*i*) <sup>−</sup> **<sup>x</sup>**ˆ<sup>−</sup>

*<sup>X</sup>* − ∗ (*i*) <sup>−</sup> **<sup>x</sup>**ˆ−(*k*|*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

<sup>√</sup>**Rv** 

*Y*(*i*) <sup>−</sup> **<sup>y</sup>**<sup>ˆ</sup> <sup>−</sup>

 **<sup>y</sup>** <sup>−</sup> **<sup>y</sup>**<sup>ˆ</sup> <sup>−</sup>

*Y*(*i*) <sup>−</sup> **<sup>y</sup>**<sup>ˆ</sup> <sup>−</sup>

T

T

**Px**(*k*) **<sup>x</sup>**(*k*|*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) <sup>−</sup> *<sup>γ</sup>*

**Px**(*k*) 

T

*<sup>X</sup>* − ∗ (*i*) <sup>−</sup> **<sup>x</sup>**ˆ−(*k*|*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

*<sup>L</sup>*+*<sup>λ</sup>* + (<sup>1</sup> <sup>−</sup> *<sup>α</sup>*<sup>2</sup> <sup>+</sup> *<sup>β</sup>*),

Unscented Transform 0.2. Set *β*, the parameter of accentuation of the central estimate

0.4. *L* = 12, the number of states

*<sup>w</sup>*(0) *<sup>c</sup> <sup>w</sup>*(1)

*<sup>w</sup>*(1...2*L*) *<sup>c</sup>* <sup>=</sup> *<sup>w</sup>*(1...2*L*) *<sup>m</sup>* <sup>=</sup> <sup>1</sup>


1.3. Set **R***<sup>f</sup>* - the process noise covariance 1.4. Set **Rn** - the measurement noise covariance

2.1.1. *<sup>X</sup>* <sup>−</sup>(*k*) = {**x**−} <sup>=</sup>

2.2.2. ˆ**x**−(*k*|*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) = <sup>∑</sup>2*<sup>L</sup>*

**<sup>x</sup>** = <sup>∑</sup>2*<sup>L</sup>*

2.2.5. *Y*(*k*|*k* − 1) = **H** (*X*)

2.2.1. *<sup>X</sup>* − ∗(*k*) = {**x**− ∗} <sup>=</sup>

*<sup>i</sup> <sup>w</sup>*(*i*) *c* 

2.2.4. Re-draw the hypotheses taking into

**x**ˆ− **x**ˆ− + *γ*

*<sup>i</sup>*=<sup>0</sup> *<sup>w</sup>*(*i*) *<sup>m</sup> Y*(*i*)

*<sup>i</sup>*=<sup>0</sup> *<sup>w</sup>*(*i*) *c* 

> *<sup>i</sup>*=<sup>0</sup> *<sup>w</sup>*(*i*) *c*

**y** 2.3.4. ˆ**x**(*k*|*k*) = **x**ˆ−(*k*|*k* − 1) + **K**(*k*)

2.1. Sigma-points and hypotheses of the states

0.5. *<sup>λ</sup>* <sup>=</sup> *<sup>α</sup>*2(*<sup>L</sup>* <sup>+</sup> *<sup>κ</sup>*) <sup>−</sup> *<sup>L</sup>*

*<sup>w</sup>*(0) *<sup>c</sup>* <sup>=</sup> *<sup>λ</sup>*

 *w*(0) *<sup>m</sup> <sup>w</sup>*(1)

0.6. **w***<sup>c</sup>* =

0.7. **w***<sup>m</sup>* =

**2. Iteration for** *k* = 1... ∞

**1. Initialization**

*w*(0) *<sup>m</sup>* = *<sup>λ</sup> L*+*λ*

1.1. Set ˆ**x**(0|1) = **x**

1.2. Set **Px**(*k*) = E

2.2. Time-update

2.2.3. **P**−

*<sup>X</sup>*(*k*|*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) =

2.3. Measurement update 2.3.1. **Py** = ∑2*<sup>L</sup>*

2.2.6. ˆ**y**<sup>−</sup> = ∑2*<sup>L</sup>*

2.3.2. **Pxy** = ∑2*<sup>L</sup>*

2.3.5. **Px**(*k*) = **P**−

2.3.3. **K**(*k*) = **PxyP**−<sup>1</sup>

**0. Parameterization**

$$n\_{i}: \quad \mathbb{R}^{2} \lor \left\{ \mathbf{x}^{int} : \mathbf{x}^{\mathrm{T}} \mathcal{T}\_{i} \left\{ \begin{bmatrix} a & 0 \\ b & 0 \end{bmatrix} \mathcal{T}\_{i}^{-1}(\mathbf{x}) \right\} < 1 \right\} \to \mathrm{SO}^{2}$$

$$\begin{aligned} \mathcal{T}\_{i} \mathbf{n}\_{i}^{(i)}(\mathbf{x}) &= \frac{\mathbf{k}}{||\mathbf{k}||} = \frac{\begin{bmatrix} \mathbf{x}\_{\varepsilon} & \mathcal{Y}\_{\varepsilon} \\ \overline{\mathbf{f} + a^{2}} & \overline{\mathbf{f} + b^{2}} \end{bmatrix}^{\mathrm{T}}}{\left\| \begin{bmatrix} \mathbf{x}\_{\varepsilon} & \mathcal{Y}\_{\varepsilon} \\ \overline{\mathbf{f} + a^{2}} & \overline{\mathbf{f} + b^{2}} \end{bmatrix}^{\mathrm{T}} \right\|} \\\ \mathcal{T}\_{i} \mathbf{n}\_{i}(\mathbf{x}) &= \frac{\begin{bmatrix} \mathcal{T}\_{i}(\mathbf{x}) & \mathcal{T}\_{i}(\mathbf{x}) \\ \overline{\mathbf{f} + a^{2}} & \overline{\mathbf{f} + b^{2}} \end{bmatrix}^{\mathrm{T}}}{\left\| \begin{bmatrix} \mathcal{T}\_{i}(\mathbf{x}) & \mathcal{T}\_{i}(\mathbf{x}) \end{bmatrix}^{\mathrm{T}} \right\|} \end{aligned} \tag{60}$$

$$\mathfrak{m}\_{i}(\mathbf{x}) = \mathcal{T}\_{i}^{-1} \left\{ \frac{\left[ \overbrace{\overline{\mathfrak{I}}\_{i}(\mathbf{x})}^{\mathrm{T}} \overbrace{\overline{\mathfrak{I}}\_{i}(\mathbf{x})}^{\mathrm{T}} \right]^{\mathrm{T}}}{\left\| \left[ \overbrace{\overline{\mathfrak{I}}\_{i}^{\mathrm{T}}(\mathbf{x})}^{\mathrm{T}} \overbrace{\overline{\mathfrak{I}}\_{i}^{\mathrm{T}}(\mathbf{x})}^{\mathrm{T}} \right]^{\mathrm{T}} \right\|\_{1}} \right\} \tag{62}$$

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).

ˆ*j*T*i*(*x*)

� �

� � � *ı*ˆT*i*(*x*)

#### **5.2 State estimation**

The full state **x** = � **<sup>v</sup>**<sup>T</sup> <sup>ω</sup><sup>T</sup> *<sup>x</sup>*<sup>T</sup> **<sup>Θ</sup>**T�<sup>T</sup> <sup>=</sup> � *uvw*|*pqr*|*xyz*|*ϕϑψ*�<sup>T</sup> 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.

#### **5.3 Measurement and process noise**

The AUV is assumed to carry a 4-beam DVL<sup>4</sup> which it can use to record the true 3D speed-over-ground measurement **v** = � *uvw*�<sup>T</sup> . Furthermore, the AUV carries a 3-axial rate gyro package capable of measuring the body-fixed angular velocities ω = � *pqr*�<sup>T</sup> . A

<sup>4</sup> Doppler Velocity Logger.

Table 2. The Scaled Unscented Transform Sigma-Point Kalman Filter Algorithm

#### **0. Parameterization**

20 Will-be-set-by-IN-TECH

��*a* 0 *b* 0 � <sup>T</sup> <sup>−</sup><sup>1</sup> *<sup>i</sup>* (*x*) � < 1 �

*ye t* + *b*<sup>2</sup>

*ye t* + *b*<sup>2</sup>

�T

�T � � � � �

> ˆ*j*T*i*(*x*) *t* + *b*<sup>2</sup>

ˆ*j*T*i*(*x*) *t* + *b*<sup>2</sup>

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

using the Scaled Unscented Transform Sigma-Point Kalman Filter (SP-UKF) introduced by

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

The AUV is assumed to carry a 4-beam DVL<sup>4</sup> which it can use to record the true 3D

*uvw*�<sup>T</sup>

rate gyro package capable of measuring the body-fixed angular velocities ω = �

�T

⎫ ⎪⎪⎪⎪⎬

⎪⎪⎪⎪⎭

*uvw*|*pqr*|*xyz*|*ϕϑψ*�<sup>T</sup> of the AUV will be estimated

. Furthermore, the AUV carries a 3-axial

*pqr*�<sup>T</sup>

. A

�T � � � � �

�T

�T � � � � � <sup>→</sup> **SO**<sup>2</sup>

(60)

(61)

(62)

*<sup>n</sup><sup>i</sup>* : **<sup>R</sup>**<sup>2</sup> \

*<sup>i</sup>* (*x*) = *<sup>k</sup>*

T*ini*(*x*) =

T*in*(*i*)

**5.2 State estimation** The full state **x** = �

van der Merwe (2004).

�

�*k*� <sup>=</sup>

� *ı*ˆT*i*(*x*) *t* + *a*<sup>2</sup>

� � � � �

**<sup>v</sup>**<sup>T</sup> <sup>ω</sup><sup>T</sup> *<sup>x</sup>*<sup>T</sup> **<sup>Θ</sup>**T�<sup>T</sup> <sup>=</sup> �

themselves is relatively small. The algorithm is listed in table 2.

**5.3 Measurement and process noise**

<sup>4</sup> Doppler Velocity Logger.

speed-over-ground measurement **v** = �

*<sup>n</sup>i*(*x*) = <sup>T</sup> <sup>−</sup><sup>1</sup> *i*

� *ı*ˆT*i*(*x*) *t* + *a*<sup>2</sup>

> ⎧ ⎪⎪⎪⎪⎨

� *ı*ˆT*i*(*x*) *t* + *a*<sup>2</sup>

⎪⎪⎪⎪⎩

� � � � �

� ˆ*ı*T*i*(*x*) *t* + *a*<sup>2</sup>

clouds has intensified – (Ahn et al., 1999; Jiang et al., 2007; Pilu et al., 1996).

*<sup>x</sup>int* : *<sup>x</sup>*TT*<sup>i</sup>*

� *xe t* + *a*<sup>2</sup>

� *xe t* + *a*<sup>2</sup>

> ˆ*j*T*i*(*x*) *t* + *b*<sup>2</sup>

ˆ*j*T*i*(*x*) *t* + *b*<sup>2</sup>

� � � � �


,

,

0.4. *L* = 12, the number of states

$$\begin{array}{ll} 0.5. & \lambda = \alpha^2 (L + \kappa) - L \\ 0.6. & \mathbf{w}\_{\mathcal{c}} = \left\{ w\_{\mathcal{c}}^{(0)} \, w\_{\mathcal{c}}^{(1)} \, \cdots \, w\_{\mathcal{c}}^{(2L)} \right\}\_{\prime} \\ & w\_{\mathcal{c}}^{(0)} = \frac{\lambda}{L + \lambda} + (1 - \alpha^2 + \beta) \\ & w\_{\mathcal{c}}^{(1 \dots 2L)} = w\_{m}^{(1 \dots 2L)} = \frac{1}{2(L + \lambda)} \\ 0.7. & \mathbf{w}\_{m} = \left\{ w\_{m}^{(0)} \, w\_{m}^{(1)} \, \cdots \, w\_{m}^{(2L)} \right\} \\ & (0) \end{array}$$

*L*+*λ*

$$w\_m^{(0)} = \frac{\lambda}{L+}$$

#### **1. Initialization**

	- the initial a priori estimate
	- the initial covariance matrix of the estimates

#### **2. Iteration for** *k* = 1... ∞

2.1. Sigma-points and hypotheses of the states 2.1.1. *<sup>X</sup>* <sup>−</sup>(*k*) = {**x**−} <sup>=</sup> **x**(*k*|*k* − 1) **x**(*k*|*k* − 1) + *γ* **Px**(*k*) **<sup>x</sup>**(*k*|*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) <sup>−</sup> *<sup>γ</sup>* **Px**(*k*) 2.2. Time-update 2.2.1. *<sup>X</sup>* − ∗(*k*) = {**x**− ∗} <sup>=</sup> **<sup>F</sup>**(*<sup>X</sup>* <sup>−</sup>(*k*))

$$\text{(2.2.2. \ \hat{\mathbf{x}}^{-}(k|\hat{k}-1) = \sum\_{i=0}^{2L} w\_{m}^{(i)} \mathbf{x}^{-\*(i)})$$

$$\begin{aligned} \text{2.2.3.} \quad & \mathbf{P\_x} = \sum\_{i}^{\text{L}} w\_c^{(i)} \left( \overline{\mathbf{\mathcal{X}}}^{-\*} \mathbf{\hat{x}}^{(i)} - \mathbf{\hat{x}}^{-} (k|k-1) \right) \left( \mathbf{\mathcal{X}}^{-\*(i)} - \mathbf{\hat{x}}^{-} (k|k-1) \right)^{\text{T}} \\ \text{2.2.4} \quad & \text{Do } \text{4} \text{nm} \text{ the bumo theorem into} \end{aligned}$$

2.2.4. Re-draw the hypotheses taking into account the process noise covariance 

$$\begin{array}{ll} \mathbf{X}(k|k-1) = \left\{\hat{\mathbf{x}}^{-}\,\hat{\mathbf{x}}^{-} + \gamma\sqrt{\mathbf{R}\_{\mathbf{V}}}\,\hat{\mathbf{x}}^{-} - \gamma\sqrt{\mathbf{R}\_{\mathbf{V}}}\right\} \\ \text{2.2.5.} \quad \mathbf{Y}(k|k-1) = \mathbf{H}\left(\mathbf{X}\right) \\ \text{2.2.6.} \quad \hat{\mathbf{y}}^{-} = \sum\_{i=0}^{2L} w\_{m}^{(i)}\mathbf{Y}^{(i)} \end{array}$$

2.3. Measurement update

$$\begin{aligned} \text{2.3.1.} \qquad \mathbf{P\_{y}} &= \sum\_{i=0}^{2L} w\_{c}^{(i)} \left( \mathbf{Y}^{(i)} - \mathbf{\hat{y}}^{-} \right) \left( \mathbf{Y}^{(i)} - \mathbf{\hat{y}}^{-} \right)^{\mathrm{T}} \\ \text{2.3.2.} \qquad \mathbf{P\_{xy}} &= \sum\_{i=0}^{2L} w\_{c}^{(i)} \left( \mathbf{X}^{(i)} - \mathbf{\hat{x}}^{-} \right) \left( \mathbf{Y}^{(i)} - \mathbf{\hat{y}}^{-} \right)^{\mathrm{T}} \end{aligned}$$


**Tyv** = **Ty***<sup>u</sup>* = **Ty***<sup>v</sup>* = **Ty***<sup>w</sup>* =

**Ty**<sup>ω</sup> = **Ty***<sup>p</sup>* = **Ty***<sup>q</sup>* = **Ty***<sup>r</sup>* =

M**y***xy* = M**y***<sup>x</sup>* = M**y***<sup>y</sup>* =

**Ty***xy* = **Ty***<sup>x</sup>* = **Ty***<sup>y</sup>* =

⎧ ⎪⎪⎪⎨

⎪⎪⎪⎩

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

**Ty***<sup>z</sup>* =

M**y<sup>Θ</sup>** = M**y***<sup>ϕ</sup>* = M**y***<sup>ϑ</sup>* = M**y***<sup>ψ</sup>* =

**Ty<sup>Θ</sup>** = **Ty***<sup>ϕ</sup>* = **Ty***<sup>ϑ</sup>* = **Ty***<sup>ψ</sup>* =

M**y***<sup>z</sup>* =

M**y**<sup>ω</sup> = M**y***<sup>p</sup>* = M**y***<sup>q</sup>* = M**y***<sup>r</sup>* =

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

Formation Guidance of AUVs Using Decentralized Control Functions 121

⎧ ⎪⎪⎪⎨

⎪⎪⎪⎩

⎧ ⎪⎪⎪⎨

⎪⎪⎪⎩

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

> ⎧ ⎪⎪⎪⎨

> ⎪⎪⎪⎩

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0.7542 0.1000 0.1000 0.0208 0.0208 0.0042 0.4739 0.3365 0.1043 0.0379 0.0284 0.0190 0.4739 0.1043 0.3365 0.0284 0.0379 0.0190 0.3825 0.1749 0.1749 0.0984 0.0984 0.0710 0.3825 0.1749 0.1749 0.0984 0.0984 0.0710 0.0270 0.1622 0.1622 0.2703 0.2703 0.1081

State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN)

State *nominal* : (*μ*1, *σ*1, *n*1)=(0, 1.0, 0.012) State ± *reliable* : (*μ*1, *σ*1, *n*1)=(±1.3, 1.5, 0.06) State ± *unreliable* : (*μ*1, *σ*1, *n*1)=(±3.85, 4.0, 1.28) State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN)

0.4809 0.1967 0.1967 0.0601 0.0601 0.0055 0.4160 0.3200 0.1440 0.0800 0.0320 0.0080 0.4160 0.1440 0.3200 0.0320 0.0800 0.0080 0.3689 0.2136 0.1359 0.1942 0.0777 0.0097 0.3689 0.1359 0.2136 0.0777 0.1942 0.0097 0.0102 0.2959 0.2959 0.1837 0.1837 0.0306

State *nominal* : (*μ*1, *σ*1, *n*1)=(0, 0.08, 0.012) State ± *reliable* : (*μ*1, *σ*1, *n*1)=(±0.11, 0.1208, 0.06) State ± *unreliable* : (*μ*1, *σ*1, *n*1)=(±0.55, 0.71, 1.28) State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN)

0.5198 0.1762 0.1762 0.0617 0.0617 0.0044 0.4020 0.4020 0.1106 0.0503 0.0302 0.0050 0.4020 0.1106 0.4020 0.0302 0.0503 0.0050 0.3704 0.2667 0.1481 0.1481 0.0593 0.0074 0.3704 0.1481 0.2667 0.0593 0.1481 0.0074 0.0667 0.2000 0.2000 0.2000 0.2000 0.1333

State *nominal* : (*μ*1, *σ*1, *n*1) = �

State <sup>±</sup> *reliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = �

State <sup>±</sup> *unreliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = �

State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN)

0.4686 0.2301 0.2301 0.0335 0.0335 0.0042 0.4014 0.2721 0.1769 0.1020 0.0340 0.0136 0.4014 0.1769 0.2721 0.0340 0.1020 0.0136 0.3982 0.1403 0.0995 0.1719 0.0995 0.0905 0.3982 0.0995 0.1403 0.0995 0.1719 0.0905 0.0526 0.1579 0.1579 0.2105 0.2105 0.2105

0.5928 0.1596 0.1596 0.0423 0.0423 0.0033 0.4978 0.2489 0.1511 0.0356 0.0533 0.0133 0.4978 0.1511 0.2489 0.0533 0.0356 0.0133 0.5234 0.1963 0.0561 0.0935 0.0748 0.0561 0.5234 0.0561 0.1963 0.0748 0.0935 0.0561 0.0588 0.2941 0.2941 0.1176 0.1176 0.1176

State *nominal* : (*μ*1, *σ*1, *n*1) = �

State <sup>±</sup> *reliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = �

State <sup>±</sup> *unreliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = �

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

�

18.8 , *<sup>π</sup>* 64.454

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

> 0, *<sup>π</sup>* <sup>220</sup> , *<sup>π</sup>* 98.05 �

<sup>±</sup> *<sup>π</sup>* <sup>192</sup> , *<sup>π</sup>* <sup>176</sup> , *<sup>π</sup>* 42.60 �

<sup>±</sup> *<sup>π</sup>* <sup>60</sup> , *<sup>π</sup>* <sup>42</sup> , *<sup>π</sup>* 10 �

> ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ �

0, *<sup>π</sup>* <sup>85</sup> , *<sup>π</sup>* 227.608

<sup>±</sup> *<sup>π</sup>* <sup>72</sup> , *<sup>π</sup>* <sup>60</sup> , *<sup>π</sup>* 144.201 �

<sup>±</sup> *<sup>π</sup>* 21.5 , *<sup>π</sup>* (66)

(67)

(68)

(69)

(70)

(71)

(72)

(73)

(74)

low-grade commercial USBL<sup>5</sup> system is assumed to provide estimates of � *x y*�<sup>T</sup> . 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 AHRS6 is assumed to provide the Tait-Bryan angle readings, **Θ** = � *ϕϑψ*�<sup>T</sup> .

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 12*nAUV* Gaussian Markov models, one for each state of each of the *nAUV* AUVs is used. All of the GMM states contain separate *univariate rate-limited white noise generators* parameterized by (*μi*, *σi*, *ni*), where *ni* 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:

$$
\bar{y}\_i^-(k) = \mu\_i + \sigma\_i \cdot \mathbf{rand} \,\tag{63}
$$

$$\mathfrak{z}\_{i}\mathfrak{z}\_{i}(k) = \text{sign}(\mathfrak{z}^{-}(k) - \mathfrak{z}(k-1)) \cdot \min\left[ \left| \mathfrak{z}^{-}(k) - \mathfrak{z}(k-1) \right| \Big| \prime \frac{n\_{i}}{T} \right] \tag{64}$$

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** = [*tij*] with *tij* 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***<sup>y</sup>* 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.

$$\mathcal{M}\_{\mathbf{y}\mathbf{y}} = \mathcal{M}\_{\mathbf{y}\_{\mathbf{y}}} = \mathcal{M}\_{\mathbf{y}\_{\mathbf{y}}} = \mathcal{M}\_{\mathbf{y}\_{\mathbf{y}}} = \begin{cases} \text{State } nominal: & (\mu\_{1}, \sigma\_{1}, n\_{1}) = (0, 0.06, 0.03815) \\ \text{State } \pm relable: & (\mu\_{1}, \sigma\_{1}, n\_{1}) = (\pm 0.09, 0.11, 0.05) \\ \text{State } \pm unrelable: & (\mu\_{1}, \sigma\_{1}, n\_{1}) = (\pm 0.2981, 0.24, 0.09) \\ \text{State } faulty: & (\mu\_{1}, \sigma\_{1}, n\_{1}) = (\pm 0.02, \text{MA}, \text{MA}) \end{cases} \tag{65}$$

<sup>5</sup> Ultra-short baseline hydroacoustic localization.

<sup>6</sup> Attitude and heading reference system.

22 Will-be-set-by-IN-TECH

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

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

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 12*nAUV* Gaussian Markov models, one for each state of each of the *nAUV* AUVs is used. All of the GMM states contain separate *univariate rate-limited white noise generators* parameterized by (*μi*, *σi*, *ni*), where *ni* is the rate limit of the additive measurement

Relying on the MATLAB normally distributed random number generator invoked by the

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** = [*tij*] with *tij* 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***<sup>y</sup>* 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

<sup>−</sup>(*k*) − *y*˜(*k* − 1)) · min

mutually dislocated sensors operating along different physical principles.

⎧ ⎪⎪⎪⎨

⎪⎪⎪⎩

*<sup>i</sup>* (*k*) = *μ<sup>i</sup>* + *σ<sup>i</sup>* · randn (63)

�� �*y*˜

State *nominal* : (*μ*1, *σ*1, *n*1)=(0, 0.06, 0.03815) State ± *reliable* : (*μ*1, *σ*1, *n*1)=(±0.09, 0.11, 0.05) State ± *unreliable* : (*μ*1, *σ*1, *n*1)=(±0.2981, 0.24, 0.09) State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN)

<sup>−</sup>(*k*) − *y*˜(*k* − 1)

� � , *ni T* �

*x y*�<sup>T</sup>

*ϕϑψ*�<sup>T</sup> . . A fusion

(64)

(65)

low-grade commercial USBL<sup>5</sup> system is assumed to provide estimates of �

AHRS6 is assumed to provide the Tait-Bryan angle readings, **Θ** = �

randn command, each state generates a number according to:

be relied on in any meaningful way.

noise in the *i*-th channel.

*y*˜ −

Where *T* is the sampling time.

M**yv** = M**y***<sup>u</sup>* = M**y***<sup>v</sup>* = M**y***<sup>w</sup>* =

<sup>5</sup> Ultra-short baseline hydroacoustic localization. <sup>6</sup> Attitude and heading reference system.

*y*˜*i*(*k*) = sign(*y*˜

**Tyv** = **Ty***<sup>u</sup>* = **Ty***<sup>v</sup>* = **Ty***<sup>w</sup>* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0.7542 0.1000 0.1000 0.0208 0.0208 0.0042 0.4739 0.3365 0.1043 0.0379 0.0284 0.0190 0.4739 0.1043 0.3365 0.0284 0.0379 0.0190 0.3825 0.1749 0.1749 0.0984 0.0984 0.0710 0.3825 0.1749 0.1749 0.0984 0.0984 0.0710 0.0270 0.1622 0.1622 0.2703 0.2703 0.1081 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (66) M**y**<sup>ω</sup> = M**y***<sup>p</sup>* = M**y***<sup>q</sup>* = M**y***<sup>r</sup>* = ⎧ ⎪⎪⎪⎨ ⎪⎪⎪⎩ State *nominal* : (*μ*1, *σ*1, *n*1) = � 0, *<sup>π</sup>* <sup>85</sup> , *<sup>π</sup>* 227.608 � State <sup>±</sup> *reliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = � <sup>±</sup> *<sup>π</sup>* <sup>72</sup> , *<sup>π</sup>* <sup>60</sup> , *<sup>π</sup>* 144.201 � State <sup>±</sup> *unreliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = � <sup>±</sup> *<sup>π</sup>* 21.5 , *<sup>π</sup>* 18.8 , *<sup>π</sup>* 64.454 � State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN) (67) **Ty**<sup>ω</sup> = **Ty***<sup>p</sup>* = **Ty***<sup>q</sup>* = **Ty***<sup>r</sup>* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0.5928 0.1596 0.1596 0.0423 0.0423 0.0033 0.4978 0.2489 0.1511 0.0356 0.0533 0.0133 0.4978 0.1511 0.2489 0.0533 0.0356 0.0133 0.5234 0.1963 0.0561 0.0935 0.0748 0.0561 0.5234 0.0561 0.1963 0.0748 0.0935 0.0561 0.0588 0.2941 0.2941 0.1176 0.1176 0.1176 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (68) M**y***xy* = M**y***<sup>x</sup>* = M**y***<sup>y</sup>* = ⎧ ⎪⎪⎪⎨ ⎪⎪⎪⎩ State *nominal* : (*μ*1, *σ*1, *n*1)=(0, 1.0, 0.012) State ± *reliable* : (*μ*1, *σ*1, *n*1)=(±1.3, 1.5, 0.06) State ± *unreliable* : (*μ*1, *σ*1, *n*1)=(±3.85, 4.0, 1.28) State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN) (69) **Ty***xy* = **Ty***<sup>x</sup>* = **Ty***<sup>y</sup>* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0.4809 0.1967 0.1967 0.0601 0.0601 0.0055 0.4160 0.3200 0.1440 0.0800 0.0320 0.0080 0.4160 0.1440 0.3200 0.0320 0.0800 0.0080 0.3689 0.2136 0.1359 0.1942 0.0777 0.0097 0.3689 0.1359 0.2136 0.0777 0.1942 0.0097 0.0102 0.2959 0.2959 0.1837 0.1837 0.0306 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (70) M**y***<sup>z</sup>* = ⎧ ⎪⎪⎪⎨ ⎪⎪⎪⎩ State *nominal* : (*μ*1, *σ*1, *n*1)=(0, 0.08, 0.012) State ± *reliable* : (*μ*1, *σ*1, *n*1)=(±0.11, 0.1208, 0.06) State ± *unreliable* : (*μ*1, *σ*1, *n*1)=(±0.55, 0.71, 1.28) State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN) (71) **Ty***<sup>z</sup>* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0.5198 0.1762 0.1762 0.0617 0.0617 0.0044 0.4020 0.4020 0.1106 0.0503 0.0302 0.0050 0.4020 0.1106 0.4020 0.0302 0.0503 0.0050 0.3704 0.2667 0.1481 0.1481 0.0593 0.0074 0.3704 0.1481 0.2667 0.0593 0.1481 0.0074 0.0667 0.2000 0.2000 0.2000 0.2000 0.1333 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (72) M**y<sup>Θ</sup>** = M**y***<sup>ϕ</sup>* = M**y***<sup>ϑ</sup>* = M**y***<sup>ψ</sup>* = ⎧ ⎪⎪⎪⎨ ⎪⎪⎪⎩ State *nominal* : (*μ*1, *σ*1, *n*1) = � 0, *<sup>π</sup>* <sup>220</sup> , *<sup>π</sup>* 98.05 � State <sup>±</sup> *reliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = � <sup>±</sup> *<sup>π</sup>* <sup>192</sup> , *<sup>π</sup>* <sup>176</sup> , *<sup>π</sup>* 42.60 � State <sup>±</sup> *unreliable* : (*μ*1, *<sup>σ</sup>*1, *<sup>n</sup>*1) = � <sup>±</sup> *<sup>π</sup>* <sup>60</sup> , *<sup>π</sup>* <sup>42</sup> , *<sup>π</sup>* 10 � State *f aulty* : (*μ*1, *σ*1, *n*1)=(NaN, NaN, NaN) (73) **Ty<sup>Θ</sup>** = **Ty***<sup>ϕ</sup>* = **Ty***<sup>ϑ</sup>* = **Ty***<sup>ψ</sup>* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0.4686 0.2301 0.2301 0.0335 0.0335 0.0042 0.4014 0.2721 0.1769 0.1020 0.0340 0.0136 0.4014 0.1769 0.2721 0.0340 0.1020 0.0136 0.3982 0.1403 0.0995 0.1719 0.0995 0.0905 0.3982 0.0995 0.1403 0.0995 0.1719 0.0905 0.0526 0.1579 0.1579 0.2105 0.2105 0.2105 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (74)

**5.4 Control signals conditioning**

*<sup>u</sup>*(*k*) =

*<sup>u</sup>*˙(*k*) = <sup>1</sup>

*<sup>ψ</sup>*˙(*k*) = <sup>1</sup>

*<sup>ψ</sup>*¨(*k*) = <sup>1</sup> *T*

**5.4.1 The constraint inequalities**

the disc described by (85):

*u*(*k* − 1)

*<sup>T</sup>* <sup>−</sup> *<sup>u</sup>*˙ <sup>≤</sup>

<sup>=</sup> <sup>1</sup> *T* 

(*k*) ← **E**(*x*ˆ *AUV*(*k*)), obtained by evaluating (2), or more precisely (29), at

*<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)<sup>2</sup> <sup>+</sup> *<sup>T</sup>*<sup>2</sup> *<sup>f</sup>* <sup>2</sup> <sup>+</sup> <sup>2</sup>*Tu f*� (81)

*<sup>T</sup>* (82)

(83)

(84)

*<sup>f</sup>*� *<sup>f</sup>*<sup>⊥</sup> T ,

*<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)<sup>2</sup> <sup>+</sup> *<sup>T</sup>*<sup>2</sup> *<sup>f</sup>* <sup>2</sup> <sup>+</sup> <sup>2</sup>*T f*�*u*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) <sup>−</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

<sup>−</sup> *<sup>ψ</sup>*˙(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

*<sup>T</sup>* (85)

*<sup>T</sup>* <sup>+</sup> *<sup>u</sup>*˙ (86)

*T f*<sup>⊥</sup> *T f*� <sup>+</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

<sup>=</sup> �*f*� is the norm of the total controlling force, admitting decomposition into

*ψ*(*k* − 1), notwithstanding possible *sideslip* resulting from **x**<sup>2</sup> = **v**<sup>2</sup> = *v* �= 0).

the low-level controllers of the AUV beyond their normal operating range.

 

> 

the components parallel and perpendicular to the direction of heading of the AUV (given by

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 locus of solutions of (81) for *f* in easier to visualize in an AUV-fixed coordinate system (one with the origin in *xi*, 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:

*<sup>T</sup> <sup>f</sup>*<sup>⊥</sup>

The locus of solutions of (82) is, similarly to the preceding case, an offset *annulus* concentric to

*<sup>T</sup> <sup>f</sup>*<sup>⊥</sup>

T 

<sup>≤</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

*<sup>f</sup>*� <sup>+</sup> *<sup>u</sup>*(*k*−1)

T ≤ *u*

*<sup>f</sup>*� <sup>+</sup> *<sup>u</sup>*(*k*−1)

*T f*<sup>⊥</sup> *T f*� <sup>+</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)

the estimate ˆ*xAUV*(*k*) rendered by the SP-UKF, will be used for the formation of commands

Formation Guidance of AUVs Using Decentralized Control Functions 123

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

accepted by the forward speed and heading controllers in (41, 44), **c** = [*u*˙ *<sup>c</sup> uc ψ*˙ *<sup>c</sup> ψ<sup>c</sup>* ].

the **R**<sup>2</sup> space is assumed in the form of *the Euler backwards formula*. So naively:

*<sup>T</sup>* (*u*(*k*) <sup>−</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>))

*<sup>T</sup>* arctan

 1 *<sup>T</sup>* arctan

The solution of *f<sup>i</sup>*

Where: - *f* id


#### **5.3.1 Sensor fault simulation**

The last mentioned state, "*fault*" doesn't generate random additive measurement noises, but outputs NaN<sup>7</sup> 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 *yi* to the value of *y*ˆ*i*. This results in the corresponding rows and columns of the **Py** 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 **Px**(*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.

#### **5.3.2 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:

$$\mathbf{R}\_{\mathbf{y}}(k) \; \; \;= \; \mathbf{y}(k)^{\mathrm{T}} \cdot \mathbf{y}(k) = \begin{bmatrix} r\_{y}^{(i,j)} \\ \mathbf{y}^{\mathrm{T}} \end{bmatrix} \tag{76}$$

$$\dot{a}\_{out} = \left\{ \arg\_j \left( \exists j, \, r\_y^{(i,j)} > 16 \cdot \mathbf{P}\_{\mathbf{x}}(k) \right) \right\} \tag{77}$$

$$\mathbf{y}\left[\left\{i\_{out}\right\}\right] \stackrel{\text{redef}}{=} \mathbf{Na} \mathbf{N} \tag{78}$$

Where **Px**(*k*) is the covariance matrix of the estimate of the full-state vector rendered by the SP-UKF.

#### **5.3.3 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).

$$\mathbf{R}\_{v}^{(true)} = \begin{bmatrix} 0.2 & 0.01 & 0.01 & 0 & 0.005 & 0.005\\ 0.01 & 0.1 & 0.0275 & 0.01 & 0.003 & 0.035\\ 0.01 & 0.0275 & 0.1 & 0.01 & 0.003 & 0\\ 0 & 0.01 & 0.01 & 0.0011 & 0.0001 & 0.00015\\ 0.005 & 0.003 & 0.03 & 0.0001 & 0.002 & 0\\ 0.005 & 0.035 & 0 & 0.00015 & 0 & 0.0022 \end{bmatrix} \tag{79}$$
 
$$\mathbf{n}\_{v}^{(true)} = \begin{bmatrix} 0.2 \ 0.2 \ 0.2 \ \frac{\pi}{36} & \frac{\pi}{36} & \frac{\pi}{36} \end{bmatrix}^{\mathrm{T}} \tag{80}$$

<sup>7</sup> Not a Number.

#### **5.4 Control signals conditioning**

24 Will-be-set-by-IN-TECH

The last mentioned state, "*fault*" doesn't generate random additive measurement noises, but outputs NaN<sup>7</sup> 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 *yi* to the value of *y*ˆ*i*. This results in the corresponding rows and columns of the **Py** 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 **Px**(*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.

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 rows of **y** are considered those for which *any* values, inspected column-wise, fulfill:

Where **Px**(*k*) is the covariance matrix of the estimate of the full-state vector rendered by the

The additive process noise is assumed to be multivariate rate-limited white noise without bias.

0.2 0.01 0.01 0 0.005 0.005 0.01 0.1 0.0275 0.01 0.003 0.035 0.01 0.0275 0.1 0.01 0.003 0 0 0.01 0.01 0.0011 0.0001 0.00015 0.005 0.003 0.03 0.0001 0.002 0 0.005 0.035 0 0.00015 0 0.0022

� *r* (*i*,*j*) *y* �

*<sup>y</sup>* > 16 · **Px**(*k*)

��

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

�<sup>T</sup> (80)

= NaN (78)

**<sup>R</sup>***y*(*k*) = **<sup>y</sup>**(*k*)<sup>T</sup> · **<sup>y</sup>**(*k*) =

� arg*<sup>i</sup>* � ∃*j*, *r* (*i*,*j*)

The used covariance matrix is given in (79) and the rate limits in a vector in (80).

0.2 0.2 0.2 *<sup>π</sup>*

<sup>36</sup> *<sup>π</sup>* <sup>36</sup> *<sup>π</sup>* 36

*iout* =

**<sup>y</sup>** [{*iout*}] redef

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

**<sup>R</sup>**(*true*) *<sup>v</sup>* <sup>=</sup>

**<sup>n</sup>**(*true*) *<sup>v</sup>* <sup>=</sup> �

1 1.0 · <sup>10</sup>−<sup>4</sup> 1.0 · <sup>10</sup>−<sup>4</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 0 0 0 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 1.0 · <sup>10</sup>−<sup>4</sup> 1 1.0 · <sup>10</sup>−<sup>4</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 0 0 0 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 1.0 · <sup>10</sup>−<sup>4</sup> 1.0 · <sup>10</sup>−<sup>4</sup> 1 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 0 0 0 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 1 6.0 · <sup>10</sup>−<sup>4</sup> 6.0 · <sup>10</sup>−<sup>4</sup> <sup>000000</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 6.0 · <sup>10</sup>−<sup>4</sup> 1 6.0 · <sup>10</sup>−<sup>4</sup> <sup>000000</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 6.0 · <sup>10</sup>−<sup>4</sup> 6.0 · <sup>10</sup>−<sup>4</sup> <sup>1000000</sup> 0 0 0 0 0 0 1 3.0 · <sup>10</sup>−<sup>4</sup> 4.0 · <sup>10</sup>−<sup>6</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 0 0 0 0 0 0 3.0 · <sup>10</sup>−<sup>4</sup> 1 4.0 · <sup>10</sup>−<sup>6</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 0 0 0 0 0 0 4.0 · <sup>10</sup>−<sup>6</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 1 0 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 0 0 0 2.0 · <sup>10</sup>−<sup>5</sup> 4.0 · <sup>10</sup>−<sup>6</sup> <sup>1000</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 0 0 0 2.0 · <sup>10</sup>−<sup>5</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 0 3.0 · <sup>10</sup>−<sup>5</sup> 1 3.0 · <sup>10</sup>−<sup>5</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 5.0 · <sup>10</sup>−<sup>7</sup> 0 0 0 2.0 · <sup>10</sup>−<sup>5</sup> 2.0 · <sup>10</sup>−<sup>5</sup> 0 3.0 · <sup>10</sup>−<sup>5</sup> 3.0 · <sup>10</sup>−<sup>5</sup> <sup>1</sup>

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(75)

(76)

(77)

(79)

**S***<sup>y</sup>* =

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

**5.3.1 Sensor fault simulation**

**5.3.2 Outlier rejection**

SP-UKF.

**5.3.3 Process noise**

<sup>7</sup> Not a Number.

outlier measurement channel.

The solution of *f<sup>i</sup>* (*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** = [*u*˙ *<sup>c</sup> uc ψ*˙ *<sup>c</sup> ψ<sup>c</sup>* ].

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**<sup>2</sup> space is assumed in the form of *the Euler backwards formula*. So naively:

$$u(k) = \sqrt{u(k-1)^2 + T^2 f^2 + 2Tuf\_{\parallel}}\tag{81}$$

$$\begin{split} \dot{u}(k) &= \frac{1}{T}(u(k) - u(k-1)) \\\\ &= \frac{1}{T}\sqrt{u(k-1)^2 + T^2 f^2 + 2Tf\_{\parallel}u(k-1)} - \frac{u(k-1)}{T} \end{split} \tag{82}$$

$$\dot{\psi}(k) = \frac{1}{T} \arctan\left(\frac{T f\_{\perp}}{T f\_{\parallel} + u(k-1)}\right) \tag{83}$$

$$\ddot{\psi}(k) = \frac{1}{T} \left[ \frac{1}{T} \arctan \left( \frac{Tf\_{\perp}}{Tf\_{\parallel} + u(k-1)} \right) - \dot{\psi}(k-1) \right] \tag{84}$$

Where:


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.

#### **5.4.1 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 *xi*, 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:

$$\left\| \left[ f\_{\parallel} + \frac{u(k-1)}{T} \: f\_{\perp} \right]^{\mathrm{T}} \right\| \leq \frac{\overline{u}}{T} \tag{85}$$

The locus of solutions of (82) is, similarly to the preceding case, an offset *annulus* concentric to the disc described by (85):

$$\frac{\mu(k-1)}{T} - \overline{u} \le \left\| \left[ f\_{\parallel} + \frac{\mu(k-1)}{T} \: f\_{\perp} \right]^{\mathrm{T}} \right\| \le \frac{\mu(k-1)}{T} + \overline{u} \tag{86}$$

50

0

cruising in an uncluttered environment.

vehicle cruising in an uncluttered environment.

*u*(*t*) [kts]

*u*(*t*) [kts]

*u*(*t*) [kts]

*u*(*t*) [kts]

0 50 100 150 200 250 300 350 400 450

Fig. 10. Paths of the 4 HILS models of AUVs based on the precursor to the NPS *Aries* vehicle

Formation Guidance of AUVs Using Decentralized Control Functions 125

0 100 200 300 400 500 600 700 800 900

0 100 200 300 400 500 600 700 800 900

0 100 200 300 400 500 600 700 800 900

0 100 200 300 400 500 600 700 800 900

Fig. 11. Speeds of the 4 HILS models of AUVs based on the precursor to the NPS *Aries*

*y* [m]

100

150

200

250

*x* [m]

300

350

400

450

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:

$$\left| \arctan \left( \frac{a\_\perp}{a\_\parallel + \frac{\mu(k-1)}{T}} \right) \right| \le T \overline{\psi} \tag{87}$$

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:

$$T\dot{\psi}(k-1) - \frac{T^2}{2}\overline{\psi} \le \arctan\left(\frac{f\_\perp}{f\_\parallel + \frac{\iota(k-1)}{T}}\right)$$

$$\le T\dot{\psi}(k-1) + \frac{T^2}{2}\overline{\psi} \tag{88}$$

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**<sup>2</sup> shaped as an annular sector.

#### **5.4.2 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** = � *ac uc rc ψc* �*<sup>T</sup>* for the low-level controllers 41, 44).

#### **6. 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.

#### **6.1 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.

26 Will-be-set-by-IN-TECH

The locus of solutions of (83) is an *angular sector of an infinite disk* (a 1-cone) concentric to the

*a*⊥ *<sup>a</sup>*� <sup>+</sup> *<sup>u</sup>*(*k*−1) *T*

The locus of solutions of (84) is likewise an *angular sector of an infinite disk* (a 1-cone) concentric

<sup>2</sup> *<sup>ψ</sup>*¨ <sup>≤</sup> arctan

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

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

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

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

⎞ ⎠ � � � � � �

⎛ ⎝

<sup>≤</sup> *<sup>T</sup>ψ*˙(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) + *<sup>T</sup>*<sup>2</sup>

*f*⊥ *<sup>f</sup>*� <sup>+</sup> *<sup>u</sup>*(*k*−1) *T*

⎞ ⎠

<sup>≤</sup> *<sup>T</sup>ψ*˙ (87)

<sup>2</sup> *<sup>ψ</sup>*¨ (88)

previous two loci, expressed in terms of:

subset of **R**<sup>2</sup> shaped as an annular sector.

**5.4.2 Clamping the total controlling force**

**c** = �

*ac uc rc ψc*

**6. Simulation results**

**6.1 Simulation 1**

AUVs cruising in formation.

re-establish the formation.

to all the other loci, which satisfies the inequality:

� � � � � �

*<sup>T</sup>ψ*˙(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>) <sup>−</sup> *<sup>T</sup>*<sup>2</sup>

�*<sup>T</sup>* for the low-level controllers 41, 44).

arctan

⎛ ⎝

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. Speeds of the 4 HILS models of AUVs based on the precursor to the NPS *Aries* vehicle cruising in an uncluttered environment.

*x* [m]

cruising in a cluttered environment.

vehicle cruising in a cluttered environment.

*u*(*t*) [kts]

*u*(*t*) [kts]

*u*(*t*) [kts]

*u*(*t*) [kts]

0

0 50 100 150 200 250 300 350 400 450 500 550

*y* [m]

Fig. 12. Paths of the 4 HILS models of AUVs based on the precursor to the NPS *Aries* vehicle

Formation Guidance of AUVs Using Decentralized Control Functions 127

0 250 500 750 1000 1250

0 250 500 750 1000 1250

0 250 500 750 1000 1250

0 250 500 750 1000 1250 Fig. 13. Speeds of the 4 HILS models of AUVs based on the precursor to the NPS *Aries*

#### **if not (85) or (86)**

if *<sup>f</sup>*� <sup>&</sup>gt; <sup>0</sup>

*— 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.*

*<sup>f</sup>*<sup>⊥</sup> = sign(*f*⊥) min{| *<sup>f</sup>*⊥| , *<sup>u</sup>*/*T*, *<sup>u</sup>*(*<sup>k</sup>* − <sup>1</sup>)/*<sup>T</sup>* + *<sup>u</sup>*˙}

$$f\_{\parallel} = \sqrt{\min\{\overline{u}/T, \mu(k-1)/T + \overline{u}\}^2 - f\_{\perp}^2} - \mu(k-1)/T$$

*— therefore, require the f*<sup>⊥</sup> *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.*

$$\begin{aligned} f\_{\parallel} &= \max \{ f\_{\parallel \prime} - \iota (k - 1) / T, \, -\overline{\boldsymbol{u}} \} \\ f\_{\perp} &= \text{sign}(f\_{\perp}) \min \{ |f\_{\perp}|, \sqrt{(\overline{\boldsymbol{u}}/T)^{2} - (f\_{\parallel} + \iota(k - 1)/T)^{2}} \} \end{aligned}$$

*— therefore, require that f*� *component to be as large as admissible, and adjust the f*<sup>⊥</sup> *accordingly*

#### **end if**

**end if**

*ψcrit* def = minimum right-hand side of (87) and (88) **if either of the left-hand sides of (87) or (88)** > *ψcrit — if, after preceding adjustments,* ∠ (*f* − *f* · **u**(*k*)/ �**u**(*k*)�) *is inadmissible* if *<sup>f</sup>*� <sup>&</sup>gt; <sup>0</sup> *<sup>f</sup>*� <sup>=</sup> *<sup>f</sup>*� cot *<sup>ψ</sup>crit* <sup>+</sup> *<sup>f</sup>*<sup>⊥</sup> cos<sup>2</sup> *<sup>ψ</sup>crit* <sup>−</sup> (*u*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*T*) sin2 *<sup>ψ</sup>crit — again, pay attention to the intention of the manoeuvre, and if the dominant behavior is acceleration, decrease the accelerating component of <sup>f</sup>, f*�*, further* **end if**

*<sup>f</sup>*<sup>⊥</sup> <sup>=</sup> sign(*f*⊥) · (*f*� <sup>+</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*T*)tan *<sup>ψ</sup>crit — clamp the f*<sup>⊥</sup> *component to an admissible value*

#### **end if**

Table 3. Controlling Force Clamping / Saturation

#### **6.2 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.

28 Will-be-set-by-IN-TECH

*— this indicates that the required* dominant *behaviour produced*

*<sup>f</sup>*<sup>⊥</sup> = sign(*f*⊥) min{| *<sup>f</sup>*⊥| , *<sup>u</sup>*/*T*, *<sup>u</sup>*(*<sup>k</sup>* − <sup>1</sup>)/*<sup>T</sup>* + *<sup>u</sup>*˙}

min{*u*/*T*, *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*<sup>T</sup>* <sup>+</sup> *<sup>u</sup>*˙}<sup>2</sup> <sup>−</sup> *<sup>f</sup>* <sup>2</sup>

*Trying to follow through on this behaviour, it is much safer*

*and adjust the f*� *accordingly*

*<sup>f</sup>*� <sup>=</sup> max{ *<sup>f</sup>*�, <sup>−</sup>*u*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*T*, <sup>−</sup>*u*˙}

*admissible, and adjust the f*<sup>⊥</sup> *accordingly*

*<sup>f</sup>*<sup>⊥</sup> <sup>=</sup> sign(*f*⊥) · (*f*� <sup>+</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*T*)tan *<sup>ψ</sup>crit — clamp the f*<sup>⊥</sup> *component to an admissible value*

Table 3. Controlling Force Clamping / Saturation

Figure 13 presents the speeds of the four vehicles.

*— therefore, require that f*� *component to be as large as*

= minimum right-hand side of (87) and (88) **if either of the left-hand sides of (87) or (88)** > *ψcrit*

*<sup>f</sup>*<sup>⊥</sup> = sign(*f*⊥) min{| *<sup>f</sup>*⊥| ,

*— otherwise the required* dominant *behaviour produced*

*by the virtual potentials framework is to accelerate the AUV further in*

*the operational wisdom concerning real-world missions and experiences,*

*— therefore, require the f*<sup>⊥</sup> *component to be as large as admissible,*

*by the virtual potentials framework is to decelerate or break the AUV;*

*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*

*— if, after preceding adjustments,* ∠ (*f* − *f* · **u**(*k*)/ �**u**(*k*)�) *is inadmissible*

*acceleration, decrease the accelerating component of <sup>f</sup>, f*�*, further*

*<sup>f</sup>*� <sup>=</sup> *<sup>f</sup>*� cot *<sup>ψ</sup>crit* <sup>+</sup> *<sup>f</sup>*<sup>⊥</sup> cos<sup>2</sup> *<sup>ψ</sup>crit* <sup>−</sup> (*u*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*T*) sin2 *<sup>ψ</sup>crit*

*the forward direction; Trying to follow through on this behaviour, having in mind*

*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.*

<sup>⊥</sup> <sup>−</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*<sup>T</sup>*

(*u*˙/*T*)<sup>2</sup> <sup>−</sup> (*f*� <sup>+</sup> *<sup>u</sup>*(*<sup>k</sup>* <sup>−</sup> <sup>1</sup>)/*T*)2}

*— again, pay attention to the intention of the manoeuvre, and if the dominant behavior is*

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.

**if not (85) or (86)** if *<sup>f</sup>*� <sup>&</sup>gt; <sup>0</sup>

> *<sup>f</sup>*� <sup>=</sup>

*it is heading towards.*

**else**

**end if end if** *ψcrit* def

if *<sup>f</sup>*� <sup>&</sup>gt; <sup>0</sup>

**end if**

**6.2 Simulation 2**

**end if**

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

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

exacerbated by reduced manoeuvring capabilities, as all vehicles reduce speed in the vicinity

Formation Guidance of AUVs Using Decentralized Control Functions 129

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

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

1. A full-state coupled model dynamics of a seaworthy, long-autonomy AUV model based

3. A non-stationary generator of measurement noise based on Gaussian Markov models with

4. An outlier-elimination scheme based on the evaluation of the state estimate covariance

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

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

1. Realistically model the representation of knowledge of the other AUVs aboard each AUV

(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

(b) Exploring the increases in complexity (and computer resource management), numerical robustness and stability issues of AUV-local estimation of other AUVs in

on rigid-body physics and hydrodynamics of viscous fluids like water,

2. An unbiased rate-limited white noise model of the process noise,

and logistically demanding operations in the real Ocean environment.

of the way-point.

**7. Conclusion**

while cruising amidst clutter.

dynamic behaviour are featured in the system:

an explicitly included fault-mode,

returned by the employed estimator,

forward speed when decelerating.

This can be approached on several fronts:

costlier multi-beam imaging sonars,

**7.1 Further work**

locally.

the formation,

#### **6.2.1 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.

#### **6.2.2 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.

#### **6.2.3 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.

#### **6.2.4 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.
