An adaptive dynamic window approach for UUV obstacle avoidance planning in 3D environments

Unmanned Underwater Vehicles (UUVs) are essential equipment for Marine development, widely used in Marine scientific research, Marine resource survey, and Marine security. The autonomous navigation planning ability in unknown environments is a critical indicator for UUV intelligence. This paper focuses on the particularity of UUV motion and the complexity of the underwater environment and proposes an adaptive Dynamic Window Approach (DWA) for UUV obstacle avoidance planning. The adaptive DWA introduces novel heading angle evaluation and adaptive dynamic strategies to solve the problems of poor adaptability of traditional DWA to complex environments and unreasonable path selection in encountering dynamic obstacles and approaching targets. Simulations verify the effectiveness and superiority of the proposed method.


Introduction
The obstacle avoidance planning system is necessary for UUV to perform many tasks.In this problem, the UUV has no prior knowledge of the environment and only relies on the real-time observation information obtained by the sensors to guide the UUV to avoid obstacles and reach the target position [1].Unlike global path planning, obstacle avoidance planning is more suitable for UUV autonomous navigation in unknown environments [2].The dynamic window method is a local obstacle avoidance strategy for mobile robots proposed by Fox et al. [3] in 1997.Different from other obstacle avoidance methods, the dynamic window method is widely used in the obstacle avoidance planning of robots with constraints on velocity and acceleration.This method includes two steps: generating a practical search space and selecting the best solution from the search space.
In recent years, DWA has made many improvements in the above two aspects [4].In terms of constructing the search space, Seder and Petrovic [5] used an occupied grid map to build the robot's search space for solving the dynamic obstacle avoidance problem and modeled dynamic obstacles as moving cells to predict their motion.Molinos et al. [6] evaluated the feasibility of dynamic obstacle paths by constructing local and optimal search space based on the Bayesian occupation filter algorithm.Wang et al. [7] introduced the pivot point technique into the construction of search space to solve the problem of obstacle avoidance planning for vessels with limited maneuverability in inland waterways.
In terms of optimal scheme selection, the traditional DWA only considers the obstacles appearing on the candidate trajectories, so the risk of collision between the robot and the obstacles near the trajectories is high in practical applications.To solve this problem, Saranrittichai et al. [8] used the histogram grid representation of obstacles to estimate the collision probability of trajectories and proposed the field DWA.The traditional DWA uses the evaluation function with constant weight in selecting the best solution, which limits the robot's obstacle avoidance ability in a complex environment.To address this problem, Yang et al. [9] used fuzzy neural networks to adjust the weights of evaluation functions to improve the obstacle avoidance ability of robots in complex environments.Aiming at the obstacle avoidance planning problem of robots with uncertainty in system dynamics, Yasuda et al. [10] modelled the DWA objective function as a random variable.They proposed a DWA objective function estimation method based on deterministic sampling.
Due to the navigation environment's complexity and the UUV movement's unique characteristics, the traditional DWA needs to be improved in UUV obstacle avoidance planning.Firstly, the conventional DWA shows weak robustness to obstacle avoidance in complex seabed environments and easily falls into local optimization.Secondly, when encountering dynamic obstacles or approaching the target, the conventional DWA is prone to generate control oscillation, causing severe interference to the motion control of UUV, resulting in obstacle avoidance failure.This paper aims to address the above problems, construct a search space according to the motion characteristics of underactuated UUVs, and design an adaptive evaluation function based on the UUV obstacle avoidance strategy.The proposed adaptive DWA effectively improves the algorithm's environment adaptability and trajectory smoothness.

Coordinate reference frames
As Figure 1

UUV motion model
The UUV motion model describes its motion state, including the changes in position, velocity, and angular velocity to time., , , , , is set to describe UUV's velocity and angular velocity in the NED.This paper studies the obstacle avoidance planning of underactuated UUVs, which have no power to sway, heave, or roll.UUV is assumed to have a stable bottom control system to maintain pose and track velocity.Then, the motion model of the underactuated UUV can be expressed as: sin tan tan cos cos sin cos sin / cos , 90 For the underactuated UUV, the control inputs in this paper are set as surge acceleration u , pitch angular velocity q and yaw angular velocity r .According to UUV's dynamics and kinematics, the motion model during task execution should also meet the following constraints:

UUV velocity sampling
In this paper, a set of acceleration and angular velocities that can generate safe motion trajectories are used to construct the search space.The sampling elements include u , q , and r .Firstly, UUV is subject to kinematic constraints during navigation, including maximum velocity and minimum velocity: Secondly, considering the dynamics of UUV, its acceleration is limited by the force and torque of the actuator: u u u q q q dt q q dt r r r dt r r dt Where min u and max u denote the maximum surge deceleration and acceleration; max q and max r are the maximum acceleration in pitch and yaw.
In addition to the limitations of the UUV motion, the obstacles also limit the search space.Specifically, in order to avoid a collision, the minimum distance d ( , , ) aqr between the UUV candidate trajectory and the obstacle should always be greater than the safe distance.It is not a direct limitation on the sampled acceleration and angular velocity but a limitation on the trajectory generated by the sampling.Assuming that the dynamic window predicts the UUV trajectory at dt time in the future, then the acceleration and angular velocity pairs should also meet: u u a q r u q a q r q r a q r r At the same time, adjusting the UUV pitch angle is not conducive to controlling the UUV attitude.Therefore, its obstacle avoidance underwater is mainly realized by adjusting the surge and yaw velocities.During obstacle avoidance, UUV first travels to the target depth at the maximum pitch angle and velocity, then negatives to the target position at the target depth.The UUV pitch velocity is constrained as q V .The acceleration and angular velocity set of UUV is:

Adaptive dynamic window approach 3.1. Tanhshrink based heading angle evaluation
The heading angle evaluation of traditional DWA is sensitive to the angle between the UUV's heading and the target position, especially when the UUV is close to the target.The sensitive evaluation drives the robot to quickly and frequently adjust the heading toward the target.However, the motion ability of underactuated UUVs is limited, and frequent yaw instructions will lead to the jitter of the steering rudder, which is not conducive to stable motion control.This paper designs a heading angle evaluation related to the target distance based on the Tanhshrink function to solve the above problems.x y z when the UUV navigates to the end of the candidate trajectory under the current sampling acceleration and angular velocity.The c dis is the distance between the current position of UUV and the target position.dis and 1  denote positive adaptive parameters, determining the range of insensitivity of heading ( , , ) aqr to   .Compared with the traditional heading evaluation function, the proposed process reduces the sensitivity of DWA to   in a specific range of zero-neighborhoods.With the decrease of c dis , the insensitive neighborhood increases.

Velocity evaluation
Traditional velocity evaluation drives robots to move at maximum velocity to acquire the lowest time cost.However, UUVs typically perform tasks at cruising velocity limited by poor maneuverability.Consequently, the velocity evaluation of the UUV is set as follows: ( ) ( ) Where u denotes UUV's cruising velocity, 2  and 3  are positive adaptive parameters.

Adaptive dynamic strategies
UUVs typically navigate in complex submarine environments and encounter various dynamic obstacles when performing operational tasks.The evaluation function with fixed weight limits the environmental adaptability of DWA.In order to alleviate this problem, adaptive strategies of evaluation function are designed for different obstacle distributions according to the motion characteristics of UUV.
strategies for dynamic obstacles.When encountering dynamic obstacles, this paper first assesses the possibility of achieving collision avoidance by adjusting the surge velocity of the UUV.The UUV's safety distance is set as d , the azimuth angle of the obstacle in Heading strategies for trap obstacles.Collision avoidance planning in trap environments is a challenge for UUV submarine navigation.This paper adopts a of navigating along the obstacle instead of autonomous exploration when the UUV falls into a trap.Specifically, when the UUV encounters a trap obstacle, the influence of   on the heading evaluation is weakened, and the heading evaluation is set to a function related to the yaw velocity.

Simulations and analysis
This paper verifies the proposed adaptive DWA through three simulation scenarios.It is assumed that the UUV detects a sector area with a radius of 120 m, a 120 horizontal angle, and a 17 vertical angle.Considering UUV's location error and motion control error, this paper sets the safe distance as 30 m.
The simulation results in a static 2D environment are illustrated in Figures 2-3.As the UUV navigates towards the target, the change of   is gradually drastic.As shown in Figure 3, the traditional heading evaluation drives UUV to adjust the heading frequently, and the yaw curve will jitter and become gradually drastic over time.As Figure 4 shows, the Tanhshrink-based heading evaluation is a function related to distance.As c dis decreases, the insensitive area to   expands, thereby avoiding frequent jitter of yaw.

Conclusion
This paper presents an adaptive DWA for UUV autonomous collision avoidance.In order to overcome the problem of frequent rudder jitter caused by the sensitivity of traditional DWA to UUV heading, this paper designs a smoother heading evaluation based on the Tanhshrink function.The sensitivity is adjustable by the distance between UUV and the target.Moreover, aiming at dynamic and trap obstacles, this paper designs dynamic collision avoidance strategies that effectively improve the adaptability of the algorithm to complex environments.

1  1 P 2 dd
, and the pitch angle is set as 1  .represents the position where the obstacle's predicted trajectory intersects with the UUV's candidate trajectory.Assume the distance between UUV and 1 P is 1 d , the time taken by the UUV to move to 1 P at max u and min u are distance between the obstacle and 1 P is set as 2 d , the azimuth and pitch angles of the obstacle in for dynamic obstacles are as follows: If after t , the obstacle does not reach 1 P ( 12 0   or 12 0   ) and

and 5 
are the regulatory factors.When the uuv encounters a trap, 5  is set as a positive value close to zero.If the uuv follows obstacles on its right, then 4 0   .And if uuv follows the left side obstacles, 4 0   .

Figure 2 .
Figure 2. UUV collision avoidance paths planned by DWA and adaptive DWA in a 2D static scenario.

Figure 4 .
Figure 4. Heading evaluation functions.The simulation results in a dynamic 2D environment are illustrated in Figures5-7.The traditional DWA searches the optimal velocity pair based on the fixed evaluation, leading to unreasonable trajectories.The adaptive DWA combines the velocity adjustment strategies and evaluation function to obtain more elegant velocity and angular velocity control instructions for avoiding dynamic obstacles.

Figure 5 .
Figure 5. UUV collision avoidance paths planned by DWA and adaptive DWA in a 2D dynamic scenario.

Figure 7 .
Figure 7. Surge velocity curves planned by DWA and adaptive DWA.The simulation results in the 3D trap environment are shown in Figures 8-10.The traditional DWA is trapped in local optimization and cannot drive the UUV to the target.Adaptive DWA successfully operates the UUV bypass U-shape obstacles at cruising velocity and smooth yaw to reach the target.

Figure 8 .
Figure 8. UUV collision avoidance paths planned by DWA and adaptive DWA in 3D trap scenario.

Figure 10 .
Figure 10.Surge velocity curves planned by DWA and adaptive DWA.