AUV path planning method based on point cloud

Abstract

The invention designs an AUV path planning method based on point cloud, aiming at the cluster control problem of a plurality of AUV underwater vehicles, through end-to-end reinforcement learning, an AUV intelligent agent can realize tasks of path planning, obstacle avoidance, target tracking and the like in a complex marine environment. The method is characterized in that an improved SAC algorithm is used, the complex motion ability of the intelligent agent is achieved through a simple reward function, and the problems that traditional AUV training is high in cost and long in time are solved. Meanwhile, a piloting-following mechanism is designed, a piloting underwater vehicle is responsible for environment perception and obstacle avoidance, and a following underwater vehicle keeps a formation to complete a detection task. A physical engine Gazebo and an ROS system are used for constructing a vivid marine environment and control system, the effectiveness of the method in cluster control is verified through experiments, and the environment exploration capability of the AUV is improved.

Description

Technical Field

[0001] The present invention relates to a path planning algorithm, and in particular to a path planning algorithm for building a real AUV simulation environment in a gazebo physics engine and realizing fixed-point navigation and obstacle avoidance based on a reinforcement algorithm. Background Art

[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 the path planning of underwater submersibles can solve complex situations that traditional control methods cannot handle. The actor-critic based reinforcement learning algorithm learns the policy network and the value network at the same time, and alternately performs policy evaluation and policy improvement. The policy evaluation aims to learn an accurate action-value function, while the policy improvement trains the policy network according to the output of the value function and increases the probability of high-value action outputs, continuously improving the policy in the process. The SAC algorithm is based on the maximum entropy theory and is an off-orbit policy algorithm based on the actor-critic framework. Compared with the on-orbit policy algorithm, the off-orbit policy algorithm makes more full use of the adopted data. Summary of the invention

[0003] In view of the problem that the existing AUV path planning algorithm cannot use point cloud observation in real time to avoid obstacles, the present invention aims to solve how to use reinforcement learning algorithm to learn efficient path planning strategy and consider the collision avoidance problem during AUV navigation. In order to solve the above technical problems, the present invention adopts the following technical solutions: A point cloud-based AUV path planning method uses a path planning technology based on the SAC algorithm, which involves multiple AUV submersibles. The algorithm obtains the optimal strategy in a specific environment through end-to-end learning, which can complete multiple tasks such as path planning, obstacle avoidance, and target tracking. It includes the following steps: In the first step, a relatively complete simulation training platform was built based on the gazebo physics engine. The marine environment including the AUV model, AUV control module, AUV perception module, flow field, and obstacles was established and unified into an abstract reinforcement learning interface. The lattice Boltzmann method was used to refine the motion model and constant flow field force of the underwater AUV, and a simulation environment with real mechanical characteristics was constructed through the BGK collision model. (1) The Gazebo physics engine and ROS system are used to build a simulation training platform, including the AUV model, control module, perception module, flow field, and obstacles to simulate the ocean environment. (2) The lattice Boltzmann method is used to refine the AUV motion model and flow field force, set the Reynolds coefficient that conforms to the characteristics of seawater, simplify Boltzmann's original gas dynamics idea, and restrict particles to propagate in nine possible directions. (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, implement multi-instance parallel simulation, improve environment sampling efficiency, and support large-scale reinforcement learning training. (5) According to the OpenAI_Gym specification, ROS is integrated with the reinforcement learning environment, encapsulating an environment that conforms to the Gym interface to simplify the development process. (6) Set the simulation environment parameters, including ocean depth, obstacle setting, AUV starting point and target point, and construct a challenging path planning task. (7) An inertial navigation system and Doppler velocimeter (DVL) are introduced to simulate the AUV position estimation, and periodic ascent and descent motions are considered to increase the difficulty and randomness of training. (8) Integrate multi-beam sonar sensors in the simulation environment to simulate the underwater obstacle avoidance capability of the AUV and perform obstacle detection through sonar detection data. (9) A flow field model based on real ocean data is used to simulate the impact of constant ocean currents on AUV motion and increase the realism of the simulation environment. In the second step, the entropy regulation mechanism in the traditional SAC reinforcement learning algorithm is improved, and a SAC algorithm based on entropy regulation is proposed. A point cloud feature extraction method based on the encoder-decoder structure is designed, and the reconstruction loss is used as an auxiliary task to accelerate the convergence of the feature extraction encoder module. (1) Introducing the concept of target entropy, through the loss function J ( α ) Automatically adjust α to make the action entropy output by the strategy close to the target entropy, thereby enhancing the exploration ability of the algorithm. (2) Use the above formula to calculate the loss function J ( α ) , and use the gradient back-propagation method to update α to dynamically adjust the exploration degree of the strategy. (3) To avoid premature convergence of the algorithm, another temperature coefficient adjustment method is added. Formula (2) is used to enhance the exploration degree in the early stage of the algorithm and gradually reduce the exploration degree in the later stage. (4) Introducing the temperature coefficient used in the loss function By combining the traditional α and the new adjustment method α' through formula (3), the exploration and utilization are balanced. The third step is to design the point cloud feature extraction network and auxiliary training loss. (1) Construct a feature extraction network, the main body of which is the encoder network. The self-supervised reconstruction of the point cloud is performed through a variational autoencoder, assuming that the intermediate features follow a Gaussian distribution. (2) The PointNet structure is used to extract point cloud features, achieve permutation invariance and robustness, and map point features to high-dimensional space through symmetric functions and multi-layer perceptrons. (3) In order to capture the local and global information of the point cloud, the local features are combined with the global features, and MLP and maximum pooling are used to obtain the encoded feature vector. (4) Chamfer Distance (CD) and EarthMover Distance (EMD) loss functions are used as reconstruction losses to assist the convergence of the feature extraction encoder module. (5) The loss function of the auxiliary task combines CD loss and EMD loss, using prior knowledge to constrain the dependent variables and accelerate the convergence of the encoder module. The fourth step is to design a suitable state space and reward function to train the agent to complete path planning and target tracking tasks. (1) A discrimination threshold is introduced to constrain the behavior of the follower to avoid collision with the leader while ensuring the coordination among the agents in the formation. (2) To enhance the generalization ability of the follower agent, the leader adopts a semi-random strategy that combines deterministic actions with random actions to generate diverse trajectories and avoid the follower agent from overfitting to the specific leader strategy. BRIEF DESCRIPTION OF THE DRAWINGS

[0004] Figure 1 It is the algorithm flow chart in the specific implementation mode of the present invention Figure 2 This is a schematic diagram of the gazebo ocean environment constructed in a specific implementation of the present invention. Figure 3 This is a structural block diagram of the point cloud feature extraction module in a specific implementation of the present invention. Figure 4 This is a schematic diagram of target tracking in a specific implementation mode of the present invention. DETAILED DESCRIPTION

[0005] In order to make the technical solution of the present invention clearer, the present invention is further described below in conjunction with the accompanying drawings. Figure 1 The flowchart of the SAC algorithm based on improved entropy regulation is shown in FIG. The present invention is specifically implemented according to the following steps: The first step is to model the AUV environment in the gazebo physics engine. (1) Create an AUV model and establish an ocean environment including the AUV model, AUV control module, AUV perception module, flow field, and obstacles, and unify them into an abstract reinforcement learning interface. The motion model and constant flow field force of the underwater AUV are refined to build a simulation environment with real mechanical properties. This provides a real training environment for the upper-level reinforcement learning algorithm. (2) The environment was encapsulated using a docker container. Docker is an open source platform for developing, shipping, and running applications. Through containerization technology, it can package ROS, Gazebo, and their dependencies together, so that they can be built once and run anywhere. By building a docker container, sampling in multiple gazebo simulation environments is achieved simultaneously, and when the gazebo and rivz graphical rendering are turned off, it does not occupy too many GPU computing resources, thus achieving a higher environment sampling efficiency. The environment in the container communicates with the upper-level environment management layer through TCP / IP through port mapping, so an environment simulation cluster can be built to sample the environment on multiple devices simultaneously, further improving the sampling efficiency of the environment and accelerating the training of reinforcement learning algorithms. (3) The reinforcement learning environment is integrated with ROS using the openai_Gym specification, and finally encapsulated into an environment that complies with the gym interface, which simplifies the process of developing reinforcement learning in ROS. The environment that complies with the gym interface mainly implements the step and reset functions. The step function inputs the current action and outputs the state at the next moment, the reward obtained when entering the next state, and the flag bit indicating whether the current round has ended. The second step is to design the entropy regulation mechanism and point cloud processing model based on the SAC algorithm. (1) Use the entropy automatic entropy adjustment mechanism, target entropy Set to a constant value. By updating the loss function J in formula (4) ( α ) , so that the real entropy approaches the target entropy In this formula, -logπ t( a t| s }t Represents the entropy of the action output by the policy network, let Represents the difference between the actual entropy and the target entropy. If δ is greater than zero, it means that the entropy of the action output by the network is too large, and the target J is optimized by the gradient descent algorithm. ( α ) You can reduce α. The reduction of α means that the expected discounted return J ( π )The proportion of entropy in the network has become smaller, and the change in the algorithm optimization goal will cause the entropy of the network output action to decrease. By dynamically adjusting the size of α, the entropy of the network output action -logπ t( a t| s t) Can automatically stabilize at the target entropy Nearby, avoiding artificial adjustment of temperature coefficient. (2) An auxiliary temperature coefficient adjustment method is 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): In the formula, α'0 represents the minimum temperature coefficient of the algorithm, n represents the number of global steps that have been executed, and ι = 10 4 is the attenuation factor. In the early stage of the algorithm, the global step number n is very small, so the temperature coefficient α' is close to 1 at the beginning. A higher temperature coefficient can enhance the algorithm's exploratory nature in the early stage. As the global step number n increases, the algorithm's temperature coefficient α' gradually decreases, but it will not be less than its minimum value α'0, which is particularly important in tasks such as path planning. The final temperature coefficient The adjustment method is shown in formula (6). (3) Use auxiliary tasks and decoders to accelerate the convergence of the feature extraction module. The point cloud extraction module is trained using the point cloud recovery and reconstruction task, and the encoder is trained using ChamferDistance (CD) and EarthMover Distance (EMD) losses in the process. ChamferDistance measures the distance between two point clouds, and the formula is as follows: Where P and Q represent two point clouds. p∈P means that point cloud p is a point in point cloud P. Similarly, q∈Q means that q is a point in point cloud Q. |·| represents the distance in Euclidean space. Chamfer Distance mainly measures the similarity of two point clouds by calculating the distance from each point to the nearest point in the other point cloud. This metric can capture the local geometric differences between point clouds. EarthMover Distance measures the minimum transportation cost between two point clouds, also known as Wasserstein distance, and the formula is as follows: where d ij Represents a point p from the point cloud P i To point q in point cloud Q jφ:P→Q represents a bijection from point cloud P to point cloud Q. |p-φ(p)| represents the Euclidean distance between point p and point φ(p). The calculation of EMD ensures that each point is used only once, and requires that the two point clouds have the same number. EarthMover Distance measures the similarity between two point clouds by minimizing the total transportation cost between them. This loss function can intuitively reflect the overall similarity of two point clouds. In the auxiliary task design of the feature extraction network, CD loss and EMD loss are used simultaneously for point cloud reconstruction. The third step is to learn the path planning obstacle avoidance and target tracking strategies in the environment based on the improved SAC algorithm. Figure 4 shown. (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 the 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 the distance between the agent and the obstacle. A positive reward is given when the agent is far away from the obstacle, and a negative reward is given when the agent is close to the obstacle. (2) Based on the relative distance between the agent and the obstacle, the path planning reward or obstacle avoidance reward is dynamically selected. The task focus of the agent is determined by setting a threshold value of 20 m to ensure that the agent takes appropriate actions in different situations. (3) Design of target tracking AUV state and reward. A state space containing the position, velocity and position information of the navigator is designed for the target tracking AUV to achieve accurate tracking of the navigator. Two agents, the navigator and the follower, are set up. The navigator is responsible for path planning and obstacle avoidance, and the follower is responsible for maintaining a safe distance from the navigator. (4) The reward design of the follower agent is based on the change in distance between it and the leader. If the distance is kept within a certain range, a positive reward is given. If the distance exceeds the certain range, different levels of rewards are given according to the change in distance. (5) A discrimination threshold is introduced to constrain the behavior of the follower to avoid collision with the leader while ensuring the coordination between the agents in the formation. (6) To enhance the generalization ability of the follower agent, the leader adopts a semi-random strategy that combines deterministic actions with random actions to generate diverse trajectories and avoid the follower agent from overfitting to the specific leader 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 the AUV model, AUV control module, AUV perception module, flow field, and obstacles; (2) Refine the AUV motion model and flow field force to build a simulation environment with realistic mechanical properties; (3) Realize communication and data transmission between components in the simulation environment through the nodes, topics and service mechanisms of the ROS system; (4) Use Docker container technology to encapsulate the simulation environment, implement multi-instance parallel simulation, and improve the efficiency of environmental sampling; (5) According to the OpenAIGym specification, ROS is integrated with the reinforcement learning environment to encapsulate an environment that conforms to the Gym interface; (6) Set simulation environment parameters and construct challenging path planning tasks; (7) Improve the entropy regulation mechanism in the traditional SAC reinforcement learning algorithm, introduce the concept of target entropy, and automatically adjust the entropy through the loss function so that the action entropy output by the strategy is close to the target entropy; (8) Construct a feature extraction network and perform self-supervised reconstruction of point clouds through a variational autoencoder; (9) Design appropriate state space and reward function to train the agent to complete path planning and target tracking tasks; (10) Introducing the inertial navigation system and Doppler velocimeter (DVL) to simulate AUV position estimation; (11) Integrate multi-beam sonar sensors to simulate the underwater obstacle avoidance capability of AUV; (12) A flow field model based on real ocean data is used to simulate the impact of constant ocean currents on AUV motion.

2. The point cloud-based AUV path planning method according to claim 1, characterized in that: The temperature coefficient adjustment method in step (7) comprises: (1) Adopting the auxiliary temperature coefficient adjustment method, the exploration is enhanced in the early stage of the algorithm and gradually reduced 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 according to claim 1, characterized in that: The feature extraction network construction method in step (8) includes: (1) Extract point cloud features using the PointNet structure to achieve permutation invariance and robustness; (2) Combine local features with global features, use multi-layer perceptron (MLP) and maximum 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 according to claim 1, characterized in that: The reward design method in step (10) includes: (1) The reward is divided into path planning reward and obstacle avoidance reward, which is determined according to the change in the distance between the agent and the target point and the obstacle; (2) Dynamically select path planning rewards or obstacle avoidance rewards and determine the agent’s task focus by setting threshold values; (3) Design the state space and reward for the target tracking AUV, and design the reward based on the change in distance to the leader.

Images