Mobile robot path planning method based on diffusion model environment generation and dual-stage sampling

Abstract

The invention discloses a mobile robot path planning method based on diffusion model environment generation and dual-stage sampling, and the method comprises the steps: building a path search guide system with environment perception capability through introducing a path planning frame of a diffusion model and dual-stage sampling; path data samples in various environments are generated in combination with a fast expansion random tree algorithm, a diffusion model is trained to achieve the ability of predicting a heuristic area from an environment map, and the ability of the model to understand obstacle layout and navigation paths is improved. Besides, in the path execution stage, a diffusion model is used for conducting reasoning sampling on a target map, the sampling space is dynamically limited, the problems of redundant nodes and path failure caused by full-graph random sampling are effectively avoided, meanwhile, a fast marching tree algorithm is executed in a heuristic area to construct a path search tree, a low-cost and high-smoothness path result is output, and the path search efficiency is improved. And stable and efficient path generation in a complex environment is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mobile robot path planning method, in particular to a mobile robot path planning method based on diffusion model environment generation and two-stage sampling. BACKGROUND

[0002] Path planning is an important part of mobile robot research, which aims to find a collision-free path from the initial state to the target state in a known or unknown environment. Traditional path planning algorithms are divided into two major schools: sampling-based and search-based.

[0003] Sampling-based path planning algorithms (such as FMT, RRT) need to sample randomly in the entire map when generating a path, which cannot guarantee the optimality of the sampling points. When there is a narrow region between the start point and the end point, a large number of candidate nodes will be added to the list, only a few of which can guide the path into the region, thereby increasing the planning time, and even failing to find a feasible path. Even if the final path is found, there are problems of large path cost and poor smoothness. SUMMARY

[0004] The present application aims to provide a mobile robot path planning method based on diffusion model environment generation and two-stage sampling to at least solve some of the problems raised in the background.

[0005] According to one aspect of the present application, a mobile robot path planning method based on diffusion model environment generation and two-stage sampling is provided, the method comprising the following steps: Step 1, constructing a two-dimensional environment map containing random obstacles, a start point and a target point, wherein the obstacles include rectangular and circular obstacles, and setting the number of each type of obstacle to generate an obstacle set; Step 2, performing first-stage sampling, generating path sample data in various environments using a rapid expansion random tree algorithm, and constructing a training data set, wherein the rapid expansion random tree algorithm includes RRT or RRT-Connect; Step 3, training a diffusion model using the path sample data set to establish a mapping relationship from the environment map to a potential heuristic region; Step 4, in the path planning stage, inputting the target environment image scaled to a preset resolution into the trained diffusion model to complete the first inference sampling in the latent space and generate a heuristic path search region; Step 5, performing second-stage sampling, applying a fast marching tree algorithm in the heuristic region to sample and generate path nodes, constructing a path search tree and outputting the final path planning result.

[0006] Preferably, in step 2, the method for generating path sample data in multiple environments by using the RRT algorithm comprises the following steps: Step 21, generate a random two-dimensional obstacle environment map and a corresponding path planning map, input the environment information and the text prompt information containing the starting point coordinates as control conditions, and input the path information as the true label; Step 22, define the environment information and the path information as 64x64 matrices, and save them in the standard size of 512x512 pixels; Step 23, generate the path by using the RRT algorithm, set the connection step size to 10, the iteration number to 3000, and the reconnection radius to 20, and perform simplification and smoothing processing on the path; Step 24, run the RRT algorithm for 20 times in each random environment, and draw all feasible paths on the image to form a final path planning map.

[0007] Preferably, in step 3, the method for training the diffusion model comprises: Step 31, initialize the Stable Diffusion model weight, set the upper limit of the total number of maps env_max and the maximum iteration number of a single point , and ; Step 32, construct a two-dimensional coordinate system and randomly generate a map containing two types of obstacles, and the number of obstacles is k, Step 33, sample points in the environment by using the uniform sampling function UniformSample, generate a path by combining the nearest neighbor node search Nearest and the step expansion Steer; Step 34, verify the feasibility of the path by using the obstacle collision detection function ObstacleFree until the preset iteration number is reached; Step 35, input the environment map and the corresponding RRT path map into the diffusion model, and complete the training by minimizing the loss function.

[0008] Preferably, the loss function is defined as .

[0009] Where θ is the model parameter, x0 is the original image, t is the randomly sampled time step, is a preset distance hyperparameter, ε is a real noise, εθ(·) is a noise prediction network, c t is text condition information, and c f is image condition information.

[0010] Preferably, in step 33, the nearest neighbor node search includes: traversing each point in the generated tree , calculating the distance to The distance, the point where the distance is smallest is... ; Step expansion include: Along the direction Increase the direction by one distance The corresponding position after is Among them, distance These are preset hyperparameters.

[0011] Preferably, in step 34 Verifying the feasibility of the path includes using judge and or and The line connecting the points avoids obstacles. If no point on the line is in an obstacle, then it is determined that the line avoids obstacles.

[0012] Preferably, in step 5, applying the fast progress tree algorithm within the heuristic region includes the following steps: Step 51: Initialize the spanning tree T, the set of unvisited nodes (Unvisitedlist), the set of open nodes (Openlist), and the set of closed nodes (Closedlist); Step 52: Output the heuristic region from the diffusion model. All points except the starting point are added to the Unvisitedlist, and the starting point is added to the Openlist; Step 53: Extract the node z with the minimum cost from the Openlist. If z is the target point, output the path; otherwise, search for node x in the Unvisitedlist with radius r. Step 54: For each x, find the nearest node y in the Openlist within the radius r, and add it to the spanning tree T after verifying that the xy path has no collision using ObstacleFree; Step 55: Move successfully connected x into Openlist and z into Closedlist, and repeat until a path is found or Openlist is empty.

[0013] Preferably, step 5 further includes a uniform sampling process for path nodes within the heuristic region: Extract the bounding rectangle of the heuristic region; Randomly generate candidate points within the boundary and verify that the point is located within the heuristic region and does not overlap with obstacles; When the number of sampling points reaches a preset value, a set of collision-free sampling points is output for use by the fast progress tree algorithm.

[0014] The application is aimed at the path planning requirements of mobile robots in complex or unknown environments, and aims to break through the technical bottlenecks of traditional sampling path planning algorithms in efficiency, accuracy and environmental adaptability, to build a general path planning mechanism with high guidance and high robustness, to realize the coordinated improvement of path generation quality and calculation efficiency, and to enhance the autonomous navigation ability of mobile robots in multiple scenarios.

[0015] Specifically, the application introduces a diffusion model and a two-stage sampling path planning framework to establish a path search guidance system with environmental perception capability, generates path data samples in various environments combined with the Rapidly-exploring Random Trees (RRT) algorithm, trains the diffusion model to realize the ability to predict heuristic regions from the environment map, improves the understanding of the model for obstacle layout and navigation path, and uses the diffusion model to reason and sample the target map in the path execution stage, dynamically limits the sampling space, effectively avoids the redundant nodes and path failure problems caused by full-map random sampling, and at the same time, executes the Fast Marching Tree (FMT) algorithm in the heuristic region to build a path search tree, and outputs a low-cost and high-smoothness path result, realizing stable and efficient path generation in complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 is a diffusion model training flowchart for the method; Figure 2 is a diffusion model guided FMT path planning flowchart for the method; Figure 3 is a diffusion model structure diagram for the method; Figure 4 is a diffusion model training data set diagram; Figure 5 is a diffusion model guided FMT path planning result diagram. DETAILED DESCRIPTION

[0017] The technical solutions in the embodiments of the application will be described clearly and completely below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, not all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those of ordinary skill in the art without creative labor are within the scope of protection of the application.

[0018] Figure 1 is a diffusion model training flowchart, as shown in the figure, the data for training needs to be generated using the Rapidly-exploring Random Trees (RRT) algorithm, and then trained on the diffusion model.

[0019] Specifically, please refer to Figure 1According to one embodiment of the present application, a mobile robot path planning method based on diffusion model environment generation and two-stage sampling is provided, comprising the following steps: Step 1, global environment initialization is performed, a two-dimensional environment map containing random obstacles and a starting point is constructed, the obstacles include two basic shapes of rectangles and circles, and the number of each type of obstacle is set to generate an obstacle set.

[0020] Step 2, first-stage sampling is performed, path sample data sets are generated in various environments using a rapid expansion random tree algorithm, and the rapid expansion random tree algorithm includes RRT or RRT-Connect.

[0021] In one embodiment, in step 2, the RRT algorithm is used to generate path sample data in various environments, which mainly includes the following specific steps: A random two-dimensional obstacle environment map, a starting point path planning map, and a text prompt information with starting point coordinate information are generated, wherein the environment information and the prompt information are input as control conditions, the path information is used as a real label to train the network, and the path information and the environment information are defined as 64x64 matrices, finally a standard size of 512x512 pixels is saved, in the environment information, the obstacles are generated in a random manner, and two basic shapes of rectangles and circles are set; In the RRT path planning part, the search tree is expanded in a random sampling manner in the environment, new nodes are constantly tried to be connected to generate a local collision-free path, and the final path is returned until the search tree joins the target node, the RRT algorithm connection step is set to 10, the iteration number is set to 3000, and the reconnection radius is set to 20, after the path search is completed, the path is simplified and smoothed to reduce the turning points to improve the model training quality, in order to adapt to the deep neural network model training, each set of environment and path is output in the form of an image, in addition, the environment coordinate sequence is saved in JSON form for convenience as prompt information to train the model; The feasible path set is used to represent the feasible region of path planning, the RRT algorithm is run 20 times under each random environment condition, and all feasible paths are drawn on the image using line superposition to form the final path planning map.

[0022] The mobile robot path planning method according to the present application further comprises step 3 of training a diffusion model using the path sample data set to establish a mapping relationship from the environment map to the potential heuristic region.

[0023] Reference Figure 2 , Figure 2 is a diffusion model guided FMT path planning flowchart, the diffusion model receives picture information containing the environment and the start and end points, generates a heuristic region, the FMT samples in the heuristic region to generate a path.

[0024] Specifically, in step 3, the methods for training the diffusion model include the following: Step 31: Initialize the diffusion model, scale the input image, and change its resolution to... ,load The model's weights are the initial weights, and the total number of maps is set. Maximum number of iterations per point Maximum number of iterations in a single session ; Step 32: Global map initialization, check if the total number of maps has been reached. If yes, proceed to step 38; otherwise, perform environment modeling and establish a two-dimensional coordinate system.

[0025] In step 32, the specific process of constructing the node sampling function is as follows: the algorithm in , Uniform sampling is performed within the range. Where, The length of the map, This represents the width of the map.

[0026] Step 33: Parameter initialization, setting the initial position on the map. finish line Set the spanning tree Construct node sampling function Number of iterations per cycle ; Step 33 also includes the following steps: Specific method: Traverse the spanning tree For each point in the dataset, calculate the distance to a random point. The distance, the point where the distance is smallest is... ; Specific methods for handling this: Along the direction Increase the direction by one distance The corresponding position after is Among them, distance It is a pre-set hyperparameter.

[0027] Step 34: If the number of iterations in a single iteration equals the maximum number of iterations in a single iteration, output "failure"; otherwise, use... Collected ,use Find the spanning tree Mid-range nearest point ,go through Processed .

[0028] Step 35, using determine and whether the connection of the two points avoids the obstacle, if the connection avoids the obstacle, add to the tree , otherwise, perform step 34; wherein, in step 35 determine and or and whether the connection of the two points avoids the obstacle, if any point on the connection is not in the obstacle, it is determined that the connection avoids the obstacle.

[0029] Step 36, if is the target point, generate the path, otherwise, perform step 34; Step 37, determine whether the single point reaches the maximum number of iterations , if the single point reaches the maximum number of iterations, perform step 32, otherwise, perform step 33; Step 38, input the data sample containing the start and end points of the map and the corresponding path graph into the diffusion model, and train to obtain the diffusion model for mobile robot path planning, wherein the training result is the diffusion model dedicated to robot path planning.

[0030] Further, in step 38, the training target is to minimize the loss function, and the complete expression of the loss function is , The derivation process includes the following steps: The diffusion model includes a forward diffusion process and a backward inference process, wherein the forward diffusion process is a process of gradually adding Gaussian noise to the initial image; a neural network containing parameters needs to be constructed to predict the noise added at each step in the forward process, and the training process is a process of maximizing the lower bound of the log-likelihood , wherein, represents that in the forward diffusion process, given the initial image , the joint distribution from to is obtained, and the joint distribution is a known quantity determined in advance before training, Neural networks from arrive Given the joint distribution of unknowns, obtain the log-likelihood. The lower bound: in To and Minimize irrelevant constant terms Divergence is equivalent to maximizing the log-likelihood, which determines the training objective and makes the model converge. The following steps were taken to obtain: in, These are text conditional information and image conditional information, respectively. and All are constants without parameters. If the constant If the gradient is not affected during training, then If during training, Then we obtain the final training loss function: In one embodiment, reference Figure 3 , Figure 3 This is a structural diagram of the diffusion model used in this method.

[0031] This scales the input image to a resolution of 128×128. and This indicates that the image is compressed and restored using a variational autoencoder, respectively.

[0032] This indicates three downsampling layers with an output resolution of m×m. This indicates an intermediate layer with an output resolution of 8×8. This indicates three upsampling layers with an output resolution of m×m. The asterisk (*) indicates that the internal parameters of these layers will not change during training, while the parameters of similar layers without the asterisk will change during backpropagation. This indicates that the output of the downsampling layer and the corresponding upsampling output are concatenated.

[0033] It is a 1×1 convolutional layer, representing zero weights. During gradient backpropagation, the weights change. This ensures that at the beginning of training, the input passing through the zero convolutional layer has no effect on the result. As training progresses and the weights change, the input passing through the zero convolutional layer will then affect the result.

[0034] The encoder, representing "time step information," outputs a vector that can be concatenated onto the compressed image. "Lock" indicates that its internal parameters will not change during gradient backpropagation during training.

[0035] This means concatenating the encoded vector of "time step information" into the structure of the non-zero convolutional layer in the network.

[0036] The mobile robot path planning method according to the embodiments of this application further includes step 4, in the path planning stage, scaling the target environment image to a preset resolution and inputting it into the trained diffusion model, completing the first inference sampling in the latent space, and generating a heuristic path search region.

[0037] The path planning method according to the embodiments of this application further includes step 5, performing a second-stage sampling, applying a fast moving tree algorithm in the heuristic region to sample and generate path nodes, constructing a path search tree and outputting the final path planning result.

[0038] Specifically, in step 5, applying the fast progress tree algorithm within the heuristic region includes the following steps: Step 511: Global environment initialization. Construct a two-dimensional environment graph containing random obstacles and starting points. The obstacles include two basic shapes: rectangles and circles. Set the number of each type of obstacle to form an obstacle set. Step 512: Parameter initialization, setting the search radius. Set the initial position on the map. finish line Set the spanning tree Not yet on the tree The set of sample points in It has been added to the tree. Furthermore, the set of sample points for any new connections will no longer be considered. It has been added to the tree. And consider the set of sample points for new connections. ; Step 513: Input the map and start and end points as conditions into the diffusion model to obtain the heuristic region. heuristic area Except Place the point , Put in ; Step 514, if If empty, path generation fails; otherwise, from Find the point with the minimum cost If found for Then return corresponding path, if empty and not , step 515 is executed; step 515, taking points as the center, as the search radius, from selecting points , taking as the center, as the search radius, calculating the distance from to each point in , finding the corresponding with the smallest distance, using to determine whether the line connecting and avoids obstacles, if the line avoids obstacles, the edge formed by connecting and is added to the tree , otherwise, taking as the center, as the search radius, selecting new points from , until the points in , taking as the center, as the search radius, are all processed.

[0039] In step 515, determines whether the line connecting and or and avoids obstacles, if any point on the line is not in the obstacle, it is determined that the line avoids obstacles.

[0040] Step 516, points that have determined that the line connecting avoids obstacles and have added the connecting edge to the tree T are removed from and added to , points are removed from and added to , and is returned to step 514.

[0041] Further, in step 5, the second-stage sampling includes the following steps: Step 521, input acquisition, acquiring the heuristic region contour output by the diffusion model, the number of required sampling points, and the environment grid map; Step 522, boundary determination, extract the outermost rectangular boundary of the heuristic region, determine the maximum and minimum limit position of the region in the horizontal and vertical directions; Step 523, sample loop start, continue to randomly select points in the rectangular boundary range until the required number of sampling points is met; Step 524, single point detection, for each candidate sampling point, perform the following two detections: if the point is located inside the outline of the heuristic region and does not overlap with any obstacle, the point is retained, otherwise the point is discarded and sampling continues; Step 525, result return, if the number of retained sampling points meets the predetermined requirement, stop the point selection process, and based on all the sampling points that pass the detection, output the collision-free sampling set.

[0042] Reference Figure 4 In Figure 4 , the blue small blocks represent the starting points and the green small blocks represent the ending points, wherein the first to third rows are the generated random initial environment map, the RRT path planning map and the comparison map respectively. The environment map is 6.4*6.4 inches in size, wherein the black and circular shapes represent obstacles with random size and position, the white color represents free space, and the blue and red square points represent the starting points and target points randomly selected from the free space respectively. The random initial environment map randomly generates 366 pairs of starting points and target points for each environment, simulating path planning problems under different conditions.

[0043] Figure 5 The heuristic region is generated by the diffusion model in the environment, and the green generated tree and path are generated by the FMT guided based on the diffusion model in the environment. In Figure 5 , the blue small blocks represent the starting points and the red small blocks represent the ending points.

[0044] The parts not involved in the present application are the same as or can be implemented by the prior art. Although the embodiments of the present application have been shown and described, it can be understood by those skilled in the art that various changes, modifications, replacements and variations can be made to the embodiments without departing from the principles and spirits of the present application, and the scope of the present application is defined by the appended claims and their equivalents.

Claims

1. A mobile robot path planning method based on diffusion model environment generation and two-stage sampling, characterized in that, The method includes the following steps: Step 1: Construct a two-dimensional environment map containing random obstacles, a starting point, and a target point. The obstacles include rectangular and circular obstacles. Set the number of each type of obstacle to generate an obstacle set. Step 2: Perform the first stage of sampling, using the fast extended random tree algorithm to generate path sample data in various environments, and construct the training dataset; Step 3: Train the diffusion model using the path sample dataset to establish a mapping relationship for generating potential heuristic regions from the environment map; Step 4: In the path planning stage, the target environment image is scaled to a preset resolution and then input into the trained diffusion model to complete the first inference sampling in the latent space and generate a heuristic path search region. Step 5: Perform the second stage of sampling. Apply the fast moving tree algorithm to the heuristic region to sample and generate path nodes, construct the path search tree, and output the final path planning result.

2. The method according to claim 1, characterized in that, In step 2, the fast expanding random tree algorithm is used to generate path sample data under various environments, including the following steps: Step 21: Generate a random two-dimensional obstacle environment map and a corresponding path planning map. The environmental information and text prompts containing the starting point coordinates are used as control conditions input, and the path information is used as the real label. Step 22: Define the environmental information and path information as a 64×64 matrix and save it in a standard size of 512×512 pixels; Step 23: Generate a path using the fast expanding random tree algorithm, setting the connection step size to 10, the number of iterations to 3000, and the reconnection radius to 20, and then simplify and smooth the path. Step 24: Run the Fast Expanding Random Tree algorithm multiple times in each random environment, and draw all feasible paths on the image by overlaying lines to form the final path planning map.

3. The method according to claim 1, characterized in that, In step 3, the training methods for the diffusion model include: Step 31: Initialize the weights of the Stable Diffusion model, and set the maximum number of maps (env_max) and the maximum number of iterations per point. and the maximum number of iterations in a single session ; Step 32: Construct a two-dimensional coordinate system and randomly generate a map containing two types of obstacles, with k obstacles in total. Step 33: Sample points in the environment using the UniformSample function, and generate paths by combining Nearest Neighbor Search and Steer step expansion; Step 34: Use the obstacle collision detection function ObstacleFree to verify the feasibility of the path until the preset number of iterations is reached; Step 35: Input the environment graph and the corresponding fast-expanding random tree path graph into the diffusion model, and complete the training by minimizing the loss function.

4. The method according to claim 3, characterized in that, The loss function is defined as ， Where θ represents the model parameters, x0 represents the original image, and t represents the time step for random sampling. Here, ε is the preset distance hyperparameter, ε is the actual noise, εθ(·) is the noise prediction network, and c t For textual conditional information, c f This refers to image condition information.

5. The method according to claim 4, characterized in that, In step 33, Nearest neighbor search Includes: Traversing the spanning tree For each point in the array, calculate the distance to a random point. The distance, the point with the smallest distance is ; Step expansion Includes: minimum distance point Along pointing to a random point Increase distance in the direction The corresponding position after is Among them, distance These are preset hyperparameters.

6. The method according to claim 5, characterized in that, In step 34 Verifying the feasibility of the path includes using judge and The line connecting the points avoids obstacles. If no point on the line is in an obstacle, then it is determined that the line avoids obstacles.

7. The method according to claim 1, characterized in that, In step 5, applying the fast progress tree algorithm within the heuristic region includes the following steps: Step 51: Initialize the spanning tree T, the set of unvisited nodes (Unvisitedlist), the set of open nodes (Openlist), and the set of closed nodes (Closedlist); Step 52: Output the heuristic region from the diffusion model. All points except the starting point are added to the Unvisitedlist, and the starting point is added to the Openlist; Step 53: Extract the node z with the minimum cost from the Openlist. If z is the target point, output the path; otherwise, search for node x in the Unvisitedlist with radius r. Step 54: For each x, find the nearest node y in the Openlist within the radius r, and add it to the spanning tree T after verifying that the xy path has no collision using ObstacleFree; Step 55: Move successfully connected x into Openlist and z into Closedlist, and repeat until a path is found or Openlist is empty.

8. The method according to claim 1, characterized in that, Step 5 also includes a uniform sampling process for path nodes within the heuristic region: Extract the bounding rectangle of the heuristic region; Randomly generate candidate points within the boundary and verify that the point is located within the heuristic region and does not overlap with obstacles; When the number of sampling points reaches a preset value, a set of collision-free sampling points is output for use by the fast progress tree algorithm.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the method as described in any one of claims 1-8.

10. A computer program product, comprising a computer program, characterized in that, The computer program is executed to implement the method as described in any one of claims 1-8.

Images