Global path planning method based on potential energy guided rapid search random tree algorithm

Abstract

The global path planning method based on potential energy guidance fast search random tree algorithm belongs to the technical field of intelligent vehicle path planning, and solves the technical problem that the traditional fast search random tree algorithm can cause the planned trajectory to travel close to the obstacles. First, the position of the obstacles and the start and end positions are determined through the two-dimensional space map planned by the vehicle. For any point in the map, the potential energy value thereof is calculated, and the potential energy values of all the obstacles are superimposed to obtain the potential energy value of the arbitrary point. The potential energy values of all the arbitrary points in the two-dimensional space map are obtained in the same way. Then, the potential energy values of each point of the whole map are normalized to obtain a potential energy map matrix. The potential energy map matrix is introduced into the fast search random tree algorithm, a potential energy threshold is set, and the randomly generated points are screened. After screening, the potential energy values are introduced into the cost function of the fast search random tree algorithm, and rewiring optimization is performed, so as to perform vehicle path planning.

Description

Technical Field

[0001] This invention belongs to the field of intelligent vehicle path planning technology, specifically involving a global path planning method based on a potential energy-guided fast search random tree algorithm. Background Technology

[0002] With the development of the times, intelligent vehicles have begun to take shape and are expected to provide drivers with a safe, comfortable, and efficient driving environment in the future. Research on intelligent vehicles mainly focuses on improving vehicle safety and comfort, as well as providing a superior human-vehicle interface. In recent years, intelligent vehicles have become a research hotspot in the field of vehicle engineering worldwide and a new driving force for the growth of the automotive industry. Many countries have incorporated them into their key intelligent transportation systems.

[0003] Current global path planning algorithms often focus solely on how obstacles disrupt path connections, neglecting the impact of obstacles themselves on the driver. Drivers typically aim to stay as far away from obstacles as possible, rather than merely "grazing" them. This ensures sufficient time for the driver to correct errors, guaranteeing overall driving safety. Traditional Fast Random Tree Search (RRT*) algorithms typically only address the single issue of obstacles disrupting node connections, often resulting in planned trajectories that closely follow obstacles, which does not reflect real-world driving conditions. Summary of the Invention

[0004] To address the issue that traditional fast random tree search (RRT*) algorithms typically only focus on the single problem of obstacles blocking node connections, often resulting in planned trajectories that closely follow obstacles, which does not reflect real-world driving conditions, this application provides a global path planning method based on the potential energy-guided fast random tree search algorithm.

[0005] This method first uses a two-dimensional spatial map of vehicle planning to identify the locations of obstacles and the start and end points. For any point on the map... Calculate the potential energy value of a point relative to each obstacle, and then sum the potential energy values ​​of all obstacles to obtain the result for any point. The potential energy value is obtained by the same method for all arbitrary points in the two-dimensional spatial map. Then, the potential energy value of each point in the entire map is normalized. The normalized value is between 0 and 1, resulting in a potential energy map matrix. The potential energy map matrix is ​​then introduced into the fast search random tree algorithm. In the initial stage of path planning, a potential energy threshold is set, and randomly generated points are selected. After selection, the potential energy value is introduced into the cost function of the fast search random tree algorithm for rewiring optimization, thereby performing vehicle path planning.

[0006] Furthermore, any point in the map does not include obstacles, and the potential energy value at the location of an obstacle is recorded as 1.

[0007] Furthermore, any point on the map The potential energy value relative to each obstacle is as follows: Obtain, among which, This represents the potential energy value. The potential energy decay rate, For point To the obstacle distance, For the scope of influence, This represents the distance from the point to the nearest point on the obstacle. , ) represents an obstacle. The coordinates of the centroid This indicates the square root operation.

[0008] further, The range of values ​​is .

[0009] Furthermore, the path planning method is as follows:

[0010] S1. Environment Initialization and Potential Energy Field Construction: Initialize the two-dimensional spatial map for vehicle planning, set the starting point X_start and the target point X_goal, and load obstacle information. Simultaneously, import the environmental potential energy map matrix, and set the maximum number of iterations K, the expansion step size, the neighborhood search radius, and the potential energy safety threshold. ;

[0011] S2. Constraint sampling based on potential energy: Randomly sample within the map space to generate candidate points X_rand, and read the potential energy value corresponding to that point in the potential energy map. ,judge Is it less than the set potential energy threshold? :

[0012] like If the point is in a high-risk area, the sampling point will be discarded and resampling will be performed.

[0013] like If the result is positive, retain that point as a valid sampling point and proceed to the next step.

[0014] S3. Node Extension and Collision Detection: Traverse all nodes in the current random tree, find the tree node X_nearest that is closest to X_rand in Euclidean distance, extend X_nearest towards X_rand with a fixed step size, and generate a new node X_new; check whether the path from X_nearest to X_new collides with an obstacle, and whether the potential energy values ​​of all discrete points on the path meet the potential energy threshold constraint; if the detection fails, abandon this extension and jump to step S2; if the detection passes, proceed to the next step.

[0015] S4. Optimal parent node selection considering both potential energy and path length: Using X_new as the center, set a set of potential parent nodes within the neighborhood search radius. Introduce a composite cost function that includes path length and accumulated potential energy to calculate the total cost when each neighborhood node is used as the parent node of X_new. Select the neighborhood node that minimizes the cost for X_new as its parent node and add X_new to the random tree.

[0016] S5. Rewiring optimization with potential weight: After selecting the parent node, traverse the other nodes in the neighborhood again; determine whether the cost of reaching these neighborhood nodes through X_new is lower than the cost of reaching these neighborhood nodes without X_new; if the cost of reaching these neighborhood nodes through X_new is lower and there is no collision, then change the parent node of the neighborhood node to X_new.

[0017] S6. Target Determination and Path Backtracking: Determine if the distance between X_new and the target point X_goal is less than a preset arrival threshold.

[0018] If the number of iterations is not less than the preset threshold, then set the number of iterations to... Return to step S2 and continue execution;

[0019] If the threshold is less than the preset threshold, the search is marked as successful. Starting from the target point X_goal, backtrack along the parent node index of each node to the starting point X_start, extract and output the final low-potential and collision-free safe path.

[0020] Furthermore, the composite cost function, which includes path length and accumulated potential energy, is as follows: ,in This represents the cost of planning the path. This represents the potential energy value of the planned path. Indicates the length of the planned path. and The coefficients are 1, and their sum is 1.

[0021] The beneficial effects of the method described in this invention are as follows:

[0022] The method of this invention ensures that the globally planned trajectory remains within a safe zone and provides the driver with sufficient time to deal with unexpected situations. By comprehensively considering the relationship between potential energy and path length, this method can plan a shorter and safer trajectory, which better meets actual driving needs.

[0023] While existing technologies incorporate potential energy guidance strategies into the RRT* algorithm or the RRT algorithm itself—for example, Chinese invention patent application "A Path Planning Method for Specific Target Search in Unknown Environments" (Publication No.: CN113467456 A), domestically published papers "Application Research of Improved Artificial Potential Field Method in Robot Path Planning" (Authors: Zhang Xiangfeng, Tian Jiaquan, Jiang Hong, Jin Xiaoqiang; Source: *Mechanical Design and Manufacturing*), and "Path Planning of Intelligent Agricultural Robot Based on APF-RRT* Algorithm" (Authors: Tan Xueyan, Huang Shufen, Zhang Hongtao, Tang Miaoqing, Zhang Yuying; Source: *Modern Information Technology*)—the underlying technical approach is primarily to guide the direction of random tree growth using the direction of forces in the gravitational potential field. The feasibility of the path is proven through a single action: detecting collisions. This means that adding a potential energy guidance strategy does not solve the technical problem of ensuring the drawn trajectory closely follows obstacles.

[0024] Unlike existing technologies that use the direction of forces in a gravitational potential field to guide the growth of random trees, this invention first generates a potential energy safety field. This involves quantifying the danger level of each point on the grid map. The generation of random trees is then guided by the quantified potential energy map matrix. High-risk sampling points are removed (in the traditional RRT* algorithm, random sampling is performed across the entire grid map without considering the risk level of a point). Collision detection is then performed. Furthermore, the cost function considers not only the path length but also the potential energy value of the path. This ensures that the trajectory generated by the method of this invention is safe and maintains a certain distance from obstacles, rather than simply considering "collision-free paths" as in existing technologies. Therefore, even if a sudden loss of control occurs, the safety margin provided by the method described in this invention can give the driver sufficient emergency reaction time, significantly improving driving safety. Attached Figure Description

[0025] Figure 1 This is a flowchart of the method described in an embodiment of the present invention;

[0026] Figure 2 This is the safety potential field diagram described in the embodiments of the present invention;

[0027] Figure 3 This is a route map planned using a global path planning method based on a potential energy-guided fast search random tree algorithm in an embodiment of the present invention. Detailed Implementation

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

[0029] Example 1

[0030] This embodiment provides a global path planning method based on a potential energy-guided fast search random tree algorithm. First, using a two-dimensional spatial map of the vehicle planning, the locations of obstacles and the start and end points are determined. For any point on the map... Calculate the potential energy value of a point relative to each obstacle, and then sum the potential energy values ​​of all obstacles to obtain the result for any point. The potential energy value is obtained by taking the same method to obtain the potential energy value of all arbitrary points in the two-dimensional spatial map. Then, the potential energy value of each point in the entire map is normalized. The normalized value is between 0 and 1. Any point in the map does not include obstacles. The potential energy value of the location of the obstacle is recorded as 1, thus obtaining the potential energy map matrix.

[0031] any point on the map The potential energy value relative to each obstacle is as follows: get.

[0032] in, This represents the potential energy value. The potential energy decay rate, For point To the obstacle distance, For the scope of influence, The range of values ​​is , This represents the distance from the point to the nearest point on the obstacle. , ) represents an obstacle. The coordinates of the centroid This indicates the square root operation.

[0033] The potential energy map matrix is ​​introduced into the RRT* path planning algorithm. In the initial stage of path planning, a potential energy threshold is set, randomly generated points are filtered, and the potential energy values ​​are introduced into the cost function of the RRT* path planning algorithm after filtering. The routing is re-optimized to perform vehicle path planning.

[0034] Using the above formula, as long as a point near the obstacle is between the influence range and the obstacle, the potential energy value of the obstacle relative to that point can be obtained. Then, by summing up the potential energy values ​​of all obstacles relative to that point, normalizing them, and drawing the entire safety potential field diagram, as shown below... Figure 2 As shown, Figure 2 The plots are generated using Metlab, with the default unit for the x and y axes being meters.

[0035] The potential energy value of the entire map is recorded for future use. Introducing the potential energy value into RRT* path planning ensures that the potential energy value of the entire trajectory path remains low. By using a potential energy-guided strategy to exclude locally high sampling points, and by considering potential energy in the cost function, the overall potential energy of the path is kept low. These two strategies effectively guarantee that the planned trajectory is in a safe state.

[0036] Example 2

[0037] This embodiment further defines Embodiment 1. The flowchart of the global path planning method based on the potential energy-guided RRT* algorithm is as follows: Figure 1 As shown.

[0038] The specific steps are as follows:

[0039] S1. Environment Initialization and Potential Energy Field Construction: Initialize the two-dimensional spatial map for vehicle planning, set the starting point X_start and the target point X_goal, and load obstacle information. Simultaneously, import the environmental potential energy map matrix, and set the maximum number of iterations K, the expansion step size, the neighborhood search radius, and the potential energy safety threshold. .

[0040] S2. Constraint sampling based on potential energy: Randomly sample within the map space to generate candidate points X_rand, and read the potential energy value corresponding to that point in the potential energy map. ,judge Is it less than the set potential energy threshold? :

[0041] like If the point is in a high-risk area, the sampling point will be discarded and resampling will be performed.

[0042] like If the result is positive, then retain that point as a valid sampling point and proceed to the next step.

[0043] S3. Node Extension and Collision Detection: Traverse all nodes in the current random tree, find the tree node X_nearest that is closest to X_rand in Euclidean distance, extend X_nearest towards X_rand by a fixed step size, and generate a new node X_new; check whether the path from X_nearest to X_new collides with an obstacle, and whether the potential energy values ​​of all discrete points on the path meet the potential energy safety threshold constraint; if the detection fails, abandon this extension and jump to step S2; if the detection passes, proceed to the next step.

[0044] S4. Optimal Parent Node Selection Considering Potential Energy and Path Length: Search for a set of potential parent nodes within a neighborhood centered on X_new and with a defined search radius. Introduce a composite cost function incorporating path length and accumulated potential energy to calculate the total cost when each neighboring node is used as a parent node. Select the neighboring node that minimizes the cost for X_new as its parent node, and add X_new to the random tree.

[0045] The composite cost function, which includes path length and accumulated potential energy, is as follows: .in This represents the cost of planning the path. This represents the potential energy value of the planned path, which is the sum of the potential energies of all points along the path in the potential energy map matrix.

[0046] Indicates the length of the planned path. and These are coefficients that add up to 1, and are related to the driver's style; a more aggressive driver will have them increased. This makes the planned path shorter.

[0047] In this step, treating each neighboring node as a parent node means treating each neighboring node as the parent node of X_new, thus obtaining a path from the starting point X_start to the parent node to the X_new node. Then, a composite cost function is used to calculate the total cost. Choosing the path that minimizes the cost of X_new means minimizing the cost of the path from X_start to the parent node to the X_new node.

[0048] S5. Rewiring optimization with potential weights: After selecting a parent node, traverse the other nodes in the neighborhood again; determine whether the cost of reaching these neighboring nodes through X_new is lower than the cost of reaching these neighboring nodes without X_new; if the cost of reaching these neighboring nodes through X_new is lower and there is no collision, then change the parent node of the neighboring node to X_new.

[0049] In this step, the same composite cost function as in step S4 is used to determine the total cost of each path.

[0050] S6. Target Determination and Path Backtracking: Determine if the distance between X_new and the target point X_goal is less than a preset arrival threshold.

[0051] If the number of iterations is not less than the preset threshold, then set the number of iterations to... Return to step S2 and continue execution;

[0052] If the threshold is less than the preset threshold, the search is marked as successful. Starting from the target point X_goal, backtrack along the parent node index of each node to the starting point X_start, extract and output the final low-potential, collision-free safe path.

[0053] After determining the start and end points and obtaining the potential energy matrix, the method described in this embodiment considers a potential energy guidance strategy to reduce the occurrence of high potential energy points during random sampling, expands new nodes outward, introduces potential energy values ​​into the cost function, reselects parent nodes, and obtains a better path. The path obtained after path planning using the method described in this embodiment is as follows: Figure 3 As shown. Figure 3 The plots are generated using Metlab, with the default unit for the x and y axes being meters.

[0054] The method described in this embodiment can be further extended. This embodiment mainly focuses on constructing a safe potential field and designing subsequent improvements to the RRT* algorithm. For the safe potential field, other potential energy models can be used to achieve the same effect, such as establishing a risk field using a Gaussian distribution or using distance or field strength to replace potential energy to establish a safe potential field. Furthermore, the path planning algorithm does not necessarily have to be a global path planning method; reinforcement learning can be adopted, with its reward function taking into account the relevant factors of potential energy and distance.

Claims

1. A global path planning method based on a potential energy-guided fast search random tree algorithm, characterized in that, The method first uses a two-dimensional spatial map planned by the vehicle to determine the location of obstacles and the start and end points. For any point on the map... Calculate the potential energy value of a point relative to each obstacle, and then sum the potential energy values ​​of all obstacles to obtain the result for any point. The potential energy value is obtained by taking the same method to obtain the potential energy value of all arbitrary points in the two-dimensional spatial map. Then, the potential energy value of each point in the entire map is normalized. The normalized value is between 0 and 1, and the potential energy map matrix is ​​obtained. The potential energy map matrix is ​​introduced into the fast search random tree algorithm. In the initial stage of path planning, a potential energy threshold is set, and randomly generated points are filtered. After filtering, the potential energy values ​​are introduced into the cost function of the fast search random tree algorithm for rewiring optimization, thereby performing vehicle path planning. Specifically: S1. Environment Initialization and Potential Energy Field Construction: Initialize the two-dimensional spatial map for vehicle planning, set the starting point X_start and the target point X_goal, and load obstacle information. Simultaneously, import the environmental potential energy map matrix, and set the maximum number of iterations K, the expansion step size, the neighborhood search radius, and the potential energy safety threshold. ; S2. Constraint sampling based on potential energy: Randomly sample within the map space to generate candidate points X_rand, and read the potential energy value corresponding to the point in the potential energy map matrix. ,judge Is it less than the set potential energy threshold? : like If the point is in a high-risk area, the sampling point will be discarded and resampling will be performed. like If the result is positive, retain that point as a valid sampling point and proceed to the next step. S3. Node Extension and Collision Detection: Traverse all nodes in the current random tree, find the tree node X_nearest that is closest to X_rand in Euclidean distance, extend X_nearest towards X_rand with a fixed step size, and generate a new node X_new; check whether the path from X_nearest to X_new collides with an obstacle, and whether the potential energy values ​​of all discrete points on the path meet the potential energy threshold constraint; if the detection fails, abandon this extension and jump to step S2; if the detection passes, proceed to the next step. S4. Optimal parent node selection considering both potential energy and path length: Using X_new as the center, set a set of potential parent nodes within the neighborhood search radius. Introduce a composite cost function that includes path length and accumulated potential energy to calculate the total cost when each neighborhood node is used as the parent node of X_new. Select the neighborhood node that minimizes the cost for X_new as its parent node and add X_new to the random tree. S5. Rewiring optimization with potential weight: After selecting the parent node, traverse the other nodes in the neighborhood again; determine whether the cost of reaching these neighborhood nodes through X_new is lower than the cost of reaching these neighborhood nodes without X_new; if the cost of reaching these neighborhood nodes through X_new is lower and there is no collision, then change the parent node of the neighborhood node to X_new. S6. Target Determination and Path Backtracking: Determine if the distance between X_new and the target point X_goal is less than a preset arrival threshold. If the number of iterations is not less than the preset threshold, then set the number of iterations to... Return to step S2 and continue execution; If the threshold is less than the preset threshold, the search is marked as successful. Starting from the target point X_goal, backtrack along the parent node index of each node to the starting point X_start, extract and output the final low-potential and collision-free safe path.

2. The global path planning method based on the potential energy-guided fast search random tree algorithm according to claim 1, characterized in that, Any point in the map does not include obstacles, and the potential energy value at the location of an obstacle is recorded as 1.

3. The global path planning method based on the potential energy-guided fast search random tree algorithm according to claim 2, characterized in that, any point on the map The potential energy value relative to each obstacle is as follows: Obtain, among which, This represents the potential energy value. The potential energy decay rate, For point To the obstacle distance, For the scope of influence, This represents the distance from the point to the nearest point on the obstacle. , ) represents an obstacle. The coordinates of the centroid This indicates the square root operation.

4. The global path planning method based on the potential energy-guided fast search random tree algorithm according to claim 3, characterized in that, The range of values ​​is .

5. The global path planning method based on the potential energy-guided fast search random tree algorithm according to claim 4, characterized in that, The composite cost function, which includes path length and accumulated potential energy, is as follows: ,in This represents the cost of planning the path. This represents the potential energy value of the planned path. Indicates the length of the planned path. and The coefficients are 1 and their sum is 1.

6. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method of claim 5.

7. A computer-readable storage medium for storing computer instructions, characterized in that, When the computer instructions are executed by the processor, they implement the steps of the method of claim 5.

Images