A route search method for intelligent vehicles based on heuristic RRT*

Abstract

The present invention relates to the technical field of vehicle path planning, and more specifically to an intelligent vehicle path search method based on heuristic RRT*. A heuristic function is introduced into the traditional RRT* algorithm, so that it has a tendency to move toward the end point when developing new nodes. First, an empty tree is created on a known map, and the first new node is generated at the starting point as the initial node. Then, a grower is updated, and new nodes are generated and added to the tree after verification of eligibility. If unqualified, they are randomly regenerated until they are qualified. The process of constructing nodes is guided by a heuristic function; then, the new nodes are rewritten and rewired to improve the node connection method in the tree. By improving the node growth method, node generation is not just randomly moving toward the end point with a certain probability, but rather preferentially growing advantageous nodes after considering the heuristic function. The present invention improves computational efficiency, reduces computational time complexity, and can better search for paths on a known map.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle path planning, and in particular to an intelligent vehicle path search method based on heuristic RRT*. Background Art

[0002] Path planning is a classic problem for safely navigating dynamic, dense, and unknown environments. To ensure safe and efficient point-to-point navigation, an appropriate algorithm should be selected. Among the many path planning algorithms, the Rapidly-Exploring Random Tree Star (RRT*) algorithm, a sampling-based path planning algorithm, exhibits good adaptability and scalability in unknown or dynamic environments. Therefore, it is widely used in fields such as robotic path planning, game development, drone flight, and autonomous driving.

[0003] The RRT* algorithm is an improvement to the traditional RRT (Rapidly-exploring Random Tree) algorithm. The RRT algorithm constructs a random tree in free space through random sampling and incremental expansion to search for a path from a starting point to a target point. However, the RRT algorithm does not guarantee the optimal path and may contain redundant nodes and corners, resulting in an unsmooth path and affecting the robot's motion performance.

[0004] To solve this problem, the RRT* algorithm introduces rewriting and rewiring to optimize the generated path. When adding a new node to the search tree, the RRT algorithm not only considers the nearest node as its parent node, but also considers all nodes within a neighborhood circle with the new node as the center and r1 as the optimization radius. Within this neighborhood circle, the RRT* algorithm will find the point with the lowest path cost after connecting to the new node and use it as the parent node of the new node; at the same time, the RRT* algorithm will let all nodes within another neighborhood circle with a radius of r2 consider whether changing their parent node to the new node makes the path better. In this way, not only is it guaranteed that the newly added node is locally optimal, but as the tree expands, the entire path will gradually approach the global optimal solution.

[0005] While the RRT* algorithm has achieved significant progress in path planning, it still has some limitations. For example, its computational efficiency is relatively low due to the need for continuous collision detection and rewriting and relinking operations, which may make it unsuitable for scenarios with high real-time requirements. Furthermore, the performance of the RRT* algorithm is sensitive to the choice of parameters such as step size and sampling strategy, requiring appropriate parameter tuning. Summary of the Invention

[0006] The purpose of this invention is to provide an intelligent vehicle path search method based on heuristic RRT*, aiming to solve the technical problem that traditional sampling-based path planning methods cannot meet the real-time requirements when autonomous driving and unmanned aerial vehicle navigation encounter complex and dynamic environments.

[0007] To achieve the above object, the present invention provides an intelligent vehicle path search method based on heuristic RRT*, comprising the following steps:

[0008] Step 1: Receive map information and initialize it, and determine to create an empty tree and initial nodes based on the map information;

[0009] Step 2: Update the grower in the tree, generate a new node by the grower, check the eligibility of the new node, if qualified, add it to the tree; if not, randomly generate a new node until the new node is qualified;

[0010] Step 3: Rewrite and rewire the new nodes to change the way the nodes in the tree are connected;

[0011] Step 4: Determine whether the new node meets the end requirements. If so, record the node and output the linked list it is in as the result; if not, go to step 2 and continue to generate new nodes.

[0012] Optionally, the map in step 1 is an already established map, which is fixed and unchanged. The map must have three members: a starting point, an end point, and an obstacle set. The obstacle set can be an empty set.

[0013] In step 1, nodes and trees are generated. The tree is a random tree. Nodes are the basic components of a random tree, representing the location of a feasible path. They have three private members: parent node pointer, node position, and heuristic function. The tree is both a collection of nodes and the commander of node growth, determining which nodes are eligible for growth. It has two private members: node set and grower.

[0014] The initial node is created at the starting position and added to the tree.

[0015] Optionally, the grower is a subset of the node set. Only nodes in the set are eligible to grow new nodes. It has two private members: grower capacity and eligible node container.

[0016] The grower capacity is the number of nodes that the grower can accommodate, that is, the number of nodes in the tree that are eligible for growth; the eligible node container is a container that stores pointers to nodes that are eligible for growth.

[0017] The heuristic function consists of four terms: starting cost term, end cost term, obstacle cost term, and curve smoothness term;

[0018] Optionally, the calculation formula of the heuristic function _inferioritylevel is

[0019]

[0020] Where _inferioritylevel is the heuristic function value; _distoorigincost is the starting cost term, which is the path distance from the node to the starting point; is the cost term of curve smoothness, which is the node N i and the positions of all its previous parent nodes; _distoendcost is the end cost item, which is N i Position to end point P goal Manhattan distance; _obstaclecost is the obstacle cost item, which is N i A magnification term representing the minimum distance from the represented robot or vehicle outline to any obstacle outline.

[0021] Optionally, the grower further has two member functions, namely, an update grower function and a growth function;

[0022] The update grower function has the function of sorting all nodes in the tree according to the heuristic function value from small to large, except for the initial node N init When any new node is added to the tree, the update grower function will be run. The new node is inserted into the appropriate position of the grower using the binary sorting principle, so that its heuristic function value is sorted from small to large. If there are multiple nodes with the same heuristic function value, the new node is always sorted before the old node.

[0023] The growth function is responsible for generating new nodes and also for detecting whether the nodes are qualified.

[0024] Optionally, the rewriting and rewiring functions in step 3 are member functions of the random tree, and the running steps are as follows:

[0025] Search all nodes in the tree, if the new node N new To any node N i The distance is less than the rewriting and rewiring range D scope , then N i Add to array N WithinScope middle;

[0026] Rewrite: If array N WithinScope A node in becomes N new After the parent node, N new The road cost is smaller, then change N new The parent node is array N WithinScope The node in the loop traverses the entire array;

[0027] Rewiring: If array NWithinScope The parent node of a node in is changed to N new After that, the road cost of the node is smaller, then change the array N WithinScope The parent node of this node is N new , looping through the entire array.

[0028] This invention provides an intelligent vehicle path search method based on heuristic RRT*. This method introduces a heuristic function into the traditional RRT* algorithm, which allows it to tend toward the endpoint when developing new nodes. First, an empty tree is created on a known map, and the first new node is generated at the starting point as the initial node. Next, a grower is updated to generate new nodes, which are added to the tree after verification. If they fail, they are randomly regenerated until they pass. The node construction process is guided by a heuristic function. The new nodes are then rewritten and rewired to improve the node connection structure within the tree. This improvement in node growth ensures that node generation does not simply randomly progress toward the endpoint with a certain probability, but rather prioritizes advantageous nodes after considering the heuristic function. This method improves computational efficiency, reduces computational time complexity, and enables better path search on a known map. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0030] Figure 1 This is a flowchart of the steps of an intelligent vehicle path search method based on heuristic RRT* of the present invention.

[0031] Figure 2 This invention is class N i Schematic diagram of the composition structure.

[0032] Figure 3 It is a schematic diagram of the composition structure of class T of the present invention.

[0033] Figure 4 It is a schematic diagram of the composition structure of the GrowthApparatus class of the present invention. DETAILED DESCRIPTION

[0034] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0035] See also Figure 1 The present invention provides an intelligent vehicle path search method based on heuristic RRT*, comprising the following steps:

[0036] S1: Receive map information and initialize it, and determine to create an empty tree and initial nodes according to the map information;

[0037] S2: Update the grower in the tree, generate a new node by the grower, check the eligibility of the new node, if qualified, add it to the tree; if not, randomly generate a new node until the new node is qualified;

[0038] S3: rewrite and rewire the new nodes, changing the way nodes in the tree are connected;

[0039] S4: Determine whether the new node meets the end requirements. If so, record the node and output the linked list it is in as the result; if not, go to step S2 and continue to generate a new node.

[0040] The following is a detailed explanation of some variables or key functions in conjunction with specific embodiments and operation steps:

[0041] 1. Node construction and assignment

[0042] Class N i is a node, the subscript i distinguishes different nodes, N i There are three private members: parent node pointer, node position, and heuristic function. Each new node needs to be initialized with these three private members. Figure 2 Schematic diagram of the node composition.

[0043] Constructing a node involves the following steps:

[0044] Step 1, assign the parent node pointer fathernode.

[0045] Step 2: Assign the node position carRWMidpoint.

[0046] Step 3: Calculate the heuristic function inferiority level.

[0047] The parent node pointer stores the pointer of the parent node of the node, the initial value is 0, except for the initial pointer N init The parent node pointer is 0, and the items of other nodes cannot be empty, so that they form a linked list to facilitate retrieval and printing results.

[0048] The node position can be the position of the robot or the target vehicle. It needs to have two elements: Cartesian coordinates and direction. In the verification program of the method of the present invention, its Cartesian coordinates are the midpoint of the rear axle of the vehicle, and the calculation formula of direction_pose is

[0049]

[0050] Where k is the node N i The fifth-order polynomial curve fitted by the positions of its first five parent nodes is in N i The slope of the tangent line at the position, α is N i The angle between the vector formed by connecting the position of the node and its parent node and the unit vector of the tangent line pointing in the positive direction of the x-axis.

[0051] The calculation formula of the heuristic function _inferioritylevel is

[0052]

[0053] _inferioritylevel is the heuristic function value.

[0054] _distoorigincost is the origin cost item, which is the path distance from the node to the origin. Its calculation formula is

[0055]

[0056] in For node N i The fitting curve distance from its parent node, which is N i In the curve fitted by the fifth-order polynomial of its first five parent nodes, N i To its parent node N i′s fathernode The curve integral of the segment is calculated as

[0057]

[0058] Where C0, C1, ..., C5 are N i The coefficients of the fifth-order polynomial curve fitted by the positions of its first five parent nodes, x i N i′s fathernode The x-axis coordinate of the node position starts at N i One tenth of the differential of the x-coordinate of the position.

[0059] is the curve smoothness cost term, which is N i and all its previous parent nodes (i.e. N iThe position of the node in the linked list) and the standard deviation of the derivative of the curve fitted by the 5th degree polynomial. Where _rank is the position of the node in its linked list, representing how many parent nodes (N) there are before the node. init _rank is 0), _smoothnessSum is the sum of curve smoothness, and the calculation formula is

[0060]

[0061] in Yes N i The sample standard deviation of the derivative of the 5th-order polynomial fitting curve at the positions of its first 5 parent nodes is calculated as follows:

[0062]

[0063] Where D0, D1, ..., D5 are N i The derivative function coefficients of the fifth-order polynomial curve fitted by the positions of its first five parent nodes, x i N i The x-axis coordinate of the parent node position starts at N i 1 / 50 of the differential of the x-axis coordinate of the position, N i and the mean of the derivatives of the fifth-order polynomial curve fitted to the positions of its first five parent nodes.

[0064] _distoendcost is the end cost term, which is N i Position to P goal The Manhattan distance (ManhattanDistance) is calculated as follows:

[0065]

[0066] in For node N i The x-axis coordinate, is the x-axis coordinate of the end point, For node N i The y-axis coordinate, is the y-axis coordinate of the end point.

[0067] _obstaclecost is the obstacle cost item, which is N i The amplification term of the minimum distance from the robot or vehicle outline represented to the outline of any obstacle is calculated as follows:

[0068]

[0069] Among them D OBmin N iThe minimum distance from the represented robot or vehicle outline to any obstacle outline.

[0070] 2. Tree composition and member functions

[0071] See also Figure 3 , class T is a random tree, consisting of node N i In addition to being a collection of all nodes, it also has the ability to filter nodes eligible for growth, rewrite, and reconnect. Its constructor is empty (does nothing). It has two private members: a nodeset and a growth apparatus. The nodeset is a container for all nodes and a single entity for all nodes during algorithm execution. The growth apparatus contains pointers to eligible nodes. Class T has two member functions: Join() and RewriteAndReconnect().

[0072] The logic of the join function is relatively simple, which is to add the new node to the node set.

[0073]

[0074] The rewrite and rewire function is more complicated. Here are the steps to run the function:

[0075] Step 1: Search all nodes in the tree and if the new node N new To any node N i The distance is less than the rewriting and rewiring range D scope , then N i Add to array N WithinScope middle.

[0076] Step 2, rewrite. If the array N WithinScope A node in becomes N new After the parent node, N new The road cost is smaller, then change N new The parent node is array N WithinScope , looping through the entire array.

[0077] Step 3, rewiring. If the array N WithinScope The parent node of a node in is changed to N new After that, the road cost of the node is smaller, then change the array N WithinScope The parent node of this node is N new , looping through the entire array.

[0078] The road cost of a node is recorded as The calculation formula is

[0079]

[0080] Among them, _distoorigincost is the starting cost item, is the cost term for curve smoothness.

[0081] 3. Composition and membership functions of the grower

[0082] See also Figure 4 The grower (GrowthApparatus) has two private members: the grower capacity (_GQCsize) and the qualified node container (_GrowthQualificationContainer). Its constructor is empty (does nothing). The grower capacity is the number of nodes that the grower can accommodate, that is, the number of nodes in the tree that are eligible for growth. The method of the present invention sets it to 500. The qualified node container is a container that stores pointers to nodes that are eligible for growth. The grower has two member functions, namely, update the grower (Refresh()) and grow (Grow()).

[0083] The update grower function has the function of sorting all nodes in the tree according to the heuristic function value from small to large (the smaller the heuristic function value of the node, the more advantageous it is), except N init When any new node is added to the tree, the update grower function will be run. The new node is inserted into the appropriate position of the grower using the binary sorting principle, so that its heuristic function value is sorted from small to large. If there are multiple nodes with the same heuristic function value, the new node is always sorted before the old node. The following are the steps for running this function:

[0084] Step 1: Create three subscripts for the container _GrowthQualificationContainer: high, low, and mid. If the number of nodes stored in _GrowthQualificationContainer is less than _GQCsize, set high to the number of nodes stored in the container; otherwise, set high to _GQCsize. Set low to 0.

[0085] Step 2, run the binary search loop. If the input node N of the function new satisfy

[0086] N new._ inferioritylevel≤_GrowthQualificationContainer[mid]._inferioritylevel, let high equal mid, otherwise let low equal mid.

[0087] Step 3: When high>low+1 is satisfied, run the loop and go to step 2; otherwise, end the loop.

[0088] Step 4: N new Place it after the _GrowthQualificationContainer[high] item in the container.

[0089] Step 5: If the container capacity is greater than 500, it is considered overflow and the last item is deleted.

[0090] The growth function is responsible for generating new nodes. Its generation rules are similar to those of the general RRT algorithm: a random point is sampled on the map, and the node closest to the random point in the eligible node container is found. This node is then directed toward the random point to generate a new node, which becomes the parent node of the new node. The distance between the new node and the old node is fixed as the growth step length. The growth function also checks whether a node is eligible. The following are the steps for this function:

[0091] Step 1: Collect random points within M, whose coordinates are two random numbers (x rand ,y rand ).

[0092] Step 2: Find the node closest to the random point in the grower and assign it to the node N selected for growth when the function is run. select .

[0093] Step 3: Generate a new node. The position of the new node is N new .carRWMidpoint() is N select The position plus N select The unit vector of the vector connected with the position of the random point and the number of growth steps

[0094]

[0095] The parent node of the new node is N select , calculate the heuristic function value _inferioritylevel of the new node.

[0096] Step 4: Detect the node. If N new If the robot or vehicle outline it represents does not collide with obstacles, and the outline trajectory formed by it and its parent node does not collide with obstacles, then the node is qualified and the new node N new As the function output; otherwise, clear N new Memory, return 0 means the generated node is unqualified or failed.

[0097] In summary, compared with the traditional RRT* algorithm, the present invention improves the computational efficiency, reduces the time complexity of the computation, and can better search for paths on a known map.

[0098] The above disclosure is only a preferred embodiment of the present invention, and certainly cannot be used to limit the scope of the rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above embodiment and equivalent changes made in accordance with the claims of the present invention are still within the scope of the invention.

Claims

1. An intelligent vehicle path search method based on heuristic RRT*, characterized in that: The following steps are involved: Step 1: Receive map information and initialize it, and determine to create an empty tree and initial nodes based on the map information; The map in step 1 is a pre-established map, which is fixed and unchanged. The map must have three members: a starting point, an end point, and an obstacle set. The obstacle set can be an empty set. In step 1, nodes and trees are generated. The tree is a random tree. Nodes are the basic components of a random tree, representing the location of a feasible path. They have three private members: parent node pointer, node position, and heuristic function. The tree is both a collection of nodes and the commander of node growth, determining which nodes are eligible for growth. It has two private members: node set and grower. The initial node is created at the starting position and added to the tree; The grower is a subset of the node set. Only nodes in the set are eligible to grow new nodes. It has two private members: grower capacity and eligible node container. The grower capacity is the number of nodes that the grower can accommodate, that is, the number of nodes in the tree that are eligible for growth; the eligible node container is a container that stores pointers to nodes that are eligible for growth; The heuristic function consists of four terms: starting cost term, end cost term, obstacle cost term, and curve smoothness term; The calculation formula of the heuristic function _inferioritylevel is Where _inferioritylevel is the heuristic function value; _distoorigincost is the starting cost term, which is the path distance from the node to the starting point; is the cost term of curve smoothness, which is the node N i and the positions of all its previous parent nodes; _distoendcost is the end cost item, which is N i Position to end point P goal Manhattan distance; _obstaclecost is the obstacle cost item, which is N i The magnification term of the minimum distance between the robot or vehicle outline represented and the outline of any obstacle; Step 2: Update the grower in the tree, generate a new node by the grower, check the eligibility of the new node, if qualified, add it to the tree; if not, randomly generate a new node until the new node is qualified; Step 3: Rewrite and rewire the new nodes to change the way the nodes in the tree are connected; Step 4: Determine whether the new node meets the end requirements. If so, record the node and output the linked list it is in as the result; if not, go to step 2 and continue to generate new nodes.

2. The intelligent vehicle path search method based on heuristic RRT* according to claim 1, characterized in that: The grower also has two member functions, namely, an update grower function and a growth function; The update grower function has the function of sorting all nodes in the tree according to the heuristic function value from small to large, except for the initial node N init When any new node is added to the tree, the update grower function will be run. The new node is inserted into the appropriate position of the grower using the binary sorting principle, so that its heuristic function value is sorted from small to large. If there are multiple nodes with the same heuristic function value, the new node is always sorted before the old node. The growth function is responsible for generating new nodes and also for detecting whether the nodes are qualified.

3. The intelligent vehicle path search method based on heuristic RRT* as claimed in claim 2, characterized in that: The rewriting and rewiring functions in step 3 are member functions of the random tree, and the running steps are as follows: Search all nodes in the tree, if the new node N new To any node N i The distance is less than the rewriting and rewiring range D scope , then N i Add to array N WithinScope middle; Rewrite: If array N WithinScope A node in becomes N new After the parent node, N new The road cost is smaller, then change N new The parent node is array N WithinScope The node in the loop traverses the entire array; Rewiring: If array N WithinScope The parent node of a node in is changed to N new After that, the road cost of the node is smaller, then change the array N WithinScope The parent node of this node is N new , looping through the entire array.

Images