Hybrid Layer of Improved Interfered Fluid Dynamic System and Nonlinear Model Predictive Control for Navigation and Control of Autonomous Underwater Vehicles

Abstract

This study introduces a hybrid control structure called Improved Interfered Fluid Dynamic System Nonlinear Model Predictive Control (IIFDS-NMPC) for the path planning and trajectory tracking of autonomous underwater vehicles (AUVs). The system consists of two layers; the upper layer utilizes the Improved Interfered Fluid Dynamic System (IIFDS) for path planning, while the lower layer employs Nonlinear Model Predictive Control (NMPC) for trajectory tracking. Extensive simulation experiments are conducted to determine optimal parameters for both static and dynamic obstacle scenarios. Additionally, real-world testing is performed using the BlueRov2 platform, incorporating multiple dynamic and static obstacles. The proposed approach achieves real-time control at a frequency of 100 Hz and exhibits impressive path tracking accuracy, with a root mean square (RMS) of 0.02 m. This research provides a valuable framework for navigation and control in practical applications.

1. Introduction

Autonomous underwater vehicles (AUVs) are increasingly utilized in various applications for ocean exploration. Path planning and trajectory tracking are fundamental requirements [1], and the smoothness, efficiency, and tracking deviation of the planned paths play crucial roles in these tasks [2]. Achieving real-time planning and control in complex environments is particularly important for efficient and safe AUV navigation [3]. Although both path planning and trajectory tracking tasks utilize the AUV motion model [4], they are treated as separate tasks within the general guide navigation control (GNC) framework [5].

Different path planning methods are employed for known and unknown obstacle environments [6]. In known static obstacle environments, algorithms like A* [7], Dijkstra [8], and RRT [9] are utilized to generate optimal paths using predetermined obstacle information. Dynamic obstacle environments, where obstacles move and change, present a more complex challenge. Randomized algorithms like PRM [10] and improved RRT [11], as well as SLAM [12] algorithms based on sensor data, are used. However, these methods often lack real-time capabilities.

Real-time path planning and tracking are essential for adapting to dynamic obstacles. To address real-time concerns, Jian, F.W. et al. introduced IIFDS [13], an APF-based method suitable for unknown dynamic environments. Extensive validation has been performed on various platforms, including unmanned boats and aerial vehicles [14,15].

AUV path tracking control has been widely researched. PID control is used in non-critical scenarios [16]. Linear quadratic regulator (LQR) control [17], while useful, may suffer from modeling inaccuracies. Nonlinear sliding mode control (SMC) can handle disturbances but may cause high-frequency oscillations [18].

Model Predictive Control (MPC) offers a trade-off between path tracking error and control constraints. Nonlinear MPC (NMPC) leverages dynamic model characteristics, making it suitable for path tracking control [19,20]. The computational time of the solver algorithm is predominantly consumed by matrix operations and constraints, factors that dictate parsing speed and variable precision. Sequential Quadratic Programming (SQP) is employed to address quadratic linear optimization problems concurrently featuring equality and inequality constraints, with the objective of identifying a locally optimal solution that satisfies the imposed constraints [21]. In the Baidu Apollo autonomous vehicle project, the SQP algorithm has been successfully applied [22].

Based on the extensive literature review presented, it becomes evident that the IIFDS offers the advantage of rapid computational capabilities, whereas NMPC excels in maintaining an optimal balance between tracking error and control effort. Building upon these insights, this paper introduces a composite control strategy that harnesses the strengths of both approaches, hereby referred to as the IIFDS-NMPC control structure (Figure 1). To validate the real-time control requirements, smooth trajectory tracking, and optimal performance in complex environments, we conducted a series of comprehensive simulations and experiments using this hybrid layer structure, encompassing scenarios with both static and dynamic obstacles.

Our contributions can be summarized as follows:

• Real-Time Hybrid Control Structure: We propose a real-time hybrid control structure for path planning and trajectory tracking (depicted in Figure 1). In this architecture, the upper layer leverages the real-time IIFDS controller, while the bottom layer employs the NMPC controller. The computation time for each step remains consistently under 0.01 s, and the RMS tracking deviation is limited to a remarkable 0.02 m.
• Detailed Design Process: We provide a comprehensive explanation of the complete design process. By following the steps outlined here, the proposed structure can be readily implemented across various platform systems, extending its applicability beyond just AUV systems.
• Real-Time Feasibility: Extensive simulations and experiments validate the real-time feasibility of our approach. Testing was conducted in environments featuring both static and dynamic obstacles, with the designed path planning controller compared against A* methods. In static obstacle simulations, our method showcased a planning time reduction of 300 times compared to the A* algorithm. In dynamic obstacle scenarios, planning time was reduced by a factor of three. During real platform testing, our approach achieved a remarkable step-by-step tracking and control time of 0.0067 s, with an RMS tracking deviation as low as 0.01867 m.
• Physical Experimentation: To the best of our knowledge, this approach is employed for the first time in physical experiments with AUVs. The STM32 interface board acquires obstacle and environmental information from the ground station, while the main control NVIDIA-X2 core is utilized for path planning and the computation of optimal control quantities under various constraints.

The subsequent sections of this paper are organized as follows: Section 2 presents the kinematic and dynamic models of the AUV system along with the mathematical representations of obstacles. Section 3 delves into the methodology and algorithmic framework of our proposed hybrid layer structure. The simulation setup and experimental evaluations are detailed in Section 4, while Section 5 offers a comprehensive discussion of the results and performance analysis. Finally, in Section 6, we conclude the paper by summarizing the findings and outlining potential avenues for future research.

2. AUV Systems and Obstacle Scenario Modeling

To design the hybrid controller for the AUV known as BlueRov2, which is manufactured by BlueROV Robotics, located in Victoria, BC, Canada [23], as illustrated in Figure 2, it is essential to develop a comprehensive mathematical model that encompasses both the AUV itself and the obstacle scenarios it may encounter. This section aims to provide an overview of the AUV model, covering its kinematic and dynamic aspects. The kinematic model establishes the relationship between the AUV’s position, velocity, and acceleration during its motion. Conversely, the dynamic model captures the dynamic changes that occur during acceleration. Furthermore, the obstacle scenarios are described using model equations to provide a clear representation of the challenges the AUV may encounter.

2.1. Coordinate Systems of AUVs

The AUV system is designed to operate with six degrees of freedom (6DOF), allowing it to achieve independent movement in three linear directions (surge, sway, and heave) and three rotational directions (roll, pitch, and yaw). These six DOF provide the AUV with the capability to navigate and maneuver effectively in three-dimensional space. To describe the AUV’s motion, two frames of reference are commonly used: the inertial frame (I-frame) and the body frame (B-frame) [24]. It is assumed that the center of gravity and the center of buoyancy of the ROV are located at the center of the robot skeleton. This configuration is illustrated in Figure 2.

2.2. Kinematic System Model

The kinematic model primarily explains the geometric relationships among the motion variables of the AUV system, without considering the forces and torques that cause system motion and changes. Conversion can be performed between the body coordinate system and the earth coordinate system variables. The parameters of the AUV system include the position vector $\eta$, the velocity vector $\nu$, and the thrust vector $\tau$ [25]. The definitions of each variable are as follows.

$$\begin{array}{cc}\hfill \eta & \hfill ={[x,y,z,\varphi ,\theta ,\psi ]}^T\hfill \\ \hfill v& \hfill ={[u,v,w,p,q,r]}^T\hfill \\ \hfill \tau & \hfill ={[X,Y,Z,K,M,N]}^T\hfill \end{array}$$

(1)

The position vector $\eta$ represents the coordinates in the Earth coordinate system, encompassing the absolute position of the origin $(x,y,z)$ and the Euler angles $(\varphi ,\theta ,\psi )$ that indicate the orientation. In contrast, the body coordinate system is defined based on the centroid of the AUV. In this system, the velocity vector $\nu$ describes the linear velocities $(u,v,w)$ along the three axes of the body coordinate system, as well as the angular velocities $(p,q,r)$ around those axes.

Furthermore, the thrust vector $\tau (X,Y,Z,K,M,N)$ denotes the external forces and torques acting upon the AUV system, expressed as a vector within the body coordinate system. To facilitate the transformation of variables between these two coordinate systems, a reversible transformation matrix $J\left(\eta \right)\in {\mathbb{R}}^{6\times 6}$ is employed. This matrix enables the conversion between the respective coordinate systems.

$$\begin{array}{cc}\hfill & \dot{\eta }=\mathrm{J}\left(\eta \right)v\hfill \\ \hfill & \hfill J\left(\eta \right)=\left[\begin{array}{cc}\hfill \mathrm{R}\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill \mathrm{T}\hfill \end{array}\right]\hfill \end{array}$$

(2)

The Euler angles $\eta ={[\varphi ,\theta ,\psi ]}^T$ correspond to the roll, pitch, and yaw angles of the AUV system. The velocity transformation matrix, denoted $R\left(\eta \right)$, facilitates the conversion from the body coordinate system to the Earth coordinate system. Similarly, the angular velocity transformation matrix, denoted $T\left(\eta \right)$, enables the conversion from the body coordinate system to the Earth coordinate system. Conversely, when transforming from the Earth coordinate system to the body coordinate system, an inverse matrix is utilized. The definitions of $R\left(\eta \right)$ and $T\left(\eta \right)$ are as follows:

$$\mathrm{R}\left(\eta \right)=\left[\begin{array}{ccc}cos\psi cos\theta & -sin\psi cos\phi +cos\psi sin\theta sin\phi & sin\psi sin\phi +cos\psi cos\phi sin\theta \\ sin\psi cos\theta & cos\psi cos\phi +sin\phi sin\theta sin\psi & -cos\psi sin\phi +sin\theta sin\psi cos\phi \\ -sin\theta & cos\theta sin\phi & cos\theta cos\phi \end{array}\right]$$

(3)

$$\mathrm{T}\left(\eta \right)=\left[\begin{array}{ccc}1& sin\phi tan\theta & cos\phi tan\theta \\ 0& cos\phi & -sin\phi \\ 0& sin\phi /cos\theta & cos\phi /cos\theta \end{array}\right]$$

(4)

When $\theta =\pm {90}^{\circ }$, $T\left(\eta \right)$ does not exist. Therefore, the angles of the AUV system have certain limitations. The roll angle $\varphi$ lies within the range of $-\pi <\varphi \le \pi$, while the pitch angle $\theta$ is restricted to $-\pi /2<\theta <\pi /2$.

2.3. Dynamic System Model

The AUV is a 6DOF nonlinear dynamic model with disturbance. The dynamics equation of the AUV system studies the relationship between external forces and motion, which can be expressed using the following formula,

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

(5)

where $M=M_{RB}+M_A$, $M_{RB}$ is a rigid body inertia matrix and $M_A$ is an added mass inertia matrix. Among them, the standard $M_{RB}$ is

$$M_{RB}=\left[\begin{array}{cccccc}m& 0& 0& 0& m{z}_G& -m{y}_G\\ 0& m& 0& -m{z}_G& 0& m{x}_G\\ 0& 0& m& m{y}_G& -m{x}_G& 0\\ 0& -m{z}_G& m{y}_G& I_x& -I_{xy}& -I_{xz}\\ m{z}_G& 0& -m{x}_G& -I_{yx}& l_y& -I_{yz}\\ -m{y}_G& m{x}_G& 0& -I_{zx}& -I_{zy}& I_z\end{array}\right]$$

(6)

If the center of mass and center of buoyancy of the AUV are concentric, the above formula can be greatly simplified.

$$M_{RB}=\left[\begin{array}{cccccc}m& 0& 0& 0& 0& 0\\ 0& m& 0& 0& 0& 0\\ 0& 0& m& 0& 0& 0\\ 0& 0& 0& I_x& 0& 0\\ 0& 0& 0& 0& l_y& 0\\ 0& 0& 0& 0& 0& I_z\end{array}\right]$$

(7)

The same type of $C_{RB}$ is the coriolis force and added mass term centripetal force,

$$C\left(v\right)=C_{RB}\left(v\right)+C_A\left(v\right)$$

(8)

where

$$C_{RB}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& m{v}_z& -m{v}_y\\ 0& 0& 0& -m{v}_z& 0& m{v}_x\\ 0& 0& 0& m{v}_y& -m{v}_x& 0\\ 0& m{v}_z& -m{v}_y& 0& I_z{\omega }_z& -I_y{\omega }_y\\ -m{v}_z& 0& m{v}_x& -I_z{\omega }_z& 0& I_x{\omega }_x\\ m{v}_y& -m{v}_x& 0& I_y{\omega }_y& -I_x{\omega }_x& 0\end{array}\right]$$

(9)

and

$$\begin{array}{cc}\hfill & \hfill C_A\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& -Z_{{\dot{v}}_z}{v}_z& Y_{{\dot{v}}_y}{v}_y\\ 0& 0& 0& Z_{{\dot{v}}_z}{v}_z& 0& -X_{{\dot{v}}_x}{v}_x\\ 0& 0& 0& -Y_{{\dot{v}}_y}{v}_y& X_{{\dot{v}}_x}{v}_x& 0\\ 0& -Z_{{\dot{v}}_z}{v}_z& Y_{{\dot{v}}_y}{v}_y& 0& -N_{{\dot{\omega }}_z}{\omega }_z& M_{{\dot{\omega }}_y}{\omega }_y\\ Z_{{\dot{v}}_z}{v}_z& 0& -X_{{\dot{v}}_x}{v}_x& N_{{\dot{\omega }}_z}{\omega }_z& 0& -K_{{\dot{\omega }}_x}{\omega }_x\\ -Y_{{\dot{v}}_y}{v}_y& X_{{\dot{v}}_x}{v}_x& 0& -M_{{\dot{\omega }}_y}{\omega }_y& K_{{\dot{\omega }}_x}{\omega }_x& 0\end{array}\right]\hfill \end{array}$$

(10)

According to the literature [26], we divide $D\left(\right|v\left|\right)$ for the surface resistance of the linear damping D and wave nonlinear quadratic damping $D_n\left(\right|v\left|\right)$ into two parts. The literature [24] states that the assumption of the AUV system has good symmetry, therefore,

$$D\left(\right|v\left|\right)=-diag\left\{X_u,Y_v,Z_w,K_p,M_q,N_r\right\}-diag\left\{X_{u\left|u\right|}\left|u\right|,Y_{v\left|v\right|}\left|v\right|,Z_{w\left|w\right|}\left|w\right|,K_{p\left|p\right|}\left|p\right|,M_{q\left|q\right|}\left|q\right|,N_{r\left|r\right|}\left|r\right|\right\}$$

(11)

The AUV system is subject to vertical downward gravity W acting on the center of gravity and vertical upward buoyancy B acting on the center of buoyancy on the vertical plane. The surface vehicle equilibrium condition is $W=B$, and it acts on the same plumb line. AUV systems are usually designed to be critical with near-zero buoyancy, i.e., gravity W is slightly less than buoyancy B. Restoring force determines the initial stability of the system; that is, when the AUV system is out of equilibrium due to external forces, the combined action of gravity and buoyancy can restore the system to equilibrium. The center of buoyancy is higher than the center of gravity, which can generate the torque of restoring equilibrium:

$$\mathbf{g}\left(\eta \right)=\left[\begin{array}{cc}\hfill & (W-B)sin\left(\theta \right)\hfill \\ \hfill & -(W-B)cos\left(\theta \right)sin\left(\varphi \right)\hfill \\ \hfill & -(W-B)cos\left(\theta \right)cos\left(\varphi \right)\hfill \\ \hfill & -\left(y_{g}W-y_{b}B\right)cos\left(\theta \right)cos\left(\varphi \right)+\left(z_{g}W-z_{b}B\right)cos\left(\theta \right)sin\left(\varphi \right)\hfill \\ \hfill & \left(z_{g}W-z_{b}B\right)sin\left(\theta \right)+\left(x_{g}W-x_{b}B\right)cos\left(\theta \right)cos\left(\varphi \right)\hfill \\ \hfill & -\left(x_{g}W-x_{b}B\right)cos\left(\theta \right)sin\left(\varphi \right)-\left(y_{g}W-y_{b}B\right)sin\left(\theta \right)\hfill \end{array}\right]$$

(12)

All the symbols used in the above equations are defined as follows: m is the mass of the AUV and $X_{u\left|u\right|},Y_{v\left|v\right|},Z_{w\left|w\right|},K_{p\left|p\right|},M_{q\left|q\right|}$, and $N_{r\left|r\right|}$ are hydrodynamic coefficients.

2.4. Obstacle Model

Static underwater obstacles mainly include rocks, shipwrecks, and artificial obstacle lights. These obstacles can be simplified into spheres, cylinders, cuboids, and semi-spheres or their combinations. Moving obstacles such as large fish and underwater moving hulls can be regarded as spheres, and various typical obstacles can be regarded as a variety of standard convex objects. The shape of q can use a unified mathematical expression [27]:

$$\Gamma \left(P\right)={\left(\frac{x-x_0}{a}\right)}^{2p}+{\left(\frac{y-y_0}{b}\right)}^{2q}+{\left(\frac{z-z_0}{c}\right)}^{2r}$$

(13)

In the given context, $(x_0,y_0,z_0)$ represents the center of the obstacle, and $(x,y,z)$ denotes the current position of the AUV. The coefficients a, b, and c, respectively, indicate the sizes of the obstacle’s axis lengths, while the coefficients p, q, and r determine the shape of the obstacle. The specific shapes and coefficient values for various convex obstacles are as follows: Sphere: $p=q=r=1$, $a=b=c$; Cylinder: $p=q=1$, $r>1$, $a=b$; Cone: $p=q=1$, $0<r<1$, $a=b$; Cuboid: $p>1$, $q>1$, $r>1$. Figure 3 illustrates typical convex obstacle shapes created by different coefficient combinations. Here, $\Gamma \left(P\right)<1$, $\Gamma \left(P\right)>1$, and $\Gamma \left(P\right)=1$ represent the interior region, exterior region, and surface of the obstacle, respectively. The simulation plots for various obstacles are shown below. In real scenarios, the obstacle environment can be created by combining different obstacles as depicted in Figure 3.

3. Hybrid Layer Controller Design

3.1. Proposed Hybrid Structure of Controller

The proposed hybrid layer controller, as in Figure 4, comprises an IIFDS path planner and an NMPC tracking controller. The IIFDS is responsible for path planning by considering the agent’s current position, obstacles, and destination. It can iteratively update the overall path based on the initial agent position and surrounding environment information, or dynamically update the next reference position based on its own position and environment information, making it suitable for real-time path planning in dynamic environments. During the path planning process, drive and legality constraints are considered to ensure a feasible path. The NMPC controller effectively tracks the trajectory and rejects dynamic disturbances. Combining the two controllers enables real-time path planning and tracking from the start point to the destination, forming a double loop of navigation and control.

3.2. Upper Layer of IIFDS Navigation Controller Design

The algorithm is based on a variational approach, where the perturbation effect of obstacles on the flow field is represented using a modified quantization matrix $\overline{M}$, resulting in a perturbed flow field [13]. The introduction of tangential velocity in [14] expands the physical characteristics of the fluid, making the perturbed streamlines more suitable for AUV operation. Prior to planning the path, all obstacles are defined according to the formula in Equation (13). The position of the destination point is set as $P_d=(x_d,y_d,z_d)$ and the number of obstacles is denoted as K. If there are no obstacles in the current path, the original streamlines go directly from the initial position to the target point with a velocity magnitude of V just as Figure 5 shown. Hence, the initial flow field velocity $u\left(P\right)$ can be represented by Equation (14), where u represents the flow velocity. Subsequently, the perturbation effect of obstacles on the flow field is quantified using the modification matrix M. Finally, the initial flow field is corrected to obtain the perturbed flow field. This perturbed flow field remains stable, meaning that the flow streamlines can avoid obstacles and eventually reach the target point.

The design process of the proposed method can be summarized as follows:

Step 1. 

Calculate the initial fluid field $u\left(P\right)$ by considering the current position and the destination.

$$u\left(P\right)=-{[\frac{V(x-x_d)}{d(P,P_d)},\frac{V(y-y_d)}{d(P,P_d)},\frac{V(z-z_d)}{d(P,P_d)}]}^T$$

(14)

In the algorithm, $u\left(P\right)$ represents the initial flow field at each computed sampling point during the operation of the AUV. This initial flow field evolves continuously as the position changes. Here, V denotes the confluence velocity and $d(P,P_d)$ represents the distance between the AUV position P and the target position $P_d$.

$$d(P,P_d)=\sqrt{{(x-x_d)}^2+{(y-y_d)}^2+{(z-z_d)}^2}$$

(15)

Step 2. 

Calculate the disturbance modification matrix $\overline{M}$ based on the presence of static or moving obstacles.

If there are K obstacles present in the environment, then the perturbed flow field velocity is obtained by modifying the initial flow field velocity $u\left(P\right)$ using the modification matrix $\overline{M}$ for each obstacle.

$$\overline{M}=\sum _{k=1}^K{\omega }_k{\overline{M}}_k$$

(16)

where $w_k$ represents the weight factor of the $k_{th}$ obstacle.

$${\omega }_k=\left\{\begin{array}{c}1,K=1\hfill \\ \prod _{i=1,i\ne k}^K\frac{{\Gamma }_i-1}{({\Gamma }_i-1)+({\Gamma }_k-1)},K\ne 1\hfill \end{array}\right.$$

(17)

where ${\Gamma }_i$ and ${\Gamma }_k$ represent the expressions for the $i_{th}$ and $k_{th}$ obstacles, respectively, calculated according to Equation (13). Due to the condition ${\omega }_{sum}={\sum }_{k=1}^K{\omega }_k\le 1$, it is necessary to normalize the weight of each obstacle at each position. The normalized weight of the $k_{th}$ obstacle is given by

$${\omega }_{k,nor}=\frac{{\omega }_k}{{\omega }_{sum}}$$

(18)

With this normalization, Equation (16) can be written as follows,

$$\overline{M}=\sum _{k=1}^K{\omega }_{k,nor}{\overline{M}}_k$$

(19)

Building upon the original IFDS [13], the introduction of the tangential matrix incorporates the perturbation matrix. Therefore, ${\overline{M}}_k$ represents the modification matrix for the $k_{th}$ obstacle.

$${\overline{M}}_k=I-\frac{n_k{n}_k^T}{{\Gamma }_k^{\frac{1}{{\rho }_k}}{n}_k^T{n}_k}+\frac{t_k{n}_k^T}{{\Gamma }_k^{\frac{1}{{\sigma }_k}}|t_k||n_k|}$$

(20)

where ${\rho }_k$ is the repulsion coefficient, ${\sigma }_k$ is the saturation coefficient, and ${\theta }_k$ is the tangential coefficient. These three factors collectively determine the avoidance time and sensitivity in obstacle avoidance.

$${\rho }_k={\rho }_k^0.e^{(1-\frac{1}{d(P,P_d)d(P,O_k)})}$$

(21)

$${\sigma }_k={\sigma }_k^0.e^{(1-\frac{1}{d(P,P_d)d(P,O_k)})}$$

(22)

where ${\rho }_k^0>0$ is the repulsion factor of the $k_{th}$ obstacle, ${\sigma }_k^0>0$ is the tangential factor of the $k_{th}$ obstacle, $d(P,P_d)$ is the distance between the AUV object and the target point, $d(P,O_k)$ is the distance between the AUV and the surface of the $k_{th}$ obstacle, I represents the identity matrix of order three, and $n_k\left(P\right)$ is the outward normal vector to the surface of the $k_{th}$ obstacle perpendicular to it.

$$n_k={(\frac{\partial {\Gamma }_k}{{\partial }_x},\frac{\partial {\Gamma }_k}{{\partial }_y},\frac{\partial {\Gamma }_k}{{\partial }_z})}^T$$

(23)

As for the other variables, $-\frac{n_k{n}_k^T}{{\Gamma }_k^{\frac{1}{{\rho }_k}}{n}_k^T{n}_k}$ is the repulsive matrix, $\frac{t_k{n}_k^T}{{\Gamma }_k^{\frac{1}{{\sigma }_k}}|t_k||n_k|}$ is a tangential matrix, and $t_k$ is the horizontal tangent vector, which can be obtained using the following expression. The tangent coordinate system $o^{\prime }-x^{\prime }{y}^{\prime }{z}^{\prime }$ is defined by setting $t_{k,1}$, $t_{k,2}$, and $n_k$ as the $x^{\prime }$, $y^{\prime }$, and $z^{\prime }$ axes, respectively. The calculation process of $t_k$ is as follows:

Define two mutually orthogonal vectors, $t_{k,1}$ and $t_{k,2}$, which are also orthogonal to $n_k$,

$$\left\{\begin{array}{c}{x}^{\prime }=t_{k,1}={[\frac{{\partial }_{{\Gamma }_k}}{{\partial }_y},-\frac{{\partial }_{{\Gamma }_k}}{{\partial }_x},0]}^T\hfill \\ y^{\prime }=t_{k,2}={[\frac{{\partial }_{{\Gamma }_k}}{{\partial }_x}\frac{{\partial }_{{\Gamma }_k}}{{\partial }_z},\frac{{\partial }_{{\Gamma }_k}}{{\partial }_y}\frac{{\partial }_{{\Gamma }_k}}{{\partial }_z},-{\left(\frac{{\partial }_{{\Gamma }_k}}{{\partial }_x}\right)}^2-{\left(\frac{{\partial }_{{\Gamma }_k}}{{\partial }_y}\right)}^2]}^T\hfill \end{array}\right.$$

(24)

By using $n_k$, $t_{k,1}$, and $t_{k,2}$ as the basis vectors for a new coordinate system, any unit vector in the plane perpendicular to $n_k$ can be expressed in the form

$$t_k^{\prime }={[cos{\theta }_k,sin{\theta }_k,0]}^T$$

(25)

where ${\theta }_k^{{}^{\prime }}\subset [-\pi ,\pi ]$ is the tangential direction factor. This angle represents the angle between any tangent vector and the $x^{\prime }$ axis. It determines the angle of avoidance direction. The $z^{\prime }$ axis is obtained through rotation. Therefore, in the inertial coordinate system, $t_k^{{}^{\prime }}$ is transformed to the $xoy$ standard coordinate system by using the tangent coordinate axis transformation matrix,

$$t_k={\Omega }_T^I{t}_k^{\prime }$$

(26)

where ${\Omega }_T^I$ is the transformation matrix from the tangent coordinate system to the inertial coordinate system,

$${\Omega }_T^I=\left[\begin{array}{ccc}\frac{r_y}{r_2}& \frac{r_x{r}_z}{r_2{r}_3}& \frac{r_x}{r_3}\\ \frac{r_x}{r_2}& \frac{r_y{r}_z}{r_2{r}_3}& \frac{r_y}{r_3}\\ 0& -\frac{r_2}{r_3}& \frac{r_z}{r_3}\end{array}\right]$$

(27)

and $r_x=\frac{{\partial }_{{\Gamma }_k}}{{\partial }_x}$, $r_y=\frac{{\partial }_{{\Gamma }_k}}{{\partial }_y}$, $r_z=\frac{{\partial }_{{\Gamma }_k}}{{\partial }_z}$, $r_2=\sqrt{r_x^2+r_y^2}$, $r_3=\sqrt{r_x^2+r_y^2+r_z^2}$.

Step 3. 

Compute the final perturbed fluid field using the results obtained from the previous two steps.

After modifying the initial flow field velocity $u\left(P\right)$, the perturbed flow field velocity becomes

$$\overline{u}=\overline{M}u\left(P\right)$$

(28)

During the actual operation of the AUV, it encounters not only static obstacles but also dynamic obstacles. In such cases, the velocity of dynamic obstacles needs to be considered in the flow field [15]. To incorporate the dynamic obstacle velocity, the process involves calculating the relative positions based on the static flow field, adding perturbation flow field effects, and finally incorporating the dynamic obstacle velocity.

The total velocity $v\left(P\right)$ of a dynamic obstacle is a combination of the obstacle’s velocity and its weight factor. If there is only one obstacle, the weight factor $\omega$ can be calculated according to Equation (17) as mentioned earlier. Assuming the velocity of the $k_{th}$ obstacle is $v_{k,obs}$, its reference motion velocity is given by

$$\overline{v_{k,obs}}=e^{-\left(\frac{({\Gamma }_k\left(P\right)-1)}{{\lambda }_k}\right)}{v}_{k,obs}$$

(29)

Step 4. 

Calculate the velocity of the dynamic obstacle under the action of the flow field.

The total vector velocity of the obstacle can be calculated using the following formula,

$$v\left(P\right)=\sum _{k=1}^K{\omega }_{k,nor}\left(P\right)\overline{v_{k,obs}}$$

(30)

Here, $v_{k,obs}$ represents the actual velocity of the $k_{th}$ obstacle at position P at a specific time point. ${\lambda }_k$ is the corresponding weight, where a higher value indicates a shorter time frame for the AUV to avoid the obstacle.

Step 5. 

Finally, determine the next position at the subsequent time step by considering the sample time and the current position.

Velocity and position of the final perturbed flow field are

$$\overline{u}\left(P\right)=\left\{\begin{array}{cc}\overline{M}\left(P\right)(u\left(P\right)-v\left(P\right))+v\left(P\right),\hfill & \mathrm{for}\mathrm{the}\mathrm{dynamic}\mathrm{obstacle}\hfill \\ \overline{M}\left(p\right)u\left(P\right),\hfill & \mathrm{for}\mathrm{the}\mathrm{static}\mathrm{obstacle}\hfill \end{array}\right.$$

(31)

$$P_{next}=P+\overline{u}\left(P\right){\Delta }_t$$

(32)

Among them, ${\Delta }_t$ is the sampling control period. A smaller value of ${\Delta }_t$ leads to more accurate results, but it also increases computational complexity. The parameters ${\rho }_k$, ${\sigma }_k$, and ${\theta }_k$ shape the streamlines. Adjusting ${\sigma }_k$ and ${\theta }_k$ avoids stagnation points and traps.

In IIFDS, the tangential matrix enables fluid flow in multiple directions around obstacles. ${\theta }_k$ determines the path plane; when it is close to 0 or $\pi$, the path is horizontal (Path 1, Path 5), favoring side avoidance. When it is close to $\pi /2$, the path is vertical (Path 3), favoring top avoidance, as in Figure 6.

For the parameters $\sigma$ and $\rho$, which represent the tangential reaction coefficient and repulsion coefficient, respectively, the larger their values, the earlier the planned path avoids obstacles, and the AUV stays farther away from obstacles, making the planned path safer, as in Figure 7 and Figure 8.

In the case of a moving obstacle environment, as shown in Figure 9, the parameter $\lambda$ is a positive value. A larger value indicates that the AUV avoids obstacles earlier, reacts faster, and achieves better avoidance effects. However, when the AUV is farther away from the obstacles, the relative reference velocity increases. This increases the possibility of collision between the AUV and the obstacles. In other words, the higher the difficulty level of avoiding obstacles for the AUV, the greater the associated risk. This is because the moving obstacles appear relatively stationary in the frame of reference of the navigation vector field.

In general, randomly selected parameters affect the length of the path and the distance from obstacles, potentially rendering the path planning infeasible. Therefore, parameter optimization is crucial for the performance of the system. By selecting appropriate parameters, the IIFDS method can always strike a balance between safety and smoothness, effectively avoiding obstacles.

3.3. Bottom Layer of NMPC-Based Trajectory Tracking Controller Design

The reference path is planned using the outer-loop path planning layer, and the reference path can be described in the following form,

$$P_r\left(t\right)=(x_d\left(t\right),y_d\left(t\right),z_d\left(t\right))$$

(33)

The path tracking error is

$$P_e\left(t\right)=P\left(t\right)-P_r\left(t\right)$$

(34)

and the control object is to guarantee the deviation of the position to converge to zero, i.e., $lim{P}_e\left(t\right)\to 0$. The reference path dynamically changes based on the movement of the plant and the distance between obstacles. If there are any dynamic obstacles present, the corresponding fluid velocity is adjusted accordingly. The inner-loop NMPC controller then follows the receding path. This process can be described in the following steps:

Step 1. 

Express the model in the nonlinear state space equation.

The nonlinear state model can be described as follows:

$$x={[\eta ,\nu ]}^T$$

(35)

and the first deviating equation consists of state, control input, and disturbance,

$$\dot{x}={[\dot{\eta },\dot{\nu }]}^T=f(x,u,\omega ,t)$$

(36)

The Nonlinear MPC model’s function is

$$\left\{\begin{array}{c}\dot{x}\left(t\right)=f(x\left(t\right),u\left(t\right),\omega \left(t\right))\hfill \\ y\left(t\right)=h\left(x\right(t\left)\right)\hfill \end{array}\right.$$

(37)

Step 2. 

Prediction process.

Regarding the state space of the nonlinear model, the next state can be determined using the Runge–Kutta (RK) method. In the prediction process, both the fourth-order and sixth-order RK methods can be employed. However, it is crucial to strike a balance between computational cost and accuracy. Considering this trade-off, utilizing the RK4 method is deemed sufficient [28].

$$\begin{array}{c}\hfill x_{t+1}=f_d(x_t,u_t,{\omega }_d,t)=x_t+\Delta t·f(x_t,u_t,{\omega }_{dt},t)\\ \hfill =x_t+\frac{1}{6}(k_1+2k_2+2k_3+k_4)\end{array}$$

(38)

$$\begin{array}{c}{k}_1={\Delta }_t·f(x_t,u_t,w_{dt})\hfill \\ k_2={\Delta }_t·f(x_t+\frac{k_1}{2},u_t,w_{dt})\hfill \\ k_3={\Delta }_t·f(x_t+\frac{k_2}{2},u_t,w_{dt})\hfill \\ k_4={\Delta }_t·f(x_t+k_3,u_t,w_{dt})\hfill \end{array}$$

(39)

The characteristics of nonlinear AUV dynamics are captured during each time step of model prediction. Furthermore, the prediction is based on the actual state and control variables $x_t$, $u_t$, rather than operating points $x_0$, $u_0$. This grants Nonlinear Model Predictive Control (NMPC) a natural advantage over other control strategies that rely on linear approximations. Additionally, the controller prediction model incorporates the propeller characteristics and disturbances from the ROV dynamics Ordinary Differential Equations (ODEs). Therefore, the discrete model $f_d$ possesses the capability to predict nonlinear ROV dynamics in the presence of 6DOFdisturbances. This implies that NMPC can better adapt to the actual dynamic characteristics and disturbances of the ROV system, rather than being limited to a specific operating point and linear approximation. Such features make NMPC particularly effective in complex, real-time control environments.

Step 3. 

Control object and the cost function.

According to the NMPC frame, the path tracking problem can be seen as a constraint optimization problem, as it is for the cost function, and N is the predictive time domain [29],

$$J=\sum _{i=1}^N\left({\omega }_1∥x_p(k+i|k)-x_{pref}∥+{\omega }_2∥x_v(k+i|k)-x_{vref}∥+{\omega }_3\parallel u(k+i|k)-u_{ref}\parallel \right)$$

(40)

$$\begin{array}{c}\hfill \mathrm{s}.\mathrm{t}{x}_0=x_{init},0\le x_t\le x_{max}\hfill \end{array}$$

(41)

Just as in Equation (40), ${\omega }_1,{\omega }_2$ are the corresponding weight matrices to position deviation $x_p$ and the velocity deviation $x_v$, ${\omega }_3$ is the weight matrix on control $\Delta u$, and ${\omega }_1\in {\mathbb{R}}^{6*1}$, ${\omega }_2\in {\mathbb{R}}^{6*1}$, and ${\omega }_3\in {\mathbb{R}}^{8*1}$. These three control objectives the nonlinear MPC would like to achieve at the same time. Firstly, the objective of the cost function is to minimize the tracking error, ensuring that the AUV approaches and stably maintains as close as possible to the reference position. The cost function adopts a formulation represented by ${\omega }_1∥x_p(k+i|k)-x_{pref}∥$, which penalizes the disparity between the current state and the positional reference. Secondly, during underwater operations, angular velocity and linear velocity need to be maintained at zero after reaching the target point. Consequently, $x_{\mathrm{vref}}$ is likewise set to ${\mathbf{0}}_{6*1}$. Last but equally significant, the optimal controller is subject to three constraints. The first two constraints, respectively, ensure stable path-keeping and remaining stationary. The design of the optimal controller aims at minimizing the overall control effort. The intermediate cost J constitutes the sum of all control variables, governed by the optimal control strategy $u^{*}$ to minimize the control effort of the AUV. The reference control variables $u_{\mathrm{ref}}$ are set to ${\mathbf{0}}_{8*1}$.

During the path tracking process of an AUV, both the operational state and control variables of the AUV are subject to certain constraints. These constraints consist of both hard and soft limits. It is imperative to adhere to these constraints, with particular emphasis on the hard constraints, as they must be strictly followed for safety reasons. The system state $x_t$ and the control variables $u_t$ are constrained as follows:

$$\begin{array}{c}\hfill x_{min}\le x_t\le x_{max}\\ \hfill u_{min}\le u_t\le u_{max}\end{array}$$

(42)

where $x_{min},u_{min}$ are the lower bound of the state vector and the control input, and $x_{max},u_{max}$ are the upper bound of the state vector and the control value. In the control process, the bound domain can be rewritten as

$$\begin{array}{c}\hfill u_{min}\le u\left(k\right)\le u_{max}\\ \hfill x_{min}\le f(x_t,u_t,w_d)\le x_{max}\end{array}$$

(43)

The aforementioned NMPC equations undergo optimization at each time step, with only the first optimized control signal $u^{*}$ being applied to the AUV. The system’s new control trajectory is optimized in a receding horizon manner. In other words, after the AUV sends the first control signal, the prediction horizon transitions from $[0,T]$ to $[t,t+T]$.

In summary, the objective of this controller is to enable the AUV to track the reference position, ultimately stabilizing at a stationary state while achieving optimal control.

3.4. Algorithm and Pseudocode

The algorithm for path planning and trajectory tracking is outlined in Algorithm 1, while the NMPC algorithm is encapsulated in Algorithm 2.

Algorithm 1: Hybrid Layer of Navigation and Tracking Controller of the AUV.

1: Initialize the initial state $x_0$, the destination position $p_d$, the velocity of AUV v, the predictive horizon N, the weight matrix ${\omega }_1,{\omega }_2,{\omega }_3$, control value bound $u_{max},u_{min}$, state bound $x_{min},x_{max}$; 2: Compute the distance from current position P to destination position $P_d$, using Equation (15). 3: while AUV has not reached the destination do 4:    Calculate the initial fluid field $u\left(P\right)$, using Equation (14), 5:    Obtain obstacle information from the environment, including the position $O(x,y,z)$ and velocity $v(x,y,z)$, through the perception layer. Based on the current position, as well as the positions and velocities of obstacles (if they are dynamic), calculate the disturbance flow field matrix $\overline{M}$, using Equations (16)–(27). 6:    Calculate the overall vector velocity of dynamic obstacles in the flow field $\overline{v_{k,obs}}$, if applicable, using Equations (29) and (30). 7:    Calculate the next potential position for the next path $P_{next}$, using Equations (31) and (32). 8:    Compute the optimal control quantity based on the reference path and Equations (33)–(43). 9:    Calculate the control quantity using Equation (40) and Algorithm 2 based on the current position P and the next position $P_{next}$, which is the path to be tracked. 10:   Measure all the states of the scenario, compute the distance from current position P to destination position $P_d$, using Equation (15). 11: end while

Algorithm 2: The Process of Computing the Control Value u in NMPC.

1: Initialize all initial state variables $x_0$, initialize all control variables u, set up the weight matrices ${\omega }_1,{\omega }_2,{\omega }_3$, and define the range of control constraints $u_{min},u_{max}$, set the predictive horizon N, obtain the reference state $x_{r}ef$ from the planned path. 2: while Not meeting the specified prediction horizon cycles N, do 3:    Calculate the next state variable based on Equation (38), 4:    Restrict the state variables within the constraints $x_{min}$, $x_{max}$ defined by Equation. 5:    Compute the cost based on the reference path and weight matrix, according to Equation (40). 6: end while 7: Sum up the cost values obtained in the aforementioned N instances to calculate the total cost, yielding the value of J based on Equation (40). 8: Compute the control input u based on the cost J and constraints $u_{max},u_{min}$, in this process, the optimal solver is SQP, and the computed control variables need to adhere to the constraints specified by Equation (43). 9: Apply the first dimension control input $u[1:]$ to control the system.

4. Computer Simulation Experiment

To validate the proposed method for path planning and AUV trajectory tracking, we utilize the AUV model described in Section 2 and employ the hybrid layer controller outlined in Section 3. The simulation experiments are conducted within the Matlab 2018 Rb environment, with all AUV parameters defined in

Table 1. Parameters of the AUV.

Parameter	Value	Parameter	Value	Parameter	Value	Parameter	Value	Parameter	Value
m	$11.5$ kg	$I_y$	$0.16$ kg.m${}^2$	$K_{\dot{p}}$	$-0.12$ kg.m${}^2$/rad	$L_{DZw}$	$-5.18$ Ns/m	$Q_{DYv}$	$-21.66$ Ns${}^2$/m${}^2$
L	$0.4571$ m	$I_z$	$0.16$ kg.m${}^2$	$M_{\dot{q}}$	$-0.12$ kg.m${}^2$/rad	$L_{DKp}$	$-0.07$ Ns/rad	$Q_{DZw}$	$-36.99$ Ns${}^2$/m${}^2$
W	$0.4361$ m	$X_{\dot{u}}$	$-5.5$ kg	$N_{\dot{r}}$	$-0.12$ kg.m${}^2$/rad	$L_{DMq}$	$-0.07$ Ns/rad	$Q_{DKp}$	$-1.55$ Ns${}^2$/rad${}^2$
H	$0.2539$ m	$Y_{\dot{v}}$	$-12.7$ kg	$L_{DXu}$	$-4.03$ Ns/m	$L_{DNr}$	$-0.07$ Ns/rad	$Q_{DMq}$	$-1.55$ Ns${}^2$/rad${}^2$
$I_x$	$0.16$ kg.m${}^2$	$Z_{\dot{w}}$	$-14.57$ kg	$L_{DYv}$	$-6.22$ Ns/m	$Q_{DXu}$	−18.18 Ns${}^2$/m${}^2$	$Q_{DNr}$	$-1.55$ Ns${}^2$/rad${}^2$

. The experimental system is equipped with an 11th Gen Intel(R) Core(TM) i9-11900H @ 2.50 GHz processor, 32.0 GB RAM, and Windows 10 operating system.

The test environment encompasses both static obstacle and dynamic obstacle scenarios. The crucial metrics for evaluation include path planning time and tracking control deviation, path planning length, smoothness, and the range of control values during tracking that should also be considered.

In Table 1, m is the mass of the AUV; L, W, and H, respectively, represent the length, width, and height of the AUV; $I_x,I_y$, and $I_z$ are the moments of inertia about the $x_b, y_b$, and $z_b$ axes; $X_{\dot{u}}, Y_{\dot{v}}$, and $Z_{\dot{w}}$ represent the hydrodynamic forces for linear motion; and $K_{\dot{p}}, M_{\dot{q}}$, and $N_{\dot{r}}$ represent the hydrodynamic forces for rotational motion. $L_{DXu}, L_{DYv}$, and $L_{DZw}$ are the damping coefficients in the translational directions, while $L_{DKp}, L_{DMq}$, and $L_{DNr}$ are the damping coefficients in the rotational directions. $Q_{DXu}, Q_{DYv}$, and $Q_{DZw}$ are the acceleration damping coefficients in the translational directions, and $Q_{DKp}, Q_{DMq}$, and $Q_{DNr}$ are the acceleration damping coefficients in the rotational directions.

4.1. Static Obstacle Avoidance and Path Planning

For the evaluation, we set the initial point as a fixed value (0, 0, 0) and consider four destination points: $(10,10,10),(10,10,9),(10,9,10)$, and $(9,10,10)$. In our test scenario, we introduce random spheres as obstacles, each with a unique position and size. Specifically, the obstacle positions are $(2,5,3),(2,2,2),(8,8,8),(6,6,6),(1,7,4)$, and $(3,7,7)$, accompanied by corresponding radii of $0.5,1,1,0.6,0.6$, and $0.5$, respectively.

The IIFDS parameters $\rho =1.5$, $\sigma =0.5$, and $\theta =0.5$ were selected. It has been noted in the literature [7] that the A* method is frequently employed for path planning. To evaluate their performance in the aforementioned environment, both the A* and IIFDS algorithms were tested.

From

Table 2. Performance comparison of different algorithms in static obstacle scenarios, and all bolded data represents extreme values.

Destination	Time (s)		Length (m)		Mean Direction Angle (deg)		Max Direction Angle (deg)
	IIFDS	A*	IIFDS	A*	IIFDS	A*	IIFDS	A*
(10,10,10)	1.014391	131.837413	18.565024	19.127552	0.543250	8.4375	1.867074	49.867805
(10,10,9)	0.573924	346.934560	17.854283	19.102506	0.562810	15.309670	1.781431	49.867805
(10,9,10)	0.672776	36.556171	17.743238	18.147479	0.489524	9.000000	1.845807	49.867805
(9,10,10)	0.568327	45.466717	18.192898	18.214942	0.702289	12.649041	2.781137	67.500000

, it is evident that for the same starting and target points, the A* algorithm exhibits longer path planning times and generates longer paths. This observation highlights the advantages of the IIFDS algorithm over the A* algorithm in terms of faster response times and shorter planned paths. Particularly, when there are direct obstacles along the path, the execution time for the A* algorithm in path planning can increase significantly. For instance, when considering target points such as $(10,10,9)$, both algorithms yield similar path lengths of approximately 17.854283 m and 19.102506 m, respectively. However, the time taken differs significantly, A* requiring 346.934560 s while IIFDS only needs 0.573924 s. This indicates a substantial difference of approximately 605×. When considering the four destination points, the length of the planned paths does not differ significantly. However, there is a significant difference in the time consumption.

From Figure 10 and Table 2, it is evident that for all anticipated planned paths, the A* algorithm exhibits larger turning angles and more pronounced changes in direction. These large turning angles can lead to severe control oscillations, thereby significantly impacting the stability of the control system. Furthermore, such abrupt fluctuations can also cause damage to the actuation mechanism. On the other hand, the IIFDS path planning algorithm generates smoother and more seamless paths, which are characterized by fewer abrupt changes in direction.

Based on the observations from Figure 10 and Table 2, the following conclusions can be drawn. The IIFDS path planning algorithm exhibits better real-time performance, as it generates shorter paths with higher smoothness. These characteristics make it more suitable for real-time control applications.

4.2. Dynamic Obstacle Avoidance and Path Planning

The IIFDS algorithm is designed to address dynamic obstacles by enabling the real-time monitoring of their positions and velocities. Based on its own position, the algorithm plans reference waypoints for the next control cycle. Different IIFDS parameters yield varying results in terms of path length and obstacle avoidance effectiveness. Therefore, in this scenario with the same start/end points and dynamic obstacles, different IIFDS parameters are set to assess their impact on path planning. This analysis aims to identify the most suitable parameters for practical obstacle avoidance environments. The start and end coordinates remain as $(0,0,0)$ and $(10,10,10)$, respectively.

The dynamic obstacle environment consists of three spherical obstacles located initially at $(5,5,7.5)$, $(3,2,6)$, and $(8,8.5,8.5)$, with radii of $(0.5,0.4,0.5)$, respectively. These obstacles move at a linear velocity of 2 m/s along the x-axis with cosine direction variation and 1 m/s along the y-axis with sine direction variation, while maintaining a fixed position along the z-axis. The IIFDS path planning controller is configured with five different parameter sets, as in

Table 3. Different IIFDS parameters for dynamic obstacles scenario of path planning.

Parameters	Dynamic Coefficient $\mathit{\lambda }$	Reaction Coefficient $\mathit{\rho }$	Reaction Coefficient $\mathit{\sigma }$	Tangential Reaction Coefficient $\mathit{\theta }$
Path1	5	1.5	0.5	0.1
Path2	15	1.5	0.5	0.1
Path3	5	0.5	0.5	0.1
Path4	5	1.5	1.5	0.1
Path5	5	1.5	0.5	0.5

. Each of the aforementioned parameter sets produces a distinct planned path (path1, path2, path3, path4, path5).

Based on Figure 11 and

Table 4. Performance comparison of different algorithms in dynamic obstacle scenarios, and all bolded data represents extreme values.

Parameters	Length (m)	Time (s)	Mean Direction Angle (deg)	Max Direction Angle (deg)
$\left(Path1\right)\lambda =5,\rho =1.5,\sigma =0.5,\theta =0.1$	17.488965	31.574731	0.736420	9.629480
$\left(Path2\right)\mathit{\lambda }\mathbf{=}\mathbf{15},\rho =1.5,\sigma =0.5,\theta =0.1$	17.512065	31.236525	0.867715	9.679838
$\left(Path3\right)\lambda =5,\mathit{\rho }\mathbf{=}\mathbf{0}\mathbf{.}\mathbf{5},\sigma =0.5,\theta =0.1$	17.334920	24.268841	0.161700	1.200126
$\left(Path4\right)\lambda =5,\rho =1.5,\mathit{\sigma }\mathbf{=}\mathbf{1}\mathbf{.}\mathbf{5},\theta =0.1$	19.144417	31.804764	0.965932	6.882223
$\left(Path5\right)\lambda =5,\rho =1.5,\sigma =0.5,\mathit{\theta }\mathbf{=}\mathbf{0}\mathbf{.}\mathbf{5}$	17.480835	30.959117	0.753329	9.309696

, it is evident that among the five parameter sets, path3 stands out with the shortest path length of 17.334920 m. With the exception of path4, which experiences a substantial increase in length, the path lengths of the remaining four sets are approximately equal. Thus, it can be inferred that the parameter $\sigma$ significantly affects the path length, whereas the other parameters have a relatively minor influence on it. Regarding planning time, path3 has the shortest duration, $24.268841$ s, suggesting that the parameter $\rho$ has a significant effect on the planning time. Furthermore, as the parameter value decreases, the planning time decreases accordingly. The path labeled “path3” exhibits both the lowest average angle, $0.1617$ deg, and the smallest maximum angle, $1.200126$ deg, of direction. Referring to Figure 11, it is evident that a significant alteration occurs in the path when the reaction coefficient shifts from $0.5$ to $1.5$. The change in this particular parameter is much more pronounced compared to the three other parameters, which also underwent a threefold change. Hence, it becomes apparent that a more gradual adjustment is necessary for this parameter.

4.3. Parameter Selection and Trajectory Tracking

From the paths planned in the above scenarios, a typical path is selected correspondingly to use the NMPC controller for tracking control, to test the influence of various variable parameters on the tracking effect. The NMPC is utilized for tracking planned paths in environments with both dynamic and static obstacles. The cost function is designed to incorporate various objectives, including position deviation, velocity deviation, and control input magnitude. Different weight parameters are set to compare the tracking performance.

Moreover, the prediction horizon directly influences the performance of the control system and computational burden. The open-loop sampling time is generally required to cover the transient response of the open-loop system. According to Figure 9 in the referenced literature [30], the open-loop response time of the BlueRov2 system is approximately 5 steps.

According to the MATLAB interpretation [31], providing a prediction horizon of 1–2 times the open-loop response time is deemed appropriate. Simulation results indicate that the computational time for 10 prediction horizons increases by nearly 2 times, but the performance remains essentially unchanged; it is suitable to set the horizon to $N=5$ for all scenarios.

The initial state $x_0={\mathbf{0}}_{12*1}$, the initial control input is $u={\mathbf{0}}_{8*5}$, in which 5 represents the prediction horizon, the upper and lower bounds for the control input are set to 40 and $-40$ respectively, and the state variable is constrained within the range of 0 to 10. According to reference [20] and the Equation (40), it is evident that a higher weight imposes a stronger constraint on the corresponding variable, resulting in a smaller range of variation. Therefore, by setting different numerical values, the impact of various weights on the simulation results is compared, and an appropriate weight is chosen as an experimental parameter.

Case 1. 

Different weight parameters for velocity and drive force.

Different weight coefficients ${\omega }_2$, ${\omega }_3$ are selected to compare the variations in velocity and control inputs during the path tracking process. The evaluation metrics include maximum values and stability.

Here, the planning path IIFDS path3 is chosen as the reference path. The velocity weight matrix ${\omega }_2$ is set to ${\mathbf{90}}_{6*1}$, ${\mathbf{30}}_{6*1}$, and ${\mathbf{10}}_{6*1}$, while the control input weight matrix is set to ${\mathbf{0}.\mathbf{2}}_{6*1}$, ${\mathbf{1}}_{6*1}$ and ${\mathbf{5}}_{6*1}$, respectively. These weight ranges are selected based on reference [20] and empirical knowledge.

Figure 12 illustrates the impact of different weights on velocity. It is evident that when ${\omega }_2={\mathbf{90}}_{6*1}$, the velocities u and r are minimized, but other velocity components, particularly v, exhibit significant fluctuations. In contrast, with a weight coefficient of ${\omega }_2={\mathbf{10}}_{6*1}$, the constraints on velocity are relatively lax, often resulting in abrupt transitions from one extreme to another. Therefore, among the three parameters mentioned, ${\omega }_2={\mathbf{30}}_{6*1}$ demonstrates the most stable behavior.

Figure 13 depicts the influence of weights on driving force. For ${\omega }_3={\mathbf{0}.\mathbf{2}}_{6*1}$, the driving forces experience minimal constraints, leading to both substantial fluctuations and frequent reaching of limit values. In the case of ${\omega }_3={\mathbf{5}}_{6*1}$, the driving forces are subject to maximum constraints, yet it is noteworthy that both the minimum and maximum driving forces occur under this parameter scenario among the three weights. It is observed that at ${\omega }_3={\mathbf{1}}_{6*1}$, all driving forces exhibit relatively smooth control processes.

Case 2. 

Different weight parameters for tracking the path planned using A*.

Among them, the static path is selected to track the path towards the target point $(10,10,10)$ using A*. As shown in

Table 5. Different NMPC parameters for trajectory tracking.

Path	Parameters	Mean Variance (m) A*/IIFDS	Max Variance (m) A*/IIFDS
path1	${\omega }_1={\mathbf{2000}}_{6*1}$	0.01384/0.01386	0.068/0.058
	${\omega }_2={\mathbf{30}}_{6*1}$
	${\omega }_3={\mathbf{1}}_{8*1}$
path2	${\omega }_1={\mathbf{1000}}_{6*1}$	0.04752/0.01586	0.3867/0.072
	${\omega }_2={\mathbf{30}}_{6*1}$
	${\omega }_3={\mathbf{1}}_{8*1}$
path3	${\omega }_1={\mathbf{400}}_{6*1}$	0.18369/0.01628	0.9737/0.082
	${\omega }_2={\mathbf{30}}_{6*1}$
	${\omega }_3={\mathbf{1}}_{8*1}$
path4	${\omega }_1={\mathbf{50}}_{6*1}$	0.4282/0.01725	1.429/0.087
	${\omega }_2={\mathbf{30}}_{6*1}$
	${\omega }_3={\mathbf{1}}_{8*1}$

and Figure 14, it can be observed that for different control parameters, the average path deviation is minimized when ${\omega }_1$ is set to 2000, with a value of $0.01384$ m. The maximum path tracking deviation is $0.068$ m. As the ${\omega }_1$ value decreases, the tracking performance deteriorates. Among the paths, path4 exhibits the worst tracking performance, with a path tracking deviation of $0.4282$ m when ${\omega }_1$ is set to 50. The maximum path tracking deviation for this case is $1.429$ m. Furthermore, from the four tracked paths, it is evident that NMPC has the ability to anticipate paths with larger turning angles and generate smoother angular trajectories. From Figure 15, it can be observed that the position and orientation stabilize around the 13th s. Similarly, from Figure 16, the velocity and angular velocity also stabilize around the 13th s. Furthermore, as shown in Figure 17, the propulsive forces reach stability around the 13th s. All three variables exhibit mutual consistency. Additionally, the eight driving force variables remain stable between −40 and 20 throughout the execution process, consistently within the range of positive and negative 40.

Case 3. 

Different weight parameters for tracking the path planned by IIFDS.

The path for tracking the dynamic obstacle was selected from Table 3, specifically path3. Different ${\omega }_1,{\omega }_2,{\omega }_3$ parameters were chosen from Table 5 to track the path. The tracking results can be seen in Figure 18 and Table 5. It is evident that the parameter ${\omega }_1$ has a negligible impact on the tracking performance.

We selected the best-performing set of tracking results from all the aforementioned paths and observed the various state variables and control quantities during the tracking process. From Figure 15 and Figure 19, it can be observed that the position exhibits a nearly linear variation during the tracking process. The attitude quickly adjusts to the desired angle at the beginning of the tracking process and remains relatively stable throughout. Similarly, as seen in Figure 16 and Figure 20, the velocity and angular velocity rapidly adjust to their operating values and maintain constant values. Upon approaching the vicinity of the target point, they quickly return to zero. As seen in Figure 17 and Figure 21, the thrust force, constrained by a maximum value, initially undergoes several adjustments to reach the maximum value of 40 N and then swiftly returns to a constant value [32]. After reaching the target point, a reverse force may appear to adjust the attitude, which aligns with the control logic.

Considering the overall situation, a larger weight coefficient imposes greater constraints on the corresponding quantity. However, a balance needs to be struck between the magnitude and stability of the corresponding quantity. Based on the above description, the parameter ${\omega }_1=2000$ exhibits excellent performance in path tracking. The weight coefficients ${\omega }_1={\mathbf{2000}}_{6*1}$, ${\omega }_2={\mathbf{30}}_{6*1}$, and ${\omega }_3={\mathbf{1}}_{6*1}$ represent a relatively well-performing set of weights.

5. Experimental Results

To validate the effectiveness of the designed IIFDS-NMPC control framework in practical applications, we conducted the following test scenario. In a water tank measuring 10 m long, 3 m wide, and 1.2 m deep, six obstacles were randomly placed. Among them, four were static obstacles, while the other two were dynamic obstacles moving in a straight line at a velocity of $v_{obs}$ = 1 m/s. All obstacles are considered to be spheres with a radius of $0.25$ m. The positioning of the AUV and obstacles was carried out using the approach described in [33], which combines image- and radar-based localization systems. The positioning system and the AUV system were independent systems that communicated with each other through a wireless serial port connection.

The AUV’s main structure followed that of BlueRov2-heavy, with the main control unit replaced by a self-designed control board based on STM32F407 and NVIDIA NX2.

The central processor in NX2 is a 6-core NVIDIA CarmelArmV 64-bit CPU. An STM32 unit is responsible for gathering all external environmental information and transmitting it to the CPU. The CPU, based on this information, executes the hybrid layer control structure designed in this paper, namely the IIFDS-NMPC control flow. The Apollo platform incorporates three solvers (SQP, Interior Point OPTimizer (IPOT), Operator Splitting Quadratic Programming (OSQP)), with OSQP as the default choice. This preference is driven by its faster solution-finding capabilities, as it does not account for curvature constraints. However, a significant drawback is its inability to guarantee curvature continuity for smoothed points. As a result, the AUV system seeks a compromise between computation time and accuracy, choosing the SQP solver. Utilizing the built-in optimal solver SQP in Python + Pyomo to solve the optimal control with constraints, it generates control commands and sends the first set of control commands to the STM32 interface board. Subsequently, the interface board forwards the control commands downstream to the drive unit. In this process, the STM32 serves as the interface board, interacting with all peripherals and transmitting data to the processor. Meanwhile, the role of the NVIDIA NX2 is to perform path planning and calculate the optimal control quantities for NMPC. The hardware architecture diagram is shown in Figure 22, and the optimization algorithm for NMPC used to obtain control signals and the IIFDS were implemented as in Figure 23. The AUV was powered by batteries during the tests, and the control operated at a frequency of 100 Hz.

As the center of the AUV is located at a depth of 0.5 m, the obstacles are set at a fixed depth of 0.5 m, and the obstacle avoidance parameter $\theta$ for IIFDS was set to 0. The other parameters were set according to the optimal simulation parameters for path3 in Table 3, with $\lambda =5$, $\rho =0.5$, and $\sigma =0.5$. The weight parameters for NMPC were set according to path1 in Table 5, with ${\omega }_1={\mathbf{2000}}_{6*1}$, ${\omega }_2={\mathbf{30}}_{6*1}$, and ${\omega }_3={\mathbf{1}}_{8*1}$. The choice in prediction horizon ($N=5$) has a crucial impact on the actual control cycle in practical applications. Therefore, through comparative measurements, a prediction horizon of $N=5$ was determined.

The starting point was $(0,0,0)$, and the target point relative to the starting point had coordinates of $(8,2,0)$. The initial speed of the AUV was 1 m/s. The actual path planning and tracking results are shown in

Table 6. Real navigation and control result.

Path Tracking Deviation RMS (m)	Average Computation Period (s)
0.01867	0.0063

and Figure 24, respectively.

Figure 25 illustrates the position and orientation measurements, which reach the target position around the 9th s. According to the collected data, the RMS of the path tracking is calculated to be 0.01867 m. Additionally, based on feedback from the microcontroller unit (MCU), the actual average computation period is determined to be 0.0063 s, which significantly satisfies the control frequency requirement of 100 Hz. Moreover, as the number of obstacles increases, the theoretical computation period is expected to further increase, providing additional redundancy for obstacle calculations. From Figure 26 and Figure 27, it can be observed that the velocity and propulsive force also reach stable values around the 9th s. The orientation measurements, velocity measurements, and propulsive forces are synchronized in time. Furthermore, the propulsive forces remain within the range of positive and negative 40 N, while the torque stays within the range of positive and negative 160 N, complying with the actual constraints of the driver. The actual experimental video can be obtained through the following link: https://youtu.be/TyMuQIc91a8 accessed on 7 October 2023.

6. Conclusions

In this study, we proposed a dual-layer control strategy, IIFDS-NMPC, for path planning and tracking control in complex obstacle environments. The upper layer utilized IIFDS for path planning, while the lower layer employed NMPC for trajectory tracking. By simulating dynamic and static obstacle environments, we selected the planning parameters for IIFDS to generate paths that are as short as possible while minimizing the time taken, and the planned path was the smoothest. NMPC weights have a direct impact on the tracking deviation and control command during the tracking process. Through comparative experiments, the selected parameters are ${\omega }_1=2000$, ${\omega }_2=30$, and ${\omega }_3=1$, ensuring the path tracking followed the planned trajectory as closely as possible, while keeping the control and state variables within their respective limits. Through real platform testing experiments using the BlueRov2, the designed dual-layer control strategy fulfilled the maximum control requirement of 100 Hz, and the path tracking accuracy was within a range of $0.02$ m. This control strategy meets the requirements for practical applications.

Although the path planning strategy, IIFDS, attempted several fixed measurements that proved suitable for the application, the parameters of this planner remained unchanged throughout the entire planning process and did not consider dynamic adjustments based on the AUV’s position and the changing external obstacle environment. These parameters are not optimal. Therefore, future research will focus on dynamically adjusting the optimal parameters.