## 2 Materials and Methods
### Docking Algorithm
If the mechanism closing was triggered but the aMussel is not charging, i.e., it has not  
Figure 9: State machine representation of the aPad high-level controller implementing the phases of the docking algorithm. been properly docked, the aPad will retry the docking procedure. An example of the final docking outcome, an aPad with four successfully docked aMussels, is given in Figure 10.  
The curve given in Figure 11, left, represents the surge speed regulation of the aPad. The closer the estimated final image position of the aMussel is to the center of the image and thus the better aligned it is with the dock, the faster the aPad will approach it. The curve in Figure 11, right, represents the yaw rate regulation of the aPad, with the goal of keeping the aMussel top cap aligned with the aPad's docking mechanism. Surge speed control references are generated using:  
\[u_{ref}(\hat{e}_{x})=k_{1}e^{-\frac{\hat{e}_{x}^{2}}{2\omega^{2}}} \tag{1}\]  
and those for yaw rate control are done using:  
\[r_{ref}(\hat{e}_{x})=k_{2}\tanh(a\hat{e}_{x}) \tag{2}\]  
where the input \(\hat{e}_{x}\) represents the estimate of the horizontal image coordinate of the aMussel scaled to an interval of \([-1,1]\), received from the filter node. A fixed yaw rate \(r_{ref,search}\) is set for the search phase of the docking process. In the process of the experimental validation of the system, the decision was made to include two sets of parameters depending on the conditions the docking was to take place in, discussed in more detail in Section 3. Values chosen for regulator parameters for working in calm water conditions and for working in a more turbulent environment are given in Table 1.  
Figure 11: Surge speed (**left**) and yaw speed (**right**) curve used during autonomous docking. \(\hat{e}_{x}\) is the estimate of the \(x\) or horizontal image coordinate of the aMussel. Figure 10: An aPad with four docked aMussels in a pool testbed. Depending on which of the four aPad docks the aMussel is being docked to (or undocked from), the surge reference needs further transformation. A rotation matrix is applied to the generated surge reference from (1):  
\[\mathbf{u}_{ref}^{\prime}= \begin{bmatrix}u_{ref,x}^{\prime}\\ u_{ref,y}^{\prime}\end{bmatrix} = \begin{bmatrix}cos(\alpha)&-sin(\alpha)\\ sin(\alpha)&cos(\alpha)\end{bmatrix} \mathbf{u}_{ref,0} \tag{3}\]  
where \(u_{ref,0}\) represents the reference for the default front-facing dock which is used to determine the vehicle's heading:  
\[\mathbf{u}_{ref,0}= \begin{bmatrix}u_{ref}\\ 0\end{bmatrix}  \tag{4}\]  
Angle \(\alpha\) is simply determined using \(\alpha=i_{dock}\frac{\pi}{2}\), with \(i_{dock}\) as the index of the chosen dock, as shown in Figure 12.  
For undocking an aMussel, the aPad opens the corresponding dock and moves \"backwards\" using the negative of the calculated surge reference for a fixed period of 10 s. This ensures the aMussel in question exits the dock and is clear of the aPad. \begin{table}
\begin{tabular}{c c c} \hline \hline
**Parameter** & **Value-Calm** & **Value-Turbulent** \\ \hline \(k_{1}\) [m/s] & 0.25 & 0.3 \\ \(k_{2}\) [rad/s] & 0.35 & 0.4 \\ \(\sigma\) & 0.245 & 0.35 \\ a & 1 & 1 \\ \(r_{ref,search}\) [rad/s] & 0.3 & 0.4 \\ \hline \hline \end{tabular}
\end{table} 
Table 1: Regulator parameters. Figure 12: aPad vehicle viewed from above with docks marked in indexing order. Example angle \(\alpha\) highlighted for dock with index \(1\). Implementation-wise, the complete aPad operating system and software components run on an Intel NUC i5 mini PC situated within its round hull. Lower-level controllers such as the described yaw and surge controllers, thruster allocation, sensor drivers including the Microsoft Kinect and related image processing utilities, as well as navigation and tracking filters are implemented in C++ and organized in packages, services, and executable nodes under the Robot Operating System (ROS) middleware paradigm.