## 2 Mathematical Model  
### Submarine Motion Equation  
Figure 1 illustrates the coordinate systems of a submarine, where the fixed coordinate system E-\(\xi\eta\xi\) is associated with the Earth and is the reference, and the moving coordinate system O-xyz moves along with the vehicle. Both coordinate systems are right-handed. This study employs the standard submarine motion equations proposed by [PERSON] in 1967 [1]. Considering that the submarine undergoes low-speed and low-amplitude motion, the influence of the propeller load is neglected. Therefore, it is assumed that the propeller load remains constant during the maneuvering process of the submarine. The specific expression is as follows:  
\[ \begin{pmatrix}m-X_{\dot{u}}&0&0&mz_{G}&-my_{G}\\ 0&m-Y_{\dot{v}}&0&-mz_{G}-Y_{\dot{p}}&0&mx_{G}-Y_{\dot{r}}\\ 0&0&m-Z_{\dot{w}}&my_{G}&-mx_{G}-Z_{\dot{q}}&0\\ 0&-mz_{G}-K_{\dot{v}}&my_{G}&I_{x}-K_{\dot{p}}&-I_{xy}&-I_{zx}-K_{\dot{r}}\\ mz_{G}&0&-mx_{G}-M_{\dot{w}}&-I_{xy}&I_{y}-M_{\dot{q}}&-I_{yz}\\ -my_{G}&mx_{G}-N_{\dot{v}}&0&-I_{zx}-N_{\dot{p}}&-I_{yz}&I_{z}-N_{\dot{r}} \end{pmatrix}\begin{pmatrix}\dot{u}\\ \dot{v}\\ \dot{w}\\ \dot{p}\\ \dot{q}\\ \dot{r}\end{pmatrix} = \begin{pmatrix}X_{1}\\ Y_{1}\\ Z_{1}\\ K_{1}\\ M_{1}\\ N_{1}\end{pmatrix}  \tag{1}\]  
where \((x_{G},y_{G},z_{G})\) denotes the coordinates of the center of gravity in the moving coordinate system; and \(u\), \(v\), \(w\), \(p\), \(q\), and \(r\) represent the linear velocities along the \(x\), \(y\), and \(z\) axes, and the angular velocities around the \(x\), \(y\), and \(z\) axes, respectively. In Equation (1), the origin of the moving coordinate system does not coincide with the center of gravity. Additionally, \(X_{1}\), \(Y_{1}\), \(Z_{1}\), \(K_{1}\), \(M_{1}\), and \(N_{1}\) represent the axial force, lateral force, vertical force, rolling moment, pitching moment, and yawing moment acting on the submarine. The specific expressions for these terms are presented in Equations (2)-(7):  
\[ \begin{array}{l}X_{1}\ =\frac{1}{2}\rho L^{4}\Big{[}X_{qq}^{\prime}q^{2}+X_{ rr}^{\prime}r^{2}+X_{pp}^{\prime}p^{2}\Big{]}+\frac{1}{2}\rho L^{2}\big{[}X_{uu}^{ \prime}u^{2}+X_{vv}^{\prime}v^{2}+X_{ww}^{\prime}w^{2}\big{]}\\ +\frac{1}{2}\rho L^{3}\Big{[}X_{rr}^{\prime}vr+X_{uu}^{\prime}uqq\Big{]}+\frac {1}{2}\rho L^{2}u^{2}\big{[}X_{\dot{q},\dot{r},\dot{\theta}}^{\prime}z+X_{ \dot{q},\dot{\phi}}^{\prime}\dot{\phi}_{\dot{\theta}}^{2}+X_{\dot{\theta},\dot {\phi}}^{\prime}\dot{\phi}_{\dot{\phi}}z^{2}\big{]}\\ -m\big{[}vr+wq-x_{G}(q^{2}+r^{2})+y_{G}pq+z_{G}pr\big{]}-(P-B)\sin\theta+F_{p} \end{array}  \tag{2}\]  
Figure 1: Coordinate systems of a submarine. \[ \begin{array}{rl}Y_{1}&=\frac{1}{2}\rho L^{4}\Big{(}Y^{\prime}_{p|p|}p|p|+Y^{ \prime}_{pq}pq+Y^{\prime}_{qr}qr\Big{)}+\frac{1}{2}\rho L^{3}\Big{(}Y^{\prime}_{ pq}vq+Y^{\prime}_{wp}wp+Y^{\prime}_{wr}wrr\Big{)}\\ &+\frac{1}{2}\rho L^{2}\Big{[}Y^{\prime}_{wp}vw+Y^{\prime}_{\delta_{r}}u^{2 \delta_{r}}\Big{]}+\frac{1}{2}\rho L^{2}\Big{[}Y^{\prime}_{q}u^{2}+Y^{\prime}_ {q}uv+Y^{\prime}_{|pv|}v\Big{[}(v^{2}+w^{2})^{1/2}\Big{]}\Big{]}\\ &+\frac{1}{2}\rho L^{3}\Big{(}Y^{\prime}_{rr}ur+Y^{\prime}_{|r|\delta_{r}}u|r| \delta_{r}+Y^{\prime}_{p}uvp+Y^{\prime}_{|r|}\frac{v}{|v|}\big{[}(v^{2}+w^{2}) ^{1/2}\Big{]}\Big{|}|r|\Big{)}\\ &-m\big{[}ur-wp+x_{G}pq-y_{G}(p^{2}+r)+z_{G}qr\big{]}+(P-B)\cos\theta\sin\varphi \end{array}  \tag{3}\]  
\[