Managed Surge Controller: A Docking Algorithm for a Non-Holonomic AUV (Sparus II) in the Presence of Ocean Currents for a Funnel-Shaped Docking Station

This paper presents a novel algorithm to dock a non-holonomic Autonomous Underwater Vehicle (AUV) into a funnel-shaped Docking Station (DS), in the presence of ocean currents. In a previous work, the authors have compared several docking algorithms through Monte Carlo simulations. In this paper, a new control algorithm is presented with a goal to improve over the previous ones to fulfil the specific needs of the ATLANTIS project. Performance of the new proposed algorithm has been compared with the results of the previous study, using the same environemnt on the Stonefish hardware-in-the-loop simulator.


Introduction
The AUV technologies presented a significant improvement during the last years. Several autonomous missions were developed in the field [1,2]. Nowadays, the common operating procedure of the AUVs is to deploy them (usually with a ship), develop the mission, and, finally, recover; in most of the cases during the same operational day. The natural next development for the consolidation of this technology is the creation of DSs, which can allow the AUVs to extend the operational time in the field. The DS have to offer protection, high-bandwidth communication channels, and the capacity to recharge the vehicle's batteries. In the literature, several examples of DS systems can be found [3][4][5][6][7][8]. Each DS concept was tailored to a specific AUV and used its own perception and docking strategies. Several descriptive surveys about docking can already be found in the literature: [9][10][11].
Previously, the team from the University of Girona developed a prototype of a funnelshaped DS that relied on an acoustic transponder and light beacons, in order to localize it from the vehicle [12]. The conclusions of those experiments have shown the need of developing a controller that considers the ocean currents. Moreover, in turbid water scenarios, the vision system was not appropriate. In the ATLANTIS project [13], one of the technologies that must be demonstrated is the semi-permanent deployment of the Sparus II AUV, using a docking station on the seabed. With this motivation, the authors have developed the following study.
In a previous publication [14], several algorithms were studied to cope with the autonomous docking, using a non-holonomic AUV, in the presence of ocean currents: the Pursuit Guidance with current compensation controller presented by [4,15], the Cross-track controller used in [3,16], the Fuzzy controller used in [17,18], the Touchdown alignment controller described in [19], the Sideslip controller based on [20], and the Sliding path controller described in [21]. None of the studied solutions offers satisfactory behavior when dealing with ocean current velocities higher than the docking velocity. Thus, a proposal of a novel control algorithm is presented in this work.
The paper is organized as follows: Section 2 presents the novel algorithm. Section 3 presents the experimental setup used in order to test the algorithm. Section 4 analyses the performance of the algorithm in different scenarios. Obtained results and the comparison with the other methods are presented in Section 5, discussed in Section 6, and finally, the conclusions are drawn in Section 7.

Proposal: Managed Surge Controller
This section presents the novel method to deal with the ocean currents controlling a non-holonomic AUV. The proposed method is called Managed Surge Controller (MSC).

Assumption 1
We assume that the AUV motion is described using two degrees of freedom: surge and yaw. This is a consequence of the commonly adopted strategy of neglecting sway for under-actuated vehicles, maneuvering at low speed.

Assumption 2
The inner loop controllers can track the desired surge velocity (u d ) and the desired yaw (ψ d ) with good accuracy.

Assumption 3
The velocity of the ocean currents is constant, irrotational, and bounded.

Assumption 4
The vehicle can measure its surge velocity, yaw angle, and the ocean current velocity.

Assumption 5
The vehicle can measure its relative position with respect to the DS.

Concepts
Inspired by the analysis done in [14], the present method improves the results obtained with the previously studied methods because it can deal with ocean currents, the velocity of which are larger than the velocity of the docking of the AUV. Figure 1 presents the basic variables involved in the process. Two different reference frames are presented, the {D} frame located at the position of the DS, and the {B} frame attached to the AUV body. The velocity in the {D} frame are symbolised byẋ andẏ with respect to the ground, and the velocities in the {B} frame are represented by u and v with respect to the water.

Model of the System
The kinematic system is represented by the following equations, recall Figure 1 and Figure 2: This equation, assuming a negligible sway velocity (Assumption 1 ), simplifies to:

Docking Scenarios
According to Figure 3, three docking scenarios can be defined, depending on the robot The current opposes to the robot speed. Therefore, a higher through water robot speed is required to achieve the desired inertial docking velocity. • Scenario B (ẋ c > 0 andẋ c <ẋ D ): The current speed, being smaller than the desired docking velocity, adds to the through water robot velocity to achieve the inertial docking speed. • Scenario C (ẋ c ≥ẋ D ): A current speed higher than the docking velocity, requires a backward through-water robot velocity to achieve the desired inertial docking velocity.
In scenarios A and B the surge velocity will normally be positive and the AUV heading will be opposite toẏ c . In contrast, in scenario C, the surge velocity will normally be negative and the heading will be in the direction ofẏ c .

Crab Angle
To be able to compensate the lateral ocean current (ẏ c ) with a non-holonomic AUV it is necessary to use a crab angle (ψ c ). This crab angle has the goal of aligning the robot to the axis of the DS (ẏ = 0), while keeping the desired docking velocity ( Dη

Entrance Problem
The problem of making a torpedo-shaped AUV enter a funnel-shaped DS can be modeled with the simplification that the DS is represented by an isosceles triangle and the AUV by a straight line directed along its main axis (see Figure 4). In Scenario II and III, the AUV completes the docking thanks to the geometrical properties of the system. Both scenarios differ in the energy lost during the collision, Scenario III being the one with the highest losses. Following the analysis of energy lost during the collision (reported in [14]), the AUV should try to perform Scenario I if possible, targeting Scenario II alternatively, and using Scenario III as the last resort. It is worth noting that Scenario I is not easy to perform with Sparus II AUV, in presence of ocean currents, due to its non-holonomic nature.

Path
A crab angle is needed to compensate for the ocean currents, therefore the method creates a path parallel to the DS axis at an appropriate distance from it, to take advantage of the torpedo-like shape of the AUV and the funnel-shaped DS, see Figure 6. The path is created calculating a gap (y g ), with respect to the axis of the DS using the crab angle, the DS and the AUV geometry. In order to calculate this gap, let us consider the geometry of the problem when the AUV reaches the DS in scenarios II and III ( Figure 7). For the entrance in Scenario II, the gap can be calculated as: The maximum crab angle admissible to enter to the DS in Scenario II is computed using the simplified funnel shape: This allows to calculate the maximum gap as: For crab angles larger than ψ c I I , the system needs to perform the entrance Scenario III. The maximum crab angle for the entrance Scenario III, in case of 2 F y < l/2 (that fulfils the mechanic characteristics of the system presented), can be calculated as (see Figure 8, and recall Figure 5): in order to guarantee that the AUV does not hit a disfavoured part of the DS. In this case the y g I I I is fixed to be equal to F y /2 (see Figure 7). In summary, (11) sets the value of y g for all the cases as:

Path Following
The Sparus II AUV has direct control over the surge velocity (i.e., u) as well as over the heading (i.e., ψ). Let the cross-track path error be defined as: With this notation, the objectives of the path following the controller are to achieve:

Control Law
The control law regulates the desired heading (ψ d ) and the desired surge (u d ). The desired heading is defined as: where β is a correction term depending on the look-ahead distance (∆) and the cross-track error (e), see Figure 9: where ∆ is defined by a constant value (k ∆ > 0) and a sign criteria (21): beingẋ ss the through water velocities of the AUV in the steady state in the {D} frame (i.e., when e is zero). The desired surge velocity is given by: where c is a correction term, introduced to adjust the response of the system: The correction modifies the basic velocity representation from Figure 2 into the one shown in Figure 10. The steady state inertial velocities can be calculated as: whereẋ is set asẋ D . Figure 10. Geometrical representation of corrected velocities. Here,ẋ ss , andẏ ss are through water velocities of the AUV in the steady state in the {D} frame (i.e., when e is zero); u ss the through water velocity of the AUV in the steady state in the {D} frame; andẋ C ,ẏ C , and u C the velocities due to c.

State Space Formulation
The system evolution is represented by the cross-track error (12), which, according to (4), has the following time derivative: Now, assuming u = u d and ψ = ψ d , the error dynamics are given by:

Equilibrium Points
An equilibrium point is reached whenė = 0, i.e., when ψ d = ψ c . If this condition is applied to (24), it follows that e eq = 0 ifẋ c =ẋ D , ∆ = 0, k 1 > 0, and k 2 > 0. In order to fulfill this condition, and becauseẋ D is a value that we can set, in the case of havinġ x D =ẋ c theẋ D will be increased according to:

Setting the Gains
The definition of k 1 and k 2 corresponds to the maximum velocity and acceleration that the correction c (20) will impose on the system. Considering the characteristics of the Sparus II, a maximum correction velocity of 0.7 m/s, and a maximum correction acceleration of 0.5 m/s 2 are desirable.
In order to set k 1 , the function y = atan(x) is analysed. This function has a horizontal asymptote at y = π/2 and in y = −π/2. Taking into account the performance of Sparus II, the horizontal asymptote of (20) must be set to y = 0.7 m/s, consequently k 1 = 0.7 · 2/π ≈ 0.4456 m/s. To compute the maximum rate of change of the correction, both k 2 and k ∆ must be known. For this reason, in a first step k 2 is set as 1 m −1 .
The parameter k ∆ is computed in the Appendix A, assuming k 2 = 1 m −1 , in order to fulfil the stability conditions formulated in Section 2.3.5.

Domain of the Controller
In order to evaluate the controller presented in this paper, a certain domain is set: Ocean current velocities must be within the range of those that the Sparus II AUV can withstand. Since the docking maneuver begins after a homing process, we can ensure that the cross-track error of the AUV position belongs to the stability domain. If during the maneuver, the cross-track error falls outside of the domain, the docking maneuver is cancelled and the whole process is repeated.

Stability
In order to demonstrate the stability of the system using the Lyapunov Direct Method, the following Lyapunov candidate is proposed: that fulfills the first and second Lyapunov conditions: V(0) = 0, and V(e) > 0 ∀e = 0 . The first order time derivative of the Lyapunov candidate can be expressed as: To demonstrate exponential stability, we show that the system fulfills the condition (following [22] for some λ > 0. The mathematical proof of stability is given in the Appendix A.

Maximum Acceleration Verification
In Section 2.3.3, a k 2 = 1 m −1 was assumed to be able to calculate a k ∆ that fulfils the stability conditions. In order to verify that the obtained gains do not result in surpassing the assumed capabilities of the Sparus II, for the presented controller, an optimization problem was formulated. In the problem, a maximum rate of change of the correction (c) is searched for, to check that it is not exceeding the maximum assumed acceleration of the robotċ = 0.5 m/s 2 . First, we simplify (20) as: to avoid deriving the signum function, and because the maximum value ofċ is not affected.
The rate of change of c is given by: The optimization problem is formulated as follows: Considering the domain set for the controller (Section 2.3.4), the parameters: and together with (25), it can be deduced that three optimization problems must be solved for three subsets of theẋ c domain: X 1 = [−0.5, 0.1], X 2 = [0.1, 0.3] and X 3 = [0.3, 0.5], and the maximum of these three problems is the solution in the whole domain (26). The problem (32) was solved using the IPOPT [23] solver and yielded a result of (0.44979 m/s 2 ).

Minimum Docking Distance
With the control laws defined, the minimum necessary distance to successfully perform docking, from a kinematic point of view, can be calculated, by solving (24). The domain of Section 2.3.4 is considered together with the follwoing parameters: applying the correction presented in (25). The maximum time to reach the equilibrium (|e| ≤ 0.05 m) can be calculated. This time applied to the desired docking velocity gives an approximation of the minimum necessary docking distance: The minimum distance necessary to dock, in the worst case scenario, is close to 25 m. Note that the maximum velocity and acceleration in yaw have not been taken into account, because (for the set parameters) it has a low influence when the working yaw angle is reached. Note also that this is a controller defined for the docking maneuver (starting at ±10 m in y D , with a heading favourable to the DS, i.e., −1.5 > ψ < 1.5), the homing maneuver will require its own additional distance.

Experimental Setup
In order to do a consistent comparison, the same experimental setup, as presented in [14], was used.

Hardware
The non-holonomic auv! used for this test was the torpedo-shaped Sparus II [24,25], see Figure 11. The AUV comes equipped with three thrusters, one vertical and a pair of horizontal, allowing for control in the surge, heave, and yaw. The control system supports inputs in force, velocity, and position. In this study we chose to control the vehicle in velocity, for the surge and heave, and in position, for the yaw. The funnel-shaped DS developed by the Univeristy of Girona [12] (see Figure 12) was represented in simulation.

Simulation
In this research an advanced open-source marine robotics simulator, called Stonefish [26], was used. Full dynamics and hydrodynamics of Sparus II were simulated, including ocean current influence, together with a complete suite of its sensors, see Figure 13. Moreover, the docking station model was recreated in the simulation, with a high attention to detail, allowing for realistic assessment of docking performance. Specifically for this research, the simulator was extended to support acoustic communication and positioning devices. More details can be found in the previous work [14].   [25], and the representation of the underwater environment.

Performance
The objective of this section is to show the performance of the algorithm in the simulated scenario. The concept utilized to develop the high-level controller presented in this paper is to use the strong features of the Sparus II, to achieve maximum possible performance. The Sparus II AUV, as a non-holonomic robot without a rudder, requires a combined action of its two horizontal thrusters, in order to control the heading. This fact added to the non-symmetric behavior of the thrusters makes a notably lower response in the heading input than in the surge velocity.
The controller presented utilizes mainly the surge in order to correct the position of the AUV and reduce e, see Figure 14.

Docking Scenarios A and B
For the docking scenarios A and B, see Figure 3, the heading of the AUV is opposite toẏ c , it can be seen in the video [27]. It can be understood, analyzing the velocity vectors, i.e.,ẋ D is bigger thanẋ c , that the surge velocity has to be positive; this being the case (and taking into consideration that we have a non-holonomic AUV), the surge has to point in the opposite direction to theẏ c , in order to be able to compensate for it.
A set of initial conditions are simulated (36), for kinematics (solving (3) and (4)), and plotted in Figures 15 and 16. They are also simulated in dynamics (using Stonefish) and plotted in Figure 17 in order to compare both results.

Docking Scenario C
For the docking scenario C, the heading of the AUV is the same as the direction oḟ x c , see the video [28]. Again, analyzing the velocity vectors, i.e.,ẋ D is smaller thanẋ c , the surge velocity has to be negative, so the surge has to point in the same direction as thė y c , in order to be able to compensate for it.
A set of initial conditions are simulated (37) for kinematics (solving (3) and (4)) and plotted in Figures 18 and 19. They are also simulated in dynamics (using Stonefish) and plotted in Figure 20 in order to compare both results.

Entrance Scenario II
An example of the performance achieved in the entrance scenario II can be found in [29], see Figure 5.

Entrance Scenario III
An example of the performance achieved in the entrance scenario III can be found in [30], see Figure 5.

Results
In order to compare the new algorithm with the state-of-the-art, the data obtained in the article [14] are used. In this previous article, different algorithms, already published in the literature, were compared at different levels. Being that level 3 is the most representative for the authors' needs, the data from this level are used to compare with the proposed algorithm, since they were tested at the same level.
As it was presented in [14], in order to estimate the quality of the docking process in a funnel-shaped DS, a novel technique based on the geometrical analysis of the entrance of the AUV was used. With this technique, we can evaluate the methods with a 'score' that ranges from 0 to 1, with 1 being the perfect docking.
In order to represent the comparison, a Boxplot is presented in Figure 21. The 3d! plot of the results is presented in Figure 22, and the 2d! plot in Figure 23.  A table with the numerical results compared can be seen in Table 1.

Discussion
The Managed Surge Controller achieved a mean score of 0.891, being the highest value for the proposed conditions. The controller is able to dock the robot in all the ocean current conditions tested in the previous study (e.g., from -0.4 m/s to 0.4 m/s on both axis). As it can be seen in Figure 21, it not only has the best mean scores, but also a low standard deviation, showing that the results are consistent.
As the score indicates and as can be further appreciated in Figures 22 and 23, a good performance in all the ocean current conditions tested during the exercise was achieved. Ocean current velocity values larger than the ones studied in the previous paper were also studied in simulation, also presenting good results. However, it is not recommended to operate the Sparus II in more harsh conditions.
The Sparus II is a torpedo-shaped AUV equipped with three thrusters. It is designed to perform extended surveys, where the precision of the position is not crucial, implying that it is not capable of performing complicated maneuvers with high precision. One of the facts that is not reflected in the score is the simplicity of the maneuver, from a practical point of view. The simpler the maneuver, the easier it is to perform it, using the real vehicle. In the docking maneuver, the msc! (msc!) follows a straight-line, focusing on utilizing the horizontal thrusters optimally to work against the ocean current forces and correcting the cross-track error.
If the msc! is compared with the Sliding path controller (presented in [21] and implemented in [14]), that achieved a mean score of 0.790, one of the main differences is the developed maneuver. The Sliding path controller implies a notable precision in the maneuver to be able to enter exactly in the desired position, which is hard to achieve using the Sparus II. It is also not capable of minimizing the cross-track error when the ocean current velocity is favorable and close to or larger than the docking velocity, when it is in the approaching path. However, during the sliding path, it still can compensate for the error.

Conclusions
This paper has presented a novel controller to dock with a non-holonomic AUV, in a funnel-shaped DS, dealing with ocean currents. The paper proves the stability of the controller and the the expected behavior is further confirmed using a very realistic dynamic simulator. In a previous work [14], after exhaustive survey, several algorithms to face the same problem were implemented and tested in the context of the ATLANTIS project, using the Stonefish simulator. This previous work concluded with a problem to study: how to minimize the cross-track error when the ocean current velocity is favorable and close to or larger than the docking velocity. In this article, the authors have developed an algorithm capable of dealing with this problem while maintaining or improving the rest of the evaluated criteria.
The novel proposal was compared with the state-of-the-art algorithms, with the criteria developed in [14], presenting the best results. In future work, this controller will be implemented in Sparus II and tested in real scenarios. Also, a new funnel-shaped DS will be designed and built, in order to meet the requirements of the ATLANTIS project. The authors expect to be able to achieve docking without the use of vision systems; and, if it is the case, with the presence of ocean currents in the context of the project.
Author Contributions: Conceptualization J.E., methodology J.E., software J.E. and P.C., validation P.R. and P.C., formal analysis J.E., investigation J.E., resources P.R., data curation does not apply, writing-original draft preparation J.E., writing-review and editing J.E., P.C., N.P., and P.R., supervision N.P. and P.R., project administration P.R., funding acquisition P.R. All authors have read and agreed to the published version of the manuscript.

Data Availability Statement:
The data used is already published in this paper.

Acknowledgments:
The authors would like to distinguish the counsel and guidance of António Pascoal from the Institute for Systems and Robotics for the development of this work. Also, the authors would like to thank the ATLANTIS project for motivating this study and the following studies to come, related to it.

Conflicts of Interest:
The autors declare no conflict of interest.
AUV position in the {D} frame (see Figure 1)   Longitude of the AUV y g Distance between the maneuver path and the axis of the DS y g,I I Distance between the maneuver path and the axis of the DS in Scenario II y g I I I Distance between the maneuver path and the axis of the DS in Scenario III y g,M Maximum distance that can be set between the maneuver path and the axis of the DS ∆ Look-ahead distance k ∆ Gain of the look-ahead distance e Cross-track erroṙ e Time derivative of e e eq Cross-track error in the equilibrium c Velocity correction of the controller k 1 Gain to set the maximum velocity of the correction k 2 Gain to set the maximum acceleration of the correction In what follow we show thatV where λ > 0, leads to exponential stability. Substituting (A3) into (A2) whereẏ c = −ẏ ss , (A6) y ss =ẋ ss tan(ψ c ), (A7) is the new condition that must be fulfilled in order to ensure exponential stability. For the sake of clarity, the following variables are defined: A 2 − |ẋ ss | k 1 atan(k 2 e) | sin(ψ d )| e, A 3 −ẏ 2 ss |ẋ ss | k 1 atan(k 2 e) | sin(ψ d )| e, and consequently Being sign(x) ≡ sign(atan(x)), implies A 2 ≤ 0 and A 3 ≤ 0. Assuming | sin(ψ d )| = 1 is a conservative solution, consequently, the expression k ∆ > −ẏ ss |ẋ ss | e − |ẋ ss | k 1 atan(k 2 e) e −ẏ 2 ss |ẋ ss | k 1 atan(k 2 e) e (A25) still represents the condition to fulfill the exponential stability for the controller. In order to calculate k ∆ a nonlinear optimization problem of the following form was solved: maẋ x c ,ẏ c ,e Considering the domain set for the controller: The problem (A26) was solved using the IPOPT solver and yielded a result of 5.1441. Consequently, for this domain, the controller is guaranteed to achieve exponential stability if where k ∆ is a value that can be set by the designer.