# Three-Dimensional Modeling of a Robotic Fish Based on Real Carp Locomotion
## 3 Implementations of the Fish-Like Motion
### Guidance and Trajectory Tracking  
The proposed model of the robotic fish including guidance and trajectory tracking consists of a three-layered hierarchical architecture: inner loop, outer loop and trajectory generator. The inner loop is controlled with the simple and practical control structure inspired by Reaching Law Control (RLC) approach. The inner loop occurs in the body frame of the robotic fish. The outer loop deals with the guidance control structure. On the other hand, the trajectory generator builds up the 3D path tracking in earth-fixed frame. The block diagram of the closed loop system is shown in Figure 14.  
In Figure 14, \(T\), \(\eta\), \(G\) and \(F\) indicate reference trajectory, current positions, reference of the guidance, and current orientations and speed, respectively. These parameters are defined as below:  
\[\left\{\begin{array}{l}T=\left[ \begin{array}{ccc}X_{ref}&Y_{ref}&Z_{ref}\\ \eta^{\prime}=\left[\begin{array}{ccc}x&y&z\\ \end{array}\right]^{T}\\ \eta^{\prime\prime}=\left[\begin{array}{ccc}\phi&\theta&\psi\\ \end{array}\right]^{T}\\ G=\left[\begin{array}{ccc}\theta_{ref}&\psi_{ref}&v_{ref}\\ \end{array}\right]^{T}\\ F=\left[\begin{array}{ccc}\theta&\psi&v\\ \end{array}\right]^{T}\end{array} \right. \tag{22}\]  
Figure 14: Block diagram of the proposed guidance and trajectory tracking with RLC. Figure 13: Thrust force of the robotic fish for free-swimming motion during 80s simulation time. It is noted that the reference matrix of the guidance is calculated by using a trigonometrical approach and that each element of the matrix can be given by Equation (23):  
\[\left\{ \begin{array}{l}\theta_{ref}=-\tan^{-1}\biggl{(}\left(z_{ref}-z\right)/ \left(\sqrt{\left(x_{ref}-x\right)^{2}+\left(y_{ref}-y\right)^{2}}\right)\biggr{)} \\ \psi_{ref}=\tan^{-1}\Bigl{(}\left(y_{ref}-y\right)/\left(x_{ref}-x\right) \Bigr{)}\\ v_{ref}=\zeta\sqrt{\left(x_{ref}-x\right)^{2}+\left(y_{ref}-y\right)^{2}+\left( z_{ref}-z\right)^{2}}\end{array} \right. \tag{23}\]  
here, \(\zeta\) is a constant speed control gain. In the general RLC approach, the discrete time reaching law [36; 37] can be given by:  
\[\Delta S(k+1)=-\sigma\text{sgn}(S(k))-\alpha S(k) \tag{24}\]  
where \(\sigma\) and \(\alpha\) are constant parameters. \(\Delta S(k+1)\) is then expressed by:  
\[\Delta S(k+1)=\frac{S(k+1)-S(k)}{T_{s}} \tag{25}\]  
\(T_{s}\) is the sampling time. With the assumption of \(\{\Delta S(0)=0\}\), discrete time switching function is given as:  
\[S(k)=\lambda e(k)+\lambda e(k) \tag{26}\]  
here, \(e(k)\) is the error of the guidance and \(\Delta e(k)\) is the derivative of the error in the \(k\)th sampling step. \(\lambda\) is the slope of the switching line and indicates a first-order dynamics of the error. The high-frequency oscillation problem known as chattering is occurred if Equation (24) is used directly in practice [37; 38]. This chattering problem is presented in Figure 15 and it can be solved by using \(sat(S(k))\) function instead of \(\text{sgn}(S(k))\). \(sat(\cdot)\) function is expressed by:  
\[sat(\Omega)=\left\{ \begin{array}{cl}\Omega&\text{if}|\Omega|\leq 1\\ \text{sgn}(\Omega)&\text{if}|\Omega|>1\end{array} \right. \tag{27}\]  
The new reaching law can be obtained as:  
\[\Delta S(k+1)=-\sigma sat(S(k))-\alpha S(k) \tag{28}\]  
The reaching law is also rewritten as below with this \(sat(\cdot)\) function in the Boundary Layer (BL):  
\[\Delta S(k+1)=-\beta S(k) \tag{29}\]  
where \(\beta=(\sigma/BL)+\alpha\). Thus, the control output becomes:  
\[\Delta v(k)=KS(k) \tag{30}\]  
Figure 15: A typical high-frequency oscillation problem. Since \(S\) is forced to reach zero with this RLC approach, exponentially reducing of the error to zero with time constant of \((1/\lambda)\) is provided. Also, an anti-windup integrator is added to stop over-integration for the protection of the designed model as seen in Figure 16.  
6-DoF motion responses completely include the generalized linear and angular positions and speeds of the robotic fish. Positions (X, Y, Z) are determined with the earth-fixed frame.