An Improved Backstepping Controller with an LESO and TDs for Robust Underwater 3D Trajectory Tracking of a Turtle-Inspired Amphibious Spherical Robot

Abstract

In this paper, a double closed-loop backstepping controller is designed for 3D trajectory tracking of a turtle-inspired amphibious spherical robot suffering from problems that include model uncertainties, environmental disturbances, and unmeasured velocity. The proposed controller scheme tackles three primary challenges: the differentiation explosion of the traditional backstepping method, unmeasured velocity, and the consideration of lumped disturbances. Beginning with an outer-loop backstepping controller, a virtual feedback variable is constructed to simplify the design of the backstepping controller. Meanwhile, to avoid the problem of differentiation explosion, tracking differentiators (TDs) are utilized to estimate the differentiation of the desired velocity in an inner-loop backstepping controller. Moreover, there are some uncertainty disturbances in the task of tracking the trajectory of a turtle-inspired amphibious spherical robot (TASR), such as the parameters of the hydrodynamic model and environmental disturbances. A linear extended state observer (LESO) is designed to estimate and compensate for the lumped disturbances. Furthermore, as the velocity states of the TASR are unmeasured, the LESO is also utilized to estimate the velocity states in surge, yaw, and heave degrees. Therefore, the TASR only needs to supply its position and orientation information for the trajectory tracking task. Note that this paper details both the design process of the proposed controller and a rigorous theoretical analysis. In addition, numerical simulations are conducted, and the results demonstrate the feasibility and superiority of the proposed method.

1. Introduction

In recent years, with the shortage of land resources, human exploration of the ocean has intensified. In order to exploit and utilize marine resources more efficiently, many bio-inspired underwater robots have been developed in countries around the world. For example, Wang et al. developed a fish-shaped robot called Robcutt II [1]. Alessandro Crespi put forward a Salamandra Robotica II amphibious robot that is able to swim and walk [2]. A soft robotic fish was designed by Katzschmann [3]. Daniele Costa et al. designed a bionic autonomous underwater robot based on the fluid shape of fish bodies [4]. Our team has researched a series of amphibious spherical robots [5,6].

Generally, research concerning bionic underwater robots emphasizes control in terms of not only external appearance but also internal motion; control is a basic necessity for completing tasks. Generally, basic motion control of underwater vehicles requires tracking a predetermined underwater trajectory. At present, a large number of studies in the literature focus on the design of trajectory tracking controllers for underwater robots.

With respect to model uncertainty, which is a common problem, a controller independent of the model is the best choice; examples include the proportional integral derivative (PID) controller [7,8], the sliding-mode controller (SMC) [9,10], the neural network controller, and so on [11,12]. PID controllers perform well in tracking trajectory but lack the ability to reject disturbances. Sliding-mode controllers offer an algorithm for trajectory tracking that is very popular due not only to its model independence but also its good anti-interference performance. Elmokadem et al. proposed a terminal sliding-mode controller for AUVs that considered system uncertainty and environmental disturbances [13]. The simulation results showed that the controller was robust under bounded disturbances. Cui adopted the Lyapunov method to design an integral sliding-mode controller that successfully solved the problems involved in controlling robots with unknown velocity, unknown disturbances, and unknown hydrodynamic model parameters [10]. Although the sliding-mode controller is not sensitive to model parameters, its control quantity exhibits a high-frequency chattering phenomenon, which is not conducive to the execution of actuators. The neural network algorithm is another commonly utilized algorithm in underwater trajectory tracking. In [11], the authors proposed an efficient neural network approach with a single-layer structure for tracking control. Miao et al. designed a novel adaptive neural network tracking controller by combining dynamic surface control (DSC) and a minimal learning parameter (MLP) [12]. In addition, a radial basis function neural network was employed to account for tracking errors. Neural-network-based tracking controllers are insensitive to the dynamic model of the robot and possess strong adaptability and learning capabilities. However, due to the changeable nature of the underwater environment, the training and learning processes are difficult, which leads to poor real-time performance of the tracking algorithm.

If the kinematic model and hydrodynamic model of an underwater vehicle can be acquired, we can design a more suitable controller according to the approximate model parameters to achieve better control effectiveness.

Some algorithms based on models have been researched for trajectory tracking, such as model predictive control (MPC), backstepping control (BSC), and so on. Zhang et al. applied MPC to achieve three-dimensional trajectory tracking of an autonomous underwater vehicle (AUV). In [14], the conventional MPC algorithm was improved upon, and an event-trigger-based MPC algorithm was applied to the straight-line trajectory tracking task of an underactuated underwater vehicle (UUV). Shen et al. developed a Lyapunov-based model predictive control (LMPC) [15]. They considered practical constraints, for example, the actuator saturation. The algorithm was simulated and tested on the Saab SeaEye Falcon model AUV and achieved good results. They also proposed a nonlinear model predictive control method that realized a different type of underwater trajectory tracking for the AUV [16]. However, in order to achieve a suitable control quantity, the MPC method needs to predict many steps in advance, which results in the dimension of the weight matrix being very large. Therefore, the computational efficiency of the MPC is reduced, and real-time performance is somewhat poor.

Lyapunov-based backstepping controls represent the mainstream method for AUV tracking controls because their control law exploits ’good’ nonlinearities in the system dynamics. Xu et al. adopted the backstepping method to determine virtual control quantity, then designed a controller that allowed a weight-bearing autonomous underwater vehicle (WAUV) to track its underwater trajectory [17]. In order to solve the differential explosion problem caused by using the backstepping method for the 3D path tracking of an underactuated underwater vehicle, Zhou et al. utilized the derivative output value of a biologically inspired model to replace the repeated calculation of multiple derivatives, which greatly reduced the complexity of the controller [18]. In [19], the backstepping method and the disturbance state observer were combined to design a controller for trajectory tracking. Although backstepping-method-based controllers can be designed quickly and efficiently, they are not suitable for controlling vehicles with large dimensions; this is especially true for backstepping controllers with more than three levels. High-order derivative terms can amplify high-frequency micro-noise, which is not ideal for the controller.

From this analysis, it can be seen that each control algorithm possesses both advantages and disadvantages. In addition, the research focus of this paper, namely the turtle-inspired spherical robot (TASR), possesses characteristics such as unmeasured velocity, uncertain model parameters, and a nonlinear system; these factors present challenges for the design of a trajectory-tracking controller. Therefore, the design of a combined controller based on multiple algorithms is needed for the underwater trajectory tracking of TASRs. In this paper, the Lyapunov-based backstepping algorithm is adopted as the main controller, making full use of its good performance with respect to nonlinear systems. TDs and LESO are added to the main controller as auxiliary algorithms. ESOs are popular due to their ability to estimate uncertain information, including disturbances and state information. Xie et al. designed a reduced-order LESO to estimate disturbances that needs velocity information as input [20]. In [21], a nonlinear ESO was constructed to estimate the unmeasured velocity and disturbances. Inspired by [21], an LESO is put forward to avoid inaccurate estimates inherent in the use of nonlinear ESOs.

The primary contributions of this paper are summarized in three parts:

(1) A backstepping-based double closed-loop controller is designed. Beginning with an outer-loop backstepping controller, a virtual feedback variable is introduced to simplify the process.

(2) TDs are shown to be capable of tracking the differential signal of the desired velocity provided by the outer-loop controller, which solves the differential explosion problem.

(3) An LESO is designed to estimate the velocity states and lumped disturbances; it only requires position and orientation information. In addition, we introduce a rotating inertial coordinate to linearize the design of an ESO in terms of surge direction.

The rest of this paper is organized as follows. The modeling of a turtle-inspired amphibious spherical robot is introduced in Section 2. In Section 3, a double closed-loop backstepping controller with TDs and an LESO is designed, and the stability proof is presented. The results of the simulation are discussed in Section 4. Finally, the conclusions and directions for future work are summarized in Section 5.

2. Modeling of a Turtle-Inspired Amphibious Spherical Robot

2.1. The Mechanisms of the TASR

Based on the amphibious characteristics of sea turtles, especially the more flexible movement of sea turtles in the water, a turtle-inspired amphibious spherical robot was designed, as shown in Figure 1. As introduced in Reference [6], the robot consists of upper and lower hemispheres that are joined with a mid-plate. The upper hemisphere contains a passive water storage tank and a sealed cabin. The electrical system is placed in the sealed cabin. The lower hemisphere consists of two separate quarter spherical shells that can be opened or closed by servo motors on both sides according to the state of the TASR. Generally speaking, the two separate quarter spherical shells close when the robot maintains a hovering state but otherwise remain open. In addition, the vector propulsion mechanism is mounted on the mid-plate. Figure 2 shows the mechanical structure of the propulsion mechanism, which consists of four sets of propulsion units [6]. Each propulsion unit possesses three joints, with a water jet motor at the end of the lowest joint. Due to the limited availability of space for equipment, the TASR is only equipped with a stereo camera, an inertial measurement unit (IMU), 12 pressure sensors, and an acoustic communication module. Hence, the TASR is only able to perceive its position by use of the global locating system, the pressure sensor, and the orientation information provided by the IMU. The concrete technical specifications of the TASR are listed in

Table 1. The technical specifications of the TASR.

Items	Parameters
Dimension (Width × Lenght × Height)	30 cm × 60 cm × 30 cm
Mass in air	6.5 Kg
Max thrust	3.8 N
Battery capacity	7.4 V rechargeable Ni-MH batteries (13,200 mAh)
Operation time	Average 100 min

.

2.2. The Circuit System of the TASR

The circuit system of the TASR is depicted in Figure 3. It contains four parts: the energy supply level, the sensor level, the decision and control level, and the executive level. The design of the circuit system involved confronting some important difficulties, including controlling multiple PWM devices, quickly processing visual information, handling decision and controller algorithms, and so on. In order to solve these problems, we adopted two control boards for the decision and control level. The TASR system contains 14 servo motors and 4 propellers in total, which require 18 PWM channels. On account of the fact that the STM32F4 series single-chip microcomputer possesses a sufficient number of PWM channels and is very commonly used, we chose it as the lower control board. The STM32F4 focuses on driving the operation of PWM devices and acquiring depth information. We utilized a Jetson series development board running a Ubuntu system as the main control board; it processes the information of the binocular camera and realizes human-computer interactions.

2.3. Allocation of Propeller Force

It is notable that the vector propulsion of the robot is very flexible. In order to ensure that the center of gravity and the centroid coincide, an “H”-shaped propulsion strategy was adopted for the 3D trajectory tracking task, as shown in Figure 4. The “H”-shaped propulsion strategy means that the four legs keep their hip and knee joints still, and each leg can only be adjusted by changing the angle of the ankle joint. In this situation, the robot can only generate three forces/movements: surge force ${\tau }_u$, heave force ${\tau }_{\nu }$, and yaw movement ${\tau }_w$, which constitute the thrust vector $\mathbf{\tau }\in {\mathbf{R}}^{3\times 1}$.

The total force/movement $\mathbf{\tau }$ of the robot is composed of the thrust of each propeller. $\mathit{F}=[F_1,F_2,F_3,F_4]$ is the thrust vector of propeller, $\mathbf{\tau }$ is defined as:

$$\mathbf{\tau }=\mathit{B}\mathit{F}$$

(1)

where $\mathit{B}\in {\mathbf{R}}^{4\times 4}$ is the allocation matrix of thrust. When the thrusters are allocated according to “H” mode, $\mathit{B}$ is defined as follows:

$$\mathit{B}=\left[\begin{array}{cccc}-cos{\gamma }_1& cos{\gamma }_2& cos{\gamma }_3& -cos{\gamma }_4\\ 0& 0& 0& 0\\ -sin{\gamma }_1& -sin{\gamma }_2& -sin{\gamma }_3& -sin{\gamma }_4\\ cos{\gamma }_1& -cos{\gamma }_2& cos{\gamma }_3& -cos{\gamma }_4\end{array}\right]$$

(2)

where ${\gamma }_i$($i=$1,2,3,4) represents the angles of the four ankles, as plotted in Figure 5. The range of ${\gamma }_i$ is set as $-\pi /6\le {\gamma }_i\le \pi /6$.

2.4. Kinematics and Dynamics Model of the TASR

For convenience of analysis, two coordinate systems are considered: the inertial frame $O-X_I{Y}_I{Z}_I$ and the robot body frame $O-X_B{Y}_B{Z}_B$, as shown in Figure 6. The inertial coordinate system is used to reflect the global position and attitude angle of the robot. When analyzing the speed and thrust of the robot, it is convenient to use the body coordinate system. The position and attitude vector of the 6-DOF robot in the inertial frame is defined as:

$$\mathbf{\eta }=[x,y,z,\varphi ,\psi ,\theta ]$$

(3)

where $x,y,$ and z represent the position of the TASR in the inertial frame, and $\varphi ,\psi ,$ and $\theta$ stand for the Euler angle of roll, pitch, and yaw. For the body coordinates, the velocity vector of the TASR is written as follows:

$$\mathbf{v}=[u,\nu ,w,p,q,r]$$

(4)

where $u,\nu ,$ and w are linear velocities of the TASR in x-direction, y-direction, and z-direction, respectively, denoted as the surge, sway, and heave, respectively. $p,q,r$ are the angular velocities around x-axis, y-axis, z-axis, respectively. As the “H”-shaped propulsion strategy is adopted for trajectory tracking, the driving force can only be provided in terms of the degrees of surge, yaw, and heave. Therefore, the degrees of sway, roll, and pitch are neglected in this paper. Let $\mathbf{\eta }=[x,y,z,\theta ]$ and $\mathbf{v}=[u,w,r]$. The relationship between $\mathbf{\eta }$ and $\mathbf{v}$ is shown as follows:

$$\dot{\mathbf{\eta }}=\mathcal{R}(\theta )\mathbf{v}$$

(5)

where

$$\mathcal{R}(\theta )=\left[\begin{array}{ccc}cos\theta & 0& 0\\ sin\theta & 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

(6)

According to the Newton method, the dynamic model of the TASR is established:

$$\mathbf{M}\dot{\mathbf{v}}+\mathit{C}\left(\mathbf{v}\right)\mathbf{v}+\mathit{D}\left(\mathbf{v}\right)\mathbf{v}+\mathit{g}(\mathbf{\eta })=\mathbf{\tau }$$

(7)

where $\mathit{M}\in {\mathbf{R}}^{3\times 3}$ is the inertia and added mass matrix; $\mathit{C}\left(\mathbf{v}\right)\in {\mathbf{R}}^{3\times 3}$ is the Coriolis and centripetal matrix; $\mathit{D}\left(\mathbf{v}\right)\in {\mathbf{R}}^{3\times 3}$ is the damping matrix, which is positive-definite; $\mathit{g}(\mathbf{\eta })$ is the restoring force and torque vector; and $\mathbf{\tau }$ is the driving force vector.

As the structure of the robot is symmetrical, the inertial matrix and added matrix can be expressed as Equation (9) [22]. Because the underwater motion speed of the robot is not high, i.e., it is less than 0.5 m/s, the Coriolis and centripetal matrix $\mathit{C}\left(\mathbf{v}\right)$ can be ignored. Moreover, because the center of gravity and the buoyant center are at the same position, which is the geometric center of the BSR, the restoring force and torque vector $\mathit{g}(\mathbf{\eta })$ can be set to 0 vector ($\mathit{g}(\mathbf{\eta })={\mathbf{0}}_{4\times 1}$). The simplified model is shown as follows:

$$\mathbf{M}\dot{\mathbf{v}}+\mathit{D}\left(\mathbf{v}\right)\mathbf{v}=\mathbf{\tau }$$

(8)

where

$$\mathit{M}=\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& 0\\ 0& 0& m_{33}\end{array}\right]$$

(9)

$$\mathit{D}\left(\mathbf{v}\right)=\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& 0\\ 0& 0& d_{33}\end{array}\right]+\left[\begin{array}{cccc}{X}_u\left|u\right|& 0& 0& \\ 0& Z_w\left|w\right|& 0\\ 0& 0& N_r\left|r\right|\end{array}\right]$$

(10)

$$\mathbf{\tau }={[{\tau }_u,{\tau }_w,{\tau }_r]}^T$$

(11)

Equations (5) and (7) can be written as:

$$\dot{\mathbf{\eta }}=\mathcal{R}(\theta )\mathbf{v}$$

(12)

$$\dot{\mathbf{v}}={\mathit{M}}^{-1}\mathbf{\tau }-{\mathit{M}}^{-1}\mathit{D}\left(\mathbf{v}\right)\mathbf{v}$$

(13)

3. Design of the Double Closed-Loop Backstepping Controller with an LESO and TDs

In order to achieve trajectory tracking for the TASR, a double closed-loop backstepping control with an LESO and TDs algorithm is proposed, as shown in Figure 7. In addition, the Lyapunov stability proof is provided. An outer-loop kinematic-model-based backstepping controller is designed to export the desired intermediate control variables according to the reference trajectory. An inner-loop backstepping controller is based on the dynamic model of the TASR. Furthermore, an LESO is added into the inner-loop controller to solve two primary problems. First, the design of the LESO is utilized to estimate the model uncertainty and the environmental disturbances. Second, the unmeasured velocity information is provided by an LESO estimation. Furthermore, to avoid the differential explosion of the inner-loop backstepping controller, TDs are introduced to track the derivative of the desired intermediate control variables from the outer-loop controller.

3.1. Outer-Loop Backstepping-Based Controller

For the sake of convenience, the dynamics mode of the TASR in Equation (13) is unfolded as the following:

$$\left[\begin{array}{c}\dot{u}\\ \dot{r}\\ \dot{w}\end{array}\right]=\left[\begin{array}{c}{a}_{1}u+a_2{u}^2+a_3{\tau }_u\\ b_{1}r+b_2{r}^2+b_3{\tau }_r\\ c_{1}w+c_2{w}^2+c_3{\tau }_w\end{array}\right]$$

(14)

The reference pose of the robot is written as ${\mathbf{\eta }}_{ref}=[x_{ref},y_{ref},z_{ref}{\theta }_{ref}]$. The derivative of the reference trajectory is denoted as ${\dot{\mathbf{\eta }}}_{ref}=[u_{ref},v_{ref},w_{ref},r_{ref}]$. Note that $u_{ref}$ and $v_{ref}$ represent velocity in the inertial frame. Further, the pose error in the TASR body coordinate system is defined as ${\mathbf{\eta }}_{e_B}=[x_{e_B},y_{e_B},{\theta }_{e_B},z_{e_B}]$. The subscript $e_B$ represents the error in the frame $O-X_B{Y}_B{Z}_B$. Figure 8 depicts the pose error in the horizontal plane. The pose error in the inertial frame can be transformed into that of the body frame by Equation (15).

$${\mathbf{\eta }}_{e_B}=\left[\begin{array}{c}{x}_{e_B}\\ y_{e_B}\\ {\theta }_{e_B}\\ z_{e_B}\end{array}\right]=\left[\begin{array}{ccc}cos\theta & 0& 0\\ -sin\theta & 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]({\mathbf{\eta }}_{ref}-\mathbf{\eta })=\mathcal{T}(\theta )({\mathbf{\eta }}_{ref}-\mathbf{\eta })$$

(15)

where $\mathcal{T}(\theta )$ is the matrix of rotation transformation, and ${\eta }_e$ can be regarded as the error between the target trajectory and the robot. If ${lim}_{t\to \infty }\left(\right|x_{e_B}|+|y_{e_B}|+|{\theta }_{e_B}|+|z_{e_B}\left|\right)=0$, the TASR is considered as having tracked the reference trajectory.

After deriving the two sides of Equation (15), the following state equation of pose error is obtained:

$${\dot{\mathbf{\eta }}}_{e_B}=\left[\begin{array}{c}{\dot{x}}_{e_B}\\ {\dot{y}}_{e_B}\\ {\dot{z}}_{e_B}\\ {\dot{\theta }}_{e_B}\end{array}\right]=\left[\begin{array}{c}r{y}_{e_B}-u+u_{ref}cos{\theta }_{e_B}\\ r{x}_{e_B}+u_{ref}sin{\theta }_{e_B}\\ w_{ref}-w\\ r_{ref}-r\end{array}\right]$$

(16)

It can be seen from Equation (16) that $x_{e_B}$, $y_{e_B}$, and ${\theta }_{e_B}$ are coupled. $x_{e_B}$ is regulated by forward speed u in the robot coordinate system. ${\theta }_{e_B}$ is regulated by heading angular velocity r. In addition, $y_{e_B}$ is regulated by $x_{e_B}$ and ${\theta }_{e_B}$ together. So, we can adjust $x_{e_B}$ and ${\theta }_{e_B}$ to make ${lim}_{t\to \infty }\left|x_{e_B}\right|=0$, ${lim}_{t\to \infty }\left|y_{e_B}\right|=0$, and ${lim}_{t\to \infty }\left|z_{e_B}\right|=0$.

According to the above analysis, a virtual feedback variable is constructed:

$${\overline{x}}_{e_B}=x_{e_B}-k_{1}tanh\left(r\right)y_{e_B}$$

(17)

where $k_1$ is a positive constant and $tanh\left(\right)$ is a common nonlinear function. A specific expression of $tanh\left(\right)$ is expressed as follows:

$$tanh\left(x\right)=\frac{e^x-e^{-x}}{e^x+e^{-x}}$$

(18)

It can be found that $|tanh(x\left)\right|\le 1$, if and only if $x\to +\infty$ or $x\to -\infty$, $|tanh(x\left)\right|=1$. The derivative of $tanh\left(x\right)$ is:

$$\frac{d(tanh(x\left)\right)}{dx}=(1-{tanh}^2\left(x\right))$$

(19)

The virtual feedback variables corresponding to Equation (17) are analyzed below. The first-order derivative of $k_{1}tanh\left(r\right)$, with respect to intermediate control variable r, is continuous and bounded. If the external control makes ${lim}_{t\to \infty }{\overline{x}}_{e_B}=0$, then ${lim}_{t\to \infty }{x}_{e_B}=k_{1}tanh\left(r\right)y_{e_B}$. According to ${\dot{y}}_{e_B}=-r{x}_e+u_{ref}sin{\theta }_{e_B}$ in Equation (16), if ${lim}_{t\to \infty }{\theta }_{e_B}=0$, then ${lim}_{t\to \infty }{\dot{y}}_{e_B}={lim}_{t\to \infty }(-r{x}_e)={lim}_{t\to \infty }(-k_{1}rtanh\left(r\right)y_{e_B})$. Next, let us take part of the Lyapunov function $V_y={\displaystyle \frac{1}{2}}{y}_{e_B}^2$. It can be seen that $V_y$ is positive-definite.The derivative of the function over time t is ${\dot{V}}_y=y_{e_B}{\dot{y}}_{e_B}=-k_{1}rtanh\left(r\right)y_{e_B}^2$. The derivative of $V_y$ is positive-definite, and $li{m}_{t\to \infty }{y}_{e_B}=0$. This shows that $y_{e_B}$ is an indirectly controlled quantity and is dominated by $x_{e_B}$ and ${\theta }_{e_B}$ once again.

The candidate function of Lyapunov is chosen:

$$V_1=\frac{1}{2}{\overline{x}}_{e_B}^2+\frac{1}{2}{y}_{e_B}^2+\frac{2}{k_3}(1-cos\left(\frac{{\theta }_{e_B}}{2}\right))+\frac{1}{2}{z}_{e_B}^2$$

(20)

where $k_3$ is a positive constant. It is obvious that Equation (20) consists of four parts, and each of them is positive-definite. When ${\overline{x}}_e$, $y_{e_B}$, ${\theta }_{e_B}$, and $z_{e_B}$ are equal to 0, then $V_1=0$. The output angle range of IMU is $\theta \in (-\pi$, $\pi )$, the yaw angle range of the reference trajectory is ${\theta }_{ref}\in (-\pi$, $\pi )$, and the angle error range is ${\theta }_e\in (-2\pi$, $2\pi )$. The derivative of Equation (20) is written as follows:

$${\dot{V}}_1={\overline{x}}_{e_B}{\dot{\overline{x}}}_{e_B}+y_e\dot{y_e}+\frac{1}{k_3}sin(\left(\frac{{\theta }_{e_B}}{2}\right)\dot{{\theta }_{e_B}}+z_{e_B}{\dot{z}}_{e_B})$$

(21)

By introducing Equations (16) and (17) into Equation (21), we obtain:

$$\begin{array}{cc}\hfill {\dot{V}}_1& ={\overline{x}}_{e_B}[r{y}_{e_B}-u+u_{ref}cos{\theta }_{e_B}-k_1(1-{tanh}^2\left(r\right))\dot{(}r)y_{e_B}-k_{1}tanh\left(r\right)(-r{x}_{e_B}+u_{ref}sin{\theta }_{e_B})]\hfill \\ \hfill & +y_e[-r{x}_{e_B}+u_{ref}sin{\theta }_{e_B}]\hfill \\ \hfill & +\frac{1}{k_3}sin\left(\frac{{\theta }_{e_B}}{2}\right)(r_{ref}-r)\hfill \\ \hfill & +z_{e_B}(w_{ref}-w)\hfill \end{array}$$

(22)

If we assign $x_{e_B}={\overline{x}}_{e_B}+k_{1}tanh\left(r\right)y_{e_B}$ into Equation (22), then we obtain

$$\begin{array}{cc}\hfill {\dot{V}}_1& ={\overline{x}}_{e_B}[-u+u_{ref}cos{\theta }_{e_B}-k_1(1-{tanh}^2\left(r\right))\dot{(}r)y_{e_B}-k_{1}tanh\left(r\right)(-r{x}_e+u_{ref}sin{\theta }_{e_B})]\hfill \\ \hfill & +[-k_{1}rtanh\left(r\right)y_{e_B}^2]\hfill \\ \hfill & +\frac{1}{k_3}sin\left(\frac{{\theta }_{e_B}}{2}\right)[(r_{ref}-r)+2k_{3}cos\left(\frac{{\theta }_{e_B}}{2}\right)u_{ref}{y}_{e_B}]\hfill \\ \hfill & +z_{e_B}(w_{ref}-w)\hfill \end{array}$$

(23)

We assume that when $t\in [0$, $+\infty )$, $u_{ref}$, $r_{ref}$, $w_{ref}$, ${\dot{u}}_{ref}$, ${\dot{r}}_{ref}$, and ${\dot{w}}_{ref}$ of the reference trajectory are all bounded, and $u_{ref}$, $r_{ref}$, and $w_{ref}$ do not converge to 0 at the same time. Then, the controller law of the outer-loop backstepping controller is presented as follows:

$$\begin{array}{l}u=u_d=u_{ref}cos{\theta }_{e_B}-k_1(1-{tanh}^2\left(r_d\right)){\dot{r}}_d{y}_{e_B}-k_{1}tanh\left(r_d\right)(-r_d{x}_{e_B}+u_{ref}sin{\theta }_{e_B})\\ \mathrm{(24a)}& +k_2(x_{e_B}-k_{1}tanh\left(r_d\right)y_{e_B})\hfill \mathrm{(24b)}& r=r_d=r_{ref}+2k_{3}cos\left(\frac{{\theta }_{e_B}}{2}\right)u_{ref}{y}_{e_B}+k_{4}sin\left(\frac{{\theta }_{e_B}}{2}\right)\hfill \mathrm{(24c)}& w=w_d=w_{ref}+k_5{z}_{e_B}\hfill \end{array}$$

where $u_d$, $r_d$, and $w_d$ represent the desired values of the intermediate control quantities u, r, and w, respectively.

When the three intermediate control variables reach their corresponding expected values, Equation (25) can be obtained.

$${\dot{V}}_1=-k_2{\overline{x}}_{e_B}^2-k_{1}rtanh\left(r\right)y_{e_B}^2-\frac{k_4}{k_3}{sin}^2\left(\frac{{\theta }_{e_B}}{2}\right)-k_5{z}_{e_B}^2$$

(25)

where $k_1,k_2,k_3,k_4,$ and $k_5$ are positive scalar. If and only if ${\overline{x}}_{e_B}$, $y_{e_B}$, ${\theta }_{e_B}$, and $z_{e_B}$ are 0, ${\dot{V}}_1=0$, then ${\dot{V}}_1$ is negative-definite. ${\dot{r}}_d$ appears in $u_d$, and ${\dot{r}}_d$ is as follows:

$${\dot{r}}_d={\dot{r}}_{ref}+2k_3({\dot{y}}_{e_B}{u}_{ref}+y_{e_B}{\dot{u}}_{ref})cos\left(\frac{{\theta }_{e_B}}{2}\right)-k_3{u}_{ref}{y}_{e_B}sin\left(\frac{{\theta }_{e_B}}{2}\right){\dot{\theta }}_{e_B}+\frac{1}{2}{k}_{4}cos\left(\frac{{\theta }_{e_B}}{2}\right){\dot{\theta }}_{e_B}$$

(26)

where ${\dot{y}}_{e_B}$ and ${\dot{\theta }}_{e_B}$ appear in ${\dot{r}}_d$ and are computed according to Equations (27) and (28):

$${\dot{y}}_{e_B}=-[r_{ref}+2k_3{y}_{e_B}{u}_{ref}cos\left(\frac{{\theta }_{e_B}}{2}\right)+k_{4}sin\left(\frac{{\theta }_{e_B}}{2}\right)]x_{e_B}+u_{ref}sin\left({\theta }_{e_B}\right)$$

(27)

$${\dot{\theta }}_e=-2k_3{y}_e{\nu }_{ref}cos\left(\frac{{\theta }_e}{2}\right)-k_{4}sin\left(\frac{{\theta }_e}{2}\right)$$

(28)

The stability analysis of the outer-loop backstepping controller is carried out as follows. Due to the fact that $u_{ref}$, $r_{ref}$, $w_{ref}$, ${\dot{u}}_{ref}$, ${\dot{r}}_{ref}$, and ${\dot{w}}_{ref}$ of the reference trajectory are all bounded, and the fact that every part of Equation (25) is negative-definite, then ${\overline{x}}_{e_B}^2$, $rtanh\left(r\right)y_{e_B}^2$, ${sin}^2\left({\displaystyle \frac{{\theta }_{e_B}}{2}}\right)$, and $z_{e_B}^2$ all converge to 0. Further, according to ${lim}_{t\to \infty }rtanh\left(r\right)y_{e_B}^2=0$, ${lim}_{t\to \infty }{y}_{e_B}=0$. Due to ${lim}_{t\to \infty }{\overline{x}}_{e_B}^2=0$, ${lim}_{t\to \infty }{x}_{e_B}=k_{1}tanh\left(r\right)y_{e_B}$. Because preconditions $u_{ref}$ and $r_{ref}$ do not converge to 0 at the same time, r does not converge to 0 in the process of $t\to \infty$, so ${lim}_{t\to \infty }{y}_{e_B}=0$ leads to ${lim}_{t\to \infty }{x}_{e_B}=0$. According to ${lim}_{t\to \infty }{sin}^2\left({\displaystyle \frac{{\theta }_{e_B}}{2}}\right)=0$, ${lim}_{t\to \infty }{\theta }_{e_B}=0$. To sum up, ${lim}_{t\to \infty }\left(\right|x_{e_B}|+|y_{e_B}|+|{\theta }_{e_B}|+z_e)=0$, and the TASR accomplishes the trajectory tracking task.

3.2. The Inner Loop of the Backstepping Controller

According to the previous section, the main problem is making the TASR track the desired trajectory by adjusting the intermediate control variables u, $\nu$, and r to their corresponding desired values. However, these intermediate control quantities cannot be set directly. The TASR system is a typical nonlinear second-order system model. Generally speaking, the actual control quantities ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ should be adjusted to make $u\to u_d$, $r\to r_d$, and $w\to w_d$, and then the robot can track the reference trajectory.

Errors between the expected speed and the actual speed of the robot are constructed in Equation (29).

$$\left[\begin{array}{c}{u}_e\\ r_e\\ w_e\end{array}\right]=\left[\begin{array}{c}{u}_d-u\\ r_d-r\\ w_d-w\end{array}\right]$$

(29)

In order to make the error converge to zero, a candidate function of Lyapunov is chosen as follows:

$$V_2=V_1+\frac{1}{2}{u}_e^2+\frac{1}{2}{r}_e^2+\frac{1}{2}{w}_e^2$$

(30)

If $V_1$ is positive-definite and the last three terms are all positive-definite, then $V_2$ is positive-definite. By computing the derivation of Equation (30), the following equation is obtained:

$${\dot{V}}_2={\dot{V}}_1+u_e{\dot{u}}_e+r_e{\dot{r}}_e+w_e{\dot{w}}_e$$

(31)

Equation (29) is written as:

$$\left[\begin{array}{c}u\\ r\\ w\end{array}\right]=\left[\begin{array}{c}{u}_d-u_e\\ r_d-r_e\\ w_d-w_e\end{array}\right]$$

(32)

Substituting Equations (23), (24a)–(24c), and (32) into (31), the following Equation is obtained:

$$\begin{array}{cc}\hfill {\dot{V}}_2=& -k_2{\overline{x}}_{e_B}^2-k_{1}rtanh\left(r\right)y_{e_B}^2-\frac{k_4}{k_3}{sin}^2\left(\frac{{\theta }_{e_B}}{2}\right)-k_5{z}_{e_B}^2\hfill \\ \hfill & +u_e({\overline{x}}_{e_B}+{\dot{u}}_e)+r_e{(\frac{1}{k_3}sin\left(\frac{{\theta }_{e_B}}{2}\right)+\dot{(}r)}_e)+w_e(z_{e_B}+{\dot{w}}_e)\hfill \end{array}$$

(33)

Assuming that $t\in [0$, $+\infty )$, $u_{ref}$, $r_{ref}$, $w_{ref}$, ${\dot{u}}_{ref}$, ${\dot{r}}_{ref}$, ${\dot{w}}_{ref}$, ${\ddot{u}}_{ref}$, ${\ddot{r}}_{ref}$, and ${\ddot{w}}_{ref}$ of the reference trajectory are all continuous and bounded. So, we combine the dynamic model given by Equation (14) and obtain the following control law:

$$\begin{array}{}\mathrm{(34a)}& {\tau }_u={\tau }_{ud}=\frac{x_{e_B}-k_{1}tanh\left(r\right)y_{e_B}+{\dot{u}}_d+k_6(u_d-u)-a_{1}u-a_2{u}^2}{a_3}\mathrm{(34b)}& {\tau }_r={\tau }_{rd}=\frac{\frac{1}{k_3}sin\left(\frac{{\theta }_{e_B}}{2}\right)+{\dot{r}}_d+k_7(r_d-r)-b_{1}r-b_2{r}^2}{b_3}\mathrm{(34c)}& {\tau }_w={\tau }_{wd}=\frac{z_{e_B}+{\dot{w}}_d+k_8(w_d-w)-c_{1}w-c_2{w}^2}{c_3}\end{array}$$

For the stability analysis, the control law provided by Equations (34a)–(34c) is introduced into the derivative of the Lyapunov candidate function, i.e., Equation (33), and the following equation is obtained:

$${\dot{V}}_2=-k_2{\overline{x}}_{e_B}^2-k_{1}rtanh\left(r\right)y_{e_B}^2-\frac{k_4}{k_3}{sin}^2\left(\frac{{\theta }_{e_B}}{2}\right)-k_5{z}_{e_B}^2-k_6{u}_e^2-k_7{r}_e^2-k_8{w}_e^2$$

(35)

The first four terms constitute ${\dot{V}}_1$. According to the previous section, ${\dot{V}}_1$ is negative-definite. If $k_6$, $k_7$, and $k_8$ remain positive, then the last three terms are negative-definite. To sum up, ${\dot{V}}_2$ is negative-definite, which means that ${lim}_{t\to \infty }{u}_e=0$, ${lim}_{t\to \infty }{r}_e=0$, and ${lim}_{t\to \infty }{w}_e=0$. In other words, when $t\to \infty$, then $u\to u_d$,$r\to r_d$, $w\to w_d$, and finally $x_{e_B}\to 0$, $y_{e_B}\to 0$, ${\theta }_{e_B}\to 0$, $z_{e_B}\to 0$, and the TASR completes the task of tracking the reference trajectory.

3.3. Design of an Inner-Loop Backstepping Controller with TDs

It can be found that the control law described in Equations (34a)–(34c) contains differentiation terms $u_d,r_d,$ and $w_d$. As outputs of the outer-loop controller, $u_d,r_d,$ and $w_d$ are described in Equations (24a)–(24c), which also contain differentiation terms. If the differentiation of Equations (24a)–(24c) is taken directly as with the traditional backstepping technique, the problem of “explosion of terms” may result. In order to avoid this problem, TDs referring to the ADRC methods described in the literature [23] are adopted to track the differentiation of $u_d$ and $r_d$. The TDs are designed as follows:

$$\left\{\begin{array}{c}{\dot{u}}_1=u_2\hfill \\ {\dot{u}}_2=\mathrm{fhan}\left(u_1-u_d,u_2,R_0,h_0\right)\hfill \end{array}\right.$$

(36)

$$\left\{\begin{array}{c}{\dot{r}}_1=r_2\hfill \\ {\dot{r}}_2=\mathrm{fhan}\left(r_1-r_d,r_2,R_1,h_1\right)\hfill \end{array}\right.$$

(37)

where $u_1$ is the tracking signal of $u_d$ and $u_2$ is the tracking signal of the differentiation of $u_d$. The same goes for $r_1$ and $r_2$. The definition of the function fhan refers to the literature [23]. The discrete forms of the above equations are presented as Equations (38) and (39):

$$\left\{\begin{array}{c}fh=\mathrm{fhan}\left(u_1\left(k\right)-u_d\left(k\right),u_2\left(k\right),R_0,h_0\right)\hfill \\ u_1(k+1)=u_1\left(k\right)+h{u}_2\left(k\right)\hfill \\ u_2(k+1)=u_2\left(k\right)+hfh\hfill \end{array}\right.$$

(38)

$$\left\{\begin{array}{c}fh=\mathrm{fhan}\left(r_1\left(k\right)-r_d\left(k\right),r_2\left(k\right),R_1,h_1\right)\hfill \\ r_1(k+1)={\dot{r}}_1\left(k\right)+h{r}_2\left(k\right)\hfill \\ r_2(k+1)={\dot{r}}_2\left(k\right)+hfh\hfill \end{array}\right.$$

(39)

h represents the sampling time. For consistency of symbols, $u_2$ is marked as ${\widehat{\dot{u}}}_d$ and $r_2$ is marked as ${\widehat{\dot{r}}}_d$. Using the $u_2,r_2$ produced by the TDs, the controller can be written as:

$$\begin{array}{}\mathrm{(40a)}& {\tau }_u={\tau }_{ud}=\frac{x_{e_B}-k_{1}tanh\left(r\right)y_{e_B}+{\widehat{\dot{u}}}_d+k_6(u_d-u)-a_{1}u-a_2{u}^2}{a_3}\mathrm{(40b)}& {\tau }_r={\tau }_{rd}=\frac{\frac{1}{k_3}sin\left(\frac{{\theta }_{e_B}}{2}\right)+{\widehat{\dot{r}}}_d+k_7(r_d-r)-b_{1}r-b_2{r}^2}{b_3}\mathrm{(40c)}& {\tau }_w={\tau }_{wd}=\frac{z_{e_B}+{\dot{w}}_d+k_8(w_d-w)-c_{1}w-c_2{w}^2}{c_3}\end{array}$$

3.4. Design of an LESO for an Inner-Loop Backstepping Controller

We must consider that the TASR is affected by ocean currents, propeller thrust fluctuations, and other factors when it is performing operations in a lake or sea. In addition, different speeds make the hydrodynamic parameters $a_i$,$b_i$, and $c_i$, $(i=1,2,3)$ change to some extent (we assume that $a_3,b_3,$ and $c_3$ change only slightly). These can be considered lumped disturbances. The dynamic equation in the actual environment with disturbances can be written as follows:

$$\left[\begin{array}{c}\dot{u}\\ \dot{r}\\ \dot{w}\end{array}\right]=\left[\begin{array}{c}{a}_{1}u+a_2{u}^2+a_3{\tau }_u+{\Omega }_u\\ b_{1}r+b_2{r}^2+b_3{\tau }_r+{\Omega }_r\\ c_{1}w+c_2{w}^2+c_3{\tau }_w+{\Omega }_w\end{array}\right]$$

(41)

where, ${\Omega }_u$, ${\Omega }_r$, and ${\Omega }_w$ are the total disturbances in u, r, and w directions from the ocean current, propeller thrust fluctuation, and so on.

However, the velocity of the TASR is unmeasured directly. It is necessary to combine the kinematic and dynamic models together to form a second-order system to design an extended state observer.

A typical nonlinear second-order system with disturbances is expressed as follows:

$$\begin{array}{cc}\hfill & {\dot{x}}_1=x_2\hfill \\ \hfill & {\dot{x}}_2=f(·)+b\tau \hfill \\ \hfill & y=x_1\hfill \end{array}$$

(42)

where $f(t,x)$ is the lumped disturbance, including a nonlinear uncertain term and an external environment disturbance, b is the model parameter, and $\tau$ is the controller input. Taking $x_1=x_1$, $x_2=x_2$, $x_3=f(t,x)+\Omega$, the state-space equation is expressed as follows:

$$\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ {\dot{x}}_3\\ y\end{array}\right]=\left[\begin{array}{c}{x}_2\\ x_3+b\tau \\ h\\ x_1\end{array}\right]$$

(43)

where h is the derivative of $f(·)$ and $\left|h\right|$ is bounded. Referring to [24], the following observer can be constructed:

$$\left[\begin{array}{c}{\dot{z}}_1\\ {\dot{z}}_2\\ {\dot{z}}_3\end{array}\right]=\left[\begin{array}{c}{z}_2-{\beta }_{1}e\\ z_3-{\beta }_{2}e+b\tau \\ -{\beta }_{3}e\end{array}\right]$$

(44)

where $e=z_1-y$, and $z_1,z_2,$ and $z_3$ are the estimates of $x_1,x_2,$ and $x_3$. ${\beta }_1,{\beta }_2,$ and ${\beta }_3$ are gain parameters. According to the literature [25], gain parameters are adjusted based on a bandwidth ${\omega }_0$. Generally, ${\beta }_1=3{\omega }_0,{\beta }_2=3{\omega }_0^2,$ and ${\beta }_3={\omega }_0^3$.

According to the kinematic model in Equation (6) and the dynamic model with disturbances in Equation (41), it can be seen that the models for yaw and heave are easily transformed into the typical nonlinear second-order system expressed in Equation (42). Hence, for the heave and yaw directions, the estimates of lumped disturbances are presented as $z_{3w}$ and $z_{3r}$, and the estimates of velocity are marked as $z_{2w}$ and $z_{2r}$, respectively.

In contrast, for the surge direction, an expression as in Equation (42) cannot be directly employed. To solve this problem, a rotation inertia coordinate, as shown in Figure 9, is established.

In the frame $O_{RI}-X_{RI}{Y}_{RI}{Z}_{RI}$, the kinematic and dynamic models of the TASR with respect to surge direction are expressed in Equation (45). Note that Equation (45) is easily transformed into the typical nonlinear system expressed in Equation (42). According to Equation (44), the lumped disturbance and velocity for surge direction can be obtained and marked as $z_{3u}$ and $z_{2u}$, respectively.

$$\begin{array}{cc}\hfill & {\dot{x}}_{RI}=u\hfill \\ \hfill & \dot{u}=a_{1}u+a_2{u}^2+a_3{\tau }_u+{\Omega }_u\hfill \end{array}$$

(45)

Following from the above analysis, the control law of the double closed-loop backstepping controller with TDs and an LESO is concluded as follows:

$$\begin{array}{}\mathrm{(46a)}& {\tau }_u={\tau }_{ud}=\frac{x_{e_B}-k_{1}tanh\left(r\right)y_{e_B}+{\widehat{\dot{u}}}_d+k_6(u_d-z_{2u})-z_{3u}}{a_3}\mathrm{(46b)}& {\tau }_r={\tau }_{rd}=\frac{\frac{1}{k_3}sin\left(\frac{{\theta }_{e_B}}{2}\right)+{\widehat{\dot{r}}}_d+k_7(r_d-z_{2r})-z_{3r}}{b_3}\mathrm{(46c)}& {\tau }_w={\tau }_{wd}=\frac{z_{e_B}+{\dot{w}}_d+k_8(w_d-z_{2w})-z_{3w}}{c_3}\end{array}$$

4. Simulation Results

To verify the feasibility of the proposed control scheme, which we refer to as TDs-LESO-BSC below, we conduct some simulation analyses using the professional software MATLAB2015a on a system running Windows 7, equipped with an Intel core i7 CPU at 3.6 GHz and 8 GB of RAM. For the simulation, some parameters of the proposed controller need to be set. The model parameters are acquired by online recursive least-squares-based model identification [22]: $a_3=2.4, b_3=476.2,$ and $c_3=2.8$. The gains in Equation (46a)–(46c) are selected as follows: $k_1=1, k_2=2, k_3=1, k_4=5, k_5=2, k_6=4, k_7=10,$ and $k_8=2$. The parameters of the TDs in Equations (38) and (39) are set as follows: $h_0=0.08, R_0=10, h_1=0.06,$ $R_1=10,$ and $h=0.01$. The bandwidth of the LESO in Equation (44) for the directions of surge, yaw, and heave are set as $35,24,$ and 30, respectively. Meanwhile, the actuated forces in the surge and heave directions are bounded between −4 N and 4 N, and the actuated torque in the yaw direction is bounded between −0.5 Nm and 0.5 Nm. The sampling time is 0.02 s.

4.1. 3D Trajectory Tracking Performance

To verify the 3D trajectory tracking performance, the reference 3D trajectory with respect to time t is described by

$$\left\{\begin{array}{cc}\hfill x_{ref}\left(t\right)& =2cos(\frac{\pi }{15}t-\frac{\pi }{2})\hfill \\ \hfill y_{ref}\left(t\right)& =2sin(\frac{\pi }{15}t-\frac{\pi }{2})\hfill \\ \hfill z_{ref}\left(t\right)& =-0.1t\hfill \end{array}\right.$$

(47)

whose shape is a standard spiral line with radius of 2 m. The original state of the TASR is set as $x=0,y=-1.5,z=0,$ and $\theta =\frac{\pi }{4}$. In addition, the following disturbances in surge, yaw, and heave degrees are introduced to the TASR. Note that gains in the sinusoidal signal are related to different values of the positive constant $\beta$. Because the max torque of the TASR with respect to the yaw is $0.5 \mathrm{N}\mathrm{m}$, the disturbance introduced for the yaw is a little smaller than that for the other two directions.

$$\left\{\begin{array}{cc}\hfill dt{b}_u& =\beta sin\left(\frac{\pi }{75}t\right)\hfill \\ \hfill dt{b}_r& =0.1\beta sin\left(\frac{\pi }{75}t\right)\hfill \\ \hfill dt{b}_w& =\beta sin\left(\frac{\pi }{75}t\right)\hfill \end{array}\right.$$

(48)

With $\beta =1$, the curves of the tracking trajectory are shown in Figure 10. The proposed tracking controller successfully drives the TASR toward the desired trajectory, which verifies the stability and feasibility of the control method. The tracking errors of the TASR states are plotted in Figure 11. All errors converge almost to zero. The required control forces are shown in Figure 12. The control inputs in the surge and heave directions converge to a sinusoidal signal, which demonstrates the anti-interference ability of the TDs-LESO-BSC. On account of the fact that the disturbance in yaw is slight, the control input converges to a steady value.

Furthermore, the estimated performance of the LESO is evaluated in Figure 13. It can be seen that the estimates of velocity and lumped disturbances are consistent with the actual values after 10 s. Accurately observed lumped disturbances improve the anti-interference properties of the controller. Further, accurate estimates of velocity are able to solve the problem that miniature bio-underwater robots cannot perceive their velocity.

For a more intuitive perspective on the tracking accuracy of the TDs, the contrasts between the actual and estimated values are described in Figure 14. Derivations of the surge and yaw velocities can be tracked accurately. Moreover, the derivation tracking performance is evaluated by the RMS errors, where the RMS derivation tracking errors for $u_d$ and $r_d$ are 0.0015 m/s² and 0.00001 rad/s², respectively. Accurate tracking of the derivation for surge and yaw velocities improves the trajectory tracking effect. In the simulation, the running time of the controller is recorded. The running times with TDs and without TDs are 0.701 s and 0.924 s, respectively. The design that includes TDs improves the efficiency of the proposed controller.

4.2. Robust Test

In order to investigate the robustness of the TDs-LESO-BSC further, we simulate TASR tracking control under different conditions with respect to lumped disturbances, with values of $\beta$ varying in $\{1.0,3.0,5.0\}$. The RMS tracking errors under different lumped disturbance conditions are computed in

Table 2. RMS tracking errors with different disturbances.

	$\mathit{\beta }=1.0$	$\mathit{\beta }=2.0$	$\mathit{\beta }=5.0$
Surge RMS $e_x$ (m)	0.001	0.002	0.002
Sway RMS $e_y$ (m)	0.015	0.022	0.023
Heave RMS $e_z$ (m)	0.000	0.001	0.004
Yaw RMS $e_{\theta }$ (rad)	0.006	0.007	0.007

. It can be seen that tracking quality is insensible to enhanced disturbances. The simulation results demonstrate the robustness of the TDs-LESO-BSC. However, the insensitivity of the tracking quality only holds for limited interference. Once the control input bounds restrict the disturbance-resisting capabilities of the TASR, the tracking quality worsens, even though the disturbances can be estimated accurately.

4.3. Comparative Simulations

To reveal the superiority of the proposed control scheme, a simulation comparing it to a traditional backstepping controller (BSC) is conducted in this paper. The parameters of the BSC are consistent with the proposed controller. With $\beta =5.0$, the curves of the tracking trajectory are shown in Figure 15. It is found that the TASR driven by the TDs-LESO-BSC controller is able to track the desired trajectory accurately. However, on account of a serious disturbance, the trajectory based on BSC fails to converge to the desired trajectory. The tracking error of the TASR can be seen in Figure 16. The position and yaw angle errors converge to 0 when the TASR is driven by the TDs-LESO-BSC controller. However, the errors seriously fluctuate during the adjustment of the BSC controller. Further, to analyze the tracking error results quantitatively, the root mean square (RMS) error is employed to evaluate tracking accuracy. As can be seen from

Table 3. The RMS errors of the TASR in the spiral trajectory case.

	TDs-LESO-BSC	BSC
Surge RMS $e_x$ (m)	0.002	0.037
Surge RMS $e_y$ (m)	0.023	0.061
Surge RMS $e_z$ (m)	0.004	0.470
Surge RMS $e_{\theta }$ (rad)	0.007	0.009

, the RMS errors of the TDs-LESO-BSC are distinctly smaller than those of the BSC, which highlights the superior 3D tracking performance of TDs-LESO-BSC.

In order to further verify the superiority of the TDs-LESO-BSC, another comparison is investigated. In this case, the desired trajectory for a raster scan is described by Equation (49). The initial state of the robot is $[x,y,z,\theta ]=[0,-1.5,-25,\pi /4]$. The parameters for the controller are the same as in the above simulation scenarios. Disturbances as defined in Equation (48) with $\beta =5.0$ are exerted. The curves of the tracking trajectory are depicted in Figure 17. It is obvious that the TDs-LESO-BSC controller is able to resist the serious disturbances and drive the TASR to track the desired trajectory. In contrast, the trajectory based on the traditional BSC exhibits significant oscillation. Figure 18 shows the contrast of the state errors. The state errors based on TDs-LESO-BSC mostly converge to zero. The position errors based on the BSC are larger, especially in the X-axial and Z-axial directions. For a more intuitive observation, the RMS values for the state errors are listed in

Table 4. The RMS errors of the TASR in the raster scan trajectory case.

	TDs-LESO-BSC	BSC
Surge RMS $e_x$ (m)	0.010	0.148
Surge RMS $e_y$ (m)	0.035	0.068
Surge RMS $e_z$ (m)	0.291	4.541
Surge RMS $e_{\theta }$ (rad)	0.020	0.023

. It can be seen that the RMS errors in the X, Y, and Z axial directions are smaller for TDs-ESO-BSC than for the BSC. The above analysis verifies the anti-interference ability and accurate trajectory tracking ability of the proposed controller.

$$\left\{\begin{array}{cc}& =[0.5t,0,0.1t-30],0<t\le 60\mathrm{s}\hfill \\ \hfill [X_{ref}\left(t\right),y_{ref}\left(t\right),z_{ref}\left(t\right)]& =[30,0.5t-30,0.1t-30],60<t\le 80\mathrm{s}\hfill \\ \hfill [X_{ref}\left(t\right),y_{ref}\left(t\right),z_{ref}\left(t\right)]& =[70-0.5t,10,0.1t-30],80<t\le 140\mathrm{s}\hfill \\ \hfill [X_{ref}\left(t\right),y_{ref}\left(t\right),z_{ref}\left(t\right)]& =[0,0.5t-60,0.1t-30],140<t\le 160\mathrm{s}\hfill \\ \hfill [X_{ref}\left(t\right),y_{ref}\left(t\right),z_{ref}\left(t\right)]& =[0.5t-80,20,0.1t-30],160<t\le 220\mathrm{s}\hfill \end{array}\right.$$

(49)

4.4. Discussion

On the basis of the above comprehensive simulation analysis, the proposed double closed-loop backstepping controller with TDs and LESO is demonstrated to be a feasible and robust trajectory tracking control method for the TASR. In addition, a comparison of simulation times verifies that the TDs not only avoid the “explosion of term” problem but also improve computing efficiency. Further, the robust test and comparative simulations indicate that the proposed control scheme, enhanced by the LESO, has a satisfactory capability to resist disturbances and model uncertainties, thus ensuring accurate trajectory tracking performance. However, the algorithm cannot reject all environmental disturbances due to the limited thrust capabilities of the robot’s systems.

In addition to this, there are notable advantages to the proposed control scheme. The proposed LESO guarantees that the controller is able to achieve trajectory tracking by relying only on position and orientation information in an unmeasured velocity situation. This scheme provides an alternative method of trajectory tracking for miniature bio-inspired underwater robots that cannot implement a DVL due to limited space.

5. Conclusions and Future Work

In this paper, we implemented a TASR control scheme, consisting of a backstepping controller, TDs, and an LESO, to handle 3D trajectory tracking tasks while considering the challenges presented by a nonlinear system, unmeasured velocity, and model parameter uncertainties; we also considered ambient interference. The double closed-loop backstepping method is known for its ability to process nonlinear systems. However, the defects of the backstepping method are obvious. For example, with an increase in system order, there are too many unknown parameters and the problem of differential explosion. Therefore, we introduced a virtual feedback variable into the outer-loop backstepping control that decreased the confined adjusted parameters. Furthermore, to avoid the problem of “explosion of terms”, TDs were designed to track the derivatives of desired velocities. For the problems of unmeasured velocity, model parameter uncertainties, and ambient interference, an LESO was designed to reject lumped disturbances and control the TASR by relying only on position and orientation information. Theoretical analysis and simulation results verified the feasibility of the proposed control scheme.

In the future, this control scheme will be implemented in a TASR to verify the robustness of the controller in a real environment.