Tracking Control in Presence of Obstacles and Uncertainties for Bioinspired Spherical Underwater Robots

Abstract

During marine missions, AUVs are susceptible to external disturbances, such as obstacles, ocean currents, etc., which can easily cause mission failure or disconnection. In this paper, considering the strong nonlinearities, external disturbances and obstacles, the kinematic and dynamic model of bioinspired Spherical Underwater Robot (SUR) was described. Subsequently, the waypoints-based trajectory tracking with obstacles and uncertainties was proposed for SUR to guarantee its safety and stability. Next, the Lyapunov theory was adopted to verify the stability and the Slide Mode Control (SMC) method is used to verify the robustness of the control system. In addition, a series of simulations were conducted to evaluate the effectiveness of proposed control strategy. Some tests, including path-following, static and moving obstacle avoidance were performed which verified the feasibility, robustness and effectiveness of the designed control scheme. Finally, a series of experiments in real environment were performed to verify the performance of the control strategy. The simulation and experimental results of the study supplied clues to the improvement of the path following capability and multi-obstacle avoidance of AUVs.

1 Introduction

Nowadays, AUVs are widely used to perform various tasks, such as exploration, inspection, operations, military research and marine environment’s observations [1, 2]. In ocean applications, path planning is an essential tool to complete the specific tasks [3, 4]. However, few studies have addressed the path following control in presence of obstacles, especially, in three-dimensional (3D) space. Thus, a path following strategy with the capability of obstacle avoidance is urgently need, which improve the accuracy and safety of AUVs.

In recent years, many path following techniques have been studied to satisfy the requirements of complex dynamic environments. Common navigation methods including the Artificial Potential Field (APF) [5], Backstepping Slide Mode Control (BSMC) [6], Fuzzy-based Control (FC) [7], Neural Network (NN), LOS guidance and Map Construction (MC) method [8] are used to solve the path following problem. However, those methods cannot counterweight various requirements, and suitable for different model control strategies. Lalish et al. [9], proposed an algorithm to solve the n-vehicle collision avoidance problem. The controller can guarantee all vehicles to follow the desired path. For robots with different motion modes, due to the differences in the dynamic models, it is necessary to comprehensively consider the robot model, including kinematic and dynamic model. Shen et al. in [10, 11], proposed a novel Lyapunov-based model predictive control in horizontal plane to solve the trajectory tracking problem of AUV. Khalaji et al. [12] have developed a highly nonlinear controller for AUVs based on Lyapunov theory. Some analytic simulation scenarios are carried out to verify the effectiveness of the designed controller. However, this method is challenging to apply in real environments. Liu et al. [13] proposed a guidance law to track the reference path safely in vertical plane using the barrier Lyapunov function. However, the above studies are all based on the analysis of simplified models, mostly solves problems in the planner, which cannot meet the needs of multiple underwater missions and extend to the spatial trajectory tracking control.

Moreover, there also exist other methods, such as Model Predictive Control (MPC) [10, 14], Dynamic Window Approach (DWA) [15] and Adaptive Control (AC), to solve the trajectory tracking problem [16, 17]. Zhang et al. [18] proposed the adaptive NN control for robotic manipulators to handle the unmodeled and uncertain dynamics. The NN is applied to achieve full-state feedback for robots. Yan et al. [19] designed a controller for AUV combining with Particle Swarm Optimization (PSO) and Waypoint Guidance (WG). However, the chattering phenomenon and smooth transition has not been solved in the above papers. Although above studies have developed new tools and solutions for 3D path following, they often have low accuracy for avoiding moving obstacles and make them restricted to apply in practice.

In our previous research, a SUR is developed which inspired by the propulsion mechanism of jellyfish [20,21,22]. The natural jellyfish are mainly composed of a round umbrella, tentacles, mouth and wrist, etc., which spray water in the body by compressing and deforming the body’s umbrella membrane to generates propulsion. By spraying water, areas with different pressure levels will be created around the body, and then jellyfish swims in the opposite directions. To imitate the advantages of jellyfish locomotion in symmetry, stability and performance, the SUR’s locomotion mechanism uses a hybrid propulsion system composing by a propeller and a water-jet thruster. The proposed hybrid propulsion system provided alternative methods for approaching targets stably, quickly and effectively. The hydrodynamic parameters, such the viscous coefficient, the drag coefficient and the pressure contour of motions we analyzed in [23, 24]. In our previous controllers, [2, 7, 25] there is still instability in trajectory tracking due to the lack of consideration of the correctness of robot modeling and the complexity of the operating environment. Thus, to design the nonlinear control law for SUR safely and accurately, and improve the robot stability, the real-time obstacle avoidance considering SUR modeling is integrated to the trajectory tracking.

As can be seen from the mentioned literature, most controllers neglect terms, such as robustness, stability and the capability of obstacle avoidance in 3D environment. In addition, the parameters, such as size (radius), uncertainties, internal and external disturbances are ignored, which lead to the instability of the whole system. Therefore, considering the complexities, nonlinearities and uncertainties of the AUV’s model in ocean environment, in this study, to follow the reference path in real-time and avoid obstacles accurately, a novel control strategy for SUR is employed.

Motivated by the above consideration, this study concentrates on the path following and obstacle avoidance (the static and moving obstacle) for SUR in 3D environment. Compared with our previous control methods, considering the stability and uncertainty of the control system, the kinematic and dynamic model of SUR are established and analyzed. A novel control strategy is developed for SUR, including path following, static and moving obstacle avoidance. Under the principle, the SUR can move and avoid obstacles along the reference trajectory. Moreover, The Lyapunov theory and SMC method are developed and implemented for SUR based on the control strategy to compensate for errors in model uncertainties and external disturbances. Furthermore, the APF method is employed into the proposed control system, which is more suitable for local real-time obstacle avoidance. Besides, the robustness, efficiency of the novel control strategy is examined and validated via a series of simulation and experiments in real environment.

The remainder of this paper is organized as follows: first, we describe the kinematic and dynamic model of SUR, and the path-following and obstacle avoidance is proposed in Sect. 2. Then, the control strategy for SUR is developed, and the stability and robustness of the proposed strategy is verified using Lyapunov and SMC method in Sect. 3. Next, Sect. 4 uses the simulation to display the performance of the proposed strategy, the SUR model established and imported to Webots, a series of experiments are carried out. Then, Sect. 5 uses the SUR to perform a series of experiments in real environment, the tracking trajectories are analyzed. In addition, Sect. 6 discusses the results of some simulation and experiments in real environment. Finally, Sect. 7 presents our conclusions.

2 Kinematic and Dynamic Model of the SUR

The structure of SUR is highly symmetrical, and the four water-jets and two propellers are located above and below the central track, respectively. The forward direction of SUR is perpendicular to the plane, where the propellers and water-jets are located. As shown in Fig. 1, the general kinematic and dynamic models of AUVs [26,27,28] can be described using the earth-fixed coordinate system $(O_{e} ,X_{e} ,Y_{e} ,Z_{e} )$ and the body-fixed coordinate system ${\dot{\text{u}}}$. Among them, the ultrasonic sensor is located under the central track and is fixed on the robot body through a bracket to make it consistent with the forward direction of the SUR. In this study, the 6-DOF nonlinear kinematic motion of SUR can be expressed as follows:

$${\varvec{\dot{\eta } = J\Theta (\eta )v}}$$

(1)

Fig. 1

Coordinate frames of the SUR

where ${{\varvec{\eta}}} = [x,y,z,\phi ,\theta ,\psi ]^{T}$ is the state vector which included the position [x, y, z] and orientation [φ, θ, ψ] in the earth-fixed frame, ${\varvec{v}} = [u,v,w,p,q,r]^{T}$ is the vector which included the linear velocity [u, v, w] and angular velocity [p, q, r]. ${\varvec{J}}_{{{\varvec{\Theta}}}} {\varvec{(\eta )}}$ is the transformation matrix between the earth-fixed and robot’s body-fixed frame, it is formalized as

$${\varvec{J}}_{{{\varvec{\Theta}}}} = \left[ {\begin{array}{*{20}c} {c\psi c\theta } & {c\psi s\theta s\phi - s\psi c\phi } & {c\phi s\theta c\psi + s\psi s\phi } & 0 & 0 & 0 \\ {s\psi c\theta } & {s\psi s\theta s\phi + c\psi c\phi } & {c\phi s\psi s\theta - c\psi s\phi } & 0 & 0 & 0 \\ { - s\theta } & {s\phi c\theta } & {c\phi c\theta } & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & {s\phi t\theta } & {c\phi t\theta } \\ 0 & 0 & 0 & 0 & {c\phi } & { - s\phi } \\ 0 & 0 & 0 & 0 & {s\phi /c\theta } & {c\phi /c\theta } \\ \end{array} } \right]$$

(2)

where s, c and t are the abbreviations of sin, cos and tan, respectively.

The dynamic model of the SUR can be expressed as follows:

$${\varvec{M\dot{v} + C(v)v + D(v)v + g(\eta ) = \tau + \omega }}_{{\varvec{e}}}$$

(3)

where ${\varvec{M}} \in R^{6 \times 6}$ is the inertia matrix, ${\varvec{C(v)}} \in R^{6 \times 6}$ represents the Coriolis–centripetal matrix, ${\varvec{D(v)}} \in R^{6 \times 6}$ describes the hydrodynamic damping matrix, ${\varvec{g(\eta )}} \in R^{6 \times 1}$ expresses the restoring forces and moments (gravity and buoyancy), ${{\varvec{\tau}}} \in R^{6 \times 1}$ describes the forces and moments acting on the SUR, ${\varvec{w}}_{{\varvec{e}}} \in R^{6 \times 1}$ is the vector of the time-varying external disturbances.

Because the SUR is an underactuated system, it mainly realizes underwater obstacle avoidance and positioning through horizontal or vertical motion during performing tasks, as shown in Fig. 2. Among them, S (S1, S2, S3) and D (D1, D2) represent the static obstacle and moving obstacle, respectively. Therefore, to facilitate later controller design, the horizontal and vertical motion equations are decomposed.

Fig. 2

Schematic diagram of the SUR when performing underwater missions

The mathematical model of the horizontal and vertical plane for SUR can be rewritten as

$$\left\{ \begin{gathered} \dot{x} = u\cos \psi - v\sin \psi \hfill \\ \dot{y} = u\sin \psi + v\cos \psi \hfill \\ \dot{\psi } = r \hfill \\ \end{gathered} \right.$$

(4)

$$\left\{ \begin{gathered} \dot{x} = u\cos \theta - v\sin \theta \hfill \\ \dot{z} = u\sin \theta + v\cos \theta \hfill \\ \dot{\theta } = q \hfill \\ \end{gathered} \right.$$

(5)

where (x, y), and (x, z) are the horizontal and vertical vector of the SUR, respectively.

Using the Lagrange method to extract the SUR dynamic model, the results satisfy:

$$\left\{ \begin{gathered} \dot{u} = \frac{{m_{22} }}{{m_{11} }}vr - \frac{{X_{u} }}{{m_{11} }}u - \frac{{X_{u\left| u \right|} u\left| u \right| + F_{u} }}{{m_{11} }} \hfill \\ \dot{v} = - \frac{{m_{11} }}{{m_{22} }}ur - \frac{{X_{u} }}{{m_{22} }}v - \frac{{Y_{v\left| v \right|} v\left| v \right| + F_{v} }}{{m_{22} }} \hfill \\ \dot{r} = \frac{{m_{11} - m_{22} }}{{m_{33} }}uv - \frac{{N_{r} }}{{m_{33} }}r - \frac{{N_{r\left| r \right|} }}{{m_{33} }}r\left| r \right| + g_{r} \hfill \\ \end{gathered} \right.$$

(6)

$$\left\{ \begin{gathered} \dot{u} = - \frac{{m_{44} }}{{m_{11} }}uw - \frac{{X_{u} }}{{m_{11} }}u - \frac{{X_{u\left| u \right|} u\left| u \right| + F_{u} }}{{m_{11} }} \hfill \\ \dot{w} = \frac{{m_{11} }}{{m_{55} }}uw - \frac{{Z_{w} }}{{m_{55} }}w - \frac{{Z_{w\left| w \right|} w\left| w \right| + F_{w} }}{{m_{55} }} \hfill \\ \dot{q} = \frac{{m_{11} - m_{44} }}{{m_{66} }}uw - \frac{{M_{q} }}{{m_{66} }}q - \frac{{M_{q\left| q \right|} }}{{m_{66} }}q\left| q \right| + g_{q} \hfill \\ \end{gathered} \right.$$

(7)

where F_u, F_v, F_w are the lateral and longitudinal forces, respectively; m₁₁, m₂₂, m₄₄, m₅₅ are the parameters of added mass, m₃₃ and m₆₆ represents the added moment; And $X_{u} ,Y_{v} ,Z_{w} ,N_{r} ,M_{q} ,X_{u\left| u \right|} ,Y_{v\left| v \right|} ,Z_{w\left| w \right|} ,N_{r\left| r \right|} ,M_{q\left| q \right|}$ is the drag coefficient, respectively, $g_{r} ,g_{q}$ are the uncertain terms.

2.1 Implemented Trajectory Tracking

To facilitate later controller design for SUR, considering the kinematic control and obstacle avoidance, the following guidance law is proposed:

$$\begin{gathered} \dot{x} = u\cos \psi \hfill \\ \dot{y} = - u\sin \psi \hfill \\ \dot{\psi } = r \hfill \\ \end{gathered}$$

(8)

$$\begin{gathered} \dot{x} = u\cos \theta \hfill \\ \dot{z} = - u\sin \theta \hfill \\ \dot{\theta } = q \hfill \\ \end{gathered}$$

(9)

where $r,q$ represent the angle between SUR and the horizontal or vertical planes, which guide the robot converge to the adjacent waypoint. In addition, $\psi ,\theta$ is bounded and converge to the SUR’s horizontal and vertical plane model.

To make SUR follow the reference path, the guidance strategy is proposed, which is very necessary to complete tasks. The starting point and the target point are defined as (X_sp, Y_sp), (X_tp, Y_tp), respectively. The waypoints (X_i, Y_i) are established to following the reference path, as shown in Fig. 3.

Fig. 3

Framework of the path following for spherical underwater robot

The 3-D trajectory tracking problems of SUR can often use the following equation to define the tracking errors (see [3, 18, 29,30,31], for example):

$$\left[ \begin{gathered} x_{e} \hfill \\ y_{e} \hfill \\ \psi_{e} \hfill \\ \end{gathered} \right] = \left[ {\begin{array}{*{20}c} {c\psi c\theta } & {s\psi c\theta } & { - s\theta } \\ { - s\psi } & {c\psi } & 0 \\ 0 & 0 & 1 \\ \end{array} } \right]\left[ \begin{gathered} x - x_{d} \hfill \\ y - y_{d} \hfill \\ \psi - \psi_{d} \hfill \\ \end{gathered} \right]$$

(10)

$$\left[ \begin{gathered} x_{e} \hfill \\ z_{e} \hfill \\ \theta_{e} \hfill \\ \end{gathered} \right] = \left[ {\begin{array}{*{20}c} {c\psi c\theta } & {s\psi c\theta } & { - s\theta } \\ {s\theta c\psi } & {s\theta s\psi } & {c\theta } \\ 0 & 0 & 1 \\ \end{array} } \right]\left[ \begin{gathered} x - x_{d} \hfill \\ z - z_{d} \hfill \\ \theta - \theta_{d} \hfill \\ \end{gathered} \right]$$

(11)

It can be seen that the trajectory tracking error of SUR’s dynamic model is coupled and complex. In this paper, the tracking error is defined as ${\varvec{e = \eta - \eta }}_{{\varvec{e}}}$ to maneuver the SUR for precisely tracking the trajectory and guarantee the safety of SUR during obstacle avoidance. Among them, ${{\varvec{\eta}}}_{{\varvec{e}}} { = [}x_{e} {,}y_{e} ,\psi_{e} {]}^{T}$ or ${{\varvec{\eta}}}_{{\varvec{e}}} { = [}x_{e} {,}z_{e} ,\theta_{e} {]}^{T}$ represents the desired position trajectory.

2.2 Implemented Obstacle Avoidance Mode

Considering the obstacle avoidance is necessary for path following of AUVs [32,33,34,35,36], to provide the ability of rounding of obstacles, an obstacle avoidance mode is developed and added to the main following controller.

As the eyes of AUVs, sensors are an indispensable tool for sensing the ocean and the surrounding environment, especially the application of underwater obstacle avoidance. Compared with other sensor detection methods, ultrasonic sensor system is widely used because of its low-cost, easy-installation, and not easy to be disturbed by electromagnetic and light. Thus, in this paper, the underwater ultrasonic module (JSN-SR0T4) is used to improve the obstacle avoidance capability of SUR and complete the mission. The relationship between ultrasonic sensor and SUR is shown in Fig. 1. The detection angle is less than 50 ̊ and the resolution is about 5 mm. In the simulation, in Sect. 4, the trajectory tracking and obstacle avoidance performance tests are performed by adding sensor nodes in Webots. Ultrasonic sensor parameters are set with reference to parameters of JSN-SR0T4. In addition, we also use the ultrasonic module to perform a series of obstacle avoidance experiments in Sect. 5 to verify the effectiveness of the proposed control strategy.

The moving obstacle satisfy the equation:

$$\left\{ \begin{gathered} \dot{x}_{{{\text{MO}}}} = u_{{{\text{MO}}}} \cos \theta_{{{\text{OB}}}} \hfill \\ \dot{z}_{{{\text{MO}}}} = - u_{{{\text{MO}}}} \sin \theta_{{{\text{OB}}}} \hfill \\ \end{gathered} \right.$$

(12)

where x_MO and z_MO represent the position, and θ_MO is the attitude of moving obstacle, u_MO represents the velocity. The position relationship between the robot and obstacle satisfies:

$$D{ = }\sqrt {(x_{O} - x_{b} )^{2} + (z_{O} - z_{b} )^{2} } \ge D_{t}$$

(13)

where D_t represents the distance between the SUR and the obstacle at time t.

In underwater environment, the SUR may encounter the static (Fig. 4a) and moving obstacles (Fig. 5b), as shown in Fig. 4. Considering the possibility of tracking control for SUR, the static obstacle and moving obstacle is analyzed to track reference path safely [8]. When SUR detects the static obstacle, as shown in Fig. 4a, if the condition $D < R_{s} ,\theta_{d} < \beta$ is satisfied, the static obstacle avoidance is activated. D is the shortest distance between the SUR and the obstacle center, $\beta$ is the angle between the tangent which pass the center of gravity of SUR and horizontal line. Noted that the static obstacle avoidance is a special case of moving obstacle avoidance, which will be further verified in Sect. 3.

Fig. 4

Principle and related parameters for SUR’s obstacle avoidance. a The principle of obstacle avoidance under the static obstacle; b The principle of obstacle avoidance under the moving obstacle

Fig. 5

The path-following strategy for SUR in presence of obstacles, among them, detecting obstacles (a–c), and performing obstacle avoidance behavior (others)

3 Control System for SUR

3.1 Tracking Mode

Considering the coupling, nonlinearity and complexity of the SUR model, the simplified tracking error model is proposed in this section:

$$\left\{ \begin{gathered} x_{e} = (x - x_{d} )\cos \beta + (y - y_{d} )\sin \beta \hfill \\ y_{e} = - (x - x_{d} )\sin \beta + (y - y_{d} )\cos \beta \hfill \\ z_{e} = z - z_{d} \hfill \\ \end{gathered} \right.$$

(14)

After the above analysis, the Lyapunov function is used to prove the stability of SUR:

$$V_{1} { = }\frac{1}{2}e_{t}^{2} = \frac{1}{2}\left(x_{e}^{2} + y_{e}^{2}\right )$$

(15)

The time derivation of V₁ yields:

$$\dot{V}_{1} = e_{t} \dot{e}_{t} = x_{e} \left(u\cos \beta - u_{d} \cos \beta_{d}\right ) + y_{e} \left(u\sin \beta - u_{d} \sin \beta_{d}\right )$$

(16)

where x_e = lcosβ, y_e = lsinβ_d, l is the distance between current position and next position, and β_d is the virtual angle that is dynamically assigned to follow SUR and avoid obstacles.

Then, we can obtain:

$$\begin{gathered} \dot{V}_{1} { = }l( - u_{d} (\cos^{2} \beta_{d} + \sin^{2} \beta_{d} ) + \cos (\beta - \beta_{d} )) \\ = - k_{1} l^{2} \le 0 \\ \end{gathered}$$

(17)

where k₁ is a positive constant.

3.2 SMC Method for SUR’s Dynamic

Then, we used the SMC method to ensure the robustness of the system in disturbances based on Lyapunov function. The design procedures are as follows:

Step 1: The error variables z₁ and z₂ are defined as follows:

$$z_{1} = \psi - \psi_{d}$$

(18)

$$\dot{z}_{1} = \dot{\psi } - \dot{\psi }_{d} = r - \dot{\psi }_{d}$$

(19)

$$z_{2} = \dot{z}_{1} + k_{2} z_{1}$$

(20)

where k₂ > 0 is a feedback gain, and z₁ is the angle error. Then, the first Lyapunov function V₂ is defined as

$$V_{2} = \frac{1}{2}z_{1}^{2}$$

(21)

$$\dot{V}_{2} = z_{1} \dot{z}_{1} = z_{1} (r - \dot{\psi }_{d} ) = z_{1} (z_{2} - k_{2} z_{1} ) = z_{1} z_{2} - k_{2} z_{1}^{2}$$

(22)

The switching function for sliding mode is expressed as S₁₁:

$$S_{1} { = }k_{3} z_{1} + z_{2} = (k_{2} + k_{3} )z_{1} + \dot{z}_{1}$$

(23)

where k₃ > 0. It is obvious that z₁, z₂ = 0 if S₁ = 0, then $\dot{V}_{2} \le 0$.

Step 2: The second Lyapunov function is expressed using S₁₁ as follows:

$$V_{3} { = }V_{2} + \frac{1}{2}S_{11}^{2}$$

(24)

$$\dot{V}_{3} { = }\dot{V}_{2} + S_{11} \dot{S}_{11} = - k_{2} z_{1}^{2} + z_{1} z_{2} + S_{11} [k_{3} (z_{2} - k_{2} z_{1} ) + \ddot{\psi } - \ddot{\psi }_{d} + k_{2} \dot{z}_{1} ]$$

(25)

Then, the control law is expressed as follows:

$$f_{r} = - k_{3} (z_{2} - k_{2} z_{1} ) - \ddot{\psi } + \ddot{\psi }_{d} - k_{2} \dot{z}_{1} - h_{1} (S_{11} + \varepsilon_{1} {\text{sgn}} (S_{11} ))$$

(26)

where h₁, ε₁ > 0, Substituting (25) into (24) yields:

$$\dot{V}_{3} = - k_{2} z_{1}^{2} + z_{1} z_{2} - h_{1} S_{11}^{2} - h_{1} \varepsilon_{1} \left| {S_{11} } \right|$$

(27)

To deal with $\dot{V}_{3}$, a symmetric matrix Q is defined as follows:

$$Q = \left[ {\begin{array}{*{20}c} {k_{2} + h_{1} c_{1}^{2} } & {h_{1} c_{1} - \frac{1}{2}} \\ {h_{1} c_{1} - \frac{1}{2}} & {h_{1} } \\ \end{array} } \right]$$

(28)

Therefore, $\dot{V}_{3}$ can be rewritten as

$$\dot{V}_{3} \le - z^{T} Qz - h_{1} \varepsilon_{1} \left| {S_{11} } \right|$$

(29)

According to the asymptotic stability theorem, if the partial derivative V₃ satisfies that $\dot{V}_{3}$ is negative definite or semi-negative definite, then the control system is asymptotically stable. Therefore, we can obtain that (28) satisfies that $\dot{V}_{3} \le 0$ if Q is positive definite matrix, and the control system is asymptotically stable under the following condition:

$$\begin{gathered} \left| Q \right| = h_{1} (k_{2} + h_{1} c_{1}^{2} ) - \left(h_{1} c_{1} - \frac{1}{2}\right)^{2} \\ = h_{1} (k_{2} + c_{1} ) - \frac{1}{4} \\ \end{gathered}$$

(30)

3.3 Obstacle Avoidance Capability

Based on the above obstacle avoidance analysis in Sect. 2, we also use the Lyapunov function candidate to verify that the SUR can converge to the boundary of the anti-obstacle area $R_{o} \le R \le R_{s}$:

$$V_{4} { = }\frac{1}{2}e^{2}$$

(31)

The time derivation along V₄ is

$$\dot{V}_{4} { = }e\dot{e} = e(\dot{D} - \dot{R}_{o} )$$

(32)

Based on the definition $\beta$ (in Fig. 4b), the relationship between D and $\beta$ can be rewritten as

$$\left\{ \begin{gathered} D\cos \beta = x_{{{\text{MO}}}} - x_{d} \hfill \\ - D\sin \beta = z_{{{\text{MO}}}} - z_{d} \hfill \\ \end{gathered} \right.$$

(33)

Substituting (12), (32) into (31) obtains:

$$\begin{gathered} \dot{V}_{4} = e\frac{{D\cos \beta (\dot{x}_{O} - \dot{x}_{d} ) - D\sin \beta (\dot{z}_{O} - \dot{z}_{d} )}}{D} \\ = e[u_{{{\text{MO}}}} \cos (\beta - \theta_{{{\text{OB}}}} ) - u_{d} \cos (\theta_{d} - \beta )] \\ \end{gathered}$$

(34)

where $\theta_{d} = \beta + {\raise0.7ex\hbox{$\pi $} \!\mathord{\left/ {\vphantom {\pi 2}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{$2$}} - \arctan {\raise0.7ex\hbox{${(z_{{{\text{MO}}}} - z_{d} )}$} \!\mathord{\left/ {\vphantom {{(z_{{{\text{MO}}}} - z_{d} )} {(x_{{{\text{MO}}}} - x_{d} )}}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{${(x_{{{\text{MO}}}} - x_{d} )}$}}$. Then, (33) can be written as

$$\begin{gathered} \dot{V}_{4} = e\left[ {u_{{{\text{MO}}}} \cos (\beta - \theta_{{{\text{OB}}}} ) - u_{{d{\text{MO}}}} \cos \left( {\frac{\pi }{2} - \arctan \frac{{z_{{{\text{MO}}}} - z_{d} }}{{x_{{{\text{MO}}}} - x_{d} }}} \right)} \right] \\ \le - \frac{{u_{{d{\text{MO}}}} e^{2} }}{{\sqrt {(z_{{{\text{MO}}}} - z_{d} )^{2} + (x_{{{\text{MO}}}} - x_{d} )^{2} } }} = - \frac{{2u_{dMO} }}{{\sqrt {(z_{{{\text{MO}}}} - z_{d} )^{2} + (x_{{{\text{MO}}}} - x_{d} )^{2} } }}V_{4} \\ \end{gathered}$$

(35)

Considering $t \to \infty$, V₄ approaches 0, thus, the stability is guaranteed.

In this study, the APF method is used to improve the capability of local obstacle avoidance. Compared to numerical methods, this method refrains offline entity and local minimization problems, also enhancing the ability to pass moving obstacles.

The repulsive potential function is considered as

$$P_{{{\text{rep}}}} = 1/\left\| {\frac{{D_{{{\text{obs}}}} }}{{R_{s} }}} \right\| + \log \left\| {\frac{{D_{{{\text{obs}}}} }}{{R_{s} }}} \right\|$$

(36)

where D_obs = D is the momentary shortest distance between the SUR and the center of obstacle, and R_s is the radius of the surrounded area of obstacles. According to $f_{rep} = - \gamma \nabla P_{rep}$, we have

$$\left\{ \begin{gathered} f_{{{\text{rep}},x}} = - \gamma (x - x_{o} )\frac{{(D_{{{\text{obs}}}}^{2} - R_{s}^{2} )}}{{D_{obs}^{6} }} \hfill \\ f_{{{\text{rep}},y}} = - \gamma (y - y_{o} )\frac{{(D_{{{\text{obs}}}}^{2} - R_{s}^{2} )}}{{D_{obs}^{6} }} \hfill \\ f_{{{\text{rep}},z}} = - \gamma (z - z_{o} )\frac{{(D_{{{\text{obs}}}}^{2} - R_{s}^{2} )}}{{D_{{{\text{obs}}}}^{6} }} \hfill \\ \end{gathered} \right.$$

(37)

where $\gamma$ is an adjustable obstacle avoidance gain. Adding the obstacle avoidance feature f_rep, to resist external disturbance, enhance stability and avoid jitter phenomenon, the continuous function $\theta (s)$ is used. Thus, the controlling input of SUR dynamic system can be written as follows:

$$f{ = }M^{ - 1} (\dot{\eta }{ - }C_{{{\text{input}}}} + \Omega \varepsilon - K\theta (s) + f_{{{\text{rep}}}} )$$

(38)

where $C_{{{\text{input}}}}$ is determined by the horizontal dynamic equation, vertical dynamic equation of SUR, and the input vector; ${{\varvec{\Omega}}}$ and K are the positive definite gain matrix and the diagonal controlling gain, respectively.

According to the above analysis, the effectiveness of the avoidance capability for SUR is verified.

4 Simulation Results

In this section, some simulation scenarios are performed to verify the effectiveness, stability and efficiency of the proposed path-tracking and obstacle avoidance scheme. Compared to the traditional method, the Webots software is adopted to design the scenarios which consider the SUR’s model and parameters, such us mass, thrusters and joints, etc. In addition, it simulates the dynamic effects realistically and verify the algorithm performance effectively.

Based on the above controller stability and robustness analysis, we construct a path following strategy, as shown in Fig. 5. The trajectory tracking strategy is implemented by adding related sensors and parameters setting in Webots.

4.1 Simulation Setup

Webots is physics robotics simulation software, which can be used for creating robot prototype or achieving autonomous control. After importing the SUR model, the parameters, such as liquid environment and node of SUR, are set in Table 1. Ultrasonic sensor is configured, including distance, detection angle, etc. Controllers are then specified through Webots Application Programming Interface (API). The sensor is activated using the enable function. In this paper, the SUR works in 3D environment and follows the reference path in presence of the static and moving obstacles.

Table 1 Specific features of the SUR prototype in simulation

After completing the setup of sensors, node properties of robot and underwater environment, we built the corresponding controller using C++. SUR completes obstacle avoidance of moving and static obstacles by following reference path to verify the performance of the proposed control strategy (SMC–APF).

4.2 Simulation Results

4.2.1 Case Study 1

In this part, the capabilities that the SUR follow the reference trajectory is explored. To verify the path following capability of SUR in 3D environment, the reference path is designed as shown in Fig. 6, which is to effectively verify the above path following strategy, in Sect. 3. Noted that, in the underwater environment, we set a certain disturbance acting on the underwater robot to check the robustness of the SMC–APF.

Fig. 6

Path following experiments for SUR in 3D environment

The process of the path-following: first, the SUR starts from the starting point, follows the reference trajectory to the Waypoint1, and then performs the diving locomotion (Waypoint1) and the ascending motion (Waypoint2), which is to verify the capability of path-following in the vertical plane, from 14 to 24 s. Then, SUR will continue to follow the reference path to the Waypoint4, and perform the path-following in the vertical plane continuously, from the Waypoint4 to the Waypoint6. Finally, SUR will follow the rest of the path, until to the target point.

To show the superiority of the SMC–APF, the trajectory of the SUR for tracking the reference path is obtained, as shown in Fig. 7, which depict the comparison of the desired path and actual trajectory of the SUR. It can be seen that the robot can track the reference path successfully and the trajectory is smooth.

Fig. 7

Path following results of the SUR in 3D environment

4.2.2 Case Study 2

AUVs need to track the different desired paths, to verify the capability of the SMC–APF to bypass the obstacles, two scenarios are considered, the static obstacle and moving obstacle, in the next section. Based on the above analysis, the SUR is supposed to track the similar trajectory, so in this part, we add three static obstacles to track the trajectory, and the capability of the obstacle avoidance in different plane is integrated, as shown in Fig. 8.

Fig. 8

Path following experiments in presence of static obstacles

The process of the static obstacle avoidance: first, the SUR starts from the starting point, and follows the reference trajectory (cruise 1). When the static obstacle (S1) is detected, the obstacle avoidance on the vertical plane will be triggered (obstacle avoidance 1). Note that when the distance detected obstacle on the vertical distance is less than or equal to the distance in horizontal plane, the obstacle avoidance will be triggered prioritized. When the obstacle is bypassed, the SUR will perform the behavior (cruise 2) continually. Then, when the static obstacle (S2) is detected, the obstacle avoidance 2 will be triggered. Using this benchmark until the SUR reaches the target point. Noted that, the obstacle (S3) is specially considered to verify obstacle avoidance in the horizontal plane.

As can be seen from Fig. 9, the SMC–APF passes the obstacles successfully, the actual trajectory is close to the reference trajectory. S1, S2 and S3 indicates the obstacle avoidance area of SUR.

Fig. 9

Path following results in presence of static obstacles

4.2.3 Case Study 3

After the static obstacle avoidance is analyzed, the moving obstacle is added, we use the SUR as the moving obstacle which is operated around the reference path, as shown in Fig. 10.

Fig. 10

Path-following experiments with static obstacle and moving obstacle

It can be seen from Fig. 10 that the SUR is performing the moving obstacle avoidance. Before 7 s, the SUR first complete the cruise mission alone the reference path. When the moving obstacle is detected, the obstacle avoidance behavior is triggered, and SUR will find a safe path using guidance principle to avoid obstacle. After 17 s, the SUR will follow the reference path and avoid obstacle, at the time = 54 s, the SUR reaches the target point.

Figure 11 displays the overall trajectory of the SUR. The trajectory of SUR verifies the capability of guidance law to the static and moving obstacle, we also can see that the SUR can quickly switch to the path-following mode after avoiding obstacle. S2 and S3 indicates the area of the static obstacle, and D1 indicates the area of the moving obstacle.

Fig. 11

Path-following results with static obstacle (S2, S3) and moving obstacle (D1); Among them, point Q and R represent the starting position and target position of the moving obstacle, respectively

5 Experimental Results

After some simulation experiments are performed, to further verify the effectiveness of proposed self-guided and obstacle avoidance strategy, some experiments in the real environment are carried out, including tracking and avoiding obstacle experiments. Noted that in the multi-obstacle experiments, only two static obstacles and one moving obstacle are set on the reference path due to limit of the size of the experimental pool.

5.1 Control System Setup

To improve the stability and logic of the control system, four parts, including Power supply layer, Decision layer, Sensor layer and Driving layer, are divided. The power monitoring system consists of three detachable batteries, one (Battery I) is used for powering the Control system and Sensor layer, and two (Battery II) is used for powering the hybrid drive device and steering engine. Battery working time is typically about 2 h during experiments. The Ultrasonic sensor can be used to collect surrounding information to ensure the robot perform tasks safely. The IMU and pressure sensor is utilized and used to perceive and adjust the robot attitude. The communication device (Micron data modem (Tritech)) is used for real-time communication.

During the tracking process, the current position of the SUR is obtained by processing the data of the IMU 9250 and Depth sensor JY901 to realize the positioning and navigation of the robot. The characteristic of the SUR is described in Table 2. In the Control system, a complementary filtering algorithm is adopted to estimate the data, which is detailed in [3], and the position of the SUR is fed back in real-time to achieve a guided closed-loop. During underwater motion, we also define the Earth-fixed coordinate system and body-fixed coordinate system. The motion state is obtained by calculating the distance between the initial position and the current position of the robot.

Table 2 Specification of the SUR

5.2 Performance of Tracking Experiments

To verify the effectiveness of the SMC–APF for SUR, first, the tracking experiment is performed, as shown in Fig. 12. The robot starts from the initial position, at time = 0 s (point A). When the robot reaches the point B and C, tracing on the vertical plane will be performed, namely, the diving and ascending process. About time = 34 s, the SUR reaches the target position. Due to some external factors, such as water waves and wind, the markers will move in a small range, which is also one of the reasons for the relatively large error in the real environment. The tracking trajectory of the SUR is shown in Fig. 13. It can be seen that the SUR can track the reference trajectory successfully using the proposed control strategy. Due to the limited depth of the pool, the set depth is 50 cm in this paper.

Fig. 12

Tracking reference path for SUR in real 3D environment, the total time is 34 s (a–h), from the initial position A to the target position.

Fig. 13

Tracking results of the SUR in real environment

5.3 Avoiding Static and Moving Obstacle Experiments

Then, the SUR tracks the reference path in presence of static and moving obstacles, as shown in Fig. 14. Due to limit of the size of the pool, this paper mainly validates the performance of avoiding obstacles in horizontal plane. Two stationary obstacles (OS 1 and OS 2) with a diameter of 0.1 m and one moving obstacle (OM 1) with a diameter of 0.35 m were placed at the reference path.

Fig. 14

The movement process of the SUR when there is static and moving obstacles; a–j the total time is 39 s, among them, a–d are the process of avoiding moving obstacle, and e–j are the process of avoiding static obstacles

First, the SUR starts from the starting point, see Fig. 14a, and follows the reference trajectory. When the OM 1 is detected, the obstacle avoidance on the horizontal plane will be triggered, see Fig. 14c. When the obstacle is bypassed, the SUR will track reference path continually. Then, when the OS 1 and OS 2 are detected, the obstacle avoidance will be triggered, see Fig. 14f, i. About 39 s, the SUR reaches the target point.

The tracking trajectory of the SUR is shown in Fig. 15. As can be seen that the SUR can bypass the obstacles successfully. O1, O2 and O3 indicates the moment when the SUR is avoiding obstacles. After avoiding obstacles, the robot can rapidly follow the reference path.

Fig. 15

Trajectory tracking result of SUR in presence of static and moving obstacles

6 Discussion

The SUR can converge to the reference path after avoiding obstacles, to further verify the performance of the SMC–APF, the tracking error is further analyzed.

In Sect. 4.2, the SUR response to the reference path is successful considering the mentioned disturbances and uncertainties. The reference path is determined by the waypoints. To more intuitively reflect the performance of the SMC–APF, the workspace error is calculated, as shown in Fig. 16. It can be seen that the related error of the SMC–APF is 0.08 m, which further verifies that the SMC–APF provides accurate and stable tracking for SUR. In addition, the simulation results show that the SMC–APF always ensures the tracking error converge to an arbitrarily small neighborhood of zero, have a small position error. Noted that, when there is no obstacle, the SUR tracks the reference trajectory, and the position error is less than 0.05 m, which further verifies that SUR has excellent tracking performance. After adding obstacles, SUR can follow the path safely, the maximum position error is less than 0.08 m, and the tracking curve has good convergence. To sum up, the SUR can quickly follow the safe path after avoiding obstacles. These simulations results validate the strong robustness, high efficiency, effectiveness and stability of the SMC–APF, which provides the solution method for further applications in reality.

Fig. 16

Position tracking errors for the simulation in tracking experiment (Scenario 1, see Fig. 7), avoiding static obstacle (Scenario 2, see Fig. 9) and moving obstacle (Scenario 3, see Fig. 11)

In addition, we analyzed the repulsive force performance of the SMC–APF method (see Figs. 9 and 11), from Waypoint A to Waypoint F. It can be seen that when encountering static and moving obstacles, the repulsive force of the obstacles will help the SUR to avoid the obstacles effectively, which is determined by the synthetic force of the obstacle repulsion force and the target attractive force. Compared with static obstacle avoidance, the deviation of dynamic obstacle avoidance from the reference trajectory is larger, but less than 2.5 cm, and larger amount of control effort is required to bypass moving obstacle to ensure the smoothness and stability of the SMC–APF.

In Sect. 5, the SUR can successfully track the reference path and avoid obstacles. To further verify the performance of the SMC–APF, a series of comparative experiments are performed. In [1], An et al. completed obstacle avoidance experiments in a pool (with an area of 300 cm × 200 cm × 100 cm). We set up a stationary obstacle (OS1) in the single obstacle avoidance experiment. The SUR starts from the starting position, time = 0 s, see Fig. 17. It takes about 13 s for SUR to complete the tracking mission using the SMC–APF. The time was optimized for about 1 s. In [2], Li et al. performed obstacle avoidance experiments in a pool (with an area of 250 cm × 200 cm × 90 cm). Mentioned in [2] that the robot tracking error is stable within 0.1 m in a series of experiments. Thus, we calculate the coordinate error, see Fig. 18b, which is stable within 5 cm compared to the ideal trajectory. Considering that the error of the sensor in water is less than 0.2 cm, the dynamic response is strong, and the measurement accuracy is high, the error caused by the sensor is ignored in the error analysis. Of course, due to different experimental conditions, there may be some deviations, which will not be refuted. Furthermore, we compare the SMC–APF with previous research methods, including FC [2], proportional–integral–derivative (PID) [26], and ant colony control (ACO) [1] methods. The initial position is (0.25 m, 0.25 m, − 0.15 m) and the target position is (2.75 m, 0.25 m, − 0.15 m). The length of the desired path is about 250 cm. The trajectory tracking result using SUR under four different methods are shown in Fig. 18. It can be seen that compared with the previous three methods, the SMC–APF has better tracking performance, and the tracking error is reduced by about 3.54 ± 0.61 cm. It can be seen that the SMC–APF can converge better than the FC, ACO and PID methods, which can quickly converge after avoiding the obstacle and track the desired trajectory. Table 3 analyzes and calculates from the three perspectives of length, tracking time and error. It can be seen that compared with the other three methods, the performance is better, namely, the time is more optimized, and the error is smaller. In summary, the SMC–APF has further improved the obstacle avoidance performance of SUR, and in future work, we will further test it in multi-robot coordinative working.

Fig. 17

The movement process of the SUR when there is static obstacle, compared with [1, 2]; From a–d, the total time is 13.5 s

Fig. 18

Trajectory tracking result of SUR in presence of static obstacles, compared the SMC–APF with previous research methods, including ACO [1], FC [2], and PID [26] methods. a Results in 3D coordinates; b Results in 2D coordinates

Table 3 Results of comparative experiments with previous research methods, including FC, PID and ACO methods

7 Conclusions and Future Work

A path-following strategy was proposed for bioinspired SUR in the presence of obstacles. First, the kinematic and dynamic model of SUR was extracted. Then, guidance and obstacle avoidance laws were developed to track the trajectory safely and quickly. The Lyapunov theory and SMC method were implemented and analyzed, which validated the stability and robustness of proposed control strategy (SMC–APF). Afterward, the capability of real-time performance, such as bypassing static or moving obstacles was verified using APF. Next, based on the proposed control scheme, a series of experimental scenarios were designed using Webots, including path following, static and moving obstacle avoidance, etc. Furthermore, the tracking and avoiding obstacle experiments in uncertain moving obstacle were conducted. Compared with [1, 2], both the tracking error and time were optimized, it could be seen that the proposed adaptive law solved the dynamic uncertainties and disturbances effectively. As a result, by the aid of the proposed strategy, the stability and accuracy of the SUR could be guaranteed using ultrasonic sensor, and the obstacles could be successfully avoided in an adaptive manner. In the future, we will apply the proposed control strategy for underwater missions.