## 5 Experiments and Results
### Experimental Results  
A series of experiments were carried out in order to validate the effectiveness of the MPC-based trajectory tracking algorithm. During the experiments, the sampling time of the robot was 20 HZ, the prediction horizon was 25, the control horizon was 15, and the weighting matrices were consistent with those in the simulations.  
The first experimental case involves tracking a desired line. The initial state of the robot is \(\mathbf{x}(0)=[0.45,1.0,35,0,0]^{T}\). Figure 20 depicts the clip of the line trajectory tracking experiment, where the red dotted line is the desired trajectory. The robot mostly converges at the desired trajectory after 15 s. Figure 21 shows the tracking results of the robot in terms of the line tracking. Figure 21a shows the time evolution of the states. It can be seen that the proposed algorithm is able to force the robot to catch up with and be loaded onto the desired trajectory. The maximum following error of the position when the robot converges to the desired trajectory is less than 0.03 m (1/10 of the diameter of the robot). In Figure 21a, the control behavior on \(\theta\) seems to be significantly poorer than that on \(x\) and \(y\). In fact, when the real yaw angle reached the desired yaw the first time, the position \(y\) was slightly far from the desired position. If the real yaw angle is kept constant at zero, there will be a static error in position \(y\). In order to eliminate the position error of \(y\), the yaw angle increased again and then tended toward the desired yaw as the error of position \(y\) approached zero. In addition, the size of the indoor pool is insufficient to remove the effects of the reflective waves at present, such that the robot is subject to external disturbances as the robot is initially close to the edge of the pool at the  
\begin{table}
\begin{tabular}{c c c} \hline \hline
**Items** & \multicolumn{2}{c}{**Parameters**} \\ \hline Intrinsic matrix & \(\left[\begin{array}{cc}551.746&0&0\\ 0.238&550.579&0\\ 370.092&220.353&1\\ \end{array}\right]\) \\ Radial Distortion & \(\left[\begin{array}{cc}0.103&-1.330&3.596\\ 1.223\times 10^{-4}&-0.002\\ \end{array}\right]\) \\ \hline \hline \end{tabular}
\end{table}
Table 2: The Parameters of the global camera.  
Figure 19: The information transmitting system of the robot.  
beginning. The required control forces of each thruster are depicted in Figure 21b. It was determined that the magnitudes of the control forces remain within the permitted range, and the changes in forces are moderate.  
The second experimental case involves tracking a desired square-shape trajectory. The initial state of the robot is \(\mathbf{x}(0)=[1.0,0.25,-140,0]^{T}\). Figures 22 and 23 show the experimental results of the second case. Figure 22 shows the image sequences of the square-tracking experiment. The robot can converge to and track along the desired trajectory. The states of the robot during the tracking of the square are plotted in the Figure 23a. During the square tracking, the yaw angle of the robot exhibited three sudden changes, whereas the positions (including \(x\) and \(y\)) were continuous, even at three corners. The sudden change in the yaw angle leads to a larger error in the yaw angle than that of the position. Furthermore, it was found that the errors when the robot converged to the desired trajectory at each edge of the square were almost zero. The control forces of the four thrusters are depicted in Figure 23b. When the yaw angle of the robot changes suddenly, the control force changes clearly to drive the robot close to the desired state as quickly as possible. Whereas, the thrusts are within the limit range of the control inputs during the entire process. A video of the square tracking experiment is attached in the Appendix B.  
Figure 21: The tracking results of the robot in line tracking. (**a**) States of the robot at different times. (**b**) Thrusts varying of the four thrusters.  
Figure 20: Image sequences of the BSR tracking a line trajectory.