A path planning method, system, device and product based on bidirectional search tree

Abstract

The application discloses a path planning method, system, device and product based on a bidirectional search tree, and relates to the technical field of robot path planning. The method obtains a starting state, a target state, boundary constraints and obstacle information, establishes a unified feasibility criterion, and constructs a bidirectional search tree. In each round, a proactive expansion tree, a contralateral tree and a dynamic sub-target are determined, the target-oriented expansion triggering probability is adjusted according to the change of the distance between the nearest nodes of the two trees, and target-oriented expansion or random expansion is performed. When the expansion is blocked, local obstacle-avoiding expansion is first performed, and when the local obstacle-avoiding expansion fails, global recovery based on a soft-min clearance field skeleton guide is performed. The candidate new node is connected to the proactive tree, and the connection between the two trees is detected. When the splicing condition is met, a candidate path is formed and the cost is calculated. After the stop condition is met, the candidate path with the minimum cost is selected and output. The application can improve the search efficiency, safety and executability of path planning in a complex limited environment.

Description

Technical Field

[0001] This application relates to the field of robot path planning technology, and in particular to a path planning method, system, device, and product based on a bidirectional search tree. It is especially suitable for collision-free path planning of mobile robots, mobile operating platforms, robotic arms, and other intelligent moving bodies in two-dimensional or three-dimensional workspaces and high-dimensional configuration spaces, and is particularly applicable to confined environments with dense obstacles, complex local passages, and high safety clearance requirements. Background Technology

[0002] Path planning is a key technology for robots to achieve autonomous movement and safely perform tasks. Existing algorithms include Rapidly-exploring Random Tree (RRT) and Rapidly-exploring Random Tree (RRT) algorithms. (Rapidly-exploring Random Tree Star, asymptotically optimal fast expanding random tree) and bidirectional RRT (BidirectionalRRT) These technologies, such as random sampling and tree structure expansion, can search for feasible paths in high-dimensional or complex spaces. Among them, bidirectional RRT... By growing two search trees from the starting state and the target state respectively, the efficiency of path search is improved to some extent.

[0003] However, in environments with dense obstacles, narrow spaces, and complex local passageways, traditional bidirectional RRT... The following shortcomings still exist in this approach: First, the sampling and expansion processes often rely on fixed target bias probabilities, making it difficult to adaptively adjust the target traction strength according to the actual convergence trend of the two trees; Second, target-oriented expansion tends to repeatedly call a few leading nodes closest to the target, resulting in insufficient tree structure coverage and easy getting stuck in local areas; Third, when the linear expansion of the target is blocked, traditional methods usually lack a hierarchical local escape mechanism, making it impossible to effectively bypass local obstacles while maintaining the overall target direction; Fourth, when local escape fails, ordinary random sampling lacks utilization of the free space structure, making it difficult to prioritize the generation of recovery samples with high clearance and global guiding significance; Fifth, some methods have inconsistent safety criteria among node expansion, two-tree connection, path simplification, and path smoothing, which can easily lead to problems where the search phase is feasible but the final execution phase has a collision risk.

[0004] Therefore, there is an urgent need for a path planning method based on bidirectional search trees that can simultaneously take into account target traction, local escape, global recovery, and full-process safety verification, so as to improve the search efficiency, safety, and executability of path planning in complex and constrained environments. Summary of the Invention

[0005] The purpose of this application is to provide a path planning method, system, device, and product based on a bidirectional search tree. This method uses a bidirectional search tree as the search skeleton. Under the constraint of a unified feasibility criterion, it dynamically adjusts the target guidance expansion trigger probability according to the change in the distance between the nearest nodes of the two trees. It also adopts a multi-mode target guidance node selection, local layered obstacle avoidance, skeleton-guided global recovery, local reconnection, and path safety post-processing mechanism to solve the problems of fixed target traction strength, insufficient recovery ability after local obstruction, lack of structural guidance in the recovery sample, and inconsistency of safety criteria before and after path processing in traditional path planning methods.

[0006] To achieve the above objectives, this application provides the following solution: Firstly, this application provides a path planning method based on a bidirectional search tree, including: The system acquires the starting state, target state, boundary constraints of the area to be planned, and obstacle information within the area to be planned, and establishes a unified feasibility criterion with minimum safe clearance as the constraint.

[0007] Build a first search tree with the starting state as the root and add the starting state to the first search tree node set; build a second search tree with the target state as the root and add the target state to the second search tree node set; let the initial value of the iteration number t be 1.

[0008] In the t-th iteration, one of the first search tree and the second search tree is selected as the active expansion tree, and the other is selected as the counterpart tree. The dynamic sub-target is then determined from the search tree point set corresponding to the counterpart tree.

[0009] When t>1, obtain the distance between the nearest node pair in the first search tree point set and the second search tree point set in the (t-1)th iteration, and determine the target-oriented expansion trigger probability in the tth iteration based on the change of the nearest node pair distance; when t=1, the target-oriented expansion trigger probability in the tth iteration is the preset initial target-oriented expansion trigger probability.

[0010] Based on the target-oriented expansion trigger probability of the t-th iteration, determine whether to perform a target-oriented expansion operation or a random expansion operation, and determine the expansion start point and expansion reference point from the active expansion tree based on the determined expansion operation. Generate expansion candidate points for the t-th iteration along the direction from the expansion start point to the expansion reference point.

[0011] If the connecting edge between the expansion candidate point and the expansion starting point satisfies the unified feasibility criterion, then the expansion candidate point will be used as the candidate new node for the t-th iteration.

[0012] If the connection edge between the expansion candidate point and the expansion starting point does not meet the unified feasibility criterion, a local obstacle bypass expansion operation is performed. If the local obstacle bypass expansion operation successfully generates a local obstacle bypass candidate point, the local obstacle bypass candidate point is used as a candidate new node for the t-th iteration. If the local obstacle bypass expansion operation fails, a global recovery expansion operation is performed to generate skeleton neighborhood recovery sample points based on the soft-min net field. If the global recovery expansion operation successfully generates a global recovery candidate point, the global recovery candidate point is used as a candidate new node for the t-th iteration. If the global recovery expansion operation fails, no candidate new node is generated for the t-th iteration.

[0013] If a candidate new node is generated in the t-th iteration, the candidate new node is added to the search tree point set corresponding to the active expansion tree, and the node closest to the candidate new node is determined in the search tree point set corresponding to the opposite side tree as the connection node for the t-th iteration.

[0014] If a candidate new node is generated in the t-th iteration and the path splicing condition is met, then the first path segment and the second path segment are spliced ​​together in the direction from the starting state to the target state to obtain the candidate path of the t-th iteration and added to the candidate path set. At the same time, the path cost of the candidate path of the t-th iteration is calculated. The path splicing condition includes that the distance between the candidate new node and the connecting node is less than a preset connection threshold, and the connection edge between the candidate new node and the connecting node satisfies a unified feasibility criterion. The first path segment is the backtracking path of the candidate new node in the active expansion tree. The second path segment is the backtracking path of the connecting node in the opposite side tree.

[0015] If no new candidate node is generated in the t-th iteration, or if a new candidate node is generated in the t-th iteration but does not meet the path splicing condition, then no candidate path will be added to the candidate path set in the t-th iteration.

[0016] If the stopping condition is met and the candidate path set is not empty, the candidate path with the minimum path cost in the candidate path set is selected as the minimum cost planning path and output; if the stopping condition is met and the candidate path set is empty, the path planning failure result is output.

[0017] If the stopping condition is not met, let t = t + 1, redetermine the active expansion tree, the counterpart tree, and the dynamic sub-objective for the (t + 1)th iteration, and repeat the steps from determining the active expansion tree, the counterpart tree, and the dynamic sub-objective to judging the stopping condition.

[0018] Secondly, this application provides a path planning system based on a bidirectional search tree, comprising: The information acquisition module is used to acquire the starting state, target state, boundary constraints of the area to be planned, and obstacle information within the area to be planned, and to establish a unified feasibility criterion with minimum safe clearance as the constraint.

[0019] The search tree initialization module is used to build a first search tree with the starting state as the root and add the starting state to the first search tree point set; build a second search tree with the target state as the root and add the target state to the second search tree point set; and set the initial value of the iteration number t to 1.

[0020] The active expansion tree determination module is used to determine, in the t-th iteration, one of the first search tree and the second search tree as the active expansion tree, and the other as the counterpart tree, and to determine the dynamic sub-target from the search tree point set corresponding to the counterpart tree.

[0021] The target-oriented expansion trigger probability determination module is used to obtain the distance between the nearest node pairs in the first search tree point set and the second search tree point set in the (t-1)th iteration when t>1, and determine the target-oriented expansion trigger probability in the tth iteration based on the change in the distance between the nearest node pairs; when t=1, the target-oriented expansion trigger probability in the tth iteration is the preset initial target-oriented expansion trigger probability.

[0022] The expansion candidate point generation module is used to determine whether to perform a target-oriented expansion operation or a random expansion operation based on the target-oriented expansion trigger probability of the t-th iteration, and to determine the expansion start point and expansion reference point from the active expansion tree based on the determined expansion operation, and to generate expansion candidate points for the t-th iteration along the direction from the expansion start point to the expansion reference point.

[0023] The extended feasibility determination module is used to select the extended candidate point as a candidate new node for the t-th iteration if the connection edge between the extended candidate point and the extended starting point satisfies the unified feasibility criterion.

[0024] The hierarchical recovery module is used to perform a local obstacle bypass expansion operation if the connection edge between the expansion candidate point and the expansion starting point does not meet the unified feasibility criterion. If the local obstacle bypass expansion operation successfully generates a local obstacle bypass candidate point, the local obstacle bypass candidate point is used as a candidate new node for the t-th iteration. If the local obstacle bypass expansion operation fails, a global recovery expansion operation is performed to generate skeleton neighborhood recovery sample points based on the soft-min net field. If the global recovery expansion operation successfully generates a global recovery candidate point, the global recovery candidate point is used as a candidate new node for the t-th iteration. If the global recovery expansion operation fails, no candidate new node is generated for the t-th iteration.

[0025] The new node access and local reconnection module is used to determine the target parent node of the candidate new node when the candidate new node is generated in the t-th iteration, access the candidate new node to the active expansion tree and perform local reconnection, and determine the node closest to the candidate new node in the search tree point set corresponding to the opposite side tree as the connection node for the t-th iteration.

[0026] The path splicing and cost calculation module is used to, if a candidate new node is generated in the t-th iteration and the path splicing condition is met, splice the first path segment and the second path segment in the direction from the starting state to the target state to obtain the candidate path for the t-th iteration and add it to the candidate path set, and calculate the path cost of the candidate path for the t-th iteration. The path splicing condition includes that the distance between the candidate new node and the connecting node is less than a preset connection threshold, and the connection edge between the candidate new node and the connecting node satisfies a unified feasibility criterion. The first path segment is the backtracking path of the candidate new node in the active expansion tree; the second path segment is the backtracking path of the connecting node in the opposite side tree. If no candidate new node is generated in the t-th iteration, or if a candidate new node is generated in the t-th iteration but the path splicing condition is not met, then no candidate path is added to the candidate path set in the t-th iteration.

[0027] The path result determination module is used to determine the path with the lowest path cost in the candidate path set as the minimum cost planning path and output it if the stopping condition is met and the candidate path set is not empty; otherwise, it outputs the path planning failure result.

[0028] The iteration control module is used to, if the stopping condition is not met, set t=t+1, redetermine the active expansion tree, the counterpart tree, and the dynamic sub-target for the (t+1)th iteration, and repeat the steps from determining the active expansion tree, the counterpart tree, and the dynamic sub-target to judging the stopping condition.

[0029] Thirdly, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the path planning method based on a bidirectional search tree described above.

[0030] Fourthly, this application provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the path planning method based on a bidirectional search tree described above.

[0031] According to the specific embodiments provided in this application, this application has the following technical effects: This application provides a path planning method, system, device, and product based on bidirectional search trees. By establishing a unified feasibility criterion constrained by minimum safety clearance, it solves the problem of inconsistent safety criteria among node expansion, bi-tree connection, path simplification, and path smoothing, achieving unified safety verification throughout the entire process and avoiding the problem of feasibility in the search phase but collision risk in the final execution phase. By determining dynamic sub-targets from the search tree point set corresponding to the opposite side tree and determining the target-oriented expansion trigger probability based on the changing trend of the distance between the nearest nodes in the bi-tree, it solves the problem of traditional bidirectional RRT. This approach addresses the problem of traditional methods relying on fixed target bias probabilities and struggling to adaptively adjust target traction strength based on the actual merging trend of the two trees. It achieves dynamic adjustment of target bias based on merging trend awareness, enhancing target traction to accelerate connection when the two trees are effectively close, and reducing the tendency for direct target expansion to minimize ineffective expansion when the search is continuously stalled. By determining the expansion starting point from the actively expanding tree based on the target-oriented expansion trigger probability and generating expansion candidate points using dynamic sub-targets as the expansion direction basis, it solves the problem of target-oriented expansion repeatedly calling a few closest leading-edge nodes, leading to insufficient tree structure coverage. This achieves multi-mode target-oriented node selection, balancing target traction, path cost, and tree structure coverage diversity. Finally, by performing local obstacle-avoidance expansion operations, it overcomes the lack of a hierarchical local escape mechanism in traditional methods, which cannot effectively bypass local obstacles while maintaining the overall target direction. To address the obstacle problem, a layered maneuver escape mechanism was implemented. After the direct expansion of the target failed, bilateral trial obstacle avoidance and local lateral displacement recovery were performed sequentially to progressively find feasible advancement directions. By performing a global recovery expansion operation, the problem of ordinary random sampling lacking utilization of free space structure and difficulty in prioritizing the generation of high-clearance recovery samples was solved. Skeleton-guided global recovery was achieved, and the recovery samples were guided to the high-clearance free space skeleton region using a soft-min clearance field, improving the effectiveness of global recovery sampling. By splicing the first path segment and the second path segment to obtain candidate paths, and outputting the minimum cost planning path with the minimum path cost in the candidate path set when the stopping condition is met, the problems of low efficiency and insufficient safety in path planning search under complex and restricted environments were solved. Efficient, safe, and executable path planning was achieved in environments with dense obstacles, narrow spaces, and complex local passages. Attached Figure Description

[0032] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0033] Figure 1 This is a flowchart illustrating a path planning method based on a bidirectional search tree, provided as an embodiment of this application.

[0034] Figure 2 This is a flowchart illustrating a path planning method based on a bidirectional search tree, provided as another embodiment of this application.

[0035] Figure 3 This is a schematic diagram of the functional modules of a path planning system based on a bidirectional search tree, provided in an embodiment of this application.

[0036] Figure 4This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0037] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0038] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0039] In one exemplary embodiment, such as Figure 1 As shown, a path planning method based on a bidirectional search tree is provided. This method is executed by a computer device, specifically by a terminal or server alone, or by both a terminal and a server. In this embodiment, it includes the following steps 101 to 111. Wherein: Step 101: Obtain the starting state, target state, boundary constraints of the area to be planned, and obstacle information within the area to be planned, and establish a unified feasibility criterion with minimum safe clearance as the constraint.

[0040] Step 102: Build a first search tree with the starting state as the root and add the starting state to the first search tree point set; build a second search tree with the target state as the root and add the target state to the second search tree point set; set the initial value of the iteration number t to 1.

[0041] Step 103: In the t-th iteration, determine one of the first search tree and the second search tree as the active expansion tree, and the other as the counterpart tree, and determine the dynamic sub-target from the search tree point set corresponding to the counterpart tree. When t=1, determine the root node of the counterpart tree, or the node in the counterpart tree that is closest to the active expansion tree, as the initial dynamic sub-target.

[0042] Step 104: When t>1, obtain the distance between the nearest node pair in the first search tree point set and the second search tree point set in the (t-1)th iteration, and determine the target-oriented expansion trigger probability in the tth iteration based on the change of the nearest node pair distance; when t=1, the target-oriented expansion trigger probability in the tth iteration is the preset initial target-oriented expansion trigger probability.

[0043] Step 105: Determine whether to perform a target-oriented expansion operation or a random expansion operation based on the target-oriented expansion trigger probability of the t-th iteration, and determine the expansion start point and expansion reference point from the active expansion tree according to the determined expansion operation. Generate expansion candidate points for the t-th iteration along the direction from the expansion start point to the expansion reference point. When performing a target-oriented expansion operation, the expansion reference point is a dynamic sub-target; when performing a random expansion operation, the expansion reference point is a randomly sampled point generated within the area to be planned.

[0044] Step 106: If the connection edge between the candidate point and the starting point satisfies the unified feasibility criterion, then the candidate point is used as the candidate new node for the t-th iteration.

[0045] Step 107: If the connection edge between the expansion candidate point and the expansion starting point does not meet the unified feasibility criterion, then a local obstacle bypass expansion operation is performed. If the local obstacle bypass expansion operation successfully generates a local obstacle bypass candidate point, the local obstacle bypass candidate point is used as a candidate new node for the t-th iteration. If the local obstacle bypass expansion operation fails, then a global recovery expansion operation is performed to generate skeleton neighborhood recovery sample points based on the soft-min net field. If the global recovery expansion operation successfully generates a global recovery candidate point, the global recovery candidate point is used as a candidate new node for the t-th iteration. If the global recovery expansion operation fails, no candidate new node is generated for the t-th iteration.

[0046] Step 108: If a candidate new node is generated in the t-th iteration, the candidate new node is added to the search tree point set corresponding to the active expansion tree, and the node closest to the candidate new node is determined in the search tree point set corresponding to the opposite side tree as the connection node for the t-th iteration.

[0047] Step 109: If a candidate new node is generated in the t-th iteration and the path splicing condition is met, then the first path segment and the second path segment are spliced ​​together in the direction from the starting state to the target state to obtain the candidate path for the t-th iteration and added to the candidate path set. Simultaneously, the path cost of the candidate path for the t-th iteration is calculated. The path splicing condition includes that the distance between the candidate new node and the connecting node is less than a preset connection threshold, and the connection edge between the candidate new node and the connecting node satisfies a unified feasibility criterion. The first path segment is the backtracking path of the candidate new node in the active expansion tree; the second path segment is the backtracking path of the connecting node in the opposite side tree. If no candidate new node is generated in the t-th iteration, or if a candidate new node is generated in the t-th iteration but the path splicing condition is not met, then no candidate path is added to the candidate path set in the t-th iteration.

[0048] Step 110: If the stopping condition is met and the candidate path set is not empty, then the candidate path with the minimum path cost in the candidate path set is taken as the minimum cost planning path and output; if the stopping condition is met and the candidate path set is empty, then the path planning failure result is output; the stopping condition includes reaching a preset maximum number of iterations, or the candidate path set is not empty and the change in the minimum path cost in the candidate path set during a consecutive preset number of iterations is not greater than a preset change threshold.

[0049] Step 111: If the stopping condition is not met, let t = t + 1, redetermine the active expansion tree, the side tree, and the dynamic sub-target for the (t + 1)th iteration, and repeat steps 103 to 110.

[0050] By implementing steps 101 to 111 above, this application can simultaneously take into account target traction, local escape, global recovery and full-process safety verification, thereby improving the search efficiency, safety and executability of path planning in complex and restricted environments.

[0051] In another exemplary embodiment of this application, step 103 specifically includes: In the t-th iteration, an active expansion tree is determined by a rotation method or a random sampling method, and the other tree is used as the opposite tree; the rotation method includes alternating between the first search tree and the second search tree as the active expansion tree; the random sampling method includes determining the active expansion tree by Bernoulli random sampling method according to a preset switching probability.

[0052] When t=1, the root node of the opposite tree or the node in the opposite tree closest to the actively expanding tree is determined as the initial dynamic sub-target. When t>1, a first dynamic sub-target update mode or a second dynamic sub-target update mode is pre-selected; in the first dynamic sub-target update mode, the dynamic sub-target is updated to the node in the opposite tree closest to the actively expanding tree with a preset update probability, and the dynamic sub-target of the (t-1)th iteration is retained with a probability of 1 minus the preset update probability; in the second dynamic sub-target update mode, the dynamic sub-target is updated to the most recently generated node in the opposite tree with the preset update probability, and the dynamic sub-target of the (t-1)th iteration is retained with a probability of 1 minus the preset update probability.

[0053] In another exemplary embodiment of this application, step 104 specifically includes: When t>1, obtain the distance between the nearest node pairs in the first search tree point set and the second search tree point set in the (t-1)th iteration, and use the difference between the nearest node pair distance in the (t-2)th iteration and the nearest node pair distance in the (t-1)th iteration as the single-step distance improvement amount in the (t-1)th iteration. A positive single-step distance improvement amount indicates that the first search tree and the second search tree are close to each other; wherein, the distance between the starting state and the target state when the search tree initialization is completed is used as the initial nearest node pair distance.

[0054] If the single-step distance improvement in the (t-1)th iteration is greater than the significant progress threshold, the continuous improvement insufficient counter will be cleared, and the smaller of the sum of the target-oriented expansion trigger probability and the step size adjustment in the (t-1)th iteration and the preset maximum target-oriented expansion trigger probability will be used as the target-oriented expansion trigger probability in the tth iteration.

[0055] If the improvement in single-step distance in the (t-1)th iteration is greater than or equal to the insufficient improvement threshold and less than or equal to the significant progress threshold, the continuous insufficient improvement counter will be cleared, and the smaller of the sum of the target-oriented expansion trigger probability and half of the upward adjustment step size in the (t-1)th iteration and the preset maximum target-oriented expansion trigger probability will be used as the target-oriented expansion trigger probability in the tth iteration.

[0056] If the single-step distance improvement in the (t-1)th iteration is less than the insufficient improvement threshold, the number of consecutive insufficient improvement counters is incremented by 1, and it is determined whether the number of consecutive insufficient improvement counters is greater than or equal to the preset number of stagnations. If the number of consecutive insufficient improvement counters is greater than or equal to the preset number of stagnations, the larger of the difference between the target-oriented expansion trigger probability and the step size reduction in the (t-1)th iteration and the preset minimum target-oriented expansion trigger probability is taken as the target-oriented expansion trigger probability in the tth iteration. If the number of consecutive insufficient improvement counters is less than the preset number of stagnations, the target-oriented expansion trigger probability in the (t-1)th iteration is taken as the target-oriented expansion trigger probability in the tth iteration. When t=2, the distance between the nearest node pairs when the first search tree and the second search tree are initialized is taken as the distance between the nearest node pairs in the 0th iteration. A positive single-step distance improvement indicates that the first search tree and the second search tree are close to each other. The initial value of the consecutive insufficient improvement counter is 0, and the significant progress threshold is greater than the insufficient improvement threshold.

[0057] In another exemplary embodiment of this application, step 105 specifically includes: Generate a uniformly distributed random number between 0 and 1.

[0058] If the uniformly distributed random number is less than the target-oriented expansion trigger probability of the t-th iteration, then the target-oriented expansion operation is performed. The target-oriented expansion operation includes selecting a node selection strategy from multiple node selection strategies according to a preset selection probability, determining the expansion starting point from the node set of the active expansion tree, taking the direction from the expansion starting point to the dynamic sub-target as the expansion direction, and generating expansion candidate points for the t-th iteration from the expansion starting point along the expansion direction with a first preset step size. The first preset step size is the smaller value between the upper limit of the single-step expansion length and the distance from the expansion starting point to the dynamic sub-target. The multiple node selection strategies include a first node selection strategy (i.e., the closest target type node selection strategy) that selects the node closest to the dynamic sub-target in the active expansion tree as the expansion starting point, constructing a cost-target collaborative score based on the cumulative path cost of each node in the active expansion tree and the distance from each node to the dynamic sub-target, selecting the node with the smallest cost-target collaborative score as the expansion starting point, a second node selection strategy (i.e., the cost-target collaborative type node selection strategy), and a third node selection strategy (i.e., the random leaf node type node selection strategy) that uniformly and randomly selects nodes from the leaf node set of the active expansion tree as the expansion starting point.

[0059] If the uniformly distributed random number is greater than or equal to the target-oriented expansion trigger probability of the t-th iteration, then a random expansion operation is performed. The random expansion operation includes generating a random sampling point in the area to be planned, selecting the node closest to the random sampling point from the active expansion tree as the expansion starting point, taking the direction from the expansion starting point to the random sampling point as the expansion direction, and generating expansion candidate points for the t-th iteration from the expansion starting point along the expansion direction with a second preset step size. The second preset step size is the smaller value between the upper limit of the single-step expansion length and the distance between the expansion starting point and the random sampling point.

[0060] In another exemplary embodiment of this application, step 107 involves performing a local obstacle-bypass expansion operation, and when the local obstacle-bypass expansion operation successfully generates local obstacle-bypass candidate points, these candidate points are used as candidate new nodes for the t-th iteration. Specifically, this includes: Using the extended starting point as the local recovery starting point, a local orthogonal basis is constructed through orthogonalization, with the direction from the local recovery starting point to the dynamic sub-target as the local principal direction; the local orthogonal basis includes the local principal direction, the first lateral direction, and the second lateral direction; In the plane formed by the local main direction and the first lateral direction, the first step test point and the second step test point are generated on the positive side and the negative side of the first lateral direction, respectively. Sequentially determine whether the connection edges between the first step trial point on each side and the local recovery starting point, and the connection edges between the second step trial point on each side and the corresponding first step trial point, satisfy the unified feasibility criterion; If both connecting edges on only one side satisfy the unified feasibility criterion, then the second trial point on the side that satisfies the unified feasibility criterion will be taken as a local obstacle bypass candidate point. If both connecting edges on both sides satisfy the unified feasibility criterion, then the obstacle bypass side score is constructed based on the distance between the second step test point on each side and the dynamic sub-target and the safe clearance of the test path on each side. The side with the highest obstacle bypass side score is determined as the optimal side, and the second step test point on the optimal side is used as a local obstacle bypass candidate point. If at least one connecting edge on both sides does not meet the unified feasibility criterion, then multiple candidate points are generated in the local orthogonal basis. Each candidate point is composed of a forward propulsion component along the local principal direction, a first lateral offset component along the first lateral direction, and a second lateral offset component along the second lateral direction. If there is at least one candidate point whose connection edge with the local recovery starting point satisfies the unified feasibility criterion, then the local lateral displacement candidate points are scored according to the forward propulsion component along the local main direction, the safe clearance, and the degree of consistency between the expansion direction and the local main direction. The candidate point with the highest local lateral displacement candidate point score is taken as the local obstacle bypass candidate point. If there is no candidate point whose connecting edge to the local recovery starting point satisfies the unified feasibility criterion, then the local obstacle bypass expansion operation is judged to have failed.

[0061] In another exemplary embodiment of this application, in step 107, if the local obstacle-around expansion operation fails, a global recovery expansion operation based on the soft-min net field to generate skeleton neighborhood recovery sample points is performed, specifically including: Construct a recovery seed point and use its position as the current position. Specifically, the recovery seed point is constructed based on the midpoint of a failed expansion edge, the perturbation position of an existing node in the active expansion tree, or a feasible random position generated within the area to be planned, and its position is used as the current position.

[0062] Based on the signed distances of each obstacle constraint and region boundary constraint within the area to be planned, a differentiable approximate minimum distance function obtained through smoothing and weighting is defined as the soft-min net space. Specifically, the signed distances from the planning state to each obstacle constraint and region boundary constraint within the area to be planned are obtained, and a differentiable approximate minimum distance function is constructed based on the smoothing parameter and each signed distance. This differentiable approximate minimum distance function is then determined as the soft-min net space.

[0063] The final seed position for the t-th iteration is determined by the following position update steps.

[0064] Calculate the soft-min net space gradient at the current position of the current sub-iteration.

[0065] Move one recovery update step along the direction of the soft-min net space gradient at the current position in the current sub-iteration to obtain the candidate update position for the current sub-iteration. Specifically, if the magnitude of the soft-min net space gradient is not less than a preset gradient threshold, move the recovery update step along the direction of the soft-min net space gradient at the current position to obtain the candidate update position; if the magnitude of the soft-min net space gradient is less than the preset gradient threshold, reconstruct the recovery seed point; if the magnitude of the soft-min net space gradient is still less than the preset gradient threshold after reaching a preset number of recovery seed reconstructions, the global recovery expansion operation is determined to have failed.

[0066] If the connection edge between the current position and the candidate update position satisfies the unified feasibility criterion, the candidate update position is located within the area to be planned, and the soft-min net value at the candidate update position is greater than or equal to the soft-min net value at the current position, then the current position is updated as the candidate update position.

[0067] If the connection edge between the current position and the candidate update position does not meet the unified feasibility criterion, or the candidate update position is located outside the area to be planned, or the soft-min net value at the candidate update position is less than the soft-min net value at the current position, then the current position remains unchanged.

[0068] If the loop stopping condition is met, the current position of the current sub-iteration is taken as the final seed position of the t-th iteration. The loop stopping condition includes the current position satisfying the free space skeleton neighborhood determination condition or reaching the preset maximum number of sub-iterations. The free space skeleton neighborhood determination condition is: after sorting the soft-min weights of each constraint corresponding to the current position in descending order of value, the maximum soft-min weight and the second largest soft-min weight are both greater than the preset lower limit of weight, and the difference between the two is less than the preset proximity threshold.

[0069] If the loop termination condition is not met, increment the sub-iteration count by 1 and repeat the above position update steps.

[0070] If the final seed position of the t-th iteration satisfies the free space skeleton neighborhood determination condition, then the local normal direction is determined according to the difference between the signed distance gradients of the constraints corresponding to the largest soft-min weight and the second largest soft-min weight. The random vector is projected onto the tangent space orthogonal to the local normal direction to obtain the tangential direction, and a random offset is applied to the final seed position along the tangential direction to generate the recovery sample point of the t-th iteration.

[0071] If the recovery sample point for the t-th iteration is successfully generated, the node closest to the recovery sample point for the t-th iteration is selected from the active expansion tree as the recovery expansion starting point, and expansion is performed in the direction from the recovery expansion starting point to the recovery sample point to generate the global recovery candidate point for the t-th iteration.

[0072] If the connection edge between the global recovery candidate point in the t-th iteration and the recovery extension starting point satisfies the unified feasibility criterion, then the global recovery candidate point in the t-th iteration is taken as the candidate new node in the t-th iteration.

[0073] If the recovery sample point for the t-th iteration is not successfully generated, or the connection edge between the global recovery candidate point for the t-th iteration and the recovery expansion starting point does not meet the unified feasibility criterion, or the final seed position for the t-th iteration does not meet the free space skeleton neighborhood determination condition, then the global recovery expansion operation is determined to have failed.

[0074] Specifically, if the final seed position of the t-th iteration satisfies the free space skeleton neighborhood determination condition, the initial value of the normal reconstruction counter is set to 0. The first and second dominant constraints corresponding to the largest soft-min weight and the second largest soft-min weight are determined, respectively. The local normal direction is determined based on the difference between the signed distance gradients of the first and second dominant constraints at the final seed position. If the magnitude of the difference between the signed distance gradients is not less than a preset normal threshold, the random vector is projected onto a tangent space orthogonal to the local normal direction to obtain the tangent direction, and a random offset of a preset magnitude is applied along the tangent direction. Generate the recovered sample point for the t-th iteration; if the magnitude of the difference between the signed distance gradients is less than a preset normal threshold, increment the normal reconstruction counter by 1 and reselect the dominant constraint combination; if the normal reconstruction counter count is less than a preset normal reconstruction count, repeat the local normal direction determination step; if the normal reconstruction counter count reaches the preset normal reconstruction count, reconstruct the recovery seed point, and repeat the position update step when the recovery seed reconstruction count is less than the preset recovery seed reconstruction count; if no effective local normal direction is obtained after reaching the preset recovery seed reconstruction count, the global recovery expansion operation is determined to have failed.

[0075] In another exemplary embodiment of this application, step 108 specifically includes: In the search tree point set corresponding to the active expansion tree, nodes whose distance from the planned state of the candidate new node in the t-th iteration is less than the preset neighborhood radius are added to the candidate parent node set.

[0076] In the set of candidate parent nodes, candidate parent nodes whose connection edges with the candidate new node in the t-th iteration satisfy the unified feasibility criterion are selected and added to the set of feasible candidate parent nodes.

[0077] Calculate the sum of the cumulative path cost from the actively expanded root node to each feasible candidate parent node in the set of feasible candidate parent nodes, and the sum of the edge lengths connecting the new candidate node to each feasible candidate parent node in the set of feasible candidate parent nodes. This yields the total generation value of each feasible candidate parent node. The feasible candidate parent node with the lowest total generation value is selected as the target parent node for the new candidate node in the t-th iteration. If the set of feasible candidate parent nodes is empty, the expansion starting point used when generating the new candidate node is taken as the target parent node.

[0078] The candidate new node of the t-th iteration is connected to the active expansion tree. The identifier of the target parent node and the cumulative path cost from the root node of the active expansion tree through the target parent node to the candidate new node of the t-th iteration are recorded. The candidate new node of the t-th iteration is added to the search tree point set corresponding to the active expansion tree.

[0079] In the search tree point set corresponding to the active expansion tree, nodes whose distance from the planning state of the candidate new node in the t-th iteration is less than the preset neighborhood radius are identified as the neighborhood node set.

[0080] Take any neighboring node in the set of neighboring nodes as the current neighboring node, and determine whether the current neighboring node meets the candidate update conditions. The candidate update conditions include that the cumulative path cost from the active expansion tree root node through the candidate new node in the t-th iteration to the current neighboring node is less than the cumulative path cost from the active expansion tree root node to the current neighboring node, and the connection edge between the current neighboring node and the candidate new node in the t-th iteration satisfies the unified feasibility criterion.

[0081] If the current neighboring node meets the candidate update condition, then update the parent node of the current neighboring node to the candidate new node in the t-th iteration, and update the cumulative path cost of the current neighboring node and all its descendant nodes.

[0082] If the current neighboring node does not meet the candidate update conditions, then the parent node of the current neighboring node remains unchanged.

[0083] The next neighbor node is used as the current neighbor node, until all neighbor nodes in the set of neighbor nodes have been traversed.

[0084] In the search tree point set corresponding to the opposite tree, the node with the smallest planning state distance to the candidate new node in the t-th iteration is determined as the connection node in the t-th iteration.

[0085] The following example illustrates this application using a specific path planning process based on a bidirectional search tree.

[0086] In one exemplary embodiment, such as Figure 2 As shown, a path planning method based on a bidirectional search tree is presented. First, the target-oriented expansion trigger probability is dynamically updated based on the change in the distance between the nearest node pairs between the starting tree and the ending tree. When the nearest distance between the two trees continuously decreases, it indicates that the current search has a good convergence trend. At this time, the target-oriented expansion trigger probability is increased to accelerate the connection between the two trees. When the two trees do not improve sufficiently or enter a stagnant state, the target-oriented expansion trigger probability is decreased to reduce repeated attempts on the blocked target direction and provide triggering opportunities for local hierarchical escape and skeleton-guided global recovery. Thus, the target-oriented expansion trigger probability is no longer a fixed empirical parameter, but a feedback variable that changes online with the search state. Second, in the target-oriented expansion stage, three node selection methods are comprehensively adopted: nearest target type, cost-target cooperative type, and random leaf node type. This ensures that the expansion process takes into account target traction, path cost, and tree structure coverage diversity, and avoids the repeated expansion of a few nearest target nodes. When a direct expansion of the target fails, a local orthogonal basis is further constructed, and bilateral trial obstacle avoidance and local lateral displacement recovery are performed sequentially. Bilateral trial is used to attempt to bypass obstacles while maintaining the forward propulsion trend, while local lateral displacement selects a safe alternative direction based on the local lateral displacement candidate point score based on propulsion amount, clearance, and directional consistency, thereby improving the escape capability in the locally obstructed area. Then, after both direct expansion of the target and local hierarchical escape fail, a recovery seed is constructed based on the midpoint of the failed edge or tree node perturbation, and the sample is guided to a high clearance region using the soft-min clearance field rise. The free space skeleton neighborhood is then determined through the dominant constraint weight balance relationship, and recovery samples are generated by combining tangential perturbation, thereby improving the effectiveness of global recovery sampling. Simultaneously, this application integrates a unified feasibility criterion throughout the entire process of node expansion, local recovery, skeleton recovery, dual-tree true connection, path simplification, path smoothing, and final verification, ensuring that the final path meets the minimum safe clearance constraint. Specifically, this includes the following steps.

[0087] This application addresses the path planning problem within a bounded planning space. The planning space is denoted as... The starting state is denoted as The target state is denoted as The set of obstacles is denoted as ,in, The number of obstacles. The planning state is denoted as... ,in, To plan the spatial dimension. For point-moving bodies or rigid moving bodies described using reference points, It indicates the location of its reference point and performs equivalent expansion of the obstacle based on the external dimensions of the moving body and safety margin; for the robotic arm, This represents the joint configuration of the robotic arm. The configuration clearance is determined by the minimum distance between the envelope of all links and the set of obstacles under this configuration. The goal of path planning is to construct a path starting from the initial state, while satisfying the planning space boundary constraints, collision-free constraints, and minimum safe clearance constraints. To the target state A feasible path.

[0088] To uniformly describe the state distances between different planning objects, the planning state distances are calculated using the normalized weighted L2 norm of the differences between each state dimension. For the position dimension, normalization is performed according to the corresponding workspace scale; for the angle dimension, the minimum period angle difference falling within a preset period interval is used for calculation; the weights of each state dimension are determined based on its allowable range of variation, kinematic sensitivity, or safety requirements. Unless otherwise stated, subsequent node distances, expansion step sizes, neighborhood radii, nearest node selection, dual-tree connection thresholds, and path lengths are all calculated based on the planning state distances; when the weights of each dimension are equal and period angles are not involved, the planning state distances degenerate into Euclidean distances.

[0089] Furthermore, the definition is based on the state. Pointing to state The effective state difference vector is Among them, the location dimension is normalized according to the corresponding workspace scale, and the angle dimension takes the minimum periodic angle difference; the planning state distance is defined as... The state difference vector, vector magnitude and interpolation direction in subsequent equations (2), (3) and (19) all adopt the effective state difference vector and the planned state distance, and the angle dimension is remapped to the preset period interval after interpolation or expansion.

[0090] Step S1: Construct the planning space, obstacle set, and unified feasibility criterion, and initialize the starting point tree and ending point tree.

[0091] Before pathfinding begins, it is necessary to establish a safety judgment foundation that is used by all subsequent steps. This step unifies environment modeling, obstacle distance calculation, node safety assessment, and edge safety assessment into a single set of geometric criteria. This is done to avoid the problem of using different safety standards for subsequent target expansion, local escape, skeleton restoration, dual-tree connection, and path post-processing, which could lead to a situation where a path deemed feasible in one stage becomes a collision risk in another.

[0092] In practice, obstacles in the planning environment are first mapped to the same coordinate system to form an obstacle set. Obstacles may include spheres, axis-aligned cuboids, finite-height cylinders, irregular safety restricted areas, or actual objects approximated by an envelope. For any planning state in the planning space, the minimum clearance from the area occupied by the moving body to the obstacle set is defined as: (1).

[0093] In equation (1), Represents any point in the planned space. Represents a set of obstacles. Indicates the first An obstacle, Indicates the total number of obstacles. Point To the obstacle The closest Euclidean distance on the surface Point The shortest distance to the entire set of obstacles. This distance is used to measure how much safety margin a waypoint has from the nearest obstacle.

[0094] To determine if a connecting edge between two path points is safe, it's not enough to only check the two endpoints; the midpoint of the connecting segment must also be examined. Therefore, for any two nodes... and , defined by point to The parameterized form of the line segment is: (2).

[0095] In equation (2), and Let represent the start and end points of the candidate connecting edges, respectively. Indicates line segment parameters, Indicates the corresponding parameters on the line segment The location point. When hour, ;when hour, Among them, in equation (2) Calculate according to the aforementioned effective state difference vector; for the angle dimension, interpolate along the direction of the minimum period angle difference, and remap the interpolation result to the preset period interval.

[0096] In practical implementation, a discrete sampling method can be used to perform safety detection on this line segment. Let... To preset the discrete detection step size, then and Number of detection points between It can be defined as: (3).

[0097] In equation (3), This represents the discrete detection steps for connecting edges, where the actual number of detection points is... +1; Represents a node With nodes Distance between planned states; This represents the maximum allowable planning state distance between adjacent detection points; This indicates rounding up to the nearest integer. The smaller the value, the denser the intermediate point detection and the more detailed the security verification, but the greater the computational load.

[0098] Based on the above discrete detection points, define line segments. The minimum clearance for each segment is: (4).

[0099] In equation (4), Indicates from arrive The minimum clearance of the entire connecting line in the sense of discrete detection. This represents the index of the discrete detection point. This value represents the safe distance from the most dangerous position on the connected edge to the set of obstacles.

[0100] Based on this, set To define the minimum safe clearance threshold, a unified feasibility criterion for candidate connection edges is defined as follows: (5).

[0101] In equation (5), Indicate candidate edges The feasibility assessment results This indicates that the edge is safe and feasible. This indicates that the edge is not feasible; Indicates the planned space. This indicates the minimum permissible safety clearance. This criterion states that a connection is only valid if the connecting segment is within the planned space and the minimum clearance for the entire connection is not less than [a certain value]. Only when the connection edge is specified is it allowed to be accepted by the search tree.

[0102] After establishing a unified feasibility criterion, the starting state and target state are read, and the starting tree and target tree are built respectively. Each tree node must store at least the node state, the parent node index, and the cumulative path cost from the root node to that node. The starting state and target state themselves must also satisfy boundary constraints, collision-free constraints, and minimum safe clearance constraints; otherwise, they need to be reset.

[0103] The result of step S1 is a unified set of obstacles, a unified distance calculation method, a unified edge feasibility criterion, and two initial search trees. All subsequent steps involving new node access, edge expansion, dual-tree connection, path pruning, or path smoothing shall use equation (5) for security verification.

[0104] Step S2: In each round of search, determine the active expansion tree, the opposite tree, and the dynamic sub-targets to provide traction direction for the subsequent two-tree convergence.

[0105] After establishing a unified security foundation in step S1, step S2 determines "which tree to expand from" and "to which target region" for each round of search. This step primarily addresses the issues of fixed expansion directions and insufficient convergence and traction between the two trees in traditional bidirectional search. By dynamically switching the active expansion tree between the starting and ending trees and maintaining dynamic sub-targets, the two trees can more actively move closer to each other, rather than relying solely on random sampling and accidental connections.

[0106] In the In the round of iteration, from and Choose one tree as the current actively expanding tree, denoted as . The other tree, designated as the opposite tree, is denoted as... The active tree can be determined using a rotation method or a Bernoulli random variable: (6).

[0107] In equation (6), Indicates the first In the t-th iteration, the tree is actively expanded to select variables. Indicates the probability of active tree switching. The parameter is The Bernoulli distribution. When At that time, it can be made To actively expand the tree; when At that time, it can be made This method allows for proactive tree expansion. By doing so, both trees have the opportunity to grow actively, helping to avoid overexpansion from unilateral searches.

[0108] In addition to the final target point, this application also maintains a dynamic sub-target for the current active tree. This sub-target can be taken from a representative node in the opposite tree, such as the node in the opposite tree closest to the active tree, the most recently generated node in the opposite tree, or a historical sub-target updated with a certain probability. If the... The latest generated node of the side tree used in the round is The dynamic sub-target can then be updated as follows: (7).

[0109] In equation (7), and These represent the dynamic sub-targets before and after the update, respectively. Indicates the first The latest generated node of the opposite side tree used in the round. A random variable indicating whether to update the sub-target. This represents the probability of updating the sub-target. This setting allows the sub-target to follow the latest growth state of the search tree without changing drastically in each round, thus maintaining a certain level of search stability.

[0110] After step S2 is completed, the active expansion tree has been determined in the current round. opposite side tree and dynamic sub-targets The subsequent step S3 will adjust the target-oriented expansion trigger probability based on the convergence trend of the two trees, and step S4 will revolve around the target-oriented trigger when it occurs. Select the expansion node and execute direct expansion.

[0111] Step S3: Based on the change in the distance between the nearest nodes in the dual-tree, update the target-oriented expansion trigger probability online to determine whether to prioritize entering the target-oriented expansion in the t-th iteration.

[0112] After determining the active tree and dynamic sub-target in step S2, step S3 further determines whether the current bidirectional search is effectively converging. This step transforms the geometric approach trend between the two trees into an online adjustment basis for the target-oriented expansion trigger probability. The problem this step addresses is that if the target-oriented expansion trigger probability is fixed, progress may be insufficient when the two trees are indeed close, while repeated expansion in infeasible directions may occur when locally blocked, leading to decreased search efficiency.

[0113] In specific implementation, in the first At the next iteration, obtain the node sets of the starting tree and the ending tree respectively. and The distance between the nearest nodes in a bitree is defined as: (8).

[0114] In equation (8), Indicates the first The distance between the nearest node pair in the planning state at the next iteration. and These represent the starting tree and the ending tree at the th node, respectively. The set of nodes at the time of the next iteration. and These represent nodes in the two trees. The smaller the distance, the closer the two trees are in the planning state space.

[0115] When determining the target-oriented expansion trigger probability in the t-th iteration, the distance between the nearest node pairs corresponding to the (t-2)-th and (t-1)-th iterations is used, and the single-step distance improvement in the (t-1)-th iteration is defined as: (9).

[0116] In equation (9), Indicates the first The iteration relative to the first Improvement in single-step distance in each iteration; and They represent the first Second and third The distance between the closest node pairs in the two search trees corresponding to the next iteration. When the distance between the nearest node pairs of the two search trees decreases, the two trees are getting closer to each other; when When, it indicates that the distance between the nearest nodes in the dual-tree has not changed; when When this occurs, it indicates that the two trees have not effectively approached each other, and the distance between the nearest node pairs has increased.

[0117] Since single-round random expansion is subject to chance, this application further sets up a continuous improvement insufficiency counter.

[0118] set up Indicates as of the date The number of consecutive improvements in the next iteration is insufficient. If the threshold for improvement is insufficient, then: (10).

[0119] In equation (10), Indicates as of the date The number of consecutive improvements in the next iteration is insufficient. Indicates as of the date The number of consecutive improvements in the next iteration is insufficient. This indicates an insufficient improvement threshold. When Reaching the preset number of pauses When this occurs, the current search can be considered to have entered a stagnant or weak progress state. The initial value of the continuous improvement insufficiency counter is set to... .

[0120] set up Indicates the first The next iteration's goal-oriented expansion trigger probability. and These represent the preset minimum target-oriented expansion trigger probability and the preset maximum target-oriented expansion trigger probability, respectively. Indicates the threshold for significant progress. This indicates an insufficient threshold for improvement. This indicates an upward adjustment of the step size. This indicates a downward adjustment of the step size, and Then the first The next iteration's goal-oriented expansion trigger probability Update according to the following rules: (11).

[0121] (12).

[0122] In equations (11) and (12), Indicates the first The iteration relative to the first The target-oriented extension trigger probability adjustment amount for the next iteration; Indicates the first The next iteration's goal-oriented expansion trigger probability. Indicates the first The target-oriented expansion trigger probability of the next iteration; and These represent the preset minimum target-oriented expansion trigger probability and the preset maximum target-oriented expansion trigger probability, respectively. and These represent the thresholds for significant progress and insufficient improvement, respectively. and These represent adjusting the step size upwards and downwards, respectively. Indicates the preset number of pauses; Indicates as of the date The number of consecutive improvements in each iteration is insufficient.

[0123] The rules in equations (11) and (12) mean that: when the two trees are obviously close, the target-oriented expansion trigger probability is increased so that the algorithm can more actively promote the meeting; when the two trees have general improvement, the target-oriented expansion trigger probability is increased by a small amount; when the continuous improvement of the two trees is insufficient and the preset number of stagnations is reached, the target-oriented expansion trigger probability is reduced so as to reduce repeated attempts on the blocked target direction and provide opportunities for subsequent local escape and skeleton recovery; when the improvement is insufficient but the preset number of stagnations has not been reached, the current target-oriented expansion trigger probability remains unchanged.

[0124] To ensure that the initial iteration is consistent with the above update rules, when At that time, the preset initial target will be guided to expand the trigger probability. As ;when When the first and second search trees are initialized, the distance between the nearest node pairs is used as the distance between the nearest nodes. Distance between the nearest node pairs in the next iteration .

[0125] The result of step S3 is to obtain the target-oriented expansion trigger probability used in the t-th iteration. This probability does not directly generate a new node, but rather affects whether step S4 prioritizes entering the goal-oriented propulsion chain.

[0126] Step S4: Generate intervals before proceeding to specific expansions. Uniform random numbers within .when When, perform a goal-oriented expansion operation; when At that time, a random expansion operation is performed.

[0127] In the goal-oriented expansion operation, a node selection method is chosen from three types according to preset strategy weights: nearest target type, cost-target collaborative type, and random leaf node type. The expansion starting point is determined accordingly, and the dynamic sub-target is used as the expansion reference point. In the random expansion operation, random sampling points are generated within the planning space, and the node closest to the random sampling point is selected from the active expansion tree as the expansion starting point. The random sampling point is used as the expansion reference point.

[0128] After obtaining the target-oriented expansion trigger probability based on the convergence trend in step S3, step S4 selects to perform either target-oriented expansion or random expansion based on this probability. Target-oriented expansion takes into account target attraction, path cost, and tree structure coverage through multi-mode node selection; random expansion is used to maintain the ability to explore the planning space and avoid the search from being overly concentrated on the direction of the blocked target.

[0129] In specific implementation, first according to Determine if the current round triggers goal-oriented expansion. If it does, proceed in the active tree. Node set Select the starting point for expansion. Each node... Record the cumulative path cost from the root of the tree to that node. The dynamic sub-target is .

[0130] The first mode is the nearest target type, which directly selects the distance. The nearest node: (13).

[0131] In equation (13), This represents the index of the node most recently selected using the target-oriented pattern. This represents the total number of active tree nodes. This mode is used to quickly advance the merging of two trees when the spatial structure allows.

[0132] The second model is the cost-goal collaborative model. To avoid selecting nodes that are close to the goal but have excessively high cumulative path costs, the cumulative cost and the distance to the goal are first normalized: (14).

[0133] In equation (14), This represents the normalized cumulative cost. Indicates the normalized target distance. and To prevent small positive numbers with a denominator of zero, a cost-objective co-scoring is then constructed: (15).

[0134] In equation (15), Represents a node The cost—goal-based collaborative scoring, Indicates the target distance weight. Select the node with the minimum cost-target collaboration score: .

[0135] In equation (16), This represents the node index selected by the cost-goal collaborative model. This model balances path cost and goal approximation, ensuring that the search tree maintains a reasonable path structure during its advancement.

[0136] The third mode is the random leaf node type. Let the set of leaf nodes in the active tree be... Then from A node is randomly selected uniformly: (17).

[0137] In equation (17), This represents the node index selected by the random leaf node type pattern. This represents the set of leaf node indices that currently have no children. This pattern allows edge branches of the tree structure to also participate in goal-oriented expansion, thereby enhancing coverage diversity.

[0138] Let the base weights of the three modes be respectively , , The currently available set of modes is Then the first The probability of this mode being selected is: (18).

[0139] In equation (18), Indicates the first The actual sampling probability of the pattern This indicates the basic weight of the pattern. This represents the set of currently available patterns. If a pattern is unavailable, for example, if the leaf node set is empty, it is automatically selected from the remaining patterns by weight normalization.

[0140] Determine the starting point and reference point of the expansion based on the selected expansion mode, and perform one step of expansion towards the reference point: (19).

[0141] In equation (19), Indicates the candidate points for direct expansion of the target. This indicates the starting point for the expansion chosen in this round. Indicates the maximum length for a single-step expansion. This indicates the actual expansion step size. When performing a target-oriented expansion operation, the expansion reference point is a dynamic sub-target; when performing a random expansion operation, the expansion reference point is a random sampling point. After generating expansion candidate points, immediately call equation (5) to determine whether the candidate connecting edge meets the unified feasibility criterion; if it meets the criterion, it is transferred to step S7 as a candidate new node; if it does not meet the criterion, it is transferred to step S5 to perform layered maneuvering to get out of trouble. Among them, the difference between the expansion reference point and the expansion starting point in equation (19) is calculated according to the aforementioned effective state difference vector, and the modulus in the denominator adopts the planned state distance; the expanded angle dimension is remapped to the preset period interval.

[0142] The significance of step S4 is that, when goal-oriented expansion is allowed, it does not blindly start from a single nearest node, but selects the expansion starting point based on the target distance, path cost, and tree structure branch coverage, thereby improving the stability and diversity of goal advancement.

[0143] Step S5: When the extended candidate edge does not meet the unified feasibility criterion, construct a local orthogonal basis and sequentially perform bilateral trial obstacle bypass and local lateral displacement recovery to restore propulsion capability in the local area.

[0144] If the extended candidate edges generated in step S4 do not meet the unified feasibility criterion, step S5 is used to attempt to restore propulsion capability within the current local region. This step utilizes local geometry to find alternative directions that can bypass obstacles without completely abandoning the direction of the dynamic sub-target, thus avoiding an immediate return to global random sampling after a straight-line extension is blocked.

[0145] First, the expansion starting point when prior goal-oriented expansion or random expansion fails. As the starting point for local recovery, the current dynamic sub-objective will still be used. As the overall guiding objective, the local main direction is defined as follows: (20).

[0146] In equation (20), Indicates starting from the current point Pointing to dynamic sub-targets The main reference direction. This direction represents the overall trend that should be maintained as much as possible for the current local recovery.

[0147] To construct a local obstacle avoidance direction, if the current node has a historical motion direction... Then project it onto In the orthogonal plane: (twenty one).

[0148] In equation (21), Indicates the direction the current node enters or the historical direction of movement. Indicates that it is perpendicular to Projection in a plane This represents the dot product of vectors. If... Greater than the degradation threshold Then let: (twenty two).

[0149] In equation (22), and These represent the first and second lateral directions of local restoration, respectively. This represents the cross product. If... If it is too small, then any value can be selected. Non-parallel reference axis construction Then, by cross product, we get This forms a locally orthogonal basis. This provides a directional basis for subsequent bilateral probing and local lateral displacement.

[0150] Equation (22) gives the method for constructing the lateral basis when the planning space dimension is 3. When the planning space dimension is greater than 3, two linearly independent reference vectors are selected in the orthogonal complement space of the local principal direction, and the first and second lateral directions are obtained by Gram-Schmidt orthogonalization or QR decomposition. If the projection of the historical motion direction in the orthogonal complement space is too small, the lateral direction is reconstructed by selecting a reference vector that is linearly independent of the local principal direction.

[0151] In the first layer of recovery, perform bilateral obstacle avoidance maneuvers. For the side-by-side obstacle avoidance maneuvers... Construct two trial points respectively: (twenty three).

[0152] In equation (23), and They represent the sides respectively. The corresponding first and second step trial points, and Indicates the forward thrust length along the main direction. and This indicates the offset length along the lateral direction. and These represent attempts to probe the positive and negative sides of the local basement, respectively.

[0153] For each side, judge sequentially and Whether it holds true. A trial on one side is considered valid only if both connections are secure. If both sides are valid, a scoring function is used to select the better side: (twenty four).

[0154] In equation (24), Indicates side Obstacle bypass side score, This indicates the clearance weight. The first term encourages the trial endpoint to be closer to the sub-objective, and the second term encourages the trial point to have a higher safe clearance. The side with the higher obstacle avoidance score is selected, and the corresponding... As a feasible new node. If only one side is valid, then the result of that side is used directly.

[0155] If both attempts fail, it indicates that the simple two-step obstacle avoidance structure cannot overcome the current local obstacle, and a second layer of recovery, namely local lateral displacement recovery, is required. This layer is located at the local base. Multiple single-step candidate points are generated internally using a combination of "forward propulsion + lateral offset": (25).

[0156] In equation (25), Indicates candidate points for local lateral displacement. Indicates the forward propulsion component, and These represent the offset components along the two lateral directions, respectively. , , Values ​​can be selected from a preset candidate set, for example... , , ,in This indicates the lateral offset magnitude.

[0157] For each candidate point, first determine Whether a point is valid or not, only safe and feasible candidate points are included in the scoring. The scoring function is defined as follows: (26).

[0158] In equation (26), This indicates the score of the candidate points for local lateral displacement. , and These represent the weights of propulsion, clearance, and directional consistency, respectively. Wherein: (27).

[0159] In equation (27), This represents the effective advance of the candidate point along the local principal direction. This represents the clearance from the candidate point to the set of obstacles. Indicates the direction of the candidate point relative to the main direction. The degree of consistency. Final selection. The largest candidate point is selected as the feasible new node.

[0160] The core of step S5 lies in forming a progressive local recovery: first, attempt to maintain strong forward continuity through bilateral probing; if this fails, then reconstruct the locally advanceable direction by scoring local lateral shift candidate points. If step S5 successfully generates a feasible new node, proceed to step S7; if step S5 still fails, it indicates that the current local structure is difficult to resolve through limited local corrections, and step S6 is required to perform skeleton-guided global recovery.

[0161] Step S6: When local recovery still fails, perform a skeleton-guided global recovery based on the soft-min headroom field, generate high headroom recovery samples, and restart the search tree expansion.

[0162] The soft-min net field is constructed based on the signed distances of each obstacle constraint and region boundary constraint within the area to be planned. It is used to smoothly approximate the minimum value among multiple signed distances while maintaining differentiability.

[0163] After the local layered escape attempt in step S5 fails, step S6 is used to re-find the recovery direction from a larger area. This step utilizes the obstacle distance field and free-space skeleton structure to generate recovery samples that are safer and more structurally guiding than ordinary random sampling. The problem this step addresses is that after a local obstacle avoidance failure, relying solely on uniform random sampling may result in samples falling into low-clearance or invalid regions, leading to low recovery efficiency; while the vicinity of the free-space skeleton typically has relatively high clearance, making it more suitable as a recovery sampling area.

[0164] First, construct the recovery seed point. The seed point for recovery can come from three types of locations: the first type is the midpoint of a failed edge, the second type is a perturbation point of an existing tree node, and the third type is a random safe point in the planning space. When a candidate edge in step S4 or S5... In case of failure, the midpoint of the failed edge is used as the seed point for recovery first: (28).

[0165] In equation (28), Indicates the restoration of the seed point. and Represents the two endpoints of a failed candidate edge. This seed is located near the blocked region and reflects the local geometric difficulties encountered in the current search.

[0166] Then, the soft-min net clearance field is defined. Let... Represents sample points To the The signed distance of a constraint or obstacle boundary is positive when the planning state is on the feasible side of the corresponding constraint, zero at the constraint boundary, and negative when it is on the infeasible side. Indicates the total number of constraints. Let represent the smoothing coefficient, then the soft-min net function is: (29).

[0167] In equation (29), Point The soft-min net clearance value at the location. Indicates the smoothing parameter. Point To the The signed distance of each constraint. This function is a differentiable approximation of the minimum distance function, used to guide the movement of samples from low clearance regions to high clearance regions.

[0168] Corresponding to equation (29), the first is defined A constraint at point The soft-min weights at point are: (30).

[0169] In equation (30), Indicates the first Each constraint has a relative dominance over the soft-min clearance at the current location, and the sum of all weights is 1. When a constraint is closest, its weight is usually larger; when multiple constraints have a near-equilibrium effect on the sample, it indicates that the sample may be located near the free space skeleton.

[0170] from Starting from the beginning, iteratively update along the upward direction of the soft-min net air field: (31).

[0171] In equation (31), Indicates the first The position of the next recovery iteration. Indicates the first Next iteration step size This represents the gradient of the soft-min net field at the current position. If the gradient magnitude is less than a preset gradient threshold, a new recovery seed point is selected. If the gradient magnitude is still less than the preset gradient threshold after reaching the preset number of recovery seed reconstructions, the current global recovery is considered a failure. If the gradient magnitude is not less than the preset gradient threshold, candidate update positions are generated along the gradient direction. An update is accepted only if the candidate update position is within the planning space, the connection edge between the current position and the candidate update position satisfies the unified feasibility criterion, and the soft-min net field value at the candidate update position is not lower than the soft-min net field value at the current position.

[0172] As a sample approaches the free-space skeleton, the influence of multiple obstacles or constraints on the sample typically tends to balance. Weighting... Sort by size, and denote the maximum weight as... The second largest weight is The condition for determining the proximity of the skeleton is: (32).

[0173] In equation (32), This represents the lower bound of the second dominant weight. This represents the threshold for the proximity of the two largest weights. When this condition is met, it indicates that the current position is subject to the approximate equilibrium effect of at least two dominant constraints, and it can be considered to be located in the free space skeleton or its neighborhood.

[0174] To avoid concentrating recovered samples at the same skeleton location, this application applies tangential perturbations near the skeleton. First, the first and second dominant constraints corresponding to the largest and second soft-min weights are determined, respectively. Then, the local normal direction is determined based on the difference in signed distance gradients between the first and second dominant constraints at the skeleton's vicinity. Next, the random vector is projected onto a tangent space orthogonal to the local normal direction and normalized to obtain the tangential random vector. Let the point satisfying the skeleton determination be... The tangential random vector is The disturbance amplitude is Then the recovered sample points are: (33).

[0175] In equation (33), This represents the final recovered sample point. Indicates the points adjacent to the skeleton. This represents a random perturbation vector generated along the tangential or approximately tangential direction of the skeleton. The disturbance amplitude is represented. This disturbance improves the distribution diversity of the recovered samples while maintaining the high clearance characteristics. When the magnitude of the difference between the signed distance gradients is not less than the preset normal threshold, the tangential direction and the recovered sample points are generated according to the effective local normal direction; when the magnitude of the difference between the signed distance gradients is less than the preset normal threshold, the number of times the normal reconstruction counter is incremented by 1 and the dominant constraint combination is reselected. When the number of times the normal reconstruction counter is less than the preset number of normal reconstructions, the local normal direction determination step is repeated; when the number of times the normal reconstruction counter reaches the preset number of normal reconstructions, the recovery seed point is reconstructed and the normal reconstruction counter is cleared to zero. When the number of times the recovery seed reconstruction is less than the preset number of times the recovery seed reconstruction is less than the preset number of times the recovery seed reconstruction is, the update steps corresponding to equations (31) to (33) are repeated; when the effective local normal direction is still not obtained after reaching the preset number of times the recovery seed reconstruction is reached, the global recovery is determined to be a failure.

[0176] If recovery sample points are successfully generated Then select the distance from the active tree. The nearest node serves as the starting point for recovery expansion. and towards Perform a one-step extension: (34).

[0177] In equation (34), This indicates the candidate new nodes generated by skeleton recovery expansion. This represents the node in the actively expanded tree that is closest to the recovered sample. This indicates the actual step size for recovery expansion. If recovery sample points are successfully generated and candidate connection edges meet the unified feasibility criterion, then the global recovery candidate points are transferred to step S7; if recovery sample points are not successfully generated, or candidate connection edges are still infeasible, then no new candidate nodes are generated in this round, and the round proceeds to the stopping condition judgment.

[0178] The significance of step S6 is that when local recovery fails, the algorithm no longer blindly and uniformly samples randomly, but instead uses the distance field to push the samples to the high-clearance free space skeleton region, thereby improving the effectiveness of global recovery sampling.

[0179] Step S7: After any extended chain successfully generates a feasible new node, the new node is connected to the current search tree, local reconnection is performed, and it is checked whether it is actually connected to the opposite tree.

[0180] After a feasible new node is successfully generated in any of steps S4, S5, or S6, step S7 formally connects the new node to the active tree and determines whether a real connection has been formed with the opposite tree. This step follows up on all previous expansion results, transforming the "candidate feasible point" into a valid node in the search tree and further determining whether the two trees have completed the executable path connection. The problem this step addresses is that generating only a feasible point does not mean that a path has been formed; it is also necessary to determine its parent node, update the cost, optimize the local structure, and rigorously verify whether the connection with the opposite tree is secure.

[0181] Let the feasible new node be The set of candidate parent nodes within the neighborhood of an active tree is For each candidate parent node Only when Only when a node is deemed safe to connect to is it allowed to become a candidate node for connection. Among all safe candidate nodes, the node with the lowest total cost is selected as the parent node. (35).

[0182] In equation (35), Indicates the index of the selected parent node. express The set of nearby candidate neighboring nodes Represents a node The cumulative cost from the root to this node. After selecting the parent node, Write to the active tree, recording its parent pointer and cumulative cost.

[0183] To further improve path quality, the following can be done: If a node already exists within the neighborhood, a partial reconnection is performed. For neighboring nodes... If through Connected to The total cost is lower than its original cost, and Then The parent node is updated to This process, while maintaining the safety criteria unchanged, gradually makes the local structure of the search tree tend to be lower cost and straighter.

[0184] After a new node is added, it's necessary to determine whether the two trees are truly connected. First, check the opposite tree. Finding distance The nearest node : (36).

[0185] In equation (36), Indicates the distance between the two trees. The most recent real node, Represent the opposite tree. Then determine the true connection condition of the two trees: (37).

[0186] In equation (37), This represents the distance threshold for the connection between the two trees. Only when... and The distance does not exceed Only when the connecting edges between the two trees satisfy a unified feasibility criterion is the dual-tree connection considered to be true.

[0187] It should be noted that the dynamic sub-target in step S2 is only used to guide the expansion direction and is not equivalent to the actual rendezvous point; the actual rendezvous must find an actual existing node in the opposite side tree and complete the distance and safety dual verification through equation (37). If equation (37) holds, backtracking is performed along the parent node chain of the candidate new node in the active expansion tree and the parent node chain of the connecting node in the opposite side tree, and splicing is performed in the direction from the starting state to the target state to obtain the candidate path for this iteration; the path cost of the candidate path is calculated and the candidate path is added to the candidate path set. If equation (37) does not hold, only the access of the candidate new node is completed in this round, and no candidate path is generated.

[0188] The path cost can be the sum of the lengths of the connecting edges between adjacent nodes in the candidate path; in other embodiments, a weighted path cost can also be constructed based on path length, safety clearance, degree of turning, or execution time.

[0189] After completing the current round of candidate path processing, determine whether the stopping condition is met. If the stopping condition is not met, return to step S2 to enter the next round of search; if the stopping condition is met, proceed to step S8. The stopping condition includes reaching a preset maximum number of iterations, or the candidate path set is not empty and the change in the minimum path cost in the candidate path set over a consecutive preset number of iterations is not greater than a preset change threshold.

[0190] Step S8: When the stopping condition is met, select the candidate path with the minimum cost and perform shortcut simplification, interpolation, B-spline smoothing and full path safety verification to output the final executable path.

[0191] After step S7 determines that the stopping condition is met, it first checks whether the candidate path set is empty. If the candidate path set is empty, it means that no feasible path that meets the path splicing condition has been obtained within the preset search conditions, and the path planning failure result is output; if the candidate path set is not empty, the candidate path with the minimum path cost is selected from it as the minimum cost planning path to be processed.

[0192] Subsequently, path simplification, path interpolation, and path smoothing are sequentially performed on the minimum cost planning path, and a unified feasibility review is re-executed on the entire processed path. The purpose of path processing is to reduce redundant nodes and local polylines, while ensuring that the processed path still meets the planning spatial boundary, collision-free, and minimum safe clearance constraints.

[0193] Let the minimum cost discrete path selected from the candidate path set be: (38).

[0194] In equation (38), This represents the discrete path with the minimum cost selected from the set of candidate paths. This indicates the number of nodes in the original path.

[0195] Then, a shortcut is performed to simplify the path. For any two non-adjacent nodes in the path... and ,like If the direct connection between the two satisfies the unified feasibility criterion and the additional clearance requirement, then the intermediate node between them can be deleted. The shortcut acceptance condition is: (39).

[0196] In equation (39), This indicates that the shortcut replaces the required lower safety clearance. Can be no less than The value is used to ensure that path simplification does not reduce the safety margin of consecutive segments. It is only allowed when direct connections are safe. Replace the original polyline path segment.

[0197] After completing the shortcut, if the distance between adjacent path points is too large, linear interpolation is performed between the adjacent points to ensure that the spacing between path points meets the execution or control requirements. Subsequently, B-spline smoothing can be applied to each coordinate component of the path to generate a continuous and smooth path. (40).

[0198] In equation (40), Represents the smoothed continuous path points. Indicates path parameters, , , These represent the B-spline functions corresponding to the three coordinate components. This indicates transpose. If this method is used for high-dimensional state spaces, the B-spline can also be extended to construct separate splines for each state dimension.

[0199] After the smooth path is generated, a full path safety check needs to be performed again. The smooth path is then re-discrete into several path points at preset intervals, and each adjacent point is evaluated segment by segment. Is it equal to 1? If any segment does not satisfy equation (5), then revert to the unsmoothed version, or reduce the smoothing intensity and smooth again; only when all path segments satisfy the unified feasibility criterion will the final path be output: .

[0200] In equation (41), Indicates the final output path. and Represents adjacent discrete points in the final path. This indicates the number of discrete points on the final path. This check ensures that the front-end search, path simplification, and smoothing processes do not violate the minimum safety clearance constraint.

[0201] Thus, this application completes a full closed loop from environmental modeling, dual-tree initialization, convergence trend adjustment, goal-oriented advancement, local hierarchical escape, global skeleton restoration, and dual-tree real connection to path post-processing.

[0202] Based on the same inventive concept, this application also provides a bidirectional search tree-based path planning system for implementing the bidirectional search tree-based path planning method described above. The solution provided by this system is similar to the implementation described in the above method. Therefore, the specific limitations of one or more bidirectional search tree-based path planning system embodiments provided below can be found in the limitations of the bidirectional search tree-based path planning method described above, and will not be repeated here.

[0203] In one exemplary embodiment, such as Figure 3 As shown, a path planning system based on a bidirectional search tree is provided, including: The information acquisition module 201 is used to acquire the starting state, target state, boundary constraints of the area to be planned, and obstacle information within the area to be planned, and to establish a unified feasibility criterion with minimum safe clearance as the constraint.

[0204] The search tree initialization module 202 is used to build a first search tree with the starting state as the root and add the starting state to the first search tree point set; build a second search tree with the target state as the root and add the target state to the second search tree point set; and set the initial value of the iteration number t to 1.

[0205] The active expansion tree determination module 203 is used to determine one of the first search tree and the second search tree as the active expansion tree and the other as the counterpart tree in the t-th iteration, and to determine the dynamic sub-target from the search tree point set corresponding to the counterpart tree.

[0206] The target-oriented expansion trigger probability determination module 204 is used to obtain the distance between the nearest node pair in the first search tree point set and the second search tree point set in the (t-1)th iteration when t>1, and determine the target-oriented expansion trigger probability in the tth iteration based on the change of the distance between the nearest node pairs; when t=1, the target-oriented expansion trigger probability in the tth iteration is the preset initial target-oriented expansion trigger probability.

[0207] The expansion candidate point generation module 205 is used to determine whether to perform a target-oriented expansion operation or a random expansion operation based on the target-oriented expansion trigger probability of the t-th iteration, and to determine the expansion start point and expansion reference point from the active expansion tree based on the determined expansion operation, and to generate expansion candidate points for the t-th iteration along the direction from the expansion start point to the expansion reference point.

[0208] The extended feasibility determination module 206 is used to select the extended candidate point as a candidate new node for the t-th iteration if the connection edge between the extended candidate point and the extended starting point satisfies the unified feasibility criterion.

[0209] The hierarchical recovery module 207 is used to perform a local obstacle bypass expansion operation if the connection edge between the expansion candidate point and the expansion starting point does not meet the unified feasibility criterion, and if the local obstacle bypass expansion operation successfully generates a local obstacle bypass candidate point, the local obstacle bypass candidate point is used as a candidate new node for the t-th iteration; if the local obstacle bypass expansion operation fails, a global recovery expansion operation is performed to generate skeleton neighborhood recovery sample points based on the soft-min net space field; if the global recovery expansion operation successfully generates a global recovery candidate point, the global recovery candidate point is used as a candidate new node for the t-th iteration; if the global recovery expansion operation fails, no candidate new node is generated for the t-th iteration.

[0210] The new node access and local reconnection module 208 is used to determine the target parent node of the candidate new node when the candidate new node is generated in the t-th iteration, access the candidate new node to the active expansion tree and perform local reconnection, and determine the node closest to the candidate new node in the search tree point set corresponding to the opposite side tree as the connection node for the t-th iteration.

[0211] The path splicing and cost calculation module 209 is used to splice the first path segment and the second path segment in the direction from the starting state to the target state if a candidate new node is generated in the t-th iteration and the path splicing condition is met, to obtain the candidate path of the t-th iteration and add it to the candidate path set, and at the same time calculate the path cost of the candidate path of the t-th iteration; the path splicing condition includes that the distance between the candidate new node and the connecting node is less than a preset connection threshold, and the connection edge between the candidate new node and the connecting node satisfies a unified feasibility criterion; the first path segment is the backtracking path of the candidate new node in the active expansion tree; the second path segment is the backtracking path of the connecting node in the opposite side tree; if no candidate new node is generated in the t-th iteration, or if a candidate new node is generated in the t-th iteration but the path splicing condition is not met, then no candidate path is added to the candidate path set in the t-th iteration.

[0212] The path result determination module 210 is used to determine the candidate path with the smallest path cost in the candidate path set as the minimum cost planning path and output it if the stopping condition is met and the candidate path set is not empty; if the stopping condition is met and the candidate path set is empty, the path planning failure result is output.

[0213] The iteration control module 211 is used to, if the stopping condition is not met, set t=t+1, redetermine the active expansion tree, the counterpart tree and the dynamic sub-target for the (t+1)th iteration, and repeat the steps from determining the active expansion tree, the counterpart tree and the dynamic sub-target to judging the stopping condition.

[0214] In summary, this application has the following beneficial effects.

[0215] 1. Improve search efficiency and bi-tree merging capability in complex environments. This application improves the bi-directional search tree's target bias adjustment based on merging trend perception, enabling the tree to dynamically adjust target guidance strength according to the actual approach state. During effective bi-tree approach, the algorithm enhances target traction to accelerate connection; during continuous search stagnation, the algorithm reduces the tendency for direct target expansion to minimize ineffective expansion. Therefore, compared to a fixed target bias approach, this application can more fully utilize search progress information, improving path search efficiency and merging success rate in complex and constrained environments.

[0216] 2. Enhanced local escape capabilities in obstacle-dense and narrow passage areas. This application maintains the branch coverage of the search tree through multi-mode target-oriented node selection; and continues to search for feasible advancement directions from the local geometry after the direct expansion to the target fails through bilateral trial obstacle avoidance and local lateral shift recovery. This mechanism preserves the advancement trend towards dynamic sub-targets while selecting safe detour directions based on local clearance and directional consistency, thereby improving the algorithm's ability to pass through obstacle-dense, concave obstacle, and narrow passage environments.

[0217] 3. Improve the safety, continuity, and engineering feasibility of the final path. This application generates high-clearance recovery samples through soft-min headroom and free-space skeleton structure, reducing inefficient random sampling and improving global recovery quality. Simultaneously, a unified safety check is performed before new node access, local reconnection, dual-tree connection, path shortcut simplification, interpolation, B-spline smoothing, and final output to prevent post-processing from violating safety constraints. Therefore, the final path not only avoids obstacles but also has fewer redundant inflections and better continuity, making it more suitable for execution by actual robots or robotic arms.

[0218] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 4 As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores path planning processing data based on a bidirectional search tree. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communication with external terminals via a network connection. When executed by the processor, the computer program implements a path planning method based on a bidirectional search tree.

[0219] Those skilled in the art will understand that Figure 4 The structures shown are merely block diagrams of some structures related to the present application and do not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than shown in the figures, or combine certain components, or have different component arrangements. In an exemplary embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.

[0220] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0221] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0222] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0223] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by computer program instructions and related hardware. The computer program can be stored in a computer-readable storage medium; the memory can include at least one of volatile memory and non-volatile memory; the processor can be a general-purpose processor or a special-purpose processor capable of executing computer programs, and this application does not limit its specific type.

[0224] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0225] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A path planning method based on a bidirectional search tree, characterized in that, include: Acquire the starting state, target state, boundary constraints of the area to be planned, and obstacle information within the area to be planned, and establish a unified feasibility criterion with minimum safe clearance as the constraint; Construct a first search tree with the starting state as the root, and add the starting state to the point set of the first search tree; Construct a second search tree with the target state as the root, and add the target state to the point set of the second search tree; Let the initial value of the iteration number t be 1; In the t-th iteration, one of the first search tree and the second search tree is selected as the active expansion tree, and the other is selected as the counterpart tree. The dynamic sub-target is then determined from the search tree point set corresponding to the counterpart tree. When t>1, obtain the distance between the nearest node pair in the first search tree point set and the second search tree point set in the (t-1)th iteration, and determine the target-oriented expansion trigger probability in the tth iteration based on the change in the distance between the nearest node pairs; when t=1, the target-oriented expansion trigger probability in the tth iteration is the preset initial target-oriented expansion trigger probability. Based on the target-oriented expansion trigger probability of the t-th iteration, determine whether to perform a target-oriented expansion operation or a random expansion operation, and determine the expansion start point and expansion reference point from the active expansion tree based on the determined expansion operation, and generate expansion candidate points for the t-th iteration along the direction from the expansion start point to the expansion reference point; If the connecting edge between the candidate node and the starting point satisfies the unified feasibility criterion, then the candidate node will be used as the candidate new node for the t-th iteration. If the connection edge between the expansion candidate point and the expansion starting point does not meet the unified feasibility criterion, a local obstacle bypass expansion operation is performed. If the local obstacle bypass expansion operation successfully generates a local obstacle bypass candidate point, the local obstacle bypass candidate point is used as a candidate new node for the t-th iteration. If the local obstacle bypass expansion operation fails, a global recovery expansion operation is performed to generate skeleton neighborhood recovery sample points based on the soft-min net field. If the global recovery expansion operation successfully generates a global recovery candidate point, the global recovery candidate point is used as a candidate new node for the t-th iteration. If the global recovery expansion operation fails, no new candidate node will be generated in the t-th iteration; If a candidate new node is generated in the t-th iteration, the candidate new node is added to the search tree point set corresponding to the active expansion tree, and the node closest to the candidate new node is determined in the search tree point set corresponding to the opposite side tree as the connection node for the t-th iteration. If a candidate new node is generated in the t-th iteration and the path splicing condition is met, then the first path segment and the second path segment are spliced ​​together in the direction from the starting state to the target state to obtain the candidate path of the t-th iteration and added to the candidate path set. At the same time, the path cost of the candidate path of the t-th iteration is calculated. The path splicing condition includes that the distance between the candidate new node and the connecting node is less than a preset connection threshold, and the connection edge between the candidate new node and the connecting node satisfies a unified feasibility criterion. The first path segment is the backtracking path of the candidate new node in the active expansion tree; the second path segment is the backtracking path of the connecting node in the opposite side tree. If no new candidate node is generated in the t-th iteration, or if a new candidate node is generated in the t-th iteration but does not meet the path splicing condition, then no candidate path will be added to the candidate path set in the t-th iteration. If the stopping condition is met and the candidate path set is not empty, then the candidate path with the minimum path cost in the candidate path set is selected as the minimum cost planning path and output. If the stopping condition is met and the candidate path set is empty, output the path planning failure result; If the stopping condition is not met, let t = t + 1, redetermine the active expansion tree, the counterpart tree, and the dynamic sub-objective for the (t + 1)th iteration, and repeat the steps from determining the active expansion tree, the counterpart tree, and the dynamic sub-objective to judging the stopping condition.

2. The path planning method based on a bidirectional search tree according to claim 1, characterized in that, In the t-th iteration, one search tree is selected from the first search tree and the second search tree as the active expansion tree, and the other is selected as the counterpart tree. The dynamic sub-target is then determined from the search tree point set corresponding to the counterpart tree, specifically including: In the t-th iteration, an active expansion tree is determined by a rotation method or a random sampling method, and the other tree is used as the opposite tree; the rotation method includes alternating between the first search tree and the second search tree as the active expansion tree; the random sampling method includes determining the active expansion tree by Bernoulli random sampling method according to a preset switching probability; When t=1, the root node of the opposite tree or the node in the opposite tree closest to the actively expanding tree is determined as the initial dynamic sub-target; when t>1, a first dynamic sub-target update mode or a second dynamic sub-target update mode is pre-selected; in the first dynamic sub-target update mode, the dynamic sub-target is updated to the node in the opposite tree closest to the actively expanding tree with a preset update probability, and the dynamic sub-target of the (t-1)th iteration is retained with a probability of 1 minus the preset update probability; in the second dynamic sub-target update mode, the dynamic sub-target is updated to the newly generated node in the opposite tree with the preset update probability, and the dynamic sub-target of the (t-1)th iteration is retained with a probability of 1 minus the preset update probability.

3. The path planning method based on a bidirectional search tree according to claim 1, characterized in that, Obtain the distance between the nearest node pairs in the first search tree node set and the second search tree node set in the (t-1)th iteration, and determine the target-oriented expansion trigger probability in the tth iteration based on the change in the distance between the nearest node pairs, specifically including: When t>1, obtain the distance between the nearest node pair in the first search tree point set and the second search tree point set in the (t-1)th iteration, and use the difference between the nearest node pair distance in the (t-2)th iteration and the nearest node pair distance in the (t-1)th iteration as the single-step distance improvement amount in the (t-1)th iteration. If the single-step distance improvement in the (t-1)th iteration is greater than the significant progress threshold, the continuous insufficient improvement counter will be cleared, and the smaller value between the sum of the target-oriented expansion trigger probability and the step size adjustment in the (t-1)th iteration and the preset maximum target-oriented expansion trigger probability will be used as the target-oriented expansion trigger probability in the tth iteration. If the single-step distance improvement in the (t-1)th iteration is greater than or equal to the insufficient improvement threshold and less than or equal to the significant progress threshold, the continuous insufficient improvement counter will be cleared, and the smaller of the sum of the target-oriented expansion trigger probability and half of the upward adjustment step size in the (t-1)th iteration and the preset maximum target-oriented expansion trigger probability will be used as the target-oriented expansion trigger probability in the tth iteration. If the single-step distance improvement in the (t-1)th iteration is less than the insufficient improvement threshold, the number of consecutive insufficient improvement counters is incremented by 1, and it is determined whether the number of consecutive insufficient improvement counters is greater than or equal to the preset number of stagnations. If the number of consecutive insufficient improvement counters is greater than or equal to the preset number of stagnations, the larger of the difference between the target-oriented expansion trigger probability and the step size reduction in the (t-1)th iteration and the preset minimum target-oriented expansion trigger probability is taken as the target-oriented expansion trigger probability in the tth iteration. If the number of consecutive insufficient improvement counters is less than the preset number of stagnations, the target-oriented expansion trigger probability in the (t-1)th iteration is taken as the target-oriented expansion trigger probability in the tth iteration. When t=2, the distance between the nearest node pairs when the first search tree and the second search tree are initialized is taken as the distance between the nearest node pairs in the 0th iteration. A positive single-step distance improvement indicates that the first search tree and the second search tree are close to each other. The initial value of the consecutive insufficient improvement counter is 0, and the significant progress threshold is greater than the insufficient improvement threshold.

4. The path planning method based on a bidirectional search tree according to claim 1, characterized in that, The execution of either a target-oriented expansion operation or a random expansion operation is determined based on the target-oriented expansion trigger probability of the t-th iteration, specifically including: Generate a uniformly distributed random number between 0 and 1; If the uniformly distributed random number is less than the target-oriented expansion trigger probability of the t-th iteration, then the target-oriented expansion operation is performed. The target-oriented expansion operation includes selecting a node selection strategy from multiple node selection strategies according to a preset selection probability, determining the expansion starting point from the node set of the active expansion tree, taking the direction from the expansion starting point to the dynamic sub-target as the expansion direction, and generating expansion candidate points for the t-th iteration from the expansion starting point along the expansion direction with a first preset step size. The first preset step size is the smaller value between the upper limit of the single-step expansion length and the distance from the expansion starting point to the dynamic sub-target. The multiple node selection strategies include a first node selection strategy that selects the node closest to the dynamic sub-target in the active expansion tree as the expansion starting point, a second node selection strategy that constructs a cost-target collaborative score based on the cumulative path cost of each node in the active expansion tree and the distance from each node to the dynamic sub-target, and selects the node with the smallest cost-target collaborative score as the expansion starting point, and a third node selection strategy that uniformly and randomly selects nodes from the leaf node set of the active expansion tree as the expansion starting point. If the uniformly distributed random number is greater than or equal to the target-oriented expansion trigger probability of the t-th iteration, then a random expansion operation is performed. The random expansion operation includes generating a random sampling point in the area to be planned, selecting the node closest to the random sampling point from the active expansion tree as the expansion starting point, taking the direction from the expansion starting point to the random sampling point as the expansion direction, and generating expansion candidate points for the t-th iteration from the expansion starting point along the expansion direction with a second preset step size. The second preset step size is the smaller value between the upper limit of the single-step expansion length and the distance between the expansion starting point and the random sampling point.

5. The path planning method based on a bidirectional search tree according to claim 1, characterized in that, Perform a local obstacle-bypass expansion operation, and when the local obstacle-bypass expansion operation successfully generates local obstacle-bypass candidate points, use these candidate points as candidate new nodes for the t-th iteration, specifically including: Using the extended starting point as the local recovery starting point, a local orthogonal basis is constructed through orthogonalization, with the direction from the local recovery starting point to the dynamic sub-target as the local principal direction; the local orthogonal basis includes the local principal direction, the first lateral direction, and the second lateral direction; In the plane formed by the local main direction and the first lateral direction, the first step test point and the second step test point are generated on the positive side and the negative side of the first lateral direction, respectively. Sequentially determine whether the connection edges between the first step trial point on each side and the local recovery starting point, and the connection edges between the second step trial point on each side and the corresponding first step trial point, satisfy the unified feasibility criterion; If both connecting edges on only one side satisfy the unified feasibility criterion, then the second trial point on the side that satisfies the unified feasibility criterion will be taken as a local obstacle bypass candidate point. If both connecting edges on both sides satisfy the unified feasibility criterion, then the obstacle bypass side score is constructed based on the distance between the second step test point on each side and the dynamic sub-target and the safe clearance of the test path on each side. The side with the highest obstacle bypass side score is determined as the optimal side, and the second step test point on the optimal side is used as a local obstacle bypass candidate point. If at least one connecting edge on both sides does not meet the unified feasibility criterion, then multiple candidate points are generated in the local orthogonal basis. Each candidate point is composed of a forward propulsion component along the local principal direction, a first lateral offset component along the first lateral direction, and a second lateral offset component along the second lateral direction. If there is at least one candidate point whose connection edge with the local recovery starting point satisfies the unified feasibility criterion, then the local lateral displacement candidate points are scored according to the forward propulsion component along the local main direction, the safe clearance, and the degree of consistency between the expansion direction and the local main direction. The candidate point with the highest local lateral displacement candidate point score is taken as the local obstacle bypass candidate point. If there is no candidate point whose connecting edge to the local recovery starting point satisfies the unified feasibility criterion, then the local obstacle bypass expansion operation is judged to have failed.

6. The path planning method based on a bidirectional search tree according to claim 1, characterized in that, If the local obstacle-around expansion operation fails, a global recovery expansion operation is performed based on the soft-min net field to generate skeleton neighborhood recovery sample points, specifically including: Construct a recovery seed point and use the position of the recovery seed point as the current position; Based on the signed distances of each obstacle constraint and the region boundary constraint within the area to be planned, the differentiable approximate minimum distance function obtained by smooth weighting is defined as the soft-min net airfield. The final seed position for the t-th iteration is determined using the following position update steps: Calculate the soft-min net space gradient at the current position of the current sub-iteration; Move along the direction of the soft-min net space gradient at the current position of the current sub-iteration by one recovery update step to obtain the candidate update position of the current sub-iteration; If the connection edge between the current position and the candidate update position in the current sub-iteration satisfies the unified feasibility criterion, the candidate update position is located within the planning area, and the soft-min net value at the candidate update position is greater than or equal to the soft-min net value at the current position, then the current position is updated as the candidate update position. If the connection edge between the current position and the candidate update position does not meet the unified feasibility criterion, or the candidate update position is located outside the area to be planned, or the soft-min net value at the candidate update position is less than the soft-min net value at the current position, then the current position remains unchanged. If the loop stopping condition is met, the current position of the current sub-iteration is taken as the final seed position of the t-th iteration. The loop stopping condition includes the current position satisfying the free space skeleton neighborhood determination condition or reaching the preset maximum number of sub-iterations. The free space skeleton neighborhood determination condition is: after sorting the soft-min weights of each constraint corresponding to the current position in descending order of value, the maximum soft-min weight and the second largest soft-min weight are both greater than the preset weight lower limit, and the difference between the two is less than the preset proximity threshold. If the loop stopping condition is not met, increment the sub-iteration count by 1 and repeat the above position update steps. If the final seed position of the t-th iteration satisfies the free space skeleton neighborhood determination condition, then the local normal direction is determined according to the difference between the signed distance gradients of the constraints corresponding to the largest soft-min weight and the second largest soft-min weight. The random vector is projected onto the tangent space orthogonal to the local normal direction to obtain the tangent direction, and a random offset is applied to the final seed position along the tangent direction to generate the recovery sample point of the t-th iteration. If the recovery sample point of the t-th iteration is successfully generated, the node closest to the recovery sample point of the t-th iteration is selected from the active expansion tree as the recovery expansion starting point, and expansion is performed in the direction from the recovery expansion starting point to the recovery sample point to generate the global recovery candidate point of the t-th iteration. If the connection edge between the global recovery candidate point in the t-th iteration and the recovery extension starting point satisfies the unified feasibility criterion, then the global recovery candidate point in the t-th iteration will be used as the candidate new node in the t-th iteration. If the recovery sample point for the t-th iteration is not successfully generated, or the connection edge between the global recovery candidate point for the t-th iteration and the recovery expansion starting point does not meet the unified feasibility criterion, or the final seed position for the t-th iteration does not meet the free space skeleton neighborhood determination condition, then the global recovery expansion operation is determined to have failed.

7. The path planning method based on a bidirectional search tree according to claim 1, characterized in that, The candidate new node is added to the search tree point set corresponding to the active expansion tree, and the node closest to the candidate new node is determined in the search tree point set corresponding to the opposite tree as the connection node for the t-th iteration. Specifically, this includes: In the search tree point set corresponding to the active expansion tree, nodes whose distance from the planning state of the candidate new node in the t-th iteration is less than the preset neighborhood radius are added to the candidate parent node set. In the set of candidate parent nodes, candidate parent nodes whose connection edges with the candidate new node in the t-th iteration satisfy the unified feasibility criterion are selected and added to the set of feasible candidate parent nodes. Calculate the sum of the cumulative path cost from the active expansion tree root node to each feasible candidate parent node in the feasible candidate parent node set and the length of the connecting edge from the candidate new node to each feasible candidate parent node in the feasible candidate parent node set to obtain the total generation value of each feasible candidate parent node. Select the feasible candidate parent node corresponding to the minimum total generation value as the target parent node of the candidate new node in the t-th iteration. If the feasible candidate parent node set is empty, the expansion starting point used when generating the candidate new node is taken as the target parent node. The candidate new node of the t-th iteration is connected to the active expansion tree. The identifier of the target parent node and the cumulative path cost from the root node of the active expansion tree through the target parent node to the candidate new node of the t-th iteration are recorded. The candidate new node of the t-th iteration is added to the search tree point set corresponding to the active expansion tree. In the search tree point set corresponding to the active expansion tree, nodes whose distance from the planning state of the candidate new node in the t-th iteration is less than the preset neighborhood radius are determined as the neighborhood node set; Take any neighboring node in the set of neighboring nodes as the current neighboring node, and determine whether the current neighboring node meets the candidate update conditions. The candidate update conditions include that the cumulative path cost from the active expansion tree root node through the candidate new node in the t-th iteration to the current neighboring node is less than the cumulative path cost from the active expansion tree root node to the current neighboring node, and the connection edge between the current neighboring node and the candidate new node in the t-th iteration satisfies the unified feasibility criterion. If the current neighboring node meets the candidate update condition, then update the parent node of the current neighboring node to the candidate new node in the t-th iteration, and update the cumulative path cost of the current neighboring node and all its descendant nodes. If the current neighboring node does not meet the candidate update condition, then keep the parent node of the current neighboring node unchanged; The next neighbor node is used as the current neighbor node, until all neighbor nodes in the neighbor node set have been traversed; In the search tree point set corresponding to the opposite tree, the node with the smallest planning state distance to the candidate new node in the t-th iteration is determined as the connection node in the t-th iteration.

8. A path planning system based on a bidirectional search tree, characterized in that, The path planning system based on a bidirectional search tree is used to implement the path planning method based on a bidirectional search tree as described in any one of claims 1 to 7, wherein the path planning system based on a bidirectional search tree includes: The information acquisition module is used to acquire the starting state, target state, boundary constraints of the area to be planned, and obstacle information within the area to be planned, and to establish a unified feasibility criterion with minimum safe clearance as the constraint. The search tree initialization module is used to build a first search tree with the starting state as the root and add the starting state to the first search tree node set; build a second search tree with the target state as the root and add the target state to the second search tree node set; and set the initial value of the iteration number t to 1. The active expansion tree determination module is used to determine one of the first search tree and the second search tree as the active expansion tree and the other as the counterpart tree in the t-th iteration, and to determine the dynamic sub-target from the search tree point set corresponding to the counterpart tree. The target-oriented expansion trigger probability determination module is used to obtain the distance between the nearest node pair in the first search tree point set and the second search tree point set in the (t-1)th iteration when t>1, and determine the target-oriented expansion trigger probability in the tth iteration based on the change in the distance between the nearest node pairs; when t=1, the target-oriented expansion trigger probability in the tth iteration is the preset initial target-oriented expansion trigger probability. The expansion candidate point generation module is used to determine whether to perform a target-oriented expansion operation or a random expansion operation based on the target-oriented expansion trigger probability of the t-th iteration, and to determine the expansion start point and expansion reference point from the active expansion tree based on the determined expansion operation, and to generate expansion candidate points for the t-th iteration along the direction from the expansion start point to the expansion reference point. The extended feasibility determination module is used to select the extended candidate point as the candidate new node for the t-th iteration if the connection edge between the extended candidate point and the extended starting point satisfies the unified feasibility criterion. The hierarchical recovery module is used to perform a local obstacle bypass expansion operation if the connection edge between the expansion candidate point and the expansion starting point does not meet the unified feasibility criterion. If the local obstacle bypass expansion operation successfully generates a local obstacle bypass candidate point, the local obstacle bypass candidate point is used as a candidate new node for the t-th iteration. If the local obstacle bypass expansion operation fails, a global recovery expansion operation is performed to generate skeleton neighborhood recovery sample points based on the soft-min net field. If the global recovery expansion operation successfully generates a global recovery candidate point, the global recovery candidate point is used as a candidate new node for the t-th iteration. If the global recovery expansion operation fails, no candidate new node is generated for the t-th iteration. The new node access and local reconnection module is used to determine the target parent node of the candidate new node when generating candidate new nodes in the t-th iteration, access the candidate new node to the active expansion tree and perform local reconnection, and determine the node closest to the candidate new node in the search tree point set corresponding to the opposite tree as the connection node of the t-th iteration. The path splicing and cost calculation module is used to, if a candidate new node is generated in the t-th iteration and the path splicing condition is met, splice the first path segment and the second path segment in the direction from the starting state to the target state to obtain the candidate path for the t-th iteration and add it to the candidate path set, and calculate the path cost of the candidate path for the t-th iteration. The path splicing condition includes that the distance between the candidate new node and the connecting node is less than a preset connection threshold, and the connection edge between the candidate new node and the connecting node satisfies a unified feasibility criterion. The first path segment is the backtracking path of the candidate new node in the active expansion tree; the second path segment is the backtracking path of the connecting node in the opposite side tree. If no candidate new node is generated in the t-th iteration, or if a candidate new node is generated in the t-th iteration but the path splicing condition is not met, then no candidate path is added to the candidate path set in the t-th iteration. The path result determination module is used to determine the path with the lowest path cost in the candidate path set as the minimum cost planning path and output it if the stopping condition is met and the candidate path set is not empty; if the stopping condition is met and the candidate path set is empty, the path planning failure result is output. The iteration control module is used to, if the stopping condition is not met, set t=t+1, redetermine the active expansion tree, the counterpart tree, and the dynamic sub-target for the (t+1)th iteration, and repeat the steps from determining the active expansion tree, the counterpart tree, and the dynamic sub-target to judging the stopping condition.

9. A computer device, comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor executes the computer program to implement the path planning method based on a bidirectional search tree as described in any one of claims 1 to 7.

10. A computer program product, comprising a computer program, characterized in that, When executed by a processor, the computer program implements the path planning method based on a bidirectional search tree as described in any one of claims 1 to 7.

Images