**2.3.2 Kinematic equation**

As mentioned in 2.3.1, both the NED and body frames have properties that are useful for underwater vehicle control design. Because both are used for different purposes, a means of converting information from one frame to the other is required. The kinematic equation, (1), achieves this task (Fossen, 1994, 2002).

$$
\dot{\eta} = \mathbf{J}(\eta)\nu \tag{1}
$$

Here, the 6 degree-of-freedom (DoF) position and orientation vector in (1), decomposed in the NED frame, is denoted by (2).

$$\boldsymbol{\eta} = \begin{bmatrix} \boldsymbol{p}^{\boldsymbol{n}} & \boldsymbol{\Theta} \end{bmatrix}^{\boldsymbol{T}} \tag{2}$$

Within (2), the three position components are given in (3),

$$\mathbf{p}^n = \begin{bmatrix} \mathbf{x}\_n & \mathbf{y}\_n & \mathbf{z}\_n \end{bmatrix}^T \tag{3}$$

and the three orientation components, also known as Euler angles, are given in (4).

Fully Coupled 6 Degree-of-Freedom Control of an Over-Actuated Autonomous Underwater Vehicle 153

, ,

1 0 sin : 0 cos cos sin 0 sin cos cos

φ

<sup>⎡</sup> <sup>−</sup> <sup>⎤</sup> <sup>⎢</sup> <sup>⎥</sup> Θ = <sup>⎢</sup> <sup>⎥</sup> <sup>⎢</sup> <sup>−</sup> <sup>⎥</sup> <sup>⎣</sup> <sup>⎦</sup>

and therefore the matrix for transforming the rotational velocities from the body frame to

φ

φ

ν*J*

*MC D g*

φ

1 sin tan cos tan 0 cos sin 0 sin cos cos cos

φ

Overall, (1) achieves rotation from the body frame to the NED frame, and by taking the inverse of (8), rotation from the NED frame to the body frame can be achieved, as shown in

( ) <sup>1</sup>

The 6 DoF nonlinear dynamic equations of motion of an underwater vehicle can be

 νν

Here, *M* denotes the 6 6 × system inertia matrix containing both rigid body and added

Similar to (17), the 6 6 × Coriolis and centripetal forces matrix, including added mass, is

*CC C* (

*D DD* (ν

 ) = + *RB A* (ν

Linear and nonlinear hydrodynamic damping are contained within the 6 6 × matrix *D*(*ν*),

) = + *<sup>n</sup>* (

Here, *D* contains the linear damping terms, and *Dn*(*ν*) contains the nonlinear damping

ν

 η τ ω) + + =+ ( ) ( ) (16)

> ν

ν

η η

<sup>⎡</sup> <sup>⎤</sup> <sup>⎢</sup> <sup>⎥</sup> Θ = <sup>−</sup> <sup>⎢</sup> <sup>⎥</sup> <sup>⎢</sup> <sup>⎥</sup> <sup>⎣</sup> <sup>⎦</sup>

θ

θ

0 0

 θ

By expanding (12), the matrix for transforming the rotational velocities from the NED frame

⎡ ⎤ ⎛ ⎞ ⎡⎤ ⎡ ⎤ ⎢ ⎥ ⎜ ⎟ ⎢⎥ ⎢ ⎥ =+ + ⎢ ⎥ ⎜ ⎟ ⎢⎥ ⎢ ⎥ ⎢ ⎥ ⎜ ⎟ ⎢⎥ ⎢ ⎥ ⎣ ⎦ ⎝ ⎠ ⎣⎦ ⎣ ⎦

0 0 *b TT nb R R <sup>x</sup>* φ

φ

ω

( ) <sup>1</sup>

*T*

− Θ

( )

*T*

conveniently expressed as (16) (Fossen, 1994, 2002).

ν + (νν

Θ

to the body frame is defined as (13),

the NED frame is given in (14).

**2.3.3 Kinetic equation** 

mass, as given by (17).

given by (18).

and given by (19).

terms.

(15).

0 0

 θ*y*

ψ

θ

θ φ

θ φ

> φθ

 φθ

 φ

<sup>−</sup> = (15)

*M* = *M M RB A* + (17)

) ( ) (18)

) (19)

(12)

(13)

(14)

$$
\boldsymbol{\Theta} = \begin{bmatrix} \boldsymbol{\phi} & \boldsymbol{\theta} & \boldsymbol{\nu} \end{bmatrix}^{\mathrm{T}} \tag{4}
$$

The 6 DoF translational and rotational velocity vector in (1), decomposed in the body frame, is denoted by (5).

$$\boldsymbol{\nu} = \begin{bmatrix} \boldsymbol{\upsilon}\_o^b & \boldsymbol{\alpha}\_{nb}^b \end{bmatrix}^T \tag{5}$$

Here, the three translational velocity components are given in (6),

$$\begin{bmatrix} \upsilon\_o^b = \begin{bmatrix} \mu & \upsilon & w \end{bmatrix}^T \end{bmatrix} \tag{6}$$

and the three rotational velocity components are given in (7).

$$
\alpha\_{nb}^b = \begin{bmatrix} p & q & r \end{bmatrix}^T \tag{7}
$$

In order to rotate from one frame to the other, a transformation matrix is used in (1). This transformation matrix is given in (8).

$$J(\eta) = \begin{bmatrix} R\_b^{\eta} \left( \Theta \right) & \mathbf{0\_{3 \times 3}} \\ \mathbf{0\_{3 \times 3}} & T\_\Theta \left( \Theta \right) \end{bmatrix} \tag{8}$$

Here, the transformation of the translational velocities from the body frame to the NED frame are achieved by rotating the translational velocities in the body frame, (6), using the Euler angles (4). Three principal rotation matrices are used in this operation, as shown in (9).

$$\boldsymbol{R}\_{\boldsymbol{x},\phi} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} \boldsymbol{R}\_{\boldsymbol{y},\theta} = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \boldsymbol{R}\_{\boldsymbol{z},\psi} = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \tag{9}$$

The order of rotation is not arbitrary, due to the compounding effect of the rotation order. Within guidance and control, it is common to use the *zyx*-convention where rotation is achieved using (10).

$$R\_b^{\prime\prime} \left( \Theta \right) = R\_{z,\mu} R\_{y,\theta} R\_{x,\phi} \tag{10}$$

Overall, this yields the translational rotation matrix (11).

$$R\_b'' \begin{bmatrix} \cos \psi \cos \theta & -\sin \psi \cos \phi + \cos \psi \sin \theta \sin \phi & \sin \psi \sin \phi + \cos \psi \sin \theta \cos \phi \\\\ \sin \psi \cos \theta & \cos \psi \cos \phi + \sin \psi \sin \theta \sin \phi & -\cos \psi \sin \phi + \sin \psi \sin \theta \cos \phi \\\\ -\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi \end{bmatrix} \tag{11}$$

The transformation of rotational velocities from the body frame to the NED frame is achieved by again applying the principal rotation matrices of (9). For ease of understanding, firstly consider the rotation from the NED frame to the body frame in which ψ is rotated by *Ry*,θ , added to θ , and this sum then rotated by *Rx*,φ and finally added to φ . This process is given in (12).

$$\boldsymbol{\alpha}\_{nb}^{b} = \begin{bmatrix} \dot{\boldsymbol{\theta}} \\ \mathbf{0} \\ \mathbf{0} \end{bmatrix} + \boldsymbol{R}\_{\mathbf{x},\boldsymbol{\theta}}^{T} \begin{pmatrix} \mathbf{0} \\ \dot{\boldsymbol{\theta}} \\ \mathbf{0} \end{pmatrix} + \boldsymbol{R}\_{\mathbf{y},\boldsymbol{\theta}}^{T} \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \\ \dot{\boldsymbol{\nu}} \end{bmatrix} \tag{12}$$

By expanding (12), the matrix for transforming the rotational velocities from the NED frame to the body frame is defined as (13),

$$T\_{\Theta}^{-1}(\Theta) \coloneqq \begin{bmatrix} 1 & 0 & -\sin\theta \\ 0 & \cos\phi & \cos\theta\sin\phi \\ 0 & -\sin\phi & \cos\theta\cos\phi \end{bmatrix} \tag{13}$$

and therefore the matrix for transforming the rotational velocities from the body frame to the NED frame is given in (14).

$$T\_{\Theta} \begin{pmatrix} \Phi \\ \end{pmatrix} = \begin{vmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{vmatrix} \tag{14}$$

Overall, (1) achieves rotation from the body frame to the NED frame, and by taking the inverse of (8), rotation from the NED frame to the body frame can be achieved, as shown in (15).

$$\boldsymbol{\nu} = \boldsymbol{f}^{-1}(\boldsymbol{\eta}) \boldsymbol{\dot{\eta}}\tag{15}$$

### **2.3.3 Kinetic equation**

152 Autonomous Underwater Vehicles

[ ]*<sup>T</sup>* Θ = φ θ ψ

The 6 DoF translational and rotational velocity vector in (1), decomposed in the body frame,

ν

*nb* ω

( ) ( )

η

,,,

*RRR xy z* φθψ

 φ

φφ

Overall, this yields the translational rotation matrix (11).

 ψ φ

 ψ φ

θ

θ

θ

Here, the three translational velocity components are given in (6),

and the three rotational velocity components are given in (7).

transformation matrix is given in (8).

φ

ψ

ψ

θ

achieved using (10).

( )

, added to

given in (12).

*n Rb*

*Ry*,θ *<sup>T</sup> b b o nb*

 ω

[ ] *<sup>b</sup> <sup>T</sup>*

[ ] *<sup>b</sup> <sup>T</sup>*

In order to rotate from one frame to the other, a transformation matrix is used in (1). This

3 3

Here, the transformation of the translational velocities from the body frame to the NED frame are achieved by rotating the translational velocities in the body frame, (6), using the Euler angles (4). Three principal rotation matrices are used in this operation, as shown in (9).

θ

 θ

The order of rotation is not arbitrary, due to the compounding effect of the rotation order. Within guidance and control, it is common to use the *zyx*-convention where rotation is

> ( ) , ,, *<sup>n</sup> R RRR b z* Θ = ψ *y* θ φ

θ φ

firstly consider the rotation from the NED frame to the body frame in which

, and this sum then rotated by *Rx*,

cos cos sin cos cos sin sin sin sin cos sin cos sin cos cos cos sin sin sin cos sin sin sin cos sin cos sin cos cos

ψθφ

The transformation of rotational velocities from the body frame to the NED frame is achieved by again applying the principal rotation matrices of (9). For ease of understanding,

ψθφ

⎡ ⎤ − + + ⎢ ⎥ Θ = <sup>+</sup> − +

− ⎣ ⎦

φ

10 0 cos 0 sin cos sin 0 0 cos sin , 0 1 0 , sin cos 0 0 sin cos sin 0 cos 0 01

⎡ ⎤⎡ ⎤⎡ ⎤ − ⎢ ⎥⎢ ⎥⎢ ⎥ =−= <sup>=</sup> − ⎣ ⎦⎣ ⎦⎣ ⎦

 <sup>×</sup> × Θ <sup>⎡</sup> <sup>Θ</sup> <sup>⎤</sup> <sup>=</sup> <sup>⎢</sup> <sup>⎥</sup> ⎢⎣ <sup>Θ</sup> ⎥⎦

0 *<sup>n</sup> Rb <sup>J</sup> <sup>T</sup>*

( ) 3 3

> θ

 θ

> ψφ

> > ψφ

and finally added to

0

is denoted by (5).

(4)

(8)

ψψ

*<sup>x</sup>* (10)

θ φ

ψθφ

ψ

φ

ψθφ

 ψ (9)

(11)

is rotated by

. This process is

ψ

<sup>=</sup> <sup>⎡</sup>*<sup>v</sup>* <sup>⎤</sup> <sup>⎣</sup> <sup>⎦</sup> (5)

*<sup>o</sup> v uvw* = (6)

= *p q r* (7)

The 6 DoF nonlinear dynamic equations of motion of an underwater vehicle can be conveniently expressed as (16) (Fossen, 1994, 2002).

$$M\dot{\nu} + \mathcal{C}(\nu)\nu + \mathcal{D}(\nu)\nu + \mathcal{g}(\eta) = \tau + o\tag{16}$$

Here, *M* denotes the 6 6 × system inertia matrix containing both rigid body and added mass, as given by (17).

$$M = M\_{RB} + M\_A \tag{17}$$

Similar to (17), the 6 6 × Coriolis and centripetal forces matrix, including added mass, is given by (18).

$$\mathbf{C}\left(\nu\right) = \mathbf{C}\_{RB}\left(\nu\right) + \mathbf{C}\_{A}\left(\nu\right) \tag{18}$$

Linear and nonlinear hydrodynamic damping are contained within the 6 6 × matrix *D*(*ν*), and given by (19).

$$D\left(\nu\right) = D + D\_n\left(\nu\right) \tag{19}$$

Here, *D* contains the linear damping terms, and *Dn*(*ν*) contains the nonlinear damping terms.

Fully Coupled 6 Degree-of-Freedom Control of an Over-Actuated Autonomous Underwater Vehicle 155

The presence of nonlinearities contained within *Dn*(*ν*), combined with the coupling effect of any non-zero off-diagonal elements within all matrices, can lead to a highly complex model

Various control strategies, and therefore control laws, have been implemented for AUV

The benchmark for control systems would be the classical proportional-integral-derivative (PID) control that has been used successfully to control many different plants, including autonomous vehicles. PID schemes are, however, not very effective in handling the nonlinear AUV dynamics with uncertain models operating in unknown environments with strong wave and current disturbances. PID schemes are therefore only generally used for

An alternative control scheme known as sliding mode control (SMC), which is a form of variable structure control, has proven far more effective and robust at handling nonlinear dynamics with modelling uncertainties and nonlinear disturbances. SMC is a nonlinear control strategy which uses a nonlinear switching term to obtain a fast transient response while still maintaining a good steady-state response. Consequently, SMC has been successfully applied by many researchers in the AUV community. One of the earliest applications of using SMC to control underwater vehicles was conducted by Yoerger and Slotine wherein the authors demonstrated through simulations studies on an ROV model, the SMC controller's robustness properties to parametric uncertainties (Yoerger & Slotine, 1985). A multivariable sliding mode controller based on state feedback with decoupled design for independently controlling velocity, steering and diving of an AUV is presented in Healey and Lienard (1993). The controller design was successfully implemented on NPS

The fundamentals of PID control is that an error signal is generated that relates the desired

Where *e*(*t*) is the error signal, *xd*(*t*) is the desired state of the plant, and *x*(*t*) is the current state of the plant, and this error signal is manipulated to introduce a corrective action, denoted

PID control is named due to the fact that the three elements that make up the corrective control signal are: *proportional* to the error signal by a factor of *KP*, a scaled factor, *KI*, of the *integral* of the error signal, and a scaled factor, *KD*, of the *derivative* of the error signal,

> () () ( ) ( ) 0

PID control is best suited to linear plants, yet has also been adopted for use on nonlinear plants even though it lacks the same level of performance that other control systems possess.

λ λ

*t PI D <sup>d</sup> t Ket K e d K et*

=+ +

*et x t xt* ( ) = − *<sup>d</sup>* ( ) ( ) (26)

∫ (27)

*dt*

very simple AUVs working in environments without any external disturbances.

containing a large number of coefficients.

ARIES AUV as reported in Marco and Healey (2001).

τ

state of the plant to the actual state (26),

**2.4 Control laws** 

**2.4.1 PID control** 

*τ*(*t*), to the plant.

respectively (27).

systems.

The 6 1 × vector of gravitational and buoyancy forces and moments are represented in (16) by *g*(*η*), and determined using (20).

$$\begin{pmatrix} (\mathcal{W} - B)\sin\theta \\ -(\mathcal{W} - B)\cos\theta\sin\phi \\ -(\mathcal{W} - B)\cos\theta\cos\phi \\ -\left(\mathcal{Y}\_{\mathcal{S}}\mathcal{W} - \mathcal{Y}\_{b}B\right)\cos\theta\cos\phi + \left(z\_{\mathcal{S}}\mathcal{W} - z\_{b}B\right)\cos\theta\sin\phi \\ \left(z\_{\mathcal{S}}\mathcal{W} - z\_{b}B\right)\sin\theta + \left(x\_{\mathcal{S}}\mathcal{W} - x\_{b}B\right)\cos\theta\cos\phi \\ -\left(x\_{\mathcal{S}}\mathcal{W} - x\_{b}B\right)\cos\theta\sin\phi - \left(y\_{\mathcal{S}}\mathcal{W} - y\_{b}B\right)\sin\theta \end{pmatrix} \tag{20}$$

Here, *W* is the weight of the vehicle, determined using *W=mg* where *m* is the dry mass of the vehicle and *g* is the acceleration due to gravity. *B* is the buoyancy of the vehicle which is due to how much fluid the vehicle displaces while underwater. This will be determined by the size and shape of the vehicle. Vectors determining the locations of the centre of gravity and the centre of buoyancy, relative to the origin of the body frame, are given by (21) and (22) respectively.

$$\mathbf{r}\_{\mathcal{S}}^{\mathcal{b}} = \begin{bmatrix} \mathbf{x}\_{\mathcal{S}} & \mathbf{y}\_{\mathcal{S}} & z\_{\mathcal{S}} \end{bmatrix}^{\mathrm{T}} \tag{21}$$

$$\mathbf{r}\_b^b = \begin{bmatrix} \mathbf{x}\_b & \mathbf{y}\_b & \mathbf{z}\_b \end{bmatrix}^T \tag{22}$$

The 6 1 × vector of control input forces is denoted by *τ*, and is given by (23).

$$
\boldsymbol{\pi} = \begin{bmatrix} \mathbf{X} & \mathbf{Y} & \mathbf{Z} & \mathbf{K} & \mathbf{M} & \mathbf{N} \end{bmatrix}^{\mathrm{T}} \tag{23}
$$

Here, the translational forces affecting surge, sway and heave are *X*, *Y*, and *Z* respectively, and the rotational moments affecting roll, pitch and yaw are *K*, *M* and *N* respectively.

The 6 1 × vector of external disturbances is denoted by *ω*.

Overall, (16) provides a compact representation for the nonlinear dynamic equations of motion of an underwater vehicle, formulated in the body frame. By applying the rotations contained within (8), (16) can be formulated in the NED frame as given in (24).

$$M\_{\eta}(\eta)\ddot{\eta} + \mathcal{C}\_{\eta}(\nu,\eta)\dot{\eta} + D\_{\eta}(\nu,\eta)\dot{\eta} + \mathcal{g}\_{\eta}(\eta) = \boldsymbol{\tau}\_{\eta}(\eta) + o\tag{24}$$

Within (24), the equations in (25) contain the rotations of the various matrices from the body frame to the NED frame.

$$\begin{aligned} M\_{\eta}(\eta) &= \mathbb{J}^{-T}(\eta) M \mathbb{J}^{-1}(\eta) \\ \mathbb{C}\_{\eta}(\nu, \eta) &= \mathbb{J}^{-T}(\eta) \Big[ \mathbb{C}(\nu) - M \mathbb{J}^{-1}(\eta) \dot{\mathbb{J}}(\eta) \Big] \mathbb{J}^{-1}(\eta) \\ D\_{\eta}(\nu, \eta) &= \mathbb{J}^{-T}(\eta) D(\nu) \mathbb{J}^{-1}(\eta) \\ \text{g}\_{\eta}(\eta) &= \mathbb{J}^{-T}(\eta) \mathbb{g}(\eta) \\ \tau\_{\eta}(\eta) &= \mathbb{J}^{-T}(\eta) \tau \end{aligned} \tag{25}$$

The presence of nonlinearities contained within *Dn*(*ν*), combined with the coupling effect of any non-zero off-diagonal elements within all matrices, can lead to a highly complex model containing a large number of coefficients.
