AUV path planning method based on point cloud

Abstract

The application designs an AUV path planning method based on point cloud, aiming at the cluster control problem of multiple AUV submarines, through end-to-end reinforcement learning, the AUV agent can realize path planning, obstacle avoidance and target tracking and other tasks in the complex marine environment. The innovation lies in using the improved SAC algorithm, through a simple reward function, the complex motion ability of the agent is realized, and the problem of high training cost and long time of traditional AUV is overcome. At the same time, the lead-follow mechanism is designed, the lead submarine is responsible for environment perception and obstacle avoidance, and the follower submarine keeps formation to complete the detection task. The physical engine Gazebo and the ROS system are used to construct a realistic marine environment and control system, and the effectiveness of the method in cluster control is verified, and the environmental exploration ability of the AUV is improved.

Description

Technical Field

[0001] This invention relates to a path planning algorithm, specifically to a path planning algorithm that constructs a realistic AUV simulation environment in the Gazebo physics engine and implements fixed-point navigation and obstacle avoidance based on an enhanced algorithm. Background Technology

[0002] Traditional AUV control methods lack autonomy and flexibility. Deep reinforcement learning uses deep neural networks to approximate nonlinear functions. Applying deep reinforcement learning to path planning for underwater vehicles can solve complex situations that traditional control methods cannot handle. A reinforcement learning algorithm based on an actor-critic framework simultaneously learns a policy network and a value network, alternating between policy evaluation and policy improvement. Policy evaluation aims to learn an accurate action value function, while policy improvement trains the policy network according to the output of the value function, increasing the probability of outputting high-value actions, thus continuously improving the policy. The SAC algorithm, based on maximum entropy theory, is an off-track policy algorithm based on the actor-critic framework. Compared to on-track policy algorithms, off-track policy algorithms make more efficient use of the data. Summary of the Invention

[0003] To address the problem that existing AUV path planning algorithms cannot utilize point cloud observations for real-time obstacle avoidance, this invention aims to solve the problem of how to learn an efficient path planning strategy using reinforcement learning algorithms, while considering the collision avoidance problem during AUV navigation. To solve the above technical problems, this invention adopts the following technical solution:

[0004] A point cloud-based AUV path planning method employs a path planning technique based on the SAC algorithm, involving multiple AUVs. The algorithm obtains the optimal strategy for a specific environment through end-to-end learning, enabling it to perform multiple tasks such as path planning, obstacle avoidance, and target tracking. The method includes the following steps:

[0005] The first step involved building a relatively complete simulation training platform based on the Gazebo physics engine. This platform established a marine environment including an AUV model, AUV control module, AUV sensing module, flow field, and obstacles, all unified into an abstract reinforcement learning interface. The lattice Boltzmann method was used to refine the motion model and constant flow field forces of the underwater AUV, and a simulation environment with realistic mechanical properties was constructed using the BGK collision model.

[0006] (1) A simulation training platform was built using the Gazebo physics engine and ROS system, including an AUV model, control module, sensing module, flow field and obstacles, to realize the simulation of the marine environment.

[0007] (2) The motion model and flow field force of the AUV are refined using the lattice Boltzmann method. The Reynolds coefficient that conforms to the characteristics of seawater is set, and the original gas dynamics idea of ​​Boltzmann is simplified. The particles are restricted to propagate in 9 possible directions.

[0008] (3) Communication and data transmission between components in the simulation environment are realized through the nodes, topics and service mechanisms of the ROS system.

[0009] (4) Use Docker container technology to encapsulate the simulation environment, realize multi-instance parallel simulation, improve environment sampling efficiency, and support large-scale reinforcement learning training.

[0010] (5) In accordance with the OpenAI_Gym specification, ROS is integrated with the reinforcement learning environment, and an environment conforming to the Gym interface is encapsulated to simplify the development process.

[0011] (6) Set simulation environment parameters, including ocean depth, obstacle settings, AUV starting point and target point, and construct a challenging path planning task.

[0012] (7) Introduce an inertial navigation system and a Doppler velocimeter (DVL) to simulate AUV position estimation, and consider periodic surfacing and diving motions to increase training difficulty and randomness.

[0013] (8) Integrate multi-beam sonar sensors in the simulation environment to simulate the underwater obstacle avoidance capability of AUV and detect obstacles through sonar detection data.

[0014] (9) A flow field model based on real ocean data is used to simulate the effect of constant ocean currents on AUV motion, thereby increasing the realism of the simulation environment.

[0015] The second step involves improving the entropy adjustment mechanism in the traditional SAC reinforcement learning algorithm by proposing an entropy-adjustment-based SAC algorithm. A point cloud feature extraction method based on an encoder-decoder structure is designed, and reconstruction loss is used as an auxiliary task to accelerate the convergence of the feature extraction encoder module.

[0016] (1) Introduce the concept of target entropy, and use the loss function Automatic adjustment This makes the action entropy output by the strategy approximate the target entropy, thus enhancing the algorithm's exploration capability.

[0017] (1)

[0018] (2) Calculate the loss function using the above formula. And update using gradient backpropagation method. The exploration level of the strategy is dynamically adjusted.

[0019] (3) To avoid premature convergence of the algorithm, another temperature coefficient adjustment method is added. The exploratory nature is enhanced in the early stage of the algorithm by formula (2), and the exploratory nature is gradually reduced in the later stage.

[0020] (2)

[0021] (4) Introduce the temperature coefficient used in the loss function By combining formula (3) with traditional and new adjustment methods Balance exploration and utilization.

[0022] (3)

[0023] The third step is to design a point cloud feature extraction network and an auxiliary training loss.

[0024] (1) Construct a feature extraction network, the main body of which is an encoder network. Self-supervised reconstruction of point clouds is performed through variational autoencoders, assuming that the intermediate features follow a Gaussian distribution.

[0025] (2) Point cloud features are extracted using the PointNet structure to achieve permutation invariance and robustness. Point features are mapped to a high-dimensional space through symmetric functions and multilayer perceptrons.

[0026] (3) In order to capture the local and global information of the point cloud, the local features are combined with the global features and processed by MLP and max pooling to obtain the encoded feature vector.

[0027] (4) The Chamfer Distance (CD) and Earth Mover Distance (EMD) loss functions are used as reconstruction losses to assist the convergence of the feature extraction encoder module.

[0028] (5) The loss function of the auxiliary task combines CD loss and EMD loss, and uses prior knowledge to constrain the dependent variable, thereby accelerating the convergence of the encoder module.

[0029] The fourth step is to design a suitable state space and reward function, and train the agent to complete path planning and target tracking tasks.

[0030] (1) Introduce a discrimination threshold to constrain the behavior of followers, avoid collisions with the leader, and at the same time ensure the coordination among agents in the formation.

[0031] (2) To enhance the generalization ability of the following agent, the navigator adopts a semi-random strategy, combining deterministic actions and random actions to generate diverse trajectories and avoid the following agent from overfitting to a specific navigator strategy.

[0032] Attached Figures / Tables

[0033] Figure 1 This is a flowchart of the algorithm in a specific embodiment of the present invention.

[0034] Figure 2 This is a schematic diagram of the Gazebo marine environment constructed in a specific embodiment of the present invention.

[0035] Figure 3 This is a structural block diagram of the point cloud feature extraction module in a specific embodiment of the present invention.

[0036] Figure 4 This is a schematic diagram of target tracking in a specific embodiment of the present invention. Detailed Implementation

[0037] To make the technical solution of the present invention clearer, the present invention will be further described below with reference to the accompanying drawings. Figure 1 This is a flowchart of a SAC algorithm based on improved entropy adjustment. The invention is implemented in the following steps:

[0038] The first step is to model the AUV environment in the Gazebo physics engine.

[0039] (1) An AUV model was created, establishing a marine environment including the AUV model, AUV control module, AUV perception module, flow field, and obstacles, and unifying them into an abstract reinforcement learning interface. The motion model and constant flow field force of the underwater AUV were refined, constructing a simulation environment with realistic mechanical properties. This provides a realistic training environment for the upper-level reinforcement learning algorithm.

[0040] (2) The environment was encapsulated using Docker containers. Docker is an open-source platform for developing, transporting, and running applications. Through containerization technology, it can package ROS, Gazebo, and their dependencies together, enabling build-once, run-anywhere functionality. By building Docker containers, sampling was achieved simultaneously in multiple Gazebo simulation environments, and without consuming excessive GPU computing resources when Gazebo and Rivz graphical rendering were disabled, thus achieving high environment sampling efficiency. The environment in the container communicates with the upper-level environment management layer via TCP / IP through port mapping, thus enabling the construction of an environment simulation cluster to perform environment sampling simultaneously on multiple devices, further improving environment sampling efficiency and accelerating the training of reinforcement learning algorithms.

[0041] (3) The reinforcement learning environment is integrated with ROS using the openai_Gym specification and finally encapsulated into an environment that conforms to the gym interface, which simplifies the process of reinforcement learning development in ROS. The environment that conforms to the gym interface mainly implements the step and reset functions. The step function takes the current action as input and outputs the state at the next moment, the reward obtained by entering the next state, and a flag indicating whether the current round has ended.

[0042] The second step is to design an entropy adjustment mechanism and a point cloud processing model based on the SAC algorithm.

[0043] (1) Using an automatic entropy adjustment mechanism, the target entropy Set it to a constant. Update the loss function in formula (4). This makes the actual entropy approximate the target entropy. .

[0044] (4)

[0045] In this formula, Let the entropy of the action output by the policy network be denoted by . This represents the difference between the actual entropy and the target entropy. If A value greater than zero indicates that the entropy of the network's output action is too high, and the objective is optimized using the gradient descent algorithm. This can reduce . The reduction in implies the expected discount return As the proportion of entropy decreases, the change in the algorithm's optimization objective leads to a corresponding decrease in the entropy of the network's output actions. This can be achieved by dynamically adjusting... The size, the entropy of the network output actions. It can automatically stabilize at the target entropy Nearby, the need for artificial adjustment of the temperature coefficient was avoided.

[0046] (2) An auxiliary temperature coefficient adjustment method was designed, which can effectively increase the exploratory nature of the algorithm in the early stage and avoid premature convergence. The designed entropy adjustment method is shown in formula (5):

[0047] (5)

[0048] in the formula This represents the minimum temperature coefficient of the algorithm. This indicates the total number of global steps that have been executed. It is the decay factor. In the early stages of the algorithm's execution, the global number of steps... The initial temperature coefficient is very small, therefore... A temperature coefficient close to 1 indicates that a higher temperature coefficient enhances the algorithm's exploratory nature in the early stages, increasing with the number of global steps. The increase of temperature coefficient of the algorithm It gradually decreases, but will not fall below its minimum value. This is especially important in tasks such as path planning.

[0049] Final temperature coefficient The adjustment method is shown in formula (6).

[0050] (6)

[0051] (3) Use auxiliary tasks and decoders to accelerate the convergence of the feature extraction module.

[0052] The point cloud extraction module is trained using a point cloud reconstruction task, and the encoder is trained using ChamferDistance (CD) and EarthMover Distance (EMD) losses during this process. ChamferDistance measures the distance between two point clouds, and the formula is as follows:

[0053] (7)

[0054] in and This represents two point clouds. Point cloud It is a point cloud One of the points. Similarly. express It is a point cloud One of the points. Representing distance in Euclidean space. Chamfer Distance primarily measures the similarity between two point clouds by calculating the distance from each point to the nearest point in the other point cloud. This metric captures local geometric differences between point clouds. EarthMover Distance measures the minimum transport cost between two point clouds, also known as Wasserstein distance, and is formulated as follows:

[0055] (4)

[0056] in Indicates from point cloud Points in To Point Cloud Points in The distance. Indicates from point cloud To Point Cloud A double shot. Point and points The Euclidean distance between them.

[0057] EMD calculations ensure that each point is used only once, requiring both point clouds to have the same number of points. EarthMover Distance measures the similarity between two point clouds by minimizing the total transport cost between them. This loss function intuitively reflects the overall similarity between the two point clouds. In the auxiliary task design of the feature extraction network, both CD loss and EMD loss are used for point cloud reconstruction.

[0058] The third step involves learning path planning, obstacle avoidance, and target tracking strategies in the environment based on the improved SAC algorithm. The target tracking performance is as follows: Figure 4 As shown.

[0059] (1) The rewards are divided into path planning rewards and obstacle avoidance rewards, which are used to evaluate the agent's performance in path planning and obstacle avoidance tasks, respectively. The path planning reward is determined based on the change in distance between the agent and the target point. A positive reward is given when the distance decreases and a negative reward is given when the distance increases. The obstacle avoidance reward is calculated based on the change in distance between the agent and the obstacle. A positive reward is given when the agent moves away from the obstacle and a negative reward is given when the agent moves closer to the obstacle.

[0060] (2) Based on the relative distance between the agent and the obstacle, dynamically select path planning reward or obstacle avoidance reward. By setting a threshold of 20m, determine the task focus of the agent to ensure that the agent takes appropriate actions in different situations.

[0061] (3) Design of AUV state and reward for target tracking. A state space containing its own position, velocity and the position of the navigator was designed for the target tracking AUV to achieve accurate tracking of the navigator. Two intelligent agents, the navigator and the follower, were set up. The navigator was responsible for path planning and obstacle avoidance, and the follower was responsible for maintaining a safe distance from the navigator.

[0062] (4) The reward design of the follower agent is based on the change in distance between the agent and the leader. Positive rewards are given when the distance is kept within a certain distance, and different levels of rewards are given when the distance exceeds the limit.

[0063] (5) Introduce a discrimination threshold to constrain the behavior of followers, avoid collisions with the leader, and ensure coordination among agents in the formation.

[0064] (6) To enhance the generalization ability of the following agent, the navigator adopts a semi-random strategy, combining deterministic actions with random actions to generate diverse trajectories and avoid the following agent from overfitting to a specific navigator strategy.

Claims

1. A point cloud-based AUV path planning method, characterized in that, The method includes: (1) Build a simulation training platform in the Gazebo physics engine, including AUV model, AUV control module, AUV sensing module, flow field and obstacles; (2) Refine the AUV motion model and flow field force to construct a simulation environment with realistic mechanical properties; (3) Communication and data transmission between components in the simulation environment are realized through the nodes, topics and service mechanisms of the ROS system; (4) Use Docker container technology to encapsulate the simulation environment, realize multi-instance parallel simulation, and improve the environment sampling efficiency; (5) Integrate ROS with the reinforcement learning environment according to the OpenAIGym specification and encapsulate an environment that conforms to the Gym interface; (6) Set simulation environment parameters and construct a challenging path planning task; (7) Improve the entropy adjustment mechanism in the traditional SAC reinforcement learning algorithm by introducing the concept of target entropy and adjusting it through the loss function. Automatically adjust entropy To make the action entropy of the policy output approximate the target entropy, the loss function is calculated using formula (1). : （1） in, The entropy represents the action output by the policy network. This represents the difference between the actual entropy and the target entropy. To prevent the algorithm from converging too early, another temperature coefficient adjustment method is added. The exploratory nature is enhanced in the early stage of the algorithm by formula (2), and the exploratory nature is gradually reduced in the later stage. （2） in, This represents the minimum temperature coefficient of the algorithm. This indicates the number of global steps that have been executed. It is the attenuation factor; Introducing the temperature coefficient used in the loss function By combining formula (3) with traditional and new adjustment methods Balance exploration and utilization; （3） (8) Construct a feature extraction network, perform self-supervised reconstruction of point clouds through variational autoencoders, extract point cloud features using the PointNet structure to achieve permutation invariance and robustness, map point features to high-dimensional space through symmetric functions and multilayer perceptrons, combine local and global features to capture local and global information of point clouds, use MLP and max pooling to process and obtain encoded feature vectors, and use Chamfer Distance (CD) and EarthMover Distance (EMD) loss functions as reconstruction losses to assist the convergence of the feature extraction encoder module; the loss function of the auxiliary task combines CD loss and EMD loss, and uses prior knowledge to constrain the dependent variable to accelerate the convergence of the encoder module; (9) Design a suitable state space and reward function, and train the agent to complete path planning and target tracking tasks; (10) Introduce an inertial navigation system and a Doppler velocimeter (DVL) to simulate AUV position estimation; (11) Integrating multi-beam sonar sensors to simulate the underwater obstacle avoidance capabilities of AUVs; (12) A flow field model based on real ocean data was used to simulate the effect of constant ocean currents on the movement of AUVs.

2. The point cloud-based AUV path planning method as described in claim 1, characterized in that: The temperature coefficient adjustment method in step (7) includes: (1) An auxiliary temperature coefficient adjustment method is adopted to enhance the exploratory nature in the early stage of the algorithm and gradually reduce the exploratory nature in the later stage; (2) Combine traditional entropy adjustment parameters and auxiliary temperature coefficient adjustment methods to balance exploration and utilization.

3. The point cloud-based AUV path planning method as described in claim 1, characterized in that: The feature extraction network construction method in step (8) includes: (1) Point cloud features are extracted using the PointNet structure to achieve permutation invariance and robustness; (2) Combine local features with global features, and use multilayer perceptron (MLP) and max pooling to obtain the encoded feature vector; (3) Chamfer Distance (CD) and Earth Mover Distance (EMD) are used as reconstruction losses to assist the convergence of the feature extraction encoder module.

4. The point cloud-based AUV path planning method as described in claim 1, characterized in that: The reward design method in step (10) includes: (1) Divide the reward into path planning reward and obstacle avoidance reward, which are determined based on the changes in the distance between the agent and the target point and the obstacle; (2) Dynamically select path planning reward or obstacle avoidance reward, and determine the task focus of the agent by setting a threshold value; (3) Design the state space and reward for the target tracking AUV, and design the reward based on the change in distance between the target and the navigator.

Images