Hydrodynamic Characteristic-Based Adaptive Model Predictive Control for the Spherical Underwater Robot under Ocean Current Disturbance

Abstract

In the navigation of underwater robots, large ocean current disturbance often causes significant tracking errors. To better resist ocean current disturbance, the hydrodynamic characteristics of the spherical underwater robot are studied, and a model predictive control strategy based on adaptive model parameters is proposed, according to these characteristics. Firstly, the hydrodynamic characteristics of the robot under static water and constant flow disturbance were obtained and analyzed by the computational fluid dynamics method. Then, the dynamic models of the robot under different disturbances could be calculated from the data obtained, based on the least square method. Finally, an adaptive model predictive control (AMPC) strategy, with an ocean current observer, was designed, based on the dynamic models. When the current disturbance velocity was twice the robot velocity, the proposed strategy reduced the tracking error by 39% and 42% in X and Y directions, respectively. In addition, the hydrodynamic characteristics were verified by experiments.

1. Introduction

With the frequent and deep utilization of resources in the water environment (such as the ocean), various types of underwater robots have been developed, including square, spherical, turtle-shaped, torpedo-shaped, fish-shaped and other bionic robots [1,2,3,4]. Among them, the bionic turtle-shaped robot has the advantages of structural stability and adaptability to underwater exploration [5,6]. The turtle-shaped robots can advance in all directions without rotating the main body, which provides flexibility in turning, rising and sinking [7,8,9]. The bionic robots have a wide range of applications in the fields of marine exploration, marine mapping, cooperative operation, etc. [10,11,12].

After more than 50 years of development, autonomous underwater vehicles (AUVs) have evolved from model building to navigation, control and formation coordination. However, the development of AUVs is still not mature enough. There are still many challenging problems to be solved, including dynamic obstacle avoidance, intelligent formation, and robot control that considers the hydrodynamic effect of complex flow fields. The movement of underwater robots relies on interaction with water. As a result, underwater robots are greatly affected by the currents in the water and are, consequently, more difficult to control in practical applications than robots on roads. AUVs often work in complex flow fields, which makes the dynamic model of AUVs highly nonlinear and strongly coupled. For example, AUVs travel in lakes and oceans with flowing water. In these environments, complex hydrodynamic interactions occur, and the acquisition of data by sensors, and the control of underwater vehicles under unknown hydrodynamic forces, are very difficult and challenging [13,14].

At present, there are three methods to study the hydrodynamic characteristics of robots: test-based methods, predictive methods, and computational fluid dynamics (CFD) methods. The test-based methods provide experimental data for the establishment of vehicular dynamic models, by dragging the robot in a pool or wind tunnel. However, the test-based methods have high costs, due to the corresponding experimental facilities, and it is difficult to implement the methods in some complex environments. In addition, the safety of such methods cannot be guaranteed [15]. Predictive methods aim to obtain hydrodynamic coefficients through theoretical analysis, commonly based on the potential flow theory, such as panel methods and strip methods [16,17]. However, these methods are applicable to typical streamlined cylindrical AUVs, and the modeling error is often large for complex AUVs [18,19]. Computational fluid dynamics (CFD) methods have been widely used to simulate the flow fields, and can also be used in the calculation of dynamic models [20]. The methods use structured or unstructured mesh to discretize the computing domain [21]. CFD technology is economical, and the calculation results are accurate [22,23,24]. The CFD model used in this study adopts the immersed boundary-lattice Boltzmann method (IB-LBM) [25]. Compared with the traditional CFD method, based on finite volume or finite element, this method may easily simulate the movement process of robots in the real situation. Moreover, this method is based on the mesoscopic scale with simple theory, low computation cost and excellent parallelism.

In the process of the robot motion control and trajectory tracking, there are generally two kinds of ways to deal with ocean current disturbance. One is to deal with current disturbance by improving the robustness of the model-free control algorithm [26,27,28]. In addition, the control input can also be compensated by the observed disturbance force through the ocean current observer [29,30,31]. Zhang et al. proposed a dynamic tracking control strategy with fault-tolerant control, and verified that the AUV could realize stable trajectory tracking in a three-dimensional ocean current environment through simulations [32]. Zhou et al. designed a reduced-order extended state observer (RESO) for wave gliders to estimate and compensate for uncertain disturbance, and the performance of the proposed method was verified by simulations and sea trials [33]. The other way to deal with current disturbance is based on model-based control algorithms, such as the model predictive control (MPC) algorithm. The disturbances observed are added to the input of the prediction model (dynamic model) to offset the error caused by the ocean current [34,35]. Dong et al. studied the influence of ocean current disturbance integrated into the kinematic and dynamic model of UMV, and simulations were carried out [36]. However, these studies only considered the influence of disturbance on the input of the prediction model, and did not consider the parameter changes of the prediction model caused by ocean current disturbance [37,38]. These parameters are related to the hydrodynamic characteristics of the robot, and the correlation has been rarely mentioned and explored in previous studies. In early studies, these parameters were generally kept fixed. Some researchers first determined approximate dynamic model parameters in CFD simulation, and then further adjusted these parameters in underwater vehicle experiments [39]. This is a relatively effective method with little disturbance and low requirement for control accuracy. However, with the increasing demand for disturbance resistance, this method cannot meet the requirements of actual scenarios. Therefore, through filtering and machine learning methods, researchers have developed real-time online recognition methods for these parameters. For example, a dynamic model identification method for AUVs, based on factorized Extreme Learning Machine (FELM), has been proposed [40,41]. However, online recognition methods require high computing capability and real-time performance of sensors. This kind of method has a contradiction in terms of the short sampling period but high demand of online calculation. To obtain the changing laws of dynamic model parameters under different disturbances, the analysis and study of hydrodynamic characteristics is necessary. These laws can then be used to solve the uncertainty of model parameters and control stability under ocean current disturbance.

After exploring the hydrodynamic characteristics, the MPC method, based on the kinematic and dynamic model, was chosen. Compared with the traditional Proportion Integration Differentiation (PID) method, the MPC based on the model makes for higher precision of the control, and controllability is stronger in complex environments [42]. In addition, for underwater robot systems, with constraints and strong real-time performance, the MPC method is more suitable for adding constraints and rolling optimization, compared with Linear Quadratic Regulator Control (LQR) and other methods [43]. Therefore, the MPC is widely used in underwater vehicle control and vehicle formation control [44,45,46].

In this paper, the aim was to solve the problem of low control precision and unsteady motion of spherical underwater robots in the presence of constant ocean current disturbance. Firstly, the force and motion of the underwater robot were obtained through CFD simulations, based on the IB-LBM method. Then, the hydrodynamic characteristics of the robot were explored, and the dynamic model of the robot was established, based on the simulation data. With the information acquired, the variations of the dynamic model with disturbance were explored to predict the hydrodynamic coefficients in the dynamic model of the robot. Finally, the kinematic and dynamic models of the robot were used as the reference models of model predictive control (MPC). The adaptive model predictive control (AMPC) strategy, based on adaptive model parameters, and an ocean current observer, based on extended state observer (ESO), were designed to complete the anti-disturbance control of the robot. The correctness of the simulation data was verified by experimental data resulting from experiments in a pool.

The rest of this paper is organized as follows. Section 2 introduces the problem to be solved, the numerical method, and the improved adaptive model predictive control (AMPC). In Section 3, the hydrodynamic characteristics of the robot are analyzed. In Section 4, validation experiments are performed to ensure the correctness of numerical simulations. Based on the Gazebo platform, the tracking performance of the robot using the proposed control strategy is analyzed. Finally, conclusions are presented in Section 5.

2. Modeling and AMPC Strategy

2.1. Problem Description and Numerical Modeling

Amphibious spherical robots have attracted great attention because of their flexible turning and stability. Our study is based on our group’s amphibious spherical robot (ASR-V). Its overall appearance is shown in Figure 1a, and its physical object is shown in Figure 1b. The approximately hemispherical robot body rests on four free legs that may be used for walking on land. In addition, the propeller in each leg allows the robot to swim in all directions under water. This study mainly focused on the underwater movement of the robot. The radius of the ASR-V shell is 0.15 m, and the total weight of this robot is 6.7 kg. The robot is designed to be used in exploration of water environments, including shallow seas, lakes and rivers.

The kinematic and dynamic models of ASR-V are based on the inertial coordinate system and the robot reference coordinate system. The inertial coordinate system $O_E-X_E,Y_E,Z_E$ is fixed on the earth’s surface and describes the position and attitude of the robot, shown as $\mathsf{\eta }={\left[x,y,z,\varphi ,\theta ,\psi \right]}^T$. The robot reference coordinate system $O_b-x_b,y_b,z_b$ is fixed on the robot body, generally used to describe the robot’s velocity, and angular velocity, shown as $v={\left[u,v,w,p,q,r\right]}^T$. The origin $O_b$ was located in the center of the robot in this study. The linear transformation between the two coordinate systems is expressed as:

$$\dot{\mathsf{\eta }}=J\left(\mathsf{\eta }\right)v$$

(1)

This is also the kinematic model of the robot. The transformation matrix $J\left(\mathsf{\eta }\right)$ is expressed as:

$$J\left(\mathsf{\eta }\right)=\left[\begin{array}{cc}{\mathrm{J}}_1\left({\mathsf{\eta }}_{\mathbf{1}}\right)& {\mathbf{0}}_{\mathbf{3}\times \mathbf{3}}\\ {\mathbf{0}}_{\mathbf{3}\times \mathbf{3}}& {\mathrm{J}}_2\left({\mathsf{\eta }}_{\mathbf{2}}\right)\end{array}\right]$$

(2)

where, ${\mathrm{J}}_1\left({\mathsf{\eta }}_{\mathbf{1}}\right)$ represents the relationship of linear velocity vectors between two coordinate systems, and ${\mathsf{\eta }}_{\mathbf{1}}=\left[x,y,z\right]$ represents the position of the robot. ${\mathrm{J}}_2\left({\mathsf{\eta }}_{\mathbf{2}}\right)$ represents the relationship of angular velocity vectors between two coordinate systems, and ${\mathsf{\eta }}_{\mathbf{2}}=\left[\varphi ,\theta ,\psi \right]$ represents the Euler Angle of the robot.

$$J_{\mathbf{1}}\left({\mathsf{\eta }}_{\mathbf{1}}\right)=\left[\begin{array}{ccc}c\theta c\psi & s\theta s\varphi c\psi -c\varphi s\psi & s\varphi s\psi -c\varphi s\theta c\psi \\ c\theta s\psi & s\theta s\varphi s\psi +c\varphi c\psi & s\theta c\varphi s\psi -s\varphi c\psi \\ -s\theta & c\theta s\varphi & c\theta c\varphi \end{array}\right]$$

(3)

$$J_{\mathbf{2}}\left({\mathsf{\eta }}_{\mathbf{2}}\right)=\left[\begin{array}{ccc}1& t\theta s\varphi & t\theta c\varphi \\ 0& c\varphi & -s\varphi \\ 0& s\varphi /c\theta & c\varphi /c\theta \end{array}\right]$$

(4)

All parts of the robot used in this paper were rigid, and it was assumed that there was no collision and no elastic deformation on the robot in the movement process. Generally, the dynamic model of the spherical underwater robot is expressed as [47]:

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

(5)

$$M=M_{\mathit{R}\mathit{B}}+M_{\mathit{A}}=diag\left\{m,m,m,I_x,I_y,I_z\right\}-diag\left\{X_{\dot{u}},Y_{\dot{v}},Z_{\dot{w}},K_{\dot{p}},M_{\dot{q}},N_{\dot{r}}\right\}$$

(6)

$$\mathit{C}\left(v\right)=C_{\mathit{R}\mathit{B}}\left(v\right)+{\mathit{C}}_A\left(v\right)$$

(7)

$$D\left(v\right)=D_{\mathbf{1}}\left(v\right)+{\mathbf{D}}_{\mathbf{2}}\left(v\right)v$$

(8)

The forces and torques acting on the robot through propulsion equipment is expressed by the vectors $\mathsf{\tau }={\left[X,Y,Z,K,M,N\right]}^T$ and $g\left(\mathsf{\eta }\right)$ is the restorative force. For ASR, the center of gravity and the center of buoyancy almost coincide, and only the horizontal motion is considered, so the $g\left(\mathsf{\eta }\right)$ term can be ignored. $M_{\mathit{R}\mathit{B}}$ is the inertia matrix of the rigid body, and $M_{\mathit{A}}$ is the additional mass force matrix. $C_{\mathit{R}\mathit{B}}$ is the rigid body Coriolis-centripetal force matrix, and $C_{\mathit{A}}$ is the additional Coriolis-centripetal force matrix. As $C_{\mathit{R}\mathit{B}}$, $C_{\mathit{A}}$ and $M_{\mathit{A}}$ are relatively small, they were ignored in this study. $D\left(v\right)$ is the hydrodynamic damping of the ASR. The $D\left(v\right)$ and $M_{\mathit{R}\mathit{B}}$ were the main concerns in our study. The dynamic model is simplified into the following form:

$$\mathsf{\tau }=M_{\mathit{R}\mathit{B}}\dot{v}+\left(D_{\mathbf{1}}\left(v\right)+{\mathit{D}}_{\mathbf{2}}\left(v\right)v\right)v,$$

(9)

Hydrodynamic damping is coupled and highly nonlinear, especially for underwater robots with complex shapes. Therefore, the variation of hydrodynamic damping coefficients was analyzed under different flow disturbances.

Figure 2 shows the computational domain and grid structure of a simplified robot. The computational domain was 4 m in length, 2 m in width and 1 m in depth. The flow field was filled with a Newtonian fluid at room temperature (20 °C) with a density ${\rho }_f=1.0\times {10}^3\mathrm{kg}/{\mathrm{m}}^3$ and kinematic viscosity ${\nu }_f=1\times {10}^{-6}{\mathrm{m}}^2/\mathrm{s}$. The robot model, of the same size and structure as the one in the experiment, was initially stationary in water.

In this study, the fluid simulation was established through the lattice Boltzmann method (LBM), based on the mesoscopic simulation scale [25]. In addition, the immersed boundary method (IBM) was used to deal with moving boundaries, making complex boundary conditions easier to set and calculate [48]. The fluid interaction process described by the IB-LB Method is more efficient [49]. In addition, this method is easy to operate in parallel.

For LBM, the D3Q19 model was used in the flow field. The single relaxation time lattice Boltzmann equation (LBE) is:

$$f_i\left(x+c_i\Delta t,t+\Delta t\right)=f_i\left(x,t\right)-\frac{1}{\tau }\left[f_i\left(x,t\right)-f_i^{eq}\left(x,t\right)\right]+G_i\left(x,t\right)\Delta t$$

(10)

In the LBE, $f_i\left(x,t\right)$ is the density distribution function at position $x$ and time $t$. $c_i$ is the discrete velocity for the D3Q19 model, shown as Formula (11). $f_i^{eq}\left(x,t\right)$ represents the equilibrium distribution function shown in Formula (12). $G_i\left(x,t\right)$ is the body force term shown in Formula (13). $\Delta t$ is the time step, and $\tau$ is the non-dimensional relaxation time:

$$(c_i,\mathrm{i}=0,\dots ,18)=\left(\begin{array}{c}{c}_{ix}\\ c_{iy}\\ c_{iz}\end{array}\right)=c(\begin{array}{c}0\\ 0\\ 0\end{array} \begin{array}{c}1\\ 0\\ 0\end{array} \begin{array}{c}-1\\ 0\\ 0\end{array} \begin{array}{c}0\\ 1\\ 0\end{array} \begin{array}{c}0\\ -1\\ 0\end{array} \begin{array}{c}0\\ 0\\ 1\end{array} \begin{array}{c}0\\ 0\\ -1\end{array} \begin{array}{c}1\\ 1\\ 0\end{array} \begin{array}{c}-1\\ -1\\ 0\end{array} \begin{array}{c}1\\ -1\\ 0\end{array} \begin{array}{c}-1\\ 1\\ 0\end{array} \begin{array}{c}1\\ 0\\ 1\end{array} \begin{array}{c}-1\\ 0\\ -1\end{array} \begin{array}{c}1\\ 0\\ -1\end{array} \begin{array}{c}-1\\ 0\\ 1\end{array} \begin{array}{c}0\\ 1\\ 1\end{array} \begin{array}{c}0\\ -1\\ -1\end{array} \begin{array}{c}0\\ 1\\ -1\end{array} \begin{array}{c}0\\ -1\\ 1\end{array})$$

(11)

$$f_i^{eq}={\omega }_i\rho \left[1+\frac{{\mathit{c}}_i·\mathit{u}}{c_s^2}+\frac{{\left({\mathit{c}}_i·\mathit{u}\right)}^2}{2c_s^4}-\frac{{\mathit{u}}^2}{2c_s^2}\right],$$

(12)

$$G_i\left(\mathit{x},t\right)=\left(1-\frac{1}{2\tau }\right){\omega }_i\left[\frac{{\mathit{c}}_i-\mathit{u}}{c_s^2}+\frac{{\mathit{c}}_i·\mathit{u}}{c_s^4}{c}_i\right]·{\mathit{f}}_{\mathrm{p}},$$

(13)

In Formula (13), ${\omega }_i$ is the weight coefficient. For the D3Q19 model, ${\omega }_i$ are given by ${\omega }_0=1/3$, ${\omega }_{1-6}=1/18$ and ${\omega }_{7-18}=1/36$. ${\mathit{f}}_{\mathrm{p}}$ is the body force acting on the fluid. $c_s=\Delta x/\sqrt{3}\Delta t$ is the lattice sound speed. The relaxation time $\tau$ is related to the kinematic viscosity $\upsilon$, and it is represented as:

$$\tau =\frac{\upsilon }{c_s^2\Delta t}+0.5$$

(14)

The macroscopic fluid density, velocity and pressure are obtained by density distribution functions:

$$\rho ={\displaystyle \sum }_i{f}_i,$$

(15)

$$\mathit{u}=\frac{1}{\rho }\left({\displaystyle \sum }_i{\mathit{e}}_i{f}_i+0.5{\mathit{f}}_{\mathrm{p}}\Delta t\right),$$

(16)

$$P={\displaystyle \sum }_i\rho c_s^2.$$

(17)

The edges of the pool were set in accordance with the no slip boundary condition. The complex moving boundaries of the robot body were calculated by the direct force immersion boundary method (IBM). The Lagrangian force density ${\mathit{F}}_L\left(s,t\right)$ is determined by:

$${\mathit{F}}_{\mathit{L}}=\mathsf{\alpha }\frac{\mathit{U}-{\mathit{u}}_{IB}}{\Delta t},$$

(18)

where $\mathsf{\alpha }$ is a positive constant. $\mathit{U}$ is the desired velocity obtained by solving the structure dynamics, ${\mathit{u}}_{IB}$ is the unforced velocity calculated by:

$${\mathit{u}}_{IB}={\displaystyle \int }\mathit{u}\left(\mathbf{x},t\right)D\left[\mathit{x}-\mathit{X}\left(s,t\right)\right]dx.$$

(19)

The force of the Lagrange point is dispersed to the Euler point by the following formula:

$$\mathit{f}\left(x,t\right)=\underset{s}{\overset{}{{\displaystyle \int }}}{\mathit{F}}_L\left(s,t\right)D\left[\mathit{x}-\mathit{X}\left(s,t\right)\right]ds,$$

(20)

$D\left[\mathit{x}-\mathit{X}\left(s,t\right)\right]$ is a smoothed approximation of the Dirac delta function.

$$D\left(x\right)=\frac{1}{h^3}d\left(\frac{x-X}{h}\right)d\left(\frac{y-Y}{h}\right)d\left(\frac{z-Z}{h}\right),$$

(21)

$$d\left(r\right)=\{\begin{array}{c}\frac{1}{8}\left(3-2\left|r\right|+\sqrt{1+4\left|r\right|-4r^2}\right), \left|r\right|\le 1\\ \frac{1}{8}\left(5-2\left|r\right|-\sqrt{-7+12\left|r\right|-4r^2}\right), 1\le \left|r\right|\le 2\\ 0, 2\le \left|r\right|\end{array}.$$

(22)

The IB-LB Method could simulate the free movement of the robot in the pool model in real time, and obtain the overall force of the robot. In our simulation the grid spacing was $\mathsf{\Delta }x=\mathsf{\Delta }y=\mathsf{\Delta }z=0.1\mathrm{m}$ in the LBM.

2.2. AMPC with ESO

MPC is a common control algorithm based on the dynamic model. The control process may be optimized in a limited time horizon. In addition, the method can deal with the constraint problem well and make motion control more stable. However, this algorithm depends highly on the model precision. The forces generated by the ocean current in the actual environments are not negligible. At the same time, the dynamic model of the moving robot changes under disturbance, and the control strategy used in static water becomes invalid.

Based on the problem of the uncertain flow disturbance in the actual environment, an AMPC control strategy with an ocean current observer was designed. The ocean current observer was based on the extended state observer (ESO), derived from active disturbance rejection control (ADRC). The ocean current observer has two applications in control decisions. First, observed ocean current disturbance can be added to the controller input as disturbance compensation. Second, the observed ocean current disturbance can modulate the dynamic model parameters, which are the basis for the MPC decisions. The overall control strategy is shown in Figure 3.

The underwater spherical robot belongs to the second order system. The predictive model of MPC is a combination of robot kinematic model and dynamic model.

$$\dot{x}=\left[\begin{array}{c}\mathrm{J}\left(\mathsf{\eta }\right)v\\ {\mathit{M}}^{-1}\left(\mathit{\tau }-\mathit{D}\left(\mathit{v}\right)\right)\end{array}\right]=f\left(\mathit{x},\mathit{\tau }\right)$$

(23)

$\mathit{x}=\left[\mathit{\eta },\mathit{v}\right]$, and $\mathit{\eta }=\left[x,y, z\right]$ is the position information of the robot, $\mathit{v}=\left[u,w, r\right]$ is the speed information of the robot. Taylor expansion is performed at a reference point ${\mathit{x}}_{\mathit{d}}$ and is subtracted from Formula (23) to obtain the linear state-space model as follows:

$$\dot{\tilde{x}}={\mathit{A}}_{\mathit{t}}\left(F_{dis}\right)\tilde{\mathit{x}}+{\mathit{B}}_{\mathit{t}}\left(F_{dis}\right)\tilde{\mathit{\tau }}$$

(24)

where, $\tilde{\mathit{x}}=\left[x-x_d,y-y_d,z-z_d,u-u_d,w-w_d,r-r_d\right]$, and $\tilde{\mathit{\tau }}=\left[{\tau }_u-{\tau }_{ud},{\tau }_w-{\tau }_{wd},{\tau }_r-{\tau }_{rd}\right]$. ${\mathit{A}}_{\mathit{t}}\left(F_{dis}\right)$ and ${\mathit{B}}_{\mathit{t}}\left(F_{dis}\right)$ in state error model (24) are adaptive parameters of the improved MPC, and related to the flow disturbance observed by the extended state observer (ESO). The state-space model is discretized and expanded into the extended state error model:

$$\mathit{\xi }\left(k+1\right)=\mathit{A}\mathit{\xi }\left(k\right)+\mathit{B}\Delta \mathit{\tau }\left(k\right),$$

(25)

$$\mathit{\eta }\left(k\right)=\mathit{C}\mathit{\xi }\left(k\right),$$

(26)

$$\mathit{A}=\left[\begin{array}{cc}\mathit{I}+\mathit{T}{\mathit{A}}_{\mathit{t}}& \mathit{T}{\mathit{B}}_{\mathit{t}}\\ {\mathbf{0}}_{\mathit{m}\times \mathit{n}}& {\mathit{I}}_{\mathit{m}}\end{array}\right], B=\left[\begin{array}{c}\mathit{T}{\mathit{B}}_{\mathit{t}}\\ {\mathit{I}}_{\mathit{m}}\end{array}\right], \mathit{C}=\left[\begin{array}{cc}{\mathit{I}}_{\mathit{n}}& {\mathbf{0}}_{\mathit{n}\times \mathit{m}}\end{array}\right],$$

(27)

where, $\mathit{\xi }\left(k\right)=\left[\begin{array}{cc}\tilde{\mathit{x}}\left(k\right)& \tilde{\mathit{\tau }}\left(k\right)\end{array}\right]{}^{\prime }$, n is the dimension of the state vector and m is the dimension of the control quantity $\mathit{\tau }$. T is the sampling time. According to the mechanical and circuit structure characteristics of the robot, the constraints of control quantity and control increment are considered as Formulae (29) and (30). The model predictive control objective function in this study considered error and control increment. By substituting the predicted output vector, the quadratic optimization problem is as follows:

$$\begin{array}{c}min\\ \Delta \mathsf{\tau }\end{array}\begin{array}{c}J\left(k\right)=\left[\Delta \mathsf{\tau }{\left(k\right)}^T,\epsilon \right]\mathit{H}\left(k\right){\left[\Delta \mathsf{\tau }{\left(k\right)}^T,\epsilon \right]}^T+\mathit{G}\left(k\right){\left[\Delta \mathsf{\tau }{\left(k\right)}^T,\epsilon \right]}^T\end{array}$$

(28)

$$\mathrm{s}.\mathrm{t}. \Delta {\mathsf{\tau }}_{\mathit{m}\mathit{i}\mathit{n}}\le \Delta \mathsf{\tau }\left(k\right)\le \Delta {\mathsf{\tau }}_{\mathit{m}\mathit{a}\mathit{x}}$$

(29)

$${\mathsf{\tau }}_{\mathit{m}\mathit{i}\mathit{n}}\le S\Delta {\mathsf{\tau }}_{\mathit{k}}+{\mathbf{1}}_{\mathit{N}\mathit{c}}\mathit{\tau }\left(k-1\right)\le {\mathsf{\tau }}_{\mathit{m}\mathit{a}\mathit{x}}$$

(30)

$H\left(k\right)$ is Hessian matrix, which is the coefficient of quadratic optimization problems. The increment of the control input sequence is obtained by solving the above equation.

In 1995, Han Jingqing proposed the ESO, which can be used to observe uncertain disturbance [50]. The ESO was used as the ocean current observer in this paper. The state-space equation of the system after state expansion is:

$$\{\begin{array}{c}\dot{\mathit{\eta }}=J\left(\mathit{\eta }\right)\mathit{v}\\ \dot{\mathit{v}}=\mathsf{\Gamma }+{\mathit{M}}^{\mathbf{-}\mathbf{1}}\mathit{\tau }\\ \begin{array}{c}\mathsf{\Gamma }\dot{}=h\\ \widehat{\mathit{y}}=\mathit{\eta }\end{array}\end{array}$$

(31)

$h$ is the first derivative of the unknown disturbance $\mathsf{\Gamma }$. Formula (31) can be written as:

$$\{\begin{array}{c}{\dot{\mathit{X}}}_{\mathit{o}}={\mathit{A}}_{\mathit{o}}{\mathit{X}}_{\mathit{o}}+{\mathit{B}}_{\mathit{o}}\mathit{\tau }+{\mathit{E}}_{\mathit{o}}\mathit{h}\\ {\mathit{y}}_{\mathit{o}}={\mathit{C}}_{\mathit{o}}{\mathit{X}}_{\mathit{o}}\end{array}$$

(32)

where, $X_o=\left[\eta ,v,\mathsf{\Gamma }\right]$, the matrix $A_o$, $B_o$, $C_o$, $E_o$ is easily derived from Formula (31). An observer of lomborg form is constructed for the extended system:

$$\{\begin{array}{c}{\dot{\widehat{\mathit{X}}}}_{\mathit{o}}={\mathit{A}}_{\mathit{o}}{\widehat{\mathit{X}}}_{\mathit{o}}+{\mathit{B}}_{\mathit{o}}\mathit{\tau }+\mathit{L}\left({\mathit{y}}_{\mathit{o}}-{\widehat{\mathit{y}}}_{\mathit{o}}\right)\\ {\widehat{\mathit{y}}}_{\mathit{o}}={\mathit{C}}_{\mathit{o}}{\widehat{\mathit{X}}}_{\mathit{o}}\end{array}$$

(33)

$\mathit{L}$ is the state observer gain matrix. ${\dot{\widehat{\mathit{X}}}}_{\mathit{o}}$ is the state of the system observed and ${\widehat{\mathit{y}}}_{\mathit{o}}$ is the state observer output. Formula (32) can be subtracted from Formula (33) to obtain:

$${\dot{\mathit{X}}}_{\mathit{o}}-{\dot{\widehat{\mathit{X}}}}_{\mathit{o}}=\left(\mathit{A}-\mathit{L}\mathit{C}\right)\left({\mathit{X}}_{\mathit{o}}-{\widehat{\mathit{X}}}_{\mathit{o}}\right)+{\mathit{E}}_{\mathit{o}}\mathit{h}$$

(34)

L is set as $\mathit{L}=\left({\beta }_1,{\beta }_2,{\beta }_3\right)$. The characteristic polynomial of $A-LC$ is:

$$f\left(\lambda \right)={\lambda }^2+{\beta }_1{\lambda }^2+{\beta }_2\lambda +{\beta }_3$$

(35)

By designing the suitable state observer gain matrix L, the extended state observer converges and the disturbance Γ is observed. Then, the corresponding ocean current disturbance is:

$$F_{dis}=\mathsf{\Gamma }-M^{-1}\mathit{D}\left(\mathit{v}\right)\mathit{v}$$

(36)

The adaptive parameters $A_t\left(F_{dis}\right)$ of the MPC are then be further determined by flow disturbance $F_{dis}$.

3. Hydrodynamic Analysis of ASR Robot

The dynamic model of the ASR robot was simplified, based on the following assumptions [51]: (1) The model neglects linear and angular coupled terms. (2) The model assumes that the robot is head–tail and left–right symmetric. (3) Any damping terms greater than second order are neglected. In this section, the hydrodynamic characteristics of the robot are analyzed and discussed through the IB-LB method, focusing on the identification of the relevant hydrodynamic damping coefficients.

3.1. Hydrodynamic Characteristics

The surge motion is the most common mode of underwater movement and defined as the expected forward direction of the robot in this paper. Accordingly, the sway velocity is perpendicular to the surge velocity and conforms to the right-hand rule. The robot may have different deflection angles along the forward direction. The deflection angle of the robot in Figure 4a was set as 0 degrees. Based on this, the hydrodynamic simulations at different angles in expected forward direction without current disturbance were designed, as shown in Figure 4. Then, the curves of the relationship between the surge velocity and the force at different angles were obtained, as shown in Figure 5a.

In Figure 5a, the hydrodynamic damping force on the robot had an approximate quadratic relationship with the surge velocity at the same deflection angle. The damping forces on the robot were also slightly affected by different deflection angles. The detailed relationship between the damping force and the deflection angle is shown in Figure 5b. At the same surge velocity, the damping force increased as the robot deflection angle changed from 0 to 45 degrees. This law was related to the structure of the robot itself.

3.2. Coefficients of Dynamic Model under Different Flow Disturbances

The hydrodynamic damping coefficients of the surge motion were obtained by regression analysis (least squares method) of the computing results in Figure 5. Since the deflection angle of the robot was arbitrary in the process of movement, the average damping forces of 0 and 45 degrees were chosen for the calculation. Through model identification, the relationship between the hydrodynamic damping and the surge velocity without current disturbance could be obtained as follows:

$$F_{dis}=-0.0307u+28.2862u^2$$

(37)

It was assumed that the damping coefficients of the robot only contained the velocity and quadratic velocity terms. From the identification formula, the damping of surge motion was mainly linearly related to the quadratic velocity, but less to the first order of velocity.

On the basis of the above hydrodynamic simulations, we obtained the relationship between the damping force and the surge velocity of the robot when the velocity disturbance existed in the sway direction of the robot. The total velocity of the robot, which was relative to the water, were obtained by combining the surge velocity and the disturbance velocity. Then, the total damping force was decomposed to obtain the damping force received in the surge direction of the robot. In Figure 6a, the variation curves of the damping force with the surge velocity at different disturbance velocities (sway velocities) are shown. It can be seen that with increase of disturbance velocities, the damping force in the surge direction became larger, the curvature decreased, and the curve was closer to linear.

Figure 7 shows the influence of the disturbance velocity (sway velocity) on the damping force under different surge velocities. It could be concluded that the greater the surge velocity, the greater the impact of the disturbance velocity. In order to further analyze the influence of disturbance on the damping coefficients, the dynamic model and correlation coefficients were obtained by linear regression analysis. The coefficients of dynamic models under different disturbances are shown in Figure 8. The change of the damping coefficients was nonlinear. In addition, the damping force was more related to the first order of the surge velocity with increase of the disturbance velocity. For example, when the disturbance velocity was 0.3 m/s, the damping force could be expressed as:

$$F_{dis}=7.1857u+15.4976u^2$$

(38)

It could be confirmed by the above analysis that the dynamic model of a robot is difficult to predict without knowing the disturbance velocity, resulting in it being difficult for the control algorithm to be effective under large hydrodynamic disturbances. Therefore, the study of hydrodynamic damping coefficients under disturbance is of great significance to develop better resistance of underwater robots to disturbance and to ensure better movement of the robots in complex current environments.

4. Experimental Evaluation and Discussion

4.1. Validation of Numerical Model

In order to verify the correctness of the numerical simulation method used in this paper, the classical simulation, where water flows around a cylinder, was carried out in a three-dimensional environment. The cylinder diameter D was set as 20 times the lattice length. The flow diagrams near the cylinder under the Reynolds number Re = 40 and Re = 100 were obtained, as shown in Figure 9. Relevant verification parameters were calculated and compared with others’ experimental data, as shown in

Table 1. Comparison of related parameters in flow around cylinder.

Re	40		100
	Cd	Lw/D	Cd	Cl	St
Russell [52]	1.60	2.29	1.43	0.322	0.172
Linnick [53]	1.54	2.28	1.38	0.337	0.169
Hu [54]	1.66	2.55	1.48	0.367	0.166
Present	1.60	1.60	1.44	0.341	0.157

. The difference between the simulation results and others’ results was small, which proved the correctness of the simulation method.

Then, we designed a motion process in which the robot achieved uniform speed under fixed thrust for simulations and experiments, as shown in Figure 10. The pool was 2.5 m in length, 1.5 m in width and 0.5 m in depth. In the experiments, the total thrust was kept constant by fixing the speed difference between the front and rear propellers. The dynamometer and the robot were then connected by thin wires to measure the propulsion force of the robot in the water at the same propeller speed. The simulation environment was set according to the pool used in the experiment. In the simulations, the robot was driven by adding propulsion force to the robot body, and, then, the velocity of the robot was measured. The relationship between the velocity and the propulsive force was obtained.

Under the same conditions respectively, four sets of experiments and simulations were carried out to verify the matching of the simulation results with the pool experiments. The comparison between simulation and experimental results is shown in Figure 11. Figure 11a–d correspond to thrust states of 1.37 N, 1.91 N, 2.36 N, and 2.91 N, respectively.

The average velocity errors when the velocity was steady in the four experiments and simulations were 6%, 6%, 15%, and 7%. Through comparison, the rationality and accuracy of the simulation method were verified. The hydrodynamic characteristics of the robot and the dynamic models under disturbance were used in the design of an adaptive model predictive controller, based on the ocean current observer.

4.2. Adaptive Model Predictive Control (AMPC) under Flow Disturbance

According to the analysis in Section 3.2, the damping coefficient of the underwater robot dynamic model varied with different disturbance velocities. The disturbance velocities were observed by the extended state observer (ESO). Then, the AMPC method was carried out by adjusting the parameters of the dynamic model. The simulation scenario is shown in Figure 12. The robot moved underwater, propelled by propellers, with its four legs arranged in an X-shape. An Unmanned Aerial Vehicle (UAV) obtained the position and velocity of the underwater robot through visual recognition, and the data was fed back to the robot for control decisions. In order to verify the performance of the proposed AMPC strategy, three groups of experiments, based on the Gazebo platform, were designed. In each group of experiments, the results of the proposed AMPC strategy with ESO compensation were compared with traditional MPC, traditional MPC with ESO compensation, and AMPC without ESO compensation.

In the first group of experiments, the robot was designed to complete a rectangular trajectory tracking at a speed of 0.1 m/s, and the experimental results are shown in Figure 13 and Figure 14. Figure 13 shows the trajectories of the robot when the disturbance velocity was 0.1 m/s in both X and Y directions. That is, there was flow disturbance of 0.1 ×*√2 m/s in a 45 degrees counterclockwise direction from the X axis. The mean absolute errors of ten experiments are shown in Figure 14. AMPC reduced the mean value of motion error. However, the compensation based ESO introduced large jitter. The reason was that the Gazebo platform was close to the actual physical environment with some unknown unstable factors and disturbance, and recognition errors of UAV also resulted in the jitter of observation and compensation values. In the first group of experiments, the ocean current disturbance was small, and the disturbance force acting on the robot was 0.5486 N. In the second group of experiments, the disturbance velocity was increased to 0.2 m/s in both X and Y directions, with the robot velocity remaining unchanged. The experimental results are shown in Figure 15 and Figure 16.

Figure 15 shows the trajectory of the robot under different control strategies with larger disturbances. Obviously, when the disturbance was large, the traditional MPC could not track the trajectory well. Both ESO and the AMPC strategy could improve the robot’s ability to track the trajectory. In addition, the AMPC strategy combined with ESO, as proposed in this paper, had the best performance. To further illustrate the performance of the proposed method, Figure 16 shows the mean absolute tracking errors of ten experiments in the X and Y directions. Similarly, it could be seen that the AMPC might better reduce the control errors of the robot. After introducing the ESO, the error decreased obviously, but some fluctuations occurred. Overall, in the case of relatively large disturbance, the AMPC with ESO proposed in this paper could better resist disturbance.

In order to further demonstrate the performance of the proposed method, the results of the above two groups of experiments were analyzed quantitatively. The average errors of the whole process when the ocean current disturbance velocities were 0.1 m/s in both the X and Y directions are shown in

Table 2. The average errors of the whole process with 0.1 m/s disturbance velocities in both the X and Y directions.

Mean Error	Traditional MPC	MPC + ESO	MPC + Adaption	MPC + ESO + Adaption
X direction (m)	0.148	0.141	0.124	0.141
Y direction (m)	0.129	0.126	0.122	0.128

. Under this condition, the performance of AMPC without ESO compensation was best, which indicated the effectiveness of the adaptation of the parameters of the models. At the same time, when the ocean disturbance was small, the jitter of the observation value of the ESO made the compensation performance not very significant. ESO, AMPC and their combined algorithms reduced the mean absolute errors of the trajectory tracking by 4%, 16%, and 4% in the X direction, and by 2%, 5%, and 1% in the Y direction, respectively.

Similarly, the average errors of the whole process when the ocean current disturbance velocities were 0.2 m/s in both the X and Y directions are shown in

Table 3. The average errors of the whole process with 0.2 m/s disturbance velocities in both the X and Y directions.

Mean Error	Traditional MPC	MPC + ESO	MPC + Adaption	MPC + ESO + Adaption
X direction (m)	0.279	0.220	0.215	0.169
Y direction (m)	0.251	0.194	0.191	0.146

. With the increase of ocean disturbance velocity, both ESO and AMPC showed good performance against the ocean current disturbance. Under this condition, ESO, AMPC and their combined algorithms reduced the mean absolute errors of the trajectory tracking by 21%, 23%, and 39% in the X direction, and by 23%, 24%, and 42% in the Y direction, respectively.

To verify the effect of the ESO, the calculated observation and compensation values are shown in Figure 17. It can be seen that the observed values fluctuated greatly, and the compensation values obtained by a filter also fluctuated to a certain extent. However, the compensation values were close to the reference values and the errors were small. The errors between the compensation values and the reference values were 0.11 N and 0.16 N. respectively. in the two disturbance environments. The fluctuations of the state observer are unavoidable in a practical environment.

In order to further verify the ability of the proposed control strategy in resisting current disturbance, a third group of experiments were designed. The robot moved in a straight line undergoing a process of acceleration (0–20 s) and then constant velocity (20–50 s) in the X direction. The disturbance velocity was 0.2 m/s in the Y direction. As shown in Figure 18, the mean absolute tracking errors of the robot’s position in the ten experiments are presented. In the acceleration stage, the errors of all methods became larger. The method proposed in this paper had stronger tracking ability and better performance. The average errors of the whole process are shown in

Table 4. The average errors of the whole process with 0.2 m/s disturbance velocities in the Y direction.

Mean Error	Traditional MPC	MPC + ESO	MPC + Adaption	MPC + ESO + Adaption
X direction (m)	0.302	0.273	0.250	0.235
Y direction (m)	0.258	0.120	0.133	0.056

. The experimental results fully demonstrated the anti-disturbance ability of the method proposed in this paper.

5. Conclusions

The navigation of underwater robots is easily influenced by current disturbances and becomes uncontrollable. To solve this problem, the influence of current disturbance on hydrodynamic characteristics and the dynamic model of a robot was investigated, by means of a simplified scenario, in this paper. Then, according to the characteristics of the kinematic and dynamic model with disturbance, AMPC with ESO was proposed. The correctness and performance of this proposed method against disturbance was verified by experiments and simulations. What makes this paper different to most studies, is the fact that the dynamic characteristics of the robot under different current disturbances were applied to the control, and provided a way to resist current disturbance at a lower research cost. In addition, the hydrodynamic characteristics, and the methods of obtaining dynamic models under different disturbances, summarized in this paper may also be applied to robots of other shapes. The exploration of the hydrodynamic characteristics of robots provides reference for future studies of the interaction between robots and underwater currents. In the future, we will consider further exploring the hydrodynamic characteristics of the robot and applying these to control of the robot under time-varying current disturbances.