# Unmanned Surface Vehicle Using a Leader-Follower Swarm Control Algorithm
## 3 Control Algorithm of the USV
### Swarm Control Algorithm  
To maintain the formation of the platoon in swarm control using the leader-follower swarm control method, a virtual point is created at a certain distance and a certain angle from the leader USV. The follower USV must follow the point. The virtual point makes it possible to maintain the desired group formation because a certain distance and a certain angle can be maintained regardless of the actual leader USV's movement. Figure 6 shows the behavior of the follower USV following the virtual point in the Cartesian coordinate system. According to [21; 22], the error between the point created virtually and the trajectory followed by the follower [PERSON] can be expressed as Equation (11). \[ \begin{bmatrix}e_{\chi 2}(t)\\ e_{\chi 2}(t)\\ e_{\theta 2}(t)\end{bmatrix} = \begin{bmatrix}\cos(\theta_{F}(t))&\sin(\theta_{F}(t))& 0\\ -\sin(\theta_{F}(t))&\cos(\theta_{F}(t))&0\\ 0&0&1\end{bmatrix}\begin{bmatrix}d^{d}_{\chi}(t)-d_{\chi}(t)\\ d^{d}_{\chi}(t)-d_{\chi}(t)\\ \theta^{d}_{F}(t)-\theta_{F}((t))\end{bmatrix} \\ = \begin{bmatrix}d^{d}_{X}(t)\cos(\phi_{ref}+e_{\theta 2}(t))-d_{X}(t) \cos(\phi(t)+e_{\theta 2}(t))\\ d^{d}_{Y}(t)\sin(\phi_{ref}+e_{\theta 2}(t))-d_{Y}(t)\sin(\phi(t)+e_{\theta 2}(t))\\ \theta_{L}(t)-\theta_{F}(t)\end{bmatrix}  \tag{11}\]  
Here, \(\left[X_{F}(t),\,Y_{F}(t),\,\theta_{F}(t)\right]\) is the position and direction angle of the follower USV; \(\left[d^{d}_{X}(t),\,d^{d}_{Y}(t)\right]\) is the position of the virtual point; and \(\theta^{d}_{F}(t)\) is the azimuth to the follower point. As \(\theta^{d}_{F}(t)\) is the same as the direction angle \(\theta_{L}(t)\) of the leader USV, it can be expressed  
Figure 5: Block diagram of the cross-track error compensation algorithm. Figure 6: Schematic of the follower unmanned surface vehicle and virtual point. differently. Substituting Equations (5) and (6) into Equation (11), the error between the virtual point and the trajectory followed by the follower USV can be obtained Equation (12). \[ \begin{split}& e_{\chi 2}(t)=(X_{L}(t)-X_{F}(t))\cos(\theta_{F}(t))+(Y_{L} (t)-Y_{F}(y))\sin(\theta_{F}(t))+d_{ref}\cos(\phi_{ref}+\theta_{L}(t)-\theta_{ F}(t))\\ & e_{\chi 2}(t)=-(X_{L}(t)-X_{F}(t))\sin(\theta_{F}(t))+(Y_{L}(t)-Y_{F}(t) )\cos(\theta_{F}(t))+d_{ref}\sin(\phi_{ref}+\theta_{L}(t)-\theta_{F}(t))\\ & e_{\theta 2}(t)=\theta_{L}(t)-\theta_{F}(t)\end{split}  \tag{12}\]  
Here, by using the calculated value, the distance error \(d_{\ell 2}=\sqrt{e_{\chi 2}(t)^{2}+e_{\chi 2}(t)^{2}}\) between the virtual point and the USV can be obtained. The controller of the follower USV with the distance and azimuth errors as control inputs is expressed as Equation (13). \[ \begin{split}& T_{\theta 2}=K_{P_{\theta 2}}e_{\theta 2}+K_{I_{\theta 2}} \int e_{\theta 2}+K_{D_{\theta 2}}\hat{e}_{\theta 2}\\ & T_{d_{\ell 2}}=K_{P_{d_{\ell 2}}}e_{d_{\ell 2}}+K_{I_{\ell 2}} \int e_{d_{\ell 2}}+K_{D_{d_{\ell 2}}}\hat{e}_{d_{\ell 2}}\end{split}  \tag{13}\]  
Here, \(T_{\theta 2}\) and \(T_{d_{\ell 2}}\) denote the direction and distance output from the controller, respectively, and \(e_{\theta 2}\) and \(e_{d_{\ell 2}}\) denote the azimuth and distance error between the virtual point and the current position of the follower USV. For effectively maintaining the leader-follower swarm formation, the control algorithm should consider rotation, as shown in Figure 7.  
Considering the rotation situation, for the design of a controller that can maintain effective formation, the exact formation control technique proposed in [23] is used. Equation (14) expresses the exact formation control. \[v_{F}=v_{L}\frac{\cos(\beta_{LF}-\psi_{ref})}{\cos(\psi_{ref})} \tag{14}\]  
Figure 7: Rotation of the leader–follower control. Here, \(v_{L}\) and \(v_{F}\) represent the linear velocity of the leader and follower USVs, respectively; \(\beta_{LF}\) represents the difference between the azimuths of the leader and follower USVs; and \(\psi_{ref}\) represents the target azimuth of the leader USV.