**2. Kinematic and Dynamic Modeling of AUV**

Six degrees of freedom motion equations of AUV can be described using the earthfixed coordinate system and body-fixed coordinate system shown in Figure 1, both of which are right-handed. The earth-fixed coordinate system *O* − xyz has its origin *O* fixed to the earth, and the body-fixed coordinate system *Ob* − *xbybzb* is a moving reference frame with its origin *Ob* fixed to AUV center of buoyancy.

The general motion of a vehicle in six degrees of freedom can be described with the following vectors:

$$\begin{array}{l} \eta\_1 = \begin{bmatrix} x & y & z \end{bmatrix}^T \eta\_2 = \begin{bmatrix} \phi & \theta & \psi \end{bmatrix}^T\\ v\_1 = \begin{bmatrix} u & v & w \end{bmatrix}^T v\_2 = \begin{bmatrix} p & q & r \end{bmatrix}^T\\ \tau\_1 = \begin{bmatrix} X & Y & Z \end{bmatrix}^T \tau\_2 = \begin{bmatrix} K & M & N \end{bmatrix}^T \end{array}$$

where *η* describes the position and orientation of the vehicle with respect to the earth-fixed reference frame, *v* denotes the linear and angular velocities with respect to the body-fixed reference frame, and *τ* describes the total forces and moments acting on the vehicle in the body-fixed reference frame.

**Figure 1.** Coordinate systems of AUV.

The coordinate transformation of the translational velocity between earth-fixed and body-fixed coordinate systems can be expressed as

$$
\begin{bmatrix}
\dot{\boldsymbol{x}} \\
\dot{\boldsymbol{y}} \\
\dot{\boldsymbol{z}}
\end{bmatrix} = J\_1 \begin{bmatrix}
\boldsymbol{u} \\
\boldsymbol{v} \\
\boldsymbol{w}
\end{bmatrix} \tag{1}
$$

where

$$J\_1 = \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}$$

The coordinate transformation relates rotational velocity between two coordinate systems and can be described as ⎡ .

> ⎢ ⎣

.

.

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{bmatrix} = J\_2 \begin{bmatrix}
p \\ q \\ r
\end{bmatrix} \tag{2}
$$

where

$$J\_2 = \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{bmatrix}$$

The locations of the AUV centers of gravity and buoyancy are defined in the body-fixed coordinate system as follows:

$$r\_G = \begin{bmatrix} \mathbf{x}\_{\mathcal{S}} & y\_{\mathcal{S}} & z\_{\mathcal{S}} \end{bmatrix}^T \quad r\_B = \begin{bmatrix} \mathbf{x}\_b & y\_b & z\_b \end{bmatrix}^T$$

Based on the theory of rigid body dynamics and the analysis of total forces and moments acting on AUV, the nonlinear motion equations for the REMUS vehicle in six degrees of freedom can be expressed as follows [24]:

$$\begin{aligned} m[\dot{u} - wr + wq - x\_{\xi}(q^2 + r^2) + y\_{\xi}(pq - \dot{r}) + z\_{\xi}(pr + \dot{q})] &= X\_{HS} + X\_{u|u|}u|u| + |\\ X\_{\dot{u}}\dot{u} + X\_{u|q|}wq + X\_{u|q|}qq + X\_{vv}vr + X\_{rr}rr + X\_{prop} \\ m[\dot{v} - wp + ur - y\_{\xi}(r^2 + p^2) + z\_{\xi}(qr - \dot{p}) + x\_{\xi}(pq + \dot{q}) + \dot{r}] &= Y\_{HS} + Y\_{|v|}v|v| + Y\_{|r|}r|r| + \\ Y\_{\dot{u}}\dot{v} + Y\_{\dot{r}}r + Y\_{uv}ur + Y\_{xy}wpr + Y\_{pq}pq + Y\_{uu}pv + Y\_{uu}u\dot{v} + Z\_{lr}l\dot{\xi} \\ m[\dot{w} - uq + vp - z\_{\xi}(p^2 + q^2) + x\_{\xi}(rp - \dot{q}) + y\_{\xi}(rq + \dot{p})] &= Z\_{HS} + Z\_{w|w|}w|w| + Z\_{u|q|}q|q| + \\ Z\_{\dot{u}}\dot{w}\dot{v} + Z\_{\dot{u}}\dot{q} + Z\_{uu}uq + Z\_{vp}vp + Z\_{u|vp|}r + Z\_{uu}uw + Z\_{uu}u^2\dot{\xi} \\ L\_{\xi}\dot{w} + (L\_{\xi x} - l\_{\xi y})qr + m[y\_{\xi}(\dot{w} - uq + vp) - z\_{\xi}(\dot{v} - uvp + ur)] &= K\_{HS} + K\_{p|p|}p|p| + K\_{p}\dot{p} + K\_{prop} \\ l\_{\xi}\dot{p}q + (l\_{\xi x} - l\_{\xi y})rp + m[\xi\_{\xi}(\dot{u} - uv + vq) - x\_{\xi$$

where *m* is AUV's mass, *Ixx*, *Iyy*, *Izz* are the moments of inertia of AUV to three coordinate axes, *XHS*,*YHS*, *ZHS*, *KHS*, *MHS*, *NHS* are hydrostatics, *Xu*|*u*|,*Yv*|*v*|,*Yr*|*r*|, *Zw*|*w*|, *Zq*|*q*|, *Kp*|*p*|, *Mw*|*w*|, *Mq*|*q*|, *Nv*|*v*|, *Nr*|*r*<sup>|</sup> are hydrodynamic drag coefficients, *Yuv*,*Yuu<sup>δ</sup>r*, *Zuw*, *Zuu<sup>δ</sup>s*, *Muw*, *Muu<sup>δ</sup>s*,*Nuv*, *Nuu<sup>δ</sup><sup>r</sup>* are lift coefficients and lift moment coefficients of body and control fins, respectively, *Xprop*, *Kprop* are propeller thrust and torque, respectively, and *δr*, *δ<sup>s</sup>* are rudder angle and stern plane angle, respectively. The remaining coefficients are added mass coefficients.

Separate the acceleration terms from the other terms in the equations of AUV motion so that the equations can be summarized in matrix form as follows:

$$
\begin{bmatrix}
\dot{u} \\
\dot{v} \\
\dot{w} \\
\dot{p} \\
\dot{q} \\
\dot{r}
\end{bmatrix} = \begin{bmatrix}
m - X\_{\dot{u}} & 0 & 0 & 0 & mz\_{\mathcal{S}} & -my\_{\mathcal{S}} \\
0 & m - Y\_{\dot{v}} & 0 & -mz\_{\mathcal{S}} & 0 & mx\_{\mathcal{S}} - Y\_{\dot{r}} \\
0 & 0 & m - Z\_{\dot{w}} & my\_{\mathcal{S}} & -mx\_{\mathcal{S}} - Z\_{\dot{q}} & 0 \\
0 & -mz\_{\mathcal{S}} & my\_{\mathcal{S}} & I\_{xx} - K\_{\dot{p}} & 0 & 0 \\
mz\_{\mathcal{S}} & 0 & -mx\_{\mathcal{S}} - M\_{\dot{w}} & 0 & I\_{yy} - M\_{\dot{q}} & 0 \\
\end{bmatrix}^{-1} \begin{bmatrix}
\sum X \\
\sum Y \\
\sum Y \\
\sum Z \\
\sum M \\
\sum M \\
\sum N
\end{bmatrix} \tag{4}
$$

where ∑ *X* ··· ∑ *N* refer to the sum of terms without acceleration. So far, six degrees of freedom nonlinear motion equations of AUV can be obtained by combining (4) with (1) and (2).
