Path Following and Collision Avoidance of a Ribbon-Fin Propelled Underwater Biomimetic Vehicle-Manipulator System

Abstract

This paper addresses the problem of path following and dynamic obstacle avoidance for an underwater biomimetic vehicle-manipulator system (UBVMS). Firstly, the general kinematic and dynamic models of underwater vehicles are presented; then, a nonlinear model predictive control (NMPC) scheme is employed to track a reference path and collision avoidance simultaneously. Moreover, to minimize the tracking error and for a higher degree of robustness, improved extended state observers are used to estimate model uncertainties and disturbances to be fed into the NMPC prediction model. On top of this, the proposed method also considers the uncertainty of the state estimator, while combining a priori estimation of the Kalman filter to reasonably predict the position of dynamic obstacles during short periods. Finally, simulations and experimental results are carried out to assess the validity of the proposed method in case of disturbances and constraints.

1. Introduction

Over the past few decades, humans have explored and exploited a great number of marine resources; autonomous underwater vehicles (AUVs) have been playing an essential role in underwater operations for marine water quality monitoring as well as oil and gas exploration and other resource surveys [1,2]. With the increasing popularity of underwater operations in the marine environment and the more complex and diverse working environment, the human requirements for underwater robots have further increased, and biomimetic underwater vehicles (BUVs) have attracted a lot of attention from researchers. Through natural selection and continuous self-evolution, fish have evolved near-perfect body mechanisms and control modes of underwater motion. Compared with traditional propeller-equipped underwater robots, BUVs have more maneuverability and can be used to achieve fine attitude adjustment [3,4], especially for low-speed cruising in some particularly restricted areas. In addition, because of the undulating fin movements, they cause less noise and less pollution to the ocean [5].

For the past twenty years, research on the design and motion control of BUVs has received rapid development. Heriot-Watt University’s Sfakiotakis used the parallel bellows actuator as an early undulatory-fin device [6]. Thereafter, Delft University of Technology designed a biomimetic autonomous underwater vehicle with an undulating fin [7]. Niu et al. successfully generated a motion modality by investigating the use of the central pattern generator (CPG) approach for motion learning of an Anguilliform robot fish.

They applied it to a BUV [8]. Wang et al. proposed a backstepping (BP) technique for a bio-inspired robot with undulatory fins to achieve closed-loop depth and course control [9]. Most researchers, however, are more focused on undulatory fin control or some closed-loop path tracking, while BUVs in the actual operation process will be subject to external forces caused by surges, and other interferences and obstacle-avoidance problems are not well-solved.

In recent years, the research commonly has addressed the unknown disturbances of underwater vehicles by designing observers to estimate the unknown disturbances and then compensating for the estimated disturbances by using controllers, which include high-gain observers and an extended state observer (ESO) [10]. In [11], the researchers proposed a motion control system composed of a MIMO PID controller and a high-gain observer, which considers the presence of unmodeled plant dynamics of ROV, measurement errors, noise, and environmental disturbances. In [12], the team proposed a neural extended state observer (ESO) to address the system parameters’ uncertainty and environmental disturbances. In [13], they proposed a linear extended state observer (LESO) applied in the case of sideslip caused by wind, current, and other disturbances in the environment. In addition to the uncertain dynamics and disturbances, another noteworthy problem is the physical constraints for ribbon-fin-propelled underwater vehicles. In recent years, the model predictive control (MPC) algorithm has become an effective optimization method to handle constrained systems [14]. A novel robust MPC method was introduced in [15] to solve the roll constraint for following a straight path. In [16], the team proposed a path-following collision-avoidance MPC method that approximates obstacle shapes as convex polygons and combines the arising norm equality with the dual collision-avoidance inequality constraints.

Most of the above literature for model uncertainty and unknown disturbances in the case of underwater robots’ or surface vessels’ path tracking problems has put forward valid solutions, but few can take into account the obstacle-avoidance problem in the control algorithm, especially in BUVs. In the natural ocean environment, once there is a disturbance, and once deviated from the predetermined trajectory, the risk of collision will greatly increase; underwater robots must safely avoid a collision, and maintain tracking on the predetermined path, which is a tough challenge for the underdriven underwater robot with only surge force and yaw moment. In addition, the control input of an underwater biomimetic vehicle-manipulator system (UBVMS) that relies on fluctuating fins for propulsion is different from that of conventional AUVs and surface vessels, which makes the model uncertainty and the estimation of unknown external disturbances more complex.

In this paper, we propose a concept of integrating an underwater biomimetic vehicle manipulator system (UBVMS) path tracking problem with the multi-object/obstacle-avoidance problem based on the framework of MPC. From a practical perspective, the actual physical platform limitations are incorporated into the constraints of the optimization problem by using the complete nonlinear dynamics constraints of the UBVMS. In experimental tests, since the scan period of the sonar commonly used for underwater obstacle detection is much longer than the sampling period of the NMPC, the speed of obstacle motion is predicted a priori using the Kalman filter. In the case of predicted collisions, not only does it improve the safety of the UBVMS but it also deviates from the defined path as little as possible. To further improve the robustness of obstacle avoidance, the collision cost term in the objective function is shaped using the uncertainty of the state estimation observer. For the consideration of environmental disturbances and the uncertainty of the UBVMS, the total external disturbances, excluding the effects of surge force and yaw moment, are computed using the introduction of extended state observers (ESOs), and the total disturbance term is finally incorporated into the prediction term in NMPC. When the NMPC control cycle ends and the UBVMS reaches a new position, the following optimal input is recalculated based on the current state, and the desired trajectory. Thus, the NMPC algorithm with disturbance compensation, and with obstacle-avoidance constraints incorporated into the control, can be practically applied to the UBVMS even in realistic harsh natural marine environments. Finally, we validate the effectiveness of the algorithm through simulation and experiments with a physical prototype.

2. UBVMS Model

2.1. Design of the UBVMS

Bio-inspired long-fin propulsion is introduced into a UVMS (underwater vehicle manipulator system). Compared with the traditional propeller propulsion, it is smaller and can achieve smooth control of the body. The bionic fin propulsors provide excellent stability for underwater operations, and the propeller can provide sufficient power to ensure better maneuverability in the actual water with disturbances. As shown in Figure 1, the UBVMS has two symmetrical long-bionic-fin propulsors, four vertically oriented small propellers, and two flippers at the end of the tail to help improve maneuverability. The UBVMS head is equipped with a depth camera, altimeter, sonar, and many other sensors to help acquire more information about the underwater environment while conducting underwater operations. More details are described in [17]. The physical parameters of UBVMS are in Table 1.

Figure 1. UBVMS.

Table 1. Physical parameters of UBVMS.

Parameter	Value	Parameter	Value
Mass	55 kg	Buoyancy	54.3 kg
Manipulator mass	2.5 kg	Fin length	0.58 m
Length	0.95 m	Maximum frequency	20 Hz
Width	0.7 m	Maximum linear velocity	0.43 m
Height	0.6 m	Maximum steering speed	0.89 rad/s

2.2. General Kinematics and Dynamics of Underwater Vehicle

The UBVMS has good stability due to its large metacentric height, so it is reasonable to neglect the motion in pitch and roll [9]. With bionic fin propulsors only, the maximum speed of the UBVMS is approximately 0.4 m/s, and the nonlinear damping is negligible at low speeds in the laboratory pool. Based on that, the 3-DOF kinematic and dynamic model can be presented as follows [18]:

$$\dot{x}=\left[\begin{array}{c}J(\phi )v\\ M^{-1}(-C(\nu )v-Dv+\tau +{\tau }_d)\end{array}\right]=f(x,u)$$

(1)

where $x=[{\eta }^T v^T{]}^T\in {ℝ}^6$ is a state vector,$\eta =[x \mathrm{y} \phi {]}^T\in {ℝ}^3$, $x(m)$ and $y(m)$ are the positions, $\phi (rad)$ is the heading angle with respect to the earth-fixed frame, and $v={[u v r]}^T\in {ℝ}^3$ represents the velocities in surge $(m/s)$, sway $(m/s)$, and yaw $(rad/s)$, decomposed in the body-fixed frame. $\tau ={[f_u 0 f_r]}^T$, $f_u(N)$, and $f_r(N\cdot m)$ are the surge forces and yaw moment produced by the bionic fin propulsors. ${\tau }_d={[{\tau }_{du} {\tau }_{dv} {\tau }_{dr}]}^T$, ${\tau }_{du}(N)$, ${\tau }_{dv}(N)$, and ${\tau }_{dr}(N\cdot m)$ describe the external forces and moment acting on surge, sway, and yaw, respectively. Based on the previous reasonable assumptions, the system inertial mass matrix (invertible) $M$ and damping matrix $D$ are a diagonal matrix and Coriolis and centrifugal matrix $C(v)$, which are given by:

$$\begin{array}{l}M\stackrel{\Delta }{=}\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& 0\\ 0& 0& m_{33}\end{array}\right], D\stackrel{\Delta }{=}\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& 0\\ 0& 0& d_{33}\end{array}\right],\\ C\stackrel{\Delta }{=}\left[\begin{array}{ccc}0& 0& -m_{22}v\\ 0& 0& m_{11}u\\ m_{22}v& -m_{11}v& 0\end{array}\right]\end{array}$$

(2)

where $J(\phi )\in SO(3)$ is the rotation matrix from body-fixed velocities $\nu$ to earth-fixed frame velocities $\dot{\eta }$, and has the following structure:

$$J(\phi )=\left[\begin{array}{ccc}\cos (\phi )& -\sin (\phi )& 0\\ \sin (\phi )& \cos (\phi )& 0\\ 0& 0& 1\end{array}\right]$$

(3)

The assumptions in this paper are as follows:

1.

The UBVMS states $x$, $y$, and $\phi$ can be measured.

2.

The total unknowns ${\tau }_{du}$, ${\tau }_{dv}$, and ${\tau }_{dr}$ are bounded, namely $\left|{\tau }_{du}\right|\le {\tau }_{dum\mathrm{a}\mathrm{x}}, \left|{\tau }_{dr}\right|\le {\tau }_{drm\mathrm{a}\mathrm{x}}, \left|{\tau }_{dv}\right|\le {\tau }_{dvm\mathrm{a}\mathrm{x}}$ and $\left|{\dot{\tau }}_{du}\right|\le \left|{\dot{\tau }}_{dumax}\right|$, $\left|{\dot{\tau }}_{dr}\right|\le \left|{\dot{\tau }}_{drmax}\right|$, $\left|{\dot{\tau }}_{dv}\right|\le \left|{\dot{\tau }}_{dvmax}\right|$.

3. Problem Formulation

3.1. Nonlinear Model Predictive Control

NMPC is a numerical optimization-based control method to achieve the desired optimal performance by systematically ensuring that different constraints’ control is satisfied. The system cost function is constructed, which refers to the error between the state and the desired system state for the next sampling periods, combined with the physical constraints of the UBVMS; then, the cost function is minimized to obtain the optimal control sequence [19,20]. The first element of the control vector sequence is then transformed and fed into the propeller, repeating optimal control at the next sample.

Based on the previously defined system state vectors $x$ and control vectors $u$, we can obtain the following optimization problem and solve it online:

$$\begin{array}{l}\underset{x(t),u(t)}{\min }J(x,u)={\displaystyle {\int }_{t=0}^T\{J_x(x(t),x_{ref}(t))+J_u(\Delta u(t))+J_c(x(t))}\}dt\\ s.\mathrm{t}. \dot{x}=f(x,u),\left(\mathrm{t}\right)\in U, x(0)=x(t_0)\end{array}$$

(4)

where $f$ represents system continuous nonlinear equations, $J_x$ is a cost function of the error with the desired state $x_{ref}$ of the system, $J_u$ and $J_c$ control input and collision cost penalties, respectively, and $U$ is a set of feasible control vector inputs. Since the NMPC is to be used with a practical system, the thrust of the bionic fin propulsor is limited, and such hard constraint must be considered at every control cycle. The input control vector constraints are as follows:

$$\mathrm{U}=\left\{\mathrm{u}\in {ℝ}^3|\left[\begin{array}{c}\begin{array}{c}{f}_{u\min }\\ f_{v\min }\end{array}\\ f_{r\min }\end{array}\right]\le u\le \left[\begin{array}{c}\begin{array}{c}{f}_{u\max }\\ f_{v\max }\end{array}\\ f_{r\max }\end{array}\right]\right\}$$

(5)

It is worth noting here that the UBVMS is an underdriven underwater vehicle and, in fact, $f_{v\min }=f_{v\max }=0$. Now, for a detailed description of the cost function part in Equation (4), the first term of the cost function $J_x(x(t), x_{ref}(t))$ represents the state cost, which penalizes deviating from the desired state $x_{ref}$; the details are as follows:

$$J_x(x(t),x_{ref}(t))=\left|\right|x(t)-x_{ref}(t)|{|}_{Q_x}^2$$

(6)

where $Q_x\in {ℝ}^{6\times 6}$ represents positive definite weight matrices. The second term of the cost function $J_u(\Delta u(t))$ is to ensure the smoothness of the system control vector throughout the control process $u$, related to the previous control action; the details are as follows:

$$J_u(\Delta u(t))= \left|\right| u(t+T)-u(t){ \left|\right|}_{Q_u}^2$$

(7)

The last term of the cost function is to expect the UBVMS to have a sufficiently safe distance from dynamic obstacles when working underwater, and we introduce a collision cost term similar to the logistic function, which ensures that the cost function is continuously differentiable. Formally,

$$J_c(x(t))={\displaystyle \sum _{i=1}^n\frac{Q_{c,i}}{1+\exp k_i(d_i(t)-r_{th,i}(t))}}$$

(8)

where $Q_{c,i}$ is a design weight, $d_i(t)$ is the Euclidean distance from the $i-th$ dynamic obstacle, and $k_i>0$ is a positive constant that denotes the smoothness of the obstacle avoidance trajectory. As the value of $k_i$ increases, the cost function becomes very sharp, which can make the optimization problem more challenging to solve, so the value of $k_i$ can be chosen to be adjusted according to different environmental situations [21]. The global reference path is provided by the global planner; we use a cubic spline to interpolate, calculate the reference yaw and divide the whole reference trajectory into segments of the same size; a sequence of path segments connecting path points $M$ forms a global reference path $\mathcal{l}$. The UBVMS chooses the closest point as the tracking target, as shown in Figure 2.

Figure 2. The global guidance principle.

3.2. Uncertainty Consideration and Disturbance Estimation

For the design of a UBVMS control system, system uncertainty is a fundamental issue that cannot be avoided, coming from the internal (structure or parameters) elements of the system on the one hand and the external (environmental disturbances) elements on the other [22]. NMPC is a model-based controller, and it is essential to improve the model’s accuracy as much as possible, so before applying it to practical objects, we need to consider the impact on the control quality due to the model’s internal uncertainty and external environmental disturbances. In addition, NMPC is also based on the model to make future state predictions. Then, in the optimization based on the prediction, if the disturbance is not compensated for in time, the control effect will become terrible, and even the system will become unstable. We add disturbance terms to the prediction model of NMPC, such as ${\tau }_{du}$, ${\tau }_{dr}$ and ${\tau }_{dv}$ shown in Equation (1). The disturbance observer will compute the disturbance in each control cycle. When operating in a complex marine environment, the operational performance of marine vehicles is susceptible to external environmental disturbances. The disturbance observers are widely applied in various marine applications and have a practical significance [23]. In [22], the estimation ability of ESO for uncertainty is discussed and analyzed, and the researchers show that ESO can deal with a large number of disturbances. ESO is the core of ADRC proposed by [24], and then Gao proposed an improved linear ADRC control algorithm, LADRC, based on the classical ADRC idea [25]. In [26], LESO is introduced to estimate the uncertain parameters and external disturbances, and the surge velocity is calculated.

In our work, ESO is used to estimate the velocity and external disturbances in the motion coordinate system, and the nonlinear function in ESO is modified by introducing a hyperbolic tangent function to replace the original nonlinear function, which is a bounded singular function with more adjustable parameters than the nonlinear function and can be used to flexibly adjust the limits of the error magnitude as follows:

$$\beta \tanh (\lambda z)=\frac{e^{\lambda z}-e^{-\lambda z}}{e^{\lambda z}+e^{-\lambda z}}$$

(9)

$\lambda$ and $\beta$ are positive parameters that can be used to adjust the limit range of the amplitude and the rate of change in the amplitude, respectively. According to the UBVMS dynamic model (see Equation (1)), the disturbance moment observer of the external environment in the yaw direction can be:

$$\left\{\begin{array}{l}\dot{\hat{\phi }}=\hat{r}-l_0\tanh ({\lambda }_1(\hat{\phi }-\phi ))\hfill \\ \dot{\hat{r}}={\hat{\tau }}_{dr}-l_1\tanh ({\lambda }_1(\hat{\phi }-\phi ))+\frac{(m_{11}-m_{22})}{m_{33}}uv-\frac{d_{33}}{m_{33}}r+b_0\frac{f_d}{m_{33}} \hfill \\ {\dot{\hat{\tau }}}_{dr}=-l_2\tanh ({\lambda }_1(\hat{\phi }-\phi ))\hfill \end{array}\right.$$

(10)

where $l_0$, $l_1$, and $l_2$ are positive parameters, which could adjust the feedback range of estimation errors. Increasing their values within a certain range can improve the estimation accuracy, but it also brings oscillations as a result [27]. $\hat{r}$, ${\hat{\tau }}_{dr}$, and $\hat{\phi }$ represent the respective estimated values, and $b_0$ is the compensation factor.

$$\left\{\begin{array}{l}\dot{\hat{\zeta }}=\hat{u}-l_3\tanh ({\lambda }_2(\hat{\zeta }-\zeta ))\hfill \\ \dot{\hat{u}}={\hat{\tau }}_{du}-l_4\tanh ({\lambda }_2(\hat{\zeta }-\zeta ))+\frac{m_{22}}{m_{11}}vr-\frac{d_{11}}{m_{11}}u+b_1\frac{f_u}{m_{11}} \hfill \\ {\dot{\hat{\tau }}}_{du}=-l_5\tanh ({\lambda }_2(\hat{\zeta }-\zeta ))\hfill \end{array}\right.$$

(11)

$$\left\{\begin{array}{l}\dot{\hat{\chi }}=\hat{v}-l_6\tanh ({\lambda }_3(\hat{\chi }-\chi ))\hfill \\ \dot{\hat{v}}=\widehat{d_v}-l_7\tanh ({\lambda }_3(\hat{\chi }-\chi ))-\frac{m_{11}}{m_{22}}ur-\frac{d_{22}}{m_{22}}v\hfill \\ {\dot{\hat{\tau }}}_{dv}=-l_8\tanh ({\lambda }_3(\hat{\chi }-\chi ))\hfill \end{array}\right.$$

(12)

$\zeta =u\cdot t$ and $\chi =v\cdot t$ denote the displacement of the surge velocity and sway velocity in the body-fixed frame, respectively, where $l_3$,…, $l_8$ are positive parameters, $\hat{u}, {\hat{\tau }}_{du}, \hat{\zeta }$, $\hat{v}$, ${\hat{\tau }}_{dv}$, and $\hat{\chi }$ represent the respective estimated values, and $b_1$ is the compensation factor, $u^{\ast }$ is the optimal control input for the solution. Notably, the estimates for the disturbance terms are added to the prediction module of the controller NMPC and updated by ESOs at each sampling cycle. There is a more intuitive representation in Figure 3 and Algorithm 1.

Algorithm 1: NMPC for UBVMS Algorithm

1: Initialize system parameters;

2: Repeat;

3:     Measure and compute estimated velocities, along with system state $x(t_0)$ disturbance term updates from ESOs;

4:    Compute future predicted states $\hat{x}(t+n{T}_s),n=1,\dots ,N$ and predicted obstacle states (17);

5:     Solve the OCP (4) and obtain the desired optimal solution sequence $u^{\ast }(t+n{T}_s),n=1,\dots ,N-1$;

6:    Allocate the desired optimal solution $u_0^{\ast }$ and convert to bionic fin propulsor;

7:    If sonar scan completed, then

8:          Kalman filter update: compute $r_{th,k}(t)$ and update $P_t^{-}$, $P_t$;

9:     Else $P_t^{-}=P_t^{-}$, $P_{\mathrm{t}}=P_{\mathrm{t}}$;

10:    End if

11:   Compute the desired reference path and save system data;

12:   Update: $nmpciter=nmpciter+1$;

13: Until stop.

Figure 3. Diagram of the proposed control scheme.

3.3. Robust Collision Avoidance

In the previous section, we discussed the uncertainty in the UBVMS; in order to further improve the robustness of control in underwater environments, the uncertainty of state estimation of dynamic obstacles in water is considered, inspired by the literature [28]. With non-negligible uncertainty in predicting obstacle states due to the inaccuracy of the prediction model and the interference of external environmental measurement noise, we should be more conservative and allow the system to approach the obstacle by increasing $r_{th,i}(t)$ along the prediction horizon. In the natural marine underwater environment, dynamic obstacles are hardly ever in actual uniform motion, so to simulate the trajectory of dynamic obstacles closer to the real situation, we chose the uniform motion mode with Gaussian noise interference as follows:

$$p_i^{obs}(t)=p_i^{obs}(t_0)+(v_i(t_0)+w_0)(t-t_0)$$

(13)

where $w_0$ is a random number $w_0(t) ~N(0,Q_0(t))$, and $v_i(t_0)$ and $p_i^{obs}(t_0)$ are the velocity and position at the current moment, respectively. To obtain an accurate estimation of the obstacle state and compute the uncertainty in the state estimator of objects in the process, for the proposed obstacle motion model (12), we choose to perform uncertainty propagation based on a Kalman filter (KF) [28]. The KF can be formulated as:

Predict:

$$\left\{\begin{array}{l}{\hat{x}}_t^{-}=F{\hat{x}}_{t-1}\hfill \\ P_t^{-}=F{P}_{t-1}{F}^T+Q_t\hfill \end{array}\right.$$

(14)

Update:

$$\left\{\begin{array}{l}{K}_t=P_t^{-}{H}^T{(H{P}_{t-1}{H}^T+R_t)}^{-1}\hfill \\ {\hat{x}}_t={\hat{x}}_t^{-}+K_t(Z_t-H{\hat{x}}_t^{-}) \hfill \\ P_t=(I-K_{t}H) P_t^{-} \hfill \end{array}\right.$$

(15)

where $x_t={[x^{obs},y^{obs},v^{obs}]}^{\mathrm{T}}$ is a state vector, $P_t^{-}$ and $P_t$ represent the a priori (predicted) and a posteriori state vectors, respectively, and $Z_t={[x^o,y^o,v^o]}^T$ is the measurement vector acquired by employing sonar at moment t. $F$ is the state transition Jacobian matrix, and $H$ is the observation matrix. $Q_t$ is the covariance matrix of the motion obstacle system noise, and $R_t$ is the covariance matrix of the observation noise, and we must note that all of these are positive parameters. The reasonable selection of $Q_t$ and $R_t$ is crucial for the development of the Kalman filter [29]. We express the uncertainty of the $i-th$ dynamic obstacle at moment t by solving the equation $P_t^{-}=F{P}_{t-1}{F}^T+Q_t$ with maximum eigenvalue ${\delta }_k(t)$. $r_{th,i}(t)$ can be calculated using the following equation:

$$r_{th,i}(t)=r_{th}+10{\delta }_i(t)$$

(16)

where $r_{th}$ represents constant parameters, and the use of an enclosing sphere to approximate the uncertainty ellipsoid makes the bound more conservative [28]. Compared to the travel space of a mobile robot on land, the moveable space of a UBVMS is ample, and it is wiser to convert the complex elliptical distance calculation to a circular distance calculation in terms of computational complexity. In addition, since usually the sonar scanning time to ascertain the obstacle position is relatively long, the time to obtain the updated obstacle position is much longer than the NMPC sampling period. Since we cannot determine the latest state of the obstacle movement in every control cycle, to carry out a safer motion control, reasonable use of the Kalman filter a priori information for obstacle position prediction is critical. The obstacle trajectories are predicted as follows:

$$p_{obs i}^{pre}(t_0+n\cdot T_s)=p_{obs }{}_i(t_0)+n\cdot T_{\mathrm{s}}\cdot v^{o-}(t_0) , \mathrm{n}\in (1,\dots ,N)$$

(17)

where $P_{obs i}(t_0)$ denotes the obstacle position at moment $t_0$, $T_s$ is the NMPC sampling period, and $v{(t_0)}^{o-}$ denotes the $i-th$ obstacle prior velocity obtained by Kalman filtering at moment $t_0$. In the NMPC prediction module, along with the number of prediction steps $n$ from 1 to $N$, the obstacle prediction position is also updated until the Kalman filter is updated for the prior velocity. The proposed NMPC algorithm is briefly described in Algorithm 1.

The control structure of the proposed system is schematically shown in Figure 3. CasADi is an open-source software framework for numerical optimization that is well suited to nonlinear optimization problems, especially those constrained by differential equations [30,31]. In the simulation and experiment part of this paper, CasADi solver plugin ipopt is selected to solve the optimal control problem.

4. Simulations and Experimental Results

4.1. Simulation Results

The physical parameters for the UBVMS are chosen as [9]: $m_{11}=57.5$, $m_{22}=61.3$, $m_{33}=1.15$, $d_{11}=53$, $d_{22}=58$, and $d_{33}=3.1$. The initial value of the system state vector is 0. The NMPC-related control parameters are as follows: ${\mathrm{T}}_{\mathrm{s}}=0.2 s$, $N_p=25$, $N_c=1$, and weighting matrices $Q_x=diag\{55.0, 55.0, 5.0, 1.0, 800, 5.0\}$ and $Q_u= \mathrm{diag}\{10.0, 25.0\}$. The collision penalty weights and prediction penalty weights are $Q_{\mathrm{c}}=20$, $Q_{cp}=10$. The desired surge velocity in the body-fixed frame is $0.2 m/s$, and $l_1=3.6$, $l_2=4.32$, $l_3=1.728$, $l_4=2.4$, $l_5=1.92$, $l_6=1.516$, $l_7=2.4$, $l_8=1.92$, and $l_9=0.528$. The external disturbance is ${\tau }_{du}=0.5\sin (0.75\times 0.1t)$, ${\tau }_{dv}=0.4\sin (0.75\times 0.1t)$, ${\tau }_{dr}=0.3\sin (0.75\times 0.12t)$.

Dynamic obstacles are initially positioned as follows: $(x_{t_0}^{obs},y_{t_0}^{obs})=(6,3)$. The radius of the UBVMS in the simulation is 0.3 m, and the radius of dynamic obstacle expansion is 0.5 m. The initial velocity of the obstacle is −0.15 m/s, as well as adding to the Gaussian distribution $N(0, 0.02)$. In the multi-obstacle simulation, the initial positions of the three dynamic obstacles are as follows: (5.4, 4.0), (4.15, 1.0), (5.0, 6.0). To better verify the effectiveness of the proposed method, the three dynamic obstacles make a linear motion with initial velocity −0.15 m/s and an additional Gaussian distribution $N(0, 0.02)$ at the approach of the UBVMS.

Figure 4 shows the more critical moments of the BUMVS in the path tracking process. The blue curve is the desired path, the small red circle is the UBVMS, the large one is the dynamic obstacle, and the marker symbol above the red circle indicates the yaw angle at this moment. The yellow circle indicates the posterior position based on the Kalman filter. In Figure 4a, it can be seen that because there is an obstacle in front of the right of the UBVMS at this moment, it chooses to deviate from the intended trajectory to the left to avoid a collision; notably, in Figure 4b, when the obstacle trajectory is predicted by combining a priori messages in the KF, the controller chooses to drive to the right with lower probability of collision. Figure 4c,d show that the UBVMS returns to the original desired path soon after avoiding this collision.

Figure 4. Process snapshots. (a) t = 35.02 s. (b) t = 37.20 s. (c) t = 46.00 s. (d) t = 52.6 s.

In Figure 5, note that there is always obvious error in path tracking without the ESO disturbance observer, while the NMPC with disturbance observation compensation can achieve error-free tracking. In addition, it can be seen that when the UBVMS has access to the predicted future trajectory of the obstacle, it chooses the opposite direction of motion to the obstacle (positive x-axis direction) to avoid collisions. The surge, sway, and yaw velocities are presented in Figure 6a–c, respectively. Here, it is interesting to note that the surge velocity quickly reaches the desired speed of 0.2 m/s after avoiding the obstacle, and sway velocity is also maintained at approximately 0 m/s. Figure 6d–f show the ability of ESOs to estimate time-varying disturbances in the external environment; although the input control vector forces and moments cause some oscillations during sharp changes, the disturbances can still be approximately calculated in a short time. Figure 7a and 7b illustrate the surge thrust and yaw moment computed by the CasADi solver, respectively. When constraining the variation in the control vector $\Delta u$ in the objective function, the control vector is smoother, and the effect is obvious for the yaw moment.

Figure 5. The curve path tracking.

Figure 6. Estimation of velocity and disturbance. (a) Surge velocity. (b) Sway velocity. (c) Yaw velocity. (d) ${\hat{d}}_u(t)$. (e) ${\hat{d}}_r(t)$. (f) ${\hat{d}}_v(t)$.

Figure 7. Surge force and yaw moment. (a) Surge force. (b) Yaw moment.

Figure 8 shows the results of the Kalman filter estimating and predicting the velocity of an obstacle with a non-uniform motion. In the same scenario as Figure 4 above, we add multiple dynamic obstacles near the predetermined desired trajectory of the UBVMS and, for each added obstacle, we carry out ten consecutive simulation experiments. The results of the scenario experiments are shown in Table 2.

Figure 8. Obstacle velocity produced by KF estimator.

Table 2. Multi-obstacle simulation results.

Number of Obstacles	Without Priori (Predicted)			With Priori (Predicted)
	Clearance Mean (m)	Traveled Distance Mean (m)	%Failures (Collisions)	Clearance Mean (m)	Traveled Distance Mean (m)	%Failures (Collisions)
1	0.0487	11.6114	60%	0.3100	11.4018	100%
2	0.0172	10.7272	20%	0.3245	10.8660	90%
3	0.0574	10.8160	20%	0.3295	11.3869	60%

The simulation results show that when a priori prediction is added to NMPC, the situation is much better than without it. First, we have fewer collisions, although we still fail four times with three dynamic obstacles added; this is probably more related to the maneuverability of the UBVMS itself, which has better stability but does not move very fast. Second, it maintains a sufficient safety distance, and in the case of only one obstacle, it not only has a 100% success rate but also has a shorter driving distance.

4.2. Experimental Results

To further verify the performance of the proposed algorithm, we conducted experiments on the UBVMS in an indoor pool. The position and yaw angle of the UBVMS $(x,y,\phi )$ were obtained by the global camera located above the pool, the data were transmitted via wired fiber optic cable, the velocity was calculated using an ESO differential tracker, and the sonar acquired the position of the obstacle.

Figure 9 shows the process of the UBVMS performing linear tracking and avoiding moving obstacles. It can be observed that the proposed control system can enable the UBVMS to smoothly perform path tracking as well as prevent a collision. When the sonar detects the obstacle at a close distance, the surge of collision cost of the objective function drives the UBVMS to deviate from the desired trajectory to avoid the collision, and then it returns to the desired path largely without overshoot and achieves zero error before reaching the target end point. The starting position of the UBVMS is the origin in geodesic coordinates, the initial velocity is 0, and the tracking path is a straight line of length 2 m.

Figure 9. Screenshot sequence of the complete experiment.

It is noteworthy that due to the relatively small size of the indoor swimming pool, when the UBVMS moves at the maximum frequency (20 Hz), the additional interference caused by the reflected wave in the swimming pool cannot be ignored. Therefore, when reaching the end of the desired path, the UBVMS still has a slight trajectory offset due to inertia and disturbance. The calculated $\tau ={[f_u 0 f_r]}^T$ is multiplied by the respective scale factor to complete the mapping from force and torque to bilateral fin frequency. Figure 10b shows the time evolution of the control frequency of bilateral fins.

Figure 10. Experimental results. (a) Desired path and current path. (b) Control frequency of bilateral fins.

5. Conclusions

In this article, we have investigated the path following and dynamic obstacle avoidance of the UBVMS propelled by undulatory ribbon-fins with model uncertainties, external environmental disturbances, and input constraints. To achieve minimized path tracking error and robust obstacle avoidance, a general kinematic/dynamic model of the underwater vehicle was developed; external forces were modeled as unmeasured disturbances and were estimated using three ESOs fed into the NMPC prediction model. Moreover, we considered the uncertainty of the state estimator, combining the a priori estimation of the Kalman filter to predict the position of dynamic obstacles. The simulation results show that the proposed method can track the curved trajectory and achieve multi-obstacle avoidance successfully in the event of time-varying disturbances. Our experiments showed that this path-following and collision-avoidance approach for the UBVMS is valid and maintains system stability.