An Obstacle Avoidance Strategy for AUV Based on State-Tracking Collision Detection and Improved Artificial Potential Field

Abstract

This paper proposes a fusion algorithm based on state-tracking collision detection and the simulated annealing potential field (SCD-SAPF) to address the challenges of obstacle avoidance for autonomous underwater vehicles (AUVs) in dynamic environments. Navigating AUVs in complex underwater environments requires robust autonomous obstacle avoidance capabilities. The SCD-SAPF algorithm aims to accurately assess collision risks and efficiently plan avoidance trajectories. The algorithm introduces an SCD model for proactive collision risk assessment, predicting collision risks between AUVs and dynamic obstacles. Additionally, it proposes a simulated annealing (SA) algorithm to optimize trajectory planning in a simulated annealing potential field (SAPF), integrating the SCD model with the SAPF algorithm to guide AUVs in obstacle avoidance by generating optimal heading and velocity outputs. Extensive simulation experiments demonstrate the effectiveness and robustness of the algorithm in various dynamic scenarios, enabling the early avoidance of dynamic obstacles and outperforming traditional methods. This research provides an accurate collision risk assessment and efficient obstacle avoidance trajectory planning, offering an innovative approach to the field of underwater robotics and supporting the enhancement of AUV autonomy and reliability in practical applications.

1. Introduction

1.1. Background

In recent years, autonomous underwater vehicles (AUVs) have garnered increasing attention as a pivotal force in underwater exploration and operations. In the military domain, AUVs can be employed for tracking and monitoring enemy activities, providing real-time intelligence, or serving as reconnaissance and guidance systems for underwater attack weapons. In underwater archaeology and rescue operations, AUVs can track or search for individuals or objects beneath the water, facilitating precise rescue or recovery missions. However, for AUVs, tracking randomly moving targets underwater remains a challenging task. The autonomous obstacle avoidance (OA) capability is still insufficient, and the dynamic nature of moving targets imposes a greater workload on the guidance, navigation, and control (GN&C) system, rendering some traditional OA methods ineffective. Additionally, external environmental interference further compounds the challenges. Consequently, the design of dynamic obstacle avoidance (DOA) algorithms for AUVs during the voyage requires increased attention, holding significant importance in relevant industries.

1.2. Literature Review

1.2.1. Dynamic Obstacle Avoidance

In dynamic and uncertain underwater environments, it is difficult or even impossible to obtain information on various obstacles before path planning. In this case, there is usually a path consisting of planned waypoints from a global path planner. However, AUVs still require an obstacle avoidance algorithm to navigate around potential obstacles along the planned trajectory. Several solutions are available for AUV DOA, including rapidly exploring random trees (RRTs) [1], fuzzy logic [2], the neural network (NN), reinforcement learning (RL), deep reinforcement learning (DRL) [3], and the artificial potential field (APF) [4,5,6]. The RRT algorithm exhibits a robust capability for detecting unknown obstacles and is well-suited for addressing obstacle avoidance issues in high-dimensional environments; however, its real-time performance is relatively poor [7,8]. The fuzzy logic algorithm is characterized by a strong robustness and good real-time performance, as it does not require precise mathematical models. However, it heavily relies on expert knowledge and may not perform well in complex and dynamic environments. The neural network (NN), reinforcement learning (RL), and deep reinforcement learning (DRL) algorithms possess powerful learning and decision-making capabilities, enabling them to plan reasonable obstacle avoidance paths in various complex and uncertain environments. However, they typically require millions of samples for learning to generate the optimal obstacle avoidance paths. This practical limitation makes it challenging to apply directly to real AUV platforms. The AUV’s DOA methods should exhibit high efficiency, strong real-time performance, low processor demands, and adaptability to complex environments. Compared to the aforementioned algorithms, the APF approach has been increasingly implemented for its simplicity in implementation and use, effectiveness in handling static and dynamic constraints, and minimal processing requirements [9]. Therefore, the selection of the APF algorithm is made to address the DOA issues during the target-tracking process.

1.2.2. Artificial Potential Field Method

The main objective of the APF-based approach is to use a virtual potential field to describe the motivation of the AUV [10]. The approach involves establishing an attractive force from the target to the robot and a repulsive force from obstacles to the robot. The robot’s motion is influenced by the combined potential field forces of attraction and repulsion. Finally, by selecting appropriate potential field functions, a motion path capable of both tracking the target and avoiding obstacles is planned. The obstacle avoidance method based on the APF in a static environment is relatively mature due to its simplicity, speed, and ease of implementation, while dynamic obstacle avoidance algorithms in complex environments are not as mature. Currently, the research on DOA methods based on the APF primarily focuses on optimizing traditional APF methods and addressing issues such as local minimum problems; improvement strategies for the APF mainly involve combining it with other algorithms [11,12,13]. Over the past three years, several DOA solutions based on the APF have been proposed, which are summarized in

Table 1. The comparison of different OA methods based on the APF.

Control Plant	2D/3D Environment	Improvement	Simulation/Experiment	Reference
Underwater Submarines	3D	Reduce the number and length of inflection points	Simulation	[11]
Underwater Snake-like Robot	2D	Virtual target points	Simulation	[12]
AUV	2D	A* plus APF	Simulation	[13]
AUV	3D	Collision detection mechanism	Simulation	[14]
Smart Ship	2D	Assessing collision risk	Experiment	[15]
Wave Glider	2D	Solve local minimum, obstacle prediction	Simulation	[16]
Wave Glider	2D	Solve local minimum, collision prediction	Simulation	[17]

. A collision detection mechanism based on geometric methods is proposed. Building upon this, an OA strategy based on the vector APF is introduced; sudden obstacles can be avoided, but mechanical constraints of the AUV are not considered [14]. Ref. [15] employs a method for assessing the collision risk by examining whether the extension of the relative velocity vector between the vessel and obstacles intersects with the repulsion influence range of obstacles. Subsequent decisions regarding obstacle avoidance are then executed. Ref. [16] introduces a time-stamped collision detection model, treating the motion of obstacles as a singular model. The extended Kalman filter method is utilized to predict motion, and a collision risk index is computed at various time instances. If a collision risk is identified, the repulsive force of dynamic obstacles is applied to the aircraft. Ref. [17] proposes an innovative collision prediction model and a fusion algorithm for dynamic obstacle avoidance, denoted as the collision prediction model—improved artificial potential field (CPM-IAPF). This methodology accomplishes obstacle avoidance by determining the necessary heading angles. However, it assumes that dynamic obstacles adhere to a constant velocity motion model, potentially resulting in ineffective obstacle avoidance behaviors. The existing DOA algorithms based on the APF are primarily designed to work under the assumption of known dynamic obstacle motion states. These algorithms often do not consider the impact of varying obstacle motion states on algorithm performance, and their robustness needs further validation.

1.3. Motivation

To address the shortcomings in AUV obstacle avoidance within complex dynamic environments, this paper proposes an innovative avoidance strategy. For the prediction of dynamic obstacles, an improved Gauss (I-Gauss) motion model is introduced to accurately track the motion states of obstacles and the AUV. The Kalman filter (KF) method is employed to predict the trajectories of obstacles. Subsequently, a collision risk zone is established based on the predicted covariance, forming the basis for the collision detection model. Taking into account the constraints of both obstacles and the AUV motion model, a rational objective function is devised. The SA algorithm is then utilized to optimize the objective function for making obstacle avoidance decisions.

The main contributions of this paper are as follows: (1) The incorporation of obstacle motion states into collision detection through the establishment of the SCD model. This enables faster obstacle avoidance actions, addressing the challenges of obstacle avoidance resulting from the limited maneuverability of AUVs. (2) The introduction of the I-Gauss motion model, enhancing the accuracy of motion state predictions. (3) The integration of the SA algorithm into obstacle avoidance, resulting in the development of the SAPF algorithm. This ensures the efficiency of obstacle avoidance paths and resolves issues related to local minima.

The rest of the paper is organized as follows: The DOA problem formulation of the AUV and the mathematical model is given in Section 2. The proposed SCD-SAPF is described in Section 3. The numerical simulations are given in Section 4 to illustrate the effectiveness and efficiency of the DOA strategy. The conclusions and discussion are given in Section 5.

2. Problem Statement

The underwater operational environment for AUVs is typically complex and contains obstacles. To achieve the precise avoidance of dynamic obstacles, it is necessary to assess the risk of collision. The common approach involves using sensors such as sonar to continuously acquire real-time information about the positions and motion states of surrounding obstacles. The motion of obstacles is modeled, and their trajectories are predicted to assess the risk of collision. However, observational errors in sensors can introduce inaccuracies in the estimation of obstacle states. Additionally, AUVs operate without a tether connection to the mother ship, relying on their battery-powered energy. They are designed for relatively low cruising speeds, typically ranging from 0~5 kn. In summary, DOA for AUVs faces two main challenges: (1) The unique characteristics of underwater operational environments result in a multitude of complex noises affecting AUV sensory systems, making it challenging to obtain accurate estimates of obstacles. (2) DOA demands the real-time planning of collision-free paths for AUVs to follow. The limited energy, slow speed, and insufficient maneuverability of AUVs pose challenges leading to obstacle avoidance failures.

To address the aforementioned challenges, the SCD-SAPF dynamic obstacle avoidance algorithm is presented, as illustrated in the overall framework shown in Figure 1. The process involves first obtaining the state information of the AUV (including its position, velocity, and acceleration). The AUVs’ onboard perception system is used to acquire measurements of dynamic obstacles (primarily focusing on position information). The established state-tracking system is then employed to predict the motion states of both obstacles and the AUV. The developed SCD model is used to assess the presence of collision risks. In the event of a collision risk, the improved SAPF algorithm is employed to calculate the required heading angle and speed for obstacle avoidance. Otherwise, the system maintains its current state. Finally, the generated signal is transmitted to the controller to execute the action.

The OA task for the AUV is achieved through the connection with the heading and velocity controllers. As illustrated in Figure 2, underwater disturbances directly affect the motion of both the AUV and dynamic obstacles, resulting in a resultant force in a certain direction. This force is incorporated as part of the motion state input into the state prediction system and OA algorithm, enabling an accurate trajectory prediction and collision risk assessment. The desired heading angles and velocities are obtained through the OA algorithm. However, due to the dynamic changes in the environment and the inherent dynamic characteristics of the robot, deviations in actual motion may occur. Therefore, the control system continually monitors the robot’s motion state and adjusts the desired values based on feedback information to ensure safe obstacle avoidance operations in dynamic environments.

During actual underwater operations, AUVs commonly rely on multi-beam forward-looking sonar sensors, which typically operate within a wide horizontally and narrow vertically conical detection range. On one hand, the sonar’s limited vertical angle results in less reliable three-dimensional information, and the bulky size of 3D forward-looking sonars demands high hardware, software, and signal processing requirements. Consequently, AUVs often resort to using 2D forward-looking sonars for practical obstacle avoidance. On the other hand, given that AUVs primarily conduct underwater tasks such as oceanographic data collection (including flow velocity, temperature, salinity, etc.) at fixed depths, the movement of AUVs along the horizontal plane at fixed depths far exceeds their movement in the vertical direction. Hence, designing horizontal dynamic obstacle avoidance algorithms for AUVs can reduce hardware demands during underwater operations and facilitate the early avoidance of moving obstacles. The obstacle avoidance algorithm designed in this paper and the scenarios are based on planar motion. It assumes the existence of an AUV control system capable of automatically executing velocity and heading commands. Using Newtonian and Lagrangian mechanics [18], AUVs’ depth-fixed kinematics and dynamics model employed in this paper is as follows:

$$\left\{\begin{array}{c}\dot{x}=Vcos\psi \\ \dot{y}=Vsin\psi \\ \dot{V}=-\frac{1}{{\tau }_v}V\pm \frac{1}{{\tau }_v}{V}_C\\ \dot{\psi }=-\frac{1}{{\tau }_{\psi }}\psi \pm \frac{1}{{\tau }_{\psi }}{\psi }_C\end{array}\right.$$

(1)

$$\left\{\begin{array}{c}\dot{u}-rv=\frac{X}{m}\\ \dot{v}+ru=\frac{Y}{m}\\ \dot{r}=\frac{N}{I_z}\end{array}\right.$$

(2)

where $x,y,\psi ,V$ denote the position, yaw angle, and velocity of the AUV in the earth-fixed co-ordinate system; ${\psi }_C,V_C$ denote the desired yaw angle and velocity; $\dot{\psi },\dot{V}$ denote the angular velocity and acceleration of the AUV; and ${\tau }_{\psi },{\tau }_V$ denote the control delay to achieve the desired speed and heading angle. $u,v,r$ denote the vessel velocities in the surge, sway and yaw, respectively; $m,X,Y,N$ denote the mass of the AUV and the external forces and moments received in the x, y, and z directions, all considered as unknown parameters, need to be specified within the specific environmental context. It should be pointed out that the external forces acting on the AUV include gravity, buoyancy, and fluid resistance, among others; adjusting the velocity and direction of the AUV dynamically in response to external environmental disturbances can simulate the principle of dynamic response to external forces.

The purpose of this constraint is to control the yaw angular velocity of the AUV during navigation, allowing the AUV to flexibly maintain an appropriate relative position with the target. This ensures the continuous monitoring and tracking of the target. Such a strategy aims to optimize the obstacle avoidance behavior while guaranteeing that the tracked target always stays within the effective observation range of the underwater sonar.

Note that the tracked target remains within the observation range during obstacle avoidance. Therefore, the angular velocity of the bow heading rotation of the AUV must be maintained within a finite range. Figure 3 illustrates the observation range of the forward-looking sonar. To ensure that the tracked target remains within the sonar observation range, the constraints, as Equation (3), are imposed on the AUV’s yaw angular velocity.

$${\psi }_2-\frac{\pi +\mathsf{\alpha }}{2}<\dot{\psi }<{\psi }_1-\frac{\pi -\mathsf{\alpha }}{2}$$

(3)

Here, $X_t$ and $X_{t+1}$ represent the state information of the target at the current and next time steps, $\mathsf{\alpha }$ represents the field of view, and ${\psi }_1$, ${\psi }_2$ represent the angle between the target and baseline.

3. Obstacle Avoidance Based on the SCD-SAPF

3.1. Traditional Artificial Field

The fundamental idea of the traditional artificial potential field (TAPF) method is to use virtual potential fields to describe the robots’ motion in the surrounding environment. The target point generates an abstract attraction to the robot, while obstacles generate an abstract repulsion. The robot moves at a constant speed in the direction of the resulting force. The principle of the APF is illustrated in Figure 4, and its force field and repulsive field functions are defined as follows:

$$U_{att}=\frac{1}{2}k{\rho }^2(x,x_g)$$

(4)

$$U_{rep}=\left\{\begin{array}{ll}\frac{1}{2}m{(\frac{1}{\rho (x,x_0)}-\frac{1}{{\rho }_0})}^2,& \rho (x,x_0)\le {\rho }_0\\ 0,& \rho (x,x_0)>{\rho }_0\end{array}\right.$$

(5)

where $U_{att}$ and $U_{rep}$ denote the attraction field and repulsion field functions; $k$ and $m$ denote the attraction field coefficient and repulsion field coefficient, respectively; and $x$, $x_g$, and $x_0$ denote the current position of the robot, target, and the obstacle. $\rho (x,x_i)$ denotes the Euclidean distance between the robot and the obstacle, and ${\rho }_0$ denotes the influence range of the obstacle. By calculating the negative gradient of the potential field function, the attraction and repulsion forces can be determined as follows:

$$F_{att}=-k\rho (x,x_g)(x_g-x)$$

(6)

$$F_{rep}=\left\{\begin{array}{ll}m(\frac{1}{\rho (x,x_o)}-\frac{1}{{\rho }_0})\frac{1}{{\rho }^2(x,x_o)}(x_o-x),& \rho (x,x_o)\le {\rho }_0\\ 0,& \rho (x,x_o)>{\rho }_0\end{array}\right.$$

(7)

The resultant force acting on the robot is given by:

$$F_{all}=F_{att}+F_{rep}$$

(8)

3.2. Dynamic Obstacle Collision Detection

3.2.1. State Tracking

To estimate the collision risk, it is necessary to obtain the state estimation of the AUV and dynamic obstacles through state tracking. Currently, almost all state tracking algorithms rely on models for the state estimation, and a good model is worth a large amount of data [19]. However, a significant challenge in target state tracking is the mismatch between the actual motion states of targets and the tracking motion models. Widely used models include the constant velocity (CV) model, the constant acceleration (CA) model, and the Singer model [20]. Ref. [21] proposed an adaptive Gauss model based on the Singer model and conducted theoretical analysis and simulations, demonstrating its excellent tracking performance for underwater targets with different motion patterns. However, this model still faces challenges: due to the constraints of the underwater environment, most underwater targets exhibit weak maneuvering states, with small values for the velocity, acceleration, and acceleration variance. When the targets’ motion state changes significantly, the small acceleration variance causes the model to fail to promptly match the targets’ motion. To achieve an accurate state estimation, fuzzy reasoning methods and the interacting multiple model (IMM) algorithms are introduced to design an improved model.

1.

Adaptive Gauss model;

The equations for the target’s motion state and observation are as follows:

$$\mathit{x}\left(k+1\right)=\mathsf{\Phi }\left(k\right)\mathit{x}\left(k\right)+\mathit{G}\left(k\right)\mathit{w}\left(k\right)$$

(9)

$$\mathit{z}\left(k\right)=\mathit{H}\left(k\right)\mathit{x}\left(k\right)+\mathit{v}\left(k\right)$$

(10)

where $\mathit{x}\left(k\right)$ is the state vector, $\mathit{z}\left(k\right)$ is the observation vector, $\mathsf{\Phi }\left(k\right)$ and $\mathit{H}\left(k\right)$ are the state and observation transition matrices, $\mathit{G}\left(k\right)$ is the observation matrix, and $\mathit{w}\left(k\right)$ and $\mathit{v}\left(k\right)$ are the state and observation noise covariance matrices, respectively.

The adaptive Gauss model [21] characterizes target maneuvering as follows:

$$\left\{\begin{array}{c}\ddot{x}\left(t\right)=\overline{a}+a\left(t\right)\\ \dot{a}\left(t\right)=-\alpha a\left(t\right)+\omega \left(t\right)\end{array}\right.$$

(11)

where $\ddot{x}\left(t\right)$ represents the targets’ maneuvering acceleration; $\overline{a}$ denotes the mean acceleration; $a\left(t\right)$ denotes zero-mean colored acceleration noise with exponential decay; $\alpha$ denotes the target maneuvering frequency; $\omega \left(t\right)$ denotes input white noise with variance ${\sigma }_{\omega }^2=2\alpha {\sigma }_a^2$; and ${\sigma }_a^2$ is the acceleration variance.

The model assumes that the acceleration $\ddot{x}\left(t\right)$ follows a Gaussian distribution with a probability density function given by:

$$p\left(\ddot{x}\left(t\right)\right)=\frac{1}{\sqrt{2\pi }{\sigma }_a}exp\left(-\frac{{\left[\ddot{x}\left(t\right)-\overline{a}\right]}^2}{2{\sigma }_a^2}\right)$$

(12)

The mean of the target’s maneuvering acceleration $\overline{a}$ is taken as the optimal estimate of the state variable $\ddot{x}\left(\mathrm{t}\right)$. The variance of the Gaussian distribution is

$${\sigma }_a^2=\frac{{\left|\dot{\widehat{\ddot{x}}}(t)\right|}^2}{b^2}$$

(13)

where $\left|\dot{\widehat{\ddot{x}}}\right|$ represents the derivative of the optimal estimate of the target acceleration $\ddot{x}\left(t\right)$, and $b$ is the adaptive coefficient for the acceleration variance, which takes a constant value.

Equations (11)–(13) together form the adaptive Gauss model.

2.

Improved Gauss model;

The acceleration variance in the adaptive Gaussian model Equation (13) is solely dependent on the rate of acceleration change. Considering the motion characteristics of underwater targets, a singular acceleration change may not distinctly capture the targets’ maneuvering variations. In addition, in this model, the maneuvering frequency $\alpha$ and the adaptive coefficient b for the acceleration variance are fixed parameters that lack adaptability. The enhancement of the adaptive Gaussian model is as follows:

• Adaptive ${\sigma }_a^2$;

The construction of a tracking system using the discrete KF algorithm based on the adaptive Gaussian model is detailed in the Appendix A. The discretized expression for Equation (13) is shown in Equation (14):

$${\sigma }_a^2=\frac{{\left(\ddot{x}(k)-\ddot{x}(k-1)\right)}^2}{T^2{b}^2}$$

(14)

Incorporating the influence of the target position and velocity on acceleration changes, the discrete acceleration variance formula is redefined as:

$${\sigma }_a^2=\frac{\lambda }{2}\left[{\left(\frac{\dot{x}\left(k\right)-2\dot{x}\left(k-1\right)+\dot{x}\left(k-2\right)}{T^2}\right)}^2+{\left(\frac{\ddot{x}\left(k\right)-\ddot{x}\left(k-1\right)}{T}\right)}^2\right]$$

(15)

where $\lambda$ represents the variance fuzzy adaptive coefficient and $T$ represents the tracking step size.

Using a fuzzy reasoning system [2] to obtain $\lambda$, set the fuzzy {VS, S, M, B, VB} to represent very small, small, medium, large, and very large, respectively. Taking the current acceleration from the state prediction and the innovation from the KF (the difference between the actual measurement and the predicted state estimate [22]) as inputs, we employ the membership functions illustrated in Figure 5 to fuzzily obtain both inputs. The output variable is denoted as $\lambda$. For computational convenience, the fuzzy input values for acceleration are normalized, restricting their range to within $\left[0,1\right]$.

Based on the characteristics of underwater target motion, the following fundamental rules can be derived: When the acceleration is very small and the innovation is small, the acceleration change is small, leading to a decrease $\lambda$; when the acceleration is very small and the innovation is small, the acceleration change is significant, leading to an increase $\lambda$. Based on these considerations, a fuzzy rule table is designed, as shown in

Table 2. Rules list of fuzzy adaptive coefficient.

$\mathit{\lambda }$		$\mathit{r}$
		VS	S	M	B	VB
$a$	VS	S	M	B	VB	VB
	S	S	M	B	B	M
	M	VS	S	M	M	M
	B	VS	S	M	S	S
	VB	VS	S	M	S	S

, in which $r$ represents the innovation. By employing the Mandani method for fuzzy inference and the weighted average method for defuzzification, the value of $\lambda$ can be obtained.

• Adaptive $\alpha$;

The fundamental idea of the interacting multiple model (IMM) is to match different motion models to the various motion patterns of maneuvering targets. To achieve the adaptive maneuvering frequency, three aforementioned improved models with different $\alpha$ are selected as three distinct models applied to the IMM algorithm. The switching of different $\alpha$ values is based on the maneuvering changes of the target. The specific process is illustrated in Figure 6. For detailed symbols and derivation formulae, please refer to [23].

To obtain accurate predictions of dynamic obstacle states for collision risk detection, a state-tracking system is established based on the I-Gauss model using the KF algorithm. The detailed process for establishing this system can be found in Appendix A.

3.2.2. SCD Model

To achieve an accurate obstacle avoidance for dynamic obstacles, it is essential to assess their potential collision risk based on their current motion states. This enables early avoidance measures. Therefore, an SCD model is proposed, as illustrated in Figure 7.

The blue dashed line represents the predicted trajectory of the dynamic obstacle, and the green dashed line represents the predicted trajectory of the AUV. Initially, the motion of the dynamic obstacle and the AUV are regarded as the I-Gauss model, and the motion state in the next step is predicted using the established KF prediction system. By varying the time step, the future trajectories of the AUV and the dynamic obstacles for the next N time steps can be obtained through Equation (16):

$$X_{k+1}=\mathsf{\Phi }{X}_k$$

(16)

where $X_k$ denotes the position at the current moment, $X_{k+1}$ represents the position at the next moment, and $\mathsf{\Phi }$ is the state transition matrix of the motion model.

Subsequently, with the predicted position of the obstacles as the center, a dynamic obstacle collision risk zone is established, which is depicted as the shaded area in Figure 7. Its radius is calculated as Equation (17):

$$R_k=tr\left(\mathsf{\Phi }{\mathit{P}}_k{\mathsf{\Phi }}^T\right)$$

(17)

where $R_k$ is the radius of the collision risk zone, ${\mathit{P}}_k$ represents the state covariance matrix of the prediction system based on Kalman filtering, and $tr$ denotes the trace of the matrix.

Finally, if the predicted trajectory of the AUV falls into the obstacle’s collision risk zone between $[t_k,t_{k+N}]$, it is considered to have a collision risk. Establishing an outer circle around the corresponding collision risk zones in the current instance and the previous instance is denoted as the detection zone; the center of the detection zone is located as follows:

$$\left\{\begin{array}{l}x=x_k+\frac{|X_{k+1}-X_k|+R_{k+1}-R_k}{2}cos\phi \\ y=y_t+\frac{|X_{k+1}-X_k|+R_k-R_{k+1}}{2}sin\phi \\ \phi =arctan\frac{y_{k+1}-y_k}{x_{k+1}-x_k}\end{array}\right.$$

(18)

Calculate the distance $d$ from the center of the detection zone to the corresponding AUV trajectory. The formula for the improved repulsive force acting on the AUV is as Equation (19):

$$F_{rep}=k(\frac{1}{d}-\frac{1}{R_k})\frac{1}{d^2}$$

(19)

where $k$ is the repulsion coefficient, and the method for its setting is introduced in Section 3.3. Unlike traditional methods, where the repulsion force points directly from the obstacle to the robot, in Equation (19), the direction of the repulsion force is perpendicular to the corresponding AUV trajectory’s radial direction.

The collision risk detection method of the SCD model is as shown in Algorithm 1.

Algorithm 1 The State-Tracking Collision Detection

Input: Observation position of moving obstacles, AUV position, and predicted number of steps N. Step 1: Input the observed position of the moving obstacle at the previous moment and the actual position of the AUV, and the Kalman filter prediction system based on the improved Gaussian model (in Appendix A) is used to predict the dynamic obstacle and AUV position at the next N moments.

$\mathrm{Step} 2: \mathrm{Calculate} \mathrm{the} \mathrm{radius} \mathrm{of} \mathrm{the} \mathrm{obstacle} \mathrm{collision} \mathrm{risk} \mathrm{zone} R_k$ at each moment according to Equation (17).

$\mathrm{Step} 3: \mathrm{If}, \mathrm{at} \mathrm{any} \mathrm{time} \mathrm{of} \mathrm{the} \mathrm{prediction}, |{\widehat{X}}_{AUV}-{\widehat{X}}_{Obs}|\le R_k$, calculate the distance $d$ from the center of the corresponding detection zone Equation (18) to the corresponding AUV trajectory.

Step 4: Calculate the repulsion using the improved Formula (19), and the direction is perpendicular to the radial direction of the corresponding AUV trajectory.

3.3. The SAPF

This has been a challenging issue in APF methods and their improvements in setting the coefficients for attraction and repulsion. If the repulsion coefficient is too large, it can lead to significant changes in the heading of the AUV, affecting its maneuverability. However, if the repulsion coefficient is too small, it may increase the risk of collisions. In environments with multiple dynamic obstacles, each with different motion states, it is necessary to adaptively adjust the repulsion coefficient based on real-time information about the target. The AUV’s velocity setting determines the step lengthening, thus influencing the voyage efficiency. It contributes to efficient obstacle avoidance by adjusting the step size.

The obstacle avoidance path should meet the requirements of being both safe and efficient. To achieve an AUV’s efficient and safe navigation, it is necessary to minimize its velocity and heading changes while maximizing its distance from dynamic obstacles. Therefore, the designed objective function is as Equation (20):

$$J={\displaystyle \sum }_{i=1}^M{\displaystyle \sum }_{j=1}^N\left|R_{ij}-l_{ij}\right|+\left|\psi -{\psi }_0\right|+\left|V-V_0\right|$$

(20)

where $M$ represents the number of obstacles in the field of view, $N$ denotes the predicted number of steps, and $R_{ij}$ represents the collision risk radius of the $i$th obstacle at time $j$. The $l_{ij}$ is composed of three parts:

$$l_{ij}=d+d_1+d_2$$

(21)

where $d$, $d_1$, $d_2$ represent the distance from the center of the detection area to the straight line of the AUV trajectory, and to the AUV’s current and predicted positions, as shown in Figure 8.

The objective function $J$ takes the AUV heading angle, velocity, and distance to obstacles into consideration. When the objective function $J$ reaches its minimum value, the planned obstacle avoidance route is the safest and most efficient. Therefore, the DOA problem is transformed into the optimization problem of minimizing the objective function, as described below:

$$\left\{\begin{array}{c}Min\left(J\right)\\ s.t.\psi ,V\end{array}\right.$$

(22)

To resolve the issue shown in Equation (22), the simulated annealing (SA) optimization algorithm is introduced; SA is a stochastic search algorithm used to solve large-scale combinatorial optimization problems. Its concept is inspired by the process of metal annealing and can be roughly described as follows: as the initial annealing temperature decreases, we continuously search for the optimal solution within feasible solutions. The SA algorithm has a probability of accepting solutions that are worse than the original solution. This ensures a convergence to the global optimum.

The SA algorithm is mainly composed of the Metropolis algorithm and annealing process, corresponding to the inner loop and outer loop, respectively [24]. The external circulation is the annealing process. When the solid is at a higher temperature, the solid is cooled according to a certain proportion according to the cooling coefficient. When the temperature drops to the termination temperature, the annealing process ends. The inner loop is the Metropolis algorithm. At each temperature of cooling, random disturbance produces several new solutions, calculates the corresponding objective function value of each new solution, and finds the optimal solution at this temperature. The optimization process for the objective function $J$ using the SA algorithm is as shown in Algorithm 2.

Algorithm 2 The SA Algorithm Optimizes the Objective Function

Step 1: $\mathrm{Set} \mathrm{initial} \mathrm{temperature} T_0$$, \mathrm{end} \mathrm{temperature} T_f$$, \mathrm{let} T=T_0$. $\mathrm{Generate} \mathrm{a} \mathrm{random} \mathrm{initial} \mathrm{solution} X_0$, $\mathrm{and} \mathrm{compute} \mathrm{object} \mathrm{function} J(X_0)$.

Step 2: Set cooling coefficient $\lambda$$, \mathrm{let} T=\lambda T$$,\lambda \in [0,1]$.

$\mathrm{Step} 3 : \mathrm{Add} \mathrm{stochastic} \mathrm{perturbation} \mathrm{to} \mathrm{the} \mathrm{current} \mathrm{solution}{X}_t$$\mathrm{that} \mathrm{generates} \mathrm{a} \mathrm{new} \mathrm{solution} X_{t+1}$$\mathrm{in} \mathrm{the} \mathrm{neighborhood}, \mathrm{and} \mathrm{compute} \mathrm{the} \mathrm{corresponding} \mathrm{object} \mathrm{function} J(X_{t+1})$$, \mathrm{compute} \Delta J=J(X_{t+1})-J(X_t)$.

Step 4: $\mathrm{If} \Delta J<0$$, J\left(X_{t+1}\right)$ is accepted; otherwise, the new one is decided according to the probability calculation. Step 5: At temperature $T$, repeat steps 3 and step 4 for $L$ times; $L$ is a constant, and means the number of iterations of the inner loop. Step 6: $\mathrm{If} T=T_f$, end; else, return to step 2 to continue cooling.

The proposed dynamic obstacle avoidance strategy in the target-tracking process is illustrated in the flowchart depicted in Figure 9:

4. Simulation and Discussion

In this section, the following three questions are answered:

• Q1. Can the SCD-SAPF algorithm solve the local minimum problem?
• Q2. Is the proposed dynamic obstacle prediction based on the I-Gauss model effective?
• Q3. The effectiveness and efficiency of the proposed SCD-SAPF algorithm for AUV dynamic obstacle avoidance must be validated. Can it adapt to a complex submarine environment?

4.1. Local Minimum Verification of the Proposed SCD-SAPF

To verify the feasibility of the SCD-SAPF algorithm, two scenarios were set up: one with a local minimum in a collinear situation and the other with non-collinearity. Simulation experiments were conducted for both scenarios using the traditional artificial potential field (T-APF) and the SCD-SAPF algorithms under the same conditions. The initial position of the AUV was set to (0, 0), with a step size of $t=0.5 s$. Motion information for the tracked target and obstacles is provided in

Table 3. Local minimal value scenario 1: non-collinear.

Number	Initial Position (m)	Velocity (m/s)	Acceleration (m/s2)
Target	(0, 30)	(0.5, 0.5)	(0, 0)
Obstacle 1	(−3.9, 10.6)	(0.4, 0.3)	(0, 0)
Obstacle 2	(6.5, 21.8)	(−0.3, −0.4)	(0, 0)

and

Table 4. Local minimal value scenario 2: collinear.

Number	Initial Position (m)	Velocity (m/s)	Acceleration (m/s2)
Target	(0, 30)	(0, 0.6)	(0, 0)
Obstacle	(0, 20)	(0, −0.4)	(0, 0)

.

The parameters for T-APF were set as follows: obstacle influence radius $\rho =2m$, and attraction and repulsion coefficients $k_{att}=1$ and $k_{rep}=5$. In the SCD-SAPF algorithm, the prediction steps were uniformly set to 5. The simulation results for T-APF and SCD-SAPF are compared in Figure 10. The arrow points to the direction of motion of the AUV, moving obstacle and target.

As shown in Figure 10a,b, when sailing to the position near (1.3, 12.9), the repulsive forces generated by the two obstacles are equal in magnitude but opposite in direction to the target attraction force. The T-APF would drive the AUV into a deadlock, causing it to stop moving. However, with the strategy proposed in this paper, the repulsion coefficient is adjusted to ensure that the net force at this point is not zero. Under the action of the net force, the AUV safely avoids obstacles. As shown in Figure 10c,d, in the case of three collinear points, the improved repulsive force described in Equation (19) causes the AUV to move to the right, escaping the local minimum state and safely avoiding obstacles. The simulation results indicate that the proposed SCD-SAPF obstacle avoidance algorithm can solve the local minimum problem.

4.2. Dynamic Obstacle Prediction Verification

Based on the I-Gauss model proposed in Section 3.2.1 and the KF prediction system provided in the Appendix A, four different motion states were designed. Simulations were conducted for the CV, CA, adaptive Gauss model, and I-Gauss model to verify the performance of the system in predicting dynamic obstacles. The four motion states are presented in

Table 5. Motion state 1.

Time (s)	Initial Position (m)		Velocity (m/s)		Acceleration (m/s2)
	X	Y	X	Y	X	Y
0~300	0	0	2	5	0	0

,

Table 6. Motion state 2.

Time (s)	Initial Position (m)		Velocity (m/s)		Acceleration (m/s2)
	X	Y	X	Y	X	Y
0~200	0	0	0	0	0.5	10
200~300	10,000	20,000	100	200	−0.2	−0.9

,

Table 7. Motion state 3.

Time (s)	Initial Position (m)		Velocity (m/s)		Acceleration (m/s2)
	X	Y	X	Y	X	Y
0~50	0	0	0	10	0.1	0
50~100	125	500	5	10	0	0
100~120	375	1000	5	10	0	−0.5
120~140	475	1100	5	0	0	0.6
140~150	575	1220	5	12	0	0
150~200	625	1340	5	12	−0.1	0
200~220	750	1940	0	12	0.4	0
220~240	830	2100	8	12	0	0
240~300	1150	2420	8	6	0	−0.1

and

Table 8. Motion state 4.

Time (s)	Initial Position (m)		Velocity (m/s)		Acceleration (m/s2)
	X	Y	X	Y	X	Y
0~62	$50\sin (\frac{t}{10})$	$50\cos (\frac{t}{10})$	$5\cos (\frac{t}{10})$	$-5\sin (\frac{t}{10})$	$-0.5\sin (\frac{t}{10})$	$-0.5\cos (\frac{t}{10})$

.

The simulation result is shown in Figure 11.

Using the root mean square error (RMSE) and mean error (ME) for the predicted positions, the performance of the model was compared. Figure 12 presents the prediction error results of the proposed state prediction system for obstacles with different motion states. It can be observed from Figure 12 that the proposed I-Gauss motion model has a higher accuracy than the existing CV, CA, and adaptive Gauss motion models. The RMSE in the x and y directions remains stable within the [0, 1] range, and the ME is stable within the range of [−0.1, 0.1]. This validates the effectiveness and stability of the established state prediction system based on the I-Gaussian model in predicting dynamic obstacles.

4.3. Feasibility and Effectiveness of the Proposed SCD-SAPF Algorithm

In the APF method, the robot moves toward the resultant force of attraction and repulsion. The coefficients of attraction and repulsion directly affect the resultant force acting on the robot, thereby influencing its direction of motion. However, setting these coefficients is a challenging task. If the repulsion coefficient is too large, it can lead to significant changes in the heading, potentially causing the tracked target to disappear from the field of view, resulting in target loss and not aligning with the actual motion of the AUV. In contrast, if the repulsion coefficient is too small, it may result in a poor obstacle avoidance performance. Moreover, for multi-objective OA, different targets have different motion states, making it impractical to use a uniform repulsion coefficient in the obstacle avoidance algorithm. It is necessary to dynamically adjust the repulsion coefficient based on real-time information about the targets. Therefore, this paper assumes a constant attraction coefficient of 1 and focuses on adjusting the repulsion coefficient to alter the robot’s motion direction. Specifically, repulsion coefficients of 1 and 10 were considered, and a comparison was made with the SCD-SAPF algorithm in the simulated experiments. The tracked targets and obstacle motion information are provided in

Table 9. Scenario 3.

Number	Initial Position (m)	Velocity (m/s)	Acceleration (m/s2)
Target	(0, 100)	(0, 1)	(0, 0)
Obstacle 1	(−8.8, 20)	(1, 0)	(0, 0)
Obstacle 2	(4.3, 126.5)	(−0.1, −2)	(0, 0)

.

The obstacle avoidance path with $k_{rep}=1$ is shown in Figure 13a, and the path with $k_{rep}=10$ was illustrated in Figure 13b. The path and the variation of the repulsion coefficient over time for the SCD-SAPF obstacle avoidance strategy are illustrated in Figure 13c,d. The maximum angular velocity and tracking time required for different repulsion coefficients are shown in

Table 10. Courses and tracking time under different repulsion coefficients in scenario 3.

Repulsion Coefficient	OA Results	Tracking Time (s)	Maximum Angular Velocity (rad/s)
1	Failure	100.841	0.394
10	Success	109.264	1.553
SCD-SAPF	Success	101.145	0.451

. It can be observed that, with a small repulsion coefficient, the AUV can safely avoid Obstacle 1, but there is a collision risk with Obstacle 2. With a large repulsion coefficient, although the AUV can safely avoid Obstacle 2, the heading change during the avoidance of Obstacle 1 does not align with reality because of the high repulsion force. In the case of adaptive repulsion coefficients, different repulsion coefficients are applied based on the state information of different obstacles, allowing for the planning of reasonable paths to safely avoid all obstacles. The arrow points to the direction of motion of the AUV, moving obstacle and target.

The TAPF method provides the direction of motion for the AUV without changing the speed magnitude. With a constant step size, unreasonable paths may be planned in dynamic obstacle avoidance scenarios. To verify the superiority of the SCD-SAPF algorithm, we set up simulation scenario 4, and the motion information of the tracked target and obstacles is shown in

Table 11. Scenario 4.

Number	Initial Position (m)	Velocity (m/s)	Acceleration (m/s2)
Target	(0, 20)	(0, 0.5)	(0, 0)
Obstacle	(−10, 7.3)	(0, 0)	(0.5, 0)

.

As shown in Figure 14a, the obstacle is accelerating uniformly. When the AUV uses the TAPF method to avoid obstacles at a constant speed, in step 5, the obstacle is on the left side of the AUV, and, in step 6, the obstacle appears on the right side of the AUV. This results in two opposing forces along the X-axis, leading to the TAPF obstacle avoidance path shown in Figure 14a. Figure 13a also illustrates the obstacle avoidance path of the SCD-SAPF algorithm.

Table 12. Courses and tracking time under different repulsion coefficients in scenario 4.

Algorithm	Tracking Time (s)	Maximum Acceleration (m/s2)	Maximum Angular Velocity (rad/s)
TAPF	50.154	0	1.747
SCD-SAPF	42.756	0.453	0.431

shows the tracking time, maximum acceleration, and maximum angular velocity required for the different algorithms. It can be observed that T-APF may plan ineffective paths with heading changes that violate AUV motion constraints. The proposed strategy, which includes velocity changes, avoids ineffective paths and effectively reduces significant changes in the heading. In this scenario, the tracking time is reduced by 14.7%, demonstrating an effective improvement in navigation efficiency. The arrow points to the direction of motion of the AUV, moving obstacle and target.

During underwater obstacle avoidance, due to the presence of complex environmental disturbances and sensor observation errors, AUV predictions of targets and obstacles may be subject to noise interference. In TAPF methods, the obstacle influence range is often a constant without considering the influence of noise. This oversight can sometimes lead to the failure of obstacle avoidance tasks. The tracked targets and obstacle motion information are provided in

Table 13. Scenario 5.

Number	Initial Position (m)	Velocity (m/s)	Acceleration (m/s2)
Target	(−1.5, 20)	(0.5, 0)	(0, 0)
Obstacle	(10.8, 17.9)	(−0.5, −0.3)	(0, 0)

.

As shown in Figure 15b, with an obstacle avoidance radius of 2 m and no noise, the AUV can safely avoid the target. However, when Gaussian noise with a mean of 0 and a variance of 0.1 is introduced along the obstacle’s path, the AUV’s path intersects with the obstacle’s path. This occurs because there is an estimation error between the tracked value and the true value of the target, with the estimation value lagging behind the true value. The obstacle avoidance radius remains unchanged, resulting in the risk of collision. In contrast, the obstacle avoidance strategy proposed in this paper adjusts the obstacle avoidance radius based on the predicted error of the obstacle’s state. Figure 15a shows the obstacle avoidance path of the SCD-SAPF, and Figure 15c demonstrates the changes in the obstacle avoidance radius during the avoidance process. When noise is present, the obstacle avoidance radius increases, providing a safer execution of obstacle avoidance tasks by the AUV.

Table 14. OA results and tracking time under different noise.

Algorithm	OA Results		Tracking Time
	Noise	Noiseless	Noise	Noiseless
TAPF	Fail	Success	26.126	25.183
SCD-SAPF	Success	Success	26.112	25.459

presents the obstacle avoidance results and tracking time for the different algorithms under various noise interferences. It can be observed that, even in the presence of noise interference, the obstacle avoidance strategy proposed in this paper can successfully navigate around obstacles with only a slight difference in obstacle avoidance time. The arrow points to the direction of motion of the AUV, moving obstacle and target.

5. Conclusions

Aiming at the problem of obstacle avoidance in dynamic environments, a dynamic obstacle avoidance strategy based on SCD-SAPF, which combines APF methods with target motion state tracking, is proposed. The SCD model construction of a motion state prediction system based on the dynamic obstacle model enables the assessment of collision risks between the AUV and obstacles. SAPF employs an SA algorithm to optimize the objective function for obtaining the optimal velocity and repulsion coefficients. This optimization allows for the calculation of the AUV’s obstacle avoidance path, addressing the inefficiencies and inadequacies of TAPF algorithms caused by fixed or poorly set parameters. The algorithm’s obstacle avoidance performance was investigated in various scenarios, and the following conclusions were drawn:

(1) The SCD-SAPF obstacle avoidance algorithm is proposed by considering the dynamic characteristics of obstacles and their associated risk zones. This enables the AUV to effectively accomplish dynamic obstacle avoidance tasks. The simulation results demonstrate that the algorithm can escape local minima and plan safe, reasonable, and efficient obstacle avoidance paths in various obstacle movement scenarios. This validates the effectiveness, feasibility, and robustness of the proposed algorithm.

(2) The I-Gauss motion model is proposed to address the mismatch issue between the true motion state of underwater moving targets and the tracking motion model. The simulation results indicate that this model further enhances the ability to adapt to different motion states compared with the adaptive Gauss model. When the target undergoes sudden maneuvers, it can quickly adapt to the new motion state. The Kalman filter-based state prediction system built on this model exhibits smaller errors, resulting in a better tracking performance of the target’s motion state.

(3) An SCD model is proposed that adaptively adjusts the obstacle range zone using the covariance information from the state prediction system. The simulation results demonstrate that the adjustable radius can effectively reduce the impact of noise, enhancing the robustness of the algorithm.

(4) The SAPF algorithm is proposed to optimize the avoidance path by designing an objective function that considers multiple future time steps and adjusting the repulsion coefficients and speed. The simulation results demonstrate that the stochastic nature of the SA algorithm addresses the issue of local minima by optimizing the repulsion coefficients and speed to ensure the efficiency of the avoidance path.

This paper focuses on the performance of the proposed SCD-SAPF algorithm, primarily addressing path design and optimization. The impact of the AUV and obstacle principal dimension on obstacle avoidance has not been discussed in detail, and related work will be conducted in future research.