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

CN121994262ACN 121994262 ACN121994262 ACN 121994262ACN-121994262-A

Abstract

The invention discloses a multipoint breadth-first search path planning method based on bending radius constraint, which comprises the steps of constructing a directional weighted graph model of an environment map, carrying out multipoint breadth-first search, initializing a priority queue comprising a plurality of candidate starting points, based on the directional weighted graph, taking the accumulated bending cost and the sum of edge weights of paths as search basis, expanding search from each starting point in parallel until termination conditions are met, backtracking from target nodes meeting the termination conditions, 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, a dynamic weighted map model is built, multi-starting-point parallel breadth-first search is adopted, bending cost is accumulated in real time, and finally a smooth path conforming to bending radius constraint is generated through smooth optimization, so that the problem of vehicle running caused by improper path curvature is systematically solved.

Inventors

HUANG RUIQING
CHEN CHENG
WANG MING
QI JIAJUN

Assignees

安徽海博智能科技有限责任公司
安徽海螺集团有限责任公司
安徽海螺水泥股份有限公司

Dates

Publication Date: 20260508
Application Date: 20260227

Claims (10)

1. The multipoint breadth-first search path planning method based on the bending radius constraint is characterized by comprising the following steps of: s1, constructing a directed weighted graph model of an environment map, wherein nodes represent path key points, and the weights of edges are dynamically determined according to the distance, curvature change and obstacle distance; S2, carrying out multipoint breadth-first search, initializing a priority queue containing a plurality of candidate starting points, and based on the directed weighted graph, taking the sum of path accumulated bending cost and edge weight as a search basis, and expanding the search from each starting point in parallel until a termination condition is met; s3, backtracking from the target node meeting the termination condition, generating an initial path node sequence, performing smooth optimization on the sequence, and outputting a final path meeting the bending radius constraint.
2. The multipoint breadth-first search path planning method according to claim 1, wherein the weight w of the edge is calculated by the following formula: wherein d is the distance between the nodes, In order for the curvature to vary between the nodes, For the distance of the node from the obstacle, Is a weight coefficient dynamically adjusted according to the scene.
3. The method for planning a multi-point breadth-first search path based on bending radius constraint according to claim 1, wherein in the step S1, nodes are stored in a structure form in the directed weighted graph model, and the nodes comprise a data layer and a pointer layer; The data layer at least comprises position and normal direction information of nodes; The pointer layer is used for indicating the connection relation of the nodes.
4. The method for planning a multi-point breadth-first search path based on bending radius constraint according to claim 1, wherein in the step S2, the initializing priority queue specifically comprises: And taking the current position of the vehicle and branch points of the alternative paths as candidate starting points to be added into a queue, setting the initial accumulated bending cost of each starting point to be 0, and arranging the initial accumulated bending cost according to the total weight of the estimated paths in an ascending order.
5. The method for planning a multi-point breadth-first search path based on bending radius constraint according to claim 1, wherein in the step S2, the search basis comprises extracting the node with the smallest total weight from the queue for expansion, calculating the bending cost increment and total weight of the non-accessed adjacent node, updating the accumulated bending cost and precursor node information of the adjacent node, and adding the node to the queue.
6. The multipoint breadth-first search path planning method based on bending radius constraint according to claim 5, wherein in the calculation of the bending cost increment and the total weight, the weight w of the edge is associated with the bending radius r, and the specific relation is: Wherein, the As the coefficient of curvature weight, the curvature weight, For the direction change penalty factor, Is the adjacent node direction angle difference.
7. The multipoint breadth-first search path planning method according to claim 6, wherein in step S2, said path accumulates bending cost Calculated by the following formula: Wherein, the For the length of the path of the i-th segment, Is the curvature of the i-th node.
8. The method for multi-point breadth first search path planning based on bend radius constraint of claim 7, wherein in step S2, said termination condition is searching for cumulative bend cost of reaching a preset target node, or current path Exceeding the safety threshold.
9. The method for planning a multi-point breadth-first search path based on a radius of curvature constraint according to claim 1, wherein in step S3, the smoothing optimization is specifically to apply cubic spline interpolation to an initial path node sequence obtained by backtracking and apply a maximum curvature constraint to obtain a smooth path with continuous curvature.
10. The multipoint breadth-first search path planning method based on bending radius constraint according to claim 1, wherein in step S2, precursor nodes of each accessed node are recorded in real time during node expansion, and a precursor node array is formed for path backtracking in step S3.

Description

Multi-point breadth-first search path planning method based on bending radius constraint Technical Field The invention relates to the technical field of supervision information fusion of surface mines, in particular to a multipoint breadth-first search path planning method based on bending radius constraint. Background With the development of unmanned technology, the real-time performance, safety and smoothness of local path planning become key challenges. The conventional path planning method often ignores continuous curvature constraint of the path in a complex dynamic environment, so that the bending radius of the generated track is too small, and a series of problems such as instability of vehicle control, aggravation of mechanical wear, reduction of riding comfort and the like are caused. In unmanned local path planning, if the path bending radius is too small, the following problems occur when the vehicle automatically runs: 1. vehicle control instability and sideslip risk Too small a radius of curvature can cause the centrifugal force to increase dramatically when the vehicle turns, exceeding the tire grip limit, causing sideslip or even rollover. The ESP system is frequently intervened, and an electronic stabilization program can be forced to be intervened at high frequency to influence the driving smoothness. 2. Exacerbating mechanical losses The steering wheel needs to rotate frequently and at a large angle, so that the abrasion of a steering motor and tires is accelerated. The continuous sharp turns can cause the suspension components to experience additional stress, shortening the service life. 3. Passenger comfort degradation The discontinuous curvature of the path (such as a tight turn and a straight road) can cause the vehicle body to shake severely, so that passengers feel carsickness. The uneven path may cause the vehicle to vibrate at high frequencies. 4. Real-time performance of planning algorithm is impaired In order to repair a path with an excessively small bending radius, the algorithm needs additional iterative optimization (such as increasing path sampling points or adjusting cost function weights), increasing the computational complexity and slowing down the decision speed. If the optimization is too long, it may not be possible to respond to the bursty obstacle in time. In order to solve the above problems, the prior art mainly designs a cost function containing curvature constraint, or adopts a hybrid optimization method combining graph search and intelligent algorithm. However, the cost function-based method needs frequent weight adjustment in a dynamic scene, the calculation delay is obvious, the hybrid optimization method is easy to sink into local optimum, the dynamic adjustment of multiple target weights takes longer time, and the method is difficult to be applied to scenes with high real-time requirements such as high-speed driving. Therefore, a new path planning method is needed that can embed the bending radius constraint depth into the search process and combine the real-time response with the path smoothness. Disclosure of Invention The invention aims to overcome the defects in the prior art, and aims to solve the problems in the prior art by adopting a multi-point breadth-first search path planning method based on bending radius constraint. A multipoint breadth-first search path planning method based on bending radius constraint comprises the following steps: s1, constructing a directed weighted graph model of an environment map, wherein nodes represent path key points, and the weights of edges are dynamically determined according to the distance, curvature change and obstacle distance; S2, carrying out multipoint breadth-first search, initializing a priority queue containing a plurality of candidate starting points, and based on the directed weighted graph, taking the sum of path accumulated bending cost and edge weight as a search basis, and expanding the search from each starting point in parallel until a termination condition is met; s3, backtracking from the target node meeting the termination condition, generating an initial path node sequence, performing smooth optimization on the sequence, and outputting a final path meeting the bending radius constraint. As a further aspect of the invention, the weight (w) of the edge is calculated by the following formula: wherein d is the distance between the nodes, In order for the curvature to vary between the nodes,For the distance of the node from the obstacle,Is a weight coefficient dynamically adjusted according to the scene. In the step S1, in the directed weighted graph model, nodes are stored by adopting a structural body shape and comprise a data layer and a pointer layer; The data layer at least comprises position and normal direction information of nodes; The pointer layer is used for indicating the connection relation of the nodes. In the step S2, the initializing the priority queue specifically includes: And ta