# SMCSPO-Based Robust Control of AUV in Underwater Environments including Disturbances
## 2 Dynamics Modeling of AUV  
### Underwater Vehicle Dynamics  
Figure 1 shows the coordinate systems representation of the AUV. The coordinate systems and dynamics equations were derived based on [PERSON]'s model [2]. The notation \(\eta=[\text{x},\text{ y},\text{ z},\text{ }\phi,\text{ }\theta,\text{ }\psi]\) describes the position and orientation of the vehicle with respect to the Earth-fixed reference frame, \(\upsilon=[\text{u},\text{v},\text{w},\text{p},\text{q},\text{r}]\) is the translational and rotational velocities of the vehicle with respect to the body-fixed reference frame, and \(\tau=[\text{X},\text{Y},\text{Z},\text{K},\text{M},\text{N}]\) is the total forces and moments acting on the vehicle with respect to the body-fixed reference frame.  
The dynamics model of the AUV is as shown in (1) to (6) [18]. The shape of the AUV is assumed to be the shape of a torpedo, and six equations of motion are defined for surge, heave, sway, roll, pitch, and yaw directions.  
\[\text{m}\dot{u}=X_{\dot{u}}^{\prime}\dot{u}+X_{u|u|}^{\prime}u|u|-X_{U|U|}^{ \prime}U|U|-(W-B)sin\theta, \tag{1}\]  
\[\text{m}\dot{v}=Y_{\dot{v}}^{\prime}\dot{v}+Y_{\dot{p}}^{\prime}\dot{p}+Y_{ \dot{r}}^{\prime}+Y_{v}^{\prime}\dot{v}+Y_{p}^{\prime}+Y_{r}^{\prime}r+(W-B)cos \theta sin\phi+Y_{\dot{\theta}_{r}}{}^{\prime}U^{2}\delta_{r}-mu\
u, \tag{2}\]  
\[\text{m}\dot{w}=Z_{\dot{w}}^{\prime}\dot{w}+Z_{\dot{q}}^{\prime}\dot{q}+Z_{v}^ {\prime}w+Z_{q}^{\prime}q+(W-B)cos\theta cos\phi+Z_{\dot{\theta}_{r}}{}^{\prime }U^{2}\delta_{\theta_{p}}-muq, \tag{3}\]  
\[I_{\text{xx}}\dot{p}=K_{\dot{v}}^{\prime}\dot{v}+K_{\dot{p}}^{\prime}\dot{p}+K _{\dot{r}}^{\prime}+K_{v}^{\prime}v+K_{p}^{\prime}p+K_{r}^{\prime}r+(z_{B}sin \phi-y_{B}cos\phi)Bcos\theta+K_{\dot{\theta}_{r}}{}^{\prime}U^{2}\delta_{\theta _{m}}, \tag{4}\]\[I_{yy}\dot{q}=M^{\prime}_{\dot{u}}\dot{w}+M^{\prime}_{\dot{q}}\dot{q}+M^{\prime}_{w }w+M^{\prime}_{q}q+x_{B}Bcos\theta cos\phi+z_{B}Bsin\theta+{M_{\delta_{s}}}^{ \prime}U^{2}\delta_{s_{p}}, \tag{5}\]  
\[I_{zz}\dot{r}=N^{\prime}_{v}\dot{v}+N^{\prime}_{p}\dot{p}+N^{\prime}_{r}r+N^{ \prime}_{v}v+N^{\prime}_{p}p+N^{\prime}_{r}r+x_{B}Bcos\theta sin\phi+y_{B}Bsin \theta+N_{\delta_{r}}^{\prime}U^{2}\delta_{r}. \tag{6}\]  
where \(\tau=[X,Y,Z,K,M,N]\) is the hydrodynamic derivative coefficient values, which contain added mass, drag force, and fin lift force terms; \(U=\sqrt{u^{2}+v^{2}+w^{2}}\) is the operating speed of the vehicle; W is the weight of the vehicle; B is the buoyancy; \(I=\left[I_{xx},I_{yy},I_{zz}\right]\) is the moment of inertia; and \(\delta=\left[\delta_{s_{p}},\delta_{s_{m}},\delta_{r}\right]\) is the angle of the control fins of the vehicle. Here, \(\delta_{r}\) is the rudder angle of attack. The other control fins, \(\delta_{s_{p}}\) and \(\delta_{s_{m}}\), are defined by the stern plane angle of attack as shown in (7) and (8).  
\[\delta_{s_{p}}=\frac{1}{2}\big{(}\delta_{s_{r}}+\delta_{s_{1}}\big{)}, \tag{7}\]  
\[\delta_{s_{m}}=\delta_{s_{r}}-\delta_{s_{1}}, \tag{8}\]  
where \(\delta_{s_{r}}\) is the right stern plane angle of attack, and \(\delta_{s_{1}}\) is the left stern plane angle of attack. The Earth-fixed coordinate system and the body-fixed coordinate system have the following transformation relation [2].  
\[\left[\begin{array}{c}\dot{\phi}\\ \dot{\theta}\\ \dot{\Psi}\end{array}\right]=\text{J}(\eta)\left[\begin{array}{c}p\\ q\\ r\end{array}\right], \tag{9}\]  
\[\text{J}(\eta)=\left[\begin{array}{ccc}1&sin\theta tan\theta&cos\phi tan \theta\\ 0&cos\phi&-sin\phi\\ 0&sin\phi/cos\theta&cos\phi/cos\theta\end{array}\right]. \tag{10}\]  
If \(\cos\theta=0\) in (10), a singularity can occur. However, pitch angles of \(\pm 90\) degrees are not considered in this study because these are abnormal AUV attitudes. Therefore, the pitch angle of the AUV is limited within \(\pm 90\) degrees.