Sampling path planning method and system based on adaptive dynamic batch optimization

Abstract

This invention provides a sampling path planning method and system based on adaptive dynamic batch optimization. The method includes: when the edge processing queue (QE) is empty and a new batch needs to be created, calculating the batch size according to an adaptive batch size calculation formula. The system includes an adaptive batch manager, a search progress monitor, a solution quality evaluator, an environment complexity analyzer, and a batch size calculator. The adaptive batch manager is configured to: receive iteration information from the search progress monitor, receive cost improvement data from the solution quality evaluator, receive environment information from the environment complexity analyzer, and calculate the batch size parameter using the batch size calculator according to the adaptive batch size calculation formula. According to this invention, it is possible to significantly improve planning efficiency, enhance environmental adaptability, support real-time dynamic applications, meet the practical needs of multi-objective optimization adaptation, reduce computational resource consumption, and improve algorithm robustness when planning paths.

Description

Technical Field

[0001] This invention belongs to the field of industrial and logistics automation, and more specifically, relates to a sampling path planning method and system based on adaptive dynamic batch optimization. Background Technology

[0002] The BIT (Batch Informed Trees) algorithm is a sampling-based optimal path planning algorithm that combines batch sampling, elliptic constraints, and heuristic pruning strategies to efficiently find the optimal path from the starting point to the target. Its core process can be divided into four stages: initialization, batch sampling and expansion, path verification and update, and pruning optimization.

[0003] Existing BIT* algorithms mainly suffer from the following technical limitations: 1. The BIT* algorithm uses a fixed batch size m, which cannot adapt to the needs of different search stages. In the early stages of the search, rapid exploration is required, and excessively large batches will introduce redundant calculations. In the later stages of the search, fine optimization is required, and excessively small batches will lead to slow convergence.

[0004] II. The BIT* algorithm is designed for static environments. When the environment changes, it needs to be completely replanned. It cannot effectively reuse historical search information and lacks a fast detection mechanism for changed areas.

[0005] III. BIT * Only optimizes paths for a single objective (path length), which cannot handle the multi-objective requirements in real-world applications and lacks consideration for indicators such as path smoothness and safety.

[0006] IV. The BIT algorithm treats all samples equally, lacking distinction of importance. Samples located near narrow channels are of higher value but are not prioritized. Samples with historically failed connections are still repeatedly tried, wasting computational resources. Summary of the Invention

[0007] In view of this, the present invention provides a sampling path planning method and system based on adaptive dynamic batch optimization.

[0008] According to a first aspect of the present invention, a sampling path planning method based on adaptive dynamic batch optimization is provided, the method comprising the following steps: When the edge processing queue QE is empty and a new batch needs to be created, the batch size is calculated according to a predetermined adaptive batch size calculation formula. The adaptive batch size calculation formula is as follows: mk=mbase×αk×(1+β×Ienv)×(1+γ×Rimprove), where mk is the number of samples in the k-th batch, mbase is the base batch size, α is the batch growth coefficient, β is the environmental complexity coefficient, Ienv is the environmental complexity index, γ is the improvement rate coefficient, and Rimprove is the solution quality improvement rate.

[0009] Optionally, the sampling path planning method further includes an environmental change detection step, which further includes: Detect the presence of environmental change ΔE(t), ΔE(t) = \{x∈X|Obst(x) \neq Obs_{t} 1}(x)\}, where Obst(x) represents the collision state of state x at time t, neq Obs_{t 1}(x) represents t The collision state of state x at time 1; If an environmental change is detected, perform the following operations: Calculate the set of tree nodes affected by environmental changes, Vaffected, where Vaffected = {v ∈ V |} x∈ΔE,∣∣v x ||≤raffected}, where raffected is the radius of influence, v represents a tree node, and V represents the set of tree nodes. x represents the existence of x, that is, the existence of a state x belonging to the set of environmental changes ΔE. ΔE represents the set of environmental changes; The incremental update strategy of the LPA* algorithm is used to perform local pruning, cost recalculation, local replanning, and information reuse on the tree node set Vaffected.

[0010] Optionally, the sampling path planning method further includes a step of optimizing the path by comprehensively considering path length, smoothness, and safety. This step of optimizing the path by comprehensively considering path length, smoothness, and safety further includes: Define the total cost function C_total, C_total = w_1 × C_length + w_2 × C_smooth + w_3 × C_safety, where C_length is the path length cost, C_smooth is the smoothness cost, C_safety is the safety cost, and w_1, w_2 and w_3 are weighting coefficients; Extending heuristics to multi-objective spaces: _{multi}(x) = [ _{length}(x), _{smooth}(x), _{safety}(x)], where, _{multi}(x) is a multi-objective synthesis heuristic function vector. _{length}(x) is the heuristic estimate of the path length target. _{smooth}(x) is the heuristic estimate of the path smoothness objective. [{safety}(x)] represents the heuristic estimate of the path safety target; Scalarization is performed using a weighted sum method: _weighted(x) = w_1 × _length(x) + w_2 × _smooth(x) + w_3 × _safety(x), where, _weighted(x) is a weighted scalarized multi-objective heuristic estimate; Maintain a set of Pareto fronts: P = {π∣¬ π′∈P,π′ π}, where P represents the Pareto front solution set, π represents a solution in the Pareto front solution set, and ¬ π′ represents the absence of π′. π means that π′ is no worse than π on all targets and is strictly better than π on at least one target.

[0011] Optionally, after the step of calculating the batch size according to a predetermined adaptive batch size calculation formula when the edge processing queue QE is empty and a new batch needs to be created, the sampling path planning method further includes a sample sampling and processing step, which further includes: Calculate an importance score for each sample: Importance(x) = α1 × HeuristicValue(x) + β1 × ConnectivityPotential(x) + γ1 × SuccessProbability(x), where HeuristicValue(x) is the heuristic value, ConnectivityPotential(x) is the connectivity potential, SuccessProbability(x) is the probability of successful connection, α1 is the heuristic value weight, β1 is the connectivity potential weight, and γ1 is the probability of successful connection weight. Samples were collected based on an importance-based sampling method. When processing samples, high-importance samples are processed with the highest priority, medium-importance samples with the second highest priority, and low-importance samples are processed only when computational resources are abundant.

[0012] According to a second aspect of the present invention, a sampling path planning system based on adaptive dynamic batch optimization is provided, which is used to implement any of the above-mentioned sampling path planning methods based on adaptive dynamic batch optimization. The sampling path planning system includes an adaptive batch manager, a search progress monitor, a solution quality evaluator, an environment complexity analyzer, and a batch size calculator; The adaptive batch manager is configured as follows: Receive iteration information from the search progress monitor, the iteration information including the current iteration number and search stage. Receive cost improvement data from the solution quality evaluator, which is used to calculate the solution quality improvement rate (Rimprove). Receive environmental information from the environmental complexity analyzer, which is used to calculate the environmental complexity index Ienv. The batch size parameter is calculated using the batch size calculator based on the adaptive batch size calculation formula.

[0013] Optionally, the sampling path planning system also includes a multi-objective heuristic engine, a path length heuristic calculator, a smoothness heuristic calculator, a safety heuristic calculator, and a Pareto frontier manager; The multi-objective heuristic engine is configured as follows: It receives obstacle distribution information from an environmental map, providing the environmental basis for calculations in path length heuristic calculators, smoothness heuristic calculators, and safety heuristic calculators. The system receives target weight parameters from the weight configuration for weighted calculation of multi-objective costs. These target weight parameters include w_1, w_2, and w_3. Obtain multi-objective heuristic values, including heuristic results corresponding to path length, smoothness, and security. It engages in bidirectional data interaction with the Pareto frontier manager to receive multi-objective cost data for new paths and output non-dominated solution update instructions.

[0014] Optionally, the sampling path planning system also includes an intelligent sampler, a sample importance evaluator, an importance-weighted sampler, and a sample history database; The intelligent sampler is configured as follows: Receive the batch size parameter from the adaptive batch manager to determine the number of samples to be generated in each batch. Receive sample importance distribution data from the sample importance evaluator. New samples conforming to the importance distribution are generated using an importance-weighted sampler and output to the implicit RGG builder to provide sample support for tree structure expansion. The sample information and connection attempt results of each sampling are updated to the sample history database.

[0015] Optionally, the sampling path planning system also includes an incremental replanner, an environmental change detector, an affected area calculator, a local update executor, and an information reuse manager; The incremental replanner is configured as follows: Receive real-time environmental data from an environmental change detector, which is used to monitor whether the environment has changed. The environmental change detection results are output to the affected area calculator, triggering the calculation of the Vaffected set of tree nodes affected by the environmental changes. It performs bidirectional data interaction with the tree structure storage, reads information about the current tree nodes and edges used to identify the affected areas from the tree structure storage, and outputs instructions for local pruning, edge updating, and node cost recalculation to the tree structure storage through the local update executor, so as to realize the local adjustment of the tree structure. Extract historical tree structure information that has not been affected by environmental changes using the information reuse manager.

[0016] Optionally, the sampling path planning system also includes a core search engine, a vertex expander, an edge processor, a queue manager, and a collision detection interface; The core search engine is configured as follows: It receives multi-objective heuristic values ​​from the multi-objective heuristic engine, providing optimality evaluation criteria for vertex expansion and edge selection; Receive new sample data from the smart sampler as a source of candidate nodes for vertex expansion; The collision detection interface interacts with the collision detection module to verify the collision feasibility of candidate edges and obtain the actual edge cost data. The tree structure is expanded and rewiring through vertex expanders and edge processors, and the order of the vertex expansion queue QV and the edge processing queue QE is maintained through a queue manager. The optimal path and cost data of new paths discovered during the search process are output to the Pareto Front Manager to update the Pareto Front solution set.

[0017] The beneficial effects of this invention are as follows: 1) When creating a new batch and determining the batch size, this invention introduces an adaptive batch size calculation formula, and then determines the batch size parameter based on the adaptive batch size calculation formula. With this setting, in the early stage of the search (when k is small), the number of samples mk in the kth batch is small, which can be explored quickly and avoid redundant calculations; in the middle stage of the search, the number of samples mk in the kth batch is moderate, balancing exploration and utilization; in the later stage of the search (when Rimprove approaches 0), the number of samples mk in the kth batch increases, accelerating convergence to the optimal solution; in complex environments (when Ienv is large), the batch size is automatically increased to improve connectivity.

[0018] 2) Given the shortcomings of existing BIT* algorithms, which are only designed for static environments, this invention establishes an environmental change detection mechanism, namely, through ΔE(t)=\{x∈X∣Obst(x) \neq Obs_{t 1}(x) \}, detect whether there is an environmental change; when an environmental change is detected, calculate the set of tree nodes affected by the environmental change, Vaffected, and use the incremental update strategy of LPA* algorithm to perform local pruning, cost recalculation, local replanning and information reuse on the set of tree nodes Vaffected; with this setting, the environmental change detection latency can be less than 10 milliseconds, the local replanning time can be reduced by more than 80% compared with the global replanning, and it supports complex scenarios such as dynamic obstacles and moving targets, meeting the real-time application needs of autonomous driving, human-machine collaboration and other applications.

[0019] 3) This invention comprehensively considers path length, smoothness, and safety for path optimization, and specifically achieves this by defining a comprehensive cost function C_total, extending the heuristic function to a multi-objective space, using a weighted sum method for scalarization, and maintaining a set of Pareto fronts. This setup can simultaneously optimize multiple objectives, namely path length, smoothness, and safety, and can provide a Pareto front solution set for users to select according to specific needs, support dynamic adjustment of objective weights to adapt to different application scenarios, and make the generated path more suitable for actual robot execution.

[0020] 4) When sampling samples, this invention calculates an importance score for each sample, thereby achieving importance sampling of the samples; when processing samples, this invention adopts a delayed processing strategy for low-importance samples, processes high-importance samples with the first priority, processes medium-importance samples with the second priority, and processes low-importance samples only when computing resources are abundant; this setting can prioritize the processing of high-value samples, improve search efficiency; reduce redundant collision detection, improve computing resource utilization by 30-50%; allocate more computing resources in key areas such as narrow passages; and avoid repeated attempts on low-value samples.

[0021] Other features and advantages of the present invention will be described in detail in the following detailed description section. Attached Figure Description

[0022] The present invention can be better understood by referring to the following description taken in conjunction with the accompanying drawings, in which the same or similar reference numerals are used throughout the drawings to denote the same or similar parts.

[0023] Figure 1 A flowchart illustrating the implementation of a sampling path planning method based on adaptive dynamic batch optimization according to an embodiment of the present invention is shown. Figure 2 A flowchart illustrating the specific implementation of the sampling path planning method based on adaptive dynamic batch optimization according to an embodiment of the present invention is shown. Figure 3 A comparison diagram of the technical advantages of the ADBO-BIT* algorithm over the BIT* algorithm according to an embodiment of the present invention is shown; Figure 4 A comparison diagram of the technical advantages of the ADBO-BIT* algorithm over the RRT* algorithm according to an embodiment of the present invention is shown; Figure 5 A comparison diagram showing the technical advantages of the ADBO-BIT* algorithm over the FMT* algorithm according to an embodiment of the present invention is provided. Detailed Implementation

[0024] To enable those skilled in the art to more fully understand the technical solutions of the present invention, exemplary embodiments of the present invention will be described more comprehensively and in detail below with reference to the accompanying drawings. Obviously, the one or more embodiments of the present invention described below are merely one or more specific ways to implement the technical solutions of the present invention, and are not exhaustive. It should be understood that other ways belonging to a general inventive concept can be used to implement the technical solutions of the present invention, and should not be limited to the embodiments described exemplary. Based on one or more embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0025] Example: Figure 1 A flowchart illustrating the implementation of the sampling path planning method based on adaptive dynamic batch optimization according to an embodiment of the present invention is shown. (Refer to...) Figure 1 The sampling path planning method based on adaptive dynamic batch optimization in this invention includes three core stages: initialization, batch processing loop, and dynamic environment response.

[0026] The initialization phase includes: ① Receive planning tasks (start point xstart, target point xgoal, environment map, optimization target weights); ② Initialize the tree structure T = (V, E), where V = {x_start}, E = ; ③ Initialize the sample set X_samples=x_goal; ④ Initialize the queues (vertex expansion queue QV=V, edge processing queue); ⑤ Calculate the initial environment complexity index Ienv; ⑥ Set the initial batch size m0.

[0027] The batch processing cycle includes: ① Batch creation: When QE is empty, calculate mk according to the formula, generate mk new samples by importance sampling, update the connection radius r_k of RGG, prune the tree structure, and add all vertices to QV; ② Vertex expansion: Select the best vertex v from QV, find the r_k neighborhood sample X_near of v, calculate the multi-objective heuristic cost of edge (v,x), add effective edges to QE, and consider rewiring edges for new vertices; ③ Edge processing: Select the best edge (v,x) from QE, perform collision detection and calculate the actual cost. If the edge provides a better path, perform rewiring (x is in the tree) or expansion (x is not in the tree) operation and update the cost of descendant nodes. ④ Solution extraction and Pareto front update: When xgoal is reached, the path is extracted and the Pareto front is updated. If the stopping condition is met, the front is output.

[0028] The dynamic environment response phase includes: ① Continuously monitor environmental changes; ② If a change is detected, the affected area (Vaffected) is calculated; ③ Prune branches locally, recalculate costs, and replan locally for affected nodes; ④ Update the Pareto frontier.

[0029] Furthermore, in this embodiment of the invention, during the batch creation stage of the batch processing loop, when the edge processing queue QE is empty and a new batch needs to be created, the batch size is calculated according to a predetermined adaptive batch size calculation formula. The adaptive batch size calculation formula is as follows: mk=mbase×αk×(1+β×Ienv)×(1+γ×Rimprove), where mk is the number of samples in the k-th batch, mbase is the base batch size, α is the batch growth coefficient (used to control the growth rate of the batch number with the number of iterations), β is the environment complexity coefficient (used to adjust the influence weight of the environment complexity index Ienv on the batch size), Ienv is the environment complexity index, γ is the improvement rate coefficient (used to adjust the influence weight of the solution quality improvement rate Rimprove on the batch size), and Rimprove is the solution quality improvement rate.

[0030] Specifically, in this embodiment of the invention, the base batch size mbase is set to 100; 0 < batch growth coefficient α ≤ 1.1, which is used to control the growth of batches with iteration; 0.1 ≤ environmental complexity coefficient β ≤ 0.5; the environmental complexity index Ienv is calculated based on obstacle density and the number of narrow passages; 0.1 ≤ improvement rate coefficient γ ≤ 0.3; the solution quality improvement rate Rimprove is calculated as the cost reduction ratio of the most recent N batches.

[0031] Furthermore, in this embodiment of the invention, during the dynamic environment response phase, the presence of an environmental change ΔE(t) is first detected, where ΔE(t) = \{x∈X∣Obst(x) \neq Obs_{t 1}(x) \}, where Obst(x) represents the collision state of state x at time t, neq Obs_{t 1}(x) represents t 1. Collision state of state x at time 1; If an environmental change is detected, perform the following operations: Calculate the set of tree nodes affected by the environmental change, Vaffected, where Vaffected = {v ∈ V | ... x∈ΔE,∣∣v x ||≤raffected}, where raffected is the radius of influence, v represents a tree node, and V represents the set of tree nodes. x represents the existence of x, that is, the existence of state x belonging to the environmental change set ΔE, where ΔE represents the environmental change set; the incremental update strategy of LPA* algorithm is used to perform local pruning, cost recalculation, local replanning and information reuse on the tree node set Vaffected.

[0032] Specifically, in this embodiment of the invention, the influence radius raffected is dynamically adjusted according to the size of the changed region; in the local pruning stage, all edges passing through the changed region are removed; in the cost recalculation stage, the g value of the affected nodes is recalculated; in the local replanning stage, only the affected subtrees are re-expanded; in the information reuse stage, the unaffected tree structure is completely preserved to achieve information reuse.

[0033] Furthermore, in this embodiment of the invention, during the edge processing stage of the batch processing loop, path optimization is performed by comprehensively considering path length, smoothness, and security, specifically including the following operations: Define the total cost function C_total, C_total = w_1 × C_length + w_2 × C_smooth + w_3 × C_safety, where C_length is the path length cost, C_smooth is the smoothness cost, C_safety is the safety cost, and w_1, w_2 and w_3 are weighting coefficients; Extending heuristics to multi-objective spaces: _{multi}(x) = [ _{length}(x), _{smooth}(x), _{safety}(x)], where, _{multi}(x) is a multi-objective synthesis heuristic function vector. _{length}(x) is the heuristic estimate of the path length target. _{smooth}(x) is the heuristic estimate of the path smoothness objective. [{safety}(x)] represents the heuristic estimate of the path safety target; Scalarization is performed using a weighted sum method: _weighted(x) = w_1 × _length(x) + w_2 × _smooth(x) + w_3 × _safety(x), where, _weighted(x) is a weighted scalarized multi-objective heuristic estimate; Maintain a set of Pareto fronts: P = {π∣¬ π′∈P,π′ π}, where P represents the Pareto front solution set (the set of non-dominated solutions), and π represents a solution (a path) in the Pareto front solution set. π′ represents the absence of π′. π means that π′ is no worse than π on all targets and is strictly better than π on at least one target.

[0034] Specifically, in this embodiment of the invention, the path length cost C_length is calculated as the sum of the distances of each segment; the smoothness cost C_smooth is calculated as the rate of change of the angle between adjacent segments; the safety cost C_safety is calculated as the reciprocal of the minimum distance from the path to the obstacle; w_1, w_2 and w_3 can be adjusted according to application requirements.

[0035] Furthermore, in this embodiment of the invention, after determining the batch size mk according to a predetermined adaptive batch size calculation formula, mk new samples are generated through importance sampling; wherein, the importance sampling specifically involves: Calculate an importance score for each sample: Importance(x) = α1 × HeuristicValue(x) + β1 × ConnectivityPotential(x) + γ1 × SuccessProbability(x), where HeuristicValue(x) is the heuristic value, ConnectivityPotential(x) is the connectivity potential, SuccessProbability(x) is the probability of successful connection, α1 is the heuristic value weight, β1 is the connectivity potential weight, and γ1 is the probability of successful connection weight. Samples are collected based on importance sampling, resulting in mk new samples; When processing mk new samples, high-importance samples are processed with the first priority, medium-importance samples with the second priority, and low-importance samples are processed only when computational resources are abundant.

[0036] Specifically, in this embodiment of the invention, the heuristic value HeuristicValue(x) is calculated as follows: The reciprocal of θ, ConnectivityPotential(x) is calculated based on the density of nearby samples, and SuccessProbability(x) is obtained based on historical connection records. Importance sampling is used instead of uniform sampling, i.e., x ~ ImportanceWeightedDistribution(X_free), and high-importance regions get more sampling opportunities. High-importance samples are those with Importance > θhigh, medium-importance samples are those with Importance greater than θlow and not greater than θhigh, and low-importance samples are those with Importance ≤ θlow. θhigh and θlow are the high-importance threshold and low-importance threshold, respectively.

[0037] Specifically, in this embodiment of the invention, the heuristic value weight α1 is used to allocate the proportion of HeuristicValue(x) in the importance score, α1≥0, and α1+β1+γ1=1 (normalized weight, which can be dynamically adjusted according to the scenario; when heuristics are prioritized, α1 takes a larger value); the connectivity potential weight β1 is used to allocate the proportion of ConnectivityPotential(x) in the importance score, β1≥0, and α1+β1+γ1=1 (normalized weight; β1 can be increased in narrow channel scenarios, prioritizing samples with strong connectivity); the connection success probability weight γ1 is used to allocate the proportion of SuccessProbability(x) in the importance score, γ1≥0, and α1+β1+γ1=1 (normalized weight; γ1 can be increased in complex environments, prioritizing samples with high historical connection success rates).

[0038] Specifically, Figure 2 A flowchart illustrating the specific implementation of the sampling path planning method based on adaptive dynamic batch optimization according to an embodiment of the present invention is shown. (Refer to...) Figure 2 The sampling path planning method based on adaptive dynamic batch optimization in this embodiment of the invention specifically includes the following steps: Step S1: Initialization.

[0039] Input: starting point xstart, target point xgoal, environment map, target weights [w1, w2, w3]; Initialize the tree, sample set Xsamples=xgoal, and queue; Calculate the initial environment complexity Ienv, and set the batch counter k=0.

[0040] Step S2: Check the stopping conditions.

[0041] If the maximum number of iterations / time limit is reached, or the Pareto front converges and meets the accuracy requirements, proceed to step S12; otherwise, proceed to step S3.

[0042] Step S3: Create a new batch (if QE is empty).

[0043] Calculate batch size: mk = mbase × αk × (1 + β × Ienv) × (1 + γ × Rimprove); Importance sampling generates mk new samples, and the connection radius r_k is updated; A pruned tree structure, with all vertices marked as "to be expanded" and added to a QV (Queries and Vertices); k = k + 1.

[0044] Step S4: Check the vertex queue.

[0045] If QV is empty, proceed to step S3; otherwise, calculate BestQueueValue(QV) and BestQueueValue(QE).

[0046] Step S5: Vertex expansion.

[0047] When BestQueueValue(QV)≤BestQueueValue(QE): Select the optimal vertex v from QV; Call ExpandVertex(v) to expand the neighborhood edges of v; Effectively incorporate QE.

[0048] Step S6: Edge processing.

[0049] Choose the optimal edge (v,x) from the QE; Collision detection, calculate the true cost c(v,x); If an edge cannot improve the current solution, discard the edge.

[0050] Step S7: Update the tree structure.

[0051] If gT(v) + c(v,x) <gT(x): If x∈V: remove the original incoming edge x and add a new edge (v,x); If x V: Add x to V, add edge (v,x), and add x to QV.

[0052] Step S8: Prune the side queue.

[0053] Remove all those that meet the requirements of QE. The edge.

[0054] Step S9: Check that the target has arrived.

[0055] If x = xgoal and a better path is found: Extract the current path π; Update the Pareto frontier: P = P∪π; Calculate the solution improvement rate (Rimprove).

[0056] Step S10: Environmental Change Detection (Executed in Parallel) Detect environmental changes ΔE; If a change is detected: Calculate the affected node set Vaffected; Perform local pruning and recalculate costs on Vaffected. The affected nodes are added to QV for local replanning.

[0057] Step S11: Return to step S2.

[0058] Step S12: Output the result.

[0059] Output the Pareto front solution set P and end the process.

[0060] Specifically, Figure 3 A comparison diagram of the technical advantages of the ADBO-BIT* algorithm compared to the BIT* algorithm in this invention is shown. Figure 4 A comparison diagram of the technical advantages of the ADBO-BIT* algorithm compared to the RRT* algorithm in this invention is shown. Figure 5 A comparison diagram showing the technical advantages of the ADBO-BIT* algorithm compared to the FMT* algorithm in this invention is provided. (Refer to...) Figures 3 to 5 The sampling path planning method based on adaptive dynamic batch optimization (ADBO-BIT* algorithm) of this invention has significant technical advantages over the BIT* algorithm in terms of batch size, environmental adaptability, optimization objective, sampling strategy, and parameter tuning. The sampling path planning method based on adaptive dynamic batch optimization (ADBO-BIT* algorithm) of this invention also has significant technical advantages over the RRT* algorithm in terms of initial solution quality, convergence speed, high-dimensional performance, and anytime characteristics. Furthermore, the sampling path planning method based on adaptive dynamic batch optimization (ADBO-BIT* algorithm) of this invention has significant technical advantages over the FMT* algorithm in terms of anytime characteristics, dynamic environment, memory efficiency, and narrow channel handling.

[0061] Accordingly, based on the sampling path planning method based on adaptive dynamic batch optimization in the embodiments of the present invention, the embodiments of the present invention also propose a sampling path planning system based on adaptive dynamic batch optimization.

[0062] The sampling path planning system based on adaptive dynamic batch optimization in this invention includes an adaptive batch manager, a search progress monitor, a solution quality evaluator, an environment complexity analyzer, and a batch size calculator. The adaptive batch manager is configured as follows: Receive iteration information from the search progress monitor, the iteration information including the current iteration number and search stage. Receive cost improvement data from the solution quality evaluator, which is used to calculate the solution quality improvement rate (Rimprove). Receive environmental information from the environmental complexity analyzer, which is used to calculate the environmental complexity index Ienv. The batch size parameter is calculated using the batch size calculator based on the adaptive batch size calculation formula.

[0063] Specifically, in this embodiment of the invention, the adaptive batch manager outputs the batch size parameter to the intelligent sampler to guide it in generating the corresponding number of samples.

[0064] Furthermore, the sampling path planning system of this embodiment also includes a multi-objective heuristic engine, a path length heuristic calculator, a smoothness heuristic calculator, a safety heuristic calculator, and a Pareto front manager; The multi-objective heuristic engine is configured as follows: It receives obstacle distribution information from an environmental map, providing the environmental basis for calculations in path length heuristic calculators, smoothness heuristic calculators, and safety heuristic calculators. The system receives target weight parameters from the weight configuration for weighted calculation of multi-objective costs. These target weight parameters include w_1, w_2, and w_3. Obtain multi-objective heuristic values, including heuristic results corresponding to path length, smoothness, and security. It engages in bidirectional data interaction with the Pareto frontier manager to receive multi-objective cost data for new paths and output non-dominated solution update instructions.

[0065] Specifically, in this embodiment of the invention, the multi-objective heuristic engine outputs multi-objective heuristic values ​​to the vertex expander and edge processor, providing an evaluation basis for the optimal selection of vertices and edges; the multi-objective heuristic engine and the Pareto front manager perform bidirectional data interaction, receiving multi-objective cost data of new paths on the one hand, and outputting non-dominated solution update instructions to it on the other hand, jointly maintaining the Pareto front solution set.

[0066] Furthermore, the sampling path planning system of this embodiment of the invention also includes an intelligent sampler, a sample importance evaluator, an importance-weighted sampler, and a sample history database; The intelligent sampler is configured as follows: Receive the batch size parameter mk from the adaptive batch manager to determine the number of samples to be generated in each batch. Receive sample importance distribution data from the sample importance evaluator. New samples conforming to the importance distribution are generated using an importance-weighted sampler and output to the implicit RGG builder to provide sample support for tree structure expansion. The sample information and connection attempt results of each sampling are updated to the sample history database.

[0067] Specifically, in this embodiment of the invention, the sample importance evaluator calculates the sample importance distribution data based on heuristic value, connectivity potential, and connection success probability; the intelligent sampler updates the sample information and connection attempt results of each sampling to the sample history database to provide historical data support for subsequent sample importance evaluation.

[0068] Furthermore, the sampling path planning system of this embodiment of the invention also includes an incremental replanner, an environmental change detector, an affected area calculator, a local update executor, and an information reuse manager; The incremental replanner is configured as follows: Receive real-time environmental data from an environmental change detector, which is used to monitor whether the environment has changed, such as obstacle movement or target point change. The environmental change detection results are output to the affected area calculator, triggering the calculation of the Vaffected set of tree nodes affected by the environmental changes. It performs bidirectional data interaction with the tree structure storage, reads information about the current tree nodes and edges used to identify the affected areas from the tree structure storage, and outputs instructions for local pruning, edge updating, and node cost recalculation to the tree structure storage through the local update executor, so as to realize the local adjustment of the tree structure. By using the information reuse manager to extract historical tree structure information that has not been affected by environmental changes, redundant calculations are avoided and replanning efficiency is improved.

[0069] Furthermore, the sampling path planning system of this embodiment of the invention also includes a core search engine, a vertex expander, an edge processor, a queue manager, and a collision detection interface; The core search engine is configured as follows: It receives multi-objective heuristic values ​​from the multi-objective heuristic engine, providing optimality evaluation criteria for vertex expansion and edge selection; Receive new sample data from the smart sampler as a source of candidate nodes for vertex expansion; The collision detection interface interacts with the collision detection module to verify the collision feasibility of candidate edges (connections between vertices and samples) and obtain the actual edge cost data. The tree structure is expanded and rewiring through vertex expanders and edge processors, and the order of the vertex expansion queue QV and the edge processing queue QE is maintained through a queue manager. The optimal path and cost data of new paths discovered during the search process are output to the Pareto Front Manager to update the Pareto Front solution set.

[0070] The embodiments of the present invention have the following beneficial effects: (1) Significantly improve planning efficiency: The time to find a solution of equal quality in the R2 stochastic environment is reduced by 40% compared to the BIT* algorithm and by 80% compared to the RRT* algorithm; the speed of convergence to the optimal solution in the R8 high-dimensional space is increased by 2 times compared to the BIT* algorithm and by 10 times compared to the RRT* algorithm; the initial solution discovery time of the 14-DOF dual robotic arm is reduced from 4 seconds to 1.5 seconds.

[0071] (2) Enhanced environmental adaptability: Automatically adapts to environments of different complexity without manual parameter adjustment; success rate in narrow passage scenarios increases from 70% to 95%; reduces batch size and redundant calculations in open environments.

[0072] (3) Supports real-time dynamic applications: environmental change detection delay < 10 milliseconds; local replanning time reduced by more than 80%; supports replanning frequency above 50Hz to meet the real-time requirements of autonomous driving.

[0073] (4) Multi-objective optimization to adapt to actual needs: Simultaneously optimize objectives such as path length, smoothness, and safety; generate paths to reduce jitter and energy consumption during robot execution; provide Pareto frontier solutions to support personalized selection.

[0074] (5) Reduced computational resource consumption: The number of collision detections is reduced by 30-50%; memory usage is reduced by 20-30%; it can run on resource-constrained platforms, avoiding repeated attempts on low-value samples.

[0075] (6) Improve the robustness of the algorithm: the adaptive mechanism ensures stable performance in different scenarios; multi-objective optimization avoids suboptimal solutions for a single objective; incremental replanning ensures the continuity of planning in dynamic environments.

[0076] While one or more embodiments of the present invention have been described above, those skilled in the art will recognize that the present invention can be implemented in any other form without departing from its spirit and scope. Therefore, the embodiments described above are illustrative and not restrictive, and many modifications and substitutions will be apparent to those skilled in the art without departing from the spirit and scope of the invention as defined in the appended claims.

Claims

1. A sampling path planning method based on adaptive dynamic batch optimization, characterized in that, Includes the following steps: When the edge processing queue QE is empty and a new batch needs to be created, the batch size is calculated according to a predetermined adaptive batch size calculation formula. The adaptive batch size calculation formula is as follows: mk=mbase×αk×(1+β×Ienv)×(1+γ×Rimprove), where mk is the number of samples in the k-th batch, mbase is the base batch size, α is the batch growth coefficient, β is the environmental complexity coefficient, Ienv is the environmental complexity index, γ is the improvement rate coefficient, and Rimprove is the solution quality improvement rate.

2. The sampling path planning method based on adaptive dynamic batch optimization according to claim 1, characterized in that, It also includes a step of detecting environmental changes, which further includes: Detect the presence of environmental change ΔE(t), ΔE(t) = \{x∈X|Obst(x) \neq Obs_{t} 1}(x) \}, where Obst(x) represents the collision state of state x at time t, neq Obs_{t 1}(x) represents t The collision state of state x at time 1; If an environmental change is detected, perform the following operations: Calculate the set of tree nodes affected by environmental changes, Vaffected, where Vaffected = {v ∈ V |} x∈ΔE,∣∣v x ||≤raffected}, where raffected is the radius of influence, v represents a tree node, and V represents the set of tree nodes. x represents the existence of x, that is, the existence of a state x belonging to the set of environmental changes ΔE. ΔE represents the set of environmental changes; The incremental update strategy of the LPA* algorithm is used to perform local pruning, cost recalculation, local replanning, and information reuse on the tree node set Vaffected.

3. The sampling path planning method based on adaptive dynamic batch optimization according to claim 2, characterized in that, It also includes a step of optimizing the path by comprehensively considering path length, smoothness, and safety, and this step of optimizing the path by comprehensively considering path length, smoothness, and safety further includes: Define the total cost function C_total, C_total = w_1 × C_length + w_2 × C_smooth + w_3 × C_safety, where C_length is the path length cost, C_smooth is the smoothness cost, C_safety is the safety cost, and w_1, w_2 and w_3 are weighting coefficients; Extending heuristics to multi-objective spaces: _{multi}(x) = [ _{length}(x), _{smooth}(x), _{safety}(x)], where, _{multi}(x) is a multi-objective synthesis heuristic function vector. _{length}(x) is the heuristic estimate of the path length target. _{smooth}(x) is the heuristic estimate of the path smoothness objective. [{safety}(x)] represents the heuristic estimate of the path safety target; Scalarization is performed using a weighted sum method: _weighted(x) = w_1 × _length(x) + w_2 × _smooth(x) + w_3 × _safety(x), where, _weighted(x) is a weighted scalarized multi-objective heuristic estimate; Maintain a set of Pareto fronts: P = {π∣¬ π′∈P,π′ π}, where P represents the Pareto front solution set, π represents a solution in the Pareto front solution set, and ¬ π′ represents the absence of π′. π means that π′ is no worse than π on all targets and is strictly better than π on at least one target.

4. The sampling path planning method based on adaptive dynamic batch optimization according to claim 3, characterized in that, After the step of calculating the batch size according to a predetermined adaptive batch size calculation formula when the edge processing queue QE is empty and a new batch needs to be created, the method further includes a sample sampling and processing step, which further includes: Calculate an importance score for each sample: Importance(x) = α1 × HeuristicValue(x) + β1 × ConnectivityPotential(x) + γ1 × SuccessProbability(x), where HeuristicValue(x) is the heuristic value, ConnectivityPotential(x) is the connectivity potential, SuccessProbability(x) is the probability of successful connection, α1 is the heuristic value weight, β1 is the connectivity potential weight, and γ1 is the probability of successful connection weight. Samples were collected based on an importance-based sampling method. When processing samples, high-importance samples are processed with the highest priority, medium-importance samples with the second highest priority, and low-importance samples are processed only when computational resources are abundant.

5. A sampling path planning system based on adaptive dynamic batch optimization, characterized in that, Used to implement the sampling path planning method based on adaptive dynamic batch optimization as described in any one of claims 1-4; The sampling path planning system includes an adaptive batch manager, a search progress monitor, a solution quality evaluator, an environment complexity analyzer, and a batch size calculator; The adaptive batch manager is configured as follows: Receive iteration information from the search progress monitor, the iteration information including the current iteration number and search stage. Receive cost improvement data from the solution quality evaluator, which is used to calculate the solution quality improvement rate (Rimprove). Receive environmental information from the environmental complexity analyzer, which is used to calculate the environmental complexity index Ienv. The batch size parameter is calculated using the batch size calculator based on the adaptive batch size calculation formula.

6. The sampling path planning system based on adaptive dynamic batch optimization according to claim 5, characterized in that, It also includes a multi-objective heuristic engine, a path length heuristic calculator, a smoothness heuristic calculator, a security heuristic calculator, and a Pareto frontier manager; The multi-objective heuristic engine is configured as follows: It receives obstacle distribution information from an environmental map, providing the environmental basis for calculations in path length heuristic calculators, smoothness heuristic calculators, and safety heuristic calculators. The system receives target weight parameters from the weight configuration for weighted calculation of multi-objective costs. These target weight parameters include w_1, w_2, and w_3. Obtain multi-objective heuristic values, including heuristic results corresponding to path length, smoothness, and security. It engages in bidirectional data interaction with the Pareto frontier manager to receive multi-objective cost data for new paths and output non-dominated solution update instructions.

7. The sampling path planning system based on adaptive dynamic batch optimization according to claim 6, characterized in that, It also includes intelligent samplers, sample importance evaluators, importance-weighted samplers, and a sample history database; The intelligent sampler is configured as follows: Receive the batch size parameter from the adaptive batch manager to determine the number of samples to be generated in each batch. Receive sample importance distribution data from the sample importance evaluator. New samples conforming to the importance distribution are generated using an importance-weighted sampler and output to the implicit RGG builder to provide sample support for tree structure expansion. The sample information and connection attempt results of each sampling are updated to the sample history database.

8. The sampling path planning system based on adaptive dynamic batch optimization according to claim 7, characterized in that, It also includes an incremental replanner, an environmental change detector, an impact area calculator, a local update executor, and an information reuse manager; The incremental replanner is configured as follows: Receive real-time environmental data from an environmental change detector, which is used to monitor whether the environment has changed. The environmental change detection results are output to the affected area calculator, triggering the calculation of the Vaffected set of tree nodes affected by the environmental changes. It performs bidirectional data interaction with the tree structure storage, reads information about the current tree nodes and edges used to identify the affected areas from the tree structure storage, and outputs instructions for local pruning, edge updating, and node cost recalculation to the tree structure storage through the local update executor, so as to realize the local adjustment of the tree structure. Extract historical tree structure information that has not been affected by environmental changes using the information reuse manager.

9. The sampling path planning system based on adaptive dynamic batch optimization according to claim 8, characterized in that, It also includes a core search engine, vertex extender, edge processor, queue manager, and collision detection interface; The core search engine is configured as follows: It receives multi-objective heuristic values ​​from the multi-objective heuristic engine, providing optimality evaluation criteria for vertex expansion and edge selection; Receive new sample data from the smart sampler as a source of candidate nodes for vertex expansion; The collision detection interface interacts with the collision detection module to verify the collision feasibility of candidate edges and obtain the actual edge cost data. The tree structure is expanded and rewiring through vertex expanders and edge processors, and the order of the vertex expansion queue QV and the edge processing queue QE is maintained through a queue manager. The optimal path and cost data of new paths discovered during the search process are output to the Pareto Front Manager to update the Pareto Front solution set.

Images