Multi-point breadth-first search path planning method based on bending radius constraint

Abstract

The invention discloses a multi-point breadth-first search path planning method based on bending radius constraint. The method comprises the following steps: constructing a directed weighted graph model of an environment map; multi-point breadth-first search is carried out, a priority queue containing a plurality of candidate starting points is initialized, and on the basis of the directed weighted graph, the path accumulation bending cost and the edge weight sum serve as search bases, and search is carried out from all the starting points in parallel till a termination condition is met; and backtracking from the target node meeting the termination condition, generating an initial path node sequence, carrying out smooth optimization on the sequence, and outputting a final path meeting the bending radius constraint. According to the method, the dynamic weighted map model is constructed, multi-starting-point parallel breadth-first search is adopted, the bending cost is accumulated in real time, and finally the smooth path conforming to the bending radius constraint is generated through smooth optimization, so that the vehicle driving problem caused by improper path curvature is systematically solved.

Description

Technical Field

[0001] This invention relates to the field of regulatory information fusion technology for open-pit mines, and in particular to a multi-point breadth-first search path planning method based on bending radius constraints. Background Technology

[0002] With the development of autonomous driving technology, the real-time performance, safety, and smoothness of local path planning have become key challenges. Traditional path planning methods often neglect the continuous curvature constraints of the path in complex dynamic environments, resulting in excessively small curvature radii of the generated trajectory. This leads to a series of problems such as vehicle instability, accelerated mechanical wear, and reduced ride comfort.

[0003] In autonomous driving local path planning, if the path curvature radius is too small, the following problems may occur when the vehicle is driving autonomously:

[0004] 1. Risk of vehicle instability and skidding A bending radius that is too small will cause a sharp increase in centrifugal force when the vehicle turns, exceeding the tire's grip limit and leading to skidding or even rollover. Frequent intervention of the ESP system: The electronic stability program may be forced to intervene frequently, affecting ride smoothness.

[0005] 2. Increased mechanical wear Frequent, large-angle turns of the steering wheel accelerate the wear and tear on the steering motor and tires. Continuous sharp turns cause additional stress on suspension components, shortening their lifespan.

[0006] 3. Decreased passenger comfort Discontinuous curvature of the road (such as a sharp turn connecting to a straight road) can cause the vehicle to shake violently, resulting in motion sickness for passengers.

[0007] Unsmooth paths can cause high-frequency vibrations in vehicles.

[0008] 4. The real-time performance of the planning algorithm is compromised. To fix paths with excessively small curvature radii, the algorithm requires additional iterative optimization (such as increasing path sampling points or adjusting cost function weights), which increases computational complexity and slows down decision-making.

[0009] If optimization takes too long, it may not be able to respond to sudden obstacles in a timely manner.

[0010] To address these issues, existing technologies primarily employ cost functions with curvature constraints or hybrid optimization methods combining graph search and intelligent algorithms. However, cost function-based methods require frequent weight adjustments in dynamic scenarios, resulting in significant computational delays. Hybrid optimization methods, on the other hand, are prone to getting trapped in local optima, and the dynamic adjustment of multi-objective weights is time-consuming, making them unsuitable for scenarios with high real-time requirements, such as high-speed driving. Therefore, a novel path planning method is urgently needed that can deeply embed curvature radius constraints into the search process while balancing real-time response and path smoothness. Summary of the Invention

[0011] The purpose of this invention is to overcome the shortcomings of the existing technology. To achieve the above objective, a multi-point breadth-first search path planning method based on bending radius constraints is adopted to solve the problems mentioned in the background technology.

[0012] A multi-point breadth-first search path planning method based on bending radius constraints includes the following steps: S1. Construct a directed weighted graph model of the environment map, where nodes represent key points of the path, and the weights of the edges are dynamically determined based on distance, curvature changes, and distance to obstacles. S2. Perform a multi-point breadth-first search, initialize a priority queue containing multiple candidate starting points, and based on the directed weighted graph, use the cumulative path curvature cost and the sum of edge weights as the search criteria to conduct the search in parallel from each starting point until the termination condition is met; wherein, the cumulative path curvature cost is calculated based on the curvature change of the path. S3. Backtrack from the target node that meets the termination condition to generate an initial path node sequence, and perform smooth optimization on the sequence to output the final path that meets the bending radius constraint.

[0013] As a further aspect of the present invention: the weight (w) of the edge is calculated using the following formula:

[0014] Where d is the distance between nodes. For the curvature variation between nodes, The distance between the node and the obstacle. These are weighting coefficients that are dynamically adjusted based on the scenario.

[0015] As a further aspect of the present invention: in step S1, in the directed weighted graph model, the nodes are stored in the form of a structure, which includes a data layer and a pointer layer; The data layer includes at least the node's position and normal direction information; The pointer layer is used to indicate the connection relationships between nodes.

[0016] As a further aspect of the present invention: in step S2, the initialization of the priority queue specifically involves: The vehicle's current location and alternative path branch points are added to the queue as candidate starting points. The initial cumulative bending cost of each starting point is set to 0, and they are sorted in ascending order according to the estimated total path weight.

[0017] As a further aspect of the present invention: in step S2, the search criteria include: taking the node with the smallest total weight from the queue for expansion, calculating the bending cost increment and total weight of its unvisited neighboring nodes, updating the cumulative bending cost and predecessor node information of the neighboring nodes, and then adding it to the queue.

[0018] As a further aspect of the present invention: in the calculation of the bending cost increment and total weight, the weight w of the edge is related to the bending radius r, specifically as follows:

[0019] in, This is the curvature weighting coefficient. The penalty coefficient for changes in direction. This represents the difference in direction angle between adjacent nodes.

[0020] As a further aspect of the present invention: in step S2, the path accumulates bending cost. Calculated using the following formula:

[0021] in, Let be the length of the i-th path segment. Let be the curvature of the i-th node.

[0022] As a further aspect of the present invention: in step S2, the termination condition is: the search reaches the preset target node, or the cumulative bending cost of the current path is reached. Exceeding the safety threshold.

[0023] As a further aspect of the present invention: in step S3, the smoothing optimization specifically involves applying cubic spline interpolation to the initial path node sequence obtained by backtracking and applying a maximum curvature constraint to obtain a smooth path with continuous curvature.

[0024] As a further aspect of the present invention: In step S2, during the node expansion process, the predecessor node of each visited node is recorded in real time to form a predecessor node array, which is used for path backtracking in step S3.

[0025] Compared with the prior art, the present invention has the following technical advantages: The above technical solution first constructs a dynamically weighted map model that integrates distance, curvature changes, and obstacle information. Then, a multi-starting-point parallel breadth-first search strategy is employed, calculating and accumulating the path's curvature cost in real time during the search process, and guiding the search to avoid sharp bends through a weighting mechanism. Finally, the obtained node sequence is smoothed and optimized to generate a final path with continuous curvature that conforms to vehicle kinematic constraints. This solution deeply embeds curvature radius constraints into the search process, achieving synergistic optimization of path safety, smoothness, and planning efficiency. By deeply integrating curvature radius constraints into the search algorithm kernel, multiple beneficial effects are achieved: significantly improving real-time response capabilities in dynamic environments and avoiding computational delays caused by repeated optimization of sharp bends in traditional methods; effectively balancing path smoothness and safety, generating a path with continuous curvature, greatly reducing the risk of vehicle sideslip and ride discomfort; enhancing the algorithm's adaptability to complex road topologies through a multi-starting-point parallel search mechanism, avoiding the local optimum trap caused by a single starting point; and ultimately, while ensuring real-time planning, outputting a high-quality path that better conforms to vehicle motion characteristics and human driving habits. Attached Figure Description

[0026] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings: Figure 1 This is a schematic diagram illustrating the steps of the path planning method according to an embodiment of this application. Detailed Implementation

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

[0028] Please refer to Figure 1 In this embodiment of the invention, a multi-point breadth-first search path planning method based on bending radius constraints includes the following steps: S1. Construct a directed weighted graph model of the environment map, where nodes represent key points of the path, and the weights of the edges are dynamically determined based on distance, curvature changes, and distance to obstacles. In this embodiment, in step S1, the nodes in the directed weighted graph model are stored in the form of a structure, which includes a data layer and a pointer layer. The data layer includes at least the node's position and normal direction information; The pointer layer is used to indicate the connection relationships between nodes.

[0029] In this embodiment, the weight w of the edge is calculated using the following formula:

[0030] Where d is the distance between nodes. For the curvature variation between nodes, The distance between the node and the obstacle. These are weighting coefficients that are dynamically adjusted based on the scenario.

[0031] Specifically, nodes are key points on a local path (such as the center point of a road or the boundary point of an obstacle).

[0032] The three weighting coefficients are dynamically adjusted according to the scenario. , , ) To achieve optimal local path planning, the first node in the directed weighted sparse graph of local paths is stored in a node array.

[0033] During the traversal, a global array Pre[i] is created to store the previous node of each visited node, so that the path nodes of the subsequent optimal local path are output sequentially. To avoid repeated visits to path nodes, a node state array V[n] is created to store the visit status of all path nodes. If [i]=1, it means that path node i has been visited; if V[i]=0, it means that path node i has not been visited. Initially, the visit status of all path nodes is set to 0.

[0034] (p1, p2, ..., p) n As nodes in a local path, each node p i (i=1, 2, ..., n) as a structure all contain a data layer and a pointer layer.

[0035] The data layer contains information about the normal direction of the points traversed by the local path, the node name, and the name of the local path to which it belongs (the local path name is NULL except for the first and last nodes). The first and last nodes indicate the start and end points of the local path and need to provide the local path name, while other nodes only serve the function of path selection and therefore do not contain data information such as the local path name.

[0036] The pointer layer of a node contains information pointing to the next node. If it is the tail node, the pointer value is NULL, indicating the end of the local path; if it is a branch node, there can be multiple pointers, the first node also contains a pointer to the local path adjacency matrix, and other nodes are NULL.

[0037] S2. Perform a multi-point breadth-first search, initialize a priority queue containing multiple candidate starting points, and based on the directed weighted graph, use the cumulative path curvature cost and the sum of edge weights as the search criteria to conduct the search in parallel from each starting point until the termination condition is met; wherein, the cumulative path curvature cost is calculated based on the curvature change of the path. Among them, the Breadth First Search (BFS) algorithm has the characteristics of hierarchical search and is suitable for application in local path planning of key location nodes based on global path planning.

[0038] This embodiment improves upon the BFS algorithm by proposing a Multi-point Breadth First Search (MBFS) algorithm based on bending radius constraints, taking into account the hierarchical nature of local paths.

[0039] After obtaining the access order of local path locations and related local path curvature information, local path planning optimization based on curvature radius is performed.

[0040] In this embodiment, step S2, specifically, involves initializing the priority queue as follows: The vehicle's current location and alternative path branch points are added to the queue as candidate starting points. The initial cumulative bending cost of each starting point is set to 0, and they are sorted in ascending order according to the estimated total path weight.

[0041] In this embodiment, the search criteria in step S2 include: taking the node with the smallest total weight from the queue for expansion, calculating the bending cost increment and total weight of its unvisited neighboring nodes, updating the cumulative bending cost and predecessor node information of the neighboring nodes, and then adding it to the queue.

[0042] The multi-starting-point parallel search mechanism is as follows: Initialize multiple candidate starting points (such as the vehicle's current position and alternative path branch points), and simultaneously perform BFS traversal to avoid the local optimum trap caused by a single starting point.

[0043] The edge weights are adjusted in real time based on the path curvature (e.g., the smaller the curvature radius, the greater the weight penalty value) to guide the search to avoid sharp bends.

[0044] In this embodiment, in the calculation of the bending cost increment and total weight, the edge weight w is related to the bending radius r, specifically as follows:

[0045] in, This is the curvature weighting coefficient. The penalty coefficient for changes in direction. This represents the difference in direction angle between adjacent nodes.

[0046] In this embodiment, in step S2, the path accumulates bending cost. Calculated using the following formula:

[0047] in, Let be the length of the i-th path segment. Let be the curvature of the i-th node.

[0048] In this embodiment, the termination condition in step S2 is: the search reaches the preset target node, or the cumulative bending cost of the current path is reached. Exceeding the safety threshold.

[0049] In this embodiment, in step S2, during the node expansion process, the predecessor node of each visited node is recorded in real time to form a predecessor node array, which is used for path backtracking in step S3.

[0050] In the specific implementation steps: S21 initializes the multi-start queue; Sort the paths in ascending order of total weight, create a priority queue, and add multiple candidate starting points (such as the vehicle's current location and alternative path branch points) to the queue.

[0051] Each starting point is initially set to a weight of 0 and marked as visited to avoid repeated exploration.

[0052] S22 Node Expansion and Weight Calculation; The node with the smallest weight is taken from the queue and used as the current expansion point.

[0053] If the current node is the target point or its cumulative curvature cost If the safety threshold is exceeded (e.g., bending radius < 5m), the search will terminate.

[0054] For each unvisited neighbor of the current node, traverse the neighboring nodes.

[0055] Based on the curvature change between adjacent nodes Given k and path length (d), calculate the bending cost increment.

[0056] Based on the combined distance, curvature change, and obstacle distance, update the three weighting coefficients. , , ).

[0057] S23 Node Attribute Update and Enqueueing; Update the properties of adjacent nodes and record their cumulative bending cost. ), and set the predecessor node as the current node.

[0058] Adjacent nodes are weighted by total weight. Add to the priority queue and mark as accessed.

[0059] S24 loops until termination; Repeat the above "pop-expand-calculate-enqueue" process until the queue is empty or the termination condition is met (reaching the target point or exceeding the bending cost limit).

[0060] Based on the local path name, retrieve p1, p2, ..., p in sequence. n Access its internal data layer. And access the internal pointer layer, enqueue the pointer of the next node; otherwise, do not access it. Here, p4 is accessed, its pointer layer node is accessed, the local path predecessor node Pre[4]=1 is recorded, and it is enqueued.

[0061] S3. Backtrack from the target node that meets the termination condition to generate an initial path node sequence, and perform smooth optimization on the sequence to output the final path that meets the bending radius constraint.

[0062] In this embodiment, in step S3, the smoothing optimization specifically involves applying cubic spline interpolation to the initial path node sequence obtained by backtracking and applying a maximum curvature constraint to obtain a smooth path with continuous curvature.

[0063] In the specific implementation steps, the path backtracking and smoothing optimization steps are as follows: Starting from the target point, trace back to the predecessor node to generate a node sequence (P={p1,p2,...,p...). n Extract the access order.

[0064] Apply cubic spline interpolation to the path (P) to constrain the maximum curvature (k). {max} Achieve curvature continuity optimization.

[0065] The beneficial effects of this invention are: 1. Real-time response capability in dynamic environments; It can simultaneously handle static obstacles, dynamic objects, and facilities such as traffic lights and speed limiters. It reduces computational latency and meets the real-time planning needs of complex urban roads.

[0066] 2. Balancing path smoothness and safety; By matching the road coordinate system trajectory and constraining curvature continuity, a smooth path that closely resembles human driving habits is generated, reducing the risk of sudden changes in lateral acceleration.

[0067] 3. Improved adaptability to complex roads; By dynamically matching the basic trajectory with the road geometry features, the probability of planning failure due to the complexity of road topology is reduced.

[0068] By using a layered processing approach (basic trajectory generation → matching → optimization), the algorithmic complexity of high-precision maps or scenes with dense obstacles is reduced.

[0069] 4. Multi-objective collaborative optimization; By considering objectives such as safety (distance to obstacles), smoothness (rate of curvature change), and efficiency (path length), a globally optimal path is generated through dynamic weight adjustment.

[0070] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention. The scope of the invention is defined by the appended claims and their equivalents, all of which should be included within the scope of protection of the invention.

Claims

1. A multi-point breadth-first search path planning method based on bending radius constraints, characterized in that, Includes the following steps: S1. Construct a directed weighted graph model of the environment map, where nodes represent key points of the path, and the weights of the edges are dynamically determined based on distance, curvature changes, and distance to obstacles. S2. Perform a multi-point breadth-first search, initialize a priority queue containing multiple candidate starting points, and based on the directed weighted graph, use the cumulative path curvature cost and the sum of edge weights as the search criteria to conduct the search in parallel from each starting point until the termination condition is met; wherein, the cumulative path curvature cost is calculated based on the curvature change of the path. S3. Backtrack from the target node that meets the termination condition to generate an initial path node sequence, and perform smooth optimization on the sequence to output the final path that meets the bending radius constraint.

2. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 1, characterized in that, The weight w of the edge is calculated using the following formula: Where d is the distance between nodes. For the curvature variation between nodes, The distance between the node and the obstacle. These are weighting coefficients that are dynamically adjusted based on the scenario.

3. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 1, characterized in that, In step S1, the nodes in the directed weighted graph model are stored in the form of a structure, which includes a data layer and a pointer layer. The data layer includes at least the node's position and normal direction information; The pointer layer is used to indicate the connection relationships between nodes.

4. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 1, characterized in that, In step S2, the initialization of the priority queue specifically involves: The vehicle's current location and alternative path branch points are added to the queue as candidate starting points. The initial cumulative bending cost of each starting point is set to 0, and they are sorted in ascending order according to the estimated total path weight.

5. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 1, characterized in that, In step S2, the search criteria include: taking the node with the smallest total weight from the queue for expansion, calculating the bending cost increment and total weight of its unvisited neighboring nodes, updating the cumulative bending cost and predecessor node information of the neighboring nodes, and then adding it to the queue.

6. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 5, characterized in that, In the calculation of the bending cost increment and total weight, the edge weight w is related to the bending radius r, specifically as follows: in, This is the curvature weighting coefficient. The penalty coefficient for changes in direction. This represents the difference in direction angle between adjacent nodes.

7. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 6, characterized in that, In step S2, the path accumulates bending cost. Calculated using the following formula: in, Let be the length of the i-th path segment. Let be the curvature of the i-th node.

8. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 7, characterized in that, In step S2, the termination condition is: the search reaches the preset target node, or the cumulative bending cost of the current path is reached. Exceeding the safety threshold.

9. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 1, characterized in that, In step S3, the smoothing optimization specifically involves applying cubic spline interpolation to the initial path node sequence obtained by backtracking and applying a maximum curvature constraint to obtain a smooth path with continuous curvature.

10. The multi-point breadth-first search path planning method based on bending radius constraints according to claim 1, characterized in that, In step S2, during the node expansion process, the predecessor node of each visited node is recorded in real time to form a predecessor node array, which is used for path backtracking in step S3.

Images